git.gag.com Git - fw/openocd/blob - src/jtag/drivers/stlink_usb.c

   1 /***************************************************************************
   2  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
   3  *   Mathias Kuester <kesmtp@freenet.de>                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2012 by Spencer Oliver                                  *
   6  *   spen@spen-soft.co.uk                                                  *
   7  *                                                                         *
   8  *   This code is based on https://github.com/texane/stlink                *
   9  *                                                                         *
  10  *   This program is free software; you can redistribute it and/or modify  *
  11  *   it under the terms of the GNU General Public License as published by  *
  12  *   the Free Software Foundation; either version 2 of the License, or     *
  13  *   (at your option) any later version.                                   *
  14  *                                                                         *
  15  *   This program is distributed in the hope that it will be useful,       *
  16  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  17  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  18  *   GNU General Public License for more details.                          *
  19  *                                                                         *
  20  *   You should have received a copy of the GNU General Public License     *
  21  *   along with this program; if not, write to the                         *
  22  *   Free Software Foundation, Inc.,                                       *
  23  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  24  ***************************************************************************/
  25
  26 #ifdef HAVE_CONFIG_H
  27 #include "config.h"
  28 #endif
  29
  30 /* project specific includes */
  31 #include <helper/binarybuffer.h>
  32 #include <jtag/interface.h>
  33 #include <jtag/hla/hla_layout.h>
  34 #include <jtag/hla/hla_transport.h>
  35 #include <jtag/hla/hla_interface.h>
  36 #include <target/target.h>
  37
  38 #include <target/cortex_m.h>
  39
  40 #include "libusb_common.h"
  41
  42 #define ENDPOINT_IN  0x80
  43 #define ENDPOINT_OUT 0x00
  44
  45 #define STLINK_WRITE_TIMEOUT 1000
  46 #define STLINK_READ_TIMEOUT 1000
  47
  48 #define STLINK_NULL_EP     0
  49 #define STLINK_RX_EP       (1|ENDPOINT_IN)
  50 #define STLINK_TX_EP       (2|ENDPOINT_OUT)
  51 #define STLINK_TRACE_EP    (3|ENDPOINT_IN)
  52 #define STLINK_SG_SIZE     (31)
  53 #define STLINK_DATA_SIZE   (4096)
  54 #define STLINK_CMD_SIZE_V2 (16)
  55 #define STLINK_CMD_SIZE_V1 (10)
  56
  57 /* the current implementation of the stlink limits
  58  * 8bit read/writes to max 64 bytes. */
  59 #define STLINK_MAX_RW8          (64)
  60
  61 enum stlink_jtag_api_version {
  62         STLINK_JTAG_API_V1 = 1,
  63         STLINK_JTAG_API_V2,
  64 };
  65
  66 /** */
  67 struct stlink_usb_version {
  68         /** */
  69         int stlink;
  70         /** */
  71         int jtag;
  72         /** */
  73         int swim;
  74         /** highest supported jtag api version */
  75         enum stlink_jtag_api_version jtag_api_max;
  76 };
  77
  78 /** */
  79 struct stlink_usb_handle_s {
  80         /** */
  81         struct jtag_libusb_device_handle *fd;
  82         /** */
  83         struct libusb_transfer *trans;
  84         /** */
  85         uint8_t cmdbuf[STLINK_SG_SIZE];
  86         /** */
  87         uint8_t cmdidx;
  88         /** */
  89         uint8_t direction;
  90         /** */
  91         uint8_t databuf[STLINK_DATA_SIZE];
  92         /** */
  93         uint32_t max_mem_packet;
  94         /** */
  95         enum hl_transports transport;
  96         /** */
  97         struct stlink_usb_version version;
  98         /** */
  99         uint16_t vid;
 100         /** */
 101         uint16_t pid;
 102         /** this is the currently used jtag api */
 103         enum stlink_jtag_api_version jtag_api;
 104         /** */
 105         struct {
 106                 /** whether SWO tracing is enabled or not */
 107                 bool enabled;
 108                 /** trace data destination file */
 109                 FILE *output_f;
 110                 /** trace module source clock (for prescaler) */
 111                 uint32_t source_hz;
 112                 /** trace module clock prescaler */
 113                 uint32_t prescale;
 114         } trace;
 115 };
 116
 117 #define STLINK_DEBUG_ERR_OK            0x80
 118 #define STLINK_DEBUG_ERR_FAULT         0x81
 119 #define STLINK_SWD_AP_WAIT             0x10
 120 #define STLINK_SWD_DP_WAIT             0x14
 121
 122 #define STLINK_CORE_RUNNING            0x80
 123 #define STLINK_CORE_HALTED             0x81
 124 #define STLINK_CORE_STAT_UNKNOWN       -1
 125
 126 #define STLINK_GET_VERSION             0xF1
 127 #define STLINK_DEBUG_COMMAND           0xF2
 128 #define STLINK_DFU_COMMAND             0xF3
 129 #define STLINK_SWIM_COMMAND            0xF4
 130 #define STLINK_GET_CURRENT_MODE        0xF5
 131 #define STLINK_GET_TARGET_VOLTAGE      0xF7
 132
 133 #define STLINK_DEV_DFU_MODE            0x00
 134 #define STLINK_DEV_MASS_MODE           0x01
 135 #define STLINK_DEV_DEBUG_MODE          0x02
 136 #define STLINK_DEV_SWIM_MODE           0x03
 137 #define STLINK_DEV_BOOTLOADER_MODE     0x04
 138 #define STLINK_DEV_UNKNOWN_MODE        -1
 139
 140 #define STLINK_DFU_EXIT                0x07
 141
 142 #define STLINK_SWIM_ENTER              0x00
 143 #define STLINK_SWIM_EXIT               0x01
 144
 145 #define STLINK_DEBUG_ENTER_JTAG            0x00
 146 #define STLINK_DEBUG_GETSTATUS             0x01
 147 #define STLINK_DEBUG_FORCEDEBUG            0x02
 148 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
 149 #define STLINK_DEBUG_APIV1_READALLREGS     0x04
 150 #define STLINK_DEBUG_APIV1_READREG         0x05
 151 #define STLINK_DEBUG_APIV1_WRITEREG        0x06
 152 #define STLINK_DEBUG_READMEM_32BIT         0x07
 153 #define STLINK_DEBUG_WRITEMEM_32BIT        0x08
 154 #define STLINK_DEBUG_RUNCORE               0x09
 155 #define STLINK_DEBUG_STEPCORE              0x0a
 156 #define STLINK_DEBUG_APIV1_SETFP           0x0b
 157 #define STLINK_DEBUG_READMEM_8BIT          0x0c
 158 #define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
 159 #define STLINK_DEBUG_APIV1_CLEARFP         0x0e
 160 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 161 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 162
 163 #define STLINK_DEBUG_ENTER_JTAG            0x00
 164 #define STLINK_DEBUG_ENTER_SWD             0xa3
 165
 166 #define STLINK_DEBUG_APIV1_ENTER           0x20
 167 #define STLINK_DEBUG_EXIT                  0x21
 168 #define STLINK_DEBUG_READCOREID            0x22
 169
 170 #define STLINK_DEBUG_APIV2_ENTER           0x30
 171 #define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
 172 #define STLINK_DEBUG_APIV2_RESETSYS        0x32
 173 #define STLINK_DEBUG_APIV2_READREG         0x33
 174 #define STLINK_DEBUG_APIV2_WRITEREG        0x34
 175 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
 176 #define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
 177
 178 #define STLINK_DEBUG_APIV2_READALLREGS     0x3A
 179 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 180 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 181
 182 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 183 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 184 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 185
 186 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 187 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 188 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 189
 190 #define STLINK_TRACE_SIZE               1024
 191 #define STLINK_TRACE_MAX_HZ             2000000
 192 #define STLINK_TRACE_MIN_VERSION        13
 193
 194 /** */
 195 enum stlink_mode {
 196         STLINK_MODE_UNKNOWN = 0,
 197         STLINK_MODE_DFU,
 198         STLINK_MODE_MASS,
 199         STLINK_MODE_DEBUG_JTAG,
 200         STLINK_MODE_DEBUG_SWD,
 201         STLINK_MODE_DEBUG_SWIM
 202 };
 203
 204 #define REQUEST_SENSE        0x03
 205 #define REQUEST_SENSE_LENGTH 18
 206
 207 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 208
 209 /** */
 210 static int stlink_usb_xfer_v1_get_status(void *handle)
 211 {
 212         struct stlink_usb_handle_s *h = 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 enum target_state stlink_usb_v2_get_status(void *handle)
 834 {
 835         int result;
 836         uint32_t status;
 837
 838         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
 839         if  (result != ERROR_OK)
 840                 return TARGET_UNKNOWN;
 841
 842         if (status & S_HALT)
 843                 return TARGET_HALTED;
 844         else if (status & S_RESET_ST)
 845                 return TARGET_RESET;
 846
 847         stlink_usb_trace_read(handle);
 848
 849         return TARGET_RUNNING;
 850 }
 851
 852 /** */
 853 static enum target_state stlink_usb_state(void *handle)
 854 {
 855         int res;
 856         struct stlink_usb_handle_s *h = handle;
 857
 858         assert(handle != NULL);
 859
 860         if (h->jtag_api == STLINK_JTAG_API_V2)
 861                 return stlink_usb_v2_get_status(handle);
 862
 863         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 864
 865         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 866         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
 867
 868         res = stlink_usb_xfer(handle, h->databuf, 2);
 869
 870         if (res != ERROR_OK)
 871                 return TARGET_UNKNOWN;
 872
 873         if (h->databuf[0] == STLINK_CORE_RUNNING)
 874                 return TARGET_RUNNING;
 875         if (h->databuf[0] == STLINK_CORE_HALTED)
 876                 return TARGET_HALTED;
 877
 878         return TARGET_UNKNOWN;
 879 }
 880
 881 /** */
 882 static int stlink_usb_reset(void *handle)
 883 {
 884         int res;
 885         struct stlink_usb_handle_s *h = handle;
 886
 887         assert(handle != NULL);
 888
 889         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 890
 891         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 892
 893         if (h->jtag_api == STLINK_JTAG_API_V1)
 894                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
 895         else
 896                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
 897
 898         res = stlink_usb_xfer(handle, h->databuf, 2);
 899
 900         if (res != ERROR_OK)
 901                 return res;
 902
 903         LOG_DEBUG("RESET: 0x%08X", h->databuf[0]);
 904
 905         /* the following is not a error under swd (using hardware srst), so return success */
 906         if (h->databuf[0] == STLINK_SWD_AP_WAIT || h->databuf[0] == STLINK_SWD_DP_WAIT)
 907                 return ERROR_OK;
 908
 909         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 910 }
 911
 912 static int stlink_usb_assert_srst(void *handle, int srst)
 913 {
 914         int res;
 915         struct stlink_usb_handle_s *h = handle;
 916
 917         assert(handle != NULL);
 918
 919         if (h->jtag_api == STLINK_JTAG_API_V1)
 920                 return ERROR_COMMAND_NOTFOUND;
 921
 922         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 923
 924         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 925         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
 926         h->cmdbuf[h->cmdidx++] = srst;
 927
 928         res = stlink_usb_xfer(handle, h->databuf, 2);
 929
 930         if (res != ERROR_OK)
 931                 return res;
 932
 933         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 934 }
 935
 936 /** */
 937 static int stlink_configure_target_trace_port(void *handle)
 938 {
 939         int res;
 940         uint32_t reg;
 941         struct stlink_usb_handle_s *h = handle;
 942
 943         assert(handle != NULL);
 944
 945         /* configure the TPI */
 946
 947         /* enable the trace subsystem */
 948         res = stlink_usb_v2_read_debug_reg(handle, DCB_DEMCR, &reg);
 949         if (res != ERROR_OK)
 950                 goto out;
 951         res = stlink_usb_write_debug_reg(handle, DCB_DEMCR, TRCENA|reg);
 952         if (res != ERROR_OK)
 953                 goto out;
 954         /* set the TPI clock prescaler */
 955         res = stlink_usb_write_debug_reg(handle, TPI_ACPR, h->trace.prescale);
 956         if (res != ERROR_OK)
 957                 goto out;
 958         /* select the pin protocol.  The STLinkv2 only supports asynchronous
 959          * UART emulation (NRZ) mode, so that's what we pick. */
 960         res = stlink_usb_write_debug_reg(handle, TPI_SPPR, 0x02);
 961         if (res != ERROR_OK)
 962                 goto out;
 963         /* disable continuous formatting */
 964         res = stlink_usb_write_debug_reg(handle, TPI_FFCR, (1<<8));
 965         if (res != ERROR_OK)
 966                 goto out;
 967
 968         /* configure the ITM */
 969
 970         /* unlock access to the ITM registers */
 971         res = stlink_usb_write_debug_reg(handle, ITM_LAR, 0xC5ACCE55);
 972         if (res != ERROR_OK)
 973                 goto out;
 974         /* enable trace with ATB ID 1 */
 975         res = stlink_usb_write_debug_reg(handle, ITM_TCR, (1<<16)|(1<<0)|(1<<2));
 976         if (res != ERROR_OK)
 977                 goto out;
 978         /* trace privilege */
 979         res = stlink_usb_write_debug_reg(handle, ITM_TPR, 1);
 980         if (res != ERROR_OK)
 981                 goto out;
 982         /* trace port enable (port 0) */
 983         res = stlink_usb_write_debug_reg(handle, ITM_TER, (1<<0));
 984         if (res != ERROR_OK)
 985                 goto out;
 986
 987         res = ERROR_OK;
 988 out:
 989         return res;
 990 }
 991
 992 /** */
 993 static void stlink_usb_trace_disable(void *handle)
 994 {
 995         int res = ERROR_OK;
 996         struct stlink_usb_handle_s *h = handle;
 997
 998         assert(handle != NULL);
 999
1000         assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
1001
1002         LOG_DEBUG("Tracing: disable\n");
1003
1004         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1005         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1006         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_STOP_TRACE_RX;
1007         res = stlink_usb_xfer(handle, h->databuf, 2);
1008
1009         if (res == ERROR_OK)
1010                 h->trace.enabled = false;
1011 }
1012
1013
1014 /** */
1015 static int stlink_usb_trace_enable(void *handle)
1016 {
1017         int res;
1018         struct stlink_usb_handle_s *h = handle;
1019
1020         assert(handle != NULL);
1021
1022         if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
1023                 uint32_t trace_hz;
1024
1025                 res = stlink_configure_target_trace_port(handle);
1026                 if (res != ERROR_OK)
1027                         LOG_ERROR("Unable to configure tracing on target\n");
1028
1029                 trace_hz = h->trace.prescale > 0 ?
1030                         h->trace.source_hz / (h->trace.prescale + 1) :
1031                         h->trace.source_hz;
1032
1033                 stlink_usb_init_buffer(handle, STLINK_RX_EP, 10);
1034
1035                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1036                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_START_TRACE_RX;
1037                 h_u16_to_le(h->cmdbuf+h->cmdidx, (uint16_t)STLINK_TRACE_SIZE);
1038                 h->cmdidx += 2;
1039                 h_u32_to_le(h->cmdbuf+h->cmdidx, trace_hz);
1040                 h->cmdidx += 4;
1041
1042                 res = stlink_usb_xfer(handle, h->databuf, 2);
1043
1044                 if (res == ERROR_OK)  {
1045                         h->trace.enabled = true;
1046                         LOG_DEBUG("Tracing: recording at %" PRIu32 "Hz\n", trace_hz);
1047                 }
1048         } else {
1049                 LOG_ERROR("Tracing is not supported by this version.");
1050                 res = ERROR_FAIL;
1051         }
1052
1053         return res;
1054 }
1055
1056 /** */
1057 static int stlink_usb_run(void *handle)
1058 {
1059         int res;
1060         struct stlink_usb_handle_s *h = handle;
1061
1062         assert(handle != NULL);
1063
1064         if (h->jtag_api == STLINK_JTAG_API_V2) {
1065                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
1066
1067                 /* Try to start tracing, if requested */
1068                 if (res == ERROR_OK && h->trace.source_hz) {
1069                         if (stlink_usb_trace_enable(handle) == ERROR_OK)
1070                                 LOG_DEBUG("Tracing: enabled\n");
1071                         else
1072                                 LOG_ERROR("Tracing: enable failed\n");
1073                 }
1074
1075                 return res;
1076         }
1077
1078         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1079
1080         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1081         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
1082
1083         res = stlink_usb_xfer(handle, h->databuf, 2);
1084
1085         if (res != ERROR_OK)
1086                 return res;
1087
1088         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1089 }
1090
1091 /** */
1092 static int stlink_usb_halt(void *handle)
1093 {
1094         int res;
1095         struct stlink_usb_handle_s *h = handle;
1096
1097         assert(handle != NULL);
1098
1099         if (h->jtag_api == STLINK_JTAG_API_V2) {
1100                 res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1101
1102                 if (res == ERROR_OK && h->trace.enabled)
1103                         stlink_usb_trace_disable(handle);
1104
1105                 return res;
1106         }
1107
1108         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1109
1110         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1111         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
1112
1113         res = stlink_usb_xfer(handle, h->databuf, 2);
1114
1115         if (res != ERROR_OK)
1116                 return res;
1117
1118         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1119 }
1120
1121 /** */
1122 static int stlink_usb_step(void *handle)
1123 {
1124         int res;
1125         struct stlink_usb_handle_s *h = handle;
1126
1127         assert(handle != NULL);
1128
1129         if (h->jtag_api == STLINK_JTAG_API_V2) {
1130                 /* TODO: this emulates the v1 api, it should really use a similar auto mask isr
1131                  * that the cortex-m3 currently does. */
1132                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
1133                 stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_MASKINTS|C_DEBUGEN);
1134                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
1135         }
1136
1137         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1138
1139         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1140         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
1141
1142         res = stlink_usb_xfer(handle, h->databuf, 2);
1143
1144         if (res != ERROR_OK)
1145                 return res;
1146
1147         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1148 }
1149
1150 /** */
1151 static int stlink_usb_read_regs(void *handle)
1152 {
1153         int res;
1154         struct stlink_usb_handle_s *h = handle;
1155
1156         assert(handle != NULL);
1157
1158         stlink_usb_init_buffer(handle, STLINK_RX_EP, 84);
1159
1160         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1161         if (h->jtag_api == STLINK_JTAG_API_V1)
1162                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
1163         else
1164                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
1165
1166         res = stlink_usb_xfer(handle, h->databuf, 84);
1167
1168         if (res != ERROR_OK)
1169                 return res;
1170
1171         return ERROR_OK;
1172 }
1173
1174 /** */
1175 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
1176 {
1177         int res;
1178         struct stlink_usb_handle_s *h = handle;
1179
1180         assert(handle != NULL);
1181
1182         stlink_usb_init_buffer(handle, STLINK_RX_EP, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1183
1184         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1185         if (h->jtag_api == STLINK_JTAG_API_V1)
1186                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
1187         else
1188                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
1189         h->cmdbuf[h->cmdidx++] = num;
1190
1191         res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
1192
1193         if (res != ERROR_OK)
1194                 return res;
1195
1196         if (h->jtag_api == STLINK_JTAG_API_V1)
1197                 *val = le_to_h_u32(h->databuf);
1198         else {
1199                 *val = le_to_h_u32(h->databuf + 4);
1200                 return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1201         }
1202
1203         return ERROR_OK;
1204 }
1205
1206 /** */
1207 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
1208 {
1209         int res;
1210         struct stlink_usb_handle_s *h = handle;
1211
1212         assert(handle != NULL);
1213
1214         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1215
1216         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1217         if (h->jtag_api == STLINK_JTAG_API_V1)
1218                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
1219         else
1220                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
1221         h->cmdbuf[h->cmdidx++] = num;
1222         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
1223         h->cmdidx += 4;
1224
1225         res = stlink_usb_xfer(handle, h->databuf, 2);
1226
1227         if (res != ERROR_OK)
1228                 return res;
1229
1230         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
1231 }
1232
1233 static int stlink_usb_get_rw_status(void *handle)
1234 {
1235         int res;
1236         struct stlink_usb_handle_s *h = handle;
1237
1238         assert(handle != NULL);
1239
1240         if (h->jtag_api == STLINK_JTAG_API_V1)
1241                 return ERROR_OK;
1242
1243         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
1244
1245         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1246         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
1247
1248         res = stlink_usb_xfer(handle, h->databuf, 2);
1249
1250         if (res != ERROR_OK)
1251                 return res;
1252
1253         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
1254 }
1255
1256 /** */
1257 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1258                           uint8_t *buffer)
1259 {
1260         int res;
1261         uint16_t read_len = len;
1262         struct stlink_usb_handle_s *h = handle;
1263
1264         assert(handle != NULL);
1265
1266         /* max 8bit read/write is 64bytes */
1267         if (len > STLINK_MAX_RW8) {
1268                 LOG_DEBUG("max buffer length exceeded");
1269                 return ERROR_FAIL;
1270         }
1271
1272         stlink_usb_init_buffer(handle, STLINK_RX_EP, read_len);
1273
1274         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1275         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
1276         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1277         h->cmdidx += 4;
1278         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1279         h->cmdidx += 2;
1280
1281         /* we need to fix read length for single bytes */
1282         if (read_len == 1)
1283                 read_len++;
1284
1285         res = stlink_usb_xfer(handle, h->databuf, read_len);
1286
1287         if (res != ERROR_OK)
1288                 return res;
1289
1290         memcpy(buffer, h->databuf, len);
1291
1292         return stlink_usb_get_rw_status(handle);
1293 }
1294
1295 /** */
1296 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1297                            const uint8_t *buffer)
1298 {
1299         int res;
1300         struct stlink_usb_handle_s *h = handle;
1301
1302         assert(handle != NULL);
1303
1304         /* max 8bit read/write is 64bytes */
1305         if (len > STLINK_MAX_RW8) {
1306                 LOG_DEBUG("max buffer length exceeded");
1307                 return ERROR_FAIL;
1308         }
1309
1310         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1311
1312         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1313         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1314         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1315         h->cmdidx += 4;
1316         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1317         h->cmdidx += 2;
1318
1319         res = stlink_usb_xfer(handle, buffer, len);
1320
1321         if (res != ERROR_OK)
1322                 return res;
1323
1324         return stlink_usb_get_rw_status(handle);
1325 }
1326
1327 /** */
1328 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1329                           uint8_t *buffer)
1330 {
1331         int res;
1332         struct stlink_usb_handle_s *h = handle;
1333
1334         assert(handle != NULL);
1335
1336         /* data must be a multiple of 4 and word aligned */
1337         if (len % 4 || addr % 4) {
1338                 LOG_DEBUG("Invalid data alignment");
1339                 return ERROR_TARGET_UNALIGNED_ACCESS;
1340         }
1341
1342         stlink_usb_init_buffer(handle, STLINK_RX_EP, len);
1343
1344         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1345         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1346         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1347         h->cmdidx += 4;
1348         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1349         h->cmdidx += 2;
1350
1351         res = stlink_usb_xfer(handle, h->databuf, len);
1352
1353         if (res != ERROR_OK)
1354                 return res;
1355
1356         memcpy(buffer, h->databuf, len);
1357
1358         return stlink_usb_get_rw_status(handle);
1359 }
1360
1361 /** */
1362 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1363                            const uint8_t *buffer)
1364 {
1365         int res;
1366         struct stlink_usb_handle_s *h = handle;
1367
1368         assert(handle != NULL);
1369
1370         /* data must be a multiple of 4 and word aligned */
1371         if (len % 4 || addr % 4) {
1372                 LOG_DEBUG("Invalid data alignment");
1373                 return ERROR_TARGET_UNALIGNED_ACCESS;
1374         }
1375
1376         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1377
1378         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1379         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1380         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1381         h->cmdidx += 4;
1382         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1383         h->cmdidx += 2;
1384
1385         res = stlink_usb_xfer(handle, buffer, len);
1386
1387         if (res != ERROR_OK)
1388                 return res;
1389
1390         return stlink_usb_get_rw_status(handle);
1391 }
1392
1393 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
1394 {
1395         uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
1396         if (max_tar_block == 0)
1397                 max_tar_block = 4;
1398         return max_tar_block;
1399 }
1400
1401 static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
1402                 uint32_t count, uint8_t *buffer)
1403 {
1404         int retval = ERROR_OK;
1405         uint32_t bytes_remaining;
1406         struct stlink_usb_handle_s *h = handle;
1407
1408         /* calculate byte count */
1409         count *= size;
1410
1411         while (count) {
1412
1413                 bytes_remaining = (size == 4) ? \
1414                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1415
1416                 if (count < bytes_remaining)
1417                         bytes_remaining = count;
1418
1419                 /* the stlink only supports 8/32bit memory read/writes
1420                  * honour 32bit, all others will be handled as 8bit access */
1421                 if (size == 4) {
1422
1423                         /* When in jtag mode the stlink uses the auto-increment functinality.
1424                          * However it expects us to pass the data correctly, this includes
1425                          * alignment and any page boundaries. We already do this as part of the
1426                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1427                          * needs implementiong manually.
1428                          * currently this only affects jtag mode, according to ST they do single
1429                          * access in SWD mode - but this may change and so we do it for both modes */
1430
1431                         /* we first need to check for any unaligned bytes */
1432                         if (addr % 4) {
1433
1434                                 uint32_t head_bytes = 4 - (addr % 4);
1435                                 retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
1436                                 if (retval != ERROR_OK)
1437                                         return retval;
1438                                 buffer += head_bytes;
1439                                 addr += head_bytes;
1440                                 count -= head_bytes;
1441                                 bytes_remaining -= head_bytes;
1442                         }
1443
1444                         if (bytes_remaining % 4)
1445                                 retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
1446                         else
1447                                 retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
1448                 } else
1449                         retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
1450
1451                 if (retval != ERROR_OK)
1452                         return retval;
1453
1454                 buffer += bytes_remaining;
1455                 addr += bytes_remaining;
1456                 count -= bytes_remaining;
1457         }
1458
1459         return retval;
1460 }
1461
1462 static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
1463                 uint32_t count, const uint8_t *buffer)
1464 {
1465         int retval = ERROR_OK;
1466         uint32_t bytes_remaining;
1467         struct stlink_usb_handle_s *h = handle;
1468
1469         /* calculate byte count */
1470         count *= size;
1471
1472         while (count) {
1473
1474                 bytes_remaining = (size == 4) ? \
1475                                 stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
1476
1477                 if (count < bytes_remaining)
1478                         bytes_remaining = count;
1479
1480                 /* the stlink only supports 8/32bit memory read/writes
1481                  * honour 32bit, all others will be handled as 8bit access */
1482                 if (size == 4) {
1483
1484                         /* When in jtag mode the stlink uses the auto-increment functinality.
1485                          * However it expects us to pass the data correctly, this includes
1486                          * alignment and any page boundaries. We already do this as part of the
1487                          * adi_v5 implementation, but the stlink is a hla adapter and so this
1488                          * needs implementiong manually.
1489                          * currently this only affects jtag mode, according to ST they do single
1490                          * access in SWD mode - but this may change and so we do it for both modes */
1491
1492                         /* we first need to check for any unaligned bytes */
1493                         if (addr % 4) {
1494
1495                                 uint32_t head_bytes = 4 - (addr % 4);
1496                                 retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
1497                                 if (retval != ERROR_OK)
1498                                         return retval;
1499                                 buffer += head_bytes;
1500                                 addr += head_bytes;
1501                                 count -= head_bytes;
1502                                 bytes_remaining -= head_bytes;
1503                         }
1504
1505                         if (bytes_remaining % 4)
1506                                 retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
1507                         else
1508                                 retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
1509
1510                 } else
1511                         retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
1512                 if (retval != ERROR_OK)
1513                         return retval;
1514
1515                 buffer += bytes_remaining;
1516                 addr += bytes_remaining;
1517                 count -= bytes_remaining;
1518         }
1519
1520         return retval;
1521 }
1522
1523 /** */
1524 static int stlink_usb_close(void *fd)
1525 {
1526         struct stlink_usb_handle_s *h = fd;
1527
1528         if (h->fd)
1529                 jtag_libusb_close(h->fd);
1530
1531         free(fd);
1532
1533         return ERROR_OK;
1534 }
1535
1536 /** */
1537 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
1538 {
1539         int err, retry_count = 1;
1540         struct stlink_usb_handle_s *h;
1541         enum stlink_jtag_api_version api;
1542
1543         LOG_DEBUG("stlink_usb_open");
1544
1545         h = calloc(1, sizeof(struct stlink_usb_handle_s));
1546
1547         if (h == 0) {
1548                 LOG_DEBUG("malloc failed");
1549                 return ERROR_FAIL;
1550         }
1551
1552         h->transport = param->transport;
1553
1554         const uint16_t vids[] = { param->vid, 0 };
1555         const uint16_t pids[] = { param->pid, 0 };
1556
1557         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
1558                 param->vid, param->pid);
1559
1560         /*
1561           On certain host USB configurations(e.g. MacBook Air)
1562           STLINKv2 dongle seems to have its FW in a funky state if,
1563           after plugging it in, you try to use openocd with it more
1564           then once (by launching and closing openocd). In cases like
1565           that initial attempt to read the FW info via
1566           stlink_usb_version will fail and the device has to be reset
1567           in order to become operational.
1568          */
1569         do {
1570                 if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
1571                         LOG_ERROR("open failed");
1572                         goto error_open;
1573                 }
1574
1575                 jtag_libusb_set_configuration(h->fd, 0);
1576
1577                 if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1578                         LOG_DEBUG("claim interface failed");
1579                         goto error_open;
1580                 }
1581
1582                 /* wrap version for first read */
1583                 switch (param->pid) {
1584                 case 0x3744:
1585                         h->version.stlink = 1;
1586                         break;
1587                 default:
1588                         h->version.stlink = 2;
1589                         break;
1590                 }
1591
1592                 /* get the device version */
1593                 err = stlink_usb_version(h);
1594
1595                 if (err == ERROR_OK) {
1596                         break;
1597                 } else if (h->version.stlink == 1 ||
1598                            retry_count == 0) {
1599                         LOG_ERROR("read version failed");
1600                         goto error_open;
1601                 } else {
1602                         err = jtag_libusb_release_interface(h->fd, 0);
1603                         if (err != ERROR_OK) {
1604                                 LOG_ERROR("release interface failed");
1605                                 goto error_open;
1606                         }
1607
1608                         err = jtag_libusb_reset_device(h->fd);
1609                         if (err != ERROR_OK) {
1610                                 LOG_ERROR("reset device failed");
1611                                 goto error_open;
1612                         }
1613
1614                         jtag_libusb_close(h->fd);
1615                         /*
1616                           Give the device one second to settle down and
1617                           reenumerate.
1618                          */
1619                         usleep(1 * 1000 * 1000);
1620                         retry_count--;
1621                 }
1622         } while (1);
1623
1624         /* compare usb vid/pid */
1625         if ((param->vid != h->vid) || (param->pid != h->pid))
1626                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1627                         param->vid, param->pid,
1628                         h->vid, h->pid);
1629
1630         /* check if mode is supported */
1631         err = ERROR_OK;
1632
1633         switch (h->transport) {
1634                 case HL_TRANSPORT_SWD:
1635                 case HL_TRANSPORT_JTAG:
1636                         if (h->version.jtag == 0)
1637                                 err = ERROR_FAIL;
1638                         break;
1639                 case HL_TRANSPORT_SWIM:
1640                         if (h->version.swim == 0)
1641                                 err = ERROR_FAIL;
1642                         break;
1643                 default:
1644                         err = ERROR_FAIL;
1645                         break;
1646         }
1647
1648         if (err != ERROR_OK) {
1649                 LOG_ERROR("mode (transport) not supported by device");
1650                 goto error_open;
1651         }
1652
1653         api = h->version.jtag_api_max;
1654
1655         LOG_INFO("using stlink api v%d", api);
1656
1657         /* set the used jtag api, this will default to the newest supported version */
1658         h->jtag_api = api;
1659
1660         if (h->jtag_api >= 2 && param->trace_source_hz > 0) {
1661                 uint32_t prescale;
1662
1663                 prescale = param->trace_source_hz > STLINK_TRACE_MAX_HZ ?
1664                         (param->trace_source_hz / STLINK_TRACE_MAX_HZ) - 1 : 0;
1665
1666                 h->trace.output_f = param->trace_f;
1667                 h->trace.source_hz = param->trace_source_hz;
1668                 h->trace.prescale = prescale;
1669         }
1670
1671         /* initialize the debug hardware */
1672         err = stlink_usb_init_mode(h, param->connect_under_reset);
1673
1674         if (err != ERROR_OK) {
1675                 LOG_ERROR("init mode failed");
1676                 goto error_open;
1677         }
1678
1679         /* get cpuid, so we can determine the max page size
1680          * start with a safe default */
1681         h->max_mem_packet = (1 << 10);
1682
1683         uint8_t buffer[4];
1684         err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
1685         if (err == ERROR_OK) {
1686                 uint32_t cpuid = le_to_h_u32(buffer);
1687                 int i = (cpuid >> 4) & 0xf;
1688                 if (i == 4 || i == 3) {
1689                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
1690                         h->max_mem_packet = (1 << 12);
1691                 }
1692         }
1693
1694         LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
1695
1696         *fd = h;
1697
1698         return ERROR_OK;
1699
1700 error_open:
1701         stlink_usb_close(h);
1702
1703         return ERROR_FAIL;
1704 }
1705
1706 /** */
1707 struct hl_layout_api_s stlink_usb_layout_api = {
1708         /** */
1709         .open = stlink_usb_open,
1710         /** */
1711         .close = stlink_usb_close,
1712         /** */
1713         .idcode = stlink_usb_idcode,
1714         /** */
1715         .state = stlink_usb_state,
1716         /** */
1717         .reset = stlink_usb_reset,
1718         /** */
1719         .assert_srst = stlink_usb_assert_srst,
1720         /** */
1721         .run = stlink_usb_run,
1722         /** */
1723         .halt = stlink_usb_halt,
1724         /** */
1725         .step = stlink_usb_step,
1726         /** */
1727         .read_regs = stlink_usb_read_regs,
1728         /** */
1729         .read_reg = stlink_usb_read_reg,
1730         /** */
1731         .write_reg = stlink_usb_write_reg,
1732         /** */
1733         .read_mem = stlink_usb_read_mem,
1734         /** */
1735         .write_mem = stlink_usb_write_mem,
1736         /** */
1737         .write_debug_reg = stlink_usb_write_debug_reg
1738 };