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