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