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  https://wiki.kubuntu.org/Kernel/Debugging/USB explains the protocoll and
  68  would allow to replace the sg access to pure libusb access
  69  */
  70
  71
  72 #define __USE_GNU
  73 #include <stdio.h>
  74 #include <string.h>
  75 #include <stdarg.h>
  76 #include <stdlib.h>
  77 #include <unistd.h>
  78 #include <fcntl.h>
  79 #include <sys/types.h>
  80 #include <sys/stat.h>
  81 #include <sys/mman.h>
  82
  83 // sgutils2 (apt-get install libsgutils2-dev)
  84 #include <scsi/sg_lib.h>
  85 #include <scsi/sg_pt.h>
  86
  87 #include "stlink-common.h"
  88 #include "stlink-sg.h"
  89
  90
  91 // Suspends execution of the calling process for
  92 // (at least) ms milliseconds.
  93
  94 static void delay(int ms) {
  95     //fprintf(stderr, "*** wait %d ms\n", ms);
  96     usleep(1000 * ms);
  97 }
  98
  99 static void clear_cdb(struct stlink_libsg *sl) {
 100     for (size_t i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 101         sl->cdb_cmd_blk[i] = 0;
 102     // set default
 103     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 104     sl->q_data_dir = Q_DATA_IN;
 105 }
 106
 107 // E.g. make the valgrind happy.
 108
 109 static void clear_buf(stlink_t *sl) {
 110     DD(sl, "*** clear_buf ***\n");
 111     for (size_t i = 0; i < sizeof (sl->q_buf); i++)
 112         sl->q_buf[i] = 0;
 113
 114 }
 115
 116 // close the device, free the allocated memory
 117
 118 void _stlink_sg_close(stlink_t *sl) {
 119     if (sl) {
 120         struct stlink_libsg *slsg = sl->backend_data;
 121         scsi_pt_close_device(slsg->sg_fd);
 122         // CAUTION!? s this right?
 123         free(slsg);
 124         free(sl);
 125     }
 126 }
 127
 128
 129 //TODO rewrite/cleanup, save the error in sl
 130
 131 static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
 132     struct stlink_libsg *sl = stl->backend_data;
 133     const int e = sl->do_scsi_pt_err;
 134     if (e < 0) {
 135         fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 136                 safe_strerror(-e));
 137         return;
 138     } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 139         fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 140         return;
 141     } else if (e == SCSI_PT_DO_TIMEOUT) {
 142         fprintf(stderr, "  pass through timeout\n");
 143         return;
 144     }
 145     const int duration = get_scsi_pt_duration_ms(ptvp);
 146     if ((stl->verbose > 1) && (duration >= 0))
 147         DD(stl, "      duration=%d ms\n", duration);
 148
 149     // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 150     // "usb-storage quirks=483:3744:r"
 151     // forces residue to be ignored and calculated, but this causes aboard if
 152     // data_len = 0 and by some other data_len values.
 153
 154     const int resid = get_scsi_pt_resid(ptvp);
 155     const int dsize = stl->q_len - resid;
 156
 157     const int cat = get_scsi_pt_result_category(ptvp);
 158     char buf[512];
 159     unsigned int slen;
 160
 161     switch (cat) {
 162         case SCSI_PT_RESULT_GOOD:
 163             if (stl->verbose && (resid > 0))
 164                 DD(stl, "      notice: requested %d bytes but "
 165                     "got %d bytes, ignore [broken] residue = %d\n",
 166                     stl->q_len, dsize, resid);
 167             break;
 168         case SCSI_PT_RESULT_STATUS:
 169             if (stl->verbose) {
 170                 sg_get_scsi_status_str(
 171                         get_scsi_pt_status_response(ptvp), sizeof (buf),
 172                         buf);
 173                 DD(stl, "  scsi status: %s\n", buf);
 174             }
 175             return;
 176         case SCSI_PT_RESULT_SENSE:
 177             slen = get_scsi_pt_sense_len(ptvp);
 178             if (stl->verbose) {
 179                 sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
 180                         > 1), sizeof (buf), buf);
 181                 DD(stl, "%s", buf);
 182             }
 183             if (stl->verbose && (resid > 0)) {
 184                 if ((stl->verbose) || (stl->q_len > 0))
 185                     DD(stl, "    requested %d bytes but "
 186                         "got %d bytes\n", stl->q_len, dsize);
 187             }
 188             return;
 189         case SCSI_PT_RESULT_TRANSPORT_ERR:
 190             if (stl->verbose) {
 191                 get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
 192                 // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 193                 // These codes potentially come from the firmware on a host adapter
 194                 // or from one of several hosts that an adapter driver controls.
 195                 // The 'host_status' field has the following values:
 196                 //      [0x07] Internal error detected in the host adapter.
 197                 // This may not be fatal (and the command may have succeeded).
 198                 DD(stl, "  transport: %s", buf);
 199             }
 200             return;
 201         case SCSI_PT_RESULT_OS_ERR:
 202             if (stl->verbose) {
 203                 get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
 204                 DD(stl, "  os: %s", buf);
 205             }
 206             return;
 207         default:
 208             fprintf(stderr, "  unknown pass through result "
 209                     "category (%d)\n", cat);
 210     }
 211 }
 212
 213 void stlink_q(stlink_t *sl) {
 214     struct stlink_libsg* sg = sl->backend_data;
 215     DD(sl, "CDB[");
 216     for (int i = 0; i < CDB_SL; i++)
 217         DD(sl, " 0x%02x", (unsigned int) sg->cdb_cmd_blk[i]);
 218     DD(sl, "]\n");
 219
 220     // Get control command descriptor of scsi structure,
 221     // (one object per command!!)
 222     struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 223     if (NULL == ptvp) {
 224         fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 225         return;
 226     }
 227
 228     set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk));
 229
 230     // set buffer for sense (error information) data
 231     set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf));
 232
 233     // Set a buffer to be used for data transferred from device
 234     if (sg->q_data_dir == Q_DATA_IN) {
 235         //clear_buf(sl);
 236         set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
 237     } else {
 238         set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
 239     }
 240     // Executes SCSI command (or at least forwards it to lower layers).
 241     sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC,
 242             sl->verbose);
 243
 244     // check for scsi errors
 245     stlink_confirm_inq(sl, ptvp);
 246     // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 247     destruct_scsi_pt_obj(ptvp);
 248 }
 249
 250 // TODO thinking, cleanup
 251
 252 void stlink_stat(stlink_t *stl, char *txt) {
 253     if (stl->q_len <= 0)
 254         return;
 255
 256     stlink_print_data(stl);
 257
 258     switch (stl->q_buf[0]) {
 259         case STLINK_OK:
 260             DD(stl, "  %s: ok\n", txt);
 261             return;
 262         case STLINK_FALSE:
 263             DD(stl, "  %s: false\n", txt);
 264             return;
 265         default:
 266             DD(stl, "  %s: unknown\n", txt);
 267     }
 268 }
 269
 270
 271 static void parse_version(stlink_t *stl) {
 272   struct stlink_libsg *sl = stl->backend_data;
 273
 274   sl->st_vid = 0;
 275   sl->stlink_pid = 0;
 276
 277   if (stl->q_len <= 0) {
 278     fprintf(stderr, "Error: could not parse the stlink version");
 279     return;
 280   }
 281
 282   uint32_t b0 = stl->q_buf[0]; //lsb
 283   uint32_t b1 = stl->q_buf[1];
 284   uint32_t b2 = stl->q_buf[2];
 285   uint32_t b3 = stl->q_buf[3];
 286   uint32_t b4 = stl->q_buf[4];
 287   uint32_t b5 = stl->q_buf[5]; //msb
 288
 289   // b0 b1 || b2 b3 | b4 b5
 290   // 4b | 6b | 6b || 2B | 2B
 291   // stlink_v | jtag_v | swim_v || st_vid | stlink_pid
 292
 293   sl->stlink_v = (b0 & 0xf0) >> 4;
 294   sl->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 295   sl->swim_v = b1 & 0x3f;
 296   sl->st_vid = (b3 << 8) | b2;
 297   sl->stlink_pid = (b5 << 8) | b4;
 298 }
 299
 300 void _stlink_sg_version(stlink_t *stl) {
 301     struct stlink_libsg *sl = stl->backend_data;
 302     D(stl, "\n*** stlink_version ***\n");
 303     clear_cdb(sl);
 304     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 305     stl->q_len = 6;
 306     sl->q_addr = 0;
 307     stlink_q(stl);
 308     parse_version(stl);
 309 }
 310
 311 // Get stlink mode:
 312 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 313 // usb dfu             || usb mass             || jtag or swd
 314
 315 int _stlink_sg_current_mode(stlink_t *stl) {
 316     struct stlink_libsg *sl = stl->backend_data;
 317     clear_cdb(sl);
 318     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 319     stl->q_len = 2;
 320     sl->q_addr = 0;
 321     stlink_q(stl);
 322     return stl->q_buf[0];
 323 }
 324
 325 // Exit the mass mode and enter the swd debug mode.
 326
 327 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 328     struct stlink_libsg *sg = sl->backend_data;
 329     clear_cdb(sg);
 330     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 331     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 332     sl->q_len = 0; // >0 -> aboard
 333     stlink_q(sl);
 334 }
 335
 336 // Exit the mass mode and enter the jtag debug mode.
 337 // (jtag is disabled in the discovery's stlink firmware)
 338
 339 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 340     struct stlink_libsg *sg = sl->backend_data;
 341     D(sl, "\n*** stlink_enter_jtag_mode ***\n");
 342     clear_cdb(sg);
 343     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 344     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 345     sl->q_len = 0;
 346     stlink_q(sl);
 347 }
 348
 349 // XXX kernel driver performs reset, the device temporally disappears
 350
 351 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 352     struct stlink_libsg *sg = sl->backend_data;
 353     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 354     clear_cdb(sg);
 355     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 356     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 357     sl->q_len = 0; // ??
 358     stlink_q(sl);
 359     /*
 360      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 361      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 362      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 363      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 364      [135121.844590] Buffer I/O error on device sdb, logical block 512
 365      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 366      [135130.274551] usb 6-1: device firmware changed
 367      [135130.274618] usb 6-1: USB disconnect, address 7
 368      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 369      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 370      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 371      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 372      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 373      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 374      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 375      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 376      [135130.629492] scsi20 : usb-storage 6-1:1.0
 377      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 378      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 379      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 380      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 381      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 382      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 383      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 384      [135131.640609]  sdb:
 385      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 386      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 387      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 388      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 389      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 390      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 391      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 392      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 393      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 394      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 395      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 396      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 397      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 398      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 399      ...
 400      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 401      */
 402 }
 403
 404 void _stlink_sg_core_id(stlink_t *sl) {
 405     struct stlink_libsg *sg = sl->backend_data;
 406     clear_cdb(sg);
 407     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 408     sl->q_len = 4;
 409     sg->q_addr = 0;
 410     stlink_q(sl);
 411     sl->core_id = read_uint32(sl->q_buf, 0);
 412 }
 413
 414 // Arm-core reset -> halted state.
 415
 416 void _stlink_sg_reset(stlink_t *sl) {
 417     struct stlink_libsg *sg = sl->backend_data;
 418     clear_cdb(sg);
 419     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 420     sl->q_len = 2;
 421     sg->q_addr = 0;
 422     stlink_q(sl);
 423     stlink_stat(sl, "core reset");
 424 }
 425
 426 // Arm-core status: halted or running.
 427
 428 void _stlink_sg_status(stlink_t *sl) {
 429     struct stlink_libsg *sg = sl->backend_data;
 430     D(sl, "\n*** stlink_status ***\n");
 431     clear_cdb(sg);
 432     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 433     sl->q_len = 2;
 434     sg->q_addr = 0;
 435     stlink_q(sl);
 436 }
 437
 438 // Force the core into the debug mode -> halted state.
 439
 440 void _stlink_sg_force_debug(stlink_t *sl) {
 441     struct stlink_libsg *sg = sl->backend_data;
 442     D(sl, "\n*** stlink_force_debug ***\n");
 443     clear_cdb(sg);
 444     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 445     sl->q_len = 2;
 446     sg->q_addr = 0;
 447     stlink_q(sl);
 448     stlink_stat(sl, "force debug");
 449 }
 450
 451 // Read all arm-core registers.
 452
 453 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 454     struct stlink_libsg *sg = sl->backend_data;
 455
 456     /* unused */
 457     regp = regp;
 458
 459     clear_cdb(sg);
 460     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 461     sl->q_len = 84;
 462     sg->q_addr = 0;
 463     stlink_q(sl);
 464     stlink_print_data(sl);
 465
 466     // TODO - most of this should be re-extracted up....
 467
 468     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 469     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 470     for (int i = 0; i < 16; i++) {
 471         sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 472         if (sl->verbose > 1)
 473             DD(sl, "r%2d = 0x%08x\n", i, sg->reg.r[i]);
 474     }
 475     sg->reg.xpsr = read_uint32(sl->q_buf, 64);
 476     sg->reg.main_sp = read_uint32(sl->q_buf, 68);
 477     sg->reg.process_sp = read_uint32(sl->q_buf, 72);
 478     sg->reg.rw = read_uint32(sl->q_buf, 76);
 479     sg->reg.rw2 = read_uint32(sl->q_buf, 80);
 480     if (sl->verbose < 2)
 481         return;
 482
 483     DD(sl, "xpsr       = 0x%08x\n", sg->reg.xpsr);
 484     DD(sl, "main_sp    = 0x%08x\n", sg->reg.main_sp);
 485     DD(sl, "process_sp = 0x%08x\n", sg->reg.process_sp);
 486     DD(sl, "rw         = 0x%08x\n", sg->reg.rw);
 487     DD(sl, "rw2        = 0x%08x\n", sg->reg.rw2);
 488 }
 489
 490 // Read an arm-core register, the index must be in the range 0..20.
 491 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 492 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 493
 494 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 495     struct stlink_libsg *sg = sl->backend_data;
 496     clear_cdb(sg);
 497     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 498     sg->cdb_cmd_blk[2] = r_idx;
 499     sl->q_len = 4;
 500     sg->q_addr = 0;
 501     stlink_q(sl);
 502     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 503     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 504     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 505     stlink_print_data(sl);
 506
 507     uint32_t r = read_uint32(sl->q_buf, 0);
 508     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 509
 510     switch (r_idx) {
 511         case 16:
 512             regp->xpsr = r;
 513             break;
 514         case 17:
 515             regp->main_sp = r;
 516             break;
 517         case 18:
 518             regp->process_sp = r;
 519             break;
 520         case 19:
 521             regp->rw = r; //XXX ?(primask, basemask etc.)
 522             break;
 523         case 20:
 524             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 525             break;
 526         default:
 527             regp->r[r_idx] = r;
 528     }
 529 }
 530
 531 // Write an arm-core register. Index:
 532 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 533 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 534
 535 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 536     struct stlink_libsg *sg = sl->backend_data;
 537     clear_cdb(sg);
 538     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 539     //   2: reg index
 540     // 3-6: reg content
 541     sg->cdb_cmd_blk[2] = idx;
 542     write_uint32(sg->cdb_cmd_blk + 3, reg);
 543     sl->q_len = 2;
 544     sg->q_addr = 0;
 545     stlink_q(sl);
 546     stlink_stat(sl, "write reg");
 547 }
 548
 549 // Write a register of the debug module of the core.
 550 // XXX ?(atomic writes)
 551 // TODO test
 552
 553 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 554     struct stlink_libsg *sg = sl->backend_data;
 555     D(sl, "\n*** stlink_write_dreg ***\n");
 556     clear_cdb(sg);
 557     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 558     // 2-5: address of reg of the debug module
 559     // 6-9: reg content
 560     write_uint32(sg->cdb_cmd_blk + 2, addr);
 561     write_uint32(sg->cdb_cmd_blk + 6, reg);
 562     sl->q_len = 2;
 563     sg->q_addr = addr;
 564     stlink_q(sl);
 565     stlink_stat(sl, "write debug reg");
 566 }
 567
 568 // Force the core exit the debug mode.
 569
 570 void _stlink_sg_run(stlink_t *sl) {
 571     struct stlink_libsg *sg = sl->backend_data;
 572     D(sl, "\n*** stlink_run ***\n");
 573     clear_cdb(sg);
 574     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 575     sl->q_len = 2;
 576     sg->q_addr = 0;
 577     stlink_q(sl);
 578     stlink_stat(sl, "run core");
 579 }
 580
 581 // Step the arm-core.
 582
 583 void _stlink_sg_step(stlink_t *sl) {
 584     struct stlink_libsg *sg = sl->backend_data;
 585     clear_cdb(sg);
 586     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 587     sl->q_len = 2;
 588     sg->q_addr = 0;
 589     stlink_q(sl);
 590     stlink_stat(sl, "step core");
 591 }
 592
 593 // TODO test
 594 // see Cortex-M3 Technical Reference Manual
 595 // TODO make delegate!
 596 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 597     D(sl, "\n*** stlink_set_hw_bp ***\n");
 598     struct stlink_libsg *sg = sl->backend_data;
 599     clear_cdb(sg);
 600     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 601     // 2:The number of the flash patch used to set the breakpoint
 602     // 3-6: Address of the breakpoint (LSB)
 603     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 604     sl->q_buf[2] = fp_nr;
 605     write_uint32(sl->q_buf, addr);
 606     sl->q_buf[7] = fp;
 607
 608     sl->q_len = 2;
 609     stlink_q(sl);
 610     stlink_stat(sl, "set flash breakpoint");
 611 }
 612
 613 // TODO test
 614
 615 // TODO make delegate!
 616 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 617     struct stlink_libsg *sg = sl->backend_data;
 618     D(sl, "\n*** stlink_clr_hw_bp ***\n");
 619     clear_cdb(sg);
 620     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 621     sg->cdb_cmd_blk[2] = fp_nr;
 622
 623     sl->q_len = 2;
 624     stlink_q(sl);
 625     stlink_stat(sl, "clear flash breakpoint");
 626 }
 627
 628 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 629
 630 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 631     struct stlink_libsg *sg = sl->backend_data;
 632     clear_cdb(sg);
 633     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 634     // 2-5: addr
 635     // 6-7: len
 636     write_uint32(sg->cdb_cmd_blk + 2, addr);
 637     write_uint16(sg->cdb_cmd_blk + 6, len);
 638
 639     // data_in 0-0x40-len
 640     // !!! len _and_ q_len must be max 6k,
 641     //     i.e. >1024 * 6 = 6144 -> aboard)
 642     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 643     //     (broken residue issue)
 644     sl->q_len = len;
 645     sg->q_addr = addr;
 646     stlink_q(sl);
 647     stlink_print_data(sl);
 648 }
 649
 650 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 651
 652 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 653     struct stlink_libsg *sg = sl->backend_data;
 654     clear_cdb(sg);
 655     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 656     // 2-5: addr
 657     // 6-7: len (>0x40 (64) -> aboard)
 658     write_uint32(sg->cdb_cmd_blk + 2, addr);
 659     write_uint16(sg->cdb_cmd_blk + 6, len);
 660
 661     // data_out 0-len
 662     sl->q_len = len;
 663     sg->q_addr = addr;
 664     sg->q_data_dir = Q_DATA_OUT;
 665     stlink_q(sl);
 666     stlink_print_data(sl);
 667 }
 668
 669 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 670
 671 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 672     struct stlink_libsg *sg = sl->backend_data;
 673     clear_cdb(sg);
 674     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 675     // 2-5: addr
 676     // 6-7: len "unlimited"
 677     write_uint32(sg->cdb_cmd_blk + 2, addr);
 678     write_uint16(sg->cdb_cmd_blk + 6, len);
 679
 680     // data_out 0-0x40-...-len
 681     sl->q_len = len;
 682     sg->q_addr = addr;
 683     sg->q_data_dir = Q_DATA_OUT;
 684     stlink_q(sl);
 685     stlink_print_data(sl);
 686 }
 687
 688 #if 0 /* not working */
 689
 690 static int write_flash_mem16
 691 (struct stlink* sl, uint32_t addr, uint16_t val) {
 692     /* half word writes */
 693     if (addr % 2) return -1;
 694
 695     /* unlock if locked */
 696     unlock_flash_if(sl);
 697
 698     /* set flash programming chosen bit */
 699     set_flash_cr_pg(sl);
 700
 701     write_uint16(sl->q_buf, val);
 702     stlink_write_mem16(sl, addr, 2);
 703
 704     /* wait for non business */
 705     wait_flash_busy(sl);
 706
 707     lock_flash(sl);
 708
 709     /* check the programmed value back */
 710     stlink_read_mem16(sl, addr, 2);
 711     if (*(const uint16_t*) sl->q_buf != val) {
 712         /* values differ at i * sizeof(uint16_t) */
 713         return -1;
 714     }
 715
 716     /* success */
 717     return 0;
 718 }
 719 #endif /* not working */
 720
 721 // Exit the jtag or swd mode and enter the mass mode.
 722
 723 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 724
 725     if (stl) {
 726         struct stlink_libsg* sl = stl->backend_data;
 727         clear_cdb(sl);
 728         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 729         stl->q_len = 0; // >0 -> aboard
 730         stlink_q(stl);
 731     }
 732 }
 733
 734
 735 // 1) open a sg device, switch the stlink from dfu to mass mode
 736 // 2) wait 5s until the kernel driver stops reseting the broken device
 737 // 3) reopen the device
 738 // 4) the device driver is now ready for a switch to jtag/swd mode
 739 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 740
 741 stlink_backend_t _stlink_sg_backend = {
 742     _stlink_sg_close,
 743     _stlink_sg_exit_debug_mode,
 744     _stlink_sg_enter_swd_mode,
 745     _stlink_sg_enter_jtag_mode,
 746     _stlink_sg_exit_dfu_mode,
 747     _stlink_sg_core_id,
 748     _stlink_sg_reset,
 749     _stlink_sg_run,
 750     _stlink_sg_status,
 751     _stlink_sg_version,
 752     _stlink_sg_read_mem32,
 753     _stlink_sg_write_mem32,
 754     _stlink_sg_write_mem8,
 755     _stlink_sg_read_all_regs,
 756     _stlink_sg_read_reg,
 757     _stlink_sg_write_reg,
 758     _stlink_sg_step,
 759     _stlink_sg_current_mode,
 760     _stlink_sg_force_debug
 761 };
 762
 763 stlink_t* stlink_open(const char *dev_name, const int verbose) {
 764     fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
 765     int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
 766     if (sg_fd < 0) {
 767         fprintf(stderr, "error opening device: %s: %s\n", dev_name,
 768                 safe_strerror(-sg_fd));
 769         return NULL;
 770     }
 771
 772     stlink_t *sl = malloc(sizeof (stlink_t));
 773     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 774     if (sl == NULL || slsg == NULL) {
 775         fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 776         return NULL;
 777     }
 778     sl->verbose = verbose;
 779     sl->backend_data = slsg;
 780     sl->backend = &_stlink_sg_backend;
 781
 782     slsg->sg_fd = sg_fd;
 783     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 784     slsg->core_id = 0;
 785     slsg->q_addr = 0;
 786     clear_buf(sl);
 787
 788     /* flash memory settings */
 789     sl->flash_base = STM32_FLASH_BASE;
 790     sl->flash_size = STM32_FLASH_SIZE;
 791     sl->flash_pgsz = STM32_FLASH_PGSZ;
 792
 793     /* system memory */
 794     sl->sys_base = STM32_SYSTEM_BASE;
 795     sl->sys_size = STM32_SYSTEM_SIZE;
 796
 797     /* sram memory settings */
 798     sl->sram_base = STM32_SRAM_BASE;
 799     sl->sram_size = STM32_SRAM_SIZE;
 800
 801     return sl;
 802 }
 803
 804
 805
 806 stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
 807
 808     stlink_t *sl = stlink_open(dev_name, verbose);
 809     if (sl == NULL) {
 810         fputs("Error: could not open stlink device\n", stderr);
 811         return NULL;
 812     }
 813
 814     stlink_version(sl);
 815     struct stlink_libsg *sg = sl->backend_data;
 816
 817     if ((sg->st_vid != USB_ST_VID) || (sg->stlink_pid != USB_STLINK_PID)) {
 818         fprintf(stderr, "Error: the device %s is not a stlink\n",
 819                 dev_name);
 820         fprintf(stderr, "       VID: got %04x expect %04x \n",
 821                 sg->st_vid, USB_ST_VID);
 822         fprintf(stderr, "       PID: got %04x expect %04x \n",
 823                 sg->stlink_pid, USB_STLINK_PID);
 824         return NULL;
 825     }
 826
 827     D(sl, "\n*** stlink_force_open ***\n");
 828     switch (stlink_current_mode(sl)) {
 829         case STLINK_DEV_MASS_MODE:
 830             return sl;
 831         case STLINK_DEV_DEBUG_MODE:
 832             // TODO go to mass?
 833             return sl;
 834     }
 835     DD(sl, "\n*** switch the stlink to mass mode ***\n");
 836     _stlink_sg_exit_dfu_mode(sl);
 837     // exit the dfu mode -> the device is gone
 838     DD(sl, "\n*** reopen the stlink device ***\n");
 839     delay(1000);
 840     stlink_close(sl);
 841     delay(5000);
 842
 843     sl = stlink_open(dev_name, verbose);
 844     if (sl == NULL) {
 845         fputs("Error: could not open stlink device\n", stderr);
 846         return NULL;
 847     }
 848     // re-query device info
 849     stlink_version(sl);
 850     return sl;
 851 }
 852
 853 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
 854     clear_buf(sl);
 855     sl->q_buf[0] = 0x12;
 856     sl->q_buf[1] = 0x34;
 857     sl->q_buf[2] = 0x56;
 858     sl->q_buf[3] = 0x78;
 859     sl->q_buf[4] = 0x90;
 860     sl->q_buf[15] = 0x42;
 861     sl->q_buf[16] = 0x43;
 862     sl->q_buf[63] = 0x42;
 863     sl->q_buf[64] = 0x43;
 864     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
 865     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
 866 }