jtag/drivers/jlink: implement register command to fix SWD

Some J-Link fw versions require registration to be performed before
SWD operation is possible. It doesn't harm anyway, vendor's utilities
do it unconditionally.

Thanks go to Segger for providing the necessary information.

Change-Id: Iabd76c743eca86e2c817a97cb93c969fec3f7ac6
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/2331
Tested-by: jenkins

diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index 57eea64d192ba352aa49d7d8cefe1df88ae53b5c..40d91cc43264c442c6ccbcc96194f54e0575a4ca 100644 (file)
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -77,6 +77,7 @@ static uint8_t usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
 #define EMU_CMD_SET_SPEED              0x05
 #define EMU_CMD_GET_STATE              0x07
 #define EMU_CMD_SET_KS_POWER   0x08
+#define EMU_CMD_REGISTER               0x09
 #define EMU_CMD_GET_SPEEDS             0xc0
 #define EMU_CMD_GET_HW_INFO            0xc1
 #define EMU_CMD_GET_COUNTERS   0xc2
@@ -112,6 +113,10 @@ static uint8_t usb_out_buffer[JLINK_OUT_BUFFER_SIZE];
 #define EMU_CMD_WRITE_MEM_ARM79                0xf7
 #define EMU_CMD_READ_MEM_ARM79         0xf8
 
+/* Register subcommands */
+#define REG_CMD_REGISTER               100
+#define REG_CMD_UNREGISTER             101
+
 /* bits return from EMU_CMD_GET_CAPS */
 #define EMU_CAP_RESERVED_1             0
 #define EMU_CAP_GET_HW_VERSION         1
@@ -433,6 +438,41 @@ static int jlink_khz(int khz, int *jtag_speed)
        return ERROR_OK;
 }
 
+static int jlink_register(void)
+{
+       int result;
+       usb_out_buffer[0] = EMU_CMD_REGISTER;
+       usb_out_buffer[1] = REG_CMD_REGISTER;
+       /* 2 - 11 is "additional parameter",
+        * 12 - 13 is connection handle, zero initially */
+       memset(&usb_out_buffer[2], 0, 10 + 2);
+
+       result = jlink_usb_write(jlink_handle, 14);
+       if (result != 14) {
+               LOG_ERROR("J-Link register write failed (%d)", result);
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       /* Returns:
+        * 0 - 1 connection handle,
+        * 2 - 3 number of information entities,
+        * 4 - 5 size of a single information struct,
+        * 6 - 7 number of additional bytes,
+        * 8 - ... reply data
+        *
+        * Try to read the whole USB bulk packet
+        */
+       result = jtag_libusb_bulk_read(jlink_handle->usb_handle, jlink_read_ep,
+                                      (char *)usb_in_buffer, sizeof(usb_in_buffer),
+                                      JLINK_USB_TIMEOUT);
+       if (!result) {
+               LOG_ERROR("J-Link register read failed (0 bytes received)");
+               return ERROR_JTAG_DEVICE_ERROR;
+       }
+
+       return ERROR_OK;
+}
+
 /*
  * select transport interface
  *
@@ -513,6 +553,10 @@ static int jlink_init(void)
                jlink_get_status();
        }
 
+       /* Registration is sometimes necessary for SWD to work */
+       if (jlink_caps & (1<<EMU_CAP_REGISTER))
+               jlink_register();
+
        /*
         * Some versions of Segger's software do not select JTAG interface by default.
         *