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