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