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

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