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