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