git.gag.com Git - fw/openocd/blob - src/jtag/drivers/stlink_usb.c

   1 /***************************************************************************
   2  *   SWIM contributions by Ake Rehnman                                     *
   3  *   Copyright (C) 2017  Ake Rehnman                                       *
   4  *   ake.rehnman(at)gmail.com                                              *
   5  *                                                                         *
   6  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
   7  *   Mathias Kuester <kesmtp@freenet.de>                                   *
   8  *                                                                         *
   9  *   Copyright (C) 2012 by Spencer Oliver                                  *
  10  *   spen@spen-soft.co.uk                                                  *
  11  *                                                                         *
  12  *   This code is based on https://github.com/texane/stlink                *
  13  *                                                                         *
  14  *   This program is free software; you can redistribute it and/or modify  *
  15  *   it under the terms of the GNU General Public License as published by  *
  16  *   the Free Software Foundation; either version 2 of the License, or     *
  17  *   (at your option) any later version.                                   *
  18  *                                                                         *
  19  *   This program is distributed in the hope that it will be useful,       *
  20  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  21  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  22  *   GNU General Public License for more details.                          *
  23  *                                                                         *
  24  *   You should have received a copy of the GNU General Public License     *
  25  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  26  ***************************************************************************/
  27
  28 #ifdef HAVE_CONFIG_H
  29 #include "config.h"
  30 #endif
  31
  32 /* project specific includes */
  33 #include <helper/binarybuffer.h>
  34 #include <jtag/interface.h>
  35 #include <jtag/hla/hla_layout.h>
  36 #include <jtag/hla/hla_transport.h>
  37 #include <jtag/hla/hla_interface.h>
  38 #include <target/target.h>
  39
  40 #include <target/cortex_m.h>
  41
  42 #include "libusb_common.h"
  43
  44 #ifdef HAVE_LIBUSB1
  45 #define USE_LIBUSB_ASYNCIO
  46 #endif
  47
  48 #define ENDPOINT_IN  0x80
  49 #define ENDPOINT_OUT 0x00
  50
  51 #define STLINK_WRITE_TIMEOUT 1000
  52 #define STLINK_READ_TIMEOUT 1000
  53
  54 #define STLINK_NULL_EP        0
  55 #define STLINK_RX_EP          (1|ENDPOINT_IN)
  56 #define STLINK_TX_EP          (2|ENDPOINT_OUT)
  57 #define STLINK_TRACE_EP       (3|ENDPOINT_IN)
  58
  59 #define STLINK_V2_1_TX_EP     (1|ENDPOINT_OUT)
  60 #define STLINK_V2_1_TRACE_EP  (2|ENDPOINT_IN)
  61
  62 #define STLINK_SG_SIZE        (31)
  63 #define STLINK_DATA_SIZE      (4096)
  64 #define STLINK_CMD_SIZE_V2    (16)
  65 #define STLINK_CMD_SIZE_V1    (10)
  66
  67 #define STLINK_V1_PID         (0x3744)
  68 #define STLINK_V2_PID         (0x3748)
  69 #define STLINK_V2_1_PID       (0x374B)
  70 #define STLINK_V2_1_NO_MSD_PID  (0x3752)
  71 #define STLINK_V3_USBLOADER_PID (0x374D)
  72 #define STLINK_V3E_PID          (0x374E)
  73 #define STLINK_V3S_PID          (0x374F)
  74 #define STLINK_V3_2VCP_PID      (0x3753)
  75
  76 /*
  77  * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
  78  * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
  79  * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes.
  80  */
  81 #define STLINK_MAX_RW8          (64)
  82 #define STLINKV3_MAX_RW8        (512)
  83
  84 /* "WAIT" responses will be retried (with exponential backoff) at
  85  * most this many times before failing to caller.
  86  */
  87 #define MAX_WAIT_RETRIES 8
  88
  89 enum stlink_jtag_api_version {
  90         STLINK_JTAG_API_V1 = 1,
  91         STLINK_JTAG_API_V2,
  92         STLINK_JTAG_API_V3,
  93 };
  94
  95 /** */
  96 struct stlink_usb_version {
  97         /** */
  98         int stlink;
  99         /** */
 100         int jtag;
 101         /** */
 102         int swim;
 103         /** jtag api version supported */
 104         enum stlink_jtag_api_version jtag_api;
 105         /** one bit for each feature supported. See macros STLINK_F_* */
 106         uint32_t flags;
 107 };
 108
 109 /** */
 110 struct stlink_usb_handle_s {
 111         /** */
 112         struct jtag_libusb_device_handle *fd;
 113         /** */
 114         struct libusb_transfer *trans;
 115         /** */
 116         uint8_t rx_ep;
 117         /** */
 118         uint8_t tx_ep;
 119         /** */
 120         uint8_t trace_ep;
 121         /** */
 122         uint8_t cmdbuf[STLINK_SG_SIZE];
 123         /** */
 124         uint8_t cmdidx;
 125         /** */
 126         uint8_t direction;
 127         /** */
 128         uint8_t databuf[STLINK_DATA_SIZE];
 129         /** */
 130         uint32_t max_mem_packet;
 131         /** */
 132         enum hl_transports transport;
 133         /** */
 134         struct stlink_usb_version version;
 135         /** */
 136         uint16_t vid;
 137         /** */
 138         uint16_t pid;
 139         /** */
 140         struct {
 141                 /** whether SWO tracing is enabled or not */
 142                 bool enabled;
 143                 /** trace module source clock */
 144                 uint32_t source_hz;
 145         } trace;
 146         /** reconnect is needed next time we try to query the
 147          * status */
 148         bool reconnect_pending;
 149 };
 150
 151 #define STLINK_SWIM_ERR_OK             0x00
 152 #define STLINK_SWIM_BUSY               0x01
 153 #define STLINK_DEBUG_ERR_OK            0x80
 154 #define STLINK_DEBUG_ERR_FAULT         0x81
 155 #define STLINK_SWD_AP_WAIT             0x10
 156 #define STLINK_SWD_AP_FAULT            0x11
 157 #define STLINK_SWD_AP_ERROR            0x12
 158 #define STLINK_SWD_AP_PARITY_ERROR     0x13
 159 #define STLINK_JTAG_WRITE_ERROR        0x0c
 160 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 161 #define STLINK_SWD_DP_WAIT             0x14
 162 #define STLINK_SWD_DP_FAULT            0x15
 163 #define STLINK_SWD_DP_ERROR            0x16
 164 #define STLINK_SWD_DP_PARITY_ERROR     0x17
 165
 166 #define STLINK_SWD_AP_WDATA_ERROR      0x18
 167 #define STLINK_SWD_AP_STICKY_ERROR     0x19
 168 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
 169
 170 #define STLINK_BAD_AP_ERROR            0x1d
 171
 172 #define STLINK_CORE_RUNNING            0x80
 173 #define STLINK_CORE_HALTED             0x81
 174 #define STLINK_CORE_STAT_UNKNOWN       -1
 175
 176 #define STLINK_GET_VERSION             0xF1
 177 #define STLINK_DEBUG_COMMAND           0xF2
 178 #define STLINK_DFU_COMMAND             0xF3
 179 #define STLINK_SWIM_COMMAND            0xF4
 180 #define STLINK_GET_CURRENT_MODE        0xF5
 181 #define STLINK_GET_TARGET_VOLTAGE      0xF7
 182
 183 #define STLINK_DEV_DFU_MODE            0x00
 184 #define STLINK_DEV_MASS_MODE           0x01
 185 #define STLINK_DEV_DEBUG_MODE          0x02
 186 #define STLINK_DEV_SWIM_MODE           0x03
 187 #define STLINK_DEV_BOOTLOADER_MODE     0x04
 188 #define STLINK_DEV_UNKNOWN_MODE        -1
 189
 190 #define STLINK_DFU_EXIT                0x07
 191
 192 /*
 193         STLINK_SWIM_ENTER_SEQ
 194         1.3ms low then 750Hz then 1.5kHz
 195
 196         STLINK_SWIM_GEN_RST
 197         STM8 DM pulls reset pin low 50us
 198
 199         STLINK_SWIM_SPEED
 200         uint8_t (0=low|1=high)
 201
 202         STLINK_SWIM_WRITEMEM
 203         uint16_t length
 204         uint32_t address
 205
 206         STLINK_SWIM_RESET
 207         send syncronization seq (16us low, response 64 clocks low)
 208 */
 209 #define STLINK_SWIM_ENTER                  0x00
 210 #define STLINK_SWIM_EXIT                   0x01
 211 #define STLINK_SWIM_READ_CAP               0x02
 212 #define STLINK_SWIM_SPEED                  0x03
 213 #define STLINK_SWIM_ENTER_SEQ              0x04
 214 #define STLINK_SWIM_GEN_RST                0x05
 215 #define STLINK_SWIM_RESET                  0x06
 216 #define STLINK_SWIM_ASSERT_RESET           0x07
 217 #define STLINK_SWIM_DEASSERT_RESET         0x08
 218 #define STLINK_SWIM_READSTATUS             0x09
 219 #define STLINK_SWIM_WRITEMEM               0x0a
 220 #define STLINK_SWIM_READMEM                0x0b
 221 #define STLINK_SWIM_READBUF                0x0c
 222
 223 #define STLINK_DEBUG_GETSTATUS             0x01
 224 #define STLINK_DEBUG_FORCEDEBUG            0x02
 225 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
 226 #define STLINK_DEBUG_APIV1_READALLREGS     0x04
 227 #define STLINK_DEBUG_APIV1_READREG         0x05
 228 #define STLINK_DEBUG_APIV1_WRITEREG        0x06
 229 #define STLINK_DEBUG_READMEM_32BIT         0x07
 230 #define STLINK_DEBUG_WRITEMEM_32BIT        0x08
 231 #define STLINK_DEBUG_RUNCORE               0x09
 232 #define STLINK_DEBUG_STEPCORE              0x0a
 233 #define STLINK_DEBUG_APIV1_SETFP           0x0b
 234 #define STLINK_DEBUG_READMEM_8BIT          0x0c
 235 #define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
 236 #define STLINK_DEBUG_APIV1_CLEARFP         0x0e
 237 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 238 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 239
 240 #define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
 241 #define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
 242 #define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
 243
 244 #define STLINK_DEBUG_APIV1_ENTER           0x20
 245 #define STLINK_DEBUG_EXIT                  0x21
 246 #define STLINK_DEBUG_READCOREID            0x22
 247
 248 #define STLINK_DEBUG_APIV2_ENTER           0x30
 249 #define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
 250 #define STLINK_DEBUG_APIV2_RESETSYS        0x32
 251 #define STLINK_DEBUG_APIV2_READREG         0x33
 252 #define STLINK_DEBUG_APIV2_WRITEREG        0x34
 253 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
 254 #define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
 255
 256 #define STLINK_DEBUG_APIV2_READALLREGS     0x3A
 257 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 258 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 259
 260 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
 261
 262 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 263 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 264 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 265 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
 266 #define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
 267
 268 #define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
 269 #define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
 270
 271 #define STLINK_APIV3_SET_COM_FREQ           0x61
 272 #define STLINK_APIV3_GET_COM_FREQ           0x62
 273
 274 #define STLINK_APIV3_GET_VERSION_EX         0xFB
 275
 276 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 277 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 278 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 279
 280 #define STLINK_TRACE_SIZE               4096
 281 #define STLINK_TRACE_MAX_HZ             2000000
 282
 283 #define STLINK_V3_MAX_FREQ_NB               10
 284
 285 /** */
 286 enum stlink_mode {
 287         STLINK_MODE_UNKNOWN = 0,
 288         STLINK_MODE_DFU,
 289         STLINK_MODE_MASS,
 290         STLINK_MODE_DEBUG_JTAG,
 291         STLINK_MODE_DEBUG_SWD,
 292         STLINK_MODE_DEBUG_SWIM
 293 };
 294
 295 #define REQUEST_SENSE        0x03
 296 #define REQUEST_SENSE_LENGTH 18
 297
 298 /*
 299  * Map the relevant features, quirks and workaround for specific firmware
 300  * version of stlink
 301  */
 302 #define STLINK_F_HAS_TRACE              (1UL << 0)
 303 #define STLINK_F_HAS_SWD_SET_FREQ       (1UL << 1)
 304 #define STLINK_F_HAS_JTAG_SET_FREQ      (1UL << 2)
 305 #define STLINK_F_HAS_MEM_16BIT          (1UL << 3)
 306 #define STLINK_F_HAS_GETLASTRWSTATUS2   (1UL << 4)
 307
 308 /* aliases */
 309 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
 310
 311 struct speed_map {
 312         int speed;
 313         int speed_divisor;
 314 };
 315
 316 /* SWD clock speed */
 317 static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 318         {4000, 0},
 319         {1800, 1}, /* default */
 320         {1200, 2},
 321         {950,  3},
 322         {480,  7},
 323         {240, 15},
 324         {125, 31},
 325         {100, 40},
 326         {50,  79},
 327         {25, 158},
 328         {15, 265},
 329         {5,  798}
 330 };
 331
 332 /* JTAG clock speed */
 333 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
 334         {18000, 2},
 335         {9000,  4},
 336         {4500,  8},
 337         {2250, 16},
 338         {1125, 32}, /* default */
 339         {562,  64},
 340         {281, 128},
 341         {140, 256}
 342 };
 343
 344 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 345 static int stlink_swim_status(void *handle);
 346
 347 /** */
 348 static unsigned int stlink_usb_block(void *handle)
 349 {
 350         struct stlink_usb_handle_s *h = handle;
 351
 352         assert(handle != NULL);
 353
 354         if (h->version.stlink == 3)
 355                 return STLINKV3_MAX_RW8;
 356         else
 357                 return STLINK_MAX_RW8;
 358 }
 359
 360
 361
 362 #ifdef USE_LIBUSB_ASYNCIO
 363
 364 static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
 365 {
 366         int *completed = transfer->user_data;
 367         *completed = 1;
 368         /* caller interprets result and frees transfer */
 369 }
 370
 371
 372 static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
 373 {
 374         int r, *completed = transfer->user_data;
 375
 376         /* Assuming a single libusb context exists.  There no existing interface into this
 377          * module to pass a libusb context.
 378          */
 379         struct libusb_context *ctx = NULL;
 380
 381         while (!*completed) {
 382                 r = libusb_handle_events_completed(ctx, completed);
 383                 if (r < 0) {
 384                         if (r == LIBUSB_ERROR_INTERRUPTED)
 385                                 continue;
 386                         libusb_cancel_transfer(transfer);
 387                         continue;
 388                 }
 389         }
 390 }
 391
 392
 393 static int transfer_error_status(const struct libusb_transfer *transfer)
 394 {
 395         int r = 0;
 396
 397         switch (transfer->status) {
 398                 case LIBUSB_TRANSFER_COMPLETED:
 399                         r = 0;
 400                         break;
 401                 case LIBUSB_TRANSFER_TIMED_OUT:
 402                         r = LIBUSB_ERROR_TIMEOUT;
 403                         break;
 404                 case LIBUSB_TRANSFER_STALL:
 405                         r = LIBUSB_ERROR_PIPE;
 406                         break;
 407                 case LIBUSB_TRANSFER_OVERFLOW:
 408                         r = LIBUSB_ERROR_OVERFLOW;
 409                         break;
 410                 case LIBUSB_TRANSFER_NO_DEVICE:
 411                         r = LIBUSB_ERROR_NO_DEVICE;
 412                         break;
 413                 case LIBUSB_TRANSFER_ERROR:
 414                 case LIBUSB_TRANSFER_CANCELLED:
 415                         r = LIBUSB_ERROR_IO;
 416                         break;
 417                 default:
 418                         r = LIBUSB_ERROR_OTHER;
 419                         break;
 420         }
 421
 422         return r;
 423 }
 424
 425 struct jtag_xfer {
 426         int ep;
 427         uint8_t *buf;
 428         size_t size;
 429         /* Internal */
 430         int retval;
 431         int completed;
 432         size_t transfer_size;
 433         struct libusb_transfer *transfer;
 434 };
 435
 436 static int jtag_libusb_bulk_transfer_n(
 437                 jtag_libusb_device_handle * dev_handle,
 438                 struct jtag_xfer *transfers,
 439                 size_t n_transfers,
 440                 int timeout)
 441 {
 442         int retval = 0;
 443         int returnval = ERROR_OK;
 444
 445
 446         for (size_t i = 0; i < n_transfers; ++i) {
 447                 transfers[i].retval = 0;
 448                 transfers[i].completed = 0;
 449                 transfers[i].transfer_size = 0;
 450                 transfers[i].transfer = libusb_alloc_transfer(0);
 451
 452                 if (transfers[i].transfer == NULL) {
 453                         for (size_t j = 0; j < i; ++j)
 454                                 libusb_free_transfer(transfers[j].transfer);
 455
 456                         LOG_DEBUG("ERROR, failed to alloc usb transfers");
 457                         for (size_t k = 0; k < n_transfers; ++k)
 458                                 transfers[k].retval = LIBUSB_ERROR_NO_MEM;
 459                         return ERROR_FAIL;
 460                 }
 461         }
 462
 463         for (size_t i = 0; i < n_transfers; ++i) {
 464                 libusb_fill_bulk_transfer(
 465                                 transfers[i].transfer,
 466                                 dev_handle,
 467                                 transfers[i].ep, transfers[i].buf, transfers[i].size,
 468                                 sync_transfer_cb, &transfers[i].completed, timeout);
 469                 transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
 470
 471                 retval = libusb_submit_transfer(transfers[i].transfer);
 472                 if (retval < 0) {
 473                         LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
 474
 475                         /* Probably no point continuing to submit transfers once a submission fails.
 476                          * As a result, tag all remaining transfers as errors.
 477                          */
 478                         for (size_t j = i; j < n_transfers; ++j)
 479                                 transfers[j].retval = retval;
 480
 481                         returnval = ERROR_FAIL;
 482                         break;
 483                 }
 484         }
 485
 486         /* Wait for every submitted USB transfer to complete.
 487         */
 488         for (size_t i = 0; i < n_transfers; ++i) {
 489                 if (transfers[i].retval == 0) {
 490                         sync_transfer_wait_for_completion(transfers[i].transfer);
 491
 492                         retval = transfer_error_status(transfers[i].transfer);
 493                         if (retval) {
 494                                 returnval = ERROR_FAIL;
 495                                 transfers[i].retval = retval;
 496                                 LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
 497                         } else {
 498                                 /* Assuming actual_length is only valid if there is no transfer error.
 499                                  */
 500                                 transfers[i].transfer_size = transfers[i].transfer->actual_length;
 501                         }
 502                 }
 503
 504                 libusb_free_transfer(transfers[i].transfer);
 505                 transfers[i].transfer = NULL;
 506         }
 507
 508         return returnval;
 509 }
 510
 511 #endif
 512
 513
 514 /** */
 515 static int stlink_usb_xfer_v1_get_status(void *handle)
 516 {
 517         struct stlink_usb_handle_s *h = handle;
 518
 519         assert(handle != NULL);
 520
 521         /* read status */
 522         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 523
 524         if (jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)h->cmdbuf,
 525                         13, STLINK_READ_TIMEOUT) != 13)
 526                 return ERROR_FAIL;
 527
 528         uint32_t t1;
 529
 530         t1 = buf_get_u32(h->cmdbuf, 0, 32);
 531
 532         /* check for USBS */
 533         if (t1 != 0x53425355)
 534                 return ERROR_FAIL;
 535         /*
 536          * CSW status:
 537          * 0 success
 538          * 1 command failure
 539          * 2 phase error
 540          */
 541         if (h->cmdbuf[12] != 0)
 542                 return ERROR_FAIL;
 543
 544         return ERROR_OK;
 545 }
 546
 547 #ifdef USE_LIBUSB_ASYNCIO
 548 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 549 {
 550         struct stlink_usb_handle_s *h = handle;
 551
 552         assert(handle != NULL);
 553
 554         size_t n_transfers = 0;
 555         struct jtag_xfer transfers[2];
 556
 557         memset(transfers, 0, sizeof(transfers));
 558
 559         transfers[0].ep = h->tx_ep;
 560         transfers[0].buf = h->cmdbuf;
 561         transfers[0].size = cmdsize;
 562
 563         ++n_transfers;
 564
 565         if (h->direction == h->tx_ep && size) {
 566                 transfers[1].ep = h->tx_ep;
 567                 transfers[1].buf = (uint8_t *)buf;
 568                 transfers[1].size = size;
 569
 570                 ++n_transfers;
 571         } else if (h->direction == h->rx_ep && size) {
 572                 transfers[1].ep = h->rx_ep;
 573                 transfers[1].buf = (uint8_t *)buf;
 574                 transfers[1].size = size;
 575
 576                 ++n_transfers;
 577         }
 578
 579         return jtag_libusb_bulk_transfer_n(
 580                         h->fd,
 581                         transfers,
 582                         n_transfers,
 583                         STLINK_WRITE_TIMEOUT);
 584 }
 585 #else
 586 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 587 {
 588         struct stlink_usb_handle_s *h = handle;
 589
 590         assert(handle != NULL);
 591
 592         if (jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)h->cmdbuf, cmdsize,
 593                         STLINK_WRITE_TIMEOUT) != cmdsize) {
 594                 return ERROR_FAIL;
 595         }
 596
 597         if (h->direction == h->tx_ep && size) {
 598                 if (jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)buf,
 599                                 size, STLINK_WRITE_TIMEOUT) != size) {
 600                         LOG_DEBUG("bulk write failed");
 601                         return ERROR_FAIL;
 602                 }
 603         } else if (h->direction == h->rx_ep && size) {
 604                 if (jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)buf,
 605                                 size, STLINK_READ_TIMEOUT) != size) {
 606                         LOG_DEBUG("bulk read failed");
 607                         return ERROR_FAIL;
 608                 }
 609         }
 610
 611         return ERROR_OK;
 612 }
 613 #endif
 614
 615 /** */
 616 static int stlink_usb_xfer_v1_get_sense(void *handle)
 617 {
 618         int res;
 619         struct stlink_usb_handle_s *h = handle;
 620
 621         assert(handle != NULL);
 622
 623         stlink_usb_init_buffer(handle, h->rx_ep, 16);
 624
 625         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
 626         h->cmdbuf[h->cmdidx++] = 0;
 627         h->cmdbuf[h->cmdidx++] = 0;
 628         h->cmdbuf[h->cmdidx++] = 0;
 629         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
 630
 631         res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
 632
 633         if (res != ERROR_OK)
 634                 return res;
 635
 636         if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
 637                 return ERROR_FAIL;
 638
 639         return ERROR_OK;
 640 }
 641
 642 /*
 643         transfers block in cmdbuf
 644         <size> indicates number of bytes in the following
 645         data phase.
 646 */
 647 static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 648 {
 649         int err, cmdsize = STLINK_CMD_SIZE_V2;
 650         struct stlink_usb_handle_s *h = handle;
 651
 652         assert(handle != NULL);
 653
 654         if (h->version.stlink == 1) {
 655                 cmdsize = STLINK_SG_SIZE;
 656                 /* put length in bCBWCBLength */
 657                 h->cmdbuf[14] = h->cmdidx-15;
 658         }
 659
 660         err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 661
 662         if (err != ERROR_OK)
 663                 return err;
 664
 665         if (h->version.stlink == 1) {
 666                 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
 667                         /* check csw status */
 668                         if (h->cmdbuf[12] == 1) {
 669                                 LOG_DEBUG("get sense");
 670                                 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
 671                                         return ERROR_FAIL;
 672                         }
 673                         return ERROR_FAIL;
 674                 }
 675         }
 676
 677         return ERROR_OK;
 678 }
 679
 680 /**
 681     Converts an STLINK status code held in the first byte of a response
 682     to an openocd error, logs any error/wait status as debug output.
 683 */
 684 static int stlink_usb_error_check(void *handle)
 685 {
 686         struct stlink_usb_handle_s *h = handle;
 687
 688         assert(handle != NULL);
 689
 690         if (h->transport == HL_TRANSPORT_SWIM) {
 691                 switch (h->databuf[0]) {
 692                         case STLINK_SWIM_ERR_OK:
 693                                 return ERROR_OK;
 694                         case STLINK_SWIM_BUSY:
 695                                 return ERROR_WAIT;
 696                         default:
 697                                 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 698                                 return ERROR_FAIL;
 699                 }
 700         }
 701
 702         /* TODO: no error checking yet on api V1 */
 703         if (h->version.jtag_api == STLINK_JTAG_API_V1)
 704                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
 705
 706         switch (h->databuf[0]) {
 707                 case STLINK_DEBUG_ERR_OK:
 708                         return ERROR_OK;
 709                 case STLINK_DEBUG_ERR_FAULT:
 710                         LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
 711                         return ERROR_FAIL;
 712                 case STLINK_SWD_AP_WAIT:
 713                         LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
 714                         return ERROR_WAIT;
 715                 case STLINK_SWD_DP_WAIT:
 716                         LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
 717                         return ERROR_WAIT;
 718                 case STLINK_JTAG_WRITE_ERROR:
 719                         LOG_DEBUG("Write error");
 720                         return ERROR_FAIL;
 721                 case STLINK_JTAG_WRITE_VERIF_ERROR:
 722                         LOG_DEBUG("Write verify error, ignoring");
 723                         return ERROR_OK;
 724                 case STLINK_SWD_AP_FAULT:
 725                         /* git://git.ac6.fr/openocd commit 657e3e885b9ee10
 726                          * returns ERROR_OK with the comment:
 727                          * Change in error status when reading outside RAM.
 728                          * This fix allows CDT plugin to visualize memory.
 729                          */
 730                         LOG_DEBUG("STLINK_SWD_AP_FAULT");
 731                         return ERROR_FAIL;
 732                 case STLINK_SWD_AP_ERROR:
 733                         LOG_DEBUG("STLINK_SWD_AP_ERROR");
 734                         return ERROR_FAIL;
 735                 case STLINK_SWD_AP_PARITY_ERROR:
 736                         LOG_DEBUG("STLINK_SWD_AP_PARITY_ERROR");
 737                         return ERROR_FAIL;
 738                 case STLINK_SWD_DP_FAULT:
 739                         LOG_DEBUG("STLINK_SWD_DP_FAULT");
 740                         return ERROR_FAIL;
 741                 case STLINK_SWD_DP_ERROR:
 742                         LOG_DEBUG("STLINK_SWD_DP_ERROR");
 743                         return ERROR_FAIL;
 744                 case STLINK_SWD_DP_PARITY_ERROR:
 745                         LOG_DEBUG("STLINK_SWD_DP_PARITY_ERROR");
 746                         return ERROR_FAIL;
 747                 case STLINK_SWD_AP_WDATA_ERROR:
 748                         LOG_DEBUG("STLINK_SWD_AP_WDATA_ERROR");
 749                         return ERROR_FAIL;
 750                 case STLINK_SWD_AP_STICKY_ERROR:
 751                         LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
 752                         return ERROR_FAIL;
 753                 case STLINK_SWD_AP_STICKYORUN_ERROR:
 754                         LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
 755                         return ERROR_FAIL;
 756                 case STLINK_BAD_AP_ERROR:
 757                         LOG_DEBUG("STLINK_BAD_AP_ERROR");
 758                         return ERROR_FAIL;
 759                 default:
 760                         LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 761                         return ERROR_FAIL;
 762         }
 763 }
 764
 765
 766 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
 767
 768     Works for commands where the STLINK_DEBUG status is returned in the first
 769     byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
 770
 771     Returns an openocd result code.
 772 */
 773 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 774 {
 775         int retries = 0;
 776         int res;
 777         struct stlink_usb_handle_s *h = handle;
 778
 779         while (1) {
 780                 if ((h->transport != HL_TRANSPORT_SWIM) || !retries) {
 781                         res = stlink_usb_xfer(handle, buf, size);
 782                         if (res != ERROR_OK)
 783                                 return res;
 784                 }
 785
 786                 if (h->transport == HL_TRANSPORT_SWIM) {
 787                         res = stlink_swim_status(handle);
 788                         if (res != ERROR_OK)
 789                                 return res;
 790                 }
 791
 792                 res = stlink_usb_error_check(handle);
 793                 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 794                         useconds_t delay_us = (1<<retries++) * 1000;
 795                         LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
 796                         usleep(delay_us);
 797                         continue;
 798                 }
 799                 return res;
 800         }
 801 }
 802
 803 /** */
 804 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 805 {
 806         struct stlink_usb_handle_s *h = handle;
 807
 808         assert(handle != NULL);
 809
 810         assert(h->version.flags & STLINK_F_HAS_TRACE);
 811
 812         if (jtag_libusb_bulk_read(h->fd, h->trace_ep, (char *)buf,
 813                         size, STLINK_READ_TIMEOUT) != size) {
 814                 LOG_ERROR("bulk trace read failed");
 815                 return ERROR_FAIL;
 816         }
 817
 818         return ERROR_OK;
 819 }
 820
 821 /*
 822         this function writes transfer length in
 823         the right place in the cb
 824 */
 825 static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
 826 {
 827         struct stlink_usb_handle_s *h = handle;
 828
 829         buf_set_u32(h->cmdbuf+8, 0, 32, size);
 830 }
 831
 832 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 833 {
 834         struct stlink_usb_handle_s *h = handle;
 835
 836         /* fill the send buffer */
 837         strcpy((char *)h->cmdbuf, "USBC");
 838         h->cmdidx += 4;
 839         /* csw tag not used */
 840         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
 841         h->cmdidx += 4;
 842         /* cbw data transfer length (in the following data phase in or out) */
 843         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 844         h->cmdidx += 4;
 845         /* cbw flags */
 846         h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
 847         h->cmdbuf[h->cmdidx++] = 0; /* lun */
 848         /* cdb clength (is filled in at xfer) */
 849         h->cmdbuf[h->cmdidx++] = 0;
 850 }
 851
 852 /** */
 853 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
 854 {
 855         struct stlink_usb_handle_s *h = handle;
 856
 857         h->direction = direction;
 858
 859         h->cmdidx = 0;
 860
 861         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 862         memset(h->databuf, 0, STLINK_DATA_SIZE);
 863
 864         if (h->version.stlink == 1)
 865                 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
 866 }
 867
 868 /** */
 869 static int stlink_usb_version(void *handle)
 870 {
 871         int res;
 872         uint32_t flags;
 873         uint16_t version;
 874         uint8_t v, x, y, jtag, swim, msd, bridge = 0;
 875         char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
 876         char *p;
 877         struct stlink_usb_handle_s *h = handle;
 878
 879         assert(handle != NULL);
 880
 881         stlink_usb_init_buffer(handle, h->rx_ep, 6);
 882
 883         h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
 884
 885         res = stlink_usb_xfer(handle, h->databuf, 6);
 886
 887         if (res != ERROR_OK)
 888                 return res;
 889
 890         version = be_to_h_u16(h->databuf);
 891         v = (version >> 12) & 0x0f;
 892         x = (version >> 6) & 0x3f;
 893         y = version & 0x3f;
 894
 895         h->vid = le_to_h_u16(h->databuf + 2);
 896         h->pid = le_to_h_u16(h->databuf + 4);
 897
 898         switch (h->pid) {
 899         case STLINK_V2_1_PID:
 900         case STLINK_V2_1_NO_MSD_PID:
 901                 if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
 902                         /* MxSy : STM8 V2.1 - SWIM only */
 903                         msd = x;
 904                         swim = y;
 905                         jtag = 0;
 906                 } else {
 907                         /* JxMy : STM32 V2.1 - JTAG/SWD only */
 908                         jtag = x;
 909                         msd = y;
 910                         swim = 0;
 911                 }
 912                 break;
 913         default:
 914                 jtag = x;
 915                 swim = y;
 916                 msd = 0;
 917                 break;
 918         }
 919
 920         /* STLINK-V3 requires a specific command */
 921         if (v == 3 && x == 0 && y == 0) {
 922                 stlink_usb_init_buffer(handle, h->rx_ep, 16);
 923
 924                 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
 925
 926                 res = stlink_usb_xfer(handle, h->databuf, 12);
 927                 if (res != ERROR_OK)
 928                         return res;
 929
 930                 v = h->databuf[0];
 931                 swim = h->databuf[1];
 932                 jtag = h->databuf[2];
 933                 msd  = h->databuf[3];
 934                 bridge = h->databuf[4];
 935                 h->vid = le_to_h_u16(h->databuf + 8);
 936                 h->pid = le_to_h_u16(h->databuf + 10);
 937         }
 938
 939         h->version.stlink = v;
 940         h->version.jtag = jtag;
 941         h->version.swim = swim;
 942
 943         flags = 0;
 944         switch (h->version.stlink) {
 945         case 1:
 946                 /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
 947                 if (h->version.jtag >= 11)
 948                         h->version.jtag_api = STLINK_JTAG_API_V2;
 949                 else
 950                         h->version.jtag_api = STLINK_JTAG_API_V1;
 951
 952                 break;
 953         case 2:
 954                 /* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
 955                 h->version.jtag_api = STLINK_JTAG_API_V2;
 956
 957                 /* API for trace from J13 */
 958                 /* API for target voltage from J13 */
 959                 if (h->version.jtag >= 13)
 960                         flags |= STLINK_F_HAS_TRACE;
 961
 962                 /* preferred API to get last R/W status from J15 */
 963                 if (h->version.jtag >= 15)
 964                         flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
 965
 966                 /* API to set SWD frequency from J22 */
 967                 if (h->version.jtag >= 22)
 968                         flags |= STLINK_F_HAS_SWD_SET_FREQ;
 969
 970                 /* API to set JTAG frequency from J24 */
 971                 if (h->version.jtag >= 24)
 972                         flags |= STLINK_F_HAS_JTAG_SET_FREQ;
 973
 974                 /* API to read/write memory at 16 bit from J26 */
 975                 if (h->version.jtag >= 26)
 976                         flags |= STLINK_F_HAS_MEM_16BIT;
 977
 978                 break;
 979         case 3:
 980                 /* all STLINK-V3 use api-v3 */
 981                 h->version.jtag_api = STLINK_JTAG_API_V3;
 982
 983                 /* STLINK-V3 is a superset of ST-LINK/V2 */
 984
 985                 /* API for trace */
 986                 /* API for target voltage */
 987                 flags |= STLINK_F_HAS_TRACE;
 988
 989                 /* preferred API to get last R/W status */
 990                 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
 991
 992                 /* API to read/write memory at 16 bit */
 993                 flags |= STLINK_F_HAS_MEM_16BIT;
 994
 995                 break;
 996         default:
 997                 break;
 998         }
 999         h->version.flags = flags;
