- /* Pad last byte so that tap_length is divisible by 8 */
- while (tap_length % 8 != 0)
- {
- /* More of the last TMS value keeps us in the same state,
- * analogous to free-running JTAG interfaces. */
- jlink_tap_append_step(last_tms, 0);
- }
-
- byte_length = tap_length / 8;
-
- usb_out_buffer[0] = JLINK_TAP_SEQUENCE_COMMAND;
- usb_out_buffer[1] = (tap_length >> 0) & 0xff;
- usb_out_buffer[2] = (tap_length >> 8) & 0xff;
-
- tms_offset = 3;
- for (i = 0; i < byte_length; i++)
- {
- usb_out_buffer[tms_offset + i] = tms_buffer[i];
- }
-
- tdi_offset = tms_offset + byte_length;
- for (i = 0; i < byte_length; i++)
- {
- usb_out_buffer[tdi_offset + i] = tdi_buffer[i];
- }
-
- result = jlink_usb_message(jlink_jtag_handle, 3 + 2 * byte_length, byte_length);
-
- if (result == byte_length)
- {
- for (i = 0; i < byte_length; i++)
- {
- tdo_buffer[i] = usb_in_buffer[i];
- }
-
- for (i = 0; i < pending_scan_results_length; i++)
- {
- pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[i];
- u8 *buffer = pending_scan_result->buffer;
- int length = pending_scan_result->length;
- int first = pending_scan_result->first;
- scan_command_t *command = pending_scan_result->command;
-
- /* Copy to buffer */
- buf_set_buf(tdo_buffer, first, buffer, 0, length);
-
- DEBUG_JTAG_IO("pending scan result, length = %d", length);
-
+ jlink_tap_append_step((tap_get_state() == TAP_RESET)?1:0, 0);
+ }
+
+ // number of full bytes (plus one if some would be left over)
+ byte_length = TAP_SCAN_BYTES(tap_length);
+
+ usb_out_buffer[0] = EMU_CMD_HW_JTAG3;
+ usb_out_buffer[1] = 0;
+ usb_out_buffer[2] = (tap_length >> 0) & 0xff;
+ usb_out_buffer[3] = (tap_length >> 8) & 0xff;
+ memcpy(usb_out_buffer + 4, tms_buffer, byte_length);
+ memcpy(usb_out_buffer + 4 + byte_length, tdi_buffer, byte_length);
+
+ jlink_last_state = jtag_debug_state_machine(tms_buffer, tdi_buffer,
+ tap_length, jlink_last_state);
+
+ result = jlink_usb_message(jlink_jtag_handle, 4 + 2 * byte_length, byte_length);
+ if (result != byte_length)
+ {
+ LOG_ERROR("jlink_tap_execute, wrong result %d (expected %d)", result, byte_length);
+ jlink_tap_init();
+ return ERROR_JTAG_QUEUE_FAILED;
+ }
+
+ memcpy(tdo_buffer, usb_in_buffer, byte_length);
+
+ for (i = 0; i < pending_scan_results_length; i++)
+ {
+ pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[i];
+ u8 *buffer = pending_scan_result->buffer;
+ int length = pending_scan_result->length;
+ int first = pending_scan_result->first;
+ scan_command_t *command = pending_scan_result->command;
+
+ /* Copy to buffer */
+ buf_set_buf(tdo_buffer, first, buffer, 0, length);
+
+ DEBUG_JTAG_IO("pending scan result, length = %d", length);
+