git.gag.com Git - fw/stlink/blob - src/stlink-sg.c

   1 /*
   2  Copyright (c) 2010 "Capt'ns Missing Link" Authors. All rights reserved.
   3  Use of this source code is governed by a BSD-style
   4  license that can be found in the LICENSE file.
   5
   6  A linux stlink access demo. The purpose of this file is to mitigate the usual
   7  "reinventing the wheel" force by incompatible licenses and give you an idea,
   8  how to access the stlink device. That doesn't mean you should be a free-loader
   9  and not contribute your improvements to this code.
  10
  11  Author: Martin Capitanio <m@capitanio.org>
  12  The stlink related constants kindly provided by Oliver Spencer (OpenOCD)
  13  for use in a GPL compatible license.
  14
  15  Code format ~ TAB = 8, K&R, linux kernel source, golang oriented
  16  Tested compatibility: linux, gcc >= 4.3.3
  17
  18  The communication is based on standard USB mass storage device
  19  BOT (Bulk Only Transfer)
  20  - Endpoint 1: BULK_IN, 64 bytes max
  21  - Endpoint 2: BULK_OUT, 64 bytes max
  22
  23  All CBW transfers are ordered with the LSB (byte 0) first (little endian).
  24  Any command must be answered before sending the next command.
  25  Each USB transfer must complete in less than 1s.
  26
  27  SB Device Class Definition for Mass Storage Devices:
  28  www.usb.org/developers/devclass_docs/usbmassbulk_10.pdf
  29
  30  dt             - Data Transfer (IN/OUT)
  31  CBW            - Command Block Wrapper
  32  CSW            - Command Status Wrapper
  33  RFU            - Reserved for Future Use
  34  scsi_pt        - SCSI pass-through
  35  sg             - SCSI generic
  36
  37  * usb-storage.quirks
  38  http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=blob_plain;f=Documentation/kernel-parameters.txt
  39  Each entry has the form VID:PID:Flags where VID and PID are Vendor and Product
  40  ID values (4-digit hex numbers) and Flags is a set of characters, each corresponding
  41  to a common usb-storage quirk flag as follows:
  42
  43  a = SANE_SENSE (collect more than 18 bytes of sense data);
  44  b = BAD_SENSE (don't collect more than 18 bytes of sense data);
  45  c = FIX_CAPACITY (decrease the reported device capacity by one sector);
  46  h = CAPACITY_HEURISTICS (decrease the reported device capacity by one sector if the number is odd);
  47  i = IGNORE_DEVICE (don't bind to this device);
  48  l = NOT_LOCKABLE (don't try to lock and unlock ejectable media);
  49  m = MAX_SECTORS_64 (don't transfer more than 64 sectors = 32 KB at a time);
  50  o = CAPACITY_OK (accept the capacity reported by the device);
  51  r = IGNORE_RESIDUE (the device reports bogus residue values);
  52  s = SINGLE_LUN (the device has only one Logical Unit);
  53  w = NO_WP_DETECT (don't test whether the medium is write-protected).
  54
  55  Example: quirks=0419:aaf5:rl,0421:0433:rc
  56  http://permalink.gmane.org/gmane.linux.usb.general/35053
  57
  58  modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:l
  59
  60  Equivalently, you can add a line saying
  61
  62  options usb-storage quirks=483:3744:l
  63
  64  to your /etc/modprobe.conf or /etc/modprobe.d/local.conf (or add the "quirks=..."
  65  part to an existing options line for usb-storage).
  66
  67  https://wiki.kubuntu.org/Kernel/Debugging/USB explains the protocoll and
  68  would allow to replace the sg access to pure libusb access
  69  */
  70
  71
  72 #define __USE_GNU
  73 #include <stdio.h>
  74 #include <string.h>
  75 #include <stdarg.h>
  76 #include <stdlib.h>
  77 #include <unistd.h>
  78 #include <fcntl.h>
  79 #include <sys/types.h>
  80 #include <sys/stat.h>
  81 #include <sys/mman.h>
  82
  83 // sgutils2 (apt-get install libsgutils2-dev)
  84 #include <scsi/sg_lib.h>
  85 #include <scsi/sg_pt.h>
  86
  87 #include "stlink-common.h"
  88 #include "stlink-sg.h"
  89
  90
  91 // Suspends execution of the calling process for
  92 // (at least) ms milliseconds.
  93
  94 static void delay(int ms) {
  95     //fprintf(stderr, "*** wait %d ms\n", ms);
  96     usleep(1000 * ms);
  97 }
  98
  99 static void clear_cdb(struct stlink_libsg *sl) {
 100     for (size_t i = 0; i < sizeof (sl->cdb_cmd_blk); i++)
 101         sl->cdb_cmd_blk[i] = 0;
 102     // set default
 103     sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND;
 104     sl->q_data_dir = Q_DATA_IN;
 105 }
 106
 107 // E.g. make the valgrind happy.
 108
 109 static void clear_buf(stlink_t *sl) {
 110     DD(sl, "*** clear_buf ***\n");
 111     for (size_t i = 0; i < sizeof (sl->q_buf); i++)
 112         sl->q_buf[i] = 0;
 113
 114 }
 115
 116 // close the device, free the allocated memory
 117
 118 void _stlink_sg_close(stlink_t *sl) {
 119     if (sl) {
 120         struct stlink_libsg *slsg = sl->backend_data;
 121         scsi_pt_close_device(slsg->sg_fd);
 122         // CAUTION!? s this right?
 123         free(slsg);
 124         free(sl);
 125     }
 126 }
 127
 128
 129 //TODO rewrite/cleanup, save the error in sl
 130
 131 static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
 132     struct stlink_libsg *sl = stl->backend_data;
 133     const int e = sl->do_scsi_pt_err;
 134     if (e < 0) {
 135         fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
 136                 safe_strerror(-e));
 137         return;
 138     } else if (e == SCSI_PT_DO_BAD_PARAMS) {
 139         fprintf(stderr, "scsi_pt error: bad pass through setup\n");
 140         return;
 141     } else if (e == SCSI_PT_DO_TIMEOUT) {
 142         fprintf(stderr, "  pass through timeout\n");
 143         return;
 144     }
 145     const int duration = get_scsi_pt_duration_ms(ptvp);
 146     if ((stl->verbose > 1) && (duration >= 0))
 147         DD(stl, "      duration=%d ms\n", duration);
 148
 149     // XXX stlink fw sends broken residue, so ignore it and use the known q_len
 150     // "usb-storage quirks=483:3744:r"
 151     // forces residue to be ignored and calculated, but this causes aboard if
 152     // data_len = 0 and by some other data_len values.
 153
 154     const int resid = get_scsi_pt_resid(ptvp);
 155     const int dsize = stl->q_len - resid;
 156
 157     const int cat = get_scsi_pt_result_category(ptvp);
 158     char buf[512];
 159     unsigned int slen;
 160
 161     switch (cat) {
 162         case SCSI_PT_RESULT_GOOD:
 163             if (stl->verbose && (resid > 0))
 164                 DD(stl, "      notice: requested %d bytes but "
 165                     "got %d bytes, ignore [broken] residue = %d\n",
 166                     stl->q_len, dsize, resid);
 167             break;
 168         case SCSI_PT_RESULT_STATUS:
 169             if (stl->verbose) {
 170                 sg_get_scsi_status_str(
 171                         get_scsi_pt_status_response(ptvp), sizeof (buf),
 172                         buf);
 173                 DD(stl, "  scsi status: %s\n", buf);
 174             }
 175             return;
 176         case SCSI_PT_RESULT_SENSE:
 177             slen = get_scsi_pt_sense_len(ptvp);
 178             if (stl->verbose) {
 179                 sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
 180                         > 1), sizeof (buf), buf);
 181                 DD(stl, "%s", buf);
 182             }
 183             if (stl->verbose && (resid > 0)) {
 184                 if ((stl->verbose) || (stl->q_len > 0))
 185                     DD(stl, "    requested %d bytes but "
 186                         "got %d bytes\n", stl->q_len, dsize);
 187             }
 188             return;
 189         case SCSI_PT_RESULT_TRANSPORT_ERR:
 190             if (stl->verbose) {
 191                 get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
 192                 // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
 193                 // These codes potentially come from the firmware on a host adapter
 194                 // or from one of several hosts that an adapter driver controls.
 195                 // The 'host_status' field has the following values:
 196                 //      [0x07] Internal error detected in the host adapter.
 197                 // This may not be fatal (and the command may have succeeded).
 198                 DD(stl, "  transport: %s", buf);
 199             }
 200             return;
 201         case SCSI_PT_RESULT_OS_ERR:
 202             if (stl->verbose) {
 203                 get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
 204                 DD(stl, "  os: %s", buf);
 205             }
 206             return;
 207         default:
 208             fprintf(stderr, "  unknown pass through result "
 209                     "category (%d)\n", cat);
 210     }
 211 }
 212
 213 void stlink_q(stlink_t *sl) {
 214     struct stlink_libsg* sg = sl->backend_data;
 215     DD(sl, "CDB[");
 216     for (int i = 0; i < CDB_SL; i++)
 217         DD(sl, " 0x%02x", (unsigned int) sg->cdb_cmd_blk[i]);
 218     DD(sl, "]\n");
 219
 220     // Get control command descriptor of scsi structure,
 221     // (one object per command!!)
 222     struct sg_pt_base *ptvp = construct_scsi_pt_obj();
 223     if (NULL == ptvp) {
 224         fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
 225         return;
 226     }
 227
 228     set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk));
 229
 230     // set buffer for sense (error information) data
 231     set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf));
 232
 233     // Set a buffer to be used for data transferred from device
 234     if (sg->q_data_dir == Q_DATA_IN) {
 235         //clear_buf(sl);
 236         set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
 237     } else {
 238         set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
 239     }
 240     // Executes SCSI command (or at least forwards it to lower layers).
 241     sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC,
 242             sl->verbose);
 243
 244     // check for scsi errors
 245     stlink_confirm_inq(sl, ptvp);
 246     // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
 247     destruct_scsi_pt_obj(ptvp);
 248 }
 249
 250 // TODO thinking, cleanup
 251
 252 void stlink_stat(stlink_t *stl, char *txt) {
 253     if (stl->q_len <= 0)
 254         return;
 255
 256     stlink_print_data(stl);
 257
 258     switch (stl->q_buf[0]) {
 259         case STLINK_OK:
 260             DD(stl, "  %s: ok\n", txt);
 261             return;
 262         case STLINK_FALSE:
 263             DD(stl, "  %s: false\n", txt);
 264             return;
 265         default:
 266             DD(stl, "  %s: unknown\n", txt);
 267     }
 268 }
 269
 270 void _stlink_sg_version(stlink_t *stl) {
 271     struct stlink_libsg *sl = stl->backend_data;
 272     D(stl, "\n*** stlink_version ***\n");
 273     clear_cdb(sl);
 274     sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
 275     stl->q_len = 6;
 276     sl->q_addr = 0;
 277     stlink_q(stl);
 278 }
 279
 280 // Get stlink mode:
 281 // STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
 282 // usb dfu             || usb mass             || jtag or swd
 283
 284 int _stlink_sg_current_mode(stlink_t *stl) {
 285     struct stlink_libsg *sl = stl->backend_data;
 286     clear_cdb(sl);
 287     sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
 288     stl->q_len = 2;
 289     sl->q_addr = 0;
 290     stlink_q(stl);
 291     return stl->q_buf[0];
 292 }
 293
 294 // Exit the mass mode and enter the swd debug mode.
 295
 296 void _stlink_sg_enter_swd_mode(stlink_t *sl) {
 297     struct stlink_libsg *sg = sl->backend_data;
 298     clear_cdb(sg);
 299     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 300     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
 301     sl->q_len = 0; // >0 -> aboard
 302     stlink_q(sl);
 303 }
 304
 305 // Exit the mass mode and enter the jtag debug mode.
 306 // (jtag is disabled in the discovery's stlink firmware)
 307
 308 void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
 309     struct stlink_libsg *sg = sl->backend_data;
 310     D(sl, "\n*** stlink_enter_jtag_mode ***\n");
 311     clear_cdb(sg);
 312     sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
 313     sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
 314     sl->q_len = 0;
 315     stlink_q(sl);
 316 }
 317
 318 // XXX kernel driver performs reset, the device temporally disappears
 319
 320 void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
 321     struct stlink_libsg *sg = sl->backend_data;
 322     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 323     clear_cdb(sg);
 324     sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
 325     sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
 326     sl->q_len = 0; // ??
 327     stlink_q(sl);
 328     /*
 329      [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
 330      [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
 331      [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
 332      [135121.844584] end_request: I/O error, dev sdb, sector 4096
 333      [135121.844590] Buffer I/O error on device sdb, logical block 512
 334      [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
 335      [135130.274551] usb 6-1: device firmware changed
 336      [135130.274618] usb 6-1: USB disconnect, address 7
 337      [135130.275186] VFS: busy inodes on changed media or resized disk sdb
 338      [135130.275424] VFS: busy inodes on changed media or resized disk sdb
 339      [135130.286758] VFS: busy inodes on changed media or resized disk sdb
 340      [135130.292796] VFS: busy inodes on changed media or resized disk sdb
 341      [135130.301481] VFS: busy inodes on changed media or resized disk sdb
 342      [135130.304316] VFS: busy inodes on changed media or resized disk sdb
 343      [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
 344      [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
 345      [135130.629492] scsi20 : usb-storage 6-1:1.0
 346      [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
 347      [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
 348      [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
 349      [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
 350      [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 351      [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
 352      [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 353      [135131.640609]  sdb:
 354      [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
 355      [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
 356      [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
 357      [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 358      [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 359      [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 360      [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 361      [135131.671570] end_request: I/O error, dev sdb, sector 63872
 362      [135131.671575] Buffer I/O error on device sdb, logical block 7984
 363      [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
 364      [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
 365      [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
 366      [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
 367      [135131.678551] end_request: I/O error, dev sdb, sector 63872
 368      ...
 369      [135131.853565] end_request: I/O error, dev sdb, sector 4096
 370      */
 371 }
 372
 373 void _stlink_sg_core_id(stlink_t *sl) {
 374     struct stlink_libsg *sg = sl->backend_data;
 375     clear_cdb(sg);
 376     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
 377     sl->q_len = 4;
 378     sg->q_addr = 0;
 379     stlink_q(sl);
 380     sl->core_id = read_uint32(sl->q_buf, 0);
 381 }
 382
 383 // Arm-core reset -> halted state.
 384
 385 void _stlink_sg_reset(stlink_t *sl) {
 386     struct stlink_libsg *sg = sl->backend_data;
 387     clear_cdb(sg);
 388     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
 389     sl->q_len = 2;
 390     sg->q_addr = 0;
 391     stlink_q(sl);
 392     stlink_stat(sl, "core reset");
 393 }
 394
 395 // Arm-core status: halted or running.
 396
 397 void _stlink_sg_status(stlink_t *sl) {
 398     struct stlink_libsg *sg = sl->backend_data;
 399     D(sl, "\n*** stlink_status ***\n");
 400     clear_cdb(sg);
 401     sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
 402     sl->q_len = 2;
 403     sg->q_addr = 0;
 404     stlink_q(sl);
 405 }
 406
 407 // Force the core into the debug mode -> halted state.
 408
 409 void _stlink_sg_force_debug(stlink_t *sl) {
 410     struct stlink_libsg *sg = sl->backend_data;
 411     D(sl, "\n*** stlink_force_debug ***\n");
 412     clear_cdb(sg);
 413     sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
 414     sl->q_len = 2;
 415     sg->q_addr = 0;
 416     stlink_q(sl);
 417     stlink_stat(sl, "force debug");
 418 }
 419
 420 // Read all arm-core registers.
 421
 422 void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
 423     struct stlink_libsg *sg = sl->backend_data;
 424
 425     /* unused */
 426     regp = regp;
 427
 428     clear_cdb(sg);
 429     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
 430     sl->q_len = 84;
 431     sg->q_addr = 0;
 432     stlink_q(sl);
 433     stlink_print_data(sl);
 434
 435     // TODO - most of this should be re-extracted up....
 436
 437     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 438     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 439     for (int i = 0; i < 16; i++) {
 440         sg->reg.r[i] = read_uint32(sl->q_buf, 4 * i);
 441         if (sl->verbose > 1)
 442             DD(sl, "r%2d = 0x%08x\n", i, sg->reg.r[i]);
 443     }
 444     sg->reg.xpsr = read_uint32(sl->q_buf, 64);
 445     sg->reg.main_sp = read_uint32(sl->q_buf, 68);
 446     sg->reg.process_sp = read_uint32(sl->q_buf, 72);
 447     sg->reg.rw = read_uint32(sl->q_buf, 76);
 448     sg->reg.rw2 = read_uint32(sl->q_buf, 80);
 449     if (sl->verbose < 2)
 450         return;
 451
 452     DD(sl, "xpsr       = 0x%08x\n", sg->reg.xpsr);
 453     DD(sl, "main_sp    = 0x%08x\n", sg->reg.main_sp);
 454     DD(sl, "process_sp = 0x%08x\n", sg->reg.process_sp);
 455     DD(sl, "rw         = 0x%08x\n", sg->reg.rw);
 456     DD(sl, "rw2        = 0x%08x\n", sg->reg.rw2);
 457 }
 458
 459 // Read an arm-core register, the index must be in the range 0..20.
 460 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 461 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 462
 463 void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 464     struct stlink_libsg *sg = sl->backend_data;
 465     clear_cdb(sg);
 466     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
 467     sg->cdb_cmd_blk[2] = r_idx;
 468     sl->q_len = 4;
 469     sg->q_addr = 0;
 470     stlink_q(sl);
 471     //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 472     // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
 473     // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 474     stlink_print_data(sl);
 475
 476     uint32_t r = read_uint32(sl->q_buf, 0);
 477     DD(sl, "r_idx (%2d) = 0x%08x\n", r_idx, r);
 478
 479     switch (r_idx) {
 480         case 16:
 481             regp->xpsr = r;
 482             break;
 483         case 17:
 484             regp->main_sp = r;
 485             break;
 486         case 18:
 487             regp->process_sp = r;
 488             break;
 489         case 19:
 490             regp->rw = r; //XXX ?(primask, basemask etc.)
 491             break;
 492         case 20:
 493             regp->rw2 = r; //XXX ?(primask, basemask etc.)
 494             break;
 495         default:
 496             regp->r[r_idx] = r;
 497     }
 498 }
 499
 500 // Write an arm-core register. Index:
 501 //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
 502 // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
 503
 504 void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 505     struct stlink_libsg *sg = sl->backend_data;
 506     clear_cdb(sg);
 507     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
 508     //   2: reg index
 509     // 3-6: reg content
 510     sg->cdb_cmd_blk[2] = idx;
 511     write_uint32(sg->cdb_cmd_blk + 3, reg);
 512     sl->q_len = 2;
 513     sg->q_addr = 0;
 514     stlink_q(sl);
 515     stlink_stat(sl, "write reg");
 516 }
 517
 518 // Write a register of the debug module of the core.
 519 // XXX ?(atomic writes)
 520 // TODO test
 521
 522 void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
 523     struct stlink_libsg *sg = sl->backend_data;
 524     D(sl, "\n*** stlink_write_dreg ***\n");
 525     clear_cdb(sg);
 526     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
 527     // 2-5: address of reg of the debug module
 528     // 6-9: reg content
 529     write_uint32(sg->cdb_cmd_blk + 2, addr);
 530     write_uint32(sg->cdb_cmd_blk + 6, reg);
 531     sl->q_len = 2;
 532     sg->q_addr = addr;
 533     stlink_q(sl);
 534     stlink_stat(sl, "write debug reg");
 535 }
 536
 537 // Force the core exit the debug mode.
 538
 539 void _stlink_sg_run(stlink_t *sl) {
 540     struct stlink_libsg *sg = sl->backend_data;
 541     D(sl, "\n*** stlink_run ***\n");
 542     clear_cdb(sg);
 543     sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
 544     sl->q_len = 2;
 545     sg->q_addr = 0;
 546     stlink_q(sl);
 547     stlink_stat(sl, "run core");
 548 }
 549
 550 // Step the arm-core.
 551
 552 void _stlink_sg_step(stlink_t *sl) {
 553     struct stlink_libsg *sg = sl->backend_data;
 554     clear_cdb(sg);
 555     sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
 556     sl->q_len = 2;
 557     sg->q_addr = 0;
 558     stlink_q(sl);
 559     stlink_stat(sl, "step core");
 560 }
 561
 562 // TODO test
 563 // see Cortex-M3 Technical Reference Manual
 564 // TODO make delegate!
 565 void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
 566     D(sl, "\n*** stlink_set_hw_bp ***\n");
 567     struct stlink_libsg *sg = sl->backend_data;
 568     clear_cdb(sg);
 569     sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
 570     // 2:The number of the flash patch used to set the breakpoint
 571     // 3-6: Address of the breakpoint (LSB)
 572     // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
 573     sl->q_buf[2] = fp_nr;
 574     write_uint32(sl->q_buf, addr);
 575     sl->q_buf[7] = fp;
 576
 577     sl->q_len = 2;
 578     stlink_q(sl);
 579     stlink_stat(sl, "set flash breakpoint");
 580 }
 581
 582 // TODO test
 583
 584 // TODO make delegate!
 585 void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
 586     struct stlink_libsg *sg = sl->backend_data;
 587     D(sl, "\n*** stlink_clr_hw_bp ***\n");
 588     clear_cdb(sg);
 589     sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
 590     sg->cdb_cmd_blk[2] = fp_nr;
 591
 592     sl->q_len = 2;
 593     stlink_q(sl);
 594     stlink_stat(sl, "clear flash breakpoint");
 595 }
 596
 597 // Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
 598
 599 void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 600     struct stlink_libsg *sg = sl->backend_data;
 601     clear_cdb(sg);
 602     sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
 603     // 2-5: addr
 604     // 6-7: len
 605     write_uint32(sg->cdb_cmd_blk + 2, addr);
 606     write_uint16(sg->cdb_cmd_blk + 6, len);
 607
 608     // data_in 0-0x40-len
 609     // !!! len _and_ q_len must be max 6k,
 610     //     i.e. >1024 * 6 = 6144 -> aboard)
 611     // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
 612     //     (broken residue issue)
 613     sl->q_len = len;
 614     sg->q_addr = addr;
 615     stlink_q(sl);
 616     stlink_print_data(sl);
 617 }
 618
 619 // Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
 620
 621 void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 622     struct stlink_libsg *sg = sl->backend_data;
 623     clear_cdb(sg);
 624     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
 625     // 2-5: addr
 626     // 6-7: len (>0x40 (64) -> aboard)
 627     write_uint32(sg->cdb_cmd_blk + 2, addr);
 628     write_uint16(sg->cdb_cmd_blk + 6, len);
 629
 630     // data_out 0-len
 631     sl->q_len = len;
 632     sg->q_addr = addr;
 633     sg->q_data_dir = Q_DATA_OUT;
 634     stlink_q(sl);
 635     stlink_print_data(sl);
 636 }
 637
 638 // Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
 639
 640 void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 641     struct stlink_libsg *sg = sl->backend_data;
 642     clear_cdb(sg);
 643     sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
 644     // 2-5: addr
 645     // 6-7: len "unlimited"
 646     write_uint32(sg->cdb_cmd_blk + 2, addr);
 647     write_uint16(sg->cdb_cmd_blk + 6, len);
 648
 649     // data_out 0-0x40-...-len
 650     sl->q_len = len;
 651     sg->q_addr = addr;
 652     sg->q_data_dir = Q_DATA_OUT;
 653     stlink_q(sl);
 654     stlink_print_data(sl);
 655 }
 656
 657 #if 0 /* not working */
 658
 659 static int write_flash_mem16
 660 (struct stlink* sl, uint32_t addr, uint16_t val) {
 661     /* half word writes */
 662     if (addr % 2) return -1;
 663
 664     /* unlock if locked */
 665     unlock_flash_if(sl);
 666
 667     /* set flash programming chosen bit */
 668     set_flash_cr_pg(sl);
 669
 670     write_uint16(sl->q_buf, val);
 671     stlink_write_mem16(sl, addr, 2);
 672
 673     /* wait for non business */
 674     wait_flash_busy(sl);
 675
 676     lock_flash(sl);
 677
 678     /* check the programmed value back */
 679     stlink_read_mem16(sl, addr, 2);
 680     if (*(const uint16_t*) sl->q_buf != val) {
 681         /* values differ at i * sizeof(uint16_t) */
 682         return -1;
 683     }
 684
 685     /* success */
 686     return 0;
 687 }
 688 #endif /* not working */
 689
 690 // Exit the jtag or swd mode and enter the mass mode.
 691
 692 void _stlink_sg_exit_debug_mode(stlink_t *stl) {
 693
 694     if (stl) {
 695         struct stlink_libsg* sl = stl->backend_data;
 696         clear_cdb(sl);
 697         sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
 698         stl->q_len = 0; // >0 -> aboard
 699         stlink_q(stl);
 700     }
 701 }
 702
 703
 704 // 1) open a sg device, switch the stlink from dfu to mass mode
 705 // 2) wait 5s until the kernel driver stops reseting the broken device
 706 // 3) reopen the device
 707 // 4) the device driver is now ready for a switch to jtag/swd mode
 708 // TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
 709
 710 stlink_backend_t _stlink_sg_backend = {
 711     _stlink_sg_close,
 712     _stlink_sg_exit_debug_mode,
 713     _stlink_sg_enter_swd_mode,
 714     _stlink_sg_enter_jtag_mode,
 715     _stlink_sg_exit_dfu_mode,
 716     _stlink_sg_core_id,
 717     _stlink_sg_reset,
 718     _stlink_sg_run,
 719     _stlink_sg_status,
 720     _stlink_sg_version,
 721     _stlink_sg_read_mem32,
 722     _stlink_sg_write_mem32,
 723     _stlink_sg_write_mem8,
 724     _stlink_sg_read_all_regs,
 725     _stlink_sg_read_reg,
 726     _stlink_sg_write_reg,
 727     _stlink_sg_step,
 728     _stlink_sg_current_mode,
 729     _stlink_sg_force_debug
 730 };
 731
 732 stlink_t* stlink_open(const char *dev_name, const int verbose) {
 733     fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name);
 734     int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose);
 735     if (sg_fd < 0) {
 736         fprintf(stderr, "error opening device: %s: %s\n", dev_name,
 737                 safe_strerror(-sg_fd));
 738         return NULL;
 739     }
 740
 741     stlink_t *sl = malloc(sizeof (stlink_t));
 742     struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
 743     if (sl == NULL || slsg == NULL) {
 744         fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
 745         return NULL;
 746     }
 747     sl->verbose = verbose;
 748     sl->backend_data = slsg;
 749     sl->backend = &_stlink_sg_backend;
 750
 751     slsg->sg_fd = sg_fd;
 752     sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 753     slsg->core_id = 0;
 754     slsg->q_addr = 0;
 755     clear_buf(sl);
 756
 757     /* flash memory settings */
 758     sl->flash_base = STM32_FLASH_BASE;
 759     sl->flash_size = STM32_FLASH_SIZE;
 760     sl->flash_pgsz = STM32_FLASH_PGSZ;
 761
 762     /* system memory */
 763     sl->sys_base = STM32_SYSTEM_BASE;
 764     sl->sys_size = STM32_SYSTEM_SIZE;
 765
 766     /* sram memory settings */
 767     sl->sram_base = STM32_SRAM_BASE;
 768     sl->sram_size = STM32_SRAM_SIZE;
 769
 770     return sl;
 771 }
 772
 773
 774
 775 stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
 776
 777     stlink_t *sl = stlink_open(dev_name, verbose);
 778     if (sl == NULL) {
 779         fputs("Error: could not open stlink device\n", stderr);
 780         return NULL;
 781     }
 782
 783     stlink_version(sl);
 784     struct stlink_libsg *sg = sl->backend_data;
 785
 786     if (sg->st_vid != USB_ST_VID || sg->stlink_pid != USB_STLINK_PID) {
 787         fprintf(stderr, "Error: the device %s is not a stlink\n",
 788                 dev_name);
 789         fprintf(stderr, "       VID: got %04x expect %04x \n",
 790                 sg->st_vid, USB_ST_VID);
 791         fprintf(stderr, "       PID: got %04x expect %04x \n",
 792                 sg->stlink_pid, USB_STLINK_PID);
 793         return NULL;
 794     }
 795
 796     D(sl, "\n*** stlink_force_open ***\n");
 797     switch (stlink_current_mode(sl)) {
 798         case STLINK_DEV_MASS_MODE:
 799             return sl;
 800         case STLINK_DEV_DEBUG_MODE:
 801             // TODO go to mass?
 802             return sl;
 803     }
 804     DD(sl, "\n*** switch the stlink to mass mode ***\n");
 805     _stlink_sg_exit_dfu_mode(sl);
 806     // exit the dfu mode -> the device is gone
 807     DD(sl, "\n*** reopen the stlink device ***\n");
 808     delay(1000);
 809     stlink_close(sl);
 810     delay(5000);
 811
 812     sl = stlink_open(dev_name, verbose);
 813     if (sl == NULL) {
 814         fputs("Error: could not open stlink device\n", stderr);
 815         return NULL;
 816     }
 817     // re-query device info
 818     stlink_version(sl);
 819     return sl;
 820 }
 821
 822 static void __attribute__((unused)) mark_buf(stlink_t *sl) {
 823     clear_buf(sl);
 824     sl->q_buf[0] = 0x12;
 825     sl->q_buf[1] = 0x34;
 826     sl->q_buf[2] = 0x56;
 827     sl->q_buf[3] = 0x78;
 828     sl->q_buf[4] = 0x90;
 829     sl->q_buf[15] = 0x42;
 830     sl->q_buf[16] = 0x43;
 831     sl->q_buf[63] = 0x42;
 832     sl->q_buf[64] = 0x43;
 833     sl->q_buf[1024 * 6 - 1] = 0x42; //6kB
 834     sl->q_buf[1024 * 8 - 1] = 0x42; //8kB
 835 }