X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fstlink-usb.c;h=7bcff82daea54a5cdeac6fd04c62d365beb361a1;hb=7abc08eeefdd4128ca55bcece97eda9c54a9e5bb;hp=895b368fa512c432d44802e972adeccbcd932f6a;hpb=bd3472751be353b9e220bfa2fa59bacb4d6b5b57;p=fw%2Fstlink

diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 895b368..7bcff82 100644
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -155,7 +155,7 @@ void _stlink_usb_version(stlink_t *sl) {
     buf[0] = STLINK_GET_VERSION;
     buf[1] = 0x80;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 6);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -163,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");
     }
@@ -171,13 +171,41 @@ 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); 
+    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
+    send_only(slu, buf, len);
 }
 
 
@@ -187,7 +215,7 @@ int _stlink_usb_current_mode(stlink_t * sl) {
     ssize_t size;
     memset(buf, 0, sizeof (sl->q_buf));
     buf[0] = STLINK_GET_CURRENT_MODE;
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return -1;
@@ -204,7 +232,7 @@ void _stlink_usb_core_id(stlink_t * sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_READCOREID;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 4);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -223,7 +251,7 @@ void _stlink_usb_status(stlink_t * sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_GETSTATUS;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -247,7 +275,7 @@ void _stlink_usb_force_debug(stlink_t *sl) {
 
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_FORCEDEBUG;
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -266,7 +294,7 @@ void _stlink_usb_enter_swd_mode(stlink_t * sl) {
     buf[1] = STLINK_SWD_ENTER;
     buf[2] = STLINK_DEBUG_ENTER_SWD;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -289,6 +317,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;
@@ -315,13 +347,17 @@ void _stlink_usb_step(stlink_t* sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_STEPCORE;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, 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;
@@ -331,7 +367,7 @@ void _stlink_usb_run(stlink_t* sl) {
     buf[0] = STLINK_DEBUG_COMMAND;
     buf[1] = STLINK_DEBUG_RUNCORE;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, 2);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -360,8 +396,9 @@ void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     unsigned char* const buf = sl->q_buf;
     ssize_t size;
 
-    /* assume len < sizeof(sl->q_buf) */
-    assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
+#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;
@@ -372,7 +409,7 @@ void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     assert (len < 256);
     buf[6] = (uint8_t) len;
 
-    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, sizeof (sl->q_buf));
+    size = send_recv(slu, buf, STLINK_CMD_SIZE, buf, len);
     if (size == -1) {
         printf("[!] send_recv\n");
         return;
@@ -384,20 +421,100 @@ void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 }
 
 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
-    DD(sl, "oops! read_all_regs not implemented for USB!\n");
+    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) {
-    DD(sl, "oops! read_reg not implemented for USB! Wanted to read reg %d\n",
-            r_idx);
-}
+    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;
 
-void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
-    DD(sl, "oops! write_reg not implemented for USB! Wanted to write %#x to %d\n",
-            reg, idx);
+    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,