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 <sys/time.h>
   6 #include <sys/types.h>
   7 #include <libusb.h>
   8 #include <errno.h>
   9 #include <unistd.h>
  10
  11 #include "stlink-common.h"
  12 #include "stlink-usb.h"
  13 #include "uglylogging.h"
  14
  15 enum SCSI_Generic_Direction {SG_DXFER_TO_DEV=0, SG_DXFER_FROM_DEV=0x80};
  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->usb_handle != NULL) {
  22             libusb_close(handle->usb_handle);
  23         }
  24
  25         libusb_exit(handle->libusb_ctx);
  26         free(handle);
  27     }
  28 }
  29
  30 ssize_t send_recv(struct stlink_libusb* handle, int terminate,
  31         unsigned char* txbuf, size_t txsize,
  32         unsigned char* rxbuf, size_t rxsize) {
  33     /* note: txbuf and rxbuf can point to the same area */
  34     int res = 0;
  35
  36     if (libusb_bulk_transfer(handle->usb_handle, handle->ep_req,
  37             txbuf,
  38             txsize,
  39             &res,
  40             3000))
  41         return -1;
  42
  43     if (rxsize != 0) {
  44         if (libusb_bulk_transfer(handle->usb_handle, handle->ep_rep,
  45                 rxbuf,
  46                 rxsize,
  47                 &res,
  48                 3000))
  49             return -1;
  50     }
  51
  52     if ((handle->protocoll == 1) && terminate) {
  53         /* Read the SG reply */
  54         unsigned char sg_buf[13];
  55         if (libusb_bulk_transfer(handle->usb_handle, handle->ep_rep,
  56                 sg_buf,
  57                 13,
  58                 &res,
  59                 3000))
  60             return -1;
  61         /* The STLink doesn't seem to evaluate the sequence number */
  62         handle->sg_transfer_idx++;
  63     }
  64
  65     return res;
  66 }
  67
  68 static inline int send_only
  69 (struct stlink_libusb* handle, int terminate,
  70  unsigned char* txbuf, size_t txsize) {
  71     return send_recv(handle, terminate, txbuf, txsize, NULL, 0);
  72 }
  73
  74
  75 static int fill_command
  76 (stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t len) {
  77     struct stlink_libusb * const slu = sl->backend_data;
  78     unsigned char* const cmd = sl->c_buf;
  79     int i = 0;
  80     memset(cmd, 0, sizeof (sl->c_buf));
  81     if(slu->protocoll == 1) {
  82         cmd[i++] = 'U';
  83         cmd[i++] = 'S';
  84         cmd[i++] = 'B';
  85         cmd[i++] = 'C';
  86         write_uint32(&cmd[i], slu->sg_transfer_idx);
  87         write_uint32(&cmd[i + 4], len);
  88         i += 8;
  89         cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
  90         cmd[i++] = 0; /* Logical unit */
  91         cmd[i++] = 0xa; /* Command length */
  92     }
  93     return i;
  94 }
  95
  96 void _stlink_usb_version(stlink_t *sl) {
  97     struct stlink_libusb * const slu = sl->backend_data;
  98     unsigned char* const data = sl->q_buf;
  99     unsigned char* const cmd  = sl->c_buf;
 100     ssize_t size;
 101     uint32_t rep_len = 6;
 102     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 103
 104     cmd[i++] = STLINK_GET_VERSION;
 105
 106     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 107     if (size == -1) {
 108         printf("[!] send_recv\n");
 109         return;
 110     }
 111 }
 112
 113 int32_t _stlink_usb_target_voltage(stlink_t *sl) {
 114     struct stlink_libusb * const slu = sl->backend_data;
 115     unsigned char* const rdata = sl->q_buf;
 116     unsigned char* const cmd  = sl->c_buf;
 117     ssize_t size;
 118     uint32_t rep_len = 8;
 119     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 120     uint32_t factor, reading;
 121     int voltage;
 122
 123     cmd[i++] = STLINK_GET_TARGET_VOLTAGE;
 124
 125     size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
 126     if (size == -1) {
 127         printf("[!] send_recv\n");
 128         return -1;
 129     } else if (size != 8) {
 130         printf("[!] wrong length\n");
 131         return -1;
 132     }
 133
 134     factor = (rdata[3] << 24) | (rdata[2] << 16) | (rdata[1] << 8) | (rdata[0] << 0);
 135     reading = (rdata[7] << 24) | (rdata[6] << 16) | (rdata[5] << 8) | (rdata[4] << 0);
 136     voltage = 2400 * reading / factor;
 137
 138     return voltage;
 139 }
 140
 141 void _stlink_usb_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
 142     struct stlink_libusb * const slu = sl->backend_data;
 143     unsigned char* const rdata = sl->q_buf;
 144     unsigned char* const cmd  = sl->c_buf;
 145     ssize_t size;
 146     const int rep_len = 8;
 147
 148     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 149     cmd[i++] = STLINK_DEBUG_COMMAND;
 150     cmd[i++] = STLINK_JTAG_READDEBUG_32BIT;
 151     write_uint32(&cmd[i], addr);
 152     size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
 153     if (size == -1) {
 154         printf("[!] send_recv\n");
 155         return;
 156     }
 157     *data = read_uint32(rdata, 4);
 158     return;
 159 }
 160
 161 void _stlink_usb_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 162     struct stlink_libusb * const slu = sl->backend_data;
 163     unsigned char* const rdata = sl->q_buf;
 164     unsigned char* const cmd  = sl->c_buf;
 165     ssize_t size;
 166     const int rep_len = 2;
 167
 168     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 169     cmd[i++] = STLINK_DEBUG_COMMAND;
 170     cmd[i++] = STLINK_JTAG_WRITEDEBUG_32BIT;
 171     write_uint32(&cmd[i], addr);
 172     write_uint32(&cmd[i + 4], data);
 173     size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
 174     if (size == -1) {
 175         printf("[!] send_recv\n");
 176         return;
 177     }
 178 }
 179
 180 void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 181     struct stlink_libusb * const slu = sl->backend_data;
 182     unsigned char* const data = sl->q_buf;
 183     unsigned char* const cmd  = sl->c_buf;
 184
 185     int i = fill_command(sl, SG_DXFER_TO_DEV, len);
 186     cmd[i++] = STLINK_DEBUG_COMMAND;
 187     cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
 188     write_uint32(&cmd[i], addr);
 189     write_uint16(&cmd[i + 4], len);
 190     send_only(slu, 0, cmd, slu->cmd_len);
 191
 192     send_only(slu, 1, data, len);
 193 }
 194
 195 void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 196     struct stlink_libusb * const slu = sl->backend_data;
 197     unsigned char* const data = sl->q_buf;
 198     unsigned char* const cmd  = sl->c_buf;
 199
 200     int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
 201     cmd[i++] = STLINK_DEBUG_COMMAND;
 202     cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
 203     write_uint32(&cmd[i], addr);
 204     write_uint16(&cmd[i + 4], len);
 205     send_only(slu, 0, cmd, slu->cmd_len);
 206     send_only(slu, 1, data, len);
 207 }
 208
 209
 210 int _stlink_usb_current_mode(stlink_t * sl) {
 211     struct stlink_libusb * const slu = sl->backend_data;
 212     unsigned char* const cmd  = sl->c_buf;
 213     unsigned char* const data = sl->q_buf;
 214     ssize_t size;
 215     int rep_len = 2;
 216     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 217
 218     cmd[i++] = STLINK_GET_CURRENT_MODE;
 219     size = send_recv(slu, 1, cmd,  slu->cmd_len, data, rep_len);
 220     if (size == -1) {
 221         printf("[!] send_recv\n");
 222         return -1;
 223     }
 224     return sl->q_buf[0];
 225 }
 226
 227 void _stlink_usb_core_id(stlink_t * sl) {
 228     struct stlink_libusb * const slu = sl->backend_data;
 229     unsigned char* const cmd  = sl->c_buf;
 230     unsigned char* const data = sl->q_buf;
 231     ssize_t size;
 232     int rep_len = 4;
 233     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 234
 235     cmd[i++] = STLINK_DEBUG_COMMAND;
 236     cmd[i++] = STLINK_DEBUG_READCOREID;
 237
 238     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 239     if (size == -1) {
 240         printf("[!] send_recv\n");
 241         return;
 242     }
 243
 244     sl->core_id = read_uint32(data, 0);
 245 }
 246
 247 void _stlink_usb_status(stlink_t * sl) {
 248     struct stlink_libusb * const slu = sl->backend_data;
 249     unsigned char* const data = sl->q_buf;
 250     unsigned char* const cmd  = sl->c_buf;
 251     ssize_t size;
 252     int rep_len = 2;
 253     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 254
 255     cmd[i++] = STLINK_DEBUG_COMMAND;
 256     cmd[i++] = STLINK_DEBUG_GETSTATUS;
 257
 258     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 259     if (size == -1) {
 260         printf("[!] send_recv\n");
 261         return;
 262     }
 263     sl->q_len = (size_t) size;
 264 }
 265
 266 void _stlink_usb_force_debug(stlink_t *sl) {
 267     struct stlink_libusb *slu = sl->backend_data;
 268     unsigned char* const data = sl->q_buf;
 269     unsigned char* const cmd  = sl->c_buf;
 270     ssize_t size;
 271     int rep_len = 2;
 272     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 273
 274     cmd[i++] = STLINK_DEBUG_COMMAND;
 275     cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
 276     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 277     if (size == -1) {
 278         printf("[!] send_recv\n");
 279         return;
 280     }
 281 }
 282
 283 void _stlink_usb_enter_swd_mode(stlink_t * sl) {
 284     struct stlink_libusb * const slu = sl->backend_data;
 285     unsigned char* const cmd  = sl->c_buf;
 286     ssize_t size;
 287     const int rep_len = 0;
 288     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 289
 290     cmd[i++] = STLINK_DEBUG_COMMAND;
 291     cmd[i++] = STLINK_DEBUG_ENTER;
 292     cmd[i++] = STLINK_DEBUG_ENTER_SWD;
 293
 294     size = send_only(slu, 1, cmd, slu->cmd_len);
 295     if (size == -1) {
 296         printf("[!] send_recv\n");
 297         return;
 298     }
 299 }
 300
 301 void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
 302     struct stlink_libusb * const slu = sl->backend_data;
 303     unsigned char* const cmd = sl->c_buf;
 304     ssize_t size;
 305     int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 306
 307     cmd[i++] = STLINK_DFU_COMMAND;
 308     cmd[i++] = STLINK_DFU_EXIT;
 309
 310     size = send_only(slu, 1, cmd, slu->cmd_len);
 311     if (size == -1) {
 312         printf("[!] send_recv\n");
 313         return;
 314     }
 315 }
 316
 317 /**
 318  * TODO - not convinced this does anything...
 319  * @param sl
 320  */
 321 void _stlink_usb_reset(stlink_t * sl) {
 322     struct stlink_libusb * const slu = sl->backend_data;
 323     unsigned char* const data = sl->q_buf;
 324     unsigned char* const cmd = sl->c_buf;
 325     ssize_t size;
 326     int rep_len = 2;
 327     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 328
 329     cmd[i++] = STLINK_DEBUG_COMMAND;
 330     cmd[i++] = STLINK_DEBUG_RESETSYS;
 331
 332     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 333     if (size == -1) {
 334         printf("[!] send_recv\n");
 335         return;
 336     }
 337 }
 338
 339
 340 void _stlink_usb_jtag_reset(stlink_t * sl, int value) {
 341     struct stlink_libusb * const slu = sl->backend_data;
 342     unsigned char* const data = sl->q_buf;
 343     unsigned char* const cmd = sl->c_buf;
 344     ssize_t size;
 345     int rep_len = 2;
 346     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 347
 348     cmd[i++] = STLINK_DEBUG_COMMAND;
 349     cmd[i++] = STLINK_JTAG_DRIVE_NRST;
 350     cmd[i++] = value;
 351
 352     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 353     if (size == -1) {
 354         printf("[!] send_recv\n");
 355         return;
 356     }
 357 }
 358
 359
 360 void _stlink_usb_step(stlink_t* sl) {
 361     struct stlink_libusb * const slu = sl->backend_data;
 362     unsigned char* const data = sl->q_buf;
 363     unsigned char* const cmd = sl->c_buf;
 364     ssize_t size;
 365     int rep_len = 2;
 366     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 367
 368     cmd[i++] = STLINK_DEBUG_COMMAND;
 369     cmd[i++] = STLINK_DEBUG_STEPCORE;
 370
 371     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 372     if (size == -1) {
 373         printf("[!] send_recv\n");
 374         return;
 375     }
 376 }
 377
 378 /**
 379  * This seems to do a good job of restarting things from the beginning?
 380  * @param sl
 381  */
 382 void _stlink_usb_run(stlink_t* sl) {
 383     struct stlink_libusb * const slu = sl->backend_data;
 384     unsigned char* const data = sl->q_buf;
 385     unsigned char* const cmd = sl->c_buf;
 386     ssize_t size;
 387     int rep_len = 2;
 388     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 389
 390     cmd[i++] = STLINK_DEBUG_COMMAND;
 391     cmd[i++] = STLINK_DEBUG_RUNCORE;
 392
 393     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 394     if (size == -1) {
 395         printf("[!] send_recv\n");
 396         return;
 397     }
 398 }
 399
 400 void _stlink_usb_exit_debug_mode(stlink_t *sl) {
 401     struct stlink_libusb * const slu = sl->backend_data;
 402     unsigned char* const cmd = sl->c_buf;
 403     ssize_t size;
 404     int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 405
 406     cmd[i++] = STLINK_DEBUG_COMMAND;
 407     cmd[i++] = STLINK_DEBUG_EXIT;
 408
 409     size = send_only(slu, 1, cmd, slu->cmd_len);
 410     if (size == -1) {
 411         printf("[!] send_only\n");
 412         return;
 413     }
 414 }
 415
 416 void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 417     struct stlink_libusb * const slu = sl->backend_data;
 418     unsigned char* const data = sl->q_buf;
 419     unsigned char* const cmd = sl->c_buf;
 420     ssize_t size;
 421     int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
 422
 423     cmd[i++] = STLINK_DEBUG_COMMAND;
 424     cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
 425     write_uint32(&cmd[i], addr);
 426     write_uint16(&cmd[i + 4], len);
 427
 428     size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
 429     if (size == -1) {
 430         printf("[!] send_recv\n");
 431         return;
 432     }
 433
 434     sl->q_len = (size_t) size;
 435
 436     stlink_print_data(sl);
 437 }
 438
 439 void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
 440     struct stlink_libusb * const slu = sl->backend_data;
 441     unsigned char* const cmd = sl->c_buf;
 442     unsigned char* const data = sl->q_buf;
 443     ssize_t size;
 444     uint32_t rep_len = 84;
 445     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 446
 447     cmd[i++] = STLINK_DEBUG_COMMAND;
 448     cmd[i++] = STLINK_DEBUG_READALLREGS;
 449     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 450     if (size == -1) {
 451         printf("[!] send_recv\n");
 452         return;
 453     }
 454     sl->q_len = (size_t) size;
 455     stlink_print_data(sl);
 456     for(i=0; i<16; i++)
 457         regp->r[i]= read_uint32(sl->q_buf, i*4);
 458     regp->xpsr       = read_uint32(sl->q_buf, 64);
 459     regp->main_sp    = read_uint32(sl->q_buf, 68);
 460     regp->process_sp = read_uint32(sl->q_buf, 72);
 461     regp->rw         = read_uint32(sl->q_buf, 76);
 462     regp->rw2        = read_uint32(sl->q_buf, 80);
 463     if (sl->verbose < 2)
 464         return;
 465
 466     DLOG("xpsr       = 0x%08x\n", read_uint32(sl->q_buf, 64));
 467     DLOG("main_sp    = 0x%08x\n", read_uint32(sl->q_buf, 68));
 468     DLOG("process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72));
 469     DLOG("rw         = 0x%08x\n", read_uint32(sl->q_buf, 76));
 470     DLOG("rw2        = 0x%08x\n", read_uint32(sl->q_buf, 80));
 471 }
 472
 473 void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 474     struct stlink_libusb * const slu = sl->backend_data;
 475     unsigned char* const data = sl->q_buf;
 476     unsigned char* const cmd  = sl->c_buf;
 477     ssize_t size;
 478     uint32_t r;
 479     uint32_t rep_len = 4;
 480     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 481
 482     cmd[i++] = STLINK_DEBUG_COMMAND;
 483     cmd[i++] = STLINK_DEBUG_READREG;
 484     cmd[i++] = (uint8_t) r_idx;
 485     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 486     if (size == -1) {
 487         printf("[!] send_recv\n");
 488         return;
 489     }
 490     sl->q_len = (size_t) size;
 491     stlink_print_data(sl);
 492     r = read_uint32(sl->q_buf, 0);
 493     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 494
 495     switch (r_idx) {
 496     case 16:
 497         regp->xpsr = r;
 498         break;
 499     case 17:
 500         regp->main_sp = r;
 501         break;
 502     case 18:
 503         regp->process_sp = r;
 504         break;
 505     case 19:
 506         regp->rw = r; /* XXX ?(primask, basemask etc.) */
 507         break;
 508     case 20:
 509         regp->rw2 = r; /* XXX ?(primask, basemask etc.) */
 510         break;
 511     default:
 512         regp->r[r_idx] = r;
 513     }
 514 }
 515
 516 /* See section C1.6 of the ARMv7-M Architecture Reference Manual */
 517 void _stlink_usb_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
 518     uint32_t r;
 519
 520     sl->q_buf[0] = (unsigned char) r_idx;
 521     for (int i = 1; i < 4; i++) {
 522         sl->q_buf[i] = 0;
 523     }
 524
 525     _stlink_usb_write_mem32(sl, DCRSR, 4);
 526     _stlink_usb_read_mem32(sl, DCRDR, 4);
 527
 528     r = read_uint32(sl->q_buf, 0);
 529     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 530
 531     switch (r_idx) {
 532     case 0x14:
 533         regp->primask = (uint8_t) (r & 0xFF);
 534         regp->basepri = (uint8_t) ((r>>8) & 0xFF);
 535         regp->faultmask = (uint8_t) ((r>>16) & 0xFF);
 536         regp->control = (uint8_t) ((r>>24) & 0xFF);
 537         break;
 538     case 0x21:
 539         regp->fpscr = r;
 540         break;
 541     default:
 542         regp->s[r_idx - 0x40] = r;
 543         break;
 544     }
 545 }
 546
 547 void _stlink_usb_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
 548     _stlink_usb_read_unsupported_reg(sl, 0x14, regp);
 549     _stlink_usb_read_unsupported_reg(sl, 0x21, regp);
 550
 551     for (int i = 0; i < 32; i++) {
 552         _stlink_usb_read_unsupported_reg(sl, 0x40+i, regp);
 553     }
 554 }
 555
 556 /* See section C1.6 of the ARMv7-M Architecture Reference Manual */
 557 void _stlink_usb_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, reg *regp) {
 558     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 559         /* These are held in the same register */
 560         _stlink_usb_read_unsupported_reg(sl, 0x14, regp);
 561
 562         val = (uint8_t) (val>>24);
 563
 564         switch (r_idx) {
 565         case 0x1C:  /* control */
 566             val = (((uint32_t) val) << 24) | (((uint32_t) regp->faultmask) << 16) | (((uint32_t) regp->basepri) << 8) | ((uint32_t) regp->primask);
 567             break;
 568         case 0x1D:  /* faultmask */
 569             val = (((uint32_t) regp->control) << 24) | (((uint32_t) val) << 16) | (((uint32_t) regp->basepri) << 8) | ((uint32_t) regp->primask);
 570             break;
 571         case 0x1E:  /* basepri */
 572             val = (((uint32_t) regp->control) << 24) | (((uint32_t) regp->faultmask) << 16) | (((uint32_t) val) << 8) | ((uint32_t) regp->primask);
 573             break;
 574         case 0x1F:  /* primask */
 575             val = (((uint32_t) regp->control) << 24) | (((uint32_t) regp->faultmask) << 16) | (((uint32_t) regp->basepri) << 8) | ((uint32_t) val);
 576             break;
 577         }
 578
 579         r_idx = 0x14;
 580     }
 581
 582     write_uint32(sl->q_buf, val);
 583
 584     _stlink_usb_write_mem32(sl, DCRDR, 4);
 585
 586     sl->q_buf[0] = (unsigned char) r_idx;
 587     sl->q_buf[1] = 0;
 588     sl->q_buf[2] = 0x01;
 589     sl->q_buf[3] = 0;
 590
 591     _stlink_usb_write_mem32(sl, DCRSR, 4);
 592 }
 593
 594 void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 595     struct stlink_libusb * const slu = sl->backend_data;
 596     unsigned char* const data = sl->q_buf;
 597     unsigned char* const cmd  = sl->c_buf;
 598     ssize_t size;
 599     uint32_t rep_len = 2;
 600     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 601
 602     cmd[i++] = STLINK_DEBUG_COMMAND;
 603     cmd[i++] = STLINK_DEBUG_WRITEREG;
 604     cmd[i++] = idx;
 605     write_uint32(&cmd[i], reg);
 606     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 607     if (size == -1) {
 608         printf("[!] send_recv\n");
 609         return;
 610     }
 611     sl->q_len = (size_t) size;
 612     stlink_print_data(sl);
 613 }
 614
 615 stlink_backend_t _stlink_usb_backend = {
 616     _stlink_usb_close,
 617     _stlink_usb_exit_debug_mode,
 618     _stlink_usb_enter_swd_mode,
 619     NULL,  // no enter_jtag_mode here...
 620     _stlink_usb_exit_dfu_mode,
 621     _stlink_usb_core_id,
 622     _stlink_usb_reset,
 623     _stlink_usb_jtag_reset,
 624     _stlink_usb_run,
 625     _stlink_usb_status,
 626     _stlink_usb_version,
 627     _stlink_usb_read_debug32,
 628     _stlink_usb_read_mem32,
 629     _stlink_usb_write_debug32,
 630     _stlink_usb_write_mem32,
 631     _stlink_usb_write_mem8,
 632     _stlink_usb_read_all_regs,
 633     _stlink_usb_read_reg,
 634     _stlink_usb_read_all_unsupported_regs,
 635     _stlink_usb_read_unsupported_reg,
 636     _stlink_usb_write_unsupported_reg,
 637     _stlink_usb_write_reg,
 638     _stlink_usb_step,
 639     _stlink_usb_current_mode,
 640     _stlink_usb_force_debug,
 641     _stlink_usb_target_voltage
 642 };
 643
 644
 645 stlink_t* stlink_open_usb(const int verbose, int reset, char *p_usb_iserial) {
 646     stlink_t* sl = NULL;
 647     struct stlink_libusb* slu = NULL;
 648     int error = -1;
 649     int config;
 650
 651     sl = malloc(sizeof (stlink_t));
 652     slu = malloc(sizeof (struct stlink_libusb));
 653     if (sl == NULL) goto on_error;
 654     if (slu == NULL) goto on_error;
 655     memset(sl, 0, sizeof (stlink_t));
 656     memset(slu, 0, sizeof (struct stlink_libusb));
 657
 658     ugly_init(verbose);
 659     sl->backend = &_stlink_usb_backend;
 660     sl->backend_data = slu;
 661
 662     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 663     if (libusb_init(&(slu->libusb_ctx))) {
 664         WLOG("failed to init libusb context, wrong version of libraries?\n");
 665         goto on_error;
 666     }
 667
 668     libusb_device **list;
 669     int cnt = libusb_get_device_list(slu->libusb_ctx, &list);
 670     struct libusb_device_descriptor desc;
 671     int devBus =0;
 672     int devAddr=0;
 673
 674     char *device = getenv("STLINK_DEVICE");
 675     if (device) {
 676         char *c = strchr(device,':');
 677         if (c==NULL) {
 678             WLOG("STLINK_DEVICE must be <USB_BUS>:<USB_ADDR> format\n");
 679             goto on_error;
 680         }
 681         devBus=atoi(device);
 682         *c++=0;
 683         devAddr=atoi(c);
 684         ILOG("bus %03d dev %03d\n",devBus, devAddr);
 685     }
 686     while (cnt--){
 687         libusb_get_device_descriptor( list[cnt], &desc );
 688         if (desc.idVendor!=USB_ST_VID) continue;
 689         if (devBus && devAddr)
 690             if ((libusb_get_bus_number(list[cnt])!=devBus) || (libusb_get_device_address(list[cnt])!=devAddr)) continue;
 691         if ( (desc.idProduct == USB_STLINK_32L_PID) || (desc.idProduct == USB_STLINK_NUCLEO_PID) ){
 692             if ((p_usb_iserial != NULL)){
 693                 unsigned char buffer[13];
 694                 struct libusb_device_handle* handle;
 695                 libusb_open(list[cnt], &handle);
 696                 libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, buffer, 13);
 697                 libusb_close(handle);
 698                 if (memcmp(p_usb_iserial,&buffer,12) == 0){
 699                     break;
 700                 }else{
 701                     continue;
 702                 }
 703             }else{
 704                 break;
 705             }
 706         }
 707         if (desc.idProduct == USB_STLINK_PID) {
 708             slu->protocoll = 1;
 709             break;
 710         }
 711     }
 712
 713     if (cnt < 0) {
 714         WLOG ("Couldn't find %s ST-Link/V2 devices\n",(devBus && devAddr)?"matched":"any");
 715         goto on_error;
 716     } else {
 717         int error = libusb_open(list[cnt], &slu->usb_handle);
 718         if( error !=0 ) {
 719             WLOG("Error %d (%s) opening ST-Link/V2 device %03d:%03d\n",
 720                 error, strerror (errno), libusb_get_bus_number(list[cnt]), libusb_get_device_address(list[cnt]));
 721             goto on_error;
 722         }
 723     }
 724
 725     libusb_free_device_list(list, 1);
 726
 727
 728     if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
 729         int r;
 730
 731         r = libusb_detach_kernel_driver(slu->usb_handle, 0);
 732         if (r<0) {
 733             WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
 734             goto on_libusb_error;
 735         }
 736     }
 737
 738     if (libusb_get_configuration(slu->usb_handle, &config)) {
 739         /* this may fail for a previous configured device */
 740         WLOG("libusb_get_configuration()\n");
 741         goto on_libusb_error;
 742     }
 743
 744     if (config != 1) {
 745         printf("setting new configuration (%d -> 1)\n", config);
 746         if (libusb_set_configuration(slu->usb_handle, 1)) {
 747             /* this may fail for a previous configured device */
 748             WLOG("libusb_set_configuration() failed\n");
 749             goto on_libusb_error;
 750         }
 751     }
 752
 753     if (libusb_claim_interface(slu->usb_handle, 0)) {
 754         WLOG("libusb_claim_interface() failed\n");
 755         goto on_libusb_error;
 756     }
 757
 758     // TODO - could use the scanning techniq from stm8 code here...
 759     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 760     if (desc.idProduct == USB_STLINK_NUCLEO_PID) {
 761         slu->ep_req = 1 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 762     } else {
 763         slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 764     }
 765
 766     slu->sg_transfer_idx = 0;
 767     // TODO - never used at the moment, always CMD_SIZE
 768     slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
 769
 770     /* success */
 771
 772     if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
 773         ILOG("-- exit_dfu_mode\n");
 774         stlink_exit_dfu_mode(sl);
 775     }
 776
 777     if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
 778         stlink_enter_swd_mode(sl);
 779     }
 780
 781     if (reset) {
 782         stlink_reset(sl);
 783         usleep(10000);
 784     }
 785     stlink_version(sl);
 786     error = stlink_load_device_params(sl);
 787
 788 on_libusb_error:
 789     if (error == -1) {
 790         stlink_close(sl);
 791         return NULL;
 792     }
 793
 794     /* success */
 795     return sl;
 796
 797 on_error:
 798     if( slu->libusb_ctx)
 799         libusb_exit(slu->libusb_ctx);
 800     if (sl != NULL) free(sl);
 801     if (slu != NULL) free(slu);
 802     return NULL;
 803 }
 804