7 #include <libusb-1.0/libusb.h>
8 #include "stlink-common.h"
9 #include "stlink-usb.h"
11 /* endianess related */
12 static inline unsigned int is_bigendian(void) {
13 static volatile const unsigned int i = 1;
14 return *(volatile const char*) &i == 0;
17 void _stlink_usb_close(stlink_t* sl) {
18 struct stlink_libusb * const handle = sl->backend_data;
19 // maybe we couldn't even get the usb device?
21 if (handle->req_trans != NULL)
22 libusb_free_transfer(handle->req_trans);
24 if (handle->rep_trans != NULL)
25 libusb_free_transfer(handle->rep_trans);
27 if (handle->usb_handle != NULL)
28 libusb_close(handle->usb_handle);
30 libusb_exit(handle->libusb_ctx);
35 static void write_uint32(unsigned char* buf, uint32_t ui) {
36 if (!is_bigendian()) { // le -> le (don't swap)
37 buf[0] = ((unsigned char*) &ui)[0];
38 buf[1] = ((unsigned char*) &ui)[1];
39 buf[2] = ((unsigned char*) &ui)[2];
40 buf[3] = ((unsigned char*) &ui)[3];
42 buf[0] = ((unsigned char*) &ui)[3];
43 buf[1] = ((unsigned char*) &ui)[2];
44 buf[2] = ((unsigned char*) &ui)[1];
45 buf[3] = ((unsigned char*) &ui)[0];
49 static void write_uint16(unsigned char* buf, uint16_t ui) {
50 if (!is_bigendian()) { // le -> le (don't swap)
51 buf[0] = ((unsigned char*) &ui)[0];
52 buf[1] = ((unsigned char*) &ui)[1];
54 buf[0] = ((unsigned char*) &ui)[1];
55 buf[1] = ((unsigned char*) &ui)[0];
59 static uint32_t read_uint32(const unsigned char *c, const int pt) {
61 char *p = (char *) &ui;
63 if (!is_bigendian()) { // le -> le (don't swap)
79 #define TRANS_FLAGS_IS_DONE (1 << 0)
80 #define TRANS_FLAGS_HAS_ERROR (1 << 1)
81 volatile unsigned long flags;
84 static void on_trans_done(struct libusb_transfer * trans) {
85 struct trans_ctx * const ctx = trans->user_data;
87 if (trans->status != LIBUSB_TRANSFER_COMPLETED)
88 ctx->flags |= TRANS_FLAGS_HAS_ERROR;
90 ctx->flags |= TRANS_FLAGS_IS_DONE;
93 int submit_wait(struct stlink_libusb *slu, struct libusb_transfer * trans) {
97 struct trans_ctx trans_ctx;
98 enum libusb_error error;
102 /* brief intrusion inside the libusb interface */
103 trans->callback = on_trans_done;
104 trans->user_data = &trans_ctx;
106 if ((error = libusb_submit_transfer(trans))) {
107 printf("libusb_submit_transfer(%d)\n", error);
111 gettimeofday(&start, NULL);
113 while (trans_ctx.flags == 0) {
114 struct timeval timeout;
117 if (libusb_handle_events_timeout(slu->libusb_ctx, &timeout)) {
118 printf("libusb_handle_events()\n");
122 gettimeofday(&now, NULL);
123 timersub(&now, &start, &diff);
124 if (diff.tv_sec >= 3) {
125 printf("libusb_handle_events() timeout\n");
130 if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
131 printf("libusb_handle_events() | has_error\n");
138 ssize_t send_recv(struct stlink_libusb* handle,
139 unsigned char* txbuf, size_t txsize,
140 unsigned char* rxbuf, size_t rxsize) {
141 /* note: txbuf and rxbuf can point to the same area */
143 libusb_fill_bulk_transfer(handle->req_trans, handle->usb_handle,
150 printf("submit_wait(req)\n");
152 if (submit_wait(handle, handle->req_trans)) return -1;
155 if (rxsize == 0) return 0;
157 /* read the response */
159 libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
160 handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
162 printf("submit_wait(rep)\n");
164 if (submit_wait(handle, handle->rep_trans)) return -1;
166 return handle->rep_trans->actual_length;
169 static inline int send_only
170 (struct stlink_libusb* handle, unsigned char* txbuf, size_t txsize) {
171 return send_recv(handle, txbuf, txsize, NULL, 0);
175 // KARL - fixme, common code! (or, one per backend)
176 // candidate for common code...
179 static int is_stlink_device(libusb_device * dev) {
180 struct libusb_device_descriptor desc;
182 if (libusb_get_device_descriptor(dev, &desc))
185 printf("device: 0x%04x, 0x%04x\n", desc.idVendor, desc.idProduct);
187 if (desc.idVendor != 0x0483)
190 if (desc.idProduct != 0x3748)
196 void _stlink_usb_version(stlink_t * sl) {
197 struct stlink_libusb * const slu = sl->backend_data;
198 unsigned char* const buf = sl->q_buf;
201 memset(buf, 0, sizeof (sl->q_buf));
202 buf[0] = STLINK_GET_VERSION;
205 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
207 printf("[!] send_recv\n");
214 for (i = 0; i < size; ++i) printf("%02x", buf[i]);
220 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
221 D(sl, "oops! no write32 support yet ;)\n");
224 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
225 D(sl, "oops! no write8 support yet ;)\n");
230 int stlink_current_mode(stlink_t * sl) {
233 struct stlink_libusb * const slu = sl->backend_data;
234 unsigned char* const buf = sl->q_buf;
237 memset(buf, 0, sizeof (sl->q_buf));
239 buf[0] = STLINK_GET_CURRENT_MODE;
241 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
243 printf("[!] send_recv\n");
247 /* mode = (int)read_uint16(buf, 0); */
251 printf("mode == 0x%x\n", mode);
258 void _stlink_usb_core_id(stlink_t * sl) {
259 struct stlink_libusb * const slu = sl->backend_data;
260 unsigned char* const buf = sl->q_buf;
263 memset(buf, 0, sizeof (sl->q_buf));
264 buf[0] = STLINK_DEBUG_COMMAND;
265 buf[1] = STLINK_DEBUG_READCOREID;
267 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
269 printf("[!] send_recv\n");
273 sl->core_id = read_uint32(buf, 0);
276 void _stlink_usb_status(stlink_t * sl) {
277 struct stlink_libusb * const slu = sl->backend_data;
278 unsigned char* const buf = sl->q_buf;
281 memset(buf, 0, sizeof (sl->q_buf));
283 buf[0] = STLINK_DEBUG_COMMAND;
284 buf[1] = STLINK_DEBUG_GETSTATUS;
286 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
288 printf("[!] send_recv\n");
292 /* todo: stlink_core_stat */
294 // FIXME - decode into sl->core_stat
296 printf("status == 0x%x\n", buf[0]);
301 void _stlink_enter_swd_mode(stlink_t * sl) {
302 struct stlink_libusb * const slu = sl->backend_data;
303 unsigned char* const buf = sl->q_buf;
306 memset(buf, 0, sizeof (sl->q_buf));
308 buf[0] = STLINK_DEBUG_COMMAND;
309 buf[1] = 0x30; /* needed byte */
310 buf[2] = STLINK_DEBUG_ENTER_JTAG;
312 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
314 printf("[!] send_recv\n");
320 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
321 struct stlink_libusb * const slu = sl->backend_data;
322 unsigned char* const buf = sl->q_buf;
325 memset(buf, 0, sizeof (sl->q_buf));
326 buf[0] = STLINK_DFU_COMMAND;
327 buf[1] = STLINK_DFU_EXIT;
329 size = send_only(slu, buf, 16);
331 printf("[!] send_recv\n");
336 void _stlink_usb_reset(stlink_t * sl) {
337 struct stlink_libusb * const slu = sl->backend_data;
338 unsigned char* const buf = sl->q_buf;
341 memset(buf, 0, sizeof (sl->q_buf));
342 buf[0] = STLINK_DEBUG_COMMAND;
343 buf[1] = STLINK_DEBUG_RESETSYS;
345 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
347 printf("[!] send_recv\n");
353 void stlink_step(stlink_t* sl) {
354 struct stlink_libusb * const slu = sl->backend_data;
355 unsigned char* const buf = sl->q_buf;
358 memset(buf, 0, sizeof (sl->q_buf));
359 buf[0] = STLINK_DEBUG_COMMAND;
360 buf[1] = STLINK_DEBUG_STEPCORE;
362 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
364 printf("[!] send_recv\n");
369 void _stlink_usb_run(stlink_t* sl) {
370 struct stlink_libusb * const slu = sl->backend_data;
371 unsigned char* const buf = sl->q_buf;
374 memset(buf, 0, sizeof (sl->q_buf));
375 buf[0] = STLINK_DEBUG_COMMAND;
376 buf[1] = STLINK_DEBUG_RUNCORE;
378 size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
380 printf("[!] send_recv\n");
386 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
387 struct stlink_libusb * const slu = sl->backend_data;
388 unsigned char* const buf = sl->q_buf;
391 memset(buf, 0, sizeof (sl->q_buf));
392 buf[0] = STLINK_DEBUG_COMMAND;
393 buf[1] = STLINK_DEBUG_EXIT;
395 size = send_only(slu, buf, 16);
397 printf("[!] send_only\n");
402 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
403 struct stlink_libusb * const slu = sl->backend_data;
404 unsigned char* const buf = sl->q_buf;
407 /* assume len < sizeof(sl->q_buf) */
409 memset(buf, 0, sizeof (sl->q_buf));
410 buf[0] = STLINK_DEBUG_COMMAND;
411 buf[1] = STLINK_DEBUG_READMEM_32BIT;
412 write_uint32(buf + 2, addr);
413 write_uint16(buf + 6, len);
415 /* windows usb logs show only one byte is used for length ... */
416 buf[6] = (uint8_t) len;
419 size = send_recv(slu, buf, 0x10, buf, sizeof (sl->q_buf));
421 printf("[!] send_recv\n");
425 sl->q_len = (size_t) size;
427 stlink_print_data(sl);
431 stlink_backend_t _stlink_usb_backend = {
433 _stlink_usb_exit_debug_mode,
434 _stlink_enter_swd_mode,
436 _stlink_usb_exit_dfu_mode,
442 _stlink_usb_write_mem32,
443 _stlink_usb_write_mem8
446 stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
448 struct stlink_libusb* slu = NULL;
450 sl = malloc(sizeof (stlink_t));
451 slu = malloc(sizeof (struct stlink_libusb));
452 if (sl == NULL) goto on_error;
453 if (slu == NULL) goto on_error;
455 sl->verbose = verbose;
457 if (slu->libusb_ctx != NULL) {
458 fprintf(stderr, "reopening with an existing context? undefined behaviour!\n");
461 if (libusb_init(&(slu->libusb_ctx))) {
462 fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
469 libusb_device** devs = NULL;
475 count = libusb_get_device_list(slu->libusb_ctx, &devs);
477 printf("libusb_get_device_list\n");
478 goto on_libusb_error;
481 for (i = 0; i < count; ++i) {
483 if (is_stlink_device(dev)) break;
485 if (i == count) return NULL;
487 if (libusb_open(dev, &(slu->usb_handle))) {
488 printf("libusb_open()\n");
489 goto on_libusb_error;
492 if (libusb_get_configuration(slu->usb_handle, &config)) {
493 /* this may fail for a previous configured device */
494 printf("libusb_get_configuration()\n");
495 goto on_libusb_error;
499 printf("setting new configuration (%d -> 1)\n", config);
500 if (libusb_set_configuration(slu->usb_handle, 1)) {
501 /* this may fail for a previous configured device */
502 printf("libusb_set_configuration()\n");
503 goto on_libusb_error;
507 if (libusb_claim_interface(slu->usb_handle, 0)) {
508 printf("libusb_claim_interface()\n");
509 goto on_libusb_error;
512 slu->req_trans = libusb_alloc_transfer(0);
513 if (slu->req_trans == NULL) {
514 printf("libusb_alloc_transfer\n");
515 goto on_libusb_error;
518 slu->rep_trans = libusb_alloc_transfer(0);
519 if (slu->rep_trans == NULL) {
520 printf("libusb_alloc_transfer\n");
521 goto on_libusb_error;
524 slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
525 slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
527 /* libusb_reset_device(slu->usb_handle); */
534 libusb_free_device_list(devs, 1);
535 fprintf(stderr, "freed libusb device list\n");
543 sl->backend = &_stlink_usb_backend;
544 sl->backend_data = slu;
549 if (sl != NULL) free(sl);
550 if (slu != NULL) free(slu);