drivers/stlink_usb: fix stlink_usb_read_regs() for API v2

[openocd.git] / src / jtag / drivers / stlink_usb.c

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 1a3cc6154b5bbac47c0cca10acc11f7df0caa22e..9ba4f4af987212360281eac2f0b8b279d94296be 100644 (file)
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -156,6 +156,7 @@ struct stlink_usb_handle_s {
 #define STLINK_SWD_AP_FAULT            0x11
 #define STLINK_SWD_AP_ERROR            0x12
 #define STLINK_SWD_AP_PARITY_ERROR     0x13
+#define STLINK_JTAG_GET_IDCODE_ERROR   0x09
 #define STLINK_JTAG_WRITE_ERROR        0x0c
 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 #define STLINK_SWD_DP_WAIT             0x14
@@ -167,6 +168,8 @@ struct stlink_usb_handle_s {
 #define STLINK_SWD_AP_STICKY_ERROR     0x19
 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
 
+#define STLINK_BAD_AP_ERROR            0x1d
+
 #define STLINK_CORE_RUNNING            0x80
 #define STLINK_CORE_HALTED             0x81
 #define STLINK_CORE_STAT_UNKNOWN       -1
@@ -713,6 +716,9 @@ static int stlink_usb_error_check(void *handle)
                case STLINK_SWD_DP_WAIT:
                        LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
                        return ERROR_WAIT;
+               case STLINK_JTAG_GET_IDCODE_ERROR:
+                       LOG_DEBUG("STLINK_JTAG_GET_IDCODE_ERROR");
+                       return ERROR_FAIL;
                case STLINK_JTAG_WRITE_ERROR:
                        LOG_DEBUG("Write error");
                        return ERROR_FAIL;
@@ -751,6 +757,9 @@ static int stlink_usb_error_check(void *handle)
                case STLINK_SWD_AP_STICKYORUN_ERROR:
                        LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
                        return ERROR_FAIL;
+               case STLINK_BAD_AP_ERROR:
+                       LOG_DEBUG("STLINK_BAD_AP_ERROR");
+                       return ERROR_FAIL;
                default:
                        LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
                        return ERROR_FAIL;
@@ -1002,7 +1011,7 @@ static int stlink_usb_version(void *handle)
        if (bridge)
                p += sprintf(p, "B%d", bridge);
        if (swim || !msd)
-               p += sprintf(p, "S%d", swim);
+               sprintf(p, "S%d", swim);
 
        LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
                v_str,
@@ -1886,20 +1895,21 @@ static int stlink_usb_read_regs(void *handle)
 
        assert(handle != NULL);
 
-       stlink_usb_init_buffer(handle, h->rx_ep, 84);
+       stlink_usb_init_buffer(handle, h->rx_ep, 88);
 
        h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-       if (h->version.jtag_api == STLINK_JTAG_API_V1)
+       if (h->version.jtag_api == STLINK_JTAG_API_V1) {
+
                h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
-       else
+               res = stlink_usb_xfer(handle, h->databuf, 84);
+               /* regs data from offset 0 */
+       } else {
                h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
+               res = stlink_usb_xfer(handle, h->databuf, 88);
+               /* status at offset 0, regs data from offset 4 */
+       }
 
-       res = stlink_usb_xfer(handle, h->databuf, 84);
-
-       if (res != ERROR_OK)
-               return res;
-
-       return ERROR_OK;
+       return res;
 }
 
 /** */