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