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

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