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