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