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

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