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