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

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