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
  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         /* TODO: no error checking yet on api V1 */
 429         if (h->jtag_api == STLINK_JTAG_API_V1)
 430                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
 431
 432         if (h->transport == HL_TRANSPORT_SWIM) {
 433                 switch (h->databuf[0]) {
 434                         case STLINK_SWIM_ERR_OK:
 435                                 return ERROR_OK;
 436                         case STLINK_SWIM_BUSY:
 437                                 return ERROR_WAIT;
 438                         default:
 439                                 LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 440                                 return ERROR_FAIL;
 441                 }
 442         }
 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("Verify error");
 461                         return ERROR_FAIL;
 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         if (connect_under_reset) {
 908                 /* if we are going to use swim we need to switch mode early */
 909                 if (emode == STLINK_MODE_DEBUG_SWIM) {
 910                         res = stlink_usb_mode_enter(handle, emode);
 911                         if (res != ERROR_OK)
 912                                 return res;
 913                 }
 914                 res = stlink_usb_assert_srst(handle, 0);
 915                 if (res != ERROR_OK)
 916                         return res;
 917         }
 918
 919         res = stlink_usb_mode_enter(handle, emode);
 920
 921         if (res != ERROR_OK)
 922                 return res;
 923
 924         res = stlink_usb_current_mode(handle, &mode);
 925
 926         if (res != ERROR_OK)
 927                 return res;
 928
 929         LOG_DEBUG("MODE: 0x%02X", mode);
 930
 931         return ERROR_OK;
 932 }
 933
 934 /* request status from last swim request */
 935 static int stlink_swim_status(void *handle)
 936 {
 937         struct stlink_usb_handle_s *h = handle;
 938         int res;
 939
 940         stlink_usb_init_buffer(handle, h->rx_ep, 4);
 941         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 942         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
 943         res = stlink_usb_xfer(handle, h->databuf, 4);
 944         if (res != ERROR_OK)
 945                 return res;
 946         return ERROR_OK;
 947 }
 948 /*
 949         the purpose of this function is unknown...
 950         capabilites? anyway for swim v6 it returns
 951         0001020600000000
 952 */
 953 __attribute__((unused))
 954 static int stlink_swim_cap(void *handle, uint8_t *cap)
 955 {
 956         struct stlink_usb_handle_s *h = handle;
 957         int res;
 958
 959         stlink_usb_init_buffer(handle, h->rx_ep, 8);
 960         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 961         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
 962         h->cmdbuf[h->cmdidx++] = 0x01;
 963         res = stlink_usb_xfer(handle, h->databuf, 8);
 964         if (res != ERROR_OK)
 965                 return res;
 966         memcpy(cap, h->databuf, 8);
 967         return ERROR_OK;
 968 }
 969
 970 /*      debug dongle assert/deassert sreset line */
 971 static int stlink_swim_assert_reset(void *handle, int reset)
 972 {
 973         struct stlink_usb_handle_s *h = handle;
 974         int res;
 975
 976         stlink_usb_init_buffer(handle, h->rx_ep, 0);
 977         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 978         if (!reset)
 979                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
 980         else
 981                 h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
 982         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
 983         if (res != ERROR_OK)
 984                 return res;
 985         return ERROR_OK;
 986 }
 987
 988 /*
 989         send swim enter seq
 990         1.3ms low then 750Hz then 1.5kHz
 991 */
 992 static int stlink_swim_enter(void *handle)
 993 {
 994         struct stlink_usb_handle_s *h = handle;
 995         int res;
 996
 997         stlink_usb_init_buffer(handle, h->rx_ep, 0);
 998         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 999         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
1000         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1001         if (res != ERROR_OK)
1002                 return res;
1003         return ERROR_OK;
1004 }
1005
1006 /*      switch high/low speed swim */
1007 static int stlink_swim_speed(void *handle, int speed)
1008 {
1009         struct stlink_usb_handle_s *h = handle;
1010         int res;
1011
1012         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1013         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1014         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
1015         if (speed)
1016                 h->cmdbuf[h->cmdidx++] = 1;
1017         else
1018                 h->cmdbuf[h->cmdidx++] = 0;
1019         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1020         if (res != ERROR_OK)
1021                 return res;
1022         return ERROR_OK;
1023 }
1024
1025 /*
1026         initiate srst from swim.
1027         nrst is pulled low for 50us.
1028 */
1029 static int stlink_swim_generate_rst(void *handle)
1030 {
1031         struct stlink_usb_handle_s *h = handle;
1032         int res;
1033
1034         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1035         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1036         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
1037         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1038         if (res != ERROR_OK)
1039                 return res;
1040         return ERROR_OK;
1041 }
1042
1043 /*
1044         send resyncronize sequence
1045         swim is pulled low for 16us
1046         reply is 64 clks low
1047 */
1048 static int stlink_swim_resync(void *handle)
1049 {
1050         struct stlink_usb_handle_s *h = handle;
1051         int res;
1052
1053         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1054         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1055         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
1056         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1057         if (res != ERROR_OK)
1058                 return res;
1059         return ERROR_OK;
1060 }
1061
1062 static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
1063 {
1064         struct stlink_usb_handle_s *h = handle;
1065         int res;
1066         unsigned int i;
1067         unsigned int datalen = 0;
1068         int cmdsize = STLINK_CMD_SIZE_V2;
1069
1070         if (len > STLINK_DATA_SIZE)
1071                 return ERROR_FAIL;
1072
1073         if (h->version.stlink == 1)
1074                 cmdsize = STLINK_SG_SIZE;
1075
1076         stlink_usb_init_buffer(handle, h->tx_ep, 0);
1077         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1078         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
1079         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1080         h->cmdidx += 2;
1081         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1082         h->cmdidx += 4;
1083         for (i = 0; i < len; i++) {
1084                 if (h->cmdidx == cmdsize)
1085                         h->databuf[datalen++] = *(data++);
1086                 else
1087                         h->cmdbuf[h->cmdidx++] = *(data++);
1088         }
1089         if (h->version.stlink == 1)
1090                 stlink_usb_set_cbw_transfer_datalength(handle, datalen);
1091
1092         res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
1093         if (res != ERROR_OK)
1094                 return res;
1095         return ERROR_OK;
1096 }
1097
1098 static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
1099 {
1100         struct stlink_usb_handle_s *h = handle;
1101         int res;
1102
1103         if (len > STLINK_DATA_SIZE)
1104                 return ERROR_FAIL;
1105
1106         stlink_usb_init_buffer(handle, h->rx_ep, 0);
1107         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1108         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
1109         h_u16_to_be(h->cmdbuf+h->cmdidx, len);
1110         h->cmdidx += 2;
1111         h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
1112         h->cmdidx += 4;
1113         res = stlink_cmd_allow_retry(handle, h->databuf, 0);
1114         if (res != ERROR_OK)
1115                 return res;
1116
1117         stlink_usb_init_buffer(handle, h->rx_ep, len);
1118         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
1119         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
1120         res = stlink_usb_xfer(handle, data, len);
1121         if (res != ERROR_OK)
1122                 return res;
1123
1124         return ERROR_OK;
1125 }
1126
1127 /** */
1128 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
1129 {
1130         int res;
1131         struct stlink_usb_handle_s *h = handle;
1132
1133         assert(handle != NULL);
1134
1135         /* there is no swim read core id cmd */
1136         if (h->transport == HL_TRANSPORT_SWIM) {
1137                 *idcode = 0;
1138                 return ERROR_OK;
1139         }
1140
1141         stlink_usb_init_buffer(handle, h->rx_ep, 4);
1142
1143         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1144         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
1145
1146         res = stlink_usb_xfer(handle, h->databuf, 4);
1147
1148         if (res != ERROR_OK)
1149                 return res;
1150
1151         *idcode = le_to_h_u32(h->databuf);
1152
1153         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
1154
1155         return ERROR_OK;
1156 }
1157
1158 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
1159 {
1160         struct stlink_usb_handle_s *h = handle;
1161         int res;
1162
1163         assert(handle != NULL);
1164
1165         stlink_usb_init_buffer(handle, h->rx_ep, 8);
1166
1167         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1168         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
1169         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1170         h->cmdidx += 4;
1171
1172         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1173         if (res != ERROR_OK)
1174                 return res;
1175
1176         *val = le_to_h_u32(h->databuf + 4);
1177         return ERROR_OK;
1178 }
1179
1180 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
1181 {
1182         struct stlink_usb_handle_s *h = handle;
1183
1184         assert(handle != NULL);
1185
1186         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1187
1188         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1189         if (h->jtag_api == STLINK_JTAG_API_V1)
1190                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
1191         else
1192                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
1193         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1194         h->cmdidx += 4;
1195         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1196         h->cmdidx += 4;
1197
1198         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1199 }
1200
1201 /** */
1202 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
1203 {
1204         struct stlink_usb_handle_s *h = handle;
1205
1206         assert(handle != NULL);
1207
1208         if (h->trace.enabled && h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
1209                 int res;
1210
1211                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1212
1213                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1214                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
1215
1216                 res = stlink_usb_xfer(handle, h->databuf, 2);
1217                 if (res != ERROR_OK)
1218                         return res;
1219
1220                 size_t bytes_avail = le_to_h_u16(h->databuf);
1221                 *size = bytes_avail < *size ? bytes_avail : *size - 1;
1222
1223                 if (*size > 0) {
1224                         res = stlink_usb_read_trace(handle, buf, *size);
1225                         if (res != ERROR_OK)
1226                                 return res;
1227                         return ERROR_OK;
1228                 }
1229         }
1230         *size = 0;
1231         return ERROR_OK;
1232 }
1233
1234 static enum target_state stlink_usb_v2_get_status(void *handle)
1235 {
1236         int result;
1237         uint32_t status;
1238
1239         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
1240         if  (result != ERROR_OK)
1241                 return TARGET_UNKNOWN;
1242
1243         if (status & S_HALT)
1244                 return TARGET_HALTED;
1245         else if (status & S_RESET_ST)
1246                 return TARGET_RESET;
1247
1248         return TARGET_RUNNING;
1249 }
1250
1251 /** */
1252 static enum target_state stlink_usb_state(void *handle)
1253 {
1254         int res;
1255         struct stlink_usb_handle_s *h = handle;
1256
1257         assert(handle != NULL);
1258
1259         if (h->transport == HL_TRANSPORT_SWIM) {
1260                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1261                 if (res != ERROR_OK)
1262                         return TARGET_UNKNOWN;
1263
1264                 res = stlink_swim_resync(handle);
1265                 if (res != ERROR_OK)
1266                         return TARGET_UNKNOWN;
1267
1268                 return ERROR_OK;
1269         }
1270
1271         if (h->reconnect_pending) {
1272                 LOG_INFO("Previous state query failed, trying to reconnect");
1273                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
1274
1275                 if (res != ERROR_OK)
1276                         return TARGET_UNKNOWN;
1277
1278                 h->reconnect_pending = false;
1279         }
1280
1281         if (h->jtag_api == STLINK_JTAG_API_V2) {
1282                 res = stlink_usb_v2_get_status(handle);
1283                 if (res == TARGET_UNKNOWN)
1284                         h->reconnect_pending = true;
1285                 return res;
1286         }
1287
1288         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1289
1290         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1291         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
1292
1293         res = stlink_usb_xfer(handle, h->databuf, 2);
1294
1295         if (res != ERROR_OK)
1296                 return TARGET_UNKNOWN;
1297
1298         if (h->databuf[0] == STLINK_CORE_RUNNING)
1299                 return TARGET_RUNNING;
1300         if (h->databuf[0] == STLINK_CORE_HALTED)
1301                 return TARGET_HALTED;
1302
1303         h->reconnect_pending = true;
1304
1305         return TARGET_UNKNOWN;
1306 }
1307
1308 static int stlink_usb_assert_srst(void *handle, int srst)
1309 {
1310         struct stlink_usb_handle_s *h = handle;
1311
1312         assert(handle != NULL);
1313
1314         if (h->transport == HL_TRANSPORT_SWIM)
1315                 return stlink_swim_assert_reset(handle, srst);
1316
1317         if (h->version.stlink == 1)
1318                 return ERROR_COMMAND_NOTFOUND;
1319
1320         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1321
1322         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1323         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
1324         h->cmdbuf[h->cmdidx++] = srst;
1325
1326         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1327 }
1328
1329 /** */
1330 static void stlink_usb_trace_disable(void *handle)
1331 {
1332         int res = ERROR_OK;
1333         struct stlink_usb_handle_s *h = handle;
1334
1335         assert(handle != NULL);
1336
1337         assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
1338
1339         LOG_DEBUG("Tracing: disable");
1340
1341         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1342         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1343         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1344         res = stlink_usb_xfer(handle, h->databuf, 2);
1345
1346         if (res == ERROR_OK)
1347                 h->trace.enabled = false;
1348 }
1349
1350
1351 /** */
1352 static int stlink_usb_trace_enable(void *handle)
1353 {
1354         int res;
1355         struct stlink_usb_handle_s *h = handle;
1356
1357         assert(handle != NULL);
1358
1359         if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
1360                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1361
1362                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1363                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1364                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1365                 h->cmdidx += 2;
1366                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
1367                 h->cmdidx += 4;
1368
1369                 res = stlink_usb_xfer(handle, h->databuf, 2);
1370
1371                 if (res == ERROR_OK)  {
1372                         h->trace.enabled = true;
1373                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
1374                 }
1375         } else {
1376                 LOG_ERROR("Tracing is not supported by this version.");
1377                 res = ERROR_FAIL;
1378         }
1379
1380         return res;
1381 }
1382
1383 /** */
1384 static int stlink_usb_reset(void *handle)
1385 {
1386         struct stlink_usb_handle_s *h = handle;
1387         int retval;
1388
1389         assert(handle != NULL);
1390
1391         if (h->transport == HL_TRANSPORT_SWIM)
1392                 return stlink_swim_generate_rst(handle);
1393
1394         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1395
1396         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1397
1398         if (h->jtag_api == STLINK_JTAG_API_V1)
1399                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
1400         else
1401                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
1402
1403         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
1404         if (retval != ERROR_OK)
1405                 return retval;
1406
1407         if (h->trace.enabled) {
1408                 stlink_usb_trace_disable(h);
1409                 return stlink_usb_trace_enable(h);
1410         }
1411
1412         return ERROR_OK;
1413 }
1414
1415 /** */
1416 static int stlink_usb_run(void *handle)
1417 {
1418         int res;
1419         struct stlink_usb_handle_s *h = handle;
1420
1421         assert(handle != NULL);
1422
1423         if (h->jtag_api == STLINK_JTAG_API_V2) {
1424                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1425
1426                 return res;
1427         }
1428
1429         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1430
1431         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1432         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1433
1434         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1435 }
1436
1437 /** */
1438 static int stlink_usb_halt(void *handle)
1439 {
1440         int res;
1441         struct stlink_usb_handle_s *h = handle;
1442
1443         assert(handle != NULL);
1444
1445         if (h->jtag_api == STLINK_JTAG_API_V2) {
1446                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1447
1448                 return res;
1449         }
1450
1451         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1452
1453         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1454         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1455
1456         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1457 }
1458
1459 /** */
1460 static int stlink_usb_step(void *handle)
1461 {
1462         struct stlink_usb_handle_s *h = handle;
1463
1464         assert(handle != NULL);
1465
1466         if (h->jtag_api == STLINK_JTAG_API_V2) {
1467                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1468                  * that the Cortex-M3 currently does. */
1469                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1470                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1471                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1472         }
1473
1474         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1475
1476         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1477         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1478
1479         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1480 }
1481
1482 /** */
1483 static int stlink_usb_read_regs(void *handle)
1484 {
1485         int res;
1486         struct stlink_usb_handle_s *h = handle;
1487
1488         assert(handle != NULL);
1489
1490         stlink_usb_init_buffer(handle, h->rx_ep, 84);
1491
1492         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1493         if (h->jtag_api == STLINK_JTAG_API_V1)
1494                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
1495         else
1496                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
1497
1498         res = stlink_usb_xfer(handle, h->databuf, 84);
1499
1500         if (res != ERROR_OK)
1501                 return res;
1502
1503         return ERROR_OK;
1504 }
1505
1506 /** */
1507 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
1508 {
1509         int res;
1510         struct stlink_usb_handle_s *h = handle;
1511
1512         assert(handle != NULL);
1513
1514         stlink_usb_init_buffer(handle, h->rx_ep, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1515
1516         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1517         if (h->jtag_api == STLINK_JTAG_API_V1)
1518                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
1519         else
1520                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
1521         h->cmdbuf[h->cmdidx++] = num;
1522
1523         if (h->jtag_api == STLINK_JTAG_API_V1) {
1524                 res = stlink_usb_xfer(handle, h->databuf, 4);
1525                 if (res != ERROR_OK)
1526                         return res;
1527                 *val = le_to_h_u32(h->databuf);
1528                 return ERROR_OK;
1529         } else {
1530                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1531                 if (res != ERROR_OK)
1532                         return res;
1533                 *val = le_to_h_u32(h->databuf + 4);
1534                 return ERROR_OK;
1535         }
1536 }
1537
1538 /** */
1539 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
1540 {
1541         struct stlink_usb_handle_s *h = handle;
1542
1543         assert(handle != NULL);
1544
1545         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1546
1547         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1548         if (h->jtag_api == STLINK_JTAG_API_V1)
1549                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
1550         else
1551                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
1552         h->cmdbuf[h->cmdidx++] = num;
1553         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1554         h->cmdidx += 4;
1555
1556         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1557 }
1558
1559 static int stlink_usb_get_rw_status(void *handle)
1560 {
1561         int res;
1562         struct stlink_usb_handle_s *h = handle;
1563
1564         assert(handle != NULL);
1565
1566         if (h->jtag_api == STLINK_JTAG_API_V1)
1567                 return ERROR_OK;
1568
1569         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1570
1571         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1572         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
1573
1574         res = stlink_usb_xfer(handle, h->databuf, 2);
1575
1576         if (res != ERROR_OK)
1577                 return res;
1578
1579         return stlink_usb_error_check(h);
1580 }
1581
1582 /** */
1583 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1584                           uint8_t *buffer)
1585 {
1586         int res;
1587         uint16_t read_len = len;
1588         struct stlink_usb_handle_s *h = handle;
1589
1590         assert(handle != NULL);
1591
1592         /* max 8bit read/write is 64bytes */
1593         if (len > STLINK_MAX_RW8) {
1594                 LOG_DEBUG("max buffer length exceeded");
1595                 return ERROR_FAIL;
1596         }
1597
1598         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
1599
1600         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1601         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
1602         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1603         h->cmdidx += 4;
1604         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1605         h->cmdidx += 2;
1606
1607         /* we need to fix read length for single bytes */
1608         if (read_len == 1)
1609                 read_len++;
1610
1611         res = stlink_usb_xfer(handle, h->databuf, read_len);
1612
1613         if (res != ERROR_OK)
1614                 return res;
1615
1616         memcpy(buffer, h->databuf, len);
1617
1618         return stlink_usb_get_rw_status(handle);
1619 }
1620
1621 /** */
1622 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1623                            const uint8_t *buffer)
1624 {
1625         int res;
1626         struct stlink_usb_handle_s *h = handle;
1627
1628         assert(handle != NULL);
1629
1630         /* max 8bit read/write is 64bytes */
1631         if (len > STLINK_MAX_RW8) {
1632                 LOG_DEBUG("max buffer length exceeded");
1633                 return ERROR_FAIL;
1634         }
1635
1636         stlink_usb_init_buffer(handle, h->tx_ep, len);
1637
1638         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1639         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1640         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1641         h->cmdidx += 4;
1642         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1643         h->cmdidx += 2;
1644
1645         res = stlink_usb_xfer(handle, buffer, len);
1646
1647         if (res != ERROR_OK)
1648                 return res;
1649
1650         return stlink_usb_get_rw_status(handle);
1651 }
1652
1653 /** */
1654 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1655                           uint8_t *buffer)
1656 {
1657         int res;
1658         struct stlink_usb_handle_s *h = handle;
1659
1660         assert(handle != NULL);
1661
1662         /* data must be a multiple of 4 and word aligned */
1663         if (len % 4 || addr % 4) {
1664                 LOG_DEBUG("Invalid data alignment");
1665                 return ERROR_TARGET_UNALIGNED_ACCESS;
1666         }
1667
1668         stlink_usb_init_buffer(handle, h->rx_ep, len);
1669
1670         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1671         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1672         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1673         h->cmdidx += 4;
1674         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1675         h->cmdidx += 2;
1676
1677         res = stlink_usb_xfer(handle, h->databuf, len);
1678
1679         if (res != ERROR_OK)
1680                 return res;
1681
1682         memcpy(buffer, h->databuf, len);
1683
1684         return stlink_usb_get_rw_status(handle);
1685 }
1686
1687 /** */
1688 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1689                            const uint8_t *buffer)
1690 {
1691         int res;
1692         struct stlink_usb_handle_s *h = handle;
1693
1694         assert(handle != NULL);
1695
1696         /* data must be a multiple of 4 and word aligned */
1697         if (len % 4 || addr % 4) {
1698                 LOG_DEBUG("Invalid data alignment");
1699                 return ERROR_TARGET_UNALIGNED_ACCESS;
1700         }
1701
1702         stlink_usb_init_buffer(handle, h->tx_ep, len);
1703
1704         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1705         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1706         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1707         h->cmdidx += 4;
1708         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1709         h->cmdidx += 2;
1710
1711         res = stlink_usb_xfer(handle, buffer, len);
1712
1713         if (res != ERROR_OK)
1714                 return res;
1715
1716         return stlink_usb_get_rw_status(handle);
1717 }
1718
1719 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
1720 {
1721         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
1722         if (max_tar_block == 0)
1723                 max_tar_block = 4;
1724         return max_tar_block;
1725 }
1726
1727 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
1728                 uint32_t count, uint8_t *buffer)
1729 {
1730         int retval = ERROR_OK;
1731         uint32_t bytes_remaining;
1732         int retries = 0;
1733         struct stlink_usb_handle_s *h = handle;
1734
1735         /* calculate byte count */
1736         count *= size;
1737
1738         while (count) {
1739
1740                 bytes_remaining = (size == 4) ? \
1741                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1742
1743                 if (count < bytes_remaining)
1744                         bytes_remaining = count;
1745
1746                 if (h->transport == HL_TRANSPORT_SWIM) {
1747                         retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
1748                         if (retval != ERROR_OK)
1749                                 return retval;
1750                 } else
1751                 /* the stlink only supports 8/32bit memory read/writes
1752                  * honour 32bit, all others will be handled as 8bit access */
1753                 if (size == 4) {
1754
1755                         /* When in jtag mode the stlink uses the auto-increment functinality.
1756                          * However it expects us to pass the data correctly, this includes
1757                          * alignment and any page boundaries. We already do this as part of the
1758                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1759                          * needs implementiong manually.
1760                          * currently this only affects jtag mode, according to ST they do single
1761                          * access in SWD mode - but this may change and so we do it for both modes */
1762
1763                         /* we first need to check for any unaligned bytes */
1764                         if (addr % 4) {
1765
1766                                 uint32_t head_bytes = 4 - (addr % 4);
1767                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
1768                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1769                                         usleep((1<<retries++) * 1000);
1770                                         continue;
1771                                 }
1772                                 if (retval != ERROR_OK)
1773                                         return retval;
1774                                 buffer += head_bytes;
1775                                 addr += head_bytes;
1776                                 count -= head_bytes;
1777                                 bytes_remaining -= head_bytes;
1778                         }
1779
1780                         if (bytes_remaining % 4)
1781                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
1782                         else
1783                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
1784                 } else
1785                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
1786
1787                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1788                         usleep((1<<retries++) * 1000);
1789                         continue;
1790                 }
1791                 if (retval != ERROR_OK)
1792                         return retval;
1793
1794                 buffer += bytes_remaining;
1795                 addr += bytes_remaining;
1796                 count -= bytes_remaining;
1797         }
1798
1799         return retval;
1800 }
1801
1802 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
1803                 uint32_t count, const uint8_t *buffer)
1804 {
1805         int retval = ERROR_OK;
1806         uint32_t bytes_remaining;
1807         int retries = 0;
1808         struct stlink_usb_handle_s *h = handle;
1809
1810         /* calculate byte count */
1811         count *= size;
1812
1813         while (count) {
1814
1815                 bytes_remaining = (size == 4) ? \
1816                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1817
1818                 if (count < bytes_remaining)
1819                         bytes_remaining = count;
1820
1821                 if (h->transport == HL_TRANSPORT_SWIM) {
1822                         retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
1823                         if (retval != ERROR_OK)
1824                                 return retval;
1825                 } else
1826                 /* the stlink only supports 8/32bit memory read/writes
1827                  * honour 32bit, all others will be handled as 8bit access */
1828                 if (size == 4) {
1829
1830                         /* When in jtag mode the stlink uses the auto-increment functinality.
1831                          * However it expects us to pass the data correctly, this includes
1832                          * alignment and any page boundaries. We already do this as part of the
1833                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1834                          * needs implementiong manually.
1835                          * currently this only affects jtag mode, according to ST they do single
1836                          * access in SWD mode - but this may change and so we do it for both modes */
1837
1838                         /* we first need to check for any unaligned bytes */
1839                         if (addr % 4) {
1840
1841                                 uint32_t head_bytes = 4 - (addr % 4);
1842                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
1843                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1844                                         usleep((1<<retries++) * 1000);
1845                                         continue;
1846                                 }
1847                                 if (retval != ERROR_OK)
1848                                         return retval;
1849                                 buffer += head_bytes;
1850                                 addr += head_bytes;
1851                                 count -= head_bytes;
1852                                 bytes_remaining -= head_bytes;
1853                         }
1854
1855                         if (bytes_remaining % 4)
1856                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
1857                         else
1858                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
1859
1860                 } else
1861                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
1862                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1863                         usleep((1<<retries++) * 1000);
1864                         continue;
1865                 }
1866                 if (retval != ERROR_OK)
1867                         return retval;
1868
1869                 buffer += bytes_remaining;
1870                 addr += bytes_remaining;
1871                 count -= bytes_remaining;
1872         }
1873
1874         return retval;
1875 }
1876
1877 /** */
1878 static int stlink_usb_override_target(const char *targetname)
1879 {
1880         return !strcmp(targetname, "cortex_m");
1881 }
1882
1883 static int stlink_speed(void *handle, int khz, bool query)
1884 {
1885         unsigned i;
1886         int speed_index = -1;
1887         int speed_diff = INT_MAX;
1888         struct stlink_usb_handle_s *h = handle;
1889
1890         if (h && (h->transport == HL_TRANSPORT_SWIM)) {
1891                 /*
1892                         we dont care what the khz rate is
1893                         we only have low and high speed...
1894                         before changing speed the SWIM_CSR HS bit
1895                         must be updated
1896                  */
1897                 if (khz == 0)
1898                         stlink_swim_speed(handle, 0);
1899                 else
1900                         stlink_swim_speed(handle, 1);
1901                 return khz;
1902         }
1903
1904         /* only supported by stlink/v2 and for firmware >= 22 */
1905         if (h && (h->version.stlink == 1 || h->version.jtag < 22))
1906                 return khz;
1907
1908         for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) {
1909                 if (khz == stlink_khz_to_speed_map[i].speed) {
1910                         speed_index = i;
1911                         break;
1912                 } else {
1913                         int current_diff = khz - stlink_khz_to_speed_map[i].speed;
1914                         /* get abs value for comparison */
1915                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
1916                         if ((current_diff < speed_diff) && khz >= stlink_khz_to_speed_map[i].speed) {
1917                                 speed_diff = current_diff;
1918                                 speed_index = i;
1919                         }
1920                 }
1921         }
1922
1923         bool match = true;
1924
1925         if (speed_index == -1) {
1926                 /* this will only be here if we cannot match the slow speed.
1927                  * use the slowest speed we support.*/
1928                 speed_index = ARRAY_SIZE(stlink_khz_to_speed_map) - 1;
1929                 match = false;
1930         } else if (i == ARRAY_SIZE(stlink_khz_to_speed_map))
1931                 match = false;
1932
1933         if (!match && query) {
1934                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
1935                                 khz, stlink_khz_to_speed_map[speed_index].speed);
1936         }
1937
1938         if (h && !query) {
1939                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor);
1940                 if (result != ERROR_OK) {
1941                         LOG_ERROR("Unable to set adapter speed");
1942                         return khz;
1943                 }
1944         }
1945
1946         return stlink_khz_to_speed_map[speed_index].speed;
1947 }
1948
1949 /** */
1950 static int stlink_usb_close(void *handle)
1951 {
1952         struct stlink_usb_handle_s *h = handle;
1953
1954         if (h && h->fd)
1955                 jtag_libusb_close(h->fd);
1956
1957         free(h);
1958
1959         return ERROR_OK;
1960 }
1961
1962 /** */
1963 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
1964 {
1965         int err, retry_count = 1;
1966         struct stlink_usb_handle_s *h;
1967         enum stlink_jtag_api_version api;
1968
1969         LOG_DEBUG("stlink_usb_open");
1970
1971         h = calloc(1, sizeof(struct stlink_usb_handle_s));
1972
1973         if (h == 0) {
1974                 LOG_DEBUG("malloc failed");
1975                 return ERROR_FAIL;
1976         }
1977
1978         h->transport = param->transport;
1979
1980         for (unsigned i = 0; param->vid[i]; i++) {
1981                 LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
1982                           param->transport, param->vid[i], param->pid[i],
1983                           param->serial ? param->serial : "");
1984         }
1985
1986         /*
1987           On certain host USB configurations(e.g. MacBook Air)
1988           STLINKv2 dongle seems to have its FW in a funky state if,
1989           after plugging it in, you try to use openocd with it more
1990           then once (by launching and closing openocd). In cases like
1991           that initial attempt to read the FW info via
1992           stlink_usb_version will fail and the device has to be reset
1993           in order to become operational.
1994          */
1995         do {
1996                 if (jtag_libusb_open(param->vid, param->pid, param->serial, &h->fd) != ERROR_OK) {
1997                         LOG_ERROR("open failed");
1998                         goto error_open;
1999                 }
2000
2001                 jtag_libusb_set_configuration(h->fd, 0);
2002
2003                 if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
2004                         LOG_DEBUG("claim interface failed");
2005                         goto error_open;
2006                 }
2007
2008                 /* RX EP is common for all versions */
2009                 h->rx_ep = STLINK_RX_EP;
2010
2011                 uint16_t pid;
2012                 if (jtag_libusb_get_pid(jtag_libusb_get_device(h->fd), &pid) != ERROR_OK) {
2013                         LOG_DEBUG("libusb_get_pid failed");
2014                         goto error_open;
2015                 }
2016
2017                 /* wrap version for first read */
2018                 switch (pid) {
2019                         case STLINK_V1_PID:
2020                                 h->version.stlink = 1;
2021                                 h->tx_ep = STLINK_TX_EP;
2022                                 h->trace_ep = STLINK_TRACE_EP;
2023                                 break;
2024                         case STLINK_V2_1_PID:
2025                                 h->version.stlink = 2;
2026                                 h->tx_ep = STLINK_V2_1_TX_EP;
2027                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
2028                                 break;
2029                         default:
2030                         /* fall through - we assume V2 to be the default version*/
2031                         case STLINK_V2_PID:
2032                                 h->version.stlink = 2;
2033                                 h->tx_ep = STLINK_TX_EP;
2034                                 h->trace_ep = STLINK_TRACE_EP;
2035                                 break;
2036                 }
2037
2038                 /* get the device version */
2039                 err = stlink_usb_version(h);
2040
2041                 if (err == ERROR_OK) {
2042                         break;
2043                 } else if (h->version.stlink == 1 ||
2044                            retry_count == 0) {
2045                         LOG_ERROR("read version failed");
2046                         goto error_open;
2047                 } else {
2048                         err = jtag_libusb_release_interface(h->fd, 0);
2049                         if (err != ERROR_OK) {
2050                                 LOG_ERROR("release interface failed");
2051                                 goto error_open;
2052                         }
2053
2054                         err = jtag_libusb_reset_device(h->fd);
2055                         if (err != ERROR_OK) {
2056                                 LOG_ERROR("reset device failed");
2057                                 goto error_open;
2058                         }
2059
2060                         jtag_libusb_close(h->fd);
2061                         /*
2062                           Give the device one second to settle down and
2063                           reenumerate.
2064                          */
2065                         usleep(1 * 1000 * 1000);
2066                         retry_count--;
2067                 }
2068         } while (1);
2069
2070         /* check if mode is supported */
2071         err = ERROR_OK;
2072
2073         switch (h->transport) {
2074                 case HL_TRANSPORT_SWD:
2075                 case HL_TRANSPORT_JTAG:
2076                         if (h->version.jtag == 0)
2077                                 err = ERROR_FAIL;
2078                         break;
2079                 case HL_TRANSPORT_SWIM:
2080                         if (h->version.swim == 0)
2081                                 err = ERROR_FAIL;
2082                         break;
2083                 default:
2084                         err = ERROR_FAIL;
2085                         break;
2086         }
2087
2088         if (err != ERROR_OK) {
2089                 LOG_ERROR("mode (transport) not supported by device");
2090                 goto error_open;
2091         }
2092
2093         api = h->version.jtag_api_max;
2094
2095         LOG_INFO("using stlink api v%d", api);
2096
2097         /* set the used jtag api, this will default to the newest supported version */
2098         h->jtag_api = api;
2099
2100         /* initialize the debug hardware */
2101         err = stlink_usb_init_mode(h, param->connect_under_reset);
2102
2103         if (err != ERROR_OK) {
2104                 LOG_ERROR("init mode failed (unable to connect to the target)");
2105                 goto error_open;
2106         }
2107
2108         if (h->transport == HL_TRANSPORT_SWIM) {
2109                 err = stlink_swim_enter(h);
2110                 if (err != ERROR_OK) {
2111                         LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
2112                         goto error_open;
2113                 }
2114                 *fd = h;
2115                 h->max_mem_packet = STLINK_DATA_SIZE;
2116                 return ERROR_OK;
2117         }
2118
2119         /* clock speed only supported by stlink/v2 and for firmware >= 22 */
2120         if (h->version.stlink >= 2 && h->version.jtag >= 22) {
2121                 LOG_DEBUG("Supported clock speeds are:");
2122
2123                 for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++)
2124                         LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed);
2125
2126                 stlink_speed(h, param->initial_interface_speed, false);
2127         }
2128
2129         /* get cpuid, so we can determine the max page size
2130          * start with a safe default */
2131         h->max_mem_packet = (1 << 10);
2132
2133         uint8_t buffer[4];
2134         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
2135         if (err == ERROR_OK) {
2136                 uint32_t cpuid = le_to_h_u32(buffer);
2137                 int i = (cpuid >> 4) & 0xf;
2138                 if (i == 4 || i == 3) {
2139                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
2140                         h->max_mem_packet = (1 << 12);
2141                 }
2142         }
2143
2144         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
2145
2146         *fd = h;
2147
2148         return ERROR_OK;
2149
2150 error_open:
2151         stlink_usb_close(h);
2152
2153         return ERROR_FAIL;
2154 }
2155
2156 int stlink_config_trace(void *handle, bool enabled, enum tpio_pin_protocol pin_protocol,
2157                         uint32_t port_size, unsigned int *trace_freq)
2158 {
2159         struct stlink_usb_handle_s *h = handle;
2160
2161         if (enabled && (h->jtag_api < 2 || pin_protocol != ASYNC_UART)) {
2162                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
2163                 return ERROR_FAIL;
2164         }
2165
2166         if (!enabled) {
2167                 stlink_usb_trace_disable(h);
2168                 return ERROR_OK;
2169         }
2170
2171         if (*trace_freq > STLINK_TRACE_MAX_HZ) {
2172                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
2173                           STLINK_TRACE_MAX_HZ);
2174                 return ERROR_FAIL;
2175         }
2176
2177         stlink_usb_trace_disable(h);
2178
2179         if (!*trace_freq)
2180                 *trace_freq = STLINK_TRACE_MAX_HZ;
2181         h->trace.source_hz = *trace_freq;
2182
2183         return stlink_usb_trace_enable(h);
2184 }
2185
2186 /** */
2187 struct hl_layout_api_s stlink_usb_layout_api = {
2188         /** */
2189         .open = stlink_usb_open,
2190         /** */
2191         .close = stlink_usb_close,
2192         /** */
2193         .idcode = stlink_usb_idcode,
2194         /** */
2195         .state = stlink_usb_state,
2196         /** */
2197         .reset = stlink_usb_reset,
2198         /** */
2199         .assert_srst = stlink_usb_assert_srst,
2200         /** */
2201         .run = stlink_usb_run,
2202         /** */
2203         .halt = stlink_usb_halt,
2204         /** */
2205         .step = stlink_usb_step,
2206         /** */
2207         .read_regs = stlink_usb_read_regs,
2208         /** */
2209         .read_reg = stlink_usb_read_reg,
2210         /** */
2211         .write_reg = stlink_usb_write_reg,
2212         /** */
2213         .read_mem = stlink_usb_read_mem,
2214         /** */
2215         .write_mem = stlink_usb_write_mem,
2216         /** */
2217         .write_debug_reg = stlink_usb_write_debug_reg,
2218         /** */
2219         .override_target = stlink_usb_override_target,
2220         /** */
2221         .speed = stlink_speed,
2222         /** */
2223         .config_trace = stlink_config_trace,
2224         /** */
2225         .poll_trace = stlink_usb_trace_read,
2226 };