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

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