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