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