git.gag.com Git - fw/stlink/blob - src/stlink-usb.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <stdint.h>
   5 #include <time.h>
   6 #include <sys/types.h>
   7 #include <libusb-1.0/libusb.h>
   8 #include "stlink-common.h"
   9 #include "stlink-usb.h"
  10
  11 void _stlink_usb_close(stlink_t* sl) {
  12     struct stlink_libusb * const handle = sl->backend_data;
  13     // maybe we couldn't even get the usb device?
  14     if (handle != NULL) {
  15         if (handle->req_trans != NULL)
  16             libusb_free_transfer(handle->req_trans);
  17
  18         if (handle->rep_trans != NULL)
  19             libusb_free_transfer(handle->rep_trans);
  20
  21         if (handle->usb_handle != NULL)
  22             libusb_close(handle->usb_handle);
  23
  24         libusb_exit(handle->libusb_ctx);
  25         free(handle);
  26     }
  27 }
  28
  29 static void write_uint32(unsigned char* buf, uint32_t ui) {
  30     if (!is_bigendian()) { // le -> le (don't swap)
  31         buf[0] = ((unsigned char*) &ui)[0];
  32         buf[1] = ((unsigned char*) &ui)[1];
  33         buf[2] = ((unsigned char*) &ui)[2];
  34         buf[3] = ((unsigned char*) &ui)[3];
  35     } else {
  36         buf[0] = ((unsigned char*) &ui)[3];
  37         buf[1] = ((unsigned char*) &ui)[2];
  38         buf[2] = ((unsigned char*) &ui)[1];
  39         buf[3] = ((unsigned char*) &ui)[0];
  40     }
  41 }
  42
  43 static void write_uint16(unsigned char* buf, uint16_t ui) {
  44     if (!is_bigendian()) { // le -> le (don't swap)
  45         buf[0] = ((unsigned char*) &ui)[0];
  46         buf[1] = ((unsigned char*) &ui)[1];
  47     } else {
  48         buf[0] = ((unsigned char*) &ui)[1];
  49         buf[1] = ((unsigned char*) &ui)[0];
  50     }
  51 }
  52
  53 static uint32_t read_uint32(const unsigned char *c, const int pt) {
  54     uint32_t ui;
  55     char *p = (char *) &ui;
  56
  57     if (!is_bigendian()) { // le -> le (don't swap)
  58         p[0] = c[pt];
  59         p[1] = c[pt + 1];
  60         p[2] = c[pt + 2];
  61         p[3] = c[pt + 3];
  62     } else {
  63         p[0] = c[pt + 3];
  64         p[1] = c[pt + 2];
  65         p[2] = c[pt + 1];
  66         p[3] = c[pt];
  67     }
  68     return ui;
  69 }
  70
  71
  72 struct trans_ctx {
  73 #define TRANS_FLAGS_IS_DONE (1 << 0)
  74 #define TRANS_FLAGS_HAS_ERROR (1 << 1)
  75     volatile unsigned long flags;
  76 };
  77
  78 static void on_trans_done(struct libusb_transfer * trans) {
  79     struct trans_ctx * const ctx = trans->user_data;
  80
  81     if (trans->status != LIBUSB_TRANSFER_COMPLETED)
  82         ctx->flags |= TRANS_FLAGS_HAS_ERROR;
  83
  84     ctx->flags |= TRANS_FLAGS_IS_DONE;
  85 }
  86
  87 int submit_wait(struct stlink_libusb *slu, struct libusb_transfer * trans) {
  88     struct timeval start;
  89     struct timeval now;
  90     struct timeval diff;
  91     struct trans_ctx trans_ctx;
  92     enum libusb_error error;
  93
  94     trans_ctx.flags = 0;
  95
  96     /* brief intrusion inside the libusb interface */
  97     trans->callback = on_trans_done;
  98     trans->user_data = &trans_ctx;
  99
 100     if ((error = libusb_submit_transfer(trans))) {
 101         printf("libusb_submit_transfer(%d)\n", error);
 102         return -1;
 103     }
 104
 105     gettimeofday(&start, NULL);
 106
 107     while (trans_ctx.flags == 0) {
 108         struct timeval timeout;
 109         timeout.tv_sec = 3;
 110         timeout.tv_usec = 0;
 111         if (libusb_handle_events_timeout(slu->libusb_ctx, &timeout)) {
 112             printf("libusb_handle_events()\n");
 113             return -1;
 114         }
 115
 116         gettimeofday(&now, NULL);
 117         timersub(&now, &start, &diff);
 118         if (diff.tv_sec >= 3) {
 119             printf("libusb_handle_events() timeout\n");
 120             return -1;
 121         }
 122     }
 123
 124     if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
 125         printf("libusb_handle_events() | has_error\n");
 126         return -1;
 127     }
 128
 129     return 0;
 130 }
 131
 132 ssize_t send_recv(struct stlink_libusb* handle,
 133         unsigned char* txbuf, size_t txsize,
 134         unsigned char* rxbuf, size_t rxsize) {
 135     /* note: txbuf and rxbuf can point to the same area */
 136
 137     libusb_fill_bulk_transfer(handle->req_trans, handle->usb_handle,
 138             handle->ep_req,
 139             txbuf, txsize,
 140             NULL, NULL,
 141             0
 142             );
 143
 144     printf("submit_wait(req)\n");
 145
 146     if (submit_wait(handle, handle->req_trans)) return -1;
 147
 148     /* send_only */
 149     if (rxsize == 0) return 0;
 150
 151     /* read the response */
 152
 153     libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
 154             handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
 155
 156     printf("submit_wait(rep)\n");
 157
 158     if (submit_wait(handle, handle->rep_trans)) return -1;
 159
 160     return handle->rep_trans->actual_length;
 161 }
 162
 163 static inline int send_only
 164 (struct stlink_libusb* handle, unsigned char* txbuf, size_t txsize) {
 165     return send_recv(handle, txbuf, txsize, NULL, 0);
 166 }
 167
 168
 169 // KARL - fixme, common code! (or, one per backend)
 170 // candidate for common code...
 171
 172
 173 static int is_stlink_device(libusb_device * dev) {
 174     struct libusb_device_descriptor desc;
 175
 176     if (libusb_get_device_descriptor(dev, &desc))
 177         return 0;
 178
 179     printf("device: 0x%04x, 0x%04x\n", desc.idVendor, desc.idProduct);
 180
 181     if (desc.idVendor != 0x0483)
 182         return 0;
 183
 184     if (desc.idProduct != 0x3748)
 185         return 0;
 186
 187     return 1;
 188 }
 189
 190 void _stlink_usb_version(stlink_t * sl) {
 191     struct stlink_libusb * const slu = sl->backend_data;
 192     unsigned char* const buf = sl->q_buf;
 193     ssize_t size;
 194
 195     memset(buf, 0, sizeof (sl->q_buf));
 196     buf[0] = STLINK_GET_VERSION;
 197     buf[1] = 0x80;
 198
 199     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 200     if (size == -1) {
 201         printf("[!] send_recv\n");
 202         return;
 203     }
 204
 205 #if 1 /* DEBUG */
 206     {
 207         unsigned int i;
 208         for (i = 0; i < size; ++i) printf("%02x", buf[i]);
 209         printf("\n");
 210     }
 211 #endif /* DEBUG */
 212 }
 213
 214 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 215     DD(sl, "oops! no write32 support yet, wanted to write %d bytes to %#x\n",
 216             len, addr);
 217 }
 218
 219 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 220     DD(sl, "oops! no write8 support yet, wanted to write %d bytes to %#x\n",
 221             len, addr);
 222 }
 223
 224
 225 int stlink_current_mode(stlink_t * sl) {
 226     int mode = -1;
 227
 228     struct stlink_libusb * const slu = sl->backend_data;
 229     unsigned char* const buf = sl->q_buf;
 230     ssize_t size;
 231
 232     memset(buf, 0, sizeof (sl->q_buf));
 233
 234     buf[0] = STLINK_GET_CURRENT_MODE;
 235
 236     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 237     if (size == -1) {
 238         printf("[!] send_recv\n");
 239         return -1;
 240     }
 241
 242     /* mode = (int)read_uint16(buf, 0); */
 243     mode = (int) buf[0];
 244
 245 #if 1 /* DEBUG */
 246     printf("mode == 0x%x\n", mode);
 247 #endif /* DEBUG */
 248
 249
 250     return mode;
 251 }
 252
 253 void _stlink_usb_core_id(stlink_t * sl) {
 254     struct stlink_libusb * const slu = sl->backend_data;
 255     unsigned char* const buf = sl->q_buf;
 256     ssize_t size;
 257
 258     memset(buf, 0, sizeof (sl->q_buf));
 259     buf[0] = STLINK_DEBUG_COMMAND;
 260     buf[1] = STLINK_DEBUG_READCOREID;
 261
 262     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 263     if (size == -1) {
 264         printf("[!] send_recv\n");
 265         return;
 266     }
 267
 268     sl->core_id = read_uint32(buf, 0);
 269 }
 270
 271 void _stlink_usb_status(stlink_t * sl) {
 272     struct stlink_libusb * const slu = sl->backend_data;
 273     unsigned char* const buf = sl->q_buf;
 274     ssize_t size;
 275
 276     memset(buf, 0, sizeof (sl->q_buf));
 277
 278     buf[0] = STLINK_DEBUG_COMMAND;
 279     buf[1] = STLINK_DEBUG_GETSTATUS;
 280
 281     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 282     if (size == -1) {
 283         printf("[!] send_recv\n");
 284         return;
 285     }
 286
 287     /* todo: stlink_core_stat */
 288
 289     // FIXME - decode into sl->core_stat
 290 #if 1 /* DEBUG */
 291     printf("status == 0x%x\n", buf[0]);
 292 #endif /* DEBUG */
 293
 294 }
 295
 296 void _stlink_enter_swd_mode(stlink_t * sl) {
 297     struct stlink_libusb * const slu = sl->backend_data;
 298     unsigned char* const buf = sl->q_buf;
 299     ssize_t size;
 300
 301     memset(buf, 0, sizeof (sl->q_buf));
 302
 303     buf[0] = STLINK_DEBUG_COMMAND;
 304     buf[1] = 0x30; /* needed byte */
 305     buf[2] = STLINK_DEBUG_ENTER_JTAG;
 306
 307     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 308     if (size == -1) {
 309         printf("[!] send_recv\n");
 310         return;
 311     }
 312
 313 }
 314
 315 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
 316     struct stlink_libusb * const slu = sl->backend_data;
 317     unsigned char* const buf = sl->q_buf;
 318     ssize_t size;
 319
 320     memset(buf, 0, sizeof (sl->q_buf));
 321     buf[0] = STLINK_DFU_COMMAND;
 322     buf[1] = STLINK_DFU_EXIT;
 323
 324     size = send_only(slu, buf, 16);
 325     if (size == -1) {
 326         printf("[!] send_recv\n");
 327         return;
 328     }
 329 }
 330
 331 void _stlink_usb_reset(stlink_t * sl) {
 332     struct stlink_libusb * const slu = sl->backend_data;
 333     unsigned char* const buf = sl->q_buf;
 334     ssize_t size;
 335
 336     memset(buf, 0, sizeof (sl->q_buf));
 337     buf[0] = STLINK_DEBUG_COMMAND;
 338     buf[1] = STLINK_DEBUG_RESETSYS;
 339
 340     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 341     if (size == -1) {
 342         printf("[!] send_recv\n");
 343         return;
 344     }
 345 }
 346
 347
 348 void stlink_step(stlink_t* sl) {
 349     struct stlink_libusb * const slu = sl->backend_data;
 350     unsigned char* const buf = sl->q_buf;
 351     ssize_t size;
 352
 353     memset(buf, 0, sizeof (sl->q_buf));
 354     buf[0] = STLINK_DEBUG_COMMAND;
 355     buf[1] = STLINK_DEBUG_STEPCORE;
 356
 357     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 358     if (size == -1) {
 359         printf("[!] send_recv\n");
 360         return;
 361     }
 362 }
 363
 364 void _stlink_usb_run(stlink_t* sl) {
 365     struct stlink_libusb * const slu = sl->backend_data;
 366     unsigned char* const buf = sl->q_buf;
 367     ssize_t size;
 368
 369     memset(buf, 0, sizeof (sl->q_buf));
 370     buf[0] = STLINK_DEBUG_COMMAND;
 371     buf[1] = STLINK_DEBUG_RUNCORE;
 372
 373     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 374     if (size == -1) {
 375         printf("[!] send_recv\n");
 376         return;
 377     }
 378
 379 }
 380
 381 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
 382     struct stlink_libusb * const slu = sl->backend_data;
 383     unsigned char* const buf = sl->q_buf;
 384     ssize_t size;
 385
 386     memset(buf, 0, sizeof (sl->q_buf));
 387     buf[0] = STLINK_DEBUG_COMMAND;
 388     buf[1] = STLINK_DEBUG_EXIT;
 389
 390     size = send_only(slu, buf, 16);
 391     if (size == -1) {
 392         printf("[!] send_only\n");
 393         return;
 394     }
 395 }
 396
 397 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 398     struct stlink_libusb * const slu = sl->backend_data;
 399     unsigned char* const buf = sl->q_buf;
 400     ssize_t size;
 401
 402     /* assume len < sizeof(sl->q_buf) */
 403
 404     memset(buf, 0, sizeof (sl->q_buf));
 405     buf[0] = STLINK_DEBUG_COMMAND;
 406     buf[1] = STLINK_DEBUG_READMEM_32BIT;
 407     write_uint32(buf + 2, addr);
 408     write_uint16(buf + 6, len);
 409 #if 0
 410     /* windows usb logs show only one byte is used for length ... */
 411     buf[6] = (uint8_t) len;
 412 #endif
 413
 414     size = send_recv(slu, buf, 0x10, buf, sizeof (sl->q_buf));
 415     if (size == -1) {
 416         printf("[!] send_recv\n");
 417         return;
 418     }
 419
 420     sl->q_len = (size_t) size;
 421
 422     stlink_print_data(sl);
 423 }
 424
 425
 426 stlink_backend_t _stlink_usb_backend = {
 427     _stlink_usb_close,
 428     _stlink_usb_exit_debug_mode,
 429     _stlink_enter_swd_mode,
 430     NULL,
 431     _stlink_usb_exit_dfu_mode,
 432     _stlink_usb_core_id,
 433     _stlink_usb_reset,
 434     _stlink_usb_run,
 435     _stlink_usb_status,
 436     _stlink_usb_version,
 437     _stlink_usb_write_mem32,
 438     _stlink_usb_write_mem8
 439 };
 440
 441 stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
 442     stlink_t* sl = NULL;
 443     struct stlink_libusb* slu = NULL;
 444
 445     sl = malloc(sizeof (stlink_t));
 446     slu = malloc(sizeof (struct stlink_libusb));
 447     if (sl == NULL) goto on_error;
 448     if (slu == NULL) goto on_error;
 449
 450     sl->verbose = verbose;
 451
 452     if (slu->libusb_ctx != NULL) {
 453         fprintf(stderr, "reopening with an existing context? undefined behaviour!\n");
 454         goto on_error;
 455     } else {
 456         if (libusb_init(&(slu->libusb_ctx))) {
 457             fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
 458             goto on_error;
 459         }
 460     }
 461
 462     int error = -1;
 463
 464     libusb_device** devs = NULL;
 465     libusb_device* dev;
 466     ssize_t i;
 467     ssize_t count;
 468     int config;
 469
 470     count = libusb_get_device_list(slu->libusb_ctx, &devs);
 471     if (count < 0) {
 472         printf("libusb_get_device_list\n");
 473         goto on_libusb_error;
 474     }
 475
 476     for (i = 0; i < count; ++i) {
 477         dev = devs[i];
 478         if (is_stlink_device(dev)) break;
 479     }
 480     if (i == count) return NULL;
 481
 482     if (libusb_open(dev, &(slu->usb_handle))) {
 483         printf("libusb_open()\n");
 484         goto on_libusb_error;
 485     }
 486
 487     if (libusb_get_configuration(slu->usb_handle, &config)) {
 488         /* this may fail for a previous configured device */
 489         printf("libusb_get_configuration()\n");
 490         goto on_libusb_error;
 491     }
 492
 493     if (config != 1) {
 494         printf("setting new configuration (%d -> 1)\n", config);
 495         if (libusb_set_configuration(slu->usb_handle, 1)) {
 496             /* this may fail for a previous configured device */
 497             printf("libusb_set_configuration()\n");
 498             goto on_libusb_error;
 499         }
 500     }
 501
 502     if (libusb_claim_interface(slu->usb_handle, 0)) {
 503         printf("libusb_claim_interface()\n");
 504         goto on_libusb_error;
 505     }
 506
 507     slu->req_trans = libusb_alloc_transfer(0);
 508     if (slu->req_trans == NULL) {
 509         printf("libusb_alloc_transfer\n");
 510         goto on_libusb_error;
 511     }
 512
 513     slu->rep_trans = libusb_alloc_transfer(0);
 514     if (slu->rep_trans == NULL) {
 515         printf("libusb_alloc_transfer\n");
 516         goto on_libusb_error;
 517     }
 518
 519     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 520     slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 521
 522     /* libusb_reset_device(slu->usb_handle); */
 523
 524     /* success */
 525     error = 0;
 526
 527 on_libusb_error:
 528     if (devs != NULL) {
 529         libusb_free_device_list(devs, 1);
 530         fprintf(stderr, "freed libusb device list\n");
 531     }
 532
 533     if (error == -1) {
 534         stlink_close(sl);
 535         return NULL;
 536     }
 537
 538     sl->backend = &_stlink_usb_backend;
 539     sl->backend_data = slu;
 540     /* success */
 541     return sl;
 542
 543 on_error:
 544     if (sl != NULL) free(sl);
 545     if (slu != NULL) free(slu);
 546     return 0;
 547 }