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