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 != USB_ST_VID)
 140         return 0;
 141
 142     if (desc.idProduct != USB_STLINK_32L_PID)
 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     DD(sl, "mode == 0x%x\n", mode);
 203     return mode;
 204 }
 205
 206 void _stlink_usb_core_id(stlink_t * sl) {
 207     struct stlink_libusb * const slu = sl->backend_data;
 208     unsigned char* const buf = sl->q_buf;
 209     ssize_t size;
 210
 211     memset(buf, 0, sizeof (sl->q_buf));
 212     buf[0] = STLINK_DEBUG_COMMAND;
 213     buf[1] = STLINK_DEBUG_READCOREID;
 214
 215     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 216     if (size == -1) {
 217         printf("[!] send_recv\n");
 218         return;
 219     }
 220
 221     sl->core_id = read_uint32(buf, 0);
 222 }
 223
 224 void _stlink_usb_status(stlink_t * sl) {
 225     struct stlink_libusb * const slu = sl->backend_data;
 226     unsigned char* const buf = sl->q_buf;
 227     ssize_t size;
 228
 229     memset(buf, 0, sizeof (sl->q_buf));
 230
 231     buf[0] = STLINK_DEBUG_COMMAND;
 232     buf[1] = STLINK_DEBUG_GETSTATUS;
 233
 234     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 235     if (size == -1) {
 236         printf("[!] send_recv\n");
 237         return;
 238     }
 239
 240     /* todo: stlink_core_stat */
 241
 242     // FIXME - decode into sl->core_stat
 243 #if 1 /* DEBUG */
 244     printf("status == 0x%x\n", buf[0]);
 245 #endif /* DEBUG */
 246
 247 }
 248
 249 void _stlink_enter_swd_mode(stlink_t * sl) {
 250     struct stlink_libusb * const slu = sl->backend_data;
 251     unsigned char* const buf = sl->q_buf;
 252     ssize_t size;
 253
 254     memset(buf, 0, sizeof (sl->q_buf));
 255
 256     buf[0] = STLINK_DEBUG_COMMAND;
 257     buf[1] = 0x30; /* needed byte */
 258     buf[2] = STLINK_DEBUG_ENTER_JTAG;
 259
 260     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 261     if (size == -1) {
 262         printf("[!] send_recv\n");
 263         return;
 264     }
 265
 266 }
 267
 268 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
 269     struct stlink_libusb * const slu = sl->backend_data;
 270     unsigned char* const buf = sl->q_buf;
 271     ssize_t size;
 272
 273     memset(buf, 0, sizeof (sl->q_buf));
 274     buf[0] = STLINK_DFU_COMMAND;
 275     buf[1] = STLINK_DFU_EXIT;
 276
 277     size = send_only(slu, buf, 16);
 278     if (size == -1) {
 279         printf("[!] send_recv\n");
 280         return;
 281     }
 282 }
 283
 284 void _stlink_usb_reset(stlink_t * sl) {
 285     struct stlink_libusb * const slu = sl->backend_data;
 286     unsigned char* const buf = sl->q_buf;
 287     ssize_t size;
 288
 289     memset(buf, 0, sizeof (sl->q_buf));
 290     buf[0] = STLINK_DEBUG_COMMAND;
 291     buf[1] = STLINK_DEBUG_RESETSYS;
 292
 293     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 294     if (size == -1) {
 295         printf("[!] send_recv\n");
 296         return;
 297     }
 298 }
 299
 300
 301 void _stlink_usb_step(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     buf[0] = STLINK_DEBUG_COMMAND;
 308     buf[1] = STLINK_DEBUG_STEPCORE;
 309
 310     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 311     if (size == -1) {
 312         printf("[!] send_recv\n");
 313         return;
 314     }
 315 }
 316
 317 void _stlink_usb_run(stlink_t* sl) {
 318     struct stlink_libusb * const slu = sl->backend_data;
 319     unsigned char* const buf = sl->q_buf;
 320     ssize_t size;
 321
 322     memset(buf, 0, sizeof (sl->q_buf));
 323     buf[0] = STLINK_DEBUG_COMMAND;
 324     buf[1] = STLINK_DEBUG_RUNCORE;
 325
 326     size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
 327     if (size == -1) {
 328         printf("[!] send_recv\n");
 329         return;
 330     }
 331
 332 }
 333
 334 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
 335     struct stlink_libusb * const slu = sl->backend_data;
 336     unsigned char* const buf = sl->q_buf;
 337     ssize_t size;
 338
 339     memset(buf, 0, sizeof (sl->q_buf));
 340     buf[0] = STLINK_DEBUG_COMMAND;
 341     buf[1] = STLINK_DEBUG_EXIT;
 342
 343     size = send_only(slu, buf, 16);
 344     if (size == -1) {
 345         printf("[!] send_only\n");
 346         return;
 347     }
 348 }
 349
 350 void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 351     struct stlink_libusb * const slu = sl->backend_data;
 352     unsigned char* const buf = sl->q_buf;
 353     ssize_t size;
 354
 355     /* assume len < sizeof(sl->q_buf) */
 356
 357     memset(buf, 0, sizeof (sl->q_buf));
 358     buf[0] = STLINK_DEBUG_COMMAND;
 359     buf[1] = STLINK_DEBUG_READMEM_32BIT;
 360     write_uint32(buf + 2, addr);
 361     write_uint16(buf + 6, len);
 362 #if 0
 363     /* windows usb logs show only one byte is used for length ... */
 364     buf[6] = (uint8_t) len;
 365 #endif
 366
 367     size = send_recv(slu, buf, 0x10, buf, sizeof (sl->q_buf));
 368     if (size == -1) {
 369         printf("[!] send_recv\n");
 370         return;
 371     }
 372
 373     sl->q_len = (size_t) size;
 374
 375     stlink_print_data(sl);
 376 }
 377
 378 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
 379     DD(sl, "oops! read_all_regs not implemented for USB!\n");
 380 }
 381
 382 void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 383     DD(sl, "oops! read_reg not implemented for USB! Wanted to read reg %d\n",
 384             r_idx);
 385 }
 386
 387 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 388     DD(sl, "oops! write_reg not implemented for USB! Wanted to write %#x to %d\n",
 389             reg, idx);
 390 }
 391
 392
 393
 394 stlink_backend_t _stlink_usb_backend = {
 395     _stlink_usb_close,
 396     _stlink_usb_exit_debug_mode,
 397     _stlink_enter_swd_mode,
 398     NULL,
 399     _stlink_usb_exit_dfu_mode,
 400     _stlink_usb_core_id,
 401     _stlink_usb_reset,
 402     _stlink_usb_run,
 403     _stlink_usb_status,
 404     _stlink_usb_version,
 405     _stlink_usb_read_mem32,
 406     _stlink_usb_write_mem32,
 407     _stlink_usb_write_mem8,
 408     _stlink_usb_read_all_regs,
 409     _stlink_usb_read_reg,
 410     _stlink_usb_write_reg,
 411     _stlink_usb_step,
 412     _stlink_usb_current_mode
 413 };
 414
 415
 416 stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
 417     stlink_t* sl = NULL;
 418     struct stlink_libusb* slu = NULL;
 419
 420     sl = malloc(sizeof (stlink_t));
 421     slu = malloc(sizeof (struct stlink_libusb));
 422     if (sl == NULL) goto on_error;
 423     if (slu == NULL) goto on_error;
 424
 425     sl->verbose = verbose;
 426
 427     if (slu->libusb_ctx != NULL) {
 428         fprintf(stderr, "reopening with an existing context? undefined behaviour!\n");
 429         goto on_error;
 430     } else {
 431         if (libusb_init(&(slu->libusb_ctx))) {
 432             fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
 433             goto on_error;
 434         }
 435     }
 436
 437     int error = -1;
 438
 439     libusb_device** devs = NULL;
 440     libusb_device* dev;
 441     ssize_t i;
 442     ssize_t count;
 443     int config;
 444
 445     count = libusb_get_device_list(slu->libusb_ctx, &devs);
 446     if (count < 0) {
 447         printf("libusb_get_device_list\n");
 448         goto on_libusb_error;
 449     }
 450
 451     for (i = 0; i < count; ++i) {
 452         dev = devs[i];
 453         if (is_stlink_device(dev)) break;
 454     }
 455     if (i == count) return NULL;
 456
 457     if (libusb_open(dev, &(slu->usb_handle))) {
 458         printf("libusb_open()\n");
 459         goto on_libusb_error;
 460     }
 461
 462     if (libusb_get_configuration(slu->usb_handle, &config)) {
 463         /* this may fail for a previous configured device */
 464         printf("libusb_get_configuration()\n");
 465         goto on_libusb_error;
 466     }
 467
 468     if (config != 1) {
 469         printf("setting new configuration (%d -> 1)\n", config);
 470         if (libusb_set_configuration(slu->usb_handle, 1)) {
 471             /* this may fail for a previous configured device */
 472             printf("libusb_set_configuration()\n");
 473             goto on_libusb_error;
 474         }
 475     }
 476
 477     if (libusb_claim_interface(slu->usb_handle, 0)) {
 478         printf("libusb_claim_interface()\n");
 479         goto on_libusb_error;
 480     }
 481
 482     slu->req_trans = libusb_alloc_transfer(0);
 483     if (slu->req_trans == NULL) {
 484         printf("libusb_alloc_transfer\n");
 485         goto on_libusb_error;
 486     }
 487
 488     slu->rep_trans = libusb_alloc_transfer(0);
 489     if (slu->rep_trans == NULL) {
 490         printf("libusb_alloc_transfer\n");
 491         goto on_libusb_error;
 492     }
 493
 494     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 495     slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 496
 497     /* libusb_reset_device(slu->usb_handle); */
 498
 499     /* success */
 500     error = 0;
 501
 502 on_libusb_error:
 503     if (devs != NULL) {
 504         libusb_free_device_list(devs, 1);
 505         fprintf(stderr, "freed libusb device list\n");
 506     }
 507
 508     if (error == -1) {
 509         stlink_close(sl);
 510         return NULL;
 511     }
 512
 513     sl->backend = &_stlink_usb_backend;
 514     sl->backend_data = slu;
 515     /* success */
 516     return sl;
 517
 518 on_error:
 519     if (sl != NULL) free(sl);
 520     if (slu != NULL) free(slu);
 521     return 0;
 522 }