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, 0b10);
 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 /** */
1433 static int stlink_usb_close(void *fd)
1434 {
1435         struct stlink_usb_handle_s *h;
1436
1437         h = (struct stlink_usb_handle_s *)fd;
1438
1439         if (h->fd)
1440                 jtag_libusb_close(h->fd);
1441
1442         free(fd);
1443
1444         return ERROR_OK;
1445 }
1446
1447 /** */
1448 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
1449 {
1450         int err;
1451         struct stlink_usb_handle_s *h;
1452         enum stlink_jtag_api_version api;
1453
1454         LOG_DEBUG("stlink_usb_open");
1455
1456         h = calloc(1, sizeof(struct stlink_usb_handle_s));
1457
1458         if (h == 0) {
1459                 LOG_DEBUG("malloc failed");
1460                 return ERROR_FAIL;
1461         }
1462
1463         h->transport = param->transport;
1464
1465         /* set max read/write buffer size in bytes */
1466         param->max_buffer = 512;
1467
1468         const uint16_t vids[] = { param->vid, 0 };
1469         const uint16_t pids[] = { param->pid, 0 };
1470
1471         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
1472                 param->vid, param->pid);
1473
1474         if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
1475                 LOG_ERROR("open failed");
1476                 goto error_open;
1477         }
1478
1479         jtag_libusb_set_configuration(h->fd, 0);
1480
1481         if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1482                 LOG_DEBUG("claim interface failed");
1483                 goto error_open;
1484         }
1485
1486         /* wrap version for first read */
1487         switch (param->pid) {
1488                 case 0x3744:
1489                         h->version.stlink = 1;
1490                         break;
1491                 default:
1492                         h->version.stlink = 2;
1493                         break;
1494         }
1495
1496         /* get the device version */
1497         err = stlink_usb_version(h);
1498
1499         if (err != ERROR_OK) {
1500                 LOG_ERROR("read version failed");
1501                 goto error_open;
1502         }
1503
1504         /* compare usb vid/pid */
1505         if ((param->vid != h->vid) || (param->pid != h->pid))
1506                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1507                         param->vid, param->pid,
1508                         h->vid, h->pid);
1509
1510         /* check if mode is supported */
1511         err = ERROR_OK;
1512
1513         switch (h->transport) {
1514                 case HL_TRANSPORT_SWD:
1515                 case HL_TRANSPORT_JTAG:
1516                         if (h->version.jtag == 0)
1517                                 err = ERROR_FAIL;
1518                         break;
1519                 case HL_TRANSPORT_SWIM:
1520                         if (h->version.swim == 0)
1521                                 err = ERROR_FAIL;
1522                         break;
1523                 default:
1524                         err = ERROR_FAIL;
1525                         break;
1526         }
1527
1528         if (err != ERROR_OK) {
1529                 LOG_ERROR("mode (transport) not supported by device");
1530                 goto error_open;
1531         }
1532
1533         api = h->version.jtag_api_max;
1534
1535         /* check that user has not requested certain api version
1536          * and if they have check it is supported */
1537         if ((param->api != 0) && (param->api <= h->version.jtag_api_max)) {
1538                 api = param->api;
1539                 LOG_INFO("using stlink api v%d", api);
1540         }
1541
1542         /* set the used jtag api, this will default to the newest supported version */
1543         h->jtag_api = api;
1544
1545         if (h->jtag_api >= 2 && param->trace_f && param->trace_source_hz > 0) {
1546                 uint32_t prescale;
1547
1548                 prescale = param->trace_source_hz > STLINK_TRACE_MAX_HZ ?
1549                         (param->trace_source_hz / STLINK_TRACE_MAX_HZ) - 1 : 0;
1550
1551                 h->trace.output_f = param->trace_f;
1552                 h->trace.source_hz = param->trace_source_hz;
1553                 h->trace.prescale = prescale;
1554         }
1555
1556         /* initialize the debug hardware */
1557         err = stlink_usb_init_mode(h, param->connect_under_reset);
1558
1559         if (err != ERROR_OK) {
1560                 LOG_ERROR("init mode failed");
1561                 goto error_open;
1562         }
1563
1564         *fd = h;
1565
1566         return ERROR_OK;
1567
1568 error_open:
1569         stlink_usb_close(h);
1570
1571         return ERROR_FAIL;
1572 }
1573
1574 /** */
1575 struct hl_layout_api_s stlink_usb_layout_api = {
1576         /** */
1577         .open = stlink_usb_open,
1578         /** */
1579         .close = stlink_usb_close,
1580         /** */
1581         .idcode = stlink_usb_idcode,
1582         /** */
1583         .state = stlink_usb_state,
1584         /** */
1585         .reset = stlink_usb_reset,
1586         /** */
1587         .assert_srst = stlink_usb_assert_srst,
1588         /** */
1589         .run = stlink_usb_run,
1590         /** */
1591         .halt = stlink_usb_halt,
1592         /** */
1593         .step = stlink_usb_step,
1594         /** */
1595         .read_regs = stlink_usb_read_regs,
1596         /** */
1597         .read_reg = stlink_usb_read_reg,
1598         /** */
1599         .write_reg = stlink_usb_write_reg,
1600         /** */
1601         .read_mem8 = stlink_usb_read_mem8,
1602         /** */
1603         .write_mem8 = stlink_usb_write_mem8,
1604         /** */
1605         .read_mem32 = stlink_usb_read_mem32,
1606         /** */
1607         .write_mem32 = stlink_usb_write_mem32,
1608         /** */
1609         .write_debug_reg = stlink_usb_write_debug_reg
1610 };