X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fstlink-usb.c;h=20e5cd22e7bcc63b56c98024165e2396369216c0;hb=299c5d272f58c7cb803d6a61ca16d9551c7697e5;hp=58c4e1d334f3733b8565afe0f4ed85b25334309c;hpb=83a5eb2ed34ed78edacd92a69f366b89728ac5d0;p=fw%2Fstlink diff --git a/src/stlink-usb.c b/src/stlink-usb.c index 58c4e1d..20e5cd2 100644 --- a/src/stlink-usb.c +++ b/src/stlink-usb.c @@ -2,9 +2,9 @@ #include #include #include -#include +#include #include -#include +#include #include "stlink-common.h" #include "stlink-usb.h" @@ -16,6 +16,19 @@ #define WLOG(format, args...) ugly_log(UWARN, LOG_TAG, format, ## args) #define fatal(format, args...) ugly_log(UFATAL, LOG_TAG, format, ## args) +/* code from bsd timersub.h +http://www.gnu-darwin.org/www001/src/ports/net/libevnet/work/libevnet-0.3.8/libnostd/bsd/sys/time/timersub.h.html +*/ +#if !defined timersub +#define timersub(a, b, r) do { \ + (r)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (r)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((r)->tv_usec < 0) { \ + --(r)->tv_sec; \ + (r)->tv_usec += 1000000; \ + } \ +} while (0) +#endif enum SCSI_Generic_Direction {SG_DXFER_TO_DEV=0, SG_DXFER_FROM_DEV=0x80}; @@ -45,7 +58,7 @@ struct trans_ctx { volatile unsigned long flags; }; -static void on_trans_done(struct libusb_transfer * trans) { +static void LIBUSB_CALL on_trans_done(struct libusb_transfer * trans) { struct trans_ctx * const ctx = trans->user_data; if (trans->status != LIBUSB_TRANSFER_COMPLETED) @@ -127,8 +140,6 @@ ssize_t send_recv(struct stlink_libusb* handle, int terminate, } if ((handle->protocoll == 1) && terminate) { - fprintf(stderr, "This is never used....\n"); - exit(EXIT_FAILURE); /* Read the SG reply */ unsigned char sg_buf[13]; libusb_fill_bulk_transfer @@ -157,7 +168,6 @@ static int fill_command int i = 0; memset(cmd, 0, sizeof (sl->c_buf)); if(slu->protocoll == 1) { - fprintf(stderr, "This is never used....\n"); cmd[i++] = 'U'; cmd[i++] = 'S'; cmd[i++] = 'B'; @@ -189,6 +199,44 @@ void _stlink_usb_version(stlink_t *sl) { } } +uint32_t _stlink_usb_read_debug32(stlink_t *sl, uint32_t addr) { + struct stlink_libusb * const slu = sl->backend_data; + unsigned char* const rdata = sl->q_buf; + unsigned char* const cmd = sl->c_buf; + ssize_t size; + const int rep_len = 8; + + int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); + cmd[i++] = STLINK_DEBUG_COMMAND; + cmd[i++] = STLINK_JTAG_READDEBUG_32BIT; + write_uint32(&cmd[i], addr); + size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len); + if (size == -1) { + printf("[!] send_recv\n"); + return 0; + } + return read_uint32(rdata, 4); +} + +void _stlink_usb_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) { + struct stlink_libusb * const slu = sl->backend_data; + unsigned char* const rdata = sl->q_buf; + unsigned char* const cmd = sl->c_buf; + ssize_t size; + const int rep_len = 2; + + int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); + cmd[i++] = STLINK_DEBUG_COMMAND; + cmd[i++] = STLINK_JTAG_WRITEDEBUG_32BIT; + write_uint32(&cmd[i], addr); + write_uint32(&cmd[i + 4], data); + size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len); + if (size == -1) { + printf("[!] send_recv\n"); + return; + } +} + void _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; @@ -348,6 +396,26 @@ void _stlink_usb_reset(stlink_t * sl) { } +void _stlink_usb_jtag_reset(stlink_t * sl, int value) { + struct stlink_libusb * const slu = sl->backend_data; + unsigned char* const data = sl->q_buf; + unsigned char* const cmd = sl->c_buf; + ssize_t size; + int rep_len = 2; + int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len); + + cmd[i++] = STLINK_DEBUG_COMMAND; + cmd[i++] = STLINK_JTAG_DRIVE_NRST; + cmd[i++] = (value)?0:1; + + size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len); + if (size == -1) { + printf("[!] send_recv\n"); + return; + } +} + + void _stlink_usb_step(stlink_t* sl) { struct stlink_libusb * const slu = sl->backend_data; unsigned char* const data = sl->q_buf; @@ -533,10 +601,13 @@ stlink_backend_t _stlink_usb_backend = { _stlink_usb_exit_dfu_mode, _stlink_usb_core_id, _stlink_usb_reset, + _stlink_usb_jtag_reset, _stlink_usb_run, _stlink_usb_status, _stlink_usb_version, + _stlink_usb_read_debug32, _stlink_usb_read_mem32, + _stlink_usb_write_debug32, _stlink_usb_write_mem32, _stlink_usb_write_mem8, _stlink_usb_read_all_regs, @@ -575,12 +646,14 @@ stlink_t* stlink_open_usb(const int verbose) { 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; + slu->usb_handle = libusb_open_device_with_vid_pid(slu->libusb_ctx, USB_ST_VID, USB_STLINK_PID); + if (slu->usb_handle == NULL) { + WLOG("Couldn't find any ST-Link/V2 devices"); + goto on_error; + } + slu->protocoll = 1; } - + if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) { int r; @@ -641,8 +714,9 @@ stlink_t* stlink_open_usb(const int verbose) { stlink_enter_swd_mode(sl); } - stlink_version(sl); + stlink_reset(sl); stlink_load_device_params(sl); + stlink_version(sl); error = 0; @@ -660,6 +734,8 @@ on_libusb_error: return sl; on_error: + if( slu->libusb_ctx) + libusb_exit(slu->libusb_ctx); if (sl != NULL) free(sl); if (slu != NULL) free(slu); return 0;