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

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