static int vsllink_jtag_execute(void)
{
int i;
int result;
if (tap_length <= 0)
return ERROR_OK;
versaloon_interface.adaptors.jtag_raw.execute(0, tdi_buffer, tms_buffer,
tdo_buffer, tap_length);
result = versaloon_interface.adaptors.peripheral_commit();
if (result == ERROR_OK) {
for (i = 0; i < pending_scan_results_length; i++) {
struct pending_scan_result *pending_scan_result =
&pending_scan_results_buffer[i];
uint8_t *buffer = pending_scan_result->buffer;
int length = pending_scan_result->length;
int src_first = pending_scan_result->src_offset;
int dest_first = pending_scan_result->dest_offset;
bool last = pending_scan_result->last;
struct scan_command *command;
command = pending_scan_result->command;
buf_set_buf(tdo_buffer, src_first, buffer, dest_first, length);
#ifdef _DEBUG_JTAG_IO_
DEBUG_JTAG_IO(
"JTAG scan read(%d bits, from src %d bits to dest %d bits):",
length, src_first, dest_first);
vsllink_debug_buffer(buffer + dest_first / 8,
DIV_ROUND_UP(length, 7));
#endif
if (last) {
if (jtag_read_buffer(buffer, command)
!= ERROR_OK) {
vsllink_tap_init();
return ERROR_JTAG_QUEUE_FAILED;
}
if (pending_scan_result->buffer != NULL)
free(pending_scan_result->buffer);
}
}
} else {
LOG_ERROR("vsllink_jtag_execute failure");
return ERROR_JTAG_QUEUE_FAILED;
}
vsllink_tap_init();
return ERROR_OK;
}
int jtag_build_buffer(const struct scan_command *cmd, uint8_t **buffer)
{
int bit_count = 0;
int i;
bit_count = jtag_scan_size(cmd);
*buffer = calloc(1,DIV_ROUND_UP(bit_count, 8));
bit_count = 0;
DEBUG_JTAG_IO("%s num_fields: %i",
cmd->ir_scan ? "IRSCAN" : "DRSCAN",
cmd->num_fields);
for (i = 0; i < cmd->num_fields; i++)
{
if (cmd->fields[i].out_value)
{
#ifdef _DEBUG_JTAG_IO_
char *char_buf = buf_to_str(cmd->fields[i].out_value,
(cmd->fields[i].num_bits > DEBUG_JTAG_IOZ)
? DEBUG_JTAG_IOZ
: cmd->fields[i].num_bits, 16);
LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i,
cmd->fields[i].num_bits, char_buf);
free(char_buf);
#endif
buf_set_buf(cmd->fields[i].out_value, 0, *buffer,
bit_count, cmd->fields[i].num_bits);
}
else
{
DEBUG_JTAG_IO("fields[%i].out_value[%i]: NULL",
i, cmd->fields[i].num_bits);
}
bit_count += cmd->fields[i].num_bits;
}
//DEBUG_JTAG_IO("bit_count totalling: %i", bit_count);
return bit_count;
}
int jtag_read_buffer(uint8_t *buffer, const struct scan_command *cmd)
{
int i;
int bit_count = 0;
int retval;
/* we return ERROR_OK, unless a check fails, or a handler reports a problem */
retval = ERROR_OK;
for (i = 0; i < cmd->num_fields; i++)
{
/* if neither in_value nor in_handler
* are specified we don't have to examine this field
*/
if (cmd->fields[i].in_value)
{
int num_bits = cmd->fields[i].num_bits;
uint8_t *captured = buf_set_buf(buffer, bit_count, malloc(DIV_ROUND_UP(num_bits, 8)), 0, num_bits);
#ifdef _DEBUG_JTAG_IO_
char *char_buf = buf_to_str(captured,
(num_bits > DEBUG_JTAG_IOZ)
? DEBUG_JTAG_IOZ
: num_bits, 16);
LOG_DEBUG("fields[%i].in_value[%i]: 0x%s",
i, num_bits, char_buf);
free(char_buf);
#endif
if (cmd->fields[i].in_value)
{
buf_cpy(captured, cmd->fields[i].in_value, num_bits);
}
free(captured);
}
bit_count += cmd->fields[i].num_bits;
}
return retval;
}
static int osbdm_flush(struct osbdm *osbdm, struct queue* queue)
{
uint8_t tms[DIV_ROUND_UP(OSBDM_SWAP_MAX, 8)];
uint8_t tdi[DIV_ROUND_UP(OSBDM_SWAP_MAX, 8)];
uint8_t tdo[DIV_ROUND_UP(OSBDM_SWAP_MAX, 8)];
int seq_back_len = 0;
while (queue->head) {
(void)memset(tms, 0, sizeof(tms));
(void)memset(tdi, 0, sizeof(tdi));
(void)memset(tdo, 0, sizeof(tdo));
int seq_len;
int swap_len;
struct sequence *seq;
/* Copy from queue to tms/tdi streams
*/
seq = queue->head;
seq_len = seq_back_len;
swap_len = 0;
while (seq && swap_len != OSBDM_SWAP_MAX) {
/* Count bit for copy at this iteration.
* len should fit into remaining space
* in tms/tdo bitstreams
*/
int len = seq->len - seq_len;
if (len > OSBDM_SWAP_MAX - swap_len)
len = OSBDM_SWAP_MAX - swap_len;
/* Set tms data */
buf_set_buf(seq->tms, seq_len, tms, swap_len, len);
/* Set tdi data if they exists */
if (seq->tdi)
buf_set_buf(seq->tdi, seq_len, tdi, swap_len, len);
swap_len += len;
seq_len += len;
if (seq_len == seq->len) {
seq = seq->next; /* Move to next sequence */
seq_len = 0;
}
}
if (osbdm_swap(osbdm, tms, tdi, tdo, swap_len))
return ERROR_FAIL;
/* Copy from tdo stream to queue
*/
for (int swap_back_len = 0; swap_back_len < swap_len; ) {
int len = queue->head->len - seq_back_len;
if (len > swap_len - swap_back_len)
len = swap_len - swap_back_len;
if (queue->head->tdo)
buf_set_buf(tdo, swap_back_len, queue->head->tdo, seq_back_len, len);
swap_back_len += len;
seq_back_len += len;
if (seq_back_len == queue->head->len) {
queue_drop_head(queue);
seq_back_len = 0;
}
}
}
return ERROR_OK;
}
