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