From 840f5e48210a6b212eda8cf67a03dfeffd886172 Mon Sep 17 00:00:00 2001
From: Fabien Le Mentec <texane@gmail.com>
Date: Sat, 15 Oct 2011 17:21:35 -0500
Subject: [PATCH] [update] unused stlinkv2 commands

---
 src/stlink-common.h |  2 +-
 src/stlink-usb.c    | 67 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/src/stlink-common.h b/src/stlink-common.h
index 237aa66..6110875 100644
--- a/src/stlink-common.h
+++ b/src/stlink-common.h
@@ -18,7 +18,7 @@ extern "C" {
     // Max data transfer size.
     // 6kB = max mem32_read block, 8kB sram
     //#define Q_BUF_LEN	96
-#define Q_BUF_LEN	1024 * 100
+#define Q_BUF_LEN			(1024 * 100)
 
     // st-link vendor cmd's
 #define USB_ST_VID			0x0483
diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 9675d37..6eb456c 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -414,6 +414,9 @@ void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     stlink_print_data(sl);
 }
 
+
+#if 1 /* stlinkv1 */
+
 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
@@ -448,6 +451,41 @@ void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
     DD(sl, "rw2        = 0x%08x\n", read_uint32(sl->q_buf, 80));
 }
 
+#else /* stlinkv2 */
+
+static void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
+    struct stlink_libusb * const slu = sl->backend_data;
+    unsigned char* const buf = sl->q_buf;
+    unsigned char* const cmd_buf = sl->c_buf;
+    ssize_t size;
+    int i;
+
+#define STLINK_JTAG_COMMAND 0xf2
+#define STLINK_JTAG_READALLREGS2 0x3a
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_JTAG_COMMAND;
+    cmd_buf[1] = STLINK_JTAG_READALLREGS2;
+    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 84);
+
+    if (size == -1) {
+        printf("[!] send_recv\n");
+        return;
+    }
+
+    sl->q_len = (size_t) size;
+
+    for(i=0; i<16; i++)
+      regp->r[i]= read_uint32(sl->q_buf, i*4);
+
+    regp->xpsr       = read_uint32(sl->q_buf, 64);
+    regp->main_sp    = read_uint32(sl->q_buf, 68);
+    regp->process_sp = read_uint32(sl->q_buf, 72);
+    regp->rw         = read_uint32(sl->q_buf, 76);
+    regp->rw2        = read_uint32(sl->q_buf, 80);
+}
+
+#endif /* stlinkv1 */
+
 void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
@@ -490,6 +528,9 @@ void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
     }
 }
 
+
+#if 1 /* stlinkv1 */
+
 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
@@ -510,6 +551,32 @@ void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
     stlink_print_data(sl);
 }
 
+#else /* stlinkv2 */
+
+void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
+    struct stlink_libusb * const slu = sl->backend_data;
+    unsigned char* const buf = sl->q_buf;
+    unsigned char *cmd_buf = sl->c_buf;
+    ssize_t size;
+
+#define STLINK_JTAG_WRITEREG2 0x34
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_JTAG_COMMAND;
+    cmd_buf[1] = STLINK_JTAG_WRITEREG2;
+    cmd_buf[2] = idx;
+    write_uint32(cmd_buf + 3, reg);
+    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 2);
+    if (size == -1) {
+        printf("[!] send_recv\n");
+        return;
+    }
+    sl->q_len = (size_t) size;
+    stlink_print_data(sl);
+}
+
+#endif /* stlinkv1 */
+
+
 stlink_backend_t _stlink_usb_backend = {
     _stlink_usb_close,
     _stlink_usb_exit_debug_mode,
-- 
2.30.2