git.gag.com Git - fw/stlink/blob - src/stlink-access-test.c

   1 /*
   2  Copyright (c) 2010 "Capt'ns Missing Link" Authors. All rights reserved.
   3  Use of this source code is governed by a BSD-style
   4  license that can be found in the LICENSE file.
   5
   6  A linux stlink access demo. The purpose of this file is to mitigate the usual
   7  "reinventing the wheel" force by incompatible licenses and give you an idea,
   8  how to access the stlink device. That doesn't mean you should be a free-loader
   9  and not contribute your improvements to this code.
  10
  11  Author: Martin Capitanio <m@capitanio.org>
  12  The stlink related constants kindly provided by Oliver Spencer (OpenOCD)
  13  for use in a GPL compatible license.
  14
  15  Notes:
  16  gcc -O0 -g3 -Wall -c -std=gnu99 -o stlink-access-test.o stlink-access-test.c
  17  gcc  -o stlink-access-test stlink-access-test.o -lsgutils2
  18
  19  Code format ~ TAB = 8, K&R, linux kernel source, golang oriented
  20  Tested compatibility: linux, gcc >= 4.3.3
  21
  22  The communication is based on standard USB mass storage device
  23  BOT (Bulk Only Transfer)
  24  - Endpoint 1: BULK_IN, 64 bytes max
  25  - Endpoint 2: BULK_OUT, 64 bytes max
  26
  27  All CBW transfers are ordered with the LSB (byte 0) first (little endian).
  28  Any command must be answered before sending the next command.
  29  Each USB transfer must complete in less than 1s.
  30
  31  SB Device Class Definition for Mass Storage Devices:
  32  www.usb.org/developers/devclass_docs/usbmassbulk_10.pdf
  33
  34  dt             - Data Transfer (IN/OUT)
  35  CBW            - Command Block Wrapper
  36  CSW            - Command Status Wrapper
  37  RFU            - Reserved for Future Use
  38  scsi_pt        - SCSI pass-through
  39  sg             - SCSI generic
  40
  41  * usb-storage.quirks
  42  http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=blob_plain;f=Documentation/kernel-parameters.txt
  43  Each entry has the form VID:PID:Flags where VID and PID are Vendor and Product
  44  ID values (4-digit hex numbers) and Flags is a set of characters, each corresponding
  45  to a common usb-storage quirk flag as follows:
  46
  47  a = SANE_SENSE (collect more than 18 bytes of sense data);
  48  b = BAD_SENSE (don't collect more than 18 bytes of sense data);
  49  c = FIX_CAPACITY (decrease the reported device capacity by one sector);
  50  h = CAPACITY_HEURISTICS (decrease the reported device capacity by one sector if the number is odd);
  51  i = IGNORE_DEVICE (don't bind to this device);
  52  l = NOT_LOCKABLE (don't try to lock and unlock ejectable media);
  53  m = MAX_SECTORS_64 (don't transfer more than 64 sectors = 32 KB at a time);
  54  o = CAPACITY_OK (accept the capacity reported by the device);
  55  r = IGNORE_RESIDUE (the device reports bogus residue values);
  56  s = SINGLE_LUN (the device has only one Logical Unit);
  57  w = NO_WP_DETECT (don't test whether the medium is write-protected).
  58
  59  Example: quirks=0419:aaf5:rl,0421:0433:rc
  60  http://permalink.gmane.org/gmane.linux.usb.general/35053
  61
  62  modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:l
  63
  64  Equivalently, you can add a line saying
  65
  66  options usb-storage quirks=483:3744:l
  67
  68  to your /etc/modprobe.conf or /etc/modprobe.d/local.conf (or add the "quirks=..."
  69  part to an existing options line for usb-storage).
  70  */
  71
  72 #define __USE_GNU
  73 #include <stdio.h>
  74 #include <string.h>
  75 #include <stdlib.h>
  76 #include <unistd.h>
  77 #include <fcntl.h>
  78 #include <sys/types.h>
  79 #include <sys/stat.h>
  80 #include <sys/mman.h>
  81
  82 // sgutils2 (apt-get install libsgutils2-dev)
  83 #include <scsi/sg_lib.h>
  84 #include <scsi/sg_pt.h>
  85
  86 // device access
  87 #define RDWR            0
  88 #define RO              1
  89 #define SG_TIMEOUT_SEC  1 // actually 1 is about 2 sec
  90 // Each CDB can be a total of 6, 10, 12, or 16 bytes, later version
  91 // of the SCSI standard also allow for variable-length CDBs (min. CDB is 6).
  92 // the stlink needs max. 10 bytes.
  93 #define CDB_6           6
  94 #define CDB_10          10
  95 #define CDB_12          12
  96 #define CDB_16          16
  97
  98 #define CDB_SL          10
  99
 100 // Query data flow direction.
 101 #define Q_DATA_OUT      0
 102 #define Q_DATA_IN       1
 103
 104 // The SCSI Request Sense command is used to obtain sense data
 105 // (error information) from a target device.
 106 // http://en.wikipedia.org/wiki/SCSI_Request_Sense_Command
 107 #define SENSE_BUF_LEN           32
 108
 109 // Max data transfer size.
 110 // 6kB = max mem32_read block, 8kB sram
 111 //#define Q_BUF_LEN     96
 112 #define Q_BUF_LEN       1024 * 100
 113
 114 // st-link vendor cmd's
 115 #define USB_ST_VID                      0x0483
 116 #define USB_STLINK_PID                  0x3744
 117
 118 // STLINK_DEBUG_RESETSYS, etc:
 119 #define STLINK_OK                       0x80
 120 #define STLINK_FALSE                    0x81
 121 #define STLINK_CORE_RUNNINIG            0x80
 122 #define STLINK_CORE_HALTED              0x81
 123 #define STLINK_CORE_STAT_UNKNOWN        -1
 124
 125 #define STLINK_GET_VERSION              0xf1
 126 #define STLINK_GET_CURRENT_MODE 0xf5
 127
 128 #define STLINK_DEBUG_COMMAND            0xF2
 129 #define STLINK_DFU_COMMAND              0xF3
 130 #define STLINK_DFU_EXIT         0x07
 131
 132 // STLINK_GET_CURRENT_MODE
 133 #define STLINK_DEV_DFU_MODE             0x00
 134 #define STLINK_DEV_MASS_MODE            0x01
 135 #define STLINK_DEV_DEBUG_MODE           0x02
 136 #define STLINK_DEV_UNKNOWN_MODE -1
 137
 138 // jtag mode cmds
 139 #define STLINK_DEBUG_ENTER              0x20
 140 #define STLINK_DEBUG_EXIT               0x21
 141 #define STLINK_DEBUG_READCOREID 0x22
 142 #define STLINK_DEBUG_GETSTATUS          0x01
 143 #define STLINK_DEBUG_FORCEDEBUG 0x02
 144 #define STLINK_DEBUG_RESETSYS           0x03
 145 #define STLINK_DEBUG_READALLREGS        0x04
 146 #define STLINK_DEBUG_READREG            0x05
 147 #define STLINK_DEBUG_WRITEREG           0x06
 148 #define STLINK_DEBUG_READMEM_32BIT      0x07
 149 #define STLINK_DEBUG_WRITEMEM_32BIT     0x08
 150 #define STLINK_DEBUG_RUNCORE            0x09
 151 #define STLINK_DEBUG_STEPCORE           0x0a
 152 #define STLINK_DEBUG_SETFP              0x0b
 153 #define STLINK_DEBUG_WRITEMEM_8BIT      0x0d
 154 #define STLINK_DEBUG_CLEARFP            0x0e
 155 #define STLINK_DEBUG_WRITEDEBUGREG      0x0f
 156 #define STLINK_DEBUG_ENTER_SWD          0xa3
 157 #define STLINK_DEBUG_ENTER_JTAG 0x00
 158
 159 typedef struct {
 160         uint32_t r[16];
 161         uint32_t xpsr;
 162         uint32_t main_sp;
 163         uint32_t process_sp;
 164         uint32_t rw;
 165         uint32_t rw2;
 166 } reg;
 167
 168 typedef uint32_t stm32_addr_t;
 169
 170 struct stlink {
 171         int sg_fd;
 172         int do_scsi_pt_err;
 173         // sg layer verboseness: 0 for no debug info, 10 for lots
 174         int verbose;
 175
 176         unsigned char cdb_cmd_blk[CDB_SL];
 177
 178         // Data transferred from or to device
 179         unsigned char q_buf[Q_BUF_LEN];
 180         int q_len;
 181         int q_data_dir; // Q_DATA_IN, Q_DATA_OUT
 182         // the start of the query data in the device memory space
 183         uint32_t q_addr;
 184
 185         // Sense (error information) data
 186         unsigned char sense_buf[SENSE_BUF_LEN];
 187
 188         uint32_t st_vid;
 189         uint32_t stlink_pid;
 190         uint32_t stlink_v;
 191         uint32_t jtag_v;
 192         uint32_t swim_v;
 193         uint32_t core_id;
 194
 195         reg reg;
 196         int core_stat;
 197
 198         /* medium density stm32 flash settings */
 199 #define STM32_FLASH_BASE 0x08000000
 200 #define STM32_FLASH_SIZE (128 * 1024)
 201 #define STM32_FLASH_PGSZ 1024
 202         stm32_addr_t flash_base;
 203         size_t flash_size;
 204         size_t flash_pgsz;
 205
 206         /* in flash system memory */
 207 #define STM32_SYSTEM_BASE 0x1ffff000
 208 #define STM32_SYSTEM_SIZE (2 * 1024)
 209         stm32_addr_t sys_base;
 210         size_t sys_size;
 211
 212         /* sram settings */
 213 #define STM32_SRAM_BASE 0x20000000
 214 #define STM32_SRAM_SIZE (8 * 1024)
 215         stm32_addr_t sram_base;
 216         size_t sram_size;
 217 };
 218
 219 static void D(struct stlink *sl, char *txt) {
 220         if (sl->verbose > 1)
 221                 fputs(txt, stderr);
 222 }
 223
 224 // Suspends execution of the calling process for
 225 // (at least) ms milliseconds.
 226 static void delay(int ms) {
 227         fprintf(stderr, "*** wait %d ms\n", ms);
 228         usleep(1000 * ms);
 229 }
 230
 231 // Endianness
 232 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 233 // const int i = 1;
 234 // #define is_bigendian() ( (*(char*)&i) == 0 )
 235 static inline unsigned int is_bigendian(void) {
 236         static volatile const unsigned int i = 1;
 237         return *(volatile const char*) &i == 0;
 238 }
 239
 240 static void write_uint32(unsigned char* buf, uint32_t ui) {
 241         if (!is_bigendian()) { // le -> le (don't swap)
 242                 buf[0] = ((unsigned char*) &ui)[0];
 243                 buf[1] = ((unsigned char*) &ui)[1];
 244                 buf[2] = ((unsigned char*) &ui)[2];
 245                 buf[3] = ((unsigned char*) &ui)[3];
 246         } else {
 247                 buf[0] = ((unsigned char*) &ui)[3];
 248                 buf[1] = ((unsigned char*) &ui)[2];
 249                 buf[2] = ((unsigned char*) &ui)[1];
 250                 buf[3] = ((unsigned char*) &ui)[0];
 251         }
 252 }
 253
 254 static void write_uint16(unsigned char* buf, uint16_t ui) {
 255         if (!is_bigendian()) { // le -> le (don't swap)
 256                 buf[0] = ((unsigned char*) &ui)[0];
 257                 buf[1] = ((unsigned char*) &ui)[1];
 258         } else {
 259                 buf[0] = ((unsigned char*) &ui)[1];
 260                 buf[1] = ((unsigned char*) &ui)[0];
 261         }
 262 }
 263
 264 static uint32_t read_uint32(const unsigned char *c, const int pt) {
 265         uint32_t ui;
 266         char *p = (char *) &ui;
 267
 268         if (!is_bigendian()) { // le -> le (don't swap)
 269                 p[0] = c[pt];
 270                 p[1] = c[pt + 1];
 271                 p[2] = c[pt + 2];
 272                 p[3] = c[pt + 3];
 273         } else {
 274                 p[0] = c[pt + 3];
 275                 p[1] = c[pt + 2];
 276                 p[2] = c[pt + 1];
 277                 p[3] = c[pt];
 278         }
 279         return ui;
 280 }
 281
 282 static void clear_cdb(struct stlink *sl) {
 283         for (int i = 0; i < sizeof(sl->cdb_cmd_blk); i++)
 284                 sl->cdb_cmd_blk[i] = 0;
 285         // set default
 286         sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 287         sl->q_data_dir = Q_DATA_IN;
 288 }
 289
 290 // E.g. make the valgrind happy.
 291 static void clear_buf(struct stlink *sl) {
 292         fprintf(stderr, "*** clear_buf ***\n");
 293         for (int i = 0; i < sizeof(sl->q_buf); i++)
 294                 sl->q_buf[i] = 0;
 295
 296 }
 297
 298 static struct stlink* stlink_open(const char *dev_name, const int verbose) {
 299         fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
 300         int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
 301         if (sg_fd < 0) {
 302                 fprintf(stderr, "error opening device: %s: %s\n", dev_name,
 303                         safe_strerror(-sg_fd));
 304                 return NULL;
 305         }
 306
 307         struct stlink *sl = malloc(sizeof(struct stlink));
 308         if (sl == NULL) {
 309                 fprintf(stderr, "struct stlink: out of memory\n");
 310                 return NULL;
 311         }
 312
 313         sl->sg_fd = sg_fd;
 314         sl->verbose = verbose;
 315         sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 316         sl->core_id = 0;
 317         sl->q_addr = 0;
 318         clear_buf(sl);
 319
 320         /* flash memory settings */
 321         sl->flash_base = STM32_FLASH_BASE;
 322         sl->flash_size = STM32_FLASH_SIZE;
 323         sl->flash_pgsz = STM32_FLASH_PGSZ;
 324
 325         /* system memory */
 326         sl->sys_base = STM32_SYSTEM_BASE;
 327         sl->sys_size = STM32_SYSTEM_SIZE;
 328
 329         /* sram memory settings */
 330         sl->sram_base = STM32_SRAM_BASE;
 331         sl->sram_size = STM32_SRAM_SIZE;
 332
 333         return sl;
 334 }
 335
 336 // close the device, free the allocated memory
 337 void stlink_close(struct stlink *sl) {
 338         D(sl, "\n*** stlink_close ***\n");
 339         if (sl) {
 340                 scsi_pt_close_device(sl->sg_fd);
 341                 free(sl);
 342         }
 343 }
 344
 345 //TODO rewrite/cleanup, save the error in sl
 346 static void stlink_confirm_inq(struct stlink *sl, struct sg_pt_base *ptvp) {
 347         const int e = sl->do_scsi_pt_err;
 348         if (e < 0) {
 349                 fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 350                         safe_strerror(-e));
 351                 return;
 352         } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 353                 fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 354                 return;
 355         } else if (e == SCSI_PT_DO_TIMEOUT) {
 356                 fprintf(stderr, "  pass through timeout\n");
 357                 return;
 358         }
 359         const int duration = get_scsi_pt_duration_ms(ptvp);
 360         if ((sl->verbose > 1) && (duration >= 0))
 361                 fprintf(stderr, "      duration=%d ms\n", duration);
 362
 363         // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 364         // "usb-storage quirks=483:3744:r"
 365         // forces residue to be ignored and calculated, but this causes aboard if
 366         // data_len = 0 and by some other data_len values.
 367
 368         const int resid = get_scsi_pt_resid(ptvp);
 369         const int dsize = sl->q_len - resid;
 370
 371         const int cat = get_scsi_pt_result_category(ptvp);
 372         char buf[512];
 373         unsigned int slen;
 374
 375         switch (cat) {
 376         case SCSI_PT_RESULT_GOOD:
 377                 if (sl->verbose && (resid > 0))
 378                         fprintf(stderr, "      notice: requested %d bytes but "
 379                                 "got %d bytes, ignore [broken] residue = %d\n",
 380                                 sl->q_len, dsize, resid);
 381                 break;
 382         case SCSI_PT_RESULT_STATUS:
 383                 if (sl->verbose) {
 384                         sg_get_scsi_status_str(
 385                                 get_scsi_pt_status_response(ptvp), sizeof(buf),
 386                                 buf);
 387                         fprintf(stderr, "  scsi status: %s\n", buf);
 388                 }
 389                 return;
 390         case SCSI_PT_RESULT_SENSE:
 391                 slen = get_scsi_pt_sense_len(ptvp);
 392                 if (sl->verbose) {
 393                         sg_get_sense_str("", sl->sense_buf, slen, (sl->verbose
 394                                 > 1), sizeof(buf), buf);
 395                         fprintf(stderr, "%s", buf);
 396                 }
 397                 if (sl->verbose && (resid > 0)) {
 398                         if ((sl->verbose) || (sl->q_len > 0))
 399                                 fprintf(stderr, "    requested %d bytes but "
 400                                         "got %d bytes\n", sl->q_len, dsize);
 401                 }
 402                 return;
 403         case SCSI_PT_RESULT_TRANSPORT_ERR:
 404                 if (sl->verbose) {
 405                         get_scsi_pt_transport_err_str(ptvp, sizeof(buf), buf);
 406                         // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 407                         // These codes potentially come from the firmware on a host adapter
 408                         // or from one of several hosts that an adapter driver controls.
 409                         // The 'host_status' field has the following values:
 410                         //      [0x07] Internal error detected in the host adapter.
 411                         // This may not be fatal (and the command may have succeeded).
 412                         fprintf(stderr, "  transport: %s", buf);
 413                 }
 414                 return;
 415         case SCSI_PT_RESULT_OS_ERR:
 416                 if (sl->verbose) {
 417                         get_scsi_pt_os_err_str(ptvp, sizeof(buf), buf);
 418                         fprintf(stderr, "  os: %s", buf);
 419                 }
 420                 return;
 421         default:
 422                 fprintf(stderr, "  unknown pass through result "
 423                         "category (%d)\n", cat);
 424         }
 425 }
 426
 427 static void stlink_q(struct stlink* sl) {
 428         fputs("CDB[", stderr);
 429         for (int i = 0; i < CDB_SL; i++)
 430                 fprintf(stderr, " 0x%02x", (unsigned int) sl->cdb_cmd_blk[i]);
 431         fputs("]\n", stderr);
 432
 433         // Get control command descriptor of scsi structure,
 434         // (one object per command!!)
 435         struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 436         if (NULL == ptvp) {
 437                 fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 438                 return;
 439         }
 440
 441         set_scsi_pt_cdb(ptvp, sl->cdb_cmd_blk, sizeof(sl->cdb_cmd_blk));
 442
 443         // set buffer for sense (error information) data
 444         set_scsi_pt_sense(ptvp, sl->sense_buf, sizeof(sl->sense_buf));
 445
 446         // Set a buffer to be used for data transferred from device
 447         if (sl->q_data_dir == Q_DATA_IN) {
 448                 //clear_buf(sl);
 449                 set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
 450         } else {
 451                 set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
 452         }
 453         // Executes SCSI command (or at least forwards it to lower layers).
 454         sl->do_scsi_pt_err = do_scsi_pt(ptvp, sl->sg_fd, SG_TIMEOUT_SEC,
 455                 sl->verbose);
 456
 457         // check for scsi errors
 458         stlink_confirm_inq(sl, ptvp);
 459         // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 460         destruct_scsi_pt_obj(ptvp);
 461 }
 462
 463 static void stlink_print_data(struct stlink *sl) {
 464         if (sl->q_len <= 0 || sl->verbose < 2)
 465                 return;
 466         if (sl->verbose > 2)
 467                 fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 468
 469         for (uint32_t i = 0; i < sl->q_len; i++) {
 470                 if (i % 16 == 0) {
 471                         if (sl->q_data_dir == Q_DATA_OUT)
 472                                 fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 473                         else
 474                                 fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 475                 }
 476                 fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 477         }
 478         fputs("\n\n", stdout);
 479 }
 480
 481 // TODO thinking, cleanup
 482 static void stlink_parse_version(struct stlink *sl) {
 483         sl->st_vid = 0;
 484         sl->stlink_pid = 0;
 485         if (sl->q_len <= 0) {
 486                 fprintf(stderr, "Error: could not parse the stlink version");
 487                 return;
 488         }
 489         stlink_print_data(sl);
 490         uint32_t b0 = sl->q_buf[0]; //lsb
 491         uint32_t b1 = sl->q_buf[1];
 492         uint32_t b2 = sl->q_buf[2];
 493         uint32_t b3 = sl->q_buf[3];
 494         uint32_t b4 = sl->q_buf[4];
 495         uint32_t b5 = sl->q_buf[5]; //msb
 496
 497         // b0 b1                       || b2 b3  | b4 b5
 498         // 4b        | 6b     | 6b     || 2B     | 2B
 499         // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 500
 501         sl->stlink_v = (b0 & 0xf0) >> 4;
 502         sl->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 503         sl->swim_v = b1 & 0x3f;
 504         sl->st_vid = (b3 << 8) | b2;
 505         sl->stlink_pid = (b5 << 8) | b4;
 506
 507         if (sl->verbose < 2)
 508                 return;
 509
 510         fprintf(stderr, "st vid         = 0x%04x (expect 0x%04x)\n",
 511                 sl->st_vid, USB_ST_VID);
 512         fprintf(stderr, "stlink pid     = 0x%04x (expect 0x%04x)\n",
 513                 sl->stlink_pid, USB_STLINK_PID);
 514         fprintf(stderr, "stlink version = 0x%x\n", sl->stlink_v);
 515         fprintf(stderr, "jtag version   = 0x%x\n", sl->jtag_v);
 516         fprintf(stderr, "swim version   = 0x%x\n", sl->swim_v);
 517         if (sl->jtag_v == 0)
 518                 fprintf(stderr,
 519                         "    notice: the firmware doesn't support a jtag/swd interface\n");
 520         if (sl->swim_v == 0)
 521                 fprintf(stderr,
 522                         "    notice: the firmware doesn't support a swim interface\n");
 523
 524 }
 525
 526 static int stlink_mode(struct stlink *sl) {
 527         if (sl->q_len <= 0)
 528                 return STLINK_DEV_UNKNOWN_MODE;
 529
 530         stlink_print_data(sl);
 531
 532         switch (sl->q_buf[0]) {
 533         case STLINK_DEV_DFU_MODE:
 534                 fprintf(stderr, "stlink mode: dfu\n");
 535                 return STLINK_DEV_DFU_MODE;
 536         case STLINK_DEV_DEBUG_MODE:
 537                 fprintf(stderr, "stlink mode: debug (jtag or swd)\n");
 538                 return STLINK_DEV_DEBUG_MODE;
 539         case STLINK_DEV_MASS_MODE:
 540                 fprintf(stderr, "stlink mode: mass\n");
 541                 return STLINK_DEV_MASS_MODE;
 542         }
 543         return STLINK_DEV_UNKNOWN_MODE;
 544 }
 545
 546 static void stlink_stat(struct stlink *sl, char *txt) {
 547         if (sl->q_len <= 0)
 548                 return;
 549
 550         stlink_print_data(sl);
 551
 552         switch (sl->q_buf[0]) {
 553         case STLINK_OK:
 554                 fprintf(stderr, "  %s: ok\n", txt);
 555                 return;
 556         case STLINK_FALSE:
 557                 fprintf(stderr, "  %s: false\n", txt);
 558                 return;
 559         default:
 560                 fprintf(stderr, "  %s: unknown\n", txt);
 561         }
 562 }
 563
 564 static void stlink_core_stat(struct stlink *sl) {
 565         if (sl->q_len <= 0)
 566                 return;
 567
 568         stlink_print_data(sl);
 569
 570         switch (sl->q_buf[0]) {
 571         case STLINK_CORE_RUNNINIG:
 572                 sl->core_stat = STLINK_CORE_RUNNINIG;
 573                 fprintf(stderr, "  core status: running\n");
 574                 return;
 575         case STLINK_CORE_HALTED:
 576                 sl->core_stat = STLINK_CORE_HALTED;
 577                 fprintf(stderr, "  core status: halted\n");
 578                 return;
 579         default:
 580                 sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 581                 fprintf(stderr, "  core status: unknown\n");
 582         }
 583 }
 584
 585 void stlink_version(struct stlink *sl) {
 586         D(sl, "\n*** stlink_version ***\n");
 587         clear_cdb(sl);
 588         sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 589         sl->q_len = 6;
 590         sl->q_addr = 0;
 591         stlink_q(sl);
 592         stlink_parse_version(sl);
 593 }
 594
 595 // Get stlink mode:
 596 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 597 // usb dfu             || usb mass             || jtag or swd
 598 int stlink_current_mode(struct stlink *sl) {
 599         D(sl, "\n*** stlink_current_mode ***\n");
 600         clear_cdb(sl);
 601         sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 602         sl->q_len = 2;
 603         sl->q_addr = 0;
 604         stlink_q(sl);
 605         return stlink_mode(sl);
 606 }
 607
 608 // Exit the mass mode and enter the swd debug mode.
 609 void stlink_enter_swd_mode(struct stlink *sl) {
 610         D(sl, "\n*** stlink_enter_swd_mode ***\n");
 611         clear_cdb(sl);
 612         sl->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 613         sl->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 614         sl->q_len = 0; // >0 -> aboard
 615         stlink_q(sl);
 616 }
 617
 618 // Exit the mass mode and enter the jtag debug mode.
 619 // (jtag is disabled in the discovery's stlink firmware)
 620 void stlink_enter_jtag_mode(struct stlink *sl) {
 621         D(sl, "\n*** stlink_enter_jtag_mode ***\n");
 622         clear_cdb(sl);
 623         sl->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 624         sl->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 625         sl->q_len = 0;
 626         stlink_q(sl);
 627 }
 628
 629 // Exit the jtag or swd mode and enter the mass mode.
 630 void stlink_exit_debug_mode(struct stlink *sl) {
 631         D(sl, "\n*** stlink_exit_debug_mode ***\n");
 632         clear_cdb(sl);
 633         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 634         sl->q_len = 0; // >0 -> aboard
 635         stlink_q(sl);
 636 }
 637
 638 // XXX kernel driver performs reset, the device temporally disappears
 639 static void stlink_exit_dfu_mode(struct stlink *sl) {
 640         D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 641         clear_cdb(sl);
 642         sl->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 643         sl->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 644         sl->q_len = 0; // ??
 645         stlink_q(sl);
 646         /*
 647          [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 648          [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 649          [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 650          [135121.844584] end_request: I/O error, dev sdb, sector 4096
 651          [135121.844590] Buffer I/O error on device sdb, logical block 512
 652          [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 653          [135130.274551] usb 6-1: device firmware changed
 654          [135130.274618] usb 6-1: USB disconnect, address 7
 655          [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 656          [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 657          [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 658          [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 659          [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 660          [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 661          [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 662          [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 663          [135130.629492] scsi20 : usb-storage 6-1:1.0
 664          [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 665          [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 666          [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 667          [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 668          [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 669          [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 670          [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 671          [135131.640609]  sdb:
 672          [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 673          [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 674          [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 675          [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 676          [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 677          [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 678          [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 679          [135131.671570] end_request: I/O error, dev sdb, sector 63872
 680          [135131.671575] Buffer I/O error on device sdb, logical block 7984
 681          [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 682          [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 683          [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 684          [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 685          [135131.678551] end_request: I/O error, dev sdb, sector 63872
 686          ...
 687          [135131.853565] end_request: I/O error, dev sdb, sector 4096
 688          */
 689 }
 690
 691 static void stlink_core_id(struct stlink *sl) {
 692         D(sl, "\n*** stlink_core_id ***\n");
 693         clear_cdb(sl);
 694         sl->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 695         sl->q_len = 4;
 696         sl->q_addr = 0;
 697         stlink_q(sl);
 698         sl->core_id = read_uint32(sl->q_buf, 0);
 699         if (sl->verbose < 2)
 700                 return;
 701         stlink_print_data(sl);
 702         fprintf(stderr, "core_id = 0x%08x\n", sl->core_id);
 703 }
 704
 705 // Arm-core reset -> halted state.
 706 void stlink_reset(struct stlink *sl) {
 707         D(sl, "\n*** stlink_reset ***\n");
 708         clear_cdb(sl);
 709         sl->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 710         sl->q_len = 2;
 711         sl->q_addr = 0;
 712         stlink_q(sl);
 713         stlink_stat(sl, "core reset");
 714 }
 715
 716 // Arm-core status: halted or running.
 717 void stlink_status(struct stlink *sl) {
 718         D(sl, "\n*** stlink_status ***\n");
 719         clear_cdb(sl);
 720         sl->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 721         sl->q_len = 2;
 722         sl->q_addr = 0;
 723         stlink_q(sl);
 724         stlink_core_stat(sl);
 725 }
 726
 727 // Force the core into the debug mode -> halted state.
 728 void stlink_force_debug(struct stlink *sl) {
 729         D(sl, "\n*** stlink_force_debug ***\n");
 730         clear_cdb(sl);
 731         sl->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 732         sl->q_len = 2;
 733         sl->q_addr = 0;
 734         stlink_q(sl);
 735         stlink_stat(sl, "force debug");
 736 }
 737
 738 // Read all arm-core registers.
 739 void stlink_read_all_regs(struct stlink *sl) {
 740         D(sl, "\n*** stlink_read_all_regs ***\n");
 741         clear_cdb(sl);
 742         sl->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 743         sl->q_len = 84;
 744         sl->q_addr = 0;
 745         stlink_q(sl);
 746         stlink_print_data(sl);
 747
 748         // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 749         // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 750         for (int i = 0; i < 16; i++) {
 751                 sl->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 752                 if (sl->verbose > 1)
 753                         fprintf(stderr, "r%2d = 0x%08x\n", i, sl->reg.r[i]);
 754         }
 755         sl->reg.xpsr = read_uint32(sl->q_buf, 64);
 756         sl->reg.main_sp = read_uint32(sl->q_buf, 68);
 757         sl->reg.process_sp = read_uint32(sl->q_buf, 72);
 758         sl->reg.rw = read_uint32(sl->q_buf, 76);
 759         sl->reg.rw2 = read_uint32(sl->q_buf, 80);
 760         if (sl->verbose < 2)
 761                 return;
 762
 763         fprintf(stderr, "xpsr       = 0x%08x\n", sl->reg.xpsr);
 764         fprintf(stderr, "main_sp    = 0x%08x\n", sl->reg.main_sp);
 765         fprintf(stderr, "process_sp = 0x%08x\n", sl->reg.process_sp);
 766         fprintf(stderr, "rw         = 0x%08x\n", sl->reg.rw);
 767         fprintf(stderr, "rw2        = 0x%08x\n", sl->reg.rw2);
 768 }
 769
 770 // Read an arm-core register, the index must be in the range 0..20.
 771 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 772 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 773 void stlink_read_reg(struct stlink *sl, int r_idx) {
 774         D(sl, "\n*** stlink_read_reg");
 775         fprintf(stderr, " (%d) ***\n", r_idx);
 776
 777         if (r_idx > 20 || r_idx < 0) {
 778                 fprintf(stderr, "Error: register index must be in [0..20]\n");
 779                 return;
 780         }
 781         clear_cdb(sl);
 782         sl->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 783         sl->cdb_cmd_blk[2] = r_idx;
 784         sl->q_len = 4;
 785         sl->q_addr = 0;
 786         stlink_q(sl);
 787         //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 788         // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 789         // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 790         stlink_print_data(sl);
 791
 792         uint32_t r = read_uint32(sl->q_buf, 0);
 793         fprintf(stderr, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 794
 795         switch (r_idx) {
 796         case 16:
 797                 sl->reg.xpsr = r;
 798                 break;
 799         case 17:
 800                 sl->reg.main_sp = r;
 801                 break;
 802         case 18:
 803                 sl->reg.process_sp = r;
 804                 break;
 805         case 19:
 806                 sl->reg.rw = r; //XXX ?(primask, basemask etc.)
 807                 break;
 808         case 20:
 809                 sl->reg.rw2 = r; //XXX ?(primask, basemask etc.)
 810                 break;
 811         default:
 812                 sl->reg.r[r_idx] = r;
 813         }
 814 }
 815
 816 // Write an arm-core register. Index:
 817 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 818 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 819 void stlink_write_reg(struct stlink *sl, uint32_t reg, int idx) {
 820         D(sl, "\n*** stlink_write_reg ***\n");
 821         clear_cdb(sl);
 822         sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 823         //   2: reg index
 824         // 3-6: reg content
 825         sl->cdb_cmd_blk[2] = idx;
 826         write_uint32(sl->cdb_cmd_blk + 3, reg);
 827         sl->q_len = 2;
 828         sl->q_addr = 0;
 829         stlink_q(sl);
 830         stlink_stat(sl, "write reg");
 831 }
 832
 833 // Write a register of the debug module of the core.
 834 // XXX ?(atomic writes)
 835 // TODO test
 836 void stlink_write_dreg(struct stlink *sl, uint32_t reg, uint32_t addr) {
 837         D(sl, "\n*** stlink_write_dreg ***\n");
 838         clear_cdb(sl);
 839         sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 840         // 2-5: address of reg of the debug module
 841         // 6-9: reg content
 842         write_uint32(sl->cdb_cmd_blk + 2, addr);
 843         write_uint32(sl->cdb_cmd_blk + 6, reg);
 844         sl->q_len = 2;
 845         sl->q_addr = addr;
 846         stlink_q(sl);
 847         stlink_stat(sl, "write debug reg");
 848 }
 849
 850 // Force the core exit the debug mode.
 851 void stlink_run(struct stlink *sl) {
 852         D(sl, "\n*** stlink_run ***\n");
 853         clear_cdb(sl);
 854         sl->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 855         sl->q_len = 2;
 856         sl->q_addr = 0;
 857         stlink_q(sl);
 858         stlink_stat(sl, "run core");
 859 }
 860
 861 // same as above with entrypoint.
 862 static unsigned int is_core_halted(struct stlink*);
 863 void stlink_run_at(struct stlink *sl, stm32_addr_t addr) {
 864         stlink_write_reg(sl, addr, 15); /* pc register */
 865
 866         stlink_run(sl);
 867
 868         while (is_core_halted(sl) == 0)
 869           usleep(3000000);
 870 }
 871
 872 // Step the arm-core.
 873 void stlink_step(struct stlink *sl) {
 874         D(sl, "\n*** stlink_step ***\n");
 875         clear_cdb(sl);
 876         sl->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 877         sl->q_len = 2;
 878         sl->q_addr = 0;
 879         stlink_q(sl);
 880         stlink_stat(sl, "step core");
 881 }
 882
 883 // TODO test
 884 // see Cortex-M3 Technical Reference Manual
 885 void stlink_set_hw_bp(struct stlink *sl, int fp_nr, uint32_t addr, int fp) {
 886         D(sl, "\n*** stlink_set_hw_bp ***\n");
 887         clear_cdb(sl);
 888         sl->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 889         // 2:The number of the flash patch used to set the breakpoint
 890         // 3-6: Address of the breakpoint (LSB)
 891         // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 892         sl->q_buf[2] = fp_nr;
 893         write_uint32(sl->q_buf, addr);
 894         sl->q_buf[7] = fp;
 895
 896         sl->q_len = 2;
 897         stlink_q(sl);
 898         stlink_stat(sl, "set flash breakpoint");
 899 }
 900
 901 // TODO test
 902 void stlink_clr_hw_bp(struct stlink *sl, int fp_nr) {
 903         D(sl, "\n*** stlink_clr_hw_bp ***\n");
 904         clear_cdb(sl);
 905         sl->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 906         sl->cdb_cmd_blk[2] = fp_nr;
 907
 908         sl->q_len = 2;
 909         stlink_q(sl);
 910         stlink_stat(sl, "clear flash breakpoint");
 911 }
 912
 913 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 914 void stlink_read_mem32(struct stlink *sl, uint32_t addr, uint16_t len) {
 915         D(sl, "\n*** stlink_read_mem32 ***\n");
 916         if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 917                 fprintf(
 918                         stderr,
 919                         "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 920                         len % 4);
 921                 return;
 922         }
 923         clear_cdb(sl);
 924         sl->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 925         // 2-5: addr
 926         // 6-7: len
 927         write_uint32(sl->cdb_cmd_blk + 2, addr);
 928         write_uint16(sl->cdb_cmd_blk + 6, len);
 929
 930         // data_in 0-0x40-len
 931         // !!! len _and_ q_len must be max 6k,
 932         //     i.e. >1024 * 6 = 6144 -> aboard)
 933         // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 934         //     (broken residue issue)
 935         sl->q_len = len;
 936         sl->q_addr = addr;
 937         stlink_q(sl);
 938         stlink_print_data(sl);
 939 }
 940
 941 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 942 void stlink_write_mem8(struct stlink *sl, uint32_t addr, uint16_t len) {
 943         D(sl, "\n*** stlink_write_mem8 ***\n");
 944         clear_cdb(sl);
 945         sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 946         // 2-5: addr
 947         // 6-7: len (>0x40 (64) -> aboard)
 948         write_uint32(sl->cdb_cmd_blk + 2, addr);
 949         write_uint16(sl->cdb_cmd_blk + 6, len);
 950
 951         // data_out 0-len
 952         sl->q_len = len;
 953         sl->q_addr = addr;
 954         sl->q_data_dir = Q_DATA_OUT;
 955         stlink_q(sl);
 956         stlink_print_data(sl);
 957 }
 958
 959 void stlink_write_mem32(struct stlink *sl, uint32_t addr, uint16_t len);
 960 void stlink_write_mem16(struct stlink *sl, uint32_t addr, uint16_t len) {
 961   stlink_write_mem32(sl, addr, 4);
 962 }
 963
 964 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 965 void stlink_write_mem32(struct stlink *sl, uint32_t addr, uint16_t len) {
 966         D(sl, "\n*** stlink_write_mem32 ***\n");
 967         if (len % 4 != 0) {
 968                 fprintf(
 969                         stderr,
 970                         "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 971                         len % 4);
 972                 return;
 973         }
 974         clear_cdb(sl);
 975         sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 976         // 2-5: addr
 977         // 6-7: len "unlimited"
 978         write_uint32(sl->cdb_cmd_blk + 2, addr);
 979         write_uint16(sl->cdb_cmd_blk + 6, len);
 980
 981         // data_out 0-0x40-...-len
 982         sl->q_len = len;
 983         sl->q_addr = addr;
 984         sl->q_data_dir = Q_DATA_OUT;
 985         stlink_q(sl);
 986         stlink_print_data(sl);
 987 }
 988
 989 /* FPEC flash controller interface, pm0063 manual
 990  */
 991
 992 #define FLASH_REGS_ADDR 0x40022000
 993 #define FLASH_REGS_SIZE 0x28
 994
 995 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
 996 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
 997 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
 998 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
 999 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
1000 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
1001 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
1002
1003 #define FLASH_RDPTR_KEY 0x00a5
1004 #define FLASH_KEY1 0x45670123
1005 #define FLASH_KEY2 0xcdef89ab
1006
1007 #define FLASH_SR_BSY 0
1008 #define FLASH_SR_EOP 5
1009
1010 #define FLASH_CR_PG 0
1011 #define FLASH_CR_PER 1
1012 #define FLASH_CR_MER 2
1013 #define FLASH_CR_STRT 6
1014 #define FLASH_CR_LOCK 7
1015
1016 static uint32_t __attribute__((unused)) read_flash_rdp(struct stlink* sl)
1017 {
1018   stlink_read_mem32(sl, FLASH_WRPR, sizeof(uint32_t));
1019   return (*(uint32_t*)sl->q_buf) & 0xff;
1020 }
1021
1022 static inline uint32_t read_flash_wrpr(struct stlink* sl)
1023 {
1024   stlink_read_mem32(sl, FLASH_WRPR, sizeof(uint32_t));
1025   return *(uint32_t*)sl->q_buf;
1026 }
1027
1028 static inline uint32_t read_flash_obr(struct stlink* sl)
1029 {
1030   stlink_read_mem32(sl, FLASH_OBR, sizeof(uint32_t));
1031   return *(uint32_t*)sl->q_buf;
1032 }
1033
1034 static inline uint32_t read_flash_cr(struct stlink* sl)
1035 {
1036   stlink_read_mem32(sl, FLASH_CR, sizeof(uint32_t));
1037   return *(uint32_t*)sl->q_buf;
1038 }
1039
1040 static inline unsigned int is_flash_locked(struct stlink* sl)
1041 {
1042   /* return non zero for true */
1043   return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
1044 }
1045
1046 static void unlock_flash(struct stlink* sl)
1047 {
1048   /* the unlock sequence consists of 2 write cycles where
1049      2 key values are written to the FLASH_KEYR register.
1050      an invalid sequence results in a definitive lock of
1051      the FPEC block until next reset.
1052   */
1053
1054   write_uint32(sl->q_buf, FLASH_KEY1);
1055   stlink_write_mem32(sl, FLASH_KEYR, sizeof(uint32_t));
1056
1057   write_uint32(sl->q_buf, FLASH_KEY2);
1058   stlink_write_mem32(sl, FLASH_KEYR, sizeof(uint32_t));
1059 }
1060
1061 static int unlock_flash_if(struct stlink* sl)
1062 {
1063   /* unlock flash if already locked */
1064
1065   if (is_flash_locked(sl))
1066   {
1067     unlock_flash(sl);
1068     if (is_flash_locked(sl))
1069       return -1;
1070   }
1071
1072   return 0;
1073 }
1074
1075 static void lock_flash(struct stlink* sl)
1076 {
1077   /* write to 1 only. reset by hw at unlock sequence */
1078
1079   const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
1080
1081   write_uint32(sl->q_buf, n);
1082   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1083 }
1084
1085 static void set_flash_cr_pg(struct stlink* sl)
1086 {
1087   const uint32_t n = 1 << FLASH_CR_PG;
1088   write_uint32(sl->q_buf, n);
1089   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1090 }
1091
1092 static void __attribute__((unused)) clear_flash_cr_pg(struct stlink* sl)
1093 {
1094   const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
1095   write_uint32(sl->q_buf, n);
1096   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1097 }
1098
1099 static void set_flash_cr_per(struct stlink* sl)
1100 {
1101   const uint32_t n = 1 << FLASH_CR_PER;
1102   write_uint32(sl->q_buf, n);
1103   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1104 }
1105
1106 static void __attribute__((unused)) clear_flash_cr_per(struct stlink* sl)
1107 {
1108   const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
1109   write_uint32(sl->q_buf, n);
1110   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1111 }
1112
1113 static void set_flash_cr_mer(struct stlink* sl)
1114 {
1115   const uint32_t n = 1 << FLASH_CR_MER;
1116   write_uint32(sl->q_buf, n);
1117   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1118 }
1119
1120 static void __attribute__((unused)) clear_flash_cr_mer(struct stlink* sl)
1121 {
1122   const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER);
1123   write_uint32(sl->q_buf, n);
1124   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1125 }
1126
1127 static void set_flash_cr_strt(struct stlink* sl)
1128 {
1129   /* assume come on the flash_cr_per path */
1130   const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT);
1131   write_uint32(sl->q_buf, n);
1132   stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t));
1133 }
1134
1135 static inline uint32_t read_flash_acr(struct stlink* sl)
1136 {
1137   stlink_read_mem32(sl, FLASH_ACR, sizeof(uint32_t));
1138   return *(uint32_t*)sl->q_buf;
1139 }
1140
1141 static inline uint32_t read_flash_sr(struct stlink* sl)
1142 {
1143   stlink_read_mem32(sl, FLASH_SR, sizeof(uint32_t));
1144   return *(uint32_t*)sl->q_buf;
1145 }
1146
1147 static inline unsigned int is_flash_busy(struct stlink* sl)
1148 {
1149   return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
1150 }
1151
1152 static void wait_flash_busy(struct stlink* sl)
1153 {
1154   /* todo: add some delays here */
1155   while (is_flash_busy(sl))
1156     ;
1157 }
1158
1159 static inline unsigned int is_flash_eop(struct stlink* sl)
1160 {
1161   return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
1162 }
1163
1164 static void __attribute__((unused)) clear_flash_sr_eop(struct stlink* sl)
1165 {
1166   const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
1167   write_uint32(sl->q_buf, n);
1168   stlink_write_mem32(sl, FLASH_SR, sizeof(uint32_t));
1169 }
1170
1171 static void __attribute__((unused)) wait_flash_eop(struct stlink* sl)
1172 {
1173   /* todo: add some delays here */
1174   while (is_flash_eop(sl) == 0)
1175     ;
1176 }
1177
1178 static inline void write_flash_ar(struct stlink* sl, uint32_t n)
1179 {
1180   write_uint32(sl->q_buf, n);
1181   stlink_write_mem32(sl, FLASH_AR, sizeof(uint32_t));
1182 }
1183
1184 #if 0 /* todo */
1185 static void disable_flash_read_protection(struct stlink* sl)
1186 {
1187   /* erase the option byte area */
1188   /* rdp = 0x00a5; */
1189   /* reset */
1190 }
1191 #endif /* todo */
1192
1193 #if 0 /* not working */
1194 static int write_flash_mem16
1195 (struct stlink* sl, uint32_t addr, uint16_t val)
1196 {
1197   /* half word writes */
1198   if (addr % 2) return -1;
1199
1200   /* unlock if locked */
1201   unlock_flash_if(sl);
1202
1203   /* set flash programming chosen bit */
1204   set_flash_cr_pg(sl);
1205
1206   write_uint16(sl->q_buf, val);
1207   stlink_write_mem16(sl, addr, 2);
1208
1209   /* wait for non business */
1210   wait_flash_busy(sl);
1211
1212   lock_flash(sl);
1213
1214   /* check the programmed value back */
1215   stlink_read_mem16(sl, addr, 2);
1216   if (*(const uint16_t*)sl->q_buf != val)
1217   {
1218     /* values differ at i * sizeof(uint16_t) */
1219     return -1;
1220   }
1221
1222   /* success */
1223   return 0;
1224 }
1225 #endif /* not working */
1226
1227 static int erase_flash_page(struct stlink* sl, stm32_addr_t page)
1228 {
1229   /* page an addr in the page to erase */
1230
1231   /* wait for ongoing op to finish */
1232   wait_flash_busy(sl);
1233
1234   /* unlock if locked */
1235   unlock_flash_if(sl);
1236
1237   /* set the page erase bit */
1238   set_flash_cr_per(sl);
1239
1240   /* select the page to erase */
1241   write_flash_ar(sl, page);
1242
1243   /* start erase operation, reset by hw with bsy bit */
1244   set_flash_cr_strt(sl);
1245
1246   /* wait for completion */
1247   wait_flash_busy(sl);
1248
1249   /* relock the flash */
1250   lock_flash(sl);
1251
1252   /* todo: verify the erased page */
1253
1254   return 0;
1255 }
1256
1257 static int __attribute__((unused)) erase_flash_mass(struct stlink* sl)
1258 {
1259   /* wait for ongoing op to finish */
1260   wait_flash_busy(sl);
1261
1262   /* unlock if locked */
1263   unlock_flash_if(sl);
1264
1265   /* set the mass erase bit */
1266   set_flash_cr_mer(sl);
1267
1268   /* start erase operation, reset by hw with bsy bit */
1269   set_flash_cr_strt(sl);
1270
1271   /* wait for completion */
1272   wait_flash_busy(sl);
1273
1274   /* relock the flash */
1275   lock_flash(sl);
1276
1277   /* todo: verify the erased memory */
1278
1279   return 0;
1280 }
1281
1282 static unsigned int is_core_halted(struct stlink* sl)
1283 {
1284   /* return non zero if core is halted */
1285   stlink_status(sl);
1286   return sl->q_buf[0] == STLINK_CORE_HALTED;
1287 }
1288
1289 static int write_loader_to_sram
1290 (struct stlink* sl, stm32_addr_t* addr, size_t* size)
1291 {
1292   /* from openocd, contrib/loaders/flash/stm32.s */
1293   static const uint8_t loader_code[] =
1294   {
1295     0x08, 0x4c,                 /* ldr  r4, STM32_FLASH_BASE */
1296     0x1c, 0x44,                 /* add  r4, r3 */
1297     /* write_half_word: */
1298     0x01, 0x23,                 /* movs r3, #0x01 */
1299     0x23, 0x61,                 /* str  r3, [r4, #STM32_FLASH_CR_OFFSET] */
1300     0x30, 0xf8, 0x02, 0x3b,     /* ldrh r3, [r0], #0x02 */
1301     0x21, 0xf8, 0x02, 0x3b,     /* strh r3, [r1], #0x02 */
1302     /* busy: */
1303     0xe3, 0x68,                 /* ldr  r3, [r4, #STM32_FLASH_SR_OFFSET] */
1304     0x13, 0xf0, 0x01, 0x0f,     /* tst  r3, #0x01 */
1305     0xfb, 0xd0,                 /* beq  busy */
1306     0x13, 0xf0, 0x14, 0x0f,     /* tst  r3, #0x14 */
1307     0x01, 0xd1,                 /* bne  exit */
1308     0x01, 0x3a,                 /* subs r2, r2, #0x01 */
1309     0xf0, 0xd1,                 /* bne  write_half_word */
1310     /* exit: */
1311     0x00, 0xbe,                 /* bkpt #0x00 */
1312     0x00, 0x20, 0x02, 0x40,     /* STM32_FLASH_BASE: .word 0x40022000 */
1313   };
1314
1315   memcpy(sl->q_buf, loader_code, sizeof(loader_code));
1316   stlink_write_mem32(sl, sl->sram_base, sizeof(loader_code));
1317
1318   *addr = sl->sram_base;
1319   *size = sizeof(loader_code);
1320
1321   /* success */
1322   return 0;
1323 }
1324
1325 typedef struct flash_loader
1326 {
1327   stm32_addr_t loader_addr; /* loader sram adddr */
1328   stm32_addr_t buf_addr; /* buffer sram address */
1329 } flash_loader_t;
1330
1331 static int write_buffer_to_sram
1332 (struct stlink* sl, flash_loader_t* fl, const uint8_t* buf, size_t size)
1333 {
1334   /* write the buffer right after the loader */
1335   memcpy(sl->q_buf, buf, size);
1336   stlink_write_mem8(sl, fl->buf_addr, size);
1337   return 0;
1338 }
1339
1340 static int init_flash_loader
1341 (struct stlink* sl, flash_loader_t* fl)
1342 {
1343   size_t size;
1344
1345   /* allocate the loader in sram */
1346   if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1)
1347   {
1348     fprintf(stderr, "write_loader_to_sram() == -1\n");
1349     return -1;
1350   }
1351
1352   /* allocate a one page buffer in sram right after loader */
1353   fl->buf_addr = fl->loader_addr + size;
1354
1355   return 0;
1356 }
1357
1358 static int run_flash_loader
1359 (struct stlink* sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size)
1360 {
1361   const size_t count = size / sizeof(uint16_t);
1362
1363   if (write_buffer_to_sram(sl, fl, buf, size) == -1)
1364   {
1365     fprintf(stderr, "write_buffer_to_sram() == -1\n");
1366     return -1;
1367   }
1368
1369   /* setup core */
1370   stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1371   stlink_write_reg(sl, target, 1); /* target */
1372   stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1373   stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1374   stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1375
1376   /* unlock and set programming mode */
1377   unlock_flash_if(sl);
1378   set_flash_cr_pg(sl);
1379
1380   /* run loader */
1381   stlink_run(sl);
1382
1383   while (is_core_halted(sl) == 0)
1384     ;
1385
1386   lock_flash(sl);
1387
1388   /* not all bytes have been written */
1389   stlink_read_reg(sl, 2);
1390   if (sl->reg.r[2] != 0)
1391   {
1392     fprintf(stderr, "write error, count == %u\n", sl->reg.r[2]);
1393     return -1;
1394   }
1395
1396   return 0;
1397 }
1398
1399 /* memory mapped file */
1400
1401 typedef struct mapped_file
1402 {
1403   uint8_t* base;
1404   size_t len;
1405 } mapped_file_t;
1406
1407 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
1408
1409 static int map_file(mapped_file_t* mf, const char* path)
1410 {
1411   int error = -1;
1412   struct stat st;
1413
1414   const int fd = open(path, O_RDONLY);
1415   if (fd == -1)
1416   {
1417     fprintf(stderr, "open(%s) == -1\n", path);
1418     return -1;
1419   }
1420
1421   if (fstat(fd, &st) == -1)
1422   {
1423     fprintf(stderr, "fstat() == -1\n");
1424     goto on_error;
1425   }
1426
1427   mf->base = (uint8_t*)mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
1428   if (mf->base == MAP_FAILED)
1429   {
1430     fprintf(stderr, "mmap() == MAP_FAILED\n");
1431     goto on_error;
1432   }
1433
1434   mf->len = st.st_size;
1435
1436   /* success */
1437   error = 0;
1438
1439  on_error:
1440   close(fd);
1441
1442   return error;
1443 }
1444
1445 static void unmap_file(mapped_file_t* mf)
1446 {
1447   munmap((void*)mf->base, mf->len);
1448   mf->base = (unsigned char*)MAP_FAILED;
1449   mf->len = 0;
1450 }
1451
1452 static int check_file
1453 (struct stlink* sl, mapped_file_t* mf, stm32_addr_t addr)
1454 {
1455   size_t off;
1456
1457   for (off = 0; off < mf->len; off += sl->flash_pgsz)
1458   {
1459     size_t aligned_size;
1460
1461     /* adjust last page size */
1462     size_t cmp_size = sl->flash_pgsz;
1463     if ((off + sl->flash_pgsz) > mf->len)
1464       cmp_size = mf->len - off;
1465
1466     aligned_size = cmp_size;
1467     if (aligned_size & (4 - 1))
1468       aligned_size = (cmp_size + 4) & ~(4 - 1);
1469
1470     stlink_read_mem32(sl, addr + off, aligned_size);
1471
1472     if (memcmp(sl->q_buf, mf->base + off, cmp_size))
1473       return -1;
1474   }
1475
1476   return 0;
1477 }
1478
1479 static int stlink_fcheck_flash
1480 (struct stlink* sl, const char* path, stm32_addr_t addr)
1481 {
1482   /* check the contents of path are at addr */
1483
1484   int res;
1485   mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1486
1487   if (map_file(&mf, path) == -1)
1488     return -1;
1489
1490   res = check_file(sl, &mf, addr);
1491
1492   unmap_file(&mf);
1493
1494   return res;
1495 }
1496
1497 static int stlink_fwrite_flash
1498 (struct stlink* sl, const char* path, stm32_addr_t addr)
1499 {
1500   /* write the file in flash at addr */
1501
1502   int error = -1;
1503   size_t off;
1504   mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1505   flash_loader_t fl;
1506
1507   if (map_file(&mf, path) == -1)
1508   {
1509     fprintf(stderr, "map_file() == -1\n");
1510     return -1;
1511   }
1512
1513   /* check addr range is inside the flash */
1514   if (addr < sl->flash_base)
1515   {
1516     fprintf(stderr, "addr too low\n");
1517     goto on_error;
1518   }
1519   else if ((addr + mf.len) < addr)
1520   {
1521     fprintf(stderr, "addr overruns\n");
1522     goto on_error;
1523   }
1524   else if ((addr + mf.len) > (sl->flash_base + sl->flash_size))
1525   {
1526     fprintf(stderr, "addr too high\n");
1527     goto on_error;
1528   }
1529   else if ((addr & 1) || (mf.len & 1))
1530   {
1531     /* todo */
1532     fprintf(stderr, "unaligned addr or size\n");
1533     goto on_error;
1534   }
1535
1536   /* erase each page. todo: mass erase faster? */
1537   for (off = 0; off < mf.len; off += sl->flash_pgsz)
1538   {
1539     /* addr must be an addr inside the page */
1540     if (erase_flash_page(sl, addr + off) == -1)
1541     {
1542       fprintf(stderr, "erase_flash_page(0x%x) == -1\n", addr + off);
1543       goto on_error;
1544     }
1545   }
1546
1547   /* flash loader initialization */
1548   if (init_flash_loader(sl, &fl) == -1)
1549   {
1550     fprintf(stderr, "init_flash_loader() == -1\n");
1551     goto on_error;
1552   }
1553
1554   /* write each page */
1555   for (off = 0; off < mf.len; off += sl->flash_pgsz)
1556   {
1557     /* adjust last page size */
1558     size_t size = sl->flash_pgsz;
1559     if ((off + sl->flash_pgsz) > mf.len)
1560       size = mf.len - off;
1561
1562     if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1)
1563     {
1564       fprintf(stderr, "run_flash_loader(0x%x) == -1\n", addr + off);
1565       goto on_error;
1566     }
1567   }
1568
1569   /* check the file ha been written */
1570   if (check_file(sl, &mf, addr) == -1)
1571   {
1572     fprintf(stderr, "check_file() == -1\n");
1573     goto on_error;
1574   }
1575
1576   /* success */
1577   error = 0;
1578
1579  on_error:
1580   unmap_file(&mf);
1581   return error;
1582 }
1583
1584 static int stlink_fwrite_sram
1585 (struct stlink* sl, const char* path, stm32_addr_t addr)
1586 {
1587   /* write the file in sram at addr */
1588
1589   int error = -1;
1590   size_t off;
1591   mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1592
1593   if (map_file(&mf, path) == -1)
1594   {
1595     fprintf(stderr, "map_file() == -1\n");
1596     return -1;
1597   }
1598
1599   /* check addr range is inside the sram */
1600   if (addr < sl->sram_base)
1601   {
1602     fprintf(stderr, "addr too low\n");
1603     goto on_error;
1604   }
1605   else if ((addr + mf.len) < addr)
1606   {
1607     fprintf(stderr, "addr overruns\n");
1608     goto on_error;
1609   }
1610   else if ((addr + mf.len) > (sl->sram_base + sl->sram_size))
1611   {
1612     fprintf(stderr, "addr too high\n");
1613     goto on_error;
1614   }
1615   else if ((addr & 3) || (mf.len & 3))
1616   {
1617     /* todo */
1618     fprintf(stderr, "unaligned addr or size\n");
1619     goto on_error;
1620   }
1621
1622   /* do the copy by 1k blocks */
1623   for (off = 0; off < mf.len; off += 1024)
1624   {
1625     size_t size = 1024;
1626     if ((off + size) > mf.len)
1627       size = mf.len - off;
1628
1629     memcpy(sl->q_buf, mf.base + off, size);
1630
1631     /* round size if needed */
1632     if (size & 3)
1633       size += 2;
1634
1635     stlink_write_mem32(sl, addr + off, size);
1636   }
1637
1638   /* check the file ha been written */
1639   if (check_file(sl, &mf, addr) == -1)
1640   {
1641     fprintf(stderr, "check_file() == -1\n");
1642     goto on_error;
1643   }
1644
1645   /* success */
1646   error = 0;
1647
1648  on_error:
1649   unmap_file(&mf);
1650   return error;
1651 }
1652
1653
1654 static int stlink_fread
1655 (struct stlink* sl, const char* path, stm32_addr_t addr, size_t size)
1656 {
1657   /* read size bytes from addr to file */
1658
1659   int error = -1;
1660   size_t off;
1661
1662   const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
1663   if (fd == -1)
1664   {
1665     fprintf(stderr, "open(%s) == -1\n", path);
1666     return -1;
1667   }
1668
1669   /* do the copy by 1k blocks */
1670   for (off = 0; off < size; off += 1024)
1671   {
1672     size_t read_size = 1024;
1673     if ((off + read_size) > size)
1674       read_size = off + read_size;
1675
1676     /* round size if needed */
1677     if (read_size & 3)
1678       read_size = (read_size + 4) & ~(3);
1679
1680     stlink_read_mem32(sl, addr + off, read_size);
1681
1682     if (write(fd, sl->q_buf, read_size) != (ssize_t)read_size)
1683     {
1684       fprintf(stderr, "write() != read_size\n");
1685       goto on_error;
1686     }
1687   }
1688
1689   /* success */
1690   error = 0;
1691
1692  on_error:
1693   close(fd);
1694
1695   return error;
1696 }
1697
1698 // 1) open a sg device, switch the stlink from dfu to mass mode
1699 // 2) wait 5s until the kernel driver stops reseting the broken device
1700 // 3) reopen the device
1701 // 4) the device driver is now ready for a switch to jtag/swd mode
1702 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
1703 struct stlink* stlink_force_open(const char *dev_name, const int verbose) {
1704         struct stlink *sl = stlink_open(dev_name, verbose);
1705         if (sl == NULL) {
1706                 fputs("Error: could not open stlink device\n", stderr);
1707                 return NULL;
1708         }
1709
1710         stlink_version(sl);
1711
1712         if (sl->st_vid != USB_ST_VID || sl->stlink_pid != USB_STLINK_PID) {
1713                 fprintf(stderr, "Error: the device %s is not a stlink\n",
1714                         dev_name);
1715                 fprintf(stderr, "       VID: got %04x expect %04x \n",
1716                         sl->st_vid, USB_ST_VID);
1717                 fprintf(stderr, "       PID: got %04x expect %04x \n",
1718                         sl->stlink_pid, USB_STLINK_PID);
1719                 return NULL;
1720         }
1721
1722         D(sl, "\n*** stlink_force_open ***\n");
1723         switch (stlink_current_mode(sl)) {
1724         case STLINK_DEV_MASS_MODE:
1725                 return sl;
1726         case STLINK_DEV_DEBUG_MODE:
1727                 // TODO go to mass?
1728                 return sl;
1729         }
1730         fprintf(stderr, "\n*** switch the stlink to mass mode ***\n");
1731         stlink_exit_dfu_mode(sl);
1732         // exit the dfu mode -> the device is gone
1733         fprintf(stderr, "\n*** reopen the stlink device ***\n");
1734         delay(1000);
1735         stlink_close(sl);
1736         delay(5000);
1737
1738         sl = stlink_open(dev_name, verbose);
1739         if (sl == NULL) {
1740                 fputs("Error: could not open stlink device\n", stderr);
1741                 return NULL;
1742         }
1743         // re-query device info
1744         stlink_version(sl);
1745         return sl;
1746 }
1747
1748 static void __attribute__((unused)) mark_buf(struct stlink *sl) {
1749         clear_buf(sl);
1750         sl->q_buf[0] = 0x12;
1751         sl->q_buf[1] = 0x34;
1752         sl->q_buf[2] = 0x56;
1753         sl->q_buf[3] = 0x78;
1754         sl->q_buf[4] = 0x90;
1755         sl->q_buf[15] = 0x42;
1756         sl->q_buf[16] = 0x43;
1757         sl->q_buf[63] = 0x42;
1758         sl->q_buf[64] = 0x43;
1759         sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
1760         sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
1761 }
1762
1763 int main(int argc, char *argv[]) {
1764         // set scpi lib debug level: 0 for no debug info, 10 for lots
1765         const int scsi_verbose = 2;
1766         char *dev_name;
1767
1768         switch (argc) {
1769         case 1:
1770                 fputs(
1771                         "\nUsage: stlink-access-test /dev/sg0, sg1, ...\n"
1772                                 "\n*** Notice: The stlink firmware violates the USB standard.\n"
1773                                 "*** If you plug-in the discovery's stlink, wait a several\n"
1774                                 "*** minutes to let the kernel driver swallow the broken device.\n"
1775                                 "*** Watch:\ntail -f /var/log/messages\n"
1776                                 "*** This command sequence can shorten the waiting time and fix some issues.\n"
1777                                 "*** Unplug the stlink and execute once as root:\n"
1778                                 "modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:lrwsro\n\n",
1779                         stderr);
1780                 return EXIT_FAILURE;
1781         case 2:
1782                 dev_name = argv[1];
1783                 break;
1784         default:
1785                 return EXIT_FAILURE;
1786         }
1787
1788         fputs("*** stlink access test ***\n", stderr);
1789         fprintf(stderr, "Using sg_lib %s : scsi_pt %s\n", sg_lib_version(),
1790                 scsi_pt_version());
1791
1792         struct stlink *sl = stlink_force_open(dev_name, scsi_verbose);
1793         if (sl == NULL)
1794                 return EXIT_FAILURE;
1795
1796         // we are in mass mode, go to swd
1797         stlink_enter_swd_mode(sl);
1798         stlink_current_mode(sl);
1799         stlink_core_id(sl);
1800         //----------------------------------------------------------------------
1801
1802         stlink_status(sl);
1803         //stlink_force_debug(sl);
1804         stlink_reset(sl);
1805         stlink_status(sl);
1806 #if 0
1807         // core system control block
1808         stlink_read_mem32(sl, 0xe000ed00, 4);
1809         fprintf(stderr, "cpu id base register: SCB_CPUID = got 0x%08x expect 0x411fc231", read_uint32(sl->q_buf, 0));
1810         // no MPU
1811         stlink_read_mem32(sl, 0xe000ed90, 4);
1812         fprintf(stderr, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0));
1813
1814         stlink_read_mem32(sl, 0xe000edf0, 4);
1815         fprintf(stderr, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0));
1816
1817         stlink_read_mem32(sl, 0x4001100c, 4);
1818         fprintf(stderr, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0));
1819 #endif
1820 #if 0
1821         // happy new year 2011: let blink all the leds
1822         // see "RM0041 Reference manual - STM32F100xx advanced ARM-based 32-bit MCUs"
1823
1824 #define GPIOC           0x40011000 // port C
1825 #define GPIOC_CRH       (GPIOC + 0x04) // port configuration register high
1826 #define GPIOC_ODR       (GPIOC + 0x0c) // port output data register
1827 #define LED_BLUE        (1<<8) // pin 8
1828 #define LED_GREEN       (1<<9) // pin 9
1829         stlink_read_mem32(sl, GPIOC_CRH, 4);
1830         uint32_t io_conf = read_uint32(sl->q_buf, 0);
1831         fprintf(stderr, "GPIOC_CRH = 0x%08x", io_conf);
1832
1833         // set: general purpose output push-pull, output mode, max speed 10 MHz.
1834         write_uint32(sl->q_buf, 0x44444411);
1835         stlink_write_mem32(sl, GPIOC_CRH, 4);
1836
1837         clear_buf(sl);
1838         for (int i = 0; i < 100; i++) {
1839                 write_uint32(sl->q_buf, LED_BLUE | LED_GREEN);
1840                 stlink_write_mem32(sl, GPIOC_ODR, 4);
1841                 /* stlink_read_mem32(sl, 0x4001100c, 4); */
1842                 /* fprintf(stderr, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0)); */
1843                 delay(100);
1844
1845                 clear_buf(sl);
1846                 stlink_write_mem32(sl, GPIOC_ODR, 4); // PC lo
1847                 delay(100);
1848         }
1849         write_uint32(sl->q_buf, io_conf); // set old state
1850
1851 #endif
1852 #if 0
1853         // TODO rtfm: stlink doesn't have flash write routines
1854         // writing to the flash area confuses the fw for the next read access
1855
1856         //stlink_read_mem32(sl, 0, 1024*6);
1857         // flash 0x08000000 128kB
1858         fputs("++++++++++ read a flash at 0x0800 0000\n", stderr);
1859         stlink_read_mem32(sl, 0x08000000, 1024 * 6); //max 6kB
1860         clear_buf(sl);
1861         stlink_read_mem32(sl, 0x08000c00, 5);
1862         stlink_read_mem32(sl, 0x08000c00, 4);
1863         mark_buf(sl);
1864         stlink_write_mem32(sl, 0x08000c00, 4);
1865         stlink_read_mem32(sl, 0x08000c00, 256);
1866         stlink_read_mem32(sl, 0x08000c00, 256);
1867 #endif
1868 #if 0
1869         // sram 0x20000000 8kB
1870         fputs("\n++++++++++ read/write 8bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
1871         clear_buf(sl);
1872         stlink_write_mem8(sl, 0x20000000, 16);
1873
1874         mark_buf(sl);
1875         stlink_write_mem8(sl, 0x20000000, 1);
1876         stlink_write_mem8(sl, 0x20000001, 1);
1877         stlink_write_mem8(sl, 0x2000000b, 3);
1878         stlink_read_mem32(sl, 0x20000000, 16);
1879 #endif
1880 #if 0
1881         // a not aligned mem32 access doesn't work indeed
1882         fputs("\n++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
1883         clear_buf(sl);
1884         stlink_write_mem8(sl, 0x20000000, 32);
1885
1886         mark_buf(sl);
1887         stlink_write_mem32(sl, 0x20000000, 1);
1888         stlink_read_mem32(sl, 0x20000000, 16);
1889         mark_buf(sl);
1890         stlink_write_mem32(sl, 0x20000001, 1);
1891         stlink_read_mem32(sl, 0x20000000, 16);
1892         mark_buf(sl);
1893         stlink_write_mem32(sl, 0x2000000b, 3);
1894         stlink_read_mem32(sl, 0x20000000, 16);
1895
1896         mark_buf(sl);
1897         stlink_write_mem32(sl, 0x20000000, 17);
1898         stlink_read_mem32(sl, 0x20000000, 32);
1899 #endif
1900 #if 0
1901         // sram 0x20000000 8kB
1902         fputs("++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++\n", stderr);
1903         mark_buf(sl);
1904         stlink_write_mem8(sl, 0x20000000, 64);
1905         stlink_read_mem32(sl, 0x20000000, 64);
1906
1907         mark_buf(sl);
1908         stlink_write_mem32(sl, 0x20000000, 1024 * 8); //8kB
1909         stlink_read_mem32(sl, 0x20000000, 1024 * 6);
1910         stlink_read_mem32(sl, 0x20000000 + 1024 * 6, 1024 * 2);
1911 #endif
1912 #if 0
1913         stlink_read_all_regs(sl);
1914         stlink_step(sl);
1915         fputs("++++++++++ write r0 = 0x12345678\n", stderr);
1916         stlink_write_reg(sl, 0x12345678, 0);
1917         stlink_read_reg(sl, 0);
1918         stlink_read_all_regs(sl);
1919 #endif
1920 #if 0
1921         stlink_run(sl);
1922         stlink_status(sl);
1923
1924         stlink_force_debug(sl);
1925         stlink_status(sl);
1926 #endif
1927 #if 1 /* read the system bootloader */
1928         fputs("\n++++++++++ reading bootloader ++++++++++++++++\n\n", stderr);
1929         stlink_fread(sl, "/tmp/barfoo", sl->sys_base, sl->sys_size);
1930 #endif
1931 #if 0 /* read the flash memory */
1932         fputs("\n+++++++ read flash memory\n\n", stderr);
1933         /* mark_buf(sl); */
1934         stlink_read_mem32(sl, 0x08000000, 4);
1935 #endif
1936 #if 0 /* flash programming */
1937         fputs("\n+++++++ program flash memory\n\n", stderr);
1938         stlink_fwrite_flash(sl, "/tmp/foobar", 0x08000000);
1939 #endif
1940 #if 0 /* check file contents */
1941         fputs("\n+++++++ check flash memory\n\n", stderr);
1942         {
1943           const int res = stlink_fcheck_flash(sl, "/tmp/foobar", 0x08000000);
1944           printf("_____ stlink_fcheck_flash() == %d\n", res);
1945         }
1946 #endif
1947 #if 0
1948         fputs("\n+++++++ sram write and execute\n\n", stderr);
1949         stlink_fwrite_sram(sl, "/tmp/foobar", sl->sram_base);
1950         stlink_run_at(sl, sl->sram_base);
1951 #endif
1952
1953         stlink_run(sl);
1954         stlink_status(sl);
1955         //----------------------------------------------------------------------
1956         // back to mass mode, just in case ...
1957         stlink_exit_debug_mode(sl);
1958         stlink_current_mode(sl);
1959         stlink_close(sl);
1960
1961         //fflush(stderr); fflush(stdout);
1962         return EXIT_SUCCESS;
1963 }