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 #include "stlink-common.h"
  84
  85 #if CONFIG_USE_LIBSG
  86 // sgutils2 (apt-get install libsgutils2-dev)
  87 #include <scsi/sg_lib.h>
  88 #include <scsi/sg_pt.h>
  89 #include "stlink-sg.h"
  90 #endif
  91
  92
  93 // Suspends execution of the calling process for
  94 // (at least) ms milliseconds.
  95
  96 static void delay(int ms) {
  97     //fprintf(stderr, "*** wait %d ms\n", ms);
  98     usleep(1000 * ms);
  99 }
 100
 101 static void clear_cdb(struct stlink_libsg *sl) {
 102     for (size_t i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 103         sl->cdb_cmd_blk[i] = 0;
 104     // set default
 105     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 106     sl->q_data_dir = Q_DATA_IN;
 107 }
 108
 109 // E.g. make the valgrind happy.
 110
 111 static void clear_buf(stlink_t *sl) {
 112     DD(sl, "*** clear_buf ***\n");
 113     for (size_t i = 0; i < sizeof (sl->q_buf); i++)
 114         sl->q_buf[i] = 0;
 115
 116 }
 117
 118 // close the device, free the allocated memory
 119
 120 void _stlink_sg_close(stlink_t *sl) {
 121     if (sl) {
 122         struct stlink_libsg *slsg = sl->backend_data;
 123         scsi_pt_close_device(slsg->sg_fd);
 124         free(slsg);
 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 void _stlink_sg_version(stlink_t *stl) {
 272     struct stlink_libsg *sl = stl->backend_data;
 273     D(stl, "\n*** stlink_version ***\n");
 274     clear_cdb(sl);
 275     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 276     stl->q_len = 6;
 277     sl->q_addr = 0;
 278     stlink_q(stl);
 279 }
 280
 281 // Get stlink mode:
 282 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 283 // usb dfu             || usb mass             || jtag or swd
 284
 285 int _stlink_sg_current_mode(stlink_t *stl) {
 286     struct stlink_libsg *sl = stl->backend_data;
 287     clear_cdb(sl);
 288     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 289     stl->q_len = 2;
 290     sl->q_addr = 0;
 291     stlink_q(stl);
 292     return stl->q_buf[0];
 293 }
 294
 295 // Exit the mass mode and enter the swd debug mode.
 296
 297 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 298     struct stlink_libsg *sg = sl->backend_data;
 299     clear_cdb(sg);
 300     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 301     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 302     sl->q_len = 0; // >0 -> aboard
 303     stlink_q(sl);
 304 }
 305
 306 // Exit the mass mode and enter the jtag debug mode.
 307 // (jtag is disabled in the discovery's stlink firmware)
 308
 309 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 310     struct stlink_libsg *sg = sl->backend_data;
 311     D(sl, "\n*** stlink_enter_jtag_mode ***\n");
 312     clear_cdb(sg);
 313     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 314     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 315     sl->q_len = 0;
 316     stlink_q(sl);
 317 }
 318
 319 // XXX kernel driver performs reset, the device temporally disappears
 320
 321 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 322     struct stlink_libsg *sg = sl->backend_data;
 323     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 324     clear_cdb(sg);
 325     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 326     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 327     sl->q_len = 0; // ??
 328     stlink_q(sl);
 329     /*
 330      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 331      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 332      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 333      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 334      [135121.844590] Buffer I/O error on device sdb, logical block 512
 335      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 336      [135130.274551] usb 6-1: device firmware changed
 337      [135130.274618] usb 6-1: USB disconnect, address 7
 338      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 339      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 340      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 341      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 342      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 343      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 344      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 345      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 346      [135130.629492] scsi20 : usb-storage 6-1:1.0
 347      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 348      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 349      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 350      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 351      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 352      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 353      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 354      [135131.640609]  sdb:
 355      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 356      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 357      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 358      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 359      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 360      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 361      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 362      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 363      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 364      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 365      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 366      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 367      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 368      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 369      ...
 370      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 371      */
 372 }
 373
 374 void _stlink_sg_core_id(stlink_t *sl) {
 375     struct stlink_libsg *sg = sl->backend_data;
 376     clear_cdb(sg);
 377     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 378     sl->q_len = 4;
 379     sg->q_addr = 0;
 380     stlink_q(sl);
 381     sl->core_id = read_uint32(sl->q_buf, 0);
 382 }
 383
 384 // Arm-core reset -> halted state.
 385
 386 void _stlink_sg_reset(stlink_t *sl) {
 387     struct stlink_libsg *sg = sl->backend_data;
 388     clear_cdb(sg);
 389     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 390     sl->q_len = 2;
 391     sg->q_addr = 0;
 392     stlink_q(sl);
 393     stlink_stat(sl, "core reset");
 394 }
 395
 396 // Arm-core status: halted or running.
 397
 398 void _stlink_sg_status(stlink_t *sl) {
 399     struct stlink_libsg *sg = sl->backend_data;
 400     D(sl, "\n*** stlink_status ***\n");
 401     clear_cdb(sg);
 402     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 403     sl->q_len = 2;
 404     sg->q_addr = 0;
 405     stlink_q(sl);
 406 }
 407
 408 // Force the core into the debug mode -> halted state.
 409
 410 void _stlink_sg_force_debug(stlink_t *sl) {
 411     struct stlink_libsg *sg = sl->backend_data;
 412     D(sl, "\n*** stlink_force_debug ***\n");
 413     clear_cdb(sg);
 414     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 415     sl->q_len = 2;
 416     sg->q_addr = 0;
 417     stlink_q(sl);
 418     stlink_stat(sl, "force debug");
 419 }
 420
 421 // Read all arm-core registers.
 422
 423 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 424     struct stlink_libsg *sg = sl->backend_data;
 425
 426     /* unused */
 427     regp = regp;
 428
 429     clear_cdb(sg);
 430     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 431     sl->q_len = 84;
 432     sg->q_addr = 0;
 433     stlink_q(sl);
 434     stlink_print_data(sl);
 435
 436     // TODO - most of this should be re-extracted up....
 437
 438     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 439     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 440     for (int i = 0; i < 16; i++) {
 441         sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 442         if (sl->verbose > 1)
 443             DD(sl, "r%2d = 0x%08x\n", i, sg->reg.r[i]);
 444     }
 445     sg->reg.xpsr = read_uint32(sl->q_buf, 64);
 446     sg->reg.main_sp = read_uint32(sl->q_buf, 68);
 447     sg->reg.process_sp = read_uint32(sl->q_buf, 72);
 448     sg->reg.rw = read_uint32(sl->q_buf, 76);
 449     sg->reg.rw2 = read_uint32(sl->q_buf, 80);
 450     if (sl->verbose < 2)
 451         return;
 452
 453     DD(sl, "xpsr       = 0x%08x\n", sg->reg.xpsr);
 454     DD(sl, "main_sp    = 0x%08x\n", sg->reg.main_sp);
 455     DD(sl, "process_sp = 0x%08x\n", sg->reg.process_sp);
 456     DD(sl, "rw         = 0x%08x\n", sg->reg.rw);
 457     DD(sl, "rw2        = 0x%08x\n", sg->reg.rw2);
 458 }
 459
 460 // Read an arm-core register, the index must be in the range 0..20.
 461 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 462 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 463
 464 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 465     struct stlink_libsg *sg = sl->backend_data;
 466     clear_cdb(sg);
 467     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 468     sg->cdb_cmd_blk[2] = r_idx;
 469     sl->q_len = 4;
 470     sg->q_addr = 0;
 471     stlink_q(sl);
 472     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 473     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 474     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 475     stlink_print_data(sl);
 476
 477     uint32_t r = read_uint32(sl->q_buf, 0);
 478     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 479
 480     switch (r_idx) {
 481         case 16:
 482             regp->xpsr = r;
 483             break;
 484         case 17:
 485             regp->main_sp = r;
 486             break;
 487         case 18:
 488             regp->process_sp = r;
 489             break;
 490         case 19:
 491             regp->rw = r; //XXX ?(primask, basemask etc.)
 492             break;
 493         case 20:
 494             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 495             break;
 496         default:
 497             regp->r[r_idx] = r;
 498     }
 499 }
 500
 501 // Write an arm-core register. Index:
 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_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 506     struct stlink_libsg *sg = sl->backend_data;
 507     clear_cdb(sg);
 508     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 509     //   2: reg index
 510     // 3-6: reg content
 511     sg->cdb_cmd_blk[2] = idx;
 512     write_uint32(sg->cdb_cmd_blk + 3, reg);
 513     sl->q_len = 2;
 514     sg->q_addr = 0;
 515     stlink_q(sl);
 516     stlink_stat(sl, "write reg");
 517 }
 518
 519 // Write a register of the debug module of the core.
 520 // XXX ?(atomic writes)
 521 // TODO test
 522
 523 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 524     struct stlink_libsg *sg = sl->backend_data;
 525     D(sl, "\n*** stlink_write_dreg ***\n");
 526     clear_cdb(sg);
 527     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 528     // 2-5: address of reg of the debug module
 529     // 6-9: reg content
 530     write_uint32(sg->cdb_cmd_blk + 2, addr);
 531     write_uint32(sg->cdb_cmd_blk + 6, reg);
 532     sl->q_len = 2;
 533     sg->q_addr = addr;
 534     stlink_q(sl);
 535     stlink_stat(sl, "write debug reg");
 536 }
 537
 538 // Force the core exit the debug mode.
 539
 540 void _stlink_sg_run(stlink_t *sl) {
 541     struct stlink_libsg *sg = sl->backend_data;
 542     D(sl, "\n*** stlink_run ***\n");
 543     clear_cdb(sg);
 544     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 545     sl->q_len = 2;
 546     sg->q_addr = 0;
 547     stlink_q(sl);
 548     stlink_stat(sl, "run core");
 549 }
 550
 551 // Step the arm-core.
 552
 553 void _stlink_sg_step(stlink_t *sl) {
 554     struct stlink_libsg *sg = sl->backend_data;
 555     clear_cdb(sg);
 556     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 557     sl->q_len = 2;
 558     sg->q_addr = 0;
 559     stlink_q(sl);
 560     stlink_stat(sl, "step core");
 561 }
 562
 563 // TODO test
 564 // see Cortex-M3 Technical Reference Manual
 565 // TODO make delegate!
 566 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 567     D(sl, "\n*** stlink_set_hw_bp ***\n");
 568     struct stlink_libsg *sg = sl->backend_data;
 569     clear_cdb(sg);
 570     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 571     // 2:The number of the flash patch used to set the breakpoint
 572     // 3-6: Address of the breakpoint (LSB)
 573     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 574     sl->q_buf[2] = fp_nr;
 575     write_uint32(sl->q_buf, addr);
 576     sl->q_buf[7] = fp;
 577
 578     sl->q_len = 2;
 579     stlink_q(sl);
 580     stlink_stat(sl, "set flash breakpoint");
 581 }
 582
 583 // TODO test
 584
 585 // TODO make delegate!
 586 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 587     struct stlink_libsg *sg = sl->backend_data;
 588     D(sl, "\n*** stlink_clr_hw_bp ***\n");
 589     clear_cdb(sg);
 590     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 591     sg->cdb_cmd_blk[2] = fp_nr;
 592
 593     sl->q_len = 2;
 594     stlink_q(sl);
 595     stlink_stat(sl, "clear flash breakpoint");
 596 }
 597
 598 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 599
 600 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 601     struct stlink_libsg *sg = sl->backend_data;
 602     clear_cdb(sg);
 603     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 604     // 2-5: addr
 605     // 6-7: len
 606     write_uint32(sg->cdb_cmd_blk + 2, addr);
 607     write_uint16(sg->cdb_cmd_blk + 6, len);
 608
 609     // data_in 0-0x40-len
 610     // !!! len _and_ q_len must be max 6k,
 611     //     i.e. >1024 * 6 = 6144 -> aboard)
 612     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 613     //     (broken residue issue)
 614     sl->q_len = len;
 615     sg->q_addr = addr;
 616     stlink_q(sl);
 617     stlink_print_data(sl);
 618 }
 619
 620 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 621
 622 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 623     struct stlink_libsg *sg = sl->backend_data;
 624     clear_cdb(sg);
 625     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 626     // 2-5: addr
 627     // 6-7: len (>0x40 (64) -> aboard)
 628     write_uint32(sg->cdb_cmd_blk + 2, addr);
 629     write_uint16(sg->cdb_cmd_blk + 6, len);
 630
 631     // data_out 0-len
 632     sl->q_len = len;
 633     sg->q_addr = addr;
 634     sg->q_data_dir = Q_DATA_OUT;
 635     stlink_q(sl);
 636     stlink_print_data(sl);
 637 }
 638
 639 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 640
 641 void _stlink_sg_write_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_WRITEMEM_32BIT;
 645     // 2-5: addr
 646     // 6-7: len "unlimited"
 647     write_uint32(sg->cdb_cmd_blk + 2, addr);
 648     write_uint16(sg->cdb_cmd_blk + 6, len);
 649
 650     // data_out 0-0x40-...-len
 651     sl->q_len = len;
 652     sg->q_addr = addr;
 653     sg->q_data_dir = Q_DATA_OUT;
 654     stlink_q(sl);
 655     stlink_print_data(sl);
 656 }
 657
 658 #if 0 /* not working */
 659
 660 static int write_flash_mem16
 661 (struct stlink* sl, uint32_t addr, uint16_t val) {
 662     /* half word writes */
 663     if (addr % 2) return -1;
 664
 665     /* unlock if locked */
 666     unlock_flash_if(sl);
 667
 668     /* set flash programming chosen bit */
 669     set_flash_cr_pg(sl);
 670
 671     write_uint16(sl->q_buf, val);
 672     stlink_write_mem16(sl, addr, 2);
 673
 674     /* wait for non business */
 675     wait_flash_busy(sl);
 676
 677     lock_flash(sl);
 678
 679     /* check the programmed value back */
 680     stlink_read_mem16(sl, addr, 2);
 681     if (*(const uint16_t*) sl->q_buf != val) {
 682         /* values differ at i * sizeof(uint16_t) */
 683         return -1;
 684     }
 685
 686     /* success */
 687     return 0;
 688 }
 689 #endif /* not working */
 690
 691 // Exit the jtag or swd mode and enter the mass mode.
 692
 693 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 694
 695     if (stl) {
 696         struct stlink_libsg* sl = stl->backend_data;
 697         clear_cdb(sl);
 698         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 699         stl->q_len = 0; // >0 -> aboard
 700         stlink_q(stl);
 701     }
 702 }
 703
 704
 705 // 1) open a sg device, switch the stlink from dfu to mass mode
 706 // 2) wait 5s until the kernel driver stops reseting the broken device
 707 // 3) reopen the device
 708 // 4) the device driver is now ready for a switch to jtag/swd mode
 709 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 710
 711 stlink_backend_t _stlink_sg_backend = {
 712     _stlink_sg_close,
 713     _stlink_sg_exit_debug_mode,
 714     _stlink_sg_enter_swd_mode,
 715     _stlink_sg_enter_jtag_mode,
 716     _stlink_sg_exit_dfu_mode,
 717     _stlink_sg_core_id,
 718     _stlink_sg_reset,
 719     _stlink_sg_run,
 720     _stlink_sg_status,
 721     _stlink_sg_version,
 722     _stlink_sg_read_mem32,
 723     _stlink_sg_write_mem32,
 724     _stlink_sg_write_mem8,
 725     _stlink_sg_read_all_regs,
 726     _stlink_sg_read_reg,
 727     _stlink_sg_write_reg,
 728     _stlink_sg_step,
 729     _stlink_sg_current_mode,
 730     _stlink_sg_force_debug
 731 };
 732
 733 stlink_t* stlink_open(const char *dev_name, const int verbose) {
 734     fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
 735     int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
 736     if (sg_fd < 0) {
 737         fprintf(stderr, "error opening device: %s: %s\n", dev_name,
 738                 safe_strerror(-sg_fd));
 739         return NULL;
 740     }
 741
 742     stlink_t *sl = malloc(sizeof (stlink_t));
 743     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 744     if (sl == NULL || slsg == NULL) {
 745         fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 746         return NULL;
 747     }
 748     sl->verbose = verbose;
 749     sl->backend_data = slsg;
 750     sl->backend = &_stlink_sg_backend;
 751
 752     slsg->sg_fd = sg_fd;
 753     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 754     slsg->q_addr = 0;
 755     clear_buf(sl);
 756
 757     /* flash memory settings */
 758     sl->flash_base = STM32_FLASH_BASE;
 759     sl->flash_size = STM32_FLASH_SIZE;
 760     sl->flash_pgsz = STM32_FLASH_PGSZ;
 761
 762     /* system memory */
 763     sl->sys_base = STM32_SYSTEM_BASE;
 764     sl->sys_size = STM32_SYSTEM_SIZE;
 765
 766     /* sram memory settings */
 767     sl->sram_base = STM32_SRAM_BASE;
 768     sl->sram_size = STM32_SRAM_SIZE;
 769
 770     return sl;
 771 }
 772
 773
 774
 775 stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
 776
 777     stlink_t *sl = stlink_open(dev_name, verbose);
 778     if (sl == NULL) {
 779         fputs("Error: could not open stlink device\n", stderr);
 780         return NULL;
 781     }
 782
 783     stlink_version(sl);
 784     struct stlink_libsg *sg = sl->backend_data;
 785
 786     if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
 787         fprintf(stderr, "Error: the device %s is not a stlink\n",
 788                 dev_name);
 789         fprintf(stderr, "       VID: got %04x expect %04x \n",
 790                 sl->version.st_vid, USB_ST_VID);
 791         fprintf(stderr, "       PID: got %04x expect %04x \n",
 792                 sl->version.stlink_pid, USB_STLINK_PID);
 793         return NULL;
 794     }
 795
 796     D(sl, "\n*** stlink_force_open ***\n");
 797     switch (stlink_current_mode(sl)) {
 798         case STLINK_DEV_MASS_MODE:
 799             return sl;
 800         case STLINK_DEV_DEBUG_MODE:
 801             // TODO go to mass?
 802             return sl;
 803     }
 804     DD(sl, "\n*** switch the stlink to mass mode ***\n");
 805     _stlink_sg_exit_dfu_mode(sl);
 806     // exit the dfu mode -> the device is gone
 807     DD(sl, "\n*** reopen the stlink device ***\n");
 808     delay(1000);
 809     stlink_close(sl);
 810     delay(5000);
 811
 812     sl = stlink_open(dev_name, verbose);
 813     if (sl == NULL) {
 814         fputs("Error: could not open stlink device\n", stderr);
 815         return NULL;
 816     }
 817     // re-query device info
 818     stlink_version(sl);
 819     return sl;
 820 }
 821
 822 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
 823     clear_buf(sl);
 824     sl->q_buf[0] = 0x12;
 825     sl->q_buf[1] = 0x34;
 826     sl->q_buf[2] = 0x56;
 827     sl->q_buf[3] = 0x78;
 828     sl->q_buf[4] = 0x90;
 829     sl->q_buf[15] = 0x42;
 830     sl->q_buf[16] = 0x43;
 831     sl->q_buf[63] = 0x42;
 832     sl->q_buf[64] = 0x43;
 833     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
 834     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
 835 }