X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fdrivers%2Fstlink_usb.c;h=7281e584396327ab2f7bcaa279ed2915da0d76c3;hp=4c5081c9e36917ce6bb8b2dc6607a0e062e83e57;hb=ce9e21b769358b6408aada59810c33544bd8df4d;hpb=ffe6bc82201ff3dceea193af530738d868258b0b

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 4c5081c9e3..7281e58439 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1761,18 +1761,6 @@ static enum target_state stlink_usb_state(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
-		res = stlink_usb_mode_enter(handle, h->st_mode);
-		if (res != ERROR_OK)
-			return TARGET_UNKNOWN;
-
-		res = stlink_swim_resync(handle);
-		if (res != ERROR_OK)
-			return TARGET_UNKNOWN;
-
-		return ERROR_OK;
-	}
-
 	if (h->reconnect_pending) {
 		LOG_INFO("Previous state query failed, trying to reconnect");
 		res = stlink_usb_mode_enter(handle, h->st_mode);
@@ -1892,9 +1880,6 @@ static int stlink_usb_reset(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
-		return stlink_swim_generate_rst(handle);
-
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -2322,11 +2307,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.
@@ -2407,11 +2387,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.
@@ -3546,24 +3521,64 @@ static void stlink_dap_op_quit(struct adiv5_dap *dap)
 
 static int stlink_swim_op_srst(void)
 {
-	return stlink_usb_reset(stlink_dap_handle);
+	return stlink_swim_generate_rst(stlink_dap_handle);
 }
 
 static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
 								   uint32_t count, uint8_t *buffer)
 {
-	return stlink_usb_read_mem(stlink_dap_handle, addr, size, count, 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)
 {
-	return stlink_usb_write_mem(stlink_dap_handle, addr, size, count, 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);
+	int retval;
+
+	retval = stlink_usb_mode_enter(stlink_dap_handle, STLINK_MODE_DEBUG_SWIM);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return stlink_swim_resync(stlink_dap_handle);
 }
 
 static int stlink_dap_config_trace(bool enabled,