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

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