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