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

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