git.gag.com Git - fw/stlink/blob - src/stlink-usb.c

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