Fix write_mem32.

Moved some of the static methods up to the top, so they can always be used, so the changes is
messier than it needs to be.

diff --git a/src/stlink-sg.c b/src/stlink-sg.c
index 64f182022da3cfdece24bc3951537b557d83436d..276986ef7fde4d1e5e47300ba3ba91eb1cf4b4db 100644 (file)
--- a/src/stlink-sg.c
+++ b/src/stlink-sg.c
@@ -107,15 +107,6 @@ static void clear_cdb(struct stlink_libsg *sl) {
     sl->q_data_dir = Q_DATA_IN;
 }
 
-// E.g. make the valgrind happy.
-
-static void clear_buf(stlink_t *sl) {
-    DLOG("*** clear_buf ***\n");
-    for (size_t i = 0; i < sizeof (sl->q_buf); i++)
-        sl->q_buf[i] = 0;
-
-}
-
 // close the device, free the allocated memory
 
 void _stlink_sg_close(stlink_t *sl) {
@@ -128,92 +119,44 @@ void _stlink_sg_close(stlink_t *sl) {
     }
 }
 
-
-//TODO rewrite/cleanup, save the error in sl
-
-#if FINISHED_WITH_SG
-static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
-    struct stlink_libsg *sl = stl->backend_data;
-    const int e = sl->do_scsi_pt_err;
-    if (e < 0) {
-        fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
-                safe_strerror(-e));
-        return;
-    } else if (e == SCSI_PT_DO_BAD_PARAMS) {
-        fprintf(stderr, "scsi_pt error: bad pass through setup\n");
-        return;
-    } else if (e == SCSI_PT_DO_TIMEOUT) {
-        fprintf(stderr, "  pass through timeout\n");
-        return;
+static int get_usb_mass_storage_status(libusb_device_handle *handle, uint8_t endpoint, uint32_t *tag)
+{
+    unsigned char csw[13];
+    memset(csw, 0, sizeof(csw));
+    int transferred;
+    int ret;
+    int try = 0;
+    do {
+        ret = libusb_bulk_transfer(handle, endpoint, (unsigned char *)&csw, sizeof(csw),
+                                   &transferred, SG_TIMEOUT_MSEC);
+        if (ret == LIBUSB_ERROR_PIPE) {
+            libusb_clear_halt(handle, endpoint);
+        }
+        try++;
+    } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
+    if (ret != LIBUSB_SUCCESS) {
+        fprintf(stderr, "%s: receiving failed: %d\n", __func__, ret);
+        return -1;
+    }
+    if (transferred != sizeof(csw)) {
+        fprintf(stderr, "%s: received unexpected amount: %d\n", __func__, transferred);
+        return -1;
     }
-    const int duration = get_scsi_pt_duration_ms(ptvp);
-    if ((stl->verbose > 1) && (duration >= 0))
-        DLOG("      duration=%d ms\n", duration);
-
-    // XXX stlink fw sends broken residue, so ignore it and use the known q_len
-    // "usb-storage quirks=483:3744:r"
-    // forces residue to be ignored and calculated, but this causes aboard if
-    // data_len = 0 and by some other data_len values.
-
-    const int resid = get_scsi_pt_resid(ptvp);
-    const int dsize = stl->q_len - resid;
-
-    const int cat = get_scsi_pt_result_category(ptvp);
-    char buf[512];
-    unsigned int slen;
 
-    switch (cat) {
-        case SCSI_PT_RESULT_GOOD:
-            if (stl->verbose && (resid > 0))
-                DLOG("      notice: requested %d bytes but "
-                    "got %d bytes, ignore [broken] residue = %d\n",
-                    stl->q_len, dsize, resid);
-            break;
-        case SCSI_PT_RESULT_STATUS:
-            if (stl->verbose) {
-                sg_get_scsi_status_str(
-                        get_scsi_pt_status_response(ptvp), sizeof (buf),
-                        buf);
-                DLOG("  scsi status: %s\n", buf);
-            }
-            return;
-        case SCSI_PT_RESULT_SENSE:
-            slen = get_scsi_pt_sense_len(ptvp);
-            if (stl->verbose) {
-                sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
-                        > 1), sizeof (buf), buf);
-                DLOG("%s", buf);
-            }
-            if (stl->verbose && (resid > 0)) {
-                if ((stl->verbose) || (stl->q_len > 0))
-                    DLOG("    requested %d bytes but "
-                        "got %d bytes\n", stl->q_len, dsize);
-            }
-            return;
-        case SCSI_PT_RESULT_TRANSPORT_ERR:
-            if (stl->verbose) {
-                get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
-                // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
-                // These codes potentially come from the firmware on a host adapter
-                // or from one of several hosts that an adapter driver controls.
-                // The 'host_status' field has the following values:
-                //     [0x07] Internal error detected in the host adapter.
-                // This may not be fatal (and the command may have succeeded).
-                DLOG("  transport: %s", buf);
-            }
-            return;
-        case SCSI_PT_RESULT_OS_ERR:
-            if (stl->verbose) {
-                get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
-                DLOG("  os: %s", buf);
-            }
-            return;
-        default:
-            fprintf(stderr, "  unknown pass through result "
-                    "category (%d)\n", cat);
+    uint32_t rsig = read_uint32(csw, 0);
+    uint32_t rtag = read_uint32(csw, 4);
+    uint32_t residue = read_uint32(csw, 8);
+#define USB_CSW_SIGNATURE 0x53425355  // 'U' 'S' 'B' 'S' (reversed)
+    if (rsig != USB_CSW_SIGNATURE) {
+        WLOG("status signature was invalid: %#x\n", rsig);
+        return -1;
     }
+    DLOG("residue was= %#x\n", residue);
+    *tag = rtag;
+    uint8_t rstatus = csw[12];
+    DLOG("rstatus = %x\n", rstatus);
+    return rstatus;
 }
