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

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