static int dtc_queue_run(void) { struct dtc_reply_queue_entry *rq_p, *rq_next; int retval; int usb_err; int bit_cnt; int x; uint8_t *dtc_p, *tdo_p; uint8_t dtc_mask, tdo_mask; uint8_t reply_buffer[USB_EP2IN_SIZE]; retval = ERROR_OK; if (dtc_queue.cmd_index < 1) return(retval); dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = DTC_CMD_STOP; /* run the cmd */ if (dtc_queue.rq_head == NULL) { usb_err = dtc_run_download(pHDev, dtc_queue.cmd_buffer, dtc_queue.cmd_index, NULL, 0 ); if (usb_err < 0) { LOG_ERROR("dtc_run_download: %s", usb_strerror()); exit(1); } } else { usb_err = dtc_run_download(pHDev, dtc_queue.cmd_buffer, dtc_queue.cmd_index, reply_buffer, dtc_queue.reply_index ); if (usb_err < 0) { LOG_ERROR("dtc_run_download: %s", usb_strerror()); exit(1); } else { /* process the reply, which empties the reply queue and frees its entries */ dtc_p = reply_buffer; /* The rigamarole with the masks and doing it bit-by-bit is due to the fact that the scan buffer is LSb-first and the DTC code is MSb-first for hardware reasons. It was that or craft a function to do the reversal, and that wouldn't work with bit-stuffing (supplying extra bits to use mostly byte operations), or any other scheme which would throw the byte alignment off. */ for ( rq_p = dtc_queue.rq_head; rq_p != NULL; rq_p = rq_next ) { tdo_p = rq_p->scan.buffer + (rq_p->scan.offset / 8); tdo_mask = 1 << (rq_p->scan.offset % 8); bit_cnt = rq_p->scan.length; if (bit_cnt >= 8) { /* bytes */ dtc_mask = 1 << (8 - 1); for ( ; bit_cnt; bit_cnt-- ) { if (*dtc_p & dtc_mask) { *tdo_p |= tdo_mask; } else { *tdo_p &=~ tdo_mask; } dtc_mask >>= 1; if (dtc_mask == 0) { dtc_p++; dtc_mask = 1 << (8 - 1); } tdo_mask <<= 1; if (tdo_mask == 0) { tdo_p++; tdo_mask = 1; } } } else { /* extra bits or last bit */ x = *dtc_p++; if (( rq_p->scan.type == SCAN_IN ) && ( rq_p->scan.offset != rq_p->scan.size - 1 )) { /* extra bits were sent as a full byte with padding on the end */ dtc_mask = 1 << (8 - 1); } else { dtc_mask = 1 << (bit_cnt - 1); } for ( ; bit_cnt; bit_cnt-- ) { if (x & dtc_mask) { *tdo_p |= tdo_mask; } else { *tdo_p &=~ tdo_mask; } dtc_mask >>= 1; tdo_mask <<= 1; if (tdo_mask == 0) { tdo_p++; tdo_mask = 1; } } } if ((rq_p->scan.offset + rq_p->scan.length) >= rq_p->scan.size) { /* feed scan buffer back into openocd and free it */ if (jtag_read_buffer(rq_p->scan.buffer, rq_p->cmd->cmd.scan) != ERROR_OK) { retval = ERROR_JTAG_QUEUE_FAILED; } free(rq_p->scan.buffer); } rq_next = rq_p->next; free(rq_p); } dtc_queue.rq_head = NULL; dtc_queue.rq_tail = NULL; }
static int dtc_queue_run(void) { struct dtc_reply_queue_entry *rq_p, *rq_next; int retval; int usb_err; int bit_cnt; int x; uint8_t *dtc_p, *tdo_p; uint8_t dtc_mask, tdo_mask; uint8_t reply_buffer[USB_EP2IN_SIZE]; assert((dtc_queue.rq_head != 0) == (dtc_queue.reply_index > 0)); assert(dtc_queue.cmd_index < USB_EP2BANK_SIZE); assert(dtc_queue.reply_index <= USB_EP2IN_SIZE); retval = ERROR_OK; if (dtc_queue.cmd_index < 1) return retval; dtc_queue.cmd_buffer[dtc_queue.cmd_index++] = DTC_CMD_STOP; usb_err = dtc_run_download(pHDev, dtc_queue.cmd_buffer, dtc_queue.cmd_index, reply_buffer, sizeof(reply_buffer) ); if (usb_err < 0) { LOG_ERROR("dtc_run_download: %s", usb_strerror()); exit(1); } if (dtc_queue.rq_head != NULL) { /* process the reply, which empties the reply queue and frees its entries */ dtc_p = reply_buffer; /* The rigamarole with the masks and doing it bit-by-bit is due to the fact that the *scan buffer is LSb-first and the DTC code is MSb-first for hardware reasons. It *was that or craft a function to do the reversal, and that wouldn't work with *bit-stuffing (supplying extra bits to use mostly byte operations), or any other *scheme which would throw the byte alignment off. */ for ( rq_p = dtc_queue.rq_head; rq_p != NULL; rq_p = rq_next ) { tdo_p = rq_p->scan.buffer + (rq_p->scan.offset / 8); tdo_mask = 1 << (rq_p->scan.offset % 8); bit_cnt = rq_p->scan.length; if (bit_cnt >= 8) { /* bytes */ dtc_mask = 1 << (8 - 1); for ( ; bit_cnt; bit_cnt-- ) { if (*dtc_p & dtc_mask) *tdo_p |= tdo_mask; else *tdo_p &= ~tdo_mask; dtc_mask >>= 1; if (dtc_mask == 0) { dtc_p++; dtc_mask = 1 << (8 - 1); } tdo_mask <<= 1; if (tdo_mask == 0) { tdo_p++; tdo_mask = 1; } } } else {