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