git.sur5r.net Git - openocd/blob - src/jtag/drivers/stlink_usb.c

   1 /***************************************************************************
   2  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
   3  *   Mathias Kuester <kesmtp@freenet.de>                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2012 by Spencer Oliver                                  *
   6  *   spen@spen-soft.co.uk                                                  *
   7  *                                                                         *
   8  *   This code is based on https://github.com/texane/stlink                *
   9  *                                                                         *
  10  *   This program is free software; you can redistribute it and/or modify  *
  11  *   it under the terms of the GNU General Public License as published by  *
  12  *   the Free Software Foundation; either version 2 of the License, or     *
  13  *   (at your option) any later version.                                   *
  14  *                                                                         *
  15  *   This program is distributed in the hope that it will be useful,       *
  16  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  17  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  18  *   GNU General Public License for more details.                          *
  19  *                                                                         *
  20  *   You should have received a copy of the GNU General Public License     *
  21  *   along with this program; if not, write to the                         *
  22  *   Free Software Foundation, Inc.,                                       *
  23  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  24  ***************************************************************************/
  25
  26 #ifdef HAVE_CONFIG_H
  27 #include "config.h"
  28 #endif
  29
  30 /* project specific includes */
  31 #include <helper/binarybuffer.h>
  32 #include <jtag/interface.h>
  33 #include <jtag/hla/hla_layout.h>
  34 #include <jtag/hla/hla_transport.h>
  35 #include <jtag/hla/hla_interface.h>
  36 #include <target/target.h>
  37
  38 #include <target/cortex_m.h>
  39
  40 #include "libusb_common.h"
  41
  42 #define ENDPOINT_IN  0x80
  43 #define ENDPOINT_OUT 0x00
  44
  45 #define STLINK_WRITE_TIMEOUT 1000
  46 #define STLINK_READ_TIMEOUT 1000
  47
  48 #define STLINK_NULL_EP        0
  49 #define STLINK_RX_EP          (1|ENDPOINT_IN)
  50 #define STLINK_TX_EP          (2|ENDPOINT_OUT)
  51 #define STLINK_TRACE_EP       (3|ENDPOINT_IN)
  52
  53 #define STLINK_V2_1_TX_EP     (1|ENDPOINT_OUT)
  54 #define STLINK_V2_1_TRACE_EP  (2|ENDPOINT_IN)
  55
  56 #define STLINK_SG_SIZE        (31)
  57 #define STLINK_DATA_SIZE      (4096)
  58 #define STLINK_CMD_SIZE_V2    (16)
  59 #define STLINK_CMD_SIZE_V1    (10)
  60
  61 #define STLINK_V1_PID         (0x3744)
  62 #define STLINK_V2_PID         (0x3748)
  63 #define STLINK_V2_1_PID       (0x374B)
  64
  65 /* the current implementation of the stlink limits
  66  * 8bit read/writes to max 64 bytes. */
  67 #define STLINK_MAX_RW8          (64)
  68
  69 /* "WAIT" responses will be retried (with exponential backoff) at
  70  * most this many times before failing to caller.
  71  */
  72 #define MAX_WAIT_RETRIES 8
  73
  74 enum stlink_jtag_api_version {
  75         STLINK_JTAG_API_V1 = 1,
  76         STLINK_JTAG_API_V2,
  77 };
  78
  79 /** */
  80 struct stlink_usb_version {
  81         /** */
  82         int stlink;
  83         /** */
  84         int jtag;
  85         /** */
  86         int swim;
  87         /** highest supported jtag api version */
  88         enum stlink_jtag_api_version jtag_api_max;
  89 };
  90
  91 /** */
  92 struct stlink_usb_handle_s {
  93         /** */
  94         struct jtag_libusb_device_handle *fd;
  95         /** */
  96         struct libusb_transfer *trans;
  97         /** */
  98         uint8_t rx_ep;
  99         /** */
 100         uint8_t tx_ep;
 101         /** */
 102         uint8_t trace_ep;
 103         /** */
 104         uint8_t cmdbuf[STLINK_SG_SIZE];
 105         /** */
 106         uint8_t cmdidx;
 107         /** */
 108         uint8_t direction;
 109         /** */
 110         uint8_t databuf[STLINK_DATA_SIZE];
 111         /** */
 112         uint32_t max_mem_packet;
 113         /** */
 114         enum hl_transports transport;
 115         /** */
 116         struct stlink_usb_version version;
 117         /** */
 118         uint16_t vid;
 119         /** */
 120         uint16_t pid;
 121         /** this is the currently used jtag api */
 122         enum stlink_jtag_api_version jtag_api;
 123         /** */
 124         struct {
 125                 /** whether SWO tracing is enabled or not */
 126                 bool enabled;
 127                 /** trace module source clock */
 128                 uint32_t source_hz;
 129         } trace;
 130         /** reconnect is needed next time we try to query the
 131          * status */
 132         bool reconnect_pending;
 133 };
 134
 135 #define STLINK_DEBUG_ERR_OK            0x80
 136 #define STLINK_DEBUG_ERR_FAULT         0x81
 137 #define STLINK_SWD_AP_WAIT             0x10
 138 #define STLINK_JTAG_WRITE_ERROR        0x0c
 139 #define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
 140 #define STLINK_SWD_DP_WAIT             0x14
 141
 142 #define STLINK_CORE_RUNNING            0x80
 143 #define STLINK_CORE_HALTED             0x81
 144 #define STLINK_CORE_STAT_UNKNOWN       -1
 145
 146 #define STLINK_GET_VERSION             0xF1
 147 #define STLINK_DEBUG_COMMAND           0xF2
 148 #define STLINK_DFU_COMMAND             0xF3
 149 #define STLINK_SWIM_COMMAND            0xF4
 150 #define STLINK_GET_CURRENT_MODE        0xF5
 151 #define STLINK_GET_TARGET_VOLTAGE      0xF7
 152
 153 #define STLINK_DEV_DFU_MODE            0x00
 154 #define STLINK_DEV_MASS_MODE           0x01
 155 #define STLINK_DEV_DEBUG_MODE          0x02
 156 #define STLINK_DEV_SWIM_MODE           0x03
 157 #define STLINK_DEV_BOOTLOADER_MODE     0x04
 158 #define STLINK_DEV_UNKNOWN_MODE        -1
 159
 160 #define STLINK_DFU_EXIT                0x07
 161
 162 #define STLINK_SWIM_ENTER              0x00
 163 #define STLINK_SWIM_EXIT               0x01
 164
 165 #define STLINK_DEBUG_ENTER_JTAG            0x00
 166 #define STLINK_DEBUG_GETSTATUS             0x01
 167 #define STLINK_DEBUG_FORCEDEBUG            0x02
 168 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
 169 #define STLINK_DEBUG_APIV1_READALLREGS     0x04
 170 #define STLINK_DEBUG_APIV1_READREG         0x05
 171 #define STLINK_DEBUG_APIV1_WRITEREG        0x06
 172 #define STLINK_DEBUG_READMEM_32BIT         0x07
 173 #define STLINK_DEBUG_WRITEMEM_32BIT        0x08
 174 #define STLINK_DEBUG_RUNCORE               0x09
 175 #define STLINK_DEBUG_STEPCORE              0x0a
 176 #define STLINK_DEBUG_APIV1_SETFP           0x0b
 177 #define STLINK_DEBUG_READMEM_8BIT          0x0c
 178 #define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
 179 #define STLINK_DEBUG_APIV1_CLEARFP         0x0e
 180 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 181 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 182
 183 #define STLINK_DEBUG_ENTER_JTAG            0x00
 184 #define STLINK_DEBUG_ENTER_SWD             0xa3
 185
 186 #define STLINK_DEBUG_APIV1_ENTER           0x20
 187 #define STLINK_DEBUG_EXIT                  0x21
 188 #define STLINK_DEBUG_READCOREID            0x22
 189
 190 #define STLINK_DEBUG_APIV2_ENTER           0x30
 191 #define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
 192 #define STLINK_DEBUG_APIV2_RESETSYS        0x32
 193 #define STLINK_DEBUG_APIV2_READREG         0x33
 194 #define STLINK_DEBUG_APIV2_WRITEREG        0x34
 195 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
 196 #define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
 197
 198 #define STLINK_DEBUG_APIV2_READALLREGS     0x3A
 199 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 200 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 201
 202 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 203 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 204 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 205 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
 206
 207 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 208 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 209 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 210
 211 #define STLINK_TRACE_SIZE               1024
 212 #define STLINK_TRACE_MAX_HZ             2000000
 213 #define STLINK_TRACE_MIN_VERSION        13
 214
 215 /** */
 216 enum stlink_mode {
 217         STLINK_MODE_UNKNOWN = 0,
 218         STLINK_MODE_DFU,
 219         STLINK_MODE_MASS,
 220         STLINK_MODE_DEBUG_JTAG,
 221         STLINK_MODE_DEBUG_SWD,
 222         STLINK_MODE_DEBUG_SWIM
 223 };
 224
 225 #define REQUEST_SENSE        0x03
 226 #define REQUEST_SENSE_LENGTH 18
 227
 228 static const struct {
 229         int speed;
 230         int speed_divisor;
 231 } stlink_khz_to_speed_map[] = {
 232         {4000, 0},
 233         {1800, 1}, /* default */
 234         {1200, 2},
 235         {950,  3},
 236         {480,  7},
 237         {240, 15},
 238         {125, 31},
 239         {100, 40},
 240         {50,  79},
 241         {25, 158},
 242         {15, 265},
 243         {5,  798}
 244 };
 245
 246 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 247
 248 /** */
 249 static int stlink_usb_xfer_v1_get_status(void *handle)
 250 {
 251         struct stlink_usb_handle_s *h = handle;
 252
 253         assert(handle != NULL);
 254
 255         /* read status */
 256         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 257
 258         if (jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)h->cmdbuf,
 259                         13, STLINK_READ_TIMEOUT) != 13)
 260                 return ERROR_FAIL;
 261
 262         uint32_t t1;
 263
 264         t1 = buf_get_u32(h->cmdbuf, 0, 32);
 265
 266         /* check for USBS */
 267         if (t1 != 0x53425355)
 268                 return ERROR_FAIL;
 269         /*
 270          * CSW status:
 271          * 0 success
 272          * 1 command failure
 273          * 2 phase error
 274          */
 275         if (h->cmdbuf[12] != 0)
 276                 return ERROR_FAIL;
 277
 278         return ERROR_OK;
 279 }
 280
 281 /** */
 282 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 283 {
 284         struct stlink_usb_handle_s *h = handle;
 285
 286         assert(handle != NULL);
 287
 288         if (jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)h->cmdbuf, cmdsize,
 289                         STLINK_WRITE_TIMEOUT) != cmdsize) {
 290                 return ERROR_FAIL;
 291         }
 292
 293         if (h->direction == h->tx_ep && size) {
 294                 if (jtag_libusb_bulk_write(h->fd, h->tx_ep, (char *)buf,
 295                                 size, STLINK_WRITE_TIMEOUT) != size) {
 296                         LOG_DEBUG("bulk write failed");
 297                         return ERROR_FAIL;
 298                 }
 299         } else if (h->direction == h->rx_ep && size) {
 300                 if (jtag_libusb_bulk_read(h->fd, h->rx_ep, (char *)buf,
 301                                 size, STLINK_READ_TIMEOUT) != size) {
 302                         LOG_DEBUG("bulk read failed");
 303                         return ERROR_FAIL;
 304                 }
 305         }
 306
 307         return ERROR_OK;
 308 }
 309
 310 /** */
 311 static int stlink_usb_xfer_v1_get_sense(void *handle)
 312 {
 313         int res;
 314         struct stlink_usb_handle_s *h = handle;
 315
 316         assert(handle != NULL);
 317
 318         stlink_usb_init_buffer(handle, h->rx_ep, 16);
 319
 320         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
 321         h->cmdbuf[h->cmdidx++] = 0;
 322         h->cmdbuf[h->cmdidx++] = 0;
 323         h->cmdbuf[h->cmdidx++] = 0;
 324         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
 325
 326         res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
 327
 328         if (res != ERROR_OK)
 329                 return res;
 330
 331         if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
 332                 return ERROR_FAIL;
 333
 334         return ERROR_OK;
 335 }
 336
 337 /** */
 338 static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 339 {
 340         int err, cmdsize = STLINK_CMD_SIZE_V2;
 341         struct stlink_usb_handle_s *h = handle;
 342
 343         assert(handle != NULL);
 344
 345         if (h->version.stlink == 1)
 346                 cmdsize = STLINK_SG_SIZE;
 347
 348         err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 349
 350         if (err != ERROR_OK)
 351                 return err;
 352
 353         if (h->version.stlink == 1) {
 354                 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
 355                         /* check csw status */
 356                         if (h->cmdbuf[12] == 1) {
 357                                 LOG_DEBUG("get sense");
 358                                 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
 359                                         return ERROR_FAIL;
 360                         }
 361                         return ERROR_FAIL;
 362                 }
 363         }
 364
 365         return ERROR_OK;
 366 }
 367
 368
 369 /**
 370     Converts an STLINK status code held in the first byte of a response
 371     to an openocd error, logs any error/wait status as debug output.
 372 */
 373 static int stlink_usb_error_check(void *handle)
 374 {
 375         struct stlink_usb_handle_s *h = handle;
 376
 377         assert(handle != NULL);
 378
 379         /* TODO: no error checking yet on api V1 */
 380         if (h->jtag_api == STLINK_JTAG_API_V1)
 381                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
 382
 383         switch (h->databuf[0]) {
 384                 case STLINK_DEBUG_ERR_OK:
 385                         return ERROR_OK;
 386                 case STLINK_DEBUG_ERR_FAULT:
 387                         LOG_DEBUG("SWD fault response (0x%x)", STLINK_DEBUG_ERR_FAULT);
 388                         return ERROR_FAIL;
 389                 case STLINK_SWD_AP_WAIT:
 390                         LOG_DEBUG("wait status SWD_AP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
 391                         return ERROR_WAIT;
 392                 case STLINK_SWD_DP_WAIT:
 393                         LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_AP_WAIT);
 394                         return ERROR_WAIT;
 395                 case STLINK_JTAG_WRITE_ERROR:
 396                         LOG_DEBUG("Write error");
 397                         return ERROR_FAIL;
 398                 case STLINK_JTAG_WRITE_VERIF_ERROR:
 399                         LOG_DEBUG("Verify error");
 400                         return ERROR_FAIL;
 401                 default:
 402                         LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
 403                         return ERROR_FAIL;
 404         }
 405 }
 406
 407
 408 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
 409
 410     Works for commands where the STLINK_DEBUG status is returned in the first
 411     byte of the response packet.
 412
 413     Returns an openocd result code.
 414 */
 415 static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 416 {
 417         int retries = 0;
 418         int res;
 419         while (1) {
 420                 res = stlink_usb_xfer(handle, buf, size);
 421                 if (res != ERROR_OK)
 422                         return res;
 423                 res = stlink_usb_error_check(handle);
 424                 if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 425                         usleep((1<<retries++) * 1000);
 426                         continue;
 427                 }
 428                 return res;
 429         }
 430 }
 431
 432 /** */
 433 static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 434 {
 435         struct stlink_usb_handle_s *h = handle;
 436
 437         assert(handle != NULL);
 438
 439         assert(h->version.stlink >= 2);
 440
 441         if (jtag_libusb_bulk_read(h->fd, h->trace_ep, (char *)buf,
 442                         size, STLINK_READ_TIMEOUT) != size) {
 443                 LOG_ERROR("bulk trace read failed");
 444                 return ERROR_FAIL;
 445         }
 446
 447         return ERROR_OK;
 448 }
 449
 450 /** */
 451 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 452 {
 453         struct stlink_usb_handle_s *h = handle;
 454
 455         /* fill the send buffer */
 456         strcpy((char *)h->cmdbuf, "USBC");
 457         h->cmdidx += 4;
 458         /* csw tag not used */
 459         h->cmdidx += 4;
 460         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 461         h->cmdidx += 4;
 462         h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
 463         h->cmdbuf[h->cmdidx++] = 0; /* lun */
 464         h->cmdbuf[h->cmdidx++] = STLINK_CMD_SIZE_V1;
 465 }
 466
 467 /** */
 468 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
 469 {
 470         struct stlink_usb_handle_s *h = handle;
 471
 472         h->direction = direction;
 473
 474         h->cmdidx = 0;
 475
 476         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 477         memset(h->databuf, 0, STLINK_DATA_SIZE);
 478
 479         if (h->version.stlink == 1)
 480                 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
 481 }
 482
 483 /** */
 484 static int stlink_usb_version(void *handle)
 485 {
 486         int res;
 487         uint16_t v;
 488         struct stlink_usb_handle_s *h = handle;
 489
 490         assert(handle != NULL);
 491
 492         stlink_usb_init_buffer(handle, h->rx_ep, 6);
 493
 494         h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
 495
 496         res = stlink_usb_xfer(handle, h->databuf, 6);
 497
 498         if (res != ERROR_OK)
 499                 return res;
 500
 501         v = (h->databuf[0] << 8) | h->databuf[1];
 502
 503         h->version.stlink = (v >> 12) & 0x0f;
 504         h->version.jtag = (v >> 6) & 0x3f;
 505         h->version.swim = v & 0x3f;
 506         h->vid = buf_get_u32(h->databuf, 16, 16);
 507         h->pid = buf_get_u32(h->databuf, 32, 16);
 508
 509         /* set the supported jtag api version
 510          * API V2 is supported since JTAG V11
 511          */
 512         if (h->version.jtag >= 11)
 513                 h->version.jtag_api_max = STLINK_JTAG_API_V2;
 514         else
 515                 h->version.jtag_api_max = STLINK_JTAG_API_V1;
 516
 517         LOG_INFO("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
 518                 h->version.stlink,
 519                 h->version.jtag,
 520                 (h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2,
 521                 h->version.swim,
 522                 h->vid,
 523                 h->pid);
 524
 525         return ERROR_OK;
 526 }
 527
 528 static int stlink_usb_check_voltage(void *handle, float *target_voltage)
 529 {
 530         struct stlink_usb_handle_s *h = handle;
 531         uint32_t adc_results[2];
 532
 533         /* only supported by stlink/v2 and for firmware >= 13 */
 534         if (h->version.stlink == 1 || h->version.jtag < 13)
 535                 return ERROR_COMMAND_NOTFOUND;
 536
 537         stlink_usb_init_buffer(handle, h->rx_ep, 8);
 538
 539         h->cmdbuf[h->cmdidx++] = STLINK_GET_TARGET_VOLTAGE;
 540
 541         int result = stlink_usb_xfer(handle, h->databuf, 8);
 542
 543         if (result != ERROR_OK)
 544                 return result;
 545
 546         /* convert result */
 547         adc_results[0] = le_to_h_u32(h->databuf);
 548         adc_results[1] = le_to_h_u32(h->databuf + 4);
 549
 550         *target_voltage = 0;
 551
 552         if (adc_results[0])
 553                 *target_voltage = 2 * ((float)adc_results[1]) * (float)(1.2 / adc_results[0]);
 554
 555         LOG_INFO("Target voltage: %f", (double)*target_voltage);
 556
 557         return ERROR_OK;
 558 }
 559
 560 static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
 561 {
 562         struct stlink_usb_handle_s *h = handle;
 563
 564         assert(handle != NULL);
 565
 566         /* only supported by stlink/v2 and for firmware >= 22 */
 567         if (h->version.stlink == 1 || h->version.jtag < 22)
 568                 return ERROR_COMMAND_NOTFOUND;
 569
 570         stlink_usb_init_buffer(handle, h->rx_ep, 2);
 571
 572         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 573         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
 574         h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
 575         h->cmdidx += 2;
 576
 577         int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
 578
 579         if (result != ERROR_OK)
 580                 return result;
 581
 582         return ERROR_OK;
 583 }
 584
 585 /** */
 586 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 587 {
 588         int res;
 589         struct stlink_usb_handle_s *h = handle;
 590
 591         assert(handle != NULL);
 592
 593         stlink_usb_init_buffer(handle, h->rx_ep, 2);
 594
 595         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
 596
 597         res = stlink_usb_xfer(handle, h->databuf, 2);
 598
 599         if (res != ERROR_OK)
 600                 return res;
 601
 602         *mode = h->databuf[0];
 603
 604         return ERROR_OK;
 605 }
 606
 607 /** */
 608 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 609 {
 610         int rx_size = 0;
 611         struct stlink_usb_handle_s *h = handle;
 612
 613         assert(handle != NULL);
 614
 615         /* on api V2 we are able the read the latest command
 616          * status
 617          * TODO: we need the test on api V1 too
 618          */
 619         if (h->jtag_api == STLINK_JTAG_API_V2)
 620                 rx_size = 2;
 621
 622         stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
 623
 624         switch (type) {
 625                 case STLINK_MODE_DEBUG_JTAG:
 626                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 627                         if (h->jtag_api == STLINK_JTAG_API_V1)
 628                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 629                         else
 630                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
 631                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG;
 632                         break;
 633                 case STLINK_MODE_DEBUG_SWD:
 634                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 635                         if (h->jtag_api == STLINK_JTAG_API_V1)
 636                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 637                         else
 638                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
 639                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD;
 640                         break;
 641                 case STLINK_MODE_DEBUG_SWIM:
 642                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 643                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
 644                         break;
 645                 case STLINK_MODE_DFU:
 646                 case STLINK_MODE_MASS:
 647                 default:
 648                         return ERROR_FAIL;
 649         }
 650
 651         return stlink_cmd_allow_retry(handle, h->databuf, rx_size);
 652 }
 653
 654 /** */
 655 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 656 {
 657         int res;
 658         struct stlink_usb_handle_s *h = handle;
 659
 660         assert(handle != NULL);
 661
 662         stlink_usb_init_buffer(handle, STLINK_NULL_EP, 0);
 663
 664         switch (type) {
 665                 case STLINK_MODE_DEBUG_JTAG:
 666                 case STLINK_MODE_DEBUG_SWD:
 667                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 668                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
 669                         break;
 670                 case STLINK_MODE_DEBUG_SWIM:
 671                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 672                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
 673                         break;
 674                 case STLINK_MODE_DFU:
 675                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
 676                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
 677                         break;
 678                 case STLINK_MODE_MASS:
 679                 default:
 680                         return ERROR_FAIL;
 681         }
 682
 683         res = stlink_usb_xfer(handle, 0, 0);
 684
 685         if (res != ERROR_OK)
 686                 return res;
 687
 688         return ERROR_OK;
 689 }
 690
 691 static int stlink_usb_assert_srst(void *handle, int srst);
 692
 693 static enum stlink_mode stlink_get_mode(enum hl_transports t)
 694 {
 695         switch (t) {
 696         case HL_TRANSPORT_SWD:
 697                 return STLINK_MODE_DEBUG_SWD;
 698         case HL_TRANSPORT_JTAG:
 699                 return STLINK_MODE_DEBUG_JTAG;
 700         case HL_TRANSPORT_SWIM:
 701                 return STLINK_MODE_DEBUG_SWIM;
 702         default:
 703                 return STLINK_MODE_UNKNOWN;
 704         }
 705 }
 706
 707 /** */
 708 static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 709 {
 710         int res;
 711         uint8_t mode;
 712         enum stlink_mode emode;
 713         struct stlink_usb_handle_s *h = handle;
 714
 715         assert(handle != NULL);
 716
 717         res = stlink_usb_current_mode(handle, &mode);
 718
 719         if (res != ERROR_OK)
 720                 return res;
 721
 722         LOG_DEBUG("MODE: 0x%02X", mode);
 723
 724         /* try to exit current mode */
 725         switch (mode) {
 726                 case STLINK_DEV_DFU_MODE:
 727                         emode = STLINK_MODE_DFU;
 728                         break;
 729                 case STLINK_DEV_DEBUG_MODE:
 730                         emode = STLINK_MODE_DEBUG_SWD;
 731                         break;
 732                 case STLINK_DEV_SWIM_MODE:
 733                         emode = STLINK_MODE_DEBUG_SWIM;
 734                         break;
 735                 case STLINK_DEV_BOOTLOADER_MODE:
 736                 case STLINK_DEV_MASS_MODE:
 737                 default:
 738                         emode = STLINK_MODE_UNKNOWN;
 739                         break;
 740         }
 741
 742         if (emode != STLINK_MODE_UNKNOWN) {
 743                 res = stlink_usb_mode_leave(handle, emode);
 744
 745                 if (res != ERROR_OK)
 746                         return res;
 747         }
 748
 749         res = stlink_usb_current_mode(handle, &mode);
 750
 751         if (res != ERROR_OK)
 752                 return res;
 753
 754         /* we check the target voltage here as an aid to debugging connection problems.
 755          * the stlink requires the target Vdd to be connected for reliable debugging.
 756          * this cmd is supported in all modes except DFU
 757          */
 758         if (mode != STLINK_DEV_DFU_MODE) {
 759
 760                 float target_voltage;
 761
 762                 /* check target voltage (if supported) */
 763                 res = stlink_usb_check_voltage(h, &target_voltage);
 764
 765                 if (res != ERROR_OK) {
 766                         if (res != ERROR_COMMAND_NOTFOUND)
 767                                 LOG_ERROR("voltage check failed");
 768                         /* attempt to continue as it is not a catastrophic failure */
 769                 } else {
 770                         /* check for a sensible target voltage, operating range is 1.65-5.5v
 771                          * according to datasheet */
 772                         if (target_voltage < 1.5)
 773                                 LOG_ERROR("target voltage may be too low for reliable debugging");
 774                 }
 775         }
 776
 777         LOG_DEBUG("MODE: 0x%02X", mode);
 778
 779         /* set selected mode */
 780         emode = stlink_get_mode(h->transport);
 781
 782         if (emode == STLINK_MODE_UNKNOWN) {
 783                 LOG_ERROR("selected mode (transport) not supported");
 784                 return ERROR_FAIL;
 785         }
 786
 787         if (connect_under_reset) {
 788                 res = stlink_usb_assert_srst(handle, 0);
 789                 if (res != ERROR_OK)
 790                         return res;
 791         }
 792
 793         res = stlink_usb_mode_enter(handle, emode);
 794
 795         if (res != ERROR_OK)
 796                 return res;
 797
 798         res = stlink_usb_current_mode(handle, &mode);
 799
 800         if (res != ERROR_OK)
 801                 return res;
 802
 803         LOG_DEBUG("MODE: 0x%02X", mode);
 804
 805         return ERROR_OK;
 806 }
 807
 808 /** */
 809 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 810 {
 811         int res;
 812         struct stlink_usb_handle_s *h = handle;
 813
 814         assert(handle != NULL);
 815
 816         stlink_usb_init_buffer(handle, h->rx_ep, 4);
 817
 818         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 819         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
 820
 821         res = stlink_usb_xfer(handle, h->databuf, 4);
 822
 823         if (res != ERROR_OK)
 824                 return res;
 825
 826         *idcode = le_to_h_u32(h->databuf);
 827
 828         LOG_DEBUG("IDCODE: 0x%08" PRIX32, *idcode);
 829
 830         return ERROR_OK;
 831 }
 832
 833 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
 834 {
 835         struct stlink_usb_handle_s *h = handle;
 836         int res;
 837
 838         assert(handle != NULL);
 839
 840         stlink_usb_init_buffer(handle, h->rx_ep, 8);
 841
 842         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 843         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
 844         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 845         h->cmdidx += 4;
 846
 847         res = stlink_cmd_allow_retry(handle, h->databuf, 8);
 848         if (res != ERROR_OK)
 849                 return res;
 850
 851         *val = le_to_h_u32(h->databuf + 4);
 852         return ERROR_OK;
 853 }
 854
 855 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 856 {
 857         struct stlink_usb_handle_s *h = handle;
 858
 859         assert(handle != NULL);
 860
 861         stlink_usb_init_buffer(handle, h->rx_ep, 2);
 862
 863         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 864         if (h->jtag_api == STLINK_JTAG_API_V1)
 865                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
 866         else
 867                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
 868         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 869         h->cmdidx += 4;
 870         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 871         h->cmdidx += 4;
 872
 873         return stlink_cmd_allow_retry(handle, h->databuf, 2);
 874 }
 875
 876 /** */
 877 static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 878 {
 879         struct stlink_usb_handle_s *h = handle;
 880
 881         assert(handle != NULL);
 882
 883         if (h->trace.enabled && h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
 884                 int res;
 885
 886                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
 887
 888                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 889                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GET_TRACE_NB;
 890
 891                 res = stlink_usb_xfer(handle, h->databuf, 2);
 892                 if (res != ERROR_OK)
 893                         return res;
 894
 895                 size_t bytes_avail = le_to_h_u16(h->databuf);
 896                 *size = bytes_avail < *size ? bytes_avail : *size - 1;
 897
 898                 if (*size > 0) {
 899                         res = stlink_usb_read_trace(handle, buf, *size);
 900                         if (res != ERROR_OK)
 901                                 return res;
 902                         return ERROR_OK;
 903                 }
 904         }
 905         *size = 0;
 906         return ERROR_OK;
 907 }
 908
 909 static enum target_state stlink_usb_v2_get_status(void *handle)
 910 {
 911         int result;
 912         uint32_t status;
 913
 914         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
 915         if  (result != ERROR_OK)
 916                 return TARGET_UNKNOWN;
 917
 918         if (status & S_HALT)
 919                 return TARGET_HALTED;
 920         else if (status & S_RESET_ST)
 921                 return TARGET_RESET;
 922
 923         return TARGET_RUNNING;
 924 }
 925
 926 /** */
 927 static enum target_state stlink_usb_state(void *handle)
 928 {
 929         int res;
 930         struct stlink_usb_handle_s *h = handle;
 931
 932         assert(handle != NULL);
 933
 934         if (h->reconnect_pending) {
 935                 LOG_INFO("Previous state query failed, trying to reconnect");
 936                 res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
 937
 938                 if (res != ERROR_OK)
 939                         return TARGET_UNKNOWN;
 940
 941                 h->reconnect_pending = false;
 942         }
 943
 944         if (h->jtag_api == STLINK_JTAG_API_V2) {
 945                 res = stlink_usb_v2_get_status(handle);
 946                 if (res == TARGET_UNKNOWN)
 947                         h->reconnect_pending = true;
 948                 return res;
 949         }
 950
 951         stlink_usb_init_buffer(handle, h->rx_ep, 2);
 952
 953         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 954         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
 955
 956         res = stlink_usb_xfer(handle, h->databuf, 2);
 957
 958         if (res != ERROR_OK)
 959                 return TARGET_UNKNOWN;
 960
 961         if (h->databuf[0] == STLINK_CORE_RUNNING)
 962                 return TARGET_RUNNING;
 963         if (h->databuf[0] == STLINK_CORE_HALTED)
 964                 return TARGET_HALTED;
 965
 966         h->reconnect_pending = true;
 967
 968         return TARGET_UNKNOWN;
 969 }
 970
 971 static int stlink_usb_assert_srst(void *handle, int srst)
 972 {
 973         struct stlink_usb_handle_s *h = handle;
 974
 975         assert(handle != NULL);
 976
 977         if (h->jtag_api == STLINK_JTAG_API_V1)
 978                 return ERROR_COMMAND_NOTFOUND;
 979
 980         stlink_usb_init_buffer(handle, h->rx_ep, 2);
 981
 982         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 983         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
 984         h->cmdbuf[h->cmdidx++] = srst;
 985
 986         return stlink_cmd_allow_retry(handle, h->databuf, 2);
 987 }
 988
 989 /** */
 990 static void stlink_usb_trace_disable(void *handle)
 991 {
 992         int res = ERROR_OK;
 993         struct stlink_usb_handle_s *h = handle;
 994
 995         assert(handle != NULL);
 996
 997         assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
 998
 999         LOG_DEBUG("Tracing: disable");
1000
1001         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1002         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1003         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1004         res = stlink_usb_xfer(handle, h->databuf, 2);
1005
1006         if (res == ERROR_OK)
1007                 h->trace.enabled = false;
1008 }
1009
1010
1011 /** */
1012 static int stlink_usb_trace_enable(void *handle)
1013 {
1014         int res;
1015         struct stlink_usb_handle_s *h = handle;
1016
1017         assert(handle != NULL);
1018
1019         if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
1020                 stlink_usb_init_buffer(handle, h->rx_ep, 10);
1021
1022                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1023                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1024                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1025                 h->cmdidx += 2;
1026                 h_u32_to_le(h->cmdbuf+h->cmdidx, h->trace.source_hz);
1027                 h->cmdidx += 4;
1028
1029                 res = stlink_usb_xfer(handle, h->databuf, 2);
1030
1031                 if (res == ERROR_OK)  {
1032                         h->trace.enabled = true;
1033                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz", h->trace.source_hz);
1034                 }
1035         } else {
1036                 LOG_ERROR("Tracing is not supported by this version.");
1037                 res = ERROR_FAIL;
1038         }
1039
1040         return res;
1041 }
1042
1043 /** */
1044 static int stlink_usb_reset(void *handle)
1045 {
1046         struct stlink_usb_handle_s *h = handle;
1047         int retval;
1048
1049         assert(handle != NULL);
1050
1051         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1052
1053         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1054
1055         if (h->jtag_api == STLINK_JTAG_API_V1)
1056                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
1057         else
1058                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
1059
1060         retval = stlink_cmd_allow_retry(handle, h->databuf, 2);
1061         if (retval != ERROR_OK)
1062                 return retval;
1063
1064         if (h->trace.enabled) {
1065                 stlink_usb_trace_disable(h);
1066                 return stlink_usb_trace_enable(h);
1067         }
1068
1069         return ERROR_OK;
1070 }
1071
1072 /** */
1073 static int stlink_usb_run(void *handle)
1074 {
1075         int res;
1076         struct stlink_usb_handle_s *h = handle;
1077
1078         assert(handle != NULL);
1079
1080         if (h->jtag_api == STLINK_JTAG_API_V2) {
1081                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1082
1083                 return res;
1084         }
1085
1086         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1087
1088         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1089         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1090
1091         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1092 }
1093
1094 /** */
1095 static int stlink_usb_halt(void *handle)
1096 {
1097         int res;
1098         struct stlink_usb_handle_s *h = handle;
1099
1100         assert(handle != NULL);
1101
1102         if (h->jtag_api == STLINK_JTAG_API_V2) {
1103                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1104
1105                 return res;
1106         }
1107
1108         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1109
1110         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1111         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1112
1113         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1114 }
1115
1116 /** */
1117 static int stlink_usb_step(void *handle)
1118 {
1119         struct stlink_usb_handle_s *h = handle;
1120
1121         assert(handle != NULL);
1122
1123         if (h->jtag_api == STLINK_JTAG_API_V2) {
1124                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1125                  * that the cortex-m3 currently does. */
1126                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1127                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1128                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1129         }
1130
1131         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1132
1133         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1134         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1135
1136         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1137 }
1138
1139 /** */
1140 static int stlink_usb_read_regs(void *handle)
1141 {
1142         int res;
1143         struct stlink_usb_handle_s *h = handle;
1144
1145         assert(handle != NULL);
1146
1147         stlink_usb_init_buffer(handle, h->rx_ep, 84);
1148
1149         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1150         if (h->jtag_api == STLINK_JTAG_API_V1)
1151                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
1152         else
1153                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
1154
1155         res = stlink_usb_xfer(handle, h->databuf, 84);
1156
1157         if (res != ERROR_OK)
1158                 return res;
1159
1160         return ERROR_OK;
1161 }
1162
1163 /** */
1164 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
1165 {
1166         int res;
1167         struct stlink_usb_handle_s *h = handle;
1168
1169         assert(handle != NULL);
1170
1171         stlink_usb_init_buffer(handle, h->rx_ep, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1172
1173         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1174         if (h->jtag_api == STLINK_JTAG_API_V1)
1175                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
1176         else
1177                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
1178         h->cmdbuf[h->cmdidx++] = num;
1179
1180         if (h->jtag_api == STLINK_JTAG_API_V1) {
1181                 res = stlink_usb_xfer(handle, h->databuf, 4);
1182                 if (res != ERROR_OK)
1183                         return res;
1184                 *val = le_to_h_u32(h->databuf);
1185                 return ERROR_OK;
1186         } else {
1187                 res = stlink_cmd_allow_retry(handle, h->databuf, 8);
1188                 if (res != ERROR_OK)
1189                         return res;
1190                 *val = le_to_h_u32(h->databuf + 4);
1191                 return ERROR_OK;
1192         }
1193 }
1194
1195 /** */
1196 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
1197 {
1198         struct stlink_usb_handle_s *h = handle;
1199
1200         assert(handle != NULL);
1201
1202         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1203
1204         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1205         if (h->jtag_api == STLINK_JTAG_API_V1)
1206                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
1207         else
1208                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
1209         h->cmdbuf[h->cmdidx++] = num;
1210         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1211         h->cmdidx += 4;
1212
1213         return stlink_cmd_allow_retry(handle, h->databuf, 2);
1214 }
1215
1216 static int stlink_usb_get_rw_status(void *handle)
1217 {
1218         int res;
1219         struct stlink_usb_handle_s *h = handle;
1220
1221         assert(handle != NULL);
1222
1223         if (h->jtag_api == STLINK_JTAG_API_V1)
1224                 return ERROR_OK;
1225
1226         stlink_usb_init_buffer(handle, h->rx_ep, 2);
1227
1228         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1229         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
1230
1231         res = stlink_usb_xfer(handle, h->databuf, 2);
1232
1233         if (res != ERROR_OK)
1234                 return res;
1235
1236         return stlink_usb_error_check(h);
1237 }
1238
1239 /** */
1240 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1241                           uint8_t *buffer)
1242 {
1243         int res;
1244         uint16_t read_len = len;
1245         struct stlink_usb_handle_s *h = handle;
1246
1247         assert(handle != NULL);
1248
1249         /* max 8bit read/write is 64bytes */
1250         if (len > STLINK_MAX_RW8) {
1251                 LOG_DEBUG("max buffer length exceeded");
1252                 return ERROR_FAIL;
1253         }
1254
1255         stlink_usb_init_buffer(handle, h->rx_ep, read_len);
1256
1257         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1258         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
1259         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1260         h->cmdidx += 4;
1261         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1262         h->cmdidx += 2;
1263
1264         /* we need to fix read length for single bytes */
1265         if (read_len == 1)
1266                 read_len++;
1267
1268         res = stlink_usb_xfer(handle, h->databuf, read_len);
1269
1270         if (res != ERROR_OK)
1271                 return res;
1272
1273         memcpy(buffer, h->databuf, len);
1274
1275         return stlink_usb_get_rw_status(handle);
1276 }
1277
1278 /** */
1279 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1280                            const uint8_t *buffer)
1281 {
1282         int res;
1283         struct stlink_usb_handle_s *h = handle;
1284
1285         assert(handle != NULL);
1286
1287         /* max 8bit read/write is 64bytes */
1288         if (len > STLINK_MAX_RW8) {
1289                 LOG_DEBUG("max buffer length exceeded");
1290                 return ERROR_FAIL;
1291         }
1292
1293         stlink_usb_init_buffer(handle, h->tx_ep, len);
1294
1295         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1296         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1297         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1298         h->cmdidx += 4;
1299         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1300         h->cmdidx += 2;
1301
1302         res = stlink_usb_xfer(handle, buffer, len);
1303
1304         if (res != ERROR_OK)
1305                 return res;
1306
1307         return stlink_usb_get_rw_status(handle);
1308 }
1309
1310 /** */
1311 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1312                           uint8_t *buffer)
1313 {
1314         int res;
1315         struct stlink_usb_handle_s *h = handle;
1316
1317         assert(handle != NULL);
1318
1319         /* data must be a multiple of 4 and word aligned */
1320         if (len % 4 || addr % 4) {
1321                 LOG_DEBUG("Invalid data alignment");
1322                 return ERROR_TARGET_UNALIGNED_ACCESS;
1323         }
1324
1325         stlink_usb_init_buffer(handle, h->rx_ep, len);
1326
1327         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1328         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1329         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1330         h->cmdidx += 4;
1331         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1332         h->cmdidx += 2;
1333
1334         res = stlink_usb_xfer(handle, h->databuf, len);
1335
1336         if (res != ERROR_OK)
1337                 return res;
1338
1339         memcpy(buffer, h->databuf, len);
1340
1341         return stlink_usb_get_rw_status(handle);
1342 }
1343
1344 /** */
1345 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1346                            const uint8_t *buffer)
1347 {
1348         int res;
1349         struct stlink_usb_handle_s *h = handle;
1350
1351         assert(handle != NULL);
1352
1353         /* data must be a multiple of 4 and word aligned */
1354         if (len % 4 || addr % 4) {
1355                 LOG_DEBUG("Invalid data alignment");
1356                 return ERROR_TARGET_UNALIGNED_ACCESS;
1357         }
1358
1359         stlink_usb_init_buffer(handle, h->tx_ep, len);
1360
1361         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1362         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1363         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1364         h->cmdidx += 4;
1365         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1366         h->cmdidx += 2;
1367
1368         res = stlink_usb_xfer(handle, buffer, len);
1369
1370         if (res != ERROR_OK)
1371                 return res;
1372
1373         return stlink_usb_get_rw_status(handle);
1374 }
1375
1376 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
1377 {
1378         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
1379         if (max_tar_block == 0)
1380                 max_tar_block = 4;
1381         return max_tar_block;
1382 }
1383
1384 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
1385                 uint32_t count, uint8_t *buffer)
1386 {
1387         int retval = ERROR_OK;
1388         uint32_t bytes_remaining;
1389         int retries = 0;
1390         struct stlink_usb_handle_s *h = handle;
1391
1392         /* calculate byte count */
1393         count *= size;
1394
1395         while (count) {
1396
1397                 bytes_remaining = (size == 4) ? \
1398                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1399
1400                 if (count < bytes_remaining)
1401                         bytes_remaining = count;
1402
1403                 /* the stlink only supports 8/32bit memory read/writes
1404                  * honour 32bit, all others will be handled as 8bit access */
1405                 if (size == 4) {
1406
1407                         /* When in jtag mode the stlink uses the auto-increment functinality.
1408                          * However it expects us to pass the data correctly, this includes
1409                          * alignment and any page boundaries. We already do this as part of the
1410                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1411                          * needs implementiong manually.
1412                          * currently this only affects jtag mode, according to ST they do single
1413                          * access in SWD mode - but this may change and so we do it for both modes */
1414
1415                         /* we first need to check for any unaligned bytes */
1416                         if (addr % 4) {
1417
1418                                 uint32_t head_bytes = 4 - (addr % 4);
1419                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
1420                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1421                                         usleep((1<<retries++) * 1000);
1422                                         continue;
1423                                 }
1424                                 if (retval != ERROR_OK)
1425                                         return retval;
1426                                 buffer += head_bytes;
1427                                 addr += head_bytes;
1428                                 count -= head_bytes;
1429                                 bytes_remaining -= head_bytes;
1430                         }
1431
1432                         if (bytes_remaining % 4)
1433                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
1434                         else
1435                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
1436                 } else
1437                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
1438
1439                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1440                         usleep((1<<retries++) * 1000);
1441                         continue;
1442                 }
1443                 if (retval != ERROR_OK)
1444                         return retval;
1445
1446                 buffer += bytes_remaining;
1447                 addr += bytes_remaining;
1448                 count -= bytes_remaining;
1449         }
1450
1451         return retval;
1452 }
1453
1454 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
1455                 uint32_t count, const uint8_t *buffer)
1456 {
1457         int retval = ERROR_OK;
1458         uint32_t bytes_remaining;
1459         int retries = 0;
1460         struct stlink_usb_handle_s *h = handle;
1461
1462         /* calculate byte count */
1463         count *= size;
1464
1465         while (count) {
1466
1467                 bytes_remaining = (size == 4) ? \
1468                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1469
1470                 if (count < bytes_remaining)
1471                         bytes_remaining = count;
1472
1473                 /* the stlink only supports 8/32bit memory read/writes
1474                  * honour 32bit, all others will be handled as 8bit access */
1475                 if (size == 4) {
1476
1477                         /* When in jtag mode the stlink uses the auto-increment functinality.
1478                          * However it expects us to pass the data correctly, this includes
1479                          * alignment and any page boundaries. We already do this as part of the
1480                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1481                          * needs implementiong manually.
1482                          * currently this only affects jtag mode, according to ST they do single
1483                          * access in SWD mode - but this may change and so we do it for both modes */
1484
1485                         /* we first need to check for any unaligned bytes */
1486                         if (addr % 4) {
1487
1488                                 uint32_t head_bytes = 4 - (addr % 4);
1489                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
1490                                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1491                                         usleep((1<<retries++) * 1000);
1492                                         continue;
1493                                 }
1494                                 if (retval != ERROR_OK)
1495                                         return retval;
1496                                 buffer += head_bytes;
1497                                 addr += head_bytes;
1498                                 count -= head_bytes;
1499                                 bytes_remaining -= head_bytes;
1500                         }
1501
1502                         if (bytes_remaining % 4)
1503                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
1504                         else
1505                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
1506
1507                 } else
1508                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
1509                 if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
1510                         usleep((1<<retries++) * 1000);
1511                         continue;
1512                 }
1513                 if (retval != ERROR_OK)
1514                         return retval;
1515
1516                 buffer += bytes_remaining;
1517                 addr += bytes_remaining;
1518                 count -= bytes_remaining;
1519         }
1520
1521         return retval;
1522 }
1523
1524 /** */
1525 static int stlink_usb_override_target(const char *targetname)
1526 {
1527         return !strcmp(targetname, "cortex_m");
1528 }
1529
1530 static int stlink_speed(void *handle, int khz, bool query)
1531 {
1532         unsigned i;
1533         int speed_index = -1;
1534         int speed_diff = INT_MAX;
1535         struct stlink_usb_handle_s *h = handle;
1536
1537         /* only supported by stlink/v2 and for firmware >= 22 */
1538         if (h && (h->version.stlink == 1 || h->version.jtag < 22))
1539                 return khz;
1540
1541         for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) {
1542                 if (khz == stlink_khz_to_speed_map[i].speed) {
1543                         speed_index = i;
1544                         break;
1545                 } else {
1546                         int current_diff = khz - stlink_khz_to_speed_map[i].speed;
1547                         /* get abs value for comparison */
1548                         current_diff = (current_diff > 0) ? current_diff : -current_diff;
1549                         if ((current_diff < speed_diff) && khz >= stlink_khz_to_speed_map[i].speed) {
1550                                 speed_diff = current_diff;
1551                                 speed_index = i;
1552                         }
1553                 }
1554         }
1555
1556         bool match = true;
1557
1558         if (speed_index == -1) {
1559                 /* this will only be here if we cannot match the slow speed.
1560                  * use the slowest speed we support.*/
1561                 speed_index = ARRAY_SIZE(stlink_khz_to_speed_map) - 1;
1562                 match = false;
1563         } else if (i == ARRAY_SIZE(stlink_khz_to_speed_map))
1564                 match = false;
1565
1566         if (!match && query) {
1567                 LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
1568                                 khz, stlink_khz_to_speed_map[speed_index].speed);
1569         }
1570
1571         if (h && !query) {
1572                 int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor);
1573                 if (result != ERROR_OK) {
1574                         LOG_ERROR("Unable to set adapter speed");
1575                         return khz;
1576                 }
1577         }
1578
1579         return stlink_khz_to_speed_map[speed_index].speed;
1580 }
1581
1582 /** */
1583 static int stlink_usb_close(void *handle)
1584 {
1585         struct stlink_usb_handle_s *h = handle;
1586
1587         if (h && h->fd)
1588                 jtag_libusb_close(h->fd);
1589
1590         free(h);
1591
1592         return ERROR_OK;
1593 }
1594
1595 /** */
1596 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
1597 {
1598         int err, retry_count = 1;
1599         struct stlink_usb_handle_s *h;
1600         enum stlink_jtag_api_version api;
1601
1602         LOG_DEBUG("stlink_usb_open");
1603
1604         h = calloc(1, sizeof(struct stlink_usb_handle_s));
1605
1606         if (h == 0) {
1607                 LOG_DEBUG("malloc failed");
1608                 return ERROR_FAIL;
1609         }
1610
1611         h->transport = param->transport;
1612
1613         const uint16_t vids[] = { param->vid, 0 };
1614         const uint16_t pids[] = { param->pid, 0 };
1615         const char *serial = param->serial;
1616
1617         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
1618                         param->transport, param->vid, param->pid,
1619                         param->serial ? param->serial : "");
1620
1621         /*
1622           On certain host USB configurations(e.g. MacBook Air)
1623           STLINKv2 dongle seems to have its FW in a funky state if,
1624           after plugging it in, you try to use openocd with it more
1625           then once (by launching and closing openocd). In cases like
1626           that initial attempt to read the FW info via
1627           stlink_usb_version will fail and the device has to be reset
1628           in order to become operational.
1629          */
1630         do {
1631                 if (jtag_libusb_open(vids, pids, serial, &h->fd) != ERROR_OK) {
1632                         LOG_ERROR("open failed");
1633                         goto error_open;
1634                 }
1635
1636                 jtag_libusb_set_configuration(h->fd, 0);
1637
1638                 if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1639                         LOG_DEBUG("claim interface failed");
1640                         goto error_open;
1641                 }
1642
1643                 /* RX EP is common for all versions */
1644                 h->rx_ep = STLINK_RX_EP;
1645
1646                 /* wrap version for first read */
1647                 switch (param->pid) {
1648                         case STLINK_V1_PID:
1649                                 h->version.stlink = 1;
1650                                 h->tx_ep = STLINK_TX_EP;
1651                                 h->trace_ep = STLINK_TRACE_EP;
1652                                 break;
1653                         case STLINK_V2_1_PID:
1654                                 h->version.stlink = 2;
1655                                 h->tx_ep = STLINK_V2_1_TX_EP;
1656                                 h->trace_ep = STLINK_V2_1_TRACE_EP;
1657                                 break;
1658                         default:
1659                         /* fall through - we assume V2 to be the default version*/
1660                         case STLINK_V2_PID:
1661                                 h->version.stlink = 2;
1662                                 h->tx_ep = STLINK_TX_EP;
1663                                 h->trace_ep = STLINK_TRACE_EP;
1664                                 break;
1665                 }
1666
1667                 /* get the device version */
1668                 err = stlink_usb_version(h);
1669
1670                 if (err == ERROR_OK) {
1671                         break;
1672                 } else if (h->version.stlink == 1 ||
1673                            retry_count == 0) {
1674                         LOG_ERROR("read version failed");
1675                         goto error_open;
1676                 } else {
1677                         err = jtag_libusb_release_interface(h->fd, 0);
1678                         if (err != ERROR_OK) {
1679                                 LOG_ERROR("release interface failed");
1680                                 goto error_open;
1681                         }
1682
1683                         err = jtag_libusb_reset_device(h->fd);
1684                         if (err != ERROR_OK) {
1685                                 LOG_ERROR("reset device failed");
1686                                 goto error_open;
1687                         }
1688
1689                         jtag_libusb_close(h->fd);
1690                         /*
1691                           Give the device one second to settle down and
1692                           reenumerate.
1693                          */
1694                         usleep(1 * 1000 * 1000);
1695                         retry_count--;
1696                 }
1697         } while (1);
1698
1699         /* compare usb vid/pid */
1700         if ((param->vid != h->vid) || (param->pid != h->pid))
1701                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1702                         param->vid, param->pid,
1703                         h->vid, h->pid);
1704
1705         /* check if mode is supported */
1706         err = ERROR_OK;
1707
1708         switch (h->transport) {
1709                 case HL_TRANSPORT_SWD:
1710                 case HL_TRANSPORT_JTAG:
1711                         if (h->version.jtag == 0)
1712                                 err = ERROR_FAIL;
1713                         break;
1714                 case HL_TRANSPORT_SWIM:
1715                         if (h->version.swim == 0)
1716                                 err = ERROR_FAIL;
1717                         break;
1718                 default:
1719                         err = ERROR_FAIL;
1720                         break;
1721         }
1722
1723         if (err != ERROR_OK) {
1724                 LOG_ERROR("mode (transport) not supported by device");
1725                 goto error_open;
1726         }
1727
1728         api = h->version.jtag_api_max;
1729
1730         LOG_INFO("using stlink api v%d", api);
1731
1732         /* set the used jtag api, this will default to the newest supported version */
1733         h->jtag_api = api;
1734
1735         /* initialize the debug hardware */
1736         err = stlink_usb_init_mode(h, param->connect_under_reset);
1737
1738         if (err != ERROR_OK) {
1739                 LOG_ERROR("init mode failed (unable to connect to the target)");
1740                 goto error_open;
1741         }
1742
1743         /* clock speed only supported by stlink/v2 and for firmware >= 22 */
1744         if (h->version.stlink >= 2 && h->version.jtag >= 22) {
1745                 LOG_DEBUG("Supported clock speeds are:");
1746
1747                 for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++)
1748                         LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed);
1749
1750                 stlink_speed(h, param->initial_interface_speed, false);
1751         }
1752
1753         /* get cpuid, so we can determine the max page size
1754          * start with a safe default */
1755         h->max_mem_packet = (1 << 10);
1756
1757         uint8_t buffer[4];
1758         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
1759         if (err == ERROR_OK) {
1760                 uint32_t cpuid = le_to_h_u32(buffer);
1761                 int i = (cpuid >> 4) & 0xf;
1762                 if (i == 4 || i == 3) {
1763                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
1764                         h->max_mem_packet = (1 << 12);
1765                 }
1766         }
1767
1768         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
1769
1770         *fd = h;
1771
1772         return ERROR_OK;
1773
1774 error_open:
1775         stlink_usb_close(h);
1776
1777         return ERROR_FAIL;
1778 }
1779
1780 int stlink_config_trace(void *handle, bool enabled, enum tpio_pin_protocol pin_protocol,
1781                         uint32_t port_size, unsigned int *trace_freq)
1782 {
1783         struct stlink_usb_handle_s *h = handle;
1784
1785         if (enabled && (h->jtag_api < 2 || pin_protocol != ASYNC_UART)) {
1786                 LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
1787                 return ERROR_FAIL;
1788         }
1789
1790         if (!enabled) {
1791                 stlink_usb_trace_disable(h);
1792                 return ERROR_OK;
1793         }
1794
1795         if (*trace_freq > STLINK_TRACE_MAX_HZ) {
1796                 LOG_ERROR("ST-LINK doesn't support SWO frequency higher than %u",
1797                           STLINK_TRACE_MAX_HZ);
1798                 return ERROR_FAIL;
1799         }
1800
1801         stlink_usb_trace_disable(h);
1802
1803         if (!*trace_freq)
1804                 *trace_freq = STLINK_TRACE_MAX_HZ;
1805         h->trace.source_hz = *trace_freq;
1806
1807         return stlink_usb_trace_enable(h);
1808 }
1809
1810 /** */
1811 struct hl_layout_api_s stlink_usb_layout_api = {
1812         /** */
1813         .open = stlink_usb_open,
1814         /** */
1815         .close = stlink_usb_close,
1816         /** */
1817         .idcode = stlink_usb_idcode,
1818         /** */
1819         .state = stlink_usb_state,
1820         /** */
1821         .reset = stlink_usb_reset,
1822         /** */
1823         .assert_srst = stlink_usb_assert_srst,
1824         /** */
1825         .run = stlink_usb_run,
1826         /** */
1827         .halt = stlink_usb_halt,
1828         /** */
1829         .step = stlink_usb_step,
1830         /** */
1831         .read_regs = stlink_usb_read_regs,
1832         /** */
1833         .read_reg = stlink_usb_read_reg,
1834         /** */
1835         .write_reg = stlink_usb_write_reg,
1836         /** */
1837         .read_mem = stlink_usb_read_mem,
1838         /** */
1839         .write_mem = stlink_usb_write_mem,
1840         /** */
1841         .write_debug_reg = stlink_usb_write_debug_reg,
1842         /** */
1843         .override_target = stlink_usb_override_target,
1844         /** */
1845         .speed = stlink_speed,
1846         /** */
1847         .config_trace = stlink_config_trace,
1848         /** */
1849         .poll_trace = stlink_usb_trace_read,
1850 };