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 int stlink_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 int stlink_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 // The stlink_fwrite_flash should not muck with mmapped files inside itself,
1382 // and should use this function instead. (Hell, what's the reason behind mmap
1383 // there?!) But, as it is not actually used anywhere, nobody cares.
1384
1385 #define WRITE_BLOCK_SIZE 0x40
1386 int stlink_write_flash(struct stlink* sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1387   int error = -1;
1388   size_t off;
1389   flash_loader_t fl;
1390
1391   /* check addr range is inside the flash */
1392   if (addr < sl->flash_base) {
1393     fprintf(stderr, "addr too low\n");
1394     return -1;
1395   } else if ((addr + len) < addr) {
1396     fprintf(stderr, "addr overruns\n");
1397     return -1;
1398   } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1399     fprintf(stderr, "addr too high\n");
1400     return -1;
1401   } else if ((addr & 1) || (len & 1)) {
1402     fprintf(stderr, "unaligned addr or size\n");
1403     return -1;
1404   }
1405
1406   /* flash loader initialization */
1407   if (init_flash_loader(sl, &fl) == -1) {
1408     fprintf(stderr, "init_flash_loader() == -1\n");
1409     return -1;
1410   }
1411
1412   /* write each page. above WRITE_BLOCK_SIZE fails? */
1413   for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
1414     /* adjust last write size */
1415     size_t size = WRITE_BLOCK_SIZE;
1416     if((off + WRITE_BLOCK_SIZE) > len)
1417       size = len - off;
1418
1419     if(run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1420       fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1421       return -1;
1422     }
1423   }
1424
1425   for(off = 0; off < len; off += sl->flash_pgsz) {
1426     size_t aligned_size;
1427
1428     /* adjust last page size */
1429     size_t cmp_size = sl->flash_pgsz;
1430     if ((off + sl->flash_pgsz) > len)
1431       cmp_size = len - off;
1432
1433     aligned_size = cmp_size;
1434     if (aligned_size & (4 - 1))
1435       aligned_size = (cmp_size + 4) & ~(4 - 1);
1436
1437     stlink_read_mem32(sl, addr + off, aligned_size);
1438
1439     if (memcmp(sl->q_buf, base + off, cmp_size))
1440       return -1;
1441   }
1442
1443   return 0;
1444 }
1445
1446 static int stlink_fwrite_flash
1447 (struct stlink* sl, const char* path, stm32_addr_t addr)
1448 {
1449   /* write the file in flash at addr */
1450
1451   int error = -1;
1452   size_t off;
1453   mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1454   flash_loader_t fl;
1455
1456   if (map_file(&mf, path) == -1)
1457   {
1458     fprintf(stderr, "map_file() == -1\n");
1459     return -1;
1460   }
1461
1462   /* check addr range is inside the flash */
1463   if (addr < sl->flash_base)
1464   {
1465     fprintf(stderr, "addr too low\n");
1466     goto on_error;
1467   }
1468   else if ((addr + mf.len) < addr)
1469   {
1470     fprintf(stderr, "addr overruns\n");
1471     goto on_error;
1472   }
1473   else if ((addr + mf.len) > (sl->flash_base + sl->flash_size))
1474   {
1475     fprintf(stderr, "addr too high\n");
1476     goto on_error;
1477   }
1478   else if ((addr & 1) || (mf.len & 1))
1479   {
1480     /* todo */
1481     fprintf(stderr, "unaligned addr or size\n");
1482     goto on_error;
1483   }
1484
1485   /* erase each page. todo: mass erase faster? */
1486   for (off = 0; off < mf.len; off += sl->flash_pgsz)
1487   {
1488     /* addr must be an addr inside the page */
1489     if (stlink_erase_flash_page(sl, addr + off) == -1)
1490     {
1491       fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
1492       goto on_error;
1493     }
1494   }
1495
1496   /* flash loader initialization */
1497   if (init_flash_loader(sl, &fl) == -1)
1498   {
1499     fprintf(stderr, "init_flash_loader() == -1\n");
1500     goto on_error;
1501   }
1502
1503   /* write each page. above WRITE_BLOCK_SIZE fails? */
1504 #define WRITE_BLOCK_SIZE 0x40
1505   for (off = 0; off < mf.len; off += WRITE_BLOCK_SIZE)
1506   {
1507     /* adjust last write size */
1508     size_t size = WRITE_BLOCK_SIZE;
1509     if ((off + WRITE_BLOCK_SIZE) > mf.len)
1510       size = mf.len - off;
1511
1512     if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1)
1513     {
1514       fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1515       goto on_error;
1516     }
1517   }
1518
1519   /* check the file ha been written */
1520   if (check_file(sl, &mf, addr) == -1)
1521   {
1522     fprintf(stderr, "check_file() == -1\n");
1523     goto on_error;
1524   }
1525
1526   /* success */
1527   error = 0;
1528
1529  on_error:
1530   unmap_file(&mf);
1531   return error;
1532 }
1533
1534 static int stlink_fwrite_sram
1535 (struct stlink* sl, const char* path, stm32_addr_t addr)
1536 {
1537   /* write the file in sram at addr */
1538
1539   int error = -1;
1540   size_t off;
1541   mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1542
1543   if (map_file(&mf, path) == -1)
1544   {
1545     fprintf(stderr, "map_file() == -1\n");
1546     return -1;
1547   }
1548
1549   /* check addr range is inside the sram */
1550   if (addr < sl->sram_base)
1551   {
1552     fprintf(stderr, "addr too low\n");
1553     goto on_error;
1554   }
1555   else if ((addr + mf.len) < addr)
1556   {
1557     fprintf(stderr, "addr overruns\n");
1558     goto on_error;
1559   }
1560   else if ((addr + mf.len) > (sl->sram_base + sl->sram_size))
1561   {
1562     fprintf(stderr, "addr too high\n");
1563     goto on_error;
1564   }
1565   else if ((addr & 3) || (mf.len & 3))
1566   {
1567     /* todo */
1568     fprintf(stderr, "unaligned addr or size\n");
1569     goto on_error;
1570   }
1571
1572   /* do the copy by 1k blocks */
1573   for (off = 0; off < mf.len; off += 1024)
1574   {
1575     size_t size = 1024;
1576     if ((off + size) > mf.len)
1577       size = mf.len - off;
1578
1579     memcpy(sl->q_buf, mf.base + off, size);
1580
1581     /* round size if needed */
1582     if (size & 3)
1583       size += 2;
1584
1585     stlink_write_mem32(sl, addr + off, size);
1586   }
1587
1588   /* check the file ha been written */
1589   if (check_file(sl, &mf, addr) == -1)
1590   {
1591     fprintf(stderr, "check_file() == -1\n");
1592     goto on_error;
1593   }
1594
1595   /* success */
1596   error = 0;
1597
1598  on_error:
1599   unmap_file(&mf);
1600   return error;
1601 }
1602
1603
1604 static int stlink_fread
1605 (struct stlink* sl, const char* path, stm32_addr_t addr, size_t size)
1606 {
1607   /* read size bytes from addr to file */
1608
1609   int error = -1;
1610   size_t off;
1611
1612   const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
1613   if (fd == -1)
1614   {
1615     fprintf(stderr, "open(%s) == -1\n", path);
1616     return -1;
1617   }
1618
1619   /* do the copy by 1k blocks */
1620   for (off = 0; off < size; off += 1024)
1621   {
1622     size_t read_size = 1024;
1623     if ((off + read_size) > size)
1624       read_size = off + read_size;
1625
1626     /* round size if needed */
1627     if (read_size & 3)
1628       read_size = (read_size + 4) & ~(3);
1629
1630     stlink_read_mem32(sl, addr + off, read_size);
1631
1632     if (write(fd, sl->q_buf, read_size) != (ssize_t)read_size)
1633     {
1634       fprintf(stderr, "write() != read_size\n");
1635       goto on_error;
1636     }
1637   }
1638
1639   /* success */
1640   error = 0;
1641
1642  on_error:
1643   close(fd);
1644
1645   return error;
1646 }
1647
1648 // 1) open a sg device, switch the stlink from dfu to mass mode
1649 // 2) wait 5s until the kernel driver stops reseting the broken device
1650 // 3) reopen the device
1651 // 4) the device driver is now ready for a switch to jtag/swd mode
1652 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
1653 struct stlink* stlink_quirk_open(const char *dev_name, const int verbose) {
1654         struct stlink *sl = stlink_open(dev_name, verbose);
1655         if (sl == NULL) {
1656                 fputs("Error: could not open stlink device\n", stderr);
1657                 return NULL;
1658         }
1659
1660         stlink_version(sl);
1661
1662         if (sl->st_vid != USB_ST_VID || sl->stlink_pid != USB_STLINK_PID) {
1663                 fprintf(stderr, "Error: the device %s is not a stlink\n",
1664                         dev_name);
1665                 fprintf(stderr, "       VID: got %04x expect %04x \n",
1666                         sl->st_vid, USB_ST_VID);
1667                 fprintf(stderr, "       PID: got %04x expect %04x \n",
1668                         sl->stlink_pid, USB_STLINK_PID);
1669                 return NULL;
1670         }
1671
1672         D(sl, "\n*** stlink_force_open ***\n");
1673         switch (stlink_current_mode(sl)) {
1674         case STLINK_DEV_MASS_MODE:
1675                 return sl;
1676         case STLINK_DEV_DEBUG_MODE:
1677                 // TODO go to mass?
1678                 return sl;
1679         }
1680         DD(sl, "\n*** switch the stlink to mass mode ***\n");
1681         stlink_exit_dfu_mode(sl);
1682         // exit the dfu mode -> the device is gone
1683         DD(sl, "\n*** reopen the stlink device ***\n");
1684         delay(1000);
1685         stlink_close(sl);
1686         delay(5000);
1687
1688         sl = stlink_open(dev_name, verbose);
1689         if (sl == NULL) {
1690                 fputs("Error: could not open stlink device\n", stderr);
1691                 return NULL;
1692         }
1693         // re-query device info
1694         stlink_version(sl);
1695         return sl;
1696 }
1697
1698 static void __attribute__((unused)) mark_buf(struct stlink *sl) {
1699         clear_buf(sl);
1700         sl->q_buf[0] = 0x12;
1701         sl->q_buf[1] = 0x34;
1702         sl->q_buf[2] = 0x56;
1703         sl->q_buf[3] = 0x78;
1704         sl->q_buf[4] = 0x90;
1705         sl->q_buf[15] = 0x42;
1706         sl->q_buf[16] = 0x43;
1707         sl->q_buf[63] = 0x42;
1708         sl->q_buf[64] = 0x43;
1709         sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
1710         sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
1711 }
1712
1713 #if 0
1714 int main(int argc, char *argv[]) {
1715         // set scpi lib debug level: 0 for no debug info, 10 for lots
1716         const int scsi_verbose = 2;
1717         char *dev_name;
1718
1719         switch (argc) {
1720         case 1:
1721                 fputs(
1722                         "\nUsage: stlink-access-test /dev/sg0, sg1, ...\n"
1723                                 "\n*** Notice: The stlink firmware violates the USB standard.\n"
1724                                 "*** If you plug-in the discovery's stlink, wait a several\n"
1725                                 "*** minutes to let the kernel driver swallow the broken device.\n"
1726                                 "*** Watch:\ntail -f /var/log/messages\n"
1727                                 "*** This command sequence can shorten the waiting time and fix some issues.\n"
1728                                 "*** Unplug the stlink and execute once as root:\n"
1729                                 "modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:lrwsro\n\n",
1730                         stderr);
1731                 return EXIT_FAILURE;
1732         case 2:
1733                 dev_name = argv[1];
1734                 break;
1735         default:
1736                 return EXIT_FAILURE;
1737         }
1738
1739         fputs("*** stlink access test ***\n", stderr);
1740         DD(sl, "Using sg_lib %s : scsi_pt %s\n", sg_lib_version(),
1741                 scsi_pt_version());
1742
1743         struct stlink *sl = stlink_force_open(dev_name, scsi_verbose);
1744         if (sl == NULL)
1745                 return EXIT_FAILURE;
1746
1747         // we are in mass mode, go to swd
1748         stlink_enter_swd_mode(sl);
1749         stlink_current_mode(sl);
1750         stlink_core_id(sl);
1751         //----------------------------------------------------------------------
1752
1753         stlink_status(sl);
1754         //stlink_force_debug(sl);
1755         stlink_reset(sl);
1756         stlink_status(sl);
1757 #if 0
1758         // core system control block
1759         stlink_read_mem32(sl, 0xe000ed00, 4);
1760         DD(sl, "cpu id base register: SCB_CPUID = got 0x%08x expect 0x411fc231", read_uint32(sl->q_buf, 0));
1761         // no MPU
1762         stlink_read_mem32(sl, 0xe000ed90, 4);
1763         DD(sl, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0));
1764
1765         stlink_read_mem32(sl, 0xe000edf0, 4);
1766         DD(sl, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0));
1767
1768         stlink_read_mem32(sl, 0x4001100c, 4);
1769         DD(sl, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0));
1770 #endif
1771 #if 0
1772         // happy new year 2011: let blink all the leds
1773         // see "RM0041 Reference manual - STM32F100xx advanced ARM-based 32-bit MCUs"
1774
1775 #define GPIOC           0x40011000 // port C
1776 #define GPIOC_CRH       (GPIOC + 0x04) // port configuration register high
1777 #define GPIOC_ODR       (GPIOC + 0x0c) // port output data register
1778 #define LED_BLUE        (1<<8) // pin 8
1779 #define LED_GREEN       (1<<9) // pin 9
1780         stlink_read_mem32(sl, GPIOC_CRH, 4);
1781         uint32_t io_conf = read_uint32(sl->q_buf, 0);
1782         DD(sl, "GPIOC_CRH = 0x%08x", io_conf);
1783
1784         // set: general purpose output push-pull, output mode, max speed 10 MHz.
1785         write_uint32(sl->q_buf, 0x44444411);
1786         stlink_write_mem32(sl, GPIOC_CRH, 4);
1787
1788         clear_buf(sl);
1789         for (int i = 0; i < 100; i++) {
1790                 write_uint32(sl->q_buf, LED_BLUE | LED_GREEN);
1791                 stlink_write_mem32(sl, GPIOC_ODR, 4);
1792                 /* stlink_read_mem32(sl, 0x4001100c, 4); */
1793                 /* DD(sl, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0)); */
1794                 delay(100);
1795
1796                 clear_buf(sl);
1797                 stlink_write_mem32(sl, GPIOC_ODR, 4); // PC lo
1798                 delay(100);
1799         }
1800         write_uint32(sl->q_buf, io_conf); // set old state
1801
1802 #endif
1803 #if 0
1804         // TODO rtfm: stlink doesn't have flash write routines
1805         // writing to the flash area confuses the fw for the next read access
1806
1807         //stlink_read_mem32(sl, 0, 1024*6);
1808         // flash 0x08000000 128kB
1809         fputs("++++++++++ read a flash at 0x0800 0000\n", stderr);
1810         stlink_read_mem32(sl, 0x08000000, 1024 * 6); //max 6kB
1811         clear_buf(sl);
1812         stlink_read_mem32(sl, 0x08000c00, 5);
1813         stlink_read_mem32(sl, 0x08000c00, 4);
1814         mark_buf(sl);
1815         stlink_write_mem32(sl, 0x08000c00, 4);
1816         stlink_read_mem32(sl, 0x08000c00, 256);
1817         stlink_read_mem32(sl, 0x08000c00, 256);
1818 #endif
1819 #if 0
1820         // sram 0x20000000 8kB
1821         fputs("\n++++++++++ read/write 8bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
1822         clear_buf(sl);
1823         stlink_write_mem8(sl, 0x20000000, 16);
1824
1825         mark_buf(sl);
1826         stlink_write_mem8(sl, 0x20000000, 1);
1827         stlink_write_mem8(sl, 0x20000001, 1);
1828         stlink_write_mem8(sl, 0x2000000b, 3);
1829         stlink_read_mem32(sl, 0x20000000, 16);
1830 #endif
1831 #if 0
1832         // a not aligned mem32 access doesn't work indeed
1833         fputs("\n++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
1834         clear_buf(sl);
1835         stlink_write_mem8(sl, 0x20000000, 32);
1836
1837         mark_buf(sl);
1838         stlink_write_mem32(sl, 0x20000000, 1);
1839         stlink_read_mem32(sl, 0x20000000, 16);
1840         mark_buf(sl);
1841         stlink_write_mem32(sl, 0x20000001, 1);
1842         stlink_read_mem32(sl, 0x20000000, 16);
1843         mark_buf(sl);
1844         stlink_write_mem32(sl, 0x2000000b, 3);
1845         stlink_read_mem32(sl, 0x20000000, 16);
1846
1847         mark_buf(sl);
1848         stlink_write_mem32(sl, 0x20000000, 17);
1849         stlink_read_mem32(sl, 0x20000000, 32);
1850 #endif
1851 #if 0
1852         // sram 0x20000000 8kB
1853         fputs("++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++\n", stderr);
1854         mark_buf(sl);
1855         stlink_write_mem8(sl, 0x20000000, 64);
1856         stlink_read_mem32(sl, 0x20000000, 64);
1857
1858         mark_buf(sl);
1859         stlink_write_mem32(sl, 0x20000000, 1024 * 8); //8kB
1860         stlink_read_mem32(sl, 0x20000000, 1024 * 6);
1861         stlink_read_mem32(sl, 0x20000000 + 1024 * 6, 1024 * 2);
1862 #endif
1863 #if 0
1864         stlink_read_all_regs(sl);
1865         stlink_step(sl);
1866         fputs("++++++++++ write r0 = 0x12345678\n", stderr);
1867         stlink_write_reg(sl, 0x12345678, 0);
1868         stlink_read_reg(sl, 0);
1869         stlink_read_all_regs(sl);
1870 #endif
1871 #if 0
1872         stlink_run(sl);
1873         stlink_status(sl);
1874
1875         stlink_force_debug(sl);
1876         stlink_status(sl);
1877 #endif
1878 #if 1 /* read the system bootloader */
1879         fputs("\n++++++++++ reading bootloader ++++++++++++++++\n\n", stderr);
1880         stlink_fread(sl, "/tmp/barfoo", sl->sys_base, sl->sys_size);
1881 #endif
1882 #if 0 /* read the flash memory */
1883         fputs("\n+++++++ read flash memory\n\n", stderr);
1884         /* mark_buf(sl); */
1885         stlink_read_mem32(sl, 0x08000000, 4);
1886 #endif
1887 #if 0 /* flash programming */
1888         fputs("\n+++++++ program flash memory\n\n", stderr);
1889         stlink_fwrite_flash(sl, "/tmp/foobar", 0x08000000);
1890 #endif
1891 #if 0 /* check file contents */
1892         fputs("\n+++++++ check flash memory\n\n", stderr);
1893         {
1894           const int res = stlink_fcheck_flash(sl, "/tmp/foobar", 0x08000000);
1895           printf("_____ stlink_fcheck_flash() == %d\n", res);
1896         }
1897 #endif
1898 #if 0
1899         fputs("\n+++++++ sram write and execute\n\n", stderr);
1900         stlink_fwrite_sram(sl, "/tmp/foobar", sl->sram_base);
1901         stlink_run_at(sl, sl->sram_base);
1902 #endif
1903
1904         stlink_run(sl);
1905         stlink_status(sl);
1906         //----------------------------------------------------------------------
1907         // back to mass mode, just in case ...
1908         stlink_exit_debug_mode(sl);
1909         stlink_current_mode(sl);
1910         stlink_close(sl);
1911
1912         //fflush(stderr); fflush(stdout);
1913         return EXIT_SUCCESS;
1914 }
1915 #endif