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