sim_jtag_socket_c.c

   1 #include <stdint.h>
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <unistd.h>
   6 #include <poll.h>
   7 #include <signal.h>
   8 #include <fcntl.h>
   9 #include <sys/types.h>
  10 #include <sys/socket.h>
  11 #include <netinet/in.h>
  12 #include "sim_vhpi_c.h"
  13
  14 /* XXX Make that some parameter */
  15 #define TCP_PORT        13245
  16 #define MAX_PACKET      32
  17
  18 static int fd = -1;
  19 static int cfd = -1;
  20
  21 static void open_socket(void)
  22 {
  23         struct sockaddr_in addr;
  24         int opt, rc, flags;
  25
  26         if (fd >= 0 || fd < -1)
  27                 return;
  28
  29         signal(SIGPIPE, SIG_IGN);
  30         fd = socket(AF_INET, SOCK_STREAM, 0);
  31         if (fd < 0) {
  32                 fprintf(stderr, "Failed to open debug socket !\r\n");
  33                 goto fail;
  34         }
  35
  36         rc = 0;
  37         flags = fcntl(fd, F_GETFL);
  38         if (flags >= 0)
  39                 rc = fcntl(fd, F_SETFL, flags | O_NONBLOCK);
  40         if (flags < 0 || rc < 0) {
  41                 fprintf(stderr, "Failed to configure debug socket !\r\n");
  42         }
  43
  44         memset(&addr, 0, sizeof(addr));
  45         addr.sin_family = AF_INET;
  46         addr.sin_port = htons(TCP_PORT);
  47         addr.sin_addr.s_addr = htonl(INADDR_ANY);
  48         opt = 1;
  49         setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
  50         rc = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
  51         if (rc < 0) {
  52                 fprintf(stderr, "Failed to bind debug socket !\r\n");
  53                 goto fail;
  54         }
  55         rc = listen(fd,1);
  56         if (rc < 0) {
  57                 fprintf(stderr, "Failed to listen to debug socket !\r\n");
  58                 goto fail;
  59         }
  60         fprintf(stdout, "Debug socket ready\r\n");
  61         return;
  62 fail:
  63         if (fd >= 0)
  64                 close(fd);
  65         fd = -2;
  66 }
  67
  68 static void check_connection(void)
  69 {
  70         struct sockaddr_in addr;
  71         socklen_t addr_len = sizeof(addr);
  72
  73         cfd = accept(fd, (struct sockaddr *)&addr, &addr_len);
  74         if (cfd < 0)
  75                 return;
  76         fprintf(stdout, "Debug client connected !\r\n");
  77 }
  78
  79 void sim_jtag_read_msg(unsigned char *out_msg, unsigned char *out_size)
  80 {
  81         unsigned char data[MAX_PACKET];
  82         unsigned char size = 0;
  83         struct pollfd fdset[1];
  84         int rc, i;
  85
  86         if (fd == -1)
  87                 open_socket();
  88         if (fd < 0)
  89                 goto finish;
  90         if (cfd < 0)
  91                 check_connection();
  92         if (cfd < 0)
  93                 goto finish;
  94
  95         memset(fdset, 0, sizeof(fdset));
  96         fdset[0].fd = cfd;
  97         fdset[0].events = POLLIN;
  98         rc = poll(fdset, 1, 0);
  99         if (rc <= 0)
 100                 goto finish;
 101         rc = read(cfd, data, MAX_PACKET);
 102         if (rc < 0)
 103                 fprintf(stderr, "Debug read error, assuming client disconnected !\r\n");
 104         if (rc == 0)
 105                 fprintf(stdout, "Debug client disconnected !\r\n");
 106         if (rc <= 0) {
 107                 close(cfd);
 108                 cfd = -1;
 109                 goto finish;
 110         }
 111
 112 #if 0
 113         fprintf(stderr, "Got message:\n\r");
 114         {
 115                 for (i=0; i<rc; i++)
 116                         fprintf(stderr, "%02x ", data[i]);
 117                 fprintf(stderr, "\n\r");
 118         }
 119 #endif
 120         size = data[0]; /* Size in bits */
 121
 122         /* Special sizes */
 123         if (size == 255) {
 124                 /* JTAG reset, message to translate */
 125                 goto finish;
 126         }
 127
 128         if (((rc - 1) * 8) < size) {
 129                 fprintf(stderr, "Debug short read: %d bytes for %d bits, truncating\r\n",
 130                         rc - 1, size);
 131                 size = (rc - 1) * 8;
 132         }
 133
 134         for (i = 0; i < size; i++) {
 135                 int byte = i >> 3;
 136                 int bit = 1 << (i & 7);
 137                 out_msg[i] = (data[byte+1] & bit) ? vhpi1 : vhpi0;
 138         }
 139 finish:
 140         to_std_logic_vector(size, out_size, 8);
 141 }
 142
 143 void sim_jtag_write_msg(unsigned char *in_msg, unsigned char *in_size)
 144 {
 145         unsigned char data[MAX_PACKET];
 146         unsigned char size;
 147         int rc, i;
 148
 149         size = from_std_logic_vector(in_size, 8);
 150         data[0] = size;
 151         for (i = 0; i < size; i++) {
 152                 int byte = i >> 3;
 153                 int bit = 1 << (i & 7);
 154                 if (in_msg[i] == vhpi1)
 155                         data[byte+1] |= bit;
 156                 else
 157                         data[byte+1] &= ~bit;
 158         }
 159         rc = (size + 7) / 8;
 160
 161 #if 0
 162         fprintf(stderr, "Sending response:\n\r");
 163         {
 164                 for (i=0; i<rc; i++)
 165                         fprintf(stderr, "%02x ", data[i]);
 166                 fprintf(stderr, "\n\r");
 167         }
 168 #endif
 169
 170         rc = write(cfd, data, rc);
 171         if (rc < 0)
 172                 fprintf(stderr, "Debug write error, ignoring\r\n");
 173 }
 174