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