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