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