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