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 <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         else if (status & S_RESET_ST)
 723                 return TARGET_RESET;
 724
 725         return TARGET_RUNNING;
 726 }
 727
 728 /** */
 729 static enum target_state stlink_usb_state(void *handle)
 730 {
 731         int res;
 732         struct stlink_usb_handle_s *h;
 733
 734         assert(handle != NULL);
 735
 736         h = (struct stlink_usb_handle_s *)handle;
 737
 738         if (h->jtag_api == STLINK_JTAG_API_V2)
 739                 return stlink_usb_v2_get_status(handle);
 740
 741         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 742
 743         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 744         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
 745
 746         res = stlink_usb_xfer(handle, h->databuf, 2);
 747
 748         if (res != ERROR_OK)
 749                 return TARGET_UNKNOWN;
 750
 751         if (h->databuf[0] == STLINK_CORE_RUNNING)
 752                 return TARGET_RUNNING;
 753         if (h->databuf[0] == STLINK_CORE_HALTED)
 754                 return TARGET_HALTED;
 755
 756         return TARGET_UNKNOWN;
 757 }
 758
 759 /** */
 760 static int stlink_usb_reset(void *handle)
 761 {
 762         int res;
 763         struct stlink_usb_handle_s *h;
 764
 765         assert(handle != NULL);
 766
 767         h = (struct stlink_usb_handle_s *)handle;
 768
 769         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 770
 771         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 772
 773         if (h->jtag_api == STLINK_JTAG_API_V1)
 774                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
 775         else
 776                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
 777
 778         res = stlink_usb_xfer(handle, h->databuf, 2);
 779
 780         if (res != ERROR_OK)
 781                 return res;
 782
 783         LOG_DEBUG("RESET: 0x%08X", h->databuf[0]);
 784
 785         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 786 }
 787
 788 static int stlink_usb_assert_srst(void *handle, int srst)
 789 {
 790         int res;
 791         struct stlink_usb_handle_s *h;
 792
 793         assert(handle != NULL);
 794
 795         h = (struct stlink_usb_handle_s *)handle;
 796
 797         if (h->jtag_api == STLINK_JTAG_API_V1)
 798                 return ERROR_COMMAND_NOTFOUND;
 799
 800         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 801
 802         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 803         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_DRIVE_NRST;
 804         h->cmdbuf[h->cmdidx++] = srst;
 805
 806         res = stlink_usb_xfer(handle, h->databuf, 2);
 807
 808         if (res != ERROR_OK)
 809                 return res;
 810
 811         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 812 }
 813
 814 /** */
 815 static int stlink_usb_run(void *handle)
 816 {
 817         int res;
 818         struct stlink_usb_handle_s *h;
 819
 820         assert(handle != NULL);
 821
 822         h = (struct stlink_usb_handle_s *)handle;
 823
 824         if (h->jtag_api == STLINK_JTAG_API_V2)
 825                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 826
 827         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 828
 829         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 830         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
 831
 832         res = stlink_usb_xfer(handle, h->databuf, 2);
 833
 834         if (res != ERROR_OK)
 835                 return res;
 836
 837         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 838 }
 839
 840 /** */
 841 static int stlink_usb_halt(void *handle)
 842 {
 843         int res;
 844         struct stlink_usb_handle_s *h;
 845
 846         assert(handle != NULL);
 847
 848         h = (struct stlink_usb_handle_s *)handle;
 849
 850         if (h->jtag_api == STLINK_JTAG_API_V2)
 851                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 852
 853         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 854
 855         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 856         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
 857
 858         res = stlink_usb_xfer(handle, h->databuf, 2);
 859
 860         if (res != ERROR_OK)
 861                 return res;
 862
 863         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 864 }
 865
 866 /** */
 867 static int stlink_usb_step(void *handle)
 868 {
 869         int res;
 870         struct stlink_usb_handle_s *h;
 871
 872         assert(handle != NULL);
 873
 874         h = (struct stlink_usb_handle_s *)handle;
 875
 876         if (h->jtag_api == STLINK_JTAG_API_V2)
 877                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_DEBUGEN);
 878
 879         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 880
 881         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 882         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
 883
 884         res = stlink_usb_xfer(handle, h->databuf, 2);
 885
 886         if (res != ERROR_OK)
 887                 return res;
 888
 889         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 890 }
 891
 892 /** */
 893 static int stlink_usb_read_regs(void *handle)
 894 {
 895         int res;
 896         struct stlink_usb_handle_s *h;
 897
 898         assert(handle != NULL);
 899
 900         h = (struct stlink_usb_handle_s *)handle;
 901
 902         stlink_usb_init_buffer(handle, STLINK_RX_EP, 84);
 903
 904         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 905         if (h->jtag_api == STLINK_JTAG_API_V1)
 906                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
 907         else
 908                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
 909
 910         res = stlink_usb_xfer(handle, h->databuf, 84);
 911
 912         if (res != ERROR_OK)
 913                 return res;
 914
 915         return ERROR_OK;
 916 }
 917
 918 /** */
 919 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 920 {
 921         int res;
 922         struct stlink_usb_handle_s *h;
 923
 924         assert(handle != NULL);
 925
 926         h = (struct stlink_usb_handle_s *)handle;
 927
 928         stlink_usb_init_buffer(handle, STLINK_RX_EP, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 929
 930         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 931         if (h->jtag_api == STLINK_JTAG_API_V1)
 932                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
 933         else
 934                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
 935         h->cmdbuf[h->cmdidx++] = num;
 936
 937         res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 938
 939         if (res != ERROR_OK)
 940                 return res;
 941
 942         if (h->jtag_api == STLINK_JTAG_API_V1)
 943                 *val = le_to_h_u32(h->databuf);
 944         else {
 945                 *val = le_to_h_u32(h->databuf + 4);
 946                 return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 947         }
 948
 949         return ERROR_OK;
 950 }
 951
 952 /** */
 953 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 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         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 963
 964         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 965         if (h->jtag_api == STLINK_JTAG_API_V1)
 966                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
 967         else
 968                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
 969         h->cmdbuf[h->cmdidx++] = num;
 970         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 971         h->cmdidx += 4;
 972
 973         res = stlink_usb_xfer(handle, h->databuf, 2);
 974
 975         if (res != ERROR_OK)
 976                 return res;
 977
 978         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 979 }
 980
 981 static int stlink_usb_get_rw_status(void *handle)
 982 {
 983         int res;
 984         struct stlink_usb_handle_s *h;
 985
 986         assert(handle != NULL);
 987
 988         h = (struct stlink_usb_handle_s *)handle;
 989
 990         if (h->jtag_api == STLINK_JTAG_API_V1)
 991                 return ERROR_OK;
 992
 993         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 994
 995         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 996         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
 997
 998         res = stlink_usb_xfer(handle, h->databuf, 2);
 999
1000         if (res != ERROR_OK)
1001                 return res;
1002
1003         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
1004 }
1005
1006 /** */
1007 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1008                           uint8_t *buffer)
1009 {
1010         int res;
1011         uint16_t read_len = len;
1012         struct stlink_usb_handle_s *h;
1013
1014         assert(handle != NULL);
1015
1016         h = (struct stlink_usb_handle_s *)handle;
1017
1018         stlink_usb_init_buffer(handle, STLINK_RX_EP, read_len);
1019
1020         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1021         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
1022         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1023         h->cmdidx += 4;
1024         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1025         h->cmdidx += 2;
1026
1027         /* we need to fix read length for single bytes */
1028         if (read_len == 1)
1029                 read_len++;
1030
1031         res = stlink_usb_xfer(handle, h->databuf, read_len);
1032
1033         if (res != ERROR_OK)
1034                 return res;
1035
1036         memcpy(buffer, h->databuf, len);
1037
1038         return stlink_usb_get_rw_status(handle);
1039 }
1040
1041 /** */
1042 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1043                            const 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         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1053
1054         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1055         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1056         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1057         h->cmdidx += 4;
1058         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1059         h->cmdidx += 2;
1060
1061         res = stlink_usb_xfer(handle, buffer, len);
1062
1063         if (res != ERROR_OK)
1064                 return res;
1065
1066         return stlink_usb_get_rw_status(handle);
1067 }
1068
1069 /** */
1070 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1071                           uint8_t *buffer)
1072 {
1073         int res;
1074         struct stlink_usb_handle_s *h;
1075
1076         assert(handle != NULL);
1077
1078         h = (struct stlink_usb_handle_s *)handle;
1079
1080         len *= 4;
1081
1082         stlink_usb_init_buffer(handle, STLINK_RX_EP, len);
1083
1084         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1085         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1086         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1087         h->cmdidx += 4;
1088         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1089         h->cmdidx += 2;
1090
1091         res = stlink_usb_xfer(handle, h->databuf, len);
1092
1093         if (res != ERROR_OK)
1094                 return res;
1095
1096         memcpy(buffer, h->databuf, len);
1097
1098         return stlink_usb_get_rw_status(handle);
1099 }
1100
1101 /** */
1102 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1103                            const uint8_t *buffer)
1104 {
1105         int res;
1106         struct stlink_usb_handle_s *h;
1107
1108         assert(handle != NULL);
1109
1110         h = (struct stlink_usb_handle_s *)handle;
1111
1112         len *= 4;
1113
1114         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1115
1116         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1117         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1118         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1119         h->cmdidx += 4;
1120         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1121         h->cmdidx += 2;
1122
1123         res = stlink_usb_xfer(handle, buffer, len);
1124
1125         if (res != ERROR_OK)
1126                 return res;
1127
1128         return stlink_usb_get_rw_status(handle);
1129 }
1130
1131 /** */
1132 static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
1133 {
1134         int err;
1135         struct stlink_usb_handle_s *h;
1136
1137         LOG_DEBUG("stlink_usb_open");
1138
1139         h = malloc(sizeof(struct stlink_usb_handle_s));
1140
1141         if (h == 0) {
1142                 LOG_DEBUG("malloc failed");
1143                 return ERROR_FAIL;
1144         }
1145
1146         h->transport = param->transport;
1147
1148         const uint16_t vids[] = { param->vid, 0 };
1149         const uint16_t pids[] = { param->pid, 0 };
1150
1151         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
1152                 param->vid, param->pid);
1153
1154         if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
1155                 LOG_ERROR("open failed");
1156                 return ERROR_FAIL;
1157         }
1158
1159         jtag_libusb_set_configuration(h->fd, 0);
1160
1161         if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1162                 LOG_DEBUG("claim interface failed");
1163                 return ERROR_FAIL;
1164         }
1165
1166         /* wrap version for first read */
1167         switch (param->pid) {
1168                 case 0x3744:
1169                         h->version.stlink = 1;
1170                         break;
1171                 default:
1172                         h->version.stlink = 2;
1173                         break;
1174         }
1175
1176         /* get the device version */
1177         err = stlink_usb_version(h);
1178
1179         if (err != ERROR_OK) {
1180                 LOG_ERROR("read version failed");
1181                 jtag_libusb_close(h->fd);
1182                 free(h);
1183                 return err;
1184         }
1185
1186         /* compare usb vid/pid */
1187         if ((param->vid != h->vid) || (param->pid != h->pid))
1188                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1189                         param->vid, param->pid,
1190                         h->vid, h->pid);
1191
1192         /* check if mode is supported */
1193         err = ERROR_OK;
1194
1195         switch (h->transport) {
1196                 case STLINK_TRANSPORT_SWD:
1197                 case STLINK_TRANSPORT_JTAG:
1198                         if (h->version.jtag == 0)
1199                                 err = ERROR_FAIL;
1200                         break;
1201                 case STLINK_TRANSPORT_SWIM:
1202                         if (h->version.swim == 0)
1203                                 err = ERROR_FAIL;
1204                         break;
1205                 default:
1206                         err = ERROR_FAIL;
1207                         break;
1208         }
1209
1210         if (err != ERROR_OK) {
1211                 LOG_ERROR("mode (transport) not supported by device");
1212                 jtag_libusb_close(h->fd);
1213                 free(h);
1214                 return err;
1215         }
1216
1217         /* set the used jtag api, this will default to the newest supported version */
1218         h->jtag_api = h->version.jtag_api_max;
1219
1220         /* initialize the debug hardware */
1221         err = stlink_usb_init_mode(h);
1222
1223         if (err != ERROR_OK) {
1224                 LOG_ERROR("init mode failed");
1225                 jtag_libusb_close(h->fd);
1226                 free(h);
1227                 return err;
1228         }
1229
1230         *fd = h;
1231
1232         return ERROR_OK;
1233 }
1234
1235 /** */
1236 static int stlink_usb_close(void *fd)
1237 {
1238         return ERROR_OK;
1239 }
1240
1241 /** */
1242 struct stlink_layout_api_s stlink_usb_layout_api = {
1243         /** */
1244         .open = stlink_usb_open,
1245         /** */
1246         .close = stlink_usb_close,
1247         /** */
1248         .idcode = stlink_usb_idcode,
1249         /** */
1250         .state = stlink_usb_state,
1251         /** */
1252         .reset = stlink_usb_reset,
1253         /** */
1254         .assert_srst = stlink_usb_assert_srst,
1255         /** */
1256         .run = stlink_usb_run,
1257         /** */
1258         .halt = stlink_usb_halt,
1259         /** */
1260         .step = stlink_usb_step,
1261         /** */
1262         .read_regs = stlink_usb_read_regs,
1263         /** */
1264         .read_reg = stlink_usb_read_reg,
1265         /** */
1266         .write_reg = stlink_usb_write_reg,
1267         /** */
1268         .read_mem8 = stlink_usb_read_mem8,
1269         /** */
1270         .write_mem8 = stlink_usb_write_mem8,
1271         /** */
1272         .read_mem32 = stlink_usb_read_mem32,
1273         /** */
1274         .write_mem32 = stlink_usb_write_mem32,
1275         /** */
1276         .write_debug_reg = stlink_usb_write_debug_reg
1277 };