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
  14 enum SCSI_Generic_Direction {SG_DXFER_TO_DEV=0, SG_DXFER_FROM_DEV=0x80};
  15
  16 void _stlink_usb_close(stlink_t* sl) {
  17     if (!sl)
  18         return;
  19
  20     struct stlink_libusb * const handle = sl->backend_data;
  21     // maybe we couldn't even get the usb device?
  22     if (handle != NULL) {
  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 ssize_t send_recv(struct stlink_libusb* handle, int terminate,
  33         unsigned char* txbuf, size_t txsize,
  34         unsigned char* rxbuf, size_t rxsize) {
  35     /* note: txbuf and rxbuf can point to the same area */
  36     int res = 0;
  37
  38     if (libusb_bulk_transfer(handle->usb_handle, handle->ep_req,
  39             txbuf,
  40             txsize,
  41             &res,
  42             3000))
  43         return -1;
  44
  45     if (rxsize != 0) {
  46         if (libusb_bulk_transfer(handle->usb_handle, handle->ep_rep,
  47                 rxbuf,
  48                 rxsize,
  49                 &res,
  50                 3000))
  51             return -1;
  52     }
  53
  54     if ((handle->protocoll == 1) && terminate) {
  55         /* Read the SG reply */
  56         unsigned char sg_buf[13];
  57         if (libusb_bulk_transfer(handle->usb_handle, handle->ep_rep,
  58                 sg_buf,
  59                 13,
  60                 &res,
  61                 3000))
  62             return -1;
  63         /* The STLink doesn't seem to evaluate the sequence number */
  64         handle->sg_transfer_idx++;
  65     }
  66
  67     return res;
  68 }
  69
  70 static inline int send_only
  71 (struct stlink_libusb* handle, int terminate,
  72  unsigned char* txbuf, size_t txsize) {
  73     return send_recv(handle, terminate, txbuf, txsize, NULL, 0);
  74 }
  75
  76
  77 static int fill_command
  78 (stlink_t * sl, enum SCSI_Generic_Direction dir, uint32_t len) {
  79     struct stlink_libusb * const slu = sl->backend_data;
  80     unsigned char* const cmd = sl->c_buf;
  81     int i = 0;
  82     memset(cmd, 0, sizeof (sl->c_buf));
  83     if(slu->protocoll == 1) {
  84         cmd[i++] = 'U';
  85         cmd[i++] = 'S';
  86         cmd[i++] = 'B';
  87         cmd[i++] = 'C';
  88         write_uint32(&cmd[i], slu->sg_transfer_idx);
  89         write_uint32(&cmd[i + 4], len);
  90         i += 8;
  91         cmd[i++] = (dir == SG_DXFER_FROM_DEV)?0x80:0;
  92         cmd[i++] = 0; /* Logical unit */
  93         cmd[i++] = 0xa; /* Command length */
  94     }
  95     return i;
  96 }
  97
  98 int _stlink_usb_version(stlink_t *sl) {
  99     struct stlink_libusb * const slu = sl->backend_data;
 100     unsigned char* const data = sl->q_buf;
 101     unsigned char* const cmd  = sl->c_buf;
 102     ssize_t size;
 103     uint32_t rep_len = 6;
 104     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 105
 106     cmd[i++] = STLINK_GET_VERSION;
 107
 108     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 109     if (size == -1) {
 110         printf("[!] send_recv\n");
 111         return size;
 112     }
 113
 114     return 0;
 115 }
 116
 117 int32_t _stlink_usb_target_voltage(stlink_t *sl) {
 118     struct stlink_libusb * const slu = sl->backend_data;
 119     unsigned char* const rdata = sl->q_buf;
 120     unsigned char* const cmd  = sl->c_buf;
 121     ssize_t size;
 122     uint32_t rep_len = 8;
 123     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 124     uint32_t factor, reading;
 125     int voltage;
 126
 127     cmd[i++] = STLINK_GET_TARGET_VOLTAGE;
 128
 129     size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
 130     if (size == -1) {
 131         printf("[!] send_recv\n");
 132         return -1;
 133     } else if (size != 8) {
 134         printf("[!] wrong length\n");
 135         return -1;
 136     }
 137
 138     factor = (rdata[3] << 24) | (rdata[2] << 16) | (rdata[1] << 8) | (rdata[0] << 0);
 139     reading = (rdata[7] << 24) | (rdata[6] << 16) | (rdata[5] << 8) | (rdata[4] << 0);
 140     voltage = 2400 * reading / factor;
 141
 142     return voltage;
 143 }
 144
 145 int _stlink_usb_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
 146     struct stlink_libusb * const slu = sl->backend_data;
 147     unsigned char* const rdata = sl->q_buf;
 148     unsigned char* const cmd  = sl->c_buf;
 149     ssize_t size;
 150     const int rep_len = 8;
 151
 152     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 153     cmd[i++] = STLINK_DEBUG_COMMAND;
 154     cmd[i++] = STLINK_JTAG_READDEBUG_32BIT;
 155     write_uint32(&cmd[i], addr);
 156     size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
 157     if (size == -1) {
 158         printf("[!] send_recv\n");
 159         return size;
 160     }
 161     *data = read_uint32(rdata, 4);
 162     return 0;
 163 }
 164
 165 int _stlink_usb_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 166     struct stlink_libusb * const slu = sl->backend_data;
 167     unsigned char* const rdata = sl->q_buf;
 168     unsigned char* const cmd  = sl->c_buf;
 169     ssize_t size;
 170     const int rep_len = 2;
 171
 172     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 173     cmd[i++] = STLINK_DEBUG_COMMAND;
 174     cmd[i++] = STLINK_JTAG_WRITEDEBUG_32BIT;
 175     write_uint32(&cmd[i], addr);
 176     write_uint32(&cmd[i + 4], data);
 177     size = send_recv(slu, 1, cmd, slu->cmd_len, rdata, rep_len);
 178     if (size == -1) {
 179         printf("[!] send_recv\n");
 180         return size;
 181     }
 182
 183     return 0;
 184 }
 185
 186 int _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 187     struct stlink_libusb * const slu = sl->backend_data;
 188     unsigned char* const data = sl->q_buf;
 189     unsigned char* const cmd  = sl->c_buf;
 190     int i, ret;
 191
 192     i = fill_command(sl, SG_DXFER_TO_DEV, len);
 193     cmd[i++] = STLINK_DEBUG_COMMAND;
 194     cmd[i++] = STLINK_DEBUG_WRITEMEM_32BIT;
 195     write_uint32(&cmd[i], addr);
 196     write_uint16(&cmd[i + 4], len);
 197     ret = send_only(slu, 0, cmd, slu->cmd_len);
 198     if (ret == -1)
 199         return ret;
 200
 201     ret = send_only(slu, 1, data, len);
 202     if (ret == -1)
 203         return ret;
 204
 205     return 0;
 206 }
 207
 208 int _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 209     struct stlink_libusb * const slu = sl->backend_data;
 210     unsigned char* const data = sl->q_buf;
 211     unsigned char* const cmd  = sl->c_buf;
 212     int i, ret;
 213
 214     i = fill_command(sl, SG_DXFER_TO_DEV, 0);
 215     cmd[i++] = STLINK_DEBUG_COMMAND;
 216     cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
 217     write_uint32(&cmd[i], addr);
 218     write_uint16(&cmd[i + 4], len);
 219     ret = send_only(slu, 0, cmd, slu->cmd_len);
 220     if (ret == -1)
 221         return ret;
 222
 223     ret = send_only(slu, 1, data, len);
 224     if (ret == -1)
 225         return ret;
 226
 227     return 0;
 228 }
 229
 230
 231 int _stlink_usb_current_mode(stlink_t * sl) {
 232     struct stlink_libusb * const slu = sl->backend_data;
 233     unsigned char* const cmd  = sl->c_buf;
 234     unsigned char* const data = sl->q_buf;
 235     ssize_t size;
 236     int rep_len = 2;
 237     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 238
 239     cmd[i++] = STLINK_GET_CURRENT_MODE;
 240     size = send_recv(slu, 1, cmd,  slu->cmd_len, data, rep_len);
 241     if (size == -1) {
 242         printf("[!] send_recv\n");
 243         return -1;
 244     }
 245     return sl->q_buf[0];
 246 }
 247
 248 int _stlink_usb_core_id(stlink_t * sl) {
 249     struct stlink_libusb * const slu = sl->backend_data;
 250     unsigned char* const cmd  = sl->c_buf;
 251     unsigned char* const data = sl->q_buf;
 252     ssize_t size;
 253     int rep_len = 4;
 254     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 255
 256     cmd[i++] = STLINK_DEBUG_COMMAND;
 257     cmd[i++] = STLINK_DEBUG_READCOREID;
 258
 259     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 260     if (size == -1) {
 261         printf("[!] send_recv\n");
 262         return -1;
 263     }
 264
 265     sl->core_id = read_uint32(data, 0);
 266     return 0;
 267 }
 268
 269 int _stlink_usb_status(stlink_t * sl) {
 270     struct stlink_libusb * const slu = sl->backend_data;
 271     unsigned char* const data = sl->q_buf;
 272     unsigned char* const cmd  = sl->c_buf;
 273     ssize_t size;
 274     int rep_len = 2;
 275     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 276
 277     cmd[i++] = STLINK_DEBUG_COMMAND;
 278     cmd[i++] = STLINK_DEBUG_GETSTATUS;
 279
 280     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 281     if (size == -1) {
 282         printf("[!] send_recv\n");
 283         return size;
 284     }
 285     sl->q_len = (size_t) size;
 286
 287     return 0;
 288 }
 289
 290 int _stlink_usb_force_debug(stlink_t *sl) {
 291     struct stlink_libusb *slu = sl->backend_data;
 292     unsigned char* const data = sl->q_buf;
 293     unsigned char* const cmd  = sl->c_buf;
 294     ssize_t size;
 295     int rep_len = 2;
 296     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 297
 298     cmd[i++] = STLINK_DEBUG_COMMAND;
 299     cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
 300     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 301     if (size == -1) {
 302         printf("[!] send_recv\n");
 303         return size;
 304     }
 305
 306     return 0;
 307 }
 308
 309 int _stlink_usb_enter_swd_mode(stlink_t * sl) {
 310     struct stlink_libusb * const slu = sl->backend_data;
 311     unsigned char* const cmd  = sl->c_buf;
 312     ssize_t size;
 313     const int rep_len = 0;
 314     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 315
 316     cmd[i++] = STLINK_DEBUG_COMMAND;
 317     cmd[i++] = STLINK_DEBUG_ENTER;
 318     cmd[i++] = STLINK_DEBUG_ENTER_SWD;
 319
 320     size = send_only(slu, 1, cmd, slu->cmd_len);
 321     if (size == -1) {
 322         printf("[!] send_recv\n");
 323         return size;
 324     }
 325
 326     return 0;
 327 }
 328
 329 int _stlink_usb_exit_dfu_mode(stlink_t* sl) {
 330     struct stlink_libusb * const slu = sl->backend_data;
 331     unsigned char* const cmd = sl->c_buf;
 332     ssize_t size;
 333     int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 334
 335     cmd[i++] = STLINK_DFU_COMMAND;
 336     cmd[i++] = STLINK_DFU_EXIT;
 337
 338     size = send_only(slu, 1, cmd, slu->cmd_len);
 339     if (size == -1) {
 340         printf("[!] send_recv\n");
 341         return size;
 342     }
 343
 344     return 0;
 345 }
 346
 347 /**
 348  * TODO - not convinced this does anything...
 349  * @param sl
 350  */
 351 int _stlink_usb_reset(stlink_t * sl) {
 352     struct stlink_libusb * const slu = sl->backend_data;
 353     unsigned char* const data = sl->q_buf;
 354     unsigned char* const cmd = sl->c_buf;
 355     ssize_t size;
 356     int rep_len = 2;
 357     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 358
 359     cmd[i++] = STLINK_DEBUG_COMMAND;
 360     cmd[i++] = STLINK_DEBUG_RESETSYS;
 361
 362     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 363     if (size == -1) {
 364         printf("[!] send_recv\n");
 365         return size;
 366     }
 367
 368     return 0;
 369 }
 370
 371
 372 int _stlink_usb_jtag_reset(stlink_t * sl, int value) {
 373     struct stlink_libusb * const slu = sl->backend_data;
 374     unsigned char* const data = sl->q_buf;
 375     unsigned char* const cmd = sl->c_buf;
 376     ssize_t size;
 377     int rep_len = 2;
 378     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 379
 380     cmd[i++] = STLINK_DEBUG_COMMAND;
 381     cmd[i++] = STLINK_JTAG_DRIVE_NRST;
 382     cmd[i++] = value;
 383
 384     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 385     if (size == -1) {
 386         printf("[!] send_recv\n");
 387         return size;
 388     }
 389
 390     return 0;
 391 }
 392
 393
 394 int _stlink_usb_step(stlink_t* sl) {
 395     struct stlink_libusb * const slu = sl->backend_data;
 396     unsigned char* const data = sl->q_buf;
 397     unsigned char* const cmd = sl->c_buf;
 398     ssize_t size;
 399     int rep_len = 2;
 400     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 401
 402     cmd[i++] = STLINK_DEBUG_COMMAND;
 403     cmd[i++] = STLINK_DEBUG_STEPCORE;
 404
 405     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 406     if (size == -1) {
 407         printf("[!] send_recv\n");
 408         return size;
 409     }
 410
 411     return 0;
 412 }
 413
 414 /**
 415  * This seems to do a good job of restarting things from the beginning?
 416  * @param sl
 417  */
 418 int _stlink_usb_run(stlink_t* sl) {
 419     struct stlink_libusb * const slu = sl->backend_data;
 420     unsigned char* const data = sl->q_buf;
 421     unsigned char* const cmd = sl->c_buf;
 422     ssize_t size;
 423     int rep_len = 2;
 424     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 425
 426     cmd[i++] = STLINK_DEBUG_COMMAND;
 427     cmd[i++] = STLINK_DEBUG_RUNCORE;
 428
 429     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 430     if (size == -1) {
 431         printf("[!] send_recv\n");
 432         return size;
 433     }
 434
 435     return 0;
 436 }
 437
 438 int _stlink_usb_exit_debug_mode(stlink_t *sl) {
 439     struct stlink_libusb * const slu = sl->backend_data;
 440     unsigned char* const cmd = sl->c_buf;
 441     ssize_t size;
 442     int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
 443
 444     cmd[i++] = STLINK_DEBUG_COMMAND;
 445     cmd[i++] = STLINK_DEBUG_EXIT;
 446
 447     size = send_only(slu, 1, cmd, slu->cmd_len);
 448     if (size == -1) {
 449         printf("[!] send_only\n");
 450         return size;
 451     }
 452
 453     return 0;
 454 }
 455
 456 int _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 457     struct stlink_libusb * const slu = sl->backend_data;
 458     unsigned char* const data = sl->q_buf;
 459     unsigned char* const cmd = sl->c_buf;
 460     ssize_t size;
 461     int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
 462
 463     cmd[i++] = STLINK_DEBUG_COMMAND;
 464     cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
 465     write_uint32(&cmd[i], addr);
 466     write_uint16(&cmd[i + 4], len);
 467
 468     size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
 469     if (size == -1) {
 470         printf("[!] send_recv\n");
 471         return size;
 472     }
 473
 474     sl->q_len = (size_t) size;
 475
 476     stlink_print_data(sl);
 477     return 0;
 478 }
 479
 480 int _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
 481     struct stlink_libusb * const slu = sl->backend_data;
 482     unsigned char* const cmd = sl->c_buf;
 483     unsigned char* const data = sl->q_buf;
 484     ssize_t size;
 485     uint32_t rep_len = 84;
 486     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 487
 488     cmd[i++] = STLINK_DEBUG_COMMAND;
 489     cmd[i++] = STLINK_DEBUG_READALLREGS;
 490     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 491     if (size == -1) {
 492         printf("[!] send_recv\n");
 493         return size;
 494     }
 495     sl->q_len = (size_t) size;
 496     stlink_print_data(sl);
 497     for(i=0; i<16; i++)
 498         regp->r[i]= read_uint32(sl->q_buf, i*4);
 499     regp->xpsr       = read_uint32(sl->q_buf, 64);
 500     regp->main_sp    = read_uint32(sl->q_buf, 68);
 501     regp->process_sp = read_uint32(sl->q_buf, 72);
 502     regp->rw         = read_uint32(sl->q_buf, 76);
 503     regp->rw2        = read_uint32(sl->q_buf, 80);
 504     if (sl->verbose < 2)
 505         return 0;
 506
 507     DLOG("xpsr       = 0x%08x\n", read_uint32(sl->q_buf, 64));
 508     DLOG("main_sp    = 0x%08x\n", read_uint32(sl->q_buf, 68));
 509     DLOG("process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72));
 510     DLOG("rw         = 0x%08x\n", read_uint32(sl->q_buf, 76));
 511     DLOG("rw2        = 0x%08x\n", read_uint32(sl->q_buf, 80));
 512
 513     return 0;
 514 }
 515
 516 int _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 517     struct stlink_libusb * const slu = sl->backend_data;
 518     unsigned char* const data = sl->q_buf;
 519     unsigned char* const cmd  = sl->c_buf;
 520     ssize_t size;
 521     uint32_t r;
 522     uint32_t rep_len = 4;
 523     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 524
 525     cmd[i++] = STLINK_DEBUG_COMMAND;
 526     cmd[i++] = STLINK_DEBUG_READREG;
 527     cmd[i++] = (uint8_t) r_idx;
 528     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 529     if (size == -1) {
 530         printf("[!] send_recv\n");
 531         return size;
 532     }
 533     sl->q_len = (size_t) size;
 534     stlink_print_data(sl);
 535     r = read_uint32(sl->q_buf, 0);
 536     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 537
 538     switch (r_idx) {
 539     case 16:
 540         regp->xpsr = r;
 541         break;
 542     case 17:
 543         regp->main_sp = r;
 544         break;
 545     case 18:
 546         regp->process_sp = r;
 547         break;
 548     case 19:
 549         regp->rw = r; /* XXX ?(primask, basemask etc.) */
 550         break;
 551     case 20:
 552         regp->rw2 = r; /* XXX ?(primask, basemask etc.) */
 553         break;
 554     default:
 555         regp->r[r_idx] = r;
 556     }
 557
 558     return 0;
 559 }
 560
 561 /* See section C1.6 of the ARMv7-M Architecture Reference Manual */
 562 int _stlink_usb_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
 563     uint32_t r;
 564     int ret;
 565
 566     sl->q_buf[0] = (unsigned char) r_idx;
 567     for (int i = 1; i < 4; i++) {
 568         sl->q_buf[i] = 0;
 569     }
 570
 571     ret = _stlink_usb_write_mem32(sl, DCRSR, 4);
 572     if (ret == -1)
 573         return ret;
 574
 575     _stlink_usb_read_mem32(sl, DCRDR, 4);
 576     if (ret == -1)
 577         return ret;
 578
 579     r = read_uint32(sl->q_buf, 0);
 580     DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
 581
 582     switch (r_idx) {
 583     case 0x14:
 584         regp->primask = (uint8_t) (r & 0xFF);
 585         regp->basepri = (uint8_t) ((r>>8) & 0xFF);
 586         regp->faultmask = (uint8_t) ((r>>16) & 0xFF);
 587         regp->control = (uint8_t) ((r>>24) & 0xFF);
 588         break;
 589     case 0x21:
 590         regp->fpscr = r;
 591         break;
 592     default:
 593         regp->s[r_idx - 0x40] = r;
 594         break;
 595     }
 596
 597     return 0;
 598 }
 599
 600 int _stlink_usb_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
 601     int ret;
 602
 603     ret = _stlink_usb_read_unsupported_reg(sl, 0x14, regp);
 604     if (ret == -1)
 605         return ret;
 606
 607     ret = _stlink_usb_read_unsupported_reg(sl, 0x21, regp);
 608     if (ret == -1)
 609         return ret;
 610
 611     for (int i = 0; i < 32; i++) {
 612         ret = _stlink_usb_read_unsupported_reg(sl, 0x40+i, regp);
 613         if (ret == -1)
 614             return ret;
 615     }
 616
 617     return 0;
 618 }
 619
 620 /* See section C1.6 of the ARMv7-M Architecture Reference Manual */
 621 int _stlink_usb_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, reg *regp) {
 622     int ret;
 623
 624     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 625         /* These are held in the same register */
 626         ret = _stlink_usb_read_unsupported_reg(sl, 0x14, regp);
 627         if (ret == -1)
 628             return ret;
 629
 630         val = (uint8_t) (val>>24);
 631
 632         switch (r_idx) {
 633         case 0x1C:  /* control */
 634             val = (((uint32_t) val) << 24) | (((uint32_t) regp->faultmask) << 16) | (((uint32_t) regp->basepri) << 8) | ((uint32_t) regp->primask);
 635             break;
 636         case 0x1D:  /* faultmask */
 637             val = (((uint32_t) regp->control) << 24) | (((uint32_t) val) << 16) | (((uint32_t) regp->basepri) << 8) | ((uint32_t) regp->primask);
 638             break;
 639         case 0x1E:  /* basepri */
 640             val = (((uint32_t) regp->control) << 24) | (((uint32_t) regp->faultmask) << 16) | (((uint32_t) val) << 8) | ((uint32_t) regp->primask);
 641             break;
 642         case 0x1F:  /* primask */
 643             val = (((uint32_t) regp->control) << 24) | (((uint32_t) regp->faultmask) << 16) | (((uint32_t) regp->basepri) << 8) | ((uint32_t) val);
 644             break;
 645         }
 646
 647         r_idx = 0x14;
 648     }
 649
 650     write_uint32(sl->q_buf, val);
 651
 652     ret = _stlink_usb_write_mem32(sl, DCRDR, 4);
 653     if (ret == -1)
 654         return ret;
 655
 656     sl->q_buf[0] = (unsigned char) r_idx;
 657     sl->q_buf[1] = 0;
 658     sl->q_buf[2] = 0x01;
 659     sl->q_buf[3] = 0;
 660
 661     return _stlink_usb_write_mem32(sl, DCRSR, 4);
 662 }
 663
 664 int _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 665     struct stlink_libusb * const slu = sl->backend_data;
 666     unsigned char* const data = sl->q_buf;
 667     unsigned char* const cmd  = sl->c_buf;
 668     ssize_t size;
 669     uint32_t rep_len = 2;
 670     int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
 671
 672     cmd[i++] = STLINK_DEBUG_COMMAND;
 673     cmd[i++] = STLINK_DEBUG_WRITEREG;
 674     cmd[i++] = idx;
 675     write_uint32(&cmd[i], reg);
 676     size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
 677     if (size == -1) {
 678         printf("[!] send_recv\n");
 679         return size;
 680     }
 681     sl->q_len = (size_t) size;
 682     stlink_print_data(sl);
 683
 684     return 0;
 685 }
 686
 687 stlink_backend_t _stlink_usb_backend = {
 688     _stlink_usb_close,
 689     _stlink_usb_exit_debug_mode,
 690     _stlink_usb_enter_swd_mode,
 691     NULL,  // no enter_jtag_mode here...
 692     _stlink_usb_exit_dfu_mode,
 693     _stlink_usb_core_id,
 694     _stlink_usb_reset,
 695     _stlink_usb_jtag_reset,
 696     _stlink_usb_run,
 697     _stlink_usb_status,
 698     _stlink_usb_version,
 699     _stlink_usb_read_debug32,
 700     _stlink_usb_read_mem32,
 701     _stlink_usb_write_debug32,
 702     _stlink_usb_write_mem32,
 703     _stlink_usb_write_mem8,
 704     _stlink_usb_read_all_regs,
 705     _stlink_usb_read_reg,
 706     _stlink_usb_read_all_unsupported_regs,
 707     _stlink_usb_read_unsupported_reg,
 708     _stlink_usb_write_unsupported_reg,
 709     _stlink_usb_write_reg,
 710     _stlink_usb_step,
 711     _stlink_usb_current_mode,
 712     _stlink_usb_force_debug,
 713     _stlink_usb_target_voltage
 714 };
 715
 716 stlink_t *stlink_open_usb(enum ugly_loglevel verbose, bool reset, char serial[16])
 717 {
 718     stlink_t* sl = NULL;
 719     struct stlink_libusb* slu = NULL;
 720     int ret = -1;
 721     int config;
 722
 723     sl = calloc(1, sizeof (stlink_t));
 724     slu = calloc(1, sizeof (struct stlink_libusb));
 725     if (sl == NULL)
 726         goto on_malloc_error;
 727     if (slu == NULL)
 728         goto on_malloc_error;
 729
 730     ugly_init(verbose);
 731     sl->backend = &_stlink_usb_backend;
 732     sl->backend_data = slu;
 733
 734     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 735     if (libusb_init(&(slu->libusb_ctx))) {
 736         WLOG("failed to init libusb context, wrong version of libraries?\n");
 737         goto on_error;
 738     }
 739
 740     libusb_device **list;
 741     int cnt = libusb_get_device_list(slu->libusb_ctx, &list);
 742     struct libusb_device_descriptor desc;
 743     int devBus =0;
 744     int devAddr=0;
 745
 746     /* @TODO: Reading a environment variable in a usb open function is not very nice, this
 747       should be refactored and moved into the CLI tools, and instead of giving USB_BUS:USB_ADDR a real stlink
 748       serial string should be passed to this function. Probably people are using this but this is very odd because
 749       as programmer can change to multiple busses and it is better to detect them based on serial.  */
 750     char *device = getenv("STLINK_DEVICE");
 751     if (device) {
 752         char *c = strchr(device,':');
 753         if (c==NULL) {
 754             WLOG("STLINK_DEVICE must be <USB_BUS>:<USB_ADDR> format\n");
 755             goto on_error;
 756         }
 757         devBus=atoi(device);
 758         *c++=0;
 759         devAddr=atoi(c);
 760         ILOG("bus %03d dev %03d\n",devBus, devAddr);
 761     }
 762
 763     while (cnt--) {
 764         libusb_get_device_descriptor( list[cnt], &desc );
 765         if (desc.idVendor != USB_ST_VID)
 766             continue;
 767
 768         if (devBus && devAddr) {
 769             if ((libusb_get_bus_number(list[cnt]) != devBus)
 770                 || (libusb_get_device_address(list[cnt]) != devAddr)) {
 771                 continue;
 772             }
 773         }
 774
 775         if ((desc.idProduct == USB_STLINK_32L_PID) || (desc.idProduct == USB_STLINK_NUCLEO_PID)) {
 776             struct libusb_device_handle *handle;
 777
 778             libusb_open(list[cnt], &handle);
 779             sl->serial_size = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber,
 780                                                                  (unsigned char *)sl->serial, sizeof(sl->serial));
 781             libusb_close(handle);
 782
 783             if ((serial == NULL) || (*serial == 0))
 784                  break;
 785
 786             if (sl->serial_size < 0)
 787                  continue;
 788
 789             if (memcmp(serial, &sl->serial, sl->serial_size) == 0)
 790                  break;
 791
 792             continue;
 793         }
 794
 795         if (desc.idProduct == USB_STLINK_PID) {
 796             slu->protocoll = 1;
 797             break;
 798         }
 799     }
 800
 801     if (cnt < 0) {
 802         WLOG ("Couldn't find %s ST-Link/V2 devices\n",(devBus && devAddr)?"matched":"any");
 803         goto on_error;
 804     } else {
 805         ret = libusb_open(list[cnt], &slu->usb_handle);
 806         if (ret != 0) {
 807             WLOG("Error %d (%s) opening ST-Link/V2 device %03d:%03d\n",
 808                  ret, strerror (errno), libusb_get_bus_number(list[cnt]), libusb_get_device_address(list[cnt]));
 809             goto on_error;
 810         }
 811     }
 812
 813     libusb_free_device_list(list, 1);
 814
 815     if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
 816         ret = libusb_detach_kernel_driver(slu->usb_handle, 0);
 817         if (ret < 0) {
 818             WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-ret));
 819             goto on_libusb_error;
 820         }
 821     }
 822
 823     if (libusb_get_configuration(slu->usb_handle, &config)) {
 824         /* this may fail for a previous configured device */
 825         WLOG("libusb_get_configuration()\n");
 826         goto on_libusb_error;
 827     }
 828
 829     if (config != 1) {
 830         printf("setting new configuration (%d -> 1)\n", config);
 831         if (libusb_set_configuration(slu->usb_handle, 1)) {
 832             /* this may fail for a previous configured device */
 833             WLOG("libusb_set_configuration() failed\n");
 834             goto on_libusb_error;
 835         }
 836     }
 837
 838     if (libusb_claim_interface(slu->usb_handle, 0)) {
 839         WLOG("Stlink usb device found, but unable to claim (probably already in use?)\n");
 840         goto on_libusb_error;
 841     }
 842
 843     // TODO - could use the scanning techniq from stm8 code here...
 844     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 845     if (desc.idProduct == USB_STLINK_NUCLEO_PID) {
 846         slu->ep_req = 1 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 847     } else {
 848         slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 849     }
 850
 851     slu->sg_transfer_idx = 0;
 852     // TODO - never used at the moment, always CMD_SIZE
 853     slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
 854
 855     if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
 856         ILOG("-- exit_dfu_mode\n");
 857         stlink_exit_dfu_mode(sl);
 858     }
 859
 860     if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
 861         stlink_enter_swd_mode(sl);
 862     }
 863
 864     if (reset) {
 865         stlink_reset(sl);
 866         usleep(10000);
 867     }
 868
 869     stlink_version(sl);
 870     ret = stlink_load_device_params(sl);
 871
 872 on_libusb_error:
 873     if (ret == -1) {
 874         stlink_close(sl);
 875         return NULL;
 876     }
 877
 878     return sl;
 879
 880 on_error:
 881     if (slu->libusb_ctx)
 882         libusb_exit(slu->libusb_ctx);
 883
 884 on_malloc_error:
 885     if (sl != NULL)
 886         free(sl);
 887     if (slu != NULL)
 888         free(slu);
 889
 890     return NULL;
 891 }
 892
 893 static size_t stlink_probe_usb_devs(libusb_device **devs, stlink_t **sldevs[]) {
 894     stlink_t **_sldevs;
 895     libusb_device *dev;
 896     int i = 0;
 897     int ret = 0;
 898     size_t slcnt = 0;
 899     size_t slcur = 0;
 900
 901     /* Count stlink */
 902     while ((dev = devs[i++]) != NULL) {
 903         struct libusb_device_descriptor desc;
 904         int r = libusb_get_device_descriptor(dev, &desc);
 905         if (r < 0) {
 906             WLOG("failed to get libusb device descriptor\n");
 907             break;
 908         }
 909
 910         if (desc.idProduct != USB_STLINK_32L_PID &&
 911             desc.idProduct != USB_STLINK_NUCLEO_PID)
 912             continue;
 913
 914         slcnt++;
 915     }
 916
 917     /* Allocate list of pointers */
 918     _sldevs = calloc(slcnt, sizeof(stlink_t *));
 919     if (!_sldevs) {
 920         *sldevs = NULL;
 921         return 0;
 922     }
 923
 924     /* Open stlinks and attach to list */
 925     i = 0;
 926     while ((dev = devs[i++]) != NULL) {
 927         struct libusb_device_descriptor desc;
 928         ret = libusb_get_device_descriptor(dev, &desc);
 929         if (ret < 0) {
 930             WLOG("failed to get libusb device descriptor\n");
 931             break;
 932         }
 933
 934         if (desc.idProduct != USB_STLINK_32L_PID &&
 935             desc.idProduct != USB_STLINK_NUCLEO_PID)
 936             continue;
 937
 938         struct libusb_device_handle* handle;
 939         char serial[13];
 940         memset(serial, 0, sizeof(serial));
 941
 942         ret = libusb_open(dev, &handle);
 943         if (ret < 0) {
 944             WLOG("failed to get libusb device descriptor\n");
 945             break;
 946         }
 947
 948         ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, (unsigned char *)&serial, sizeof(serial));
 949         if (ret < 0)
 950           *serial = NULL;
 951
 952         libusb_close(handle);
 953
 954         stlink_t *sl = NULL;
 955         sl = stlink_open_usb(0, 1, serial);
 956         if (!sl)
 957             continue;
 958
 959         _sldevs[slcur] = sl;
 960         slcur++;
 961     }
 962
 963     /* Something went wrong */
 964     if (ret < 0) {
 965         free(_sldevs);
 966         *sldevs = NULL;
 967         return 0;
 968     }
 969
 970     *sldevs = _sldevs;
 971     return slcnt;
 972 }
 973
 974 size_t stlink_probe_usb(stlink_t **stdevs[]) {
 975     libusb_device **devs;
 976     stlink_t **sldevs;
 977
 978     size_t slcnt = 0;
 979     int r;
 980     ssize_t cnt;
 981
 982     r = libusb_init(NULL);
 983     if (r < 0)
 984         return 0;
 985
 986     cnt = libusb_get_device_list(NULL, &devs);
 987     if (cnt < 0)
 988         return 0;
 989
 990     slcnt = stlink_probe_usb_devs(devs, &sldevs);
 991     libusb_free_device_list(devs, 1);
 992
 993     libusb_exit(NULL);
 994
 995     *stdevs = sldevs;
 996     return slcnt;
 997 }
 998
 999 void stlink_probe_usb_free(stlink_t ***stdevs, size_t size) {
1000     if (stdevs == NULL || *stdevs == NULL || size == 0)
1001         return;
1002
1003     for (size_t n = 0; n < size; n++)
1004         stlink_close((*stdevs)[n]);
1005     free(*stdevs);
1006     *stdevs = NULL;
1007 }