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

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