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