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