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