int ugly_init_named(int maximum_threshold, const char *name) { ugly_init(maximum_threshold); if (use_syslog) { openlog(name, LOG_PID, LOG_USER); } return 0; }
stlink_t* stlink_v1_open_inner(const int verbose) { ugly_init(verbose); stlink_t *sl = stlink_open(verbose); if (sl == NULL) { fputs("Error: could not open stlink device\n", stderr); return NULL; } stlink_version(sl); if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) { ugly_log(UERROR, LOG_TAG, "WTF? successfully opened, but unable to read version details. BROKEN!\n"); return NULL; } DLOG("Reading current mode...\n"); switch (stlink_current_mode(sl)) { case STLINK_DEV_MASS_MODE: return sl; case STLINK_DEV_DEBUG_MODE: // TODO go to mass? return sl; default: ILOG("Current mode unusable, trying to get back to a useful state...\n"); break; } DLOG("Attempting to exit DFU mode\n"); _stlink_sg_exit_dfu_mode(sl); // re-query device info (and retest) stlink_version(sl); if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) { ugly_log(UERROR, LOG_TAG, "WTF? successfully opened, but unable to read version details. BROKEN!\n"); return NULL; } return sl; }
/* * Do mad wild shit that makes us metric boatloads of cash money */ int main(int argc, char** argv) { ugly_init(99); if (argc < 2) { DLOG("(pass a mq host as argument, otherwise we use localhost)\n"); setup_mq("localhost"); } else { DLOG("Connecting to %s\n", argv[1]); setup_mq(argv[1]); } if (argc < 3) { key = "karl_loves_you"; DLOG("using default shared key of %s\n", key); } else { key = argv[2]; DLOG("using supplied shared key of %s\n", key); } while (1) { run_tasks(); } return (EXIT_SUCCESS); }
stlink_t* stlink_open_usb(const int verbose) { stlink_t* sl = NULL; struct stlink_libusb* slu = NULL; int error = -1; libusb_device** devs = NULL; libusb_device* dev; ssize_t i; ssize_t count; int config; char *iSerial = NULL; sl = malloc(sizeof (stlink_t)); slu = malloc(sizeof (struct stlink_libusb)); if (sl == NULL) goto on_error; if (slu == NULL) goto on_error; memset(sl, 0, sizeof (stlink_t)); memset(slu, 0, sizeof (struct stlink_libusb)); ugly_init(verbose); sl->backend = &_stlink_usb_backend; sl->backend_data = slu; sl->core_stat = STLINK_CORE_STAT_UNKNOWN; /* flash memory settings */ sl->flash_base = STM32_FLASH_BASE; sl->flash_size = STM32_FLASH_SIZE; sl->flash_pgsz = STM32_FLASH_PGSZ; /* system memory */ sl->sys_base = STM32_SYSTEM_BASE; sl->sys_size = STM32_SYSTEM_SIZE; /* sram memory settings */ sl->sram_base = STM32_SRAM_BASE; sl->sram_size = STM32L_SRAM_SIZE; if (libusb_init(&(slu->libusb_ctx))) { WLOG("failed to init libusb context, wrong version of libraries?\n"); goto on_error; } slu->usb_handle = libusb_open_device_with_vid_pid(slu->libusb_ctx, USB_ST_VID, USB_STLINK_32L_PID); if (slu->usb_handle == NULL) { // TODO - free usb context too... free(slu); WLOG("Couldn't find any ST-Link/V2 devices"); return NULL; } if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) { int r; r = libusb_detach_kernel_driver(slu->usb_handle, 0); if (r<0) { WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r)); goto on_libusb_error; } } if (libusb_get_configuration(slu->usb_handle, &config)) { /* this may fail for a previous configured device */ WLOG("libusb_get_configuration()\n"); goto on_libusb_error; } if (config != 1) { printf("setting new configuration (%d -> 1)\n", config); if (libusb_set_configuration(slu->usb_handle, 1)) { /* this may fail for a previous configured device */ WLOG("libusb_set_configuration() failed\n"); goto on_libusb_error; } } if (libusb_claim_interface(slu->usb_handle, 0)) { WLOG("libusb_claim_interface() failed\n"); goto on_libusb_error; } slu->req_trans = libusb_alloc_transfer(0); if (slu->req_trans == NULL) { WLOG("libusb_alloc_transfer failed\n"); goto on_libusb_error; } slu->rep_trans = libusb_alloc_transfer(0); if (slu->rep_trans == NULL) { WLOG("libusb_alloc_transfer failed\n"); goto on_libusb_error; } // TODO - could use the scanning techniq from stm8 code here... slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN; slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT; slu->sg_transfer_idx = 0; // TODO - never used at the moment, always CMD_SIZE slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE; /* success */ if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) { ILOG("-- exit_dfu_mode\n"); stlink_exit_dfu_mode(sl); } stlink_version(sl); error = 0; on_libusb_error: if (devs != NULL) { libusb_free_device_list(devs, 1); } if (error == -1) { stlink_close(sl); return NULL; } /* success */ return sl; on_error: if (sl != NULL) free(sl); if (slu != NULL) free(slu); 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; DLOG("sending length set to: %d\n", sending_length); // send.... do { DLOG("attempting tx...\n"); 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; } DLOG("Actually sent: %d, returning tag: %d\n", real_transferred, tag); 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"); return; } unsigned char sense[REQUEST_SENSE_LENGTH]; int transferred; int ret; int try = 0; do { ret = libusb_bulk_transfer(handle, endpoint_in, sense, sizeof(sense), &transferred, SG_TIMEOUT_MSEC); if (ret == LIBUSB_ERROR_PIPE) { libusb_clear_halt(handle, endpoint_in); } try++; } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3)); if (ret != LIBUSB_SUCCESS) { WLOG("receiving sense failed: %d\n", ret); return; } if (transferred != sizeof(sense)) { WLOG("received unexpected amount of sense: %d != %d\n", transferred, sizeof(sense)); } uint32_t received_tag; int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag); if (status != 0) { WLOG("receiving sense failed with status: %02x\n", status); return; } if (sense[0] != 0x70 && sense[0] != 0x71) { WLOG("No sense data\n"); } else { WLOG("Sense KCQ: %02X %02X %02X\n", sense[2] & 0x0f, sense[12], sense[13]); } } //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!!! uint8_t cdb_len = 10; // FIXME varies!!! uint8_t lun = 0; // always zero... uint32_t tag = send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, cdb_len, lun, LIBUSB_ENDPOINT_IN, sl->q_len); // now wait for our response... // length copied from stlink-usb... int rx_length = sl->q_len; int try = 0; int real_transferred; int ret; if (rx_length > 0) { do { DLOG("attempting rx\n"); ret = libusb_bulk_transfer(sg->usb_handle, sg->ep_rep, sl->q_buf, rx_length, &real_transferred, SG_TIMEOUT_MSEC); if (ret == LIBUSB_ERROR_PIPE) { libusb_clear_halt(sg->usb_handle, sg->ep_req); } try++; } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3)); if (ret != LIBUSB_SUCCESS) { WLOG("Receiving failed: %d\n", ret); return -1; } if (real_transferred != rx_length) { WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length); } } 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(sg->usb_handle, sg->ep_rep, &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(sg->usb_handle, sg->ep_rep, sg->ep_req); return -1; } if (received_tag != tag) { WLOG("received tag %d but expected %d\n", received_tag, tag); //return -1; } if (rx_length > 0 && real_transferred != rx_length) { return -1; } return 0; DLOG("Actually received: %d\n", real_transferred); #if FINISHED_WITH_SG // Get control command descriptor of scsi structure, // (one object per command!!) struct sg_pt_base *ptvp = construct_scsi_pt_obj(); if (NULL == ptvp) { fprintf(stderr, "construct_scsi_pt_obj: out of memory\n"); return; } set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk)); // set buffer for sense (error information) data set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf)); // Set a buffer to be used for data transferred from device if (sg->q_data_dir == Q_DATA_IN) { //clear_buf(sl); set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len); } else { set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len); } // Executes SCSI command (or at least forwards it to lower layers). sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC, sl->verbose); // check for scsi errors 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 void stlink_stat(stlink_t *stl, char *txt) { if (stl->q_len <= 0) return; stlink_print_data(stl); switch (stl->q_buf[0]) { case STLINK_OK: DLOG(" %s: ok\n", txt); return; case STLINK_FALSE: DLOG(" %s: false\n", txt); return; default: DLOG(" %s: unknown\n", txt); } } void _stlink_sg_version(stlink_t *stl) { struct stlink_libsg *sl = stl->backend_data; 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); // HACK use my own private version right now... } // Get stlink mode: // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE // usb dfu || usb mass || jtag or swd int _stlink_sg_current_mode(stlink_t *stl) { struct stlink_libsg *sl = stl->backend_data; clear_cdb(sl); sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE; stl->q_len = 2; sl->q_addr = 0; stlink_q(stl); return stl->q_buf[0]; } // Exit the mass mode and enter the swd debug mode. void _stlink_sg_enter_swd_mode(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER; sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD; sl->q_len = 0; // >0 -> aboard stlink_q(sl); } // Exit the mass mode and enter the jtag debug mode. // (jtag is disabled in the discovery's stlink firmware) void _stlink_sg_enter_jtag_mode(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; 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; sl->q_len = 0; stlink_q(sl); } // XXX kernel driver performs reset, the device temporally disappears void _stlink_sg_exit_dfu_mode(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; 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; sl->q_len = 0; // ?? stlink_q(sl); /* [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00 [135121.844584] end_request: I/O error, dev sdb, sector 4096 [135121.844590] Buffer I/O error on device sdb, logical block 512 [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7 [135130.274551] usb 6-1: device firmware changed [135130.274618] usb 6-1: USB disconnect, address 7 [135130.275186] VFS: busy inodes on changed media or resized disk sdb [135130.275424] VFS: busy inodes on changed media or resized disk sdb [135130.286758] VFS: busy inodes on changed media or resized disk sdb [135130.292796] VFS: busy inodes on changed media or resized disk sdb [135130.301481] VFS: busy inodes on changed media or resized disk sdb [135130.304316] VFS: busy inodes on changed media or resized disk sdb [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8 [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1 [135130.629492] scsi20 : usb-storage 6-1:1.0 [135131.625600] scsi 20:0:0:0: Direct-Access STM32 PQ: 0 ANSI: 0 [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0 [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB) [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through [135131.640609] sdb: [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current] [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00 [135131.671570] end_request: I/O error, dev sdb, sector 63872 [135131.671575] Buffer I/O error on device sdb, logical block 7984 [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current] [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00 [135131.678551] end_request: I/O error, dev sdb, sector 63872 ... [135131.853565] end_request: I/O error, dev sdb, sector 4096 */ } void _stlink_sg_core_id(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID; sl->q_len = 4; sg->q_addr = 0; stlink_q(sl); sl->core_id = read_uint32(sl->q_buf, 0); } // Arm-core reset -> halted state. void _stlink_sg_reset(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS; sl->q_len = 2; sg->q_addr = 0; stlink_q(sl); stlink_stat(sl, "core reset"); } // Arm-core status: halted or running. void _stlink_sg_status(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; DLOG("\n*** stlink_status ***\n"); clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS; sl->q_len = 2; sg->q_addr = 0; stlink_q(sl); } // Force the core into the debug mode -> halted state. void _stlink_sg_force_debug(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; DLOG("\n*** stlink_force_debug ***\n"); clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG; sl->q_len = 2; sg->q_addr = 0; stlink_q(sl); stlink_stat(sl, "force debug"); } // Read all arm-core registers. void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS; sl->q_len = 84; sg->q_addr = 0; stlink_q(sl); stlink_print_data(sl); // TODO - most of this should be re-extracted up.... // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83 // r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 for (int i = 0; i < 16; i++) { regp->r[i] = read_uint32(sl->q_buf, 4 * i); if (sl->verbose > 1) DLOG("r%2d = 0x%08x\n", i, regp->r[i]); } 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; DLOG("xpsr = 0x%08x\n", regp->xpsr); DLOG("main_sp = 0x%08x\n", regp->main_sp); DLOG("process_sp = 0x%08x\n", regp->process_sp); DLOG("rw = 0x%08x\n", regp->rw); DLOG("rw2 = 0x%08x\n", regp->rw2); } // Read an arm-core register, the index must be in the range 0..20. // 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20 // r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG; sg->cdb_cmd_blk[2] = r_idx; sl->q_len = 4; sg->q_addr = 0; stlink_q(sl); // 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20 // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83 // r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 stlink_print_data(sl); uint32_t r = read_uint32(sl->q_buf, 0); DLOG("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; } } // Write an arm-core register. Index: // 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20 // r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG; // 2: reg index // 3-6: reg content sg->cdb_cmd_blk[2] = idx; write_uint32(sg->cdb_cmd_blk + 3, reg); sl->q_len = 2; sg->q_addr = 0; stlink_q(sl); stlink_stat(sl, "write reg"); } // Write a register of the debug module of the core. // XXX ?(atomic writes) // TODO test void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) { struct stlink_libsg *sg = sl->backend_data; 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 // 6-9: reg content write_uint32(sg->cdb_cmd_blk + 2, addr); write_uint32(sg->cdb_cmd_blk + 6, reg); sl->q_len = 2; sg->q_addr = addr; stlink_q(sl); stlink_stat(sl, "write debug reg"); } // Force the core exit the debug mode. void _stlink_sg_run(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; DLOG("\n*** stlink_run ***\n"); clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE; sl->q_len = 2; sg->q_addr = 0; stlink_q(sl); stlink_stat(sl, "run core"); } // Step the arm-core. void _stlink_sg_step(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE; sl->q_len = 2; sg->q_addr = 0; stlink_q(sl); stlink_stat(sl, "step core"); } // TODO test // 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) { 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; // 2:The number of the flash patch used to set the breakpoint // 3-6: Address of the breakpoint (LSB) // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00) sl->q_buf[2] = fp_nr; write_uint32(sl->q_buf, addr); sl->q_buf[7] = fp; sl->q_len = 2; stlink_q(sl); stlink_stat(sl, "set flash breakpoint"); } // TODO test // TODO make delegate! void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) { struct stlink_libsg *sg = sl->backend_data; 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; sl->q_len = 2; stlink_q(sl); stlink_stat(sl, "clear flash breakpoint"); } // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes) void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT; // 2-5: addr // 6-7: len write_uint32(sg->cdb_cmd_blk + 2, addr); write_uint16(sg->cdb_cmd_blk + 6, len); // data_in 0-0x40-len // !!! len _and_ q_len must be max 6k, // i.e. >1024 * 6 = 6144 -> aboard) // !!! if len < q_len: 64*k, 1024*n, n=1..5 -> aboard // (broken residue issue) sl->q_len = len; sg->q_addr = addr; stlink_q(sl); stlink_print_data(sl); } // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes. void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT; // 2-5: addr // 6-7: len (>0x40 (64) -> aboard) write_uint32(sg->cdb_cmd_blk + 2, addr); write_uint16(sg->cdb_cmd_blk + 6, len); // 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); } // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes. void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT; // 2-5: addr // 6-7: len "unlimited" write_uint32(sg->cdb_cmd_blk + 2, addr); write_uint16(sg->cdb_cmd_blk + 6, len); // 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); } #if 0 /* not working */ static int write_flash_mem16 (struct stlink* sl, uint32_t addr, uint16_t val) { /* half word writes */ if (addr % 2) return -1; /* unlock if locked */ unlock_flash_if(sl); /* set flash programming chosen bit */ set_flash_cr_pg(sl); write_uint16(sl->q_buf, val); stlink_write_mem16(sl, addr, 2); /* wait for non business */ wait_flash_busy(sl); lock_flash(sl); /* check the programmed value back */ stlink_read_mem16(sl, addr, 2); if (*(const uint16_t*) sl->q_buf != val) { /* values differ at i * sizeof(uint16_t) */ return -1; } /* success */ return 0; } #endif /* not working */ // Exit the jtag or swd mode and enter the mass mode. void _stlink_sg_exit_debug_mode(stlink_t *stl) { if (stl) { struct stlink_libsg* sl = stl->backend_data; clear_cdb(sl); sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT; stl->q_len = 0; // >0 -> aboard stlink_q(stl); } } // 1) open a sg device, switch the stlink from dfu to mass mode // 2) wait 5s until the kernel driver stops reseting the broken device // 3) reopen the device // 4) the device driver is now ready for a switch to jtag/swd mode // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device stlink_backend_t _stlink_sg_backend = { _stlink_sg_close, _stlink_sg_exit_debug_mode, _stlink_sg_enter_swd_mode, _stlink_sg_enter_jtag_mode, _stlink_sg_exit_dfu_mode, _stlink_sg_core_id, _stlink_sg_reset, _stlink_sg_run, _stlink_sg_status, _stlink_sg_version, _stlink_sg_read_mem32, _stlink_sg_write_mem32, _stlink_sg_write_mem8, _stlink_sg_read_all_regs, _stlink_sg_read_reg, _stlink_sg_write_reg, _stlink_sg_step, _stlink_sg_current_mode, _stlink_sg_force_debug }; 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"); 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; sl->core_stat = STLINK_CORE_STAT_UNKNOWN; slsg->q_addr = 0; /* flash memory settings */ sl->flash_base = STM32_FLASH_BASE; sl->flash_size = STM32_FLASH_SIZE; sl->flash_pgsz = STM32_FLASH_PGSZ; /* system memory */ sl->sys_base = STM32_SYSTEM_BASE; sl->sys_size = STM32_SYSTEM_SIZE; /* sram memory settings */ sl->sram_base = STM32_SRAM_BASE; sl->sram_size = STM32_SRAM_SIZE; return sl; } stlink_t* stlink_v1_open(const int verbose) { ugly_init(verbose); stlink_t *sl = stlink_open(verbose); if (sl == NULL) { fputs("Error: could not open stlink device\n", stderr); return NULL; } stlink_version(sl); if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) { ugly_log(UERROR, LOG_TAG, "WTF? successfully opened, but unable to read version details. BROKEN!\n"); return NULL; } DLOG("Reading current mode...\n"); switch (stlink_current_mode(sl)) { case STLINK_DEV_MASS_MODE: return sl; case STLINK_DEV_DEBUG_MODE: // TODO go to mass? return sl; } DLOG("Attempting to exit DFU mode\n"); _stlink_sg_exit_dfu_mode(sl); // exit the dfu mode -> the device is gone DLOG("\n*** reopen the stlink device ***\n"); delay(1000); stlink_close(sl); delay(5000); DLOG("Attempting to reopen the stlink...\n"); sl = stlink_open(verbose); if (sl == NULL) { fputs("Error: could not open stlink device\n", stderr); return NULL; } // re-query device info stlink_version(sl); return sl; }