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 #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                                         if (h->trace.output_f) {
 859                                                 /* Log retrieved trace output */
 860                                                 if (fwrite(buf, 1, size, h->trace.output_f) > 0)
 861                                                         fflush(h->trace.output_f);
 862                                         }
 863                                 }
 864                         }
 865                 }
 866         }
 867 }
 868
 869 static enum target_state stlink_usb_v2_get_status(void *handle)
 870 {
 871         int result;
 872         uint32_t status;
 873
 874         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
 875         if  (result != ERROR_OK)
 876                 return TARGET_UNKNOWN;
 877
 878         if (status & S_HALT)
 879                 return TARGET_HALTED;
 880         else if (status & S_RESET_ST)
 881                 return TARGET_RESET;
 882
 883         stlink_usb_trace_read(handle);
 884
 885         return TARGET_RUNNING;
 886 }
 887
 888 /** */
 889 static enum target_state stlink_usb_state(void *handle)
 890 {
 891         int res;
 892         struct stlink_usb_handle_s *h;
 893
 894         assert(handle != NULL);
 895
 896         h = (struct stlink_usb_handle_s *)handle;
 897
 898         if (h->jtag_api == STLINK_JTAG_API_V2)
 899                 return stlink_usb_v2_get_status(handle);
 900
 901         stlink_usb_init_buffer(handle, STLINK_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         return TARGET_UNKNOWN;
 917 }
 918
 919 /** */
 920 static int stlink_usb_reset(void *handle)
 921 {
 922         int res;
 923         struct stlink_usb_handle_s *h;
 924
 925         assert(handle != NULL);
 926
 927         h = (struct stlink_usb_handle_s *)handle;
 928
 929         stlink_usb_init_buffer(handle, STLINK_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;
 956
 957         assert(handle != NULL);
 958
 959         h = (struct stlink_usb_handle_s *)handle;
 960
 961         if (h->jtag_api == STLINK_JTAG_API_V1)
 962                 return ERROR_COMMAND_NOTFOUND;
 963
 964         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 965
 966         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 967         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
 968         h->cmdbuf[h->cmdidx++] = srst;
 969
 970         res = stlink_usb_xfer(handle, h->databuf, 2);
 971
 972         if (res != ERROR_OK)
 973                 return res;
 974
 975         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 976 }
 977
 978 /** */
 979 static int stlink_configure_target_trace_port(void *handle)
 980 {
 981         int res;
 982         uint32_t reg;
 983         struct stlink_usb_handle_s *h;
 984
 985         assert(handle != NULL);
 986
 987         h = (struct stlink_usb_handle_s *)handle;
 988
 989         /* configure the TPI */
 990
 991         /* enable the trace subsystem */
 992         res = stlink_usb_v2_read_debug_reg(handle, DCB_DEMCR, &reg);
 993         if (res != ERROR_OK)
 994                 goto out;
 995         res = stlink_usb_write_debug_reg(handle, DCB_DEMCR, TRCENA|reg);
 996         if (res != ERROR_OK)
 997                 goto out;
 998         /* set the TPI clock prescaler */
 999         res = stlink_usb_write_debug_reg(handle, TPI_ACPR, h->trace.prescale);
1000         if (res != ERROR_OK)
1001                 goto out;
1002         /* select the pin protocol.  The STLinkv2 only supports asynchronous
1003          * UART emulation (NRZ) mode, so that's what we pick. */
1004         res = stlink_usb_write_debug_reg(handle, TPI_SPPR, 0x02);
1005         if (res != ERROR_OK)
1006                 goto out;
1007         /* disable continuous formatting */
1008         res = stlink_usb_write_debug_reg(handle, TPI_FFCR, (1<<8));
1009         if (res != ERROR_OK)
1010                 goto out;
1011
1012         /* configure the ITM */
1013
1014         /* unlock access to the ITM registers */
1015         res = stlink_usb_write_debug_reg(handle, ITM_LAR, 0xC5ACCE55);
1016         if (res != ERROR_OK)
1017                 goto out;
1018         /* enable trace with ATB ID 1 */
1019         res = stlink_usb_write_debug_reg(handle, ITM_TCR, (1<<16)|(1<<0)|(1<<2));
1020         if (res != ERROR_OK)
1021                 goto out;
1022         /* trace privilege */
1023         res = stlink_usb_write_debug_reg(handle, ITM_TPR, 1);
1024         if (res != ERROR_OK)
1025                 goto out;
1026         /* trace port enable (port 0) */
1027         res = stlink_usb_write_debug_reg(handle, ITM_TER, (1<<0));
1028         if (res != ERROR_OK)
1029                 goto out;
1030
1031         res = ERROR_OK;
1032 out:
1033         return res;
1034 }
1035
1036 /** */
1037 static void stlink_usb_trace_disable(void *handle)
1038 {
1039         int res = ERROR_OK;
1040         struct stlink_usb_handle_s *h;
1041
1042         assert(handle != NULL);
1043
1044         h = (struct stlink_usb_handle_s *)handle;
1045
1046         assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
1047
1048         LOG_DEBUG("Tracing: disable\n");
1049
1050         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1051         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1052         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1053         res = stlink_usb_xfer(handle, h->databuf, 2);
1054
1055         if (res == ERROR_OK)
1056                 h->trace.enabled = false;
1057 }
1058
1059
1060 /** */
1061 static int stlink_usb_trace_enable(void *handle)
1062 {
1063         int res;
1064         struct stlink_usb_handle_s *h;
1065
1066         assert(handle != NULL);
1067
1068         h = (struct stlink_usb_handle_s *)handle;
1069
1070         if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
1071                 uint32_t trace_hz;
1072
1073                 res = stlink_configure_target_trace_port(handle);
1074                 if (res != ERROR_OK)
1075                         LOG_ERROR("Unable to configure tracing on target\n");
1076
1077                 trace_hz = h->trace.prescale > 0 ?
1078                         h->trace.source_hz / (h->trace.prescale + 1) :
1079                         h->trace.source_hz;
1080
1081                 stlink_usb_init_buffer(handle, STLINK_RX_EP, 10);
1082
1083                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1084                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1085                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1086                 h->cmdidx += 2;
1087                 h_u32_to_le(h->cmdbuf+h->cmdidx, trace_hz);
1088                 h->cmdidx += 4;
1089
1090                 res = stlink_usb_xfer(handle, h->databuf, 2);
1091
1092                 if (res == ERROR_OK)  {
1093                         h->trace.enabled = true;
1094                         LOG_DEBUG("Tracing: recording at %uHz\n", trace_hz);
1095                 }
1096         } else {
1097                 LOG_ERROR("Tracing is not supported by this version.");
1098                 res = ERROR_FAIL;
1099         }
1100
1101         return res;
1102 }
1103
1104 /** */
1105 static int stlink_usb_run(void *handle)
1106 {
1107         int res;
1108         struct stlink_usb_handle_s *h;
1109
1110         assert(handle != NULL);
1111
1112         h = (struct stlink_usb_handle_s *)handle;
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) {
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, STLINK_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;
1146
1147         assert(handle != NULL);
1148
1149         h = (struct stlink_usb_handle_s *)handle;
1150
1151         if (h->jtag_api == STLINK_JTAG_API_V2) {
1152                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1153
1154                 if (res == ERROR_OK && h->trace.enabled)
1155                         stlink_usb_trace_disable(handle);
1156
1157                 return res;
1158         }
1159
1160         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1161
1162         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1163         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1164
1165         res = stlink_usb_xfer(handle, h->databuf, 2);
1166
1167         if (res != ERROR_OK)
1168                 return res;
1169
1170         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1171 }
1172
1173 /** */
1174 static int stlink_usb_step(void *handle)
1175 {
1176         int res;
1177         struct stlink_usb_handle_s *h;
1178
1179         assert(handle != NULL);
1180
1181         h = (struct stlink_usb_handle_s *)handle;
1182
1183         if (h->jtag_api == STLINK_JTAG_API_V2) {
1184                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1185                  * that the cortex-m3 currently does. */
1186                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1187                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1188                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1189         }
1190
1191         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1192
1193         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1194         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1195
1196         res = stlink_usb_xfer(handle, h->databuf, 2);
1197
1198         if (res != ERROR_OK)
1199                 return res;
1200
1201         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1202 }
1203
1204 /** */
1205 static int stlink_usb_read_regs(void *handle)
1206 {
1207         int res;
1208         struct stlink_usb_handle_s *h;
1209
1210         assert(handle != NULL);
1211
1212         h = (struct stlink_usb_handle_s *)handle;
1213
1214         stlink_usb_init_buffer(handle, STLINK_RX_EP, 84);
1215
1216         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1217         if (h->jtag_api == STLINK_JTAG_API_V1)
1218                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
1219         else
1220                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
1221
1222         res = stlink_usb_xfer(handle, h->databuf, 84);
1223
1224         if (res != ERROR_OK)
1225                 return res;
1226
1227         return ERROR_OK;
1228 }
1229
1230 /** */
1231 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
1232 {
1233         int res;
1234         struct stlink_usb_handle_s *h;
1235
1236         assert(handle != NULL);
1237
1238         h = (struct stlink_usb_handle_s *)handle;
1239
1240         stlink_usb_init_buffer(handle, STLINK_RX_EP, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1241
1242         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1243         if (h->jtag_api == STLINK_JTAG_API_V1)
1244                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
1245         else
1246                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
1247         h->cmdbuf[h->cmdidx++] = num;
1248
1249         res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1250
1251         if (res != ERROR_OK)
1252                 return res;
1253
1254         if (h->jtag_api == STLINK_JTAG_API_V1)
1255                 *val = le_to_h_u32(h->databuf);
1256         else {
1257                 *val = le_to_h_u32(h->databuf + 4);
1258                 return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1259         }
1260
1261         return ERROR_OK;
1262 }
1263
1264 /** */
1265 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
1266 {
1267         int res;
1268         struct stlink_usb_handle_s *h;
1269
1270         assert(handle != NULL);
1271
1272         h = (struct stlink_usb_handle_s *)handle;
1273
1274         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1275
1276         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1277         if (h->jtag_api == STLINK_JTAG_API_V1)
1278                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
1279         else
1280                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
1281         h->cmdbuf[h->cmdidx++] = num;
1282         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1283         h->cmdidx += 4;
1284
1285         res = stlink_usb_xfer(handle, h->databuf, 2);
1286
1287         if (res != ERROR_OK)
1288                 return res;
1289
1290         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1291 }
1292
1293 static int stlink_usb_get_rw_status(void *handle)
1294 {
1295         int res;
1296         struct stlink_usb_handle_s *h;
1297
1298         assert(handle != NULL);
1299
1300         h = (struct stlink_usb_handle_s *)handle;
1301
1302         if (h->jtag_api == STLINK_JTAG_API_V1)
1303                 return ERROR_OK;
1304
1305         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1306
1307         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1308         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
1309
1310         res = stlink_usb_xfer(handle, h->databuf, 2);
1311
1312         if (res != ERROR_OK)
1313                 return res;
1314
1315         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
1316 }
1317
1318 /** */
1319 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1320                           uint8_t *buffer)
1321 {
1322         int res;
1323         uint16_t read_len = len;
1324         struct stlink_usb_handle_s *h;
1325
1326         assert(handle != NULL);
1327
1328         /* max 8bit read/write is 64bytes */
1329         if (len > STLINK_MAX_RW8) {
1330                 LOG_DEBUG("max buffer length exceeded");
1331                 return ERROR_FAIL;
1332         }
1333
1334         h = (struct stlink_usb_handle_s *)handle;
1335
1336         stlink_usb_init_buffer(handle, STLINK_RX_EP, read_len);
1337
1338         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1339         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
1340         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1341         h->cmdidx += 4;
1342         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1343         h->cmdidx += 2;
1344
1345         /* we need to fix read length for single bytes */
1346         if (read_len == 1)
1347                 read_len++;
1348
1349         res = stlink_usb_xfer(handle, h->databuf, read_len);
1350
1351         if (res != ERROR_OK)
1352                 return res;
1353
1354         memcpy(buffer, h->databuf, len);
1355
1356         return stlink_usb_get_rw_status(handle);
1357 }
1358
1359 /** */
1360 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1361                            const uint8_t *buffer)
1362 {
1363         int res;
1364         struct stlink_usb_handle_s *h;
1365
1366         assert(handle != NULL);
1367
1368         /* max 8bit read/write is 64bytes */
1369         if (len > STLINK_MAX_RW8) {
1370                 LOG_DEBUG("max buffer length exceeded");
1371                 return ERROR_FAIL;
1372         }
1373
1374         h = (struct stlink_usb_handle_s *)handle;
1375
1376         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1377
1378         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1379         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1380         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1381         h->cmdidx += 4;
1382         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1383         h->cmdidx += 2;
1384
1385         res = stlink_usb_xfer(handle, buffer, len);
1386
1387         if (res != ERROR_OK)
1388                 return res;
1389
1390         return stlink_usb_get_rw_status(handle);
1391 }
1392
1393 /** */
1394 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1395                           uint8_t *buffer)
1396 {
1397         int res;
1398         struct stlink_usb_handle_s *h;
1399
1400         assert(handle != NULL);
1401
1402         /* data must be a multiple of 4 and word aligned */
1403         if (len % 4 || addr % 4) {
1404                 LOG_DEBUG("Invalid data alignment");
1405                 return ERROR_TARGET_UNALIGNED_ACCESS;
1406         }
1407
1408         h = (struct stlink_usb_handle_s *)handle;
1409
1410         stlink_usb_init_buffer(handle, STLINK_RX_EP, len);
1411
1412         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1413         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1414         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1415         h->cmdidx += 4;
1416         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1417         h->cmdidx += 2;
1418
1419         res = stlink_usb_xfer(handle, h->databuf, len);
1420
1421         if (res != ERROR_OK)
1422                 return res;
1423
1424         memcpy(buffer, h->databuf, len);
1425
1426         return stlink_usb_get_rw_status(handle);
1427 }
1428
1429 /** */
1430 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1431                            const uint8_t *buffer)
1432 {
1433         int res;
1434         struct stlink_usb_handle_s *h;
1435
1436         assert(handle != NULL);
1437
1438         /* data must be a multiple of 4 and word aligned */
1439         if (len % 4 || addr % 4) {
1440                 LOG_DEBUG("Invalid data alignment");
1441                 return ERROR_TARGET_UNALIGNED_ACCESS;
1442         }
1443
1444         h = (struct stlink_usb_handle_s *)handle;
1445
1446         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1447
1448         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1449         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1450         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1451         h->cmdidx += 4;
1452         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1453         h->cmdidx += 2;
1454
1455         res = stlink_usb_xfer(handle, buffer, len);
1456
1457         if (res != ERROR_OK)
1458                 return res;
1459
1460         return stlink_usb_get_rw_status(handle);
1461 }
1462
1463 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
1464 {
1465         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
1466         if (max_tar_block == 0)
1467                 max_tar_block = 4;
1468         return max_tar_block;
1469 }
1470
1471 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
1472                 uint32_t count, uint8_t *buffer)
1473 {
1474         int retval = ERROR_OK;
1475         uint32_t bytes_remaining;
1476         struct stlink_usb_handle_s *h = (struct stlink_usb_handle_s *)handle;
1477
1478         /* calculate byte count */
1479         count *= size;
1480
1481         while (count) {
1482
1483                 bytes_remaining = (size == 4) ? \
1484                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1485
1486                 if (count < bytes_remaining)
1487                         bytes_remaining = count;
1488
1489                 /* the stlink only supports 8/32bit memory read/writes
1490                  * honour 32bit, all others will be handled as 8bit access */
1491                 if (size == 4) {
1492
1493                         /* When in jtag mode the stlink uses the auto-increment functinality.
1494                          * However it expects us to pass the data correctly, this includes
1495                          * alignment and any page boundaries. We already do this as part of the
1496                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1497                          * needs implementiong manually.
1498                          * currently this only affects jtag mode, according to ST they do single
1499                          * access in SWD mode - but this may change and so we do it for both modes */
1500
1501                         /* we first need to check for any unaligned bytes */
1502                         if (addr % 4) {
1503
1504                                 uint32_t head_bytes = 4 - (addr % 4);
1505                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
1506                                 if (retval != ERROR_OK)
1507                                         return retval;
1508                                 buffer += head_bytes;
1509                                 addr += head_bytes;
1510                                 count -= head_bytes;
1511                                 bytes_remaining -= head_bytes;
1512                         }
1513
1514                         if (bytes_remaining % 4)
1515                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
1516                         else
1517                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
1518                 } else
1519                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
1520
1521                 if (retval != ERROR_OK)
1522                         return retval;
1523
1524                 buffer += bytes_remaining;
1525                 addr += bytes_remaining;
1526                 count -= bytes_remaining;
1527         }
1528
1529         return retval;
1530 }
1531
1532 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
1533                 uint32_t count, const uint8_t *buffer)
1534 {
1535         int retval = ERROR_OK;
1536         uint32_t bytes_remaining;
1537         struct stlink_usb_handle_s *h = (struct stlink_usb_handle_s *)handle;
1538
1539         /* calculate byte count */
1540         count *= size;
1541
1542         while (count) {
1543
1544                 bytes_remaining = (size == 4) ? \
1545                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1546
1547                 if (count < bytes_remaining)
1548                         bytes_remaining = count;
1549
1550                 /* the stlink only supports 8/32bit memory read/writes
1551                  * honour 32bit, all others will be handled as 8bit access */
1552                 if (size == 4) {
1553
1554                         /* When in jtag mode the stlink uses the auto-increment functinality.
1555                          * However it expects us to pass the data correctly, this includes
1556                          * alignment and any page boundaries. We already do this as part of the
1557                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1558                          * needs implementiong manually.
1559                          * currently this only affects jtag mode, according to ST they do single
1560                          * access in SWD mode - but this may change and so we do it for both modes */
1561
1562                         /* we first need to check for any unaligned bytes */
1563                         if (addr % 4) {
1564
1565                                 uint32_t head_bytes = 4 - (addr % 4);
1566                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
1567                                 if (retval != ERROR_OK)
1568                                         return retval;
1569                                 buffer += head_bytes;
1570                                 addr += head_bytes;
1571                                 count -= head_bytes;
1572                                 bytes_remaining -= head_bytes;
1573                         }
1574
1575                         if (bytes_remaining % 4)
1576                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
1577                         else
1578                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
1579
1580                 } else
1581                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
1582                 if (retval != ERROR_OK)
1583                         return retval;
1584
1585                 buffer += bytes_remaining;
1586                 addr += bytes_remaining;
1587                 count -= bytes_remaining;
1588         }
1589
1590         return retval;
1591 }
1592
1593 /** */
1594 static int stlink_usb_close(void *fd)
1595 {
1596         struct stlink_usb_handle_s *h;
1597
1598         h = (struct stlink_usb_handle_s *)fd;
1599
1600         if (h->fd)
1601                 jtag_libusb_close(h->fd);
1602
1603         free(fd);
1604
1605         return ERROR_OK;
1606 }
1607
1608 /** */
1609 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
1610 {
1611         int err, retry_count = 1;
1612         struct stlink_usb_handle_s *h;
1613         enum stlink_jtag_api_version api;
1614
1615         LOG_DEBUG("stlink_usb_open");
1616
1617         h = calloc(1, sizeof(struct stlink_usb_handle_s));
1618
1619         if (h == 0) {
1620                 LOG_DEBUG("malloc failed");
1621                 return ERROR_FAIL;
1622         }
1623
1624         h->transport = param->transport;
1625
1626         const uint16_t vids[] = { param->vid, 0 };
1627         const uint16_t pids[] = { param->pid, 0 };
1628
1629         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
1630                 param->vid, param->pid);
1631
1632         /*
1633           On certain host USB configurations(e.g. MacBook Air)
1634           STLINKv2 dongle seems to have its FW in a funky state if,
1635           after plugging it in, you try to use openocd with it more
1636           then once (by launching and closing openocd). In cases like
1637           that initial attempt to read the FW info via
1638           stlink_usb_version will fail and the device has to be reset
1639           in order to become operational.
1640          */
1641         do {
1642                 if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
1643                         LOG_ERROR("open failed");
1644                         goto error_open;
1645                 }
1646
1647                 jtag_libusb_set_configuration(h->fd, 0);
1648
1649                 if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1650                         LOG_DEBUG("claim interface failed");
1651                         goto error_open;
1652                 }
1653
1654                 /* wrap version for first read */
1655                 switch (param->pid) {
1656                 case 0x3744:
1657                         h->version.stlink = 1;
1658                         break;
1659                 default:
1660                         h->version.stlink = 2;
1661                         break;
1662                 }
1663
1664                 /* get the device version */
1665                 err = stlink_usb_version(h);
1666
1667                 if (err == ERROR_OK) {
1668                         break;
1669                 } else if (h->version.stlink == 1 ||
1670                            retry_count == 0) {
1671                         LOG_ERROR("read version failed");
1672                         goto error_open;
1673                 } else {
1674                         err = jtag_libusb_release_interface(h->fd, 0);
1675                         if (err != ERROR_OK) {
1676                                 LOG_ERROR("release interface failed");
1677                                 goto error_open;
1678                         }
1679
1680                         err = jtag_libusb_reset_device(h->fd);
1681                         if (err != ERROR_OK) {
1682                                 LOG_ERROR("reset device failed");
1683                                 goto error_open;
1684                         }
1685
1686                         jtag_libusb_close(h->fd);
1687                         /*
1688                           Give the device one second to settle down and
1689                           reenumerate.
1690                          */
1691                         usleep(1 * 1000 * 1000);
1692                         retry_count--;
1693                 }
1694         } while (1);
1695
1696         /* compare usb vid/pid */
1697         if ((param->vid != h->vid) || (param->pid != h->pid))
1698                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1699                         param->vid, param->pid,
1700                         h->vid, h->pid);
1701
1702         /* check if mode is supported */
1703         err = ERROR_OK;
1704
1705         switch (h->transport) {
1706                 case HL_TRANSPORT_SWD:
1707                 case HL_TRANSPORT_JTAG:
1708                         if (h->version.jtag == 0)
1709                                 err = ERROR_FAIL;
1710                         break;
1711                 case HL_TRANSPORT_SWIM:
1712                         if (h->version.swim == 0)
1713                                 err = ERROR_FAIL;
1714                         break;
1715                 default:
1716                         err = ERROR_FAIL;
1717                         break;
1718         }
1719
1720         if (err != ERROR_OK) {
1721                 LOG_ERROR("mode (transport) not supported by device");
1722                 goto error_open;
1723         }
1724
1725         api = h->version.jtag_api_max;
1726
1727         /* check that user has not requested certain api version
1728          * and if they have check it is supported */
1729         if ((param->api != 0) && (param->api <= h->version.jtag_api_max)) {
1730                 api = param->api;
1731                 LOG_INFO("using stlink api v%d", api);
1732         }
1733
1734         /* set the used jtag api, this will default to the newest supported version */
1735         h->jtag_api = api;
1736
1737         if (h->jtag_api >= 2 && param->trace_source_hz > 0) {
1738                 uint32_t prescale;
1739
1740                 prescale = param->trace_source_hz > STLINK_TRACE_MAX_HZ ?
1741                         (param->trace_source_hz / STLINK_TRACE_MAX_HZ) - 1 : 0;
1742
1743                 h->trace.output_f = param->trace_f;
1744                 h->trace.source_hz = param->trace_source_hz;
1745                 h->trace.prescale = prescale;
1746         }
1747
1748         /* initialize the debug hardware */
1749         err = stlink_usb_init_mode(h, param->connect_under_reset);
1750
1751         if (err != ERROR_OK) {
1752                 LOG_ERROR("init mode failed");
1753                 goto error_open;
1754         }
1755
1756         /* get cpuid, so we can determine the max page size
1757          * start with a safe default */
1758         h->max_mem_packet = (1 << 10);
1759
1760         uint8_t buffer[4];
1761         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
1762         if (err == ERROR_OK) {
1763                 uint32_t cpuid = le_to_h_u32(buffer);
1764                 int i = (cpuid >> 4) & 0xf;
1765                 if (i == 4 || i == 3) {
1766                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
1767                         h->max_mem_packet = (1 << 12);
1768                 }
1769         }
1770
1771         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
1772
1773         *fd = h;
1774
1775         return ERROR_OK;
1776
1777 error_open:
1778         stlink_usb_close(h);
1779
1780         return ERROR_FAIL;
1781 }
1782
1783 /** */
1784 struct hl_layout_api_s stlink_usb_layout_api = {
1785         /** */
1786         .open = stlink_usb_open,
1787         /** */
1788         .close = stlink_usb_close,
1789         /** */
1790         .idcode = stlink_usb_idcode,
1791         /** */
1792         .state = stlink_usb_state,
1793         /** */
1794         .reset = stlink_usb_reset,
1795         /** */
1796         .assert_srst = stlink_usb_assert_srst,
1797         /** */
1798         .run = stlink_usb_run,
1799         /** */
1800         .halt = stlink_usb_halt,
1801         /** */
1802         .step = stlink_usb_step,
1803         /** */
1804         .read_regs = stlink_usb_read_regs,
1805         /** */
1806         .read_reg = stlink_usb_read_reg,
1807         /** */
1808         .write_reg = stlink_usb_write_reg,
1809         /** */
1810         .read_mem = stlink_usb_read_mem,
1811         /** */
1812         .write_mem = stlink_usb_write_mem,
1813         /** */
1814         .write_debug_reg = stlink_usb_write_debug_reg
1815 };