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

   1 /***************************************************************************
   2  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
   3  *   Mathias Kuester <kesmtp@freenet.de>                                   *
   4  *                                                                         *
   5  *   This code is based on https://github.com/texane/stlink                *
   6  *                                                                         *
   7  *   This program is free software; you can redistribute it and/or modify  *
   8  *   it under the terms of the GNU General Public License as published by  *
   9  *   the Free Software Foundation; either version 2 of the License, or     *
  10  *   (at your option) any later version.                                   *
  11  *                                                                         *
  12  *   This program is distributed in the hope that it will be useful,       *
  13  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  14  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  15  *   GNU General Public License for more details.                          *
  16  *                                                                         *
  17  *   You should have received a copy of the GNU General Public License     *
  18  *   along with this program; if not, write to the                         *
  19  *   Free Software Foundation, Inc.,                                       *
  20  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  21  ***************************************************************************/
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 /* project specific includes */
  28 #include <helper/binarybuffer.h>
  29 #include <jtag/interface.h>
  30 #include <jtag/stlink/stlink_layout.h>
  31 #include <jtag/stlink/stlink_transport.h>
  32 #include <jtag/stlink/stlink_interface.h>
  33 #include <target/target.h>
  34
  35 #include <target/cortex_m.h>
  36
  37 #include "libusb_common.h"
  38
  39 #define ENDPOINT_IN     0x80
  40 #define ENDPOINT_OUT    0x00
  41
  42 #define STLINK_NULL_EP          0
  43 #define STLINK_RX_EP            (1|ENDPOINT_IN)
  44 #define STLINK_TX_EP            (2|ENDPOINT_OUT)
  45 #define STLINK_SG_SIZE          (31)
  46 #define STLINK_DATA_SIZE        (4*128)
  47 #define STLINK_CMD_SIZE_V2      (16)
  48 #define STLINK_CMD_SIZE_V1      (10)
  49
  50 enum stlink_jtag_api_version {
  51         STLINK_JTAG_API_V1 = 0,
  52         STLINK_JTAG_API_V2,
  53 };
  54
  55 /** */
  56 struct stlink_usb_version {
  57         /** */
  58         int stlink;
  59         /** */
  60         int jtag;
  61         /** */
  62         int swim;
  63         /** highest supported jtag api version */
  64         enum stlink_jtag_api_version jtag_api_max;
  65 };
  66
  67 /** */
  68 struct stlink_usb_handle_s {
  69         /** */
  70         struct jtag_libusb_device_handle *fd;
  71         /** */
  72         struct libusb_transfer *trans;
  73         /** */
  74         uint8_t cmdbuf[STLINK_SG_SIZE];
  75         /** */
  76         uint8_t cmdidx;
  77         /** */
  78         uint8_t direction;
  79         /** */
  80         uint8_t databuf[STLINK_DATA_SIZE];
  81         /** */
  82         enum stlink_transports transport;
  83         /** */
  84         struct stlink_usb_version version;
  85         /** */
  86         uint16_t vid;
  87         /** */
  88         uint16_t pid;
  89         /** this is the currently used jtag api */
  90         enum stlink_jtag_api_version jtag_api;
  91 };
  92
  93 #define STLINK_DEBUG_ERR_OK                     0x80
  94 #define STLINK_DEBUG_ERR_FAULT                  0x81
  95 #define STLINK_CORE_RUNNING                     0x80
  96 #define STLINK_CORE_HALTED                      0x81
  97 #define STLINK_CORE_STAT_UNKNOWN                -1
  98
  99 #define STLINK_GET_VERSION                      0xF1
 100 #define STLINK_DEBUG_COMMAND                    0xF2
 101 #define STLINK_DFU_COMMAND                      0xF3
 102 #define STLINK_SWIM_COMMAND                     0xF4
 103 #define STLINK_GET_CURRENT_MODE                 0xF5
 104
 105 #define STLINK_DEV_DFU_MODE                     0x00
 106 #define STLINK_DEV_MASS_MODE                    0x01
 107 #define STLINK_DEV_DEBUG_MODE                   0x02
 108 #define STLINK_DEV_SWIM_MODE                    0x03
 109 #define STLINK_DEV_BOOTLOADER_MODE              0x04
 110 #define STLINK_DEV_UNKNOWN_MODE                 -1
 111
 112 #define STLINK_DFU_EXIT                         0x07
 113
 114 #define STLINK_SWIM_ENTER                       0x00
 115 #define STLINK_SWIM_EXIT                        0x01
 116
 117 #define STLINK_DEBUG_ENTER_JTAG                 0x00
 118 #define STLINK_DEBUG_GETSTATUS                  0x01
 119 #define STLINK_DEBUG_FORCEDEBUG                 0x02
 120 #define STLINK_DEBUG_APIV1_RESETSYS             0x03
 121 #define STLINK_DEBUG_APIV1_READALLREGS          0x04
 122 #define STLINK_DEBUG_APIV1_READREG              0x05
 123 #define STLINK_DEBUG_APIV1_WRITEREG             0x06
 124 #define STLINK_DEBUG_READMEM_32BIT              0x07
 125 #define STLINK_DEBUG_WRITEMEM_32BIT             0x08
 126 #define STLINK_DEBUG_RUNCORE                    0x09
 127 #define STLINK_DEBUG_STEPCORE                   0x0a
 128 #define STLINK_DEBUG_APIV1_SETFP                0x0b
 129 #define STLINK_DEBUG_READMEM_8BIT               0x0c
 130 #define STLINK_DEBUG_WRITEMEM_8BIT              0x0d
 131 #define STLINK_DEBUG_APIV1_CLEARFP              0x0e
 132 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG        0x0f
 133 #define STLINK_DEBUG_APIV1_SETWATCHPOINT        0x10
 134
 135 #define STLINK_DEBUG_ENTER_JTAG                 0x00
 136 #define STLINK_DEBUG_ENTER_SWD                  0xa3
 137
 138 #define STLINK_DEBUG_APIV1_ENTER                0x20
 139 #define STLINK_DEBUG_EXIT                       0x21
 140 #define STLINK_DEBUG_READCOREID                 0x22
 141
 142 #define STLINK_DEBUG_APIV2_ENTER                0x30
 143 #define STLINK_DEBUG_APIV2_READ_IDCODES         0x31
 144 #define STLINK_DEBUG_APIV2_RESETSYS             0x32
 145 #define STLINK_DEBUG_APIV2_READREG              0x33
 146 #define STLINK_DEBUG_APIV2_WRITEREG             0x34
 147 #define STLINK_DEBUG_APIV2_WRITEDEBUGREG        0x35
 148 #define STLINK_DEBUG_APIV2_READDEBUGREG 0x36
 149
 150 #define STLINK_DEBUG_APIV2_READALLREGS          0x3A
 151 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS      0x3B
 152 #define STLINK_DEBUG_APIV2_DRIVE_NRST           0x3C
 153
 154 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW       0x00
 155 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH      0x01
 156 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE     0x02
 157
 158 /** */
 159 enum stlink_mode {
 160         STLINK_MODE_UNKNOWN = 0,
 161         STLINK_MODE_DFU,
 162         STLINK_MODE_MASS,
 163         STLINK_MODE_DEBUG_JTAG,
 164         STLINK_MODE_DEBUG_SWD,
 165         STLINK_MODE_DEBUG_SWIM
 166 };
 167
 168 #define REQUEST_SENSE           0x03
 169 #define REQUEST_SENSE_LENGTH    18
 170
 171 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 172
 173 /** */
 174 static int stlink_usb_xfer_v1_get_status(void *handle)
 175 {
 176         struct stlink_usb_handle_s *h;
 177
 178         assert(handle != NULL);
 179
 180         h = (struct stlink_usb_handle_s *)handle;
 181
 182         /* read status */
 183         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 184
 185         if (jtag_libusb_bulk_read(h->fd, STLINK_RX_EP, (char *)h->cmdbuf,
 186                                 13, 1000) != 13)
 187                 return ERROR_FAIL;
 188
 189         uint32_t t1;
 190
 191         t1 = buf_get_u32(h->cmdbuf, 0, 32);
 192
 193         /* check for USBS */
 194         if (t1 != 0x53425355)
 195                 return ERROR_FAIL;
 196         /*
 197          * CSW status:
 198          * 0 success
 199          * 1 command failure
 200          * 2 phase error
 201          */
 202         if (h->cmdbuf[12] != 0)
 203                 return ERROR_FAIL;
 204
 205         return ERROR_OK;
 206 }
 207
 208 /** */
 209 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 210 {
 211         struct stlink_usb_handle_s *h;
 212
 213         assert(handle != NULL);
 214
 215         h = (struct stlink_usb_handle_s *)handle;
 216
 217         if (jtag_libusb_bulk_write(h->fd, STLINK_TX_EP, (char *)h->cmdbuf, cmdsize,
 218                                    1000) != cmdsize) {
 219                 return ERROR_FAIL;
 220         }
 221
 222         if (h->direction == STLINK_TX_EP && size) {
 223                 if (jtag_libusb_bulk_write(h->fd, STLINK_TX_EP, (char *)buf,
 224                                           size, 1000) != size) {
 225                         LOG_DEBUG("bulk write failed");
 226                         return ERROR_FAIL;
 227                 }
 228         } else if (h->direction == STLINK_RX_EP && size) {
 229                 if (jtag_libusb_bulk_read(h->fd, STLINK_RX_EP, (char *)buf,
 230                                           size, 1000) != size) {
 231                         LOG_DEBUG("bulk read failed");
 232                         return ERROR_FAIL;
 233                 }
 234         }
 235
 236         return ERROR_OK;
 237 }
 238
 239 /** */
 240 static int stlink_usb_xfer_v1_get_sense(void *handle)
 241 {
 242         int res;
 243         struct stlink_usb_handle_s *h;
 244
 245         assert(handle != NULL);
 246
 247         h = (struct stlink_usb_handle_s *)handle;
 248
 249         stlink_usb_init_buffer(handle, STLINK_RX_EP, 16);
 250
 251         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE;
 252         h->cmdbuf[h->cmdidx++] = 0;
 253         h->cmdbuf[h->cmdidx++] = 0;
 254         h->cmdbuf[h->cmdidx++] = 0;
 255         h->cmdbuf[h->cmdidx++] = REQUEST_SENSE_LENGTH;
 256
 257         res = stlink_usb_xfer_rw(handle, REQUEST_SENSE_LENGTH, h->databuf, 16);
 258
 259         if (res != ERROR_OK)
 260                 return res;
 261
 262         if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK)
 263                 return ERROR_FAIL;
 264
 265         return ERROR_OK;
 266 }
 267
 268 /** */
 269 static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 270 {
 271         int err, cmdsize = STLINK_CMD_SIZE_V2;
 272         struct stlink_usb_handle_s *h;
 273
 274         assert(handle != NULL);
 275
 276         h = (struct stlink_usb_handle_s *)handle;
 277
 278         if (h->version.stlink == 1)
 279                 cmdsize = STLINK_SG_SIZE;
 280
 281         err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 282
 283         if (err != ERROR_OK)
 284                 return err;
 285
 286         if (h->version.stlink == 1) {
 287                 if (stlink_usb_xfer_v1_get_status(handle) != ERROR_OK) {
 288                         /* check csw status */
 289                         if (h->cmdbuf[12] == 1) {
 290                                 LOG_DEBUG("get sense");
 291                                 if (stlink_usb_xfer_v1_get_sense(handle) != ERROR_OK)
 292                                         return ERROR_FAIL;
 293                         }
 294                         return ERROR_FAIL;
 295                 }
 296         }
 297
 298         return ERROR_OK;
 299 }
 300
 301 /** */
 302 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 303 {
 304         struct stlink_usb_handle_s *h;
 305
 306         h = (struct stlink_usb_handle_s *)handle;
 307
 308         /* fill the send buffer */
 309         strcpy((char *)h->cmdbuf, "USBC");
 310         h->cmdidx += 4;
 311         /* csw tag not used */
 312         h->cmdidx += 4;
 313         buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 314         h->cmdidx += 4;
 315         h->cmdbuf[h->cmdidx++] = (direction == STLINK_RX_EP ? ENDPOINT_IN : ENDPOINT_OUT);
 316         h->cmdbuf[h->cmdidx++] = 0; /* lun */
 317         h->cmdbuf[h->cmdidx++] = STLINK_CMD_SIZE_V1;
 318 }
 319
 320 /** */
 321 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size)
 322 {
 323         struct stlink_usb_handle_s *h;
 324
 325         h = (struct stlink_usb_handle_s *)handle;
 326
 327         h->direction = direction;
 328
 329         h->cmdidx = 0;
 330
 331         memset(h->cmdbuf, 0, STLINK_SG_SIZE);
 332         memset(h->databuf, 0, STLINK_DATA_SIZE);
 333
 334         if (h->version.stlink == 1)
 335                 stlink_usb_xfer_v1_create_cmd(handle, direction, size);
 336 }
 337
 338 static const char * const stlink_usb_error_msg[] = {
 339         "unknown"
 340 };
 341
 342 /** */
 343 static int stlink_usb_error_check(void *handle)
 344 {
 345         int res;
 346         const char *err_msg = 0;
 347         struct stlink_usb_handle_s *h;
 348
 349         assert(handle != NULL);
 350
 351         h = (struct stlink_usb_handle_s *)handle;
 352
 353         /* TODO: no error checking yet on api V1 */
 354         if (h->jtag_api == STLINK_JTAG_API_V1)
 355                 h->databuf[0] = STLINK_DEBUG_ERR_OK;
 356
 357         switch (h->databuf[0]) {
 358                 case STLINK_DEBUG_ERR_OK:
 359                         res = ERROR_OK;
 360                         break;
 361                 case STLINK_DEBUG_ERR_FAULT:
 362                 default:
 363                         err_msg = stlink_usb_error_msg[0];
 364                         res = ERROR_FAIL;
 365                         break;
 366         }
 367
 368         if (res != ERROR_OK)
 369                 LOG_DEBUG("status error: %d ('%s')", h->databuf[0], err_msg);
 370
 371         return res;
 372 }
 373
 374 /** */
 375 static int stlink_usb_version(void *handle)
 376 {
 377         int res;
 378         uint16_t v;
 379         struct stlink_usb_handle_s *h;
 380
 381         assert(handle != NULL);
 382
 383         h = (struct stlink_usb_handle_s *)handle;
 384
 385         stlink_usb_init_buffer(handle, STLINK_RX_EP, 6);
 386
 387         h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION;
 388
 389         res = stlink_usb_xfer(handle, h->databuf, 6);
 390
 391         if (res != ERROR_OK)
 392                 return res;
 393
 394         v = (h->databuf[0] << 8) | h->databuf[1];
 395
 396         h->version.stlink = (v >> 12) & 0x0f;
 397         h->version.jtag = (v >> 6) & 0x3f;
 398         h->version.swim = v & 0x3f;
 399         h->vid = buf_get_u32(h->databuf, 16, 16);
 400         h->pid = buf_get_u32(h->databuf, 32, 16);
 401
 402         /* set the supported jtag api version
 403          * V1 doesn't support API V2 at all
 404          * V2 support API V2 since JTAG V13
 405          */
 406         if ((h->version.stlink == 2) && (h->version.jtag > 12))
 407                 h->version.jtag_api_max = STLINK_JTAG_API_V2;
 408         else
 409                 h->version.jtag_api_max = STLINK_JTAG_API_V1;
 410
 411         LOG_DEBUG("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
 412                 h->version.stlink,
 413                 h->version.jtag,
 414                 (h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2,
 415                 h->version.swim,
 416                 h->vid,
 417                 h->pid);
 418
 419         return ERROR_OK;
 420 }
 421
 422 /** */
 423 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 424 {
 425         int res;
 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, 2);
 433
 434         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
 435
 436         res = stlink_usb_xfer(handle, h->databuf, 2);
 437
 438         if (res != ERROR_OK)
 439                 return res;
 440
 441         *mode = h->databuf[0];
 442
 443         return ERROR_OK;
 444 }
 445
 446 /** */
 447 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 448 {
 449         int res;
 450         int rx_size = 0;
 451         struct stlink_usb_handle_s *h;
 452
 453         assert(handle != NULL);
 454
 455         h = (struct stlink_usb_handle_s *)handle;
 456
 457         /* on api V2 we are able the read the latest command
 458          * status
 459          * TODO: we need the test on api V1 too
 460          */
 461         if (h->jtag_api == STLINK_JTAG_API_V2)
 462                 rx_size = 2;
 463
 464         stlink_usb_init_buffer(handle, STLINK_RX_EP, rx_size);
 465
 466         switch (type) {
 467                 case STLINK_MODE_DEBUG_JTAG:
 468                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 469                         if (h->jtag_api == STLINK_JTAG_API_V1)
 470                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 471                         else
 472                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
 473                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG;
 474                         break;
 475                 case STLINK_MODE_DEBUG_SWD:
 476                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 477                         if (h->jtag_api == STLINK_JTAG_API_V1)
 478                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 479                         else
 480                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
 481                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD;
 482                         break;
 483                 case STLINK_MODE_DEBUG_SWIM:
 484                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 485                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
 486                         break;
 487                 case STLINK_MODE_DFU:
 488                 case STLINK_MODE_MASS:
 489                 default:
 490                         return ERROR_FAIL;
 491         }
 492
 493         res = stlink_usb_xfer(handle, h->databuf, rx_size);
 494
 495         if (res != ERROR_OK)
 496                 return res;
 497
 498         res = stlink_usb_error_check(h);
 499
 500         return res;
 501 }
 502
 503 /** */
 504 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 505 {
 506         int res;
 507         struct stlink_usb_handle_s *h;
 508
 509         assert(handle != NULL);
 510
 511         h = (struct stlink_usb_handle_s *)handle;
 512
 513         stlink_usb_init_buffer(handle, STLINK_NULL_EP, 0);
 514
 515         switch (type) {
 516                 case STLINK_MODE_DEBUG_JTAG:
 517                 case STLINK_MODE_DEBUG_SWD:
 518                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 519                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
 520                         break;
 521                 case STLINK_MODE_DEBUG_SWIM:
 522                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 523                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
 524                         break;
 525                 case STLINK_MODE_DFU:
 526                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
 527                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
 528                         break;
 529                 case STLINK_MODE_MASS:
 530                 default:
 531                         return ERROR_FAIL;
 532         }
 533
 534         res = stlink_usb_xfer(handle, 0, 0);
 535
 536         if (res != ERROR_OK)
 537                 return res;
 538
 539         return ERROR_OK;
 540 }
 541
 542 /** */
 543 static int stlink_usb_init_mode(void *handle)
 544 {
 545         int res;
 546         uint8_t mode;
 547         enum stlink_mode emode;
 548         struct stlink_usb_handle_s *h;
 549
 550         assert(handle != NULL);
 551
 552         h = (struct stlink_usb_handle_s *)handle;
 553
 554         res = stlink_usb_current_mode(handle, &mode);
 555
 556         if (res != ERROR_OK)
 557                 return res;
 558
 559         LOG_DEBUG("MODE: 0x%02X", mode);
 560
 561         /* try to exit current mode */
 562         switch (mode) {
 563                 case STLINK_DEV_DFU_MODE:
 564                         emode = STLINK_MODE_DFU;
 565                         break;
 566                 case STLINK_DEV_DEBUG_MODE:
 567                         emode = STLINK_MODE_DEBUG_SWD;
 568                         break;
 569                 case STLINK_DEV_SWIM_MODE:
 570                         emode = STLINK_MODE_DEBUG_SWIM;
 571                         break;
 572                 case STLINK_DEV_BOOTLOADER_MODE:
 573                 case STLINK_DEV_MASS_MODE:
 574                 default:
 575                         emode = STLINK_MODE_UNKNOWN;
 576                         break;
 577         }
 578
 579         if (emode != STLINK_MODE_UNKNOWN) {
 580                 res = stlink_usb_mode_leave(handle, emode);
 581
 582                 if (res != ERROR_OK)
 583                         return res;
 584         }
 585
 586         res = stlink_usb_current_mode(handle, &mode);
 587
 588         if (res != ERROR_OK)
 589                 return res;
 590
 591         LOG_DEBUG("MODE: 0x%02X", mode);
 592
 593         /* set selected mode */
 594         switch (h->transport) {
 595                 case STLINK_TRANSPORT_SWD:
 596                         emode = STLINK_MODE_DEBUG_SWD;
 597                         break;
 598                 case STLINK_TRANSPORT_JTAG:
 599                         emode = STLINK_MODE_DEBUG_JTAG;
 600                         break;
 601                 case STLINK_TRANSPORT_SWIM:
 602                         emode = STLINK_MODE_DEBUG_SWIM;
 603                         break;
 604                 default:
 605                         emode = STLINK_MODE_UNKNOWN;
 606                         break;
 607         }
 608
 609         if (emode == STLINK_MODE_UNKNOWN) {
 610                 LOG_ERROR("selected mode (transport) not supported");
 611                 return ERROR_FAIL;
 612         }
 613
 614         res = stlink_usb_mode_enter(handle, emode);
 615
 616         if (res != ERROR_OK)
 617                 return res;
 618
 619         res = stlink_usb_current_mode(handle, &mode);
 620
 621         if (res != ERROR_OK)
 622                 return res;
 623
 624         LOG_DEBUG("MODE: 0x%02X", mode);
 625
 626         return ERROR_OK;
 627 }
 628
 629 /** */
 630 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 631 {
 632         int res;
 633         struct stlink_usb_handle_s *h;
 634
 635         assert(handle != NULL);
 636
 637         h = (struct stlink_usb_handle_s *)handle;
 638
 639         stlink_usb_init_buffer(handle, STLINK_RX_EP, 4);
 640
 641         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 642         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
 643
 644         res = stlink_usb_xfer(handle, h->databuf, 4);
 645
 646         if (res != ERROR_OK)
 647                 return res;
 648
 649         *idcode = le_to_h_u32(h->databuf);
 650
 651         LOG_DEBUG("IDCODE: 0x%08X", *idcode);
 652
 653         return ERROR_OK;
 654 }
 655
 656 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
 657 {
 658         struct stlink_usb_handle_s *h;
 659         int res;
 660
 661         assert(handle != NULL);
 662
 663         h = (struct stlink_usb_handle_s *)handle;
 664
 665         stlink_usb_init_buffer(handle, STLINK_RX_EP, 8);
 666
 667         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 668         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
 669         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 670         h->cmdidx += 4;
 671
 672         res = stlink_usb_xfer(handle, h->databuf, 8);
 673
 674         if (res != ERROR_OK)
 675                 return res;
 676
 677         *val = le_to_h_u32(h->databuf + 4);
 678
 679         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 680 }
 681
 682 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 683 {
 684         int res;
 685         struct stlink_usb_handle_s *h;
 686
 687         assert(handle != NULL);
 688
 689         h = (struct stlink_usb_handle_s *)handle;
 690
 691         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 692
 693         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 694         if (h->jtag_api == STLINK_JTAG_API_V1)
 695                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
 696         else
 697                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
 698         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 699         h->cmdidx += 4;
 700         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 701         h->cmdidx += 4;
 702
 703         res = stlink_usb_xfer(handle, h->databuf, 2);
 704
 705         if (res != ERROR_OK)
 706                 return res;
 707
 708         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 709 }
 710
 711 static enum target_state stlink_usb_v2_get_status(void *handle)
 712 {
 713         int result;
 714         uint32_t status;
 715
 716         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
 717         if  (result != ERROR_OK)
 718                 return TARGET_UNKNOWN;
 719
 720         if (status & S_HALT)
 721                 return TARGET_HALTED;
 722
 723         return TARGET_RUNNING;
 724 }
 725
 726 /** */
 727 static enum target_state stlink_usb_state(void *handle)
 728 {
 729         int res;
 730         struct stlink_usb_handle_s *h;
 731
 732         assert(handle != NULL);
 733
 734         h = (struct stlink_usb_handle_s *)handle;
 735
 736         if (h->jtag_api == STLINK_JTAG_API_V2)
 737                 return stlink_usb_v2_get_status(handle);
 738
 739         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 740
 741         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 742         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
 743
 744         res = stlink_usb_xfer(handle, h->databuf, 2);
 745
 746         if (res != ERROR_OK)
 747                 return TARGET_UNKNOWN;
 748
 749         if (h->databuf[0] == STLINK_CORE_RUNNING)
 750                 return TARGET_RUNNING;
 751         if (h->databuf[0] == STLINK_CORE_HALTED)
 752                 return TARGET_HALTED;
 753
 754         return TARGET_UNKNOWN;
 755 }
 756
 757 /** */
 758 static int stlink_usb_reset(void *handle)
 759 {
 760         int res;
 761         struct stlink_usb_handle_s *h;
 762
 763         assert(handle != NULL);
 764
 765         h = (struct stlink_usb_handle_s *)handle;
 766
 767         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 768
 769         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 770
 771         if (h->jtag_api == STLINK_JTAG_API_V1)
 772                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
 773         else
 774                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
 775
 776         res = stlink_usb_xfer(handle, h->databuf, 2);
 777
 778         if (res != ERROR_OK)
 779                 return res;
 780
 781         LOG_DEBUG("RESET: 0x%08X", h->databuf[0]);
 782
 783         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 784 }
 785
 786 /** */
 787 static int stlink_usb_run(void *handle)
 788 {
 789         int res;
 790         struct stlink_usb_handle_s *h;
 791
 792         assert(handle != NULL);
 793
 794         h = (struct stlink_usb_handle_s *)handle;
 795
 796         if (h->jtag_api == STLINK_JTAG_API_V2)
 797                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 798
 799         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 800
 801         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 802         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
 803
 804         res = stlink_usb_xfer(handle, h->databuf, 2);
 805
 806         if (res != ERROR_OK)
 807                 return res;
 808
 809         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 810 }
 811
 812 /** */
 813 static int stlink_usb_halt(void *handle)
 814 {
 815         int res;
 816         struct stlink_usb_handle_s *h;
 817
 818         assert(handle != NULL);
 819
 820         h = (struct stlink_usb_handle_s *)handle;
 821
 822         if (h->jtag_api == STLINK_JTAG_API_V2)
 823                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 824
 825         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 826
 827         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 828         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
 829
 830         res = stlink_usb_xfer(handle, h->databuf, 2);
 831
 832         if (res != ERROR_OK)
 833                 return res;
 834
 835         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 836 }
 837
 838 /** */
 839 static int stlink_usb_step(void *handle)
 840 {
 841         int res;
 842         struct stlink_usb_handle_s *h;
 843
 844         assert(handle != NULL);
 845
 846         h = (struct stlink_usb_handle_s *)handle;
 847
 848         if (h->jtag_api == STLINK_JTAG_API_V2)
 849                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_DEBUGEN);
 850
 851         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 852
 853         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 854         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
 855
 856         res = stlink_usb_xfer(handle, h->databuf, 2);
 857
 858         if (res != ERROR_OK)
 859                 return res;
 860
 861         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 862 }
 863
 864 /** */
 865 static int stlink_usb_read_regs(void *handle)
 866 {
 867         int res;
 868         struct stlink_usb_handle_s *h;
 869
 870         assert(handle != NULL);
 871
 872         h = (struct stlink_usb_handle_s *)handle;
 873
 874         stlink_usb_init_buffer(handle, STLINK_RX_EP, 84);
 875
 876         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 877         if (h->jtag_api == STLINK_JTAG_API_V1)
 878                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
 879         else
 880                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
 881
 882         res = stlink_usb_xfer(handle, h->databuf, 84);
 883
 884         if (res != ERROR_OK)
 885                 return res;
 886
 887         return ERROR_OK;
 888 }
 889
 890 /** */
 891 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 892 {
 893         int res;
 894         struct stlink_usb_handle_s *h;
 895
 896         assert(handle != NULL);
 897
 898         h = (struct stlink_usb_handle_s *)handle;
 899
 900         stlink_usb_init_buffer(handle, STLINK_RX_EP, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 901
 902         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 903         if (h->jtag_api == STLINK_JTAG_API_V1)
 904                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
 905         else
 906                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
 907         h->cmdbuf[h->cmdidx++] = num;
 908
 909         res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 910
 911         if (res != ERROR_OK)
 912                 return res;
 913
 914         if (h->jtag_api == STLINK_JTAG_API_V1)
 915                 *val = le_to_h_u32(h->databuf);
 916         else {
 917                 *val = le_to_h_u32(h->databuf + 4);
 918                 return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 919         }
 920
 921         return ERROR_OK;
 922 }
 923
 924 /** */
 925 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 926 {
 927         int res;
 928         struct stlink_usb_handle_s *h;
 929
 930         assert(handle != NULL);
 931
 932         h = (struct stlink_usb_handle_s *)handle;
 933
 934         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 935
 936         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 937         if (h->jtag_api == STLINK_JTAG_API_V1)
 938                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
 939         else
 940                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
 941         h->cmdbuf[h->cmdidx++] = num;
 942         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 943         h->cmdidx += 4;
 944
 945         res = stlink_usb_xfer(handle, h->databuf, 2);
 946
 947         if (res != ERROR_OK)
 948                 return res;
 949
 950         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 951 }
 952
 953 static int stlink_usb_get_rw_status(void *handle)
 954 {
 955         int res;
 956         struct stlink_usb_handle_s *h;
 957
 958         assert(handle != NULL);
 959
 960         h = (struct stlink_usb_handle_s *)handle;
 961
 962         if (h->jtag_api == STLINK_JTAG_API_V1)
 963                 return ERROR_OK;
 964
 965         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 966
 967         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 968         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
 969
 970         res = stlink_usb_xfer(handle, h->databuf, 2);
 971
 972         if (res != ERROR_OK)
 973                 return res;
 974
 975         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
 976 }
 977
 978 /** */
 979 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
 980                           uint8_t *buffer)
 981 {
 982         int res;
 983         uint16_t read_len = len;
 984         struct stlink_usb_handle_s *h;
 985
 986         assert(handle != NULL);
 987
 988         h = (struct stlink_usb_handle_s *)handle;
 989
 990         stlink_usb_init_buffer(handle, STLINK_RX_EP, read_len);
 991
 992         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 993         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
 994         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 995         h->cmdidx += 4;
 996         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 997         h->cmdidx += 2;
 998
 999         /* we need to fix read length for single bytes */
1000         if (read_len == 1)
1001                 read_len++;
1002
1003         res = stlink_usb_xfer(handle, h->databuf, read_len);
1004
1005         if (res != ERROR_OK)
1006                 return res;
1007
1008         memcpy(buffer, h->databuf, len);
1009
1010         return stlink_usb_get_rw_status(handle);
1011 }
1012
1013 /** */
1014 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1015                            const uint8_t *buffer)
1016 {
1017         int res;
1018         struct stlink_usb_handle_s *h;
1019
1020         assert(handle != NULL);
1021
1022         h = (struct stlink_usb_handle_s *)handle;
1023
1024         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1025
1026         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1027         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1028         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1029         h->cmdidx += 4;
1030         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1031         h->cmdidx += 2;
1032
1033         res = stlink_usb_xfer(handle, buffer, len);
1034
1035         if (res != ERROR_OK)
1036                 return res;
1037
1038         return stlink_usb_get_rw_status(handle);
1039 }
1040
1041 /** */
1042 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1043                           uint8_t *buffer)
1044 {
1045         int res;
1046         struct stlink_usb_handle_s *h;
1047
1048         assert(handle != NULL);
1049
1050         h = (struct stlink_usb_handle_s *)handle;
1051
1052         len *= 4;
1053
1054         stlink_usb_init_buffer(handle, STLINK_RX_EP, len);
1055
1056         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1057         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1058         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1059         h->cmdidx += 4;
1060         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1061         h->cmdidx += 2;
1062
1063         res = stlink_usb_xfer(handle, h->databuf, len);
1064
1065         if (res != ERROR_OK)
1066                 return res;
1067
1068         memcpy(buffer, h->databuf, len);
1069
1070         return stlink_usb_get_rw_status(handle);
1071 }
1072
1073 /** */
1074 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1075                            const uint8_t *buffer)
1076 {
1077         int res;
1078         struct stlink_usb_handle_s *h;
1079
1080         assert(handle != NULL);
1081
1082         h = (struct stlink_usb_handle_s *)handle;
1083
1084         len *= 4;
1085
1086         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1087
1088         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1089         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1090         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1091         h->cmdidx += 4;
1092         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1093         h->cmdidx += 2;
1094
1095         res = stlink_usb_xfer(handle, buffer, len);
1096
1097         if (res != ERROR_OK)
1098                 return res;
1099
1100         return stlink_usb_get_rw_status(handle);
1101 }
1102
1103 /** */
1104 static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
1105 {
1106         int err;
1107         struct stlink_usb_handle_s *h;
1108
1109         LOG_DEBUG("stlink_usb_open");
1110
1111         h = malloc(sizeof(struct stlink_usb_handle_s));
1112
1113         if (h == 0) {
1114                 LOG_DEBUG("malloc failed");
1115                 return ERROR_FAIL;
1116         }
1117
1118         h->transport = param->transport;
1119
1120         const uint16_t vids[] = { param->vid, 0 };
1121         const uint16_t pids[] = { param->pid, 0 };
1122
1123         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
1124                 param->vid, param->pid);
1125
1126         if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
1127                 LOG_ERROR("open failed");
1128                 return ERROR_FAIL;
1129         }
1130
1131         jtag_libusb_set_configuration(h->fd, 0);
1132
1133         if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1134                 LOG_DEBUG("claim interface failed");
1135                 return ERROR_FAIL;
1136         }
1137
1138         /* wrap version for first read */
1139         switch (param->pid) {
1140                 case 0x3744:
1141                         h->version.stlink = 1;
1142                         break;
1143                 default:
1144                         h->version.stlink = 2;
1145                         break;
1146         }
1147
1148         /* get the device version */
1149         err = stlink_usb_version(h);
1150
1151         if (err != ERROR_OK) {
1152                 LOG_ERROR("read version failed");
1153                 jtag_libusb_close(h->fd);
1154                 free(h);
1155                 return err;
1156         }
1157
1158         /* compare usb vid/pid */
1159         if ((param->vid != h->vid) || (param->pid != h->pid))
1160                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1161                         param->vid, param->pid,
1162                         h->vid, h->pid);
1163
1164         /* check if mode is supported */
1165         err = ERROR_OK;
1166
1167         switch (h->transport) {
1168                 case STLINK_TRANSPORT_SWD:
1169                 case STLINK_TRANSPORT_JTAG:
1170                         if (h->version.jtag == 0)
1171                                 err = ERROR_FAIL;
1172                         break;
1173                 case STLINK_TRANSPORT_SWIM:
1174                         if (h->version.swim == 0)
1175                                 err = ERROR_FAIL;
1176                         break;
1177                 default:
1178                         err = ERROR_FAIL;
1179                         break;
1180         }
1181
1182         if (err != ERROR_OK) {
1183                 LOG_ERROR("mode (transport) not supported by device");
1184                 jtag_libusb_close(h->fd);
1185                 free(h);
1186                 return err;
1187         }
1188
1189         /* set the used jtag api, this will default to the newest supported version */
1190         h->jtag_api = h->version.jtag_api_max;
1191
1192         /* initialize the debug hardware */
1193         err = stlink_usb_init_mode(h);
1194
1195         if (err != ERROR_OK) {
1196                 LOG_ERROR("init mode failed");
1197                 jtag_libusb_close(h->fd);
1198                 free(h);
1199                 return err;
1200         }
1201
1202         *fd = h;
1203
1204         return ERROR_OK;
1205 }
1206
1207 /** */
1208 static int stlink_usb_close(void *fd)
1209 {
1210         return ERROR_OK;
1211 }
1212
1213 /** */
1214 struct stlink_layout_api_s stlink_usb_layout_api = {
1215         /** */
1216         .open = stlink_usb_open,
1217         /** */
1218         .close = stlink_usb_close,
1219         /** */
1220         .idcode = stlink_usb_idcode,
1221         /** */
1222         .state = stlink_usb_state,
1223         /** */
1224         .reset = stlink_usb_reset,
1225         /** */
1226         .run = stlink_usb_run,
1227         /** */
1228         .halt = stlink_usb_halt,
1229         /** */
1230         .step = stlink_usb_step,
1231         /** */
1232         .read_regs = stlink_usb_read_regs,
1233         /** */
1234         .read_reg = stlink_usb_read_reg,
1235         /** */
1236         .write_reg = stlink_usb_write_reg,
1237         /** */
1238         .read_mem8 = stlink_usb_read_mem8,
1239         /** */
1240         .write_mem8 = stlink_usb_write_mem8,
1241         /** */
1242         .read_mem32 = stlink_usb_read_mem32,
1243         /** */
1244         .write_mem32 = stlink_usb_write_mem32,
1245 };