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 {
