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 <helper/bits.h>
  35 #include <jtag/interface.h>
  36 #include <jtag/hla/hla_layout.h>
  37 #include <jtag/hla/hla_transport.h>
  38 #include <jtag/hla/hla_interface.h>
  39 #include <target/target.h>
  40 #include <transport/transport.h>
  41
  42 #include <target/cortex_m.h>
  43
  44 #include "libusb_helper.h"
  45
  46 #ifdef HAVE_LIBUSB1
  47 #define USE_LIBUSB_ASYNCIO
  48 #endif
  49
  50 #define STLINK_SERIAL_LEN 24
  51
  52 #define ENDPOINT_IN  0x80
  53 #define ENDPOINT_OUT 0x00
  54
  55 #define STLINK_WRITE_TIMEOUT 1000
  56 #define STLINK_READ_TIMEOUT 1000
  57
  58 #define STLINK_RX_EP          (1|ENDPOINT_IN)
  59 #define STLINK_TX_EP          (2|ENDPOINT_OUT)
  60 #define STLINK_TRACE_EP       (3|ENDPOINT_IN)
  61
  62 #define STLINK_V2_1_TX_EP     (1|ENDPOINT_OUT)
  63 #define STLINK_V2_1_TRACE_EP  (2|ENDPOINT_IN)
  64
  65 #define STLINK_SG_SIZE        (31)
  66 #define STLINK_DATA_SIZE      (4096)
  67 #define STLINK_CMD_SIZE_V2    (16)
  68 #define STLINK_CMD_SIZE_V1    (10)
  69
  70 #define STLINK_V1_PID         (0x3744)
  71 #define STLINK_V2_PID         (0x3748)
  72 #define STLINK_V2_1_PID       (0x374B)
  73 #define STLINK_V2_1_NO_MSD_PID  (0x3752)
  74 #define STLINK_V3_USBLOADER_PID (0x374D)
  75 #define STLINK_V3E_PID          (0x374E)
  76 #define STLINK_V3S_PID          (0x374F)
  77 #define STLINK_V3_2VCP_PID      (0x3753)
  78
  79 /*
  80  * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
  81  * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
  82  * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes from FW V3J6.
  83  */
  84 #define STLINK_MAX_RW8          (64)
  85 #define STLINKV3_MAX_RW8        (512)
  86
  87 /* "WAIT" responses will be retried (with exponential backoff) at
  88  * most this many times before failing to caller.
  89  */
  90 #define MAX_WAIT_RETRIES 8
  91
  92 enum stlink_jtag_api_version {
  93         STLINK_JTAG_API_V1 = 1,
  94         STLINK_JTAG_API_V2,
  95         STLINK_JTAG_API_V3,
  96 };
  97
  98 /** */
  99 struct stlink_usb_version {
 100         /** */
 101         int stlink;
 102         /** */
 103         int jtag;
 104         /** */
 105         int swim;
 106         /** jtag api version supported */
 107         enum stlink_jtag_api_version jtag_api;
 108         /** one bit for each feature supported. See macros STLINK_F_* */
 109         uint32_t flags;
 110 };
 111
 112 /** */
 113 struct stlink_usb_handle_s {
 114         /** */
 115         struct libusb_device_handle *fd;
 116         /** */
 117         struct libusb_transfer *trans;
 118         /** */
 119         uint8_t rx_ep;
 120         /** */
 121         uint8_t tx_ep;
 122         /** */
 123         uint8_t trace_ep;
 124         /** */
 125         uint8_t cmdbuf[STLINK_SG_SIZE];
 126         /** */
 127         uint8_t cmdidx;
 128         /** */
 129         uint8_t direction;
 130         /** */
 131         uint8_t databuf[STLINK_DATA_SIZE];
 132         /** */
 133         uint32_t max_mem_packet;
 134         /** */
 135         enum hl_transports transport;
 136         /** */
 137         struct stlink_usb_version version;
 138         /** */
 139         uint16_t vid;
 140         /** */
 141         uint16_t pid;
 142         /** */
 143         struct {
 144                 /** whether SWO tracing is enabled or not */
 145                 bool enabled;
 146                 /** trace module source clock */
 147                 uint32_t source_hz;
 148         } trace;
 149         /** reconnect is needed next time we try to query the
 150          * status */
 151         bool reconnect_pending;
 152 };
 153
 154 #define STLINK_SWIM_ERR_OK             0x00
 155 #define STLINK_SWIM_BUSY               0x01
 156 #define STLINK_DEBUG_ERR_OK            0x80
 157 #define STLINK_DEBUG_ERR_FAULT         0x81
 158 #define STLINK_SWD_AP_WAIT             0x10
 159 #define STLINK_SWD_AP_FAULT            0x11
 160 #define STLINK_SWD_AP_ERROR            0x12
 161 #define STLINK_SWD_AP_PARITY_ERROR     0x13
 162 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09
 163 #define STLINK_JTAG_WRITE_ERROR        0x0c
 164 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 165 #define STLINK_SWD_DP_WAIT             0x14
 166 #define STLINK_SWD_DP_FAULT            0x15
 167 #define STLINK_SWD_DP_ERROR            0x16
 168 #define STLINK_SWD_DP_PARITY_ERROR     0x17
 169
 170 #define STLINK_SWD_AP_WDATA_ERROR      0x18
 171 #define STLINK_SWD_AP_STICKY_ERROR     0x19
 172 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
 173
 174 #define STLINK_BAD_AP_ERROR            0x1d
 175
 176 #define STLINK_CORE_RUNNING            0x80
 177 #define STLINK_CORE_HALTED             0x81
 178 #define STLINK_CORE_STAT_UNKNOWN       -1
 179
 180 #define STLINK_GET_VERSION             0xF1
 181 #define STLINK_DEBUG_COMMAND           0xF2
 182 #define STLINK_DFU_COMMAND             0xF3
 183 #define STLINK_SWIM_COMMAND            0xF4
 184 #define STLINK_GET_CURRENT_MODE        0xF5
 185 #define STLINK_GET_TARGET_VOLTAGE      0xF7
 186
 187 #define STLINK_DEV_DFU_MODE            0x00
 188 #define STLINK_DEV_MASS_MODE           0x01
 189 #define STLINK_DEV_DEBUG_MODE          0x02
 190 #define STLINK_DEV_SWIM_MODE           0x03
 191 #define STLINK_DEV_BOOTLOADER_MODE     0x04
 192 #define STLINK_DEV_UNKNOWN_MODE        -1
 193
 194 #define STLINK_DFU_EXIT                0x07
 195
 196 /*
 197         STLINK_SWIM_ENTER_SEQ
 198         1.3ms low then 750Hz then 1.5kHz
 199
 200         STLINK_SWIM_GEN_RST
 201         STM8 DM pulls reset pin low 50us
 202
 203         STLINK_SWIM_SPEED
 204         uint8_t (0=low|1=high)
 205
 206         STLINK_SWIM_WRITEMEM
 207         uint16_t length
 208         uint32_t address
 209
 210         STLINK_SWIM_RESET
 211         send syncronization seq (16us low, response 64 clocks low)
 212 */
 213 #define STLINK_SWIM_ENTER                  0x00
 214 #define STLINK_SWIM_EXIT                   0x01
 215 #define STLINK_SWIM_READ_CAP               0x02
 216 #define STLINK_SWIM_SPEED                  0x03
 217 #define STLINK_SWIM_ENTER_SEQ              0x04
 218 #define STLINK_SWIM_GEN_RST                0x05
 219 #define STLINK_SWIM_RESET                  0x06
 220 #define STLINK_SWIM_ASSERT_RESET           0x07
 221 #define STLINK_SWIM_DEASSERT_RESET         0x08
 222 #define STLINK_SWIM_READSTATUS             0x09
 223 #define STLINK_SWIM_WRITEMEM               0x0a
 224 #define STLINK_SWIM_READMEM                0x0b
 225 #define STLINK_SWIM_READBUF                0x0c
 226
 227 #define STLINK_DEBUG_GETSTATUS             0x01
 228 #define STLINK_DEBUG_FORCEDEBUG            0x02
 229 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
 230 #define STLINK_DEBUG_APIV1_READALLREGS     0x04
 231 #define STLINK_DEBUG_APIV1_READREG         0x05
 232 #define STLINK_DEBUG_APIV1_WRITEREG        0x06
 233 #define STLINK_DEBUG_READMEM_32BIT         0x07
 234 #define STLINK_DEBUG_WRITEMEM_32BIT        0x08
 235 #define STLINK_DEBUG_RUNCORE               0x09
 236 #define STLINK_DEBUG_STEPCORE              0x0a
 237 #define STLINK_DEBUG_APIV1_SETFP           0x0b
 238 #define STLINK_DEBUG_READMEM_8BIT          0x0c
 239 #define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
 240 #define STLINK_DEBUG_APIV1_CLEARFP         0x0e
 241 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 242 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 243
 244 #define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
 245 #define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
 246 #define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
 247
 248 #define STLINK_DEBUG_APIV1_ENTER           0x20
 249 #define STLINK_DEBUG_EXIT                  0x21
 250 #define STLINK_DEBUG_READCOREID            0x22
 251
 252 #define STLINK_DEBUG_APIV2_ENTER           0x30
 253 #define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
 254 #define STLINK_DEBUG_APIV2_RESETSYS        0x32
 255 #define STLINK_DEBUG_APIV2_READREG         0x33
 256 #define STLINK_DEBUG_APIV2_WRITEREG        0x34
 257 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
 258 #define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
 259
 260 #define STLINK_DEBUG_APIV2_READALLREGS     0x3A
 261 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 262 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 263
 264 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
 265
 266 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 267 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 268 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 269 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
 270 #define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
 271 #define STLINK_DEBUG_APIV2_READ_DAP_REG    0x45
 272 #define STLINK_DEBUG_APIV2_WRITE_DAP_REG   0x46
 273 #define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
 274 #define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
 275
 276 #define STLINK_DEBUG_APIV2_INIT_AP         0x4B
 277 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
 278
 279 #define STLINK_APIV3_SET_COM_FREQ           0x61
 280 #define STLINK_APIV3_GET_COM_FREQ           0x62
 281
 282 #define STLINK_APIV3_GET_VERSION_EX         0xFB
 283
 284 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 285 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 286 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 287
 288 #define STLINK_DEBUG_PORT_ACCESS            0xffff
 289
 290 #define STLINK_TRACE_SIZE               4096
 291 #define STLINK_TRACE_MAX_HZ             2000000
 292
 293 #define STLINK_V3_MAX_FREQ_NB               10
 294
 295 /** */
 296 enum stlink_mode {
 297         STLINK_MODE_UNKNOWN = 0,
 298         STLINK_MODE_DFU,
 299         STLINK_MODE_MASS,
 300         STLINK_MODE_DEBUG_JTAG,
 301         STLINK_MODE_DEBUG_SWD,
 302         STLINK_MODE_DEBUG_SWIM
 303 };
 304
 305 #define REQUEST_SENSE        0x03
 306 #define REQUEST_SENSE_LENGTH 18
 307
 308 /*
 309  * Map the relevant features, quirks and workaround for specific firmware
 310  * version of stlink
 311  */
 312 #define STLINK_F_HAS_TRACE              BIT(0)
 313 #define STLINK_F_HAS_SWD_SET_FREQ       BIT(1)
 314 #define STLINK_F_HAS_JTAG_SET_FREQ      BIT(2)
 315 #define STLINK_F_HAS_MEM_16BIT          BIT(3)
 316 #define STLINK_F_HAS_GETLASTRWSTATUS2   BIT(4)
 317 #define STLINK_F_HAS_DAP_REG            BIT(5)
 318 #define STLINK_F_QUIRK_JTAG_DP_READ     BIT(6)
 319 #define STLINK_F_HAS_AP_INIT            BIT(7)
 320 #define STLINK_F_HAS_DPBANKSEL          BIT(8)
 321 #define STLINK_F_HAS_RW8_512BYTES       BIT(9)
 322
 323 /* aliases */
 324 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
 325
 326 struct speed_map {
 327         int speed;
 328         int speed_divisor;
 329 };
 330
 331 /* SWD clock speed */
 332 static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 333         {4000, 0},
 334         {1800, 1}, /* default */
 335         {1200, 2},
 336         {950,  3},
 337         {480,  7},
 338         {240, 15},
 339         {125, 31},
 340         {100, 40},
 341         {50,  79},
 342         {25, 158},
 343         {15, 265},
 344         {5,  798}
 345 };
 346
 347 /* JTAG clock speed */
 348 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
 349         {9000,  4},
 350         {4500,  8},
 351         {2250, 16},
 352         {1125, 32}, /* default */
 353         {562,  64},
 354         {281, 128},
 355         {140, 256}
 356 };
 357
 358 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 359 static int stlink_swim_status(void *handle);
 360 void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size);
 361 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map);
 362 static int stlink_speed(void *handle, int khz, bool query);
 363
 364 /** */
 365 static unsigned int stlink_usb_block(void *handle)
 366 {
 367         struct stlink_usb_handle_s *h = handle;
 368
 369         assert(handle != NULL);
 370
 371         if (h->version.flags & STLINK_F_HAS_RW8_512BYTES)
 372                 return STLINKV3_MAX_RW8;
 373         else
 374                 return STLINK_MAX_RW8;
 375 }
 376
 377
 378
 379 #ifdef USE_LIBUSB_ASYNCIO
 380
 381 static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
 382 {
 383         int *completed = transfer->user_data;
 384         *completed = 1;
 385         /* caller interprets result and frees transfer */
 386 }
 387
 388
 389 static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
 390 {
 391         int r, *completed = transfer->user_data;
 392
 393         /* Assuming a single libusb context exists.  There no existing interface into this
 394          * module to pass a libusb context.
 395          */
 396         struct libusb_context *ctx = NULL;
 397
 398         while (!*completed) {
 399                 r = libusb_handle_events_completed(ctx, completed);
 400                 if (r < 0) {
 401                         if (r == LIBUSB_ERROR_INTERRUPTED)
 402                                 continue;
 403                         libusb_cancel_transfer(transfer);
 404                         continue;
 405                 }
 406         }
 407 }
 408
 409
 410 static int transfer_error_status(const struct libusb_transfer *transfer)
 411 {
 412         int r = 0;
 413
 414         switch (transfer->status) {
 415                 case LIBUSB_TRANSFER_COMPLETED:
 416                         r = 0;
 417                         break;
 418                 case LIBUSB_TRANSFER_TIMED_OUT:
 419                         r = LIBUSB_ERROR_TIMEOUT;
 420                         break;
 421                 case LIBUSB_TRANSFER_STALL:
 422                         r = LIBUSB_ERROR_PIPE;
 423                         break;
 424                 case LIBUSB_TRANSFER_OVERFLOW:
 425                         r = LIBUSB_ERROR_OVERFLOW;
 426                         break;
 427                 case LIBUSB_TRANSFER_NO_DEVICE:
 428                         r = LIBUSB_ERROR_NO_DEVICE;
 429                         break;
 430                 case LIBUSB_TRANSFER_ERROR:
 431                 case LIBUSB_TRANSFER_CANCELLED:
 432                         r = LIBUSB_ERROR_IO;
 433                         break;
 434                 default:
 435                         r = LIBUSB_ERROR_OTHER;
 436                         break;
 437         }
 438
 439         return r;
 440 }
 441
 442 struct jtag_xfer {
 443         int ep;
 444         uint8_t *buf;
 445         size_t size;
 446         /* Internal */
 447         int retval;
 448         int completed;
 449         size_t transfer_size;
 450         struct libusb_transfer *transfer;
 451 };
 452
 453 static int jtag_libusb_bulk_transfer_n(
 454                 struct libusb_device_handle *dev_handle,
 455                 struct jtag_xfer *transfers,
 456                 size_t n_transfers,
 457                 int timeout)
 458 {
 459         int retval = 0;
 460         int returnval = ERROR_OK;
 461
 462
 463         for (size_t i = 0; i < n_transfers; ++i) {
 464                 transfers[i].retval = 0;
 465                 transfers[i].completed = 0;
 466                 transfers[i].transfer_size = 0;
 467                 transfers[i].transfer = libusb_alloc_transfer(0);
 468
 469                 if (transfers[i].transfer == NULL) {
 470                         for (size_t j = 0; j < i; ++j)
 471                                 libusb_free_transfer(transfers[j].transfer);
 472
 473                         LOG_DEBUG("ERROR, failed to alloc usb transfers");
 474                         for (size_t k = 0; k < n_transfers; ++k)
 475                                 transfers[k].retval = LIBUSB_ERROR_NO_MEM;
 476                         return ERROR_FAIL;
 477                 }
 478         }
 479
 480         for (size_t i = 0; i < n_transfers; ++i) {
 481                 libusb_fill_bulk_transfer(
 482                                 transfers[i].transfer,
 483                                 dev_handle,
 484                                 transfers[i].ep, transfers[i].buf, transfers[i].size,
 485                                 sync_transfer_cb, &transfers[i].completed, timeout);
 486                 transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
 487
 488                 retval = libusb_submit_transfer(transfers[i].transfer);
 489                 if (retval < 0) {
 490                         LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
 491
 492                         /* Probably no point continuing to submit transfers once a submission fails.
 493                          * As a result, tag all remaining transfers as errors.
 494                          */
 495                         for (size_t j = i; j < n_transfers; ++j)
 496                                 transfers[j].retval = retval;
 497
 498                         returnval = ERROR_FAIL;
 499                         break;
 500                 }
 501         }
 502
 503         /* Wait for every submitted USB transfer to complete.
 504         */
 505         for (size_t i = 0; i < n_transfers; ++i) {
 506                 if (transfers[i].retval == 0) {
 507                         sync_transfer_wait_for_completion(transfers[i].transfer);
 508
 509                         retval = transfer_error_status(transfers[i].transfer);
 510                         if (retval) {
 511                                 returnval = ERROR_FAIL;
 512                                 transfers[i].retval = retval;
 513                                 LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
 514                         } else {
 515                                 /* Assuming actual_length is only valid if there is no transfer error.
 516                                  */
 517                                 transfers[i].transfer_size = transfers[i].transfer->actual_length;
 518                         }
 519                 }
 520
 521                 libusb_free_transfer(transfers[i].transfer);
 522                 transfers[i].transfer = NULL;
 523         }
 524
 525         return returnval;
 526 }
 527
 528 #endif
 529
 530
 531 /** */
 532 static int stlink_usb_xfer_v1_get_status(void *handle)
 533 {
 534         struct stlink_usb_handle_s *h = handle;
 535         int tr, ret;
 536
 537         assert(handle != NULL);
 538
 539         /* read status */
 540         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 541
 542         ret = jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)h->cmdbuf, 13,
 543                                     STLINK_READ_TIMEOUT, &tr);
 544         if (ret || tr != 13)
 545                 return ERROR_FAIL;
 546
 547         uint32_t t1;
 548
 549         t1 = buf_get_u32(h->cmdbuf, 0, 32);
 550
 551         /* check for USBS */
 552         if (t1 != 0x53425355)
 553                 return ERROR_FAIL;
 554         /*
 555          * CSW status:
 556          * 0 success
 557          * 1 command failure
 558          * 2 phase error
 559          */
 560         if (h->cmdbuf[12] != 0)
 561                 return ERROR_FAIL;
 562
 563         return ERROR_OK;
 564 }
 565
 566 #ifdef USE_LIBUSB_ASYNCIO
 567 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 568 {
 569         struct stlink_usb_handle_s *h = handle;
 570
 571         assert(handle != NULL);
 572
 573         size_t n_transfers = 0;
 574         struct jtag_xfer transfers[2];
 575
 576         memset(transfers, 0, sizeof(transfers));
 577
 578         transfers[0].ep = h->tx_ep;
 579         transfers[0].buf = h->cmdbuf;
 580         transfers[0].size = cmdsize;
 581
 582         ++n_transfers;
 583
 584         if (h->direction == h->tx_ep && size) {
 585                 transfers[1].ep = h->tx_ep;
 586                 transfers[1].buf = (uint8_t *)buf;
 587                 transfers[1].size = size;
 588
 589                 ++n_transfers;
 590         } else if (h->direction == h->rx_ep && size) {
 591                 transfers[1].ep = h->rx_ep;
 592                 transfers[1].buf = (uint8_t *)buf;
 593                 transfers[1].size = size;
 594
 595                 ++n_transfers;
 596         }
 597
 598         return jtag_libusb_bulk_transfer_n(
 599                         h->fd,
 600                         transfers,
 601                         n_transfers,
 602                         STLINK_WRITE_TIMEOUT);
 603 }
 604 #else
 605 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 606 {
 607         struct stlink_usb_handle_s *h = handle;
 608         int tr, ret;
 609
 610         assert(handle != NULL);
 611
 612         ret = jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)h->cmdbuf,
 613                                      cmdsize, STLINK_WRITE_TIMEOUT, &tr);
 614         if (ret || tr != cmdsize)
 615                 return ERROR_FAIL;
 616
 617         if (h->direction == h->tx_ep && size) {
 618                 ret = jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)buf,
 619                                              size, STLINK_WRITE_TIMEOUT, &tr);
 620                 if (ret || tr != size) {
 621                         LOG_DEBUG("bulk write failed");
 622                         return ERROR_FAIL;
 623                 }
 624         } else if (h->direction == h->rx_ep && size) {
 625                 ret = jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)buf,
 626                                             size, STLINK_READ_TIMEOUT, &tr);
 627                 if (ret || tr != size) {
 628                         LOG_DEBUG("bulk read failed");
 629                         return ERROR_FAIL;
 630                 }
 631         }
 632
 633         return ERROR_OK;
 634 }
 635 #endif
 636
 637 /** */
 638 static int stlink_usb_xfer_v1_get_sense(void *handle)
 639 {
 640         int res;
 641         struct stlink_usb_handle_s *h = handle;
 642
 643         assert(handle != NULL);
 644
 645         stlink_usb_init_buffer(handle, h->rx_ep, 16);
 646
 647         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
 648         h->cmdbuf[h->cmdidx++] = 0;
 649         h->cmdbuf[h->cmdidx++] = 0;
 650         h->cmdbuf[h->cmdidx++] = 0;
 651         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
 652
 653         res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
 654
 655         if (res != ERROR_OK)
 656                 return res;
 657
 658         if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
 659                 return ERROR_FAIL;
 660
 661         return ERROR_OK;
 662 }
 663
 664 /*
 665         transfers block in cmdbuf
 666         <size> indicates number of bytes in the following
 667         data phase.
 668         Ignore the (eventual) error code in the received packet.
 669 */
 670 static int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
 671 {
 672         int err, cmdsize = STLINK_CMD_SIZE_V2;
 673         struct stlink_usb_handle_s *h = handle;
 674
 675         assert(handle != NULL);
 676
 677         if (h->version.stlink == 1) {
 678                 cmdsize = STLINK_SG_SIZE;
 679                 /* put length in bCBWCBLength */
 680                 h->cmdbuf[14] = h->cmdidx-15;
 681         }
 682
 683         err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 684
 685         if (err != ERROR_OK)
 686                 return err;
 687
 688         if (h->version.stlink == 1) {
 689                 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
 690                         /* check csw status */
 691                         if (h->cmdbuf[12] == 1) {
 692                                 LOG_DEBUG("get sense");
 693                                 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
 694                                         return ERROR_FAIL;
 695                         }
 696                         return ERROR_FAIL;
 697                 }
 698         }
 699
 700         return ERROR_OK;
 701 }
 702
 703 /**
 704     Converts an STLINK status code held in the first byte of a response
 705     to an openocd error, logs any error/wait status as debug output.
 706 */
 707 static int stlink_usb_error_check(void *handle)
 708 {
 709         struct stlink_usb_handle_s *h = handle;
 710
 711         assert(handle != NULL);
 712
 713         if (h->transport == HL_TRANSPORT_SWIM) {
 714                 switch (h->databuf[0]) {
 715                         case STLINK_SWIM_ERR_OK:
 716                                 return ERROR_OK;
 717                         case STLINK_SWIM_BUSY:
 718                                 return ERROR_WAIT;
 719                         default:
 720                                 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 721                                 return ERROR_FAIL;
 722                 }
 723         }
 724
 725         /* TODO: no error checking yet on api V1 */
 726         if (h->version.jtag_api == STLINK_JTAG_API_V1)
 727                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
 728
 729         switch (h->databuf[0]) {
 730                 case STLINK_DEBUG_ERR_OK:
 731                         return ERROR_OK;
 732                 case STLINK_DEBUG_ERR_FAULT:
 733                         LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
 734                         return ERROR_FAIL;
 735                 case STLINK_SWD_AP_WAIT:
 736                         LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
 737                         return ERROR_WAIT;
 738                 case STLINK_SWD_DP_WAIT:
 739                         LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
 740                         return ERROR_WAIT;
 741                 case STLINK_JTAG_GET_IDCODE_ERROR:
 742                         LOG_DEBUG("STLINK_JTAG_GET_IDCODE_ERROR");
 743                         return ERROR_FAIL;
 744                 case STLINK_JTAG_WRITE_ERROR:
 745                         LOG_DEBUG("Write error");
 746                         return ERROR_FAIL;
 747                 case STLINK_JTAG_WRITE_VERIF_ERROR:
 748                         LOG_DEBUG("Write verify error, ignoring");
 749                         return ERROR_OK;
 750                 case STLINK_SWD_AP_FAULT:
 751                         /* git://git.ac6.fr/openocd commit 657e3e885b9ee10
 752                          * returns ERROR_OK with the comment:
 753                          * Change in error status when reading outside RAM.
 754                          * This fix allows CDT plugin to visualize memory.
 755                          */
 756                         LOG_DEBUG("STLINK_SWD_AP_FAULT");
 757                         return ERROR_FAIL;
 758                 case STLINK_SWD_AP_ERROR:
 759                         LOG_DEBUG("STLINK_SWD_AP_ERROR");
 760                         return ERROR_FAIL;
 761                 case STLINK_SWD_AP_PARITY_ERROR:
 762                         LOG_DEBUG("STLINK_SWD_AP_PARITY_ERROR");
 763                         return ERROR_FAIL;
 764                 case STLINK_SWD_DP_FAULT:
 765                         LOG_DEBUG("STLINK_SWD_DP_FAULT");
 766                         return ERROR_FAIL;
 767                 case STLINK_SWD_DP_ERROR:
 768                         LOG_DEBUG("STLINK_SWD_DP_ERROR");
 769                         return ERROR_FAIL;
 770                 case STLINK_SWD_DP_PARITY_ERROR:
 771                         LOG_DEBUG("STLINK_SWD_DP_PARITY_ERROR");
 772                         return ERROR_FAIL;
 773                 case STLINK_SWD_AP_WDATA_ERROR:
 774                         LOG_DEBUG("STLINK_SWD_AP_WDATA_ERROR");
 775                         return ERROR_FAIL;
 776                 case STLINK_SWD_AP_STICKY_ERROR:
 777                         LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
 778                         return ERROR_FAIL;
 779                 case STLINK_SWD_AP_STICKYORUN_ERROR:
 780                         LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
 781                         return ERROR_FAIL;
 782                 case STLINK_BAD_AP_ERROR:
 783                         LOG_DEBUG("STLINK_BAD_AP_ERROR");
 784                         return ERROR_FAIL;
 785                 default:
 786                         LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 787                         return ERROR_FAIL;
 788         }
 789 }
 790
 791 /*
 792  * Wrapper around stlink_usb_xfer_noerrcheck()
 793  * to check the error code in the received packet
 794  */
 795 static int stlink_usb_xfer_errcheck(void *handle, const uint8_t *buf, int size)
 796 {
 797         int retval;
 798
 799         assert(size > 0);
 800
 801         retval = stlink_usb_xfer_noerrcheck(handle, buf, size);
 802         if (retval != ERROR_OK)
 803                 return retval;
 804
 805         return stlink_usb_error_check(handle);
 806 }
 807
 808 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
 809
 810     Works for commands where the STLINK_DEBUG status is returned in the first
 811     byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
 812
 813     Returns an openocd result code.
 814 */
 815 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 816 {
 817         int retries = 0;
 818         int res;
 819         struct stlink_usb_handle_s *h = handle;
 820
 821         while (1) {
 822                 if ((h->transport != HL_TRANSPORT_SWIM) || !retries) {
 823                         res = stlink_usb_xfer_noerrcheck(handle, buf, size);
 824                         if (res != ERROR_OK)
 825                                 return res;
 826                 }
 827
 828                 if (h->transport == HL_TRANSPORT_SWIM) {
 829                         res = stlink_swim_status(handle);
 830                         if (res != ERROR_OK)
 831                                 return res;
 832                 }
 833
 834                 res = stlink_usb_error_check(handle);
 835                 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 836                         unsigned int delay_us = (1<<retries++) * 1000;
 837                         LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
 838                         usleep(delay_us);
 839                         continue;
 840                 }
 841                 return res;
 842         }
 843 }
 844
 845 /** */
 846 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 847 {
 848         struct stlink_usb_handle_s *h = handle;
 849         int tr, ret;
 850
 851         assert(handle != NULL);
 852
 853         assert(h->version.flags & STLINK_F_HAS_TRACE);
 854
 855         ret = jtag_libusb_bulk_read(h->fd, h->trace_ep, (char *)buf, size,
 856                                     STLINK_READ_TIMEOUT, &tr);
 857         if (ret || tr != size) {
 858                 LOG_ERROR("bulk trace read failed");
 859                 return ERROR_FAIL;
 860         }
 861
 862         return ERROR_OK;
 863 }
 864
 865 /*
 866         this function writes transfer length in
 867         the right place in the cb
 868 */
 869 static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
 870 {
 871         struct stlink_usb_handle_s *h = handle;
 872
 873         buf_set_u32(h->cmdbuf+8, 0, 32, size);
 874 }
 875
 876 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 877 {
 878         struct stlink_usb_handle_s *h = handle;
 879
 880         /* fill the send buffer */
 881         strcpy((char *)h->cmdbuf, "USBC");
 882         h->cmdidx += 4;
 883         /* csw tag not used */
 884         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
 885         h->cmdidx += 4;
 886         /* cbw data transfer length (in the following data phase in or out) */
 887         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 888         h->cmdidx += 4;
 889         /* cbw flags */
 890         h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
 891         h->cmdbuf[h->cmdidx++] = 0; /* lun */
 892         /* cdb clength (is filled in at xfer) */
 893         h->cmdbuf[h->cmdidx++] = 0;
 894 }
 895
 896 /** */
 897 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
 898 {
 899         struct stlink_usb_handle_s *h = handle;
 900
 901         h->direction = direction;
 902
 903         h->cmdidx = 0;
 904
 905         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 906         memset(h->databuf, 0, STLINK_DATA_SIZE);
 907
 908         if (h->version.stlink == 1)
 909                 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
 910 }
 911
 912 /** */
 913 static int stlink_usb_version(void *handle)
 914 {
 915         int res;
 916         uint32_t flags;
 917         uint16_t version;
 918         uint8_t v, x, y, jtag, swim, msd, bridge = 0;
 919         char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
 920         char *p;
 921         struct stlink_usb_handle_s *h = handle;
 922
 923         assert(handle != NULL);
 924
 925         stlink_usb_init_buffer(handle, h->rx_ep, 6);
 926
 927         h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
 928
 929         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 6);
 930
 931         if (res != ERROR_OK)
 932                 return res;
 933
 934         version = be_to_h_u16(h->databuf);
 935         v = (version >> 12) & 0x0f;
 936         x = (version >> 6) & 0x3f;
 937         y = version & 0x3f;
 938
 939         h->vid = le_to_h_u16(h->databuf + 2);
 940         h->pid = le_to_h_u16(h->databuf + 4);
 941
 942         switch (h->pid) {
 943         case STLINK_V2_1_PID:
 944         case STLINK_V2_1_NO_MSD_PID:
 945                 if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
 946                         /* MxSy : STM8 V2.1 - SWIM only */
 947                         msd = x;
 948                         swim = y;
 949                         jtag = 0;
 950                 } else {
 951                         /* JxMy : STM32 V2.1 - JTAG/SWD only */
 952                         jtag = x;
 953                         msd = y;
 954                         swim = 0;
 955                 }
 956                 break;
 957         default:
 958                 jtag = x;
 959                 swim = y;
 960                 msd = 0;
 961                 break;
 962         }
 963
 964         /* STLINK-V3 requires a specific command */
 965         if (v == 3 && x == 0 && y == 0) {
 966                 stlink_usb_init_buffer(handle, h->rx_ep, 16);
 967
 968                 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
 969
 970                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 12);
 971                 if (res != ERROR_OK)
 972                         return res;
 973
 974                 v = h->databuf[0];
 975                 swim = h->databuf[1];
 976                 jtag = h->databuf[2];
 977                 msd  = h->databuf[3];
 978                 bridge = h->databuf[4];
 979                 h->vid = le_to_h_u16(h->databuf + 8);
 980                 h->pid = le_to_h_u16(h->databuf + 10);
 981         }
 982
 983         h->version.stlink = v;
 984         h->version.jtag = jtag;
 985         h->version.swim = swim;
 986
 987         flags = 0;
 988         switch (h->version.stlink) {
 989         case 1:
 990                 /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
 991                 if (h->version.jtag >= 11)
 992                         h->version.jtag_api = STLINK_JTAG_API_V2;
 993                 else
 994                         h->version.jtag_api = STLINK_JTAG_API_V1;
 995
 996                 break;
 997         case 2:
 998                 /* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
 999                 h->version.jtag_api = STLINK_JTAG_API_V2;
1000
1001                 /* API for trace from J13 */
1002                 /* API for target voltage from J13 */
1003                 if (h->version.jtag >= 13)
1004                         flags |= STLINK_F_HAS_TRACE;
1005
1006                 /* preferred API to get last R/W status from J15 */
1007                 if (h->version.jtag >= 15)
1008                         flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1009
1010                 /* API to set SWD frequency from J22 */
1011                 if (h->version.jtag >= 22)
1012                         flags |= STLINK_F_HAS_SWD_SET_FREQ;
1013
1014                 /* API to set JTAG frequency from J24 */
1015                 /* API to access DAP registers from J24 */
1016                 if (h->version.jtag >= 24) {
1017                         flags |= STLINK_F_HAS_JTAG_SET_FREQ;
1018                         flags |= STLINK_F_HAS_DAP_REG;
1019                 }
1020
1021                 /* Quirk for read DP in JTAG mode (V2 only) from J24, fixed in J32 */
1022                 if (h->version.jtag >= 24 && h->version.jtag < 32)
1023                         flags |= STLINK_F_QUIRK_JTAG_DP_READ;
1024
1025                 /* API to read/write memory at 16 bit from J26 */
1026                 if (h->version.jtag >= 26)
1027                         flags |= STLINK_F_HAS_MEM_16BIT;
1028
1029                 /* API required to init AP before any AP access from J28 */
1030                 if (h->version.jtag >= 28)
1031                         flags |= STLINK_F_HAS_AP_INIT;
1032
1033                 /* Banked regs (DPv1 & DPv2) support from V2J32 */
1034                 if (h->version.jtag >= 32)
1035                         flags |= STLINK_F_HAS_DPBANKSEL;
1036
1037                 break;
1038         case 3:
1039                 /* all STLINK-V3 use api-v3 */
1040                 h->version.jtag_api = STLINK_JTAG_API_V3;
1041
1042                 /* STLINK-V3 is a superset of ST-LINK/V2 */
1043
1044                 /* API for trace */
1045                 /* API for target voltage */
1046                 flags |= STLINK_F_HAS_TRACE;
1047
1048                 /* preferred API to get last R/W status */
1049                 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1050
1051                 /* API to access DAP registers */
1052                 flags |= STLINK_F_HAS_DAP_REG;
1053
1054                 /* API to read/write memory at 16 bit */
1055                 flags |= STLINK_F_HAS_MEM_16BIT;
1056
1057                 /* API required to init AP before any AP access */
1058                 flags |= STLINK_F_HAS_AP_INIT;
1059
1060                 /* Banked regs (DPv1 & DPv2) support from V3J2 */
1061                 if (h->version.jtag >= 2)
1062                         flags |= STLINK_F_HAS_DPBANKSEL;
1063
1064                 /* 8bit read/write max packet size 512 bytes from V3J6 */
1065                 if (h->version.jtag >= 6)
1066                         flags |= STLINK_F_HAS_RW8_512BYTES;
1067
1068                 break;
1069         default:
1070                 break;
1071         }
1072         h->version.flags = flags;
1073
1074         p = v_str;
1075         p += sprintf(p, "V%d", v);
1076         if (jtag || !msd)
1077                 p += sprintf(p, "J%d", jtag);
1078         if (msd)
1079                 p += sprintf(p, "M%d", msd);
1080         if (bridge)
1081                 p += sprintf(p, "B%d", bridge);
1082         if (swim || !msd)
1083                 sprintf(p, "S%d", swim);
1084
1085         LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
1086                 v_str,
1087                 h->version.jtag_api,
1088                 h->vid,
1089                 h->pid);
1090
1091         return ERROR_OK;
1092 }
1093
1094 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
1095 {
1096         struct stlink_usb_handle_s *h = handle;
1097         uint32_t adc_results[2];
1098
1099         /* no error message, simply quit with error */
1100         if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
1101                 return ERROR_COMMAND_NOTFOUND;
1102
1103         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1104
1105         h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
1106
1107         int result = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1108
1109         if (result != ERROR_OK)
1110                 return result;
1111
1112         /* convert result */
1113         adc_results[0] = le_to_h_u32(h->databuf);
1114         adc_results[1] = le_to_h_u32(h->databuf + 4);
1115
1116         *target_voltage = 0;
1117
1118         if (adc_results[0])
1119                 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
1120
1121         LOG_INFO("Target voltage: %f", (double)*target_voltage);
1122
1123         return ERROR_OK;
1124 }
1125
1126 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
1127 {
1128         struct stlink_usb_handle_s *h = handle;
1129
1130         assert(handle != NULL);
1131
1132         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
1133                 return ERROR_COMMAND_NOTFOUND;
1134
1135         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1136
1137         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1138         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
1139         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1140         h->cmdidx += 2;
1141
1142         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1143
1144         if (result != ERROR_OK)
1145                 return result;
1146
1147         return ERROR_OK;
1148 }
1149
1150 static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
1151 {
1152         struct stlink_usb_handle_s *h = handle;
1153
1154         assert(handle != NULL);
1155
1156         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
1157                 return ERROR_COMMAND_NOTFOUND;
1158
1159         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1160
1161         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1162         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
1163         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1164         h->cmdidx += 2;
1165
1166         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1167
1168         if (result != ERROR_OK)
1169                 return result;
1170
1171         return ERROR_OK;
1172 }
1173
1174 /** */
1175 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
1176 {
1177         int res;
1178         struct stlink_usb_handle_s *h = handle;
1179
1180         assert(handle != NULL);
1181
1182         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1183
1184         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
1185
1186         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1187
1188         if (res != ERROR_OK)
1189                 return res;
1190
1191         *mode = h->databuf[0];
1192
1193         return ERROR_OK;
1194 }
1195
1196 /** */
1197 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
1198 {
1199         int rx_size = 0;
1200         struct stlink_usb_handle_s *h = handle;
1201
1202         assert(handle != NULL);
1203
1204         /* on api V2 we are able the read the latest command
1205          * status
1206          * TODO: we need the test on api V1 too
1207          */
1208         if (h->version.jtag_api != STLINK_JTAG_API_V1)
1209                 rx_size = 2;
1210
1211         stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
1212
1213         switch (type) {
1214                 case STLINK_MODE_DEBUG_JTAG:
1215                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1216                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1217                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1218                         else
1219                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1220                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
1221                         break;
1222                 case STLINK_MODE_DEBUG_SWD:
1223                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1224                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1225                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1226                         else
1227                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1228                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
1229                         break;
1230                 case STLINK_MODE_DEBUG_SWIM:
1231                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1232                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
1233                         /* no answer for this function... */
1234                         rx_size = 0;
1235                         break;
1236                 case STLINK_MODE_DFU:
1237                 case STLINK_MODE_MASS:
1238                 default:
1239                         return ERROR_FAIL;
1240         }
1241
1242         return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
1243 }
1244
1245 /** */
1246 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
1247 {
1248         int res;
1249         struct stlink_usb_handle_s *h = handle;
1250
1251         assert(handle != NULL);
1252
1253         /* command with no reply, use a valid endpoint but zero size */
1254         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1255
1256         switch (type) {
1257                 case STLINK_MODE_DEBUG_JTAG:
1258                 case STLINK_MODE_DEBUG_SWD:
1259                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1260                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
1261                         break;
1262                 case STLINK_MODE_DEBUG_SWIM:
1263                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1264                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
1265                         break;
1266                 case STLINK_MODE_DFU:
1267                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
1268                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
1269                         break;
1270                 case STLINK_MODE_MASS:
1271                 default:
1272                         return ERROR_FAIL;
1273         }
1274
1275         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1276
1277         if (res != ERROR_OK)
1278                 return res;
1279
1280         return ERROR_OK;
1281 }
1282
1283 static int stlink_usb_assert_srst(void *handle, int srst);
1284
1285 static enum stlink_mode stlink_get_mode(enum hl_transports t)
1286 {
1287         switch (t) {
1288         case HL_TRANSPORT_SWD:
1289                 return STLINK_MODE_DEBUG_SWD;
1290         case HL_TRANSPORT_JTAG:
1291                 return STLINK_MODE_DEBUG_JTAG;
1292         case HL_TRANSPORT_SWIM:
1293                 return STLINK_MODE_DEBUG_SWIM;
1294         default:
1295                 return STLINK_MODE_UNKNOWN;
1296         }
1297 }
1298
1299 /** */
1300 static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int initial_interface_speed)
1301 {
1302         int res;
1303         uint8_t mode;
1304         enum stlink_mode emode;
1305         struct stlink_usb_handle_s *h = handle;
1306
1307         assert(handle != NULL);
1308
1309         res = stlink_usb_current_mode(handle, &mode);
1310
1311         if (res != ERROR_OK)
1312                 return res;
1313
1314         LOG_DEBUG("MODE: 0x%02X", mode);
1315
1316         /* try to exit current mode */
1317         switch (mode) {
1318                 case STLINK_DEV_DFU_MODE:
1319                         emode = STLINK_MODE_DFU;
1320                         break;
1321                 case STLINK_DEV_DEBUG_MODE:
1322                         emode = STLINK_MODE_DEBUG_SWD;
1323                         break;
1324                 case STLINK_DEV_SWIM_MODE:
1325                         emode = STLINK_MODE_DEBUG_SWIM;
1326                         break;
1327                 case STLINK_DEV_BOOTLOADER_MODE:
1328                 case STLINK_DEV_MASS_MODE:
1329                 default:
1330                         emode = STLINK_MODE_UNKNOWN;
1331                         break;
1332         }
1333
1334         if (emode != STLINK_MODE_UNKNOWN) {
1335                 res = stlink_usb_mode_leave(handle, emode);
1336
1337                 if (res != ERROR_OK)
1338                         return res;
1339         }
1340
1341         res = stlink_usb_current_mode(handle, &mode);
1342
1343         if (res != ERROR_OK)
1344                 return res;
1345
1346         /* we check the target voltage here as an aid to debugging connection problems.
1347          * the stlink requires the target Vdd to be connected for reliable debugging.
1348          * this cmd is supported in all modes except DFU
1349          */
1350         if (mode != STLINK_DEV_DFU_MODE) {
1351
1352                 float target_voltage;
1353
1354                 /* check target voltage (if supported) */
1355                 res = stlink_usb_check_voltage(h, &target_voltage);
1356
1357                 if (res != ERROR_OK) {
1358                         if (res != ERROR_COMMAND_NOTFOUND)
1359                                 LOG_ERROR("voltage check failed");
1360                         /* attempt to continue as it is not a catastrophic failure */
1361                 } else {
1362                         /* check for a sensible target voltage, operating range is 1.65-5.5v
1363                          * according to datasheet */
1364                         if (target_voltage < 1.5)
1365                                 LOG_ERROR("target voltage may be too low for reliable debugging");
1366                 }
1367         }
1368
1369         LOG_DEBUG("MODE: 0x%02X", mode);
1370
1371         /* set selected mode */
1372         emode = stlink_get_mode(h->transport);
1373
1374         if (emode == STLINK_MODE_UNKNOWN) {
1375                 LOG_ERROR("selected mode (transport) not supported");
1376                 return ERROR_FAIL;
1377         }
1378
1379         /* set the speed before entering the mode, as the chip discovery phase should be done at this speed too */
1380         if (h->transport == HL_TRANSPORT_JTAG) {
1381                 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
1382                         stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
1383                         stlink_speed(h, initial_interface_speed, false);
1384                 }
1385         } else if (h->transport == HL_TRANSPORT_SWD) {
1386                 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
1387                         stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
1388                         stlink_speed(h, initial_interface_speed, false);
1389                 }
1390         }
1391
1392         if (h->version.jtag_api == STLINK_JTAG_API_V3) {
1393                 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
1394
1395                 stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map);
1396                 stlink_dump_speed_map(map, ARRAY_SIZE(map));
1397                 stlink_speed(h, initial_interface_speed, false);
1398         }
1399
1400         /* preliminary SRST assert:
1401          * We want SRST is asserted before activating debug signals (mode_enter).
1402          * As the required mode has not been set, the adapter may not know what pin to use.
1403          * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1404          * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1405          * after power on, SWIM_RST stays unchanged */
1406         if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1407                 stlink_usb_assert_srst(handle, 0);
1408                 /* do not check the return status here, we will
1409                    proceed and enter the desired mode below
1410                    and try asserting srst again. */
1411
1412         res = stlink_usb_mode_enter(handle, emode);
1413         if (res != ERROR_OK)
1414                 return res;
1415
1416         /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1417         if (connect_under_reset) {
1418                 res = stlink_usb_assert_srst(handle, 0);
1419                 if (res != ERROR_OK)
1420                         return res;
1421         }
1422
1423         res = stlink_usb_current_mode(handle, &mode);
1424
1425         if (res != ERROR_OK)
1426                 return res;
1427
1428         LOG_DEBUG("MODE: 0x%02X", mode);
1429
1430         return ERROR_OK;
1431 }
1432
1433 /* request status from last swim request */
1434 static int stlink_swim_status(void *handle)
1435 {
1436         struct stlink_usb_handle_s *h = handle;
1437         int res;
1438
1439         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1440         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1441         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1442         /* error is checked by the caller */
1443         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1444         if (res != ERROR_OK)
1445                 return res;
1446         return ERROR_OK;
1447 }
1448 /*
1449         the purpose of this function is unknown...
1450         capabilites? anyway for swim v6 it returns
1451         0001020600000000
1452 */
1453 __attribute__((unused))
1454 static int stlink_swim_cap(void *handle, uint8_t *cap)
1455 {
1456         struct stlink_usb_handle_s *h = handle;
1457         int res;
1458
1459         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1460         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1461         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1462         h->cmdbuf[h->cmdidx++] = 0x01;
1463         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1464         if (res != ERROR_OK)
1465                 return res;
1466         memcpy(cap, h->databuf, 8);
1467         return ERROR_OK;
1468 }
1469
1470 /*      debug dongle assert/deassert sreset line */
1471 static int stlink_swim_assert_reset(void *handle, int reset)
1472 {
1473         struct stlink_usb_handle_s *h = handle;
1474         int res;
1475
1476         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1477         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1478         if (!reset)
1479                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1480         else
1481                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1482         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1483         if (res != ERROR_OK)
1484                 return res;
1485         return ERROR_OK;
1486 }
1487
1488 /*
1489         send swim enter seq
1490         1.3ms low then 750Hz then 1.5kHz
1491 */
1492 static int stlink_swim_enter(void *handle)
1493 {
1494         struct stlink_usb_handle_s *h = handle;
1495         int res;
1496
1497         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1498         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1499         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1500         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1501         if (res != ERROR_OK)
1502                 return res;
1503         return ERROR_OK;
1504 }
1505
1506 /*      switch high/low speed swim */
1507 static int stlink_swim_speed(void *handle, int speed)
1508 {
1509         struct stlink_usb_handle_s *h = handle;
1510         int res;
1511
1512         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1513         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1514         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1515         if (speed)
1516                 h->cmdbuf[h->cmdidx++] = 1;
1517         else
1518                 h->cmdbuf[h->cmdidx++] = 0;
1519         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1520         if (res != ERROR_OK)
1521                 return res;
1522         return ERROR_OK;
1523 }
1524
1525 /*
1526         initiate srst from swim.
1527         nrst is pulled low for 50us.
1528 */
1529 static int stlink_swim_generate_rst(void *handle)
1530 {
1531         struct stlink_usb_handle_s *h = handle;
1532         int res;
1533
1534         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1535         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1536         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1537         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1538         if (res != ERROR_OK)
1539                 return res;
1540         return ERROR_OK;
1541 }
1542
1543 /*
1544         send resyncronize sequence
1545         swim is pulled low for 16us
1546         reply is 64 clks low
1547 */
1548 static int stlink_swim_resync(void *handle)
1549 {
1550         struct stlink_usb_handle_s *h = handle;
1551         int res;
1552
1553         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1554         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1555         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1556         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1557         if (res != ERROR_OK)
1558                 return res;
1559         return ERROR_OK;
1560 }
1561
1562 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1563 {
1564         struct stlink_usb_handle_s *h = handle;
1565         int res;
1566         unsigned int i;
1567         unsigned int datalen = 0;
1568         int cmdsize = STLINK_CMD_SIZE_V2;
1569
1570         if (len > STLINK_DATA_SIZE)
1571                 return ERROR_FAIL;
1572
1573         if (h->version.stlink == 1)
1574                 cmdsize = STLINK_SG_SIZE;
1575
1576         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1577         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1578         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1579         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1580         h->cmdidx += 2;
1581         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1582         h->cmdidx += 4;
1583         for (i = 0; i < len; i++) {
1584                 if (h->cmdidx == cmdsize)
1585                         h->databuf[datalen++] = *(data++);
1586                 else
1587                         h->cmdbuf[h->cmdidx++] = *(data++);
1588         }
1589         if (h->version.stlink == 1)
1590                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1591
1592         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1593         if (res != ERROR_OK)
1594                 return res;
1595         return ERROR_OK;
1596 }
1597
1598 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1599 {
1600         struct stlink_usb_handle_s *h = handle;
1601         int res;
1602
1603         if (len > STLINK_DATA_SIZE)
1604                 return ERROR_FAIL;
1605
1606         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1607         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1608         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1609         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1610         h->cmdidx += 2;
1611         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1612         h->cmdidx += 4;
1613         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1614         if (res != ERROR_OK)
1615                 return res;
1616
1617         stlink_usb_init_buffer(handle, h->rx_ep, len);
1618         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1619         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1620         res = stlink_usb_xfer_noerrcheck(handle, data, len);
1621         if (res != ERROR_OK)
1622                 return res;
1623
1624         return ERROR_OK;
1625 }
1626
1627 /** */
1628 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1629 {
1630         int res, offset;
1631         struct stlink_usb_handle_s *h = handle;
1632
1633         assert(handle != NULL);
1634
1635         /* there is no swim read core id cmd */
1636         if (h->transport == HL_TRANSPORT_SWIM) {
1637                 *idcode = 0;
1638                 return ERROR_OK;
1639         }
1640
1641         stlink_usb_init_buffer(handle, h->rx_ep, 12);
1642
1643         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1644         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1645                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1646
1647                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1648                 offset = 0;
1649         } else {
1650                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
1651
1652                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
1653                 offset = 4;
1654         }
1655
1656         if (res != ERROR_OK)
1657                 return res;
1658
1659         *idcode = le_to_h_u32(h->databuf + offset);
1660
1661         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1662
1663         return ERROR_OK;
1664 }
1665
1666 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1667 {
1668         struct stlink_usb_handle_s *h = handle;
1669         int res;
1670
1671         assert(handle != NULL);
1672
1673         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1674
1675         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1676         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1677         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1678         h->cmdidx += 4;
1679
1680         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1681         if (res != ERROR_OK)
1682                 return res;
1683
1684         *val = le_to_h_u32(h->databuf + 4);
1685         return ERROR_OK;
1686 }
1687
1688 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
1689 {
1690         struct stlink_usb_handle_s *h = handle;
1691
1692         assert(handle != NULL);
1693
1694         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1695
1696         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1697         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1698                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
1699         else
1700                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
1701         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1702         h->cmdidx += 4;
1703         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1704         h->cmdidx += 4;
1705
1706         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1707 }
1708
1709 /** */
1710 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
1711 {
1712         struct stlink_usb_handle_s *h = handle;
1713
1714         assert(handle != NULL);
1715
1716         if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
1717                 int res;
1718
1719                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1720
1721                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1722                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1723
1724                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1725                 if (res != ERROR_OK)
1726                         return res;
1727
1728                 size_t bytes_avail = le_to_h_u16(h->databuf);
1729                 *size = bytes_avail < *size ? bytes_avail : *size - 1;
1730
1731                 if (*size > 0) {
1732                         res = stlink_usb_read_trace(handle, buf, *size);
1733                         if (res != ERROR_OK)
1734                                 return res;
1735                         return ERROR_OK;
1736                 }
1737         }
1738         *size = 0;
1739         return ERROR_OK;
1740 }
1741
1742 static enum target_state stlink_usb_v2_get_status(void *handle)
1743 {
1744         int result;
1745         uint32_t status;
1746
1747         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
1748         if  (result != ERROR_OK)
1749                 return TARGET_UNKNOWN;
1750
1751         if (status & S_HALT)
1752                 return TARGET_HALTED;
1753         else if (status & S_RESET_ST)
1754                 return TARGET_RESET;
1755
1756         return TARGET_RUNNING;
1757 }
1758
1759 /** */
1760 static enum target_state stlink_usb_state(void *handle)
1761 {
1762         int res;
1763         struct stlink_usb_handle_s *h = handle;
1764
1765         assert(handle != NULL);
1766
1767         if (h->transport == HL_TRANSPORT_SWIM) {
1768                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1769                 if (res != ERROR_OK)
1770                         return TARGET_UNKNOWN;
1771
1772                 res = stlink_swim_resync(handle);
1773                 if (res != ERROR_OK)
1774                         return TARGET_UNKNOWN;
1775
1776                 return ERROR_OK;
1777         }
1778
1779         if (h->reconnect_pending) {
1780                 LOG_INFO("Previous state query failed, trying to reconnect");
1781                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1782
1783                 if (res != ERROR_OK)
1784                         return TARGET_UNKNOWN;
1785
1786                 h->reconnect_pending = false;
1787         }
1788
1789         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1790                 res = stlink_usb_v2_get_status(handle);
1791                 if (res == TARGET_UNKNOWN)
1792                         h->reconnect_pending = true;
1793                 return res;
1794         }
1795
1796         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1797
1798         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1799         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
1800
1801         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1802
1803         if (res != ERROR_OK)
1804                 return TARGET_UNKNOWN;
1805
1806         if (h->databuf[0] == STLINK_CORE_RUNNING)
1807                 return TARGET_RUNNING;
1808         if (h->databuf[0] == STLINK_CORE_HALTED)
1809                 return TARGET_HALTED;
1810
1811         h->reconnect_pending = true;
1812
1813         return TARGET_UNKNOWN;
1814 }
1815
1816 static int stlink_usb_assert_srst(void *handle, int srst)
1817 {
1818         struct stlink_usb_handle_s *h = handle;
1819
1820         assert(handle != NULL);
1821
1822         if (h->transport == HL_TRANSPORT_SWIM)
1823                 return stlink_swim_assert_reset(handle, srst);
1824
1825         if (h->version.stlink == 1)
1826                 return ERROR_COMMAND_NOTFOUND;
1827
1828         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1829
1830         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1831         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
1832         h->cmdbuf[h->cmdidx++] = srst;
1833
1834         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1835 }
1836
1837 /** */
1838 static void stlink_usb_trace_disable(void *handle)
1839 {
1840         int res = ERROR_OK;
1841         struct stlink_usb_handle_s *h = handle;
1842
1843         assert(handle != NULL);
1844
1845         assert(h->version.flags & STLINK_F_HAS_TRACE);
1846
1847         LOG_DEBUG("Tracing: disable");
1848
1849         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1850         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1851         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1852         res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
1853
1854         if (res == ERROR_OK)
1855                 h->trace.enabled = false;
1856 }
1857
1858
1859 /** */
1860 static int stlink_usb_trace_enable(void *handle)
1861 {
1862         int res;
1863         struct stlink_usb_handle_s *h = handle;
1864
1865         assert(handle != NULL);
1866
1867         if (h->version.flags & STLINK_F_HAS_TRACE) {
1868                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1869
1870                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1871                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1872                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1873                 h->cmdidx += 2;
1874                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
1875                 h->cmdidx += 4;
1876
1877                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
1878
1879                 if (res == ERROR_OK)  {
1880                         h->trace.enabled = true;
1881                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
1882                 }
1883         } else {
1884                 LOG_ERROR("Tracing is not supported by this version.");
1885                 res = ERROR_FAIL;
1886         }
1887
1888         return res;
1889 }
1890
1891 /** */
1892 static int stlink_usb_reset(void *handle)
1893 {
1894         struct stlink_usb_handle_s *h = handle;
1895         int retval;
1896
1897         assert(handle != NULL);
1898
1899         if (h->transport == HL_TRANSPORT_SWIM)
1900                 return stlink_swim_generate_rst(handle);
1901
1902         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1903
1904         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1905
1906         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1907                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
1908         else
1909                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
1910
1911         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
1912         if (retval != ERROR_OK)
1913                 return retval;
1914
1915         if (h->trace.enabled) {
1916                 stlink_usb_trace_disable(h);
1917                 return stlink_usb_trace_enable(h);
1918         }
1919
1920         return ERROR_OK;
1921 }
1922
1923 /** */
1924 static int stlink_usb_run(void *handle)
1925 {
1926         int res;
1927         struct stlink_usb_handle_s *h = handle;
1928
1929         assert(handle != NULL);
1930
1931         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1932                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1933
1934                 return res;
1935         }
1936
1937         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1938
1939         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1940         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1941
1942         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1943 }
1944
1945 /** */
1946 static int stlink_usb_halt(void *handle)
1947 {
1948         int res;
1949         struct stlink_usb_handle_s *h = handle;
1950
1951         assert(handle != NULL);
1952
1953         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1954                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1955
1956                 return res;
1957         }
1958
1959         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1960
1961         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1962         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1963
1964         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1965 }
1966
1967 /** */
1968 static int stlink_usb_step(void *handle)
1969 {
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                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1976                  * that the Cortex-M3 currently does. */
1977                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1978                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1979                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1980         }
1981
1982         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1983
1984         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1985         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1986
1987         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1988 }
1989
1990 /** */
1991 static int stlink_usb_read_regs(void *handle)
1992 {
1993         int res;
1994         struct stlink_usb_handle_s *h = handle;
1995
1996         assert(handle != NULL);
1997
1998         stlink_usb_init_buffer(handle, h->rx_ep, 88);
1999
2000         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2001         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2002
2003                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
2004                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
2005                 /* regs data from offset 0 */
2006         } else {
2007                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
2008                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
2009                 /* status at offset 0, regs data from offset 4 */
2010         }
2011
2012         return res;
2013 }
2014
2015 /** */
2016 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
2017 {
2018         int res;
2019         struct stlink_usb_handle_s *h = handle;
2020
2021         assert(handle != NULL);
2022
2023         stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
2024
2025         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2026         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2027                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
2028         else
2029                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
2030         h->cmdbuf[h->cmdidx++] = num;
2031
2032         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2033                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2034                 if (res != ERROR_OK)
2035                         return res;
2036                 *val = le_to_h_u32(h->databuf);
2037                 return ERROR_OK;
2038         } else {
2039                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2040                 if (res != ERROR_OK)
2041                         return res;
2042                 *val = le_to_h_u32(h->databuf + 4);
2043                 return ERROR_OK;
2044         }
2045 }
2046
2047 /** */
2048 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
2049 {
2050         struct stlink_usb_handle_s *h = handle;
2051
2052         assert(handle != NULL);
2053
2054         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2055
2056         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2057         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2058                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
2059         else
2060                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
2061         h->cmdbuf[h->cmdidx++] = num;
2062         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2063         h->cmdidx += 4;
2064
2065         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2066 }
2067
2068 static int stlink_usb_get_rw_status(void *handle)
2069 {
2070         struct stlink_usb_handle_s *h = handle;
2071
2072         assert(handle != NULL);
2073
2074         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2075                 return ERROR_OK;
2076
2077         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2078
2079         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2080         if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
2081                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
2082                 return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2083         } else {
2084                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
2085                 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2086         }
2087 }
2088
2089 /** */
2090 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
2091                           uint8_t *buffer)
2092 {
2093         int res;
2094         uint16_t read_len = len;
2095         struct stlink_usb_handle_s *h = handle;
2096
2097         assert(handle != NULL);
2098
2099         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2100         if (len > stlink_usb_block(h)) {
2101                 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2102                 return ERROR_FAIL;
2103         }
2104
2105         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2106
2107         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2108         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2109         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2110         h->cmdidx += 4;
2111         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2112         h->cmdidx += 2;
2113
2114         /* we need to fix read length for single bytes */
2115         if (read_len == 1)
2116                 read_len++;
2117
2118         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
2119
2120         if (res != ERROR_OK)
2121                 return res;
2122
2123         memcpy(buffer, h->databuf, len);
2124
2125         return stlink_usb_get_rw_status(handle);
2126 }
2127
2128 /** */
2129 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
2130                            const uint8_t *buffer)
2131 {
2132         int res;
2133         struct stlink_usb_handle_s *h = handle;
2134
2135         assert(handle != NULL);
2136
2137         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2138         if (len > stlink_usb_block(h)) {
2139                 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2140                 return ERROR_FAIL;
2141         }
2142
2143         stlink_usb_init_buffer(handle, h->tx_ep, len);
2144
2145         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2146         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2147         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2148         h->cmdidx += 4;
2149         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2150         h->cmdidx += 2;
2151
2152         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2153
2154         if (res != ERROR_OK)
2155                 return res;
2156
2157         return stlink_usb_get_rw_status(handle);
2158 }
2159
2160 /** */
2161 static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
2162                           uint8_t *buffer)
2163 {
2164         int res;
2165         struct stlink_usb_handle_s *h = handle;
2166
2167         assert(handle != NULL);
2168
2169         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2170                 return ERROR_COMMAND_NOTFOUND;
2171
2172         /* data must be a multiple of 2 and half-word aligned */
2173         if (len % 2 || addr % 2) {
2174                 LOG_DEBUG("Invalid data alignment");
2175                 return ERROR_TARGET_UNALIGNED_ACCESS;
2176         }
2177
2178         stlink_usb_init_buffer(handle, h->rx_ep, len);
2179
2180         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2181         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2182         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2183         h->cmdidx += 4;
2184         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2185         h->cmdidx += 2;
2186
2187         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2188
2189         if (res != ERROR_OK)
2190                 return res;
2191
2192         memcpy(buffer, h->databuf, len);
2193
2194         return stlink_usb_get_rw_status(handle);
2195 }
2196
2197 /** */
2198 static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
2199                            const uint8_t *buffer)
2200 {
2201         int res;
2202         struct stlink_usb_handle_s *h = handle;
2203
2204         assert(handle != NULL);
2205
2206         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2207                 return ERROR_COMMAND_NOTFOUND;
2208
2209         /* data must be a multiple of 2 and half-word aligned */
2210         if (len % 2 || addr % 2) {
2211                 LOG_DEBUG("Invalid data alignment");
2212                 return ERROR_TARGET_UNALIGNED_ACCESS;
2213         }
2214
2215         stlink_usb_init_buffer(handle, h->tx_ep, len);
2216
2217         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2218         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2219         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2220         h->cmdidx += 4;
2221         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2222         h->cmdidx += 2;
2223
2224         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2225
2226         if (res != ERROR_OK)
2227                 return res;
2228
2229         return stlink_usb_get_rw_status(handle);
2230 }
2231
2232 /** */
2233 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
2234                           uint8_t *buffer)
2235 {
2236         int res;
2237         struct stlink_usb_handle_s *h = handle;
2238
2239         assert(handle != NULL);
2240
2241         /* data must be a multiple of 4 and word aligned */
2242         if (len % 4 || addr % 4) {
2243                 LOG_DEBUG("Invalid data alignment");
2244                 return ERROR_TARGET_UNALIGNED_ACCESS;
2245         }
2246
2247         stlink_usb_init_buffer(handle, h->rx_ep, len);
2248
2249         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2250         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2251         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2252         h->cmdidx += 4;
2253         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2254         h->cmdidx += 2;
2255
2256         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2257
2258         if (res != ERROR_OK)
2259                 return res;
2260
2261         memcpy(buffer, h->databuf, len);
2262
2263         return stlink_usb_get_rw_status(handle);
2264 }
2265
2266 /** */
2267 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
2268                            const uint8_t *buffer)
2269 {
2270         int res;
2271         struct stlink_usb_handle_s *h = handle;
2272
2273         assert(handle != NULL);
2274
2275         /* data must be a multiple of 4 and word aligned */
2276         if (len % 4 || addr % 4) {
2277                 LOG_DEBUG("Invalid data alignment");
2278                 return ERROR_TARGET_UNALIGNED_ACCESS;
2279         }
2280
2281         stlink_usb_init_buffer(handle, h->tx_ep, len);
2282
2283         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2284         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2285         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2286         h->cmdidx += 4;
2287         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2288         h->cmdidx += 2;
2289
2290         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2291
2292         if (res != ERROR_OK)
2293                 return res;
2294
2295         return stlink_usb_get_rw_status(handle);
2296 }
2297
2298 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2299 {
2300         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2301         if (max_tar_block == 0)
2302                 max_tar_block = 4;
2303         return max_tar_block;
2304 }
2305
2306 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2307                 uint32_t count, uint8_t *buffer)
2308 {
2309         int retval = ERROR_OK;
2310         uint32_t bytes_remaining;
2311         int retries = 0;
2312         struct stlink_usb_handle_s *h = handle;
2313
2314         /* calculate byte count */
2315         count *= size;
2316
2317         /* switch to 8 bit if stlink does not support 16 bit memory read */
2318         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2319                 size = 1;
2320
2321         while (count) {
2322
2323                 bytes_remaining = (size != 1) ? \
2324                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2325
2326                 if (count < bytes_remaining)
2327                         bytes_remaining = count;
2328
2329                 if (h->transport == HL_TRANSPORT_SWIM) {
2330                         retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
2331                         if (retval != ERROR_OK)
2332                                 return retval;
2333                 } else
2334                 /*
2335                  * all stlink support 8/32bit memory read/writes and only from
2336                  * stlink V2J26 there is support for 16 bit memory read/write.
2337                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2338                  * as 8bit access.
2339                  */
2340                 if (size != 1) {
2341
2342                         /* When in jtag mode the stlink uses the auto-increment functionality.
2343                          * However it expects us to pass the data correctly, this includes
2344                          * alignment and any page boundaries. We already do this as part of the
2345                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2346                          * needs implementing manually.
2347                          * currently this only affects jtag mode, according to ST they do single
2348                          * access in SWD mode - but this may change and so we do it for both modes */
2349
2350                         /* we first need to check for any unaligned bytes */
2351                         if (addr & (size - 1)) {
2352
2353                                 uint32_t head_bytes = size - (addr & (size - 1));
2354                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
2355                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2356                                         usleep((1<<retries++) * 1000);
2357                                         continue;
2358                                 }
2359                                 if (retval != ERROR_OK)
2360                                         return retval;
2361                                 buffer += head_bytes;
2362                                 addr += head_bytes;
2363                                 count -= head_bytes;
2364                                 bytes_remaining -= head_bytes;
2365                         }
2366
2367                         if (bytes_remaining & (size - 1))
2368                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
2369                         else if (size == 2)
2370                                 retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
2371                         else
2372                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
2373                 } else
2374                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
2375
2376                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2377                         usleep((1<<retries++) * 1000);
2378                         continue;
2379                 }
2380                 if (retval != ERROR_OK)
2381                         return retval;
2382
2383                 buffer += bytes_remaining;
2384                 addr += bytes_remaining;
2385                 count -= bytes_remaining;
2386         }
2387
2388         return retval;
2389 }
2390
2391 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2392                 uint32_t count, const uint8_t *buffer)
2393 {
2394         int retval = ERROR_OK;
2395         uint32_t bytes_remaining;
2396         int retries = 0;
2397         struct stlink_usb_handle_s *h = handle;
2398
2399         /* calculate byte count */
2400         count *= size;
2401
2402         /* switch to 8 bit if stlink does not support 16 bit memory read */
2403         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2404                 size = 1;
2405
2406         while (count) {
2407
2408                 bytes_remaining = (size != 1) ? \
2409                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2410
2411                 if (count < bytes_remaining)
2412                         bytes_remaining = count;
2413
2414                 if (h->transport == HL_TRANSPORT_SWIM) {
2415                         retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
2416                         if (retval != ERROR_OK)
2417                                 return retval;
2418                 } else
2419                 /*
2420                  * all stlink support 8/32bit memory read/writes and only from
2421                  * stlink V2J26 there is support for 16 bit memory read/write.
2422                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2423                  * as 8bit access.
2424                  */
2425                 if (size != 1) {
2426
2427                         /* When in jtag mode the stlink uses the auto-increment functionality.
2428                          * However it expects us to pass the data correctly, this includes
2429                          * alignment and any page boundaries. We already do this as part of the
2430                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2431                          * needs implementing manually.
2432                          * currently this only affects jtag mode, according to ST they do single
2433                          * access in SWD mode - but this may change and so we do it for both modes */
2434
2435                         /* we first need to check for any unaligned bytes */
2436                         if (addr & (size - 1)) {
2437
2438                                 uint32_t head_bytes = size - (addr & (size - 1));
2439                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
2440                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2441                                         usleep((1<<retries++) * 1000);
2442                                         continue;
2443                                 }
2444                                 if (retval != ERROR_OK)
2445                                         return retval;
2446                                 buffer += head_bytes;
2447                                 addr += head_bytes;
2448                                 count -= head_bytes;
2449                                 bytes_remaining -= head_bytes;
2450                         }
2451
2452                         if (bytes_remaining & (size - 1))
2453                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
2454                         else if (size == 2)
2455                                 retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
2456                         else
2457                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
2458
2459                 } else
2460                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
2461                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2462                         usleep((1<<retries++) * 1000);
2463                         continue;
2464                 }
2465                 if (retval != ERROR_OK)
2466                         return retval;
2467
2468                 buffer += bytes_remaining;
2469                 addr += bytes_remaining;
2470                 count -= bytes_remaining;
2471         }
2472
2473         return retval;
2474 }
2475
2476 /** */
2477 static int stlink_usb_override_target(const char *targetname)
2478 {
2479         return !strcmp(targetname, "cortex_m");
2480 }
2481
2482 static int stlink_speed_swim(void *handle, int khz, bool query)
2483 {
2484         /*
2485                         we dont care what the khz rate is
2486                         we only have low and high speed...
2487                         before changing speed the SWIM_CSR HS bit
2488                         must be updated
2489          */
2490         if (khz == 0)
2491                 stlink_swim_speed(handle, 0);
2492         else
2493                 stlink_swim_speed(handle, 1);
2494         return khz;
2495 }
2496
2497 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
2498 {
2499         unsigned int i;
2500         int speed_index = -1;
2501         int speed_diff = INT_MAX;
2502         int last_valid_speed = -1;
2503         bool match = true;
2504
2505         for (i = 0; i < map_size; i++) {
2506                 if (!map[i].speed)
2507                         continue;
2508                 last_valid_speed = i;
2509                 if (khz == map[i].speed) {
2510                         speed_index = i;
2511                         break;
2512                 } else {
2513                         int current_diff = khz - map[i].speed;
2514                         /* get abs value for comparison */
2515                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
2516                         if ((current_diff < speed_diff) && khz >= map[i].speed) {
2517                                 speed_diff = current_diff;
2518                                 speed_index = i;
2519                         }
2520                 }
2521         }
2522
2523         if (speed_index == -1) {
2524                 /* this will only be here if we cannot match the slow speed.
2525                  * use the slowest speed we support.*/
2526                 speed_index = last_valid_speed;
2527                 match = false;
2528         } else if (i == map_size)
2529                 match = false;
2530
2531         if (!match && query) {
2532                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
2533                                 khz, map[speed_index].speed);
2534         }
2535
2536         return speed_index;
2537 }
2538
2539 static int stlink_speed_swd(void *handle, int khz, bool query)
2540 {
2541         int speed_index;
2542         struct stlink_usb_handle_s *h = handle;
2543
2544         /* old firmware cannot change it */
2545         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
2546                 return khz;
2547
2548         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
2549                 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
2550
2551         if (!query) {
2552                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
2553                 if (result != ERROR_OK) {
2554                         LOG_ERROR("Unable to set adapter speed");
2555                         return khz;
2556                 }
2557         }
2558
2559         return stlink_khz_to_speed_map_swd[speed_index].speed;
2560 }
2561
2562 static int stlink_speed_jtag(void *handle, int khz, bool query)
2563 {
2564         int speed_index;
2565         struct stlink_usb_handle_s *h = handle;
2566
2567         /* old firmware cannot change it */
2568         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
2569                 return khz;
2570
2571         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
2572                 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
2573
2574         if (!query) {
2575                 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
2576                 if (result != ERROR_OK) {
2577                         LOG_ERROR("Unable to set adapter speed");
2578                         return khz;
2579                 }
2580         }
2581
2582         return stlink_khz_to_speed_map_jtag[speed_index].speed;
2583 }
2584
2585 void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
2586 {
2587         unsigned int i;
2588
2589         LOG_DEBUG("Supported clock speeds are:");
2590         for (i = 0; i < map_size; i++)
2591                 if (map[i].speed)
2592                         LOG_DEBUG("%d kHz", map[i].speed);
2593 }
2594
2595 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
2596 {
2597         struct stlink_usb_handle_s *h = handle;
2598         int i;
2599
2600         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2601                 LOG_ERROR("Unknown command");
2602                 return 0;
2603         }
2604
2605         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2606
2607         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2608         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
2609         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2610
2611         int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
2612
2613         int size = h->databuf[8];
2614
2615         if (size > STLINK_V3_MAX_FREQ_NB)
2616                 size = STLINK_V3_MAX_FREQ_NB;
2617
2618         for (i = 0; i < size; i++) {
2619                 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
2620                 map[i].speed_divisor = i;
2621         }
2622
2623         /* set to zero all the next entries */
2624         for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
2625                 map[i].speed = 0;
2626
2627         return res;
2628 }
2629
2630 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
2631 {
2632         struct stlink_usb_handle_s *h = handle;
2633
2634         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2635                 LOG_ERROR("Unknown command");
2636                 return 0;
2637         }
2638
2639         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2640
2641         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2642         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
2643         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2644         h->cmdbuf[h->cmdidx++] = 0;
2645
2646         h_u32_to_le(&h->cmdbuf[4], frequency);
2647
2648         return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
2649 }
2650
2651 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
2652 {
2653         struct stlink_usb_handle_s *h = handle;
2654         int speed_index;
2655         struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2656
2657         stlink_get_com_freq(h, is_jtag, map);
2658
2659         speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
2660
2661         if (!query) {
2662                 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
2663                 if (result != ERROR_OK) {
2664                         LOG_ERROR("Unable to set adapter speed");
2665                         return khz;
2666                 }
2667         }
2668         return map[speed_index].speed;
2669 }
2670
2671 static int stlink_speed(void *handle, int khz, bool query)
2672 {
2673         struct stlink_usb_handle_s *h = handle;
2674
2675         if (!handle)
2676                 return khz;
2677
2678         switch (h->transport) {
2679         case HL_TRANSPORT_SWIM:
2680                 return stlink_speed_swim(handle, khz, query);
2681                 break;
2682         case HL_TRANSPORT_SWD:
2683                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2684                         return stlink_speed_v3(handle, false, khz, query);
2685                 else
2686                         return stlink_speed_swd(handle, khz, query);
2687                 break;
2688         case HL_TRANSPORT_JTAG:
2689                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2690                         return stlink_speed_v3(handle, true, khz, query);
2691                 else
2692                         return stlink_speed_jtag(handle, khz, query);
2693                 break;
2694         default:
2695                 break;
2696         }
2697
2698         return khz;
2699 }
2700
2701 /** */
2702 static int stlink_usb_close(void *handle)
2703 {
2704         int res;
2705         uint8_t mode;
2706         enum stlink_mode emode;
2707         struct stlink_usb_handle_s *h = handle;
2708
2709         if (h && h->fd)
2710                 res = stlink_usb_current_mode(handle, &mode);
2711         else
2712                 res = ERROR_FAIL;
2713         /* do not exit if return code != ERROR_OK,
2714            it prevents us from closing jtag_libusb */
2715
2716         if (res == ERROR_OK) {
2717                 /* try to exit current mode */
2718                 switch (mode) {
2719                         case STLINK_DEV_DFU_MODE:
2720                                 emode = STLINK_MODE_DFU;
2721                                 break;
2722                         case STLINK_DEV_DEBUG_MODE:
2723                                 emode = STLINK_MODE_DEBUG_SWD;
2724                                 break;
2725                         case STLINK_DEV_SWIM_MODE:
2726                                 emode = STLINK_MODE_DEBUG_SWIM;
2727                                 break;
2728                         case STLINK_DEV_BOOTLOADER_MODE:
2729                         case STLINK_DEV_MASS_MODE:
2730                         default:
2731                                 emode = STLINK_MODE_UNKNOWN;
2732                                 break;
2733                 }
2734
2735                 if (emode != STLINK_MODE_UNKNOWN)
2736                         stlink_usb_mode_leave(handle, emode);
2737                         /* do not check return code, it prevent
2738                         us from closing jtag_libusb */
2739         }
2740
2741         if (h && h->fd)
2742                 jtag_libusb_close(h->fd);
2743
2744         free(h);
2745
2746         return ERROR_OK;
2747 }
2748
2749 /* Compute ST-Link serial number from the device descriptor
2750  * this function will help to work-around a bug in old ST-Link/V2 DFU
2751  * the buggy DFU returns an incorrect serial in the USB descriptor
2752  * example for the following serial "57FF72067265575742132067"
2753  *  - the correct descriptor serial is:
2754  *    0x32, 0x03, 0x35, 0x00, 0x37, 0x00, 0x46, 0x00, 0x46, 0x00, 0x37, 0x00, 0x32, 0x00 ...
2755  *    this contains the length (0x32 = 50), the type (0x3 = DT_STRING) and the serial in unicode format
2756  *    the serial part is: 0x0035, 0x0037, 0x0046, 0x0046, 0x0037, 0x0032 ... >>  57FF72 ...
2757  *    this format could be read correctly by 'libusb_get_string_descriptor_ascii'
2758  *    so this case is managed by libusb_helper::string_descriptor_equal
2759  *  - the buggy DFU is not doing any unicode conversion and returns a raw serial data in the descriptor
2760  *    0x1a, 0x03, 0x57, 0x00, 0xFF, 0x00, 0x72, 0x00 ...
2761  *            >>    57          FF          72       ...
2762  *    based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
2763  *    and then we have just to convert the raw data into printable characters using sprintf
2764  */
2765 char *stlink_usb_get_alternate_serial(libusb_device_handle *device,
2766                 struct libusb_device_descriptor *dev_desc)
2767 {
2768         int usb_retval;
2769         unsigned char desc_serial[(STLINK_SERIAL_LEN + 1) * 2];
2770
2771         if (dev_desc->iSerialNumber == 0)
2772                 return NULL;
2773
2774         /* get the LANGID from String Descriptor Zero */
2775         usb_retval = libusb_get_string_descriptor(device, 0, 0, desc_serial,
2776                         sizeof(desc_serial));
2777
2778         if (usb_retval < LIBUSB_SUCCESS) {
2779                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
2780                                 libusb_error_name(usb_retval), usb_retval);
2781                 return NULL;
2782         } else if (usb_retval < 4) {
2783                 /* the size should be least 4 bytes to contain a minimum of 1 supported LANGID */
2784                 LOG_ERROR("could not get the LANGID");
2785                 return NULL;
2786         }
2787
2788         uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
2789
2790         /* get the serial */
2791         usb_retval = libusb_get_string_descriptor(device, dev_desc->iSerialNumber,
2792                         langid, desc_serial, sizeof(desc_serial));
2793
2794         unsigned char len = desc_serial[0];
2795
2796         if (usb_retval < LIBUSB_SUCCESS) {
2797                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
2798                                 libusb_error_name(usb_retval), usb_retval);
2799                 return NULL;
2800         } else if (desc_serial[1] != LIBUSB_DT_STRING || len > usb_retval) {
2801                 LOG_ERROR("invalid string in ST-LINK USB serial descriptor");
2802                 return NULL;
2803         }
2804
2805         if (len == ((STLINK_SERIAL_LEN + 1) * 2)) {
2806                 /* good ST-Link adapter, this case is managed by
2807                  * libusb::libusb_get_string_descriptor_ascii */
2808                 return NULL;
2809         } else if (len != ((STLINK_SERIAL_LEN / 2 + 1) * 2)) {
2810                 LOG_ERROR("unexpected serial length (%d) in descriptor", len);
2811                 return NULL;
2812         }
2813
2814         /* else (len == 26) => buggy ST-Link */
2815
2816         char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
2817         if (alternate_serial == NULL)
2818                 return NULL;
2819
2820         for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
2821                 sprintf(alternate_serial + i, "%02X", desc_serial[i + 2]);
2822
2823         alternate_serial[STLINK_SERIAL_LEN] = '\0';
2824
2825         return alternate_serial;
2826 }
2827
2828 /** */
2829 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
2830 {
2831         int err, retry_count = 1;
2832         struct stlink_usb_handle_s *h;
2833
2834         LOG_DEBUG("stlink_usb_open");
2835
2836         h = calloc(1, sizeof(struct stlink_usb_handle_s));
2837
2838         if (h == 0) {
2839                 LOG_DEBUG("malloc failed");
2840                 return ERROR_FAIL;
2841         }
2842
2843         h->transport = param->transport;
2844
2845         for (unsigned i = 0; param->vid[i]; i++) {
2846                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
2847                           param->transport, param->vid[i], param->pid[i],
2848                           param->serial ? param->serial : "");
2849         }
2850
2851         /*
2852           On certain host USB configurations(e.g. MacBook Air)
2853           STLINKv2 dongle seems to have its FW in a funky state if,
2854           after plugging it in, you try to use openocd with it more
2855           then once (by launching and closing openocd). In cases like
2856           that initial attempt to read the FW info via
2857           stlink_usb_version will fail and the device has to be reset
2858           in order to become operational.
2859          */
2860         do {
2861                 if (jtag_libusb_open(param->vid, param->pid, param->serial,
2862                                 &h->fd, stlink_usb_get_alternate_serial) != ERROR_OK) {
2863                         LOG_ERROR("open failed");
2864                         goto error_open;
2865                 }
2866
2867                 jtag_libusb_set_configuration(h->fd, 0);
2868
2869                 if (libusb_claim_interface(h->fd, 0) != ERROR_OK) {
2870                         LOG_DEBUG("claim interface failed");
2871                         goto error_open;
2872                 }
2873
2874                 /* RX EP is common for all versions */
2875                 h->rx_ep = STLINK_RX_EP;
2876
2877                 uint16_t pid;
2878                 if (jtag_libusb_get_pid(libusb_get_device(h->fd), &pid) != ERROR_OK) {
2879                         LOG_DEBUG("libusb_get_pid failed");
2880                         goto error_open;
2881                 }
2882
2883                 /* wrap version for first read */
2884                 switch (pid) {
2885                         case STLINK_V1_PID:
2886                                 h->version.stlink = 1;
2887                                 h->tx_ep = STLINK_TX_EP;
2888                                 break;
2889                         case STLINK_V3_USBLOADER_PID:
2890                         case STLINK_V3E_PID:
2891                         case STLINK_V3S_PID:
2892                         case STLINK_V3_2VCP_PID:
2893                                 h->version.stlink = 3;
2894                                 h->tx_ep = STLINK_V2_1_TX_EP;
2895                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2896                                 break;
2897                         case STLINK_V2_1_PID:
2898                         case STLINK_V2_1_NO_MSD_PID:
2899                                 h->version.stlink = 2;
2900                                 h->tx_ep = STLINK_V2_1_TX_EP;
2901                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2902                                 break;
2903                         default:
2904                         /* fall through - we assume V2 to be the default version*/
2905                         case STLINK_V2_PID:
2906                                 h->version.stlink = 2;
2907                                 h->tx_ep = STLINK_TX_EP;
2908                                 h->trace_ep = STLINK_TRACE_EP;
2909                                 break;
2910                 }
2911
2912                 /* get the device version */
2913                 err = stlink_usb_version(h);
2914
2915                 if (err == ERROR_OK) {
2916                         break;
2917                 } else if (h->version.stlink == 1 ||
2918                            retry_count == 0) {
2919                         LOG_ERROR("read version failed");
2920                         goto error_open;
2921                 } else {
2922                         err = libusb_release_interface(h->fd, 0);
2923                         if (err != ERROR_OK) {
2924                                 LOG_ERROR("release interface failed");
2925                                 goto error_open;
2926                         }
2927
2928                         err = libusb_reset_device(h->fd);
2929                         if (err != ERROR_OK) {
2930                                 LOG_ERROR("reset device failed");
2931                                 goto error_open;
2932                         }
2933
2934                         jtag_libusb_close(h->fd);
2935                         /*
2936                           Give the device one second to settle down and
2937                           reenumerate.
2938                          */
2939                         usleep(1 * 1000 * 1000);
2940                         retry_count--;
2941                 }
2942         } while (1);
2943
2944         /* check if mode is supported */
2945         err = ERROR_OK;
2946
2947         switch (h->transport) {
2948                 case HL_TRANSPORT_SWD:
2949                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2950                                 err = ERROR_FAIL;
2951                         /* fall-through */
2952                 case HL_TRANSPORT_JTAG:
2953                         if (h->version.jtag == 0)
2954                                 err = ERROR_FAIL;
2955                         break;
2956                 case HL_TRANSPORT_SWIM:
2957                         if (h->version.swim == 0)
2958                                 err = ERROR_FAIL;
2959                         break;
2960                 default:
2961                         err = ERROR_FAIL;
2962                         break;
2963         }
2964
2965         if (err != ERROR_OK) {
2966                 LOG_ERROR("mode (transport) not supported by device");
2967                 goto error_open;
2968         }
2969
2970         /* initialize the debug hardware */
2971         err = stlink_usb_init_mode(h, param->connect_under_reset, param->initial_interface_speed);
2972
2973         if (err != ERROR_OK) {
2974                 LOG_ERROR("init mode failed (unable to connect to the target)");
2975                 goto error_open;
2976         }
2977
2978         if (h->transport == HL_TRANSPORT_SWIM) {
2979                 err = stlink_swim_enter(h);
2980                 if (err != ERROR_OK) {
2981                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
2982                         goto error_open;
2983                 }
2984                 *fd = h;
2985                 h->max_mem_packet = STLINK_DATA_SIZE;
2986                 return ERROR_OK;
2987         }
2988
2989         /* get cpuid, so we can determine the max page size
2990          * start with a safe default */
2991         h->max_mem_packet = (1 << 10);
2992
2993         uint8_t buffer[4];
2994         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
2995         if (err == ERROR_OK) {
2996                 uint32_t cpuid = le_to_h_u32(buffer);
2997                 int i = (cpuid >> 4) & 0xf;
2998                 if (i == 4 || i == 3) {
2999                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
3000                         h->max_mem_packet = (1 << 12);
3001                 }
3002         }
3003
3004         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
3005
3006         *fd = h;
3007
3008         return ERROR_OK;
3009
3010 error_open:
3011         stlink_usb_close(h);
3012
3013         return ERROR_FAIL;
3014 }
3015
3016 int stlink_config_trace(void *handle, bool enabled,
3017                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3018                 unsigned int *trace_freq, unsigned int traceclkin_freq,
3019                 uint16_t *prescaler)
3020 {
3021         struct stlink_usb_handle_s *h = handle;
3022         uint16_t presc;
3023
3024         if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
3025                         pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
3026                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
3027                 return ERROR_FAIL;
3028         }
3029
3030         if (!enabled) {
3031                 stlink_usb_trace_disable(h);
3032                 return ERROR_OK;
3033         }
3034
3035         if (*trace_freq > STLINK_TRACE_MAX_HZ) {
3036                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
3037                           STLINK_TRACE_MAX_HZ);
3038                 return ERROR_FAIL;
3039         }
3040
3041         stlink_usb_trace_disable(h);
3042
3043         if (!*trace_freq)
3044                 *trace_freq = STLINK_TRACE_MAX_HZ;
3045
3046         presc = traceclkin_freq / *trace_freq;
3047
3048         if (traceclkin_freq % *trace_freq > 0)
3049                 presc++;
3050
3051         if (presc > TPIU_ACPR_MAX_SWOSCALER) {
3052                 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3053                         "frequency.");
3054                 return ERROR_FAIL;
3055         }
3056
3057         *prescaler = presc;
3058         h->trace.source_hz = *trace_freq;
3059
3060         return stlink_usb_trace_enable(h);
3061 }
3062
3063 /** */
3064 static int stlink_usb_init_access_port(void *handle, unsigned char ap_num)
3065 {
3066         struct stlink_usb_handle_s *h = handle;
3067
3068         assert(handle != NULL);
3069
3070         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3071                 return ERROR_COMMAND_NOTFOUND;
3072
3073         LOG_DEBUG_IO("init ap_num = %d", ap_num);
3074         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3075         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3076         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_INIT_AP;
3077         h->cmdbuf[h->cmdidx++] = ap_num;
3078
3079         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3080 }
3081
3082 /** */
3083 static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
3084 {
3085         struct stlink_usb_handle_s *h = handle;
3086
3087         assert(handle != NULL);
3088
3089         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3090                 return ERROR_COMMAND_NOTFOUND;
3091
3092         LOG_DEBUG_IO("close ap_num = %d", ap_num);
3093         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3094         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3095         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_CLOSE_AP_DBG;
3096         h->cmdbuf[h->cmdidx++] = ap_num;
3097
3098         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3099 }
3100
3101 /** */
3102 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
3103                         unsigned short addr, uint32_t *val)
3104 {
3105         struct stlink_usb_handle_s *h = handle;
3106         int retval;
3107
3108         assert(handle != NULL);
3109
3110         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3111                 return ERROR_COMMAND_NOTFOUND;
3112
3113         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3114         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3115         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_DAP_REG;
3116         h_u16_to_le(&h->cmdbuf[2], dap_port);
3117         h_u16_to_le(&h->cmdbuf[4], addr);
3118
3119         retval = stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3120         *val = le_to_h_u32(h->databuf + 4);
3121         LOG_DEBUG_IO("dap_port_read = %d, addr =  0x%x, value = 0x%x", dap_port, addr, *val);
3122         return retval;
3123 }
3124
3125 /** */
3126 static int stlink_write_dap_register(void *handle, unsigned short dap_port,
3127                         unsigned short addr, uint32_t val)
3128 {
3129         struct stlink_usb_handle_s *h = handle;
3130
3131         assert(handle != NULL);
3132
3133         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3134                 return ERROR_COMMAND_NOTFOUND;
3135
3136         LOG_DEBUG_IO("dap_write port = %d, addr = 0x%x, value = 0x%x", dap_port, addr, val);
3137         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3138         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3139         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITE_DAP_REG;
3140         h_u16_to_le(&h->cmdbuf[2], dap_port);
3141         h_u16_to_le(&h->cmdbuf[4], addr);
3142         h_u32_to_le(&h->cmdbuf[6], val);
3143         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3144 }
3145
3146 /** */
3147 struct hl_layout_api_s stlink_usb_layout_api = {
3148         /** */
3149         .open = stlink_usb_open,
3150         /** */
3151         .close = stlink_usb_close,
3152         /** */
3153         .idcode = stlink_usb_idcode,
3154         /** */
3155         .state = stlink_usb_state,
3156         /** */
3157         .reset = stlink_usb_reset,
3158         /** */
3159         .assert_srst = stlink_usb_assert_srst,
3160         /** */
3161         .run = stlink_usb_run,
3162         /** */
3163         .halt = stlink_usb_halt,
3164         /** */
3165         .step = stlink_usb_step,
3166         /** */
3167         .read_regs = stlink_usb_read_regs,
3168         /** */
3169         .read_reg = stlink_usb_read_reg,
3170         /** */
3171         .write_reg = stlink_usb_write_reg,
3172         /** */
3173         .read_mem = stlink_usb_read_mem,
3174         /** */
3175         .write_mem = stlink_usb_write_mem,
3176         /** */
3177         .write_debug_reg = stlink_usb_write_debug_reg,
3178         /** */
3179         .override_target = stlink_usb_override_target,
3180         /** */
3181         .speed = stlink_speed,
3182         /** */
3183         .config_trace = stlink_config_trace,
3184         /** */
3185         .poll_trace = stlink_usb_trace_read,
3186 };
3187
3188 /*****************************************************************************
3189  * DAP direct interface
3190  */
3191
3192 static struct stlink_usb_handle_s *stlink_dap_handle;
3193 static struct hl_interface_param_s stlink_dap_param;
3194 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
3195 static int stlink_dap_error = ERROR_OK;
3196
3197 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
3198                 uint32_t *data);
3199
3200 /** */
3201 static int stlink_dap_record_error(int error)
3202 {
3203         if (stlink_dap_error == ERROR_OK)
3204                 stlink_dap_error = error;
3205         return ERROR_OK;
3206 }
3207
3208 /** */
3209 static int stlink_dap_get_and_clear_error(void)
3210 {
3211         int retval = stlink_dap_error;
3212         stlink_dap_error = ERROR_OK;
3213         return retval;
3214 }
3215
3216 /** */
3217 static int stlink_dap_open_ap(unsigned short apsel)
3218 {
3219         int retval;
3220
3221         /* nothing to do on old versions */
3222         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
3223                 return ERROR_OK;
3224
3225         if (apsel > DP_APSEL_MAX)
3226                 return ERROR_FAIL;
3227
3228         if (test_bit(apsel, opened_ap))
3229                 return ERROR_OK;
3230
3231         retval = stlink_usb_init_access_port(stlink_dap_handle, apsel);
3232         if (retval != ERROR_OK)
3233                 return retval;
3234
3235         LOG_DEBUG("AP %d enabled", apsel);
3236         set_bit(apsel, opened_ap);
3237         return ERROR_OK;
3238 }
3239
3240 /** */
3241 static int stlink_dap_closeall_ap(void)
3242 {
3243         int retval, apsel;
3244
3245         /* nothing to do on old versions */
3246         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
3247                 return ERROR_OK;
3248
3249         for (apsel = 0; apsel <= DP_APSEL_MAX; apsel++) {
3250                 if (!test_bit(apsel, opened_ap))
3251                         continue;
3252                 retval = stlink_usb_close_access_port(stlink_dap_handle, apsel);
3253                 if (retval != ERROR_OK)
3254                         return retval;
3255                 clear_bit(apsel, opened_ap);
3256         }
3257         return ERROR_OK;
3258 }
3259
3260 /** */
3261 static int stlink_dap_reinit_interface(void)
3262 {
3263         int retval;
3264         enum stlink_mode mode;
3265
3266         /*
3267          * On JTAG only, it should be enough to call stlink_usb_reset(). But on
3268          * some firmware version it does not work as expected, and there is no
3269          * equivalent for SWD.
3270          * At least for now, to reset the interface quit from JTAG/SWD mode then
3271          * select the mode again.
3272          */
3273
3274         mode = stlink_get_mode(stlink_dap_param.transport);
3275         if (!stlink_dap_handle->reconnect_pending) {
3276                 stlink_dap_handle->reconnect_pending = true;
3277                 stlink_usb_mode_leave(stlink_dap_handle, mode);
3278         }
3279
3280         retval = stlink_usb_mode_enter(stlink_dap_handle, mode);
3281         if (retval != ERROR_OK)
3282                 return retval;
3283
3284         stlink_dap_handle->reconnect_pending = false;
3285         /* on new FW, calling mode-leave closes all the opened AP; reopen them! */
3286         if (stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT)
3287                 for (int apsel = 0; apsel <= DP_APSEL_MAX; apsel++)
3288                         if (test_bit(apsel, opened_ap)) {
3289                                 clear_bit(apsel, opened_ap);
3290                                 stlink_dap_open_ap(apsel);
3291                         }
3292         return ERROR_OK;
3293 }
3294
3295 /** */
3296 static int stlink_dap_op_connect(struct adiv5_dap *dap)
3297 {
3298         uint32_t idcode;
3299         int retval;
3300
3301         LOG_INFO("stlink_dap_op_connect(%sconnect)", dap->do_reconnect ? "re" : "");
3302
3303         /* Check if we should reset srst already when connecting, but not if reconnecting. */
3304         if (!dap->do_reconnect) {
3305                 enum reset_types jtag_reset_config = jtag_get_reset_config();
3306
3307                 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
3308                         if (jtag_reset_config & RESET_SRST_NO_GATING)
3309                                 adapter_assert_reset();
3310                         else
3311                                 LOG_WARNING("\'srst_nogate\' reset_config option is required");
3312                 }
3313         }
3314
3315         dap->do_reconnect = false;
3316         dap_invalidate_cache(dap);
3317
3318         retval = dap_dp_init(dap);
3319         if (retval != ERROR_OK) {
3320                 dap->do_reconnect = true;
3321                 return retval;
3322         }
3323
3324         retval = stlink_usb_idcode(stlink_dap_handle, &idcode);
3325         if (retval == ERROR_OK)
3326                 LOG_INFO("%s %#8.8" PRIx32,
3327                         (stlink_dap_handle->transport == HL_TRANSPORT_JTAG) ? "JTAG IDCODE" : "SWD DPIDR",
3328                         idcode);
3329         else
3330                 dap->do_reconnect = true;
3331
3332         return retval;
3333 }
3334
3335 /** */
3336 static int stlink_dap_check_reconnect(struct adiv5_dap *dap)
3337 {
3338         int retval;
3339
3340         if (!dap->do_reconnect)
3341                 return ERROR_OK;
3342
3343         retval = stlink_dap_reinit_interface();
3344         if (retval != ERROR_OK)
3345                 return retval;
3346
3347         return stlink_dap_op_connect(dap);
3348 }
3349
3350 /** */
3351 static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
3352 {
3353         /* Ignore the request */
3354         return ERROR_OK;
3355 }
3356
3357 /** */
3358 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
3359                 uint32_t *data)
3360 {
3361         uint32_t dummy;
3362         int retval;
3363
3364         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
3365                 if (reg & 0x000000F0) {
3366                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
3367                         return ERROR_COMMAND_NOTFOUND;
3368                 }
3369
3370         retval = stlink_dap_check_reconnect(dap);
3371         if (retval != ERROR_OK)
3372                 return retval;
3373
3374         data = data ? : &dummy;
3375         if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ
3376                 && stlink_dap_handle->transport == HL_TRANSPORT_JTAG) {
3377                 /* Quirk required in JTAG. Read RDBUFF to get the data */
3378                 retval = stlink_read_dap_register(stlink_dap_handle,
3379                                         STLINK_DEBUG_PORT_ACCESS, reg, &dummy);
3380                 if (retval == ERROR_OK)
3381                         retval = stlink_read_dap_register(stlink_dap_handle,
3382                                                 STLINK_DEBUG_PORT_ACCESS, DP_RDBUFF, data);
3383         } else {
3384                 retval = stlink_read_dap_register(stlink_dap_handle,
3385                                         STLINK_DEBUG_PORT_ACCESS, reg, data);
3386         }
3387
3388         return stlink_dap_record_error(retval);
3389 }
3390
3391 /** */
3392 static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
3393                 uint32_t data)
3394 {
3395         int retval;
3396
3397         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
3398                 if (reg & 0x000000F0) {
3399                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
3400                         return ERROR_COMMAND_NOTFOUND;
3401                 }
3402
3403         if (reg == DP_SELECT && (data & DP_SELECT_DPBANK) != 0) {
3404                 /* ignored if STLINK_F_HAS_DPBANKSEL, not properly managed otherwise */
3405                 LOG_DEBUG("Ignoring DPBANKSEL while write SELECT");
3406                 data &= ~DP_SELECT_DPBANK;
3407         }
3408
3409         retval = stlink_dap_check_reconnect(dap);
3410         if (retval != ERROR_OK)
3411                 return retval;
3412
3413         /* ST-Link does not like that we set CORUNDETECT */
3414         if (reg == DP_CTRL_STAT)
3415                 data &= ~CORUNDETECT;
3416
3417         retval = stlink_write_dap_register(stlink_dap_handle,
3418                                 STLINK_DEBUG_PORT_ACCESS, reg, data);
3419         return stlink_dap_record_error(retval);
3420 }
3421
3422 /** */
3423 static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
3424                 uint32_t *data)
3425 {
3426         struct adiv5_dap *dap = ap->dap;
3427         uint32_t dummy;
3428         int retval;
3429
3430         retval = stlink_dap_check_reconnect(dap);
3431         if (retval != ERROR_OK)
3432                 return retval;
3433
3434         if (reg != AP_REG_IDR) {
3435                 retval = stlink_dap_open_ap(ap->ap_num);
3436                 if (retval != ERROR_OK)
3437                         return retval;
3438         }
3439         data = data ? : &dummy;
3440         retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
3441                                  data);
3442         dap->stlink_flush_ap_write = false;
3443         return stlink_dap_record_error(retval);
3444 }
3445
3446 /** */
3447 static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
3448                 uint32_t data)
3449 {
3450         struct adiv5_dap *dap = ap->dap;
3451         int retval;
3452
3453         retval = stlink_dap_check_reconnect(dap);
3454         if (retval != ERROR_OK)
3455                 return retval;
3456
3457         retval = stlink_dap_open_ap(ap->ap_num);
3458         if (retval != ERROR_OK)
3459                 return retval;
3460
3461         retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
3462                                 data);
3463         dap->stlink_flush_ap_write = true;
3464         return stlink_dap_record_error(retval);
3465 }
3466
3467 /** */
3468 static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
3469 {
3470         LOG_WARNING("stlink_dap_op_queue_ap_abort()");
3471         return ERROR_OK;
3472 }
3473
3474 /** */
3475 static int stlink_dap_op_run(struct adiv5_dap *dap)
3476 {
3477         uint32_t ctrlstat, pwrmask;
3478         int retval, saved_retval;
3479
3480         /* Here no LOG_DEBUG. This is called continuously! */
3481
3482         /*
3483          * ST-Link returns immediately after a DAP write, without waiting for it
3484          * to complete.
3485          * Run a dummy read to DP_RDBUFF, as suggested in
3486          * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html
3487          */
3488         if (dap->stlink_flush_ap_write) {
3489                 dap->stlink_flush_ap_write = false;
3490                 retval = stlink_dap_op_queue_dp_read(dap, DP_RDBUFF, NULL);
3491                 if (retval != ERROR_OK) {
3492                         dap->do_reconnect = true;
3493                         return retval;
3494                 }
3495         }
3496
3497         saved_retval = stlink_dap_get_and_clear_error();
3498
3499         retval = stlink_dap_op_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
3500         if (retval != ERROR_OK) {
3501                 dap->do_reconnect = true;
3502                 return retval;
3503         }
3504         retval = stlink_dap_get_and_clear_error();
3505         if (retval != ERROR_OK) {
3506                 LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
3507                 dap->do_reconnect = true;
3508                 return retval;
3509         }
3510
3511         if (ctrlstat & SSTICKYERR) {
3512                 if (stlink_dap_param.transport == HL_TRANSPORT_JTAG)
3513                         retval = stlink_dap_op_queue_dp_write(dap, DP_CTRL_STAT,
3514                                         ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
3515                 else
3516                         retval = stlink_dap_op_queue_dp_write(dap, DP_ABORT, STKERRCLR);
3517                 if (retval != ERROR_OK) {
3518                         dap->do_reconnect = true;
3519                         return retval;
3520                 }
3521                 retval = stlink_dap_get_and_clear_error();
3522                 if (retval != ERROR_OK) {
3523                         dap->do_reconnect = true;
3524                         return retval;
3525                 }
3526         }
3527
3528         /* check for power lost */
3529         pwrmask = dap->dp_ctrl_stat & (CDBGPWRUPREQ | CSYSPWRUPREQ);
3530         if ((ctrlstat & pwrmask) != pwrmask)
3531                 dap->do_reconnect = true;
3532
3533         return saved_retval;
3534 }
3535
3536 /** */
3537 static void stlink_dap_op_quit(struct adiv5_dap *dap)
3538 {
3539         int retval;
3540
3541         retval = stlink_dap_closeall_ap();
3542         if (retval != ERROR_OK)
3543                 LOG_ERROR("Error closing APs");
3544 }
3545
3546 static int stlink_dap_config_trace(bool enabled,
3547                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3548                 unsigned int *trace_freq, unsigned int traceclkin_freq,
3549                 uint16_t *prescaler)
3550 {
3551         return stlink_config_trace(stlink_dap_handle, enabled, pin_protocol,
3552                                                            port_size, trace_freq, traceclkin_freq,
3553                                                            prescaler);
3554 }
3555
3556 static int stlink_dap_trace_read(uint8_t *buf, size_t *size)
3557 {
3558         return stlink_usb_trace_read(stlink_dap_handle, buf, size);
3559 }
3560
3561 /** */
3562 COMMAND_HANDLER(stlink_dap_serial_command)
3563 {
3564         LOG_DEBUG("stlink_dap_serial_command");
3565
3566         if (CMD_ARGC != 1) {
3567                 LOG_ERROR("Expected exactly one argument for \"st-link serial <serial-number>\".");
3568                 return ERROR_COMMAND_SYNTAX_ERROR;
3569         }
3570
3571         if (stlink_dap_param.serial) {
3572                 LOG_WARNING("Command \"st-link serial\" already used. Replacing previous value");
3573                 free((void *)stlink_dap_param.serial);
3574         }
3575
3576         stlink_dap_param.serial = strdup(CMD_ARGV[0]);
3577         return ERROR_OK;
3578 }
3579
3580 /** */
3581 COMMAND_HANDLER(stlink_dap_vid_pid)
3582 {
3583         unsigned int i, max_usb_ids = HLA_MAX_USB_IDS;
3584
3585         if (CMD_ARGC > max_usb_ids * 2) {
3586                 LOG_WARNING("ignoring extra IDs in vid_pid "
3587                         "(maximum is %d pairs)", max_usb_ids);
3588                 CMD_ARGC = max_usb_ids * 2;
3589         }
3590         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
3591                 LOG_WARNING("incomplete vid_pid configuration directive");
3592                 return ERROR_COMMAND_SYNTAX_ERROR;
3593         }
3594         for (i = 0; i < CMD_ARGC; i += 2) {
3595                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], stlink_dap_param.vid[i / 2]);
3596                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], stlink_dap_param.pid[i / 2]);
3597         }
3598
3599         /* null termination */
3600         stlink_dap_param.vid[i / 2] = stlink_dap_param.pid[i / 2] = 0;
3601
3602         return ERROR_OK;
3603 }
3604
3605 /** */
3606 static const struct command_registration stlink_dap_subcommand_handlers[] = {
3607         {
3608                 .name = "serial",
3609                 .handler = stlink_dap_serial_command,
3610                 .mode = COMMAND_CONFIG,
3611                 .help = "set the serial number of the adapter",
3612                 .usage = "<serial_number>",
3613         },
3614         {
3615                 .name = "vid_pid",
3616                 .handler = stlink_dap_vid_pid,
3617                 .mode = COMMAND_CONFIG,
3618                 .help = "USB VID and PID of the adapter",
3619                 .usage = "(vid pid)+",
3620         },
3621         COMMAND_REGISTRATION_DONE
3622 };
3623
3624 /** */
3625 static const struct command_registration stlink_dap_command_handlers[] = {
3626         {
3627                 .name = "st-link",
3628                 .mode = COMMAND_ANY,
3629                 .help = "perform st-link management",
3630                 .chain = stlink_dap_subcommand_handlers,
3631                 .usage = "",
3632         },
3633         COMMAND_REGISTRATION_DONE
3634 };
3635
3636 /** */
3637 static int stlink_dap_init(void)
3638 {
3639         enum reset_types jtag_reset_config = jtag_get_reset_config();
3640         int retval;
3641
3642         LOG_DEBUG("stlink_dap_init()");
3643
3644         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
3645                 if (jtag_reset_config & RESET_SRST_NO_GATING)
3646                         stlink_dap_param.connect_under_reset = true;
3647                 else
3648                         LOG_WARNING("\'srst_nogate\' reset_config option is required");
3649         }
3650
3651         if (transport_is_dapdirect_swd())
3652                 stlink_dap_param.transport = HL_TRANSPORT_SWD;
3653         else if (transport_is_dapdirect_jtag())
3654                 stlink_dap_param.transport = HL_TRANSPORT_JTAG;
3655         else {
3656                 LOG_ERROR("Unsupported transport");
3657                 return ERROR_FAIL;
3658         }
3659
3660         retval = stlink_usb_open(&stlink_dap_param, (void **)&stlink_dap_handle);
3661         if (retval != ERROR_OK)
3662                 return retval;
3663
3664         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
3665                 LOG_ERROR("ST-Link version does not support DAP direct transport");
3666                 return ERROR_FAIL;
3667         }
3668         return ERROR_OK;
3669 }
3670
3671 /** */
3672 static int stlink_dap_quit(void)
3673 {
3674         LOG_DEBUG("stlink_dap_quit()");
3675
3676         free((void *)stlink_dap_param.serial);
3677         stlink_dap_param.serial = NULL;
3678
3679         return stlink_usb_close(stlink_dap_handle);
3680 }
3681
3682 /** */
3683 static int stlink_dap_reset(int req_trst, int req_srst)
3684 {
3685         LOG_DEBUG("stlink_dap_reset(%d)", req_srst);
3686         return stlink_usb_assert_srst(stlink_dap_handle,
3687                 req_srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
3688                                  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
3689 }
3690
3691 /** */
3692 static int stlink_dap_speed(int speed)
3693 {
3694         if (speed == 0) {
3695                 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
3696                 return ERROR_JTAG_NOT_IMPLEMENTED;
3697         }
3698
3699         stlink_dap_param.initial_interface_speed = speed;
3700         stlink_speed(stlink_dap_handle, speed, false);
3701         return ERROR_OK;
3702 }
3703
3704 /** */
3705 static int stlink_dap_khz(int khz, int *jtag_speed)
3706 {
3707         if (khz == 0) {
3708                 LOG_ERROR("RCLK not supported");
3709                 return ERROR_FAIL;
3710         }
3711
3712         *jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
3713         return ERROR_OK;
3714 }
3715
3716 /** */
3717 static int stlink_dap_speed_div(int speed, int *khz)
3718 {
3719         *khz = speed;
3720         return ERROR_OK;
3721 }
3722
3723 static const struct dap_ops stlink_dap_ops = {
3724         .connect = stlink_dap_op_connect,
3725         .send_sequence = stlink_dap_op_send_sequence,
3726         .queue_dp_read = stlink_dap_op_queue_dp_read,
3727         .queue_dp_write = stlink_dap_op_queue_dp_write,
3728         .queue_ap_read = stlink_dap_op_queue_ap_read,
3729         .queue_ap_write = stlink_dap_op_queue_ap_write,
3730         .queue_ap_abort = stlink_dap_op_queue_ap_abort,
3731         .run = stlink_dap_op_run,
3732         .sync = NULL, /* optional */
3733         .quit = stlink_dap_op_quit, /* optional */
3734 };
3735
3736 static const char *const stlink_dap_transport[] = { "dapdirect_jtag", "dapdirect_swd", NULL };
3737
3738 struct adapter_driver stlink_dap_adapter_driver = {
3739         .name = "st-link",
3740         .transports = stlink_dap_transport,
3741         .commands = stlink_dap_command_handlers,
3742
3743         .init = stlink_dap_init,
3744         .quit = stlink_dap_quit,
3745         .reset = stlink_dap_reset,
3746         .speed = stlink_dap_speed,
3747         .khz = stlink_dap_khz,
3748         .speed_div = stlink_dap_speed_div,
3749         .config_trace = stlink_dap_config_trace,
3750         .poll_trace = stlink_dap_trace_read,
3751
3752         .dap_jtag_ops = &stlink_dap_ops,
3753         .dap_swd_ops = &stlink_dap_ops,
3754 };