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