]> git.sur5r.net Git - openocd/blobdiff - src/jtag/jlink.c
Dick Hollenbeck <dick@softplc.com> and Jeff Williams tap_get_tms_path_len()
[openocd] / src / jtag / jlink.c
index 5c3dbbe7d132e9873ef041972d828ef3a61d1637..028a36cf3b0bba46a792e7922a107dd2bb79e859 100644 (file)
 
 #include "log.h"
 
-/* enable this to debug communication
- */
-#if 0
-#define _DEBUG_USB_COMMS_
-#endif
-
-#ifdef _DEBUG_JTAG_IO_
-#define DEBUG_JTAG_IO(expr ...)        LOG_DEBUG(expr)
-#else
-#define DEBUG_JTAG_IO(expr ...)
-#endif
-
 #define VID 0x1366
 #define PID 0x0101
 
@@ -54,8 +42,9 @@
 
 #define JLINK_USB_TIMEOUT              1000
 
-#define JLINK_IN_BUFFER_SIZE                   8192
-#define JLINK_OUT_BUFFER_SIZE                  8192
+// See Section 1.3.2 of the Segger JLink USB protocol manual
+#define JLINK_IN_BUFFER_SIZE                   2048
+#define JLINK_OUT_BUFFER_SIZE                  2*2048+4
 #define JLINK_EMU_RESULT_BUFFER_SIZE   64
 
 /* Global USB buffers */
@@ -64,14 +53,16 @@ static u8 usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
 static u8 usb_emu_result_buffer[JLINK_EMU_RESULT_BUFFER_SIZE];
 
 /* Constants for JLink command */
-#define EMU_CMD_VERSION     0x01
-#define EMU_CMD_SET_SPEED   0x05
-#define EMU_CMD_GET_STATE   0x07
-#define EMU_CMD_HW_JTAG3    0xcf
-#define EMU_CMD_HW_RESET0   0xdc
-#define EMU_CMD_HW_RESET1   0xdd
-#define EMU_CMD_HW_TRST0    0xde
-#define EMU_CMD_HW_TRST1    0xdf
+#define EMU_CMD_VERSION                0x01
+#define EMU_CMD_SET_SPEED              0x05
+#define EMU_CMD_GET_STATE              0x07
+#define EMU_CMD_HW_JTAG3               0xcf
+#define EMU_CMD_GET_MAX_MEM_BLOCK   0xd4
+#define EMU_CMD_HW_RESET0              0xdc
+#define EMU_CMD_HW_RESET1              0xdd
+#define EMU_CMD_HW_TRST0               0xde
+#define EMU_CMD_HW_TRST1               0xdf
+#define EMU_CMD_GET_CAPS               0xe8
 
 /* max speed 12MHz v5.0 jlink */
 #define JLINK_MAX_SPEED 12000
@@ -125,6 +116,8 @@ static int jlink_get_version_info(void);
 static void jlink_debug_buffer(u8 *buffer, int length);
 #endif
 
+static enum tap_state jlink_last_state = TAP_RESET;
+
 static jlink_jtag_t* jlink_jtag_handle;
 
 /***************************************************************************/
@@ -142,95 +135,113 @@ jtag_interface_t jlink_interface =
        .quit = jlink_quit
 };
 
