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