- int tms_len;
- uint16_t tms_append_byte;
-
- if (vsllink_tms_data_len > 0)
- {
- // there are vsllink_tms_data_len more tms bits to be shifted
- // so there are vsllink_tms_data_len + num_cycles tms bits in all
- tms_len = vsllink_tms_data_len + num_cycles;
- if (tms > 0)
- {
- // append '1' for tms
- tms_append_byte = (uint16_t)((((1 << num_cycles) - 1) << vsllink_tms_data_len) & 0xFFFF);
- }
- else
- {
- // append '0' for tms
- tms_append_byte = 0;
- }
- if (tms_len <= 16)
- {
- // merge into last tms shift
- if (tms_len < 8)
- {
- // just add to vsllink_tms_data_len
- // same result if tun through
- //vsllink_tms_data_len += num_cycles;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] |= (uint8_t)(tms_append_byte & 0xFF);
- }
- else if (tms_len == 8)
- {
- // end last tms shift command
- // just reduce it, and append last tms byte
- (*vsllink_tms_cmd_pos)--;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
- }
- else if (tms_len < 16)
- {
- if ((*vsllink_tms_cmd_pos & VSLLINK_CMDJTAGSEQ_LENMSK) < VSLLINK_CMDJTAGSEQ_LENMSK)
- {
- // every tms shift command can contain VSLLINK_CMDJTAGSEQ_LENMSK + 1 bytes in most
- // there is enought tms length in the current tms shift command
- // increase the tms byte length by 1 and set the last byte to 0
- (*vsllink_tms_cmd_pos)++;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = (uint8_t)(tms_append_byte >> 8);
- }
- else
- {
- // every tms shift command can contain VSLLINK_CMDJTAGSEQ_LENMSK + 1 bytes in most
- // not enough tms length in the current tms shift command
- // so a new command should be added
- // first decrease byte length of last tms shift command
- (*vsllink_tms_cmd_pos)--;
- // append last tms byte and move the command pointer to the next empty position
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
- // add new command(3 bytes)
- vsllink_tap_ensure_space(0, 3);
- vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = (uint8_t)(tms_append_byte >> 8);
- }
- }
- else if (tms_len == 16)
- {
- // end last tms shift command
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (uint8_t)(tms_append_byte >> 8);
- }
-
- vsllink_tms_data_len = tms_len & 7;
- if (vsllink_tms_data_len == 0)
- {
- vsllink_tms_cmd_pos = NULL;
- }
- num_cycles = 0;
- }
- else
- {
- // more shifts will be needed
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] |= (uint8_t)(tms_append_byte & 0xFF);
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (uint8_t)(tms_append_byte >> 8);
-
- num_cycles -= 16 - vsllink_tms_data_len;
- vsllink_tms_data_len = 0;
- vsllink_tms_cmd_pos = NULL;
- }
- }
- // from here vsllink_tms_data_len == 0 or num_cycles == 0
-
- if (vsllink_tms_data_len > 0)
- {
- // num_cycles == 0
- // no need to shift
- if (num_cycles > 0)
- {
- LOG_ERROR("There MUST be some bugs in the driver");
- exit(-1);
- }
- }
- else
- {
- // get number of bytes left to be sent
- tms_len = num_cycles >> 3;
- if (tms_len > 0)
- {
- vsllink_tap_ensure_space(1, 5);
- // if tms_len > 0, vsllink_tms_data_len == 0
- // so just add new command
- // LSB of the command byte is the tms value when do the shifting
- if (tms > 0)
- {
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSCLOCK | 1;
- }
- else
- {
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSCLOCK;
- }
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 0) & 0xff;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 8) & 0xff;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 16) & 0xff;
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = (tms_len >> 24) & 0xff;
-
- vsllink_usb_in_want_length += 1;
- pending_scan_results_buffer[pending_scan_results_length].buffer = NULL;
- pending_scan_results_length++;
-
- if (tms_len > 0xFFFF)
- {
- vsllink_tap_execute();
- }
- }
-
- // post-process
- vsllink_tms_data_len = num_cycles & 7;
- if (vsllink_tms_data_len > 0)
- {
- vsllink_tap_ensure_space(0, 3);
- vsllink_tms_cmd_pos = &vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx];
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx++] = VSLLINK_CMDJTAGSEQ_TMSBYTE | 1;
- if (tms > 0)
- {
- // append '1' for tms
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = (1 << vsllink_tms_data_len) - 1;
- }
- else
- {
- // append '0' for tms
- vsllink_usb_out_buffer[vsllink_usb_out_buffer_idx] = 0x00;
- }
- }
- }