-#endif
 
 static int dump_CDB_command(uint8_t *cdb, uint8_t cdb_len) {
     char dbugblah[100];
@@ -295,45 +238,6 @@ int send_usb_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint
 }
 
 
-static int get_usb_mass_storage_status(libusb_device_handle *handle, uint8_t endpoint, uint32_t *tag)
-{
-    unsigned char csw[13];
-    memset(csw, 0, sizeof(csw));
-    int transferred;
-    int ret;
-    int try = 0;
-    do {
-        ret = libusb_bulk_transfer(handle, endpoint, (unsigned char *)&csw, sizeof(csw),
-                                   &transferred, SG_TIMEOUT_MSEC);
-        if (ret == LIBUSB_ERROR_PIPE) {
-            libusb_clear_halt(handle, endpoint);
-        }
-        try++;
-    } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
-    if (ret != LIBUSB_SUCCESS) {
-        fprintf(stderr, "%s: receiving failed: %d\n", __func__, ret);
-        return -1;
-    }
-    if (transferred != sizeof(csw)) {
-        fprintf(stderr, "%s: received unexpected amount: %d\n", __func__, transferred);
-        return -1;
-    }
-
-    uint32_t rsig = read_uint32(csw, 0);
-    uint32_t rtag = read_uint32(csw, 4);
-    uint32_t residue = read_uint32(csw, 8);
-#define USB_CSW_SIGNATURE 0x53425355  // 'U' 'S' 'B' 'S' (reversed)
-    if (rsig != USB_CSW_SIGNATURE) {
-        WLOG("status signature was invalid: %#x\n", rsig);
-        return -1;
-    }
-    DLOG("residue was= %#x\n", residue);
-    *tag = rtag;
-    uint8_t rstatus = csw[12];
-    DLOG("rstatus = %x\n", rstatus);
-    return rstatus;
-}
-
 /**
  * Straight from stm8 stlink code...
  * @param handle
@@ -388,6 +292,143 @@ get_sense(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_ou
     }
 }
 
+
+//TODO rewrite/cleanup, save the error in sl
+
+#if FINISHED_WITH_SG
+static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
+    struct stlink_libsg *sl = stl->backend_data;
+    const int e = sl->do_scsi_pt_err;
+    if (e < 0) {
+        fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
+                safe_strerror(-e));
+        return;
+    } else if (e == SCSI_PT_DO_BAD_PARAMS) {
+        fprintf(stderr, "scsi_pt error: bad pass through setup\n");
+        return;
+    } else if (e == SCSI_PT_DO_TIMEOUT) {
+        fprintf(stderr, "  pass through timeout\n");
+        return;
+    }
+    const int duration = get_scsi_pt_duration_ms(ptvp);
+    if ((stl->verbose > 1) && (duration >= 0))
+        DLOG("      duration=%d ms\n", duration);
+
+    // XXX stlink fw sends broken residue, so ignore it and use the known q_len
+    // "usb-storage quirks=483:3744:r"
+    // forces residue to be ignored and calculated, but this causes aboard if
+    // data_len = 0 and by some other data_len values.
+
+    const int resid = get_scsi_pt_resid(ptvp);
+    const int dsize = stl->q_len - resid;
+
+    const int cat = get_scsi_pt_result_category(ptvp);
+    char buf[512];
+    unsigned int slen;
+
+    switch (cat) {
+        case SCSI_PT_RESULT_GOOD:
+            if (stl->verbose && (resid > 0))
+                DLOG("      notice: requested %d bytes but "
+                    "got %d bytes, ignore [broken] residue = %d\n",
+                    stl->q_len, dsize, resid);
+            break;
+        case SCSI_PT_RESULT_STATUS:
+            if (stl->verbose) {
+                sg_get_scsi_status_str(
+                        get_scsi_pt_status_response(ptvp), sizeof (buf),
+                        buf);
+                DLOG("  scsi status: %s\n", buf);
+            }
+            return;
+        case SCSI_PT_RESULT_SENSE:
+            slen = get_scsi_pt_sense_len(ptvp);
+            if (stl->verbose) {
+                sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
+                        > 1), sizeof (buf), buf);
+                DLOG("%s", buf);
+            }
+            if (stl->verbose && (resid > 0)) {
+                if ((stl->verbose) || (stl->q_len > 0))
+                    DLOG("    requested %d bytes but "
+                        "got %d bytes\n", stl->q_len, dsize);
+            }
+            return;
+        case SCSI_PT_RESULT_TRANSPORT_ERR:
+            if (stl->verbose) {
+                get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
+                // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
+                // These codes potentially come from the firmware on a host adapter
+                // or from one of several hosts that an adapter driver controls.
+                // The 'host_status' field has the following values:
+                //     [0x07] Internal error detected in the host adapter.
+                // This may not be fatal (and the command may have succeeded).
+                DLOG("  transport: %s", buf);
+            }
+            return;
+        case SCSI_PT_RESULT_OS_ERR:
+            if (stl->verbose) {
+                get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
+                DLOG("  os: %s", buf);
+            }
+            return;
+        default:
+            fprintf(stderr, "  unknown pass through result "
+                    "category (%d)\n", cat);
+    }
+}
+#endif
+
+/**
+ * Just send a buffer on an endpoint, no questions asked.
+ * Handles repeats, and time outs.  Also handles reading status reports and sense
+ * @param handle libusb device *
+ * @param endpoint_out sends 
+ * @param endpoint_in used to read status reports back in 
+ * @param cbuf  what to send
+ * @param length how much to send
+ * @return number of bytes actually sent, or -1 for failures.
+ */
+int send_usb_data_only(libusb_device_handle *handle, unsigned char endpoint_out,
+    unsigned char endpoint_in, unsigned char *cbuf, unsigned int length) {
+    int ret;
+    int real_transferred;
+    int try;
+    do {
+        DLOG("attempting tx...\n");
+        ret = libusb_bulk_transfer(handle, endpoint_out, cbuf, length,
+                                   &real_transferred, SG_TIMEOUT_MSEC);
+        if (ret == LIBUSB_ERROR_PIPE) {
+            libusb_clear_halt(handle, endpoint_out);
+        }
+        try++;
+    } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
+    if (ret != LIBUSB_SUCCESS) {
+        WLOG("sending failed: %d\n", ret);
+        return -1;
+    }
+    DLOG("Actually sent: %d\n", real_transferred);
+    
+    // now, swallow up the status, so that things behave nicely...
+    uint32_t received_tag;
+    // -ve is for my errors, 0 is good, +ve is libusb sense status bytes
+    int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
+    if (status < 0) {
+        WLOG("receiving status failed: %d\n", status);
+        return -1;
+    }
+    if (status != 0) {
+        WLOG("receiving status not passed :(: %02x\n", status);
+    }
+    if (status == 1) {
+        get_sense(handle, endpoint_in, endpoint_out);
+        return -1;
+    }
+    
+    return real_transferred;
+}
+
+
 int stlink_q(stlink_t *sl) {
     struct stlink_libsg* sg = sl->backend_data;
     //uint8_t cdb_len = 6;  // FIXME varies!!!
@@ -865,11 +906,10 @@ void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
     write_uint32(sg->cdb_cmd_blk + 2, addr);
     write_uint16(sg->cdb_cmd_blk + 6, len);
 
-    // data_out 0-len
-    sl->q_len = len;
-    sg->q_addr = addr;
-    sg->q_data_dir = Q_DATA_OUT;
-    stlink_q(sl);
+    // this sends the command...
+    send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
+    // This sends the data...
+    send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
     stlink_print_data(sl);
 }
 
