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          * API V2 is supported since JTAG V11
 404          */
 405         if (h->version.jtag >= 11)
 406                 h->version.jtag_api_max = STLINK_JTAG_API_V2;
 407         else
 408                 h->version.jtag_api_max = STLINK_JTAG_API_V1;
 409
 410         LOG_DEBUG("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
 411                 h->version.stlink,
 412                 h->version.jtag,
 413                 (h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2,
 414                 h->version.swim,
 415                 h->vid,
 416                 h->pid);
 417
 418         return ERROR_OK;
 419 }
 420
 421 /** */
 422 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 423 {
 424         int res;
 425         struct stlink_usb_handle_s *h;
 426
 427         assert(handle != NULL);
 428
 429         h = (struct stlink_usb_handle_s *)handle;
 430
 431         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 432
 433         h->cmdbuf[h->cmdidx++] = STLINK_GET_CURRENT_MODE;
 434
 435         res = stlink_usb_xfer(handle, h->databuf, 2);
 436
 437         if (res != ERROR_OK)
 438                 return res;
 439
 440         *mode = h->databuf[0];
 441
 442         return ERROR_OK;
 443 }
 444
 445 /** */
 446 static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 447 {
 448         int res;
 449         int rx_size = 0;
 450         struct stlink_usb_handle_s *h;
 451
 452         assert(handle != NULL);
 453
 454         h = (struct stlink_usb_handle_s *)handle;
 455
 456         /* on api V2 we are able the read the latest command
 457          * status
 458          * TODO: we need the test on api V1 too
 459          */
 460         if (h->jtag_api == STLINK_JTAG_API_V2)
 461                 rx_size = 2;
 462
 463         stlink_usb_init_buffer(handle, STLINK_RX_EP, rx_size);
 464
 465         switch (type) {
 466                 case STLINK_MODE_DEBUG_JTAG:
 467                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 468                         if (h->jtag_api == STLINK_JTAG_API_V1)
 469                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 470                         else
 471                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
 472                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG;
 473                         break;
 474                 case STLINK_MODE_DEBUG_SWD:
 475                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 476                         if (h->jtag_api == STLINK_JTAG_API_V1)
 477                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 478                         else
 479                                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
 480                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD;
 481                         break;
 482                 case STLINK_MODE_DEBUG_SWIM:
 483                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 484                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
 485                         break;
 486                 case STLINK_MODE_DFU:
 487                 case STLINK_MODE_MASS:
 488                 default:
 489                         return ERROR_FAIL;
 490         }
 491
 492         res = stlink_usb_xfer(handle, h->databuf, rx_size);
 493
 494         if (res != ERROR_OK)
 495                 return res;
 496
 497         res = stlink_usb_error_check(h);
 498
 499         return res;
 500 }
 501
 502 /** */
 503 static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 504 {
 505         int res;
 506         struct stlink_usb_handle_s *h;
 507
 508         assert(handle != NULL);
 509
 510         h = (struct stlink_usb_handle_s *)handle;
 511
 512         stlink_usb_init_buffer(handle, STLINK_NULL_EP, 0);
 513
 514         switch (type) {
 515                 case STLINK_MODE_DEBUG_JTAG:
 516                 case STLINK_MODE_DEBUG_SWD:
 517                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 518                         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_EXIT;
 519                         break;
 520                 case STLINK_MODE_DEBUG_SWIM:
 521                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 522                         h->cmdbuf[h->cmdidx++] = STLINK_SWIM_EXIT;
 523                         break;
 524                 case STLINK_MODE_DFU:
 525                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_COMMAND;
 526                         h->cmdbuf[h->cmdidx++] = STLINK_DFU_EXIT;
 527                         break;
 528                 case STLINK_MODE_MASS:
 529                 default:
 530                         return ERROR_FAIL;
 531         }
 532
 533         res = stlink_usb_xfer(handle, 0, 0);
 534
 535         if (res != ERROR_OK)
 536                 return res;
 537
 538         return ERROR_OK;
 539 }
 540
 541 /** */
 542 static int stlink_usb_init_mode(void *handle)
 543 {
 544         int res;
 545         uint8_t mode;
 546         enum stlink_mode emode;
 547         struct stlink_usb_handle_s *h;
 548
 549         assert(handle != NULL);
 550
 551         h = (struct stlink_usb_handle_s *)handle;
 552
 553         res = stlink_usb_current_mode(handle, &mode);
 554
 555         if (res != ERROR_OK)
 556                 return res;
 557
 558         LOG_DEBUG("MODE: 0x%02X", mode);
 559
 560         /* try to exit current mode */
 561         switch (mode) {
 562                 case STLINK_DEV_DFU_MODE:
 563                         emode = STLINK_MODE_DFU;
 564                         break;
 565                 case STLINK_DEV_DEBUG_MODE:
 566                         emode = STLINK_MODE_DEBUG_SWD;
 567                         break;
 568                 case STLINK_DEV_SWIM_MODE:
 569                         emode = STLINK_MODE_DEBUG_SWIM;
 570                         break;
 571                 case STLINK_DEV_BOOTLOADER_MODE:
 572                 case STLINK_DEV_MASS_MODE:
 573                 default:
 574                         emode = STLINK_MODE_UNKNOWN;
 575                         break;
 576         }
 577
 578         if (emode != STLINK_MODE_UNKNOWN) {
 579                 res = stlink_usb_mode_leave(handle, emode);
 580
 581                 if (res != ERROR_OK)
 582                         return res;
 583         }
 584
 585         res = stlink_usb_current_mode(handle, &mode);
 586
 587         if (res != ERROR_OK)
 588                 return res;
 589
 590         LOG_DEBUG("MODE: 0x%02X", mode);
 591
 592         /* set selected mode */
 593         switch (h->transport) {
 594                 case STLINK_TRANSPORT_SWD:
 595                         emode = STLINK_MODE_DEBUG_SWD;
 596                         break;
 597                 case STLINK_TRANSPORT_JTAG:
 598                         emode = STLINK_MODE_DEBUG_JTAG;
 599                         break;
 600                 case STLINK_TRANSPORT_SWIM:
 601                         emode = STLINK_MODE_DEBUG_SWIM;
 602                         break;
 603                 default:
 604                         emode = STLINK_MODE_UNKNOWN;
 605                         break;
 606         }
 607
 608         if (emode == STLINK_MODE_UNKNOWN) {
 609                 LOG_ERROR("selected mode (transport) not supported");
 610                 return ERROR_FAIL;
 611         }
 612
 613         res = stlink_usb_mode_enter(handle, emode);
 614
 615         if (res != ERROR_OK)
 616                 return res;
 617
 618         res = stlink_usb_current_mode(handle, &mode);
 619
 620         if (res != ERROR_OK)
 621                 return res;
 622
 623         LOG_DEBUG("MODE: 0x%02X", mode);
 624
 625         return ERROR_OK;
 626 }
 627
 628 /** */
 629 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 630 {
 631         int res;
 632         struct stlink_usb_handle_s *h;
 633
 634         assert(handle != NULL);
 635
 636         h = (struct stlink_usb_handle_s *)handle;
 637
 638         stlink_usb_init_buffer(handle, STLINK_RX_EP, 4);
 639
 640         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 641         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READCOREID;
 642
 643         res = stlink_usb_xfer(handle, h->databuf, 4);
 644
 645         if (res != ERROR_OK)
 646                 return res;
 647
 648         *idcode = le_to_h_u32(h->databuf);
 649
 650         LOG_DEBUG("IDCODE: 0x%08X", *idcode);
 651
 652         return ERROR_OK;
 653 }
 654
 655 static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *val)
 656 {
 657         struct stlink_usb_handle_s *h;
 658         int res;
 659
 660         assert(handle != NULL);
 661
 662         h = (struct stlink_usb_handle_s *)handle;
 663
 664         stlink_usb_init_buffer(handle, STLINK_RX_EP, 8);
 665
 666         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 667         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG;
 668         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 669         h->cmdidx += 4;
 670
 671         res = stlink_usb_xfer(handle, h->databuf, 8);
 672
 673         if (res != ERROR_OK)
 674                 return res;
 675
 676         *val = le_to_h_u32(h->databuf + 4);
 677
 678         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 679 }
 680
 681 static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 682 {
 683         int res;
 684         struct stlink_usb_handle_s *h;
 685
 686         assert(handle != NULL);
 687
 688         h = (struct stlink_usb_handle_s *)handle;
 689
 690         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 691
 692         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 693         if (h->jtag_api == STLINK_JTAG_API_V1)
 694                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
 695         else
 696                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
 697         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
 698         h->cmdidx += 4;
 699         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 700         h->cmdidx += 4;
 701
 702         res = stlink_usb_xfer(handle, h->databuf, 2);
 703
 704         if (res != ERROR_OK)
 705                 return res;
 706
 707         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 708 }
 709
 710 static enum target_state stlink_usb_v2_get_status(void *handle)
 711 {
 712         int result;
 713         uint32_t status;
 714
 715         result = stlink_usb_v2_read_debug_reg(handle, DCB_DHCSR, &status);
 716         if  (result != ERROR_OK)
 717                 return TARGET_UNKNOWN;
 718
 719         if (status & S_HALT)
 720                 return TARGET_HALTED;
 721         else if (status & S_RESET_ST)
 722                 return TARGET_RESET;
 723
 724         return TARGET_RUNNING;
 725 }
 726
 727 /** */
 728 static enum target_state stlink_usb_state(void *handle)
 729 {
 730         int res;
 731         struct stlink_usb_handle_s *h;
 732
 733         assert(handle != NULL);
 734
 735         h = (struct stlink_usb_handle_s *)handle;
 736
 737         if (h->jtag_api == STLINK_JTAG_API_V2)
 738                 return stlink_usb_v2_get_status(handle);
 739
 740         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 741
 742         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 743         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_GETSTATUS;
 744
 745         res = stlink_usb_xfer(handle, h->databuf, 2);
 746
 747         if (res != ERROR_OK)
 748                 return TARGET_UNKNOWN;
 749
 750         if (h->databuf[0] == STLINK_CORE_RUNNING)
 751                 return TARGET_RUNNING;
 752         if (h->databuf[0] == STLINK_CORE_HALTED)
 753                 return TARGET_HALTED;
 754
 755         return TARGET_UNKNOWN;
 756 }
 757
 758 /** */
 759 static int stlink_usb_reset(void *handle)
 760 {
 761         int res;
 762         struct stlink_usb_handle_s *h;
 763
 764         assert(handle != NULL);
 765
 766         h = (struct stlink_usb_handle_s *)handle;
 767
 768         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 769
 770         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 771
 772         if (h->jtag_api == STLINK_JTAG_API_V1)
 773                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
 774         else
 775                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
 776
 777         res = stlink_usb_xfer(handle, h->databuf, 2);
 778
 779         if (res != ERROR_OK)
 780                 return res;
 781
 782         LOG_DEBUG("RESET: 0x%08X", h->databuf[0]);
 783
 784         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 785 }
 786
 787 static int stlink_usb_assert_srst(void *handle, int srst)
 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_V1)
 797                 return ERROR_COMMAND_NOTFOUND;
 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_APIV2_DRIVE_NRST;
 803         h->cmdbuf[h->cmdidx++] = srst;
 804
 805         res = stlink_usb_xfer(handle, h->databuf, 2);
 806
 807         if (res != ERROR_OK)
 808                 return res;
 809
 810         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 811 }
 812
 813 /** */
 814 static int stlink_usb_run(void *handle)
 815 {
 816         int res;
 817         struct stlink_usb_handle_s *h;
 818
 819         assert(handle != NULL);
 820
 821         h = (struct stlink_usb_handle_s *)handle;
 822
 823         if (h->jtag_api == STLINK_JTAG_API_V2)
 824                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 825
 826         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 827
 828         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 829         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_RUNCORE;
 830
 831         res = stlink_usb_xfer(handle, h->databuf, 2);
 832
 833         if (res != ERROR_OK)
 834                 return res;
 835
 836         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 837 }
 838
 839 /** */
 840 static int stlink_usb_halt(void *handle)
 841 {
 842         int res;
 843         struct stlink_usb_handle_s *h;
 844
 845         assert(handle != NULL);
 846
 847         h = (struct stlink_usb_handle_s *)handle;
 848
 849         if (h->jtag_api == STLINK_JTAG_API_V2)
 850                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 851
 852         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 853
 854         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 855         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_FORCEDEBUG;
 856
 857         res = stlink_usb_xfer(handle, h->databuf, 2);
 858
 859         if (res != ERROR_OK)
 860                 return res;
 861
 862         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 863 }
 864
 865 /** */
 866 static int stlink_usb_step(void *handle)
 867 {
 868         int res;
 869         struct stlink_usb_handle_s *h;
 870
 871         assert(handle != NULL);
 872
 873         h = (struct stlink_usb_handle_s *)handle;
 874
 875         if (h->jtag_api == STLINK_JTAG_API_V2)
 876                 return stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_STEP|C_DEBUGEN);
 877
 878         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 879
 880         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 881         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_STEPCORE;
 882
 883         res = stlink_usb_xfer(handle, h->databuf, 2);
 884
 885         if (res != ERROR_OK)
 886                 return res;
 887
 888         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 889 }
 890
 891 /** */
 892 static int stlink_usb_read_regs(void *handle)
 893 {
 894         int res;
 895         struct stlink_usb_handle_s *h;
 896
 897         assert(handle != NULL);
 898
 899         h = (struct stlink_usb_handle_s *)handle;
 900
 901         stlink_usb_init_buffer(handle, STLINK_RX_EP, 84);
 902
 903         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 904         if (h->jtag_api == STLINK_JTAG_API_V1)
 905                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
 906         else
 907                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
 908
 909         res = stlink_usb_xfer(handle, h->databuf, 84);
 910
 911         if (res != ERROR_OK)
 912                 return res;
 913
 914         return ERROR_OK;
 915 }
 916
 917 /** */
 918 static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 919 {
 920         int res;
 921         struct stlink_usb_handle_s *h;
 922
 923         assert(handle != NULL);
 924
 925         h = (struct stlink_usb_handle_s *)handle;
 926
 927         stlink_usb_init_buffer(handle, STLINK_RX_EP, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 928
 929         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 930         if (h->jtag_api == STLINK_JTAG_API_V1)
 931                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
 932         else
 933                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
 934         h->cmdbuf[h->cmdidx++] = num;
 935
 936         res = stlink_usb_xfer(handle, h->databuf, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 937
 938         if (res != ERROR_OK)
 939                 return res;
 940
 941         if (h->jtag_api == STLINK_JTAG_API_V1)
 942                 *val = le_to_h_u32(h->databuf);
 943         else {
 944                 *val = le_to_h_u32(h->databuf + 4);
 945                 return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 946         }
 947
 948         return ERROR_OK;
 949 }
 950
 951 /** */
 952 static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 953 {
 954         int res;
 955         struct stlink_usb_handle_s *h;
 956
 957         assert(handle != NULL);
 958
 959         h = (struct stlink_usb_handle_s *)handle;
 960
 961         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 962
 963         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 964         if (h->jtag_api == STLINK_JTAG_API_V1)
 965                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
 966         else
 967                 h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
 968         h->cmdbuf[h->cmdidx++] = num;
 969         h_u32_to_le(h->cmdbuf+h->cmdidx, val);
 970         h->cmdidx += 4;
 971
 972         res = stlink_usb_xfer(handle, h->databuf, 2);
 973
 974         if (res != ERROR_OK)
 975                 return res;
 976
 977         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : ERROR_FAIL;
 978 }
 979
 980 static int stlink_usb_get_rw_status(void *handle)
 981 {
 982         int res;
 983         struct stlink_usb_handle_s *h;
 984
 985         assert(handle != NULL);
 986
 987         h = (struct stlink_usb_handle_s *)handle;
 988
 989         if (h->jtag_api == STLINK_JTAG_API_V1)
 990                 return ERROR_OK;
 991
 992         stlink_usb_init_buffer(handle, STLINK_RX_EP, 2);
 993
 994         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 995         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
 996
 997         res = stlink_usb_xfer(handle, h->databuf, 2);
 998
 999         if (res != ERROR_OK)
1000                 return res;
1001
1002         return h->databuf[0] == STLINK_DEBUG_ERR_OK ? ERROR_OK : res;
1003 }
1004
1005 /** */
1006 static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
1007                           uint8_t *buffer)
1008 {
1009         int res;
1010         uint16_t read_len = len;
1011         struct stlink_usb_handle_s *h;
1012
1013         assert(handle != NULL);
1014
1015         h = (struct stlink_usb_handle_s *)handle;
1016
1017         stlink_usb_init_buffer(handle, STLINK_RX_EP, read_len);
1018
1019         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1020         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_8BIT;
1021         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1022         h->cmdidx += 4;
1023         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1024         h->cmdidx += 2;
1025
1026         /* we need to fix read length for single bytes */
1027         if (read_len == 1)
1028                 read_len++;
1029
1030         res = stlink_usb_xfer(handle, h->databuf, read_len);
1031
1032         if (res != ERROR_OK)
1033                 return res;
1034
1035         memcpy(buffer, h->databuf, len);
1036
1037         return stlink_usb_get_rw_status(handle);
1038 }
1039
1040 /** */
1041 static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
1042                            const uint8_t *buffer)
1043 {
1044         int res;
1045         struct stlink_usb_handle_s *h;
1046
1047         assert(handle != NULL);
1048
1049         h = (struct stlink_usb_handle_s *)handle;
1050
1051         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1052
1053         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1054         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_8BIT;
1055         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1056         h->cmdidx += 4;
1057         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1058         h->cmdidx += 2;
1059
1060         res = stlink_usb_xfer(handle, buffer, len);
1061
1062         if (res != ERROR_OK)
1063                 return res;
1064
1065         return stlink_usb_get_rw_status(handle);
1066 }
1067
1068 /** */
1069 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
1070                           uint8_t *buffer)
1071 {
1072         int res;
1073         struct stlink_usb_handle_s *h;
1074
1075         assert(handle != NULL);
1076
1077         h = (struct stlink_usb_handle_s *)handle;
1078
1079         len *= 4;
1080
1081         stlink_usb_init_buffer(handle, STLINK_RX_EP, len);
1082
1083         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1084         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT;
1085         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1086         h->cmdidx += 4;
1087         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1088         h->cmdidx += 2;
1089
1090         res = stlink_usb_xfer(handle, h->databuf, len);
1091
1092         if (res != ERROR_OK)
1093                 return res;
1094
1095         memcpy(buffer, h->databuf, len);
1096
1097         return stlink_usb_get_rw_status(handle);
1098 }
1099
1100 /** */
1101 static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
1102                            const uint8_t *buffer)
1103 {
1104         int res;
1105         struct stlink_usb_handle_s *h;
1106
1107         assert(handle != NULL);
1108
1109         h = (struct stlink_usb_handle_s *)handle;
1110
1111         len *= 4;
1112
1113         stlink_usb_init_buffer(handle, STLINK_TX_EP, len);
1114
1115         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
1116         h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT;
1117         h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
1118         h->cmdidx += 4;
1119         h_u16_to_le(h->cmdbuf+h->cmdidx, len);
1120         h->cmdidx += 2;
1121
1122         res = stlink_usb_xfer(handle, buffer, len);
1123
1124         if (res != ERROR_OK)
1125                 return res;
1126
1127         return stlink_usb_get_rw_status(handle);
1128 }
1129
1130 /** */
1131 static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
1132 {
1133         int err;
1134         struct stlink_usb_handle_s *h;
1135
1136         LOG_DEBUG("stlink_usb_open");
1137
1138         h = malloc(sizeof(struct stlink_usb_handle_s));
1139
1140         if (h == 0) {
1141                 LOG_DEBUG("malloc failed");
1142                 return ERROR_FAIL;
1143         }
1144
1145         h->transport = param->transport;
1146
1147         const uint16_t vids[] = { param->vid, 0 };
1148         const uint16_t pids[] = { param->pid, 0 };
1149
1150         LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
1151                 param->vid, param->pid);
1152
1153         if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
1154                 LOG_ERROR("open failed");
1155                 return ERROR_FAIL;
1156         }
1157
1158         jtag_libusb_set_configuration(h->fd, 0);
1159
1160         if (jtag_libusb_claim_interface(h->fd, 0) != ERROR_OK) {
1161                 LOG_DEBUG("claim interface failed");
1162                 return ERROR_FAIL;
1163         }
1164
1165         /* wrap version for first read */
1166         switch (param->pid) {
1167                 case 0x3744:
1168                         h->version.stlink = 1;
1169                         break;
1170                 default:
1171                         h->version.stlink = 2;
1172                         break;
1173         }
1174
1175         /* get the device version */
1176         err = stlink_usb_version(h);
1177
1178         if (err != ERROR_OK) {
1179                 LOG_ERROR("read version failed");
1180                 jtag_libusb_close(h->fd);
1181                 free(h);
1182                 return err;
1183         }
1184
1185         /* compare usb vid/pid */
1186         if ((param->vid != h->vid) || (param->pid != h->pid))
1187                 LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
1188                         param->vid, param->pid,
1189                         h->vid, h->pid);
1190
1191         /* check if mode is supported */
1192         err = ERROR_OK;
1193
1194         switch (h->transport) {
1195                 case STLINK_TRANSPORT_SWD:
1196                 case STLINK_TRANSPORT_JTAG:
1197                         if (h->version.jtag == 0)
1198                                 err = ERROR_FAIL;
1199                         break;
1200                 case STLINK_TRANSPORT_SWIM:
1201                         if (h->version.swim == 0)
1202                                 err = ERROR_FAIL;
1203                         break;
1204                 default:
1205                         err = ERROR_FAIL;
1206                         break;
1207         }
1208
1209         if (err != ERROR_OK) {
1210                 LOG_ERROR("mode (transport) not supported by device");
1211                 jtag_libusb_close(h->fd);
1212                 free(h);
1213                 return err;
1214         }
1215
1216         /* set the used jtag api, this will default to the newest supported version */
1217         h->jtag_api = h->version.jtag_api_max;
1218
1219         /* initialize the debug hardware */
1220         err = stlink_usb_init_mode(h);
1221
1222         if (err != ERROR_OK) {
1223                 LOG_ERROR("init mode failed");
1224                 jtag_libusb_close(h->fd);
1225                 free(h);
1226                 return err;
1227         }
1228
1229         *fd = h;
1230
1231         return ERROR_OK;
1232 }
1233
1234 /** */
1235 static int stlink_usb_close(void *fd)
1236 {
1237         return ERROR_OK;
1238 }
1239
1240 /** */
1241 struct stlink_layout_api_s stlink_usb_layout_api = {
1242         /** */
1243         .open = stlink_usb_open,
1244         /** */
1245         .close = stlink_usb_close,
1246         /** */
1247         .idcode = stlink_usb_idcode,
1248         /** */
1249         .state = stlink_usb_state,
1250         /** */
1251         .reset = stlink_usb_reset,
1252         /** */
1253         .assert_srst = stlink_usb_assert_srst,
1254         /** */
1255         .run = stlink_usb_run,
1256         /** */
1257         .halt = stlink_usb_halt,
1258         /** */
1259         .step = stlink_usb_step,
1260         /** */
1261         .read_regs = stlink_usb_read_regs,
1262         /** */
1263         .read_reg = stlink_usb_read_reg,
1264         /** */
1265         .write_reg = stlink_usb_write_reg,
1266         /** */
1267         .read_mem8 = stlink_usb_read_mem8,
1268         /** */
1269         .write_mem8 = stlink_usb_write_mem8,
1270         /** */
1271         .read_mem32 = stlink_usb_read_mem32,
1272         /** */
1273         .write_mem32 = stlink_usb_write_mem32,
1274         /** */
1275         .write_debug_reg = stlink_usb_write_debug_reg
1276 };