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