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

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2020 by Tarek Bochkati                                  *
   5  *   Tarek Bochkati <tarek.bouchkati@gmail.com>                            *
   6  *                                                                         *
   7  *   SWIM contributions by Ake Rehnman                                     *
   8  *   Copyright (C) 2017  Ake Rehnman                                       *
   9  *   ake.rehnman(at)gmail.com                                              *
  10  *                                                                         *
  11  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
  12  *   Mathias Kuester <kesmtp@freenet.de>                                   *
  13  *                                                                         *
  14  *   Copyright (C) 2012 by Spencer Oliver                                  *
  15  *   spen@spen-soft.co.uk                                                  *
  16  *                                                                         *
  17  *   This code is based on https://github.com/texane/stlink                *
  18  ***************************************************************************/
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 /* project specific includes */
  25 #include <helper/align.h>
  26 #include <helper/binarybuffer.h>
  27 #include <helper/bits.h>
  28 #include <helper/system.h>
  29 #include <helper/time_support.h>
  30 #include <jtag/adapter.h>
  31 #include <jtag/interface.h>
  32 #include <jtag/hla/hla_layout.h>
  33 #include <jtag/hla/hla_transport.h>
  34 #include <jtag/hla/hla_interface.h>
  35 #include <jtag/swim.h>
  36 #include <target/arm_adi_v5.h>
  37 #include <target/target.h>
  38 #include <transport/transport.h>
  39
  40 #include <target/cortex_m.h>
  41
  42 #include <helper/system.h>
  43
  44 #ifdef HAVE_ARPA_INET_H
  45 #include <arpa/inet.h>
  46 #endif
  47
  48 #ifdef HAVE_NETINET_TCP_H
  49 #include <netinet/tcp.h>
  50 #endif
  51
  52 #include "libusb_helper.h"
  53
  54 #ifdef HAVE_LIBUSB1
  55 #define USE_LIBUSB_ASYNCIO
  56 #endif
  57
  58 #define STLINK_SERIAL_LEN 24
  59
  60 #define ENDPOINT_IN  0x80
  61 #define ENDPOINT_OUT 0x00
  62
  63 #define STLINK_WRITE_TIMEOUT  (LIBUSB_TIMEOUT_MS)
  64 #define STLINK_READ_TIMEOUT   (LIBUSB_TIMEOUT_MS)
  65
  66 #define STLINK_RX_EP          (1|ENDPOINT_IN)
  67 #define STLINK_TX_EP          (2|ENDPOINT_OUT)
  68 #define STLINK_TRACE_EP       (3|ENDPOINT_IN)
  69
  70 #define STLINK_V2_1_TX_EP     (1|ENDPOINT_OUT)
  71 #define STLINK_V2_1_TRACE_EP  (2|ENDPOINT_IN)
  72
  73 #define STLINK_SG_SIZE        (31)
  74 #define STLINK_DATA_SIZE      (6144)
  75 #define STLINK_CMD_SIZE_V2    (16)
  76 #define STLINK_CMD_SIZE_V1    (10)
  77
  78 #define STLINK_V1_PID         (0x3744)
  79 #define STLINK_V2_PID         (0x3748)
  80 #define STLINK_V2_1_PID       (0x374B)
  81 #define STLINK_V2_1_NO_MSD_PID  (0x3752)
  82 #define STLINK_V3_USBLOADER_PID (0x374D)
  83 #define STLINK_V3E_PID          (0x374E)
  84 #define STLINK_V3S_PID          (0x374F)
  85 #define STLINK_V3_2VCP_PID      (0x3753)
  86 #define STLINK_V3E_NO_MSD_PID   (0x3754)
  87
  88 /*
  89  * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
  90  * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
  91  * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes from FW V3J6.
  92  *
  93  * For 16 and 32bit read/writes stlink handles USB packet split and the limit
  94  * is the internal buffer size of 6144 bytes.
  95  * TODO: override ADIv5 layer's tar_autoincr_block that limits the transfer
  96  * to 1024 or 4096 bytes
  97  */
  98 #define STLINK_MAX_RW8          (64)
  99 #define STLINKV3_MAX_RW8        (512)
 100 #define STLINK_MAX_RW16_32      STLINK_DATA_SIZE
 101 #define STLINK_SWIM_DATA_SIZE   STLINK_DATA_SIZE
 102
 103 /* "WAIT" responses will be retried (with exponential backoff) at
 104  * most this many times before failing to caller.
 105  */
 106 #define MAX_WAIT_RETRIES 8
 107
 108 /* HLA is currently limited at AP#0 and no control on CSW */
 109 #define STLINK_HLA_AP_NUM       0
 110 #define STLINK_HLA_CSW          0
 111
 112 enum stlink_jtag_api_version {
 113         STLINK_JTAG_API_V1 = 1,
 114         STLINK_JTAG_API_V2,
 115         STLINK_JTAG_API_V3,
 116 };
 117
 118 enum stlink_mode {
 119         STLINK_MODE_UNKNOWN = 0,
 120         STLINK_MODE_DFU,
 121         STLINK_MODE_MASS,
 122         STLINK_MODE_DEBUG_JTAG,
 123         STLINK_MODE_DEBUG_SWD,
 124         STLINK_MODE_DEBUG_SWIM
 125 };
 126
 127 /** */
 128 struct stlink_usb_version {
 129         /** */
 130         int stlink;
 131         /** */
 132         int jtag;
 133         /** */
 134         int swim;
 135         /** jtag api version supported */
 136         enum stlink_jtag_api_version jtag_api;
 137         /** one bit for each feature supported. See macros STLINK_F_* */
 138         uint32_t flags;
 139 };
 140
 141 struct stlink_usb_priv_s {
 142         /** */
 143         struct libusb_device_handle *fd;
 144         /** */
 145         struct libusb_transfer *trans;
 146 };
 147
 148 struct stlink_tcp_version {
 149         uint32_t api;
 150         uint32_t major;
 151         uint32_t minor;
 152         uint32_t build;
 153 };
 154
 155 struct stlink_tcp_priv_s {
 156         /** */
 157         int fd;
 158         /** */
 159         bool connected;
 160         /** */
 161         uint32_t device_id;
 162         /** */
 163         uint32_t connect_id;
 164         /** */
 165         uint8_t *send_buf;
 166         /** */
 167         uint8_t *recv_buf;
 168         /** */
 169         struct stlink_tcp_version version;
 170 };
 171
 172 struct stlink_backend_s {
 173         /** */
 174         int (*open)(void *handle, struct hl_interface_param_s *param);
 175         /** */
 176         int (*close)(void *handle);
 177         /** */
 178         int (*xfer_noerrcheck)(void *handle, const uint8_t *buf, int size);
 179         /** */
 180         int (*read_trace)(void *handle, const uint8_t *buf, int size);
 181 };
 182
 183 /* TODO: make queue size dynamic */
 184 /* TODO: don't allocate queue for HLA */
 185 #define MAX_QUEUE_DEPTH (4096)
 186
 187 enum queue_cmd {
 188         CMD_DP_READ = 1,
 189         CMD_DP_WRITE,
 190
 191         CMD_AP_READ,
 192         CMD_AP_WRITE,
 193
 194         /*
 195          * encode the bytes size in the enum's value. This makes easy to extract it
 196          * with a simple logic AND, by using the macro CMD_MEM_AP_2_SIZE() below
 197          */
 198         CMD_MEM_AP_READ8   = 0x10 + 1,
 199         CMD_MEM_AP_READ16  = 0x10 + 2,
 200         CMD_MEM_AP_READ32  = 0x10 + 4,
 201
 202         CMD_MEM_AP_WRITE8  = 0x20 + 1,
 203         CMD_MEM_AP_WRITE16 = 0x20 + 2,
 204         CMD_MEM_AP_WRITE32 = 0x20 + 4,
 205 };
 206
 207 #define CMD_MEM_AP_2_SIZE(cmd) ((cmd) & 7)
 208
 209 struct dap_queue {
 210         enum queue_cmd cmd;
 211         union {
 212                 struct dp_r {
 213                         unsigned int reg;
 214                         struct adiv5_dap *dap;
 215                         uint32_t *p_data;
 216                 } dp_r;
 217                 struct dp_w {
 218                         unsigned int reg;
 219                         struct adiv5_dap *dap;
 220                         uint32_t data;
 221                 } dp_w;
 222                 struct ap_r {
 223                         unsigned int reg;
 224                         struct adiv5_ap *ap;
 225                         uint32_t *p_data;
 226                 } ap_r;
 227                 struct ap_w {
 228                         unsigned int reg;
 229                         struct adiv5_ap *ap;
 230                         uint32_t data;
 231                         bool changes_csw_default;
 232                 } ap_w;
 233                 struct mem_ap {
 234                         uint32_t addr;
 235                         struct adiv5_ap *ap;
 236                         union {
 237                                 uint32_t *p_data;
 238                                 uint32_t data;
 239                         };
 240                         uint32_t csw;
 241                 } mem_ap;
 242         };
 243 };
 244
 245 /** */
 246 struct stlink_usb_handle_s {
 247         /** */
 248         struct stlink_backend_s *backend;
 249         /** */
 250         union {
 251                 struct stlink_usb_priv_s usb_backend_priv;
 252                 struct stlink_tcp_priv_s tcp_backend_priv;
 253         };
 254         /** */
 255         uint8_t rx_ep;
 256         /** */
 257         uint8_t tx_ep;
 258         /** */
 259         uint8_t trace_ep;
 260         /** */
 261         uint8_t *cmdbuf;
 262         /** */
 263         uint8_t cmdidx;
 264         /** */
 265         uint8_t direction;
 266         /** */
 267         uint8_t *databuf;
 268         /** */
 269         uint32_t max_mem_packet;
 270         /** */
 271         enum stlink_mode st_mode;
 272         /** */
 273         struct stlink_usb_version version;
 274         /** */
 275         uint16_t vid;
 276         /** */
 277         uint16_t pid;
 278         /** */
 279         struct {
 280                 /** whether SWO tracing is enabled or not */
 281                 bool enabled;
 282                 /** trace module source clock */
 283                 uint32_t source_hz;
 284         } trace;
 285         /** reconnect is needed next time we try to query the
 286          * status */
 287         bool reconnect_pending;
 288         /** queue of dap_direct operations */
 289         struct dap_queue queue[MAX_QUEUE_DEPTH];
 290         /** first element available in the queue */
 291         unsigned int queue_index;
 292 };
 293
 294 /** */
 295 static inline int stlink_usb_open(void *handle, struct hl_interface_param_s *param)
 296 {
 297         struct stlink_usb_handle_s *h = handle;
 298         return h->backend->open(handle, param);
 299 }
 300
 301 /** */
 302 static inline int stlink_usb_close(void *handle)
 303 {
 304         struct stlink_usb_handle_s *h = handle;
 305         return h->backend->close(handle);
 306 }
 307 /** */
 308 static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
 309 {
 310         struct stlink_usb_handle_s *h = handle;
 311         return h->backend->xfer_noerrcheck(handle, buf, size);
 312 }
 313
 314 #define STLINK_SWIM_ERR_OK             0x00
 315 #define STLINK_SWIM_BUSY               0x01
 316 #define STLINK_DEBUG_ERR_OK            0x80
 317 #define STLINK_DEBUG_ERR_FAULT         0x81
 318 #define STLINK_SWD_AP_WAIT             0x10
 319 #define STLINK_SWD_AP_FAULT            0x11
 320 #define STLINK_SWD_AP_ERROR            0x12
 321 #define STLINK_SWD_AP_PARITY_ERROR     0x13
 322 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09
 323 #define STLINK_JTAG_WRITE_ERROR        0x0c
 324 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 325 #define STLINK_SWD_DP_WAIT             0x14
 326 #define STLINK_SWD_DP_FAULT            0x15
 327 #define STLINK_SWD_DP_ERROR            0x16
 328 #define STLINK_SWD_DP_PARITY_ERROR     0x17
 329
 330 #define STLINK_SWD_AP_WDATA_ERROR      0x18
 331 #define STLINK_SWD_AP_STICKY_ERROR     0x19
 332 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
 333
 334 #define STLINK_BAD_AP_ERROR            0x1d
 335
 336 #define STLINK_CORE_RUNNING            0x80
 337 #define STLINK_CORE_HALTED             0x81
 338 #define STLINK_CORE_STAT_UNKNOWN       -1
 339
 340 #define STLINK_GET_VERSION             0xF1
 341 #define STLINK_DEBUG_COMMAND           0xF2
 342 #define STLINK_DFU_COMMAND             0xF3
 343 #define STLINK_SWIM_COMMAND            0xF4
 344 #define STLINK_GET_CURRENT_MODE        0xF5
 345 #define STLINK_GET_TARGET_VOLTAGE      0xF7
 346
 347 #define STLINK_DEV_DFU_MODE            0x00
 348 #define STLINK_DEV_MASS_MODE           0x01
 349 #define STLINK_DEV_DEBUG_MODE          0x02
 350 #define STLINK_DEV_SWIM_MODE           0x03
 351 #define STLINK_DEV_BOOTLOADER_MODE     0x04
 352 #define STLINK_DEV_UNKNOWN_MODE        -1
 353
 354 #define STLINK_DFU_EXIT                0x07
 355
 356 /*
 357         STLINK_SWIM_ENTER_SEQ
 358         1.3ms low then 750Hz then 1.5kHz
 359
 360         STLINK_SWIM_GEN_RST
 361         STM8 DM pulls reset pin low 50us
 362
 363         STLINK_SWIM_SPEED
 364         uint8_t (0=low|1=high)
 365
 366         STLINK_SWIM_WRITEMEM
 367         uint16_t length
 368         uint32_t address
 369
 370         STLINK_SWIM_RESET
 371         send synchronization seq (16us low, response 64 clocks low)
 372 */
 373 #define STLINK_SWIM_ENTER                  0x00
 374 #define STLINK_SWIM_EXIT                   0x01
 375 #define STLINK_SWIM_READ_CAP               0x02
 376 #define STLINK_SWIM_SPEED                  0x03
 377 #define STLINK_SWIM_ENTER_SEQ              0x04
 378 #define STLINK_SWIM_GEN_RST                0x05
 379 #define STLINK_SWIM_RESET                  0x06
 380 #define STLINK_SWIM_ASSERT_RESET           0x07
 381 #define STLINK_SWIM_DEASSERT_RESET         0x08
 382 #define STLINK_SWIM_READSTATUS             0x09
 383 #define STLINK_SWIM_WRITEMEM               0x0a
 384 #define STLINK_SWIM_READMEM                0x0b
 385 #define STLINK_SWIM_READBUF                0x0c
 386
 387 #define STLINK_DEBUG_GETSTATUS             0x01
 388 #define STLINK_DEBUG_FORCEDEBUG            0x02
 389 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
 390 #define STLINK_DEBUG_APIV1_READALLREGS     0x04
 391 #define STLINK_DEBUG_APIV1_READREG         0x05
 392 #define STLINK_DEBUG_APIV1_WRITEREG        0x06
 393 #define STLINK_DEBUG_READMEM_32BIT         0x07
 394 #define STLINK_DEBUG_WRITEMEM_32BIT        0x08
 395 #define STLINK_DEBUG_RUNCORE               0x09
 396 #define STLINK_DEBUG_STEPCORE              0x0a
 397 #define STLINK_DEBUG_APIV1_SETFP           0x0b
 398 #define STLINK_DEBUG_READMEM_8BIT          0x0c
 399 #define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
 400 #define STLINK_DEBUG_APIV1_CLEARFP         0x0e
 401 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 402 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 403
 404 #define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
 405 #define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
 406 #define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
 407
 408 #define STLINK_DEBUG_APIV1_ENTER           0x20
 409 #define STLINK_DEBUG_EXIT                  0x21
 410 #define STLINK_DEBUG_READCOREID            0x22
 411
 412 #define STLINK_DEBUG_APIV2_ENTER           0x30
 413 #define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
 414 #define STLINK_DEBUG_APIV2_RESETSYS        0x32
 415 #define STLINK_DEBUG_APIV2_READREG         0x33
 416 #define STLINK_DEBUG_APIV2_WRITEREG        0x34
 417 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
 418 #define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
 419
 420 #define STLINK_DEBUG_APIV2_READALLREGS     0x3A
 421 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 422 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 423
 424 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
 425
 426 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 427 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 428 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 429 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
 430 #define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
 431 #define STLINK_DEBUG_APIV2_READ_DAP_REG    0x45
 432 #define STLINK_DEBUG_APIV2_WRITE_DAP_REG   0x46
 433 #define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
 434 #define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
 435
 436 #define STLINK_DEBUG_APIV2_INIT_AP         0x4B
 437 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
 438
 439 #define STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC         0x50
 440 #define STLINK_DEBUG_APIV2_RW_MISC_OUT     0x51
 441 #define STLINK_DEBUG_APIV2_RW_MISC_IN      0x52
 442
 443 #define STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC          0x54
 444
 445 #define STLINK_APIV3_SET_COM_FREQ           0x61
 446 #define STLINK_APIV3_GET_COM_FREQ           0x62
 447
 448 #define STLINK_APIV3_GET_VERSION_EX         0xFB
 449
 450 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 451 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 452 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 453
 454 #define STLINK_DEBUG_PORT_ACCESS            0xffff
 455
 456 #define STLINK_TRACE_SIZE               4096
 457 #define STLINK_TRACE_MAX_HZ             2000000
 458 #define STLINK_V3_TRACE_MAX_HZ          24000000
 459
 460 #define STLINK_V3_MAX_FREQ_NB               10
 461
 462 #define REQUEST_SENSE        0x03
 463 #define REQUEST_SENSE_LENGTH 18
 464
 465 /* STLINK TCP commands */
 466 #define STLINK_TCP_CMD_REFRESH_DEVICE_LIST   0x00
 467 #define STLINK_TCP_CMD_GET_NB_DEV            0x01
 468 #define STLINK_TCP_CMD_GET_DEV_INFO          0x02
 469 #define STLINK_TCP_CMD_OPEN_DEV              0x03
 470 #define STLINK_TCP_CMD_CLOSE_DEV             0x04
 471 #define STLINK_TCP_CMD_SEND_USB_CMD          0x05
 472 #define STLINK_TCP_CMD_GET_SERVER_VERSION    0x06
 473 #define STLINK_TCP_CMD_GET_NB_OF_DEV_CLIENTS 0x07
 474
 475 /* STLINK TCP constants */
 476 #define OPENOCD_STLINK_TCP_API_VERSION       1
 477 #define STLINK_TCP_REQUEST_WRITE             0
 478 #define STLINK_TCP_REQUEST_READ              1
 479 #define STLINK_TCP_REQUEST_READ_SWO          3
 480 #define STLINK_TCP_SS_SIZE                   4
 481 #define STLINK_TCP_USB_CMD_SIZE              32
 482 #define STLINK_TCP_SERIAL_SIZE               32
 483 #define STLINK_TCP_SEND_BUFFER_SIZE          10240
 484 #define STLINK_TCP_RECV_BUFFER_SIZE          10240
 485
 486 /* STLINK TCP command status */
 487 #define STLINK_TCP_SS_OK                     0x00000001
 488 #define STLINK_TCP_SS_MEMORY_PROBLEM         0x00001000
 489 #define STLINK_TCP_SS_TIMEOUT                0x00001001
 490 #define STLINK_TCP_SS_BAD_PARAMETER          0x00001002
 491 #define STLINK_TCP_SS_OPEN_ERR               0x00001003
 492 #define STLINK_TCP_SS_TRUNCATED_DATA         0x00001052
 493 #define STLINK_TCP_SS_CMD_NOT_AVAILABLE      0x00001053
 494 #define STLINK_TCP_SS_TCP_ERROR              0x00002001
 495 #define STLINK_TCP_SS_TCP_CANT_CONNECT       0x00002002
 496 #define STLINK_TCP_SS_TCP_CLOSE_ERROR        0x00002003
 497 #define STLINK_TCP_SS_TCP_BUSY               0x00002004
 498 #define STLINK_TCP_SS_WIN32_ERROR            0x00010000
 499
 500 /*
 501  * Map the relevant features, quirks and workaround for specific firmware
 502  * version of stlink
 503  */
 504 #define STLINK_F_HAS_TRACE              BIT(0)  /* v2>=j13 || v3     */
 505 #define STLINK_F_HAS_GETLASTRWSTATUS2   BIT(1)  /* v2>=j15 || v3     */
 506 #define STLINK_F_HAS_SWD_SET_FREQ       BIT(2)  /* v2>=j22           */
 507 #define STLINK_F_HAS_JTAG_SET_FREQ      BIT(3)  /* v2>=j24           */
 508 #define STLINK_F_QUIRK_JTAG_DP_READ     BIT(4)  /* v2>=j24 && v2<j32 */
 509 #define STLINK_F_HAS_DAP_REG            BIT(5)  /* v2>=j24 || v3     */
 510 #define STLINK_F_HAS_MEM_16BIT          BIT(6)  /* v2>=j26 || v3     */
 511 #define STLINK_F_HAS_AP_INIT            BIT(7)  /* v2>=j28 || v3     */
 512 #define STLINK_F_FIX_CLOSE_AP           BIT(8)  /* v2>=j29 || v3     */
 513 #define STLINK_F_HAS_DPBANKSEL          BIT(9)  /* v2>=j32 || v3>=j2 */
 514 #define STLINK_F_HAS_RW8_512BYTES       BIT(10) /*            v3>=j6 */
 515
 516 /* aliases */
 517 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
 518 #define STLINK_F_HAS_FPU_REG            STLINK_F_HAS_GETLASTRWSTATUS2
 519 #define STLINK_F_HAS_MEM_WR_NO_INC      STLINK_F_HAS_MEM_16BIT
 520 #define STLINK_F_HAS_MEM_RD_NO_INC      STLINK_F_HAS_DPBANKSEL
 521 #define STLINK_F_HAS_RW_MISC            STLINK_F_HAS_DPBANKSEL
 522 #define STLINK_F_HAS_CSW                STLINK_F_HAS_DPBANKSEL
 523
 524 #define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
 525
 526 struct speed_map {
 527         int speed;
 528         int speed_divisor;
 529 };
 530
 531 /* SWD clock speed */
 532 static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 533         {4000, 0},
 534         {1800, 1}, /* default */
 535         {1200, 2},
 536         {950,  3},
 537         {480,  7},
 538         {240, 15},
 539         {125, 31},
 540         {100, 40},
 541         {50,  79},
 542         {25, 158},
 543         {15, 265},
 544         {5,  798}
 545 };
 546
 547 /* JTAG clock speed */
 548 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
 549         {9000,  4},
 550         {4500,  8},
 551         {2250, 16},
 552         {1125, 32}, /* default */
 553         {562,  64},
 554         {281, 128},
 555         {140, 256}
 556 };
 557
 558 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 559 static int stlink_swim_status(void *handle);
 560 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size);
 561 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map);
 562 static int stlink_speed(void *handle, int khz, bool query);
 563 static int stlink_usb_open_ap(void *handle, unsigned short apsel);
 564
 565 /** */
 566 static unsigned int stlink_usb_block(void *handle)
 567 {
 568         struct stlink_usb_handle_s *h = handle;
 569
 570         assert(handle);
 571
 572         if (h->version.flags & STLINK_F_HAS_RW8_512BYTES)
 573                 return STLINKV3_MAX_RW8;
 574         else
 575                 return STLINK_MAX_RW8;
 576 }
 577
 578 #ifdef USE_LIBUSB_ASYNCIO
 579
 580 static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
 581 {
 582         int *completed = transfer->user_data;
 583         *completed = 1;
 584         /* caller interprets result and frees transfer */
 585 }
 586
 587
 588 static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
 589 {
 590         int r, *completed = transfer->user_data;
 591
 592         while (!*completed) {
 593                 r = jtag_libusb_handle_events_completed(completed);
 594                 if (r < 0) {
 595                         if (r == LIBUSB_ERROR_INTERRUPTED)
 596                                 continue;
 597                         libusb_cancel_transfer(transfer);
 598                         continue;
 599                 }
 600         }
 601 }
 602
 603
 604 static int transfer_error_status(const struct libusb_transfer *transfer)
 605 {
 606         int r = 0;
 607
 608         switch (transfer->status) {
 609                 case LIBUSB_TRANSFER_COMPLETED:
 610                         r = 0;
 611                         break;
 612                 case LIBUSB_TRANSFER_TIMED_OUT:
 613                         r = LIBUSB_ERROR_TIMEOUT;
 614                         break;
 615                 case LIBUSB_TRANSFER_STALL:
 616                         r = LIBUSB_ERROR_PIPE;
 617                         break;
 618                 case LIBUSB_TRANSFER_OVERFLOW:
 619                         r = LIBUSB_ERROR_OVERFLOW;
 620                         break;
 621                 case LIBUSB_TRANSFER_NO_DEVICE:
 622                         r = LIBUSB_ERROR_NO_DEVICE;
 623                         break;
 624                 case LIBUSB_TRANSFER_ERROR:
 625                 case LIBUSB_TRANSFER_CANCELLED:
 626                         r = LIBUSB_ERROR_IO;
 627                         break;
 628                 default:
 629                         r = LIBUSB_ERROR_OTHER;
 630                         break;
 631         }
 632
 633         return r;
 634 }
 635
 636 struct jtag_xfer {
 637         int ep;
 638         uint8_t *buf;
 639         size_t size;
 640         /* Internal */
 641         int retval;
 642         int completed;
 643         size_t transfer_size;
 644         struct libusb_transfer *transfer;
 645 };
 646
 647 static int jtag_libusb_bulk_transfer_n(
 648                 struct libusb_device_handle *dev_handle,
 649                 struct jtag_xfer *transfers,
 650                 size_t n_transfers,
 651                 int timeout)
 652 {
 653         int retval = 0;
 654         int returnval = ERROR_OK;
 655
 656
 657         for (size_t i = 0; i < n_transfers; ++i) {
 658                 transfers[i].retval = 0;
 659                 transfers[i].completed = 0;
 660                 transfers[i].transfer_size = 0;
 661                 transfers[i].transfer = libusb_alloc_transfer(0);
 662
 663                 if (!transfers[i].transfer) {
 664                         for (size_t j = 0; j < i; ++j)
 665                                 libusb_free_transfer(transfers[j].transfer);
 666
 667                         LOG_DEBUG("ERROR, failed to alloc usb transfers");
 668                         for (size_t k = 0; k < n_transfers; ++k)
 669                                 transfers[k].retval = LIBUSB_ERROR_NO_MEM;
 670                         return ERROR_FAIL;
 671                 }
 672         }
 673
 674         for (size_t i = 0; i < n_transfers; ++i) {
 675                 libusb_fill_bulk_transfer(
 676                                 transfers[i].transfer,
 677                                 dev_handle,
 678                                 transfers[i].ep, transfers[i].buf, transfers[i].size,
 679                                 sync_transfer_cb, &transfers[i].completed, timeout);
 680                 transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
 681
 682                 retval = libusb_submit_transfer(transfers[i].transfer);
 683                 if (retval < 0) {
 684                         LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
 685
 686                         /* Probably no point continuing to submit transfers once a submission fails.
 687                          * As a result, tag all remaining transfers as errors.
 688                          */
 689                         for (size_t j = i; j < n_transfers; ++j)
 690                                 transfers[j].retval = retval;
 691
 692                         returnval = ERROR_FAIL;
 693                         break;
 694                 }
 695         }
 696
 697         /* Wait for every submitted USB transfer to complete.
 698         */
 699         for (size_t i = 0; i < n_transfers; ++i) {
 700                 if (transfers[i].retval == 0) {
 701                         sync_transfer_wait_for_completion(transfers[i].transfer);
 702
 703                         retval = transfer_error_status(transfers[i].transfer);
 704                         if (retval) {
 705                                 returnval = ERROR_FAIL;
 706                                 transfers[i].retval = retval;
 707                                 LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
 708                         } else {
 709                                 /* Assuming actual_length is only valid if there is no transfer error.
 710                                  */
 711                                 transfers[i].transfer_size = transfers[i].transfer->actual_length;
 712                         }
 713                 }
 714
 715                 libusb_free_transfer(transfers[i].transfer);
 716                 transfers[i].transfer = NULL;
 717         }
 718
 719         return returnval;
 720 }
 721
 722 #endif
 723
 724
 725 /** */
 726 static int stlink_usb_xfer_v1_get_status(void *handle)
 727 {
 728         struct stlink_usb_handle_s *h = handle;
 729         int tr, ret;
 730
 731         assert(handle);
 732
 733         /* read status */
 734         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 735
 736         ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->rx_ep, (char *)h->cmdbuf, 13,
 737                                     STLINK_READ_TIMEOUT, &tr);
 738         if (ret || tr != 13)
 739                 return ERROR_FAIL;
 740
 741         uint32_t t1;
 742
 743         t1 = buf_get_u32(h->cmdbuf, 0, 32);
 744
 745         /* check for USBS */
 746         if (t1 != 0x53425355)
 747                 return ERROR_FAIL;
 748         /*
 749          * CSW status:
 750          * 0 success
 751          * 1 command failure
 752          * 2 phase error
 753          */
 754         if (h->cmdbuf[12] != 0)
 755                 return ERROR_FAIL;
 756
 757         return ERROR_OK;
 758 }
 759
 760 #ifdef USE_LIBUSB_ASYNCIO
 761 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 762 {
 763         struct stlink_usb_handle_s *h = handle;
 764
 765         assert(handle);
 766
 767         size_t n_transfers = 0;
 768         struct jtag_xfer transfers[2];
 769
 770         memset(transfers, 0, sizeof(transfers));
 771
 772         transfers[0].ep = h->tx_ep;
 773         transfers[0].buf = h->cmdbuf;
 774         transfers[0].size = cmdsize;
 775
 776         ++n_transfers;
 777
 778         if (h->direction == h->tx_ep && size) {
 779                 transfers[1].ep = h->tx_ep;
 780                 transfers[1].buf = (uint8_t *)buf;
 781                 transfers[1].size = size;
 782
 783                 ++n_transfers;
 784         } else if (h->direction == h->rx_ep && size) {
 785                 transfers[1].ep = h->rx_ep;
 786                 transfers[1].buf = (uint8_t *)buf;
 787                 transfers[1].size = size;
 788
 789                 ++n_transfers;
 790         }
 791
 792         return jtag_libusb_bulk_transfer_n(
 793                         h->usb_backend_priv.fd,
 794                         transfers,
 795                         n_transfers,
 796                         STLINK_WRITE_TIMEOUT);
 797 }
 798 #else
 799 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 800 {
 801         struct stlink_usb_handle_s *h = handle;
 802         int tr, ret;
 803
 804         assert(handle);
 805
 806         ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)h->cmdbuf,
 807                                      cmdsize, STLINK_WRITE_TIMEOUT, &tr);
 808         if (ret || tr != cmdsize)
 809                 return ERROR_FAIL;
 810
 811         if (h->direction == h->tx_ep && size) {
 812                 ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)buf,
 813                                              size, STLINK_WRITE_TIMEOUT, &tr);
 814                 if (ret || tr != size) {
 815                         LOG_DEBUG("bulk write failed");
 816                         return ERROR_FAIL;
 817                 }
 818         } else if (h->direction == h->rx_ep && size) {
 819                 ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->rx_ep, (char *)buf,
 820                                             size, STLINK_READ_TIMEOUT, &tr);
 821                 if (ret || tr != size) {
 822                         LOG_DEBUG("bulk read failed");
 823                         return ERROR_FAIL;
 824                 }
 825         }
 826
 827         return ERROR_OK;
 828 }
 829 #endif
 830
 831 /** */
 832 static int stlink_usb_xfer_v1_get_sense(void *handle)
 833 {
 834         int res;
 835         struct stlink_usb_handle_s *h = handle;
 836
 837         assert(handle);
 838
 839         stlink_usb_init_buffer(handle, h->rx_ep, 16);
 840
 841         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
 842         h->cmdbuf[h->cmdidx++] = 0;
 843         h->cmdbuf[h->cmdidx++] = 0;
 844         h->cmdbuf[h->cmdidx++] = 0;
 845         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
 846
 847         res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
 848
 849         if (res != ERROR_OK)
 850                 return res;
 851
 852         if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
 853                 return ERROR_FAIL;
 854
 855         return ERROR_OK;
 856 }
 857
 858 /** */
 859 static int stlink_usb_usb_read_trace(void *handle, const uint8_t *buf, int size)
 860 {
 861         struct stlink_usb_handle_s *h = handle;
 862         int tr, ret;
 863
 864         ret = jtag_libusb_bulk_read(h->usb_backend_priv.fd, h->trace_ep, (char *)buf, size,
 865                                     STLINK_READ_TIMEOUT, &tr);
 866         if (ret || tr != size) {
 867                 LOG_ERROR("bulk trace read failed");
 868                 return ERROR_FAIL;
 869         }
 870
 871         return ERROR_OK;
 872 }
 873
 874 /*
 875         transfers block in cmdbuf
 876         <size> indicates number of bytes in the following
 877         data phase.
 878         Ignore the (eventual) error code in the received packet.
 879 */
 880 static int stlink_usb_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
 881 {
 882         int err, cmdsize = STLINK_CMD_SIZE_V2;
 883         struct stlink_usb_handle_s *h = handle;
 884
 885         assert(handle);
 886
 887         if (h->version.stlink == 1) {
 888                 cmdsize = STLINK_SG_SIZE;
 889                 /* put length in bCBWCBLength */
 890                 h->cmdbuf[14] = h->cmdidx-15;
 891         }
 892
 893         err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 894
 895         if (err != ERROR_OK)
 896                 return err;
 897
 898         if (h->version.stlink == 1) {
 899                 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
 900                         /* check csw status */
 901                         if (h->cmdbuf[12] == 1) {
 902                                 LOG_DEBUG("get sense");
 903                                 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
 904                                         return ERROR_FAIL;
 905                         }
 906                         return ERROR_FAIL;
 907                 }
 908         }
 909
 910         return ERROR_OK;
 911 }
 912
 913
 914 static int stlink_tcp_send_cmd(void *handle, int send_size, int recv_size, bool check_tcp_status)
 915 {
 916         struct stlink_usb_handle_s *h = handle;
 917
 918         assert(handle);
 919
 920         /* send the TCP command */
 921         int sent_size = send(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.send_buf, send_size, 0);
 922         if (sent_size != send_size) {
 923                 LOG_ERROR("failed to send USB CMD");
 924                 if (sent_size == -1)
 925                         LOG_DEBUG("socket send error: %s (errno %d)", strerror(errno), errno);
 926                 else
 927                         LOG_DEBUG("sent size %d (expected %d)", sent_size, send_size);
 928                 return ERROR_FAIL;
 929         }
 930
 931         /* read the TCP response */
 932         int retval = ERROR_OK;
 933         int remaining_bytes = recv_size;
 934         uint8_t *recv_buf = h->tcp_backend_priv.recv_buf;
 935         const int64_t timeout = timeval_ms() + 1000; /* 1 second */
 936
 937         while (remaining_bytes > 0) {
 938                 if (timeval_ms() > timeout) {
 939                         LOG_DEBUG("received size %d (expected %d)", recv_size - remaining_bytes, recv_size);
 940                         retval = ERROR_TIMEOUT_REACHED;
 941                         break;
 942                 }
 943
 944                 keep_alive();
 945                 int received = recv(h->tcp_backend_priv.fd, (void *)recv_buf, remaining_bytes, 0);
 946
 947                 if (received == -1) {
 948                         LOG_DEBUG("socket recv error: %s (errno %d)", strerror(errno), errno);
 949                         retval = ERROR_FAIL;
 950                         break;
 951                 }
 952
 953                 recv_buf += received;
 954                 remaining_bytes -= received;
 955         }
 956
 957         if (retval != ERROR_OK) {
 958                 LOG_ERROR("failed to receive USB CMD response");
 959                 return retval;
 960         }
 961
 962         if (check_tcp_status) {
 963                 uint32_t tcp_ss = le_to_h_u32(h->tcp_backend_priv.recv_buf);
 964                 if (tcp_ss != STLINK_TCP_SS_OK) {
 965                         if (tcp_ss == STLINK_TCP_SS_TCP_BUSY) {
 966                                 LOG_DEBUG("TCP busy");
 967                                 return ERROR_WAIT;
 968                         }
 969
 970                         LOG_ERROR("TCP error status 0x%X", tcp_ss);
 971                         return ERROR_FAIL;
 972                 }
 973         }
 974
 975         return ERROR_OK;
 976 }
 977
 978 /** */
 979 static int stlink_tcp_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
 980 {
 981         struct stlink_usb_handle_s *h = handle;
 982
 983         int send_size = STLINK_TCP_USB_CMD_SIZE;
 984         int recv_size = STLINK_TCP_SS_SIZE;
 985
 986         assert(handle);
 987
 988         /* prepare the TCP command */
 989         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_SEND_USB_CMD;
 990         memset(&h->tcp_backend_priv.send_buf[1], 0, 3); /* reserved for alignment and future use, must be zero */
 991         h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
 992         /* tcp_backend_priv.send_buf[8..23] already contains the constructed stlink command */
 993         h->tcp_backend_priv.send_buf[24] = h->direction;
 994         memset(&h->tcp_backend_priv.send_buf[25], 0, 3);  /* reserved for alignment and future use, must be zero */
 995
 996         h_u32_to_le(&h->tcp_backend_priv.send_buf[28], size);
 997
 998         /*
 999          * if the xfer is a write request (tx_ep)
1000          *  > then buf content will be copied
1001          * into &cmdbuf[32].
1002          * else : the xfer is a read or trace read request (rx_ep or trace_ep)
1003          *  > the buf content will be filled from &databuf[4].
1004          *
1005          * note : if h->direction is trace_ep, h->cmdbuf is zeros.
1006          */
1007
1008         if (h->direction == h->tx_ep) { /* STLINK_TCP_REQUEST_WRITE */
1009                 send_size += size;
1010                 if (send_size > STLINK_TCP_SEND_BUFFER_SIZE) {
1011                         LOG_ERROR("STLINK_TCP command buffer overflow");
1012                         return ERROR_FAIL;
1013                 }
1014                 memcpy(&h->tcp_backend_priv.send_buf[32], buf, size);
1015         } else { /* STLINK_TCP_REQUEST_READ or STLINK_TCP_REQUEST_READ_SWO */
1016                 recv_size += size;
1017                 if (recv_size > STLINK_TCP_RECV_BUFFER_SIZE) {
1018                         LOG_ERROR("STLINK_TCP data buffer overflow");
1019                         return ERROR_FAIL;
1020                 }
1021         }
1022
1023         int ret = stlink_tcp_send_cmd(h, send_size, recv_size, true);
1024         if (ret != ERROR_OK)
1025                 return ret;
1026
1027         if (h->direction != h->tx_ep) {
1028                 /* the read data is located in tcp_backend_priv.recv_buf[4] */
1029                 /* most of the case it will be copying the data from tcp_backend_priv.recv_buf[4]
1030                  * to handle->cmd_buff which are the same, so let's avoid unnecessary copying */
1031                 if (buf != &h->tcp_backend_priv.recv_buf[4])
1032                         memcpy((uint8_t *)buf, &h->tcp_backend_priv.recv_buf[4], size);
1033         }
1034
1035         return ERROR_OK;
1036 }
1037
1038 /** */
1039 static int stlink_tcp_read_trace(void *handle, const uint8_t *buf, int size)
1040 {
1041         struct stlink_usb_handle_s *h = handle;
1042
1043         stlink_usb_init_buffer(h, h->trace_ep, 0);
1044         return stlink_tcp_xfer_noerrcheck(handle, buf, size);
1045 }
1046
1047 /**
1048     Converts an STLINK status code held in the first byte of a response
1049     to an openocd error, logs any error/wait status as debug output.
1050 */
1051 static int stlink_usb_error_check(void *handle)
1052 {
1053         struct stlink_usb_handle_s *h = handle;
1054
1055         assert(handle);
1056
1057         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1058                 switch (h->databuf[0]) {
1059                         case STLINK_SWIM_ERR_OK:
1060                                 return ERROR_OK;
1061                         case STLINK_SWIM_BUSY:
1062                                 return ERROR_WAIT;
1063                         default:
1064                                 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
1065                                 return ERROR_FAIL;
1066                 }
1067         }
1068
1069         /* TODO: no error checking yet on api V1 */
1070         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1071                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
1072
1073         switch (h->databuf[0]) {
1074                 case STLINK_DEBUG_ERR_OK:
1075                         return ERROR_OK;
1076                 case STLINK_DEBUG_ERR_FAULT:
1077                         LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
1078                         return ERROR_FAIL;
1079                 case STLINK_SWD_AP_WAIT:
1080                         LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
1081                         return ERROR_WAIT;
1082                 case STLINK_SWD_DP_WAIT:
1083                         LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
1084                         return ERROR_WAIT;
1085                 case STLINK_JTAG_GET_IDCODE_ERROR:
1086                         LOG_DEBUG("STLINK_JTAG_GET_IDCODE_ERROR");
1087                         return ERROR_FAIL;
1088                 case STLINK_JTAG_WRITE_ERROR:
1089                         LOG_DEBUG("Write error");
1090                         return ERROR_FAIL;
1091                 case STLINK_JTAG_WRITE_VERIF_ERROR:
1092                         LOG_DEBUG("Write verify error, ignoring");
1093                         return ERROR_OK;
1094                 case STLINK_SWD_AP_FAULT:
1095                         /* git://git.ac6.fr/openocd commit 657e3e885b9ee10
1096                          * returns ERROR_OK with the comment:
1097                          * Change in error status when reading outside RAM.
1098                          * This fix allows CDT plugin to visualize memory.
1099                          */
1100                         LOG_DEBUG("STLINK_SWD_AP_FAULT");
1101                         return ERROR_FAIL;
1102                 case STLINK_SWD_AP_ERROR:
1103                         LOG_DEBUG("STLINK_SWD_AP_ERROR");
1104                         return ERROR_FAIL;
1105                 case STLINK_SWD_AP_PARITY_ERROR:
1106                         LOG_DEBUG("STLINK_SWD_AP_PARITY_ERROR");
1107                         return ERROR_FAIL;
1108                 case STLINK_SWD_DP_FAULT:
1109                         LOG_DEBUG("STLINK_SWD_DP_FAULT");
1110                         return ERROR_FAIL;
1111                 case STLINK_SWD_DP_ERROR:
1112                         LOG_DEBUG("STLINK_SWD_DP_ERROR");
1113                         return ERROR_FAIL;
1114                 case STLINK_SWD_DP_PARITY_ERROR:
1115                         LOG_DEBUG("STLINK_SWD_DP_PARITY_ERROR");
1116                         return ERROR_FAIL;
1117                 case STLINK_SWD_AP_WDATA_ERROR:
1118                         LOG_DEBUG("STLINK_SWD_AP_WDATA_ERROR");
1119                         return ERROR_FAIL;
1120                 case STLINK_SWD_AP_STICKY_ERROR:
1121                         LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
1122                         return ERROR_FAIL;
1123                 case STLINK_SWD_AP_STICKYORUN_ERROR:
1124                         LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
1125                         return ERROR_FAIL;
1126                 case STLINK_BAD_AP_ERROR:
1127                         LOG_DEBUG("STLINK_BAD_AP_ERROR");
1128                         return ERROR_FAIL;
1129                 default:
1130                         LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
1131                         return ERROR_FAIL;
1132         }
1133 }
1134
1135 /*
1136  * Wrapper around stlink_usb_xfer_noerrcheck()
1137  * to check the error code in the received packet
1138  */
1139 static int stlink_usb_xfer_errcheck(void *handle, const uint8_t *buf, int size)
1140 {
1141         int retval;
1142
1143         assert(size > 0);
1144
1145         retval = stlink_usb_xfer_noerrcheck(handle, buf, size);
1146         if (retval != ERROR_OK)
1147                 return retval;
1148
1149         return stlink_usb_error_check(handle);
1150 }
1151
1152 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
1153
1154     Works for commands where the STLINK_DEBUG status is returned in the first
1155     byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
1156
1157     Returns an openocd result code.
1158 */
1159 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
1160 {
1161         int retries = 0;
1162         int res;
1163         struct stlink_usb_handle_s *h = handle;
1164
1165         while (1) {
1166                 if ((h->st_mode != STLINK_MODE_DEBUG_SWIM) || !retries) {
1167                         res = stlink_usb_xfer_noerrcheck(handle, buf, size);
1168                         if (res != ERROR_OK)
1169                                 return res;
1170                 }
1171
1172                 if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1173                         res = stlink_swim_status(handle);
1174                         if (res != ERROR_OK)
1175                                 return res;
1176                 }
1177
1178                 res = stlink_usb_error_check(handle);
1179                 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1180                         unsigned int delay_us = (1<<retries++) * 1000;
1181                         LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
1182                         usleep(delay_us);
1183                         continue;
1184                 }
1185                 return res;
1186         }
1187 }
1188
1189 /** */
1190 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
1191 {
1192         struct stlink_usb_handle_s *h = handle;
1193
1194         assert(handle);
1195
1196         assert(h->version.flags & STLINK_F_HAS_TRACE);
1197
1198         return h->backend->read_trace(handle, buf, size);
1199 }
1200
1201 /*
1202         this function writes transfer length in
1203         the right place in the cb
1204 */
1205 static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
1206 {
1207         struct stlink_usb_handle_s *h = handle;
1208
1209         buf_set_u32(h->cmdbuf+8, 0, 32, size);
1210 }
1211
1212 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
1213 {
1214         struct stlink_usb_handle_s *h = handle;
1215
1216         /* fill the send buffer */
1217         strcpy((char *)h->cmdbuf, "USBC");
1218         h->cmdidx += 4;
1219         /* csw tag not used */
1220         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
1221         h->cmdidx += 4;
1222         /* cbw data transfer length (in the following data phase in or out) */
1223         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
1224         h->cmdidx += 4;
1225         /* cbw flags */
1226         h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
1227         h->cmdbuf[h->cmdidx++] = 0; /* lun */
1228         /* cdb clength (is filled in at xfer) */
1229         h->cmdbuf[h->cmdidx++] = 0;
1230 }
1231
1232 /** */
1233 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
1234 {
1235         struct stlink_usb_handle_s *h = handle;
1236
1237         h->direction = direction;
1238
1239         h->cmdidx = 0;
1240
1241         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
1242         memset(h->databuf, 0, STLINK_DATA_SIZE);
1243
1244         if (h->version.stlink == 1)
1245                 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
1246 }
1247
1248 /** */
1249 static int stlink_usb_version(void *handle)
1250 {
1251         int res;
1252         uint32_t flags;
1253         uint16_t version;
1254         uint8_t v, x, y, jtag, swim, msd, bridge = 0;
1255         char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
1256         char *p;
1257         struct stlink_usb_handle_s *h = handle;
1258
1259         assert(handle);
1260
1261         stlink_usb_init_buffer(handle, h->rx_ep, 6);
1262
1263         h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
1264
1265         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 6);
1266
1267         if (res != ERROR_OK)
1268                 return res;
1269
1270         version = be_to_h_u16(h->databuf);
1271         v = (version >> 12) & 0x0f;
1272         x = (version >> 6) & 0x3f;
1273         y = version & 0x3f;
1274
1275         h->vid = le_to_h_u16(h->databuf + 2);
1276         h->pid = le_to_h_u16(h->databuf + 4);
1277
1278         switch (h->pid) {
1279         case STLINK_V2_1_PID:
1280         case STLINK_V2_1_NO_MSD_PID:
1281                 if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
1282                         /* MxSy : STM8 V2.1 - SWIM only */
1283                         msd = x;
1284                         swim = y;
1285                         jtag = 0;
1286                 } else {
1287                         /* JxMy : STM32 V2.1 - JTAG/SWD only */
1288                         jtag = x;
1289                         msd = y;
1290                         swim = 0;
1291                 }
1292                 break;
1293         default:
1294                 jtag = x;
1295                 swim = y;
1296                 msd = 0;
1297                 break;
1298         }
1299
1300         /* STLINK-V3 requires a specific command */
1301         if (v == 3 && x == 0 && y == 0) {
1302                 stlink_usb_init_buffer(handle, h->rx_ep, 16);
1303
1304                 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
1305
1306                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 12);
1307                 if (res != ERROR_OK)
1308                         return res;
1309
1310                 v = h->databuf[0];
1311                 swim = h->databuf[1];
1312                 jtag = h->databuf[2];
1313                 msd  = h->databuf[3];
1314                 bridge = h->databuf[4];
1315                 h->vid = le_to_h_u16(h->databuf + 8);
1316                 h->pid = le_to_h_u16(h->databuf + 10);
1317         }
1318
1319         h->version.stlink = v;
1320         h->version.jtag = jtag;
1321         h->version.swim = swim;
1322
1323         flags = 0;
1324         switch (h->version.stlink) {
1325         case 1:
1326                 /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
1327                 if (h->version.jtag >= 11)
1328                         h->version.jtag_api = STLINK_JTAG_API_V2;
1329                 else
1330                         h->version.jtag_api = STLINK_JTAG_API_V1;
1331
1332                 break;
1333         case 2:
1334                 /* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
1335                 h->version.jtag_api = STLINK_JTAG_API_V2;
1336
1337                 /* API for trace from J13 */
1338                 /* API for target voltage from J13 */
1339                 if (h->version.jtag >= 13)
1340                         flags |= STLINK_F_HAS_TRACE;
1341
1342                 /* preferred API to get last R/W status from J15 */
1343                 if (h->version.jtag >= 15)
1344                         flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1345
1346                 /* API to set SWD frequency from J22 */
1347                 if (h->version.jtag >= 22)
1348                         flags |= STLINK_F_HAS_SWD_SET_FREQ;
1349
1350                 /* API to set JTAG frequency from J24 */
1351                 /* API to access DAP registers from J24 */
1352                 if (h->version.jtag >= 24) {
1353                         flags |= STLINK_F_HAS_JTAG_SET_FREQ;
1354                         flags |= STLINK_F_HAS_DAP_REG;
1355                 }
1356
1357                 /* Quirk for read DP in JTAG mode (V2 only) from J24, fixed in J32 */
1358                 if (h->version.jtag >= 24 && h->version.jtag < 32)
1359                         flags |= STLINK_F_QUIRK_JTAG_DP_READ;
1360
1361                 /* API to read/write memory at 16 bit from J26 */
1362                 /* API to write memory without address increment from J26 */
1363                 if (h->version.jtag >= 26)
1364                         flags |= STLINK_F_HAS_MEM_16BIT;
1365
1366                 /* API required to init AP before any AP access from J28 */
1367                 if (h->version.jtag >= 28)
1368                         flags |= STLINK_F_HAS_AP_INIT;
1369
1370                 /* API required to return proper error code on close AP from J29 */
1371                 if (h->version.jtag >= 29)
1372                         flags |= STLINK_F_FIX_CLOSE_AP;
1373
1374                 /* Banked regs (DPv1 & DPv2) support from V2J32 */
1375                 /* API to read memory without address increment from V2J32 */
1376                 /* Memory R/W supports CSW from V2J32 */
1377                 if (h->version.jtag >= 32)
1378                         flags |= STLINK_F_HAS_DPBANKSEL;
1379
1380                 break;
1381         case 3:
1382                 /* all STLINK-V3 use api-v3 */
1383                 h->version.jtag_api = STLINK_JTAG_API_V3;
1384
1385                 /* STLINK-V3 is a superset of ST-LINK/V2 */
1386
1387                 /* API for trace */
1388                 /* API for target voltage */
1389                 flags |= STLINK_F_HAS_TRACE;
1390
1391                 /* preferred API to get last R/W status */
1392                 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
1393
1394                 /* API to access DAP registers */
1395                 flags |= STLINK_F_HAS_DAP_REG;
1396
1397                 /* API to read/write memory at 16 bit */
1398                 /* API to write memory without address increment */
1399                 flags |= STLINK_F_HAS_MEM_16BIT;
1400
1401                 /* API required to init AP before any AP access */
1402                 flags |= STLINK_F_HAS_AP_INIT;
1403
1404                 /* API required to return proper error code on close AP */
1405                 flags |= STLINK_F_FIX_CLOSE_AP;
1406
1407                 /* Banked regs (DPv1 & DPv2) support from V3J2 */
1408                 /* API to read memory without address increment from V3J2 */
1409                 /* Memory R/W supports CSW from V3J2 */
1410                 if (h->version.jtag >= 2)
1411                         flags |= STLINK_F_HAS_DPBANKSEL;
1412
1413                 /* 8bit read/write max packet size 512 bytes from V3J6 */
1414                 if (h->version.jtag >= 6)
1415                         flags |= STLINK_F_HAS_RW8_512BYTES;
1416
1417                 break;
1418         default:
1419                 break;
1420         }
1421         h->version.flags = flags;
1422
1423         p = v_str;
1424         p += sprintf(p, "V%d", v);
1425         if (jtag || !msd)
1426                 p += sprintf(p, "J%d", jtag);
1427         if (msd)
1428                 p += sprintf(p, "M%d", msd);
1429         if (bridge)
1430                 p += sprintf(p, "B%d", bridge);
1431         if (swim || !msd)
1432                 sprintf(p, "S%d", swim);
1433
1434         LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
1435                 v_str,
1436                 h->version.jtag_api,
1437                 h->vid,
1438                 h->pid);
1439
1440         return ERROR_OK;
1441 }
1442
1443 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
1444 {
1445         struct stlink_usb_handle_s *h = handle;
1446         uint32_t adc_results[2];
1447
1448         /* no error message, simply quit with error */
1449         if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
1450                 return ERROR_COMMAND_NOTFOUND;
1451
1452         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1453
1454         h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
1455
1456         int result = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1457
1458         if (result != ERROR_OK)
1459                 return result;
1460
1461         /* convert result */
1462         adc_results[0] = le_to_h_u32(h->databuf);
1463         adc_results[1] = le_to_h_u32(h->databuf + 4);
1464
1465         *target_voltage = 0;
1466
1467         if (adc_results[0])
1468                 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
1469
1470         LOG_INFO("Target voltage: %f", (double)*target_voltage);
1471
1472         return ERROR_OK;
1473 }
1474
1475 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
1476 {
1477         struct stlink_usb_handle_s *h = handle;
1478
1479         assert(handle);
1480
1481         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
1482                 return ERROR_COMMAND_NOTFOUND;
1483
1484         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1485
1486         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1487         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
1488         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1489         h->cmdidx += 2;
1490
1491         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1492
1493         if (result != ERROR_OK)
1494                 return result;
1495
1496         return ERROR_OK;
1497 }
1498
1499 static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
1500 {
1501         struct stlink_usb_handle_s *h = handle;
1502
1503         assert(handle);
1504
1505         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
1506                 return ERROR_COMMAND_NOTFOUND;
1507
1508         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1509
1510         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1511         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
1512         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1513         h->cmdidx += 2;
1514
1515         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1516
1517         if (result != ERROR_OK)
1518                 return result;
1519
1520         return ERROR_OK;
1521 }
1522
1523 /** */
1524 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
1525 {
1526         int res;
1527         struct stlink_usb_handle_s *h = handle;
1528
1529         assert(handle);
1530
1531         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1532
1533         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
1534
1535         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1536
1537         if (res != ERROR_OK)
1538                 return res;
1539
1540         *mode = h->databuf[0];
1541
1542         return ERROR_OK;
1543 }
1544
1545 /** */
1546 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
1547 {
1548         int rx_size = 0;
1549         struct stlink_usb_handle_s *h = handle;
1550
1551         assert(handle);
1552
1553         /* on api V2 we are able the read the latest command
1554          * status
1555          * TODO: we need the test on api V1 too
1556          */
1557         if (h->version.jtag_api != STLINK_JTAG_API_V1)
1558                 rx_size = 2;
1559
1560         stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
1561
1562         switch (type) {
1563                 case STLINK_MODE_DEBUG_JTAG:
1564                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1565                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1566                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1567                         else
1568                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1569                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
1570                         break;
1571                 case STLINK_MODE_DEBUG_SWD:
1572                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1573                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1574                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1575                         else
1576                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1577                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
1578                         break;
1579                 case STLINK_MODE_DEBUG_SWIM:
1580                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1581                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
1582                         /* swim enter does not return any response or status */
1583                         return stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1584                 case STLINK_MODE_DFU:
1585                 case STLINK_MODE_MASS:
1586                 default:
1587                         return ERROR_FAIL;
1588         }
1589
1590         return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
1591 }
1592
1593 /** */
1594 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
1595 {
1596         int res;
1597         struct stlink_usb_handle_s *h = handle;
1598
1599         assert(handle);
1600
1601         /* command with no reply, use a valid endpoint but zero size */
1602         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1603
1604         switch (type) {
1605                 case STLINK_MODE_DEBUG_JTAG:
1606                 case STLINK_MODE_DEBUG_SWD:
1607                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1608                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
1609                         break;
1610                 case STLINK_MODE_DEBUG_SWIM:
1611                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1612                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
1613                         break;
1614                 case STLINK_MODE_DFU:
1615                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
1616                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
1617                         break;
1618                 case STLINK_MODE_MASS:
1619                 default:
1620                         return ERROR_FAIL;
1621         }
1622
1623         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 0);
1624
1625         if (res != ERROR_OK)
1626                 return res;
1627
1628         return ERROR_OK;
1629 }
1630
1631 static int stlink_usb_assert_srst(void *handle, int srst);
1632
1633 static enum stlink_mode stlink_get_mode(enum hl_transports t)
1634 {
1635         switch (t) {
1636         case HL_TRANSPORT_SWD:
1637                 return STLINK_MODE_DEBUG_SWD;
1638         case HL_TRANSPORT_JTAG:
1639                 return STLINK_MODE_DEBUG_JTAG;
1640         default:
1641                 return STLINK_MODE_UNKNOWN;
1642         }
1643 }
1644
1645 /** */
1646 static int stlink_usb_exit_mode(void *handle)
1647 {
1648         int res;
1649         uint8_t mode;
1650         enum stlink_mode emode;
1651
1652         assert(handle);
1653
1654         res = stlink_usb_current_mode(handle, &mode);
1655
1656         if (res != ERROR_OK)
1657                 return res;
1658
1659         LOG_DEBUG("MODE: 0x%02X", mode);
1660
1661         /* try to exit current mode */
1662         switch (mode) {
1663                 case STLINK_DEV_DFU_MODE:
1664                         emode = STLINK_MODE_DFU;
1665                         break;
1666                 case STLINK_DEV_DEBUG_MODE:
1667                         emode = STLINK_MODE_DEBUG_SWD;
1668                         break;
1669                 case STLINK_DEV_SWIM_MODE:
1670                         emode = STLINK_MODE_DEBUG_SWIM;
1671                         break;
1672                 case STLINK_DEV_BOOTLOADER_MODE:
1673                 case STLINK_DEV_MASS_MODE:
1674                 default:
1675                         emode = STLINK_MODE_UNKNOWN;
1676                         break;
1677         }
1678
1679         if (emode != STLINK_MODE_UNKNOWN)
1680                 return stlink_usb_mode_leave(handle, emode);
1681
1682         return ERROR_OK;
1683 }
1684
1685 /** */
1686 static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int initial_interface_speed)
1687 {
1688         int res;
1689         uint8_t mode;
1690         enum stlink_mode emode;
1691         struct stlink_usb_handle_s *h = handle;
1692
1693         assert(handle);
1694
1695         res = stlink_usb_exit_mode(handle);
1696         if (res != ERROR_OK)
1697                 return res;
1698
1699         res = stlink_usb_current_mode(handle, &mode);
1700
1701         if (res != ERROR_OK)
1702                 return res;
1703
1704         /* we check the target voltage here as an aid to debugging connection problems.
1705          * the stlink requires the target Vdd to be connected for reliable debugging.
1706          * this cmd is supported in all modes except DFU
1707          */
1708         if (mode != STLINK_DEV_DFU_MODE) {
1709
1710                 float target_voltage;
1711
1712                 /* check target voltage (if supported) */
1713                 res = stlink_usb_check_voltage(h, &target_voltage);
1714
1715                 if (res != ERROR_OK) {
1716                         if (res != ERROR_COMMAND_NOTFOUND)
1717                                 LOG_ERROR("voltage check failed");
1718                         /* attempt to continue as it is not a catastrophic failure */
1719                 } else {
1720                         /* check for a sensible target voltage, operating range is 1.65-5.5v
1721                          * according to datasheet */
1722                         if (target_voltage < 1.5)
1723                                 LOG_ERROR("target voltage may be too low for reliable debugging");
1724                 }
1725         }
1726
1727         LOG_DEBUG("MODE: 0x%02X", mode);
1728
1729         /* set selected mode */
1730         emode = h->st_mode;
1731
1732         if (emode == STLINK_MODE_UNKNOWN) {
1733                 LOG_ERROR("selected mode (transport) not supported");
1734                 return ERROR_FAIL;
1735         }
1736
1737         /* set the speed before entering the mode, as the chip discovery phase should be done at this speed too */
1738         if (emode == STLINK_MODE_DEBUG_JTAG) {
1739                 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
1740                         stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
1741                         stlink_speed(h, initial_interface_speed, false);
1742                 }
1743         } else if (emode == STLINK_MODE_DEBUG_SWD) {
1744                 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
1745                         stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
1746                         stlink_speed(h, initial_interface_speed, false);
1747                 }
1748         }
1749
1750         if (h->version.jtag_api == STLINK_JTAG_API_V3 &&
1751                         (emode == STLINK_MODE_DEBUG_JTAG || emode == STLINK_MODE_DEBUG_SWD)) {
1752                 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
1753
1754                 stlink_get_com_freq(h, (emode == STLINK_MODE_DEBUG_JTAG), map);
1755                 stlink_dump_speed_map(map, ARRAY_SIZE(map));
1756                 stlink_speed(h, initial_interface_speed, false);
1757         }
1758
1759         /* preliminary SRST assert:
1760          * We want SRST is asserted before activating debug signals (mode_enter).
1761          * As the required mode has not been set, the adapter may not know what pin to use.
1762          * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1763          * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1764          * after power on, SWIM_RST stays unchanged */
1765         if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1766                 stlink_usb_assert_srst(handle, 0);
1767                 /* do not check the return status here, we will
1768                    proceed and enter the desired mode below
1769                    and try asserting srst again. */
1770
1771         res = stlink_usb_mode_enter(handle, emode);
1772         if (res != ERROR_OK)
1773                 return res;
1774
1775         /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1776         if (connect_under_reset) {
1777                 res = stlink_usb_assert_srst(handle, 0);
1778                 if (res != ERROR_OK)
1779                         return res;
1780         }
1781
1782         res = stlink_usb_current_mode(handle, &mode);
1783
1784         if (res != ERROR_OK)
1785                 return res;
1786
1787         LOG_DEBUG("MODE: 0x%02X", mode);
1788
1789         return ERROR_OK;
1790 }
1791
1792 /* request status from last swim request */
1793 static int stlink_swim_status(void *handle)
1794 {
1795         struct stlink_usb_handle_s *h = handle;
1796         int res;
1797
1798         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1799         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1800         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1801         /* error is checked by the caller */
1802         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1803         if (res != ERROR_OK)
1804                 return res;
1805         return ERROR_OK;
1806 }
1807 /*
1808         the purpose of this function is unknown...
1809         capabilities? anyway for swim v6 it returns
1810         0001020600000000
1811 */
1812 __attribute__((unused))
1813 static int stlink_swim_cap(void *handle, uint8_t *cap)
1814 {
1815         struct stlink_usb_handle_s *h = handle;
1816         int res;
1817
1818         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1819         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1820         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1821         h->cmdbuf[h->cmdidx++] = 0x01;
1822         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1823         if (res != ERROR_OK)
1824                 return res;
1825         memcpy(cap, h->databuf, 8);
1826         return ERROR_OK;
1827 }
1828
1829 /*      debug dongle assert/deassert sreset line */
1830 static int stlink_swim_assert_reset(void *handle, int reset)
1831 {
1832         struct stlink_usb_handle_s *h = handle;
1833         int res;
1834
1835         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1836         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1837         if (!reset)
1838                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1839         else
1840                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1841         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1842         if (res != ERROR_OK)
1843                 return res;
1844         return ERROR_OK;
1845 }
1846
1847 /*
1848         send swim enter seq
1849         1.3ms low then 750Hz then 1.5kHz
1850 */
1851 static int stlink_swim_enter(void *handle)
1852 {
1853         struct stlink_usb_handle_s *h = handle;
1854         int res;
1855
1856         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1857         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1858         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1859         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1860         if (res != ERROR_OK)
1861                 return res;
1862         return ERROR_OK;
1863 }
1864
1865 /*      switch high/low speed swim */
1866 static int stlink_swim_speed(void *handle, int speed)
1867 {
1868         struct stlink_usb_handle_s *h = handle;
1869         int res;
1870
1871         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1872         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1873         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1874         if (speed)
1875                 h->cmdbuf[h->cmdidx++] = 1;
1876         else
1877                 h->cmdbuf[h->cmdidx++] = 0;
1878         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1879         if (res != ERROR_OK)
1880                 return res;
1881         return ERROR_OK;
1882 }
1883
1884 /*
1885         initiate srst from swim.
1886         nrst is pulled low for 50us.
1887 */
1888 static int stlink_swim_generate_rst(void *handle)
1889 {
1890         struct stlink_usb_handle_s *h = handle;
1891         int res;
1892
1893         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1894         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1895         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1896         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1897         if (res != ERROR_OK)
1898                 return res;
1899         return ERROR_OK;
1900 }
1901
1902 /*
1903         send resynchronize sequence
1904         swim is pulled low for 16us
1905         reply is 64 clks low
1906 */
1907 static int stlink_swim_resync(void *handle)
1908 {
1909         struct stlink_usb_handle_s *h = handle;
1910         int res;
1911
1912         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1913         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1914         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1915         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1916         if (res != ERROR_OK)
1917                 return res;
1918         return ERROR_OK;
1919 }
1920
1921 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1922 {
1923         struct stlink_usb_handle_s *h = handle;
1924         int res;
1925         unsigned int i;
1926         unsigned int datalen = 0;
1927         int cmdsize = STLINK_CMD_SIZE_V2;
1928
1929         if (len > STLINK_SWIM_DATA_SIZE)
1930                 return ERROR_FAIL;
1931
1932         if (h->version.stlink == 1)
1933                 cmdsize = STLINK_SG_SIZE;
1934
1935         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1936         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1937         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1938         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1939         h->cmdidx += 2;
1940         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1941         h->cmdidx += 4;
1942         for (i = 0; i < len; i++) {
1943                 if (h->cmdidx == cmdsize)
1944                         h->databuf[datalen++] = *(data++);
1945                 else
1946                         h->cmdbuf[h->cmdidx++] = *(data++);
1947         }
1948         if (h->version.stlink == 1)
1949                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1950
1951         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1952         if (res != ERROR_OK)
1953                 return res;
1954         return ERROR_OK;
1955 }
1956
1957 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1958 {
1959         struct stlink_usb_handle_s *h = handle;
1960         int res;
1961
1962         if (len > STLINK_SWIM_DATA_SIZE)
1963                 return ERROR_FAIL;
1964
1965         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1966         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1967         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1968         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1969         h->cmdidx += 2;
1970         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1971         h->cmdidx += 4;
1972         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1973         if (res != ERROR_OK)
1974                 return res;
1975
1976         stlink_usb_init_buffer(handle, h->rx_ep, len);
1977         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1978         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1979         res = stlink_usb_xfer_noerrcheck(handle, data, len);
1980         if (res != ERROR_OK)
1981                 return res;
1982
1983         return ERROR_OK;
1984 }
1985
1986 /** */
1987 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1988 {
1989         int res, offset;
1990         struct stlink_usb_handle_s *h = handle;
1991
1992         assert(handle);
1993
1994         /* there is no swim read core id cmd */
1995         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1996                 *idcode = 0;
1997                 return ERROR_OK;
1998         }
1999
2000         stlink_usb_init_buffer(handle, h->rx_ep, 12);
2001
2002         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2003         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2004                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
2005
2006                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2007                 offset = 0;
2008         } else {
2009                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
2010
2011                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2012                 offset = 4;
2013         }
2014
2015         if (res != ERROR_OK)
2016                 return res;
2017
2018         *idcode = le_to_h_u32(h->databuf + offset);
2019
2020         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
2021
2022         return ERROR_OK;
2023 }
2024
2025 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
2026 {
2027         struct stlink_usb_handle_s *h = handle;
2028         int res;
2029
2030         assert(handle);
2031
2032         stlink_usb_init_buffer(handle, h->rx_ep, 8);
2033
2034         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2035         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
2036         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2037         h->cmdidx += 4;
2038
2039         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2040         if (res != ERROR_OK)
2041                 return res;
2042
2043         *val = le_to_h_u32(h->databuf + 4);
2044         return ERROR_OK;
2045 }
2046
2047 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
2048 {
2049         struct stlink_usb_handle_s *h = handle;
2050
2051         assert(handle);
2052
2053         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2054
2055         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2056         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2057                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
2058         else
2059                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
2060         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2061         h->cmdidx += 4;
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 /** */
2069 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
2070 {
2071         struct stlink_usb_handle_s *h = handle;
2072
2073         assert(handle);
2074
2075         if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
2076                 int res;
2077
2078                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2079
2080                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2081                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
2082
2083                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2084                 if (res != ERROR_OK)
2085                         return res;
2086
2087                 size_t bytes_avail = le_to_h_u16(h->databuf);
2088                 *size = bytes_avail < *size ? bytes_avail : *size;
2089
2090                 if (*size > 0) {
2091                         res = stlink_usb_read_trace(handle, buf, *size);
2092                         if (res != ERROR_OK)
2093                                 return res;
2094                         return ERROR_OK;
2095                 }
2096         }
2097         *size = 0;
2098         return ERROR_OK;
2099 }
2100
2101 static enum target_state stlink_usb_v2_get_status(void *handle)
2102 {
2103         int result;
2104         uint32_t status;
2105
2106         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
2107         if  (result != ERROR_OK)
2108                 return TARGET_UNKNOWN;
2109
2110         if (status & S_HALT)
2111                 return TARGET_HALTED;
2112         else if (status & S_RESET_ST)
2113                 return TARGET_RESET;
2114
2115         return TARGET_RUNNING;
2116 }
2117
2118 /** */
2119 static enum target_state stlink_usb_state(void *handle)
2120 {
2121         int res;
2122         struct stlink_usb_handle_s *h = handle;
2123
2124         assert(handle);
2125
2126         if (h->reconnect_pending) {
2127                 LOG_INFO("Previous state query failed, trying to reconnect");
2128                 res = stlink_usb_mode_enter(handle, h->st_mode);
2129                 if (res != ERROR_OK)
2130                         return TARGET_UNKNOWN;
2131
2132                 h->reconnect_pending = false;
2133         }
2134
2135         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2136                 res = stlink_usb_v2_get_status(handle);
2137                 if (res == TARGET_UNKNOWN)
2138                         h->reconnect_pending = true;
2139                 return res;
2140         }
2141
2142         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2143
2144         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2145         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
2146
2147         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2148
2149         if (res != ERROR_OK)
2150                 return TARGET_UNKNOWN;
2151
2152         if (h->databuf[0] == STLINK_CORE_RUNNING)
2153                 return TARGET_RUNNING;
2154         if (h->databuf[0] == STLINK_CORE_HALTED)
2155                 return TARGET_HALTED;
2156
2157         h->reconnect_pending = true;
2158
2159         return TARGET_UNKNOWN;
2160 }
2161
2162 static int stlink_usb_assert_srst(void *handle, int srst)
2163 {
2164         struct stlink_usb_handle_s *h = handle;
2165
2166         assert(handle);
2167
2168         if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
2169                 return stlink_swim_assert_reset(handle, srst);
2170
2171         if (h->version.stlink == 1)
2172                 return ERROR_COMMAND_NOTFOUND;
2173
2174         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2175
2176         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2177         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
2178         h->cmdbuf[h->cmdidx++] = srst;
2179
2180         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2181 }
2182
2183 /** */
2184 static void stlink_usb_trace_disable(void *handle)
2185 {
2186         int res = ERROR_OK;
2187         struct stlink_usb_handle_s *h = handle;
2188
2189         assert(handle);
2190
2191         assert(h->version.flags & STLINK_F_HAS_TRACE);
2192
2193         LOG_DEBUG("Tracing: disable");
2194
2195         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2196         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2197         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
2198         res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2199
2200         if (res == ERROR_OK)
2201                 h->trace.enabled = false;
2202 }
2203
2204
2205 /** */
2206 static int stlink_usb_trace_enable(void *handle)
2207 {
2208         int res;
2209         struct stlink_usb_handle_s *h = handle;
2210
2211         assert(handle);
2212
2213         if (h->version.flags & STLINK_F_HAS_TRACE) {
2214                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2215
2216                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2217                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
2218                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
2219                 h->cmdidx += 2;
2220                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
2221                 h->cmdidx += 4;
2222
2223                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2224
2225                 if (res == ERROR_OK)  {
2226                         h->trace.enabled = true;
2227                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
2228                 }
2229         } else {
2230                 LOG_ERROR("Tracing is not supported by this version.");
2231                 res = ERROR_FAIL;
2232         }
2233
2234         return res;
2235 }
2236
2237 /** */
2238 static int stlink_usb_reset(void *handle)
2239 {
2240         struct stlink_usb_handle_s *h = handle;
2241         int retval;
2242
2243         assert(handle);
2244
2245         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2246
2247         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2248
2249         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2250                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
2251         else
2252                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
2253
2254         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
2255         if (retval != ERROR_OK)
2256                 return retval;
2257
2258         if (h->trace.enabled) {
2259                 stlink_usb_trace_disable(h);
2260                 return stlink_usb_trace_enable(h);
2261         }
2262
2263         return ERROR_OK;
2264 }
2265
2266 /** */
2267 static int stlink_usb_run(void *handle)
2268 {
2269         int res;
2270         struct stlink_usb_handle_s *h = handle;
2271
2272         assert(handle);
2273
2274         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2275                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
2276
2277                 return res;
2278         }
2279
2280         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2281
2282         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2283         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
2284
2285         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2286 }
2287
2288 /** */
2289 static int stlink_usb_halt(void *handle)
2290 {
2291         int res;
2292         struct stlink_usb_handle_s *h = handle;
2293
2294         assert(handle);
2295
2296         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2297                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2298
2299                 return res;
2300         }
2301
2302         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2303
2304         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2305         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
2306
2307         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2308 }
2309
2310 /** */
2311 static int stlink_usb_step(void *handle)
2312 {
2313         struct stlink_usb_handle_s *h = handle;
2314
2315         assert(handle);
2316
2317         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2318                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
2319                  * that the Cortex-M3 currently does. */
2320                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
2321                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
2322                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2323         }
2324
2325         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2326
2327         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2328         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
2329
2330         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2331 }
2332
2333 /** */
2334 static int stlink_usb_read_regs(void *handle)
2335 {
2336         int res;
2337         struct stlink_usb_handle_s *h = handle;
2338
2339         assert(handle);
2340
2341         stlink_usb_init_buffer(handle, h->rx_ep, 88);
2342
2343         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2344         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2345
2346                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
2347                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
2348                 /* regs data from offset 0 */
2349         } else {
2350                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
2351                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
2352                 /* status at offset 0, regs data from offset 4 */
2353         }
2354
2355         return res;
2356 }
2357
2358 /** */
2359 static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
2360 {
2361         int res;
2362         struct stlink_usb_handle_s *h = handle;
2363
2364         assert(handle);
2365
2366         if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2367                 res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
2368                 if (res != ERROR_OK)
2369                         return res;
2370
2371                 /* FIXME: poll DHCSR.S_REGRDY before read DCRDR */
2372                 return stlink_usb_v2_read_debug_reg(h, DCB_DCRDR, val);
2373         }
2374
2375         stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
2376
2377         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2378         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2379                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
2380         else
2381                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
2382         h->cmdbuf[h->cmdidx++] = regsel;
2383
2384         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2385                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2386                 if (res != ERROR_OK)
2387                         return res;
2388                 *val = le_to_h_u32(h->databuf);
2389                 return ERROR_OK;
2390         } else {
2391                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2392                 if (res != ERROR_OK)
2393                         return res;
2394                 *val = le_to_h_u32(h->databuf + 4);
2395                 return ERROR_OK;
2396         }
2397 }
2398
2399 /** */
2400 static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
2401 {
2402         struct stlink_usb_handle_s *h = handle;
2403
2404         assert(handle);
2405
2406         if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2407                 int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
2408                 if (res != ERROR_OK)
2409                         return res;
2410
2411                 return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WNR | (regsel & 0x7f));
2412                 /* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
2413         }
2414
2415         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2416
2417         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2418         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2419                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
2420         else
2421                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
2422         h->cmdbuf[h->cmdidx++] = regsel;
2423         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2424         h->cmdidx += 4;
2425
2426         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2427 }
2428
2429 static int stlink_usb_get_rw_status(void *handle)
2430 {
2431         struct stlink_usb_handle_s *h = handle;
2432
2433         assert(handle);
2434
2435         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2436                 return ERROR_OK;
2437
2438         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2439
2440         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2441         if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
2442                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
2443                 return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2444         } else {
2445                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
2446                 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2447         }
2448 }
2449
2450 /** */
2451 static int stlink_usb_read_mem8(void *handle, uint8_t ap_num, uint32_t csw,
2452                 uint32_t addr, uint16_t len, uint8_t *buffer)
2453 {
2454         int res;
2455         uint16_t read_len = len;
2456         struct stlink_usb_handle_s *h = handle;
2457
2458         assert(handle);
2459
2460         if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2461                 return ERROR_COMMAND_NOTFOUND;
2462
2463         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2464         if (len > stlink_usb_block(h)) {
2465                 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2466                 return ERROR_FAIL;
2467         }
2468
2469         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2470
2471         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2472         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2473         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2474         h->cmdidx += 4;
2475         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2476         h->cmdidx += 2;
2477         h->cmdbuf[h->cmdidx++] = ap_num;
2478         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2479         h->cmdidx += 3;
2480
2481         /* we need to fix read length for single bytes */
2482         if (read_len == 1)
2483                 read_len++;
2484
2485         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
2486
2487         if (res != ERROR_OK)
2488                 return res;
2489
2490         memcpy(buffer, h->databuf, len);
2491
2492         return stlink_usb_get_rw_status(handle);
2493 }
2494
2495 /** */
2496 static int stlink_usb_write_mem8(void *handle, uint8_t ap_num, uint32_t csw,
2497                 uint32_t addr, uint16_t len, const uint8_t *buffer)
2498 {
2499         int res;
2500         struct stlink_usb_handle_s *h = handle;
2501
2502         assert(handle);
2503
2504         if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2505                 return ERROR_COMMAND_NOTFOUND;
2506
2507         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2508         if (len > stlink_usb_block(h)) {
2509                 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2510                 return ERROR_FAIL;
2511         }
2512
2513         stlink_usb_init_buffer(handle, h->tx_ep, len);
2514
2515         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2516         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2517         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2518         h->cmdidx += 4;
2519         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2520         h->cmdidx += 2;
2521         h->cmdbuf[h->cmdidx++] = ap_num;
2522         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2523         h->cmdidx += 3;
2524
2525         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2526
2527         if (res != ERROR_OK)
2528                 return res;
2529
2530         return stlink_usb_get_rw_status(handle);
2531 }
2532
2533 /** */
2534 static int stlink_usb_read_mem16(void *handle, uint8_t ap_num, uint32_t csw,
2535                 uint32_t addr, uint16_t len, uint8_t *buffer)
2536 {
2537         int res;
2538         struct stlink_usb_handle_s *h = handle;
2539
2540         assert(handle);
2541
2542         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2543                 return ERROR_COMMAND_NOTFOUND;
2544
2545         if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2546                 return ERROR_COMMAND_NOTFOUND;
2547
2548         if (len > STLINK_MAX_RW16_32) {
2549                 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2550                 return ERROR_FAIL;
2551         }
2552
2553         /* data must be a multiple of 2 and half-word aligned */
2554         if (len % 2 || addr % 2) {
2555                 LOG_DEBUG("Invalid data alignment");
2556                 return ERROR_TARGET_UNALIGNED_ACCESS;
2557         }
2558
2559         stlink_usb_init_buffer(handle, h->rx_ep, len);
2560
2561         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2562         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2563         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2564         h->cmdidx += 4;
2565         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2566         h->cmdidx += 2;
2567         h->cmdbuf[h->cmdidx++] = ap_num;
2568         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2569         h->cmdidx += 3;
2570
2571         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2572
2573         if (res != ERROR_OK)
2574                 return res;
2575
2576         memcpy(buffer, h->databuf, len);
2577
2578         return stlink_usb_get_rw_status(handle);
2579 }
2580
2581 /** */
2582 static int stlink_usb_write_mem16(void *handle, uint8_t ap_num, uint32_t csw,
2583                 uint32_t addr, uint16_t len, const uint8_t *buffer)
2584 {
2585         int res;
2586         struct stlink_usb_handle_s *h = handle;
2587
2588         assert(handle);
2589
2590         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2591                 return ERROR_COMMAND_NOTFOUND;
2592
2593         if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2594                 return ERROR_COMMAND_NOTFOUND;
2595
2596         if (len > STLINK_MAX_RW16_32) {
2597                 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2598                 return ERROR_FAIL;
2599         }
2600
2601         /* data must be a multiple of 2 and half-word aligned */
2602         if (len % 2 || addr % 2) {
2603                 LOG_DEBUG("Invalid data alignment");
2604                 return ERROR_TARGET_UNALIGNED_ACCESS;
2605         }
2606
2607         stlink_usb_init_buffer(handle, h->tx_ep, len);
2608
2609         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2610         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2611         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2612         h->cmdidx += 4;
2613         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2614         h->cmdidx += 2;
2615         h->cmdbuf[h->cmdidx++] = ap_num;
2616         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2617         h->cmdidx += 3;
2618
2619         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2620
2621         if (res != ERROR_OK)
2622                 return res;
2623
2624         return stlink_usb_get_rw_status(handle);
2625 }
2626
2627 /** */
2628 static int stlink_usb_read_mem32(void *handle, uint8_t ap_num, uint32_t csw,
2629                 uint32_t addr, uint16_t len, uint8_t *buffer)
2630 {
2631         int res;
2632         struct stlink_usb_handle_s *h = handle;
2633
2634         assert(handle);
2635
2636         if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2637                 return ERROR_COMMAND_NOTFOUND;
2638
2639         if (len > STLINK_MAX_RW16_32) {
2640                 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2641                 return ERROR_FAIL;
2642         }
2643
2644         /* data must be a multiple of 4 and word aligned */
2645         if (len % 4 || addr % 4) {
2646                 LOG_DEBUG("Invalid data alignment");
2647                 return ERROR_TARGET_UNALIGNED_ACCESS;
2648         }
2649
2650         stlink_usb_init_buffer(handle, h->rx_ep, len);
2651
2652         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2653         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2654         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2655         h->cmdidx += 4;
2656         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2657         h->cmdidx += 2;
2658         h->cmdbuf[h->cmdidx++] = ap_num;
2659         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2660         h->cmdidx += 3;
2661
2662         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2663
2664         if (res != ERROR_OK)
2665                 return res;
2666
2667         memcpy(buffer, h->databuf, len);
2668
2669         return stlink_usb_get_rw_status(handle);
2670 }
2671
2672 /** */
2673 static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t csw,
2674                 uint32_t addr, uint16_t len, const uint8_t *buffer)
2675 {
2676         int res;
2677         struct stlink_usb_handle_s *h = handle;
2678
2679         assert(handle);
2680
2681         if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
2682                 return ERROR_COMMAND_NOTFOUND;
2683
2684         if (len > STLINK_MAX_RW16_32) {
2685                 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2686                 return ERROR_FAIL;
2687         }
2688
2689         /* data must be a multiple of 4 and word aligned */
2690         if (len % 4 || addr % 4) {
2691                 LOG_DEBUG("Invalid data alignment");
2692                 return ERROR_TARGET_UNALIGNED_ACCESS;
2693         }
2694
2695         stlink_usb_init_buffer(handle, h->tx_ep, len);
2696
2697         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2698         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2699         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2700         h->cmdidx += 4;
2701         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2702         h->cmdidx += 2;
2703         h->cmdbuf[h->cmdidx++] = ap_num;
2704         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2705         h->cmdidx += 3;
2706
2707         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2708
2709         if (res != ERROR_OK)
2710                 return res;
2711
2712         return stlink_usb_get_rw_status(handle);
2713 }
2714
2715 static int stlink_usb_read_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
2716                 uint32_t addr, uint16_t len, uint8_t *buffer)
2717 {
2718         struct stlink_usb_handle_s *h = handle;
2719
2720         assert(handle != NULL);
2721
2722         if (!(h->version.flags & STLINK_F_HAS_MEM_RD_NO_INC))
2723                 return ERROR_COMMAND_NOTFOUND;
2724
2725         if (len > STLINK_MAX_RW16_32) {
2726                 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2727                 return ERROR_FAIL;
2728         }
2729
2730         /* data must be a multiple of 4 and word aligned */
2731         if (len % 4 || addr % 4) {
2732                 LOG_DEBUG("Invalid data alignment");
2733                 return ERROR_TARGET_UNALIGNED_ACCESS;
2734         }
2735
2736         stlink_usb_init_buffer(handle, h->rx_ep, len);
2737
2738         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2739         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC;
2740         h_u32_to_le(h->cmdbuf + h->cmdidx, addr);
2741         h->cmdidx += 4;
2742         h_u16_to_le(h->cmdbuf + h->cmdidx, len);
2743         h->cmdidx += 2;
2744         h->cmdbuf[h->cmdidx++] = ap_num;
2745         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2746         h->cmdidx += 3;
2747
2748         int retval = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2749         if (retval != ERROR_OK)
2750                 return retval;
2751
2752         memcpy(buffer, h->databuf, len);
2753
2754         return stlink_usb_get_rw_status(handle);
2755 }
2756
2757 static int stlink_usb_write_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
2758                 uint32_t addr, uint16_t len, const uint8_t *buffer)
2759 {
2760         struct stlink_usb_handle_s *h = handle;
2761
2762         assert(handle != NULL);
2763
2764         if (!(h->version.flags & STLINK_F_HAS_MEM_WR_NO_INC))
2765                 return ERROR_COMMAND_NOTFOUND;
2766
2767         if (len > STLINK_MAX_RW16_32) {
2768                 LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
2769                 return ERROR_FAIL;
2770         }
2771
2772         /* data must be a multiple of 4 and word aligned */
2773         if (len % 4 || addr % 4) {
2774                 LOG_DEBUG("Invalid data alignment");
2775                 return ERROR_TARGET_UNALIGNED_ACCESS;
2776         }
2777
2778         stlink_usb_init_buffer(handle, h->tx_ep, len);
2779
2780         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2781         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC;
2782         h_u32_to_le(h->cmdbuf + h->cmdidx, addr);
2783         h->cmdidx += 4;
2784         h_u16_to_le(h->cmdbuf + h->cmdidx, len);
2785         h->cmdidx += 2;
2786         h->cmdbuf[h->cmdidx++] = ap_num;
2787         h_u24_to_le(h->cmdbuf + h->cmdidx, csw >> 8);
2788         h->cmdidx += 3;
2789
2790         int retval = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2791         if (retval != ERROR_OK)
2792                 return retval;
2793
2794         return stlink_usb_get_rw_status(handle);
2795 }
2796
2797 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2798 {
2799         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2800         if (max_tar_block == 0)
2801                 max_tar_block = 4;
2802         return max_tar_block;
2803 }
2804
2805 static int stlink_usb_read_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
2806                 uint32_t addr, uint32_t size, uint32_t count, uint8_t *buffer)
2807 {
2808         int retval = ERROR_OK;
2809         uint32_t bytes_remaining;
2810         int retries = 0;
2811         struct stlink_usb_handle_s *h = handle;
2812
2813         /* calculate byte count */
2814         count *= size;
2815
2816         /* switch to 8 bit if stlink does not support 16 bit memory read */
2817         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2818                 size = 1;
2819
2820         while (count) {
2821                 bytes_remaining = (size != 1) ?
2822                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2823
2824                 if (count < bytes_remaining)
2825                         bytes_remaining = count;
2826
2827                 /*
2828                  * all stlink support 8/32bit memory read/writes and only from
2829                  * stlink V2J26 there is support for 16 bit memory read/write.
2830                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2831                  * as 8bit access.
2832                  */
2833                 if (size != 1) {
2834                         /* When in jtag mode the stlink uses the auto-increment functionality.
2835                          * However it expects us to pass the data correctly, this includes
2836                          * alignment and any page boundaries. We already do this as part of the
2837                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2838                          * needs implementing manually.
2839                          * currently this only affects jtag mode, according to ST they do single
2840                          * access in SWD mode - but this may change and so we do it for both modes */
2841
2842                         /* we first need to check for any unaligned bytes */
2843                         if (addr & (size - 1)) {
2844                                 uint32_t head_bytes = size - (addr & (size - 1));
2845                                 retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
2846                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2847                                         usleep((1 << retries++) * 1000);
2848                                         continue;
2849                                 }
2850                                 if (retval != ERROR_OK)
2851                                         return retval;
2852                                 buffer += head_bytes;
2853                                 addr += head_bytes;
2854                                 count -= head_bytes;
2855                                 bytes_remaining -= head_bytes;
2856                         }
2857
2858                         if (bytes_remaining & (size - 1))
2859                                 retval = stlink_usb_read_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
2860                         else if (size == 2)
2861                                 retval = stlink_usb_read_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
2862                         else
2863                                 retval = stlink_usb_read_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
2864                 } else {
2865                         retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
2866                 }
2867
2868                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2869                         usleep((1 << retries++) * 1000);
2870                         continue;
2871                 }
2872                 if (retval != ERROR_OK)
2873                         return retval;
2874
2875                 buffer += bytes_remaining;
2876                 addr += bytes_remaining;
2877                 count -= bytes_remaining;
2878         }
2879
2880         return retval;
2881 }
2882
2883 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2884                 uint32_t count, uint8_t *buffer)
2885 {
2886         return stlink_usb_read_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
2887                                                                   addr, size, count, buffer);
2888 }
2889
2890 static int stlink_usb_write_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
2891                 uint32_t addr, uint32_t size, uint32_t count, const uint8_t *buffer)
2892 {
2893         int retval = ERROR_OK;
2894         uint32_t bytes_remaining;
2895         int retries = 0;
2896         struct stlink_usb_handle_s *h = handle;
2897
2898         /* calculate byte count */
2899         count *= size;
2900
2901         /* switch to 8 bit if stlink does not support 16 bit memory read */
2902         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2903                 size = 1;
2904
2905         while (count) {
2906
2907                 bytes_remaining = (size != 1) ?
2908                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2909
2910                 if (count < bytes_remaining)
2911                         bytes_remaining = count;
2912
2913                 /*
2914                  * all stlink support 8/32bit memory read/writes and only from
2915                  * stlink V2J26 there is support for 16 bit memory read/write.
2916                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2917                  * as 8bit access.
2918                  */
2919                 if (size != 1) {
2920
2921                         /* When in jtag mode the stlink uses the auto-increment functionality.
2922                          * However it expects us to pass the data correctly, this includes
2923                          * alignment and any page boundaries. We already do this as part of the
2924                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2925                          * needs implementing manually.
2926                          * currently this only affects jtag mode, according to ST they do single
2927                          * access in SWD mode - but this may change and so we do it for both modes */
2928
2929                         /* we first need to check for any unaligned bytes */
2930                         if (addr & (size - 1)) {
2931
2932                                 uint32_t head_bytes = size - (addr & (size - 1));
2933                                 retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
2934                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2935                                         usleep((1<<retries++) * 1000);
2936                                         continue;
2937                                 }
2938                                 if (retval != ERROR_OK)
2939                                         return retval;
2940                                 buffer += head_bytes;
2941                                 addr += head_bytes;
2942                                 count -= head_bytes;
2943                                 bytes_remaining -= head_bytes;
2944                         }
2945
2946                         if (bytes_remaining & (size - 1))
2947                                 retval = stlink_usb_write_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
2948                         else if (size == 2)
2949                                 retval = stlink_usb_write_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
2950                         else
2951                                 retval = stlink_usb_write_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
2952
2953                 } else
2954                         retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
2955                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2956                         usleep((1<<retries++) * 1000);
2957                         continue;
2958                 }
2959                 if (retval != ERROR_OK)
2960                         return retval;
2961
2962                 buffer += bytes_remaining;
2963                 addr += bytes_remaining;
2964                 count -= bytes_remaining;
2965         }
2966
2967         return retval;
2968 }
2969
2970 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2971                 uint32_t count, const uint8_t *buffer)
2972 {
2973         return stlink_usb_write_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
2974                                                                    addr, size, count, buffer);
2975 }
2976
2977 /** */
2978 static int stlink_usb_override_target(const char *targetname)
2979 {
2980         return !strcmp(targetname, "cortex_m");
2981 }
2982
2983 static int stlink_speed_swim(void *handle, int khz, bool query)
2984 {
2985         int retval;
2986
2987         /*
2988                         we only have low and high speed...
2989                         before changing speed the SWIM_CSR HS bit
2990                         must be updated
2991          */
2992         if (!query) {
2993                 retval = stlink_swim_speed(handle, (khz < SWIM_FREQ_HIGH) ? 0 : 1);
2994                 if (retval != ERROR_OK)
2995                         LOG_ERROR("Unable to set adapter speed");
2996         }
2997
2998         return (khz < SWIM_FREQ_HIGH) ? SWIM_FREQ_LOW : SWIM_FREQ_HIGH;
2999 }
3000
3001 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
3002 {
3003         unsigned int i;
3004         int speed_index = -1;
3005         int speed_diff = INT_MAX;
3006         int last_valid_speed = -1;
3007         bool match = true;
3008
3009         for (i = 0; i < map_size; i++) {
3010                 if (!map[i].speed)
3011                         continue;
3012                 last_valid_speed = i;
3013                 if (khz == map[i].speed) {
3014                         speed_index = i;
3015                         break;
3016                 } else {
3017                         int current_diff = khz - map[i].speed;
3018                         /* get abs value for comparison */
3019                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
3020                         if ((current_diff < speed_diff) && khz >= map[i].speed) {
3021                                 speed_diff = current_diff;
3022                                 speed_index = i;
3023                         }
3024                 }
3025         }
3026
3027         if (speed_index == -1) {
3028                 /* this will only be here if we cannot match the slow speed.
3029                  * use the slowest speed we support.*/
3030                 speed_index = last_valid_speed;
3031                 match = false;
3032         } else if (i == map_size)
3033                 match = false;
3034
3035         if (!match && query) {
3036                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz",
3037                                 khz, map[speed_index].speed);
3038         }
3039
3040         return speed_index;
3041 }
3042
3043 static int stlink_speed_swd(void *handle, int khz, bool query)
3044 {
3045         int speed_index;
3046         struct stlink_usb_handle_s *h = handle;
3047
3048         /* old firmware cannot change it */
3049         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
3050                 return khz;
3051
3052         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
3053                 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
3054
3055         if (!query) {
3056                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
3057                 if (result != ERROR_OK) {
3058                         LOG_ERROR("Unable to set adapter speed");
3059                         return khz;
3060                 }
3061         }
3062
3063         return stlink_khz_to_speed_map_swd[speed_index].speed;
3064 }
3065
3066 static int stlink_speed_jtag(void *handle, int khz, bool query)
3067 {
3068         int speed_index;
3069         struct stlink_usb_handle_s *h = handle;
3070
3071         /* old firmware cannot change it */
3072         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
3073                 return khz;
3074
3075         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
3076                 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
3077
3078         if (!query) {
3079                 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
3080                 if (result != ERROR_OK) {
3081                         LOG_ERROR("Unable to set adapter speed");
3082                         return khz;
3083                 }
3084         }
3085
3086         return stlink_khz_to_speed_map_jtag[speed_index].speed;
3087 }
3088
3089 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
3090 {
3091         unsigned int i;
3092
3093         LOG_DEBUG("Supported clock speeds are:");
3094         for (i = 0; i < map_size; i++)
3095                 if (map[i].speed)
3096                         LOG_DEBUG("%d kHz", map[i].speed);
3097 }
3098
3099 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
3100 {
3101         struct stlink_usb_handle_s *h = handle;
3102         int i;
3103
3104         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
3105                 LOG_ERROR("Unknown command");
3106                 return 0;
3107         }
3108
3109         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3110
3111         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3112         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
3113         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
3114
3115         int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
3116
3117         int size = h->databuf[8];
3118
3119         if (size > STLINK_V3_MAX_FREQ_NB)
3120                 size = STLINK_V3_MAX_FREQ_NB;
3121
3122         for (i = 0; i < size; i++) {
3123                 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
3124                 map[i].speed_divisor = i;
3125         }
3126
3127         /* set to zero all the next entries */
3128         for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
3129                 map[i].speed = 0;
3130
3131         return res;
3132 }
3133
3134 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
3135 {
3136         struct stlink_usb_handle_s *h = handle;
3137
3138         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
3139                 LOG_ERROR("Unknown command");
3140                 return 0;
3141         }
3142
3143         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3144
3145         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3146         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
3147         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
3148         h->cmdbuf[h->cmdidx++] = 0;
3149
3150         h_u32_to_le(&h->cmdbuf[4], frequency);
3151
3152         return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3153 }
3154
3155 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
3156 {
3157         struct stlink_usb_handle_s *h = handle;
3158         int speed_index;
3159         struct speed_map map[STLINK_V3_MAX_FREQ_NB];
3160
3161         stlink_get_com_freq(h, is_jtag, map);
3162
3163         speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
3164
3165         if (!query) {
3166                 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
3167                 if (result != ERROR_OK) {
3168                         LOG_ERROR("Unable to set adapter speed");
3169                         return khz;
3170                 }
3171         }
3172         return map[speed_index].speed;
3173 }
3174
3175 static int stlink_speed(void *handle, int khz, bool query)
3176 {
3177         struct stlink_usb_handle_s *h = handle;
3178
3179         if (!handle)
3180                 return khz;
3181
3182         switch (h->st_mode) {
3183         case STLINK_MODE_DEBUG_SWIM:
3184                 return stlink_speed_swim(handle, khz, query);
3185         case STLINK_MODE_DEBUG_SWD:
3186                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
3187                         return stlink_speed_v3(handle, false, khz, query);
3188                 else
3189                         return stlink_speed_swd(handle, khz, query);
3190                 break;
3191         case STLINK_MODE_DEBUG_JTAG:
3192                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
3193                         return stlink_speed_v3(handle, true, khz, query);
3194                 else
3195                         return stlink_speed_jtag(handle, khz, query);
3196                 break;
3197         default:
3198                 break;
3199         }
3200
3201         return khz;
3202 }
3203
3204 /** */
3205 static int stlink_usb_usb_close(void *handle)
3206 {
3207         struct stlink_usb_handle_s *h = handle;
3208
3209         if (!h)
3210                 return ERROR_OK;
3211
3212         if (h->usb_backend_priv.fd) {
3213                 stlink_usb_exit_mode(h);
3214                 /* do not check return code, it prevent
3215                 us from closing jtag_libusb */
3216                 jtag_libusb_close(h->usb_backend_priv.fd);
3217         }
3218
3219         free(h->cmdbuf);
3220         free(h->databuf);
3221
3222         return ERROR_OK;
3223 }
3224
3225 /** */
3226 static int stlink_tcp_close(void *handle)
3227 {
3228         struct stlink_usb_handle_s *h = handle;
3229
3230         if (!h)
3231                 return ERROR_OK;
3232
3233         int ret = ERROR_OK;
3234         if (h->tcp_backend_priv.connected) {
3235                 if (h->tcp_backend_priv.connect_id) {
3236                         stlink_usb_exit_mode(h);
3237
3238                         /* close the stlink */
3239                         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_CLOSE_DEV;
3240                         memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
3241                         h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
3242                         ret = stlink_tcp_send_cmd(h, 8, 4, true);
3243                         if (ret != ERROR_OK)
3244                                 LOG_ERROR("cannot close the STLINK");
3245                 }
3246
3247                 if (close_socket(h->tcp_backend_priv.fd) != 0)
3248                         LOG_ERROR("error closing the socket, errno: %s", strerror(errno));
3249         }
3250
3251         free(h->tcp_backend_priv.send_buf);
3252         free(h->tcp_backend_priv.recv_buf);
3253
3254         return ret;
3255 }
3256
3257 /** */
3258 static int stlink_close(void *handle)
3259 {
3260         if (handle) {
3261                 struct stlink_usb_handle_s *h = handle;
3262
3263                 stlink_usb_close(handle);
3264
3265                 free(h);
3266         }
3267
3268         return ERROR_OK;
3269 }
3270
3271 /* Compute ST-Link serial number from the device descriptor
3272  * this function will help to work-around a bug in old ST-Link/V2 DFU
3273  * the buggy DFU returns an incorrect serial in the USB descriptor
3274  * example for the following serial "57FF72067265575742132067"
3275  *  - the correct descriptor serial is:
3276  *    0x32, 0x03, 0x35, 0x00, 0x37, 0x00, 0x46, 0x00, 0x46, 0x00, 0x37, 0x00, 0x32, 0x00 ...
3277  *    this contains the length (0x32 = 50), the type (0x3 = DT_STRING) and the serial in unicode format
3278  *    the serial part is: 0x0035, 0x0037, 0x0046, 0x0046, 0x0037, 0x0032 ... >>  57FF72 ...
3279  *    this format could be read correctly by 'libusb_get_string_descriptor_ascii'
3280  *    so this case is managed by libusb_helper::string_descriptor_equal
3281  *  - the buggy DFU is not doing any unicode conversion and returns a raw serial data in the descriptor
3282  *    0x1a, 0x03, 0x57, 0x00, 0xFF, 0x00, 0x72, 0x00 ...
3283  *            >>    57          FF          72       ...
3284  *    based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
3285  *    and then we have just to convert the raw data into printable characters using sprintf
3286  */
3287 static char *stlink_usb_get_alternate_serial(struct libusb_device_handle *device,
3288                 struct libusb_device_descriptor *dev_desc)
3289 {
3290         int usb_retval;
3291         unsigned char desc_serial[(STLINK_SERIAL_LEN + 1) * 2];
3292
3293         if (dev_desc->iSerialNumber == 0)
3294                 return NULL;
3295
3296         /* get the LANGID from String Descriptor Zero */
3297         usb_retval = libusb_get_string_descriptor(device, 0, 0, desc_serial,
3298                         sizeof(desc_serial));
3299
3300         if (usb_retval < LIBUSB_SUCCESS) {
3301                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
3302                                 libusb_error_name(usb_retval), usb_retval);
3303                 return NULL;
3304         } else if (usb_retval < 4) {
3305                 /* the size should be least 4 bytes to contain a minimum of 1 supported LANGID */
3306                 LOG_ERROR("could not get the LANGID");
3307                 return NULL;
3308         }
3309
3310         uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
3311
3312         /* get the serial */
3313         usb_retval = libusb_get_string_descriptor(device, dev_desc->iSerialNumber,
3314                         langid, desc_serial, sizeof(desc_serial));
3315
3316         unsigned char len = desc_serial[0];
3317
3318         if (usb_retval < LIBUSB_SUCCESS) {
3319                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
3320                                 libusb_error_name(usb_retval), usb_retval);
3321                 return NULL;
3322         } else if (desc_serial[1] != LIBUSB_DT_STRING || len > usb_retval) {
3323                 LOG_ERROR("invalid string in ST-LINK USB serial descriptor");
3324                 return NULL;
3325         }
3326
3327         if (len == ((STLINK_SERIAL_LEN + 1) * 2)) {
3328                 /* good ST-Link adapter, this case is managed by
3329                  * libusb::libusb_get_string_descriptor_ascii */
3330                 return NULL;
3331         } else if (len != ((STLINK_SERIAL_LEN / 2 + 1) * 2)) {
3332                 LOG_ERROR("unexpected serial length (%d) in descriptor", len);
3333                 return NULL;
3334         }
3335
3336         /* else (len == 26) => buggy ST-Link */
3337
3338         char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
3339         if (!alternate_serial)
3340                 return NULL;
3341
3342         for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
3343                 sprintf(alternate_serial + i, "%02X", desc_serial[i + 2]);
3344
3345         alternate_serial[STLINK_SERIAL_LEN] = '\0';
3346
3347         return alternate_serial;
3348 }
3349
3350 /** */
3351 static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
3352 {
3353         struct stlink_usb_handle_s *h = handle;
3354         int err, retry_count = 1;
3355
3356         h->cmdbuf = malloc(STLINK_SG_SIZE);
3357         h->databuf = malloc(STLINK_DATA_SIZE);
3358
3359         if (!h->cmdbuf || !h->databuf)
3360                 return ERROR_FAIL;
3361
3362         /*
3363           On certain host USB configurations(e.g. MacBook Air)
3364           STLINKv2 dongle seems to have its FW in a funky state if,
3365           after plugging it in, you try to use openocd with it more
3366           then once (by launching and closing openocd). In cases like
3367           that initial attempt to read the FW info via
3368           stlink_usb_version will fail and the device has to be reset
3369           in order to become operational.
3370          */
3371         do {
3372                 if (jtag_libusb_open(param->vid, param->pid,
3373                                 &h->usb_backend_priv.fd, stlink_usb_get_alternate_serial) != ERROR_OK) {
3374                         LOG_ERROR("open failed");
3375                         return ERROR_FAIL;
3376                 }
3377
3378                 jtag_libusb_set_configuration(h->usb_backend_priv.fd, 0);
3379
3380                 if (libusb_claim_interface(h->usb_backend_priv.fd, 0) != ERROR_OK) {
3381                         LOG_DEBUG("claim interface failed");
3382                         return ERROR_FAIL;
3383                 }
3384
3385                 /* RX EP is common for all versions */
3386                 h->rx_ep = STLINK_RX_EP;
3387
3388                 uint16_t pid;
3389                 if (jtag_libusb_get_pid(libusb_get_device(h->usb_backend_priv.fd), &pid) != ERROR_OK) {
3390                         LOG_DEBUG("libusb_get_pid failed");
3391                         return ERROR_FAIL;
3392                 }
3393
3394                 /* wrap version for first read */
3395                 switch (pid) {
3396                         case STLINK_V1_PID:
3397                                 h->version.stlink = 1;
3398                                 h->tx_ep = STLINK_TX_EP;
3399                                 break;
3400                         case STLINK_V3_USBLOADER_PID:
3401                         case STLINK_V3E_PID:
3402                         case STLINK_V3S_PID:
3403                         case STLINK_V3_2VCP_PID:
3404                         case STLINK_V3E_NO_MSD_PID:
3405                                 h->version.stlink = 3;
3406                                 h->tx_ep = STLINK_V2_1_TX_EP;
3407                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
3408                                 break;
3409                         case STLINK_V2_1_PID:
3410                         case STLINK_V2_1_NO_MSD_PID:
3411                                 h->version.stlink = 2;
3412                                 h->tx_ep = STLINK_V2_1_TX_EP;
3413                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
3414                                 break;
3415                         default:
3416                         /* fall through - we assume V2 to be the default version*/
3417                         case STLINK_V2_PID:
3418                                 h->version.stlink = 2;
3419                                 h->tx_ep = STLINK_TX_EP;
3420                                 h->trace_ep = STLINK_TRACE_EP;
3421                                 break;
3422                 }
3423
3424                 /* get the device version */
3425                 err = stlink_usb_version(h);
3426
3427                 if (err == ERROR_OK) {
3428                         break;
3429                 } else if (h->version.stlink == 1 ||
3430                            retry_count == 0) {
3431                         LOG_ERROR("read version failed");
3432                         return ERROR_FAIL;
3433                 } else {
3434                         err = libusb_release_interface(h->usb_backend_priv.fd, 0);
3435                         if (err != ERROR_OK) {
3436                                 LOG_ERROR("release interface failed");
3437                                 return ERROR_FAIL;
3438                         }
3439
3440                         err = libusb_reset_device(h->usb_backend_priv.fd);
3441                         if (err != ERROR_OK) {
3442                                 LOG_ERROR("reset device failed");
3443                                 return ERROR_FAIL;
3444                         }
3445
3446                         jtag_libusb_close(h->usb_backend_priv.fd);
3447                         /*
3448                           Give the device one second to settle down and
3449                           reenumerate.
3450                          */
3451                         usleep(1 * 1000 * 1000);
3452                         retry_count--;
3453                 }
3454         } while (1);
3455
3456         return ERROR_OK;
3457 }
3458
3459 /** */
3460 static int stlink_tcp_open(void *handle, struct hl_interface_param_s *param)
3461 {
3462         struct stlink_usb_handle_s *h = handle;
3463         int ret;
3464
3465         /* SWIM is not supported using stlink-server */
3466         if (h->st_mode ==  STLINK_MODE_DEBUG_SWIM) {
3467                 LOG_ERROR("stlink-server does not support SWIM mode");
3468                 return ERROR_FAIL;
3469         }
3470
3471         h->tcp_backend_priv.send_buf = malloc(STLINK_TCP_SEND_BUFFER_SIZE);
3472         h->tcp_backend_priv.recv_buf = malloc(STLINK_TCP_RECV_BUFFER_SIZE);
3473
3474         if (!h->tcp_backend_priv.send_buf || !h->tcp_backend_priv.recv_buf)
3475                 return ERROR_FAIL;
3476
3477         h->cmdbuf = &h->tcp_backend_priv.send_buf[8];
3478         h->databuf = &h->tcp_backend_priv.recv_buf[4];
3479
3480         /* configure directions */
3481         h->rx_ep = STLINK_TCP_REQUEST_READ;
3482         h->tx_ep = STLINK_TCP_REQUEST_WRITE;
3483         h->trace_ep = STLINK_TCP_REQUEST_READ_SWO;
3484
3485         h->tcp_backend_priv.fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
3486         h->tcp_backend_priv.connected = false;
3487         h->tcp_backend_priv.device_id = 0;
3488         h->tcp_backend_priv.connect_id = 0;
3489
3490         if (h->tcp_backend_priv.fd == -1) {
3491                 LOG_ERROR("error creating the socket, errno: %s", strerror(errno));
3492                 return ERROR_FAIL;
3493         }
3494
3495         struct sockaddr_in serv;
3496         memset(&serv, 0, sizeof(struct sockaddr_in));
3497         serv.sin_family = AF_INET;
3498         serv.sin_port = htons(param->stlink_tcp_port);
3499         serv.sin_addr.s_addr = inet_addr("127.0.0.1");
3500
3501         LOG_DEBUG("socket : %x", h->tcp_backend_priv.fd);
3502
3503         int optval = 1;
3504         if (setsockopt(h->tcp_backend_priv.fd, IPPROTO_TCP, TCP_NODELAY, (const void *)&optval, sizeof(int)) == -1) {
3505                 LOG_ERROR("cannot set sock option 'TCP_NODELAY', errno: %s", strerror(errno));
3506                 return ERROR_FAIL;
3507         }
3508
3509         optval = STLINK_TCP_RECV_BUFFER_SIZE;
3510         if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_RCVBUF, (const void *)&optval, sizeof(int)) == -1) {
3511                 LOG_ERROR("cannot set sock option 'SO_RCVBUF', errno: %s", strerror(errno));
3512                 return ERROR_FAIL;
3513         }
3514
3515         optval = STLINK_TCP_SEND_BUFFER_SIZE;
3516         if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_SNDBUF, (const void *)&optval, sizeof(int)) == -1) {
3517                 LOG_ERROR("cannot set sock option 'SO_SNDBUF', errno: %s", strerror(errno));
3518                 return ERROR_FAIL;
3519         }
3520
3521         if (connect(h->tcp_backend_priv.fd, (const struct sockaddr *)&serv, sizeof(serv)) == -1) {
3522                 LOG_ERROR("cannot connect to stlink server, errno: %s", strerror(errno));
3523                 return ERROR_FAIL;
3524         }
3525
3526         h->tcp_backend_priv.connected = true;
3527
3528         LOG_INFO("connected to stlink-server");
3529
3530         /* print stlink-server version */
3531         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_SERVER_VERSION;
3532         h->tcp_backend_priv.send_buf[1] = OPENOCD_STLINK_TCP_API_VERSION;
3533         memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
3534         ret = stlink_tcp_send_cmd(h, 4, 16, false);
3535         if (ret != ERROR_OK) {
3536                 LOG_ERROR("cannot get the stlink-server version");
3537                 return ERROR_FAIL;
3538         }
3539
3540         h->tcp_backend_priv.version.api = le_to_h_u32(&h->tcp_backend_priv.recv_buf[0]);
3541         h->tcp_backend_priv.version.major = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3542         h->tcp_backend_priv.version.minor = le_to_h_u32(&h->tcp_backend_priv.recv_buf[8]);
3543         h->tcp_backend_priv.version.build = le_to_h_u32(&h->tcp_backend_priv.recv_buf[12]);
3544         LOG_INFO("stlink-server API v%d, version %d.%d.%d",
3545                         h->tcp_backend_priv.version.api,
3546                         h->tcp_backend_priv.version.major,
3547                         h->tcp_backend_priv.version.minor,
3548                         h->tcp_backend_priv.version.build);
3549
3550         /* in stlink-server API v1 sending more than 1428 bytes will cause stlink-server
3551          * to crash in windows: select a safe default value (1K) */
3552         if (h->tcp_backend_priv.version.api < 2)
3553                 h->max_mem_packet = (1 << 10);
3554
3555         /* refresh stlink list (re-enumerate) */
3556         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_REFRESH_DEVICE_LIST;
3557         h->tcp_backend_priv.send_buf[1] = 0; /* don't clear the list, just refresh it */
3558         ret = stlink_tcp_send_cmd(h, 2, 4, true);
3559         if (ret != ERROR_OK)
3560                 return ret;
3561
3562         /* get the number of connected stlinks */
3563         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_NB_DEV;
3564         ret = stlink_tcp_send_cmd(h, 1, 4, false);
3565         if (ret != ERROR_OK)
3566                 return ret;
3567
3568         uint32_t connected_stlinks = le_to_h_u32(h->tcp_backend_priv.recv_buf);
3569
3570         if (connected_stlinks == 0) {
3571                 LOG_ERROR("no ST-LINK detected");
3572                 return ERROR_FAIL;
3573         }
3574
3575         LOG_DEBUG("%d ST-LINK detected", connected_stlinks);
3576
3577         if (connected_stlinks > 255) {
3578                 LOG_WARNING("STLink server cannot handle more than 255 ST-LINK connected");
3579                 connected_stlinks = 255;
3580         }
3581
3582         /* list all connected ST-Link and seek for the requested vid:pid and serial */
3583         char serial[STLINK_TCP_SERIAL_SIZE + 1] = {0};
3584         uint8_t stlink_used;
3585         bool stlink_id_matched = false;
3586         const char *adapter_serial = adapter_get_required_serial();
3587         bool stlink_serial_matched = !adapter_serial;
3588
3589         for (uint32_t stlink_id = 0; stlink_id < connected_stlinks; stlink_id++) {
3590                 /* get the stlink info */
3591                 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_DEV_INFO;
3592                 h->tcp_backend_priv.send_buf[1] = (uint8_t)stlink_id;
3593                 memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
3594                 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], 41); /* size of TDeviceInfo2 */
3595                 ret = stlink_tcp_send_cmd(h, 8, 45, true);
3596                 if (ret != ERROR_OK)
3597                         return ret;
3598
3599                 h->tcp_backend_priv.device_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3600                 memcpy(serial, &h->tcp_backend_priv.recv_buf[8], STLINK_TCP_SERIAL_SIZE);
3601                 h->vid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[40]);
3602                 h->pid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[42]);
3603                 stlink_used = h->tcp_backend_priv.recv_buf[44];
3604
3605                 /* check the vid:pid */
3606                 for (int i = 0; param->vid[i]; i++) {
3607                         if (param->vid[i] == h->vid && param->pid[i] == h->pid) {
3608                                 stlink_id_matched = true;
3609                                 break;
3610                         }
3611                 }
3612
3613                 if (!stlink_id_matched)
3614                         continue;
3615
3616                 /* check the serial if specified */
3617                 if (adapter_serial) {
3618                         /* ST-Link server fixes the buggy serial returned by old ST-Link DFU
3619                          * for further details refer to stlink_usb_get_alternate_serial
3620                          * so if the user passes the buggy serial, we need to fix it before
3621                          * comparing with the serial returned by ST-Link server */
3622                         if (strlen(adapter_serial) == STLINK_SERIAL_LEN / 2) {
3623                                 char fixed_serial[STLINK_SERIAL_LEN + 1];
3624
3625                                 for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
3626                                         sprintf(fixed_serial + i, "%02X", adapter_serial[i / 2]);
3627
3628                                 fixed_serial[STLINK_SERIAL_LEN] = '\0';
3629
3630                                 stlink_serial_matched = strcmp(fixed_serial, serial) == 0;
3631                         } else {
3632                                 stlink_serial_matched = strcmp(adapter_serial, serial) == 0;
3633                         }
3634                 }
3635
3636                 if (!stlink_serial_matched)
3637                         LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
3638                                         serial, adapter_serial);
3639                 else /* exit the search loop if there is match */
3640                         break;
3641         }
3642
3643         if (!stlink_id_matched) {
3644                 LOG_ERROR("ST-LINK open failed (vid/pid mismatch)");
3645                 return ERROR_FAIL;
3646         }
3647
3648         if (!stlink_serial_matched) {
3649                 LOG_ERROR("ST-LINK open failed (serial mismatch)");
3650                 return ERROR_FAIL;
3651         }
3652
3653         /* check if device is 'exclusively' used by another application */
3654         if (stlink_used) {
3655                 LOG_ERROR("the selected device is already used");
3656                 return ERROR_FAIL;
3657         }
3658
3659         LOG_DEBUG("transport: vid: 0x%04x pid: 0x%04x serial: %s", h->vid, h->pid, serial);
3660
3661         /* now let's open the stlink */
3662         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_OPEN_DEV;
3663         memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
3664         h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.device_id);
3665         ret = stlink_tcp_send_cmd(h, 8, 8, true);
3666         if (ret != ERROR_OK)
3667                 return ret;
3668
3669         h->tcp_backend_priv.connect_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3670
3671         /* get stlink version */
3672         return stlink_usb_version(h);
3673 }
3674
3675 static struct stlink_backend_s stlink_usb_backend = {
3676         .open = stlink_usb_usb_open,
3677         .close = stlink_usb_usb_close,
3678         .xfer_noerrcheck = stlink_usb_usb_xfer_noerrcheck,
3679         .read_trace = stlink_usb_usb_read_trace,
3680 };
3681
3682 static struct stlink_backend_s stlink_tcp_backend = {
3683         .open = stlink_tcp_open,
3684         .close = stlink_tcp_close,
3685         .xfer_noerrcheck = stlink_tcp_xfer_noerrcheck,
3686         .read_trace = stlink_tcp_read_trace,
3687 };
3688
3689 static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode, void **fd)
3690 {
3691         struct stlink_usb_handle_s *h;
3692
3693         LOG_DEBUG("stlink_open");
3694
3695         h = calloc(1, sizeof(struct stlink_usb_handle_s));
3696
3697         if (h == 0) {
3698                 LOG_DEBUG("malloc failed");
3699                 return ERROR_FAIL;
3700         }
3701
3702         h->st_mode = mode;
3703
3704         for (unsigned i = 0; param->vid[i]; i++) {
3705                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
3706                           h->st_mode, param->vid[i], param->pid[i],
3707                           adapter_get_required_serial() ? adapter_get_required_serial() : "");
3708         }
3709
3710         if (param->use_stlink_tcp)
3711                 h->backend = &stlink_tcp_backend;
3712         else
3713                 h->backend = &stlink_usb_backend;
3714
3715         if (stlink_usb_open(h, param) != ERROR_OK)
3716                 goto error_open;
3717
3718         /* check if mode is supported */
3719         int err = ERROR_OK;
3720
3721         switch (h->st_mode) {
3722                 case STLINK_MODE_DEBUG_SWD:
3723                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
3724                                 err = ERROR_FAIL;
3725                         /* fall-through */
3726                 case STLINK_MODE_DEBUG_JTAG:
3727                         if (h->version.jtag == 0)
3728                                 err = ERROR_FAIL;
3729                         break;
3730                 case STLINK_MODE_DEBUG_SWIM:
3731                         if (h->version.swim == 0)
3732                                 err = ERROR_FAIL;
3733                         break;
3734                 default:
3735                         err = ERROR_FAIL;
3736                         break;
3737         }
3738
3739         if (err != ERROR_OK) {
3740                 LOG_ERROR("mode (transport) not supported by device");
3741                 goto error_open;
3742         }
3743
3744         /* initialize the debug hardware */
3745         err = stlink_usb_init_mode(h, param->connect_under_reset, param->initial_interface_speed);
3746
3747         if (err != ERROR_OK) {
3748                 LOG_ERROR("init mode failed (unable to connect to the target)");
3749                 goto error_open;
3750         }
3751
3752         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
3753                 err = stlink_swim_enter(h);
3754                 if (err != ERROR_OK) {
3755                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
3756                         goto error_open;
3757                 }
3758                 *fd = h;
3759                 h->max_mem_packet = STLINK_SWIM_DATA_SIZE;
3760                 return ERROR_OK;
3761         }
3762
3763         /* set max_mem_packet if it was not set by the low-level interface */
3764         if (h->max_mem_packet == 0) {
3765                 /* get cpuid, so we can determine the max page size
3766                  * start with a safe default */
3767                 h->max_mem_packet = (1 << 10);
3768
3769                 uint8_t buffer[4];
3770                 stlink_usb_open_ap(h, STLINK_HLA_AP_NUM);
3771                 err = stlink_usb_read_mem32(h, STLINK_HLA_AP_NUM, STLINK_HLA_CSW, CPUID, 4, buffer);
3772                 if (err == ERROR_OK) {
3773                         uint32_t cpuid = le_to_h_u32(buffer);
3774                         int i = (cpuid >> 4) & 0xf;
3775                         if (i == 4 || i == 3) {
3776                                 /* Cortex-M3/M4 has 4096 bytes autoincrement range */
3777                                 h->max_mem_packet = (1 << 12);
3778                         }
3779                 }
3780
3781                 LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
3782         }
3783
3784         *fd = h;
3785
3786         return ERROR_OK;
3787
3788 error_open:
3789         stlink_close(h);
3790         return ERROR_FAIL;
3791 }
3792
3793 static int stlink_usb_hl_open(struct hl_interface_param_s *param, void **fd)
3794 {
3795         return stlink_open(param, stlink_get_mode(param->transport), fd);
3796 }
3797
3798 static int stlink_config_trace(void *handle, bool enabled,
3799                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3800                 unsigned int *trace_freq, unsigned int traceclkin_freq,
3801                 uint16_t *prescaler)
3802 {
3803         struct stlink_usb_handle_s *h = handle;
3804
3805         if (!(h->version.flags & STLINK_F_HAS_TRACE)) {
3806                 LOG_ERROR("The attached ST-LINK version doesn't support trace");
3807                 return ERROR_FAIL;
3808         }
3809
3810         if (!enabled) {
3811                 stlink_usb_trace_disable(h);
3812                 return ERROR_OK;
3813         }
3814
3815         assert(trace_freq);
3816         assert(prescaler);
3817
3818         if (pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART) {
3819                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
3820                 return ERROR_FAIL;
3821         }
3822
3823         unsigned int max_trace_freq = (h->version.stlink == 3) ?
3824                         STLINK_V3_TRACE_MAX_HZ : STLINK_TRACE_MAX_HZ;
3825
3826         /* Only concern ourselves with the frequency if the STlink is processing it. */
3827         if (*trace_freq > max_trace_freq) {
3828                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
3829                           max_trace_freq);
3830                 return ERROR_FAIL;
3831         }
3832
3833         if (!*trace_freq)
3834                 *trace_freq = max_trace_freq;
3835
3836         unsigned int presc = (traceclkin_freq + *trace_freq / 2) / *trace_freq;
3837         if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1) {
3838                 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3839                         "frequency.");
3840                 return ERROR_FAIL;
3841         }
3842
3843         /* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
3844         unsigned int max_deviation = (traceclkin_freq * 3) / 100;
3845         if (presc * *trace_freq < traceclkin_freq - max_deviation ||
3846                         presc * *trace_freq > traceclkin_freq + max_deviation) {
3847                 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3848                         "frequency.");
3849                 return ERROR_FAIL;
3850         }
3851
3852         *prescaler = presc;
3853
3854         stlink_usb_trace_disable(h);
3855
3856         h->trace.source_hz = *trace_freq;
3857
3858         return stlink_usb_trace_enable(h);
3859 }
3860
3861 /** */
3862 static int stlink_usb_init_access_port(void *handle, unsigned char ap_num)
3863 {
3864         struct stlink_usb_handle_s *h = handle;
3865
3866         assert(handle);
3867
3868         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3869                 return ERROR_COMMAND_NOTFOUND;
3870
3871         LOG_DEBUG_IO("init ap_num = %d", ap_num);
3872         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3873         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3874         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_INIT_AP;
3875         h->cmdbuf[h->cmdidx++] = ap_num;
3876
3877         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3878 }
3879
3880 /** */
3881 static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
3882 {
3883         struct stlink_usb_handle_s *h = handle;
3884
3885         assert(handle);
3886
3887         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3888                 return ERROR_COMMAND_NOTFOUND;
3889
3890         LOG_DEBUG_IO("close ap_num = %d", ap_num);
3891         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3892         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3893         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_CLOSE_AP_DBG;
3894         h->cmdbuf[h->cmdidx++] = ap_num;
3895
3896         /* ignore incorrectly returned error on bogus FW */
3897         if (h->version.flags & STLINK_F_FIX_CLOSE_AP)
3898                 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3899         else
3900                 return stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
3901
3902 }
3903
3904 static int stlink_usb_rw_misc_out(void *handle, uint32_t items, const uint8_t *buffer)
3905 {
3906         struct stlink_usb_handle_s *h = handle;
3907         unsigned int buflen = ALIGN_UP(items, 4) + 4 * items;
3908
3909         LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
3910
3911         assert(handle != NULL);
3912
3913         if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
3914                 return ERROR_COMMAND_NOTFOUND;
3915
3916         stlink_usb_init_buffer(handle, h->tx_ep, buflen);
3917
3918         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3919         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_OUT;
3920         h_u32_to_le(&h->cmdbuf[2], items);
3921
3922         return stlink_usb_xfer_noerrcheck(handle, buffer, buflen);
3923 }
3924
3925 static int stlink_usb_rw_misc_in(void *handle, uint32_t items, uint8_t *buffer)
3926 {
3927         struct stlink_usb_handle_s *h = handle;
3928         unsigned int buflen = 2 * 4 * items;
3929
3930         LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
3931
3932         assert(handle != NULL);
3933
3934         if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
3935                 return ERROR_COMMAND_NOTFOUND;
3936
3937         stlink_usb_init_buffer(handle, h->rx_ep, buflen);
3938
3939         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3940         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_IN;
3941
3942         int res = stlink_usb_xfer_noerrcheck(handle, h->databuf, buflen);
3943         if (res != ERROR_OK)
3944                 return res;
3945
3946         memcpy(buffer, h->databuf, buflen);
3947
3948         return ERROR_OK;
3949 }
3950
3951 /** */
3952 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
3953                         unsigned short addr, uint32_t *val)
3954 {
3955         struct stlink_usb_handle_s *h = handle;
3956         int retval;
3957
3958         assert(handle);
3959
3960         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3961                 return ERROR_COMMAND_NOTFOUND;
3962
3963         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3964         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3965         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_DAP_REG;
3966         h_u16_to_le(&h->cmdbuf[2], dap_port);
3967         h_u16_to_le(&h->cmdbuf[4], addr);
3968
3969         retval = stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3970         *val = le_to_h_u32(h->databuf + 4);
3971         LOG_DEBUG_IO("dap_port_read = %d, addr =  0x%x, value = 0x%" PRIx32, dap_port, addr, *val);
3972         return retval;
3973 }
3974
3975 /** */
3976 static int stlink_write_dap_register(void *handle, unsigned short dap_port,
3977                         unsigned short addr, uint32_t val)
3978 {
3979         struct stlink_usb_handle_s *h = handle;
3980
3981         assert(handle);
3982
3983         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3984                 return ERROR_COMMAND_NOTFOUND;
3985
3986         LOG_DEBUG_IO("dap_write port = %d, addr = 0x%x, value = 0x%" PRIx32, dap_port, addr, val);
3987         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3988         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3989         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITE_DAP_REG;
3990         h_u16_to_le(&h->cmdbuf[2], dap_port);
3991         h_u16_to_le(&h->cmdbuf[4], addr);
3992         h_u32_to_le(&h->cmdbuf[6], val);
3993         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3994 }
3995
3996 /** */
3997 struct hl_layout_api_s stlink_usb_layout_api = {
3998         /** */
3999         .open = stlink_usb_hl_open,
4000         /** */
4001         .close = stlink_close,
4002         /** */
4003         .idcode = stlink_usb_idcode,
4004         /** */
4005         .state = stlink_usb_state,
4006         /** */
4007         .reset = stlink_usb_reset,
4008         /** */
4009         .assert_srst = stlink_usb_assert_srst,
4010         /** */
4011         .run = stlink_usb_run,
4012         /** */
4013         .halt = stlink_usb_halt,
4014         /** */
4015         .step = stlink_usb_step,
4016         /** */
4017         .read_regs = stlink_usb_read_regs,
4018         /** */
4019         .read_reg = stlink_usb_read_reg,
4020         /** */
4021         .write_reg = stlink_usb_write_reg,
4022         /** */
4023         .read_mem = stlink_usb_read_mem,
4024         /** */
4025         .write_mem = stlink_usb_write_mem,
4026         /** */
4027         .write_debug_reg = stlink_usb_write_debug_reg,
4028         /** */
4029         .override_target = stlink_usb_override_target,
4030         /** */
4031         .speed = stlink_speed,
4032         /** */
4033         .config_trace = stlink_config_trace,
4034         /** */
4035         .poll_trace = stlink_usb_trace_read,
4036 };
4037
4038 /*****************************************************************************
4039  * DAP direct interface
4040  */
4041
4042 static struct stlink_usb_handle_s *stlink_dap_handle;
4043 static struct hl_interface_param_s stlink_dap_param;
4044 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
4045 static uint32_t last_csw_default[DP_APSEL_MAX + 1];
4046 static int stlink_dap_error = ERROR_OK;
4047
4048 /** */
4049 static int stlink_dap_record_error(int error)
4050 {
4051         if (stlink_dap_error == ERROR_OK)
4052                 stlink_dap_error = error;
4053         return ERROR_OK;
4054 }
4055
4056 /** */
4057 static int stlink_dap_get_and_clear_error(void)
4058 {
4059         int retval = stlink_dap_error;
4060         stlink_dap_error = ERROR_OK;
4061         return retval;
4062 }
4063
4064 static int stlink_dap_get_error(void)
4065 {
4066         return stlink_dap_error;
4067 }
4068
4069 static int stlink_usb_open_ap(void *handle, unsigned short apsel)
4070 {
4071         struct stlink_usb_handle_s *h = handle;
4072         int retval;
4073
4074         /* nothing to do on old versions */
4075         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
4076                 return ERROR_OK;
4077
4078         if (apsel > DP_APSEL_MAX)
4079                 return ERROR_FAIL;
4080
4081         if (test_bit(apsel, opened_ap))
4082                 return ERROR_OK;
4083
4084         retval = stlink_usb_init_access_port(h, apsel);
4085         if (retval != ERROR_OK)
4086                 return retval;
4087
4088         LOG_DEBUG("AP %d enabled", apsel);
4089         set_bit(apsel, opened_ap);
4090         last_csw_default[apsel] = 0;
4091         return ERROR_OK;
4092 }
4093
4094 static int stlink_dap_open_ap(unsigned short apsel)
4095 {
4096         return stlink_usb_open_ap(stlink_dap_handle, apsel);
4097 }
4098
4099 /** */
4100 static int stlink_dap_closeall_ap(void)
4101 {
4102         int retval, apsel;
4103
4104         /* nothing to do on old versions */
4105         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
4106                 return ERROR_OK;
4107
4108         for (apsel = 0; apsel <= DP_APSEL_MAX; apsel++) {
4109                 if (!test_bit(apsel, opened_ap))
4110                         continue;
4111                 retval = stlink_usb_close_access_port(stlink_dap_handle, apsel);
4112                 if (retval != ERROR_OK)
4113                         return retval;
4114                 clear_bit(apsel, opened_ap);
4115         }
4116         return ERROR_OK;
4117 }
4118
4119 /** */
4120 static int stlink_dap_reinit_interface(void)
4121 {
4122         int retval;
4123
4124         /*
4125          * On JTAG only, it should be enough to call stlink_usb_reset(). But on
4126          * some firmware version it does not work as expected, and there is no
4127          * equivalent for SWD.
4128          * At least for now, to reset the interface quit from JTAG/SWD mode then
4129          * select the mode again.
4130          */
4131
4132         if (!stlink_dap_handle->reconnect_pending) {
4133                 stlink_dap_handle->reconnect_pending = true;
4134                 stlink_usb_mode_leave(stlink_dap_handle, stlink_dap_handle->st_mode);
4135         }
4136
4137         retval = stlink_usb_mode_enter(stlink_dap_handle, stlink_dap_handle->st_mode);
4138         if (retval != ERROR_OK)
4139                 return retval;
4140
4141         stlink_dap_handle->reconnect_pending = false;
4142         /* on new FW, calling mode-leave closes all the opened AP; reopen them! */
4143         if (stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT)
4144                 for (unsigned int apsel = 0; apsel <= DP_APSEL_MAX; apsel++)
4145                         if (test_bit(apsel, opened_ap)) {
4146                                 clear_bit(apsel, opened_ap);
4147                                 stlink_dap_open_ap(apsel);
4148                         }
4149         return ERROR_OK;
4150 }
4151
4152 /** */
4153 static int stlink_dap_op_connect(struct adiv5_dap *dap)
4154 {
4155         uint32_t idcode;
4156         int retval;
4157
4158         LOG_INFO("stlink_dap_op_connect(%sconnect)", dap->do_reconnect ? "re" : "");
4159
4160         /* Check if we should reset srst already when connecting, but not if reconnecting. */
4161         if (!dap->do_reconnect) {
4162                 enum reset_types jtag_reset_config = jtag_get_reset_config();
4163
4164                 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
4165                         if (jtag_reset_config & RESET_SRST_NO_GATING)
4166                                 adapter_assert_reset();
4167                         else
4168                                 LOG_WARNING("\'srst_nogate\' reset_config option is required");
4169                 }
4170         }
4171
4172         dap->do_reconnect = false;
4173         dap_invalidate_cache(dap);
4174         for (unsigned int i = 0; i <= DP_APSEL_MAX; i++)
4175                 last_csw_default[i] = 0;
4176
4177         retval = dap_dp_init(dap);
4178         if (retval != ERROR_OK) {
4179                 dap->do_reconnect = true;
4180                 return retval;
4181         }
4182
4183         retval = stlink_usb_idcode(stlink_dap_handle, &idcode);
4184         if (retval == ERROR_OK)
4185                 LOG_INFO("%s %#8.8" PRIx32,
4186                         (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) ? "JTAG IDCODE" : "SWD DPIDR",
4187                         idcode);
4188         else
4189                 dap->do_reconnect = true;
4190
4191         return retval;
4192 }
4193
4194 /** */
4195 static int stlink_dap_check_reconnect(struct adiv5_dap *dap)
4196 {
4197         int retval;
4198
4199         if (!dap->do_reconnect)
4200                 return ERROR_OK;
4201
4202         retval = stlink_dap_reinit_interface();
4203         if (retval != ERROR_OK)
4204                 return retval;
4205
4206         return stlink_dap_op_connect(dap);
4207 }
4208
4209 /** */
4210 static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
4211 {
4212         /* Ignore the request */
4213         return ERROR_OK;
4214 }
4215
4216 /** */
4217 static int stlink_dap_dp_read(struct adiv5_dap *dap, unsigned int reg, uint32_t *data)
4218 {
4219         uint32_t dummy;
4220         int retval;
4221
4222         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
4223                 if (reg & 0x000000F0) {
4224                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
4225                         return ERROR_COMMAND_NOTFOUND;
4226                 }
4227
4228         data = data ? data : &dummy;
4229         if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ
4230                 && stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) {
4231                 /* Quirk required in JTAG. Read RDBUFF to get the data */
4232                 retval = stlink_read_dap_register(stlink_dap_handle,
4233                                         STLINK_DEBUG_PORT_ACCESS, reg, &dummy);
4234                 if (retval == ERROR_OK)
4235                         retval = stlink_read_dap_register(stlink_dap_handle,
4236                                                 STLINK_DEBUG_PORT_ACCESS, DP_RDBUFF, data);
4237         } else {
4238                 retval = stlink_read_dap_register(stlink_dap_handle,
4239                                         STLINK_DEBUG_PORT_ACCESS, reg, data);
4240         }
4241
4242         return retval;
4243 }
4244
4245 /** */
4246 static int stlink_dap_dp_write(struct adiv5_dap *dap, unsigned int reg, uint32_t data)
4247 {
4248         int retval;
4249
4250         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
4251                 if (reg & 0x000000F0) {
4252                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
4253                         return ERROR_COMMAND_NOTFOUND;
4254                 }
4255
4256         if (reg == DP_SELECT && (data & DP_SELECT_DPBANK) != 0) {
4257                 /* ignored if STLINK_F_HAS_DPBANKSEL, not properly managed otherwise */
4258                 LOG_DEBUG("Ignoring DPBANKSEL while write SELECT");
4259                 data &= ~DP_SELECT_DPBANK;
4260         }
4261
4262         /* ST-Link does not like that we set CORUNDETECT */
4263         if (reg == DP_CTRL_STAT)
4264                 data &= ~CORUNDETECT;
4265
4266         retval = stlink_write_dap_register(stlink_dap_handle,
4267                                 STLINK_DEBUG_PORT_ACCESS, reg, data);
4268         return retval;
4269 }
4270
4271 /** */
4272 static int stlink_dap_ap_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
4273 {
4274         struct adiv5_dap *dap = ap->dap;
4275         uint32_t dummy;
4276         int retval;
4277
4278         if (is_adiv6(dap)) {
4279                 static bool error_flagged;
4280                 if (!error_flagged)
4281                         LOG_ERROR("ADIv6 dap not supported by stlink dap-direct mode");
4282                 error_flagged = true;
4283                 return ERROR_FAIL;
4284         }
4285
4286         if (reg != ADIV5_AP_REG_IDR) {
4287                 retval = stlink_dap_open_ap(ap->ap_num);
4288                 if (retval != ERROR_OK)
4289                         return retval;
4290         }
4291         data = data ? data : &dummy;
4292         retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
4293                                  data);
4294         dap->stlink_flush_ap_write = false;
4295         return retval;
4296 }
4297
4298 /** */
4299 static int stlink_dap_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
4300 {
4301         struct adiv5_dap *dap = ap->dap;
4302         int retval;
4303
4304         if (is_adiv6(dap)) {
4305                 static bool error_flagged;
4306                 if (!error_flagged)
4307                         LOG_ERROR("ADIv6 dap not supported by stlink dap-direct mode");
4308                 error_flagged = true;
4309                 return ERROR_FAIL;
4310         }
4311
4312         retval = stlink_dap_open_ap(ap->ap_num);
4313         if (retval != ERROR_OK)
4314                 return retval;
4315
4316         retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
4317                                 data);
4318         dap->stlink_flush_ap_write = true;
4319         return retval;
4320 }
4321
4322 /** */
4323 static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
4324 {
4325         LOG_WARNING("stlink_dap_op_queue_ap_abort()");
4326         return ERROR_OK;
4327 }
4328
4329 #define RW_MISC_CMD_ADDRESS     1
4330 #define RW_MISC_CMD_WRITE       2
4331 #define RW_MISC_CMD_READ        3
4332 #define RW_MISC_CMD_APNUM       5
4333
4334 static int stlink_usb_misc_rw_segment(void *handle, const struct dap_queue *q, unsigned int len, unsigned int items)
4335 {
4336         uint8_t buf[2 * 4 * items];
4337
4338         LOG_DEBUG("Queue: %u commands in %u items", len, items);
4339
4340         uint32_t ap_num = DP_APSEL_INVALID;
4341         unsigned int cmd_index = 0;
4342         unsigned int val_index = ALIGN_UP(items, 4);
4343         for (unsigned int i = 0; i < len; i++) {
4344                 if (ap_num != q[i].mem_ap.ap->ap_num) {
4345                         ap_num = q[i].mem_ap.ap->ap_num;
4346                         buf[cmd_index++] = RW_MISC_CMD_APNUM;
4347                         h_u32_to_le(&buf[val_index], ap_num);
4348                         val_index += 4;
4349                 }
4350
4351                 switch (q[i].cmd) {
4352                 case CMD_MEM_AP_READ32:
4353                         buf[cmd_index++] = RW_MISC_CMD_READ;
4354                         h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
4355                         val_index += 4;
4356                         break;
4357                 case CMD_MEM_AP_WRITE32:
4358                         buf[cmd_index++] = RW_MISC_CMD_ADDRESS;
4359                         h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
4360                         val_index += 4;
4361                         buf[cmd_index++] = RW_MISC_CMD_WRITE;
4362                         h_u32_to_le(&buf[val_index], q[i].mem_ap.data);
4363                         val_index += 4;
4364                         break;
4365                 default:
4366                         /* Not supposed to happen */
4367                         return ERROR_FAIL;
4368                 }
4369         }
4370         /* pad after last command */
4371         while (!IS_ALIGNED(cmd_index, 4))
4372                 buf[cmd_index++] = 0;
4373
4374         int retval = stlink_usb_rw_misc_out(handle, items, buf);
4375         if (retval != ERROR_OK)
4376                 return retval;
4377
4378         retval = stlink_usb_rw_misc_in(handle, items, buf);
4379         if (retval != ERROR_OK)
4380                 return retval;
4381
4382         ap_num = DP_APSEL_INVALID;
4383         val_index = 0;
4384         unsigned int err_index = 4 * items;
4385         for (unsigned int i = 0; i < len; i++) {
4386                 uint32_t errcode = le_to_h_u32(&buf[err_index]);
4387                 if (errcode != STLINK_DEBUG_ERR_OK) {
4388                         LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4389                         return ERROR_FAIL;
4390                 }
4391                 if (ap_num != q[i].mem_ap.ap->ap_num) {
4392                         ap_num = q[i].mem_ap.ap->ap_num;
4393                         err_index += 4;
4394                         val_index += 4;
4395                         errcode = le_to_h_u32(&buf[err_index]);
4396                         if (errcode != STLINK_DEBUG_ERR_OK) {
4397                                 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4398                                 return ERROR_FAIL;
4399                         }
4400                 }
4401
4402                 if (q[i].cmd == CMD_MEM_AP_READ32) {
4403                         *q[i].mem_ap.p_data = le_to_h_u32(&buf[val_index]);
4404                 } else { /* q[i]->cmd == CMD_MEM_AP_WRITE32 */
4405                         err_index += 4;
4406                         val_index += 4;
4407                         errcode = le_to_h_u32(&buf[err_index]);
4408                         if (errcode != STLINK_DEBUG_ERR_OK) {
4409                                 LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
4410                                 return ERROR_FAIL;
4411                         }
4412                 }
4413                 err_index += 4;
4414                 val_index += 4;
4415         }
4416
4417         return ERROR_OK;
4418 }
4419
4420 static int stlink_usb_buf_rw_segment(void *handle, const struct dap_queue *q, unsigned int count)
4421 {
4422         uint32_t bufsize = count * CMD_MEM_AP_2_SIZE(q[0].cmd);
4423         uint8_t buf[bufsize];
4424         uint8_t ap_num = q[0].mem_ap.ap->ap_num;
4425         uint32_t addr = q[0].mem_ap.addr;
4426         uint32_t csw = q[0].mem_ap.csw;
4427
4428         int retval = stlink_dap_open_ap(ap_num);
4429         if (retval != ERROR_OK)
4430                 return retval;
4431
4432         switch (q[0].cmd) {
4433         case CMD_MEM_AP_WRITE8:
4434                 for (unsigned int i = 0; i < count; i++)
4435                         buf[i] = q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 3);
4436                 return stlink_usb_write_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4437
4438         case CMD_MEM_AP_WRITE16:
4439                 for (unsigned int i = 0; i < count; i++)
4440                         h_u16_to_le(&buf[2 * i], q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 2));
4441                 return stlink_usb_write_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4442
4443         case CMD_MEM_AP_WRITE32:
4444                 for (unsigned int i = 0; i < count; i++)
4445                         h_u32_to_le(&buf[4 * i], q[i].mem_ap.data);
4446                 if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4447                         return stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4448                 else
4449                         return stlink_usb_write_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4450
4451         case CMD_MEM_AP_READ8:
4452                 retval = stlink_usb_read_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4453                 if (retval == ERROR_OK)
4454                         for (unsigned int i = 0; i < count; i++)
4455                                 *q[i].mem_ap.p_data = buf[i] << 8 * (q[i].mem_ap.addr & 3);
4456                 return retval;
4457
4458         case CMD_MEM_AP_READ16:
4459                 retval = stlink_usb_read_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4460                 if (retval == ERROR_OK)
4461                         for (unsigned int i = 0; i < count; i++)
4462                                 *q[i].mem_ap.p_data = le_to_h_u16(&buf[2 * i]) << 8 * (q[i].mem_ap.addr & 2);
4463                 return retval;
4464
4465         case CMD_MEM_AP_READ32:
4466                 if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4467                         retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4468                 else
4469                         retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
4470                 if (retval == ERROR_OK)
4471                         for (unsigned int i = 0; i < count; i++)
4472                                 *q[i].mem_ap.p_data = le_to_h_u32(&buf[4 * i]);
4473                 return retval;
4474
4475         default:
4476                 return ERROR_FAIL;
4477         };
4478 }
4479
4480 /* TODO: recover these values with cmd STLINK_DEBUG_APIV2_RW_MISC_GET_MAX (0x53) */
4481 #define STLINK_V2_RW_MISC_SIZE (64)
4482 #define STLINK_V3_RW_MISC_SIZE (1227)
4483
4484 static int stlink_usb_count_misc_rw_queue(void *handle, const struct dap_queue *q, unsigned int len,
4485                 unsigned int *pkt_items)
4486 {
4487         struct stlink_usb_handle_s *h = handle;
4488         unsigned int i, items = 0;
4489         uint32_t ap_num = DP_APSEL_INVALID;
4490         unsigned int misc_max_items = (h->version.stlink == 2) ? STLINK_V2_RW_MISC_SIZE : STLINK_V3_RW_MISC_SIZE;
4491
4492         if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
4493                 return 0;
4494         /*
4495          * Before stlink-server API v3, RW_MISC sequence doesn't lock the st-link,
4496          * so are not safe in shared mode.
4497          * Don't use it with TCP backend to prevent any issue in case of sharing.
4498          * This further degrades the performance, on top of TCP server overhead.
4499          */
4500         if (h->backend == &stlink_tcp_backend && h->tcp_backend_priv.version.api < 3)
4501                 return 0;
4502
4503         for (i = 0; i < len; i++) {
4504                 if (q[i].cmd != CMD_MEM_AP_READ32 && q[i].cmd != CMD_MEM_AP_WRITE32)
4505                         break;
4506                 unsigned int count = 1;
4507                 if (ap_num != q[i].mem_ap.ap->ap_num) {
4508                         count++;
4509                         ap_num = q[i].mem_ap.ap->ap_num;
4510                 }
4511                 if (q[i].cmd == CMD_MEM_AP_WRITE32)
4512                         count++;
4513                 if (items + count > misc_max_items)
4514                         break;
4515                 items += count;
4516         }
4517
4518         *pkt_items = items;
4519
4520         return i;
4521 }
4522
4523 static int stlink_usb_count_buf_rw_queue(const struct dap_queue *q, unsigned int len)
4524 {
4525         uint32_t incr = CMD_MEM_AP_2_SIZE(q[0].cmd);
4526         unsigned int len_max;
4527
4528         if (incr == 1)
4529                 len_max = stlink_usb_block(stlink_dap_handle);
4530         else
4531                 len_max = STLINK_MAX_RW16_32 / incr;
4532
4533         /* check for no address increment, 32 bits only */
4534         if (len > 1 && incr == 4 && q[0].mem_ap.addr == q[1].mem_ap.addr)
4535                 incr = 0;
4536
4537         if (len > len_max)
4538                 len = len_max;
4539
4540         for (unsigned int i = 1; i < len; i++)
4541                 if (q[i].cmd != q[0].cmd ||
4542                         q[i].mem_ap.ap != q[0].mem_ap.ap ||
4543                         q[i].mem_ap.csw != q[0].mem_ap.csw ||
4544                         q[i].mem_ap.addr != q[i - 1].mem_ap.addr + incr)
4545                         return i;
4546
4547         return len;
4548 }
4549
4550 static int stlink_usb_mem_rw_queue(void *handle, const struct dap_queue *q, unsigned int len, unsigned int *skip)
4551 {
4552         unsigned int count, misc_items = 0;
4553         int retval;
4554
4555         unsigned int count_misc = stlink_usb_count_misc_rw_queue(handle, q, len, &misc_items);
4556         unsigned int count_buf = stlink_usb_count_buf_rw_queue(q, len);
4557
4558         if (count_misc > count_buf) {
4559                 count = count_misc;
4560                 retval = stlink_usb_misc_rw_segment(handle, q, count, misc_items);
4561         } else {
4562                 count = count_buf;
4563                 retval = stlink_usb_buf_rw_segment(handle, q, count_buf);
4564         }
4565         if (retval != ERROR_OK)
4566                 return retval;
4567
4568         *skip = count;
4569         return ERROR_OK;
4570 }
4571
4572 static void stlink_dap_run_internal(struct adiv5_dap *dap)
4573 {
4574         int retval = stlink_dap_check_reconnect(dap);
4575         if (retval != ERROR_OK) {
4576                 stlink_dap_handle->queue_index = 0;
4577                 stlink_dap_record_error(retval);
4578                 return;
4579         }
4580
4581         unsigned int i = stlink_dap_handle->queue_index;
4582         struct dap_queue *q = &stlink_dap_handle->queue[0];
4583
4584         while (i && stlink_dap_get_error() == ERROR_OK) {
4585                 unsigned int skip = 1;
4586
4587                 switch (q->cmd) {
4588                 case CMD_DP_READ:
4589                         retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data);
4590                         break;
4591                 case CMD_DP_WRITE:
4592                         retval = stlink_dap_dp_write(q->dp_w.dap, q->dp_w.reg, q->dp_w.data);
4593                         break;
4594                 case CMD_AP_READ:
4595                         retval = stlink_dap_ap_read(q->ap_r.ap, q->ap_r.reg, q->ap_r.p_data);
4596                         break;
4597                 case CMD_AP_WRITE:
4598                         /* ignore increment packed, not supported */
4599                         if (q->ap_w.reg == ADIV5_MEM_AP_REG_CSW)
4600                                 q->ap_w.data &= ~CSW_ADDRINC_PACKED;
4601                         retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data);
4602                         break;
4603
4604                 case CMD_MEM_AP_READ8:
4605                 case CMD_MEM_AP_READ16:
4606                 case CMD_MEM_AP_READ32:
4607                 case CMD_MEM_AP_WRITE8:
4608                 case CMD_MEM_AP_WRITE16:
4609                 case CMD_MEM_AP_WRITE32:
4610                         retval = stlink_usb_mem_rw_queue(stlink_dap_handle, q, i, &skip);
4611                         break;
4612
4613                 default:
4614                         LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd);
4615                         retval = ERROR_FAIL;
4616                         break;
4617                 }
4618                 stlink_dap_record_error(retval);
4619                 q += skip;
4620                 i -= skip;
4621         }
4622
4623         stlink_dap_handle->queue_index = 0;
4624 }
4625
4626 /** */
4627 static int stlink_dap_run_finalize(struct adiv5_dap *dap)
4628 {
4629         uint32_t ctrlstat, pwrmask;
4630         int retval, saved_retval;
4631
4632         /* Here no LOG_DEBUG. This is called continuously! */
4633
4634         /*
4635          * ST-Link returns immediately after a DAP write, without waiting for it
4636          * to complete.
4637          * Run a dummy read to DP_RDBUFF, as suggested in
4638          * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html
4639          */
4640         if (dap->stlink_flush_ap_write) {
4641                 dap->stlink_flush_ap_write = false;
4642                 retval = stlink_dap_dp_read(dap, DP_RDBUFF, NULL);
4643                 if (retval != ERROR_OK) {
4644                         dap->do_reconnect = true;
4645                         return retval;
4646                 }
4647         }
4648
4649         saved_retval = stlink_dap_get_and_clear_error();
4650
4651         retval = stlink_dap_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
4652         if (retval != ERROR_OK) {
4653                 LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
4654                 dap->do_reconnect = true;
4655                 return retval;
4656         }
4657
4658         if (ctrlstat & SSTICKYERR) {
4659                 if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG)
4660                         retval = stlink_dap_dp_write(dap, DP_CTRL_STAT,
4661                                         ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
4662                 else
4663                         retval = stlink_dap_dp_write(dap, DP_ABORT, STKERRCLR);
4664                 if (retval != ERROR_OK) {
4665                         dap->do_reconnect = true;
4666                         return retval;
4667                 }
4668         }
4669
4670         /* check for power lost */
4671         pwrmask = dap->dp_ctrl_stat & (CDBGPWRUPREQ | CSYSPWRUPREQ);
4672         if ((ctrlstat & pwrmask) != pwrmask)
4673                 dap->do_reconnect = true;
4674
4675         return saved_retval;
4676 }
4677
4678 static int stlink_dap_op_queue_run(struct adiv5_dap *dap)
4679 {
4680         stlink_dap_run_internal(dap);
4681         return stlink_dap_run_finalize(dap);
4682 }
4683
4684 /** */
4685 static void stlink_dap_op_quit(struct adiv5_dap *dap)
4686 {
4687         int retval;
4688
4689         retval = stlink_dap_closeall_ap();
4690         if (retval != ERROR_OK)
4691                 LOG_ERROR("Error closing APs");
4692 }
4693
4694 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned int reg,
4695         uint32_t *data)
4696 {
4697         if (stlink_dap_get_error() != ERROR_OK)
4698                 return ERROR_OK;
4699
4700         unsigned int i = stlink_dap_handle->queue_index++;
4701         struct dap_queue *q = &stlink_dap_handle->queue[i];
4702         q->cmd = CMD_DP_READ;
4703         q->dp_r.reg = reg;
4704         q->dp_r.dap = dap;
4705         q->dp_r.p_data = data;
4706
4707         if (i == MAX_QUEUE_DEPTH - 1)
4708                 stlink_dap_run_internal(dap);
4709
4710         return ERROR_OK;
4711 }
4712
4713 static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned int reg,
4714         uint32_t data)
4715 {
4716         if (stlink_dap_get_error() != ERROR_OK)
4717                 return ERROR_OK;
4718
4719         unsigned int i = stlink_dap_handle->queue_index++;
4720         struct dap_queue *q = &stlink_dap_handle->queue[i];
4721         q->cmd = CMD_DP_WRITE;
4722         q->dp_w.reg = reg;
4723         q->dp_w.dap = dap;
4724         q->dp_w.data = data;
4725
4726         if (i == MAX_QUEUE_DEPTH - 1)
4727                 stlink_dap_run_internal(dap);
4728
4729         return ERROR_OK;
4730 }
4731
4732 static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned int reg,
4733         uint32_t *data)
4734 {
4735         if (stlink_dap_get_error() != ERROR_OK)
4736                 return ERROR_OK;
4737
4738         unsigned int i = stlink_dap_handle->queue_index++;
4739         struct dap_queue *q = &stlink_dap_handle->queue[i];
4740
4741         /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_RD_NO_INC
4742          * and STLINK_F_HAS_RW_MISC */
4743         if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
4744                         (reg == ADIV5_MEM_AP_REG_DRW || reg == ADIV5_MEM_AP_REG_BD0 || reg == ADIV5_MEM_AP_REG_BD1 ||
4745                          reg == ADIV5_MEM_AP_REG_BD2 || reg == ADIV5_MEM_AP_REG_BD3)) {
4746                 /* de-queue previous write-TAR */
4747                 struct dap_queue *prev_q = q - 1;
4748                 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_TAR) {
4749                         stlink_dap_handle->queue_index = i;
4750                         i--;
4751                         q = prev_q;
4752                         prev_q--;
4753                 }
4754                 /* de-queue previous write-CSW if it didn't changed ap->csw_default */
4755                 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_CSW &&
4756                                 !prev_q->ap_w.changes_csw_default) {
4757                         stlink_dap_handle->queue_index = i;
4758                         q = prev_q;
4759                 }
4760
4761                 switch (ap->csw_value & CSW_SIZE_MASK) {
4762                 case CSW_8BIT:
4763                         q->cmd = CMD_MEM_AP_READ8;
4764                         break;
4765                 case CSW_16BIT:
4766                         q->cmd = CMD_MEM_AP_READ16;
4767                         break;
4768                 case CSW_32BIT:
4769                         q->cmd = CMD_MEM_AP_READ32;
4770                         break;
4771                 default:
4772                         LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
4773                         stlink_dap_record_error(ERROR_FAIL);
4774                         return ERROR_FAIL;
4775                 }
4776
4777                 q->mem_ap.addr = (reg == ADIV5_MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
4778                 q->mem_ap.ap = ap;
4779                 q->mem_ap.p_data = data;
4780                 q->mem_ap.csw = ap->csw_default;
4781
4782                 /* force TAR and CSW update */
4783                 ap->tar_valid = false;
4784                 ap->csw_value = 0;
4785         } else {
4786                 q->cmd = CMD_AP_READ;
4787                 q->ap_r.reg = reg;
4788                 q->ap_r.ap = ap;
4789                 q->ap_r.p_data = data;
4790         }
4791
4792         if (i == MAX_QUEUE_DEPTH - 1)
4793                 stlink_dap_run_internal(ap->dap);
4794
4795         return ERROR_OK;
4796 }
4797
4798 static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned int reg,
4799         uint32_t data)
4800 {
4801         if (stlink_dap_get_error() != ERROR_OK)
4802                 return ERROR_OK;
4803
4804         unsigned int i = stlink_dap_handle->queue_index++;
4805         struct dap_queue *q = &stlink_dap_handle->queue[i];
4806
4807         /* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_WR_NO_INC
4808          * and STLINK_F_HAS_RW_MISC */
4809         if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
4810                         (reg == ADIV5_MEM_AP_REG_DRW || reg == ADIV5_MEM_AP_REG_BD0 || reg == ADIV5_MEM_AP_REG_BD1 ||
4811                          reg == ADIV5_MEM_AP_REG_BD2 || reg == ADIV5_MEM_AP_REG_BD3)) {
4812                 /* de-queue previous write-TAR */
4813                 struct dap_queue *prev_q = q - 1;
4814                 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_TAR) {
4815                         stlink_dap_handle->queue_index = i;
4816                         i--;
4817                         q = prev_q;
4818                         prev_q--;
4819                 }
4820                 /* de-queue previous write-CSW if it didn't changed ap->csw_default */
4821                 if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == ADIV5_MEM_AP_REG_CSW &&
4822                                 !prev_q->ap_w.changes_csw_default) {
4823                         stlink_dap_handle->queue_index = i;
4824                         q = prev_q;
4825                 }
4826
4827                 switch (ap->csw_value & CSW_SIZE_MASK) {
4828                 case CSW_8BIT:
4829                         q->cmd = CMD_MEM_AP_WRITE8;
4830                         break;
4831                 case CSW_16BIT:
4832                         q->cmd = CMD_MEM_AP_WRITE16;
4833                         break;
4834                 case CSW_32BIT:
4835                         q->cmd = CMD_MEM_AP_WRITE32;
4836                         break;
4837                 default:
4838                         LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
4839                         stlink_dap_record_error(ERROR_FAIL);
4840                         return ERROR_FAIL;
4841                 }
4842
4843                 q->mem_ap.addr = (reg == ADIV5_MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
4844                 q->mem_ap.ap = ap;
4845                 q->mem_ap.data = data;
4846                 q->mem_ap.csw = ap->csw_default;
4847
4848                 /* force TAR and CSW update */
4849                 ap->tar_valid = false;
4850                 ap->csw_value = 0;
4851         } else {
4852                 q->cmd = CMD_AP_WRITE;
4853                 q->ap_w.reg = reg;
4854                 q->ap_w.ap = ap;
4855                 q->ap_w.data = data;
4856                 uint8_t ap_num = ap->ap_num;
4857                 if (reg == ADIV5_MEM_AP_REG_CSW && ap->csw_default != last_csw_default[ap_num]) {
4858                         q->ap_w.changes_csw_default = true;
4859                         last_csw_default[ap_num] = ap->csw_default;
4860                 } else {
4861                         q->ap_w.changes_csw_default = false;
4862                 }
4863         }
4864
4865         if (i == MAX_QUEUE_DEPTH - 1)
4866                 stlink_dap_run_internal(ap->dap);
4867
4868         return ERROR_OK;
4869 }
4870
4871 static int stlink_swim_op_srst(void)
4872 {
4873         return stlink_swim_generate_rst(stlink_dap_handle);
4874 }
4875
4876 static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
4877                                                                    uint32_t count, uint8_t *buffer)
4878 {
4879         int retval;
4880         uint32_t bytes_remaining;
4881
4882         LOG_DEBUG_IO("read at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4883         count *= size;
4884
4885         while (count) {
4886                 bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
4887                 retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4888                 if (retval != ERROR_OK)
4889                         return retval;
4890
4891                 buffer += bytes_remaining;
4892                 addr += bytes_remaining;
4893                 count -= bytes_remaining;
4894         }
4895
4896         return ERROR_OK;
4897 }
4898
4899 static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
4900                                                                         uint32_t count, const uint8_t *buffer)
4901 {
4902         int retval;
4903         uint32_t bytes_remaining;
4904
4905         LOG_DEBUG_IO("write at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4906         count *= size;
4907
4908         while (count) {
4909                 bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
4910                 retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4911                 if (retval != ERROR_OK)
4912                         return retval;
4913
4914                 buffer += bytes_remaining;
4915                 addr += bytes_remaining;
4916                 count -= bytes_remaining;
4917         }
4918
4919         return ERROR_OK;
4920 }
4921
4922 static int stlink_swim_op_reconnect(void)
4923 {
4924         int retval;
4925
4926         retval = stlink_usb_mode_enter(stlink_dap_handle, STLINK_MODE_DEBUG_SWIM);
4927         if (retval != ERROR_OK)
4928                 return retval;
4929
4930         return stlink_swim_resync(stlink_dap_handle);
4931 }
4932
4933 static int stlink_dap_config_trace(bool enabled,
4934                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
4935                 unsigned int *trace_freq, unsigned int traceclkin_freq,
4936                 uint16_t *prescaler)
4937 {
4938         return stlink_config_trace(stlink_dap_handle, enabled, pin_protocol,
4939                                                            port_size, trace_freq, traceclkin_freq,
4940                                                            prescaler);
4941 }
4942
4943 static int stlink_dap_trace_read(uint8_t *buf, size_t *size)
4944 {
4945         return stlink_usb_trace_read(stlink_dap_handle, buf, size);
4946 }
4947
4948 /** */
4949 COMMAND_HANDLER(stlink_dap_vid_pid)
4950 {
4951         unsigned int i, max_usb_ids = HLA_MAX_USB_IDS;
4952
4953         if (CMD_ARGC > max_usb_ids * 2) {
4954                 LOG_WARNING("ignoring extra IDs in vid_pid "
4955                         "(maximum is %d pairs)", max_usb_ids);
4956                 CMD_ARGC = max_usb_ids * 2;
4957         }
4958         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
4959                 LOG_WARNING("incomplete vid_pid configuration directive");
4960                 return ERROR_COMMAND_SYNTAX_ERROR;
4961         }
4962         for (i = 0; i < CMD_ARGC; i += 2) {
4963                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], stlink_dap_param.vid[i / 2]);
4964                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], stlink_dap_param.pid[i / 2]);
4965         }
4966
4967         /* null termination */
4968         stlink_dap_param.vid[i / 2] = stlink_dap_param.pid[i / 2] = 0;
4969
4970         return ERROR_OK;
4971 }
4972
4973 /** */
4974 COMMAND_HANDLER(stlink_dap_backend_command)
4975 {
4976         /* default values */
4977         bool use_stlink_tcp = false;
4978         uint16_t stlink_tcp_port = 7184;
4979
4980         if (CMD_ARGC == 0 || CMD_ARGC > 2)
4981                 return ERROR_COMMAND_SYNTAX_ERROR;
4982         else if (strcmp(CMD_ARGV[0], "usb") == 0) {
4983                 if (CMD_ARGC > 1)
4984                         return ERROR_COMMAND_SYNTAX_ERROR;
4985                 /* else use_stlink_tcp = false (already the case ) */
4986         } else if (strcmp(CMD_ARGV[0], "tcp") == 0) {
4987                 use_stlink_tcp = true;
4988                 if (CMD_ARGC == 2)
4989                         COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_tcp_port);
4990         } else
4991                 return ERROR_COMMAND_SYNTAX_ERROR;
4992
4993         stlink_dap_param.use_stlink_tcp = use_stlink_tcp;
4994         stlink_dap_param.stlink_tcp_port = stlink_tcp_port;
4995
4996         return ERROR_OK;
4997 }
4998
4999 #define BYTES_PER_LINE 16
5000 COMMAND_HANDLER(stlink_dap_cmd_command)
5001 {
5002         unsigned int rx_n, tx_n;
5003         struct stlink_usb_handle_s *h = stlink_dap_handle;
5004
5005         if (CMD_ARGC < 2)
5006                 return ERROR_COMMAND_SYNTAX_ERROR;
5007
5008         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], rx_n);
5009         tx_n = CMD_ARGC - 1;
5010         if (tx_n > STLINK_SG_SIZE || rx_n > STLINK_DATA_SIZE) {
5011                 LOG_ERROR("max %x byte sent and %d received", STLINK_SG_SIZE, STLINK_DATA_SIZE);
5012                 return ERROR_COMMAND_SYNTAX_ERROR;
5013         }
5014
5015         stlink_usb_init_buffer(h, h->rx_ep, rx_n);
5016
5017         for (unsigned int i = 0; i < tx_n; i++) {
5018                 uint8_t byte;
5019                 COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i + 1], byte);
5020                 h->cmdbuf[h->cmdidx++] = byte;
5021         }
5022
5023         int retval = stlink_usb_xfer_noerrcheck(h, h->databuf, rx_n);
5024         if (retval != ERROR_OK) {
5025                 LOG_ERROR("Error %d", retval);
5026                 return retval;
5027         }
5028
5029         for (unsigned int i = 0; i < rx_n; i++)
5030                 command_print_sameline(CMD, "0x%02x%c", h->databuf[i],
5031                         ((i == (rx_n - 1)) || ((i % BYTES_PER_LINE) == (BYTES_PER_LINE - 1))) ? '\n' : ' ');
5032
5033         return ERROR_OK;
5034 }
5035
5036 /** */
5037 static const struct command_registration stlink_dap_subcommand_handlers[] = {
5038         {
5039                 .name = "vid_pid",
5040                 .handler = stlink_dap_vid_pid,
5041                 .mode = COMMAND_CONFIG,
5042                 .help = "USB VID and PID of the adapter",
5043                 .usage = "(vid pid)+",
5044         },
5045         {
5046                 .name = "backend",
5047                 .handler = &stlink_dap_backend_command,
5048                 .mode = COMMAND_CONFIG,
5049                 .help = "select which ST-Link backend to use",
5050                 .usage = "usb | tcp [port]",
5051         },
5052         {
5053                 .name = "cmd",
5054                 .handler = stlink_dap_cmd_command,
5055                 .mode = COMMAND_EXEC,
5056                 .help = "send arbitrary command",
5057                 .usage = "rx_n (tx_byte)+",
5058         },
5059         COMMAND_REGISTRATION_DONE
5060 };
5061
5062 /** */
5063 static const struct command_registration stlink_dap_command_handlers[] = {
5064         {
5065                 .name = "st-link",
5066                 .mode = COMMAND_ANY,
5067                 .help = "perform st-link management",
5068                 .chain = stlink_dap_subcommand_handlers,
5069                 .usage = "",
5070         },
5071         COMMAND_REGISTRATION_DONE
5072 };
5073
5074 /** */
5075 static int stlink_dap_init(void)
5076 {
5077         enum reset_types jtag_reset_config = jtag_get_reset_config();
5078         enum stlink_mode mode;
5079         int retval;
5080
5081         LOG_DEBUG("stlink_dap_init()");
5082
5083         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
5084                 if (jtag_reset_config & RESET_SRST_NO_GATING)
5085                         stlink_dap_param.connect_under_reset = true;
5086                 else
5087                         LOG_WARNING("\'srst_nogate\' reset_config option is required");
5088         }
5089
5090         if (transport_is_dapdirect_swd())
5091                 mode = STLINK_MODE_DEBUG_SWD;
5092         else if (transport_is_dapdirect_jtag())
5093                 mode = STLINK_MODE_DEBUG_JTAG;
5094         else if (transport_is_swim())
5095                 mode = STLINK_MODE_DEBUG_SWIM;
5096         else {
5097                 LOG_ERROR("Unsupported transport");
5098                 return ERROR_FAIL;
5099         }
5100
5101         retval = stlink_open(&stlink_dap_param, mode, (void **)&stlink_dap_handle);
5102         if (retval != ERROR_OK)
5103                 return retval;
5104
5105         if ((mode != STLINK_MODE_DEBUG_SWIM) &&
5106                 !(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
5107                 LOG_ERROR("ST-Link version does not support DAP direct transport");
5108                 return ERROR_FAIL;
5109         }
5110         return ERROR_OK;
5111 }
5112
5113 /** */
5114 static int stlink_dap_quit(void)
5115 {
5116         LOG_DEBUG("stlink_dap_quit()");
5117
5118         return stlink_close(stlink_dap_handle);
5119 }
5120
5121 /** */
5122 static int stlink_dap_reset(int req_trst, int req_srst)
5123 {
5124         LOG_DEBUG("stlink_dap_reset(%d)", req_srst);
5125         return stlink_usb_assert_srst(stlink_dap_handle,
5126                 req_srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
5127                                  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
5128 }
5129
5130 /** */
5131 static int stlink_dap_speed(int speed)
5132 {
5133         if (speed == 0) {
5134                 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
5135                 return ERROR_JTAG_NOT_IMPLEMENTED;
5136         }
5137
5138         stlink_dap_param.initial_interface_speed = speed;
5139         stlink_speed(stlink_dap_handle, speed, false);
5140         return ERROR_OK;
5141 }
5142
5143 /** */
5144 static int stlink_dap_khz(int khz, int *jtag_speed)
5145 {
5146         if (khz == 0) {
5147                 LOG_ERROR("RCLK not supported");
5148                 return ERROR_FAIL;
5149         }
5150
5151         *jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
5152         return ERROR_OK;
5153 }
5154
5155 /** */
5156 static int stlink_dap_speed_div(int speed, int *khz)
5157 {
5158         *khz = speed;
5159         return ERROR_OK;
5160 }
5161
5162 static const struct dap_ops stlink_dap_ops = {
5163         .connect = stlink_dap_op_connect,
5164         .send_sequence = stlink_dap_op_send_sequence,
5165         .queue_dp_read = stlink_dap_op_queue_dp_read,
5166         .queue_dp_write = stlink_dap_op_queue_dp_write,
5167         .queue_ap_read = stlink_dap_op_queue_ap_read,
5168         .queue_ap_write = stlink_dap_op_queue_ap_write,
5169         .queue_ap_abort = stlink_dap_op_queue_ap_abort,
5170         .run = stlink_dap_op_queue_run,
5171         .sync = NULL, /* optional */
5172         .quit = stlink_dap_op_quit, /* optional */
5173 };
5174
5175 static const struct swim_driver stlink_swim_ops = {
5176         .srst = stlink_swim_op_srst,
5177         .read_mem = stlink_swim_op_read_mem,
5178         .write_mem = stlink_swim_op_write_mem,
5179         .reconnect = stlink_swim_op_reconnect,
5180 };
5181
5182 static const char *const stlink_dap_transport[] = { "dapdirect_swd", "dapdirect_jtag", "swim", NULL };
5183
5184 struct adapter_driver stlink_dap_adapter_driver = {
5185         .name = "st-link",
5186         .transports = stlink_dap_transport,
5187         .commands = stlink_dap_command_handlers,
5188
5189         .init = stlink_dap_init,
5190         .quit = stlink_dap_quit,
5191         .reset = stlink_dap_reset,
5192         .speed = stlink_dap_speed,
5193         .khz = stlink_dap_khz,
5194         .speed_div = stlink_dap_speed_div,
5195         .config_trace = stlink_dap_config_trace,
5196         .poll_trace = stlink_dap_trace_read,
5197
5198         .dap_jtag_ops = &stlink_dap_ops,
5199         .dap_swd_ops = &stlink_dap_ops,
5200         .swim_ops = &stlink_swim_ops,
5201 };