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     char *device = getenv("STLINK_DEVICE");
 747     if (device) {
 748         char *c = strchr(device,':');
 749         if (c==NULL) {
 750             WLOG("STLINK_DEVICE must be <USB_BUS>:<USB_ADDR> format\n");
 751             goto on_error;
 752         }
 753         devBus=atoi(device);
 754         *c++=0;
 755         devAddr=atoi(c);
 756         ILOG("bus %03d dev %03d\n",devBus, devAddr);
 757     }
 758
 759     while (cnt--) {
 760         libusb_get_device_descriptor( list[cnt], &desc );
 761         if (desc.idVendor != USB_ST_VID)
 762             continue;
 763
 764         if (devBus && devAddr) {
 765             if ((libusb_get_bus_number(list[cnt]) != devBus)
 766                 || (libusb_get_device_address(list[cnt]) != devAddr)) {
 767                 continue;
 768             }
 769         }
 770
 771         if ((desc.idProduct == USB_STLINK_32L_PID) || (desc.idProduct == USB_STLINK_NUCLEO_PID)) {
 772             struct libusb_device_handle *handle;
 773
 774             libusb_open(list[cnt], &handle);
 775             sl->serial_size = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber,
 776                                                                  (unsigned char *)sl->serial, sizeof(sl->serial));
 777             libusb_close(handle);
 778
 779             if ((serial == NULL) || (*serial == 0))
 780                  break;
 781
 782             if (sl->serial_size < 0)
 783                  continue;
 784
 785             if (memcmp(serial, &sl->serial, sl->serial_size) == 0)
 786                  break;
 787
 788             continue;
 789         }
 790
 791         if (desc.idProduct == USB_STLINK_PID) {
 792             slu->protocoll = 1;
 793             break;
 794         }
 795     }
 796
 797     if (cnt < 0) {
 798         WLOG ("Couldn't find %s ST-Link/V2 devices\n",(devBus && devAddr)?"matched":"any");
 799         goto on_error;
 800     } else {
 801         ret = libusb_open(list[cnt], &slu->usb_handle);
 802         if (ret != 0) {
 803             WLOG("Error %d (%s) opening ST-Link/V2 device %03d:%03d\n",
 804                  ret, strerror (errno), libusb_get_bus_number(list[cnt]), libusb_get_device_address(list[cnt]));
 805             goto on_error;
 806         }
 807     }
 808
 809     libusb_free_device_list(list, 1);
 810
 811     if (libusb_kernel_driver_active(slu->usb_handle, 0) == 1) {
 812         ret = libusb_detach_kernel_driver(slu->usb_handle, 0);
 813         if (ret < 0) {
 814             WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-ret));
 815             goto on_libusb_error;
 816         }
 817     }
 818
 819     if (libusb_get_configuration(slu->usb_handle, &config)) {
 820         /* this may fail for a previous configured device */
 821         WLOG("libusb_get_configuration()\n");
 822         goto on_libusb_error;
 823     }
 824
 825     if (config != 1) {
 826         printf("setting new configuration (%d -> 1)\n", config);
 827         if (libusb_set_configuration(slu->usb_handle, 1)) {
 828             /* this may fail for a previous configured device */
 829             WLOG("libusb_set_configuration() failed\n");
 830             goto on_libusb_error;
 831         }
 832     }
 833
 834     if (libusb_claim_interface(slu->usb_handle, 0)) {
 835         WLOG("libusb_claim_interface() failed\n");
 836         goto on_libusb_error;
 837     }
 838
 839     // TODO - could use the scanning techniq from stm8 code here...
 840     slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
 841     if (desc.idProduct == USB_STLINK_NUCLEO_PID) {
 842         slu->ep_req = 1 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 843     } else {
 844         slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
 845     }
 846
 847     slu->sg_transfer_idx = 0;
 848     // TODO - never used at the moment, always CMD_SIZE
 849     slu->cmd_len = (slu->protocoll == 1)? STLINK_SG_SIZE: STLINK_CMD_SIZE;
 850
 851     if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
 852         ILOG("-- exit_dfu_mode\n");
 853         stlink_exit_dfu_mode(sl);
 854     }
 855
 856     if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) {
 857         stlink_enter_swd_mode(sl);
 858     }
 859
 860     if (reset) {
 861         stlink_reset(sl);
 862         usleep(10000);
 863     }
 864
 865     stlink_version(sl);
 866     ret = stlink_load_device_params(sl);
 867
 868 on_libusb_error:
 869     if (ret == -1) {
 870         stlink_close(sl);
 871         return NULL;
 872     }
 873
 874     return sl;
 875
 876 on_error:
 877     if (slu->libusb_ctx)
 878         libusb_exit(slu->libusb_ctx);
 879
 880 on_malloc_error:
 881     if (sl != NULL)
 882         free(sl);
 883     if (slu != NULL)
 884         free(slu);
 885
 886     return NULL;
 887 }
 888
 889 static size_t stlink_probe_usb_devs(libusb_device **devs, stlink_t **sldevs[]) {
 890     stlink_t **_sldevs;
 891     libusb_device *dev;
 892     int i = 0;
 893     int ret = 0;
 894     size_t slcnt = 0;
 895     size_t slcur = 0;
 896
 897     /* Count stlink */
 898     while ((dev = devs[i++]) != NULL) {
 899         struct libusb_device_descriptor desc;
 900         int r = libusb_get_device_descriptor(dev, &desc);
 901         if (r < 0) {
 902             WLOG("failed to get libusb device descriptor\n");
 903             break;
 904         }
 905
 906         if (desc.idProduct != USB_STLINK_32L_PID &&
 907             desc.idProduct != USB_STLINK_NUCLEO_PID)
 908             continue;
 909
 910         slcnt++;
 911     }
 912
 913     /* Allocate list of pointers */
 914     _sldevs = calloc(slcnt, sizeof(stlink_t *));
 915     if (!_sldevs) {
 916         *sldevs = NULL;
 917         return 0;
 918     }
 919
 920     /* Open stlinks and attach to list */
 921     i = 0;
 922     while ((dev = devs[i++]) != NULL) {
 923         struct libusb_device_descriptor desc;
 924         ret = libusb_get_device_descriptor(dev, &desc);
 925         if (ret < 0) {
 926             WLOG("failed to get libusb device descriptor\n");
 927             break;
 928         }
 929
 930         if (desc.idProduct != USB_STLINK_32L_PID &&
 931             desc.idProduct != USB_STLINK_NUCLEO_PID)
 932             continue;
 933
 934         struct libusb_device_handle* handle;
 935         char serial[13];
 936         memset(serial, 0, sizeof(serial));
 937
 938         ret = libusb_open(dev, &handle);
 939         if (ret < 0) {
 940             WLOG("failed to get libusb device descriptor\n");
 941             break;
 942         }
 943
 944         ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, (unsigned char *)&serial, sizeof(serial));
 945         if (ret < 0)
 946           *serial = NULL;
 947
 948         libusb_close(handle);
 949
 950         stlink_t *sl = NULL;
 951         sl = stlink_open_usb(0, 1, serial);
 952         if (!sl)
 953             continue;
 954
 955         _sldevs[slcur] = sl;
 956         slcur++;
 957     }
 958
 959     /* Something went wrong */
 960     if (ret < 0) {
 961         free(_sldevs);
 962         *sldevs = NULL;
 963         return 0;
 964     }
 965
 966     *sldevs = _sldevs;
 967     return slcnt;
 968 }
 969
 970 size_t stlink_probe_usb(stlink_t **stdevs[]) {
 971     libusb_device **devs;
 972     stlink_t **sldevs;
 973
 974     size_t slcnt = 0;
 975     int r;
 976     ssize_t cnt;
 977
 978     r = libusb_init(NULL);
 979     if (r < 0)
 980         return 0;
 981
 982     cnt = libusb_get_device_list(NULL, &devs);
 983     if (cnt < 0)
 984         return 0;
 985
 986     slcnt = stlink_probe_usb_devs(devs, &sldevs);
 987     libusb_free_device_list(devs, 1);
 988
 989     libusb_exit(NULL);
 990
 991     *stdevs = sldevs;
 992     return slcnt;
 993 }
 994
 995 void stlink_probe_usb_free(stlink_t ***stdevs, size_t size) {
 996     if (stdevs == NULL || *stdevs == NULL || size == 0)
 997         return;
 998
 999     for (size_t n = 0; n < size; n++)
1000         stlink_close((*stdevs)[n]);
1001     free(*stdevs);
1002     *stdevs = NULL;
1003 }