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