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