1000
1001         p = v_str;
1002         p += sprintf(p, "V%d", v);
1003         if (jtag || !msd)
1004                 p += sprintf(p, "J%d", jtag);
1005         if (msd)
1006                 p += sprintf(p, "M%d", msd);
1007         if (bridge)
1008                 p += sprintf(p, "B%d", bridge);
1009         if (swim || !msd)
1010                 sprintf(p, "S%d", swim);
1011
1012         LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
1013                 v_str,
1014                 h->version.jtag_api,
1015                 h->vid,
1016                 h->pid);
1017
1018         return ERROR_OK;
1019 }
1020
1021 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
1022 {
1023         struct stlink_usb_handle_s *h = handle;
1024         uint32_t adc_results[2];
1025
1026         /* no error message, simply quit with error */
1027         if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
1028                 return ERROR_COMMAND_NOTFOUND;
1029
1030         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1031
1032         h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
1033
1034         int result = stlink_usb_xfer(handle, h->databuf, 8);
1035
1036         if (result != ERROR_OK)
1037                 return result;
1038
1039         /* convert result */
1040         adc_results[0] = le_to_h_u32(h->databuf);
1041         adc_results[1] = le_to_h_u32(h->databuf + 4);
1042
1043         *target_voltage = 0;
1044
1045         if (adc_results[0])
1046                 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
1047
1048         LOG_INFO("Target voltage: %f", (double)*target_voltage);
1049
1050         return ERROR_OK;
1051 }
1052
1053 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
1054 {
1055         struct stlink_usb_handle_s *h = handle;
1056
1057         assert(handle != NULL);
1058
1059         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
1060                 return ERROR_COMMAND_NOTFOUND;
1061
1062         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1063
1064         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1065         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
1066         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1067         h->cmdidx += 2;
1068
1069         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1070
1071         if (result != ERROR_OK)
1072                 return result;
1073
1074         return ERROR_OK;
1075 }
1076
1077 static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
1078 {
1079         struct stlink_usb_handle_s *h = handle;
1080
1081         assert(handle != NULL);
1082
1083         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
1084                 return ERROR_COMMAND_NOTFOUND;
1085
1086         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1087
1088         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1089         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
1090         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1091         h->cmdidx += 2;
1092
1093         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1094
1095         if (result != ERROR_OK)
1096                 return result;
1097
1098         return ERROR_OK;
1099 }
1100
1101 /** */
1102 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
1103 {
1104         int res;
1105         struct stlink_usb_handle_s *h = handle;
1106
1107         assert(handle != NULL);
1108
1109         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1110
1111         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
1112
1113         res = stlink_usb_xfer(handle, h->databuf, 2);
1114
1115         if (res != ERROR_OK)
1116                 return res;
1117
1118         *mode = h->databuf[0];
1119
1120         return ERROR_OK;
1121 }
1122
1123 /** */
1124 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
1125 {
1126         int rx_size = 0;
1127         struct stlink_usb_handle_s *h = handle;
1128
1129         assert(handle != NULL);
1130
1131         /* on api V2 we are able the read the latest command
1132          * status
1133          * TODO: we need the test on api V1 too
1134          */
1135         if (h->version.jtag_api != STLINK_JTAG_API_V1)
1136                 rx_size = 2;
1137
1138         stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
1139
1140         switch (type) {
1141                 case STLINK_MODE_DEBUG_JTAG:
1142                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1143                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1144                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1145                         else
1146                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1147                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
1148                         break;
1149                 case STLINK_MODE_DEBUG_SWD:
1150                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1151                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1152                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1153                         else
1154                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1155                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
1156                         break;
1157                 case STLINK_MODE_DEBUG_SWIM:
1158                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1159                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
1160                         /* no answer for this function... */
1161                         rx_size = 0;
1162                         break;
1163                 case STLINK_MODE_DFU:
1164                 case STLINK_MODE_MASS:
1165                 default:
1166                         return ERROR_FAIL;
1167         }
1168
1169         return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
1170 }
1171
1172 /** */
1173 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
1174 {
1175         int res;
1176         struct stlink_usb_handle_s *h = handle;
1177
1178         assert(handle != NULL);
1179
1180         stlink_usb_init_buffer(handle, STLINK_NULL_EP, 0);
1181
1182         switch (type) {
1183                 case STLINK_MODE_DEBUG_JTAG:
1184                 case STLINK_MODE_DEBUG_SWD:
1185                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1186                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
1187                         break;
1188                 case STLINK_MODE_DEBUG_SWIM:
1189                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1190                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
1191                         break;
1192                 case STLINK_MODE_DFU:
1193                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
1194                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
1195                         break;
1196                 case STLINK_MODE_MASS:
1197                 default:
1198                         return ERROR_FAIL;
1199         }
1200
1201         res = stlink_usb_xfer(handle, 0, 0);
1202
1203         if (res != ERROR_OK)
1204                 return res;
1205
1206         return ERROR_OK;
1207 }
1208
1209 static int stlink_usb_assert_srst(void *handle, int srst);
1210
1211 static enum stlink_mode stlink_get_mode(enum hl_transports t)
1212 {
1213         switch (t) {
1214         case HL_TRANSPORT_SWD:
1215                 return STLINK_MODE_DEBUG_SWD;
1216         case HL_TRANSPORT_JTAG:
1217                 return STLINK_MODE_DEBUG_JTAG;
1218         case HL_TRANSPORT_SWIM:
1219                 return STLINK_MODE_DEBUG_SWIM;
1220         default:
1221                 return STLINK_MODE_UNKNOWN;
1222         }
1223 }
1224
1225 /** */
1226 static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
1227 {
1228         int res;
1229         uint8_t mode;
1230         enum stlink_mode emode;
1231         struct stlink_usb_handle_s *h = handle;
1232
1233         assert(handle != NULL);
1234
1235         res = stlink_usb_current_mode(handle, &mode);
1236
1237         if (res != ERROR_OK)
1238                 return res;
1239
1240         LOG_DEBUG("MODE: 0x%02X", mode);
1241
1242         /* try to exit current mode */
1243         switch (mode) {
1244                 case STLINK_DEV_DFU_MODE:
1245                         emode = STLINK_MODE_DFU;
1246                         break;
1247                 case STLINK_DEV_DEBUG_MODE:
1248                         emode = STLINK_MODE_DEBUG_SWD;
1249                         break;
1250                 case STLINK_DEV_SWIM_MODE:
1251                         emode = STLINK_MODE_DEBUG_SWIM;
1252                         break;
1253                 case STLINK_DEV_BOOTLOADER_MODE:
1254                 case STLINK_DEV_MASS_MODE:
1255                 default:
1256                         emode = STLINK_MODE_UNKNOWN;
1257                         break;
1258         }
1259
1260         if (emode != STLINK_MODE_UNKNOWN) {
1261                 res = stlink_usb_mode_leave(handle, emode);
1262
1263                 if (res != ERROR_OK)
1264                         return res;
1265         }
1266
1267         res = stlink_usb_current_mode(handle, &mode);
1268
1269         if (res != ERROR_OK)
1270                 return res;
1271
1272         /* we check the target voltage here as an aid to debugging connection problems.
1273          * the stlink requires the target Vdd to be connected for reliable debugging.
1274          * this cmd is supported in all modes except DFU
1275          */
1276         if (mode != STLINK_DEV_DFU_MODE) {
1277
1278                 float target_voltage;
1279
1280                 /* check target voltage (if supported) */
1281                 res = stlink_usb_check_voltage(h, &target_voltage);
1282
1283                 if (res != ERROR_OK) {
1284                         if (res != ERROR_COMMAND_NOTFOUND)
1285                                 LOG_ERROR("voltage check failed");
1286                         /* attempt to continue as it is not a catastrophic failure */
1287                 } else {
1288                         /* check for a sensible target voltage, operating range is 1.65-5.5v
1289                          * according to datasheet */
1290                         if (target_voltage < 1.5)
1291                                 LOG_ERROR("target voltage may be too low for reliable debugging");
1292                 }
1293         }
1294
1295         LOG_DEBUG("MODE: 0x%02X", mode);
1296
1297         /* set selected mode */
1298         emode = stlink_get_mode(h->transport);
1299
1300         if (emode == STLINK_MODE_UNKNOWN) {
1301                 LOG_ERROR("selected mode (transport) not supported");
1302                 return ERROR_FAIL;
1303         }
1304
1305         /* preliminary SRST assert:
1306          * We want SRST is asserted before activating debug signals (mode_enter).
1307          * As the required mode has not been set, the adapter may not know what pin to use.
1308          * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1309          * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1310          * after power on, SWIM_RST stays unchanged */
1311         if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1312                 stlink_usb_assert_srst(handle, 0);
1313                 /* do not check the return status here, we will
1314                    proceed and enter the desired mode below
1315                    and try asserting srst again. */
1316
1317         res = stlink_usb_mode_enter(handle, emode);
1318         if (res != ERROR_OK)
1319                 return res;
1320
1321         /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1322         if (connect_under_reset) {
1323                 res = stlink_usb_assert_srst(handle, 0);
1324                 if (res != ERROR_OK)
1325                         return res;
1326         }
1327
1328         res = stlink_usb_current_mode(handle, &mode);
1329
1330         if (res != ERROR_OK)
1331                 return res;
1332
1333         LOG_DEBUG("MODE: 0x%02X", mode);
1334
1335         return ERROR_OK;
1336 }
1337
1338 /* request status from last swim request */
1339 static int stlink_swim_status(void *handle)
1340 {
1341         struct stlink_usb_handle_s *h = handle;
1342         int res;
1343
1344         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1345         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1346         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1347         res = stlink_usb_xfer(handle, h->databuf, 4);
1348         if (res != ERROR_OK)
1349                 return res;
1350         return ERROR_OK;
1351 }
1352 /*
1353         the purpose of this function is unknown...
1354         capabilites? anyway for swim v6 it returns
1355         0001020600000000
1356 */
1357 __attribute__((unused))
1358 static int stlink_swim_cap(void *handle, uint8_t *cap)
1359 {
1360         struct stlink_usb_handle_s *h = handle;
1361         int res;
1362
1363         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1364         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1365         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1366         h->cmdbuf[h->cmdidx++] = 0x01;
1367         res = stlink_usb_xfer(handle, h->databuf, 8);
1368         if (res != ERROR_OK)
1369                 return res;
1370         memcpy(cap, h->databuf, 8);
1371         return ERROR_OK;
1372 }
1373
1374 /*      debug dongle assert/deassert sreset line */
1375 static int stlink_swim_assert_reset(void *handle, int reset)
1376 {
1377         struct stlink_usb_handle_s *h = handle;
1378         int res;
1379
1380         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1381         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1382         if (!reset)
1383                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1384         else
1385                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1386         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1387         if (res != ERROR_OK)
1388                 return res;
1389         return ERROR_OK;
1390 }
1391
1392 /*
1393         send swim enter seq
1394         1.3ms low then 750Hz then 1.5kHz
1395 */
1396 static int stlink_swim_enter(void *handle)
1397 {
1398         struct stlink_usb_handle_s *h = handle;
1399         int res;
1400
1401         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1402         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1403         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1404         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1405         if (res != ERROR_OK)
1406                 return res;
1407         return ERROR_OK;
1408 }
1409
1410 /*      switch high/low speed swim */
1411 static int stlink_swim_speed(void *handle, int speed)
1412 {
1413         struct stlink_usb_handle_s *h = handle;
1414         int res;
1415
1416         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1417         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1418         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1419         if (speed)
1420                 h->cmdbuf[h->cmdidx++] = 1;
1421         else
1422                 h->cmdbuf[h->cmdidx++] = 0;
1423         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1424         if (res != ERROR_OK)
1425                 return res;
1426         return ERROR_OK;
1427 }
1428
1429 /*
1430         initiate srst from swim.
1431         nrst is pulled low for 50us.
1432 */
1433 static int stlink_swim_generate_rst(void *handle)
1434 {
1435         struct stlink_usb_handle_s *h = handle;
1436         int res;
1437
1438         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1439         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1440         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1441         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1442         if (res != ERROR_OK)
1443                 return res;
1444         return ERROR_OK;
1445 }
1446
1447 /*
1448         send resyncronize sequence
1449         swim is pulled low for 16us
1450         reply is 64 clks low
1451 */
1452 static int stlink_swim_resync(void *handle)
1453 {
1454         struct stlink_usb_handle_s *h = handle;
1455         int res;
1456
1457         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1458         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1459         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1460         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1461         if (res != ERROR_OK)
1462                 return res;
1463         return ERROR_OK;
1464 }
1465
1466 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1467 {
1468         struct stlink_usb_handle_s *h = handle;
1469         int res;
1470         unsigned int i;
1471         unsigned int datalen = 0;
1472         int cmdsize = STLINK_CMD_SIZE_V2;
1473
1474         if (len > STLINK_DATA_SIZE)
1475                 return ERROR_FAIL;
1476
1477         if (h->version.stlink == 1)
1478                 cmdsize = STLINK_SG_SIZE;
1479
1480         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1481         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1482         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1483         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1484         h->cmdidx += 2;
1485         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1486         h->cmdidx += 4;
1487         for (i = 0; i < len; i++) {
1488                 if (h->cmdidx == cmdsize)
1489                         h->databuf[datalen++] = *(data++);
1490                 else
1491                         h->cmdbuf[h->cmdidx++] = *(data++);
1492         }
1493         if (h->version.stlink == 1)
1494                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1495
1496         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1497         if (res != ERROR_OK)
1498                 return res;
1499         return ERROR_OK;
1500 }
1501
1502 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1503 {
1504         struct stlink_usb_handle_s *h = handle;
1505         int res;
1506
1507         if (len > STLINK_DATA_SIZE)
1508                 return ERROR_FAIL;
1509
1510         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1511         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1512         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1513         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1514         h->cmdidx += 2;
1515         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1516         h->cmdidx += 4;
1517         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1518         if (res != ERROR_OK)
1519                 return res;
1520
1521         stlink_usb_init_buffer(handle, h->rx_ep, len);
1522         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1523         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1524         res = stlink_usb_xfer(handle, data, len);
1525         if (res != ERROR_OK)
1526                 return res;
1527
1528         return ERROR_OK;
1529 }
1530
1531 /** */
1532 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1533 {
1534         int res;
1535         struct stlink_usb_handle_s *h = handle;
1536
1537         assert(handle != NULL);
1538
1539         /* there is no swim read core id cmd */
1540         if (h->transport == HL_TRANSPORT_SWIM) {
1541                 *idcode = 0;
1542                 return ERROR_OK;
1543         }
1544
1545         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1546
1547         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1548         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1549
1550         res = stlink_usb_xfer(handle, h->databuf, 4);
1551
1552         if (res != ERROR_OK)
1553                 return res;
1554
1555         *idcode = le_to_h_u32(h->databuf);
1556
1557         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1558
1559         return ERROR_OK;
1560 }
1561
1562 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1563 {
1564         struct stlink_usb_handle_s *h = handle;
1565         int res;
1566
1567         assert(handle != NULL);
1568
1569         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1570
1571         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1572         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1573         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1574         h->cmdidx += 4;
1575
1576         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1577         if (res != ERROR_OK)
1578                 return res;
1579
1580         *val = le_to_h_u32(h->databuf + 4);
1581         return ERROR_OK;
1582 }
1583
1584 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
1585 {
1586         struct stlink_usb_handle_s *h = handle;
1587
1588         assert(handle != NULL);
1589
1590         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1591
1592         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1593         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1594                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
1595         else
1596                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
1597         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1598         h->cmdidx += 4;
1599         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1600         h->cmdidx += 4;
1601
1602         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1603 }
1604
1605 /** */
1606 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
1607 {
1608         struct stlink_usb_handle_s *h = handle;
1609
1610         assert(handle != NULL);
1611
1612         if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
1613                 int res;
1614
1615                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1616
1617                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1618                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1619
1620                 res = stlink_usb_xfer(handle, h->databuf, 2);
1621                 if (res != ERROR_OK)
1622                         return res;
1623
1624                 size_t bytes_avail = le_to_h_u16(h->databuf);
1625                 *size = bytes_avail < *size ? bytes_avail : *size - 1;
1626
1627                 if (*size > 0) {
1628                         res = stlink_usb_read_trace(handle, buf, *size);
1629                         if (res != ERROR_OK)
1630                                 return res;
1631                         return ERROR_OK;
1632                 }
1633         }
1634         *size = 0;
1635         return ERROR_OK;
1636 }
1637
1638 static enum target_state stlink_usb_v2_get_status(void *handle)
1639 {
1640         int result;
1641         uint32_t status;
1642
1643         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
1644         if  (result != ERROR_OK)
1645                 return TARGET_UNKNOWN;
1646
1647         if (status & S_HALT)
1648                 return TARGET_HALTED;
1649         else if (status & S_RESET_ST)
1650                 return TARGET_RESET;
1651
1652         return TARGET_RUNNING;
1653 }
1654
1655 /** */
1656 static enum target_state stlink_usb_state(void *handle)
1657 {
1658         int res;
1659         struct stlink_usb_handle_s *h = handle;
1660
1661         assert(handle != NULL);
1662
1663         if (h->transport == HL_TRANSPORT_SWIM) {
1664                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1665                 if (res != ERROR_OK)
1666                         return TARGET_UNKNOWN;
1667
1668                 res = stlink_swim_resync(handle);
1669                 if (res != ERROR_OK)
1670                         return TARGET_UNKNOWN;
1671
1672                 return ERROR_OK;
1673         }
1674
1675         if (h->reconnect_pending) {
1676                 LOG_INFO("Previous state query failed, trying to reconnect");
1677                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1678
1679                 if (res != ERROR_OK)
1680                         return TARGET_UNKNOWN;
1681
1682                 h->reconnect_pending = false;
1683         }
1684
1685         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1686                 res = stlink_usb_v2_get_status(handle);
1687                 if (res == TARGET_UNKNOWN)
1688                         h->reconnect_pending = true;
1689                 return res;
1690         }
1691
1692         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1693
1694         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1695         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
1696
1697         res = stlink_usb_xfer(handle, h->databuf, 2);
1698
1699         if (res != ERROR_OK)
1700                 return TARGET_UNKNOWN;
1701
1702         if (h->databuf[0] == STLINK_CORE_RUNNING)
1703                 return TARGET_RUNNING;
1704         if (h->databuf[0] == STLINK_CORE_HALTED)
1705                 return TARGET_HALTED;
1706
1707         h->reconnect_pending = true;
1708
1709         return TARGET_UNKNOWN;
1710 }
1711
1712 static int stlink_usb_assert_srst(void *handle, int srst)
1713 {
1714         struct stlink_usb_handle_s *h = handle;
1715
1716         assert(handle != NULL);
1717
1718         if (h->transport == HL_TRANSPORT_SWIM)
1719                 return stlink_swim_assert_reset(handle, srst);
1720
1721         if (h->version.stlink == 1)
1722                 return ERROR_COMMAND_NOTFOUND;
1723
1724         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1725
1726         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1727         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
1728         h->cmdbuf[h->cmdidx++] = srst;
1729
1730         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1731 }
1732
1733 /** */
1734 static void stlink_usb_trace_disable(void *handle)
1735 {
1736         int res = ERROR_OK;
1737         struct stlink_usb_handle_s *h = handle;
1738
1739         assert(handle != NULL);
1740
1741         assert(h->version.flags & STLINK_F_HAS_TRACE);
1742
1743         LOG_DEBUG("Tracing: disable");
1744
1745         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1746         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1747         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1748         res = stlink_usb_xfer(handle, h->databuf, 2);
1749
1750         if (res == ERROR_OK)
1751                 h->trace.enabled = false;
1752 }
1753
1754
1755 /** */
1756 static int stlink_usb_trace_enable(void *handle)
1757 {
1758         int res;
1759         struct stlink_usb_handle_s *h = handle;
1760
1761         assert(handle != NULL);
1762
1763         if (h->version.flags & STLINK_F_HAS_TRACE) {
1764                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1765
1766                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1767                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1768                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1769                 h->cmdidx += 2;
1770                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
1771                 h->cmdidx += 4;
1772
1773                 res = stlink_usb_xfer(handle, h->databuf, 2);
1774
1775                 if (res == ERROR_OK)  {
1776                         h->trace.enabled = true;
1777                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
1778                 }
1779         } else {
1780                 LOG_ERROR("Tracing is not supported by this version.");
1781                 res = ERROR_FAIL;
1782         }
1783
1784         return res;
1785 }
1786
1787 /** */
1788 static int stlink_usb_reset(void *handle)
1789 {
1790         struct stlink_usb_handle_s *h = handle;
1791         int retval;
1792
1793         assert(handle != NULL);
1794
1795         if (h->transport == HL_TRANSPORT_SWIM)
1796                 return stlink_swim_generate_rst(handle);
1797
1798         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1799
1800         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1801
1802         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1803                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
1804         else
1805                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
1806
1807         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
1808         if (retval != ERROR_OK)
1809                 return retval;
1810
1811         if (h->trace.enabled) {
1812                 stlink_usb_trace_disable(h);
1813                 return stlink_usb_trace_enable(h);
1814         }
1815
1816         return ERROR_OK;
1817 }
1818
1819 /** */
1820 static int stlink_usb_run(void *handle)
1821 {
1822         int res;
1823         struct stlink_usb_handle_s *h = handle;
1824
1825         assert(handle != NULL);
1826
1827         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1828                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1829
1830                 return res;
1831         }
1832
1833         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1834
1835         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1836         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1837
1838         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1839 }
1840
1841 /** */
1842 static int stlink_usb_halt(void *handle)
1843 {
1844         int res;
1845         struct stlink_usb_handle_s *h = handle;
1846
1847         assert(handle != NULL);
1848
1849         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1850                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1851
1852                 return res;
1853         }
1854
1855         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1856
1857         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1858         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1859
1860         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1861 }
1862
1863 /** */
1864 static int stlink_usb_step(void *handle)
1865 {
1866         struct stlink_usb_handle_s *h = handle;
1867
1868         assert(handle != NULL);
1869
1870         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1871                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1872                  * that the Cortex-M3 currently does. */
1873                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1874                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1875                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1876         }
1877
1878         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1879
1880         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1881         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1882
1883         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1884 }
1885
1886 /** */
1887 static int stlink_usb_read_regs(void *handle)
1888 {
1889         int res;
1890         struct stlink_usb_handle_s *h = handle;
1891
1892         assert(handle != NULL);
1893
1894         stlink_usb_init_buffer(handle, h->rx_ep, 84);
1895
1896         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1897         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1898                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
1899         else
1900                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
1901
1902         res = stlink_usb_xfer(handle, h->databuf, 84);
1903
1904         if (res != ERROR_OK)
1905                 return res;
1906
1907         return ERROR_OK;
1908 }
1909
1910 /** */
1911 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
1912 {
1913         int res;
1914         struct stlink_usb_handle_s *h = handle;
1915
1916         assert(handle != NULL);
1917
1918         stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1919
1920         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1921         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1922                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
1923         else
1924                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
1925         h->cmdbuf[h->cmdidx++] = num;
1926
1927         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1928                 res = stlink_usb_xfer(handle, h->databuf, 4);
1929                 if (res != ERROR_OK)
1930                         return res;
1931                 *val = le_to_h_u32(h->databuf);
1932                 return ERROR_OK;
1933         } else {
1934                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1935                 if (res != ERROR_OK)
1936                         return res;
1937                 *val = le_to_h_u32(h->databuf + 4);
1938                 return ERROR_OK;
1939         }
1940 }
1941
1942 /** */
1943 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
1944 {
1945         struct stlink_usb_handle_s *h = handle;
1946
1947         assert(handle != NULL);
1948
1949         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1950
1951         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1952         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1953                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
1954         else
1955                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
1956         h->cmdbuf[h->cmdidx++] = num;
1957         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1958         h->cmdidx += 4;
1959
1960         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1961 }
1962
1963 static int stlink_usb_get_rw_status(void *handle)
1964 {
1965         int res;
1966         struct stlink_usb_handle_s *h = handle;
1967
1968         assert(handle != NULL);
1969
1970         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1971                 return ERROR_OK;
1972
1973         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1974
1975         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1976         if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
1977                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
1978
1979                 res = stlink_usb_xfer(handle, h->databuf, 12);
1980         } else {
1981                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
1982
1983                 res = stlink_usb_xfer(handle, h->databuf, 2);
1984         }
1985
1986         if (res != ERROR_OK)
1987                 return res;
1988
1989         return stlink_usb_error_check(h);
1990 }
1991
1992 /** */
1993 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1994                           uint8_t *buffer)
1995 {
1996         int res;
1997         uint16_t read_len = len;
1998         struct stlink_usb_handle_s *h = handle;
1999
2000         assert(handle != NULL);
2001
2002         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2003         if (len > stlink_usb_block(h)) {
2004                 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2005                 return ERROR_FAIL;
2006         }
2007
2008         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2009
2010         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2011         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2012         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2013         h->cmdidx += 4;
2014         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2015         h->cmdidx += 2;
2016
2017         /* we need to fix read length for single bytes */
2018         if (read_len == 1)
2019                 read_len++;
2020
2021         res = stlink_usb_xfer(handle, h->databuf, read_len);
2022
2023         if (res != ERROR_OK)
2024                 return res;
2025
2026         memcpy(buffer, h->databuf, len);
2027
2028         return stlink_usb_get_rw_status(handle);
2029 }
2030
2031 /** */
2032 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
2033                            const uint8_t *buffer)
2034 {
2035         int res;
2036         struct stlink_usb_handle_s *h = handle;
2037
2038         assert(handle != NULL);
2039
2040         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2041         if (len > stlink_usb_block(h)) {
2042                 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2043                 return ERROR_FAIL;
2044         }
2045
2046         stlink_usb_init_buffer(handle, h->tx_ep, len);
2047
2048         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2049         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2050         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2051         h->cmdidx += 4;
2052         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2053         h->cmdidx += 2;
2054
2055         res = stlink_usb_xfer(handle, buffer, len);
2056
2057         if (res != ERROR_OK)
2058                 return res;
2059
2060         return stlink_usb_get_rw_status(handle);
2061 }
2062
2063 /** */
2064 static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
2065                           uint8_t *buffer)
2066 {
2067         int res;
2068         struct stlink_usb_handle_s *h = handle;
2069
2070         assert(handle != NULL);
2071
2072         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2073                 return ERROR_COMMAND_NOTFOUND;
2074
2075         /* data must be a multiple of 2 and half-word aligned */
2076         if (len % 2 || addr % 2) {
2077                 LOG_DEBUG("Invalid data alignment");
2078                 return ERROR_TARGET_UNALIGNED_ACCESS;
2079         }
2080
2081         stlink_usb_init_buffer(handle, h->rx_ep, len);
2082
2083         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2084         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2085         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2086         h->cmdidx += 4;
2087         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2088         h->cmdidx += 2;
2089
2090         res = stlink_usb_xfer(handle, h->databuf, len);
2091
2092         if (res != ERROR_OK)
2093                 return res;
2094
2095         memcpy(buffer, h->databuf, len);
2096
2097         return stlink_usb_get_rw_status(handle);
2098 }
2099
2100 /** */
2101 static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
2102                            const uint8_t *buffer)
2103 {
2104         int res;
2105         struct stlink_usb_handle_s *h = handle;
2106
2107         assert(handle != NULL);
2108
2109         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2110                 return ERROR_COMMAND_NOTFOUND;
2111
2112         /* data must be a multiple of 2 and half-word aligned */
2113         if (len % 2 || addr % 2) {
2114                 LOG_DEBUG("Invalid data alignment");
2115                 return ERROR_TARGET_UNALIGNED_ACCESS;
2116         }
2117
2118         stlink_usb_init_buffer(handle, h->tx_ep, len);
2119
2120         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2121         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2122         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2123         h->cmdidx += 4;
2124         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2125         h->cmdidx += 2;
2126
2127         res = stlink_usb_xfer(handle, buffer, len);
2128
2129         if (res != ERROR_OK)
2130                 return res;
2131
2132         return stlink_usb_get_rw_status(handle);
2133 }
2134
2135 /** */
2136 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
2137                           uint8_t *buffer)
2138 {
2139         int res;
2140         struct stlink_usb_handle_s *h = handle;
2141
2142         assert(handle != NULL);
2143
2144         /* data must be a multiple of 4 and word aligned */
2145         if (len % 4 || addr % 4) {
2146                 LOG_DEBUG("Invalid data alignment");
2147                 return ERROR_TARGET_UNALIGNED_ACCESS;
2148         }
2149
2150         stlink_usb_init_buffer(handle, h->rx_ep, len);
2151
2152         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2153         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2154         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2155         h->cmdidx += 4;
2156         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2157         h->cmdidx += 2;
2158
2159         res = stlink_usb_xfer(handle, h->databuf, len);
2160
2161         if (res != ERROR_OK)
2162                 return res;
2163
2164         memcpy(buffer, h->databuf, len);
2165
2166         return stlink_usb_get_rw_status(handle);
2167 }
2168
2169 /** */
2170 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
2171                            const uint8_t *buffer)
2172 {
2173         int res;
2174         struct stlink_usb_handle_s *h = handle;
2175
2176         assert(handle != NULL);
2177
2178         /* data must be a multiple of 4 and word aligned */
2179         if (len % 4 || addr % 4) {
2180                 LOG_DEBUG("Invalid data alignment");
2181                 return ERROR_TARGET_UNALIGNED_ACCESS;
2182         }
2183
2184         stlink_usb_init_buffer(handle, h->tx_ep, len);
2185
2186         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2187         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2188         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2189         h->cmdidx += 4;
2190         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2191         h->cmdidx += 2;
2192
2193         res = stlink_usb_xfer(handle, buffer, len);
2194
2195         if (res != ERROR_OK)
2196                 return res;
2197
2198         return stlink_usb_get_rw_status(handle);
2199 }
2200
2201 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2202 {
2203         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2204         if (max_tar_block == 0)
2205                 max_tar_block = 4;
2206         return max_tar_block;
2207 }
2208
2209 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2210                 uint32_t count, uint8_t *buffer)
2211 {
2212         int retval = ERROR_OK;
2213         uint32_t bytes_remaining;
2214         int retries = 0;
2215         struct stlink_usb_handle_s *h = handle;
2216
2217         /* calculate byte count */
2218         count *= size;
2219
2220         /* switch to 8 bit if stlink does not support 16 bit memory read */
2221         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2222                 size = 1;
2223
2224         while (count) {
2225
2226                 bytes_remaining = (size != 1) ? \
2227                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2228
2229                 if (count < bytes_remaining)
2230                         bytes_remaining = count;
2231
2232                 if (h->transport == HL_TRANSPORT_SWIM) {
2233                         retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
2234                         if (retval != ERROR_OK)
2235                                 return retval;
2236                 } else
2237                 /*
2238                  * all stlink support 8/32bit memory read/writes and only from
2239                  * stlink V2J26 there is support for 16 bit memory read/write.
2240                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2241                  * as 8bit access.
2242                  */
2243                 if (size != 1) {
2244
2245                         /* When in jtag mode the stlink uses the auto-increment functionality.
2246                          * However it expects us to pass the data correctly, this includes
2247                          * alignment and any page boundaries. We already do this as part of the
2248                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2249                          * needs implementing manually.
2250                          * currently this only affects jtag mode, according to ST they do single
2251                          * access in SWD mode - but this may change and so we do it for both modes */
2252
2253                         /* we first need to check for any unaligned bytes */
2254                         if (addr & (size - 1)) {
2255
2256                                 uint32_t head_bytes = size - (addr & (size - 1));
2257                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
2258                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2259                                         usleep((1<<retries++) * 1000);
2260                                         continue;
2261                                 }
2262                                 if (retval != ERROR_OK)
2263                                         return retval;
2264                                 buffer += head_bytes;
2265                                 addr += head_bytes;
2266                                 count -= head_bytes;
2267                                 bytes_remaining -= head_bytes;
2268                         }
2269
2270                         if (bytes_remaining & (size - 1))
2271                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
2272                         else if (size == 2)
2273                                 retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
2274                         else
2275                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
2276                 } else
2277                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
2278
2279                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2280                         usleep((1<<retries++) * 1000);
2281                         continue;
2282                 }
2283                 if (retval != ERROR_OK)
2284                         return retval;
2285
2286                 buffer += bytes_remaining;
2287                 addr += bytes_remaining;
2288                 count -= bytes_remaining;
2289         }
2290
2291         return retval;
2292 }
2293
2294 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2295                 uint32_t count, const uint8_t *buffer)
2296 {
2297         int retval = ERROR_OK;
2298         uint32_t bytes_remaining;
2299         int retries = 0;
2300         struct stlink_usb_handle_s *h = handle;
2301
2302         /* calculate byte count */
2303         count *= size;
2304
2305         /* switch to 8 bit if stlink does not support 16 bit memory read */
2306         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2307                 size = 1;
2308
2309         while (count) {
2310
2311                 bytes_remaining = (size != 1) ? \
2312                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2313
2314                 if (count < bytes_remaining)
2315                         bytes_remaining = count;
2316
2317                 if (h->transport == HL_TRANSPORT_SWIM) {
2318                         retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
2319                         if (retval != ERROR_OK)
2320                                 return retval;
2321                 } else
2322                 /*
2323                  * all stlink support 8/32bit memory read/writes and only from
2324                  * stlink V2J26 there is support for 16 bit memory read/write.
2325                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2326                  * as 8bit access.
2327                  */
2328                 if (size != 1) {
2329
2330                         /* When in jtag mode the stlink uses the auto-increment functionality.
2331                          * However it expects us to pass the data correctly, this includes
2332                          * alignment and any page boundaries. We already do this as part of the
2333                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2334                          * needs implementing manually.
2335                          * currently this only affects jtag mode, according to ST they do single
2336                          * access in SWD mode - but this may change and so we do it for both modes */
2337
2338                         /* we first need to check for any unaligned bytes */
2339                         if (addr & (size - 1)) {
2340
2341                                 uint32_t head_bytes = size - (addr & (size - 1));
2342                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
2343                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2344                                         usleep((1<<retries++) * 1000);
2345                                         continue;
2346                                 }
2347                                 if (retval != ERROR_OK)
2348                                         return retval;
2349                                 buffer += head_bytes;
2350                                 addr += head_bytes;
2351                                 count -= head_bytes;
2352                                 bytes_remaining -= head_bytes;
2353                         }
2354
2355                         if (bytes_remaining & (size - 1))
2356                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
2357                         else if (size == 2)
2358                                 retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
2359                         else
2360                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
2361
2362                 } else
2363                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
2364                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2365                         usleep((1<<retries++) * 1000);
2366                         continue;
2367                 }
2368                 if (retval != ERROR_OK)
2369                         return retval;
2370
2371                 buffer += bytes_remaining;
2372                 addr += bytes_remaining;
2373                 count -= bytes_remaining;
2374         }
2375
2376         return retval;
2377 }
2378
2379 /** */
2380 static int stlink_usb_override_target(const char *targetname)
2381 {
2382         return !strcmp(targetname, "cortex_m");
2383 }
2384
2385 static int stlink_speed_swim(void *handle, int khz, bool query)
2386 {
2387         /*
2388                         we dont care what the khz rate is
2389                         we only have low and high speed...
2390                         before changing speed the SWIM_CSR HS bit
2391                         must be updated
2392          */
2393         if (khz == 0)
2394                 stlink_swim_speed(handle, 0);
2395         else
2396                 stlink_swim_speed(handle, 1);
2397         return khz;
2398 }
2399
2400 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
2401 {
2402         unsigned int i;
2403         int speed_index = -1;
2404         int speed_diff = INT_MAX;
2405         int last_valid_speed = -1;
2406         bool match = true;
2407
2408         for (i = 0; i < map_size; i++) {
2409                 if (!map[i].speed)
2410                         continue;
2411                 last_valid_speed = i;
2412                 if (khz == map[i].speed) {
2413                         speed_index = i;
2414                         break;
2415                 } else {
2416                         int current_diff = khz - map[i].speed;
2417                         /* get abs value for comparison */
2418                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
2419                         if ((current_diff < speed_diff) && khz >= map[i].speed) {
2420                                 speed_diff = current_diff;
2421                                 speed_index = i;
2422                         }
2423                 }
2424         }
2425
2426         if (speed_index == -1) {
2427                 /* this will only be here if we cannot match the slow speed.
2428                  * use the slowest speed we support.*/
2429                 speed_index = last_valid_speed;
2430                 match = false;
2431         } else if (i == map_size)
2432                 match = false;
2433
2434         if (!match && query) {
2435                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
2436                                 khz, map[speed_index].speed);
2437         }
2438
2439         return speed_index;
2440 }
2441
2442 static int stlink_speed_swd(void *handle, int khz, bool query)
2443 {
2444         int speed_index;
2445         struct stlink_usb_handle_s *h = handle;
2446
2447         /* old firmware cannot change it */
2448         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
2449                 return khz;
2450
2451         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
2452                 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
2453
2454         if (!query) {
2455                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
2456                 if (result != ERROR_OK) {
2457                         LOG_ERROR("Unable to set adapter speed");
2458                         return khz;
2459                 }
2460         }
2461
2462         return stlink_khz_to_speed_map_swd[speed_index].speed;
2463 }
2464
2465 static int stlink_speed_jtag(void *handle, int khz, bool query)
2466 {
2467         int speed_index;
2468         struct stlink_usb_handle_s *h = handle;
2469
2470         /* old firmware cannot change it */
2471         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
2472                 return khz;
2473
2474         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
2475                 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
2476
2477         if (!query) {
2478                 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
2479                 if (result != ERROR_OK) {
2480                         LOG_ERROR("Unable to set adapter speed");
2481                         return khz;
2482                 }
2483         }
2484
2485         return stlink_khz_to_speed_map_jtag[speed_index].speed;
2486 }
2487
2488 void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
2489 {
2490         unsigned int i;
2491
2492         LOG_DEBUG("Supported clock speeds are:");
2493         for (i = 0; i < map_size; i++)
2494                 if (map[i].speed)
2495                         LOG_DEBUG("%d kHz", map[i].speed);
2496 }
2497
2498 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
2499 {
2500         struct stlink_usb_handle_s *h = handle;
2501         int i;
2502
2503         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2504                 LOG_ERROR("Unknown command");
2505                 return 0;
2506         }
2507
2508         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2509
2510         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2511         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
2512         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2513
2514         int res = stlink_usb_xfer(handle, h->databuf, 52);
2515
2516         int size = h->databuf[8];
2517
2518         if (size > STLINK_V3_MAX_FREQ_NB)
2519                 size = STLINK_V3_MAX_FREQ_NB;
2520
2521         for (i = 0; i < size; i++) {
2522                 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
2523                 map[i].speed_divisor = i;
2524         }
2525
2526         /* set to zero all the next entries */
2527         for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
2528                 map[i].speed = 0;
2529
2530         return res;
2531 }
2532
2533 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
2534 {
2535         struct stlink_usb_handle_s *h = handle;
2536
2537         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2538                 LOG_ERROR("Unknown command");
2539                 return 0;
2540         }
2541
2542         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2543
2544         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2545         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
2546         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2547         h->cmdbuf[h->cmdidx++] = 0;
2548
2549         h_u32_to_le(&h->cmdbuf[4], frequency);
2550
2551         return stlink_usb_xfer(handle, h->databuf, 8);
2552 }
2553
2554 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
2555 {
2556         struct stlink_usb_handle_s *h = handle;
2557         int speed_index;
2558         struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2559
2560         stlink_get_com_freq(h, is_jtag, map);
2561
2562         speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
2563
2564         if (!query) {
2565                 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
2566                 if (result != ERROR_OK) {
2567                         LOG_ERROR("Unable to set adapter speed");
2568                         return khz;
2569                 }
2570         }
2571         return map[speed_index].speed;
2572 }
2573
2574 static int stlink_speed(void *handle, int khz, bool query)
2575 {
2576         struct stlink_usb_handle_s *h = handle;
2577
2578         if (!handle)
2579                 return khz;
2580
2581         switch (h->transport) {
2582         case HL_TRANSPORT_SWIM:
2583                 return stlink_speed_swim(handle, khz, query);
2584                 break;
2585         case HL_TRANSPORT_SWD:
2586                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2587                         return stlink_speed_v3(handle, false, khz, query);
2588                 else
2589                         return stlink_speed_swd(handle, khz, query);
2590                 break;
2591         case HL_TRANSPORT_JTAG:
2592                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2593                         return stlink_speed_v3(handle, true, khz, query);
2594                 else
2595                         return stlink_speed_jtag(handle, khz, query);
2596                 break;
2597         default:
2598                 break;
2599         }
2600
2601         return khz;
2602 }
2603
2604 /** */
2605 static int stlink_usb_close(void *handle)
2606 {
2607         int res;
2608         uint8_t mode;
2609         enum stlink_mode emode;
2610         struct stlink_usb_handle_s *h = handle;
2611
2612         if (h && h->fd)
2613                 res = stlink_usb_current_mode(handle, &mode);
2614         else
2615                 res = ERROR_FAIL;
2616         /* do not exit if return code != ERROR_OK,
2617            it prevents us from closing jtag_libusb */
2618
2619         if (res == ERROR_OK) {
2620                 /* try to exit current mode */
2621                 switch (mode) {
2622                         case STLINK_DEV_DFU_MODE:
2623                                 emode = STLINK_MODE_DFU;
2624                                 break;
2625                         case STLINK_DEV_DEBUG_MODE:
2626                                 emode = STLINK_MODE_DEBUG_SWD;
2627                                 break;
2628                         case STLINK_DEV_SWIM_MODE:
2629                                 emode = STLINK_MODE_DEBUG_SWIM;
2630                                 break;
2631                         case STLINK_DEV_BOOTLOADER_MODE:
2632                         case STLINK_DEV_MASS_MODE:
2633                         default:
2634                                 emode = STLINK_MODE_UNKNOWN;
2635                                 break;
2636                 }
2637
2638                 if (emode != STLINK_MODE_UNKNOWN)
2639                         stlink_usb_mode_leave(handle, emode);
2640                         /* do not check return code, it prevent
2641                         us from closing jtag_libusb */
2642         }
2643
2644         if (h && h->fd)
2645                 jtag_libusb_close(h->fd);
2646
2647         free(h);
2648
2649         return ERROR_OK;
2650 }
2651
2652 /** */
2653 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
2654 {
2655         int err, retry_count = 1;
2656         struct stlink_usb_handle_s *h;
2657
2658         LOG_DEBUG("stlink_usb_open");
2659
2660         h = calloc(1, sizeof(struct stlink_usb_handle_s));
2661
2662         if (h == 0) {
2663                 LOG_DEBUG("malloc failed");
2664                 return ERROR_FAIL;
2665         }
2666
2667         h->transport = param->transport;
2668
2669         for (unsigned i = 0; param->vid[i]; i++) {
2670                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
2671                           param->transport, param->vid[i], param->pid[i],
2672                           param->serial ? param->serial : "");
2673         }
2674
2675         /*
2676           On certain host USB configurations(e.g. MacBook Air)
2677           STLINKv2 dongle seems to have its FW in a funky state if,
2678           after plugging it in, you try to use openocd with it more
2679           then once (by launching and closing openocd). In cases like
2680           that initial attempt to read the FW info via
2681           stlink_usb_version will fail and the device has to be reset
2682           in order to become operational.
2683          */
2684         do {
2685                 if (jtag_libusb_open(param->vid, param->pid, param->serial, &h->fd) != ERROR_OK) {
2686                         LOG_ERROR("open failed");
2687                         goto error_open;
2688                 }
2689
2690                 jtag_libusb_set_configuration(h->fd, 0);
2691
2692                 if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
2693                         LOG_DEBUG("claim interface failed");
2694                         goto error_open;
2695                 }
2696
2697                 /* RX EP is common for all versions */
2698                 h->rx_ep = STLINK_RX_EP;
2699
2700                 uint16_t pid;
2701                 if (jtag_libusb_get_pid(jtag_libusb_get_device(h->fd), &pid) != ERROR_OK) {
2702                         LOG_DEBUG("libusb_get_pid failed");
2703                         goto error_open;
2704                 }
2705
2706                 /* wrap version for first read */
2707                 switch (pid) {
2708                         case STLINK_V1_PID:
2709                                 h->version.stlink = 1;
2710                                 h->tx_ep = STLINK_TX_EP;
2711                                 break;
2712                         case STLINK_V3_USBLOADER_PID:
2713                         case STLINK_V3E_PID:
2714                         case STLINK_V3S_PID:
2715                         case STLINK_V3_2VCP_PID:
2716                                 h->version.stlink = 3;
2717                                 h->tx_ep = STLINK_V2_1_TX_EP;
2718                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2719                                 break;
2720                         case STLINK_V2_1_PID:
2721                         case STLINK_V2_1_NO_MSD_PID:
2722                                 h->version.stlink = 2;
2723                                 h->tx_ep = STLINK_V2_1_TX_EP;
2724                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2725                                 break;
2726                         default:
2727                         /* fall through - we assume V2 to be the default version*/
2728                         case STLINK_V2_PID:
2729                                 h->version.stlink = 2;
2730                                 h->tx_ep = STLINK_TX_EP;
2731                                 h->trace_ep = STLINK_TRACE_EP;
2732                                 break;
2733                 }
2734
2735                 /* get the device version */
2736                 err = stlink_usb_version(h);
2737
2738                 if (err == ERROR_OK) {
2739                         break;
2740                 } else if (h->version.stlink == 1 ||
2741                            retry_count == 0) {
2742                         LOG_ERROR("read version failed");
2743                         goto error_open;
2744                 } else {
2745                         err = jtag_libusb_release_interface(h->fd, 0);
2746                         if (err != ERROR_OK) {
2747                                 LOG_ERROR("release interface failed");
2748                                 goto error_open;
2749                         }
2750
2751                         err = jtag_libusb_reset_device(h->fd);
2752                         if (err != ERROR_OK) {
2753                                 LOG_ERROR("reset device failed");
2754                                 goto error_open;
2755                         }
2756
2757                         jtag_libusb_close(h->fd);
2758                         /*
2759                           Give the device one second to settle down and
2760                           reenumerate.
2761                          */
2762                         usleep(1 * 1000 * 1000);
2763                         retry_count--;
2764                 }
2765         } while (1);
2766
2767         /* check if mode is supported */
2768         err = ERROR_OK;
2769
2770         switch (h->transport) {
2771                 case HL_TRANSPORT_SWD:
2772                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2773                                 err = ERROR_FAIL;
2774                         /* fall-through */
2775                 case HL_TRANSPORT_JTAG:
2776                         if (h->version.jtag == 0)
2777                                 err = ERROR_FAIL;
2778                         break;
2779                 case HL_TRANSPORT_SWIM:
2780                         if (h->version.swim == 0)
2781                                 err = ERROR_FAIL;
2782                         break;
2783                 default:
2784                         err = ERROR_FAIL;
2785                         break;
2786         }
2787
2788         if (err != ERROR_OK) {
2789                 LOG_ERROR("mode (transport) not supported by device");
2790                 goto error_open;
2791         }
2792
2793         /* initialize the debug hardware */
2794         err = stlink_usb_init_mode(h, param->connect_under_reset);
2795
2796         if (err != ERROR_OK) {
2797                 LOG_ERROR("init mode failed (unable to connect to the target)");
2798                 goto error_open;
2799         }
2800
2801         if (h->transport == HL_TRANSPORT_SWIM) {
2802                 err = stlink_swim_enter(h);
2803                 if (err != ERROR_OK) {
2804                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
2805                         goto error_open;
2806                 }
2807                 *fd = h;
2808                 h->max_mem_packet = STLINK_DATA_SIZE;
2809                 return ERROR_OK;
2810         }
2811
2812         if (h->transport == HL_TRANSPORT_JTAG) {
2813                 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
2814                         stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
2815                         stlink_speed(h, param->initial_interface_speed, false);
2816                 }
2817         } else if (h->transport == HL_TRANSPORT_SWD) {
2818                 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
2819                         stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
2820                         stlink_speed(h, param->initial_interface_speed, false);
2821                 }
2822         }
2823
2824         if (h->version.jtag_api == STLINK_JTAG_API_V3) {
2825                 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2826
2827                 stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map);
2828                 stlink_dump_speed_map(map, ARRAY_SIZE(map));
2829                 stlink_speed(h, param->initial_interface_speed, false);
2830         }
2831
2832         /* get cpuid, so we can determine the max page size
2833          * start with a safe default */
2834         h->max_mem_packet = (1 << 10);
2835
2836         uint8_t buffer[4];
2837         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
2838         if (err == ERROR_OK) {
2839                 uint32_t cpuid = le_to_h_u32(buffer);
2840                 int i = (cpuid >> 4) & 0xf;
2841                 if (i == 4 || i == 3) {
2842                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
2843                         h->max_mem_packet = (1 << 12);
2844                 }
2845         }
2846
2847         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
2848
2849         *fd = h;
2850
2851         return ERROR_OK;
2852
2853 error_open:
2854         stlink_usb_close(h);
2855
2856         return ERROR_FAIL;
2857 }
2858
2859 int stlink_config_trace(void *handle, bool enabled, enum tpiu_pin_protocol pin_protocol,
2860                         uint32_t port_size, unsigned int *trace_freq)
2861 {
2862         struct stlink_usb_handle_s *h = handle;
2863
2864         if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
2865                         pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
2866                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
2867                 return ERROR_FAIL;
2868         }
2869
2870         if (!enabled) {
2871                 stlink_usb_trace_disable(h);
2872                 return ERROR_OK;
2873         }
2874
2875         if (*trace_freq > STLINK_TRACE_MAX_HZ) {
2876                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
2877                           STLINK_TRACE_MAX_HZ);
2878                 return ERROR_FAIL;
2879         }
2880
2881         stlink_usb_trace_disable(h);
2882
2883         if (!*trace_freq)
2884                 *trace_freq = STLINK_TRACE_MAX_HZ;
2885         h->trace.source_hz = *trace_freq;
2886
2887         return stlink_usb_trace_enable(h);
2888 }
2889
2890 /** */
2891 struct hl_layout_api_s stlink_usb_layout_api = {
2892         /** */
2893         .open = stlink_usb_open,
2894         /** */
2895         .close = stlink_usb_close,
2896         /** */
2897         .idcode = stlink_usb_idcode,
2898         /** */
2899         .state = stlink_usb_state,
2900         /** */
2901         .reset = stlink_usb_reset,
2902         /** */
2903         .assert_srst = stlink_usb_assert_srst,
2904         /** */
2905         .run = stlink_usb_run,
2906         /** */
2907         .halt = stlink_usb_halt,
2908         /** */
2909         .step = stlink_usb_step,
2910         /** */
2911         .read_regs = stlink_usb_read_regs,
2912         /** */
2913         .read_reg = stlink_usb_read_reg,
2914         /** */
2915         .write_reg = stlink_usb_write_reg,
2916         /** */
2917         .read_mem = stlink_usb_read_mem,
2918         /** */
2919         .write_mem = stlink_usb_write_mem,
2920         /** */
2921         .write_debug_reg = stlink_usb_write_debug_reg,
2922         /** */
2923         .override_target = stlink_usb_override_target,
2924         /** */
2925         .speed = stlink_speed,
2926         /** */
2927         .config_trace = stlink_config_trace,
2928         /** */
2929         .poll_trace = stlink_usb_trace_read,
2930 };