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