git.gag.com Git - fw/stlink/blob - src/stlink-sg.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
  69 #define __USE_GNU
  70 #include <stdio.h>
  71 #include <string.h>
  72 #include <stdarg.h>
  73 #include <stdlib.h>
  74 #include <unistd.h>
  75 #include <fcntl.h>
  76 #include <sys/types.h>
  77 #include <sys/stat.h>
  78 #include <sys/mman.h>
  79
  80 // sgutils2 (apt-get install libsgutils2-dev)
  81 #include <scsi/sg_lib.h>
  82 #include <scsi/sg_pt.h>
  83
  84 #include "stlink-common.h"
  85 #include "stlink-sg.h"
  86
  87
  88 // Suspends execution of the calling process for
  89 // (at least) ms milliseconds.
  90
  91 static void delay(int ms) {
  92     //fprintf(stderr, "*** wait %d ms\n", ms);
  93     usleep(1000 * ms);
  94 }
  95
  96 static void write_uint32(unsigned char* buf, uint32_t ui) {
  97     if (!is_bigendian()) { // le -> le (don't swap)
  98         buf[0] = ((unsigned char*) &ui)[0];
  99         buf[1] = ((unsigned char*) &ui)[1];
 100         buf[2] = ((unsigned char*) &ui)[2];
 101         buf[3] = ((unsigned char*) &ui)[3];
 102     } else {
 103         buf[0] = ((unsigned char*) &ui)[3];
 104         buf[1] = ((unsigned char*) &ui)[2];
 105         buf[2] = ((unsigned char*) &ui)[1];
 106         buf[3] = ((unsigned char*) &ui)[0];
 107     }
 108 }
 109
 110 static void write_uint16(unsigned char* buf, uint16_t ui) {
 111     if (!is_bigendian()) { // le -> le (don't swap)
 112         buf[0] = ((unsigned char*) &ui)[0];
 113         buf[1] = ((unsigned char*) &ui)[1];
 114     } else {
 115         buf[0] = ((unsigned char*) &ui)[1];
 116         buf[1] = ((unsigned char*) &ui)[0];
 117     }
 118 }
 119
 120 static uint32_t read_uint32(const unsigned char *c, const int pt) {
 121     uint32_t ui;
 122     char *p = (char *) &ui;
 123
 124     if (!is_bigendian()) { // le -> le (don't swap)
 125         p[0] = c[pt];
 126         p[1] = c[pt + 1];
 127         p[2] = c[pt + 2];
 128         p[3] = c[pt + 3];
 129     } else {
 130         p[0] = c[pt + 3];
 131         p[1] = c[pt + 2];
 132         p[2] = c[pt + 1];
 133         p[3] = c[pt];
 134     }
 135     return ui;
 136 }
 137
 138 static void clear_cdb(struct stlink_libsg *sl) {
 139     for (int i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 140         sl->cdb_cmd_blk[i] = 0;
 141     // set default
 142     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 143     sl->q_data_dir = Q_DATA_IN;
 144 }
 145
 146 // E.g. make the valgrind happy.
 147
 148 static void clear_buf(stlink_t *sl) {
 149     DD(sl, "*** clear_buf ***\n");
 150     for (int i = 0; i < sizeof (sl->q_buf); i++)
 151         sl->q_buf[i] = 0;
 152
 153 }
 154
 155 // close the device, free the allocated memory
 156
 157 void _stlink_sg_close(stlink_t *sl) {
 158     if (sl) {
 159         struct stlink_libsg *slsg = sl->backend_data;
 160         scsi_pt_close_device(slsg->sg_fd);
 161         // CAUTION!? s this right?
 162         free(slsg);
 163         free(sl);
 164     }
 165 }
 166
 167 // Exit the jtag or swd mode and enter the mass mode.
 168
 169 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 170
 171     if (stl) {
 172         struct stlink_libsg* sl = stl->backend_data;
 173         clear_cdb(sl);
 174         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 175         stl->q_len = 0; // >0 -> aboard
 176         stlink_q(stl);
 177     }
 178 }
 179
 180
 181
 182 //TODO rewrite/cleanup, save the error in sl
 183
 184 static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
 185     struct stlink_libsg *sl = stl->backend_data;
 186     const int e = sl->do_scsi_pt_err;
 187     if (e < 0) {
 188         fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 189                 safe_strerror(-e));
 190         return;
 191     } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 192         fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 193         return;
 194     } else if (e == SCSI_PT_DO_TIMEOUT) {
 195         fprintf(stderr, "  pass through timeout\n");
 196         return;
 197     }
 198     const int duration = get_scsi_pt_duration_ms(ptvp);
 199     if ((stl->verbose > 1) && (duration >= 0))
 200         DD(stl, "      duration=%d ms\n", duration);
 201
 202     // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 203     // "usb-storage quirks=483:3744:r"
 204     // forces residue to be ignored and calculated, but this causes aboard if
 205     // data_len = 0 and by some other data_len values.
 206
 207     const int resid = get_scsi_pt_resid(ptvp);
 208     const int dsize = stl->q_len - resid;
 209
 210     const int cat = get_scsi_pt_result_category(ptvp);
 211     char buf[512];
 212     unsigned int slen;
 213
 214     switch (cat) {
 215         case SCSI_PT_RESULT_GOOD:
 216             if (stl->verbose && (resid > 0))
 217                 DD(stl, "      notice: requested %d bytes but "
 218                     "got %d bytes, ignore [broken] residue = %d\n",
 219                     stl->q_len, dsize, resid);
 220             break;
 221         case SCSI_PT_RESULT_STATUS:
 222             if (stl->verbose) {
 223                 sg_get_scsi_status_str(
 224                         get_scsi_pt_status_response(ptvp), sizeof (buf),
 225                         buf);
 226                 DD(stl, "  scsi status: %s\n", buf);
 227             }
 228             return;
 229         case SCSI_PT_RESULT_SENSE:
 230             slen = get_scsi_pt_sense_len(ptvp);
 231             if (stl->verbose) {
 232                 sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
 233                         > 1), sizeof (buf), buf);
 234                 DD(stl, "%s", buf);
 235             }
 236             if (stl->verbose && (resid > 0)) {
 237                 if ((stl->verbose) || (stl->q_len > 0))
 238                     DD(stl, "    requested %d bytes but "
 239                         "got %d bytes\n", stl->q_len, dsize);
 240             }
 241             return;
 242         case SCSI_PT_RESULT_TRANSPORT_ERR:
 243             if (stl->verbose) {
 244                 get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
 245                 // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 246                 // These codes potentially come from the firmware on a host adapter
 247                 // or from one of several hosts that an adapter driver controls.
 248                 // The 'host_status' field has the following values:
 249                 //      [0x07] Internal error detected in the host adapter.
 250                 // This may not be fatal (and the command may have succeeded).
 251                 DD(stl, "  transport: %s", buf);
 252             }
 253             return;
 254         case SCSI_PT_RESULT_OS_ERR:
 255             if (stl->verbose) {
 256                 get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
 257                 DD(stl, "  os: %s", buf);
 258             }
 259             return;
 260         default:
 261             fprintf(stderr, "  unknown pass through result "
 262                     "category (%d)\n", cat);
 263     }
 264 }
 265
 266 void stlink_q(stlink_t *stl) {
 267     struct stlink_libsg* sl = stl->backend_data;
 268     DD(stl, "CDB[");
 269     for (int i = 0; i < CDB_SL; i++)
 270         DD(stl, " 0x%02x", (unsigned int) sl->cdb_cmd_blk[i]);
 271     DD(stl, "]\n");
 272
 273     // Get control command descriptor of scsi structure,
 274     // (one object per command!!)
 275     struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 276     if (NULL == ptvp) {
 277         fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 278         return;
 279     }
 280
 281     set_scsi_pt_cdb(ptvp, sl->cdb_cmd_blk, sizeof (sl->cdb_cmd_blk));
 282
 283     // set buffer for sense (error information) data
 284     set_scsi_pt_sense(ptvp, sl->sense_buf, sizeof (sl->sense_buf));
 285
 286     // Set a buffer to be used for data transferred from device
 287     if (sl->q_data_dir == Q_DATA_IN) {
 288         //clear_buf(sl);
 289         set_scsi_pt_data_in(ptvp, stl->q_buf, stl->q_len);
 290     } else {
 291         set_scsi_pt_data_out(ptvp, stl->q_buf, stl->q_len);
 292     }
 293     // Executes SCSI command (or at least forwards it to lower layers).
 294     sl->do_scsi_pt_err = do_scsi_pt(ptvp, sl->sg_fd, SG_TIMEOUT_SEC,
 295             stl->verbose);
 296
 297     // check for scsi errors
 298     stlink_confirm_inq(stl, ptvp);
 299     // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 300     destruct_scsi_pt_obj(ptvp);
 301 }
 302
 303 // TODO thinking, cleanup
 304
 305 void stlink_parse_version(stlink_t *stl) {
 306     struct stlink_libsg *sl = stl->backend_data;
 307     sl->st_vid = 0;
 308     sl->stlink_pid = 0;
 309     if (stl->q_len <= 0) {
 310         fprintf(stderr, "Error: could not parse the stlink version");
 311         return;
 312     }
 313     stlink_print_data(stl);
 314     uint32_t b0 = stl->q_buf[0]; //lsb
 315     uint32_t b1 = stl->q_buf[1];
 316     uint32_t b2 = stl->q_buf[2];
 317     uint32_t b3 = stl->q_buf[3];
 318     uint32_t b4 = stl->q_buf[4];
 319     uint32_t b5 = stl->q_buf[5]; //msb
 320
 321     // b0 b1                       || b2 b3  | b4 b5
 322     // 4b        | 6b     | 6b     || 2B     | 2B
 323     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 324
 325     sl->stlink_v = (b0 & 0xf0) >> 4;
 326     sl->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 327     sl->swim_v = b1 & 0x3f;
 328     sl->st_vid = (b3 << 8) | b2;
 329     sl->stlink_pid = (b5 << 8) | b4;
 330
 331     if (stl->verbose < 2)
 332         return;
 333
 334     DD(stl, "st vid         = 0x%04x (expect 0x%04x)\n",
 335             sl->st_vid, USB_ST_VID);
 336     DD(stl, "stlink pid     = 0x%04x (expect 0x%04x)\n",
 337             sl->stlink_pid, USB_STLINK_PID);
 338     DD(stl, "stlink version = 0x%x\n", sl->stlink_v);
 339     DD(stl, "jtag version   = 0x%x\n", sl->jtag_v);
 340     DD(stl, "swim version   = 0x%x\n", sl->swim_v);
 341     if (sl->jtag_v == 0)
 342         DD(stl,
 343             "    notice: the firmware doesn't support a jtag/swd interface\n");
 344     if (sl->swim_v == 0)
 345         DD(stl,
 346             "    notice: the firmware doesn't support a swim interface\n");
 347
 348 }
 349
 350 int stlink_mode(stlink_t *stl) {
 351     if (stl->q_len <= 0)
 352         return STLINK_DEV_UNKNOWN_MODE;
 353
 354     stlink_print_data(stl);
 355
 356     switch (stl->q_buf[0]) {
 357         case STLINK_DEV_DFU_MODE:
 358             DD(stl, "stlink mode: dfu\n");
 359             return STLINK_DEV_DFU_MODE;
 360         case STLINK_DEV_DEBUG_MODE:
 361             DD(stl, "stlink mode: debug (jtag or swd)\n");
 362             return STLINK_DEV_DEBUG_MODE;
 363         case STLINK_DEV_MASS_MODE:
 364             DD(stl, "stlink mode: mass\n");
 365             return STLINK_DEV_MASS_MODE;
 366     }
 367     return STLINK_DEV_UNKNOWN_MODE;
 368 }
 369
 370 void stlink_stat(stlink_t *stl, char *txt) {
 371     if (stl->q_len <= 0)
 372         return;
 373
 374     stlink_print_data(stl);
 375
 376     switch (stl->q_buf[0]) {
 377         case STLINK_OK:
 378             DD(stl, "  %s: ok\n", txt);
 379             return;
 380         case STLINK_FALSE:
 381             DD(stl, "  %s: false\n", txt);
 382             return;
 383         default:
 384             DD(stl, "  %s: unknown\n", txt);
 385     }
 386 }
 387
 388 void _stlink_sg_version(stlink_t *stl) {
 389     struct stlink_libsg *sl = stl->backend_data;
 390     D(stl, "\n*** stlink_version ***\n");
 391     clear_cdb(sl);
 392     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 393     stl->q_len = 6;
 394     sl->q_addr = 0;
 395     stlink_q(stl);
 396     stlink_parse_version(stl);
 397 }
 398
 399 // Get stlink mode:
 400 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 401 // usb dfu             || usb mass             || jtag or swd
 402
 403 int stlink_current_mode(stlink_t *stl) {
 404     struct stlink_libsg *sl = stl->backend_data;
 405     D(stl, "\n*** stlink_current_mode ***\n");
 406     clear_cdb(sl);
 407     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 408     stl->q_len = 2;
 409     sl->q_addr = 0;
 410     stlink_q(stl);
 411     return stlink_mode(stl);
 412 }
 413
 414 // Exit the mass mode and enter the swd debug mode.
 415
 416 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 417     struct stlink_libsg *sg = sl->backend_data;
 418     clear_cdb(sg);
 419     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 420     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 421     sl->q_len = 0; // >0 -> aboard
 422     stlink_q(sl);
 423 }
 424
 425 // Exit the mass mode and enter the jtag debug mode.
 426 // (jtag is disabled in the discovery's stlink firmware)
 427
 428 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 429     struct stlink_libsg *sg = sl->backend_data;
 430     D(sl, "\n*** stlink_enter_jtag_mode ***\n");
 431     clear_cdb(sg);
 432     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 433     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 434     sl->q_len = 0;
 435     stlink_q(sl);
 436 }
 437
 438 // XXX kernel driver performs reset, the device temporally disappears
 439
 440 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 441     struct stlink_libsg *sg = sl->backend_data;
 442     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 443     clear_cdb(sg);
 444     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 445     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 446     sl->q_len = 0; // ??
 447     stlink_q(sl);
 448     /*
 449      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 450      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 451      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 452      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 453      [135121.844590] Buffer I/O error on device sdb, logical block 512
 454      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 455      [135130.274551] usb 6-1: device firmware changed
 456      [135130.274618] usb 6-1: USB disconnect, address 7
 457      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 458      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 459      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 460      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 461      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 462      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 463      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 464      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 465      [135130.629492] scsi20 : usb-storage 6-1:1.0
 466      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 467      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 468      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 469      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 470      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 471      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 472      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 473      [135131.640609]  sdb:
 474      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 475      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 476      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 477      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 478      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 479      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 480      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 481      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 482      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 483      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 484      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 485      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 486      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 487      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 488      ...
 489      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 490      */
 491 }
 492
 493 void _stlink_sg_core_id(stlink_t *sl) {
 494     struct stlink_libsg *sg = sl->backend_data;
 495     clear_cdb(sg);
 496     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 497     sl->q_len = 4;
 498     sg->q_addr = 0;
 499     stlink_q(sl);
 500     sl->core_id = read_uint32(sl->q_buf, 0);
 501     if (sl->verbose < 2)
 502         return;
 503     stlink_print_data(sl);
 504 }
 505
 506 // Arm-core reset -> halted state.
 507
 508 void _stlink_sg_reset(stlink_t *sl) {
 509     struct stlink_libsg *sg = sl->backend_data;
 510     D(sl, "\n*** stlink_reset ***\n");
 511     clear_cdb(sg);
 512     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 513     sl->q_len = 2;
 514     sg->q_addr = 0;
 515     stlink_q(sl);
 516     stlink_stat(sl, "core reset");
 517 }
 518
 519 // Arm-core status: halted or running.
 520
 521 void _stlink_sg_status(struct stlink_libsg *sl) {
 522     D(sl, "\n*** stlink_status ***\n");
 523     clear_cdb(sl);
 524     sl->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 525     sl->q_len = 2;
 526     sl->q_addr = 0;
 527     stlink_q(sl);
 528 }
 529
 530 // Force the core into the debug mode -> halted state.
 531
 532 void stlink_force_debug(struct stlink_libsg *sl) {
 533     D(sl, "\n*** stlink_force_debug ***\n");
 534     clear_cdb(sl);
 535     sl->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 536     sl->q_len = 2;
 537     sl->q_addr = 0;
 538     stlink_q(sl);
 539     stlink_stat(sl, "force debug");
 540 }
 541
 542 // Read all arm-core registers.
 543
 544 void stlink_read_all_regs(struct stlink_libsg *sl) {
 545     D(sl, "\n*** stlink_read_all_regs ***\n");
 546     clear_cdb(sl);
 547     sl->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 548     sl->q_len = 84;
 549     sl->q_addr = 0;
 550     stlink_q(sl);
 551     stlink_print_data(sl);
 552
 553     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 554     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 555     for (int i = 0; i < 16; i++) {
 556         sl->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 557         if (sl->verbose > 1)
 558             DD(sl, "r%2d = 0x%08x\n", i, sl->reg.r[i]);
 559     }
 560     sl->reg.xpsr = read_uint32(sl->q_buf, 64);
 561     sl->reg.main_sp = read_uint32(sl->q_buf, 68);
 562     sl->reg.process_sp = read_uint32(sl->q_buf, 72);
 563     sl->reg.rw = read_uint32(sl->q_buf, 76);
 564     sl->reg.rw2 = read_uint32(sl->q_buf, 80);
 565     if (sl->verbose < 2)
 566         return;
 567
 568     DD(sl, "xpsr       = 0x%08x\n", sl->reg.xpsr);
 569     DD(sl, "main_sp    = 0x%08x\n", sl->reg.main_sp);
 570     DD(sl, "process_sp = 0x%08x\n", sl->reg.process_sp);
 571     DD(sl, "rw         = 0x%08x\n", sl->reg.rw);
 572     DD(sl, "rw2        = 0x%08x\n", sl->reg.rw2);
 573 }
 574
 575 // Read an arm-core register, the index must be in the range 0..20.
 576 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 577 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 578
 579 void stlink_read_reg(struct stlink_libsg *sl, int r_idx) {
 580     D(sl, "\n*** stlink_read_reg");
 581     DD(sl, " (%d) ***\n", r_idx);
 582
 583     if (r_idx > 20 || r_idx < 0) {
 584         fprintf(stderr, "Error: register index must be in [0..20]\n");
 585         return;
 586     }
 587     clear_cdb(sl);
 588     sl->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 589     sl->cdb_cmd_blk[2] = r_idx;
 590     sl->q_len = 4;
 591     sl->q_addr = 0;
 592     stlink_q(sl);
 593     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 594     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 595     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 596     stlink_print_data(sl);
 597
 598     uint32_t r = read_uint32(sl->q_buf, 0);
 599     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 600
 601     switch (r_idx) {
 602         case 16:
 603             sl->reg.xpsr = r;
 604             break;
 605         case 17:
 606             sl->reg.main_sp = r;
 607             break;
 608         case 18:
 609             sl->reg.process_sp = r;
 610             break;
 611         case 19:
 612             sl->reg.rw = r; //XXX ?(primask, basemask etc.)
 613             break;
 614         case 20:
 615             sl->reg.rw2 = r; //XXX ?(primask, basemask etc.)
 616             break;
 617         default:
 618             sl->reg.r[r_idx] = r;
 619     }
 620 }
 621
 622 // Write an arm-core register. Index:
 623 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 624 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 625
 626 void stlink_write_reg(struct stlink_libsg *sl, uint32_t reg, int idx) {
 627     D(sl, "\n*** stlink_write_reg ***\n");
 628     clear_cdb(sl);
 629     sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 630     //   2: reg index
 631     // 3-6: reg content
 632     sl->cdb_cmd_blk[2] = idx;
 633     write_uint32(sl->cdb_cmd_blk + 3, reg);
 634     sl->q_len = 2;
 635     sl->q_addr = 0;
 636     stlink_q(sl);
 637     stlink_stat(sl, "write reg");
 638 }
 639
 640 // Write a register of the debug module of the core.
 641 // XXX ?(atomic writes)
 642 // TODO test
 643
 644 void stlink_write_dreg(struct stlink_libsg *sl, uint32_t reg, uint32_t addr) {
 645     D(sl, "\n*** stlink_write_dreg ***\n");
 646     clear_cdb(sl);
 647     sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 648     // 2-5: address of reg of the debug module
 649     // 6-9: reg content
 650     write_uint32(sl->cdb_cmd_blk + 2, addr);
 651     write_uint32(sl->cdb_cmd_blk + 6, reg);
 652     sl->q_len = 2;
 653     sl->q_addr = addr;
 654     stlink_q(sl);
 655     stlink_stat(sl, "write debug reg");
 656 }
 657
 658 // Force the core exit the debug mode.
 659
 660 void _stlink_sg_run(stlink_t *stl) {
 661     struct stlink_libsg sl = stl->backend_data;
 662     D(stl, "\n*** stlink_run ***\n");
 663     clear_cdb(sl);
 664     sl->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 665     sl->q_len = 2;
 666     sl->q_addr = 0;
 667     stlink_q(sl);
 668     stlink_stat(sl, "run core");
 669 }
 670
 671 // same as above with entrypoint.
 672 static unsigned int is_core_halted(struct stlink_libsg*);
 673
 674 void stlink_run_at(struct stlink *sl_libsg, stm32_addr_t addr) {
 675     stlink_write_reg(sl, addr, 15); /* pc register */
 676
 677     stlink_run(sl);
 678
 679     while (is_core_halted(sl) == 0)
 680         usleep(3000000);
 681 }
 682
 683 // Step the arm-core.
 684
 685 void stlink_step(struct stlink_libsg *sl) {
 686     D(sl, "\n*** stlink_step ***\n");
 687     clear_cdb(sl);
 688     sl->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 689     sl->q_len = 2;
 690     sl->q_addr = 0;
 691     stlink_q(sl);
 692     stlink_stat(sl, "step core");
 693 }
 694
 695 // TODO test
 696 // see Cortex-M3 Technical Reference Manual
 697
 698 void stlink_set_hw_bp(struct stlink_libsg *sl, int fp_nr, uint32_t addr, int fp) {
 699     D(sl, "\n*** stlink_set_hw_bp ***\n");
 700     clear_cdb(sl);
 701     sl->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 702     // 2:The number of the flash patch used to set the breakpoint
 703     // 3-6: Address of the breakpoint (LSB)
 704     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 705     sl->q_buf[2] = fp_nr;
 706     write_uint32(sl->q_buf, addr);
 707     sl->q_buf[7] = fp;
 708
 709     sl->q_len = 2;
 710     stlink_q(sl);
 711     stlink_stat(sl, "set flash breakpoint");
 712 }
 713
 714 // TODO test
 715
 716 void stlink_clr_hw_bp(struct stlink_libsg *sl, int fp_nr) {
 717     D(sl, "\n*** stlink_clr_hw_bp ***\n");
 718     clear_cdb(sl);
 719     sl->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 720     sl->cdb_cmd_blk[2] = fp_nr;
 721
 722     sl->q_len = 2;
 723     stlink_q(sl);
 724     stlink_stat(sl, "clear flash breakpoint");
 725 }
 726
 727 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 728
 729 void stlink_read_mem32(struct stlink_libsg *sl, uint32_t addr, uint16_t len) {
 730     D(sl, "\n*** stlink_read_mem32 ***\n");
 731     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 732         fprintf(
 733                 stderr,
 734                 "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 735                 len % 4);
 736         return;
 737     }
 738     clear_cdb(sl);
 739     sl->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 740     // 2-5: addr
 741     // 6-7: len
 742     write_uint32(sl->cdb_cmd_blk + 2, addr);
 743     write_uint16(sl->cdb_cmd_blk + 6, len);
 744
 745     // data_in 0-0x40-len
 746     // !!! len _and_ q_len must be max 6k,
 747     //     i.e. >1024 * 6 = 6144 -> aboard)
 748     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 749     //     (broken residue issue)
 750     sl->q_len = len;
 751     sl->q_addr = addr;
 752     stlink_q(sl);
 753     stlink_print_data(sl);
 754 }
 755
 756 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 757
 758 void _stlink_sg_write_mem8(struct stlink_libsg *sl, uint32_t addr, uint16_t len) {
 759     D(sl, "\n*** stlink_write_mem8 ***\n");
 760     clear_cdb(sl);
 761     sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 762     // 2-5: addr
 763     // 6-7: len (>0x40 (64) -> aboard)
 764     write_uint32(sl->cdb_cmd_blk + 2, addr);
 765     write_uint16(sl->cdb_cmd_blk + 6, len);
 766
 767     // data_out 0-len
 768     sl->q_len = len;
 769     sl->q_addr = addr;
 770     sl->q_data_dir = Q_DATA_OUT;
 771     stlink_q(sl);
 772     stlink_print_data(sl);
 773 }
 774
 775 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 776
 777 void _stlink_sg_write_mem32(struct stlink_libsg *sl, uint32_t addr, uint16_t len) {
 778     clear_cdb(sl);
 779     sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 780     // 2-5: addr
 781     // 6-7: len "unlimited"
 782     write_uint32(sl->cdb_cmd_blk + 2, addr);
 783     write_uint16(sl->cdb_cmd_blk + 6, len);
 784
 785     // data_out 0-0x40-...-len
 786     sl->q_len = len;
 787     sl->q_addr = addr;
 788     sl->q_data_dir = Q_DATA_OUT;
 789     stlink_q(sl);
 790     stlink_print_data(sl);
 791 }
 792
 793 /* FPEC flash controller interface, pm0063 manual
 794  */
 795
 796 #define FLASH_REGS_ADDR 0x40022000
 797 #define FLASH_REGS_SIZE 0x28
 798
 799 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
 800 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
 801 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
 802 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
 803 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
 804 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
 805 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
 806
 807 #define FLASH_RDPTR_KEY 0x00a5
 808 #define FLASH_KEY1 0x45670123
 809 #define FLASH_KEY2 0xcdef89ab
 810
 811 #define FLASH_SR_BSY 0
 812 #define FLASH_SR_EOP 5
 813
 814 #define FLASH_CR_PG 0
 815 #define FLASH_CR_PER 1
 816 #define FLASH_CR_MER 2
 817 #define FLASH_CR_STRT 6
 818 #define FLASH_CR_LOCK 7
 819
 820 static uint32_t __attribute__((unused)) read_flash_rdp(struct stlink_libsg* sl) {
 821     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 822     return (*(uint32_t*) sl->q_buf) & 0xff;
 823 }
 824
 825 static inline uint32_t read_flash_wrpr(struct stlink_libsg* sl) {
 826     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 827     return *(uint32_t*) sl->q_buf;
 828 }
 829
 830 static inline uint32_t read_flash_obr(struct stlink_libsg* sl) {
 831     stlink_read_mem32(sl, FLASH_OBR, sizeof (uint32_t));
 832     return *(uint32_t*) sl->q_buf;
 833 }
 834
 835 static inline uint32_t read_flash_cr(struct stlink_libsg* sl) {
 836     stlink_read_mem32(sl, FLASH_CR, sizeof (uint32_t));
 837     return *(uint32_t*) sl->q_buf;
 838 }
 839
 840 static inline unsigned int is_flash_locked(struct stlink_libsg* sl) {
 841     /* return non zero for true */
 842     return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 843 }
 844
 845 static void unlock_flash(struct stlink_libsg* sl) {
 846     /* the unlock sequence consists of 2 write cycles where
 847        2 key values are written to the FLASH_KEYR register.
 848        an invalid sequence results in a definitive lock of
 849        the FPEC block until next reset.
 850      */
 851
 852     write_uint32(sl->q_buf, FLASH_KEY1);
 853     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 854
 855     write_uint32(sl->q_buf, FLASH_KEY2);
 856     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 857 }
 858
 859 static int unlock_flash_if(struct stlink_libsg* sl) {
 860     /* unlock flash if already locked */
 861
 862     if (is_flash_locked(sl)) {
 863         unlock_flash(sl);
 864         if (is_flash_locked(sl))
 865             return -1;
 866     }
 867
 868     return 0;
 869 }
 870
 871 static void lock_flash(struct stlink_libsg* sl) {
 872     /* write to 1 only. reset by hw at unlock sequence */
 873
 874     const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 875
 876     write_uint32(sl->q_buf, n);
 877     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 878 }
 879
 880 static void set_flash_cr_pg(struct stlink_libsg* sl) {
 881     const uint32_t n = 1 << FLASH_CR_PG;
 882     write_uint32(sl->q_buf, n);
 883     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 884 }
 885
 886 static void __attribute__((unused)) clear_flash_cr_pg(struct stlink_libsg* sl) {
 887     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 888     write_uint32(sl->q_buf, n);
 889     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 890 }
 891
 892 static void set_flash_cr_per(struct stlink_libsg* sl) {
 893     const uint32_t n = 1 << FLASH_CR_PER;
 894     write_uint32(sl->q_buf, n);
 895     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 896 }
 897
 898 static void __attribute__((unused)) clear_flash_cr_per(struct stlink_libsg* sl) {
 899     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 900     write_uint32(sl->q_buf, n);
 901     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 902 }
 903
 904 static void set_flash_cr_mer(struct stlink_libsg* sl) {
 905     const uint32_t n = 1 << FLASH_CR_MER;
 906     write_uint32(sl->q_buf, n);
 907     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 908 }
 909
 910 static void __attribute__((unused)) clear_flash_cr_mer(struct stlink_libsg* sl) {
 911     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER);
 912     write_uint32(sl->q_buf, n);
 913     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 914 }
 915
 916 static void set_flash_cr_strt(struct stlink_libsg* sl) {
 917     /* assume come on the flash_cr_per path */
 918     const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT);
 919     write_uint32(sl->q_buf, n);
 920     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 921 }
 922
 923 static inline uint32_t read_flash_acr(struct stlink_libsg* sl) {
 924     stlink_read_mem32(sl, FLASH_ACR, sizeof (uint32_t));
 925     return *(uint32_t*) sl->q_buf;
 926 }
 927
 928 static inline uint32_t read_flash_sr(struct stlink_libsg* sl) {
 929     stlink_read_mem32(sl, FLASH_SR, sizeof (uint32_t));
 930     return *(uint32_t*) sl->q_buf;
 931 }
 932
 933 static inline unsigned int is_flash_busy(struct stlink_libsg* sl) {
 934     return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 935 }
 936
 937 static void wait_flash_busy(struct stlink_libsg* sl) {
 938     /* todo: add some delays here */
 939     while (is_flash_busy(sl))
 940         ;
 941 }
 942
 943 static inline unsigned int is_flash_eop(struct stlink_libsg* sl) {
 944     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 945 }
 946
 947 static void __attribute__((unused)) clear_flash_sr_eop(struct stlink_libsg* sl) {
 948     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 949     write_uint32(sl->q_buf, n);
 950     stlink_write_mem32(sl, FLASH_SR, sizeof (uint32_t));
 951 }
 952
 953 static void __attribute__((unused)) wait_flash_eop(struct stlink_libsg* sl) {
 954     /* todo: add some delays here */
 955     while (is_flash_eop(sl) == 0)
 956         ;
 957 }
 958
 959 static inline void write_flash_ar(struct stlink_libsg* sl, uint32_t n) {
 960     write_uint32(sl->q_buf, n);
 961     stlink_write_mem32(sl, FLASH_AR, sizeof (uint32_t));
 962 }
 963
 964 #if 0 /* todo */
 965
 966 static void disable_flash_read_protection(struct stlink* sl) {
 967     /* erase the option byte area */
 968     /* rdp = 0x00a5; */
 969     /* reset */
 970 }
 971 #endif /* todo */
 972
 973 #if 0 /* not working */
 974
 975 static int write_flash_mem16
 976 (struct stlink* sl, uint32_t addr, uint16_t val) {
 977     /* half word writes */
 978     if (addr % 2) return -1;
 979
 980     /* unlock if locked */
 981     unlock_flash_if(sl);
 982
 983     /* set flash programming chosen bit */
 984     set_flash_cr_pg(sl);
 985
 986     write_uint16(sl->q_buf, val);
 987     stlink_write_mem16(sl, addr, 2);
 988
 989     /* wait for non business */
 990     wait_flash_busy(sl);
 991
 992     lock_flash(sl);
 993
 994     /* check the programmed value back */
 995     stlink_read_mem16(sl, addr, 2);
 996     if (*(const uint16_t*) sl->q_buf != val) {
 997         /* values differ at i * sizeof(uint16_t) */
 998         return -1;
 999     }
1000
1001     /* success */
1002     return 0;
1003 }
1004 #endif /* not working */
1005
1006 int stlink_erase_flash_page(struct stlink_libsg* sl, stm32_addr_t page) {
1007     /* page an addr in the page to erase */
1008
1009     /* wait for ongoing op to finish */
1010     wait_flash_busy(sl);
1011
1012     /* unlock if locked */
1013     unlock_flash_if(sl);
1014
1015     /* set the page erase bit */
1016     set_flash_cr_per(sl);
1017
1018     /* select the page to erase */
1019     write_flash_ar(sl, page);
1020
1021     /* start erase operation, reset by hw with bsy bit */
1022     set_flash_cr_strt(sl);
1023
1024     /* wait for completion */
1025     wait_flash_busy(sl);
1026
1027     /* relock the flash */
1028     lock_flash(sl);
1029
1030     /* todo: verify the erased page */
1031
1032     return 0;
1033 }
1034
1035 int stlink_erase_flash_mass(struct stlink_libsg* sl) {
1036     /* wait for ongoing op to finish */
1037     wait_flash_busy(sl);
1038
1039     /* unlock if locked */
1040     unlock_flash_if(sl);
1041
1042     /* set the mass erase bit */
1043     set_flash_cr_mer(sl);
1044
1045     /* start erase operation, reset by hw with bsy bit */
1046     set_flash_cr_strt(sl);
1047
1048     /* wait for completion */
1049     wait_flash_busy(sl);
1050
1051     /* relock the flash */
1052     lock_flash(sl);
1053
1054     /* todo: verify the erased memory */
1055
1056     return 0;
1057 }
1058
1059 static unsigned int is_core_halted(struct stlink_libsg* sl) {
1060     /* return non zero if core is halted */
1061     stlink_status(sl);
1062     return sl->q_buf[0] == STLINK_CORE_HALTED;
1063 }
1064
1065 static int write_loader_to_sram
1066 (struct stlink* sl, stm32_addr_t* addr, size_t* size) {
1067     /* from openocd, contrib/loaders/flash/stm32.s */
1068     static const uint8_t loader_code[] ={
1069         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1070         0x1c, 0x44, /* add      r4, r3 */
1071         /* write_half_word: */
1072         0x01, 0x23, /* movs     r3, #0x01 */
1073         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1074         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1075         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1076         /* busy: */
1077         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1078         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1079         0xfb, 0xd0, /* beq      busy */
1080         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1081         0x01, 0xd1, /* bne      exit */
1082         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1083         0xf0, 0xd1, /* bne      write_half_word */
1084         /* exit: */
1085         0x00, 0xbe, /* bkpt     #0x00 */
1086         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1087     };
1088
1089     memcpy(sl->q_buf, loader_code, sizeof (loader_code));
1090     stlink_write_mem32(sl, sl->sram_base, sizeof (loader_code));
1091
1092     *addr = sl->sram_base;
1093     *size = sizeof (loader_code);
1094
1095     /* success */
1096     return 0;
1097 }
1098
1099
1100 int init_flash_loader(stlink_t *stl, flash_loader_t* fl) {
1101     struct stlink_libsg* sl = stl->backend_data;
1102     size_t size;
1103
1104     /* allocate the loader in sram */
1105     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1106         fprintf(stderr, "write_loader_to_sram() == -1\n");
1107         return -1;
1108     }
1109
1110     /* allocate a one page buffer in sram right after loader */
1111     fl->buf_addr = fl->loader_addr + size;
1112
1113     return 0;
1114 }
1115
1116 static int run_flash_loader
1117 (stlink_t *stl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1118     struct stlink_libsg* sl = stl->backend_data;
1119     const size_t count = size / sizeof (uint16_t);
1120
1121     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1122         fprintf(stderr, "write_buffer_to_sram() == -1\n");
1123         return -1;
1124     }
1125
1126     /* setup core */
1127     stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1128     stlink_write_reg(sl, target, 1); /* target */
1129     stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1130     stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1131     stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1132
1133     /* unlock and set programming mode */
1134     unlock_flash_if(sl);
1135     set_flash_cr_pg(sl);
1136
1137     /* run loader */
1138     stlink_run(sl);
1139
1140     while (is_core_halted(sl) == 0)
1141         ;
1142
1143     lock_flash(sl);
1144
1145     /* not all bytes have been written */
1146     stlink_read_reg(sl, 2);
1147     if (sl->reg.r[2] != 0) {
1148         fprintf(stderr, "write error, count == %u\n", sl->reg.r[2]);
1149         return -1;
1150     }
1151
1152     return 0;
1153 }
1154
1155 static int stlink_fcheck_flash
1156 (struct stlink_libsg* sl, const char* path, stm32_addr_t addr) {
1157     /* check the contents of path are at addr */
1158
1159     int res;
1160     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1161
1162     if (map_file(&mf, path) == -1)
1163         return -1;
1164
1165     res = check_file(sl, &mf, addr);
1166
1167     unmap_file(&mf);
1168
1169     return res;
1170 }
1171
1172 // The stlink_fwrite_flash should not muck with mmapped files inside itself,
1173 // and should use this function instead. (Hell, what's the reason behind mmap
1174 // there?!) But, as it is not actually used anywhere, nobody cares.
1175
1176 #define WRITE_BLOCK_SIZE 0x40
1177
1178 int stlink_write_flash(struct stlink_libsg* sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1179     int error = -1;
1180     size_t off;
1181     flash_loader_t fl;
1182
1183     /* check addr range is inside the flash */
1184     if (addr < sl->flash_base) {
1185         fprintf(stderr, "addr too low\n");
1186         return -1;
1187     } else if ((addr + len) < addr) {
1188         fprintf(stderr, "addr overruns\n");
1189         return -1;
1190     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1191         fprintf(stderr, "addr too high\n");
1192         return -1;
1193     } else if ((addr & 1) || (len & 1)) {
1194         fprintf(stderr, "unaligned addr or size\n");
1195         return -1;
1196     }
1197
1198     /* flash loader initialization */
1199     if (init_flash_loader(sl, &fl) == -1) {
1200         fprintf(stderr, "init_flash_loader() == -1\n");
1201         return -1;
1202     }
1203
1204     /* write each page. above WRITE_BLOCK_SIZE fails? */
1205     for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
1206         /* adjust last write size */
1207         size_t size = WRITE_BLOCK_SIZE;
1208         if ((off + WRITE_BLOCK_SIZE) > len)
1209             size = len - off;
1210
1211         if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1212             fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1213             return -1;
1214         }
1215     }
1216
1217     for (off = 0; off < len; off += sl->flash_pgsz) {
1218         size_t aligned_size;
1219
1220         /* adjust last page size */
1221         size_t cmp_size = sl->flash_pgsz;
1222         if ((off + sl->flash_pgsz) > len)
1223             cmp_size = len - off;
1224
1225         aligned_size = cmp_size;
1226         if (aligned_size & (4 - 1))
1227             aligned_size = (cmp_size + 4) & ~(4 - 1);
1228
1229         stlink_read_mem32(sl, addr + off, aligned_size);
1230
1231         if (memcmp(sl->q_buf, base + off, cmp_size))
1232             return -1;
1233     }
1234
1235     return 0;
1236 }
1237
1238 int stlink_fwrite_flash
1239 (stlink_t *sl, const char* path, stm32_addr_t addr) {
1240     /* write the file in flash at addr */
1241
1242     int error = -1;
1243     size_t off;
1244     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1245     flash_loader_t fl;
1246
1247     if (map_file(&mf, path) == -1) {
1248         fprintf(stderr, "map_file() == -1\n");
1249         return -1;
1250     }
1251
1252     /* check addr range is inside the flash */
1253     if (addr < sl->flash_base) {
1254         fprintf(stderr, "addr too low\n");
1255         goto on_error;
1256     } else if ((addr + mf.len) < addr) {
1257         fprintf(stderr, "addr overruns\n");
1258         goto on_error;
1259     } else if ((addr + mf.len) > (sl->flash_base + sl->flash_size)) {
1260         fprintf(stderr, "addr too high\n");
1261         goto on_error;
1262     } else if ((addr & 1) || (mf.len & 1)) {
1263         /* todo */
1264         fprintf(stderr, "unaligned addr or size\n");
1265         goto on_error;
1266     }
1267
1268     /* erase each page. todo: mass erase faster? */
1269     for (off = 0; off < mf.len; off += sl->flash_pgsz) {
1270         /* addr must be an addr inside the page */
1271         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1272             fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
1273             goto on_error;
1274         }
1275     }
1276
1277     /* flash loader initialization */
1278     if (init_flash_loader(sl, &fl) == -1) {
1279         fprintf(stderr, "init_flash_loader() == -1\n");
1280         goto on_error;
1281     }
1282
1283     /* write each page. above WRITE_BLOCK_SIZE fails? */
1284 #define WRITE_BLOCK_SIZE 0x40
1285     for (off = 0; off < mf.len; off += WRITE_BLOCK_SIZE) {
1286         /* adjust last write size */
1287         size_t size = WRITE_BLOCK_SIZE;
1288         if ((off + WRITE_BLOCK_SIZE) > mf.len)
1289             size = mf.len - off;
1290
1291         if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1) {
1292             fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1293             goto on_error;
1294         }
1295     }
1296
1297     /* check the file ha been written */
1298     if (check_file(sl, &mf, addr) == -1) {
1299         fprintf(stderr, "check_file() == -1\n");
1300         goto on_error;
1301     }
1302
1303     /* success */
1304     error = 0;
1305
1306 on_error:
1307     unmap_file(&mf);
1308     return error;
1309 }
1310
1311 // 1) open a sg device, switch the stlink from dfu to mass mode
1312 // 2) wait 5s until the kernel driver stops reseting the broken device
1313 // 3) reopen the device
1314 // 4) the device driver is now ready for a switch to jtag/swd mode
1315 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
1316
1317 stlink_backend_t _stlink_sg_backend = {
1318     _stlink_sg_close,
1319     _stlink_sg_exit_debug_mode,
1320     _stlink_sg_enter_swd_mode,
1321     _stlink_sg_enter_jtag_mode,
1322     _stlink_sg_exit_dfu_mode,
1323     _stlink_sg_core_id,
1324     _stlink_sg_reset,
1325     _stlink_sg_run,
1326     _stlink_sg_status,
1327     _stlink_sg_version,
1328     _stlink_sg_write_mem32,
1329     _stlink_sg_write_mem8
1330 };
1331
1332 stlink_t* stlink_open(const char *dev_name, const int verbose) {
1333     fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
1334     int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
1335     if (sg_fd < 0) {
1336         fprintf(stderr, "error opening device: %s: %s\n", dev_name,
1337                 safe_strerror(-sg_fd));
1338         return NULL;
1339     }
1340
1341     stlink_t *sl = malloc(sizeof (stlink_t));
1342     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
1343     if (sl == NULL || slsg == NULL) {
1344         fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
1345         return NULL;
1346     }
1347     sl->verbose = verbose;
1348     sl->backend_data = slsg;
1349     sl->backend = &_stlink_sg_backend;
1350
1351     slsg->sg_fd = sg_fd;
1352     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
1353     slsg->core_id = 0;
1354     slsg->q_addr = 0;
1355     clear_buf(sl);
1356
1357     /* flash memory settings */
1358     sl->flash_base = STM32_FLASH_BASE;
1359     sl->flash_size = STM32_FLASH_SIZE;
1360     sl->flash_pgsz = STM32_FLASH_PGSZ;
1361
1362     /* system memory */
1363     sl->sys_base = STM32_SYSTEM_BASE;
1364     sl->sys_size = STM32_SYSTEM_SIZE;
1365
1366     /* sram memory settings */
1367     sl->sram_base = STM32_SRAM_BASE;
1368     sl->sram_size = STM32_SRAM_SIZE;
1369
1370     return sl;
1371 }
1372
1373
1374
1375 stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
1376
1377     stlink_t *sl = stlink_open(dev_name, verbose);
1378     if (sl == NULL) {
1379         fputs("Error: could not open stlink device\n", stderr);
1380         return NULL;
1381     }
1382
1383     stlink_version(sl);
1384     struct stlink_libsg *sg = sl->backend_data;
1385
1386     if (sg->st_vid != USB_ST_VID || sg->stlink_pid != USB_STLINK_PID) {
1387         fprintf(stderr, "Error: the device %s is not a stlink\n",
1388                 dev_name);
1389         fprintf(stderr, "       VID: got %04x expect %04x \n",
1390                 sg->st_vid, USB_ST_VID);
1391         fprintf(stderr, "       PID: got %04x expect %04x \n",
1392                 sg->stlink_pid, USB_STLINK_PID);
1393         return NULL;
1394     }
1395
1396     D(sl, "\n*** stlink_force_open ***\n");
1397     switch (stlink_current_mode(sl)) {
1398         case STLINK_DEV_MASS_MODE:
1399             return sl;
1400         case STLINK_DEV_DEBUG_MODE:
1401             // TODO go to mass?
1402             return sl;
1403     }
1404     DD(sl, "\n*** switch the stlink to mass mode ***\n");
1405     _stlink_sg_exit_dfu_mode(sl);
1406     // exit the dfu mode -> the device is gone
1407     DD(sl, "\n*** reopen the stlink device ***\n");
1408     delay(1000);
1409     stlink_close(sl);
1410     delay(5000);
1411
1412     sl = stlink_open(dev_name, verbose);
1413     if (sl == NULL) {
1414         fputs("Error: could not open stlink device\n", stderr);
1415         return NULL;
1416     }
1417     // re-query device info
1418     stlink_version(sl);
1419     return sl;
1420 }
1421
1422 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
1423     clear_buf(sl);
1424     sl->q_buf[0] = 0x12;
1425     sl->q_buf[1] = 0x34;
1426     sl->q_buf[2] = 0x56;
1427     sl->q_buf[3] = 0x78;
1428     sl->q_buf[4] = 0x90;
1429     sl->q_buf[15] = 0x42;
1430     sl->q_buf[16] = 0x43;
1431     sl->q_buf[63] = 0x42;
1432     sl->q_buf[64] = 0x43;
1433     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
1434     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
1435 }
1436 #endif