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         default:
1292                 return STLINK_MODE_UNKNOWN;
1293         }
1294 }
1295
1296 /** */
1297 static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int initial_interface_speed)
1298 {
1299         int res;
1300         uint8_t mode;
1301         enum stlink_mode emode;
1302         struct stlink_usb_handle_s *h = handle;
1303
1304         assert(handle != NULL);
1305
1306         res = stlink_usb_current_mode(handle, &mode);
1307
1308         if (res != ERROR_OK)
1309                 return res;
1310
1311         LOG_DEBUG("MODE: 0x%02X", mode);
1312
1313         /* try to exit current mode */
1314         switch (mode) {
1315                 case STLINK_DEV_DFU_MODE:
1316                         emode = STLINK_MODE_DFU;
1317                         break;
1318                 case STLINK_DEV_DEBUG_MODE:
1319                         emode = STLINK_MODE_DEBUG_SWD;
1320                         break;
1321                 case STLINK_DEV_SWIM_MODE:
1322                         emode = STLINK_MODE_DEBUG_SWIM;
1323                         break;
1324                 case STLINK_DEV_BOOTLOADER_MODE:
1325                 case STLINK_DEV_MASS_MODE:
1326                 default:
1327                         emode = STLINK_MODE_UNKNOWN;
1328                         break;
1329         }
1330
1331         if (emode != STLINK_MODE_UNKNOWN) {
1332                 res = stlink_usb_mode_leave(handle, emode);
1333
1334                 if (res != ERROR_OK)
1335                         return res;
1336         }
1337
1338         res = stlink_usb_current_mode(handle, &mode);
1339
1340         if (res != ERROR_OK)
1341                 return res;
1342
1343         /* we check the target voltage here as an aid to debugging connection problems.
1344          * the stlink requires the target Vdd to be connected for reliable debugging.
1345          * this cmd is supported in all modes except DFU
1346          */
1347         if (mode != STLINK_DEV_DFU_MODE) {
1348
1349                 float target_voltage;
1350
1351                 /* check target voltage (if supported) */
1352                 res = stlink_usb_check_voltage(h, &target_voltage);
1353
1354                 if (res != ERROR_OK) {
1355                         if (res != ERROR_COMMAND_NOTFOUND)
1356                                 LOG_ERROR("voltage check failed");
1357                         /* attempt to continue as it is not a catastrophic failure */
1358                 } else {
1359                         /* check for a sensible target voltage, operating range is 1.65-5.5v
1360                          * according to datasheet */
1361                         if (target_voltage < 1.5)
1362                                 LOG_ERROR("target voltage may be too low for reliable debugging");
1363                 }
1364         }
1365
1366         LOG_DEBUG("MODE: 0x%02X", mode);
1367
1368         /* set selected mode */
1369         emode = h->st_mode;
1370
1371         if (emode == STLINK_MODE_UNKNOWN) {
1372                 LOG_ERROR("selected mode (transport) not supported");
1373                 return ERROR_FAIL;
1374         }
1375
1376         /* set the speed before entering the mode, as the chip discovery phase should be done at this speed too */
1377         if (emode == STLINK_MODE_DEBUG_JTAG) {
1378                 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
1379                         stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
1380                         stlink_speed(h, initial_interface_speed, false);
1381                 }
1382         } else if (emode == STLINK_MODE_DEBUG_SWD) {
1383                 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
1384                         stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
1385                         stlink_speed(h, initial_interface_speed, false);
1386                 }
1387         }
1388
1389         if (h->version.jtag_api == STLINK_JTAG_API_V3) {
1390                 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
1391
1392                 stlink_get_com_freq(h, (emode == STLINK_MODE_DEBUG_JTAG), map);
1393                 stlink_dump_speed_map(map, ARRAY_SIZE(map));
1394                 stlink_speed(h, initial_interface_speed, false);
1395         }
1396
1397         /* preliminary SRST assert:
1398          * We want SRST is asserted before activating debug signals (mode_enter).
1399          * As the required mode has not been set, the adapter may not know what pin to use.
1400          * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1401          * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1402          * after power on, SWIM_RST stays unchanged */
1403         if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1404                 stlink_usb_assert_srst(handle, 0);
1405                 /* do not check the return status here, we will
1406                    proceed and enter the desired mode below
1407                    and try asserting srst again. */
1408
1409         res = stlink_usb_mode_enter(handle, emode);
1410         if (res != ERROR_OK)
1411                 return res;
1412
1413         /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1414         if (connect_under_reset) {
1415                 res = stlink_usb_assert_srst(handle, 0);
1416                 if (res != ERROR_OK)
1417                         return res;
1418         }
1419
1420         res = stlink_usb_current_mode(handle, &mode);
1421
1422         if (res != ERROR_OK)
1423                 return res;
1424
1425         LOG_DEBUG("MODE: 0x%02X", mode);
1426
1427         return ERROR_OK;
1428 }
1429
1430 /* request status from last swim request */
1431 static int stlink_swim_status(void *handle)
1432 {
1433         struct stlink_usb_handle_s *h = handle;
1434         int res;
1435
1436         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1437         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1438         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1439         /* error is checked by the caller */
1440         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1441         if (res != ERROR_OK)
1442                 return res;
1443         return ERROR_OK;
1444 }
1445 /*
1446         the purpose of this function is unknown...
1447         capabilites? anyway for swim v6 it returns
1448         0001020600000000
1449 */
1450 __attribute__((unused))
1451 static int stlink_swim_cap(void *handle, uint8_t *cap)
1452 {
1453         struct stlink_usb_handle_s *h = handle;
1454         int res;
1455
1456         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1457         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1458         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1459         h->cmdbuf[h->cmdidx++] = 0x01;
1460         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 8);
1461         if (res != ERROR_OK)
1462                 return res;
1463         memcpy(cap, h->databuf, 8);
1464         return ERROR_OK;
1465 }
1466
1467 /*      debug dongle assert/deassert sreset line */
1468 static int stlink_swim_assert_reset(void *handle, int reset)
1469 {
1470         struct stlink_usb_handle_s *h = handle;
1471         int res;
1472
1473         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1474         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1475         if (!reset)
1476                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1477         else
1478                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1479         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1480         if (res != ERROR_OK)
1481                 return res;
1482         return ERROR_OK;
1483 }
1484
1485 /*
1486         send swim enter seq
1487         1.3ms low then 750Hz then 1.5kHz
1488 */
1489 static int stlink_swim_enter(void *handle)
1490 {
1491         struct stlink_usb_handle_s *h = handle;
1492         int res;
1493
1494         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1495         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1496         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1497         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1498         if (res != ERROR_OK)
1499                 return res;
1500         return ERROR_OK;
1501 }
1502
1503 /*      switch high/low speed swim */
1504 static int stlink_swim_speed(void *handle, int speed)
1505 {
1506         struct stlink_usb_handle_s *h = handle;
1507         int res;
1508
1509         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1510         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1511         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1512         if (speed)
1513                 h->cmdbuf[h->cmdidx++] = 1;
1514         else
1515                 h->cmdbuf[h->cmdidx++] = 0;
1516         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1517         if (res != ERROR_OK)
1518                 return res;
1519         return ERROR_OK;
1520 }
1521
1522 /*
1523         initiate srst from swim.
1524         nrst is pulled low for 50us.
1525 */
1526 static int stlink_swim_generate_rst(void *handle)
1527 {
1528         struct stlink_usb_handle_s *h = handle;
1529         int res;
1530
1531         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1532         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1533         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1534         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1535         if (res != ERROR_OK)
1536                 return res;
1537         return ERROR_OK;
1538 }
1539
1540 /*
1541         send resyncronize sequence
1542         swim is pulled low for 16us
1543         reply is 64 clks low
1544 */
1545 static int stlink_swim_resync(void *handle)
1546 {
1547         struct stlink_usb_handle_s *h = handle;
1548         int res;
1549
1550         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1551         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1552         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1553         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1554         if (res != ERROR_OK)
1555                 return res;
1556         return ERROR_OK;
1557 }
1558
1559 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1560 {
1561         struct stlink_usb_handle_s *h = handle;
1562         int res;
1563         unsigned int i;
1564         unsigned int datalen = 0;
1565         int cmdsize = STLINK_CMD_SIZE_V2;
1566
1567         if (len > STLINK_DATA_SIZE)
1568                 return ERROR_FAIL;
1569
1570         if (h->version.stlink == 1)
1571                 cmdsize = STLINK_SG_SIZE;
1572
1573         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1574         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1575         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1576         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1577         h->cmdidx += 2;
1578         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1579         h->cmdidx += 4;
1580         for (i = 0; i < len; i++) {
1581                 if (h->cmdidx == cmdsize)
1582                         h->databuf[datalen++] = *(data++);
1583                 else
1584                         h->cmdbuf[h->cmdidx++] = *(data++);
1585         }
1586         if (h->version.stlink == 1)
1587                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1588
1589         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1590         if (res != ERROR_OK)
1591                 return res;
1592         return ERROR_OK;
1593 }
1594
1595 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1596 {
1597         struct stlink_usb_handle_s *h = handle;
1598         int res;
1599
1600         if (len > STLINK_DATA_SIZE)
1601                 return ERROR_FAIL;
1602
1603         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1604         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1605         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1606         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1607         h->cmdidx += 2;
1608         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1609         h->cmdidx += 4;
1610         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1611         if (res != ERROR_OK)
1612                 return res;
1613
1614         stlink_usb_init_buffer(handle, h->rx_ep, len);
1615         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1616         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1617         res = stlink_usb_xfer_noerrcheck(handle, data, len);
1618         if (res != ERROR_OK)
1619                 return res;
1620
1621         return ERROR_OK;
1622 }
1623
1624 /** */
1625 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1626 {
1627         int res, offset;
1628         struct stlink_usb_handle_s *h = handle;
1629
1630         assert(handle != NULL);
1631
1632         /* there is no swim read core id cmd */
1633         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1634                 *idcode = 0;
1635                 return ERROR_OK;
1636         }
1637
1638         stlink_usb_init_buffer(handle, h->rx_ep, 12);
1639
1640         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1641         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1642                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1643
1644                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
1645                 offset = 0;
1646         } else {
1647                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_IDCODES;
1648
1649                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 12);
1650                 offset = 4;
1651         }
1652
1653         if (res != ERROR_OK)
1654                 return res;
1655
1656         *idcode = le_to_h_u32(h->databuf + offset);
1657
1658         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1659
1660         return ERROR_OK;
1661 }
1662
1663 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1664 {
1665         struct stlink_usb_handle_s *h = handle;
1666         int res;
1667
1668         assert(handle != NULL);
1669
1670         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1671
1672         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1673         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1674         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1675         h->cmdidx += 4;
1676
1677         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1678         if (res != ERROR_OK)
1679                 return res;
1680
1681         *val = le_to_h_u32(h->databuf + 4);
1682         return ERROR_OK;
1683 }
1684
1685 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
1686 {
1687         struct stlink_usb_handle_s *h = handle;
1688
1689         assert(handle != NULL);
1690
1691         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1692
1693         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1694         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1695                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
1696         else
1697                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
1698         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1699         h->cmdidx += 4;
1700         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1701         h->cmdidx += 4;
1702
1703         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1704 }
1705
1706 /** */
1707 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
1708 {
1709         struct stlink_usb_handle_s *h = handle;
1710
1711         assert(handle != NULL);
1712
1713         if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
1714                 int res;
1715
1716                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1717
1718                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1719                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1720
1721                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1722                 if (res != ERROR_OK)
1723                         return res;
1724
1725                 size_t bytes_avail = le_to_h_u16(h->databuf);
1726                 *size = bytes_avail < *size ? bytes_avail : *size - 1;
1727
1728                 if (*size > 0) {
1729                         res = stlink_usb_read_trace(handle, buf, *size);
1730                         if (res != ERROR_OK)
1731                                 return res;
1732                         return ERROR_OK;
1733                 }
1734         }
1735         *size = 0;
1736         return ERROR_OK;
1737 }
1738
1739 static enum target_state stlink_usb_v2_get_status(void *handle)
1740 {
1741         int result;
1742         uint32_t status;
1743
1744         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
1745         if  (result != ERROR_OK)
1746                 return TARGET_UNKNOWN;
1747
1748         if (status & S_HALT)
1749                 return TARGET_HALTED;
1750         else if (status & S_RESET_ST)
1751                 return TARGET_RESET;
1752
1753         return TARGET_RUNNING;
1754 }
1755
1756 /** */
1757 static enum target_state stlink_usb_state(void *handle)
1758 {
1759         int res;
1760         struct stlink_usb_handle_s *h = handle;
1761
1762         assert(handle != NULL);
1763
1764         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
1765                 res = stlink_usb_mode_enter(handle, h->st_mode);
1766                 if (res != ERROR_OK)
1767                         return TARGET_UNKNOWN;
1768
1769                 res = stlink_swim_resync(handle);
1770                 if (res != ERROR_OK)
1771                         return TARGET_UNKNOWN;
1772
1773                 return ERROR_OK;
1774         }
1775
1776         if (h->reconnect_pending) {
1777                 LOG_INFO("Previous state query failed, trying to reconnect");
1778                 res = stlink_usb_mode_enter(handle, h->st_mode);
1779                 if (res != ERROR_OK)
1780                         return TARGET_UNKNOWN;
1781
1782                 h->reconnect_pending = false;
1783         }
1784
1785         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1786                 res = stlink_usb_v2_get_status(handle);
1787                 if (res == TARGET_UNKNOWN)
1788                         h->reconnect_pending = true;
1789                 return res;
1790         }
1791
1792         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1793
1794         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1795         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
1796
1797         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 2);
1798
1799         if (res != ERROR_OK)
1800                 return TARGET_UNKNOWN;
1801
1802         if (h->databuf[0] == STLINK_CORE_RUNNING)
1803                 return TARGET_RUNNING;
1804         if (h->databuf[0] == STLINK_CORE_HALTED)
1805                 return TARGET_HALTED;
1806
1807         h->reconnect_pending = true;
1808
1809         return TARGET_UNKNOWN;
1810 }
1811
1812 static int stlink_usb_assert_srst(void *handle, int srst)
1813 {
1814         struct stlink_usb_handle_s *h = handle;
1815
1816         assert(handle != NULL);
1817
1818         if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
1819                 return stlink_swim_assert_reset(handle, srst);
1820
1821         if (h->version.stlink == 1)
1822                 return ERROR_COMMAND_NOTFOUND;
1823
1824         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1825
1826         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1827         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
1828         h->cmdbuf[h->cmdidx++] = srst;
1829
1830         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1831 }
1832
1833 /** */
1834 static void stlink_usb_trace_disable(void *handle)
1835 {
1836         int res = ERROR_OK;
1837         struct stlink_usb_handle_s *h = handle;
1838
1839         assert(handle != NULL);
1840
1841         assert(h->version.flags & STLINK_F_HAS_TRACE);
1842
1843         LOG_DEBUG("Tracing: disable");
1844
1845         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1846         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1847         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1848         res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
1849
1850         if (res == ERROR_OK)
1851                 h->trace.enabled = false;
1852 }
1853
1854
1855 /** */
1856 static int stlink_usb_trace_enable(void *handle)
1857 {
1858         int res;
1859         struct stlink_usb_handle_s *h = handle;
1860
1861         assert(handle != NULL);
1862
1863         if (h->version.flags & STLINK_F_HAS_TRACE) {
1864                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1865
1866                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1867                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1868                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1869                 h->cmdidx += 2;
1870                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
1871                 h->cmdidx += 4;
1872
1873                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 2);
1874
1875                 if (res == ERROR_OK)  {
1876                         h->trace.enabled = true;
1877                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
1878                 }
1879         } else {
1880                 LOG_ERROR("Tracing is not supported by this version.");
1881                 res = ERROR_FAIL;
1882         }
1883
1884         return res;
1885 }
1886
1887 /** */
1888 static int stlink_usb_reset(void *handle)
1889 {
1890         struct stlink_usb_handle_s *h = handle;
1891         int retval;
1892
1893         assert(handle != NULL);
1894
1895         if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
1896                 return stlink_swim_generate_rst(handle);
1897
1898         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1899
1900         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1901
1902         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1903                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
1904         else
1905                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
1906
1907         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
1908         if (retval != ERROR_OK)
1909                 return retval;
1910
1911         if (h->trace.enabled) {
1912                 stlink_usb_trace_disable(h);
1913                 return stlink_usb_trace_enable(h);
1914         }
1915
1916         return ERROR_OK;
1917 }
1918
1919 /** */
1920 static int stlink_usb_run(void *handle)
1921 {
1922         int res;
1923         struct stlink_usb_handle_s *h = handle;
1924
1925         assert(handle != NULL);
1926
1927         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1928                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1929
1930                 return res;
1931         }
1932
1933         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1934
1935         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1936         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1937
1938         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1939 }
1940
1941 /** */
1942 static int stlink_usb_halt(void *handle)
1943 {
1944         int res;
1945         struct stlink_usb_handle_s *h = handle;
1946
1947         assert(handle != NULL);
1948
1949         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1950                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1951
1952                 return res;
1953         }
1954
1955         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1956
1957         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1958         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1959
1960         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1961 }
1962
1963 /** */
1964 static int stlink_usb_step(void *handle)
1965 {
1966         struct stlink_usb_handle_s *h = handle;
1967
1968         assert(handle != NULL);
1969
1970         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1971                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1972                  * that the Cortex-M3 currently does. */
1973                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1974                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1975                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1976         }
1977
1978         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1979
1980         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1981         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1982
1983         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1984 }
1985
1986 /** */
1987 static int stlink_usb_read_regs(void *handle)
1988 {
1989         int res;
1990         struct stlink_usb_handle_s *h = handle;
1991
1992         assert(handle != NULL);
1993
1994         stlink_usb_init_buffer(handle, h->rx_ep, 88);
1995
1996         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1997         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1998
1999                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
2000                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 84);
2001                 /* regs data from offset 0 */
2002         } else {
2003                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
2004                 res = stlink_usb_xfer_errcheck(handle, h->databuf, 88);
2005                 /* status at offset 0, regs data from offset 4 */
2006         }
2007
2008         return res;
2009 }
2010
2011 /** */
2012 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
2013 {
2014         int res;
2015         struct stlink_usb_handle_s *h = handle;
2016
2017         assert(handle != NULL);
2018
2019         stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
2020
2021         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2022         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2023                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
2024         else
2025                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
2026         h->cmdbuf[h->cmdidx++] = num;
2027
2028         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
2029                 res = stlink_usb_xfer_noerrcheck(handle, h->databuf, 4);
2030                 if (res != ERROR_OK)
2031                         return res;
2032                 *val = le_to_h_u32(h->databuf);
2033                 return ERROR_OK;
2034         } else {
2035                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
2036                 if (res != ERROR_OK)
2037                         return res;
2038                 *val = le_to_h_u32(h->databuf + 4);
2039                 return ERROR_OK;
2040         }
2041 }
2042
2043 /** */
2044 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
2045 {
2046         struct stlink_usb_handle_s *h = handle;
2047
2048         assert(handle != NULL);
2049
2050         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2051
2052         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2053         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2054                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
2055         else
2056                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
2057         h->cmdbuf[h->cmdidx++] = num;
2058         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
2059         h->cmdidx += 4;
2060
2061         return stlink_cmd_allow_retry(handle, h->databuf, 2);
2062 }
2063
2064 static int stlink_usb_get_rw_status(void *handle)
2065 {
2066         struct stlink_usb_handle_s *h = handle;
2067
2068         assert(handle != NULL);
2069
2070         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2071                 return ERROR_OK;
2072
2073         stlink_usb_init_buffer(handle, h->rx_ep, 2);
2074
2075         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2076         if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
2077                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
2078                 return stlink_usb_xfer_errcheck(handle, h->databuf, 12);
2079         } else {
2080                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
2081                 return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
2082         }
2083 }
2084
2085 /** */
2086 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
2087                           uint8_t *buffer)
2088 {
2089         int res;
2090         uint16_t read_len = len;
2091         struct stlink_usb_handle_s *h = handle;
2092
2093         assert(handle != NULL);
2094
2095         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2096         if (len > stlink_usb_block(h)) {
2097                 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2098                 return ERROR_FAIL;
2099         }
2100
2101         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2102
2103         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2104         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2105         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2106         h->cmdidx += 4;
2107         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2108         h->cmdidx += 2;
2109
2110         /* we need to fix read length for single bytes */
2111         if (read_len == 1)
2112                 read_len++;
2113
2114         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, read_len);
2115
2116         if (res != ERROR_OK)
2117                 return res;
2118
2119         memcpy(buffer, h->databuf, len);
2120
2121         return stlink_usb_get_rw_status(handle);
2122 }
2123
2124 /** */
2125 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
2126                            const uint8_t *buffer)
2127 {
2128         int res;
2129         struct stlink_usb_handle_s *h = handle;
2130
2131         assert(handle != NULL);
2132
2133         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2134         if (len > stlink_usb_block(h)) {
2135                 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2136                 return ERROR_FAIL;
2137         }
2138
2139         stlink_usb_init_buffer(handle, h->tx_ep, len);
2140
2141         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2142         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2143         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2144         h->cmdidx += 4;
2145         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2146         h->cmdidx += 2;
2147
2148         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2149
2150         if (res != ERROR_OK)
2151                 return res;
2152
2153         return stlink_usb_get_rw_status(handle);
2154 }
2155
2156 /** */
2157 static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
2158                           uint8_t *buffer)
2159 {
2160         int res;
2161         struct stlink_usb_handle_s *h = handle;
2162
2163         assert(handle != NULL);
2164
2165         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2166                 return ERROR_COMMAND_NOTFOUND;
2167
2168         /* data must be a multiple of 2 and half-word aligned */
2169         if (len % 2 || addr % 2) {
2170                 LOG_DEBUG("Invalid data alignment");
2171                 return ERROR_TARGET_UNALIGNED_ACCESS;
2172         }
2173
2174         stlink_usb_init_buffer(handle, h->rx_ep, len);
2175
2176         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2177         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2178         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2179         h->cmdidx += 4;
2180         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2181         h->cmdidx += 2;
2182
2183         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2184
2185         if (res != ERROR_OK)
2186                 return res;
2187
2188         memcpy(buffer, h->databuf, len);
2189
2190         return stlink_usb_get_rw_status(handle);
2191 }
2192
2193 /** */
2194 static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
2195                            const uint8_t *buffer)
2196 {
2197         int res;
2198         struct stlink_usb_handle_s *h = handle;
2199
2200         assert(handle != NULL);
2201
2202         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2203                 return ERROR_COMMAND_NOTFOUND;
2204
2205         /* data must be a multiple of 2 and half-word aligned */
2206         if (len % 2 || addr % 2) {
2207                 LOG_DEBUG("Invalid data alignment");
2208                 return ERROR_TARGET_UNALIGNED_ACCESS;
2209         }
2210
2211         stlink_usb_init_buffer(handle, h->tx_ep, len);
2212
2213         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2214         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2215         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2216         h->cmdidx += 4;
2217         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2218         h->cmdidx += 2;
2219
2220         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2221
2222         if (res != ERROR_OK)
2223                 return res;
2224
2225         return stlink_usb_get_rw_status(handle);
2226 }
2227
2228 /** */
2229 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
2230                           uint8_t *buffer)
2231 {
2232         int res;
2233         struct stlink_usb_handle_s *h = handle;
2234
2235         assert(handle != NULL);
2236
2237         /* data must be a multiple of 4 and word aligned */
2238         if (len % 4 || addr % 4) {
2239                 LOG_DEBUG("Invalid data alignment");
2240                 return ERROR_TARGET_UNALIGNED_ACCESS;
2241         }
2242
2243         stlink_usb_init_buffer(handle, h->rx_ep, len);
2244
2245         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2246         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2247         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2248         h->cmdidx += 4;
2249         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2250         h->cmdidx += 2;
2251
2252         res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
2253
2254         if (res != ERROR_OK)
2255                 return res;
2256
2257         memcpy(buffer, h->databuf, len);
2258
2259         return stlink_usb_get_rw_status(handle);
2260 }
2261
2262 /** */
2263 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
2264                            const uint8_t *buffer)
2265 {
2266         int res;
2267         struct stlink_usb_handle_s *h = handle;
2268
2269         assert(handle != NULL);
2270
2271         /* data must be a multiple of 4 and word aligned */
2272         if (len % 4 || addr % 4) {
2273                 LOG_DEBUG("Invalid data alignment");
2274                 return ERROR_TARGET_UNALIGNED_ACCESS;
2275         }
2276
2277         stlink_usb_init_buffer(handle, h->tx_ep, len);
2278
2279         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2280         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2281         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2282         h->cmdidx += 4;
2283         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2284         h->cmdidx += 2;
2285
2286         res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
2287
2288         if (res != ERROR_OK)
2289                 return res;
2290
2291         return stlink_usb_get_rw_status(handle);
2292 }
2293
2294 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2295 {
2296         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2297         if (max_tar_block == 0)
2298                 max_tar_block = 4;
2299         return max_tar_block;
2300 }
2301
2302 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2303                 uint32_t count, uint8_t *buffer)
2304 {
2305         int retval = ERROR_OK;
2306         uint32_t bytes_remaining;
2307         int retries = 0;
2308         struct stlink_usb_handle_s *h = handle;
2309
2310         /* calculate byte count */
2311         count *= size;
2312
2313         /* switch to 8 bit if stlink does not support 16 bit memory read */
2314         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2315                 size = 1;
2316
2317         while (count) {
2318
2319                 bytes_remaining = (size != 1) ?
2320                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2321
2322                 if (count < bytes_remaining)
2323                         bytes_remaining = count;
2324
2325                 /*
2326                  * all stlink support 8/32bit memory read/writes and only from
2327                  * stlink V2J26 there is support for 16 bit memory read/write.
2328                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2329                  * as 8bit access.
2330                  */
2331                 if (size != 1) {
2332
2333                         /* When in jtag mode the stlink uses the auto-increment functionality.
2334                          * However it expects us to pass the data correctly, this includes
2335                          * alignment and any page boundaries. We already do this as part of the
2336                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2337                          * needs implementing manually.
2338                          * currently this only affects jtag mode, according to ST they do single
2339                          * access in SWD mode - but this may change and so we do it for both modes */
2340
2341                         /* we first need to check for any unaligned bytes */
2342                         if (addr & (size - 1)) {
2343
2344                                 uint32_t head_bytes = size - (addr & (size - 1));
2345                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
2346                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2347                                         usleep((1<<retries++) * 1000);
2348                                         continue;
2349                                 }
2350                                 if (retval != ERROR_OK)
2351                                         return retval;
2352                                 buffer += head_bytes;
2353                                 addr += head_bytes;
2354                                 count -= head_bytes;
2355                                 bytes_remaining -= head_bytes;
2356                         }
2357
2358                         if (bytes_remaining & (size - 1))
2359                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
2360                         else if (size == 2)
2361                                 retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
2362                         else
2363                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
2364                 } else
2365                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
2366
2367                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2368                         usleep((1<<retries++) * 1000);
2369                         continue;
2370                 }
2371                 if (retval != ERROR_OK)
2372                         return retval;
2373
2374                 buffer += bytes_remaining;
2375                 addr += bytes_remaining;
2376                 count -= bytes_remaining;
2377         }
2378
2379         return retval;
2380 }
2381
2382 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2383                 uint32_t count, const uint8_t *buffer)
2384 {
2385         int retval = ERROR_OK;
2386         uint32_t bytes_remaining;
2387         int retries = 0;
2388         struct stlink_usb_handle_s *h = handle;
2389
2390         /* calculate byte count */
2391         count *= size;
2392
2393         /* switch to 8 bit if stlink does not support 16 bit memory read */
2394         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2395                 size = 1;
2396
2397         while (count) {
2398
2399                 bytes_remaining = (size != 1) ?
2400                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2401
2402                 if (count < bytes_remaining)
2403                         bytes_remaining = count;
2404
2405                 /*
2406                  * all stlink support 8/32bit memory read/writes and only from
2407                  * stlink V2J26 there is support for 16 bit memory read/write.
2408                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2409                  * as 8bit access.
2410                  */
2411                 if (size != 1) {
2412
2413                         /* When in jtag mode the stlink uses the auto-increment functionality.
2414                          * However it expects us to pass the data correctly, this includes
2415                          * alignment and any page boundaries. We already do this as part of the
2416                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2417                          * needs implementing manually.
2418                          * currently this only affects jtag mode, according to ST they do single
2419                          * access in SWD mode - but this may change and so we do it for both modes */
2420
2421                         /* we first need to check for any unaligned bytes */
2422                         if (addr & (size - 1)) {
2423
2424                                 uint32_t head_bytes = size - (addr & (size - 1));
2425                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
2426                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2427                                         usleep((1<<retries++) * 1000);
2428                                         continue;
2429                                 }
2430                                 if (retval != ERROR_OK)
2431                                         return retval;
2432                                 buffer += head_bytes;
2433                                 addr += head_bytes;
2434                                 count -= head_bytes;
2435                                 bytes_remaining -= head_bytes;
2436                         }
2437
2438                         if (bytes_remaining & (size - 1))
2439                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
2440                         else if (size == 2)
2441                                 retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
2442                         else
2443                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
2444
2445                 } else
2446                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
2447                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2448                         usleep((1<<retries++) * 1000);
2449                         continue;
2450                 }
2451                 if (retval != ERROR_OK)
2452                         return retval;
2453
2454                 buffer += bytes_remaining;
2455                 addr += bytes_remaining;
2456                 count -= bytes_remaining;
2457         }
2458
2459         return retval;
2460 }
2461
2462 /** */
2463 static int stlink_usb_override_target(const char *targetname)
2464 {
2465         return !strcmp(targetname, "cortex_m");
2466 }
2467
2468 static int stlink_speed_swim(void *handle, int khz, bool query)
2469 {
2470         int retval;
2471
2472         /*
2473                         we only have low and high speed...
2474                         before changing speed the SWIM_CSR HS bit
2475                         must be updated
2476          */
2477         if (!query) {
2478                 retval = stlink_swim_speed(handle, (khz < SWIM_FREQ_HIGH) ? 0 : 1);
2479                 if (retval != ERROR_OK)
2480                         LOG_ERROR("Unable to set adapter speed");
2481         }
2482
2483         return (khz < SWIM_FREQ_HIGH) ? SWIM_FREQ_LOW : SWIM_FREQ_HIGH;
2484 }
2485
2486 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
2487 {
2488         unsigned int i;
2489         int speed_index = -1;
2490         int speed_diff = INT_MAX;
2491         int last_valid_speed = -1;
2492         bool match = true;
2493
2494         for (i = 0; i < map_size; i++) {
2495                 if (!map[i].speed)
2496                         continue;
2497                 last_valid_speed = i;
2498                 if (khz == map[i].speed) {
2499                         speed_index = i;
2500                         break;
2501                 } else {
2502                         int current_diff = khz - map[i].speed;
2503                         /* get abs value for comparison */
2504                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
2505                         if ((current_diff < speed_diff) && khz >= map[i].speed) {
2506                                 speed_diff = current_diff;
2507                                 speed_index = i;
2508                         }
2509                 }
2510         }
2511
2512         if (speed_index == -1) {
2513                 /* this will only be here if we cannot match the slow speed.
2514                  * use the slowest speed we support.*/
2515                 speed_index = last_valid_speed;
2516                 match = false;
2517         } else if (i == map_size)
2518                 match = false;
2519
2520         if (!match && query) {
2521                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz",
2522                                 khz, map[speed_index].speed);
2523         }
2524
2525         return speed_index;
2526 }
2527
2528 static int stlink_speed_swd(void *handle, int khz, bool query)
2529 {
2530         int speed_index;
2531         struct stlink_usb_handle_s *h = handle;
2532
2533         /* old firmware cannot change it */
2534         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
2535                 return khz;
2536
2537         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
2538                 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
2539
2540         if (!query) {
2541                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
2542                 if (result != ERROR_OK) {
2543                         LOG_ERROR("Unable to set adapter speed");
2544                         return khz;
2545                 }
2546         }
2547
2548         return stlink_khz_to_speed_map_swd[speed_index].speed;
2549 }
2550
2551 static int stlink_speed_jtag(void *handle, int khz, bool query)
2552 {
2553         int speed_index;
2554         struct stlink_usb_handle_s *h = handle;
2555
2556         /* old firmware cannot change it */
2557         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
2558                 return khz;
2559
2560         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
2561                 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
2562
2563         if (!query) {
2564                 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
2565                 if (result != ERROR_OK) {
2566                         LOG_ERROR("Unable to set adapter speed");
2567                         return khz;
2568                 }
2569         }
2570
2571         return stlink_khz_to_speed_map_jtag[speed_index].speed;
2572 }
2573
2574 void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
2575 {
2576         unsigned int i;
2577
2578         LOG_DEBUG("Supported clock speeds are:");
2579         for (i = 0; i < map_size; i++)
2580                 if (map[i].speed)
2581                         LOG_DEBUG("%d kHz", map[i].speed);
2582 }
2583
2584 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
2585 {
2586         struct stlink_usb_handle_s *h = handle;
2587         int i;
2588
2589         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2590                 LOG_ERROR("Unknown command");
2591                 return 0;
2592         }
2593
2594         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2595
2596         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2597         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
2598         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2599
2600         int res = stlink_usb_xfer_errcheck(handle, h->databuf, 52);
2601
2602         int size = h->databuf[8];
2603
2604         if (size > STLINK_V3_MAX_FREQ_NB)
2605                 size = STLINK_V3_MAX_FREQ_NB;
2606
2607         for (i = 0; i < size; i++) {
2608                 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
2609                 map[i].speed_divisor = i;
2610         }
2611
2612         /* set to zero all the next entries */
2613         for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
2614                 map[i].speed = 0;
2615
2616         return res;
2617 }
2618
2619 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
2620 {
2621         struct stlink_usb_handle_s *h = handle;
2622
2623         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2624                 LOG_ERROR("Unknown command");
2625                 return 0;
2626         }
2627
2628         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2629
2630         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2631         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
2632         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2633         h->cmdbuf[h->cmdidx++] = 0;
2634
2635         h_u32_to_le(&h->cmdbuf[4], frequency);
2636
2637         return stlink_usb_xfer_errcheck(handle, h->databuf, 8);
2638 }
2639
2640 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
2641 {
2642         struct stlink_usb_handle_s *h = handle;
2643         int speed_index;
2644         struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2645
2646         stlink_get_com_freq(h, is_jtag, map);
2647
2648         speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
2649
2650         if (!query) {
2651                 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
2652                 if (result != ERROR_OK) {
2653                         LOG_ERROR("Unable to set adapter speed");
2654                         return khz;
2655                 }
2656         }
2657         return map[speed_index].speed;
2658 }
2659
2660 static int stlink_speed(void *handle, int khz, bool query)
2661 {
2662         struct stlink_usb_handle_s *h = handle;
2663
2664         if (!handle)
2665                 return khz;
2666
2667         switch (h->st_mode) {
2668         case STLINK_MODE_DEBUG_SWIM:
2669                 return stlink_speed_swim(handle, khz, query);
2670         case STLINK_MODE_DEBUG_SWD:
2671                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2672                         return stlink_speed_v3(handle, false, khz, query);
2673                 else
2674                         return stlink_speed_swd(handle, khz, query);
2675                 break;
2676         case STLINK_MODE_DEBUG_JTAG:
2677                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2678                         return stlink_speed_v3(handle, true, khz, query);
2679                 else
2680                         return stlink_speed_jtag(handle, khz, query);
2681                 break;
2682         default:
2683                 break;
2684         }
2685
2686         return khz;
2687 }
2688
2689 /** */
2690 static int stlink_usb_close(void *handle)
2691 {
2692         int res;
2693         uint8_t mode;
2694         enum stlink_mode emode;
2695         struct stlink_usb_handle_s *h = handle;
2696
2697         if (h && h->fd)
2698                 res = stlink_usb_current_mode(handle, &mode);
2699         else
2700                 res = ERROR_FAIL;
2701         /* do not exit if return code != ERROR_OK,
2702            it prevents us from closing jtag_libusb */
2703
2704         if (res == ERROR_OK) {
2705                 /* try to exit current mode */
2706                 switch (mode) {
2707                         case STLINK_DEV_DFU_MODE:
2708                                 emode = STLINK_MODE_DFU;
2709                                 break;
2710                         case STLINK_DEV_DEBUG_MODE:
2711                                 emode = STLINK_MODE_DEBUG_SWD;
2712                                 break;
2713                         case STLINK_DEV_SWIM_MODE:
2714                                 emode = STLINK_MODE_DEBUG_SWIM;
2715                                 break;
2716                         case STLINK_DEV_BOOTLOADER_MODE:
2717                         case STLINK_DEV_MASS_MODE:
2718                         default:
2719                                 emode = STLINK_MODE_UNKNOWN;
2720                                 break;
2721                 }
2722
2723                 if (emode != STLINK_MODE_UNKNOWN)
2724                         stlink_usb_mode_leave(handle, emode);
2725                         /* do not check return code, it prevent
2726                         us from closing jtag_libusb */
2727         }
2728
2729         if (h && h->fd)
2730                 jtag_libusb_close(h->fd);
2731
2732         free(h);
2733
2734         return ERROR_OK;
2735 }
2736
2737 /* Compute ST-Link serial number from the device descriptor
2738  * this function will help to work-around a bug in old ST-Link/V2 DFU
2739  * the buggy DFU returns an incorrect serial in the USB descriptor
2740  * example for the following serial "57FF72067265575742132067"
2741  *  - the correct descriptor serial is:
2742  *    0x32, 0x03, 0x35, 0x00, 0x37, 0x00, 0x46, 0x00, 0x46, 0x00, 0x37, 0x00, 0x32, 0x00 ...
2743  *    this contains the length (0x32 = 50), the type (0x3 = DT_STRING) and the serial in unicode format
2744  *    the serial part is: 0x0035, 0x0037, 0x0046, 0x0046, 0x0037, 0x0032 ... >>  57FF72 ...
2745  *    this format could be read correctly by 'libusb_get_string_descriptor_ascii'
2746  *    so this case is managed by libusb_helper::string_descriptor_equal
2747  *  - the buggy DFU is not doing any unicode conversion and returns a raw serial data in the descriptor
2748  *    0x1a, 0x03, 0x57, 0x00, 0xFF, 0x00, 0x72, 0x00 ...
2749  *            >>    57          FF          72       ...
2750  *    based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
2751  *    and then we have just to convert the raw data into printable characters using sprintf
2752  */
2753 char *stlink_usb_get_alternate_serial(libusb_device_handle *device,
2754                 struct libusb_device_descriptor *dev_desc)
2755 {
2756         int usb_retval;
2757         unsigned char desc_serial[(STLINK_SERIAL_LEN + 1) * 2];
2758
2759         if (dev_desc->iSerialNumber == 0)
2760                 return NULL;
2761
2762         /* get the LANGID from String Descriptor Zero */
2763         usb_retval = libusb_get_string_descriptor(device, 0, 0, desc_serial,
2764                         sizeof(desc_serial));
2765
2766         if (usb_retval < LIBUSB_SUCCESS) {
2767                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
2768                                 libusb_error_name(usb_retval), usb_retval);
2769                 return NULL;
2770         } else if (usb_retval < 4) {
2771                 /* the size should be least 4 bytes to contain a minimum of 1 supported LANGID */
2772                 LOG_ERROR("could not get the LANGID");
2773                 return NULL;
2774         }
2775
2776         uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
2777
2778         /* get the serial */
2779         usb_retval = libusb_get_string_descriptor(device, dev_desc->iSerialNumber,
2780                         langid, desc_serial, sizeof(desc_serial));
2781
2782         unsigned char len = desc_serial[0];
2783
2784         if (usb_retval < LIBUSB_SUCCESS) {
2785                 LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
2786                                 libusb_error_name(usb_retval), usb_retval);
2787                 return NULL;
2788         } else if (desc_serial[1] != LIBUSB_DT_STRING || len > usb_retval) {
2789                 LOG_ERROR("invalid string in ST-LINK USB serial descriptor");
2790                 return NULL;
2791         }
2792
2793         if (len == ((STLINK_SERIAL_LEN + 1) * 2)) {
2794                 /* good ST-Link adapter, this case is managed by
2795                  * libusb::libusb_get_string_descriptor_ascii */
2796                 return NULL;
2797         } else if (len != ((STLINK_SERIAL_LEN / 2 + 1) * 2)) {
2798                 LOG_ERROR("unexpected serial length (%d) in descriptor", len);
2799                 return NULL;
2800         }
2801
2802         /* else (len == 26) => buggy ST-Link */
2803
2804         char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
2805         if (alternate_serial == NULL)
2806                 return NULL;
2807
2808         for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
2809                 sprintf(alternate_serial + i, "%02X", desc_serial[i + 2]);
2810
2811         alternate_serial[STLINK_SERIAL_LEN] = '\0';
2812
2813         return alternate_serial;
2814 }
2815
2816 /** */
2817 static int stlink_usb_open(struct hl_interface_param_s *param, enum stlink_mode mode, void **fd)
2818 {
2819         int err, retry_count = 1;
2820         struct stlink_usb_handle_s *h;
2821
2822         LOG_DEBUG("stlink_usb_open");
2823
2824         h = calloc(1, sizeof(struct stlink_usb_handle_s));
2825
2826         if (h == 0) {
2827                 LOG_DEBUG("malloc failed");
2828                 return ERROR_FAIL;
2829         }
2830
2831         h->st_mode = mode;
2832
2833         for (unsigned i = 0; param->vid[i]; i++) {
2834                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
2835                           h->st_mode, param->vid[i], param->pid[i],
2836                           param->serial ? param->serial : "");
2837         }
2838
2839         /*
2840           On certain host USB configurations(e.g. MacBook Air)
2841           STLINKv2 dongle seems to have its FW in a funky state if,
2842           after plugging it in, you try to use openocd with it more
2843           then once (by launching and closing openocd). In cases like
2844           that initial attempt to read the FW info via
2845           stlink_usb_version will fail and the device has to be reset
2846           in order to become operational.
2847          */
2848         do {
2849                 if (jtag_libusb_open(param->vid, param->pid, param->serial,
2850                                 &h->fd, stlink_usb_get_alternate_serial) != ERROR_OK) {
2851                         LOG_ERROR("open failed");
2852                         goto error_open;
2853                 }
2854
2855                 jtag_libusb_set_configuration(h->fd, 0);
2856
2857                 if (libusb_claim_interface(h->fd, 0) != ERROR_OK) {
2858                         LOG_DEBUG("claim interface failed");
2859                         goto error_open;
2860                 }
2861
2862                 /* RX EP is common for all versions */
2863                 h->rx_ep = STLINK_RX_EP;
2864
2865                 uint16_t pid;
2866                 if (jtag_libusb_get_pid(libusb_get_device(h->fd), &pid) != ERROR_OK) {
2867                         LOG_DEBUG("libusb_get_pid failed");
2868                         goto error_open;
2869                 }
2870
2871                 /* wrap version for first read */
2872                 switch (pid) {
2873                         case STLINK_V1_PID:
2874                                 h->version.stlink = 1;
2875                                 h->tx_ep = STLINK_TX_EP;
2876                                 break;
2877                         case STLINK_V3_USBLOADER_PID:
2878                         case STLINK_V3E_PID:
2879                         case STLINK_V3S_PID:
2880                         case STLINK_V3_2VCP_PID:
2881                                 h->version.stlink = 3;
2882                                 h->tx_ep = STLINK_V2_1_TX_EP;
2883                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2884                                 break;
2885                         case STLINK_V2_1_PID:
2886                         case STLINK_V2_1_NO_MSD_PID:
2887                                 h->version.stlink = 2;
2888                                 h->tx_ep = STLINK_V2_1_TX_EP;
2889                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2890                                 break;
2891                         default:
2892                         /* fall through - we assume V2 to be the default version*/
2893                         case STLINK_V2_PID:
2894                                 h->version.stlink = 2;
2895                                 h->tx_ep = STLINK_TX_EP;
2896                                 h->trace_ep = STLINK_TRACE_EP;
2897                                 break;
2898                 }
2899
2900                 /* get the device version */
2901                 err = stlink_usb_version(h);
2902
2903                 if (err == ERROR_OK) {
2904                         break;
2905                 } else if (h->version.stlink == 1 ||
2906                            retry_count == 0) {
2907                         LOG_ERROR("read version failed");
2908                         goto error_open;
2909                 } else {
2910                         err = libusb_release_interface(h->fd, 0);
2911                         if (err != ERROR_OK) {
2912                                 LOG_ERROR("release interface failed");
2913                                 goto error_open;
2914                         }
2915
2916                         err = libusb_reset_device(h->fd);
2917                         if (err != ERROR_OK) {
2918                                 LOG_ERROR("reset device failed");
2919                                 goto error_open;
2920                         }
2921
2922                         jtag_libusb_close(h->fd);
2923                         /*
2924                           Give the device one second to settle down and
2925                           reenumerate.
2926                          */
2927                         usleep(1 * 1000 * 1000);
2928                         retry_count--;
2929                 }
2930         } while (1);
2931
2932         /* check if mode is supported */
2933         err = ERROR_OK;
2934
2935         switch (h->st_mode) {
2936                 case STLINK_MODE_DEBUG_SWD:
2937                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2938                                 err = ERROR_FAIL;
2939                         /* fall-through */
2940                 case STLINK_MODE_DEBUG_JTAG:
2941                         if (h->version.jtag == 0)
2942                                 err = ERROR_FAIL;
2943                         break;
2944                 case STLINK_MODE_DEBUG_SWIM:
2945                         if (h->version.swim == 0)
2946                                 err = ERROR_FAIL;
2947                         break;
2948                 default:
2949                         err = ERROR_FAIL;
2950                         break;
2951         }
2952
2953         if (err != ERROR_OK) {
2954                 LOG_ERROR("mode (transport) not supported by device");
2955                 goto error_open;
2956         }
2957
2958         /* initialize the debug hardware */
2959         err = stlink_usb_init_mode(h, param->connect_under_reset, param->initial_interface_speed);
2960
2961         if (err != ERROR_OK) {
2962                 LOG_ERROR("init mode failed (unable to connect to the target)");
2963                 goto error_open;
2964         }
2965
2966         if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
2967                 err = stlink_swim_enter(h);
2968                 if (err != ERROR_OK) {
2969                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
2970                         goto error_open;
2971                 }
2972                 *fd = h;
2973                 h->max_mem_packet = STLINK_DATA_SIZE;
2974                 return ERROR_OK;
2975         }
2976
2977         /* get cpuid, so we can determine the max page size
2978          * start with a safe default */
2979         h->max_mem_packet = (1 << 10);
2980
2981         uint8_t buffer[4];
2982         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
2983         if (err == ERROR_OK) {
2984                 uint32_t cpuid = le_to_h_u32(buffer);
2985                 int i = (cpuid >> 4) & 0xf;
2986                 if (i == 4 || i == 3) {
2987                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
2988                         h->max_mem_packet = (1 << 12);
2989                 }
2990         }
2991
2992         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
2993
2994         *fd = h;
2995
2996         return ERROR_OK;
2997
2998 error_open:
2999         stlink_usb_close(h);
3000
3001         return ERROR_FAIL;
3002 }
3003
3004 static int stlink_usb_hl_open(struct hl_interface_param_s *param, void **fd)
3005 {
3006         return stlink_usb_open(param, stlink_get_mode(param->transport), fd);
3007 }
3008
3009 int stlink_config_trace(void *handle, bool enabled,
3010                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3011                 unsigned int *trace_freq, unsigned int traceclkin_freq,
3012                 uint16_t *prescaler)
3013 {
3014         struct stlink_usb_handle_s *h = handle;
3015         uint16_t presc;
3016
3017         if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
3018                         pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
3019                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
3020                 return ERROR_FAIL;
3021         }
3022
3023         if (!enabled) {
3024                 stlink_usb_trace_disable(h);
3025                 return ERROR_OK;
3026         }
3027
3028         if (*trace_freq > STLINK_TRACE_MAX_HZ) {
3029                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
3030                           STLINK_TRACE_MAX_HZ);
3031                 return ERROR_FAIL;
3032         }
3033
3034         stlink_usb_trace_disable(h);
3035
3036         if (!*trace_freq)
3037                 *trace_freq = STLINK_TRACE_MAX_HZ;
3038
3039         presc = traceclkin_freq / *trace_freq;
3040
3041         if (traceclkin_freq % *trace_freq > 0)
3042                 presc++;
3043
3044         if (presc > TPIU_ACPR_MAX_SWOSCALER) {
3045                 LOG_ERROR("SWO frequency is not suitable. Please choose a different "
3046                         "frequency.");
3047                 return ERROR_FAIL;
3048         }
3049
3050         *prescaler = presc;
3051         h->trace.source_hz = *trace_freq;
3052
3053         return stlink_usb_trace_enable(h);
3054 }
3055
3056 /** */
3057 static int stlink_usb_init_access_port(void *handle, unsigned char ap_num)
3058 {
3059         struct stlink_usb_handle_s *h = handle;
3060
3061         assert(handle != NULL);
3062
3063         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3064                 return ERROR_COMMAND_NOTFOUND;
3065
3066         LOG_DEBUG_IO("init ap_num = %d", ap_num);
3067         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3068         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3069         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_INIT_AP;
3070         h->cmdbuf[h->cmdidx++] = ap_num;
3071
3072         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3073 }
3074
3075 /** */
3076 static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
3077 {
3078         struct stlink_usb_handle_s *h = handle;
3079
3080         assert(handle != NULL);
3081
3082         if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
3083                 return ERROR_COMMAND_NOTFOUND;
3084
3085         LOG_DEBUG_IO("close ap_num = %d", ap_num);
3086         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3087         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3088         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_CLOSE_AP_DBG;
3089         h->cmdbuf[h->cmdidx++] = ap_num;
3090
3091         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3092 }
3093
3094 /** */
3095 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
3096                         unsigned short addr, uint32_t *val)
3097 {
3098         struct stlink_usb_handle_s *h = handle;
3099         int retval;
3100
3101         assert(handle != NULL);
3102
3103         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3104                 return ERROR_COMMAND_NOTFOUND;
3105
3106         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3107         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3108         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READ_DAP_REG;
3109         h_u16_to_le(&h->cmdbuf[2], dap_port);
3110         h_u16_to_le(&h->cmdbuf[4], addr);
3111
3112         retval = stlink_usb_xfer_errcheck(handle, h->databuf, 8);
3113         *val = le_to_h_u32(h->databuf + 4);
3114         LOG_DEBUG_IO("dap_port_read = %d, addr =  0x%x, value = 0x%x", dap_port, addr, *val);
3115         return retval;
3116 }
3117
3118 /** */
3119 static int stlink_write_dap_register(void *handle, unsigned short dap_port,
3120                         unsigned short addr, uint32_t val)
3121 {
3122         struct stlink_usb_handle_s *h = handle;
3123
3124         assert(handle != NULL);
3125
3126         if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
3127                 return ERROR_COMMAND_NOTFOUND;
3128
3129         LOG_DEBUG_IO("dap_write port = %d, addr = 0x%x, value = 0x%x", dap_port, addr, val);
3130         stlink_usb_init_buffer(handle, h->rx_ep, 16);
3131         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
3132         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITE_DAP_REG;
3133         h_u16_to_le(&h->cmdbuf[2], dap_port);
3134         h_u16_to_le(&h->cmdbuf[4], addr);
3135         h_u32_to_le(&h->cmdbuf[6], val);
3136         return stlink_usb_xfer_errcheck(handle, h->databuf, 2);
3137 }
3138
3139 /** */
3140 struct hl_layout_api_s stlink_usb_layout_api = {
3141         /** */
3142         .open = stlink_usb_hl_open,
3143         /** */
3144         .close = stlink_usb_close,
3145         /** */
3146         .idcode = stlink_usb_idcode,
3147         /** */
3148         .state = stlink_usb_state,
3149         /** */
3150         .reset = stlink_usb_reset,
3151         /** */
3152         .assert_srst = stlink_usb_assert_srst,
3153         /** */
3154         .run = stlink_usb_run,
3155         /** */
3156         .halt = stlink_usb_halt,
3157         /** */
3158         .step = stlink_usb_step,
3159         /** */
3160         .read_regs = stlink_usb_read_regs,
3161         /** */
3162         .read_reg = stlink_usb_read_reg,
3163         /** */
3164         .write_reg = stlink_usb_write_reg,
3165         /** */
3166         .read_mem = stlink_usb_read_mem,
3167         /** */
3168         .write_mem = stlink_usb_write_mem,
3169         /** */
3170         .write_debug_reg = stlink_usb_write_debug_reg,
3171         /** */
3172         .override_target = stlink_usb_override_target,
3173         /** */
3174         .speed = stlink_speed,
3175         /** */
3176         .config_trace = stlink_config_trace,
3177         /** */
3178         .poll_trace = stlink_usb_trace_read,
3179 };
3180
3181 /*****************************************************************************
3182  * DAP direct interface
3183  */
3184
3185 static struct stlink_usb_handle_s *stlink_dap_handle;
3186 static struct hl_interface_param_s stlink_dap_param;
3187 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
3188 static int stlink_dap_error = ERROR_OK;
3189
3190 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
3191                 uint32_t *data);
3192
3193 /** */
3194 static int stlink_dap_record_error(int error)
3195 {
3196         if (stlink_dap_error == ERROR_OK)
3197                 stlink_dap_error = error;
3198         return ERROR_OK;
3199 }
3200
3201 /** */
3202 static int stlink_dap_get_and_clear_error(void)
3203 {
3204         int retval = stlink_dap_error;
3205         stlink_dap_error = ERROR_OK;
3206         return retval;
3207 }
3208
3209 /** */
3210 static int stlink_dap_open_ap(unsigned short apsel)
3211 {
3212         int retval;
3213
3214         /* nothing to do on old versions */
3215         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
3216                 return ERROR_OK;
3217
3218         if (apsel > DP_APSEL_MAX)
3219                 return ERROR_FAIL;
3220
3221         if (test_bit(apsel, opened_ap))
3222                 return ERROR_OK;
3223
3224         retval = stlink_usb_init_access_port(stlink_dap_handle, apsel);
3225         if (retval != ERROR_OK)
3226                 return retval;
3227
3228         LOG_DEBUG("AP %d enabled", apsel);
3229         set_bit(apsel, opened_ap);
3230         return ERROR_OK;
3231 }
3232
3233 /** */
3234 static int stlink_dap_closeall_ap(void)
3235 {
3236         int retval, apsel;
3237
3238         /* nothing to do on old versions */
3239         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT))
3240                 return ERROR_OK;
3241
3242         for (apsel = 0; apsel <= DP_APSEL_MAX; apsel++) {
3243                 if (!test_bit(apsel, opened_ap))
3244                         continue;
3245                 retval = stlink_usb_close_access_port(stlink_dap_handle, apsel);
3246                 if (retval != ERROR_OK)
3247                         return retval;
3248                 clear_bit(apsel, opened_ap);
3249         }
3250         return ERROR_OK;
3251 }
3252
3253 /** */
3254 static int stlink_dap_reinit_interface(void)
3255 {
3256         int retval;
3257
3258         /*
3259          * On JTAG only, it should be enough to call stlink_usb_reset(). But on
3260          * some firmware version it does not work as expected, and there is no
3261          * equivalent for SWD.
3262          * At least for now, to reset the interface quit from JTAG/SWD mode then
3263          * select the mode again.
3264          */
3265
3266         if (!stlink_dap_handle->reconnect_pending) {
3267                 stlink_dap_handle->reconnect_pending = true;
3268                 stlink_usb_mode_leave(stlink_dap_handle, stlink_dap_handle->st_mode);
3269         }
3270
3271         retval = stlink_usb_mode_enter(stlink_dap_handle, stlink_dap_handle->st_mode);
3272         if (retval != ERROR_OK)
3273                 return retval;
3274
3275         stlink_dap_handle->reconnect_pending = false;
3276         /* on new FW, calling mode-leave closes all the opened AP; reopen them! */
3277         if (stlink_dap_handle->version.flags & STLINK_F_HAS_AP_INIT)
3278                 for (int apsel = 0; apsel <= DP_APSEL_MAX; apsel++)
3279                         if (test_bit(apsel, opened_ap)) {
3280                                 clear_bit(apsel, opened_ap);
3281                                 stlink_dap_open_ap(apsel);
3282                         }
3283         return ERROR_OK;
3284 }
3285
3286 /** */
3287 static int stlink_dap_op_connect(struct adiv5_dap *dap)
3288 {
3289         uint32_t idcode;
3290         int retval;
3291
3292         LOG_INFO("stlink_dap_op_connect(%sconnect)", dap->do_reconnect ? "re" : "");
3293
3294         /* Check if we should reset srst already when connecting, but not if reconnecting. */
3295         if (!dap->do_reconnect) {
3296                 enum reset_types jtag_reset_config = jtag_get_reset_config();
3297
3298                 if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
3299                         if (jtag_reset_config & RESET_SRST_NO_GATING)
3300                                 adapter_assert_reset();
3301                         else
3302                                 LOG_WARNING("\'srst_nogate\' reset_config option is required");
3303                 }
3304         }
3305
3306         dap->do_reconnect = false;
3307         dap_invalidate_cache(dap);
3308
3309         retval = dap_dp_init(dap);
3310         if (retval != ERROR_OK) {
3311                 dap->do_reconnect = true;
3312                 return retval;
3313         }
3314
3315         retval = stlink_usb_idcode(stlink_dap_handle, &idcode);
3316         if (retval == ERROR_OK)
3317                 LOG_INFO("%s %#8.8" PRIx32,
3318                         (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) ? "JTAG IDCODE" : "SWD DPIDR",
3319                         idcode);
3320         else
3321                 dap->do_reconnect = true;
3322
3323         return retval;
3324 }
3325
3326 /** */
3327 static int stlink_dap_check_reconnect(struct adiv5_dap *dap)
3328 {
3329         int retval;
3330
3331         if (!dap->do_reconnect)
3332                 return ERROR_OK;
3333
3334         retval = stlink_dap_reinit_interface();
3335         if (retval != ERROR_OK)
3336                 return retval;
3337
3338         return stlink_dap_op_connect(dap);
3339 }
3340
3341 /** */
3342 static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
3343 {
3344         /* Ignore the request */
3345         return ERROR_OK;
3346 }
3347
3348 /** */
3349 static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
3350                 uint32_t *data)
3351 {
3352         uint32_t dummy;
3353         int retval;
3354
3355         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
3356                 if (reg & 0x000000F0) {
3357                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
3358                         return ERROR_COMMAND_NOTFOUND;
3359                 }
3360
3361         retval = stlink_dap_check_reconnect(dap);
3362         if (retval != ERROR_OK)
3363                 return retval;
3364
3365         data = data ? : &dummy;
3366         if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ
3367                 && stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) {
3368                 /* Quirk required in JTAG. Read RDBUFF to get the data */
3369                 retval = stlink_read_dap_register(stlink_dap_handle,
3370                                         STLINK_DEBUG_PORT_ACCESS, reg, &dummy);
3371                 if (retval == ERROR_OK)
3372                         retval = stlink_read_dap_register(stlink_dap_handle,
3373                                                 STLINK_DEBUG_PORT_ACCESS, DP_RDBUFF, data);
3374         } else {
3375                 retval = stlink_read_dap_register(stlink_dap_handle,
3376                                         STLINK_DEBUG_PORT_ACCESS, reg, data);
3377         }
3378
3379         return stlink_dap_record_error(retval);
3380 }
3381
3382 /** */
3383 static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
3384                 uint32_t data)
3385 {
3386         int retval;
3387
3388         if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_DPBANKSEL))
3389                 if (reg & 0x000000F0) {
3390                         LOG_ERROR("Banked DP registers not supported in current STLink FW");
3391                         return ERROR_COMMAND_NOTFOUND;
3392                 }
3393
3394         if (reg == DP_SELECT && (data & DP_SELECT_DPBANK) != 0) {
3395                 /* ignored if STLINK_F_HAS_DPBANKSEL, not properly managed otherwise */
3396                 LOG_DEBUG("Ignoring DPBANKSEL while write SELECT");
3397                 data &= ~DP_SELECT_DPBANK;
3398         }
3399
3400         retval = stlink_dap_check_reconnect(dap);
3401         if (retval != ERROR_OK)
3402                 return retval;
3403
3404         /* ST-Link does not like that we set CORUNDETECT */
3405         if (reg == DP_CTRL_STAT)
3406                 data &= ~CORUNDETECT;
3407
3408         retval = stlink_write_dap_register(stlink_dap_handle,
3409                                 STLINK_DEBUG_PORT_ACCESS, reg, data);
3410         return stlink_dap_record_error(retval);
3411 }
3412
3413 /** */
3414 static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
3415                 uint32_t *data)
3416 {
3417         struct adiv5_dap *dap = ap->dap;
3418         uint32_t dummy;
3419         int retval;
3420
3421         retval = stlink_dap_check_reconnect(dap);
3422         if (retval != ERROR_OK)
3423                 return retval;
3424
3425         if (reg != AP_REG_IDR) {
3426                 retval = stlink_dap_open_ap(ap->ap_num);
3427                 if (retval != ERROR_OK)
3428                         return retval;
3429         }
3430         data = data ? : &dummy;
3431         retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
3432                                  data);
3433         dap->stlink_flush_ap_write = false;
3434         return stlink_dap_record_error(retval);
3435 }
3436
3437 /** */
3438 static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
3439                 uint32_t data)
3440 {
3441         struct adiv5_dap *dap = ap->dap;
3442         int retval;
3443
3444         retval = stlink_dap_check_reconnect(dap);
3445         if (retval != ERROR_OK)
3446                 return retval;
3447
3448         retval = stlink_dap_open_ap(ap->ap_num);
3449         if (retval != ERROR_OK)
3450                 return retval;
3451
3452         retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
3453                                 data);
3454         dap->stlink_flush_ap_write = true;
3455         return stlink_dap_record_error(retval);
3456 }
3457
3458 /** */
3459 static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
3460 {
3461         LOG_WARNING("stlink_dap_op_queue_ap_abort()");
3462         return ERROR_OK;
3463 }
3464
3465 /** */
3466 static int stlink_dap_op_run(struct adiv5_dap *dap)
3467 {
3468         uint32_t ctrlstat, pwrmask;
3469         int retval, saved_retval;
3470
3471         /* Here no LOG_DEBUG. This is called continuously! */
3472
3473         /*
3474          * ST-Link returns immediately after a DAP write, without waiting for it
3475          * to complete.
3476          * Run a dummy read to DP_RDBUFF, as suggested in
3477          * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html
3478          */
3479         if (dap->stlink_flush_ap_write) {
3480                 dap->stlink_flush_ap_write = false;
3481                 retval = stlink_dap_op_queue_dp_read(dap, DP_RDBUFF, NULL);
3482                 if (retval != ERROR_OK) {
3483                         dap->do_reconnect = true;
3484                         return retval;
3485                 }
3486         }
3487
3488         saved_retval = stlink_dap_get_and_clear_error();
3489
3490         retval = stlink_dap_op_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
3491         if (retval != ERROR_OK) {
3492                 dap->do_reconnect = true;
3493                 return retval;
3494         }
3495         retval = stlink_dap_get_and_clear_error();
3496         if (retval != ERROR_OK) {
3497                 LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
3498                 dap->do_reconnect = true;
3499                 return retval;
3500         }
3501
3502         if (ctrlstat & SSTICKYERR) {
3503                 if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG)
3504                         retval = stlink_dap_op_queue_dp_write(dap, DP_CTRL_STAT,
3505                                         ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
3506                 else
3507                         retval = stlink_dap_op_queue_dp_write(dap, DP_ABORT, STKERRCLR);
3508                 if (retval != ERROR_OK) {
3509                         dap->do_reconnect = true;
3510                         return retval;
3511                 }
3512                 retval = stlink_dap_get_and_clear_error();
3513                 if (retval != ERROR_OK) {
3514                         dap->do_reconnect = true;
3515                         return retval;
3516                 }
3517         }
3518
3519         /* check for power lost */
3520         pwrmask = dap->dp_ctrl_stat & (CDBGPWRUPREQ | CSYSPWRUPREQ);
3521         if ((ctrlstat & pwrmask) != pwrmask)
3522                 dap->do_reconnect = true;
3523
3524         return saved_retval;
3525 }
3526
3527 /** */
3528 static void stlink_dap_op_quit(struct adiv5_dap *dap)
3529 {
3530         int retval;
3531
3532         retval = stlink_dap_closeall_ap();
3533         if (retval != ERROR_OK)
3534                 LOG_ERROR("Error closing APs");
3535 }
3536
3537 static int stlink_swim_op_srst(void)
3538 {
3539         return stlink_usb_reset(stlink_dap_handle);
3540 }
3541
3542 static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
3543                                                                    uint32_t count, uint8_t *buffer)
3544 {
3545         int retval;
3546         uint32_t bytes_remaining;
3547
3548         LOG_DEBUG_IO("read at 0x%08x len %d*0x%08x", addr, size, count);
3549         count *= size;
3550
3551         while (count) {
3552                 bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
3553                 retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
3554                 if (retval != ERROR_OK)
3555                         return retval;
3556
3557                 buffer += bytes_remaining;
3558                 addr += bytes_remaining;
3559                 count -= bytes_remaining;
3560         }
3561
3562         return ERROR_OK;
3563 }
3564
3565 static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
3566                                                                         uint32_t count, const uint8_t *buffer)
3567 {
3568         int retval;
3569         uint32_t bytes_remaining;
3570
3571         LOG_DEBUG_IO("write at 0x%08x len %d*0x%08x", addr, size, count);
3572         count *= size;
3573
3574         while (count) {
3575                 bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
3576                 retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
3577                 if (retval != ERROR_OK)
3578                         return retval;
3579
3580                 buffer += bytes_remaining;
3581                 addr += bytes_remaining;
3582                 count -= bytes_remaining;
3583         }
3584
3585         return ERROR_OK;
3586 }
3587
3588 static int stlink_swim_op_reconnect(void)
3589 {
3590         return stlink_usb_state(stlink_dap_handle);
3591 }
3592
3593 static int stlink_dap_config_trace(bool enabled,
3594                 enum tpiu_pin_protocol pin_protocol, uint32_t port_size,
3595                 unsigned int *trace_freq, unsigned int traceclkin_freq,
3596                 uint16_t *prescaler)
3597 {
3598         return stlink_config_trace(stlink_dap_handle, enabled, pin_protocol,
3599                                                            port_size, trace_freq, traceclkin_freq,
3600                                                            prescaler);
3601 }
3602
3603 static int stlink_dap_trace_read(uint8_t *buf, size_t *size)
3604 {
3605         return stlink_usb_trace_read(stlink_dap_handle, buf, size);
3606 }
3607
3608 /** */
3609 COMMAND_HANDLER(stlink_dap_serial_command)
3610 {
3611         LOG_DEBUG("stlink_dap_serial_command");
3612
3613         if (CMD_ARGC != 1) {
3614                 LOG_ERROR("Expected exactly one argument for \"st-link serial <serial-number>\".");
3615                 return ERROR_COMMAND_SYNTAX_ERROR;
3616         }
3617
3618         if (stlink_dap_param.serial) {
3619                 LOG_WARNING("Command \"st-link serial\" already used. Replacing previous value");
3620                 free((void *)stlink_dap_param.serial);
3621         }
3622
3623         stlink_dap_param.serial = strdup(CMD_ARGV[0]);
3624         return ERROR_OK;
3625 }
3626
3627 /** */
3628 COMMAND_HANDLER(stlink_dap_vid_pid)
3629 {
3630         unsigned int i, max_usb_ids = HLA_MAX_USB_IDS;
3631
3632         if (CMD_ARGC > max_usb_ids * 2) {
3633                 LOG_WARNING("ignoring extra IDs in vid_pid "
3634                         "(maximum is %d pairs)", max_usb_ids);
3635                 CMD_ARGC = max_usb_ids * 2;
3636         }
3637         if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
3638                 LOG_WARNING("incomplete vid_pid configuration directive");
3639                 return ERROR_COMMAND_SYNTAX_ERROR;
3640         }
3641         for (i = 0; i < CMD_ARGC; i += 2) {
3642                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], stlink_dap_param.vid[i / 2]);
3643                 COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], stlink_dap_param.pid[i / 2]);
3644         }
3645
3646         /* null termination */
3647         stlink_dap_param.vid[i / 2] = stlink_dap_param.pid[i / 2] = 0;
3648
3649         return ERROR_OK;
3650 }
3651
3652 /** */
3653 static const struct command_registration stlink_dap_subcommand_handlers[] = {
3654         {
3655                 .name = "serial",
3656                 .handler = stlink_dap_serial_command,
3657                 .mode = COMMAND_CONFIG,
3658                 .help = "set the serial number of the adapter",
3659                 .usage = "<serial_number>",
3660         },
3661         {
3662                 .name = "vid_pid",
3663                 .handler = stlink_dap_vid_pid,
3664                 .mode = COMMAND_CONFIG,
3665                 .help = "USB VID and PID of the adapter",
3666                 .usage = "(vid pid)+",
3667         },
3668         COMMAND_REGISTRATION_DONE
3669 };
3670
3671 /** */
3672 static const struct command_registration stlink_dap_command_handlers[] = {
3673         {
3674                 .name = "st-link",
3675                 .mode = COMMAND_ANY,
3676                 .help = "perform st-link management",
3677                 .chain = stlink_dap_subcommand_handlers,
3678                 .usage = "",
3679         },
3680         COMMAND_REGISTRATION_DONE
3681 };
3682
3683 /** */
3684 static int stlink_dap_init(void)
3685 {
3686         enum reset_types jtag_reset_config = jtag_get_reset_config();
3687         enum stlink_mode mode;
3688         int retval;
3689
3690         LOG_DEBUG("stlink_dap_init()");
3691
3692         if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
3693                 if (jtag_reset_config & RESET_SRST_NO_GATING)
3694                         stlink_dap_param.connect_under_reset = true;
3695                 else
3696                         LOG_WARNING("\'srst_nogate\' reset_config option is required");
3697         }
3698
3699         if (transport_is_dapdirect_swd())
3700                 mode = STLINK_MODE_DEBUG_SWD;
3701         else if (transport_is_dapdirect_jtag())
3702                 mode = STLINK_MODE_DEBUG_JTAG;
3703         else if (transport_is_swim())
3704                 mode = STLINK_MODE_DEBUG_SWIM;
3705         else {
3706                 LOG_ERROR("Unsupported transport");
3707                 return ERROR_FAIL;
3708         }
3709
3710         retval = stlink_usb_open(&stlink_dap_param, mode, (void **)&stlink_dap_handle);
3711         if (retval != ERROR_OK)
3712                 return retval;
3713
3714         if ((mode != STLINK_MODE_DEBUG_SWIM) &&
3715                 !(stlink_dap_handle->version.flags & STLINK_F_HAS_DAP_REG)) {
3716                 LOG_ERROR("ST-Link version does not support DAP direct transport");
3717                 return ERROR_FAIL;
3718         }
3719         return ERROR_OK;
3720 }
3721
3722 /** */
3723 static int stlink_dap_quit(void)
3724 {
3725         LOG_DEBUG("stlink_dap_quit()");
3726
3727         free((void *)stlink_dap_param.serial);
3728         stlink_dap_param.serial = NULL;
3729
3730         return stlink_usb_close(stlink_dap_handle);
3731 }
3732
3733 /** */
3734 static int stlink_dap_reset(int req_trst, int req_srst)
3735 {
3736         LOG_DEBUG("stlink_dap_reset(%d)", req_srst);
3737         return stlink_usb_assert_srst(stlink_dap_handle,
3738                 req_srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
3739                                  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH);
3740 }
3741
3742 /** */
3743 static int stlink_dap_speed(int speed)
3744 {
3745         if (speed == 0) {
3746                 LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
3747                 return ERROR_JTAG_NOT_IMPLEMENTED;
3748         }
3749
3750         stlink_dap_param.initial_interface_speed = speed;
3751         stlink_speed(stlink_dap_handle, speed, false);
3752         return ERROR_OK;
3753 }
3754
3755 /** */
3756 static int stlink_dap_khz(int khz, int *jtag_speed)
3757 {
3758         if (khz == 0) {
3759                 LOG_ERROR("RCLK not supported");
3760                 return ERROR_FAIL;
3761         }
3762
3763         *jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
3764         return ERROR_OK;
3765 }
3766
3767 /** */
3768 static int stlink_dap_speed_div(int speed, int *khz)
3769 {
3770         *khz = speed;
3771         return ERROR_OK;
3772 }
3773
3774 static const struct dap_ops stlink_dap_ops = {
3775         .connect = stlink_dap_op_connect,
3776         .send_sequence = stlink_dap_op_send_sequence,
3777         .queue_dp_read = stlink_dap_op_queue_dp_read,
3778         .queue_dp_write = stlink_dap_op_queue_dp_write,
3779         .queue_ap_read = stlink_dap_op_queue_ap_read,
3780         .queue_ap_write = stlink_dap_op_queue_ap_write,
3781         .queue_ap_abort = stlink_dap_op_queue_ap_abort,
3782         .run = stlink_dap_op_run,
3783         .sync = NULL, /* optional */
3784         .quit = stlink_dap_op_quit, /* optional */
3785 };
3786
3787 static const struct swim_driver stlink_swim_ops = {
3788         .srst = stlink_swim_op_srst,
3789         .read_mem = stlink_swim_op_read_mem,
3790         .write_mem = stlink_swim_op_write_mem,
3791         .reconnect = stlink_swim_op_reconnect,
3792 };
3793
3794 static const char *const stlink_dap_transport[] = { "dapdirect_jtag", "dapdirect_swd", "swim", NULL };
3795
3796 struct adapter_driver stlink_dap_adapter_driver = {
3797         .name = "st-link",
3798         .transports = stlink_dap_transport,
3799         .commands = stlink_dap_command_handlers,
3800
3801         .init = stlink_dap_init,
3802         .quit = stlink_dap_quit,
3803         .reset = stlink_dap_reset,
3804         .speed = stlink_dap_speed,
3805         .khz = stlink_dap_khz,
3806         .speed_div = stlink_dap_speed_div,
3807         .config_trace = stlink_dap_config_trace,
3808         .poll_trace = stlink_dap_trace_read,
3809
3810         .dap_jtag_ops = &stlink_dap_ops,
3811         .dap_swd_ops = &stlink_dap_ops,
3812         .swim_ops = &stlink_swim_ops,
3813 };