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