7 #include <libusb-1.0/libusb.h>
8 #include "stlink-common.h"
9 #include "stlink-usb.h"
11 enum SCSI_Generic_Direction {SG_DXFER_TO_DEV=0, SG_DXFER_FROM_DEV=0x80};
13 void _stlink_usb_close(stlink_t* sl) {
14 struct stlink_libusb * const handle = sl->backend_data;
15 // maybe we couldn't even get the usb device?
17 if (handle->req_trans != NULL)
18 libusb_free_transfer(handle->req_trans);
20 if (handle->rep_trans != NULL)
21 libusb_free_transfer(handle->rep_trans);
23 if (handle->usb_handle != NULL) {
24 libusb_close(handle->usb_handle);
27 libusb_exit(handle->libusb_ctx);
34 #define TRANS_FLAGS_IS_DONE (1 << 0)
35 #define TRANS_FLAGS_HAS_ERROR (1 << 1)
36 volatile unsigned long flags;
39 static void on_trans_done(struct libusb_transfer * trans) {
40 struct trans_ctx * const ctx = trans->user_data;
42 if (trans->status != LIBUSB_TRANSFER_COMPLETED)
43 ctx->flags |= TRANS_FLAGS_HAS_ERROR;
45 ctx->flags |= TRANS_FLAGS_IS_DONE;
48 int submit_wait(struct stlink_libusb *slu, struct libusb_transfer * trans) {
52 struct trans_ctx trans_ctx;
53 enum libusb_error error;
57 /* brief intrusion inside the libusb interface */
58 trans->callback = on_trans_done;
59 trans->user_data = &trans_ctx;
61 if ((error = libusb_submit_transfer(trans))) {
62 printf("libusb_submit_transfer(%d)\n", error);
66 gettimeofday(&start, NULL);
68 while (trans_ctx.flags == 0) {
69 struct timeval timeout;
72 if (libusb_handle_events_timeout(slu->libusb_ctx, &timeout)) {
73 printf("libusb_handle_events()\n");
77 gettimeofday(&now, NULL);
78 timersub(&now, &start, &diff);
79 if (diff.tv_sec >= 3) {
80 printf("libusb_handle_events() timeout\n");
85 if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
86 printf("libusb_handle_events() | has_error\n");
93 ssize_t send_recv(struct stlink_libusb* handle, int terminate,
94 unsigned char* txbuf, size_t txsize,
95 unsigned char* rxbuf, size_t rxsize) {
96 /* note: txbuf and rxbuf can point to the same area */
99 libusb_fill_bulk_transfer(handle->req_trans, handle->usb_handle,
106 if (submit_wait(handle, handle->req_trans)) return -1;
111 /* read the response */
113 libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
114 handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
116 if (submit_wait(handle, handle->rep_trans)) return -1;
117 res = handle->rep_trans->actual_length;
119 if ((handle->protocoll == 1) && terminate) {
120 /* Read the SG reply */
121 unsigned char sg_buf[13];
122 libusb_fill_bulk_transfer
123 (handle->rep_trans, handle->usb_handle,
124 handle->ep_rep, sg_buf, 13, NULL, NULL, 0);
125 res = submit_wait(handle, handle->rep_trans);
126 /* The STLink doesn't seem to evaluate the sequence number */
127 handle->sg_transfer_idx++;
131 return handle->rep_trans->actual_length;
134 static inline int send_only
135 (struct stlink_libusb* handle, int terminate,
136 unsigned char* txbuf, size_t txsize) {
137 return send_recv(handle, terminate, txbuf, txsize, NULL, 0);
141 /* Search for a STLINK device, either any or teh one with the given PID
142 * Return the protocoll version
144 static int is_stlink_device(libusb_device * dev, uint16_t pid) {
145 struct libusb_device_descriptor desc;
148 if (libusb_get_device_descriptor(dev, &desc))
151 if (desc.idVendor != USB_ST_VID)
154 if ((desc.idProduct != USB_STLINK_32L_PID) &&
155 (desc.idProduct != USB_STLINK_PID ))
158 if(pid && (pid != desc.idProduct))
160 if (desc.idProduct == USB_STLINK_PID )
168 static int fill_command
169 (stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t len) {
170 struct stlink_libusb * const slu = sl->backend_data;
171 unsigned char* const cmd = sl->c_buf;
173 memset(cmd, 0, sizeof (sl->c_buf));
174 if(slu->protocoll == 1) {
179 write_uint32(&cmd[i], slu->sg_transfer_idx);
180 write_uint32(&cmd[i + 4], len);
182 cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
183 cmd[i++] = 0; /* Logical unit */
184 cmd[i++] = 0xa; /* Command length */
189 void _stlink_usb_version(stlink_t *sl) {
190 struct stlink_libusb * const slu = sl->backend_data;
191 unsigned char* const data = sl->q_buf;
192 unsigned char* const cmd = sl->c_buf;
194 uint32_t rep_len = 6;
195 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
197 cmd[i++] = STLINK_GET_VERSION;
199 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
201 printf("[!] send_recv\n");
206 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
207 struct stlink_libusb * const slu = sl->backend_data;
208 unsigned char* const data = sl->q_buf;
209 unsigned char* const cmd = sl->c_buf;
211 int i = fill_command(sl, SG_DXFER_TO_DEV, len);
212 cmd[i++] = STLINK_DEBUG_COMMAND;
213 cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
214 write_uint32(&cmd[i], addr);
215 write_uint16(&cmd[i + 4], len);
216 send_only(slu, 0, cmd, slu->cmd_len);
218 send_only(slu, 1, data, len);
221 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
222 struct stlink_libusb * const slu = sl->backend_data;
223 unsigned char* const data = sl->q_buf;
224 unsigned char* const cmd = sl->c_buf;
226 int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
227 cmd[i++] = STLINK_DEBUG_COMMAND;
228 cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
229 write_uint32(&cmd[i], addr);
230 write_uint16(&cmd[i + 4], len);
231 send_only(slu, 0, cmd, slu->cmd_len);
232 send_only(slu, 1, data, len);
236 int _stlink_usb_current_mode(stlink_t * sl) {
237 struct stlink_libusb * const slu = sl->backend_data;
238 unsigned char* const cmd = sl->c_buf;
239 unsigned char* const data = sl->q_buf;
242 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
244 cmd[i++] = STLINK_GET_CURRENT_MODE;
245 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
247 printf("[!] send_recv\n");
253 void _stlink_usb_core_id(stlink_t * sl) {
254 struct stlink_libusb * const slu = sl->backend_data;
255 unsigned char* const cmd = sl->c_buf;
256 unsigned char* const data = sl->q_buf;
259 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
261 cmd[i++] = STLINK_DEBUG_COMMAND;
262 cmd[i++] = STLINK_DEBUG_READCOREID;
264 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
266 printf("[!] send_recv\n");
270 sl->core_id = read_uint32(data, 0);
273 void _stlink_usb_status(stlink_t * sl) {
274 struct stlink_libusb * const slu = sl->backend_data;
275 unsigned char* const data = sl->q_buf;
276 unsigned char* const cmd = sl->c_buf;
279 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
281 cmd[i++] = STLINK_DEBUG_COMMAND;
282 cmd[i++] = STLINK_DEBUG_GETSTATUS;
284 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
286 printf("[!] send_recv\n");
291 void _stlink_usb_force_debug(stlink_t *sl) {
292 struct stlink_libusb *slu = sl->backend_data;
293 unsigned char* const data = sl->q_buf;
294 unsigned char* const cmd = sl->c_buf;
297 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
299 cmd[i++] = STLINK_DEBUG_COMMAND;
300 cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
301 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
303 printf("[!] send_recv\n");
308 void _stlink_usb_enter_swd_mode(stlink_t * sl) {
309 struct stlink_libusb * const slu = sl->backend_data;
310 unsigned char* const cmd = sl->c_buf;
312 const int rep_len = 0;
313 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
315 cmd[i++] = STLINK_DEBUG_COMMAND;
316 cmd[i++] = STLINK_DEBUG_ENTER;
317 cmd[i++] = STLINK_DEBUG_ENTER_SWD;
319 size = send_only(slu, 1, cmd, slu->cmd_len);
321 printf("[!] send_recv\n");
326 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
327 struct stlink_libusb * const slu = sl->backend_data;
328 unsigned char* const cmd = sl->c_buf;
330 int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
332 cmd[i++] = STLINK_DFU_COMMAND;
333 cmd[i++] = STLINK_DFU_EXIT;
335 size = send_only(slu, 1, cmd, slu->cmd_len);
337 printf("[!] send_recv\n");
343 * TODO - not convinced this does anything...
346 void _stlink_usb_reset(stlink_t * sl) {
347 struct stlink_libusb * const slu = sl->backend_data;
348 unsigned char* const data = sl->q_buf;
349 unsigned char* const cmd = sl->c_buf;
352 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
354 cmd[i++] = STLINK_DEBUG_COMMAND;
355 cmd[i++] = STLINK_DEBUG_RESETSYS;
357 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
359 printf("[!] send_recv\n");
365 void _stlink_usb_step(stlink_t* sl) {
366 struct stlink_libusb * const slu = sl->backend_data;
367 unsigned char* const data = sl->q_buf;
368 unsigned char* const cmd = sl->c_buf;
371 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
373 cmd[i++] = STLINK_DEBUG_COMMAND;
374 cmd[i++] = STLINK_DEBUG_STEPCORE;
376 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
378 printf("[!] send_recv\n");
384 * This seems to do a good job of restarting things from the beginning?
387 void _stlink_usb_run(stlink_t* sl) {
388 struct stlink_libusb * const slu = sl->backend_data;
389 unsigned char* const data = sl->q_buf;
390 unsigned char* const cmd = sl->c_buf;
393 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
395 cmd[i++] = STLINK_DEBUG_COMMAND;
396 cmd[i++] = STLINK_DEBUG_RUNCORE;
398 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
400 printf("[!] send_recv\n");
405 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
406 struct stlink_libusb * const slu = sl->backend_data;
407 unsigned char* const cmd = sl->c_buf;
409 int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
411 cmd[i++] = STLINK_DEBUG_COMMAND;
412 cmd[i++] = STLINK_DEBUG_EXIT;
414 size = send_only(slu, 1, cmd, slu->cmd_len);
416 printf("[!] send_only\n");
421 void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
422 struct stlink_libusb * const slu = sl->backend_data;
423 unsigned char* const data = sl->q_buf;
424 unsigned char* const cmd = sl->c_buf;
426 int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
428 cmd[i++] = STLINK_DEBUG_COMMAND;
429 cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
430 write_uint32(&cmd[i], addr);
431 write_uint16(&cmd[i + 4], len);
433 size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
435 printf("[!] send_recv\n");
439 sl->q_len = (size_t) size;
441 stlink_print_data(sl);
444 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
445 struct stlink_libusb * const slu = sl->backend_data;
446 unsigned char* const cmd = sl->c_buf;
447 unsigned char* const data = sl->q_buf;
449 uint32_t rep_len = 84;
450 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
452 cmd[i++] = STLINK_DEBUG_COMMAND;
453 cmd[i++] = STLINK_DEBUG_READALLREGS;
454 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
456 printf("[!] send_recv\n");
459 sl->q_len = (size_t) size;
460 stlink_print_data(sl);
462 regp->r[i]= read_uint32(sl->q_buf, i*4);
463 regp->xpsr = read_uint32(sl->q_buf, 64);
464 regp->main_sp = read_uint32(sl->q_buf, 68);
465 regp->process_sp = read_uint32(sl->q_buf, 72);
466 regp->rw = read_uint32(sl->q_buf, 76);
467 regp->rw2 = read_uint32(sl->q_buf, 80);
471 DD(sl, "xpsr = 0x%08x\n", read_uint32(sl->q_buf, 64));
472 DD(sl, "main_sp = 0x%08x\n", read_uint32(sl->q_buf, 68));
473 DD(sl, "process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72));
474 DD(sl, "rw = 0x%08x\n", read_uint32(sl->q_buf, 76));
475 DD(sl, "rw2 = 0x%08x\n", read_uint32(sl->q_buf, 80));
478 void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
479 struct stlink_libusb * const slu = sl->backend_data;
480 unsigned char* const data = sl->q_buf;
481 unsigned char* const cmd = sl->c_buf;
484 uint32_t rep_len = 4;
485 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
487 cmd[i++] = STLINK_DEBUG_COMMAND;
488 cmd[i++] = STLINK_DEBUG_READREG;
489 cmd[i++] = (uint8_t) r_idx;
490 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
492 printf("[!] send_recv\n");
495 sl->q_len = (size_t) size;
496 stlink_print_data(sl);
497 r = read_uint32(sl->q_buf, 0);
498 DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
508 regp->process_sp = r;
511 regp->rw = r; /* XXX ?(primask, basemask etc.) */
514 regp->rw2 = r; /* XXX ?(primask, basemask etc.) */
521 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
522 struct stlink_libusb * const slu = sl->backend_data;
523 unsigned char* const data = sl->q_buf;
524 unsigned char* const cmd = sl->c_buf;
526 uint32_t rep_len = 2;
527 int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
529 cmd[i++] = STLINK_DEBUG_COMMAND;
530 cmd[i++] = STLINK_DEBUG_WRITEREG;
532 write_uint32(&cmd[i], reg);
533 size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
535 printf("[!] send_recv\n");
538 sl->q_len = (size_t) size;
539 stlink_print_data(sl);
542 stlink_backend_t _stlink_usb_backend = {
544 _stlink_usb_exit_debug_mode,
545 _stlink_usb_enter_swd_mode,
546 NULL, // no enter_jtag_mode here...
547 _stlink_usb_exit_dfu_mode,
553 _stlink_usb_read_mem32,
554 _stlink_usb_write_mem32,
555 _stlink_usb_write_mem8,
556 _stlink_usb_read_all_regs,
557 _stlink_usb_read_reg,
558 _stlink_usb_write_reg,
560 _stlink_usb_current_mode,
561 _stlink_usb_force_debug
565 stlink_t* stlink_open_usb(const int verbose) {
567 struct stlink_libusb* slu = NULL;
569 libusb_device** devs = NULL;
574 char *iSerial = NULL;
576 sl = malloc(sizeof (stlink_t));
577 slu = malloc(sizeof (struct stlink_libusb));
578 if (sl == NULL) goto on_error;
579 if (slu == NULL) goto on_error;
580 memset(sl, 0, sizeof (stlink_t));
581 memset(slu, 0, sizeof (struct stlink_libusb));
583 sl->verbose = verbose;
584 sl->backend = &_stlink_usb_backend;
585 sl->backend_data = slu;
587 sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
589 if (libusb_init(&(slu->libusb_ctx))) {
590 fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
594 count = libusb_get_device_list(slu->libusb_ctx, &devs);
596 printf("libusb_get_device_list\n");
597 goto on_libusb_error;
600 for (i = 0; i < count; ++i) {
602 slu->protocoll = is_stlink_device(dev, 0);
603 if (slu->protocoll > 0) break;
605 if (i == count) goto on_libusb_error;
607 if (libusb_open(dev, &(slu->usb_handle))) {
608 printf("libusb_open()\n");
609 goto on_libusb_error;
613 unsigned char serial[256];
614 struct libusb_device_descriptor desc;
617 r = libusb_get_device_descriptor(dev, &desc);
619 printf("Can't get descriptor to match Iserial\n");
620 goto on_libusb_error;
622 r = libusb_get_string_descriptor_ascii
623 (slu->usb_handle, desc.iSerialNumber, serial, 256);
625 printf("Can't get Serialnumber to match Iserial\n");
626 goto on_libusb_error;
628 if (strcmp((char*)serial, iSerial)) {
629 printf("Mismatch in serial numbers, dev %s vs given %s\n",
631 goto on_libusb_error;
635 if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
638 r = libusb_detach_kernel_driver(slu->usb_handle, 0);
640 printf("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
641 goto on_libusb_error;
644 if (libusb_get_configuration(slu->usb_handle, &config)) {
645 /* this may fail for a previous configured device */
646 printf("libusb_get_configuration()\n");
647 goto on_libusb_error;
651 printf("setting new configuration (%d -> 1)\n", config);
652 if (libusb_set_configuration(slu->usb_handle, 1)) {
653 /* this may fail for a previous configured device */
654 printf("libusb_set_configuration()\n");
655 goto on_libusb_error;
659 if (libusb_claim_interface(slu->usb_handle, 0)) {
660 printf("libusb_claim_interface()\n");
661 goto on_libusb_error;
664 slu->req_trans = libusb_alloc_transfer(0);
665 if (slu->req_trans == NULL) {
666 printf("libusb_alloc_transfer\n");
667 goto on_libusb_error;
670 slu->rep_trans = libusb_alloc_transfer(0);
671 if (slu->rep_trans == NULL) {
672 printf("libusb_alloc_transfer\n");
673 goto on_libusb_error;
676 slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
677 slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
679 slu->sg_transfer_idx = 0;
680 slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
684 if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
685 stlink_exit_dfu_mode(sl);
688 if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
689 stlink_enter_swd_mode(sl);
694 /* per device family initialization */
695 stlink_identify_device(sl);
697 if (sl->chip_id == STM32F4_CHIP_ID) {
699 /* flash memory settings */
700 sl->flash_base = STM32_FLASH_BASE;
701 sl->flash_size = STM32F4_FLASH_SIZE;
702 sl->flash_pgsz = STM32F4_FLASH_PGSZ; //Dummy, pages size is variable in this config
705 sl->sys_base = STM32_SYSTEM_BASE;
706 sl->sys_size = STM32_SYSTEM_SIZE;
708 /* sram memory settings */
709 sl->sram_base = STM32_SRAM_BASE;
710 sl->sram_size = STM32_SRAM_SIZE;
714 else if (sl->core_id == STM32L_CORE_ID) {
716 /* flash memory settings */
717 sl->flash_base = STM32_FLASH_BASE;
718 sl->flash_size = STM32_FLASH_SIZE;
719 sl->flash_pgsz = STM32L_FLASH_PGSZ;
722 sl->sys_base = STM32_SYSTEM_BASE;
723 sl->sys_size = STM32_SYSTEM_SIZE;
725 /* sram memory settings */
726 sl->sram_base = STM32_SRAM_BASE;
727 sl->sram_size = STM32L_SRAM_SIZE;
730 else if (sl->core_id == STM32VL_CORE_ID) {
732 /* flash memory settings */
733 sl->flash_base = STM32_FLASH_BASE;
734 sl->flash_size = STM32_FLASH_SIZE;
735 sl->flash_pgsz = STM32_FLASH_PGSZ;
738 sl->sys_base = STM32_SYSTEM_BASE;
739 sl->sys_size = STM32_SYSTEM_SIZE;
741 /* sram memory settings */
742 sl->sram_base = STM32_SRAM_BASE;
743 sl->sram_size = STM32_SRAM_SIZE;
747 fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
748 goto on_libusb_error;
755 libusb_free_device_list(devs, 1);
767 if (sl != NULL) free(sl);
768 if (slu != NULL) free(slu);