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

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