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