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

   1 #include <assert.h>
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <stdint.h>
   6 #include <time.h>
   7 #include <sys/types.h>
   8 #include <libusb-1.0/libusb.h>
   9 #include "stlink-common.h"
  10 #include "stlink-usb.h"
  11
  12 enum SCSI_Generic_Direction {SG_DXFER_TO_DEV=0, SG_DXFER_FROM_DEV=0x80};
  13
  14 void _stlink_usb_close(stlink_t* sl) {
  15     struct stlink_libusb * const handle = sl->backend_data;
  16     // maybe we couldn't even get the usb device?
  17     if (handle != NULL) {
  18         if (handle->req_trans != NULL)
  19             libusb_free_transfer(handle->req_trans);
  20
  21         if (handle->rep_trans != NULL)
  22             libusb_free_transfer(handle->rep_trans);
  23
  24         if (handle->usb_handle != NULL) {
  25             libusb_close(handle->usb_handle);
  26         }
  27
  28         libusb_exit(handle->libusb_ctx);
  29         free(handle);
  30     }
  31 }
  32
  33
  34 struct trans_ctx {
  35 #define TRANS_FLAGS_IS_DONE (1 << 0)
  36 #define TRANS_FLAGS_HAS_ERROR (1 << 1)
  37     volatile unsigned long flags;
  38 };
  39
  40 static void on_trans_done(struct libusb_transfer * trans) {
  41     struct trans_ctx * const ctx = trans->user_data;
  42
  43     if (trans->status != LIBUSB_TRANSFER_COMPLETED)
  44         ctx->flags |= TRANS_FLAGS_HAS_ERROR;
  45
  46     ctx->flags |= TRANS_FLAGS_IS_DONE;
  47 }
  48
  49 int submit_wait(struct stlink_libusb *slu, struct libusb_transfer * trans) {
  50     struct timeval start;
  51     struct timeval now;
  52     struct timeval diff;
  53     struct trans_ctx trans_ctx;
  54     enum libusb_error error;
  55
  56     trans_ctx.flags = 0;
  57
  58     /* brief intrusion inside the libusb interface */
  59     trans->callback = on_trans_done;
  60     trans->user_data = &trans_ctx;
  61
  62     if ((error = libusb_submit_transfer(trans))) {
  63         printf("libusb_submit_transfer(%d)\n", error);
  64         return -1;
  65     }
  66
  67     gettimeofday(&start, NULL);
  68
  69     while (trans_ctx.flags == 0) {
  70         struct timeval timeout;
  71         timeout.tv_sec = 3;
  72         timeout.tv_usec = 0;
  73         if (libusb_handle_events_timeout(slu->libusb_ctx, &timeout)) {
  74             printf("libusb_handle_events()\n");
  75             return -1;
  76         }
  77
  78         gettimeofday(&now, NULL);
  79         timersub(&now, &start, &diff);
  80         if (diff.tv_sec >= 3) {
  81             printf("libusb_handle_events() timeout\n");
  82             return -1;
  83         }
  84     }
  85
  86     if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
  87         printf("libusb_handle_events() | has_error\n");
  88         return -1;
  89     }
  90
  91     return 0;
  92 }
  93
  94 ssize_t send_recv(struct stlink_libusb* handle, int terminate,
  95         unsigned char* txbuf, size_t txsize,
  96         unsigned char* rxbuf, size_t rxsize) {
  97     /* note: txbuf and rxbuf can point to the same area */
  98     int res = 0;
  99
 100     libusb_fill_bulk_transfer(handle->req_trans, handle->usb_handle,
 101             handle->ep_req,
 102             txbuf, txsize,
 103             NULL, NULL,
 104             0
 105             );
 106
 107     if (submit_wait(handle, handle->req_trans)) return -1;
 108
 109     /* send_only */
 110     if (rxsize != 0) {
 111
 112         /* read the response */
 113
 114         libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
 115                                   handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
 116
 117         if (submit_wait(handle, handle->rep_trans)) return -1;
 118         res = handle->rep_trans->actual_length;
 119     }
 120     if ((handle->protocoll == 1) && terminate) {
 121         /* Read the SG reply */
 122         unsigned char sg_buf[13];
 123         libusb_fill_bulk_transfer
 124             (handle->rep_trans, handle->usb_handle,
 125              handle->ep_rep, sg_buf, 13, NULL, NULL, 0);
 126         res = submit_wait(handle, handle->rep_trans);
 127         /* The STLink doesn't seem to evaluate the sequence number */
 128         handle->sg_transfer_idx++;
 129         if (res ) return -1;
 130     }
 131
 132     return handle->rep_trans->actual_length;
 133 }
 134
 135 static inline int send_only
 136 (struct stlink_libusb* handle, int terminate,
 137  unsigned char* txbuf, size_t txsize) {
 138     return send_recv(handle, terminate, txbuf, txsize, NULL, 0);
 139 }
 140
 141
 142 /* Search for a STLINK device, either any or teh one with the given PID
 143  * Return the protocoll version
 144  */
 145 static int is_stlink_device(libusb_device * dev, uint16_t pid) {
 146     struct libusb_device_descriptor desc;
 147     int version;
 148
 149     if (libusb_get_device_descriptor(dev, &desc))
 150         return 0;
 151
 152     if (desc.idVendor != USB_ST_VID)
 153         return 0;
 154
 155     if ((desc.idProduct != USB_STLINK_32L_PID) &&
 156         (desc.idProduct != USB_STLINK_PID ))
 157         return 0;
 158
 159     if(pid && (pid != desc.idProduct))
 160         return 0;
 161     if (desc.idProduct == USB_STLINK_PID )
 162         version = 1;
 163     else
 164         version = 2;
 165
 166     return version;
 167 }
 168
 169 static int fill_command
 170 (stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t len) {
 171     struct stlink_libusb * const slu = sl->backend_data;
 172     unsigned char* const cmd = sl->c_buf;
 173     int i = 0;
 174     memset(cmd, 0, sizeof (sl->q_buf));
 175     if(slu->protocoll == 1) {
 176         cmd[i++] = 'U';
 177         cmd[i++] = 'S';
 178         cmd[i++] = 'B';
 179         cmd[i++] = 'C';
 180         write_uint32(&cmd[i], slu->sg_transfer_idx);
 181         i+= 4;
 182         write_uint32(&cmd[i], len);
 183         i+= 4;
 184         cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
 185         cmd[i++] = 0; /* Logical unit */
 186         cmd[i++] = 0xa; /* Command length */
 187     }
 188     return i;
 189 }
 190
 191 void _stlink_usb_version(stlink_t *sl) {
 192     struct stlink_libusb * const slu = sl->backend_data;
 193     unsigned char* const data = sl->q_buf;
 194     unsigned char* const cmd  = sl->c_buf;
 195     ssize_t size;
 196     uint32_t rep_len = 6;
 197     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 198
 199     cmd[i++] = STLINK_GET_VERSION;
 200     cmd[i++] = 0x80;
 201
 202     size = send_recv(slu, 1, cmd, slu->cmd_len, data, 6);
 203     if (size == -1) {
 204         printf("[!] send_recv\n");
 205         return;
 206     }
 207 }
 208
 209 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 210     struct stlink_libusb * const slu = sl->backend_data;
 211     unsigned char* const data = sl->q_buf;
 212     unsigned char* const cmd  = sl->c_buf;
 213
 214     int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
 215     cmd[i++] = STLINK_DEBUG_COMMAND;
 216     cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
 217     write_uint32(&cmd[i], addr);
 218     i += 4;
 219     write_uint16(&cmd[i], len);
 220     send_only(slu, 0, cmd, slu->cmd_len);
 221
 222 #if Q_BUF_LEN < UINT16_MAX
 223     assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
 224 #endif
 225     assert((len & 3) == 0);
 226     send_only(slu, 1, data, len);
 227 }
 228
 229 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 230     struct stlink_libusb * const slu = sl->backend_data;
 231     unsigned char* const data = sl->q_buf;
 232     unsigned char* const cmd  = sl->c_buf;
 233
 234     int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
 235     cmd[i++] = STLINK_DEBUG_COMMAND;
 236     cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
 237     write_uint32(&cmd[i], addr);
 238     i += 4;
 239     write_uint16(&cmd[i], len);
 240     send_only(slu, 0, cmd, slu->cmd_len);
 241
 242 #if Q_BUF_LEN < UINT16_MAX
 243     assert(len < sizeof(sl->q_buf));  // makes a compiler warning? always true?
 244 #endif
 245     send_only(slu, 1, data, len);
 246 }
 247
 248
 249 int _stlink_usb_current_mode(stlink_t * sl) {
 250     struct stlink_libusb * const slu = sl->backend_data;
 251     unsigned char* const cmd  = sl->c_buf;
 252     unsigned char* const data = sl->q_buf;
 253     ssize_t size;
 254     int rep_len = 2;
 255     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 256
 257     cmd[i++] = STLINK_GET_CURRENT_MODE;
 258     size = send_recv(slu, 1, cmd,  slu->cmd_len, data, rep_len);
 259     if (size == -1) {
 260         printf("[!] send_recv\n");
 261         return -1;
 262     }
 263     return sl->q_buf[0];
 264 }
 265
 266 void _stlink_usb_core_id(stlink_t * sl) {
 267     struct stlink_libusb * const slu = sl->backend_data;
 268     unsigned char* const cmd  = sl->c_buf;
 269     unsigned char* const data = sl->q_buf;
 270     ssize_t size;
 271     int rep_len = 4;
 272     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 273
 274     cmd[i++] = STLINK_DEBUG_COMMAND;
 275     cmd[i++] = STLINK_DEBUG_READCOREID;
 276
 277     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 278     if (size == -1) {
 279         printf("[!] send_recv\n");
 280         return;
 281     }
 282
 283     sl->core_id = read_uint32(data, 0);
 284 }
 285
 286 void _stlink_usb_status(stlink_t * sl) {
 287     struct stlink_libusb * const slu = sl->backend_data;
 288     unsigned char* const data = sl->q_buf;
 289     unsigned char* const cmd  = sl->c_buf;
 290     ssize_t size;
 291     int rep_len = 2;
 292     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 293
 294     cmd[i++] = STLINK_DEBUG_COMMAND;
 295     cmd[i++] = STLINK_DEBUG_GETSTATUS;
 296
 297     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 298     if (size == -1) {
 299         printf("[!] send_recv\n");
 300         return;
 301     }
 302 }
 303
 304 void _stlink_usb_force_debug(stlink_t *sl) {
 305     struct stlink_libusb *slu = sl->backend_data;
 306     unsigned char* const data = sl->q_buf;
 307     unsigned char* const cmd  = sl->c_buf;
 308     ssize_t size;
 309     int rep_len = 2;
 310     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 311
 312     cmd[i++] = STLINK_DEBUG_COMMAND;
 313     cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
 314     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 315     if (size == -1) {
 316         printf("[!] send_recv\n");
 317         return;
 318     }
 319 }
 320
 321 void _stlink_usb_enter_swd_mode(stlink_t * sl) {
 322     struct stlink_libusb * const slu = sl->backend_data;
 323     unsigned char* const cmd  = sl->c_buf;
 324     ssize_t size;
 325     int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 326
 327     cmd[i++] = STLINK_DEBUG_COMMAND;
 328     cmd[i++] = STLINK_DEBUG_ENTER;
 329     cmd[i++] = STLINK_DEBUG_ENTER_SWD;
 330
 331     size = send_only(slu, 1, cmd, slu->cmd_len);
 332     if (size == -1) {
 333         printf("[!] send_recv\n");
 334         return;
 335     }
 336 }
 337
 338 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
 339     struct stlink_libusb * const slu = sl->backend_data;
 340     unsigned char* const cmd = sl->c_buf;
 341     ssize_t size;
 342     int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 343
 344     cmd[i++] = STLINK_DFU_COMMAND;
 345     cmd[i++] = STLINK_DFU_EXIT;
 346
 347     size = send_only(slu, 1, cmd, slu->cmd_len);
 348     if (size == -1) {
 349         printf("[!] send_recv\n");
 350         return;
 351     }
 352 }
 353
 354 /**
 355  * TODO - not convinced this does anything...
 356  * @param sl
 357  */
 358 void _stlink_usb_reset(stlink_t * sl) {
 359     struct stlink_libusb * const slu = sl->backend_data;
 360     unsigned char* const data = sl->q_buf;
 361     unsigned char* const cmd = sl->c_buf;
 362     ssize_t size;
 363     int rep_len = 2;
 364     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 365
 366     cmd[i++] = STLINK_DEBUG_COMMAND;
 367     cmd[i++] = STLINK_DEBUG_RESETSYS;
 368
 369     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 370     if (size == -1) {
 371         printf("[!] send_recv\n");
 372         return;
 373     }
 374 }
 375
 376
 377 void _stlink_usb_step(stlink_t* sl) {
 378     struct stlink_libusb * const slu = sl->backend_data;
 379     unsigned char* const data = sl->q_buf;
 380     unsigned char* const cmd = sl->c_buf;
 381     ssize_t size;
 382     int rep_len = 2;
 383     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 384
 385     cmd[i++] = STLINK_DEBUG_COMMAND;
 386     cmd[i++] = STLINK_DEBUG_STEPCORE;
 387
 388     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 389     if (size == -1) {
 390         printf("[!] send_recv\n");
 391         return;
 392     }
 393 }
 394
 395 /**
 396  * This seems to do a good job of restarting things from the beginning?
 397  * @param sl
 398  */
 399 void _stlink_usb_run(stlink_t* sl) {
 400     struct stlink_libusb * const slu = sl->backend_data;
 401     unsigned char* const data = sl->q_buf;
 402     unsigned char* const cmd = sl->c_buf;
 403     ssize_t size;
 404     int rep_len = 2;
 405     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 406
 407     cmd[i++] = STLINK_DEBUG_COMMAND;
 408     cmd[i++] = STLINK_DEBUG_RUNCORE;
 409
 410     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 411     if (size == -1) {
 412         printf("[!] send_recv\n");
 413         return;
 414     }
 415 }
 416
 417 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
 418     struct stlink_libusb * const slu = sl->backend_data;
 419     unsigned char* const cmd = sl->c_buf;
 420     ssize_t size;
 421     int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
 422
 423     cmd[i++] = STLINK_DEBUG_COMMAND;
 424     cmd[i++] = STLINK_DEBUG_EXIT;
 425
 426     size = send_only(slu, 1, cmd, slu->cmd_len);
 427     if (size == -1) {
 428         printf("[!] send_only\n");
 429         return;
 430     }
 431 }
 432
 433 void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 434     struct stlink_libusb * const slu = sl->backend_data;
 435     unsigned char* const data = sl->q_buf;
 436     unsigned char* const cmd = sl->c_buf;
 437     ssize_t size;
 438     int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
 439
 440 #if Q_BUF_LEN < UINT16_MAX
 441     assert(len < sizeof(sl->q_buf));
 442 #endif
 443
 444     cmd[i++] = STLINK_DEBUG_COMMAND;
 445     cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
 446     write_uint32(&cmd[i], addr);
 447     i += 4;
 448     /* windows usb logs show only one byte is used for length ... */
 449     // Presumably, this is because usb transfers can't be 16 bits worth of bytes long...
 450     assert (len < 256);
 451     cmd[i] = (uint8_t) len;
 452
 453     size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
 454     if (size == -1) {
 455         printf("[!] send_recv\n");
 456         return;
 457     }
 458
 459     sl->q_len = (size_t) size;
 460
 461     stlink_print_data(sl);
 462 }
 463
 464 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
 465     struct stlink_libusb * const slu = sl->backend_data;
 466     unsigned char* const cmd = sl->c_buf;
 467     unsigned char* const data = sl->q_buf;
 468     ssize_t size;
 469     uint32_t rep_len = 84;
 470     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 471
 472     cmd[i++] = STLINK_DEBUG_COMMAND;
 473     cmd[i++] = STLINK_DEBUG_READALLREGS;
 474     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 475     if (size == -1) {
 476         printf("[!] send_recv\n");
 477         return;
 478     }
 479     sl->q_len = (size_t) size;
 480     stlink_print_data(sl);
 481     for(i=0; i<16; i++)
 482         regp->r[i]= read_uint32(sl->q_buf, i*4);
 483     regp->xpsr       = read_uint32(sl->q_buf, 64);
 484     regp->main_sp    = read_uint32(sl->q_buf, 68);
 485     regp->process_sp = read_uint32(sl->q_buf, 72);
 486     regp->rw         = read_uint32(sl->q_buf, 76);
 487     regp->rw2        = read_uint32(sl->q_buf, 80);
 488     if (sl->verbose < 2)
 489         return;
 490
 491     DD(sl, "xpsr       = 0x%08x\n", read_uint32(sl->q_buf, 64));
 492     DD(sl, "main_sp    = 0x%08x\n", read_uint32(sl->q_buf, 68));
 493     DD(sl, "process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72));
 494     DD(sl, "rw         = 0x%08x\n", read_uint32(sl->q_buf, 76));
 495     DD(sl, "rw2        = 0x%08x\n", read_uint32(sl->q_buf, 80));
 496 }
 497
 498 void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 499     struct stlink_libusb * const slu = sl->backend_data;
 500     unsigned char* const data = sl->q_buf;
 501     unsigned char* const cmd  = sl->c_buf;
 502     ssize_t size;
 503     uint32_t r;
 504     uint32_t rep_len = 4;
 505     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 506
 507     cmd[i++] = STLINK_DEBUG_COMMAND;
 508     cmd[i++] = STLINK_DEBUG_READREG;
 509     cmd[i++] = (uint8_t) r_idx;
 510     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 511     if (size == -1) {
 512         printf("[!] send_recv\n");
 513         return;
 514     }
 515     sl->q_len = (size_t) size;
 516     stlink_print_data(sl);
 517     r = read_uint32(sl->q_buf, 0);
 518     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 519
 520     switch (r_idx) {
 521     case 16:
 522         regp->xpsr = r;
 523         break;
 524     case 17:
 525         regp->main_sp = r;
 526         break;
 527     case 18:
 528         regp->process_sp = r;
 529         break;
 530     case 19:
 531         regp->rw = r; /* XXX ?(primask, basemask etc.) */
 532         break;
 533     case 20:
 534         regp->rw2 = r; /* XXX ?(primask, basemask etc.) */
 535         break;
 536     default:
 537         regp->r[r_idx] = r;
 538     }
 539 }
 540
 541 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 542     struct stlink_libusb * const slu = sl->backend_data;
 543     unsigned char* const data = sl->q_buf;
 544     unsigned char* const cmd  = sl->c_buf;
 545     ssize_t size;
 546     uint32_t rep_len = 2;
 547     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 548
 549     cmd[i++] = STLINK_DEBUG_COMMAND;
 550     cmd[i++] = STLINK_DEBUG_WRITEREG;
 551     cmd[i++] = idx;
 552     write_uint32(&cmd[i], reg);
 553     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 554     if (size == -1) {
 555         printf("[!] send_recv\n");
 556         return;
 557     }
 558     sl->q_len = (size_t) size;
 559     stlink_print_data(sl);
 560 }
 561
 562 stlink_backend_t _stlink_usb_backend = {
 563     _stlink_usb_close,
 564     _stlink_usb_exit_debug_mode,
 565     _stlink_usb_enter_swd_mode,
 566     NULL,  // no enter_jtag_mode here...
 567     _stlink_usb_exit_dfu_mode,
 568     _stlink_usb_core_id,
 569     _stlink_usb_reset,
 570     _stlink_usb_run,
 571     _stlink_usb_status,
 572     _stlink_usb_version,
 573     _stlink_usb_read_mem32,
 574     _stlink_usb_write_mem32,
 575     _stlink_usb_write_mem8,
 576     _stlink_usb_read_all_regs,
 577     _stlink_usb_read_reg,
 578     _stlink_usb_write_reg,
 579     _stlink_usb_step,
 580     _stlink_usb_current_mode,
 581     _stlink_usb_force_debug
 582 };
 583
 584
 585 stlink_t* stlink_open_usb(const int verbose) {
 586     stlink_t* sl = NULL;
 587     struct stlink_libusb* slu = NULL;
 588     int error = -1;
 589     libusb_device** devs = NULL;
 590     libusb_device* dev;
 591     ssize_t i;
 592     ssize_t count;
 593     int config;
 594     char *iSerial = NULL;
 595
 596     sl = malloc(sizeof (stlink_t));
 597     slu = malloc(sizeof (struct stlink_libusb));
 598     if (sl == NULL) goto on_error;
 599     if (slu == NULL) goto on_error;
 600     memset(sl, 0, sizeof (stlink_t));
 601     memset(slu, 0, sizeof (struct stlink_libusb));
 602
 603     sl->verbose = verbose;
 604     sl->backend = &_stlink_usb_backend;
 605     sl->backend_data = slu;
 606
 607     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 608
 609     /* flash memory settings */
 610     sl->flash_base = STM32_FLASH_BASE;
 611     sl->flash_size = STM32_FLASH_SIZE;
 612     sl->flash_pgsz = STM32_FLASH_PGSZ;
 613
 614     /* system memory */
 615     sl->sys_base = STM32_SYSTEM_BASE;
 616     sl->sys_size = STM32_SYSTEM_SIZE;
 617
 618     /* sram memory settings */
 619     sl->sram_base = STM32_SRAM_BASE;
 620     sl->sram_size = STM32L_SRAM_SIZE;
 621
 622     if (libusb_init(&(slu->libusb_ctx))) {
 623         fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
 624         goto on_error;
 625     }
 626
 627     count = libusb_get_device_list(slu->libusb_ctx, &devs);
 628     if (count < 0) {
 629         printf("libusb_get_device_list\n");
 630         goto on_libusb_error;
 631     }
 632
 633     for (i = 0; i < count; ++i) {
 634         dev = devs[i];
 635         slu->protocoll = is_stlink_device(dev, 0);
 636         if (slu->protocoll > 0) break;
 637     }
 638     if (i == count) goto on_libusb_error;
 639
 640     if (libusb_open(dev, &(slu->usb_handle))) {
 641         printf("libusb_open()\n");
 642         goto on_libusb_error;
 643     }
 644
 645     if (iSerial) {
 646         unsigned char serial[256];
 647         struct libusb_device_descriptor desc;
 648         int r;
 649
 650         r = libusb_get_device_descriptor(dev, &desc);
 651         if (r<0) {
 652             printf("Can't get descriptor to match Iserial\n");
 653             goto on_libusb_error;
 654         }
 655         r = libusb_get_string_descriptor_ascii
 656             (slu->usb_handle, desc.iSerialNumber, serial, 256);
 657         if (r<0) {
 658             printf("Can't get Serialnumber to match Iserial\n");
 659             goto on_libusb_error;
 660         }
 661         if (strcmp((char*)serial, iSerial)) {
 662             printf("Mismatch in serial numbers, dev %s vs given %s\n",
 663                    serial, iSerial);
 664             goto on_libusb_error;
 665         }
 666     }
 667
 668     if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
 669         int r;
 670
 671         r = libusb_detach_kernel_driver(slu->usb_handle, 0);
 672         if (r<0)
 673             printf("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
 674         goto on_libusb_error;
 675     }
 676
 677     libusb_reset_device(slu->usb_handle);
 678
 679     if (libusb_get_configuration(slu->usb_handle, &config)) {
 680         /* this may fail for a previous configured device */
 681         printf("libusb_get_configuration()\n");
 682         goto on_libusb_error;
 683     }
 684
 685     if (config != 1) {
 686         printf("setting new configuration (%d -> 1)\n", config);
 687         if (libusb_set_configuration(slu->usb_handle, 1)) {
 688             /* this may fail for a previous configured device */
 689             printf("libusb_set_configuration()\n");
 690             goto on_libusb_error;
 691         }
 692     }
 693
 694     if (libusb_claim_interface(slu->usb_handle, 0)) {
 695         printf("libusb_claim_interface()\n");
 696         goto on_libusb_error;
 697     }
 698
 699     slu->req_trans = libusb_alloc_transfer(0);
 700     if (slu->req_trans == NULL) {
 701         printf("libusb_alloc_transfer\n");
 702         goto on_libusb_error;
 703     }
 704
 705     slu->rep_trans = libusb_alloc_transfer(0);
 706     if (slu->rep_trans == NULL) {
 707         printf("libusb_alloc_transfer\n");
 708         goto on_libusb_error;
 709     }
 710
 711     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 712     slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 713
 714     /* libusb_reset_device(slu->usb_handle); */
 715     slu->sg_transfer_idx = 0;
 716     slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
 717
 718     /* success */
 719     if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
 720       printf("-- exit_dfu_mode\n");
 721       stlink_exit_dfu_mode(sl);
 722     }
 723     stlink_version(sl);
 724     error = 0;
 725
 726 on_libusb_error:
 727     if (devs != NULL) {
 728         libusb_free_device_list(devs, 1);
 729     }
 730
 731     if (error == -1) {
 732         stlink_close(sl);
 733         return NULL;
 734     }
 735
 736     /* success */
 737     return sl;
 738
 739 on_error:
 740     if (sl != NULL) free(sl);
 741     if (slu != NULL) free(slu);
 742     return 0;
 743 }
 744