jtag_vpi: multiple improvements
[fw/openocd] / src / jtag / drivers / jtag_vpi.c
1 /*
2  * JTAG to VPI driver
3  *
4  * Copyright (C) 2013 Franck Jullien, <elec4fun@gmail.com>
5  *
6  * See file CREDITS for list of people who contributed to this
7  * project.
8  *
9  * This program is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU General Public License as
11  * published by the Free Software Foundation; either version 2 of
12  * the License, or (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
21  */
22
23 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include <jtag/interface.h>
28 #ifdef HAVE_ARPA_INET_H
29 #include <arpa/inet.h>
30 #endif
31
32 #ifndef _WIN32
33 #include <netinet/tcp.h>
34 #endif
35
36 #include <string.h>
37
38 #define NO_TAP_SHIFT    0
39 #define TAP_SHIFT       1
40
41 #define SERVER_ADDRESS  "127.0.0.1"
42 #define SERVER_PORT     5555
43
44 #define XFERT_MAX_SIZE          512
45
46 #define CMD_RESET               0
47 #define CMD_TMS_SEQ             1
48 #define CMD_SCAN_CHAIN          2
49 #define CMD_SCAN_CHAIN_FLIP_TMS 3
50 #define CMD_STOP_SIMU           4
51
52 int server_port = SERVER_PORT;
53 char *server_address;
54
55 int sockfd;
56 struct sockaddr_in serv_addr;
57
58 /* One jtag_vpi "packet" as sent over a TCP channel. */
59 struct vpi_cmd {
60         union {
61                 uint32_t cmd;
62                 unsigned char cmd_buf[4];
63         };
64         unsigned char buffer_out[XFERT_MAX_SIZE];
65         unsigned char buffer_in[XFERT_MAX_SIZE];
66         union {
67                 uint32_t length;
68                 unsigned char length_buf[4];
69         };
70         union {
71                 uint32_t nb_bits;
72                 unsigned char nb_bits_buf[4];
73         };
74 };
75
76 static char *jtag_vpi_cmd_to_str(int cmd_num)
77 {
78         switch (cmd_num) {
79         case CMD_RESET:
80                 return "CMD_RESET";
81         case CMD_TMS_SEQ:
82                 return "CMD_TMS_SEQ";
83         case CMD_SCAN_CHAIN:
84                 return "CMD_SCAN_CHAIN";
85         case CMD_SCAN_CHAIN_FLIP_TMS:
86                 return "CMD_SCAN_CHAIN_FLIP_TMS";
87         case CMD_STOP_SIMU:
88                 return "CMD_STOP_SIMU";
89         default:
90                 return "<unknown>";
91         }
92 }
93
94 static int jtag_vpi_send_cmd(struct vpi_cmd *vpi)
95 {
96         int retval;
97
98         /* Optional low-level JTAG debug */
99         if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) {
100                 if (vpi->nb_bits > 0) {
101                         /* command with a non-empty data payload */
102                         char *char_buf = buf_to_str(vpi->buffer_out,
103                                         (vpi->nb_bits > DEBUG_JTAG_IOZ)
104                                                 ? DEBUG_JTAG_IOZ
105                                                 : vpi->nb_bits,
106                                         16);
107                         LOG_DEBUG_IO("sending JTAG VPI cmd: cmd=%s, "
108                                         "length=%" PRIu32 ", "
109                                         "nb_bits=%" PRIu32 ", "
110                                         "buf_out=0x%s%s",
111                                         jtag_vpi_cmd_to_str(vpi->cmd),
112                                         vpi->length,
113                                         vpi->nb_bits,
114                                         char_buf,
115                                         (vpi->nb_bits > DEBUG_JTAG_IOZ) ? "(...)" : "");
116                         free(char_buf);
117                 } else {
118                         /* command without data payload */
119                         LOG_DEBUG_IO("sending JTAG VPI cmd: cmd=%s, "
120                                         "length=%" PRIu32 ", "
121                                         "nb_bits=%" PRIu32,
122                                         jtag_vpi_cmd_to_str(vpi->cmd),
123                                         vpi->length,
124                                         vpi->nb_bits);
125                 }
126         }
127
128         /* Use little endian when transmitting/receiving jtag_vpi cmds.
129            The choice of little endian goes against usual networking conventions
130            but is intentional to remain compatible with most older OpenOCD builds
131            (i.e. builds on little-endian platforms). */
132         h_u32_to_le(vpi->cmd_buf, vpi->cmd);
133         h_u32_to_le(vpi->length_buf, vpi->length);
134         h_u32_to_le(vpi->nb_bits_buf, vpi->nb_bits);
135
136 retry_write:
137         retval = write_socket(sockfd, vpi, sizeof(struct vpi_cmd));
138
139         if (retval < 0) {
140                 /* Account for the case when socket write is interrupted. */
141 #ifdef _WIN32
142                 int wsa_err = WSAGetLastError();
143                 if (wsa_err == WSAEINTR)
144                         goto retry_write;
145 #else
146                 if (errno == EINTR)
147                         goto retry_write;
148 #endif
149                 /* Otherwise this is an error using the socket, most likely fatal
150                    for the connection. B*/
151                 log_socket_error("jtag_vpi xmit");
152                 /* TODO: Clean way how adapter drivers can report fatal errors
153                    to upper layers of OpenOCD and let it perform an orderly shutdown? */
154                 exit(-1);
155         } else if (retval < (int)sizeof(struct vpi_cmd)) {
156                 /* This means we could not send all data, which is most likely fatal
157                    for the jtag_vpi connection (the underlying TCP connection likely not
158                    usable anymore) */
159                 LOG_ERROR("Could not send all data through jtag_vpi connection.");
160                 exit(-1);
161         }
162
163         /* Otherwise the packet has been sent successfully. */
164         return ERROR_OK;
165 }
166
167 static int jtag_vpi_receive_cmd(struct vpi_cmd *vpi)
168 {
169         unsigned bytes_buffered = 0;
170         while (bytes_buffered < sizeof(struct vpi_cmd)) {
171                 int bytes_to_receive = sizeof(struct vpi_cmd) - bytes_buffered;
172                 int retval = read_socket(sockfd, ((char *)vpi) + bytes_buffered, bytes_to_receive);
173                 if (retval < 0) {
174 #ifdef _WIN32
175                         int wsa_err = WSAGetLastError();
176                         if (wsa_err == WSAEINTR) {
177                                 /* socket read interrupted by WSACancelBlockingCall() */
178                                 continue;
179                         }
180 #else
181                         if (errno == EINTR) {
182                                 /* socket read interrupted by a signal */
183                                 continue;
184                         }
185 #endif
186                         /* Otherwise, this is an error when accessing the socket. */
187                         log_socket_error("jtag_vpi recv");
188                         exit(-1);
189                 } else if (retval == 0) {
190                         /* Connection closed by the other side */
191                         LOG_ERROR("Connection prematurely closed by jtag_vpi server.");
192                         exit(-1);
193                 }
194                 /* Otherwise, we have successfully received some data */
195                 bytes_buffered += retval;
196         }
197
198         /* Use little endian when transmitting/receiving jtag_vpi cmds. */
199         vpi->cmd = le_to_h_u32(vpi->cmd_buf);
200         vpi->length = le_to_h_u32(vpi->length_buf);
201         vpi->nb_bits = le_to_h_u32(vpi->nb_bits_buf);
202
203         return ERROR_OK;
204 }
205
206 /**
207  * jtag_vpi_reset - ask to reset the JTAG device
208  * @trst: 1 if TRST is to be asserted
209  * @srst: 1 if SRST is to be asserted
210  */
211 static int jtag_vpi_reset(int trst, int srst)
212 {
213         struct vpi_cmd vpi;
214         memset(&vpi, 0, sizeof(struct vpi_cmd));
215
216         vpi.cmd = CMD_RESET;
217         vpi.length = 0;
218         return jtag_vpi_send_cmd(&vpi);
219 }
220
221 /**
222  * jtag_vpi_tms_seq - ask a TMS sequence transition to JTAG
223  * @bits: TMS bits to be written (bit0, bit1 .. bitN)
224  * @nb_bits: number of TMS bits (between 1 and 8)
225  *
226  * Write a serie of TMS transitions, where each transition consists in :
227  *  - writing out TCK=0, TMS=<new_state>, TDI=<???>
228  *  - writing out TCK=1, TMS=<new_state>, TDI=<???> which triggers the transition
229  * The function ensures that at the end of the sequence, the clock (TCK) is put
230  * low.
231  */
232 static int jtag_vpi_tms_seq(const uint8_t *bits, int nb_bits)
233 {
234         struct vpi_cmd vpi;
235         int nb_bytes;
236
237         memset(&vpi, 0, sizeof(struct vpi_cmd));
238         nb_bytes = DIV_ROUND_UP(nb_bits, 8);
239
240         vpi.cmd = CMD_TMS_SEQ;
241         memcpy(vpi.buffer_out, bits, nb_bytes);
242         vpi.length = nb_bytes;
243         vpi.nb_bits = nb_bits;
244
245         return jtag_vpi_send_cmd(&vpi);
246 }
247
248 /**
249  * jtag_vpi_path_move - ask a TMS sequence transition to JTAG
250  * @cmd: path transition
251  *
252  * Write a serie of TMS transitions, where each transition consists in :
253  *  - writing out TCK=0, TMS=<new_state>, TDI=<???>
254  *  - writing out TCK=1, TMS=<new_state>, TDI=<???> which triggers the transition
255  * The function ensures that at the end of the sequence, the clock (TCK) is put
256  * low.
257  */
258
259 static int jtag_vpi_path_move(struct pathmove_command *cmd)
260 {
261         uint8_t trans[DIV_ROUND_UP(cmd->num_states, 8)];
262
263         memset(trans, 0, DIV_ROUND_UP(cmd->num_states, 8));
264
265         for (int i = 0; i < cmd->num_states; i++) {
266                 if (tap_state_transition(tap_get_state(), true) == cmd->path[i])
267                         buf_set_u32(trans, i, 1, 1);
268                 tap_set_state(cmd->path[i]);
269         }
270
271         return jtag_vpi_tms_seq(trans, cmd->num_states);
272 }
273
274 /**
275  * jtag_vpi_tms - ask a tms command
276  * @cmd: tms command
277  */
278 static int jtag_vpi_tms(struct tms_command *cmd)
279 {
280         return jtag_vpi_tms_seq(cmd->bits, cmd->num_bits);
281 }
282
283 static int jtag_vpi_state_move(tap_state_t state)
284 {
285         if (tap_get_state() == state)
286                 return ERROR_OK;
287
288         uint8_t tms_scan = tap_get_tms_path(tap_get_state(), state);
289         int tms_len = tap_get_tms_path_len(tap_get_state(), state);
290
291         int retval = jtag_vpi_tms_seq(&tms_scan, tms_len);
292         if (retval != ERROR_OK)
293                 return retval;
294
295         tap_set_state(state);
296
297         return ERROR_OK;
298 }
299
300 static int jtag_vpi_queue_tdi_xfer(uint8_t *bits, int nb_bits, int tap_shift)
301 {
302         struct vpi_cmd vpi;
303         int nb_bytes = DIV_ROUND_UP(nb_bits, 8);
304
305         memset(&vpi, 0, sizeof(struct vpi_cmd));
306
307         vpi.cmd = tap_shift ? CMD_SCAN_CHAIN_FLIP_TMS : CMD_SCAN_CHAIN;
308
309         if (bits)
310                 memcpy(vpi.buffer_out, bits, nb_bytes);
311         else
312                 memset(vpi.buffer_out, 0xff, nb_bytes);
313
314         vpi.length = nb_bytes;
315         vpi.nb_bits = nb_bits;
316
317         int retval = jtag_vpi_send_cmd(&vpi);
318         if (retval != ERROR_OK)
319                 return retval;
320
321         retval = jtag_vpi_receive_cmd(&vpi);
322         if (retval != ERROR_OK)
323                 return retval;
324
325         /* Optional low-level JTAG debug */
326         if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) {
327                 char *char_buf = buf_to_str(vpi.buffer_in,
328                                 (nb_bits > DEBUG_JTAG_IOZ) ? DEBUG_JTAG_IOZ : nb_bits,
329                                 16);
330                 LOG_DEBUG_IO("recvd JTAG VPI data: nb_bits=%d, buf_in=0x%s%s",
331                         nb_bits, char_buf, (nb_bits > DEBUG_JTAG_IOZ) ? "(...)" : "");
332                 free(char_buf);
333         }
334
335         if (bits)
336                 memcpy(bits, vpi.buffer_in, nb_bytes);
337
338         return ERROR_OK;
339 }
340
341 /**
342  * jtag_vpi_queue_tdi - short description
343  * @bits: bits to be queued on TDI (or NULL if 0 are to be queued)
344  * @nb_bits: number of bits
345  */
346 static int jtag_vpi_queue_tdi(uint8_t *bits, int nb_bits, int tap_shift)
347 {
348         int nb_xfer = DIV_ROUND_UP(nb_bits, XFERT_MAX_SIZE * 8);
349         int retval;
350
351         while (nb_xfer) {
352                 if (nb_xfer ==  1) {
353                         retval = jtag_vpi_queue_tdi_xfer(bits, nb_bits, tap_shift);
354                         if (retval != ERROR_OK)
355                                 return retval;
356                 } else {
357                         retval = jtag_vpi_queue_tdi_xfer(bits, XFERT_MAX_SIZE * 8, NO_TAP_SHIFT);
358                         if (retval != ERROR_OK)
359                                 return retval;
360                         nb_bits -= XFERT_MAX_SIZE * 8;
361                         if (bits)
362                                 bits += XFERT_MAX_SIZE;
363                 }
364
365                 nb_xfer--;
366         }
367
368         return ERROR_OK;
369 }
370
371 /**
372  * jtag_vpi_clock_tms - clock a TMS transition
373  * @tms: the TMS to be sent
374  *
375  * Triggers a TMS transition (ie. one JTAG TAP state move).
376  */
377 static int jtag_vpi_clock_tms(int tms)
378 {
379         const uint8_t tms_0 = 0;
380         const uint8_t tms_1 = 1;
381
382         return jtag_vpi_tms_seq(tms ? &tms_1 : &tms_0, 1);
383 }
384
385 /**
386  * jtag_vpi_scan - launches a DR-scan or IR-scan
387  * @cmd: the command to launch
388  *
389  * Launch a JTAG IR-scan or DR-scan
390  *
391  * Returns ERROR_OK if OK, ERROR_xxx if a read/write error occured.
392  */
393 static int jtag_vpi_scan(struct scan_command *cmd)
394 {
395         int scan_bits;
396         uint8_t *buf = NULL;
397         int retval = ERROR_OK;
398
399         scan_bits = jtag_build_buffer(cmd, &buf);
400
401         if (cmd->ir_scan) {
402                 retval = jtag_vpi_state_move(TAP_IRSHIFT);
403                 if (retval != ERROR_OK)
404                         return retval;
405         } else {
406                 retval = jtag_vpi_state_move(TAP_DRSHIFT);
407                 if (retval != ERROR_OK)
408                         return retval;
409         }
410
411         if (cmd->end_state == TAP_DRSHIFT) {
412                 retval = jtag_vpi_queue_tdi(buf, scan_bits, NO_TAP_SHIFT);
413                 if (retval != ERROR_OK)
414                         return retval;
415         } else {
416                 retval = jtag_vpi_queue_tdi(buf, scan_bits, TAP_SHIFT);
417                 if (retval != ERROR_OK)
418                         return retval;
419         }
420
421         if (cmd->end_state != TAP_DRSHIFT) {
422                 /*
423                  * As our JTAG is in an unstable state (IREXIT1 or DREXIT1), move it
424                  * forward to a stable IRPAUSE or DRPAUSE.
425                  */
426                 retval = jtag_vpi_clock_tms(0);
427                 if (retval != ERROR_OK)
428                         return retval;
429
430                 if (cmd->ir_scan)
431                         tap_set_state(TAP_IRPAUSE);
432                 else
433                         tap_set_state(TAP_DRPAUSE);
434         }
435
436         retval = jtag_read_buffer(buf, cmd);
437         if (retval != ERROR_OK)
438                 return retval;
439
440         if (buf)
441                 free(buf);
442
443         if (cmd->end_state != TAP_DRSHIFT) {
444                 retval = jtag_vpi_state_move(cmd->end_state);
445                 if (retval != ERROR_OK)
446                         return retval;
447         }
448
449         return ERROR_OK;
450 }
451
452 static int jtag_vpi_runtest(int cycles, tap_state_t state)
453 {
454         int retval;
455
456         retval = jtag_vpi_state_move(TAP_IDLE);
457         if (retval != ERROR_OK)
458                 return retval;
459
460         retval = jtag_vpi_queue_tdi(NULL, cycles, NO_TAP_SHIFT);
461         if (retval != ERROR_OK)
462                 return retval;
463
464         return jtag_vpi_state_move(state);
465 }
466
467 static int jtag_vpi_stableclocks(int cycles)
468 {
469         uint8_t tms_bits[4];
470         int cycles_remain = cycles;
471         int nb_bits;
472         int retval;
473         const int CYCLES_ONE_BATCH = sizeof(tms_bits) * 8;
474
475         assert(cycles >= 0);
476
477         /* use TMS=1 in TAP RESET state, TMS=0 in all other stable states */
478         memset(&tms_bits, (tap_get_state() == TAP_RESET) ? 0xff : 0x00, sizeof(tms_bits));
479
480         /* send the TMS bits */
481         while (cycles_remain > 0) {
482                 nb_bits = (cycles_remain < CYCLES_ONE_BATCH) ? cycles_remain : CYCLES_ONE_BATCH;
483                 retval = jtag_vpi_tms_seq(tms_bits, nb_bits);
484                 if (retval != ERROR_OK)
485                         return retval;
486                 cycles_remain -= nb_bits;
487         }
488
489         return ERROR_OK;
490 }
491
492 static int jtag_vpi_execute_queue(void)
493 {
494         struct jtag_command *cmd;
495         int retval = ERROR_OK;
496
497         for (cmd = jtag_command_queue; retval == ERROR_OK && cmd != NULL;
498              cmd = cmd->next) {
499                 switch (cmd->type) {
500                 case JTAG_RESET:
501                         retval = jtag_vpi_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
502                         break;
503                 case JTAG_RUNTEST:
504                         retval = jtag_vpi_runtest(cmd->cmd.runtest->num_cycles,
505                                                   cmd->cmd.runtest->end_state);
506                         break;
507                 case JTAG_STABLECLOCKS:
508                         retval = jtag_vpi_stableclocks(cmd->cmd.stableclocks->num_cycles);
509                         break;
510                 case JTAG_TLR_RESET:
511                         retval = jtag_vpi_state_move(cmd->cmd.statemove->end_state);
512                         break;
513                 case JTAG_PATHMOVE:
514                         retval = jtag_vpi_path_move(cmd->cmd.pathmove);
515                         break;
516                 case JTAG_TMS:
517                         retval = jtag_vpi_tms(cmd->cmd.tms);
518                         break;
519                 case JTAG_SLEEP:
520                         jtag_sleep(cmd->cmd.sleep->us);
521                         break;
522                 case JTAG_SCAN:
523                         retval = jtag_vpi_scan(cmd->cmd.scan);
524                         break;
525                 default:
526                         LOG_ERROR("BUG: unknown JTAG command type 0x%X",
527                                   cmd->type);
528                         retval = ERROR_FAIL;
529                         break;
530                 }
531         }
532
533         return retval;
534 }
535
536 static int jtag_vpi_init(void)
537 {
538         int flag = 1;
539
540         sockfd = socket(AF_INET, SOCK_STREAM, 0);
541         if (sockfd < 0) {
542                 LOG_ERROR("Could not create socket");
543                 return ERROR_FAIL;
544         }
545
546         memset(&serv_addr, 0, sizeof(serv_addr));
547
548         serv_addr.sin_family = AF_INET;
549         serv_addr.sin_port = htons(server_port);
550
551         if (!server_address)
552                 server_address = strdup(SERVER_ADDRESS);
553
554         serv_addr.sin_addr.s_addr = inet_addr(server_address);
555
556         if (serv_addr.sin_addr.s_addr == INADDR_NONE) {
557                 LOG_ERROR("inet_addr error occured");
558                 return ERROR_FAIL;
559         }
560
561         if (connect(sockfd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
562                 close(sockfd);
563                 LOG_ERROR("Can't connect to %s : %u", server_address, server_port);
564                 return ERROR_COMMAND_CLOSE_CONNECTION;
565         }
566
567         if (serv_addr.sin_addr.s_addr == htonl(INADDR_LOOPBACK)) {
568                 /* This increases performance drematically for local
569                  * connections, which is the most likely arrangement
570                  * for a VPI connection. */
571                 setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int));
572         }
573
574         LOG_INFO("Connection to %s : %u succeed", server_address, server_port);
575
576         return ERROR_OK;
577 }
578
579 static int jtag_vpi_quit(void)
580 {
581         free(server_address);
582         return close_socket(sockfd);
583 }
584
585 COMMAND_HANDLER(jtag_vpi_set_port)
586 {
587         if (CMD_ARGC == 0)
588                 LOG_WARNING("You need to set a port number");
589         else
590                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], server_port);
591
592         LOG_INFO("Set server port to %u", server_port);
593
594         return ERROR_OK;
595 }
596
597 COMMAND_HANDLER(jtag_vpi_set_address)
598 {
599         free(server_address);
600
601         if (CMD_ARGC == 0) {
602                 LOG_WARNING("You need to set an address");
603                 server_address = strdup(SERVER_ADDRESS);
604         } else
605                 server_address = strdup(CMD_ARGV[0]);
606
607         LOG_INFO("Set server address to %s", server_address);
608
609         return ERROR_OK;
610 }
611
612 static const struct command_registration jtag_vpi_command_handlers[] = {
613         {
614                 .name = "jtag_vpi_set_port",
615                 .handler = &jtag_vpi_set_port,
616                 .mode = COMMAND_CONFIG,
617                 .help = "set the port of the VPI server",
618                 .usage = "tcp_port_num",
619         },
620         {
621                 .name = "jtag_vpi_set_address",
622                 .handler = &jtag_vpi_set_address,
623                 .mode = COMMAND_CONFIG,
624                 .help = "set the address of the VPI server",
625                 .usage = "ipv4_addr",
626         },
627         COMMAND_REGISTRATION_DONE
628 };
629
630 struct jtag_interface jtag_vpi_interface = {
631         .name = "jtag_vpi",
632         .supported = DEBUG_CAP_TMS_SEQ,
633         .commands = jtag_vpi_command_handlers,
634         .transports = jtag_only,
635
636         .init = jtag_vpi_init,
637         .quit = jtag_vpi_quit,
638         .execute_queue = jtag_vpi_execute_queue,
639 };