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