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

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