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