stlink: add hardware srst functions to stlink/v2

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

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index e373a27a19e7d5a5d6777ec01f92a7ab2ed5b01c..d40436a0a6fdda28c70bd8efd753055f7fc1f080 100644 (file)
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -32,6 +32,8 @@
 #include <jtag/stlink/stlink_interface.h>
 #include <target/target.h>
 
+#include <target/cortex_m.h>
+
 #include "libusb_common.h"
 
 #define ENDPOINT_IN    0x80
@@ -142,9 +144,11 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV2_RESETSYS            0x32
 #define STLINK_DEBUG_APIV2_READREG             0x33
 #define STLINK_DEBUG_APIV2_WRITEREG            0x34
+#define STLINK_DEBUG_APIV2_WRITEDEBUGREG       0x35
+#define STLINK_DEBUG_APIV2_READDEBUGREG        0x36
 
 #define STLINK_DEBUG_APIV2_READALLREGS         0x3A
-
+#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS     0x3B
 #define STLINK_DEBUG_APIV2_DRIVE_NRST          0x3C
 
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW      0x00
@@ -649,6 +653,78 @@ static int stlink_usb_idcode(void *handle, uint32_t *idcode)
        return ERROR_OK;
 }
 
+static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
+{
+       struct stlink_usb_handle_s *h;
+       int res;
+
+       assert(handle != NULL);
+
+       h = (struct stlink_usb_handle_s *)handle;
+
+       stlink_usb_init_buffer(handle, STLINK_RX_EP, 8);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
+       h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+       h->cmdidx += 4;
+
+       res = stlink_usb_xfer(handle, h->databuf, 8);
+
+       if (res != ERROR_OK)
+               return res;
+
+       *val = le_to_h_u32(h->databuf + 4);
+
+       return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+}
+
+static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
+{
+       int res;
+       struct stlink_usb_handle_s *h;
+
+       assert(handle != NULL);
+
+       h = (struct stlink_usb_handle_s *)handle;
+
+       stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       if (h->jtag_api == STLINK_JTAG_API_V1)
+               h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
+       else
+               h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
+       h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+       h->cmdidx += 4;
+       h_u32_to_le(h->cmdbuf+h->cmdidx, val);
+       h->cmdidx += 4;
+
+       res = stlink_usb_xfer(handle, h->databuf, 2);
+
+       if (res != ERROR_OK)
+               return res;
+
+       return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+}
+
+static enum target_state stlink_usb_v2_get_status(void *handle)
+{
+       int result;
+       uint32_t status;
+
+       result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
+       if  (result != ERROR_OK)
+               return TARGET_UNKNOWN;
+
+       if (status & S_HALT)
+               return TARGET_HALTED;
+       else if (status & S_RESET_ST)
+               return TARGET_RESET;
+
+       return TARGET_RUNNING;
+}
+
 /** */
 static enum target_state stlink_usb_state(void *handle)
 {
@@ -660,7 +736,7 @@ static enum target_state stlink_usb_state(void *handle)
        h = (struct stlink_usb_handle_s *)handle;
 
        if (h->jtag_api == STLINK_JTAG_API_V2)
-               return TARGET_UNKNOWN;
+               return stlink_usb_v2_get_status(handle);
 
        stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 
@@ -709,6 +785,32 @@ static int stlink_usb_reset(void *handle)
        return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 }
 