@@ -884,11 +924,11 @@ void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     write_uint32(sg->cdb_cmd_blk + 2, addr);
     write_uint16(sg->cdb_cmd_blk + 6, len);
 
-    // data_out 0-0x40-...-len
-    sl->q_len = len;
-    sg->q_addr = addr;
-    sg->q_data_dir = Q_DATA_OUT;
-    stlink_q(sl);
+    // this sends the command...
+    send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
+    // This sends the data...
+    send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
+
     stlink_print_data(sl);
 }
 
@@ -970,6 +1010,7 @@ stlink_backend_t _stlink_sg_backend = {
 static stlink_t* stlink_open(const int verbose) {
     
     stlink_t *sl = malloc(sizeof (stlink_t));
+    memset(sl, 0, sizeof(stlink_t));
     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
     if (sl == NULL || slsg == NULL) {
         WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
@@ -1055,7 +1096,6 @@ static stlink_t* stlink_open(const int verbose) {
 
     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
     slsg->q_addr = 0;
-    clear_buf(sl);
 
     /* flash memory settings */
     sl->flash_base = STM32_FLASH_BASE;
@@ -1119,18 +1159,3 @@ stlink_t* stlink_v1_open(const int verbose) {
     stlink_version(sl);
     return sl;
 }
-
-static void __attribute__((unused)) mark_buf(stlink_t *sl) {
-    clear_buf(sl);
-    sl->q_buf[0] = 0x12;
-    sl->q_buf[1] = 0x34;
-    sl->q_buf[2] = 0x56;
-    sl->q_buf[3] = 0x78;
-    sl->q_buf[4] = 0x90;
-    sl->q_buf[15] = 0x42;
-    sl->q_buf[16] = 0x43;
-    sl->q_buf[63] = 0x42;
-    sl->q_buf[64] = 0x43;
-    sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
-    sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
-}

diff --git a/src/test_sg.c b/src/test_sg.c
index a8a7022c80cb8e9c1aa854919271b84dda125b37..d9e560c2375612ae37608ea8872c0c30281fa807 100644 (file)
--- a/src/test_sg.c
+++ b/src/test_sg.c
@@ -6,7 +6,31 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>
 #include "stlink-common.h"
+#include "uglylogging.h"
+
+#define LOG_TAG __FILE__
+#define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
+#define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
+#define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
+#define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
+
+static void __attribute__((unused)) mark_buf(stlink_t *sl) {
+    memset(sl->q_buf, 0, sizeof(sl->q_buf));
+    sl->q_buf[0] = 0xaa;
+    sl->q_buf[1] = 0xbb;
+    sl->q_buf[2] = 0xcc;
+    sl->q_buf[3] = 0xdd;
+    sl->q_buf[4] = 0x11;
+    sl->q_buf[15] = 0x22;
+    sl->q_buf[16] = 0x33;
+    sl->q_buf[63] = 0x44;
+    sl->q_buf[64] = 0x69;
+    sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
+    sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
+}
+
 
 int main(int argc, char *argv[]) {
        // set scpi lib debug level: 0 for no debug info, 10 for lots
@@ -40,14 +64,14 @@ int main(int argc, char *argv[]) {
        //stlink_force_debug(sl);
        stlink_reset(sl);
        stlink_status(sl);
-#if 0
        // core system control block
        stlink_read_mem32(sl, 0xe000ed00, 4);
-       DD(sl, "cpu id base register: SCB_CPUID = got 0x%08x expect 0x411fc231", read_uint32(sl->q_buf, 0));
+       DLOG("cpu id base register: SCB_CPUID = got 0x%08x expect 0x411fc231\n", read_uint32(sl->q_buf, 0));
        // no MPU
        stlink_read_mem32(sl, 0xe000ed90, 4);
-       DD(sl, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0));
+       DLOG("mpu type register: MPU_TYPER = got 0x%08x expect 0x0\n", read_uint32(sl->q_buf, 0));
 
+#if 0
        stlink_read_mem32(sl, 0xe000edf0, 4);
        DD(sl, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0));
 
@@ -105,10 +129,10 @@ int main(int argc, char *argv[]) {
 #if 0
        // sram 0x20000000 8kB
        fputs("\n++++++++++ read/write 8bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
-       clear_buf(sl);
+    memset(sl->q_buf, 0, sizeof(sl->q_buf));
+    mark_buf(sl);
        stlink_write_mem8(sl, 0x20000000, 16);
 
-       mark_buf(sl);
        stlink_write_mem8(sl, 0x20000000, 1);
        stlink_write_mem8(sl, 0x20000001, 1);
        stlink_write_mem8(sl, 0x2000000b, 3);
@@ -117,9 +141,7 @@ int main(int argc, char *argv[]) {
 #if 0
        // a not aligned mem32 access doesn't work indeed
        fputs("\n++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
-       clear_buf(sl);
-       stlink_write_mem8(sl, 0x20000000, 32);
-
+    memset(sl->q_buf, 0, sizeof(sl->q_buf));
        mark_buf(sl);
        stlink_write_mem32(sl, 0x20000000, 1);
        stlink_read_mem32(sl, 0x20000000, 16);
@@ -134,9 +156,10 @@ int main(int argc, char *argv[]) {
        stlink_write_mem32(sl, 0x20000000, 17);
        stlink_read_mem32(sl, 0x20000000, 32);
 #endif
-#if 0
+#if 1
        // sram 0x20000000 8kB
        fputs("++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++\n", stderr);
+    memset(sl->q_buf, 0, sizeof(sl->q_buf));
        mark_buf(sl);
        stlink_write_mem8(sl, 0x20000000, 64);
        stlink_read_mem32(sl, 0x20000000, 64);