X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fstlink-usb.c;h=b417d41aa1ea7269a92d94b2dc20e74ab1e465e0;hb=06a737c4cc2aa1238fcb44f422e139e1c147c41f;hp=4a2fe16bfd946d398ef9c983d4be316ab3b6b2ba;hpb=93ea941dff98b31ff6498a9b222c5bb1d40fe8a1;p=fw%2Fstlink

diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 4a2fe16..b417d41 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -1,3 +1,4 @@
+#include <assert.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -26,48 +27,6 @@ void _stlink_usb_close(stlink_t* sl) {
     }
 }
 
-static void write_uint32(unsigned char* buf, uint32_t ui) {
-    if (!is_bigendian()) { // le -> le (don't swap)
-        buf[0] = ((unsigned char*) &ui)[0];
-        buf[1] = ((unsigned char*) &ui)[1];
-        buf[2] = ((unsigned char*) &ui)[2];
-        buf[3] = ((unsigned char*) &ui)[3];
-    } else {
-        buf[0] = ((unsigned char*) &ui)[3];
-        buf[1] = ((unsigned char*) &ui)[2];
-        buf[2] = ((unsigned char*) &ui)[1];
-        buf[3] = ((unsigned char*) &ui)[0];
-    }
-}
-
-static void write_uint16(unsigned char* buf, uint16_t ui) {
-    if (!is_bigendian()) { // le -> le (don't swap)
-        buf[0] = ((unsigned char*) &ui)[0];
-        buf[1] = ((unsigned char*) &ui)[1];
-    } else {
-        buf[0] = ((unsigned char*) &ui)[1];
-        buf[1] = ((unsigned char*) &ui)[0];
-    }
-}
-
-static uint32_t read_uint32(const unsigned char *c, const int pt) {
-    uint32_t ui;
-    char *p = (char *) &ui;
-
-    if (!is_bigendian()) { // le -> le (don't swap)
-        p[0] = c[pt];
-        p[1] = c[pt + 1];
-        p[2] = c[pt + 2];
-        p[3] = c[pt + 3];
-    } else {
-        p[0] = c[pt + 3];
-        p[1] = c[pt + 2];
-        p[2] = c[pt + 1];
-        p[3] = c[pt];
-    }
-    return ui;
-}
-
 
 struct trans_ctx {
 #define TRANS_FLAGS_IS_DONE (1 << 0)
@@ -178,16 +137,16 @@ static int is_stlink_device(libusb_device * dev) {
 
     printf("device: 0x%04x, 0x%04x\n", desc.idVendor, desc.idProduct);
 
-    if (desc.idVendor != 0x0483)
+    if (desc.idVendor != USB_ST_VID)
         return 0;
 
-    if (desc.idProduct != 0x3748)
+    if (desc.idProduct != USB_STLINK_32L_PID)
         return 0;
 
     return 1;
 }
 
-void _stlink_usb_version(stlink_t * sl) {
+void _stlink_usb_version(stlink_t *sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
     ssize_t size;
@@ -196,7 +155,7 @@ void _stlink_usb_version(stlink_t * sl) {
     buf[0] = STLINK_GET_VERSION;
     buf[1] = 0x80;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 6);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -204,7 +163,7 @@ void _stlink_usb_version(stlink_t * sl) {
 
 #if 1 /* DEBUG */
     {
-        unsigned int i;
+        ssize_t i;
         for (i = 0; i < size; ++i) printf("%02x", buf[i]);
         printf("\n");
     }
@@ -212,42 +171,58 @@ void _stlink_usb_version(stlink_t * sl) {
 }
 
 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
-    DD(sl, "oops! no write32 support yet, wanted to write %d bytes to %#x\n",
-            len, addr);
+    struct stlink_libusb * const slu = sl->backend_data;
+    unsigned char* const buf = sl->q_buf;
+    unsigned char *cmd_buf = sl->c_buf;
+
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_DEBUG_COMMAND;
+    cmd_buf[1] =  STLINK_DEBUG_WRITEMEM_32BIT;
+    write_uint32(cmd_buf + 2, addr);
+    write_uint16(cmd_buf + 6, len);
+    send_only(slu, cmd_buf, STLINK_CMD_SIZE);
+
+#if Q_BUF_LEN < UINT16_MAX
+    assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
+#endif
+    assert((len & 3) == 0); 
+    stlink_print_data(sl);
+    send_only(slu, buf, len);
+
 }
 
 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
-    DD(sl, "oops! no write8 support yet, wanted to write %d bytes to %#x\n",
-            len, addr);
-}
+    struct stlink_libusb * const slu = sl->backend_data;
+    unsigned char* const buf = sl->q_buf;
+    unsigned char *cmd_buf = sl->c_buf;
 
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_DEBUG_COMMAND;
+    cmd_buf[1] =  STLINK_DEBUG_WRITEMEM_8BIT;
+    write_uint32(cmd_buf + 2, addr);
+    write_uint16(cmd_buf + 6, len);
+    send_only(slu, cmd_buf, STLINK_CMD_SIZE);
+
+#if Q_BUF_LEN < UINT16_MAX
+    assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
+#endif
+    stlink_print_data(sl);
+    send_only(slu, buf, len);
+}
 
-int stlink_current_mode(stlink_t * sl) {
-    int mode = -1;
 
+int _stlink_usb_current_mode(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
     ssize_t size;
-
     memset(buf, 0, sizeof (sl->q_buf));
-
     buf[0] = STLINK_GET_CURRENT_MODE;
-
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return -1;
     }
-
-    /* mode = (int)read_uint16(buf, 0); */
-    mode = (int) buf[0];
-
-#if 1 /* DEBUG */
-    printf("mode == 0x%x\n", mode);
-#endif /* DEBUG */
-
-
-    return mode;
+    return sl->q_buf[0];
 }
 
 void _stlink_usb_core_id(stlink_t * sl) {
@@ -259,7 +234,7 @@ void _stlink_usb_core_id(stlink_t * sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_READCOREID;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 4);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -278,7 +253,7 @@ void _stlink_usb_status(stlink_t * sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_GETSTATUS;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -293,7 +268,24 @@ void _stlink_usb_status(stlink_t * sl) {
 
 }
 
-void _stlink_enter_swd_mode(stlink_t * sl) {
+void _stlink_usb_force_debug(stlink_t *sl) {
+    struct stlink_libusb *slu = sl->backend_data;
+    unsigned char* const buf = sl->q_buf;
+    ssize_t size;
+
+    memset(buf, 0, sizeof (sl->q_buf));
+
+    buf[0] = STLINK_DEBUG_COMMAND;
+    buf[1] = STLINK_DEBUG_FORCEDEBUG;
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
+    if (size == -1) {
+        printf("[!] send_recv\n");
+        return;
+    }
+}
+
+
+void _stlink_usb_enter_swd_mode(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
     ssize_t size;
@@ -301,15 +293,14 @@ void _stlink_enter_swd_mode(stlink_t * sl) {
     memset(buf, 0, sizeof (sl->q_buf));
 
     buf[0] = STLINK_DEBUG_COMMAND;
-    buf[1] = 0x30; /* needed byte */
-    buf[2] = STLINK_DEBUG_ENTER_JTAG;
+    buf[1] = STLINK_SWD_ENTER;
+    buf[2] = STLINK_DEBUG_ENTER_SWD;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
-
 }
 
 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
@@ -328,6 +319,10 @@ void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
     }
 }
 
+/**
+ * TODO - not convinced this does anything...
+ * @param sl
+ */
 void _stlink_usb_reset(stlink_t * sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
@@ -337,7 +332,7 @@ void _stlink_usb_reset(stlink_t * sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_RESETSYS;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -345,7 +340,7 @@ void _stlink_usb_reset(stlink_t * sl) {
 }
 
 
-void stlink_step(stlink_t* sl) {
+void _stlink_usb_step(stlink_t* sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
     ssize_t size;
@@ -354,13 +349,17 @@ void stlink_step(stlink_t* sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_STEPCORE;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
     }
 }
 
+/**
+ * This seems to do a good job of restarting things from the beginning?
+ * @param sl
+ */
 void _stlink_usb_run(stlink_t* sl) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
@@ -370,7 +369,7 @@ void _stlink_usb_run(stlink_t* sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_RUNCORE;
 
-    size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -394,24 +393,25 @@ void _stlink_usb_exit_debug_mode(stlink_t *sl) {
     }
 }
 
-void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
+void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     struct stlink_libusb * const slu = sl->backend_data;
     unsigned char* const buf = sl->q_buf;
     ssize_t size;
 
-    /* assume len < sizeof(sl->q_buf) */
+#if Q_BUF_LEN < UINT16_MAX
+    assert(len < sizeof(sl->q_buf));
+#endif
 
     memset(buf, 0, sizeof (sl->q_buf));
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_READMEM_32BIT;
     write_uint32(buf + 2, addr);
-    write_uint16(buf + 6, len);
-#if 0
     /* windows usb logs show only one byte is used for length ... */
+    // Presumably, this is because usb transfers can't be 16 bits worth of bytes long...
+    assert (len < 256);
     buf[6] = (uint8_t) len;
-#endif
 
-    size = send_recv(slu, buf, 0x10, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -422,22 +422,125 @@ void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     stlink_print_data(sl);
 }
 
+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;
+
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_DEBUG_COMMAND;
+    cmd_buf[1] = STLINK_DEBUG_READALLREGS;
+    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;
+    stlink_print_data(sl);
+    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);
+    if (sl->verbose < 2)
+        return;
+
+    DD(sl, "xpsr       = 0x%08x\n", read_uint32(sl->q_buf, 64));
+    DD(sl, "main_sp    = 0x%08x\n", read_uint32(sl->q_buf, 68));
+    DD(sl, "process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72));
+    DD(sl, "rw         = 0x%08x\n", read_uint32(sl->q_buf, 76));
+    DD(sl, "rw2        = 0x%08x\n", read_uint32(sl->q_buf, 80));
+}
+
+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;
+    unsigned char* const cmd_buf = sl->c_buf;
+    ssize_t size;
+    uint32_t r;
+
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_DEBUG_COMMAND;
+    cmd_buf[1] = STLINK_DEBUG_READREG;
+    cmd_buf[2] = (uint8_t) r_idx;
+    size = send_recv(slu, cmd_buf, STLINK_CMD_SIZE, buf, 4);
+    if (size == -1) {
+        printf("[!] send_recv\n");
+        return;
+    }
+    sl->q_len = (size_t) size;
+    stlink_print_data(sl);
+    r = read_uint32(sl->q_buf, 0);
+    DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
+    
+    switch (r_idx) {
+    case 16:
+        regp->xpsr = r;
+        break;
+    case 17:
+        regp->main_sp = r;
+        break;
+    case 18:
+        regp->process_sp = r;
+        break;
+    case 19:
+        regp->rw = r; //XXX ?(primask, basemask etc.)
+        break;
+    case 20:
+        regp->rw2 = r; //XXX ?(primask, basemask etc.)
+        break;
+    default:
+        regp->r[r_idx] = r;
+    }
+}
+
+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;
+
+    memset(cmd_buf, 0, STLINK_CMD_SIZE);
+    cmd_buf[0] = STLINK_DEBUG_COMMAND;
+    cmd_buf[1] = STLINK_DEBUG_WRITEREG;
+    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);
+}
 
 stlink_backend_t _stlink_usb_backend = {
     _stlink_usb_close,
     _stlink_usb_exit_debug_mode,
-    _stlink_enter_swd_mode,
-    NULL,
+    _stlink_usb_enter_swd_mode,
+    NULL,  // no enter_jtag_mode here...
     _stlink_usb_exit_dfu_mode,
     _stlink_usb_core_id,
     _stlink_usb_reset,
     _stlink_usb_run,
     _stlink_usb_status,
     _stlink_usb_version,
+    _stlink_usb_read_mem32,
     _stlink_usb_write_mem32,
-    _stlink_usb_write_mem8
+    _stlink_usb_write_mem8,
+    _stlink_usb_read_all_regs,
+    _stlink_usb_read_reg,
+    _stlink_usb_write_reg,
+    _stlink_usb_step,
+    _stlink_usb_current_mode,
+    _stlink_usb_force_debug
 };
 
+
 stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
     stlink_t* sl = NULL;
     struct stlink_libusb* slu = NULL;
@@ -544,4 +647,5 @@ on_error:
     if (sl != NULL) free(sl);
     if (slu != NULL) free(slu);
     return 0;
-}
\ No newline at end of file
+}
+