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

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