-static int jlink_execute_queue(void)
+static void jlink_execute_end_state(jtag_command_t *cmd)
+{
+       DEBUG_JTAG_IO("end_state: %i", cmd->cmd.end_state->end_state);
+
+       if (cmd->cmd.end_state->end_state != TAP_INVALID)
+               jlink_end_state(cmd->cmd.end_state->end_state);
+}
+
+static void jlink_execute_runtest(jtag_command_t *cmd)
+{
+       DEBUG_JTAG_IO("runtest %i cycles, end in %i",
+                       cmd->cmd.runtest->num_cycles,
+                       cmd->cmd.runtest->end_state);
+
+       if (cmd->cmd.runtest->end_state != TAP_INVALID)
+               jlink_end_state(cmd->cmd.runtest->end_state);
+
+       jlink_runtest(cmd->cmd.runtest->num_cycles);
+}
+
+static void jlink_execute_statemove(jtag_command_t *cmd)
+{
+       DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
+
+       if (cmd->cmd.statemove->end_state != TAP_INVALID)
+       {
+               jlink_end_state(cmd->cmd.statemove->end_state);
+       }
+       jlink_state_move();
+}
+
+static void jlink_execute_pathmove(jtag_command_t *cmd)
+{
+       DEBUG_JTAG_IO("pathmove: %i states, end in %i",
+               cmd->cmd.pathmove->num_states,
+               cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+
+       jlink_path_move(cmd->cmd.pathmove->num_states,
+                       cmd->cmd.pathmove->path);
+}
+
+static void jlink_execute_scan(jtag_command_t *cmd)
 {
-       jtag_command_t *cmd = jtag_command_queue;
        int scan_size;
        enum scan_type type;
        u8 *buffer;
 
-       while (cmd != NULL)
-       {
-               switch (cmd->type)
-               {
-                       case JTAG_END_STATE:
-                               DEBUG_JTAG_IO("end_state: %i", cmd->cmd.end_state->end_state);
-
-                               if (cmd->cmd.end_state->end_state != TAP_INVALID)
-                               {
-                                       jlink_end_state(cmd->cmd.end_state->end_state);
-                               }
-                               break;
-
-                       case JTAG_RUNTEST:
-                               DEBUG_JTAG_IO( "runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, \
-                                       cmd->cmd.runtest->end_state);
-
-                               if (cmd->cmd.runtest->end_state != TAP_INVALID)
-                               {
-                                       jlink_end_state(cmd->cmd.runtest->end_state);
-                               }
-                               jlink_runtest(cmd->cmd.runtest->num_cycles);
-                               break;
-
-                       case JTAG_STATEMOVE:
-                               DEBUG_JTAG_IO("statemove end in %i", cmd->cmd.statemove->end_state);
-
-                               if (cmd->cmd.statemove->end_state != TAP_INVALID)
-                               {
-                                       jlink_end_state(cmd->cmd.statemove->end_state);
-                               }
-                               jlink_state_move();
-                               break;
-
-                       case JTAG_PATHMOVE:
-                               DEBUG_JTAG_IO("pathmove: %i states, end in %i", \
-                                       cmd->cmd.pathmove->num_states, \
-                                       cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
-
-                               jlink_path_move(cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path);
-                               break;
-
-                       case JTAG_SCAN:
-                               DEBUG_JTAG_IO("scan end in %i", cmd->cmd.scan->end_state);
-
-                               if (cmd->cmd.scan->end_state != TAP_INVALID)
-                               {
-                                       jlink_end_state(cmd->cmd.scan->end_state);
-                               }
-
-                               scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
-                               DEBUG_JTAG_IO("scan input, length = %d", scan_size);
+       DEBUG_JTAG_IO("scan end in %i", cmd->cmd.scan->end_state);
+
+       if (cmd->cmd.scan->end_state != TAP_INVALID)
+               jlink_end_state(cmd->cmd.scan->end_state);
+
+       scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+       DEBUG_JTAG_IO("scan input, length = %d", scan_size);
 
 #ifdef _DEBUG_USB_COMMS_
-                               jlink_debug_buffer(buffer, (scan_size + 7) / 8);
+       jlink_debug_buffer(buffer, (scan_size + 7) / 8);
 #endif
-                               type = jtag_scan_type(cmd->cmd.scan);
-                               jlink_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size, cmd->cmd.scan);
-                               break;
-
-                       case JTAG_RESET:
-                               DEBUG_JTAG_IO("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-
-                               jlink_tap_execute();
-
-                               if (cmd->cmd.reset->trst == 1)
-                               {
-                                       tap_set_state(TAP_RESET);
-                               }
-                               jlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-                               break;
-
-                       case JTAG_SLEEP:
-                               DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
-                               jlink_tap_execute();
-                               jtag_sleep(cmd->cmd.sleep->us);
-                               break;
-
-                       default:
-                               LOG_ERROR("BUG: unknown JTAG command type encountered");
-                               exit(-1);
-               }
+       type = jtag_scan_type(cmd->cmd.scan);
+       jlink_scan(cmd->cmd.scan->ir_scan,
+                       type, buffer, scan_size, cmd->cmd.scan);
+}
+
+static void jlink_execute_reset(jtag_command_t *cmd)
+{
+       DEBUG_JTAG_IO("reset trst: %i srst %i",
+                       cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+
+       jlink_tap_execute();
+
+       if (cmd->cmd.reset->trst == 1)
+               tap_set_state(TAP_RESET);
+
+       jlink_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+}
+
+static void jlink_execute_sleep(jtag_command_t *cmd)
+{
+       DEBUG_JTAG_IO("sleep %i", cmd->cmd.sleep->us);
+       jlink_tap_execute();
+       jtag_sleep(cmd->cmd.sleep->us);
+}
+
+static void jlink_execute_command(jtag_command_t *cmd)
+{
+       switch (cmd->type)
+       {
+       case JTAG_END_STATE: jlink_execute_end_state(cmd); break;
+       case JTAG_RUNTEST:   jlink_execute_runtest(cmd); break;
+       case JTAG_STATEMOVE: jlink_execute_statemove(cmd); break;
+       case JTAG_PATHMOVE:  jlink_execute_pathmove(cmd); break;
+       case JTAG_SCAN:      jlink_execute_scan(cmd); break;
+       case JTAG_RESET:     jlink_execute_reset(cmd); break;
+       case JTAG_SLEEP:     jlink_execute_sleep(cmd); break;
+       default:
+               LOG_ERROR("BUG: unknown JTAG command type encountered");
+               exit(-1);
+       }
+}
+
+static int jlink_execute_queue(void)
+{
+       jtag_command_t *cmd = jtag_command_queue;
+
+       while (cmd != NULL)
+       {
+               jlink_execute_command(cmd);
                cmd = cmd->next;
        }
 
@@ -242,31 +253,26 @@ static int jlink_speed(int speed)
 {
        int result;
 
-       if (speed <= JLINK_MAX_SPEED)
+       if (speed > JLINK_MAX_SPEED)
        {
-               /* check for RTCK setting */
-               if (speed == 0)
-                       speed = -1;
+               LOG_INFO("Ignoring speed request: %dkHz exceeds %dkHz maximum",
+                               speed, JLINK_MAX_SPEED);
+               return ERROR_OK;
+       }
 
-               usb_out_buffer[0] = EMU_CMD_SET_SPEED;
-               usb_out_buffer[1] = (speed >> 0) & 0xff;
-               usb_out_buffer[2] = (speed >> 8) & 0xff;
+       /* check for RTCK setting */
+       if (speed == 0)
+               speed = -1;
 
-               result = jlink_usb_write(jlink_jtag_handle, 3);
+       usb_out_buffer[0] = EMU_CMD_SET_SPEED;
+       usb_out_buffer[1] = (speed >> 0) & 0xff;
+       usb_out_buffer[2] = (speed >> 8) & 0xff;
 
-               if (result == 3)
-               {
-                       return ERROR_OK;
-               }
-               else
-               {
-                       LOG_ERROR("J-Link setting speed failed (%d)", result);
-                       return ERROR_JTAG_DEVICE_ERROR;
-               }
-       }
-       else
+       result = jlink_usb_write(jlink_jtag_handle, 3);
+       if (result != 3)
        {
-               LOG_INFO("Requested speed %dkHz exceeds maximum of %dkHz, ignored", speed, JLINK_MAX_SPEED);
+               LOG_ERROR("J-Link setting speed failed (%d)", result);
+               return ERROR_JTAG_DEVICE_ERROR;
        }
 
        return ERROR_OK;
@@ -327,6 +333,7 @@ static int jlink_init(void)
 
        jlink_reset(0, 0);
        jlink_tap_init();
+       jlink_speed(jtag_speed);
 
        return ERROR_OK;
 }
@@ -461,6 +468,8 @@ static void jlink_reset(int trst, int srst)
        if (srst == 0)
        {
                jlink_simple_command(EMU_CMD_HW_RESET1);
+               jlink_end_state(TAP_RESET);
+               jlink_state_move();
        }
        else if (srst == 1)
        {
@@ -470,6 +479,8 @@ static void jlink_reset(int trst, int srst)
        if (trst == 0)
        {
                jlink_simple_command(EMU_CMD_HW_TRST1);
+               jlink_end_state(TAP_RESET);
+               jlink_state_move();
        }
        else if (trst == 1)
        {
@@ -497,53 +508,82 @@ static int jlink_get_status(void)
        int result;
 
        jlink_simple_command(EMU_CMD_GET_STATE);
-       result = jlink_usb_read(jlink_jtag_handle, 8);
-
-       if (result == 8)
-       {
-               int vref = usb_in_buffer[0] + (usb_in_buffer[1] << 8);
-               LOG_INFO("Vref = %d.%d TCK = %d TDI = %d TDO = %d TMS = %d SRST = %d TRST = %d\n", \
-                       vref / 1000, vref % 1000, \
-                       usb_in_buffer[2], usb_in_buffer[3], usb_in_buffer[4], \
-                       usb_in_buffer[5], usb_in_buffer[6], usb_in_buffer[7]);
 
-               if (vref < 1500)
-               {
-                       LOG_ERROR("Vref too low. Check Target Power\n");
-               }
-       }
-       else
+       result = jlink_usb_read(jlink_jtag_handle, 8);
+       if (result != 8)
        {
                LOG_ERROR("J-Link command EMU_CMD_GET_STATE failed (%d)\n", result);
+               return ERROR_JTAG_DEVICE_ERROR;
        }
 
+       int vref = usb_in_buffer[0] + (usb_in_buffer[1] << 8);
+       LOG_INFO("Vref = %d.%d TCK = %d TDI = %d TDO = %d TMS = %d SRST = %d TRST = %d\n", \
+               vref / 1000, vref % 1000, \
+               usb_in_buffer[2], usb_in_buffer[3], usb_in_buffer[4], \
+               usb_in_buffer[5], usb_in_buffer[6], usb_in_buffer[7]);
+
+       if (vref < 1500)
+               LOG_ERROR("Vref too low. Check Target Power\n");
+
        return ERROR_OK;
 }
 
 static int jlink_get_version_info(void)
 {
        int result;
-       int len = 0;
+       int len;
+       u32 jlink_caps, jlink_max_size;
 
        /* query hardware version */
        jlink_simple_command(EMU_CMD_VERSION);
+
        result = jlink_usb_read(jlink_jtag_handle, 2);
+       if (2 != result)
+       {
+               LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)\n", result);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
 
-       if (result == 2)
+       len = buf_get_u32(usb_in_buffer, 0, 16);
+       result = jlink_usb_read(jlink_jtag_handle, len);
+       if (result != len)
        {
-               len = buf_get_u32(usb_in_buffer, 0, 16);
-               result = jlink_usb_read(jlink_jtag_handle, len);
+               LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)\n", result);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
 
-               if (result == len)
-               {
-                       usb_in_buffer[result] = 0;
-                       LOG_INFO("%s", (char *)usb_in_buffer);
-                       return ERROR_OK;
-               }
+       usb_in_buffer[result] = 0;
+       LOG_INFO("%s", (char *)usb_in_buffer);
+
+       /* query hardware capabilities */
+       jlink_simple_command(EMU_CMD_GET_CAPS);
+
+       result = jlink_usb_read(jlink_jtag_handle, 4);
+       if (4 != result)
+       {
+               LOG_ERROR("J-Link command EMU_CMD_GET_CAPS failed (%d)\n", result);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       jlink_caps = buf_get_u32(usb_in_buffer, 0, 32);
+       LOG_INFO("JLink caps 0x%x", jlink_caps);
+
+
+       /* query hardware maximum memory block */
+       jlink_simple_command(EMU_CMD_GET_MAX_MEM_BLOCK);
+
+       result = jlink_usb_read(jlink_jtag_handle, 4);
+       if (4 != result)
+       {
+               LOG_ERROR("J-Link command EMU_CMD_GET_MAX_MEM_BLOCK failed (%d)\n", result);
+               return ERROR_JTAG_DEVICE_ERROR;
        }
 
-       LOG_ERROR("J-Link command EMU_CMD_VERSION failed (%d)\n", result);
-       return ERROR_JTAG_DEVICE_ERROR;
+       jlink_max_size = buf_get_u32(usb_in_buffer, 0, 32);
+       LOG_INFO("JLink max mem block %i", jlink_max_size);
+
+
+       return ERROR_OK;
 }
 
 static int jlink_handle_jlink_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
@@ -563,7 +603,7 @@ static int jlink_handle_jlink_info_command(struct command_context_s *cmd_ctx, ch
 /* 2048 is the max value we can use here */
 #define JLINK_TAP_BUFFER_SIZE 2048
 
-static int tap_length;
+static unsigned tap_length;
 static u8 tms_buffer[JLINK_TAP_BUFFER_SIZE];
 static u8 tdi_buffer[JLINK_TAP_BUFFER_SIZE];
 static u8 tdo_buffer[JLINK_TAP_BUFFER_SIZE];
@@ -605,40 +645,36 @@ static void jlink_tap_append_step(int tms, int tdi)
        last_tms = tms;
        int index = tap_length / 8;
 
-       if (index < JLINK_TAP_BUFFER_SIZE)
+       if (index >= JLINK_TAP_BUFFER_SIZE)
        {
-               int bit_index = tap_length % 8;
-               u8 bit = 1 << bit_index;
+               LOG_ERROR("jlink_tap_append_step: overflow");
+               exit(-1);
+       }
 
-               if (tms)
-               {
-                       tms_buffer[index] |= bit;
-               }
-               else
-               {
-                       tms_buffer[index] &= ~bit;
-               }
+       int bit_index = tap_length % 8;
+       u8 bit = 1 << bit_index;
 
-               if (tdi)
-               {
-                       tdi_buffer[index] |= bit;
-               }
-               else
-               {
-                       tdi_buffer[index] &= ~bit;
-               }
+       // we do not pad TMS, so be sure to initialize all bits
+       if (0 == bit_index)
+               tms_buffer[index] = tdi_buffer[index] = 0;
 
-               tap_length++;
-       }
+       if (tms)
+               tms_buffer[index] |= bit;
        else
-       {
-               LOG_ERROR("jlink_tap_append_step, overflow");
-       }
+               tms_buffer[index] &= ~bit;
+
+       if (tdi)
+               tdi_buffer[index] |= bit;
+       else
+               tdi_buffer[index] &= ~bit;
+
+       tap_length++;
 }
 
 static void jlink_tap_append_scan(int length, u8 *buffer, scan_command_t *command)
 {
-       pending_scan_result_t *pending_scan_result = &pending_scan_results_buffer[pending_scan_results_length];
+       pending_scan_result_t *pending_scan_result =
+               &pending_scan_results_buffer[pending_scan_results_length];
        int i;
 
        pending_scan_result->first = tap_length;
@@ -648,7 +684,9 @@ static void jlink_tap_append_scan(int length, u8 *buffer, scan_command_t *comman
 
        for (i = 0; i < length; i++)
        {
-               jlink_tap_append_step((i < length-1 ? 0 : 1), (buffer[i/8] >> (i%8)) & 1);
+               int tms = i < length - 1 ? 0 : 1;
+               int tdi = buffer[i / 8] & (1 << (i % 8));
+               jlink_tap_append_step(tms, tdi);
        }
        pending_scan_results_length++;
 }
@@ -658,87 +696,65 @@ static void jlink_tap_append_scan(int length, u8 *buffer, scan_command_t *comman
 static int jlink_tap_execute(void)
 {
        int byte_length;
-       int tms_offset;
-       int tdi_offset;
        int i;
        int result;
 
-       if (tap_length > 0)
-       {
-               /* 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);
-               }
+       if (!tap_length)
+               return ERROR_OK;
 
-               byte_length = tap_length / 8;
+       // 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;
+       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);
 
-               tms_offset = 4;
-               for (i = 0; i < byte_length; i++)
-               {
-                       usb_out_buffer[tms_offset + i] = tms_buffer[i];
-               }
+       jlink_last_state = jtag_debug_state_machine(tms_buffer, tdi_buffer,
+                       tap_length, jlink_last_state);
 
-               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, 4 + 2 * byte_length, byte_length);
+       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;
+       }
 
-               if (result == byte_length)
-               {
-                       for (i = 0; i < byte_length; i++)
-                       {
-                               tdo_buffer[i] = usb_in_buffer[i];
-                       }
+       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;
+       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);
+               /* Copy to buffer */
+               buf_set_buf(tdo_buffer, first, buffer, 0, length);
 
-                               DEBUG_JTAG_IO("pending scan result, length = %d", length);
+               DEBUG_JTAG_IO("pending scan result, length = %d", length);
 
 #ifdef _DEBUG_USB_COMMS_
-                               jlink_debug_buffer(buffer, byte_length);
+               jlink_debug_buffer(buffer, byte_length);
 #endif
 
-                               if (jtag_read_buffer(buffer, command) != ERROR_OK)
-                               {
-                                       jlink_tap_init();
-                                       return ERROR_JTAG_QUEUE_FAILED;
-                               }
-
-                               if (pending_scan_result->buffer != NULL)
-                               {
-                                       free(pending_scan_result->buffer);
-                               }
-                       }
-               }
-               else
+               if (jtag_read_buffer(buffer, command) != ERROR_OK)
                {
-                       LOG_ERROR("jlink_tap_execute, wrong result %d, expected %d", result, byte_length);
+                       jlink_tap_init();
                        return ERROR_JTAG_QUEUE_FAILED;
                }
 
-               jlink_tap_init();
+               if (pending_scan_result->buffer != NULL)
+               {
+                       free(pending_scan_result->buffer);
+               }
        }
 
+       jlink_tap_init();
        return ERROR_OK;
 }
 
@@ -804,59 +820,48 @@ static int jlink_usb_message(jlink_jtag_t *jlink_jtag, int out_length, int in_le
        int result2;
 
        result = jlink_usb_write(jlink_jtag, out_length);
-       if (result == out_length)
+       if (result != out_length)
        {
-               result = jlink_usb_read(jlink_jtag, in_length);
-               if (result == in_length || result == in_length+1)
-               {
-                       if (result == in_length)
-                       {
-                               /* Must read the result from the EMU too */
-                               result2 = jlink_usb_read_emu_result(jlink_jtag);
-                               if (1 == result2)
-                               {
-                                       /* Check the result itself */
-                                       if (0 == usb_emu_result_buffer[0])
-                                       {
-                                               return result;
-                                       }
-                                       else
-                                       {
-                                               LOG_ERROR("jlink_usb_read_emu_result (requested=0, result=%d)", usb_emu_result_buffer[0]);
-                                               return -1;
-                                       }
-                               }
-                               else
-                               {
-                                       LOG_ERROR("jlink_usb_read_emu_result len (requested=1, result=%d)", result2);
-                                       return -1;
-                               }
-                       }
-                       else
-                       {
-                               /* Check the result itself */
-                               if (0 == usb_in_buffer[result-1])
-                               {
-                                       return result-1;
-                               }
-                               else
-                               {
-                                       LOG_ERROR("jlink_usb_read_emu_result (requested=0, result=%d)", usb_in_buffer[result]);
-                                       return -1;
-                               }
-                       }
-               }
-               else
+               LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)",
+                               out_length, result);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       result = jlink_usb_read(jlink_jtag, in_length);
+       if ((result != in_length) && (result != (in_length + 1)))
+       {
+               LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)",
+                               in_length, result);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       if (result == in_length)
+       {
+               /* Must read the result from the EMU too */
+               result2 = jlink_usb_read_emu_result(jlink_jtag);
+               if (1 != result2)
                {
-                       LOG_ERROR("usb_bulk_read failed (requested=%d, result=%d)", in_length, result);
-                       return -1;
+                       LOG_ERROR("jlink_usb_read_emu_result failed "
+                               "(requested=1, result=%d)", result2);
+                       return ERROR_JTAG_DEVICE_ERROR;
                }
+
+               /* Check the result itself */
+               result2 = usb_emu_result_buffer[0];
        }
        else
        {
-               LOG_ERROR("usb_bulk_write failed (requested=%d, result=%d)", out_length, result);
-               return -1;
+               /* Save the result, then remove it from return value */
+               result2 = usb_in_buffer[result--];
        }
+
+       if (result2)
+       {
+               LOG_ERROR("jlink_usb_message failed with result=%d)", result2);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       return result;
 }
 
 /* calls the given usb_bulk_* function, allowing for the data to trickle in with some timeouts  */
@@ -963,7 +968,7 @@ static void jlink_debug_buffer(u8 *buffer, int length)
                        snprintf(s, 4, " %02x", buffer[j]);
                        strcat(line, s);
                }
-               LOG_DEBUG(line);
+               LOG_DEBUG("%s", line);
        }
 }
 #endif