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

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