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