[fix] warnings

[fw/stlink] / src / stlink-usb.c

diff --git a/src/stlink-usb.c b/src/stlink-usb.c
index 16f934304027f9a90ef1fb71dbf12e7aeb6e9d67..9675d37f671c8cf5c4e4a75cac7e9a13d531342d 100644 (file)
--- a/src/stlink-usb.c
+++ b/src/stlink-usb.c
@@ -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,37 @@ 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);
+
+    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);
+
+    stlink_print_data(sl);
+    send_only(slu, buf, len);
 }
 
 
@@ -289,6 +313,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;
@@ -322,6 +350,10 @@ void _stlink_usb_step(stlink_t* sl) {
     }
 }
 
+/**
+ * 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;
@@ -361,7 +393,6 @@ void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     ssize_t size;
 
     /* assume len < sizeof(sl->q_buf) */
-    assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
 
     memset(buf, 0, sizeof (sl->q_buf));
     buf[0] = STLINK_DEBUG_COMMAND;
@@ -384,20 +415,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,
@@ -426,6 +537,9 @@ stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
     stlink_t* sl = NULL;
     struct stlink_libusb* slu = NULL;
 
+    /* unused */
+    dev_name = dev_name;
+
     sl = malloc(sizeof (stlink_t));
     slu = malloc(sizeof (struct stlink_libusb));
     if (sl == NULL) goto on_error;
@@ -528,4 +642,5 @@ on_error:
     if (sl != NULL) free(sl);
     if (slu != NULL) free(slu);
     return 0;
-}
\ No newline at end of file
+}
+