stlink: simplify mem R/W with SWIM

[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 aef02c5635fac05609c8af08c758e83c1ecc2778..c866dc54d3007fbb3866c000e521cc33d4bf9b2d 100644 (file)
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -36,6 +36,7 @@
 #include <jtag/hla/hla_layout.h>
 #include <jtag/hla/hla_transport.h>
 #include <jtag/hla/hla_interface.h>
+#include <jtag/swim.h>
 #include <target/target.h>
 #include <transport/transport.h>
 
@@ -1287,8 +1288,6 @@ static enum stlink_mode stlink_get_mode(enum hl_transports t)
                return STLINK_MODE_DEBUG_SWD;
        case HL_TRANSPORT_JTAG:
                return STLINK_MODE_DEBUG_JTAG;
-       case HL_TRANSPORT_SWIM:
-               return STLINK_MODE_DEBUG_SWIM;
        default:
                return STLINK_MODE_UNKNOWN;
        }
@@ -2323,11 +2322,6 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
                if (count < bytes_remaining)
                        bytes_remaining = count;
 
-               if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
-                       retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
-                       if (retval != ERROR_OK)
-                               return retval;
-               } else
                /*
                 * all stlink support 8/32bit memory read/writes and only from
                 * stlink V2J26 there is support for 16 bit memory read/write.
@@ -2408,11 +2402,6 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
                if (count < bytes_remaining)
                        bytes_remaining = count;
 
-               if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
-                       retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
-                       if (retval != ERROR_OK)
-                               return retval;
-               } else
                /*
                 * all stlink support 8/32bit memory read/writes and only from
                 * stlink V2J26 there is support for 16 bit memory read/write.
@@ -2478,17 +2467,20 @@ static int stlink_usb_override_target(const char *targetname)
 
 static int stlink_speed_swim(void *handle, int khz, bool query)
 {
+       int retval;
+
        /*
-                       we dont care what the khz rate is
                        we only have low and high speed...
                        before changing speed the SWIM_CSR HS bit
                        must be updated
         */
-       if (khz == 0)
-               stlink_swim_speed(handle, 0);
-       else
-               stlink_swim_speed(handle, 1);
-       return khz;
+       if (!query) {
+               retval = stlink_swim_speed(handle, (khz < SWIM_FREQ_HIGH) ? 0 : 1);
+               if (retval != ERROR_OK)
+                       LOG_ERROR("Unable to set adapter speed");
+       }
+
+       return (khz < SWIM_FREQ_HIGH) ? SWIM_FREQ_LOW : SWIM_FREQ_HIGH;
 }
 
 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
@@ -3542,6 +3534,62 @@ static void stlink_dap_op_quit(struct adiv5_dap *dap)
                LOG_ERROR("Error closing APs");
 }
 
+static int stlink_swim_op_srst(void)
+{
+       return stlink_usb_reset(stlink_dap_handle);
+}
+
+static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
+                                                                  uint32_t count, uint8_t *buffer)
+{
+       int retval;
+       uint32_t bytes_remaining;
+
+       LOG_DEBUG_IO("read at 0x%08x len %d*0x%08x", addr, size, count);
+       count *= size;
+
+       while (count) {
+               bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
+               retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
+               if (retval != ERROR_OK)
+                       return retval;
+
+               buffer += bytes_remaining;
+               addr += bytes_remaining;
+               count -= bytes_remaining;
+       }
+
+       return ERROR_OK;
+}
+
+static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
+                                                                       uint32_t count, const uint8_t *buffer)
+{
+       int retval;
+       uint32_t bytes_remaining;
+
+       LOG_DEBUG_IO("write at 0x%08x len %d*0x%08x", addr, size, count);
+       count *= size;
+
+       while (count) {
+               bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
+               retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
+               if (retval != ERROR_OK)
+                       return retval;
+
+               buffer += bytes_remaining;
+               addr += bytes_remaining;
+               count -= bytes_remaining;
+       }
+
+       return ERROR_OK;
+}
+
+static int stlink_swim_op_reconnect(void)
+{
+       return stlink_usb_state(stlink_dap_handle);
+}
+
 static int stlink_dap_config_trace(bool enabled,
                enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
                unsigned int *trace_freq, unsigned int traceclkin_freq,
@@ -3652,6 +3700,8 @@ static int stlink_dap_init(void)
                mode = STLINK_MODE_DEBUG_SWD;
        else if (transport_is_dapdirect_jtag())
                mode = STLINK_MODE_DEBUG_JTAG;
+       else if (transport_is_swim())
+               mode = STLINK_MODE_DEBUG_SWIM;
        else {
                LOG_ERROR("Unsupported transport");
                return ERROR_FAIL;
@@ -3661,7 +3711,8 @@ static int stlink_dap_init(void)
        if (retval != ERROR_OK)
                return retval;
 
-       if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
+       if ((mode != STLINK_MODE_DEBUG_SWIM) &&
+               !(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
                LOG_ERROR("ST-Link version does not support DAP direct transport");
                return ERROR_FAIL;
        }
@@ -3733,7 +3784,14 @@ static const struct dap_ops stlink_dap_ops = {
        .quit = stlink_dap_op_quit, /* optional */
 };
 
-static const char *const stlink_dap_transport[] = { "dapdirect_jtag", "dapdirect_swd", NULL };
+static const struct swim_driver stlink_swim_ops = {
+       .srst = stlink_swim_op_srst,
+       .read_mem = stlink_swim_op_read_mem,
+       .write_mem = stlink_swim_op_write_mem,
+       .reconnect = stlink_swim_op_reconnect,
+};
+
+static const char *const stlink_dap_transport[] = { "dapdirect_jtag", "dapdirect_swd", "swim", NULL };
 
 struct adapter_driver stlink_dap_adapter_driver = {
        .name = "st-link",
@@ -3751,4 +3809,5 @@ struct adapter_driver stlink_dap_adapter_driver = {
 
        .dap_jtag_ops = &stlink_dap_ops,
        .dap_swd_ops = &stlink_dap_ops,
+       .swim_ops = &stlink_swim_ops,
 };