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