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