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