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