stlink: add read/write 8bit memory

This patch add layout api funtions and implementation
to read/write 8bit memory.

Change-Id: I8d145eb07e5afa9ce1830578e57d80a80d21e7dc
Signed-off-by: Mathias K <kesmtp@freenet.de>
Reviewed-on: http://openocd.zylin.com/366
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

2 files changed, 78 insertions, 5 deletions

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 3b262f93..cae468f1 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -87,6 +87,7 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_RUNCORE		0x09
 #define STLINK_DEBUG_STEPCORE		0x0a
 #define STLINK_DEBUG_SETFP		0x0b
+#define STLINK_DEBUG_READMEM_8BIT	0x0c
 #define STLINK_DEBUG_WRITEMEM_8BIT	0x0d
 #define STLINK_DEBUG_CLEARFP		0x0e
 #define STLINK_DEBUG_WRITEDEBUGREG	0x0f
@@ -97,7 +98,7 @@ struct stlink_usb_handle_s {
 #define STLINK_SWD_READCOREID		0x32
 
 /** */
-int stlink_usb_recv(void *handle, uint8_t *txbuf, int txsize, uint8_t *rxbuf,
+int stlink_usb_recv(void *handle, const uint8_t *txbuf, int txsize, uint8_t *rxbuf,
 		    int rxsize)
 {
 	struct stlink_usb_handle_s *h;
@@ -524,6 +525,70 @@ int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 }
 
 /** */
+int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
+			  uint8_t *buffer)
+{
+	int res;
+	uint16_t read_len = len;
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	stlink_usb_init_buffer(handle);
+
+	h->txbuf[0] = STLINK_DEBUG_COMMAND;
+	h->txbuf[1] = STLINK_DEBUG_READMEM_8BIT;
+	h_u32_to_le(h->txbuf + 2, addr);
+	h_u16_to_le(h->txbuf + 2 + 4, len);
+
+	/* we need to fix read length for single bytes */
+	if (read_len == 1)
+		read_len++;
+
+	res = stlink_usb_recv(handle, h->txbuf, STLINK_CMD_SIZE, h->rxbuf, read_len);
+
+	if (res != ERROR_OK)
+		return res;
+
+	memcpy(buffer, h->rxbuf, len);
+
+	return ERROR_OK;
+}
+
+/** */
+int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
+			   const uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	stlink_usb_init_buffer(handle);
+
+	h->txbuf[0] = STLINK_DEBUG_COMMAND;
+	h->txbuf[1] = STLINK_DEBUG_WRITEMEM_8BIT;
+	h_u32_to_le(h->txbuf + 2, addr);
+	h_u16_to_le(h->txbuf + 2 + 4, len);
+
+	res = stlink_usb_recv(handle, h->txbuf, STLINK_CMD_SIZE, 0, 0);
+
+	if (res != ERROR_OK)
+		return res;
+
+	res = stlink_usb_recv(handle, (uint8_t *) buffer, len, 0, 0);
+
+	if (res != ERROR_OK)
+		return res;
+
+	return ERROR_OK;
+}
+
+/** */
 int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 			  uint32_t *buffer)
 {
@@ -555,7 +620,7 @@ int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 
 /** */
 int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
-			   uint32_t *buffer)
+			   const uint32_t *buffer)
 {
 	int res;
 	struct stlink_usb_handle_s *h;
@@ -583,8 +648,6 @@ int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
 	if (res != ERROR_OK)
 		return res;
 
-	memcpy(buffer, h->rxbuf, len);
-
 	return ERROR_OK;
 }
 
@@ -652,6 +715,10 @@ struct stlink_layout_api_s stlink_layout_api = {
 	/** */
 	.write_reg = stlink_usb_write_reg,
 	/** */
+	.read_mem8 = stlink_usb_read_mem8,
+	/** */
+	.write_mem8 = stlink_usb_write_mem8,
+	/** */
 	.read_mem32 = stlink_usb_read_mem32,
 	/** */
 	.write_mem32 = stlink_usb_write_mem32,
diff --git a/src/jtag/stlink/stlink_layout.h b/src/jtag/stlink/stlink_layout.h
index 46517a78..bf1e2c68 100644
--- a/src/jtag/stlink/stlink_layout.h
+++ b/src/jtag/stlink/stlink_layout.h
@@ -48,11 +48,17 @@ struct stlink_layout_api_s {
 	/** */
 	int (*write_reg) (void *fd, int num, uint32_t val);
 	/** */
+	int (*read_mem8) (void *handle, uint32_t addr, uint16_t len,
+			   uint8_t *buffer);
+	/** */
+	int (*write_mem8) (void *handle, uint32_t addr, uint16_t len,
+			    const uint8_t *buffer);
+	/** */
 	int (*read_mem32) (void *handle, uint32_t addr, uint16_t len,
 			   uint32_t *buffer);
 	/** */
 	int (*write_mem32) (void *handle, uint32_t addr, uint16_t len,
-			    uint32_t *buffer);
+			    const uint32_t *buffer);
 	/** */
 	int (*idcode) (void *fd, uint32_t *idcode);
 	/** */