676231b79f9eaf11e0ba092b9f8e8cbec370f01b
[fw/openocd] / src / jtag / drivers / osbdm.c
1 /* SPDX-License-Identifier: GPL-2.0-or-later */
2
3 /***************************************************************************
4  *   Copyright (C) 2012 by Jan Dakinevich                                  *
5  *   jan.dakinevich@gmail.com                                              *
6  ***************************************************************************/
7 #ifdef HAVE_CONFIG_H
8 #       include "config.h"
9 #endif
10
11 #include <helper/log.h>
12 #include <helper/binarybuffer.h>
13 #include <helper/command.h>
14 #include <jtag/interface.h>
15 #include "libusb_helper.h"
16
17 struct sequence {
18         int len;
19         void *tms;
20         void *tdo;
21         const void *tdi;
22         struct sequence *next;
23 };
24
25 struct queue {
26         struct sequence *head;
27         struct sequence *tail;
28 };
29
30 static struct sequence *queue_add_tail(struct queue *queue, int len)
31 {
32         if (len <= 0) {
33                 LOG_ERROR("BUG: sequences with zero length are not allowed");
34                 return NULL;
35         }
36
37         struct sequence *next;
38         next = malloc(sizeof(*next));
39         if (next) {
40                 next->tms = calloc(1, DIV_ROUND_UP(len, 8));
41                 if (next->tms) {
42                         next->len = len;
43                         next->tdo = NULL;
44                         next->tdi = NULL;
45                         next->next = NULL;
46
47                         if (!queue->head) {
48                                 /* Queue is empty at the moment */
49                                 queue->head = next;
50                         } else {
51                                 /* Queue already contains at least one sequence */
52                                 queue->tail->next = next;
53                         }
54
55                         queue->tail = next;
56                 } else {
57                         free(next);
58                         next = NULL;
59                 }
60         }
61
62         if (!next)
63                 LOG_ERROR("Not enough memory");
64
65         return next;
66 }
67
68 static void queue_drop_head(struct queue *queue)
69 {
70         struct sequence *head = queue->head->next; /* New head */
71         free(queue->head->tms);
72         free(queue->head);
73         queue->head = head;
74 }
75
76 static void queue_free(struct queue *queue)
77 {
78         if (queue) {
79                 while (queue->head)
80                         queue_drop_head(queue);
81
82                 free(queue);
83         }
84 }
85
86 static struct queue *queue_alloc(void)
87 {
88         struct queue *queue = malloc(sizeof(*queue));
89         if (queue)
90                 queue->head = NULL;
91         else
92                 LOG_ERROR("Not enough memory");
93
94         return queue;
95 }
96
97 /* Size of usb communication buffer */
98 #define OSBDM_USB_BUFSIZE 64
99 /* Timeout for USB transfer, ms */
100 #define OSBDM_USB_TIMEOUT 1000
101 /* Write end point */
102 #define OSBDM_USB_EP_WRITE 0x01
103 /* Read end point */
104 #define OSBDM_USB_EP_READ 0x82
105
106 /* Initialize OSBDM device */
107 #define OSBDM_CMD_INIT 0x11
108 /* Execute special, not-BDM command. But only this
109  * command is used for JTAG operation */
110 #define OSBDM_CMD_SPECIAL 0x27
111 /* Execute JTAG swap (tms/tdi -> tdo) */
112 #define OSBDM_CMD_SPECIAL_SWAP 0x05
113 /* Reset control */
114 #define OSBDM_CMD_SPECIAL_SRST 0x01
115 /* Maximum bit-length in one swap */
116 #define OSBDM_SWAP_MAX (((OSBDM_USB_BUFSIZE - 6) / 5) * 16)
117
118 /* Lists of valid VID/PID pairs
119  */
120 static const uint16_t osbdm_vid[] = { 0x15a2, 0x15a2, 0x15a2, 0 };
121 static const uint16_t osbdm_pid[] = { 0x0042, 0x0058, 0x005e, 0 };
122
123 struct osbdm {
124         struct libusb_device_handle *devh; /* USB handle */
125         uint8_t buffer[OSBDM_USB_BUFSIZE]; /* Data to send and receive */
126         int count; /* Count data to send and to read */
127 };
128
129 /* osbdm instance
130  */
131 static struct osbdm osbdm_context;
132
133 static int osbdm_send_and_recv(struct osbdm *osbdm)
134 {
135         /* Send request */
136         int count, ret;
137
138         ret = jtag_libusb_bulk_write(osbdm->devh, OSBDM_USB_EP_WRITE,
139                                      (char *)osbdm->buffer, osbdm->count,
140                                      OSBDM_USB_TIMEOUT, &count);
141         if (ret || count != osbdm->count) {
142                 LOG_ERROR("OSBDM communication error: can't write");
143                 return ERROR_FAIL;
144         }
145
146         /* Save command code for next checking */
147         uint8_t cmd_saved = osbdm->buffer[0];
148
149         /* Reading answer */
150         ret = jtag_libusb_bulk_read(osbdm->devh, OSBDM_USB_EP_READ,
151                                     (char *)osbdm->buffer, OSBDM_USB_BUFSIZE,
152                                     OSBDM_USB_TIMEOUT, &osbdm->count);
153         /* Now perform basic checks for data sent by BDM device
154          */
155         if (ret) {
156                 LOG_ERROR("OSBDM communication error: can't read");
157                 return ERROR_FAIL;
158         }
159
160         if (osbdm->count < 2) {
161                 LOG_ERROR("OSBDM communication error: reply too small");
162                 return ERROR_FAIL;
163         }
164
165         if (osbdm->count != osbdm->buffer[1])  {
166                 LOG_ERROR("OSBDM communication error: reply size mismatch");
167                 return ERROR_FAIL;
168         }
169
170         if (cmd_saved != osbdm->buffer[0]) {
171                 LOG_ERROR("OSBDM communication error: reply command mismatch");
172                 return ERROR_FAIL;
173         }
174
175         return ERROR_OK;
176 }
177
178 static int osbdm_srst(struct osbdm *osbdm, int srst)
179 {
180         osbdm->count = 0;
181         (void)memset(osbdm->buffer, 0, OSBDM_USB_BUFSIZE);
182
183         /* Composing request
184          */
185         osbdm->buffer[osbdm->count++] = OSBDM_CMD_SPECIAL; /* Command */
186         osbdm->buffer[osbdm->count++] = OSBDM_CMD_SPECIAL_SRST; /* Subcommand */
187         /* Length in bytes - not used */
188         osbdm->buffer[osbdm->count++] = 0;
189         osbdm->buffer[osbdm->count++] = 0;
190         /* SRST state */
191         osbdm->buffer[osbdm->count++] = (srst ? 0 : 0x08);
192
193         /* Sending data
194          */
195         if (osbdm_send_and_recv(osbdm) != ERROR_OK)
196                 return ERROR_FAIL;
197
198         return ERROR_OK;
199 }
200
201 static int osbdm_swap(struct osbdm *osbdm, void *tms, void *tdi,
202         void *tdo, int length)
203 {
204         if (length > OSBDM_SWAP_MAX) {
205                 LOG_ERROR("BUG: bit sequence too long");
206                 return ERROR_FAIL;
207         }
208
209         if (length <= 0) {
210                 LOG_ERROR("BUG: bit sequence equal or less than 0");
211                 return ERROR_FAIL;
212         }
213
214         int swap_count = DIV_ROUND_UP(length, 16);
215
216         /* cleanup */
217         osbdm->count = 0;
218         (void)memset(osbdm->buffer, 0, OSBDM_USB_BUFSIZE);
219
220         /* Composing request
221          */
222
223         osbdm->buffer[osbdm->count++] = OSBDM_CMD_SPECIAL; /* Command */
224         osbdm->buffer[osbdm->count++] = OSBDM_CMD_SPECIAL_SWAP; /* Subcommand */
225         /* Length in bytes - not used */
226         osbdm->buffer[osbdm->count++] = 0;
227         osbdm->buffer[osbdm->count++] = 0;
228         /* Swap count */
229         osbdm->buffer[osbdm->count++] = 0;
230         osbdm->buffer[osbdm->count++] = (uint8_t)swap_count;
231
232         for (int bit_idx = 0; bit_idx < length; ) {
233                 /* Bit count in swap */
234                 int bit_count = length - bit_idx;
235                 if (bit_count > 16)
236                         bit_count = 16;
237
238                 osbdm->buffer[osbdm->count++] = (uint8_t)bit_count;
239
240                 /* Copying TMS and TDI data to output buffer */
241                 uint32_t tms_data = buf_get_u32(tms, bit_idx, bit_count);
242                 uint32_t tdi_data = buf_get_u32(tdi, bit_idx, bit_count);
243                 osbdm->buffer[osbdm->count++] = (uint8_t)(tdi_data >> 8);
244                 osbdm->buffer[osbdm->count++] = (uint8_t)tdi_data;
245                 osbdm->buffer[osbdm->count++] = (uint8_t)(tms_data >> 8);
246                 osbdm->buffer[osbdm->count++] = (uint8_t)tms_data;
247
248                 /* Next bit offset */
249                 bit_idx += bit_count;
250         }
251
252         assert(osbdm->count <= OSBDM_USB_BUFSIZE);
253
254         /* Sending data
255          */
256         if (osbdm_send_and_recv(osbdm) != ERROR_OK)
257                 return ERROR_FAIL;
258
259         /*      Extra check
260          */
261         if (((osbdm->buffer[2] << 8) | osbdm->buffer[3]) != 2 * swap_count) {
262                 LOG_ERROR("OSBDM communication error: invalid swap command reply");
263                 return ERROR_FAIL;
264         }
265
266         /* Copy TDO response
267          */
268         uint8_t *buffer = osbdm->buffer + 4;
269         for (int bit_idx = 0; bit_idx < length; ) {
270                 int bit_count = length - bit_idx;
271                 if (bit_count > 16)
272                         bit_count = 16;
273
274                 /* Prepare data */
275                 uint32_t tdo_data = 0;
276                 tdo_data |= (*buffer++) << 8;
277                 tdo_data |= (*buffer++);
278                 tdo_data >>= (16 - bit_count);
279
280                 /* Copy TDO to return */
281                 buf_set_u32(tdo, bit_idx, bit_count, tdo_data);
282
283                 bit_idx += bit_count;
284         }
285
286         return ERROR_OK;
287 }
288
289 static int osbdm_flush(struct osbdm *osbdm, struct queue *queue)
290 {
291         uint8_t tms[DIV_ROUND_UP(OSBDM_SWAP_MAX, 8)];
292         uint8_t tdi[DIV_ROUND_UP(OSBDM_SWAP_MAX, 8)];
293         uint8_t tdo[DIV_ROUND_UP(OSBDM_SWAP_MAX, 8)];
294
295         int seq_back_len = 0;
296
297         while (queue->head) {
298                 (void)memset(tms, 0, sizeof(tms));
299                 (void)memset(tdi, 0, sizeof(tdi));
300                 (void)memset(tdo, 0, sizeof(tdo));
301
302                 int seq_len;
303                 int swap_len;
304                 struct sequence *seq;
305
306                 /* Copy from queue to tms/tdi streams
307                  */
308                 seq = queue->head;
309                 seq_len = seq_back_len;
310                 swap_len = 0;
311
312                 while (seq && swap_len != OSBDM_SWAP_MAX) {
313                         /* Count bit for copy at this iteration.
314                          * len should fit into remaining space
315                          * in tms/tdo bitstreams
316                          */
317                         int len = seq->len - seq_len;
318                         if (len > OSBDM_SWAP_MAX - swap_len)
319                                 len = OSBDM_SWAP_MAX - swap_len;
320
321                         /* Set tms data */
322                         buf_set_buf(seq->tms, seq_len, tms, swap_len, len);
323
324                         /* Set tdi data if they exists */
325                         if (seq->tdi)
326                                 buf_set_buf(seq->tdi, seq_len, tdi, swap_len, len);
327
328                         swap_len += len;
329                         seq_len += len;
330                         if (seq_len == seq->len) {
331                                 seq = seq->next; /* Move to next sequence */
332                                 seq_len = 0;
333                         }
334                 }
335
336                 if (osbdm_swap(osbdm, tms, tdi, tdo, swap_len))
337                         return ERROR_FAIL;
338
339                 /* Copy from tdo stream to queue
340                  */
341
342                 for (int swap_back_len = 0; swap_back_len < swap_len; ) {
343                         int len = queue->head->len - seq_back_len;
344                         if (len > swap_len - swap_back_len)
345                                 len = swap_len - swap_back_len;
346
347                         if (queue->head->tdo)
348                                 buf_set_buf(tdo, swap_back_len, queue->head->tdo, seq_back_len, len);
349
350                         swap_back_len += len;
351                         seq_back_len += len;
352                         if (seq_back_len == queue->head->len) {
353                                 queue_drop_head(queue);
354                                 seq_back_len = 0;
355                         }
356                 }
357         }
358
359         return ERROR_OK;
360 }
361
362 /*      Basic operation for opening USB device */
363 static int osbdm_open(struct osbdm *osbdm)
364 {
365         (void)memset(osbdm, 0, sizeof(*osbdm));
366         if (jtag_libusb_open(osbdm_vid, osbdm_pid, &osbdm->devh, NULL) != ERROR_OK)
367                 return ERROR_FAIL;
368
369         if (libusb_claim_interface(osbdm->devh, 0) != ERROR_OK)
370                 return ERROR_FAIL;
371
372         return ERROR_OK;
373 }
374
375 static int osbdm_quit(void)
376 {
377         jtag_libusb_close(osbdm_context.devh);
378         return ERROR_OK;
379 }
380
381 static int osbdm_add_pathmove(
382         struct queue *queue,
383         tap_state_t *path,
384         int num_states)
385 {
386         assert(num_states <= 32);
387
388         struct sequence *next = queue_add_tail(queue, num_states);
389         if (!next) {
390                 LOG_ERROR("BUG: can't allocate bit sequence");
391                 return ERROR_FAIL;
392         }
393
394         uint32_t tms = 0;
395         for (int i = 0; i < num_states; i++) {
396                 if (tap_state_transition(tap_get_state(), 1) == path[i]) {
397                         tms |= (1 << i);
398                 } else if (tap_state_transition(tap_get_state(), 0) == path[i]) {
399                         tms &= ~(1 << i); /* This line not so needed */
400                 } else {
401                         LOG_ERROR("BUG: %s -> %s isn't a valid TAP state transition",
402                                 tap_state_name(tap_get_state()),
403                                 tap_state_name(path[i]));
404                         return ERROR_FAIL;
405                 }
406
407                 tap_set_state(path[i]);
408         }
409
410         buf_set_u32(next->tms, 0, num_states, tms);
411         tap_set_end_state(tap_get_state());
412
413         return ERROR_OK;
414 }
415
416 static int osbdm_add_statemove(
417         struct queue *queue,
418         tap_state_t new_state,
419         int skip_first)
420 {
421         int len = 0;
422         int tms = 0;
423
424         tap_set_end_state(new_state);
425         if (tap_get_end_state() == TAP_RESET) {
426                 /* Ignore current state */
427                 tms = 0xff;
428                 len = 5;
429         } else if (tap_get_state() != tap_get_end_state()) {
430                 tms = tap_get_tms_path(tap_get_state(), new_state);
431                 len = tap_get_tms_path_len(tap_get_state(), new_state);
432         }
433
434         if (len && skip_first) {
435                 len--;
436                 tms >>= 1;
437         }
438
439         if (len) {
440                 struct sequence *next = queue_add_tail(queue, len);
441                 if (!next) {
442                         LOG_ERROR("BUG: can't allocate bit sequence");
443                         return ERROR_FAIL;
444                 }
445                 buf_set_u32(next->tms, 0, len, tms);
446         }
447
448         tap_set_state(tap_get_end_state());
449         return ERROR_OK;
450 }
451
452 static int osbdm_add_stableclocks(
453         struct queue *queue,
454         int count)
455 {
456         if (!tap_is_state_stable(tap_get_state())) {
457                 LOG_ERROR("BUG: current state (%s) is not stable",
458                         tap_state_name(tap_get_state()));
459                 return ERROR_FAIL;
460         }
461
462         struct sequence *next = queue_add_tail(queue, count);
463         if (!next) {
464                 LOG_ERROR("BUG: can't allocate bit sequence");
465                 return ERROR_FAIL;
466         }
467
468         if (tap_get_state() == TAP_RESET)
469                 (void)memset(next->tms, 0xff, DIV_ROUND_UP(count, 8));
470
471         return ERROR_OK;
472 }
473
474 static int osbdm_add_tms(
475         struct queue *queue,
476         const uint8_t *tms,
477         int num_bits)
478 {
479         struct sequence *next = queue_add_tail(queue, num_bits);
480         if (!next) {
481                 LOG_ERROR("BUG: can't allocate bit sequence");
482                 return ERROR_FAIL;
483         }
484         buf_set_buf(tms, 0, next->tms, 0, num_bits);
485
486         return ERROR_OK;
487 }
488
489 static int osbdm_add_scan(
490         struct queue *queue,
491         struct scan_field *fields,
492         int num_fields,
493         tap_state_t end_state,
494         bool ir_scan)
495 {
496         /* Move to desired shift state */
497         if (ir_scan) {
498                 if (tap_get_state() != TAP_IRSHIFT) {
499                         if (osbdm_add_statemove(queue, TAP_IRSHIFT, 0) != ERROR_OK)
500                                 return ERROR_FAIL;
501                 }
502         } else {
503                 if (tap_get_state() != TAP_DRSHIFT) {
504                         if (osbdm_add_statemove(queue, TAP_DRSHIFT, 0) != ERROR_OK)
505                                 return ERROR_FAIL;
506                 }
507         }
508
509         /* Add scan */
510         tap_set_end_state(end_state);
511         for (int idx = 0; idx < num_fields; idx++) {
512                 struct sequence *next = queue_add_tail(queue, fields[idx].num_bits);
513                 if (!next) {
514                         LOG_ERROR("Can't allocate bit sequence");
515                         return ERROR_FAIL;
516                 }
517
518                 (void)memset(next->tms, 0, DIV_ROUND_UP(fields[idx].num_bits, 8));
519                 next->tdi = fields[idx].out_value;
520                 next->tdo = fields[idx].in_value;
521         }
522
523         /* Move to end state
524          */
525         if (tap_get_state() != tap_get_end_state()) {
526                 /* Exit from IRSHIFT/DRSHIFT */
527                 buf_set_u32(queue->tail->tms, queue->tail->len - 1, 1, 1);
528
529                 /* Move with skip_first flag */
530                 if (osbdm_add_statemove(queue, tap_get_end_state(), 1) != ERROR_OK)
531                         return ERROR_FAIL;
532         }
533
534         return ERROR_OK;
535 }
536
537 static int osbdm_add_runtest(
538         struct queue *queue,
539         int num_cycles,
540         tap_state_t end_state)
541 {
542         if (osbdm_add_statemove(queue, TAP_IDLE, 0) != ERROR_OK)
543                 return ERROR_FAIL;
544
545         if (osbdm_add_stableclocks(queue, num_cycles) != ERROR_OK)
546                 return ERROR_FAIL;
547
548         if (osbdm_add_statemove(queue, end_state, 0) != ERROR_OK)
549                 return ERROR_FAIL;
550
551         return ERROR_OK;
552 }
553
554 static int osbdm_execute_command(
555         struct osbdm *osbdm,
556         struct queue *queue,
557         struct jtag_command *cmd)
558 {
559         int retval = ERROR_OK;
560
561         switch (cmd->type) {
562         case JTAG_RESET:
563                 if (cmd->cmd.reset->trst) {
564                         LOG_ERROR("BUG: nTRST signal is not supported");
565                         retval = ERROR_FAIL;
566                 } else {
567                         retval = osbdm_flush(osbdm, queue);
568                         if (retval == ERROR_OK)
569                                 retval = osbdm_srst(osbdm, cmd->cmd.reset->srst);
570                 }
571                 break;
572
573         case JTAG_PATHMOVE:
574                 retval = osbdm_add_pathmove(
575                         queue,
576                         cmd->cmd.pathmove->path,
577                         cmd->cmd.pathmove->num_states);
578                 break;
579
580         case JTAG_TLR_RESET:
581                 retval = osbdm_add_statemove(
582                         queue,
583                         cmd->cmd.statemove->end_state,
584                         0);
585                 break;
586
587         case JTAG_STABLECLOCKS:
588                 retval = osbdm_add_stableclocks(
589                         queue,
590                         cmd->cmd.stableclocks->num_cycles);
591                 break;
592
593         case JTAG_TMS:
594                 retval = osbdm_add_tms(
595                         queue,
596                         cmd->cmd.tms->bits,
597                         cmd->cmd.tms->num_bits);
598                 break;
599
600         case JTAG_SCAN:
601                 retval = osbdm_add_scan(
602                         queue,
603                         cmd->cmd.scan->fields,
604                         cmd->cmd.scan->num_fields,
605                         cmd->cmd.scan->end_state,
606                         cmd->cmd.scan->ir_scan);
607                 break;
608
609         case JTAG_SLEEP:
610                 retval = osbdm_flush(osbdm, queue);
611                 if (retval == ERROR_OK)
612                         jtag_sleep(cmd->cmd.sleep->us);
613                 break;
614
615         case JTAG_RUNTEST:
616                 retval = osbdm_add_runtest(
617                         queue,
618                         cmd->cmd.runtest->num_cycles,
619                         cmd->cmd.runtest->end_state);
620                 break;
621
622         default:
623                 LOG_ERROR("BUG: unknown JTAG command type encountered");
624                 retval = ERROR_FAIL;
625                 break;
626         }
627
628         return retval;
629 }
630
631 static int osbdm_execute_queue(void)
632 {
633         int retval = ERROR_OK;
634
635         struct queue *queue = queue_alloc();
636         if (!queue) {
637                 LOG_ERROR("BUG: can't allocate bit queue");
638                 retval = ERROR_FAIL;
639         } else {
640                 struct jtag_command *cmd = jtag_command_queue;
641
642                 while (retval == ERROR_OK && cmd) {
643                         retval = osbdm_execute_command(&osbdm_context, queue, cmd);
644                         cmd = cmd->next;
645                 }
646
647                 if (retval == ERROR_OK)
648                         retval = osbdm_flush(&osbdm_context, queue);
649
650                 queue_free(queue);
651         }
652
653         if (retval != ERROR_OK) {
654                 LOG_ERROR("FATAL: can't execute jtag command");
655                 exit(-1);
656         }
657
658         return retval;
659 }
660
661 static int osbdm_init(void)
662 {
663         /* Open device */
664         if (osbdm_open(&osbdm_context) != ERROR_OK) {
665                 LOG_ERROR("Can't open OSBDM device");
666                 return ERROR_FAIL;
667         } else {
668                 /* Device successfully opened */
669                 LOG_DEBUG("OSBDM init");
670         }
671
672         /* Perform initialize command */
673         osbdm_context.count = 0;
674         osbdm_context.buffer[osbdm_context.count++] = OSBDM_CMD_INIT;
675         if (osbdm_send_and_recv(&osbdm_context) != ERROR_OK)
676                 return ERROR_FAIL;
677
678         return ERROR_OK;
679 }
680
681 static struct jtag_interface osbdm_interface = {
682         .execute_queue = osbdm_execute_queue,
683 };
684
685 struct adapter_driver osbdm_adapter_driver = {
686         .name = "osbdm",
687         .transports = jtag_only,
688
689         .init = osbdm_init,
690         .quit = osbdm_quit,
691
692         .jtag_ops = &osbdm_interface,
693 };