#include <sys/stat.h>
#include <sys/mman.h>
-// sgutils2 (apt-get install libsgutils2-dev)
-#include <scsi/sg_lib.h>
-#include <scsi/sg_pt.h>
-
#include "stlink-common.h"
#include "stlink-sg.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)
// Suspends execution of the calling process for
// (at least) ms milliseconds.
// E.g. make the valgrind happy.
static void clear_buf(stlink_t *sl) {
- DD(sl, "*** clear_buf ***\n");
+ DLOG("*** clear_buf ***\n");
for (size_t i = 0; i < sizeof (sl->q_buf); i++)
sl->q_buf[i] = 0;
void _stlink_sg_close(stlink_t *sl) {
if (sl) {
+#if FINISHED_WITH_SG
struct stlink_libsg *slsg = sl->backend_data;
scsi_pt_close_device(slsg->sg_fd);
free(slsg);
+#endif
}
}
//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;
}
const int duration = get_scsi_pt_duration_ms(ptvp);
if ((stl->verbose > 1) && (duration >= 0))
- DD(stl, " duration=%d ms\n", duration);
+ 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"
switch (cat) {
case SCSI_PT_RESULT_GOOD:
if (stl->verbose && (resid > 0))
- DD(stl, " notice: requested %d bytes but "
+ DLOG(" notice: requested %d bytes but "
"got %d bytes, ignore [broken] residue = %d\n",
stl->q_len, dsize, resid);
break;
sg_get_scsi_status_str(
get_scsi_pt_status_response(ptvp), sizeof (buf),
buf);
- DD(stl, " scsi status: %s\n", buf);
+ DLOG(" scsi status: %s\n", buf);
}
return;
case SCSI_PT_RESULT_SENSE:
if (stl->verbose) {
sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
> 1), sizeof (buf), buf);
- DD(stl, "%s", buf);
+ DLOG("%s", buf);
}
if (stl->verbose && (resid > 0)) {
if ((stl->verbose) || (stl->q_len > 0))
- DD(stl, " requested %d bytes but "
+ DLOG(" requested %d bytes but "
"got %d bytes\n", stl->q_len, dsize);
}
return;
// 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).
- DD(stl, " transport: %s", buf);
+ DLOG(" transport: %s", buf);
}
return;
case SCSI_PT_RESULT_OS_ERR:
if (stl->verbose) {
get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
- DD(stl, " os: %s", buf);
+ DLOG(" os: %s", buf);
}
return;
default:
"category (%d)\n", cat);
}
}
+#endif
void stlink_q(stlink_t *sl) {
+#if FINISHED_WITH_SG
struct stlink_libsg* sg = sl->backend_data;
- DD(sl, "CDB[");
+ DLOG("CDB[");
for (int i = 0; i < CDB_SL; i++)
- DD(sl, " 0x%02x", (unsigned int) sg->cdb_cmd_blk[i]);
- DD(sl, "]\n");
+ DLOG(" 0x%02x", (unsigned int) sg->cdb_cmd_blk[i]);
+ DLOG("]\n");
// Get control command descriptor of scsi structure,
// (one object per command!!)
stlink_confirm_inq(sl, ptvp);
// TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
destruct_scsi_pt_obj(ptvp);
+#endif
}
// TODO thinking, cleanup
switch (stl->q_buf[0]) {
case STLINK_OK:
- DD(stl, " %s: ok\n", txt);
+ DLOG(" %s: ok\n", txt);
return;
case STLINK_FALSE:
- DD(stl, " %s: false\n", txt);
+ DLOG(" %s: false\n", txt);
return;
default:
- DD(stl, " %s: unknown\n", txt);
+ DLOG(" %s: unknown\n", txt);
}
}
-static void parse_version(stlink_t *stl) {
- struct stlink_libsg *sl = stl->backend_data;
-
- sl->st_vid = 0;
- sl->stlink_pid = 0;
-
- if (stl->q_len <= 0) {
- fprintf(stderr, "Error: could not parse the stlink version");
- return;
- }
-
- uint32_t b0 = stl->q_buf[0]; //lsb
- uint32_t b1 = stl->q_buf[1];
- uint32_t b2 = stl->q_buf[2];
- uint32_t b3 = stl->q_buf[3];
- uint32_t b4 = stl->q_buf[4];
- uint32_t b5 = stl->q_buf[5]; //msb
-
- // b0 b1 || b2 b3 | b4 b5
- // 4b | 6b | 6b || 2B | 2B
- // stlink_v | jtag_v | swim_v || st_vid | stlink_pid
-
- sl->stlink_v = (b0 & 0xf0) >> 4;
- sl->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
- sl->swim_v = b1 & 0x3f;
- sl->st_vid = (b3 << 8) | b2;
- sl->stlink_pid = (b5 << 8) | b4;
-}
-
void _stlink_sg_version(stlink_t *stl) {
struct stlink_libsg *sl = stl->backend_data;
- D(stl, "\n*** stlink_version ***\n");
+ DLOG("\n*** stlink_version ***\n");
clear_cdb(sl);
sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
stl->q_len = 6;
sl->q_addr = 0;
- stlink_q(stl);
- parse_version(stl);
+// stlink_q(stl);
+ // HACK use my own private version right now...
+
+ int try = 0;
+ int ret = 0;
+ int real_transferred;
+
+/*
+ uint32_t dCBWSignature;
+ uint32_t dCBWTag;
+ uint32_t dCBWDataTransferLength;
+ uint8_t bmCBWFlags;
+ uint8_t bCBWLUN;
+ uint8_t bCBWCBLength;
+ uint8_t CBWCB[16];
+ */
+
+#if using_old_code_examples
+ /*
+ * and from old code?
+ cmd[i++] = 'U';
+ cmd[i++] = 'S';
+ cmd[i++] = 'B';
+ cmd[i++] = 'C';
+ write_uint32(&cmd[i], sg->sg_transfer_idx);
+ write_uint32(&cmd[i + 4], len);
+ i += 8;
+ cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
+ cmd[i++] = 0; /* Logical unit */
+ cmd[i++] = 0xa; /* Command length */
+ */
+#endif
+
+ int i = 0;
+ stl->c_buf[i++] = 'U';
+ stl->c_buf[i++] = 'S';
+ stl->c_buf[i++] = 'B';
+ stl->c_buf[i++] = 'C';
+ // tag is allegedly ignored... TODO - verify
+ write_uint32(&stl->c_buf[i], 1);
+ // TODO - Does this even matter? verify with more commands....
+ uint32_t command_length = STLINK_CMD_SIZE;
+ write_uint32(&stl->c_buf[i+4], command_length);
+ i+= 8;
+ stl->c_buf[i++] = LIBUSB_ENDPOINT_IN;
+ // assumption: lun is always 0;
+ stl->c_buf[i++] = 0;
+
+ stl->c_buf[i++] = sizeof(sl->cdb_cmd_blk);
+
+ // duh, now the actual data...
+ memcpy(&(stl->c_buf[i]), sl->cdb_cmd_blk, sizeof(sl->cdb_cmd_blk));
+
+ int sending_length = STLINK_SG_SIZE;
+ DLOG("sending length set to: %d\n", sending_length);
+
+ // send....
+ do {
+ DLOG("attempting tx...\n");
+ ret = libusb_bulk_transfer(sl->usb_handle, sl->ep_req, stl->c_buf, sending_length,
+ &real_transferred, 3 * 1000);
+ if (ret == LIBUSB_ERROR_PIPE) {
+ libusb_clear_halt(sl->usb_handle, sl->ep_req);
+ }
+ try++;
+ } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
+ if (ret != LIBUSB_SUCCESS) {
+ WLOG("sending failed: %d\n", ret);
+ return;
+ }
+ DLOG("Actually sent: %d\n", real_transferred);
+
+ // now wait for our response...
+ // length copied from stlink-usb...
+ int rx_length = 6;
+ try = 0;
+ do {
+ DLOG("attempting rx\n");
+ ret = libusb_bulk_transfer(sl->usb_handle, sl->ep_rep, stl->q_buf, rx_length,
+ &real_transferred, 3 * 1000);
+ if (ret == LIBUSB_ERROR_PIPE) {
+ libusb_clear_halt(sl->usb_handle, sl->ep_req);
+ }
+ try++;
+ } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
+
+ if (ret != LIBUSB_SUCCESS) {
+ WLOG("Receiving failed: %d\n", ret);
+ return;
+ }
+
+ if (real_transferred != rx_length) {
+ WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length);
+ }
+
+ DLOG("Actually received: %d\n", real_transferred);
}
// Get stlink mode:
void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_enter_jtag_mode ***\n");
+ DLOG("\n*** stlink_enter_jtag_mode ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_exit_dfu_mode ***\n");
+ DLOG("\n*** stlink_exit_dfu_mode ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
void _stlink_sg_status(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_status ***\n");
+ DLOG("\n*** stlink_status ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
sl->q_len = 2;
void _stlink_sg_force_debug(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_force_debug ***\n");
+ DLOG("\n*** stlink_force_debug ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
sl->q_len = 2;
for (int i = 0; i < 16; i++) {
sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
if (sl->verbose > 1)
- DD(sl, "r%2d = 0x%08x\n", i, sg->reg.r[i]);
+ DLOG("r%2d = 0x%08x\n", i, sg->reg.r[i]);
}
sg->reg.xpsr = read_uint32(sl->q_buf, 64);
sg->reg.main_sp = read_uint32(sl->q_buf, 68);
if (sl->verbose < 2)
return;
- DD(sl, "xpsr = 0x%08x\n", sg->reg.xpsr);
- DD(sl, "main_sp = 0x%08x\n", sg->reg.main_sp);
- DD(sl, "process_sp = 0x%08x\n", sg->reg.process_sp);
- DD(sl, "rw = 0x%08x\n", sg->reg.rw);
- DD(sl, "rw2 = 0x%08x\n", sg->reg.rw2);
+ DLOG("xpsr = 0x%08x\n", sg->reg.xpsr);
+ DLOG("main_sp = 0x%08x\n", sg->reg.main_sp);
+ DLOG("process_sp = 0x%08x\n", sg->reg.process_sp);
+ DLOG("rw = 0x%08x\n", sg->reg.rw);
+ DLOG("rw2 = 0x%08x\n", sg->reg.rw2);
}
// Read an arm-core register, the index must be in the range 0..20.
stlink_print_data(sl);
uint32_t r = read_uint32(sl->q_buf, 0);
- DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
+ DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
switch (r_idx) {
case 16:
void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_write_dreg ***\n");
+ DLOG("\n*** stlink_write_dreg ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
// 2-5: address of reg of the debug module
void _stlink_sg_run(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_run ***\n");
+ DLOG("\n*** stlink_run ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
sl->q_len = 2;
// see Cortex-M3 Technical Reference Manual
// TODO make delegate!
void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
- D(sl, "\n*** stlink_set_hw_bp ***\n");
+ DLOG("\n*** stlink_set_hw_bp ***\n");
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
// TODO make delegate!
void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
struct stlink_libsg *sg = sl->backend_data;
- D(sl, "\n*** stlink_clr_hw_bp ***\n");
+ DLOG("\n*** stlink_clr_hw_bp ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
sg->cdb_cmd_blk[2] = fp_nr;
_stlink_sg_force_debug
};
-stlink_t* stlink_open(const char *dev_name, const int verbose) {
- fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
- int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
- if (sg_fd < 0) {
- fprintf(stderr, "error opening device: %s: %s\n", dev_name,
- safe_strerror(-sg_fd));
- return NULL;
- }
-
+static stlink_t* stlink_open(const char *dev_name, const int verbose) {
+ DLOG("*** stlink_open [%s] ***\n", dev_name);
+
stlink_t *sl = malloc(sizeof (stlink_t));
struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
if (sl == NULL || slsg == NULL) {
- fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
+ WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
+ return NULL;
+ }
+
+ if (libusb_init(&(slsg->libusb_ctx))) {
+ WLOG("failed to init libusb context, wrong version of libraries?\n");
+ free(sl);
+ free(slsg);
+ return NULL;
+ }
+
+ libusb_set_debug(slsg->libusb_ctx, 3);
+
+ slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
+ if (slsg->usb_handle == NULL) {
+ WLOG("Failed to find an stlink v1 by VID:PID\n");
+ libusb_close(slsg->usb_handle);
+ free(sl);
+ free(slsg);
+ return NULL;
+ }
+
+ // TODO
+ // Could read the interface config descriptor, and assert lots of the assumptions
+
+ // assumption: numInterfaces is always 1...
+ if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
+ int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
+ if (r < 0) {
+ WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
+ libusb_close(slsg->usb_handle);
+ free(sl);
+ free(slsg);
+ return NULL;
+ }
+ DLOG("Kernel driver was successfully detached\n");
+ }
+
+ int config;
+ if (libusb_get_configuration(slsg->usb_handle, &config)) {
+ /* this may fail for a previous configured device */
+ WLOG("libusb_get_configuration()\n");
+ libusb_close(slsg->usb_handle);
+ free(sl);
+ free(slsg);
+ return NULL;
+
+ }
+
+ // assumption: bConfigurationValue is always 1
+ if (config != 1) {
+ WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
+ DLOG("setting new configuration (%d -> 1)\n", config);
+ if (libusb_set_configuration(slsg->usb_handle, 1)) {
+ /* this may fail for a previous configured device */
+ WLOG("libusb_set_configuration() failed\n");
+ libusb_close(slsg->usb_handle);
+ free(sl);
+ free(slsg);
+ return NULL;
+ }
+ }
+
+ if (libusb_claim_interface(slsg->usb_handle, 0)) {
+ WLOG("libusb_claim_interface() failed\n");
+ libusb_close(slsg->usb_handle);
+ free(sl);
+ free(slsg);
return NULL;
}
+
+ // assumption: endpoint config is fixed mang. really.
+ slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
+ slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
+
+ DLOG("Successfully opened stlinkv1 by libusb :)\n");
+
sl->verbose = verbose;
sl->backend_data = slsg;
sl->backend = &_stlink_sg_backend;
- slsg->sg_fd = sg_fd;
sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
- slsg->core_id = 0;
slsg->q_addr = 0;
clear_buf(sl);
-stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
-
+stlink_t* stlink_v1_open(const char *dev_name, const int verbose) {
+ ugly_init(verbose);
stlink_t *sl = stlink_open(dev_name, verbose);
if (sl == NULL) {
fputs("Error: could not open stlink device\n", stderr);
stlink_version(sl);
struct stlink_libsg *sg = sl->backend_data;
- if ((sg->st_vid != USB_ST_VID) || (sg->stlink_pid != USB_STLINK_PID)) {
+ if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
fprintf(stderr, "Error: the device %s is not a stlink\n",
dev_name);
fprintf(stderr, " VID: got %04x expect %04x \n",
- sg->st_vid, USB_ST_VID);
+ sl->version.st_vid, USB_ST_VID);
fprintf(stderr, " PID: got %04x expect %04x \n",
- sg->stlink_pid, USB_STLINK_PID);
+ sl->version.stlink_pid, USB_STLINK_PID);
return NULL;
}
- D(sl, "\n*** stlink_force_open ***\n");
+ DLOG("\n*** stlink_force_open ***\n");
switch (stlink_current_mode(sl)) {
case STLINK_DEV_MASS_MODE:
return sl;
// TODO go to mass?
return sl;
}
- DD(sl, "\n*** switch the stlink to mass mode ***\n");
+ DLOG("\n*** switch the stlink to mass mode ***\n");
_stlink_sg_exit_dfu_mode(sl);
// exit the dfu mode -> the device is gone
- DD(sl, "\n*** reopen the stlink device ***\n");
+ DLOG("\n*** reopen the stlink device ***\n");
delay(1000);
stlink_close(sl);
delay(5000);