diff --git a/include/stlink.h b/include/stlink.h index db737acb0..05618e35f 100644 --- a/include/stlink.h +++ b/include/stlink.h @@ -24,10 +24,17 @@ extern "C" { //#define Q_BUF_LEN 96 #define Q_BUF_LEN (1024 * 100) - // STLINK_DEBUG_RESETSYS, etc: +// STLINK_DEBUG_RESETSYS, etc: +enum target_state { + TARGET_UNKNOWN = 0, + TARGET_RUNNING = 1, + TARGET_HALTED = 2, + TARGET_RESET = 3, + TARGET_DEBUG_RUNNING = 4, +}; + #define STLINK_CORE_RUNNING 0x80 #define STLINK_CORE_HALTED 0x81 -#define STLINK_CORE_STAT_UNKNOWN -1 #define STLINK_GET_VERSION 0xf1 #define STLINK_GET_CURRENT_MODE 0xf5 @@ -75,8 +82,42 @@ extern "C" { #define STLINK_V3_MAX_FREQ_NB 10 +/* Cortex Debug Control Block */ +#define DCB_DHCSR 0xE000EDF0 +#define DCB_DCRSR 0xE000EDF4 +#define DCB_DCRDR 0xE000EDF8 +#define DCB_DEMCR 0xE000EDFC + +/* DCB_DHCSR bit and field definitions */ +#define DBGKEY (0xA05F << 16) +#define C_DEBUGEN (1 << 0) +#define C_HALT (1 << 1) +#define C_STEP (1 << 2) +#define C_MASKINTS (1 << 3) +#define S_REGRDY (1 << 16) +#define S_HALT (1 << 17) +#define S_SLEEP (1 << 18) +#define S_LOCKUP (1 << 19) +#define S_RETIRE_ST (1 << 24) +#define S_RESET_ST (1 << 25) + + +/* + * Map the relevant features, quirks and workaround for specific firmware + * version of stlink + */ +#define STLINK_F_HAS_TRACE (1<<0) +#define STLINK_F_HAS_SWD_SET_FREQ (1<<1) +#define STLINK_F_HAS_JTAG_SET_FREQ (1<<2) +#define STLINK_F_HAS_MEM_16BIT (1<<3) +#define STLINK_F_HAS_GETLASTRWSTATUS2 (1<<4) +#define STLINK_F_HAS_DAP_REG (1<<5) +#define STLINK_F_QUIRK_JTAG_DP_READ (1<<6) +#define STLINK_F_HAS_AP_INIT (1<<7) +#define STLINK_F_HAS_DPBANKSEL (1<<8) +#define STLINK_F_HAS_RW8_512BYTES (1<<9) + - /* Enough space to hold both a V2 command or a V1 command packaged as generic scsi*/ #define C_BUF_LEN 32 enum stlink_flash_type { @@ -120,12 +161,22 @@ typedef struct flash_loader { uint8_t revision; } cortex_m3_cpuid_t; + enum stlink_jtag_api_version { + STLINK_JTAG_API_V1 = 1, + STLINK_JTAG_API_V2, + STLINK_JTAG_API_V3, + }; + typedef struct stlink_version_ { uint32_t stlink_v; uint32_t jtag_v; uint32_t swim_v; uint32_t st_vid; uint32_t stlink_pid; + /** jtag api version supported */ + enum stlink_jtag_api_version jtag_api; + /** one bit for each feature supported. See macros STLINK_F_* */ + uint32_t flags; } stlink_version_t; enum transport_type { @@ -154,7 +205,7 @@ typedef struct flash_loader { int opt; uint32_t core_id; // set by stlink_core_id(), result from STLINK_DEBUGREADCOREID uint32_t chip_id; // set by stlink_load_device_params(), used to identify flash and sram - int core_stat; // set by stlink_status(), values STLINK_CORE_xxxxx + enum target_state core_stat; // set by stlink_status() char serial[STLINK_SERIAL_MAX_SIZE]; int serial_size; diff --git a/include/stlink/commands.h b/include/stlink/commands.h index acf664cb1..a6830ff26 100644 --- a/include/stlink/commands.h +++ b/include/stlink/commands.h @@ -2,26 +2,35 @@ #define STLINK_COMMANDS_H_ enum stlink_debug_commands { - STLINK_DEBUG_ENTER_JTAG = 0x00, - STLINK_DEBUG_GETSTATUS = 0x01, - STLINK_DEBUG_FORCEDEBUG = 0x02, - STLINK_DEBUG_RESETSYS = 0x03, - STLINK_DEBUG_READALLREGS = 0x04, - STLINK_DEBUG_READREG = 0x05, - STLINK_DEBUG_WRITEREG = 0x06, - STLINK_DEBUG_READMEM_32BIT = 0x07, - STLINK_DEBUG_WRITEMEM_32BIT = 0x08, - STLINK_DEBUG_RUNCORE = 0x09, - STLINK_DEBUG_STEPCORE = 0x0a, - STLINK_DEBUG_SETFP = 0x0b, - STLINK_DEBUG_WRITEMEM_8BIT = 0x0d, - STLINK_DEBUG_CLEARFP = 0x0e, - STLINK_DEBUG_WRITEDEBUGREG = 0x0f, - STLINK_DEBUG_ENTER = 0x20, - STLINK_DEBUG_EXIT = 0x21, - STLINK_DEBUG_READCOREID = 0x22, - STLINK_DEBUG_APIV2_ENTER = 0x30, - STLINK_DEBUG_ENTER_SWD = 0xa3 + STLINK_DEBUG_ENTER_JTAG = 0x00, + STLINK_DEBUG_GETSTATUS = 0x01, + STLINK_DEBUG_FORCEDEBUG = 0x02, + STLINK_DEBUG_APIV1_RESETSYS = 0x03, + STLINK_DEBUG_APIV1_READALLREGS = 0x04, + STLINK_DEBUG_APIV1_READREG = 0x05, + STLINK_DEBUG_APIV1_WRITEREG = 0x06, + STLINK_DEBUG_READMEM_32BIT = 0x07, + STLINK_DEBUG_WRITEMEM_32BIT = 0x08, + STLINK_DEBUG_RUNCORE = 0x09, + STLINK_DEBUG_STEPCORE = 0x0a, + STLINK_DEBUG_APIV1_SETFP = 0x0b, + STLINK_DEBUG_WRITEMEM_8BIT = 0x0d, + STLINK_DEBUG_APIV1_CLEARFP = 0x0e, + STLINK_DEBUG_APIV1_WRITEDEBUGREG = 0x0f, + STLINK_DEBUG_APIV1_ENTER = 0x20, + STLINK_DEBUG_EXIT = 0x21, + STLINK_DEBUG_READCOREID = 0x22, + STLINK_DEBUG_APIV2_ENTER = 0x30, + STLINK_DEBUG_APIV2_READ_IDCODES = 0x31, + STLINK_DEBUG_APIV2_RESETSYS = 0x32, + STLINK_DEBUG_APIV2_READREG = 0x33, + STLINK_DEBUG_APIV2_WRITEREG = 0x34, + STLINK_DEBUG_APIV2_WRITEDEBUGREG = 0x35, + STLINK_DEBUG_APIV2_READDEBUGREG = 0x36, + STLINK_DEBUG_APIV2_READALLREGS = 0x3A, + STLINK_DEBUG_APIV2_GETLASTRWSTATUS = 0x3B, + STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 = 0x3E, + STLINK_DEBUG_ENTER_SWD = 0xa3 }; #endif /* STLINK_COMMANDS_H_ */ diff --git a/src/common.c b/src/common.c index 0dc6bc967..32c892420 100644 --- a/src/common.c +++ b/src/common.c @@ -993,6 +993,7 @@ int stlink_status(stlink_t *sl) { * @param slv output parsed version object */ void _parse_version(stlink_t *sl, stlink_version_t *slv) { + sl->version.flags = 0; if (sl->version.stlink_v < 3) { uint32_t b0 = sl->q_buf[0]; //lsb uint32_t b1 = sl->q_buf[1]; @@ -1010,6 +1011,15 @@ void _parse_version(stlink_t *sl, stlink_version_t *slv) { slv->swim_v = b1 & 0x3f; slv->st_vid = (b3 << 8) | b2; slv->stlink_pid = (b5 << 8) | b4; + /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */ + if (slv->stlink_v == 1) + slv->jtag_api = slv->jtag_v > 11? STLINK_JTAG_API_V2 : STLINK_JTAG_API_V1; + else { + slv->jtag_api = STLINK_JTAG_API_V2; + /* preferred API to get last R/W status from J15 */ + if (sl->version.jtag_v >= 15) + sl->version.flags |= STLINK_F_HAS_GETLASTRWSTATUS2; + } } else { // V3 uses different version format, for reference see OpenOCD source // (that was written from docs available from ST under NDA): @@ -1019,6 +1029,9 @@ void _parse_version(stlink_t *sl, stlink_version_t *slv) { slv->jtag_v = sl->q_buf[2]; slv->st_vid = (uint32_t)((sl->q_buf[9] << 8) | sl->q_buf[8]); slv->stlink_pid = (uint32_t)((sl->q_buf[11] << 8) | sl->q_buf[10]); + slv->jtag_api = STLINK_JTAG_API_V3; + /* preferred API to get last R/W status */ + sl->version.flags |= STLINK_F_HAS_GETLASTRWSTATUS2; } return; } @@ -1176,13 +1189,8 @@ int stlink_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, struct s bool stlink_is_core_halted(stlink_t *sl) { - bool ret = false; - stlink_status(sl); - if (sl->q_buf[0] == STLINK_CORE_HALTED) - ret = true; - - return ret; + return sl->core_stat == TARGET_HALTED; } int stlink_step(stlink_t *sl) { @@ -1249,21 +1257,21 @@ void stlink_run_at(stlink_t *sl, stm32_addr_t addr) { // this function is called by stlink_status() // do not call stlink_core_stat() directly, always use stlink_status() void stlink_core_stat(stlink_t *sl) { - if (sl->q_len <= 0) - return; - - switch (sl->q_buf[0]) { - case STLINK_CORE_RUNNING: - sl->core_stat = STLINK_CORE_RUNNING; + switch (sl->core_stat ) { + case TARGET_RUNNING: DLOG(" core status: running\n"); return; - case STLINK_CORE_HALTED: - sl->core_stat = STLINK_CORE_HALTED; + case TARGET_HALTED: DLOG(" core status: halted\n"); return; + case TARGET_RESET: + DLOG(" core status: reset\n"); + return; + case TARGET_DEBUG_RUNNING: + DLOG(" core status: debug running\n"); + return; default: - sl->core_stat = STLINK_CORE_STAT_UNKNOWN; - fprintf(stderr, " core status: unknown\n"); + DLOG(" core status: unknown\n"); } } @@ -1271,7 +1279,7 @@ void stlink_print_data(stlink_t * sl) { if (sl->q_len <= 0 || sl->verbose < UDEBUG) return; if (sl->verbose > 2) - fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len); + DLOG("data_len = %d 0x%x\n", sl->q_len, sl->q_len); for (int i = 0; i < sl->q_len; i++) { if (i % 16 == 0) { @@ -1282,9 +1290,9 @@ void stlink_print_data(stlink_t * sl) { fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i); */ } - fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]); + DLOG(" %02x", (unsigned int) sl->q_buf[i]); } - fputs("\n\n", stdout); + DLOG("\n\n"); } /* memory mapped file */ @@ -2232,17 +2240,15 @@ int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t /* erase each page */ int page_count = 0; for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + (uint32_t) off)) { - /* addr must be an addr inside the page */ + // addr must be an addr inside the page if (stlink_erase_flash_page(sl, addr + (uint32_t) off) == -1) { ELOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off); return -1; } - fprintf(stdout,"\rFlash page at addr: 0x%08lx erased", + ILOG("Flash page at addr: 0x%08lx erased\n", (unsigned long)(addr + off)); - fflush(stdout); page_count++; } - fprintf(stdout,"\n"); ILOG("Finished erasing %d pages of %d (%#x) bytes\n", page_count, sl->flash_pgsz, sl->flash_pgsz); diff --git a/src/flash_loader.c b/src/flash_loader.c index a1c9c3757..6d3d83fdb 100644 --- a/src/flash_loader.c +++ b/src/flash_loader.c @@ -330,7 +330,9 @@ int stlink_flash_loader_write_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* } memcpy(sl->q_buf, loader_code, loader_size); - stlink_write_mem32(sl, sl->sram_base, loader_size); + int ret = stlink_write_mem32(sl, sl->sram_base, loader_size); + if (ret) + return ret; *addr = sl->sram_base; *size = loader_size; diff --git a/src/gdbserver/gdb-server.c b/src/gdbserver/gdb-server.c index 2b5b1d3cb..dadb3f56e 100644 --- a/src/gdbserver/gdb-server.c +++ b/src/gdbserver/gdb-server.c @@ -1443,7 +1443,7 @@ int serve(stlink_t *sl, st_state_t *st) { if (ret) { DLOG("Semihost: status failed\n"); } - if (sl->core_stat == STLINK_CORE_HALTED) { + if(sl->core_stat == TARGET_HALTED) { struct stlink_reg reg; stm32_addr_t pc; stm32_addr_t addr; diff --git a/src/logging.c b/src/logging.c index 59c2ea298..c00c172ae 100644 --- a/src/logging.c +++ b/src/logging.c @@ -11,7 +11,7 @@ #include "stlink/logging.h" -static int max_level = UINFO; +static int max_level = UDEBUG; int ugly_init(int maximum_threshold) { max_level = maximum_threshold; diff --git a/src/sg.c b/src/sg.c index 031cc64e5..8e6bb34e8 100644 --- a/src/sg.c +++ b/src/sg.c @@ -427,7 +427,7 @@ int _stlink_sg_current_mode(stlink_t *stl) { int _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[1] = STLINK_DEBUG_APIV1_ENTER; sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD; sl->q_len = 0; // >0 -> aboard return stlink_q(sl); @@ -439,7 +439,7 @@ int _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[1] = STLINK_DEBUG_APIV1_ENTER; sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG; sl->q_len = 0; return stlink_q(sl); @@ -519,7 +519,7 @@ int _stlink_sg_core_id(stlink_t *sl) { int _stlink_sg_reset(stlink_t *sl) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); - sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS; + sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_RESETSYS; sl->q_len = 2; sg->q_addr = 0; if (stlink_q(sl)) @@ -579,7 +579,7 @@ int _stlink_sg_read_all_regs(stlink_t *sl, struct stlink_reg *regp) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); - sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS; + sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_READALLREGS; sl->q_len = 84; sg->q_addr = 0; if (stlink_q(sl)) @@ -620,7 +620,7 @@ int _stlink_sg_read_all_regs(stlink_t *sl, struct stlink_reg *regp) { int _stlink_sg_read_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) { struct stlink_libsg *sg = sl->backend_data; clear_cdb(sg); - sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG; + sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_READREG; sg->cdb_cmd_blk[2] = r_idx; sl->q_len = 4; sg->q_addr = 0; @@ -665,7 +665,7 @@ int _stlink_sg_read_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) { int _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; + sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_WRITEREG; // 2: reg index // 3-6: reg content sg->cdb_cmd_blk[2] = idx; @@ -686,7 +686,7 @@ 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; + sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_WRITEDEBUGREG; // 2-5: address of reg of the debug module // 6-9: reg content write_uint32(sg->cdb_cmd_blk + 2, addr); @@ -733,7 +733,7 @@ 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; + sg->cdb_cmd_blk[1] = STLINK_DEBUG_APIV1_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) @@ -752,7 +752,7 @@ 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[1] = STLINK_DEBUG_APIV1_CLEARFP; sg->cdb_cmd_blk[2] = fp_nr; sl->q_len = 2; @@ -1014,7 +1014,7 @@ static stlink_t* stlink_open(const int verbose) { sl->backend_data = slsg; sl->backend = &_stlink_sg_backend; - sl->core_stat = STLINK_CORE_STAT_UNKNOWN; + sl->core_stat = TARGET_UNKNOWN; slsg->q_addr = 0; return sl; diff --git a/src/usb.c b/src/usb.c index c9ea7039a..0edf35e46 100644 --- a/src/usb.c +++ b/src/usb.c @@ -79,34 +79,34 @@ void _stlink_usb_close(stlink_t* sl) { } ssize_t send_recv(struct stlink_libusb* handle, int terminate, - unsigned char* txbuf, size_t txsize, - unsigned char* rxbuf, size_t rxsize) { + unsigned char* txbuf, size_t txsize, + unsigned char* rxbuf, size_t rxsize) { /* note: txbuf and rxbuf can point to the same area */ int res = 0; int t; t = libusb_bulk_transfer(handle->usb_handle, handle->ep_req, - txbuf, - (int) txsize, - &res, - 3000); + txbuf, + (int) txsize, + &res, + 3000); if (t) { printf("[!] send_recv send request failed: %s\n", libusb_error_name(t)); return -1; } else if ((size_t)res != txsize) { printf("[!] send_recv send request wrote %u bytes (instead of %u).\n", - (unsigned int)res, (unsigned int)txsize); + (unsigned int)res, (unsigned int)txsize); } if (rxsize != 0) { t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, - rxbuf, - (int) rxsize, - &res, - 3000); + rxbuf, + (int) rxsize, + &res, + 3000); if (t) { printf("[!] send_recv read reply failed: %s\n", - libusb_error_name(t)); + libusb_error_name(t)); return -1; } } @@ -115,13 +115,13 @@ ssize_t send_recv(struct stlink_libusb* handle, int terminate, /* Read the SG reply */ unsigned char sg_buf[13]; t = libusb_bulk_transfer(handle->usb_handle, handle->ep_rep, - sg_buf, - 13, - &res, - 3000); + sg_buf, + 13, + &res, + 3000); if (t) { printf("[!] send_recv read storage failed: %s\n", - libusb_error_name(t)); + libusb_error_name(t)); return -1; } /* The STLink doesn't seem to evaluate the sequence number */ @@ -260,6 +260,30 @@ int _stlink_usb_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) { return 0; } +int _stlink_usb_get_rw_status(stlink_t *sl) +{ + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + return -1; + + unsigned char* const rdata = sl->q_buf; + struct stlink_libusb * const slu = sl->backend_data; + unsigned char* const cmd = sl->c_buf; + int i, ret = 0; + + i = fill_command(sl, SG_DXFER_FROM_DEV, 12); + cmd[i++] = STLINK_DEBUG_COMMAND; + if (sl->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) { + cmd[i++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2; + ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 12); + } else { + cmd[i++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS; + ret = send_recv(slu, 1, cmd, slu->cmd_len, rdata, 2); + } + if (ret < 0) + return -1; + return 0; +} + int _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) { struct stlink_libusb * const slu = sl->backend_data; unsigned char* const data = sl->q_buf; @@ -274,12 +298,11 @@ int _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) { ret = send_only(slu, 0, cmd, slu->cmd_len); if (ret == -1) return ret; - - ret = send_only(slu, 1, data, len); + ret = send_only(slu, 0, data, len); if (ret == -1) return ret; - return 0; + return _stlink_usb_get_rw_status(sl); } int _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) { @@ -327,11 +350,14 @@ int _stlink_usb_core_id(stlink_t * sl) { unsigned char* const cmd = sl->c_buf; unsigned char* const data = sl->q_buf; ssize_t size; - int rep_len = 4; + int rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 12; int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; - cmd[i++] = STLINK_DEBUG_READCOREID; + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + cmd[i++] = STLINK_DEBUG_READCOREID; + else + cmd[i++] = STLINK_DEBUG_APIV2_READ_IDCODES; size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); if (size == -1) { @@ -339,11 +365,39 @@ int _stlink_usb_core_id(stlink_t * sl) { return -1; } - sl->core_id = read_uint32(data, 0); + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + sl->core_id = read_uint32(data, 0); + else + sl->core_id = read_uint32(data, 4); return 0; } +int _stlink_usb_status_v2(stlink_t *sl) +{ + int result; + uint32_t status; + + result = _stlink_usb_read_debug32(sl, DCB_DHCSR, &status); + DLOG("core status: %08X\n", status); + if (result != 0) { + sl->core_stat = TARGET_UNKNOWN; + } else { + if (status & S_HALT) { + sl->core_stat = TARGET_HALTED; + } else if (status & S_RESET_ST) { + sl->core_stat = TARGET_RESET; + } else { + sl->core_stat = TARGET_RUNNING; + } + } + + return result; +} int _stlink_usb_status(stlink_t * sl) { + if (sl->version.jtag_api != STLINK_JTAG_API_V1) { + return _stlink_usb_status_v2(sl); + } + struct stlink_libusb * const slu = sl->backend_data; unsigned char* const data = sl->q_buf; unsigned char* const cmd = sl->c_buf; @@ -360,6 +414,14 @@ int _stlink_usb_status(stlink_t * sl) { return (int) size; } sl->q_len = (int) size; + if (sl->q_len > 1) { + if (sl->q_buf[0] == STLINK_CORE_RUNNING) + sl->core_stat = TARGET_RUNNING; + else if (sl->q_buf[0] == STLINK_CORE_HALTED) + sl->core_stat = TARGET_HALTED; + else + sl->core_stat = TARGET_UNKNOWN; + } return 0; } @@ -392,14 +454,13 @@ int _stlink_usb_enter_swd_mode(stlink_t * sl) { int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; - cmd[i++] = sl->version.stlink_v > 1? STLINK_DEBUG_APIV2_ENTER : STLINK_DEBUG_ENTER; + cmd[i++] = sl->version.jtag_api == STLINK_JTAG_API_V1 ? STLINK_DEBUG_APIV1_ENTER : STLINK_DEBUG_APIV2_ENTER; cmd[i++] = STLINK_DEBUG_ENTER_SWD; - if (rep_len == 0) { - size = send_only(slu, 1, cmd, slu->cmd_len); + size = send_only(slu, 1, cmd, slu->cmd_len); } else { - size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); + size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); } if (size == -1) { printf("[!] send_recv STLINK_DEBUG_ENTER\n"); @@ -437,7 +498,10 @@ int _stlink_usb_reset(stlink_t * sl) { int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; - cmd[i++] = STLINK_DEBUG_RESETSYS; + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + cmd[i++] = STLINK_DEBUG_APIV1_RESETSYS; + else + cmd[i++] = STLINK_DEBUG_APIV2_RESETSYS; size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); if (size == -1) { @@ -447,7 +511,7 @@ int _stlink_usb_reset(stlink_t * sl) { // Reset through AIRCR so NRST does not need to be connected return stlink_write_debug32(sl, STLINK_REG_AIRCR, - STLINK_REG_AIRCR_VECTKEY | STLINK_REG_AIRCR_SYSRESETREQ); + STLINK_REG_AIRCR_VECTKEY | STLINK_REG_AIRCR_SYSRESETREQ); } @@ -499,6 +563,14 @@ int _stlink_usb_step(stlink_t* sl) { */ int _stlink_usb_run(stlink_t* sl) { struct stlink_libusb * const slu = sl->backend_data; + + int res; + if (sl->version.jtag_api != STLINK_JTAG_API_V1) { + res = _stlink_usb_write_debug32(sl, DCB_DHCSR, DBGKEY|C_DEBUGEN); + return res; + } + + unsigned char* const data = sl->q_buf; unsigned char* const cmd = sl->c_buf; ssize_t size; @@ -543,7 +615,7 @@ int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { return 0; } else if (sl->version.stlink_v == 3) { int speed_index; - uint32_t map[STLINK_V3_MAX_FREQ_NB]; + uint32_t map[STLINK_V3_MAX_FREQ_NB]; i = fill_command(sl, SG_DXFER_FROM_DEV, 16); cmd[i++] = STLINK_DEBUG_COMMAND; @@ -561,14 +633,14 @@ int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { if (speeds_size > STLINK_V3_MAX_FREQ_NB) speeds_size = STLINK_V3_MAX_FREQ_NB; - for (i = 0; i < speeds_size; i++) - map[i] = le_to_h_u32(&data[12 + 4 * i]); + for (i = 0; i < speeds_size; i++) + map[i] = le_to_h_u32(&data[12 + 4 * i]); /* set to zero all the next entries */ for (i = speeds_size; i < STLINK_V3_MAX_FREQ_NB; i++) - map[i] = 0; + map[i] = 0; - speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), 1800); + speed_index = _stlink_match_speed_map(map, STLINK_ARRAY_SIZE(map), 1800); i = fill_command(sl, SG_DXFER_FROM_DEV, 16); @@ -576,12 +648,12 @@ int _stlink_usb_set_swdclk(stlink_t* sl, uint16_t clk_divisor) { cmd[i++] = STLINK_APIV3_SET_COM_FREQ; cmd[i++] = 0; // SWD mode cmd[i++] = 0; - cmd[i++] = (uint8_t)((map[speed_index] >> 0) & 0xFF); - cmd[i++] = (uint8_t)((map[speed_index] >> 8) & 0xFF); - cmd[i++] = (uint8_t)((map[speed_index] >> 16) & 0xFF); - cmd[i++] = (uint8_t)((map[speed_index] >> 24) & 0xFF); + cmd[i++] = (uint8_t)((map[speed_index] >> 0) & 0xFF); + cmd[i++] = (uint8_t)((map[speed_index] >> 8) & 0xFF); + cmd[i++] = (uint8_t)((map[speed_index] >> 16) & 0xFF); + cmd[i++] = (uint8_t)((map[speed_index] >> 24) & 0xFF); - size = send_recv(slu, 1, cmd, slu->cmd_len, data, 8); + size = send_recv(slu, 1, cmd, slu->cmd_len, data, 8); if (size == -1) { printf("[!] send_recv STLINK_APIV3_SET_COM_FREQ\n"); return (int) size; @@ -639,33 +711,40 @@ int _stlink_usb_read_all_regs(stlink_t *sl, struct stlink_reg *regp) { unsigned char* const cmd = sl->c_buf; unsigned char* const data = sl->q_buf; ssize_t size; - uint32_t rep_len = 84; + uint32_t rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 84 : 88; int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; - cmd[i++] = STLINK_DEBUG_READALLREGS; + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + cmd[i++] = STLINK_DEBUG_APIV1_READALLREGS; + else + cmd[i++] = STLINK_DEBUG_APIV2_READALLREGS; size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); if (size == -1) { printf("[!] send_recv STLINK_DEBUG_READALLREGS\n"); return (int) size; } + + /* V1: regs data from offset 0 */ + /* V2: status at offset 0, regs data from offset 4 */ + int reg_offset = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 0 : 4; sl->q_len = (int) size; stlink_print_data(sl); - for (i=0; i<16; i++) + for(i=reg_offset; 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); + regp->xpsr = read_uint32(sl->q_buf, reg_offset + 64); + regp->main_sp = read_uint32(sl->q_buf, reg_offset + 68); + regp->process_sp = read_uint32(sl->q_buf, reg_offset + 72); + regp->rw = read_uint32(sl->q_buf, reg_offset + 76); + regp->rw2 = read_uint32(sl->q_buf, reg_offset + 80); if (sl->verbose < 2) return 0; - DLOG("xpsr = 0x%08x\n", read_uint32(sl->q_buf, 64)); - DLOG("main_sp = 0x%08x\n", read_uint32(sl->q_buf, 68)); - DLOG("process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72)); - DLOG("rw = 0x%08x\n", read_uint32(sl->q_buf, 76)); - DLOG("rw2 = 0x%08x\n", read_uint32(sl->q_buf, 80)); + DLOG("xpsr = 0x%08x\n", read_uint32(sl->q_buf, reg_offset + 64)); + DLOG("main_sp = 0x%08x\n", read_uint32(sl->q_buf, reg_offset + 68)); + DLOG("process_sp = 0x%08x\n", read_uint32(sl->q_buf, reg_offset + 72)); + DLOG("rw = 0x%08x\n", read_uint32(sl->q_buf, reg_offset + 76)); + DLOG("rw2 = 0x%08x\n", read_uint32(sl->q_buf, reg_offset + 80)); return 0; } @@ -676,11 +755,15 @@ int _stlink_usb_read_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) { unsigned char* const cmd = sl->c_buf; ssize_t size; uint32_t r; - uint32_t rep_len = 4; + uint32_t rep_len = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8; + int reg_offset = sl->version.jtag_api == STLINK_JTAG_API_V1 ? 0 : 4; int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; - cmd[i++] = STLINK_DEBUG_READREG; + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + cmd[i++] = STLINK_DEBUG_APIV1_READREG; + else + cmd[i++] = STLINK_DEBUG_APIV2_READREG; cmd[i++] = (uint8_t) r_idx; size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); if (size == -1) { @@ -689,7 +772,7 @@ int _stlink_usb_read_reg(stlink_t *sl, int r_idx, struct stlink_reg *regp) { } sl->q_len = (int) size; stlink_print_data(sl); - r = read_uint32(sl->q_buf, 0); + r = read_uint32(sl->q_buf, reg_offset); DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r); switch (r_idx) { @@ -827,7 +910,10 @@ int _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) { int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); cmd[i++] = STLINK_DEBUG_COMMAND; - cmd[i++] = STLINK_DEBUG_WRITEREG; + if (sl->version.jtag_api == STLINK_JTAG_API_V1) + cmd[i++] = STLINK_DEBUG_APIV1_WRITEREG; + else + cmd[i++] = STLINK_DEBUG_APIV2_WRITEREG; cmd[i++] = idx; write_uint32(&cmd[i], reg); size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); @@ -835,7 +921,7 @@ int _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) { printf("[!] send_recv STLINK_DEBUG_WRITEREG\n"); return (int) size; } -sl->q_len = (int) size; + sl->q_len = (int) size; stlink_print_data(sl); return 0; @@ -889,7 +975,7 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[ST sl->backend = &_stlink_usb_backend; sl->backend_data = slu; - sl->core_stat = STLINK_CORE_STAT_UNKNOWN; + sl->core_stat = TARGET_UNKNOWN; if (libusb_init(&(slu->libusb_ctx))) { WLOG("failed to init libusb context, wrong version of libraries?\n"); goto on_error; @@ -934,7 +1020,7 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[ST if (devBus && devAddr) { if ((libusb_get_bus_number(list[cnt]) != devBus) - || (libusb_get_device_address(list[cnt]) != devAddr)) { + || (libusb_get_device_address(list[cnt]) != devAddr)) { continue; } } @@ -945,9 +1031,19 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[ST if (ret) continue; + sl->serial_size = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, (unsigned char *)sl->serial, sizeof(sl->serial)); + if ((serial == NULL) || (*serial == 0)) + break; + + if (sl->serial_size < 0) + continue; + + if (memcmp(serial, &sl->serial, sl->serial_size) == 0) + break; + libusb_close(handle); /* could not read serial, continue */ @@ -1032,6 +1128,9 @@ stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[ST // Initialize stlink version (sl->version) stlink_version(sl); + // Initialize stlink version (sl->version) + stlink_version(sl); + if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) { // This seems to work, and is unnecessary information for the user. // Demoted to debug -- REW @@ -1099,7 +1198,6 @@ static size_t stlink_probe_usb_devs(libusb_device **devs, stlink_t **sldevs[]) { WLOG("skipping ST device : %#04x:%#04x)\n", desc.idVendor, desc.idProduct); continue; } - slcnt++; } @@ -1136,7 +1234,6 @@ static size_t stlink_probe_usb_devs(libusb_device **devs, stlink_t **sldevs[]) { } break; } - ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, (unsigned char *)&serial, sizeof(serial)); libusb_close(handle);