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