+static int stlink_usb_assert_srst(void *handle, int srst)
+{
+       int res;
+       struct stlink_usb_handle_s *h;
+
+       assert(handle != NULL);
+
+       h = (struct stlink_usb_handle_s *)handle;
+
+       if (h->jtag_api == STLINK_JTAG_API_V1)
+               return ERROR_COMMAND_NOTFOUND;
+
+       stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
+       h->cmdbuf[h->cmdidx++] = srst;
+
+       res = stlink_usb_xfer(handle, h->databuf, 2);
+
+       if (res != ERROR_OK)
+               return res;
+
+       return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+}
+
 /** */
 static int stlink_usb_run(void *handle)
 {
@@ -720,7 +822,7 @@ static int stlink_usb_run(void *handle)
        h = (struct stlink_usb_handle_s *)handle;
 
        if (h->jtag_api == STLINK_JTAG_API_V2)
-               return ERROR_FAIL;
+               return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 
        stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 
@@ -746,7 +848,7 @@ static int stlink_usb_halt(void *handle)
        h = (struct stlink_usb_handle_s *)handle;
 
        if (h->jtag_api == STLINK_JTAG_API_V2)
-               return ERROR_FAIL;
+               return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 
        stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 
@@ -772,7 +874,7 @@ static int stlink_usb_step(void *handle)
        h = (struct stlink_usb_handle_s *)handle;
 
        if (h->jtag_api == STLINK_JTAG_API_V2)
-               return ERROR_FAIL;
+               return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_DEBUGEN);
 
        stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 
@@ -823,7 +925,7 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 
        h = (struct stlink_usb_handle_s *)handle;
 
-       stlink_usb_init_buffer(handle, STLINK_RX_EP, 4);
+       stlink_usb_init_buffer(handle, STLINK_RX_EP, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 
        h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
        if (h->jtag_api == STLINK_JTAG_API_V1)
@@ -832,12 +934,17 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
                h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
        h->cmdbuf[h->cmdidx++] = num;
 
-       res = stlink_usb_xfer(handle, h->databuf, 4);
+       res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 
        if (res != ERROR_OK)
                return res;
 
-       *val = le_to_h_u32(h->databuf);
+       if (h->jtag_api == STLINK_JTAG_API_V1)
+               *val = le_to_h_u32(h->databuf);
+       else {
+               *val = le_to_h_u32(h->databuf + 4);
+               return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
+       }
 
        return ERROR_OK;
 }
@@ -871,6 +978,31 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
        return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 }
 
+static int stlink_usb_get_rw_status(void *handle)
+{
+       int res;
+       struct stlink_usb_handle_s *h;
+
+       assert(handle != NULL);
+
+       h = (struct stlink_usb_handle_s *)handle;
+
+       if (h->jtag_api == STLINK_JTAG_API_V1)
+               return ERROR_OK;
+
+       stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
+
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+       h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
+
+       res = stlink_usb_xfer(handle, h->databuf, 2);
+
+       if (res != ERROR_OK)
+               return res;
+
+       return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
+}
+
 /** */
 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
                          uint8_t *buffer)
@@ -903,7 +1035,7 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
 
        memcpy(buffer, h->databuf, len);
 
-       return ERROR_OK;
+       return stlink_usb_get_rw_status(handle);
 }
 
 /** */
@@ -931,7 +1063,7 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
        if (res != ERROR_OK)
                return res;
 
-       return ERROR_OK;
+       return stlink_usb_get_rw_status(handle);
 }
 
 /** */
@@ -963,7 +1095,7 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 
        memcpy(buffer, h->databuf, len);
 
-       return ERROR_OK;
+       return stlink_usb_get_rw_status(handle);
 }
 
 /** */
@@ -993,7 +1125,7 @@ static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
        if (res != ERROR_OK)
                return res;
 
-       return ERROR_OK;
+       return stlink_usb_get_rw_status(handle);
 }
 
 /** */
@@ -1082,8 +1214,8 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
                return err;
        }
 
-       /* set the used jtag api */
-       h->jtag_api = STLINK_JTAG_API_V1;
+       /* set the used jtag api, this will default to the newest supported version */
+       h->jtag_api = h->version.jtag_api_max;
 
        /* initialize the debug hardware */
        err = stlink_usb_init_mode(h);
@@ -1119,6 +1251,8 @@ struct stlink_layout_api_s stlink_usb_layout_api = {
        /** */
        .reset = stlink_usb_reset,
        /** */
+       .assert_srst = stlink_usb_assert_srst,
+       /** */
        .run = stlink_usb_run,
        /** */
        .halt = stlink_usb_halt,