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
  13 /* XXX Make that some parameter */
  14 #define TCP_PORT        13245
  15 #define MAX_PACKET      32
  16
  17 #define vhpi0   2       /* forcing 0 */
  18 #define vhpi1   3       /* forcing 1 */
  19
  20 static void to_std_logic_vector(unsigned long val, unsigned char *p,
  21                                 unsigned long len)
  22 {
  23         if (len > 64) {
  24                 fprintf(stderr, "%s: invalid length %lu\n", __func__, len);
  25                 exit(1);
  26         }
  27
  28         for (unsigned long i = 0; i < len; i++) {
  29                 if ((val >> (len-1-i) & 1))
  30                         *p = vhpi1;
  31                 else
  32                         *p = vhpi0;
  33
  34                 p++;
  35         }
  36 }
  37
  38 static uint64_t from_std_logic_vector(unsigned char *p, unsigned long len)
  39 {
  40         unsigned long ret = 0;
  41
  42         if (len > 64) {
  43                 fprintf(stderr, "%s: invalid length %lu\n", __func__, len);
  44                 exit(1);
  45         }
  46
  47         for (unsigned long i = 0; i < len; i++) {
  48                 unsigned char bit;
  49
  50                 if (*p == vhpi0) {
  51                         bit = 0;
  52                 } else if (*p == vhpi1) {
  53                         bit = 1;
  54                 } else {
  55                         fprintf(stderr, "%s: bad bit %d\n", __func__, *p);
  56                         bit = 0;
  57                 }
  58
  59                 ret = (ret << 1) | bit;
  60                 p++;
  61         }
  62
  63         return ret;
  64 }
  65
  66 static int fd = -1;
  67 static int cfd = -1;
  68
  69 static void open_socket(void)
  70 {
  71         struct sockaddr_in addr;
  72         int opt, rc, flags;
  73
  74         if (fd >= 0 || fd < -1)
  75                 return;
  76
  77         signal(SIGPIPE, SIG_IGN);
  78         fd = socket(AF_INET, SOCK_STREAM, 0);
  79         if (fd < 0) {
  80                 fprintf(stderr, "Failed to open debug socket !\r\n");
  81                 goto fail;
  82         }
  83
  84         rc = 0;
  85         flags = fcntl(fd, F_GETFL);
  86         if (flags >= 0)
  87                 rc = fcntl(fd, F_SETFL, flags | O_NONBLOCK);
  88         if (flags < 0 || rc < 0) {
  89                 fprintf(stderr, "Failed to configure debug socket !\r\n");
  90         }
  91
  92         memset(&addr, 0, sizeof(addr));
  93         addr.sin_family = AF_INET;
  94         addr.sin_port = htons(TCP_PORT);
  95         addr.sin_addr.s_addr = htonl(INADDR_ANY);
  96         opt = 1;
  97         setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
  98         rc = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
  99         if (rc < 0) {
 100                 fprintf(stderr, "Failed to bind debug socket !\r\n");
 101                 goto fail;
 102         }
 103         rc = listen(fd,1);
 104         if (rc < 0) {
 105                 fprintf(stderr, "Failed to listen to debug socket !\r\n");
 106                 goto fail;
 107         }
 108         fprintf(stdout, "Debug socket ready\r\n");
 109         return;
 110 fail:
 111         if (fd >= 0)
 112                 close(fd);
 113         fd = -2;
 114 }
 115
 116 static void check_connection(void)
 117 {
 118         struct sockaddr_in addr;
 119         socklen_t addr_len = sizeof(addr);
 120
 121         cfd = accept(fd, (struct sockaddr *)&addr, &addr_len);
 122         if (cfd < 0)
 123                 return;
 124         fprintf(stdout, "Debug client connected !\r\n");
 125 }
 126
 127 void sim_jtag_read_msg(unsigned char *out_msg, unsigned char *out_size)
 128 {
 129         unsigned char data[MAX_PACKET];
 130         unsigned char size = 0;
 131         struct pollfd fdset[1];
 132         int rc, i;
 133
 134         if (fd == -1)
 135                 open_socket();
 136         if (fd < 0)
 137                 goto finish;
 138         if (cfd < 0)
 139                 check_connection();
 140         if (cfd < 0)
 141                 goto finish;
 142
 143         memset(fdset, 0, sizeof(fdset));
 144         fdset[0].fd = cfd;
 145         fdset[0].events = POLLIN;
 146         rc = poll(fdset, 1, 0);
 147         if (rc <= 0)
 148                 goto finish;
 149         rc = read(cfd, data, MAX_PACKET);
 150         if (rc < 0)
 151                 fprintf(stderr, "Debug read error, assuming client disconnected !\r\n");
 152         if (rc == 0)
 153                 fprintf(stdout, "Debug client disconnected !\r\n");
 154         if (rc <= 0) {
 155                 close(cfd);
 156                 cfd = -1;
 157                 goto finish;
 158         }
 159
 160 #if 0
 161         fprintf(stderr, "Got message:\n\r");
 162         {
 163                 for (i=0; i<rc; i++)
 164                         fprintf(stderr, "%02x ", data[i]);
 165                 fprintf(stderr, "\n\r");
 166         }
 167 #endif
 168         size = data[0]; /* Size in bits */
 169
 170         /* Special sizes */
 171         if (size == 255) {
 172                 /* JTAG reset, message to translate */
 173                 goto finish;
 174         }
 175
 176         if (((rc - 1) * 8) < size) {
 177                 fprintf(stderr, "Debug short read: %d bytes for %d bits, truncating\r\n",
 178                         rc - 1, size);
 179                 size = (rc - 1) * 8;
 180         }
 181
 182         for (i = 0; i < size; i++) {
 183                 int byte = i >> 3;
 184                 int bit = 1 << (i & 7);
 185                 out_msg[i] = (data[byte+1] & bit) ? vhpi1 : vhpi0;
 186         }
 187 finish:
 188         to_std_logic_vector(size, out_size, 8);
 189 }
 190
 191 void sim_jtag_write_msg(unsigned char *in_msg, unsigned char *in_size)
 192 {
 193         unsigned char data[MAX_PACKET];
 194         unsigned char size;
 195         int rc, i;
 196
 197         size = from_std_logic_vector(in_size, 8);
 198         data[0] = size;
 199         for (i = 0; i < size; i++) {
 200                 int byte = i >> 3;
 201                 int bit = 1 << (i & 7);
 202                 if (in_msg[i] == vhpi1)
 203                         data[byte+1] |= bit;
 204                 else
 205                         data[byte+1] &= ~bit;
 206         }
 207         rc = (size + 7) / 8;
 208
 209 #if 0
 210         fprintf(stderr, "Sending response:\n\r");
 211         {
 212                 for (i=0; i<rc; i++)
 213                         fprintf(stderr, "%02x ", data[i]);
 214                 fprintf(stderr, "\n\r");
 215         }
 216 #endif
 217
 218         rc = write(cfd, data, rc);
 219         if (rc < 0)
 220                 fprintf(stderr, "Debug write error, ignoring\r\n");
 221 }
 222