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 <jtag/interface.h>
  35 #include <jtag/hla/hla_layout.h>
  36 #include <jtag/hla/hla_transport.h>
  37 #include <jtag/hla/hla_interface.h>
  38 #include <target/target.h>
  39
  40 #include <target/cortex_m.h>
  41
  42 #include "libusb_common.h"
  43
  44 #ifdef HAVE_LIBUSB1
  45 #define USE_LIBUSB_ASYNCIO
  46 #endif
  47
  48 #define ENDPOINT_IN  0x80
  49 #define ENDPOINT_OUT 0x00
  50
  51 #define STLINK_WRITE_TIMEOUT 1000
  52 #define STLINK_READ_TIMEOUT 1000
  53
  54 #define STLINK_NULL_EP        0
  55 #define STLINK_RX_EP          (1|ENDPOINT_IN)
  56 #define STLINK_TX_EP          (2|ENDPOINT_OUT)
  57 #define STLINK_TRACE_EP       (3|ENDPOINT_IN)
  58
  59 #define STLINK_V2_1_TX_EP     (1|ENDPOINT_OUT)
  60 #define STLINK_V2_1_TRACE_EP  (2|ENDPOINT_IN)
  61
  62 #define STLINK_SG_SIZE        (31)
  63 #define STLINK_DATA_SIZE      (4096)
  64 #define STLINK_CMD_SIZE_V2    (16)
  65 #define STLINK_CMD_SIZE_V1    (10)
  66
  67 #define STLINK_V1_PID         (0x3744)
  68 #define STLINK_V2_PID         (0x3748)
  69 #define STLINK_V2_1_PID       (0x374B)
  70 #define STLINK_V2_1_NO_MSD_PID  (0x3752)
  71 #define STLINK_V3_USBLOADER_PID (0x374D)
  72 #define STLINK_V3E_PID          (0x374E)
  73 #define STLINK_V3S_PID          (0x374F)
  74 #define STLINK_V3_2VCP_PID      (0x3753)
  75
  76 /*
  77  * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
  78  * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
  79  * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes.
  80  */
  81 #define STLINK_MAX_RW8          (64)
  82 #define STLINKV3_MAX_RW8        (512)
  83
  84 /* "WAIT" responses will be retried (with exponential backoff) at
  85  * most this many times before failing to caller.
  86  */
  87 #define MAX_WAIT_RETRIES 8
  88
  89 enum stlink_jtag_api_version {
  90         STLINK_JTAG_API_V1 = 1,
  91         STLINK_JTAG_API_V2,
  92         STLINK_JTAG_API_V3,
  93 };
  94
  95 /** */
  96 struct stlink_usb_version {
  97         /** */
  98         int stlink;
  99         /** */
 100         int jtag;
 101         /** */
 102         int swim;
 103         /** jtag api version supported */
 104         enum stlink_jtag_api_version jtag_api;
 105         /** one bit for each feature supported. See macros STLINK_F_* */
 106         uint32_t flags;
 107 };
 108
 109 /** */
 110 struct stlink_usb_handle_s {
 111         /** */
 112         struct jtag_libusb_device_handle *fd;
 113         /** */
 114         struct libusb_transfer *trans;
 115         /** */
 116         uint8_t rx_ep;
 117         /** */
 118         uint8_t tx_ep;
 119         /** */
 120         uint8_t trace_ep;
 121         /** */
 122         uint8_t cmdbuf[STLINK_SG_SIZE];
 123         /** */
 124         uint8_t cmdidx;
 125         /** */
 126         uint8_t direction;
 127         /** */
 128         uint8_t databuf[STLINK_DATA_SIZE];
 129         /** */
 130         uint32_t max_mem_packet;
 131         /** */
 132         enum hl_transports transport;
 133         /** */
 134         struct stlink_usb_version version;
 135         /** */
 136         uint16_t vid;
 137         /** */
 138         uint16_t pid;
 139         /** */
 140         struct {
 141                 /** whether SWO tracing is enabled or not */
 142                 bool enabled;
 143                 /** trace module source clock */
 144                 uint32_t source_hz;
 145         } trace;
 146         /** reconnect is needed next time we try to query the
 147          * status */
 148         bool reconnect_pending;
 149 };
 150
 151 #define STLINK_SWIM_ERR_OK             0x00
 152 #define STLINK_SWIM_BUSY               0x01
 153 #define STLINK_DEBUG_ERR_OK            0x80
 154 #define STLINK_DEBUG_ERR_FAULT         0x81
 155 #define STLINK_SWD_AP_WAIT             0x10
 156 #define STLINK_SWD_AP_FAULT            0x11
 157 #define STLINK_SWD_AP_ERROR            0x12
 158 #define STLINK_SWD_AP_PARITY_ERROR     0x13
 159 #define STLINK_JTAG_GET_IDCODE_ERROR   0x09
 160 #define STLINK_JTAG_WRITE_ERROR        0x0c
 161 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 162 #define STLINK_SWD_DP_WAIT             0x14
 163 #define STLINK_SWD_DP_FAULT            0x15
 164 #define STLINK_SWD_DP_ERROR            0x16
 165 #define STLINK_SWD_DP_PARITY_ERROR     0x17
 166
 167 #define STLINK_SWD_AP_WDATA_ERROR      0x18
 168 #define STLINK_SWD_AP_STICKY_ERROR     0x19
 169 #define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
 170
 171 #define STLINK_BAD_AP_ERROR            0x1d
 172
 173 #define STLINK_CORE_RUNNING            0x80
 174 #define STLINK_CORE_HALTED             0x81
 175 #define STLINK_CORE_STAT_UNKNOWN       -1
 176
 177 #define STLINK_GET_VERSION             0xF1
 178 #define STLINK_DEBUG_COMMAND           0xF2
 179 #define STLINK_DFU_COMMAND             0xF3
 180 #define STLINK_SWIM_COMMAND            0xF4
 181 #define STLINK_GET_CURRENT_MODE        0xF5
 182 #define STLINK_GET_TARGET_VOLTAGE      0xF7
 183
 184 #define STLINK_DEV_DFU_MODE            0x00
 185 #define STLINK_DEV_MASS_MODE           0x01
 186 #define STLINK_DEV_DEBUG_MODE          0x02
 187 #define STLINK_DEV_SWIM_MODE           0x03
 188 #define STLINK_DEV_BOOTLOADER_MODE     0x04
 189 #define STLINK_DEV_UNKNOWN_MODE        -1
 190
 191 #define STLINK_DFU_EXIT                0x07
 192
 193 /*
 194         STLINK_SWIM_ENTER_SEQ
 195         1.3ms low then 750Hz then 1.5kHz
 196
 197         STLINK_SWIM_GEN_RST
 198         STM8 DM pulls reset pin low 50us
 199
 200         STLINK_SWIM_SPEED
 201         uint8_t (0=low|1=high)
 202
 203         STLINK_SWIM_WRITEMEM
 204         uint16_t length
 205         uint32_t address
 206
 207         STLINK_SWIM_RESET
 208         send syncronization seq (16us low, response 64 clocks low)
 209 */
 210 #define STLINK_SWIM_ENTER                  0x00
 211 #define STLINK_SWIM_EXIT                   0x01
 212 #define STLINK_SWIM_READ_CAP               0x02
 213 #define STLINK_SWIM_SPEED                  0x03
 214 #define STLINK_SWIM_ENTER_SEQ              0x04
 215 #define STLINK_SWIM_GEN_RST                0x05
 216 #define STLINK_SWIM_RESET                  0x06
 217 #define STLINK_SWIM_ASSERT_RESET           0x07
 218 #define STLINK_SWIM_DEASSERT_RESET         0x08
 219 #define STLINK_SWIM_READSTATUS             0x09
 220 #define STLINK_SWIM_WRITEMEM               0x0a
 221 #define STLINK_SWIM_READMEM                0x0b
 222 #define STLINK_SWIM_READBUF                0x0c
 223
 224 #define STLINK_DEBUG_GETSTATUS             0x01
 225 #define STLINK_DEBUG_FORCEDEBUG            0x02
 226 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
 227 #define STLINK_DEBUG_APIV1_READALLREGS     0x04
 228 #define STLINK_DEBUG_APIV1_READREG         0x05
 229 #define STLINK_DEBUG_APIV1_WRITEREG        0x06
 230 #define STLINK_DEBUG_READMEM_32BIT         0x07
 231 #define STLINK_DEBUG_WRITEMEM_32BIT        0x08
 232 #define STLINK_DEBUG_RUNCORE               0x09
 233 #define STLINK_DEBUG_STEPCORE              0x0a
 234 #define STLINK_DEBUG_APIV1_SETFP           0x0b
 235 #define STLINK_DEBUG_READMEM_8BIT          0x0c
 236 #define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
 237 #define STLINK_DEBUG_APIV1_CLEARFP         0x0e
 238 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 239 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 240
 241 #define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
 242 #define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
 243 #define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
 244
 245 #define STLINK_DEBUG_APIV1_ENTER           0x20
 246 #define STLINK_DEBUG_EXIT                  0x21
 247 #define STLINK_DEBUG_READCOREID            0x22
 248
 249 #define STLINK_DEBUG_APIV2_ENTER           0x30
 250 #define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
 251 #define STLINK_DEBUG_APIV2_RESETSYS        0x32
 252 #define STLINK_DEBUG_APIV2_READREG         0x33
 253 #define STLINK_DEBUG_APIV2_WRITEREG        0x34
 254 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
 255 #define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
 256
 257 #define STLINK_DEBUG_APIV2_READALLREGS     0x3A
 258 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 259 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 260
 261 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
 262
 263 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 264 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 265 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 266 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
 267 #define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
 268
 269 #define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
 270 #define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
 271
 272 #define STLINK_APIV3_SET_COM_FREQ           0x61
 273 #define STLINK_APIV3_GET_COM_FREQ           0x62
 274
 275 #define STLINK_APIV3_GET_VERSION_EX         0xFB
 276
 277 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 278 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 279 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 280
 281 #define STLINK_TRACE_SIZE               4096
 282 #define STLINK_TRACE_MAX_HZ             2000000
 283
 284 #define STLINK_V3_MAX_FREQ_NB               10
 285
 286 /** */
 287 enum stlink_mode {
 288         STLINK_MODE_UNKNOWN = 0,
 289         STLINK_MODE_DFU,
 290         STLINK_MODE_MASS,
 291         STLINK_MODE_DEBUG_JTAG,
 292         STLINK_MODE_DEBUG_SWD,
 293         STLINK_MODE_DEBUG_SWIM
 294 };
 295
 296 #define REQUEST_SENSE        0x03
 297 #define REQUEST_SENSE_LENGTH 18
 298
 299 /*
 300  * Map the relevant features, quirks and workaround for specific firmware
 301  * version of stlink
 302  */
 303 #define STLINK_F_HAS_TRACE              (1UL << 0)
 304 #define STLINK_F_HAS_SWD_SET_FREQ       (1UL << 1)
 305 #define STLINK_F_HAS_JTAG_SET_FREQ      (1UL << 2)
 306 #define STLINK_F_HAS_MEM_16BIT          (1UL << 3)
 307 #define STLINK_F_HAS_GETLASTRWSTATUS2   (1UL << 4)
 308
 309 /* aliases */
 310 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
 311
 312 struct speed_map {
 313         int speed;
 314         int speed_divisor;
 315 };
 316
 317 /* SWD clock speed */
 318 static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 319         {4000, 0},
 320         {1800, 1}, /* default */
 321         {1200, 2},
 322         {950,  3},
 323         {480,  7},
 324         {240, 15},
 325         {125, 31},
 326         {100, 40},
 327         {50,  79},
 328         {25, 158},
 329         {15, 265},
 330         {5,  798}
 331 };
 332
 333 /* JTAG clock speed */
 334 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
 335         {18000, 2},
 336         {9000,  4},
 337         {4500,  8},
 338         {2250, 16},
 339         {1125, 32}, /* default */
 340         {562,  64},
 341         {281, 128},
 342         {140, 256}
 343 };
 344
 345 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 346 static int stlink_swim_status(void *handle);
 347
 348 /** */
 349 static unsigned int stlink_usb_block(void *handle)
 350 {
 351         struct stlink_usb_handle_s *h = handle;
 352
 353         assert(handle != NULL);
 354
 355         if (h->version.stlink == 3)
 356                 return STLINKV3_MAX_RW8;
 357         else
 358                 return STLINK_MAX_RW8;
 359 }
 360
 361
 362
 363 #ifdef USE_LIBUSB_ASYNCIO
 364
 365 static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
 366 {
 367         int *completed = transfer->user_data;
 368         *completed = 1;
 369         /* caller interprets result and frees transfer */
 370 }
 371
 372
 373 static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
 374 {
 375         int r, *completed = transfer->user_data;
 376
 377         /* Assuming a single libusb context exists.  There no existing interface into this
 378          * module to pass a libusb context.
 379          */
 380         struct libusb_context *ctx = NULL;
 381
 382         while (!*completed) {
 383                 r = libusb_handle_events_completed(ctx, completed);
 384                 if (r < 0) {
 385                         if (r == LIBUSB_ERROR_INTERRUPTED)
 386                                 continue;
 387                         libusb_cancel_transfer(transfer);
 388                         continue;
 389                 }
 390         }
 391 }
 392
 393
 394 static int transfer_error_status(const struct libusb_transfer *transfer)
 395 {
 396         int r = 0;
 397
 398         switch (transfer->status) {
 399                 case LIBUSB_TRANSFER_COMPLETED:
 400                         r = 0;
 401                         break;
 402                 case LIBUSB_TRANSFER_TIMED_OUT:
 403                         r = LIBUSB_ERROR_TIMEOUT;
 404                         break;
 405                 case LIBUSB_TRANSFER_STALL:
 406                         r = LIBUSB_ERROR_PIPE;
 407                         break;
 408                 case LIBUSB_TRANSFER_OVERFLOW:
 409                         r = LIBUSB_ERROR_OVERFLOW;
 410                         break;
 411                 case LIBUSB_TRANSFER_NO_DEVICE:
 412                         r = LIBUSB_ERROR_NO_DEVICE;
 413                         break;
 414                 case LIBUSB_TRANSFER_ERROR:
 415                 case LIBUSB_TRANSFER_CANCELLED:
 416                         r = LIBUSB_ERROR_IO;
 417                         break;
 418                 default:
 419                         r = LIBUSB_ERROR_OTHER;
 420                         break;
 421         }
 422
 423         return r;
 424 }
 425
 426 struct jtag_xfer {
 427         int ep;
 428         uint8_t *buf;
 429         size_t size;
 430         /* Internal */
 431         int retval;
 432         int completed;
 433         size_t transfer_size;
 434         struct libusb_transfer *transfer;
 435 };
 436
 437 static int jtag_libusb_bulk_transfer_n(
 438                 jtag_libusb_device_handle * dev_handle,
 439                 struct jtag_xfer *transfers,
 440                 size_t n_transfers,
 441                 int timeout)
 442 {
 443         int retval = 0;
 444         int returnval = ERROR_OK;
 445
 446
 447         for (size_t i = 0; i < n_transfers; ++i) {
 448                 transfers[i].retval = 0;
 449                 transfers[i].completed = 0;
 450                 transfers[i].transfer_size = 0;
 451                 transfers[i].transfer = libusb_alloc_transfer(0);
 452
 453                 if (transfers[i].transfer == NULL) {
 454                         for (size_t j = 0; j < i; ++j)
 455                                 libusb_free_transfer(transfers[j].transfer);
 456
 457                         LOG_DEBUG("ERROR, failed to alloc usb transfers");
 458                         for (size_t k = 0; k < n_transfers; ++k)
 459                                 transfers[k].retval = LIBUSB_ERROR_NO_MEM;
 460                         return ERROR_FAIL;
 461                 }
 462         }
 463
 464         for (size_t i = 0; i < n_transfers; ++i) {
 465                 libusb_fill_bulk_transfer(
 466                                 transfers[i].transfer,
 467                                 dev_handle,
 468                                 transfers[i].ep, transfers[i].buf, transfers[i].size,
 469                                 sync_transfer_cb, &transfers[i].completed, timeout);
 470                 transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
 471
 472                 retval = libusb_submit_transfer(transfers[i].transfer);
 473                 if (retval < 0) {
 474                         LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
 475
 476                         /* Probably no point continuing to submit transfers once a submission fails.
 477                          * As a result, tag all remaining transfers as errors.
 478                          */
 479                         for (size_t j = i; j < n_transfers; ++j)
 480                                 transfers[j].retval = retval;
 481
 482                         returnval = ERROR_FAIL;
 483                         break;
 484                 }
 485         }
 486
 487         /* Wait for every submitted USB transfer to complete.
 488         */
 489         for (size_t i = 0; i < n_transfers; ++i) {
 490                 if (transfers[i].retval == 0) {
 491                         sync_transfer_wait_for_completion(transfers[i].transfer);
 492
 493                         retval = transfer_error_status(transfers[i].transfer);
 494                         if (retval) {
 495                                 returnval = ERROR_FAIL;
 496                                 transfers[i].retval = retval;
 497                                 LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
 498                         } else {
 499                                 /* Assuming actual_length is only valid if there is no transfer error.
 500                                  */
 501                                 transfers[i].transfer_size = transfers[i].transfer->actual_length;
 502                         }
 503                 }
 504
 505                 libusb_free_transfer(transfers[i].transfer);
 506                 transfers[i].transfer = NULL;
 507         }
 508
 509         return returnval;
 510 }
 511
 512 #endif
 513
 514
 515 /** */
 516 static int stlink_usb_xfer_v1_get_status(void *handle)
 517 {
 518         struct stlink_usb_handle_s *h = handle;
 519
 520         assert(handle != NULL);
 521
 522         /* read status */
 523         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 524
 525         if (jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)h->cmdbuf,
 526                         13, STLINK_READ_TIMEOUT) != 13)
 527                 return ERROR_FAIL;
 528
 529         uint32_t t1;
 530
 531         t1 = buf_get_u32(h->cmdbuf, 0, 32);
 532
 533         /* check for USBS */
 534         if (t1 != 0x53425355)
 535                 return ERROR_FAIL;
 536         /*
 537          * CSW status:
 538          * 0 success
 539          * 1 command failure
 540          * 2 phase error
 541          */
 542         if (h->cmdbuf[12] != 0)
 543                 return ERROR_FAIL;
 544
 545         return ERROR_OK;
 546 }
 547
 548 #ifdef USE_LIBUSB_ASYNCIO
 549 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 550 {
 551         struct stlink_usb_handle_s *h = handle;
 552
 553         assert(handle != NULL);
 554
 555         size_t n_transfers = 0;
 556         struct jtag_xfer transfers[2];
 557
 558         memset(transfers, 0, sizeof(transfers));
 559
 560         transfers[0].ep = h->tx_ep;
 561         transfers[0].buf = h->cmdbuf;
 562         transfers[0].size = cmdsize;
 563
 564         ++n_transfers;
 565
 566         if (h->direction == h->tx_ep && size) {
 567                 transfers[1].ep = h->tx_ep;
 568                 transfers[1].buf = (uint8_t *)buf;
 569                 transfers[1].size = size;
 570
 571                 ++n_transfers;
 572         } else if (h->direction == h->rx_ep && size) {
 573                 transfers[1].ep = h->rx_ep;
 574                 transfers[1].buf = (uint8_t *)buf;
 575                 transfers[1].size = size;
 576
 577                 ++n_transfers;
 578         }
 579
 580         return jtag_libusb_bulk_transfer_n(
 581                         h->fd,
 582                         transfers,
 583                         n_transfers,
 584                         STLINK_WRITE_TIMEOUT);
 585 }
 586 #else
 587 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 588 {
 589         struct stlink_usb_handle_s *h = handle;
 590
 591         assert(handle != NULL);
 592
 593         if (jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)h->cmdbuf, cmdsize,
 594                         STLINK_WRITE_TIMEOUT) != cmdsize) {
 595                 return ERROR_FAIL;
 596         }
 597
 598         if (h->direction == h->tx_ep && size) {
 599                 if (jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)buf,
 600                                 size, STLINK_WRITE_TIMEOUT) != size) {
 601                         LOG_DEBUG("bulk write failed");
 602                         return ERROR_FAIL;
 603                 }
 604         } else if (h->direction == h->rx_ep && size) {
 605                 if (jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)buf,
 606                                 size, STLINK_READ_TIMEOUT) != size) {
 607                         LOG_DEBUG("bulk read failed");
 608                         return ERROR_FAIL;
 609                 }
 610         }
 611
 612         return ERROR_OK;
 613 }
 614 #endif
 615
 616 /** */
 617 static int stlink_usb_xfer_v1_get_sense(void *handle)
 618 {
 619         int res;
 620         struct stlink_usb_handle_s *h = handle;
 621
 622         assert(handle != NULL);
 623
 624         stlink_usb_init_buffer(handle, h->rx_ep, 16);
 625
 626         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
 627         h->cmdbuf[h->cmdidx++] = 0;
 628         h->cmdbuf[h->cmdidx++] = 0;
 629         h->cmdbuf[h->cmdidx++] = 0;
 630         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
 631
 632         res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
 633
 634         if (res != ERROR_OK)
 635                 return res;
 636
 637         if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
 638                 return ERROR_FAIL;
 639
 640         return ERROR_OK;
 641 }
 642
 643 /*
 644         transfers block in cmdbuf
 645         <size> indicates number of bytes in the following
 646         data phase.
 647 */
 648 static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 649 {
 650         int err, cmdsize = STLINK_CMD_SIZE_V2;
 651         struct stlink_usb_handle_s *h = handle;
 652
 653         assert(handle != NULL);
 654
 655         if (h->version.stlink == 1) {
 656                 cmdsize = STLINK_SG_SIZE;
 657                 /* put length in bCBWCBLength */
 658                 h->cmdbuf[14] = h->cmdidx-15;
 659         }
 660
 661         err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 662
 663         if (err != ERROR_OK)
 664                 return err;
 665
 666         if (h->version.stlink == 1) {
 667                 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
 668                         /* check csw status */
 669                         if (h->cmdbuf[12] == 1) {
 670                                 LOG_DEBUG("get sense");
 671                                 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
 672                                         return ERROR_FAIL;
 673                         }
 674                         return ERROR_FAIL;
 675                 }
 676         }
 677
 678         return ERROR_OK;
 679 }
 680
 681 /**
 682     Converts an STLINK status code held in the first byte of a response
 683     to an openocd error, logs any error/wait status as debug output.
 684 */
 685 static int stlink_usb_error_check(void *handle)
 686 {
 687         struct stlink_usb_handle_s *h = handle;
 688
 689         assert(handle != NULL);
 690
 691         if (h->transport == HL_TRANSPORT_SWIM) {
 692                 switch (h->databuf[0]) {
 693                         case STLINK_SWIM_ERR_OK:
 694                                 return ERROR_OK;
 695                         case STLINK_SWIM_BUSY:
 696                                 return ERROR_WAIT;
 697                         default:
 698                                 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 699                                 return ERROR_FAIL;
 700                 }
 701         }
 702
 703         /* TODO: no error checking yet on api V1 */
 704         if (h->version.jtag_api == STLINK_JTAG_API_V1)
 705                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
 706
 707         switch (h->databuf[0]) {
 708                 case STLINK_DEBUG_ERR_OK:
 709                         return ERROR_OK;
 710                 case STLINK_DEBUG_ERR_FAULT:
 711                         LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
 712                         return ERROR_FAIL;
 713                 case STLINK_SWD_AP_WAIT:
 714                         LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
 715                         return ERROR_WAIT;
 716                 case STLINK_SWD_DP_WAIT:
 717                         LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT);
 718                         return ERROR_WAIT;
 719                 case STLINK_JTAG_GET_IDCODE_ERROR:
 720                         LOG_DEBUG("STLINK_JTAG_GET_IDCODE_ERROR");
 721                         return ERROR_FAIL;
 722                 case STLINK_JTAG_WRITE_ERROR:
 723                         LOG_DEBUG("Write error");
 724                         return ERROR_FAIL;
 725                 case STLINK_JTAG_WRITE_VERIF_ERROR:
 726                         LOG_DEBUG("Write verify error, ignoring");
 727                         return ERROR_OK;
 728                 case STLINK_SWD_AP_FAULT:
 729                         /* git://git.ac6.fr/openocd commit 657e3e885b9ee10
 730                          * returns ERROR_OK with the comment:
 731                          * Change in error status when reading outside RAM.
 732                          * This fix allows CDT plugin to visualize memory.
 733                          */
 734                         LOG_DEBUG("STLINK_SWD_AP_FAULT");
 735                         return ERROR_FAIL;
 736                 case STLINK_SWD_AP_ERROR:
 737                         LOG_DEBUG("STLINK_SWD_AP_ERROR");
 738                         return ERROR_FAIL;
 739                 case STLINK_SWD_AP_PARITY_ERROR:
 740                         LOG_DEBUG("STLINK_SWD_AP_PARITY_ERROR");
 741                         return ERROR_FAIL;
 742                 case STLINK_SWD_DP_FAULT:
 743                         LOG_DEBUG("STLINK_SWD_DP_FAULT");
 744                         return ERROR_FAIL;
 745                 case STLINK_SWD_DP_ERROR:
 746                         LOG_DEBUG("STLINK_SWD_DP_ERROR");
 747                         return ERROR_FAIL;
 748                 case STLINK_SWD_DP_PARITY_ERROR:
 749                         LOG_DEBUG("STLINK_SWD_DP_PARITY_ERROR");
 750                         return ERROR_FAIL;
 751                 case STLINK_SWD_AP_WDATA_ERROR:
 752                         LOG_DEBUG("STLINK_SWD_AP_WDATA_ERROR");
 753                         return ERROR_FAIL;
 754                 case STLINK_SWD_AP_STICKY_ERROR:
 755                         LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
 756                         return ERROR_FAIL;
 757                 case STLINK_SWD_AP_STICKYORUN_ERROR:
 758                         LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
 759                         return ERROR_FAIL;
 760                 case STLINK_BAD_AP_ERROR:
 761                         LOG_DEBUG("STLINK_BAD_AP_ERROR");
 762                         return ERROR_FAIL;
 763                 default:
 764                         LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 765                         return ERROR_FAIL;
 766         }
 767 }
 768
 769
 770 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
 771
 772     Works for commands where the STLINK_DEBUG status is returned in the first
 773     byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
 774
 775     Returns an openocd result code.
 776 */
 777 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 778 {
 779         int retries = 0;
 780         int res;
 781         struct stlink_usb_handle_s *h = handle;
 782
 783         while (1) {
 784                 if ((h->transport != HL_TRANSPORT_SWIM) || !retries) {
 785                         res = stlink_usb_xfer(handle, buf, size);
 786                         if (res != ERROR_OK)
 787                                 return res;
 788                 }
 789
 790                 if (h->transport == HL_TRANSPORT_SWIM) {
 791                         res = stlink_swim_status(handle);
 792                         if (res != ERROR_OK)
 793                                 return res;
 794                 }
 795
 796                 res = stlink_usb_error_check(handle);
 797                 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 798                         useconds_t delay_us = (1<<retries++) * 1000;
 799                         LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
 800                         usleep(delay_us);
 801                         continue;
 802                 }
 803                 return res;
 804         }
 805 }
 806
 807 /** */
 808 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 809 {
 810         struct stlink_usb_handle_s *h = handle;
 811
 812         assert(handle != NULL);
 813
 814         assert(h->version.flags & STLINK_F_HAS_TRACE);
 815
 816         if (jtag_libusb_bulk_read(h->fd, h->trace_ep, (char *)buf,
 817                         size, STLINK_READ_TIMEOUT) != size) {
 818                 LOG_ERROR("bulk trace read failed");
 819                 return ERROR_FAIL;
 820         }
 821
 822         return ERROR_OK;
 823 }
 824
 825 /*
 826         this function writes transfer length in
 827         the right place in the cb
 828 */
 829 static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
 830 {
 831         struct stlink_usb_handle_s *h = handle;
 832
 833         buf_set_u32(h->cmdbuf+8, 0, 32, size);
 834 }
 835
 836 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 837 {
 838         struct stlink_usb_handle_s *h = handle;
 839
 840         /* fill the send buffer */
 841         strcpy((char *)h->cmdbuf, "USBC");
 842         h->cmdidx += 4;
 843         /* csw tag not used */
 844         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
 845         h->cmdidx += 4;
 846         /* cbw data transfer length (in the following data phase in or out) */
 847         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 848         h->cmdidx += 4;
 849         /* cbw flags */
 850         h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
 851         h->cmdbuf[h->cmdidx++] = 0; /* lun */
 852         /* cdb clength (is filled in at xfer) */
 853         h->cmdbuf[h->cmdidx++] = 0;
 854 }
 855
 856 /** */
 857 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
 858 {
 859         struct stlink_usb_handle_s *h = handle;
 860
 861         h->direction = direction;
 862
 863         h->cmdidx = 0;
 864
 865         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 866         memset(h->databuf, 0, STLINK_DATA_SIZE);
 867
 868         if (h->version.stlink == 1)
 869                 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
 870 }
 871
 872 /** */
 873 static int stlink_usb_version(void *handle)
 874 {
 875         int res;
 876         uint32_t flags;
 877         uint16_t version;
 878         uint8_t v, x, y, jtag, swim, msd, bridge = 0;
 879         char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
 880         char *p;
 881         struct stlink_usb_handle_s *h = handle;
 882
 883         assert(handle != NULL);
 884
 885         stlink_usb_init_buffer(handle, h->rx_ep, 6);
 886
 887         h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
 888
 889         res = stlink_usb_xfer(handle, h->databuf, 6);
 890
 891         if (res != ERROR_OK)
 892                 return res;
 893
 894         version = be_to_h_u16(h->databuf);
 895         v = (version >> 12) & 0x0f;
 896         x = (version >> 6) & 0x3f;
 897         y = version & 0x3f;
 898
 899         h->vid = le_to_h_u16(h->databuf + 2);
 900         h->pid = le_to_h_u16(h->databuf + 4);
 901
 902         switch (h->pid) {
 903         case STLINK_V2_1_PID:
 904         case STLINK_V2_1_NO_MSD_PID:
 905                 if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
 906                         /* MxSy : STM8 V2.1 - SWIM only */
 907                         msd = x;
 908                         swim = y;
 909                         jtag = 0;
 910                 } else {
 911                         /* JxMy : STM32 V2.1 - JTAG/SWD only */
 912                         jtag = x;
 913                         msd = y;
 914                         swim = 0;
 915                 }
 916                 break;
 917         default:
 918                 jtag = x;
 919                 swim = y;
 920                 msd = 0;
 921                 break;
 922         }
 923
 924         /* STLINK-V3 requires a specific command */
 925         if (v == 3 && x == 0 && y == 0) {
 926                 stlink_usb_init_buffer(handle, h->rx_ep, 16);
 927
 928                 h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
 929
 930                 res = stlink_usb_xfer(handle, h->databuf, 12);
 931                 if (res != ERROR_OK)
 932                         return res;
 933
 934                 v = h->databuf[0];
 935                 swim = h->databuf[1];
 936                 jtag = h->databuf[2];
 937                 msd  = h->databuf[3];
 938                 bridge = h->databuf[4];
 939                 h->vid = le_to_h_u16(h->databuf + 8);
 940                 h->pid = le_to_h_u16(h->databuf + 10);
 941         }
 942
 943         h->version.stlink = v;
 944         h->version.jtag = jtag;
 945         h->version.swim = swim;
 946
 947         flags = 0;
 948         switch (h->version.stlink) {
 949         case 1:
 950                 /* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
 951                 if (h->version.jtag >= 11)
 952                         h->version.jtag_api = STLINK_JTAG_API_V2;
 953                 else
 954                         h->version.jtag_api = STLINK_JTAG_API_V1;
 955
 956                 break;
 957         case 2:
 958                 /* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
 959                 h->version.jtag_api = STLINK_JTAG_API_V2;
 960
 961                 /* API for trace from J13 */
 962                 /* API for target voltage from J13 */
 963                 if (h->version.jtag >= 13)
 964                         flags |= STLINK_F_HAS_TRACE;
 965
 966                 /* preferred API to get last R/W status from J15 */
 967                 if (h->version.jtag >= 15)
 968                         flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
 969
 970                 /* API to set SWD frequency from J22 */
 971                 if (h->version.jtag >= 22)
 972                         flags |= STLINK_F_HAS_SWD_SET_FREQ;
 973
 974                 /* API to set JTAG frequency from J24 */
 975                 if (h->version.jtag >= 24)
 976                         flags |= STLINK_F_HAS_JTAG_SET_FREQ;
 977
 978                 /* API to read/write memory at 16 bit from J26 */
 979                 if (h->version.jtag >= 26)
 980                         flags |= STLINK_F_HAS_MEM_16BIT;
 981
 982                 break;
 983         case 3:
 984                 /* all STLINK-V3 use api-v3 */
 985                 h->version.jtag_api = STLINK_JTAG_API_V3;
 986
 987                 /* STLINK-V3 is a superset of ST-LINK/V2 */
 988
 989                 /* API for trace */
 990                 /* API for target voltage */
 991                 flags |= STLINK_F_HAS_TRACE;
 992
 993                 /* preferred API to get last R/W status */
 994                 flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
 995
 996                 /* API to read/write memory at 16 bit */
 997                 flags |= STLINK_F_HAS_MEM_16BIT;
 998
 999                 break;
1000         default:
1001                 break;
1002         }
1003         h->version.flags = flags;
1004
1005         p = v_str;
1006         p += sprintf(p, "V%d", v);
1007         if (jtag || !msd)
1008                 p += sprintf(p, "J%d", jtag);
1009         if (msd)
1010                 p += sprintf(p, "M%d", msd);
1011         if (bridge)
1012                 p += sprintf(p, "B%d", bridge);
1013         if (swim || !msd)
1014                 sprintf(p, "S%d", swim);
1015
1016         LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
1017                 v_str,
1018                 h->version.jtag_api,
1019                 h->vid,
1020                 h->pid);
1021
1022         return ERROR_OK;
1023 }
1024
1025 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
1026 {
1027         struct stlink_usb_handle_s *h = handle;
1028         uint32_t adc_results[2];
1029
1030         /* no error message, simply quit with error */
1031         if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
1032                 return ERROR_COMMAND_NOTFOUND;
1033
1034         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1035
1036         h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
1037
1038         int result = stlink_usb_xfer(handle, h->databuf, 8);
1039
1040         if (result != ERROR_OK)
1041                 return result;
1042
1043         /* convert result */
1044         adc_results[0] = le_to_h_u32(h->databuf);
1045         adc_results[1] = le_to_h_u32(h->databuf + 4);
1046
1047         *target_voltage = 0;
1048
1049         if (adc_results[0])
1050                 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
1051
1052         LOG_INFO("Target voltage: %f", (double)*target_voltage);
1053
1054         return ERROR_OK;
1055 }
1056
1057 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
1058 {
1059         struct stlink_usb_handle_s *h = handle;
1060
1061         assert(handle != NULL);
1062
1063         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
1064                 return ERROR_COMMAND_NOTFOUND;
1065
1066         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1067
1068         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1069         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
1070         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1071         h->cmdidx += 2;
1072
1073         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1074
1075         if (result != ERROR_OK)
1076                 return result;
1077
1078         return ERROR_OK;
1079 }
1080
1081 static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
1082 {
1083         struct stlink_usb_handle_s *h = handle;
1084
1085         assert(handle != NULL);
1086
1087         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
1088                 return ERROR_COMMAND_NOTFOUND;
1089
1090         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1091
1092         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1093         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
1094         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
1095         h->cmdidx += 2;
1096
1097         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
1098
1099         if (result != ERROR_OK)
1100                 return result;
1101
1102         return ERROR_OK;
1103 }
1104
1105 /** */
1106 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
1107 {
1108         int res;
1109         struct stlink_usb_handle_s *h = handle;
1110
1111         assert(handle != NULL);
1112
1113         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1114
1115         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
1116
1117         res = stlink_usb_xfer(handle, h->databuf, 2);
1118
1119         if (res != ERROR_OK)
1120                 return res;
1121
1122         *mode = h->databuf[0];
1123
1124         return ERROR_OK;
1125 }
1126
1127 /** */
1128 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
1129 {
1130         int rx_size = 0;
1131         struct stlink_usb_handle_s *h = handle;
1132
1133         assert(handle != NULL);
1134
1135         /* on api V2 we are able the read the latest command
1136          * status
1137          * TODO: we need the test on api V1 too
1138          */
1139         if (h->version.jtag_api != STLINK_JTAG_API_V1)
1140                 rx_size = 2;
1141
1142         stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
1143
1144         switch (type) {
1145                 case STLINK_MODE_DEBUG_JTAG:
1146                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1147                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1148                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1149                         else
1150                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1151                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
1152                         break;
1153                 case STLINK_MODE_DEBUG_SWD:
1154                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1155                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1156                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
1157                         else
1158                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
1159                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
1160                         break;
1161                 case STLINK_MODE_DEBUG_SWIM:
1162                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1163                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
1164                         /* no answer for this function... */
1165                         rx_size = 0;
1166                         break;
1167                 case STLINK_MODE_DFU:
1168                 case STLINK_MODE_MASS:
1169                 default:
1170                         return ERROR_FAIL;
1171         }
1172
1173         return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
1174 }
1175
1176 /** */
1177 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
1178 {
1179         int res;
1180         struct stlink_usb_handle_s *h = handle;
1181
1182         assert(handle != NULL);
1183
1184         stlink_usb_init_buffer(handle, STLINK_NULL_EP, 0);
1185
1186         switch (type) {
1187                 case STLINK_MODE_DEBUG_JTAG:
1188                 case STLINK_MODE_DEBUG_SWD:
1189                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1190                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
1191                         break;
1192                 case STLINK_MODE_DEBUG_SWIM:
1193                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1194                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
1195                         break;
1196                 case STLINK_MODE_DFU:
1197                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
1198                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
1199                         break;
1200                 case STLINK_MODE_MASS:
1201                 default:
1202                         return ERROR_FAIL;
1203         }
1204
1205         res = stlink_usb_xfer(handle, 0, 0);
1206
1207         if (res != ERROR_OK)
1208                 return res;
1209
1210         return ERROR_OK;
1211 }
1212
1213 static int stlink_usb_assert_srst(void *handle, int srst);
1214
1215 static enum stlink_mode stlink_get_mode(enum hl_transports t)
1216 {
1217         switch (t) {
1218         case HL_TRANSPORT_SWD:
1219                 return STLINK_MODE_DEBUG_SWD;
1220         case HL_TRANSPORT_JTAG:
1221                 return STLINK_MODE_DEBUG_JTAG;
1222         case HL_TRANSPORT_SWIM:
1223                 return STLINK_MODE_DEBUG_SWIM;
1224         default:
1225                 return STLINK_MODE_UNKNOWN;
1226         }
1227 }
1228
1229 /** */
1230 static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
1231 {
1232         int res;
1233         uint8_t mode;
1234         enum stlink_mode emode;
1235         struct stlink_usb_handle_s *h = handle;
1236
1237         assert(handle != NULL);
1238
1239         res = stlink_usb_current_mode(handle, &mode);
1240
1241         if (res != ERROR_OK)
1242                 return res;
1243
1244         LOG_DEBUG("MODE: 0x%02X", mode);
1245
1246         /* try to exit current mode */
1247         switch (mode) {
1248                 case STLINK_DEV_DFU_MODE:
1249                         emode = STLINK_MODE_DFU;
1250                         break;
1251                 case STLINK_DEV_DEBUG_MODE:
1252                         emode = STLINK_MODE_DEBUG_SWD;
1253                         break;
1254                 case STLINK_DEV_SWIM_MODE:
1255                         emode = STLINK_MODE_DEBUG_SWIM;
1256                         break;
1257                 case STLINK_DEV_BOOTLOADER_MODE:
1258                 case STLINK_DEV_MASS_MODE:
1259                 default:
1260                         emode = STLINK_MODE_UNKNOWN;
1261                         break;
1262         }
1263
1264         if (emode != STLINK_MODE_UNKNOWN) {
1265                 res = stlink_usb_mode_leave(handle, emode);
1266
1267                 if (res != ERROR_OK)
1268                         return res;
1269         }
1270
1271         res = stlink_usb_current_mode(handle, &mode);
1272
1273         if (res != ERROR_OK)
1274                 return res;
1275
1276         /* we check the target voltage here as an aid to debugging connection problems.
1277          * the stlink requires the target Vdd to be connected for reliable debugging.
1278          * this cmd is supported in all modes except DFU
1279          */
1280         if (mode != STLINK_DEV_DFU_MODE) {
1281
1282                 float target_voltage;
1283
1284                 /* check target voltage (if supported) */
1285                 res = stlink_usb_check_voltage(h, &target_voltage);
1286
1287                 if (res != ERROR_OK) {
1288                         if (res != ERROR_COMMAND_NOTFOUND)
1289                                 LOG_ERROR("voltage check failed");
1290                         /* attempt to continue as it is not a catastrophic failure */
1291                 } else {
1292                         /* check for a sensible target voltage, operating range is 1.65-5.5v
1293                          * according to datasheet */
1294                         if (target_voltage < 1.5)
1295                                 LOG_ERROR("target voltage may be too low for reliable debugging");
1296                 }
1297         }
1298
1299         LOG_DEBUG("MODE: 0x%02X", mode);
1300
1301         /* set selected mode */
1302         emode = stlink_get_mode(h->transport);
1303
1304         if (emode == STLINK_MODE_UNKNOWN) {
1305                 LOG_ERROR("selected mode (transport) not supported");
1306                 return ERROR_FAIL;
1307         }
1308
1309         /* preliminary SRST assert:
1310          * We want SRST is asserted before activating debug signals (mode_enter).
1311          * As the required mode has not been set, the adapter may not know what pin to use.
1312          * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
1313          * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
1314          * after power on, SWIM_RST stays unchanged */
1315         if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
1316                 stlink_usb_assert_srst(handle, 0);
1317                 /* do not check the return status here, we will
1318                    proceed and enter the desired mode below
1319                    and try asserting srst again. */
1320
1321         res = stlink_usb_mode_enter(handle, emode);
1322         if (res != ERROR_OK)
1323                 return res;
1324
1325         /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
1326         if (connect_under_reset) {
1327                 res = stlink_usb_assert_srst(handle, 0);
1328                 if (res != ERROR_OK)
1329                         return res;
1330         }
1331
1332         res = stlink_usb_current_mode(handle, &mode);
1333
1334         if (res != ERROR_OK)
1335                 return res;
1336
1337         LOG_DEBUG("MODE: 0x%02X", mode);
1338
1339         return ERROR_OK;
1340 }
1341
1342 /* request status from last swim request */
1343 static int stlink_swim_status(void *handle)
1344 {
1345         struct stlink_usb_handle_s *h = handle;
1346         int res;
1347
1348         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1349         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1350         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
1351         res = stlink_usb_xfer(handle, h->databuf, 4);
1352         if (res != ERROR_OK)
1353                 return res;
1354         return ERROR_OK;
1355 }
1356 /*
1357         the purpose of this function is unknown...
1358         capabilites? anyway for swim v6 it returns
1359         0001020600000000
1360 */
1361 __attribute__((unused))
1362 static int stlink_swim_cap(void *handle, uint8_t *cap)
1363 {
1364         struct stlink_usb_handle_s *h = handle;
1365         int res;
1366
1367         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1368         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1369         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
1370         h->cmdbuf[h->cmdidx++] = 0x01;
1371         res = stlink_usb_xfer(handle, h->databuf, 8);
1372         if (res != ERROR_OK)
1373                 return res;
1374         memcpy(cap, h->databuf, 8);
1375         return ERROR_OK;
1376 }
1377
1378 /*      debug dongle assert/deassert sreset line */
1379 static int stlink_swim_assert_reset(void *handle, int reset)
1380 {
1381         struct stlink_usb_handle_s *h = handle;
1382         int res;
1383
1384         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1385         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1386         if (!reset)
1387                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
1388         else
1389                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
1390         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1391         if (res != ERROR_OK)
1392                 return res;
1393         return ERROR_OK;
1394 }
1395
1396 /*
1397         send swim enter seq
1398         1.3ms low then 750Hz then 1.5kHz
1399 */
1400 static int stlink_swim_enter(void *handle)
1401 {
1402         struct stlink_usb_handle_s *h = handle;
1403         int res;
1404
1405         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1406         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1407         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1408         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1409         if (res != ERROR_OK)
1410                 return res;
1411         return ERROR_OK;
1412 }
1413
1414 /*      switch high/low speed swim */
1415 static int stlink_swim_speed(void *handle, int speed)
1416 {
1417         struct stlink_usb_handle_s *h = handle;
1418         int res;
1419
1420         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1421         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1422         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1423         if (speed)
1424                 h->cmdbuf[h->cmdidx++] = 1;
1425         else
1426                 h->cmdbuf[h->cmdidx++] = 0;
1427         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1428         if (res != ERROR_OK)
1429                 return res;
1430         return ERROR_OK;
1431 }
1432
1433 /*
1434         initiate srst from swim.
1435         nrst is pulled low for 50us.
1436 */
1437 static int stlink_swim_generate_rst(void *handle)
1438 {
1439         struct stlink_usb_handle_s *h = handle;
1440         int res;
1441
1442         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1443         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1444         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1445         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1446         if (res != ERROR_OK)
1447                 return res;
1448         return ERROR_OK;
1449 }
1450
1451 /*
1452         send resyncronize sequence
1453         swim is pulled low for 16us
1454         reply is 64 clks low
1455 */
1456 static int stlink_swim_resync(void *handle)
1457 {
1458         struct stlink_usb_handle_s *h = handle;
1459         int res;
1460
1461         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1462         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1463         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1464         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1465         if (res != ERROR_OK)
1466                 return res;
1467         return ERROR_OK;
1468 }
1469
1470 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1471 {
1472         struct stlink_usb_handle_s *h = handle;
1473         int res;
1474         unsigned int i;
1475         unsigned int datalen = 0;
1476         int cmdsize = STLINK_CMD_SIZE_V2;
1477
1478         if (len > STLINK_DATA_SIZE)
1479                 return ERROR_FAIL;
1480
1481         if (h->version.stlink == 1)
1482                 cmdsize = STLINK_SG_SIZE;
1483
1484         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1485         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1486         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1487         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1488         h->cmdidx += 2;
1489         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1490         h->cmdidx += 4;
1491         for (i = 0; i < len; i++) {
1492                 if (h->cmdidx == cmdsize)
1493                         h->databuf[datalen++] = *(data++);
1494                 else
1495                         h->cmdbuf[h->cmdidx++] = *(data++);
1496         }
1497         if (h->version.stlink == 1)
1498                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1499
1500         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1501         if (res != ERROR_OK)
1502                 return res;
1503         return ERROR_OK;
1504 }
1505
1506 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1507 {
1508         struct stlink_usb_handle_s *h = handle;
1509         int res;
1510
1511         if (len > STLINK_DATA_SIZE)
1512                 return ERROR_FAIL;
1513
1514         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1515         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1516         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1517         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1518         h->cmdidx += 2;
1519         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1520         h->cmdidx += 4;
1521         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1522         if (res != ERROR_OK)
1523                 return res;
1524
1525         stlink_usb_init_buffer(handle, h->rx_ep, len);
1526         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1527         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1528         res = stlink_usb_xfer(handle, data, len);
1529         if (res != ERROR_OK)
1530                 return res;
1531
1532         return ERROR_OK;
1533 }
1534
1535 /** */
1536 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1537 {
1538         int res;
1539         struct stlink_usb_handle_s *h = handle;
1540
1541         assert(handle != NULL);
1542
1543         /* there is no swim read core id cmd */
1544         if (h->transport == HL_TRANSPORT_SWIM) {
1545                 *idcode = 0;
1546                 return ERROR_OK;
1547         }
1548
1549         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1550
1551         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1552         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1553
1554         res = stlink_usb_xfer(handle, h->databuf, 4);
1555
1556         if (res != ERROR_OK)
1557                 return res;
1558
1559         *idcode = le_to_h_u32(h->databuf);
1560
1561         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1562
1563         return ERROR_OK;
1564 }
1565
1566 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1567 {
1568         struct stlink_usb_handle_s *h = handle;
1569         int res;
1570
1571         assert(handle != NULL);
1572
1573         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1574
1575         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1576         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1577         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1578         h->cmdidx += 4;
1579
1580         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1581         if (res != ERROR_OK)
1582                 return res;
1583
1584         *val = le_to_h_u32(h->databuf + 4);
1585         return ERROR_OK;
1586 }
1587
1588 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
1589 {
1590         struct stlink_usb_handle_s *h = handle;
1591
1592         assert(handle != NULL);
1593
1594         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1595
1596         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1597         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1598                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
1599         else
1600                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
1601         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1602         h->cmdidx += 4;
1603         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1604         h->cmdidx += 4;
1605
1606         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1607 }
1608
1609 /** */
1610 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
1611 {
1612         struct stlink_usb_handle_s *h = handle;
1613
1614         assert(handle != NULL);
1615
1616         if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
1617                 int res;
1618
1619                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1620
1621                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1622                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1623
1624                 res = stlink_usb_xfer(handle, h->databuf, 2);
1625                 if (res != ERROR_OK)
1626                         return res;
1627
1628                 size_t bytes_avail = le_to_h_u16(h->databuf);
1629                 *size = bytes_avail < *size ? bytes_avail : *size - 1;
1630
1631                 if (*size > 0) {
1632                         res = stlink_usb_read_trace(handle, buf, *size);
1633                         if (res != ERROR_OK)
1634                                 return res;
1635                         return ERROR_OK;
1636                 }
1637         }
1638         *size = 0;
1639         return ERROR_OK;
1640 }
1641
1642 static enum target_state stlink_usb_v2_get_status(void *handle)
1643 {
1644         int result;
1645         uint32_t status;
1646
1647         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
1648         if  (result != ERROR_OK)
1649                 return TARGET_UNKNOWN;
1650
1651         if (status & S_HALT)
1652                 return TARGET_HALTED;
1653         else if (status & S_RESET_ST)
1654                 return TARGET_RESET;
1655
1656         return TARGET_RUNNING;
1657 }
1658
1659 /** */
1660 static enum target_state stlink_usb_state(void *handle)
1661 {
1662         int res;
1663         struct stlink_usb_handle_s *h = handle;
1664
1665         assert(handle != NULL);
1666
1667         if (h->transport == HL_TRANSPORT_SWIM) {
1668                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1669                 if (res != ERROR_OK)
1670                         return TARGET_UNKNOWN;
1671
1672                 res = stlink_swim_resync(handle);
1673                 if (res != ERROR_OK)
1674                         return TARGET_UNKNOWN;
1675
1676                 return ERROR_OK;
1677         }
1678
1679         if (h->reconnect_pending) {
1680                 LOG_INFO("Previous state query failed, trying to reconnect");
1681                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1682
1683                 if (res != ERROR_OK)
1684                         return TARGET_UNKNOWN;
1685
1686                 h->reconnect_pending = false;
1687         }
1688
1689         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1690                 res = stlink_usb_v2_get_status(handle);
1691                 if (res == TARGET_UNKNOWN)
1692                         h->reconnect_pending = true;
1693                 return res;
1694         }
1695
1696         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1697
1698         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1699         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
1700
1701         res = stlink_usb_xfer(handle, h->databuf, 2);
1702
1703         if (res != ERROR_OK)
1704                 return TARGET_UNKNOWN;
1705
1706         if (h->databuf[0] == STLINK_CORE_RUNNING)
1707                 return TARGET_RUNNING;
1708         if (h->databuf[0] == STLINK_CORE_HALTED)
1709                 return TARGET_HALTED;
1710
1711         h->reconnect_pending = true;
1712
1713         return TARGET_UNKNOWN;
1714 }
1715
1716 static int stlink_usb_assert_srst(void *handle, int srst)
1717 {
1718         struct stlink_usb_handle_s *h = handle;
1719
1720         assert(handle != NULL);
1721
1722         if (h->transport == HL_TRANSPORT_SWIM)
1723                 return stlink_swim_assert_reset(handle, srst);
1724
1725         if (h->version.stlink == 1)
1726                 return ERROR_COMMAND_NOTFOUND;
1727
1728         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1729
1730         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1731         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
1732         h->cmdbuf[h->cmdidx++] = srst;
1733
1734         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1735 }
1736
1737 /** */
1738 static void stlink_usb_trace_disable(void *handle)
1739 {
1740         int res = ERROR_OK;
1741         struct stlink_usb_handle_s *h = handle;
1742
1743         assert(handle != NULL);
1744
1745         assert(h->version.flags & STLINK_F_HAS_TRACE);
1746
1747         LOG_DEBUG("Tracing: disable");
1748
1749         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1750         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1751         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1752         res = stlink_usb_xfer(handle, h->databuf, 2);
1753
1754         if (res == ERROR_OK)
1755                 h->trace.enabled = false;
1756 }
1757
1758
1759 /** */
1760 static int stlink_usb_trace_enable(void *handle)
1761 {
1762         int res;
1763         struct stlink_usb_handle_s *h = handle;
1764
1765         assert(handle != NULL);
1766
1767         if (h->version.flags & STLINK_F_HAS_TRACE) {
1768                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1769
1770                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1771                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1772                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1773                 h->cmdidx += 2;
1774                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
1775                 h->cmdidx += 4;
1776
1777                 res = stlink_usb_xfer(handle, h->databuf, 2);
1778
1779                 if (res == ERROR_OK)  {
1780                         h->trace.enabled = true;
1781                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
1782                 }
1783         } else {
1784                 LOG_ERROR("Tracing is not supported by this version.");
1785                 res = ERROR_FAIL;
1786         }
1787
1788         return res;
1789 }
1790
1791 /** */
1792 static int stlink_usb_reset(void *handle)
1793 {
1794         struct stlink_usb_handle_s *h = handle;
1795         int retval;
1796
1797         assert(handle != NULL);
1798
1799         if (h->transport == HL_TRANSPORT_SWIM)
1800                 return stlink_swim_generate_rst(handle);
1801
1802         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1803
1804         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1805
1806         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1807                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
1808         else
1809                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
1810
1811         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
1812         if (retval != ERROR_OK)
1813                 return retval;
1814
1815         if (h->trace.enabled) {
1816                 stlink_usb_trace_disable(h);
1817                 return stlink_usb_trace_enable(h);
1818         }
1819
1820         return ERROR_OK;
1821 }
1822
1823 /** */
1824 static int stlink_usb_run(void *handle)
1825 {
1826         int res;
1827         struct stlink_usb_handle_s *h = handle;
1828
1829         assert(handle != NULL);
1830
1831         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1832                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1833
1834                 return res;
1835         }
1836
1837         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1838
1839         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1840         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1841
1842         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1843 }
1844
1845 /** */
1846 static int stlink_usb_halt(void *handle)
1847 {
1848         int res;
1849         struct stlink_usb_handle_s *h = handle;
1850
1851         assert(handle != NULL);
1852
1853         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1854                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1855
1856                 return res;
1857         }
1858
1859         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1860
1861         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1862         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1863
1864         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1865 }
1866
1867 /** */
1868 static int stlink_usb_step(void *handle)
1869 {
1870         struct stlink_usb_handle_s *h = handle;
1871
1872         assert(handle != NULL);
1873
1874         if (h->version.jtag_api != STLINK_JTAG_API_V1) {
1875                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1876                  * that the Cortex-M3 currently does. */
1877                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1878                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1879                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1880         }
1881
1882         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1883
1884         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1885         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1886
1887         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1888 }
1889
1890 /** */
1891 static int stlink_usb_read_regs(void *handle)
1892 {
1893         int res;
1894         struct stlink_usb_handle_s *h = handle;
1895
1896         assert(handle != NULL);
1897
1898         stlink_usb_init_buffer(handle, h->rx_ep, 88);
1899
1900         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1901         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1902
1903                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
1904                 res = stlink_usb_xfer(handle, h->databuf, 84);
1905                 /* regs data from offset 0 */
1906         } else {
1907                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
1908                 res = stlink_usb_xfer(handle, h->databuf, 88);
1909                 /* status at offset 0, regs data from offset 4 */
1910         }
1911
1912         return res;
1913 }
1914
1915 /** */
1916 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
1917 {
1918         int res;
1919         struct stlink_usb_handle_s *h = handle;
1920
1921         assert(handle != NULL);
1922
1923         stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1924
1925         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1926         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1927                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
1928         else
1929                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
1930         h->cmdbuf[h->cmdidx++] = num;
1931
1932         if (h->version.jtag_api == STLINK_JTAG_API_V1) {
1933                 res = stlink_usb_xfer(handle, h->databuf, 4);
1934                 if (res != ERROR_OK)
1935                         return res;
1936                 *val = le_to_h_u32(h->databuf);
1937                 return ERROR_OK;
1938         } else {
1939                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1940                 if (res != ERROR_OK)
1941                         return res;
1942                 *val = le_to_h_u32(h->databuf + 4);
1943                 return ERROR_OK;
1944         }
1945 }
1946
1947 /** */
1948 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
1949 {
1950         struct stlink_usb_handle_s *h = handle;
1951
1952         assert(handle != NULL);
1953
1954         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1955
1956         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1957         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1958                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
1959         else
1960                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
1961         h->cmdbuf[h->cmdidx++] = num;
1962         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1963         h->cmdidx += 4;
1964
1965         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1966 }
1967
1968 static int stlink_usb_get_rw_status(void *handle)
1969 {
1970         int res;
1971         struct stlink_usb_handle_s *h = handle;
1972
1973         assert(handle != NULL);
1974
1975         if (h->version.jtag_api == STLINK_JTAG_API_V1)
1976                 return ERROR_OK;
1977
1978         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1979
1980         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1981         if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
1982                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
1983
1984                 res = stlink_usb_xfer(handle, h->databuf, 12);
1985         } else {
1986                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
1987
1988                 res = stlink_usb_xfer(handle, h->databuf, 2);
1989         }
1990
1991         if (res != ERROR_OK)
1992                 return res;
1993
1994         return stlink_usb_error_check(h);
1995 }
1996
1997 /** */
1998 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1999                           uint8_t *buffer)
2000 {
2001         int res;
2002         uint16_t read_len = len;
2003         struct stlink_usb_handle_s *h = handle;
2004
2005         assert(handle != NULL);
2006
2007         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2008         if (len > stlink_usb_block(h)) {
2009                 LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
2010                 return ERROR_FAIL;
2011         }
2012
2013         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
2014
2015         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2016         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
2017         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2018         h->cmdidx += 4;
2019         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2020         h->cmdidx += 2;
2021
2022         /* we need to fix read length for single bytes */
2023         if (read_len == 1)
2024                 read_len++;
2025
2026         res = stlink_usb_xfer(handle, h->databuf, read_len);
2027
2028         if (res != ERROR_OK)
2029                 return res;
2030
2031         memcpy(buffer, h->databuf, len);
2032
2033         return stlink_usb_get_rw_status(handle);
2034 }
2035
2036 /** */
2037 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
2038                            const uint8_t *buffer)
2039 {
2040         int res;
2041         struct stlink_usb_handle_s *h = handle;
2042
2043         assert(handle != NULL);
2044
2045         /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
2046         if (len > stlink_usb_block(h)) {
2047                 LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
2048                 return ERROR_FAIL;
2049         }
2050
2051         stlink_usb_init_buffer(handle, h->tx_ep, len);
2052
2053         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2054         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
2055         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2056         h->cmdidx += 4;
2057         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2058         h->cmdidx += 2;
2059
2060         res = stlink_usb_xfer(handle, buffer, len);
2061
2062         if (res != ERROR_OK)
2063                 return res;
2064
2065         return stlink_usb_get_rw_status(handle);
2066 }
2067
2068 /** */
2069 static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
2070                           uint8_t *buffer)
2071 {
2072         int res;
2073         struct stlink_usb_handle_s *h = handle;
2074
2075         assert(handle != NULL);
2076
2077         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2078                 return ERROR_COMMAND_NOTFOUND;
2079
2080         /* data must be a multiple of 2 and half-word aligned */
2081         if (len % 2 || addr % 2) {
2082                 LOG_DEBUG("Invalid data alignment");
2083                 return ERROR_TARGET_UNALIGNED_ACCESS;
2084         }
2085
2086         stlink_usb_init_buffer(handle, h->rx_ep, len);
2087
2088         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2089         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
2090         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2091         h->cmdidx += 4;
2092         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2093         h->cmdidx += 2;
2094
2095         res = stlink_usb_xfer(handle, h->databuf, len);
2096
2097         if (res != ERROR_OK)
2098                 return res;
2099
2100         memcpy(buffer, h->databuf, len);
2101
2102         return stlink_usb_get_rw_status(handle);
2103 }
2104
2105 /** */
2106 static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
2107                            const uint8_t *buffer)
2108 {
2109         int res;
2110         struct stlink_usb_handle_s *h = handle;
2111
2112         assert(handle != NULL);
2113
2114         if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2115                 return ERROR_COMMAND_NOTFOUND;
2116
2117         /* data must be a multiple of 2 and half-word aligned */
2118         if (len % 2 || addr % 2) {
2119                 LOG_DEBUG("Invalid data alignment");
2120                 return ERROR_TARGET_UNALIGNED_ACCESS;
2121         }
2122
2123         stlink_usb_init_buffer(handle, h->tx_ep, len);
2124
2125         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2126         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
2127         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2128         h->cmdidx += 4;
2129         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2130         h->cmdidx += 2;
2131
2132         res = stlink_usb_xfer(handle, buffer, len);
2133
2134         if (res != ERROR_OK)
2135                 return res;
2136
2137         return stlink_usb_get_rw_status(handle);
2138 }
2139
2140 /** */
2141 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
2142                           uint8_t *buffer)
2143 {
2144         int res;
2145         struct stlink_usb_handle_s *h = handle;
2146
2147         assert(handle != NULL);
2148
2149         /* data must be a multiple of 4 and word aligned */
2150         if (len % 4 || addr % 4) {
2151                 LOG_DEBUG("Invalid data alignment");
2152                 return ERROR_TARGET_UNALIGNED_ACCESS;
2153         }
2154
2155         stlink_usb_init_buffer(handle, h->rx_ep, len);
2156
2157         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2158         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
2159         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2160         h->cmdidx += 4;
2161         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2162         h->cmdidx += 2;
2163
2164         res = stlink_usb_xfer(handle, h->databuf, len);
2165
2166         if (res != ERROR_OK)
2167                 return res;
2168
2169         memcpy(buffer, h->databuf, len);
2170
2171         return stlink_usb_get_rw_status(handle);
2172 }
2173
2174 /** */
2175 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
2176                            const uint8_t *buffer)
2177 {
2178         int res;
2179         struct stlink_usb_handle_s *h = handle;
2180
2181         assert(handle != NULL);
2182
2183         /* data must be a multiple of 4 and word aligned */
2184         if (len % 4 || addr % 4) {
2185                 LOG_DEBUG("Invalid data alignment");
2186                 return ERROR_TARGET_UNALIGNED_ACCESS;
2187         }
2188
2189         stlink_usb_init_buffer(handle, h->tx_ep, len);
2190
2191         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2192         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
2193         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
2194         h->cmdidx += 4;
2195         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
2196         h->cmdidx += 2;
2197
2198         res = stlink_usb_xfer(handle, buffer, len);
2199
2200         if (res != ERROR_OK)
2201                 return res;
2202
2203         return stlink_usb_get_rw_status(handle);
2204 }
2205
2206 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
2207 {
2208         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
2209         if (max_tar_block == 0)
2210                 max_tar_block = 4;
2211         return max_tar_block;
2212 }
2213
2214 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
2215                 uint32_t count, uint8_t *buffer)
2216 {
2217         int retval = ERROR_OK;
2218         uint32_t bytes_remaining;
2219         int retries = 0;
2220         struct stlink_usb_handle_s *h = handle;
2221
2222         /* calculate byte count */
2223         count *= size;
2224
2225         /* switch to 8 bit if stlink does not support 16 bit memory read */
2226         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2227                 size = 1;
2228
2229         while (count) {
2230
2231                 bytes_remaining = (size != 1) ? \
2232                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2233
2234                 if (count < bytes_remaining)
2235                         bytes_remaining = count;
2236
2237                 if (h->transport == HL_TRANSPORT_SWIM) {
2238                         retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
2239                         if (retval != ERROR_OK)
2240                                 return retval;
2241                 } else
2242                 /*
2243                  * all stlink support 8/32bit memory read/writes and only from
2244                  * stlink V2J26 there is support for 16 bit memory read/write.
2245                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2246                  * as 8bit access.
2247                  */
2248                 if (size != 1) {
2249
2250                         /* When in jtag mode the stlink uses the auto-increment functionality.
2251                          * However it expects us to pass the data correctly, this includes
2252                          * alignment and any page boundaries. We already do this as part of the
2253                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2254                          * needs implementing manually.
2255                          * currently this only affects jtag mode, according to ST they do single
2256                          * access in SWD mode - but this may change and so we do it for both modes */
2257
2258                         /* we first need to check for any unaligned bytes */
2259                         if (addr & (size - 1)) {
2260
2261                                 uint32_t head_bytes = size - (addr & (size - 1));
2262                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
2263                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2264                                         usleep((1<<retries++) * 1000);
2265                                         continue;
2266                                 }
2267                                 if (retval != ERROR_OK)
2268                                         return retval;
2269                                 buffer += head_bytes;
2270                                 addr += head_bytes;
2271                                 count -= head_bytes;
2272                                 bytes_remaining -= head_bytes;
2273                         }
2274
2275                         if (bytes_remaining & (size - 1))
2276                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
2277                         else if (size == 2)
2278                                 retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
2279                         else
2280                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
2281                 } else
2282                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
2283
2284                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2285                         usleep((1<<retries++) * 1000);
2286                         continue;
2287                 }
2288                 if (retval != ERROR_OK)
2289                         return retval;
2290
2291                 buffer += bytes_remaining;
2292                 addr += bytes_remaining;
2293                 count -= bytes_remaining;
2294         }
2295
2296         return retval;
2297 }
2298
2299 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
2300                 uint32_t count, const uint8_t *buffer)
2301 {
2302         int retval = ERROR_OK;
2303         uint32_t bytes_remaining;
2304         int retries = 0;
2305         struct stlink_usb_handle_s *h = handle;
2306
2307         /* calculate byte count */
2308         count *= size;
2309
2310         /* switch to 8 bit if stlink does not support 16 bit memory read */
2311         if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
2312                 size = 1;
2313
2314         while (count) {
2315
2316                 bytes_remaining = (size != 1) ? \
2317                                 stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
2318
2319                 if (count < bytes_remaining)
2320                         bytes_remaining = count;
2321
2322                 if (h->transport == HL_TRANSPORT_SWIM) {
2323                         retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
2324                         if (retval != ERROR_OK)
2325                                 return retval;
2326                 } else
2327                 /*
2328                  * all stlink support 8/32bit memory read/writes and only from
2329                  * stlink V2J26 there is support for 16 bit memory read/write.
2330                  * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
2331                  * as 8bit access.
2332                  */
2333                 if (size != 1) {
2334
2335                         /* When in jtag mode the stlink uses the auto-increment functionality.
2336                          * However it expects us to pass the data correctly, this includes
2337                          * alignment and any page boundaries. We already do this as part of the
2338                          * adi_v5 implementation, but the stlink is a hla adapter and so this
2339                          * needs implementing manually.
2340                          * currently this only affects jtag mode, according to ST they do single
2341                          * access in SWD mode - but this may change and so we do it for both modes */
2342
2343                         /* we first need to check for any unaligned bytes */
2344                         if (addr & (size - 1)) {
2345
2346                                 uint32_t head_bytes = size - (addr & (size - 1));
2347                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
2348                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2349                                         usleep((1<<retries++) * 1000);
2350                                         continue;
2351                                 }
2352                                 if (retval != ERROR_OK)
2353                                         return retval;
2354                                 buffer += head_bytes;
2355                                 addr += head_bytes;
2356                                 count -= head_bytes;
2357                                 bytes_remaining -= head_bytes;
2358                         }
2359
2360                         if (bytes_remaining & (size - 1))
2361                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
2362                         else if (size == 2)
2363                                 retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
2364                         else
2365                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
2366
2367                 } else
2368                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
2369                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
2370                         usleep((1<<retries++) * 1000);
2371                         continue;
2372                 }
2373                 if (retval != ERROR_OK)
2374                         return retval;
2375
2376                 buffer += bytes_remaining;
2377                 addr += bytes_remaining;
2378                 count -= bytes_remaining;
2379         }
2380
2381         return retval;
2382 }
2383
2384 /** */
2385 static int stlink_usb_override_target(const char *targetname)
2386 {
2387         return !strcmp(targetname, "cortex_m");
2388 }
2389
2390 static int stlink_speed_swim(void *handle, int khz, bool query)
2391 {
2392         /*
2393                         we dont care what the khz rate is
2394                         we only have low and high speed...
2395                         before changing speed the SWIM_CSR HS bit
2396                         must be updated
2397          */
2398         if (khz == 0)
2399                 stlink_swim_speed(handle, 0);
2400         else
2401                 stlink_swim_speed(handle, 1);
2402         return khz;
2403 }
2404
2405 static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
2406 {
2407         unsigned int i;
2408         int speed_index = -1;
2409         int speed_diff = INT_MAX;
2410         int last_valid_speed = -1;
2411         bool match = true;
2412
2413         for (i = 0; i < map_size; i++) {
2414                 if (!map[i].speed)
2415                         continue;
2416                 last_valid_speed = i;
2417                 if (khz == map[i].speed) {
2418                         speed_index = i;
2419                         break;
2420                 } else {
2421                         int current_diff = khz - map[i].speed;
2422                         /* get abs value for comparison */
2423                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
2424                         if ((current_diff < speed_diff) && khz >= map[i].speed) {
2425                                 speed_diff = current_diff;
2426                                 speed_index = i;
2427                         }
2428                 }
2429         }
2430
2431         if (speed_index == -1) {
2432                 /* this will only be here if we cannot match the slow speed.
2433                  * use the slowest speed we support.*/
2434                 speed_index = last_valid_speed;
2435                 match = false;
2436         } else if (i == map_size)
2437                 match = false;
2438
2439         if (!match && query) {
2440                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
2441                                 khz, map[speed_index].speed);
2442         }
2443
2444         return speed_index;
2445 }
2446
2447 static int stlink_speed_swd(void *handle, int khz, bool query)
2448 {
2449         int speed_index;
2450         struct stlink_usb_handle_s *h = handle;
2451
2452         /* old firmware cannot change it */
2453         if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
2454                 return khz;
2455
2456         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
2457                 ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
2458
2459         if (!query) {
2460                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
2461                 if (result != ERROR_OK) {
2462                         LOG_ERROR("Unable to set adapter speed");
2463                         return khz;
2464                 }
2465         }
2466
2467         return stlink_khz_to_speed_map_swd[speed_index].speed;
2468 }
2469
2470 static int stlink_speed_jtag(void *handle, int khz, bool query)
2471 {
2472         int speed_index;
2473         struct stlink_usb_handle_s *h = handle;
2474
2475         /* old firmware cannot change it */
2476         if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
2477                 return khz;
2478
2479         speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
2480                 ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
2481
2482         if (!query) {
2483                 int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
2484                 if (result != ERROR_OK) {
2485                         LOG_ERROR("Unable to set adapter speed");
2486                         return khz;
2487                 }
2488         }
2489
2490         return stlink_khz_to_speed_map_jtag[speed_index].speed;
2491 }
2492
2493 void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
2494 {
2495         unsigned int i;
2496
2497         LOG_DEBUG("Supported clock speeds are:");
2498         for (i = 0; i < map_size; i++)
2499                 if (map[i].speed)
2500                         LOG_DEBUG("%d kHz", map[i].speed);
2501 }
2502
2503 static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
2504 {
2505         struct stlink_usb_handle_s *h = handle;
2506         int i;
2507
2508         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2509                 LOG_ERROR("Unknown command");
2510                 return 0;
2511         }
2512
2513         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2514
2515         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2516         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
2517         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2518
2519         int res = stlink_usb_xfer(handle, h->databuf, 52);
2520
2521         int size = h->databuf[8];
2522
2523         if (size > STLINK_V3_MAX_FREQ_NB)
2524                 size = STLINK_V3_MAX_FREQ_NB;
2525
2526         for (i = 0; i < size; i++) {
2527                 map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
2528                 map[i].speed_divisor = i;
2529         }
2530
2531         /* set to zero all the next entries */
2532         for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
2533                 map[i].speed = 0;
2534
2535         return res;
2536 }
2537
2538 static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
2539 {
2540         struct stlink_usb_handle_s *h = handle;
2541
2542         if (h->version.jtag_api != STLINK_JTAG_API_V3) {
2543                 LOG_ERROR("Unknown command");
2544                 return 0;
2545         }
2546
2547         stlink_usb_init_buffer(handle, h->rx_ep, 16);
2548
2549         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
2550         h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
2551         h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
2552         h->cmdbuf[h->cmdidx++] = 0;
2553
2554         h_u32_to_le(&h->cmdbuf[4], frequency);
2555
2556         return stlink_usb_xfer(handle, h->databuf, 8);
2557 }
2558
2559 static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
2560 {
2561         struct stlink_usb_handle_s *h = handle;
2562         int speed_index;
2563         struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2564
2565         stlink_get_com_freq(h, is_jtag, map);
2566
2567         speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
2568
2569         if (!query) {
2570                 int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
2571                 if (result != ERROR_OK) {
2572                         LOG_ERROR("Unable to set adapter speed");
2573                         return khz;
2574                 }
2575         }
2576         return map[speed_index].speed;
2577 }
2578
2579 static int stlink_speed(void *handle, int khz, bool query)
2580 {
2581         struct stlink_usb_handle_s *h = handle;
2582
2583         if (!handle)
2584                 return khz;
2585
2586         switch (h->transport) {
2587         case HL_TRANSPORT_SWIM:
2588                 return stlink_speed_swim(handle, khz, query);
2589                 break;
2590         case HL_TRANSPORT_SWD:
2591                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2592                         return stlink_speed_v3(handle, false, khz, query);
2593                 else
2594                         return stlink_speed_swd(handle, khz, query);
2595                 break;
2596         case HL_TRANSPORT_JTAG:
2597                 if (h->version.jtag_api == STLINK_JTAG_API_V3)
2598                         return stlink_speed_v3(handle, true, khz, query);
2599                 else
2600                         return stlink_speed_jtag(handle, khz, query);
2601                 break;
2602         default:
2603                 break;
2604         }
2605
2606         return khz;
2607 }
2608
2609 /** */
2610 static int stlink_usb_close(void *handle)
2611 {
2612         int res;
2613         uint8_t mode;
2614         enum stlink_mode emode;
2615         struct stlink_usb_handle_s *h = handle;
2616
2617         if (h && h->fd)
2618                 res = stlink_usb_current_mode(handle, &mode);
2619         else
2620                 res = ERROR_FAIL;
2621         /* do not exit if return code != ERROR_OK,
2622            it prevents us from closing jtag_libusb */
2623
2624         if (res == ERROR_OK) {
2625                 /* try to exit current mode */
2626                 switch (mode) {
2627                         case STLINK_DEV_DFU_MODE:
2628                                 emode = STLINK_MODE_DFU;
2629                                 break;
2630                         case STLINK_DEV_DEBUG_MODE:
2631                                 emode = STLINK_MODE_DEBUG_SWD;
2632                                 break;
2633                         case STLINK_DEV_SWIM_MODE:
2634                                 emode = STLINK_MODE_DEBUG_SWIM;
2635                                 break;
2636                         case STLINK_DEV_BOOTLOADER_MODE:
2637                         case STLINK_DEV_MASS_MODE:
2638                         default:
2639                                 emode = STLINK_MODE_UNKNOWN;
2640                                 break;
2641                 }
2642
2643                 if (emode != STLINK_MODE_UNKNOWN)
2644                         stlink_usb_mode_leave(handle, emode);
2645                         /* do not check return code, it prevent
2646                         us from closing jtag_libusb */
2647         }
2648
2649         if (h && h->fd)
2650                 jtag_libusb_close(h->fd);
2651
2652         free(h);
2653
2654         return ERROR_OK;
2655 }
2656
2657 /** */
2658 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
2659 {
2660         int err, retry_count = 1;
2661         struct stlink_usb_handle_s *h;
2662
2663         LOG_DEBUG("stlink_usb_open");
2664
2665         h = calloc(1, sizeof(struct stlink_usb_handle_s));
2666
2667         if (h == 0) {
2668                 LOG_DEBUG("malloc failed");
2669                 return ERROR_FAIL;
2670         }
2671
2672         h->transport = param->transport;
2673
2674         for (unsigned i = 0; param->vid[i]; i++) {
2675                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
2676                           param->transport, param->vid[i], param->pid[i],
2677                           param->serial ? param->serial : "");
2678         }
2679
2680         /*
2681           On certain host USB configurations(e.g. MacBook Air)
2682           STLINKv2 dongle seems to have its FW in a funky state if,
2683           after plugging it in, you try to use openocd with it more
2684           then once (by launching and closing openocd). In cases like
2685           that initial attempt to read the FW info via
2686           stlink_usb_version will fail and the device has to be reset
2687           in order to become operational.
2688          */
2689         do {
2690                 if (jtag_libusb_open(param->vid, param->pid, param->serial, &h->fd) != ERROR_OK) {
2691                         LOG_ERROR("open failed");
2692                         goto error_open;
2693                 }
2694
2695                 jtag_libusb_set_configuration(h->fd, 0);
2696
2697                 if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
2698                         LOG_DEBUG("claim interface failed");
2699                         goto error_open;
2700                 }
2701
2702                 /* RX EP is common for all versions */
2703                 h->rx_ep = STLINK_RX_EP;
2704
2705                 uint16_t pid;
2706                 if (jtag_libusb_get_pid(jtag_libusb_get_device(h->fd), &pid) != ERROR_OK) {
2707                         LOG_DEBUG("libusb_get_pid failed");
2708                         goto error_open;
2709                 }
2710
2711                 /* wrap version for first read */
2712                 switch (pid) {
2713                         case STLINK_V1_PID:
2714                                 h->version.stlink = 1;
2715                                 h->tx_ep = STLINK_TX_EP;
2716                                 break;
2717                         case STLINK_V3_USBLOADER_PID:
2718                         case STLINK_V3E_PID:
2719                         case STLINK_V3S_PID:
2720                         case STLINK_V3_2VCP_PID:
2721                                 h->version.stlink = 3;
2722                                 h->tx_ep = STLINK_V2_1_TX_EP;
2723                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2724                                 break;
2725                         case STLINK_V2_1_PID:
2726                         case STLINK_V2_1_NO_MSD_PID:
2727                                 h->version.stlink = 2;
2728                                 h->tx_ep = STLINK_V2_1_TX_EP;
2729                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2730                                 break;
2731                         default:
2732                         /* fall through - we assume V2 to be the default version*/
2733                         case STLINK_V2_PID:
2734                                 h->version.stlink = 2;
2735                                 h->tx_ep = STLINK_TX_EP;
2736                                 h->trace_ep = STLINK_TRACE_EP;
2737                                 break;
2738                 }
2739
2740                 /* get the device version */
2741                 err = stlink_usb_version(h);
2742
2743                 if (err == ERROR_OK) {
2744                         break;
2745                 } else if (h->version.stlink == 1 ||
2746                            retry_count == 0) {
2747                         LOG_ERROR("read version failed");
2748                         goto error_open;
2749                 } else {
2750                         err = jtag_libusb_release_interface(h->fd, 0);
2751                         if (err != ERROR_OK) {
2752                                 LOG_ERROR("release interface failed");
2753                                 goto error_open;
2754                         }
2755
2756                         err = jtag_libusb_reset_device(h->fd);
2757                         if (err != ERROR_OK) {
2758                                 LOG_ERROR("reset device failed");
2759                                 goto error_open;
2760                         }
2761
2762                         jtag_libusb_close(h->fd);
2763                         /*
2764                           Give the device one second to settle down and
2765                           reenumerate.
2766                          */
2767                         usleep(1 * 1000 * 1000);
2768                         retry_count--;
2769                 }
2770         } while (1);
2771
2772         /* check if mode is supported */
2773         err = ERROR_OK;
2774
2775         switch (h->transport) {
2776                 case HL_TRANSPORT_SWD:
2777                         if (h->version.jtag_api == STLINK_JTAG_API_V1)
2778                                 err = ERROR_FAIL;
2779                         /* fall-through */
2780                 case HL_TRANSPORT_JTAG:
2781                         if (h->version.jtag == 0)
2782                                 err = ERROR_FAIL;
2783                         break;
2784                 case HL_TRANSPORT_SWIM:
2785                         if (h->version.swim == 0)
2786                                 err = ERROR_FAIL;
2787                         break;
2788                 default:
2789                         err = ERROR_FAIL;
2790                         break;
2791         }
2792
2793         if (err != ERROR_OK) {
2794                 LOG_ERROR("mode (transport) not supported by device");
2795                 goto error_open;
2796         }
2797
2798         /* initialize the debug hardware */
2799         err = stlink_usb_init_mode(h, param->connect_under_reset);
2800
2801         if (err != ERROR_OK) {
2802                 LOG_ERROR("init mode failed (unable to connect to the target)");
2803                 goto error_open;
2804         }
2805
2806         if (h->transport == HL_TRANSPORT_SWIM) {
2807                 err = stlink_swim_enter(h);
2808                 if (err != ERROR_OK) {
2809                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
2810                         goto error_open;
2811                 }
2812                 *fd = h;
2813                 h->max_mem_packet = STLINK_DATA_SIZE;
2814                 return ERROR_OK;
2815         }
2816
2817         if (h->transport == HL_TRANSPORT_JTAG) {
2818                 if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
2819                         stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
2820                         stlink_speed(h, param->initial_interface_speed, false);
2821                 }
2822         } else if (h->transport == HL_TRANSPORT_SWD) {
2823                 if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
2824                         stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
2825                         stlink_speed(h, param->initial_interface_speed, false);
2826                 }
2827         }
2828
2829         if (h->version.jtag_api == STLINK_JTAG_API_V3) {
2830                 struct speed_map map[STLINK_V3_MAX_FREQ_NB];
2831
2832                 stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map);
2833                 stlink_dump_speed_map(map, ARRAY_SIZE(map));
2834                 stlink_speed(h, param->initial_interface_speed, false);
2835         }
2836
2837         /* get cpuid, so we can determine the max page size
2838          * start with a safe default */
2839         h->max_mem_packet = (1 << 10);
2840
2841         uint8_t buffer[4];
2842         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
2843         if (err == ERROR_OK) {
2844                 uint32_t cpuid = le_to_h_u32(buffer);
2845                 int i = (cpuid >> 4) & 0xf;
2846                 if (i == 4 || i == 3) {
2847                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
2848                         h->max_mem_packet = (1 << 12);
2849                 }
2850         }
2851
2852         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
2853
2854         *fd = h;
2855
2856         return ERROR_OK;
2857
2858 error_open:
2859         stlink_usb_close(h);
2860
2861         return ERROR_FAIL;
2862 }
2863
2864 int stlink_config_trace(void *handle, bool enabled, enum tpiu_pin_protocol pin_protocol,
2865                         uint32_t port_size, unsigned int *trace_freq)
2866 {
2867         struct stlink_usb_handle_s *h = handle;
2868
2869         if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
2870                         pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
2871                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
2872                 return ERROR_FAIL;
2873         }
2874
2875         if (!enabled) {
2876                 stlink_usb_trace_disable(h);
2877                 return ERROR_OK;
2878         }
2879
2880         if (*trace_freq > STLINK_TRACE_MAX_HZ) {
2881                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
2882                           STLINK_TRACE_MAX_HZ);
2883                 return ERROR_FAIL;
2884         }
2885
2886         stlink_usb_trace_disable(h);
2887
2888         if (!*trace_freq)
2889                 *trace_freq = STLINK_TRACE_MAX_HZ;
2890         h->trace.source_hz = *trace_freq;
2891
2892         return stlink_usb_trace_enable(h);
2893 }
2894
2895 /** */
2896 struct hl_layout_api_s stlink_usb_layout_api = {
2897         /** */
2898         .open = stlink_usb_open,
2899         /** */
2900         .close = stlink_usb_close,
2901         /** */
2902         .idcode = stlink_usb_idcode,
2903         /** */
2904         .state = stlink_usb_state,
2905         /** */
2906         .reset = stlink_usb_reset,
2907         /** */
2908         .assert_srst = stlink_usb_assert_srst,
2909         /** */
2910         .run = stlink_usb_run,
2911         /** */
2912         .halt = stlink_usb_halt,
2913         /** */
2914         .step = stlink_usb_step,
2915         /** */
2916         .read_regs = stlink_usb_read_regs,
2917         /** */
2918         .read_reg = stlink_usb_read_reg,
2919         /** */
2920         .write_reg = stlink_usb_write_reg,
2921         /** */
2922         .read_mem = stlink_usb_read_mem,
2923         /** */
2924         .write_mem = stlink_usb_write_mem,
2925         /** */
2926         .write_debug_reg = stlink_usb_write_debug_reg,
2927         /** */
2928         .override_target = stlink_usb_override_target,
2929         /** */
2930         .speed = stlink_speed,
2931         /** */
2932         .config_trace = stlink_config_trace,
2933         /** */
2934         .poll_trace = stlink_usb_trace_read,
2935 };