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