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