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

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