+static int dump_CDB_command(uint8_t *cdb, uint8_t cdb_len) {
+ char dbugblah[100];
+ char *dbugp = dbugblah;
+ dbugp += sprintf(dbugp, "Sending CDB [");
+ for (uint8_t i = 0; i < cdb_len; i++) {
+ dbugp += sprintf(dbugp, " %#02x", (unsigned int) cdb[i]);
+ }
+ sprintf(dbugp, "]\n");
+ DLOG(dbugblah);
+ return 0;
+}
+
+/**
+ * Wraps a CDB mass storage command in the appropriate gunk to get it down
+ * @param handle
+ * @param endpoint
+ * @param cdb
+ * @param cdb_length
+ * @param lun
+ * @param flags
+ * @param expected_rx_size
+ * @return
+ */
+int send_usb_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint_out,
+ uint8_t *cdb, uint8_t cdb_length,
+ uint8_t lun, uint8_t flags, uint32_t expected_rx_size) {
+ DLOG("Sending usb m-s cmd: cdblen:%d, rxsize=%d\n", cdb_length, expected_rx_size);
+ dump_CDB_command(cdb, cdb_length);
+
+ static uint32_t tag;
+ if (tag == 0) {
+ tag = 1;
+ }
+
+ int try = 0;
+ int ret = 0;
+ int real_transferred;
+ int i = 0;
+
+ uint8_t c_buf[STLINK_SG_SIZE];
+ // tag is allegedly ignored... TODO - verify
+ c_buf[i++] = 'U';
+ c_buf[i++] = 'S';
+ c_buf[i++] = 'B';
+ c_buf[i++] = 'C';
+ write_uint32(&c_buf[i], tag);
+ uint32_t this_tag = tag++;
+ write_uint32(&c_buf[i+4], expected_rx_size);
+ i+= 8;
+ c_buf[i++] = flags;
+ c_buf[i++] = lun;
+
+ c_buf[i++] = cdb_length;
+
+ // Now the actual CDB request
+ assert(cdb_length <= CDB_SL);
+ memcpy(&(c_buf[i]), cdb, cdb_length);
+
+ int sending_length = STLINK_SG_SIZE;
+
+ // send....
+ do {
+ ret = libusb_bulk_transfer(handle, endpoint_out, c_buf, sending_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;
+ }
+ return this_tag;
+}
+
+
+/**
+ * Straight from stm8 stlink code...
+ * @param handle
+ * @param endpoint_in
+ * @param endpoint_out
+ */
+ static void
+get_sense(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
+{
+ DLOG("Fetching sense...\n");
+ uint8_t cdb[16];
+ memset(cdb, 0, sizeof(cdb));
+#define REQUEST_SENSE 0x03
+#define REQUEST_SENSE_LENGTH 18
+ cdb[0] = REQUEST_SENSE;
+ cdb[4] = REQUEST_SENSE_LENGTH;
+ uint32_t tag = send_usb_mass_storage_command(handle, endpoint_out, cdb, sizeof(cdb), 0,
+ LIBUSB_ENDPOINT_IN, REQUEST_SENSE_LENGTH);
+ if (tag == 0) {
+ WLOG("refusing to send request sense with tag 0\n");