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) {
 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                         (emode == STLINK_MODE_DEBUG_JTAG || emode == STLINK_MODE_DEBUG_SWD)) {
1631                 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
1632
1633                 stlink_get_com_freq(h, (emode == STLINK_MODE_DEBUG_JTAG), map);
1634                 stlink_dump_speed_map(map, ARRAY_SIZE(map));
1635                 stlink_speed(h, initial_interface_speed, false);
1636         }
1637
1638         /* preliminary SRST assert:
1639          * We want SRST is asserted before activating debug signals (mode_enter).
1640          * As the required mode has not been set, the adapter may not know what pin to use.
1641          * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1642          * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1643          * after power on, SWIM_RST stays unchanged */
1644         if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1645                 stlink_usb_assert_srst(handle, 0);
1646                 /* do not check the return status here, we will
1647                    proceed and enter the desired mode below
1648                    and try asserting srst again. */
1649
1650         res = stlink_usb_mode_enter(handle, emode);
1651         if (res != ERROR_OK)
1652                 return res;
1653
1654         /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1655         if (connect_under_reset) {
1656                 res = stlink_usb_assert_srst(handle, 0);
1657                 if (res != ERROR_OK)
1658                         return res;
1659         }
1660
1661         res = stlink_usb_current_mode(handle, &mode);
1662
1663         if (res != ERROR_OK)
1664                 return res;
1665
1666         LOG_DEBUG("MODE: 0x%02X", mode);
1667
1668         return ERROR_OK;
1669 }
1670
1671 /* request status from last swim request */
1672 static int stlink_swim_status(void *handle)
1673 {
1674         struct stlink_usb_handle_s *h = handle;
1675         int res;
1676
1677         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1678         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1679         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1680         /* error is checked by the caller */
1681         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1682         if (res != ERROR_OK)
1683                 return res;
1684         return ERROR_OK;
1685 }
1686 /*
1687         the purpose of this function is unknown...
1688         capabilities? anyway for swim v6 it returns
1689         0001020600000000
1690 */
1691 __attribute__((unused))
1692 static int stlink_swim_cap(void *handle, uint8_t *cap)
1693 {
1694         struct stlink_usb_handle_s *h = handle;
1695         int res;
1696
1697         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1698         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1699         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1700         h->cmdbuf[h->cmdidx++] = 0x01;
1701         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1702         if (res != ERROR_OK)
1703                 return res;
1704         memcpy(cap, h->databuf, 8);
1705         return ERROR_OK;
1706 }
1707
1708 /*      debug dongle assert/deassert sreset line */
1709 static int stlink_swim_assert_reset(void *handle, int reset)
1710 {
1711         struct stlink_usb_handle_s *h = handle;
1712         int res;
1713
1714         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1715         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1716         if (!reset)
1717                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1718         else
1719                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1720         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1721         if (res != ERROR_OK)
1722                 return res;
1723         return ERROR_OK;
1724 }
1725
1726 /*
1727         send swim enter seq
1728         1.3ms low then 750Hz then 1.5kHz
1729 */
1730 static int stlink_swim_enter(void *handle)
1731 {
1732         struct stlink_usb_handle_s *h = handle;
1733         int res;
1734
1735         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1736         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1737         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1738         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1739         if (res != ERROR_OK)
1740                 return res;
1741         return ERROR_OK;
1742 }
1743
1744 /*      switch high/low speed swim */
1745 static int stlink_swim_speed(void *handle, int speed)
1746 {
1747         struct stlink_usb_handle_s *h = handle;
1748         int res;
1749
1750         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1751         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1752         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1753         if (speed)
1754                 h->cmdbuf[h->cmdidx++] = 1;
1755         else
1756                 h->cmdbuf[h->cmdidx++] = 0;
1757         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1758         if (res != ERROR_OK)
1759                 return res;
1760         return ERROR_OK;
1761 }
1762
1763 /*
1764         initiate srst from swim.
1765         nrst is pulled low for 50us.
1766 */
1767 static int stlink_swim_generate_rst(void *handle)
1768 {
1769         struct stlink_usb_handle_s *h = handle;
1770         int res;
1771
1772         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1773         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1774         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1775         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1776         if (res != ERROR_OK)
1777                 return res;
1778         return ERROR_OK;
1779 }
1780
1781 /*
1782         send resynchronize sequence
1783         swim is pulled low for 16us
1784         reply is 64 clks low
1785 */
1786 static int stlink_swim_resync(void *handle)
1787 {
1788         struct stlink_usb_handle_s *h = handle;
1789         int res;
1790
1791         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1792         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1793         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1794         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1795         if (res != ERROR_OK)
1796                 return res;
1797         return ERROR_OK;
1798 }
1799
1800 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1801 {
1802         struct stlink_usb_handle_s *h = handle;
1803         int res;
1804         unsigned int i;
1805         unsigned int datalen = 0;
1806         int cmdsize = STLINK_CMD_SIZE_V2;
1807
1808         if (len > STLINK_DATA_SIZE)
1809                 return ERROR_FAIL;
1810
1811         if (h->version.stlink == 1)
1812                 cmdsize = STLINK_SG_SIZE;
1813
1814         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1815         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1816         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1817         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1818         h->cmdidx += 2;
1819         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1820         h->cmdidx += 4;
1821         for (i = 0; i < len; i++) {
1822                 if (h->cmdidx == cmdsize)
1823                         h->databuf[datalen++] = *(data++);
1824                 else
1825                         h->cmdbuf[h->cmdidx++] = *(data++);
1826         }
1827         if (h->version.stlink == 1)
1828                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1829
1830         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1831         if (res != ERROR_OK)
1832                 return res;
1833         return ERROR_OK;
1834 }
1835
1836 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1837 {
1838         struct stlink_usb_handle_s *h = handle;
1839         int res;
1840
1841         if (len > STLINK_DATA_SIZE)
1842                 return ERROR_FAIL;
1843
1844         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1845         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1846         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1847         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1848         h->cmdidx += 2;
1849         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1850         h->cmdidx += 4;
1851         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1852         if (res != ERROR_OK)
1853                 return res;
1854
1855         stlink_usb_init_buffer(handle, h->rx_ep, len);
1856         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1857         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1858         res = stlink_usb_xfer_noerrcheck(handle, data, len);
1859         if (res != ERROR_OK)
1860                 return res;
1861
1862         return ERROR_OK;
1863 }
1864
1865 /** */
1866 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1867 {
1868         int res, offset;
1869         struct stlink_usb_handle_s *h = handle;
1870
1871         assert(handle);
1872
1873         /* there is no swim read core id cmd */
1874         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1875                 *idcode = 0;
1876                 return ERROR_OK;
1877         }
1878
1879         stlink_usb_init_buffer(handle, h->rx_ep, 12);
1880
1881         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1882         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1883                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1884
1885                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1886                 offset = 0;
1887         } else {
1888                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
1889
1890                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
1891                 offset = 4;
1892         }
1893
1894         if (res != ERROR_OK)
1895                 return res;
1896
1897         *idcode = le_to_h_u32(h->databuf + offset);
1898
1899         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1900
1901         return ERROR_OK;
1902 }
1903
1904 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1905 {
1906         struct stlink_usb_handle_s *h = handle;
1907         int res;
1908
1909         assert(handle);
1910
1911         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1912
1913         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1914         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1915         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1916         h->cmdidx += 4;
1917
1918         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1919         if (res != ERROR_OK)
1920                 return res;
1921
1922         *val = le_to_h_u32(h->databuf + 4);
1923         return ERROR_OK;
1924 }
1925
1926 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
1927 {
1928         struct stlink_usb_handle_s *h = handle;
1929
1930         assert(handle);
1931
1932         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1933
1934         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1935         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1936                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
1937         else
1938                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
1939         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1940         h->cmdidx += 4;
1941         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1942         h->cmdidx += 4;
1943
1944         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1945 }
1946
1947 /** */
1948 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
1949 {
1950         struct stlink_usb_handle_s *h = handle;
1951
1952         assert(handle);
1953
1954         if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
1955                 int res;
1956
1957                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1958
1959                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1960                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1961
1962                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1963                 if (res != ERROR_OK)
1964                         return res;
1965
1966                 size_t bytes_avail = le_to_h_u16(h->databuf);
1967                 *size = bytes_avail < *size ? bytes_avail : *size;
1968
1969                 if (*size > 0) {
1970                         res = stlink_usb_read_trace(handle, buf, *size);
1971                         if (res != ERROR_OK)
1972                                 return res;
1973                         return ERROR_OK;
1974                 }
1975         }
1976         *size = 0;
1977         return ERROR_OK;
1978 }
1979
1980 static enum target_state stlink_usb_v2_get_status(void *handle)
1981 {
1982         int result;
1983         uint32_t status;
1984
1985         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
1986         if  (result != ERROR_OK)
1987                 return TARGET_UNKNOWN;
1988
1989         if (status & S_HALT)
1990                 return TARGET_HALTED;
1991         else if (status & S_RESET_ST)
1992                 return TARGET_RESET;
1993
1994         return TARGET_RUNNING;
1995 }
1996
1997 /** */
1998 static enum target_state stlink_usb_state(void *handle)
1999 {
2000         int res;
2001         struct stlink_usb_handle_s *h = handle;
2002
2003         assert(handle);
2004
2005         if (h->reconnect_pending) {
2006                 LOG_INFO("Previous state query failed, trying to reconnect");
2007                 res = stlink_usb_mode_enter(handle, h->st_mode);
2008                 if (res != ERROR_OK)
2009                         return TARGET_UNKNOWN;
2010
2011                 h->reconnect_pending = false;
2012         }
2013
2014         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2015                 res = stlink_usb_v2_get_status(handle);
2016                 if (res == TARGET_UNKNOWN)
2017                         h->reconnect_pending = true;
2018                 return res;
2019         }
2020
2021         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2022
2023         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2024         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
2025
2026         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
2027
2028         if (res != ERROR_OK)
2029                 return TARGET_UNKNOWN;
2030
2031         if (h->databuf[0] == STLINK_CORE_RUNNING)
2032                 return TARGET_RUNNING;
2033         if (h->databuf[0] == STLINK_CORE_HALTED)
2034                 return TARGET_HALTED;
2035
2036         h->reconnect_pending = true;
2037
2038         return TARGET_UNKNOWN;
2039 }
2040
2041 static int stlink_usb_assert_srst(void *handle, int srst)
2042 {
2043         struct stlink_usb_handle_s *h = handle;
2044
2045         assert(handle);
2046
2047         if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
2048                 return stlink_swim_assert_reset(handle, srst);
2049
2050         if (h->version.stlink == 1)
2051                 return ERROR_COMMAND_NOTFOUND;
2052
2053         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2054
2055         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2056         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
2057         h->cmdbuf[h->cmdidx++] = srst;
2058
2059         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2060 }
2061
2062 /** */
2063 static void stlink_usb_trace_disable(void *handle)
2064 {
2065         int res = ERROR_OK;
2066         struct stlink_usb_handle_s *h = handle;
2067
2068         assert(handle);
2069
2070         assert(h->version.flags & STLINK_F_HAS_TRACE);
2071
2072         LOG_DEBUG("Tracing: disable");
2073
2074         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2075         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2076         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
2077         res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2078
2079         if (res == ERROR_OK)
2080                 h->trace.enabled = false;
2081 }
2082
2083
2084 /** */
2085 static int stlink_usb_trace_enable(void *handle)
2086 {
2087         int res;
2088         struct stlink_usb_handle_s *h = handle;
2089
2090         assert(handle);
2091
2092         if (h->version.flags & STLINK_F_HAS_TRACE) {
2093                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
2094
2095                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2096                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
2097                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
2098                 h->cmdidx += 2;
2099                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
2100                 h->cmdidx += 4;
2101
2102                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2103
2104                 if (res == ERROR_OK)  {
2105                         h->trace.enabled = true;
2106                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
2107                 }
2108         } else {
2109                 LOG_ERROR("Tracing is not supported by this version.");
2110                 res = ERROR_FAIL;
2111         }
2112
2113         return res;
2114 }
2115
2116 /** */
2117 static int stlink_usb_reset(void *handle)
2118 {
2119         struct stlink_usb_handle_s *h = handle;
2120         int retval;
2121
2122         assert(handle);
2123
2124         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2125
2126         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2127
2128         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2129                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
2130         else
2131                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
2132
2133         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
2134         if (retval != ERROR_OK)
2135                 return retval;
2136
2137         if (h->trace.enabled) {
2138                 stlink_usb_trace_disable(h);
2139                 return stlink_usb_trace_enable(h);
2140         }
2141
2142         return ERROR_OK;
2143 }
2144
2145 /** */
2146 static int stlink_usb_run(void *handle)
2147 {
2148         int res;
2149         struct stlink_usb_handle_s *h = handle;
2150
2151         assert(handle);
2152
2153         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2154                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
2155
2156                 return res;
2157         }
2158
2159         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2160
2161         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2162         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
2163
2164         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2165 }
2166
2167 /** */
2168 static int stlink_usb_halt(void *handle)
2169 {
2170         int res;
2171         struct stlink_usb_handle_s *h = handle;
2172
2173         assert(handle);
2174
2175         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2176                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2177
2178                 return res;
2179         }
2180
2181         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2182
2183         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2184         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
2185
2186         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2187 }
2188
2189 /** */
2190 static int stlink_usb_step(void *handle)
2191 {
2192         struct stlink_usb_handle_s *h = handle;
2193
2194         assert(handle);
2195
2196         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
2197                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
2198                  * that the Cortex-M3 currently does. */
2199                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
2200                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
2201                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
2202         }
2203
2204         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2205
2206         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2207         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
2208
2209         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2210 }
2211
2212 /** */
2213 static int stlink_usb_read_regs(void *handle)
2214 {
2215         int res;
2216         struct stlink_usb_handle_s *h = handle;
2217
2218         assert(handle);
2219
2220         stlink_usb_init_buffer(handle, h->rx_ep, 88);
2221
2222         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2223         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2224
2225                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
2226                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
2227                 /* regs data from offset 0 */
2228         } else {
2229                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
2230                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
2231                 /* status at offset 0, regs data from offset 4 */
2232         }
2233
2234         return res;
2235 }
2236
2237 /** */
2238 static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
2239 {
2240         int res;
2241         struct stlink_usb_handle_s *h = handle;
2242
2243         assert(handle);
2244
2245         if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2246                 res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
2247                 if (res != ERROR_OK)
2248                         return res;
2249
2250                 /* FIXME: poll DHCSR.S_REGRDY before read DCRDR */
2251                 return stlink_usb_v2_read_debug_reg(h, DCB_DCRDR, val);
2252         }
2253
2254         stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
2255
2256         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2257         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2258                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
2259         else
2260                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
2261         h->cmdbuf[h->cmdidx++] = regsel;
2262
2263         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2264                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2265                 if (res != ERROR_OK)
2266                         return res;
2267                 *val = le_to_h_u32(h->databuf);
2268                 return ERROR_OK;
2269         } else {
2270                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2271                 if (res != ERROR_OK)
2272                         return res;
2273                 *val = le_to_h_u32(h->databuf + 4);
2274                 return ERROR_OK;
2275         }
2276 }
2277
2278 /** */
2279 static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
2280 {
2281         struct stlink_usb_handle_s *h = handle;
2282
2283         assert(handle);
2284
2285         if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
2286                 int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
2287                 if (res != ERROR_OK)
2288                         return res;
2289
2290                 return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WNR | (regsel & 0x7f));
2291                 /* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
2292         }
2293
2294         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2295
2296         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2297         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2298                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
2299         else
2300                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
2301         h->cmdbuf[h->cmdidx++] = regsel;
2302         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2303         h->cmdidx += 4;
2304
2305         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2306 }
2307
2308 static int stlink_usb_get_rw_status(void *handle)
2309 {
2310         struct stlink_usb_handle_s *h = handle;
2311
2312         assert(handle);
2313
2314         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2315                 return ERROR_OK;
2316
2317         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2318
2319         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2320         if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
2321                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
2322                 return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2323         } else {
2324                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
2325                 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2326         }
2327 }
2328
2329 /** */
2330 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
2331                           uint8_t *buffer)
2332 {
2333         int res;
2334         uint16_t read_len = len;
2335         struct stlink_usb_handle_s *h = handle;
2336
2337         assert(handle);
2338
2339         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2340         if (len > stlink_usb_block(h)) {
2341                 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2342                 return ERROR_FAIL;
2343         }
2344
2345         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2346
2347         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2348         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2349         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2350         h->cmdidx += 4;
2351         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2352         h->cmdidx += 2;
2353
2354         /* we need to fix read length for single bytes */
2355         if (read_len == 1)
2356                 read_len++;
2357
2358         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
2359
2360         if (res != ERROR_OK)
2361                 return res;
2362
2363         memcpy(buffer, h->databuf, len);
2364
2365         return stlink_usb_get_rw_status(handle);
2366 }
2367
2368 /** */
2369 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
2370                            const uint8_t *buffer)
2371 {
2372         int res;
2373         struct stlink_usb_handle_s *h = handle;
2374
2375         assert(handle);
2376
2377         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2378         if (len > stlink_usb_block(h)) {
2379                 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2380                 return ERROR_FAIL;
2381         }
2382
2383         stlink_usb_init_buffer(handle, h->tx_ep, len);
2384
2385         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2386         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2387         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2388         h->cmdidx += 4;
2389         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2390         h->cmdidx += 2;
2391
2392         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2393
2394         if (res != ERROR_OK)
2395                 return res;
2396
2397         return stlink_usb_get_rw_status(handle);
2398 }
2399
2400 /** */
2401 static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
2402                           uint8_t *buffer)
2403 {
2404         int res;
2405         struct stlink_usb_handle_s *h = handle;
2406
2407         assert(handle);
2408
2409         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2410                 return ERROR_COMMAND_NOTFOUND;
2411
2412         /* data must be a multiple of 2 and half-word aligned */
2413         if (len % 2 || addr % 2) {
2414                 LOG_DEBUG("Invalid data alignment");
2415                 return ERROR_TARGET_UNALIGNED_ACCESS;
2416         }
2417
2418         stlink_usb_init_buffer(handle, h->rx_ep, len);
2419
2420         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2421         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2422         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2423         h->cmdidx += 4;
2424         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2425         h->cmdidx += 2;
2426
2427         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2428
2429         if (res != ERROR_OK)
2430                 return res;
2431
2432         memcpy(buffer, h->databuf, len);
2433
2434         return stlink_usb_get_rw_status(handle);
2435 }
2436
2437 /** */
2438 static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
2439                            const uint8_t *buffer)
2440 {
2441         int res;
2442         struct stlink_usb_handle_s *h = handle;
2443
2444         assert(handle);
2445
2446         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2447                 return ERROR_COMMAND_NOTFOUND;
2448
2449         /* data must be a multiple of 2 and half-word aligned */
2450         if (len % 2 || addr % 2) {
2451                 LOG_DEBUG("Invalid data alignment");
2452                 return ERROR_TARGET_UNALIGNED_ACCESS;
2453         }
2454
2455         stlink_usb_init_buffer(handle, h->tx_ep, len);
2456
2457         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2458         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2459         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2460         h->cmdidx += 4;
2461         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2462         h->cmdidx += 2;
2463
2464         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2465
2466         if (res != ERROR_OK)
2467                 return res;
2468
2469         return stlink_usb_get_rw_status(handle);
2470 }
2471
2472 /** */
2473 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
2474                           uint8_t *buffer)
2475 {
2476         int res;
2477         struct stlink_usb_handle_s *h = handle;
2478
2479         assert(handle);
2480
2481         /* data must be a multiple of 4 and word aligned */
2482         if (len % 4 || addr % 4) {
2483                 LOG_DEBUG("Invalid data alignment");
2484                 return ERROR_TARGET_UNALIGNED_ACCESS;
2485         }
2486
2487         stlink_usb_init_buffer(handle, h->rx_ep, len);
2488
2489         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2490         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2491         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2492         h->cmdidx += 4;
2493         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2494         h->cmdidx += 2;
2495
2496         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2497
2498         if (res != ERROR_OK)
2499                 return res;
2500
2501         memcpy(buffer, h->databuf, len);
2502
2503         return stlink_usb_get_rw_status(handle);
2504 }
2505
2506 /** */
2507 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
2508                            const uint8_t *buffer)
2509 {
2510         int res;
2511         struct stlink_usb_handle_s *h = handle;
2512
2513         assert(handle);
2514
2515         /* data must be a multiple of 4 and word aligned */
2516         if (len % 4 || addr % 4) {
2517                 LOG_DEBUG("Invalid data alignment");
2518                 return ERROR_TARGET_UNALIGNED_ACCESS;
2519         }
2520
2521         stlink_usb_init_buffer(handle, h->tx_ep, len);
2522
2523         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2524         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2525         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2526         h->cmdidx += 4;
2527         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2528         h->cmdidx += 2;
2529
2530         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2531
2532         if (res != ERROR_OK)
2533                 return res;
2534
2535         return stlink_usb_get_rw_status(handle);
2536 }
2537
2538 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2539 {
2540         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2541         if (max_tar_block == 0)
2542                 max_tar_block = 4;
2543         return max_tar_block;
2544 }
2545
2546 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2547                 uint32_t count, uint8_t *buffer)
2548 {
2549         int retval = ERROR_OK;
2550         uint32_t bytes_remaining;
2551         int retries = 0;
2552         struct stlink_usb_handle_s *h = handle;
2553
2554         /* calculate byte count */
2555         count *= size;
2556
2557         /* switch to 8 bit if stlink does not support 16 bit memory read */
2558         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2559                 size = 1;
2560
2561         while (count) {
2562
2563                 bytes_remaining = (size != 1) ?
2564                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2565
2566                 if (count < bytes_remaining)
2567                         bytes_remaining = count;
2568
2569                 /*
2570                  * all stlink support 8/32bit memory read/writes and only from
2571                  * stlink V2J26 there is support for 16 bit memory read/write.
2572                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2573                  * as 8bit access.
2574                  */
2575                 if (size != 1) {
2576
2577                         /* When in jtag mode the stlink uses the auto-increment functionality.
2578                          * However it expects us to pass the data correctly, this includes
2579                          * alignment and any page boundaries. We already do this as part of the
2580                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2581                          * needs implementing manually.
2582                          * currently this only affects jtag mode, according to ST they do single
2583                          * access in SWD mode - but this may change and so we do it for both modes */
2584
2585                         /* we first need to check for any unaligned bytes */
2586                         if (addr & (size - 1)) {
2587
2588                                 uint32_t head_bytes = size - (addr & (size - 1));
2589                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
2590                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2591                                         usleep((1<<retries++) * 1000);
2592                                         continue;
2593                                 }
2594                                 if (retval != ERROR_OK)
2595                                         return retval;
2596                                 buffer += head_bytes;
2597                                 addr += head_bytes;
2598                                 count -= head_bytes;
2599                                 bytes_remaining -= head_bytes;
2600                         }
2601
2602                         if (bytes_remaining & (size - 1))
2603                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
2604                         else if (size == 2)
2605                                 retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
2606                         else
2607                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
2608                 } else
2609                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
2610
2611                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2612                         usleep((1<<retries++) * 1000);
2613                         continue;
2614                 }
2615                 if (retval != ERROR_OK)
2616                         return retval;
2617
2618                 buffer += bytes_remaining;
2619                 addr += bytes_remaining;
2620                 count -= bytes_remaining;
2621         }
2622
2623         return retval;
2624 }
2625
2626 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2627                 uint32_t count, const uint8_t *buffer)
2628 {
2629         int retval = ERROR_OK;
2630         uint32_t bytes_remaining;
2631         int retries = 0;
2632         struct stlink_usb_handle_s *h = handle;
2633
2634         /* calculate byte count */
2635         count *= size;
2636
2637         /* switch to 8 bit if stlink does not support 16 bit memory read */
2638         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2639                 size = 1;
2640
2641         while (count) {
2642
2643                 bytes_remaining = (size != 1) ?
2644                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2645
2646                 if (count < bytes_remaining)
2647                         bytes_remaining = count;
2648
2649                 /*
2650                  * all stlink support 8/32bit memory read/writes and only from
2651                  * stlink V2J26 there is support for 16 bit memory read/write.
2652                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2653                  * as 8bit access.
2654                  */
2655                 if (size != 1) {
2656
2657                         /* When in jtag mode the stlink uses the auto-increment functionality.
2658                          * However it expects us to pass the data correctly, this includes
2659                          * alignment and any page boundaries. We already do this as part of the
2660                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2661                          * needs implementing manually.
2662                          * currently this only affects jtag mode, according to ST they do single
2663                          * access in SWD mode - but this may change and so we do it for both modes */
2664
2665                         /* we first need to check for any unaligned bytes */
2666                         if (addr & (size - 1)) {
2667
2668                                 uint32_t head_bytes = size - (addr & (size - 1));
2669                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
2670                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2671                                         usleep((1<<retries++) * 1000);
2672                                         continue;
2673                                 }
2674                                 if (retval != ERROR_OK)
2675                                         return retval;
2676                                 buffer += head_bytes;
2677                                 addr += head_bytes;
2678                                 count -= head_bytes;
2679                                 bytes_remaining -= head_bytes;
2680                         }
2681
2682                         if (bytes_remaining & (size - 1))
2683                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
2684                         else if (size == 2)
2685                                 retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
2686                         else
2687                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
2688
2689                 } else
2690                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
2691                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2692                         usleep((1<<retries++) * 1000);
2693                         continue;
2694                 }
2695                 if (retval != ERROR_OK)
2696                         return retval;
2697
2698                 buffer += bytes_remaining;
2699                 addr += bytes_remaining;
2700                 count -= bytes_remaining;
2701         }
2702
2703         return retval;
2704 }
2705
2706 /** */
2707 static int stlink_usb_override_target(const char *targetname)
2708 {
2709         return !strcmp(targetname, "cortex_m");
2710 }
2711
2712 static int stlink_speed_swim(void *handle, int khz, bool query)
2713 {
2714         int retval;
2715
2716         /*
2717                         we only have low and high speed...
2718                         before changing speed the SWIM_CSR HS bit
2719                         must be updated
2720          */
2721         if (!query) {
2722                 retval = stlink_swim_speed(handle, (khz < SWIM_FREQ_HIGH) ? 0 : 1);
2723                 if (retval != ERROR_OK)
2724                         LOG_ERROR("Unable to set adapter speed");
2725         }
2726
2727         return (khz < SWIM_FREQ_HIGH) ? SWIM_FREQ_LOW : SWIM_FREQ_HIGH;
2728 }
2729
2730 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
2731 {
2732         unsigned int i;
2733         int speed_index = -1;
2734         int speed_diff = INT_MAX;
2735         int last_valid_speed = -1;
2736         bool match = true;
2737
2738         for (i = 0; i < map_size; i++) {
2739                 if (!map[i].speed)
2740                         continue;
2741                 last_valid_speed = i;
2742                 if (khz == map[i].speed) {
2743                         speed_index = i;
2744                         break;
2745                 } else {
2746                         int current_diff = khz - map[i].speed;
2747                         /* get abs value for comparison */
2748                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
2749                         if ((current_diff < speed_diff) && khz >= map[i].speed) {
2750                                 speed_diff = current_diff;
2751                                 speed_index = i;
2752                         }
2753                 }
2754         }
2755
2756         if (speed_index == -1) {
2757                 /* this will only be here if we cannot match the slow speed.
2758                  * use the slowest speed we support.*/
2759                 speed_index = last_valid_speed;
2760                 match = false;
2761         } else if (i == map_size)
2762                 match = false;
2763
2764         if (!match && query) {
2765                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz",
2766                                 khz, map[speed_index].speed);
2767         }
2768
2769         return speed_index;
2770 }
2771
2772 static int stlink_speed_swd(void *handle, int khz, bool query)
2773 {
2774         int speed_index;
2775         struct stlink_usb_handle_s *h = handle;
2776
2777         /* old firmware cannot change it */
2778         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
2779                 return khz;
2780
2781         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
2782                 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
2783
2784         if (!query) {
2785                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
2786                 if (result != ERROR_OK) {
2787                         LOG_ERROR("Unable to set adapter speed");
2788                         return khz;
2789                 }
2790         }
2791
2792         return stlink_khz_to_speed_map_swd[speed_index].speed;
2793 }
2794
2795 static int stlink_speed_jtag(void *handle, int khz, bool query)
2796 {
2797         int speed_index;
2798         struct stlink_usb_handle_s *h = handle;
2799
2800         /* old firmware cannot change it */
2801         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
2802                 return khz;
2803
2804         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
2805                 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
2806
2807         if (!query) {
2808                 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
2809                 if (result != ERROR_OK) {
2810                         LOG_ERROR("Unable to set adapter speed");
2811                         return khz;
2812                 }
2813         }
2814
2815         return stlink_khz_to_speed_map_jtag[speed_index].speed;
2816 }
2817
2818 static void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
2819 {
2820         unsigned int i;
2821
2822         LOG_DEBUG("Supported clock speeds are:");
2823         for (i = 0; i < map_size; i++)
2824                 if (map[i].speed)
2825                         LOG_DEBUG("%d kHz", map[i].speed);
2826 }
2827
2828 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
2829 {
2830         struct stlink_usb_handle_s *h = handle;
2831         int i;
2832
2833         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2834                 LOG_ERROR("Unknown command");
2835                 return 0;
2836         }
2837
2838         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2839
2840         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2841         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
2842         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2843
2844         int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
2845
2846         int size = h->databuf[8];
2847
2848         if (size > STLINK_V3_MAX_FREQ_NB)
2849                 size = STLINK_V3_MAX_FREQ_NB;
2850
2851         for (i = 0; i < size; i++) {
2852                 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
2853                 map[i].speed_divisor = i;
2854         }
2855
2856         /* set to zero all the next entries */
2857         for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
2858                 map[i].speed = 0;
2859
2860         return res;
2861 }
2862
2863 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
2864 {
2865         struct stlink_usb_handle_s *h = handle;
2866
2867         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2868                 LOG_ERROR("Unknown command");
2869                 return 0;
2870         }
2871
2872         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2873
2874         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2875         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
2876         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2877         h->cmdbuf[h->cmdidx++] = 0;
2878
2879         h_u32_to_le(&h->cmdbuf[4], frequency);
2880
2881         return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
2882 }
2883
2884 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
2885 {
2886         struct stlink_usb_handle_s *h = handle;
2887         int speed_index;
2888         struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2889
2890         stlink_get_com_freq(h, is_jtag, map);
2891
2892         speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
2893
2894         if (!query) {
2895                 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
2896                 if (result != ERROR_OK) {
2897                         LOG_ERROR("Unable to set adapter speed");
2898                         return khz;
2899                 }
2900         }
2901         return map[speed_index].speed;
2902 }
2903
2904 static int stlink_speed(void *handle, int khz, bool query)
2905 {
2906         struct stlink_usb_handle_s *h = handle;
2907
2908         if (!handle)
2909                 return khz;
2910
2911         switch (h->st_mode) {
2912         case STLINK_MODE_DEBUG_SWIM:
2913                 return stlink_speed_swim(handle, khz, query);
2914         case STLINK_MODE_DEBUG_SWD:
2915                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2916                         return stlink_speed_v3(handle, false, khz, query);
2917                 else
2918                         return stlink_speed_swd(handle, khz, query);
2919                 break;
2920         case STLINK_MODE_DEBUG_JTAG:
2921                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2922                         return stlink_speed_v3(handle, true, khz, query);
2923                 else
2924                         return stlink_speed_jtag(handle, khz, query);
2925                 break;
2926         default:
2927                 break;
2928         }
2929
2930         return khz;
2931 }
2932
2933 /** */
2934 static int stlink_usb_usb_close(void *handle)
2935 {
2936         struct stlink_usb_handle_s *h = handle;
2937
2938         if (!h)
2939                 return ERROR_OK;
2940
2941         if (h->usb_backend_priv.fd) {
2942                 stlink_usb_exit_mode(h);
2943                 /* do not check return code, it prevent
2944                 us from closing jtag_libusb */
2945                 jtag_libusb_close(h->usb_backend_priv.fd);
2946         }
2947
2948         free(h->cmdbuf);
2949         free(h->databuf);
2950
2951         return ERROR_OK;
2952 }
2953
2954 /** */
2955 static int stlink_tcp_close(void *handle)
2956 {
2957         struct stlink_usb_handle_s *h = handle;
2958
2959         if (!h)
2960                 return ERROR_OK;
2961
2962         int ret = ERROR_OK;
2963         if (h->tcp_backend_priv.connected) {
2964                 if (h->tcp_backend_priv.connect_id) {
2965                         stlink_usb_exit_mode(h);
2966
2967                         /* close the stlink */
2968                         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_CLOSE_DEV;
2969                         memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
2970                         h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
2971                         ret = stlink_tcp_send_cmd(h, 8, 4, true);
2972                         if (ret != ERROR_OK)
2973                                 LOG_ERROR("cannot close the STLINK");
2974                 }
2975
2976                 if (close_socket(h->tcp_backend_priv.fd) != 0)
2977                         LOG_ERROR("error closing the socket, errno: %s", strerror(errno));
2978         }
2979
2980         free(h->tcp_backend_priv.send_buf);
2981         free(h->tcp_backend_priv.recv_buf);
2982
2983         return ret;
2984 }
2985
2986 /** */
2987 static int stlink_close(void *handle)
2988 {
2989         if (handle) {
2990                 struct stlink_usb_handle_s *h = handle;
2991
2992                 stlink_usb_close(handle);
2993
2994                 free(h);
2995         }
2996
2997         return ERROR_OK;
2998 }
2999
3000 /* Compute ST-Link serial number from the device descriptor
3001  * this function will help to work-around a bug in old ST-Link/V2 DFU
3002  * the buggy DFU returns an incorrect serial in the USB descriptor
3003  * example for the following serial "57FF72067265575742132067"
3004  *  - the correct descriptor serial is:
3005  *    0x32, 0x03, 0x35, 0x00, 0x37, 0x00, 0x46, 0x00, 0x46, 0x00, 0x37, 0x00, 0x32, 0x00 ...
3006  *    this contains the length (0x32 = 50), the type (0x3 = DT_STRING) and the serial in unicode format
3007  *    the serial part is: 0x0035, 0x0037, 0x0046, 0x0046, 0x0037, 0x0032 ... >>  57FF72 ...
3008  *    this format could be read correctly by 'libusb_get_string_descriptor_ascii'
3009  *    so this case is managed by libusb_helper::string_descriptor_equal
3010  *  - the buggy DFU is not doing any unicode conversion and returns a raw serial data in the descriptor
3011  *    0x1a, 0x03, 0x57, 0x00, 0xFF, 0x00, 0x72, 0x00 ...
3012  *            >>    57          FF          72       ...
3013  *    based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
3014  *    and then we have just to convert the raw data into printable characters using sprintf
3015  */
3016 static char *stlink_usb_get_alternate_serial(struct libusb_device_handle *device,
3017                 struct libusb_device_descriptor *dev_desc)
3018 {
3019         int usb_retval;
3020         unsigned char desc_serial[(STLINK_SERIAL_LEN + 1) * 2];
3021
3022         if (dev_desc->iSerialNumber == 0)
3023                 return NULL;
3024
3025         /* get the LANGID from String Descriptor Zero */
3026         usb_retval = libusb_get_string_descriptor(device, 0, 0, desc_serial,
3027                         sizeof(desc_serial));
3028
3029         if (usb_retval < LIBUSB_SUCCESS) {
3030                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
3031                                 libusb_error_name(usb_retval), usb_retval);
3032                 return NULL;
3033         } else if (usb_retval < 4) {
3034                 /* the size should be least 4 bytes to contain a minimum of 1 supported LANGID */
3035                 LOG_ERROR("could not get the LANGID");
3036                 return NULL;
3037         }
3038
3039         uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
3040
3041         /* get the serial */
3042         usb_retval = libusb_get_string_descriptor(device, dev_desc->iSerialNumber,
3043                         langid, desc_serial, sizeof(desc_serial));
3044
3045         unsigned char len = desc_serial[0];
3046
3047         if (usb_retval < LIBUSB_SUCCESS) {
3048                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
3049                                 libusb_error_name(usb_retval), usb_retval);
3050                 return NULL;
3051         } else if (desc_serial[1] != LIBUSB_DT_STRING || len > usb_retval) {
3052                 LOG_ERROR("invalid string in ST-LINK USB serial descriptor");
3053                 return NULL;
3054         }
3055
3056         if (len == ((STLINK_SERIAL_LEN + 1) * 2)) {
3057                 /* good ST-Link adapter, this case is managed by
3058                  * libusb::libusb_get_string_descriptor_ascii */
3059                 return NULL;
3060         } else if (len != ((STLINK_SERIAL_LEN / 2 + 1) * 2)) {
3061                 LOG_ERROR("unexpected serial length (%d) in descriptor", len);
3062                 return NULL;
3063         }
3064
3065         /* else (len == 26) => buggy ST-Link */
3066
3067         char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
3068         if (!alternate_serial)
3069                 return NULL;
3070
3071         for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
3072                 sprintf(alternate_serial + i, "%02X", desc_serial[i + 2]);
3073
3074         alternate_serial[STLINK_SERIAL_LEN] = '\0';
3075
3076         return alternate_serial;
3077 }
3078
3079 /** */
3080 static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
3081 {
3082         struct stlink_usb_handle_s *h = handle;
3083         int err, retry_count = 1;
3084
3085         h->cmdbuf = malloc(STLINK_SG_SIZE);
3086         h->databuf = malloc(STLINK_DATA_SIZE);
3087
3088         if (!h->cmdbuf || !h->databuf)
3089                 return ERROR_FAIL;
3090
3091         /*
3092           On certain host USB configurations(e.g. MacBook Air)
3093           STLINKv2 dongle seems to have its FW in a funky state if,
3094           after plugging it in, you try to use openocd with it more
3095           then once (by launching and closing openocd). In cases like
3096           that initial attempt to read the FW info via
3097           stlink_usb_version will fail and the device has to be reset
3098           in order to become operational.
3099          */
3100         do {
3101                 if (jtag_libusb_open(param->vid, param->pid, param->serial,
3102                                 &h->usb_backend_priv.fd, stlink_usb_get_alternate_serial) != ERROR_OK) {
3103                         LOG_ERROR("open failed");
3104                         return ERROR_FAIL;
3105                 }
3106
3107                 jtag_libusb_set_configuration(h->usb_backend_priv.fd, 0);
3108
3109                 if (libusb_claim_interface(h->usb_backend_priv.fd, 0) != ERROR_OK) {
3110                         LOG_DEBUG("claim interface failed");
3111                         return ERROR_FAIL;
3112                 }
3113
3114                 /* RX EP is common for all versions */
3115                 h->rx_ep = STLINK_RX_EP;
3116
3117                 uint16_t pid;
3118                 if (jtag_libusb_get_pid(libusb_get_device(h->usb_backend_priv.fd), &pid) != ERROR_OK) {
3119                         LOG_DEBUG("libusb_get_pid failed");
3120                         return ERROR_FAIL;
3121                 }
3122
3123                 /* wrap version for first read */
3124                 switch (pid) {
3125                         case STLINK_V1_PID:
3126                                 h->version.stlink = 1;
3127                                 h->tx_ep = STLINK_TX_EP;
3128                                 break;
3129                         case STLINK_V3_USBLOADER_PID:
3130                         case STLINK_V3E_PID:
3131                         case STLINK_V3S_PID:
3132                         case STLINK_V3_2VCP_PID:
3133                                 h->version.stlink = 3;
3134                                 h->tx_ep = STLINK_V2_1_TX_EP;
3135                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
3136                                 break;
3137                         case STLINK_V2_1_PID:
3138                         case STLINK_V2_1_NO_MSD_PID:
3139                                 h->version.stlink = 2;
3140                                 h->tx_ep = STLINK_V2_1_TX_EP;
3141                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
3142                                 break;
3143                         default:
3144                         /* fall through - we assume V2 to be the default version*/
3145                         case STLINK_V2_PID:
3146                                 h->version.stlink = 2;
3147                                 h->tx_ep = STLINK_TX_EP;
3148                                 h->trace_ep = STLINK_TRACE_EP;
3149                                 break;
3150                 }
3151
3152                 /* get the device version */
3153                 err = stlink_usb_version(h);
3154
3155                 if (err == ERROR_OK) {
3156                         break;
3157                 } else if (h->version.stlink == 1 ||
3158                            retry_count == 0) {
3159                         LOG_ERROR("read version failed");
3160                         return ERROR_FAIL;
3161                 } else {
3162                         err = libusb_release_interface(h->usb_backend_priv.fd, 0);
3163                         if (err != ERROR_OK) {
3164                                 LOG_ERROR("release interface failed");
3165                                 return ERROR_FAIL;
3166                         }
3167
3168                         err = libusb_reset_device(h->usb_backend_priv.fd);
3169                         if (err != ERROR_OK) {
3170                                 LOG_ERROR("reset device failed");
3171                                 return ERROR_FAIL;
3172                         }
3173
3174                         jtag_libusb_close(h->usb_backend_priv.fd);
3175                         /*
3176                           Give the device one second to settle down and
3177                           reenumerate.
3178                          */
3179                         usleep(1 * 1000 * 1000);
3180                         retry_count--;
3181                 }
3182         } while (1);
3183
3184         return ERROR_OK;
3185 }
3186
3187 /** */
3188 static int stlink_tcp_open(void *handle, struct hl_interface_param_s *param)
3189 {
3190         struct stlink_usb_handle_s *h = handle;
3191         int ret;
3192
3193         /* SWIM is not supported using stlink-server */
3194         if (h->st_mode ==  STLINK_MODE_DEBUG_SWIM) {
3195                 LOG_ERROR("stlink-server does not support SWIM mode");
3196                 return ERROR_FAIL;
3197         }
3198
3199         h->tcp_backend_priv.send_buf = malloc(STLINK_TCP_SEND_BUFFER_SIZE);
3200         h->tcp_backend_priv.recv_buf = malloc(STLINK_TCP_RECV_BUFFER_SIZE);
3201
3202         if (!h->tcp_backend_priv.send_buf || !h->tcp_backend_priv.recv_buf)
3203                 return ERROR_FAIL;
3204
3205         h->cmdbuf = &h->tcp_backend_priv.send_buf[8];
3206         h->databuf = &h->tcp_backend_priv.recv_buf[4];
3207
3208         /* configure directions */
3209         h->rx_ep = STLINK_TCP_REQUEST_READ;
3210         h->tx_ep = STLINK_TCP_REQUEST_WRITE;
3211         h->trace_ep = STLINK_TCP_REQUEST_READ_SWO;
3212
3213         h->tcp_backend_priv.fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
3214         h->tcp_backend_priv.connected = false;
3215         h->tcp_backend_priv.device_id = 0;
3216         h->tcp_backend_priv.connect_id = 0;
3217
3218         if (h->tcp_backend_priv.fd == -1) {
3219                 LOG_ERROR("error creating the socket, errno: %s", strerror(errno));
3220                 return ERROR_FAIL;
3221         }
3222
3223         struct sockaddr_in serv;
3224         memset(&serv, 0, sizeof(struct sockaddr_in));
3225         serv.sin_family = AF_INET;
3226         serv.sin_port = htons(param->stlink_tcp_port);
3227         serv.sin_addr.s_addr = inet_addr("127.0.0.1");
3228
3229         LOG_DEBUG("socket : %x", h->tcp_backend_priv.fd);
3230
3231         int optval = 1;
3232         if (setsockopt(h->tcp_backend_priv.fd, IPPROTO_TCP, TCP_NODELAY, (const void *)&optval, sizeof(int)) == -1) {
3233                 LOG_ERROR("cannot set sock option 'TCP_NODELAY', errno: %s", strerror(errno));
3234                 return ERROR_FAIL;
3235         }
3236
3237         optval = STLINK_TCP_RECV_BUFFER_SIZE;
3238         if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_RCVBUF, (const void *)&optval, sizeof(int)) == -1) {
3239                 LOG_ERROR("cannot set sock option 'SO_RCVBUF', errno: %s", strerror(errno));
3240                 return ERROR_FAIL;
3241         }
3242
3243         optval = STLINK_TCP_SEND_BUFFER_SIZE;
3244         if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_SNDBUF, (const void *)&optval, sizeof(int)) == -1) {
3245                 LOG_ERROR("cannot set sock option 'SO_SNDBUF', errno: %s", strerror(errno));
3246                 return ERROR_FAIL;
3247         }
3248
3249         if (connect(h->tcp_backend_priv.fd, (const struct sockaddr *)&serv, sizeof(serv)) == -1) {
3250                 LOG_ERROR("cannot connect to stlink server, errno: %s", strerror(errno));
3251                 return ERROR_FAIL;
3252         }
3253
3254         h->tcp_backend_priv.connected = true;
3255
3256         LOG_INFO("connected to stlink-server");
3257
3258         /* print stlink-server version */
3259         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_SERVER_VERSION;
3260         h->tcp_backend_priv.send_buf[1] = OPENOCD_STLINK_TCP_API_VERSION;
3261         memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
3262         ret = stlink_tcp_send_cmd(h, 4, 16, false);
3263         if (ret != ERROR_OK) {
3264                 LOG_ERROR("cannot get the stlink-server version");
3265                 return ERROR_FAIL;
3266         }
3267
3268         uint32_t api_ver = le_to_h_u32(&h->tcp_backend_priv.recv_buf[0]);
3269         uint32_t ver_major = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3270         uint32_t ver_minor = le_to_h_u32(&h->tcp_backend_priv.recv_buf[8]);
3271         uint32_t ver_build = le_to_h_u32(&h->tcp_backend_priv.recv_buf[12]);
3272         LOG_INFO("stlink-server API v%d, version %d.%d.%d",
3273                         api_ver, ver_major, ver_minor, ver_build);
3274
3275         /* in stlink-server API v1 sending more than 1428 bytes will cause stlink-server
3276          * to crash in windows: select a safe default value (1K) */
3277         if (api_ver < 2)
3278                 h->max_mem_packet = (1 << 10);
3279
3280         /* refresh stlink list (re-enumerate) */
3281         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_REFRESH_DEVICE_LIST;
3282         h->tcp_backend_priv.send_buf[1] = 0; /* don't clear the list, just refresh it */
3283         ret = stlink_tcp_send_cmd(h, 2, 4, true);
3284         if (ret != ERROR_OK)
3285                 return ret;
3286
3287         /* get the number of connected stlinks */
3288         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_NB_DEV;
3289         ret = stlink_tcp_send_cmd(h, 1, 4, false);
3290         if (ret != ERROR_OK)
3291                 return ret;
3292
3293         uint32_t connected_stlinks = le_to_h_u32(h->tcp_backend_priv.recv_buf);
3294
3295         if (connected_stlinks == 0) {
3296                 LOG_ERROR("no ST-LINK detected");
3297                 return ERROR_FAIL;
3298         }
3299
3300         LOG_DEBUG("%d ST-LINK detected", connected_stlinks);
3301
3302         if (connected_stlinks > 255) {
3303                 LOG_WARNING("STLink server cannot handle more than 255 ST-LINK connected");
3304                 connected_stlinks = 255;
3305         }
3306
3307         /* list all connected ST-Link and seek for the requested vid:pid and serial */
3308         char serial[STLINK_TCP_SERIAL_SIZE + 1] = {0};
3309         uint8_t stlink_used;
3310         bool stlink_id_matched = false;
3311         bool stlink_serial_matched = (!param->serial);
3312
3313         for (uint32_t stlink_id = 0; stlink_id < connected_stlinks; stlink_id++) {
3314                 /* get the stlink info */
3315                 h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_DEV_INFO;
3316                 h->tcp_backend_priv.send_buf[1] = (uint8_t)stlink_id;
3317                 memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
3318                 h_u32_to_le(&h->tcp_backend_priv.send_buf[4], 41); /* size of TDeviceInfo2 */
3319                 ret = stlink_tcp_send_cmd(h, 8, 45, true);
3320                 if (ret != ERROR_OK)
3321                         return ret;
3322
3323                 h->tcp_backend_priv.device_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3324                 memcpy(serial, &h->tcp_backend_priv.recv_buf[8], STLINK_TCP_SERIAL_SIZE);
3325                 h->vid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[40]);
3326                 h->pid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[42]);
3327                 stlink_used = h->tcp_backend_priv.recv_buf[44];
3328
3329                 /* check the vid:pid */
3330                 for (int i = 0; param->vid[i]; i++) {
3331                         if (param->vid[i] == h->vid && param->pid[i] == h->pid) {
3332                                 stlink_id_matched = true;
3333                                 break;
3334                         }
3335                 }
3336
3337                 if (!stlink_id_matched)
3338                         continue;
3339
3340                 /* check the serial if specified */
3341                 if (param->serial) {
3342                         /* ST-Link server fixes the buggy serial returned by old ST-Link DFU
3343                          * for further details refer to stlink_usb_get_alternate_serial
3344                          * so if the user passes the buggy serial, we need to fix it before
3345                          * comparing with the serial returned by ST-Link server */
3346                         if (strlen(param->serial) == STLINK_SERIAL_LEN / 2) {
3347                                 char fixed_serial[STLINK_SERIAL_LEN + 1];
3348
3349                                 for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
3350                                         sprintf(fixed_serial + i, "%02X", param->serial[i / 2]);
3351
3352                                 fixed_serial[STLINK_SERIAL_LEN] = '\0';
3353
3354                                 stlink_serial_matched = strcmp(fixed_serial, serial) == 0;
3355                         } else
3356                                 stlink_serial_matched = strcmp(param->serial, serial) == 0;
3357                 }
3358
3359                 if (!stlink_serial_matched)
3360                         LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
3361                                         serial, param->serial);
3362                 else /* exit the search loop if there is match */
3363                         break;
3364         }
3365
3366         if (!stlink_id_matched) {
3367                 LOG_ERROR("ST-LINK open failed (vid/pid mismatch)");
3368                 return ERROR_FAIL;
3369         }
3370
3371         if (!stlink_serial_matched) {
3372                 LOG_ERROR("ST-LINK open failed (serial mismatch)");
3373                 return ERROR_FAIL;
3374         }
3375
3376         /* check if device is 'exclusively' used by another application */
3377         if (stlink_used) {
3378                 LOG_ERROR("the selected device is already used");
3379                 return ERROR_FAIL;
3380         }
3381
3382         LOG_DEBUG("transport: vid: 0x%04x pid: 0x%04x serial: %s", h->vid, h->pid, serial);
3383
3384         /* now let's open the stlink */
3385         h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_OPEN_DEV;
3386         memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
3387         h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.device_id);
3388         ret = stlink_tcp_send_cmd(h, 8, 8, true);
3389         if (ret != ERROR_OK)
3390                 return ret;
3391
3392         h->tcp_backend_priv.connect_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
3393
3394         /* get stlink version */
3395         return stlink_usb_version(h);
3396 }
3397
3398 static struct stlink_backend_s stlink_usb_backend = {
3399         .open = stlink_usb_usb_open,
3400         .close = stlink_usb_usb_close,
3401         .xfer_noerrcheck = stlink_usb_usb_xfer_noerrcheck,
3402         .read_trace = stlink_usb_usb_read_trace,
3403 };
3404
3405 static struct stlink_backend_s stlink_tcp_backend = {
3406         .open = stlink_tcp_open,
3407         .close = stlink_tcp_close,
3408         .xfer_noerrcheck = stlink_tcp_xfer_noerrcheck,
3409         .read_trace = stlink_tcp_read_trace,
3410 };
3411
3412 static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode, void **fd)
3413 {
3414         struct stlink_usb_handle_s *h;
3415
3416         LOG_DEBUG("stlink_open");
3417
3418         h = calloc(1, sizeof(struct stlink_usb_handle_s));
3419
3420         if (h == 0) {
3421                 LOG_DEBUG("malloc failed");
3422                 return ERROR_FAIL;
3423         }
3424
3425         h->st_mode = mode;
3426
3427         for (unsigned i = 0; param->vid[i]; i++) {
3428                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
3429                           h->st_mode, param->vid[i], param->pid[i],
3430                           param->serial ? param->serial : "");
3431         }
3432
3433         if (param->use_stlink_tcp)
3434                 h->backend = &stlink_tcp_backend;
3435         else
3436                 h->backend = &stlink_usb_backend;
3437
3438         if (stlink_usb_open(h, param) != ERROR_OK)
3439                 goto error_open;
3440
3441         /* check if mode is supported */
3442         int err = ERROR_OK;
3443
3444         switch (h->st_mode) {
3445                 case STLINK_MODE_DEBUG_SWD:
3446                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
3447                                 err = ERROR_FAIL;
3448                         /* fall-through */
3449                 case STLINK_MODE_DEBUG_JTAG:
3450                         if (h->version.jtag == 0)
3451                                 err = ERROR_FAIL;
3452                         break;
3453                 case STLINK_MODE_DEBUG_SWIM:
3454                         if (h->version.swim == 0)
3455                                 err = ERROR_FAIL;
3456                         break;
3457                 default:
3458                         err = ERROR_FAIL;
3459                         break;
3460         }
3461
3462         if (err != ERROR_OK) {
3463                 LOG_ERROR("mode (transport) not supported by device");
3464                 goto error_open;
3465         }
3466
3467         /* initialize the debug hardware */
3468         err = stlink_usb_init_mode(h, param->connect_under_reset, param->initial_interface_speed);
3469
3470         if (err != ERROR_OK) {
3471                 LOG_ERROR("init mode failed (unable to connect to the target)");
3472                 goto error_open;
3473         }
3474
3475         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
3476                 err = stlink_swim_enter(h);
3477                 if (err != ERROR_OK) {
3478                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
3479                         goto error_open;
3480                 }
3481                 *fd = h;
3482                 h->max_mem_packet = STLINK_DATA_SIZE;
3483                 return ERROR_OK;
3484         }
3485
3486         /* set max_mem_packet if it was not set by the low-level interface */
3487         if (h->max_mem_packet == 0) {
3488                 /* get cpuid, so we can determine the max page size
3489                  * start with a safe default */
3490                 h->max_mem_packet = (1 << 10);
3491
3492                 uint8_t buffer[4];
3493                 stlink_usb_open_ap(h, 0);
3494                 err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
3495                 if (err == ERROR_OK) {
3496                         uint32_t cpuid = le_to_h_u32(buffer);
3497                         int i = (cpuid >> 4) & 0xf;
3498                         if (i == 4 || i == 3) {
3499                                 /* Cortex-M3/M4 has 4096 bytes autoincrement range */
3500                                 h->max_mem_packet = (1 << 12);
3501                         }
3502                 }
3503
3504                 LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
3505         }
3506
3507         *fd = h;
3508
3509         return ERROR_OK;
3510
3511 error_open:
3512         stlink_close(h);
3513         return ERROR_FAIL;
3514 }
3515
3516 static int stlink_usb_hl_open(struct hl_interface_param_s *param, void **fd)
3517 {
3518         return stlink_open(param, stlink_get_mode(param->transport), fd);
3519 }
3520
3521 static int stlink_config_trace(void *handle, bool enabled,
3522                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3523                 unsigned int *trace_freq, unsigned int traceclkin_freq,
3524                 uint16_t *prescaler)
3525 {
3526         struct stlink_usb_handle_s *h = handle;
3527
3528         if (!(h->version.flags & STLINK_F_HAS_TRACE)) {
3529                 LOG_ERROR("The attached ST-LINK version doesn't support trace");
3530                 return ERROR_FAIL;
3531         }
3532
3533         if (!enabled) {
3534                 stlink_usb_trace_disable(h);
3535                 return ERROR_OK;
3536         }
3537
3538         assert(trace_freq);
3539         assert(prescaler);
3540
3541         if (pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART) {
3542                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
3543                 return ERROR_FAIL;
3544         }
3545
3546         unsigned int max_trace_freq = (h->version.stlink == 3) ?
3547                         STLINK_V3_TRACE_MAX_HZ : STLINK_TRACE_MAX_HZ;
3548
3549         /* Only concern ourselves with the frequency if the STlink is processing it. */
3550         if (*trace_freq > max_trace_freq) {
3551                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
3552                           max_trace_freq);
3553                 return ERROR_FAIL;
3554         }
3555
3556         if (!*trace_freq)
3557                 *trace_freq = max_trace_freq;
3558
3559         unsigned int presc = (traceclkin_freq + *trace_freq / 2) / *trace_freq;
3560         if (presc == 0 || presc > TPIU_ACPR_MAX_SWOSCALER + 1) {
3561                 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3562                         "frequency.");
3563                 return ERROR_FAIL;
3564         }
3565
3566         /* Probe's UART speed must be within 3% of the TPIU's SWO baud rate. */
3567         unsigned int max_deviation = (traceclkin_freq * 3) / 100;
3568         if (presc * *trace_freq < traceclkin_freq - max_deviation ||
3569                         presc * *trace_freq > traceclkin_freq + max_deviation) {
3570                 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3571                         "frequency.");
3572                 return ERROR_FAIL;
3573         }
3574
3575         *prescaler = presc;
3576
3577         stlink_usb_trace_disable(h);
3578
3579         h->trace.source_hz = *trace_freq;
3580
3581         return stlink_usb_trace_enable(h);
3582 }
3583
3584 /** */
3585 static int stlink_usb_init_access_port(void *handle, unsigned char ap_num)
3586 {
3587         struct stlink_usb_handle_s *h = handle;
3588
3589         assert(handle);
3590
3591         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3592                 return ERROR_COMMAND_NOTFOUND;
3593
3594         LOG_DEBUG_IO("init ap_num = %d", ap_num);
3595         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3596         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3597         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_INIT_AP;
3598         h->cmdbuf[h->cmdidx++] = ap_num;
3599
3600         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3601 }
3602
3603 /** */
3604 static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
3605 {
3606         struct stlink_usb_handle_s *h = handle;
3607
3608         assert(handle);
3609
3610         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3611                 return ERROR_COMMAND_NOTFOUND;
3612
3613         LOG_DEBUG_IO("close ap_num = %d", ap_num);
3614         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3615         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3616         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_CLOSE_AP_DBG;
3617         h->cmdbuf[h->cmdidx++] = ap_num;
3618
3619         /* ignore incorrectly returned error on bogus FW */
3620         if (h->version.flags & STLINK_F_FIX_CLOSE_AP)
3621                 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3622         else
3623                 return stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
3624
3625 }
3626
3627 /** */
3628 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
3629                         unsigned short addr, uint32_t *val)
3630 {
3631         struct stlink_usb_handle_s *h = handle;
3632         int retval;
3633
3634         assert(handle);
3635
3636         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3637                 return ERROR_COMMAND_NOTFOUND;
3638
3639         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3640         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3641         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_DAP_REG;
3642         h_u16_to_le(&h->cmdbuf[2], dap_port);
3643         h_u16_to_le(&h->cmdbuf[4], addr);
3644
3645         retval = stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3646         *val = le_to_h_u32(h->databuf + 4);
3647         LOG_DEBUG_IO("dap_port_read = %d, addr =  0x%x, value = 0x%" PRIx32, dap_port, addr, *val);
3648         return retval;
3649 }
3650
3651 /** */
3652 static int stlink_write_dap_register(void *handle, unsigned short dap_port,
3653                         unsigned short addr, uint32_t val)
3654 {
3655         struct stlink_usb_handle_s *h = handle;
3656
3657         assert(handle);
3658
3659         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3660                 return ERROR_COMMAND_NOTFOUND;
3661
3662         LOG_DEBUG_IO("dap_write port = %d, addr = 0x%x, value = 0x%" PRIx32, dap_port, addr, val);
3663         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3664         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3665         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITE_DAP_REG;
3666         h_u16_to_le(&h->cmdbuf[2], dap_port);
3667         h_u16_to_le(&h->cmdbuf[4], addr);
3668         h_u32_to_le(&h->cmdbuf[6], val);
3669         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3670 }
3671
3672 /** */
3673 struct hl_layout_api_s stlink_usb_layout_api = {
3674         /** */
3675         .open = stlink_usb_hl_open,
3676         /** */
3677         .close = stlink_close,
3678         /** */
3679         .idcode = stlink_usb_idcode,
3680         /** */
3681         .state = stlink_usb_state,
3682         /** */
3683         .reset = stlink_usb_reset,
3684         /** */
3685         .assert_srst = stlink_usb_assert_srst,
3686         /** */
3687         .run = stlink_usb_run,
3688         /** */
3689         .halt = stlink_usb_halt,
3690         /** */
3691         .step = stlink_usb_step,
3692         /** */
3693         .read_regs = stlink_usb_read_regs,
3694         /** */
3695         .read_reg = stlink_usb_read_reg,
3696         /** */
3697         .write_reg = stlink_usb_write_reg,
3698         /** */
3699         .read_mem = stlink_usb_read_mem,
3700         /** */
3701         .write_mem = stlink_usb_write_mem,
3702         /** */
3703         .write_debug_reg = stlink_usb_write_debug_reg,
3704         /** */
3705         .override_target = stlink_usb_override_target,
3706         /** */
3707         .speed = stlink_speed,
3708         /** */
3709         .config_trace = stlink_config_trace,
3710         /** */
3711         .poll_trace = stlink_usb_trace_read,
3712 };
3713
3714 /*****************************************************************************
3715  * DAP direct interface
3716  */
3717
3718 static struct stlink_usb_handle_s *stlink_dap_handle;
3719 static struct hl_interface_param_s stlink_dap_param;
3720 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
3721 static int stlink_dap_error = ERROR_OK;
3722
3723 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
3724                 uint32_t *data);
3725
3726 /** */
3727 static int stlink_dap_record_error(int error)
3728 {
3729         if (stlink_dap_error == ERROR_OK)
3730                 stlink_dap_error = error;
3731         return ERROR_OK;
3732 }
3733
3734 /** */
3735 static int stlink_dap_get_and_clear_error(void)
3736 {
3737         int retval = stlink_dap_error;
3738         stlink_dap_error = ERROR_OK;
3739         return retval;
3740 }
3741
3742 static int stlink_usb_open_ap(void *handle, unsigned short apsel)
3743 {
3744         struct stlink_usb_handle_s *h = handle;
3745         int retval;
3746
3747         /* nothing to do on old versions */
3748         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3749                 return ERROR_OK;
3750
3751         if (apsel > DP_APSEL_MAX)
3752                 return ERROR_FAIL;
3753
3754         if (test_bit(apsel, opened_ap))
3755                 return ERROR_OK;
3756
3757         retval = stlink_usb_init_access_port(h, apsel);
3758         if (retval != ERROR_OK)
3759                 return retval;
3760
3761         LOG_DEBUG("AP %d enabled", apsel);
3762         set_bit(apsel, opened_ap);
3763         return ERROR_OK;
3764 }
3765
3766 static int stlink_dap_open_ap(unsigned short apsel)
3767 {
3768         return stlink_usb_open_ap(stlink_dap_handle, apsel);
3769 }
3770
3771 /** */
3772 static int stlink_dap_closeall_ap(void)
3773 {
3774         int retval, apsel;
3775
3776         /* nothing to do on old versions */
3777         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
3778                 return ERROR_OK;
3779
3780         for (apsel = 0; apsel <= DP_APSEL_MAX; apsel++) {
3781                 if (!test_bit(apsel, opened_ap))
3782                         continue;
3783                 retval = stlink_usb_close_access_port(stlink_dap_handle, apsel);
3784                 if (retval != ERROR_OK)
3785                         return retval;
3786                 clear_bit(apsel, opened_ap);
3787         }
3788         return ERROR_OK;
3789 }
3790
3791 /** */
3792 static int stlink_dap_reinit_interface(void)
3793 {
3794         int retval;
3795
3796         /*
3797          * On JTAG only, it should be enough to call stlink_usb_reset(). But on
3798          * some firmware version it does not work as expected, and there is no
3799          * equivalent for SWD.
3800          * At least for now, to reset the interface quit from JTAG/SWD mode then
3801          * select the mode again.
3802          */
3803
3804         if (!stlink_dap_handle->reconnect_pending) {
3805                 stlink_dap_handle->reconnect_pending = true;
3806                 stlink_usb_mode_leave(stlink_dap_handle, stlink_dap_handle->st_mode);
3807         }
3808
3809         retval = stlink_usb_mode_enter(stlink_dap_handle, stlink_dap_handle->st_mode);
3810         if (retval != ERROR_OK)
3811                 return retval;
3812
3813         stlink_dap_handle->reconnect_pending = false;
3814         /* on new FW, calling mode-leave closes all the opened AP; reopen them! */
3815         if (stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT)
3816                 for (int apsel = 0; apsel <= DP_APSEL_MAX; apsel++)
3817                         if (test_bit(apsel, opened_ap)) {
3818                                 clear_bit(apsel, opened_ap);
3819                                 stlink_dap_open_ap(apsel);
3820                         }
3821         return ERROR_OK;
3822 }
3823
3824 /** */
3825 static int stlink_dap_op_connect(struct adiv5_dap *dap)
3826 {
3827         uint32_t idcode;
3828         int retval;
3829
3830         LOG_INFO("stlink_dap_op_connect(%sconnect)", dap->do_reconnect ? "re" : "");
3831
3832         /* Check if we should reset srst already when connecting, but not if reconnecting. */
3833         if (!dap->do_reconnect) {
3834                 enum reset_types jtag_reset_config = jtag_get_reset_config();
3835
3836                 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
3837                         if (jtag_reset_config & RESET_SRST_NO_GATING)
3838                                 adapter_assert_reset();
3839                         else
3840                                 LOG_WARNING("\'srst_nogate\' reset_config option is required");
3841                 }
3842         }
3843
3844         dap->do_reconnect = false;
3845         dap_invalidate_cache(dap);
3846
3847         retval = dap_dp_init(dap);
3848         if (retval != ERROR_OK) {
3849                 dap->do_reconnect = true;
3850                 return retval;
3851         }
3852
3853         retval = stlink_usb_idcode(stlink_dap_handle, &idcode);
3854         if (retval == ERROR_OK)
3855                 LOG_INFO("%s %#8.8" PRIx32,
3856                         (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) ? "JTAG IDCODE" : "SWD DPIDR",
3857                         idcode);
3858         else
3859                 dap->do_reconnect = true;
3860
3861         return retval;
3862 }
3863
3864 /** */
3865 static int stlink_dap_check_reconnect(struct adiv5_dap *dap)
3866 {
3867         int retval;
3868
3869         if (!dap->do_reconnect)
3870                 return ERROR_OK;
3871
3872         retval = stlink_dap_reinit_interface();
3873         if (retval != ERROR_OK)
3874                 return retval;
3875
3876         return stlink_dap_op_connect(dap);
3877 }
3878
3879 /** */
3880 static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
3881 {
3882         /* Ignore the request */
3883         return ERROR_OK;
3884 }
3885
3886 /** */
3887 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
3888                 uint32_t *data)
3889 {
3890         uint32_t dummy;
3891         int retval;
3892
3893         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
3894                 if (reg & 0x000000F0) {
3895                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
3896                         return ERROR_COMMAND_NOTFOUND;
3897                 }
3898
3899         retval = stlink_dap_check_reconnect(dap);
3900         if (retval != ERROR_OK)
3901                 return retval;
3902
3903         data = data ? data : &dummy;
3904         if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ
3905                 && stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) {
3906                 /* Quirk required in JTAG. Read RDBUFF to get the data */
3907                 retval = stlink_read_dap_register(stlink_dap_handle,
3908                                         STLINK_DEBUG_PORT_ACCESS, reg, &dummy);
3909                 if (retval == ERROR_OK)
3910                         retval = stlink_read_dap_register(stlink_dap_handle,
3911                                                 STLINK_DEBUG_PORT_ACCESS, DP_RDBUFF, data);
3912         } else {
3913                 retval = stlink_read_dap_register(stlink_dap_handle,
3914                                         STLINK_DEBUG_PORT_ACCESS, reg, data);
3915         }
3916
3917         return stlink_dap_record_error(retval);
3918 }
3919
3920 /** */
3921 static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
3922                 uint32_t data)
3923 {
3924         int retval;
3925
3926         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
3927                 if (reg & 0x000000F0) {
3928                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
3929                         return ERROR_COMMAND_NOTFOUND;
3930                 }
3931
3932         if (reg == DP_SELECT && (data & DP_SELECT_DPBANK) != 0) {
3933                 /* ignored if STLINK_F_HAS_DPBANKSEL, not properly managed otherwise */
3934                 LOG_DEBUG("Ignoring DPBANKSEL while write SELECT");
3935                 data &= ~DP_SELECT_DPBANK;
3936         }
3937
3938         retval = stlink_dap_check_reconnect(dap);
3939         if (retval != ERROR_OK)
3940                 return retval;
3941
3942         /* ST-Link does not like that we set CORUNDETECT */
3943         if (reg == DP_CTRL_STAT)
3944                 data &= ~CORUNDETECT;
3945
3946         retval = stlink_write_dap_register(stlink_dap_handle,
3947                                 STLINK_DEBUG_PORT_ACCESS, reg, data);
3948         return stlink_dap_record_error(retval);
3949 }
3950
3951 /** */
3952 static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
3953                 uint32_t *data)
3954 {
3955         struct adiv5_dap *dap = ap->dap;
3956         uint32_t dummy;
3957         int retval;
3958
3959         retval = stlink_dap_check_reconnect(dap);
3960         if (retval != ERROR_OK)
3961                 return retval;
3962
3963         if (reg != AP_REG_IDR) {
3964                 retval = stlink_dap_open_ap(ap->ap_num);
3965                 if (retval != ERROR_OK)
3966                         return retval;
3967         }
3968         data = data ? data : &dummy;
3969         retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
3970                                  data);
3971         dap->stlink_flush_ap_write = false;
3972         return stlink_dap_record_error(retval);
3973 }
3974
3975 /** */
3976 static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
3977                 uint32_t data)
3978 {
3979         struct adiv5_dap *dap = ap->dap;
3980         int retval;
3981
3982         retval = stlink_dap_check_reconnect(dap);
3983         if (retval != ERROR_OK)
3984                 return retval;
3985
3986         retval = stlink_dap_open_ap(ap->ap_num);
3987         if (retval != ERROR_OK)
3988                 return retval;
3989
3990         retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
3991                                 data);
3992         dap->stlink_flush_ap_write = true;
3993         return stlink_dap_record_error(retval);
3994 }
3995
3996 /** */
3997 static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
3998 {
3999         LOG_WARNING("stlink_dap_op_queue_ap_abort()");
4000         return ERROR_OK;
4001 }
4002
4003 /** */
4004 static int stlink_dap_op_run(struct adiv5_dap *dap)
4005 {
4006         uint32_t ctrlstat, pwrmask;
4007         int retval, saved_retval;
4008
4009         /* Here no LOG_DEBUG. This is called continuously! */
4010
4011         /*
4012          * ST-Link returns immediately after a DAP write, without waiting for it
4013          * to complete.
4014          * Run a dummy read to DP_RDBUFF, as suggested in
4015          * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html
4016          */
4017         if (dap->stlink_flush_ap_write) {
4018                 dap->stlink_flush_ap_write = false;
4019                 retval = stlink_dap_op_queue_dp_read(dap, DP_RDBUFF, NULL);
4020                 if (retval != ERROR_OK) {
4021                         dap->do_reconnect = true;
4022                         return retval;
4023                 }
4024         }
4025
4026         saved_retval = stlink_dap_get_and_clear_error();
4027
4028         retval = stlink_dap_op_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
4029         if (retval != ERROR_OK) {
4030                 dap->do_reconnect = true;
4031                 return retval;
4032         }
4033         retval = stlink_dap_get_and_clear_error();
4034         if (retval != ERROR_OK) {
4035                 LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
4036                 dap->do_reconnect = true;
4037                 return retval;
4038         }
4039
4040         if (ctrlstat & SSTICKYERR) {
4041                 if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG)
4042                         retval = stlink_dap_op_queue_dp_write(dap, DP_CTRL_STAT,
4043                                         ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
4044                 else
4045                         retval = stlink_dap_op_queue_dp_write(dap, DP_ABORT, STKERRCLR);
4046                 if (retval != ERROR_OK) {
4047                         dap->do_reconnect = true;
4048                         return retval;
4049                 }
4050                 retval = stlink_dap_get_and_clear_error();
4051                 if (retval != ERROR_OK) {
4052                         dap->do_reconnect = true;
4053                         return retval;
4054                 }
4055         }
4056
4057         /* check for power lost */
4058         pwrmask = dap->dp_ctrl_stat & (CDBGPWRUPREQ | CSYSPWRUPREQ);
4059         if ((ctrlstat & pwrmask) != pwrmask)
4060                 dap->do_reconnect = true;
4061
4062         return saved_retval;
4063 }
4064
4065 /** */
4066 static void stlink_dap_op_quit(struct adiv5_dap *dap)
4067 {
4068         int retval;
4069
4070         retval = stlink_dap_closeall_ap();
4071         if (retval != ERROR_OK)
4072                 LOG_ERROR("Error closing APs");
4073 }
4074
4075 static int stlink_swim_op_srst(void)
4076 {
4077         return stlink_swim_generate_rst(stlink_dap_handle);
4078 }
4079
4080 static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
4081                                                                    uint32_t count, uint8_t *buffer)
4082 {
4083         int retval;
4084         uint32_t bytes_remaining;
4085
4086         LOG_DEBUG_IO("read at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4087         count *= size;
4088
4089         while (count) {
4090                 bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
4091                 retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4092                 if (retval != ERROR_OK)
4093                         return retval;
4094
4095                 buffer += bytes_remaining;
4096                 addr += bytes_remaining;
4097                 count -= bytes_remaining;
4098         }
4099
4100         return ERROR_OK;
4101 }
4102
4103 static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
4104                                                                         uint32_t count, const uint8_t *buffer)
4105 {
4106         int retval;
4107         uint32_t bytes_remaining;
4108
4109         LOG_DEBUG_IO("write at 0x%08" PRIx32 " len %" PRIu32 "*0x%08" PRIx32, addr, size, count);
4110         count *= size;
4111
4112         while (count) {
4113                 bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
4114                 retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
4115                 if (retval != ERROR_OK)
4116                         return retval;
4117
4118                 buffer += bytes_remaining;
4119                 addr += bytes_remaining;
4120                 count -= bytes_remaining;
4121         }
4122
4123         return ERROR_OK;
4124 }
4125
4126 static int stlink_swim_op_reconnect(void)
4127 {
4128         int retval;
4129
4130         retval = stlink_usb_mode_enter(stlink_dap_handle, STLINK_MODE_DEBUG_SWIM);
4131         if (retval != ERROR_OK)
4132                 return retval;
4133
4134         return stlink_swim_resync(stlink_dap_handle);
4135 }
4136
4137 static int stlink_dap_config_trace(bool enabled,
4138                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
4139                 unsigned int *trace_freq, unsigned int traceclkin_freq,
4140                 uint16_t *prescaler)
4141 {
4142         return stlink_config_trace(stlink_dap_handle, enabled, pin_protocol,
4143                                                            port_size, trace_freq, traceclkin_freq,
4144                                                            prescaler);
4145 }
4146
4147 static int stlink_dap_trace_read(uint8_t *buf, size_t *size)
4148 {
4149         return stlink_usb_trace_read(stlink_dap_handle, buf, size);
4150 }
4151
4152 /** */
4153 COMMAND_HANDLER(stlink_dap_serial_command)
4154 {
4155         LOG_DEBUG("stlink_dap_serial_command");
4156
4157         if (CMD_ARGC != 1) {
4158                 LOG_ERROR("Expected exactly one argument for \"st-link serial <serial-number>\".");
4159                 return ERROR_COMMAND_SYNTAX_ERROR;
4160         }
4161
4162         if (stlink_dap_param.serial) {
4163                 LOG_WARNING("Command \"st-link serial\" already used. Replacing previous value");
4164                 free((void *)stlink_dap_param.serial);
4165         }
4166
4167         stlink_dap_param.serial = strdup(CMD_ARGV[0]);
4168         return ERROR_OK;
4169 }
4170
4171 /** */
4172 COMMAND_HANDLER(stlink_dap_vid_pid)
4173 {
4174         unsigned int i, max_usb_ids = HLA_MAX_USB_IDS;
4175
4176         if (CMD_ARGC > max_usb_ids * 2) {
4177                 LOG_WARNING("ignoring extra IDs in vid_pid "
4178                         "(maximum is %d pairs)", max_usb_ids);
4179                 CMD_ARGC = max_usb_ids * 2;
4180         }
4181         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
4182                 LOG_WARNING("incomplete vid_pid configuration directive");
4183                 return ERROR_COMMAND_SYNTAX_ERROR;
4184         }
4185         for (i = 0; i < CMD_ARGC; i += 2) {
4186                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], stlink_dap_param.vid[i / 2]);
4187                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], stlink_dap_param.pid[i / 2]);
4188         }
4189
4190         /* null termination */
4191         stlink_dap_param.vid[i / 2] = stlink_dap_param.pid[i / 2] = 0;
4192
4193         return ERROR_OK;
4194 }
4195
4196 /** */
4197 COMMAND_HANDLER(stlink_dap_backend_command)
4198 {
4199         /* default values */
4200         bool use_stlink_tcp = false;
4201         uint16_t stlink_tcp_port = 7184;
4202
4203         if (CMD_ARGC == 0 || CMD_ARGC > 2)
4204                 return ERROR_COMMAND_SYNTAX_ERROR;
4205         else if (strcmp(CMD_ARGV[0], "usb") == 0) {
4206                 if (CMD_ARGC > 1)
4207                         return ERROR_COMMAND_SYNTAX_ERROR;
4208                 /* else use_stlink_tcp = false (already the case ) */
4209         } else if (strcmp(CMD_ARGV[0], "tcp") == 0) {
4210                 use_stlink_tcp = true;
4211                 if (CMD_ARGC == 2)
4212                         COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_tcp_port);
4213         } else
4214                 return ERROR_COMMAND_SYNTAX_ERROR;
4215
4216         stlink_dap_param.use_stlink_tcp = use_stlink_tcp;
4217         stlink_dap_param.stlink_tcp_port = stlink_tcp_port;
4218
4219         return ERROR_OK;
4220 }
4221
4222 /** */
4223 static const struct command_registration stlink_dap_subcommand_handlers[] = {
4224         {
4225                 .name = "serial",
4226                 .handler = stlink_dap_serial_command,
4227                 .mode = COMMAND_CONFIG,
4228                 .help = "set the serial number of the adapter",
4229                 .usage = "<serial_number>",
4230         },
4231         {
4232                 .name = "vid_pid",
4233                 .handler = stlink_dap_vid_pid,
4234                 .mode = COMMAND_CONFIG,
4235                 .help = "USB VID and PID of the adapter",
4236                 .usage = "(vid pid)+",
4237         },
4238         {
4239                 .name = "backend",
4240                 .handler = &stlink_dap_backend_command,
4241                 .mode = COMMAND_CONFIG,
4242                 .help = "select which ST-Link backend to use",
4243                 .usage = "usb | tcp [port]",
4244         },
4245         COMMAND_REGISTRATION_DONE
4246 };
4247
4248 /** */
4249 static const struct command_registration stlink_dap_command_handlers[] = {
4250         {
4251                 .name = "st-link",
4252                 .mode = COMMAND_ANY,
4253                 .help = "perform st-link management",
4254                 .chain = stlink_dap_subcommand_handlers,
4255                 .usage = "",
4256         },
4257         COMMAND_REGISTRATION_DONE
4258 };
4259
4260 /** */
4261 static int stlink_dap_init(void)
4262 {
4263         enum reset_types jtag_reset_config = jtag_get_reset_config();
4264         enum stlink_mode mode;
4265         int retval;
4266
4267         LOG_DEBUG("stlink_dap_init()");
4268
4269         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
4270                 if (jtag_reset_config & RESET_SRST_NO_GATING)
4271                         stlink_dap_param.connect_under_reset = true;
4272                 else
4273                         LOG_WARNING("\'srst_nogate\' reset_config option is required");
4274         }
4275
4276         if (transport_is_dapdirect_swd())
4277                 mode = STLINK_MODE_DEBUG_SWD;
4278         else if (transport_is_dapdirect_jtag())
4279                 mode = STLINK_MODE_DEBUG_JTAG;
4280         else if (transport_is_swim())
4281                 mode = STLINK_MODE_DEBUG_SWIM;
4282         else {
4283                 LOG_ERROR("Unsupported transport");
4284                 return ERROR_FAIL;
4285         }
4286
4287         retval = stlink_open(&stlink_dap_param, mode, (void **)&stlink_dap_handle);
4288         if (retval != ERROR_OK)
4289                 return retval;
4290
4291         if ((mode != STLINK_MODE_DEBUG_SWIM) &&
4292                 !(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
4293                 LOG_ERROR("ST-Link version does not support DAP direct transport");
4294                 return ERROR_FAIL;
4295         }
4296         return ERROR_OK;
4297 }
4298
4299 /** */
4300 static int stlink_dap_quit(void)
4301 {
4302         LOG_DEBUG("stlink_dap_quit()");
4303
4304         free((void *)stlink_dap_param.serial);
4305         stlink_dap_param.serial = NULL;
4306
4307         return stlink_close(stlink_dap_handle);
4308 }
4309
4310 /** */
4311 static int stlink_dap_reset(int req_trst, int req_srst)
4312 {
4313         LOG_DEBUG("stlink_dap_reset(%d)", req_srst);
4314         return stlink_usb_assert_srst(stlink_dap_handle,
4315                 req_srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
4316                                  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
4317 }
4318
4319 /** */
4320 static int stlink_dap_speed(int speed)
4321 {
4322         if (speed == 0) {
4323                 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
4324                 return ERROR_JTAG_NOT_IMPLEMENTED;
4325         }
4326
4327         stlink_dap_param.initial_interface_speed = speed;
4328         stlink_speed(stlink_dap_handle, speed, false);
4329         return ERROR_OK;
4330 }
4331
4332 /** */
4333 static int stlink_dap_khz(int khz, int *jtag_speed)
4334 {
4335         if (khz == 0) {
4336                 LOG_ERROR("RCLK not supported");
4337                 return ERROR_FAIL;
4338         }
4339
4340         *jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
4341         return ERROR_OK;
4342 }
4343
4344 /** */
4345 static int stlink_dap_speed_div(int speed, int *khz)
4346 {
4347         *khz = speed;
4348         return ERROR_OK;
4349 }
4350
4351 static const struct dap_ops stlink_dap_ops = {
4352         .connect = stlink_dap_op_connect,
4353         .send_sequence = stlink_dap_op_send_sequence,
4354         .queue_dp_read = stlink_dap_op_queue_dp_read,
4355         .queue_dp_write = stlink_dap_op_queue_dp_write,
4356         .queue_ap_read = stlink_dap_op_queue_ap_read,
4357         .queue_ap_write = stlink_dap_op_queue_ap_write,
4358         .queue_ap_abort = stlink_dap_op_queue_ap_abort,
4359         .run = stlink_dap_op_run,
4360         .sync = NULL, /* optional */
4361         .quit = stlink_dap_op_quit, /* optional */
4362 };
4363
4364 static const struct swim_driver stlink_swim_ops = {
4365         .srst = stlink_swim_op_srst,
4366         .read_mem = stlink_swim_op_read_mem,
4367         .write_mem = stlink_swim_op_write_mem,
4368         .reconnect = stlink_swim_op_reconnect,
4369 };
4370
4371 static const char *const stlink_dap_transport[] = { "dapdirect_swd", "dapdirect_jtag", "swim", NULL };
4372
4373 struct adapter_driver stlink_dap_adapter_driver = {
4374         .name = "st-link",
4375         .transports = stlink_dap_transport,
4376         .commands = stlink_dap_command_handlers,
4377
4378         .init = stlink_dap_init,
4379         .quit = stlink_dap_quit,
4380         .reset = stlink_dap_reset,
4381         .speed = stlink_dap_speed,
4382         .khz = stlink_dap_khz,
4383         .speed_div = stlink_dap_speed_div,
4384         .config_trace = stlink_dap_config_trace,
4385         .poll_trace = stlink_dap_trace_read,
4386
4387         .dap_jtag_ops = &stlink_dap_ops,
4388         .dap_swd_ops = &stlink_dap_ops,
4389         .swim_ops = &stlink_swim_ops,
4390 };