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