git.gag.com Git - fw/openocd/blob - src/jtag/drivers/stlink_usb.c

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