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

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