From 27b50a3df817492716d21cf8ec91bcf074e2a479 Mon Sep 17 00:00:00 2001 From: texane Date: Fri, 14 Jan 2011 02:54:52 -0600 Subject: [PATCH] [initial] --- AUTHORS | 3 + LICENSE | 27 + README | 5 + build/Makefile | 27 + src/stlink-access-test.c | 1617 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 1679 insertions(+) create mode 100644 AUTHORS create mode 100644 LICENSE create mode 100644 README create mode 100644 build/Makefile create mode 100644 src/stlink-access-test.c diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000..b069adf --- /dev/null +++ b/AUTHORS @@ -0,0 +1,3 @@ +Martin Capitanio +Spencer Oliver +Lementec Fabien diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..37b124e --- /dev/null +++ b/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2011 The "Capt'ns Missing Link" Authors. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + + * Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above +copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the +distribution. + * Neither the name of Martin Capitanio nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README b/README new file mode 100644 index 0000000..9861697 --- /dev/null +++ b/README @@ -0,0 +1,5 @@ +cd build +make + +edit Makefile +make run diff --git a/build/Makefile b/build/Makefile new file mode 100644 index 0000000..34c25c6 --- /dev/null +++ b/build/Makefile @@ -0,0 +1,27 @@ +PRG := stlink-access-test +DEV := /dev/sg2 + +all: $(PRG) + +LIBS := \ + -lsgutils2 + +OBJS += \ + stlink-access-test.o + +$(PRG): $(OBJS) + @echo 'Invoking: GCC C Linker' + gcc -o$(PRG) $(OBJS) $(LIBS) + +%.o: ../src/%.c + @echo 'Building file: $<' + gcc -O0 -g3 -Wall -c -fmessage-length=0 -std=gnu99 -MMD -MP \ + -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)"\ + -o"$@" "$<" + +clean: + @rm -vf *.d *.o $(PRG) + +run: all + cp $(PRG) /tmp/ + sudo /tmp/$(PRG) $(DEV) diff --git a/src/stlink-access-test.c b/src/stlink-access-test.c new file mode 100644 index 0000000..1ce672f --- /dev/null +++ b/src/stlink-access-test.c @@ -0,0 +1,1617 @@ +/* + Copyright (c) 2010 "Capt'ns Missing Link" Authors. All rights reserved. + Use of this source code is governed by a BSD-style + license that can be found in the LICENSE file. + + A linux stlink access demo. The purpose of this file is to mitigate the usual + "reinventing the wheel" force by incompatible licenses and give you an idea, + how to access the stlink device. That doesn't mean you should be a free-loader + and not contribute your improvements to this code. + + Author: Martin Capitanio + The stlink related constants kindly provided by Oliver Spencer (OpenOCD) + for use in a GPL compatible license. + + Notes: + gcc -O0 -g3 -Wall -c -std=gnu99 -o stlink-access-test.o stlink-access-test.c + gcc -o stlink-access-test stlink-access-test.o -lsgutils2 + + Code format ~ TAB = 8, K&R, linux kernel source, golang oriented + Tested compatibility: linux, gcc >= 4.3.3 + + The communication is based on standard USB mass storage device + BOT (Bulk Only Transfer) + - Endpoint 1: BULK_IN, 64 bytes max + - Endpoint 2: BULK_OUT, 64 bytes max + + All CBW transfers are ordered with the LSB (byte 0) first (little endian). + Any command must be answered before sending the next command. + Each USB transfer must complete in less than 1s. + + SB Device Class Definition for Mass Storage Devices: + www.usb.org/developers/devclass_docs/usbmassbulk_10.pdf + + dt - Data Transfer (IN/OUT) + CBW - Command Block Wrapper + CSW - Command Status Wrapper + RFU - Reserved for Future Use + scsi_pt - SCSI pass-through + sg - SCSI generic + + * usb-storage.quirks + http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=blob_plain;f=Documentation/kernel-parameters.txt + Each entry has the form VID:PID:Flags where VID and PID are Vendor and Product + ID values (4-digit hex numbers) and Flags is a set of characters, each corresponding + to a common usb-storage quirk flag as follows: + + a = SANE_SENSE (collect more than 18 bytes of sense data); + b = BAD_SENSE (don't collect more than 18 bytes of sense data); + c = FIX_CAPACITY (decrease the reported device capacity by one sector); + h = CAPACITY_HEURISTICS (decrease the reported device capacity by one sector if the number is odd); + i = IGNORE_DEVICE (don't bind to this device); + l = NOT_LOCKABLE (don't try to lock and unlock ejectable media); + m = MAX_SECTORS_64 (don't transfer more than 64 sectors = 32 KB at a time); + o = CAPACITY_OK (accept the capacity reported by the device); + r = IGNORE_RESIDUE (the device reports bogus residue values); + s = SINGLE_LUN (the device has only one Logical Unit); + w = NO_WP_DETECT (don't test whether the medium is write-protected). + + Example: quirks=0419:aaf5:rl,0421:0433:rc + http://permalink.gmane.org/gmane.linux.usb.general/35053 + + modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:l + + Equivalently, you can add a line saying + + options usb-storage quirks=483:3744:l + + to your /etc/modprobe.conf or /etc/modprobe.d/local.conf (or add the "quirks=..." + part to an existing options line for usb-storage). + */ + +#define __USE_GNU +#include +#include +#include +#include + +// sgutils2 (apt-get install libsgutils2-dev) +#include +#include + +// device access +#define RDWR 0 +#define RO 1 +#define SG_TIMEOUT_SEC 1 // actually 1 is about 2 sec +// Each CDB can be a total of 6, 10, 12, or 16 bytes, later version +// of the SCSI standard also allow for variable-length CDBs (min. CDB is 6). +// the stlink needs max. 10 bytes. +#define CDB_6 6 +#define CDB_10 10 +#define CDB_12 12 +#define CDB_16 16 + +#define CDB_SL 10 + +// Query data flow direction. +#define Q_DATA_OUT 0 +#define Q_DATA_IN 1 + +// The SCSI Request Sense command is used to obtain sense data +// (error information) from a target device. +// http://en.wikipedia.org/wiki/SCSI_Request_Sense_Command +#define SENSE_BUF_LEN 32 + +// Max data transfer size. +// 6kB = max mem32_read block, 8kB sram +//#define Q_BUF_LEN 96 +#define Q_BUF_LEN 1024 * 100 + +// st-link vendor cmd's +#define USB_ST_VID 0x0483 +#define USB_STLINK_PID 0x3744 + +// STLINK_DEBUG_RESETSYS, etc: +#define STLINK_OK 0x80 +#define STLINK_FALSE 0x81 +#define STLINK_CORE_RUNNINIG 0x80 +#define STLINK_CORE_HALTED 0x81 +#define STLINK_CORE_STAT_UNKNOWN -1 + +#define STLINK_GET_VERSION 0xf1 +#define STLINK_GET_CURRENT_MODE 0xf5 + +#define STLINK_DEBUG_COMMAND 0xF2 +#define STLINK_DFU_COMMAND 0xF3 +#define STLINK_DFU_EXIT 0x07 + +// STLINK_GET_CURRENT_MODE +#define STLINK_DEV_DFU_MODE 0x00 +#define STLINK_DEV_MASS_MODE 0x01 +#define STLINK_DEV_DEBUG_MODE 0x02 +#define STLINK_DEV_UNKNOWN_MODE -1 + +// jtag mode cmds +#define STLINK_DEBUG_ENTER 0x20 +#define STLINK_DEBUG_EXIT 0x21 +#define STLINK_DEBUG_READCOREID 0x22 +#define STLINK_DEBUG_GETSTATUS 0x01 +#define STLINK_DEBUG_FORCEDEBUG 0x02 +#define STLINK_DEBUG_RESETSYS 0x03 +#define STLINK_DEBUG_READALLREGS 0x04 +#define STLINK_DEBUG_READREG 0x05 +#define STLINK_DEBUG_WRITEREG 0x06 +#define STLINK_DEBUG_READMEM_32BIT 0x07 +#define STLINK_DEBUG_WRITEMEM_32BIT 0x08 +#define STLINK_DEBUG_RUNCORE 0x09 +#define STLINK_DEBUG_STEPCORE 0x0a +#define STLINK_DEBUG_SETFP 0x0b +#define STLINK_DEBUG_WRITEMEM_8BIT 0x0d +#define STLINK_DEBUG_CLEARFP 0x0e +#define STLINK_DEBUG_WRITEDEBUGREG 0x0f +#define STLINK_DEBUG_ENTER_SWD 0xa3 +#define STLINK_DEBUG_ENTER_JTAG 0x00 + +typedef struct { + uint32_t r[16]; + uint32_t xpsr; + uint32_t main_sp; + uint32_t process_sp; + uint32_t rw; + uint32_t rw2; +} reg; + +struct stlink { + int sg_fd; + int do_scsi_pt_err; + // sg layer verboseness: 0 for no debug info, 10 for lots + int verbose; + + unsigned char cdb_cmd_blk[CDB_SL]; + + // Data transferred from or to device + unsigned char q_buf[Q_BUF_LEN]; + int q_len; + int q_data_dir; // Q_DATA_IN, Q_DATA_OUT + // the start of the query data in the device memory space + uint32_t q_addr; + + // Sense (error information) data + unsigned char sense_buf[SENSE_BUF_LEN]; + + uint32_t st_vid; + uint32_t stlink_pid; + uint32_t stlink_v; + uint32_t jtag_v; + uint32_t swim_v; + uint32_t core_id; + + reg reg; + int core_stat; +}; + +static void D(struct stlink *sl, char *txt) { + if (sl->verbose > 1) + fputs(txt, stderr); +} + +// Suspends execution of the calling process for +// (at least) ms milliseconds. +static void delay(int ms) { + fprintf(stderr, "*** wait %d ms\n", ms); + usleep(1000 * ms); +} + +// Endianness +// http://www.ibm.com/developerworks/aix/library/au-endianc/index.html +// const int i = 1; +// #define is_bigendian() ( (*(char*)&i) == 0 ) +static inline unsigned int is_bigendian(void) { + static volatile const unsigned int i = 1; + return *(volatile const char*) &i == 0; +} + +static void write_uint32(unsigned char* buf, uint32_t ui) { + if (!is_bigendian()) { // le -> le (don't swap) + buf[0] = ((unsigned char*) &ui)[0]; + buf[1] = ((unsigned char*) &ui)[1]; + buf[2] = ((unsigned char*) &ui)[2]; + buf[3] = ((unsigned char*) &ui)[3]; + } else { + buf[0] = ((unsigned char*) &ui)[3]; + buf[1] = ((unsigned char*) &ui)[2]; + buf[2] = ((unsigned char*) &ui)[1]; + buf[3] = ((unsigned char*) &ui)[0]; + } +} + +static void write_uint16(unsigned char* buf, uint16_t ui) { + if (!is_bigendian()) { // le -> le (don't swap) + buf[0] = ((unsigned char*) &ui)[0]; + buf[1] = ((unsigned char*) &ui)[1]; + } else { + buf[0] = ((unsigned char*) &ui)[1]; + buf[1] = ((unsigned char*) &ui)[0]; + } +} + +static uint32_t read_uint32(const unsigned char *c, const int pt) { + uint32_t ui; + char *p = (char *) &ui; + + if (!is_bigendian()) { // le -> le (don't swap) + p[0] = c[pt]; + p[1] = c[pt + 1]; + p[2] = c[pt + 2]; + p[3] = c[pt + 3]; + } else { + p[0] = c[pt + 3]; + p[1] = c[pt + 2]; + p[2] = c[pt + 1]; + p[3] = c[pt]; + } + return ui; +} + +static void clear_cdb(struct stlink *sl) { + for (int i = 0; i < sizeof(sl->cdb_cmd_blk); i++) + sl->cdb_cmd_blk[i] = 0; + // set default + sl->cdb_cmd_blk[0] = STLINK_DEBUG_COMMAND; + sl->q_data_dir = Q_DATA_IN; +} + +// E.g. make the valgrind happy. +static void clear_buf(struct stlink *sl) { + fprintf(stderr, "*** clear_buf ***\n"); + for (int i = 0; i < sizeof(sl->q_buf); i++) + sl->q_buf[i] = 0; + +} + +static struct stlink* stlink_open(const char *dev_name, const int verbose) { + fprintf(stderr, "\n*** stlink_open [%s] ***\n", dev_name); + int sg_fd = scsi_pt_open_device(dev_name, RDWR, verbose); + if (sg_fd < 0) { + fprintf(stderr, "error opening device: %s: %s\n", dev_name, + safe_strerror(-sg_fd)); + return NULL; + } + + struct stlink *sl = malloc(sizeof(struct stlink)); + if (sl == NULL) { + fprintf(stderr, "struct stlink: out of memory\n"); + return NULL; + } + + sl->sg_fd = sg_fd; + sl->verbose = verbose; + sl->core_stat = STLINK_CORE_STAT_UNKNOWN; + sl->core_id = 0; + sl->q_addr = 0; + clear_buf(sl); + return sl; +} + +// close the device, free the allocated memory +void stlink_close(struct stlink *sl) { + D(sl, "\n*** stlink_close ***\n"); + if (sl) { + scsi_pt_close_device(sl->sg_fd); + free(sl); + } +} + +//TODO rewrite/cleanup, save the error in sl +static void stlink_confirm_inq(struct stlink *sl, struct sg_pt_base *ptvp) { + const int e = sl->do_scsi_pt_err; + if (e < 0) { + fprintf(stderr, "scsi_pt error: pass through os error: %s\n", + safe_strerror(-e)); + return; + } else if (e == SCSI_PT_DO_BAD_PARAMS) { + fprintf(stderr, "scsi_pt error: bad pass through setup\n"); + return; + } else if (e == SCSI_PT_DO_TIMEOUT) { + fprintf(stderr, " pass through timeout\n"); + return; + } + const int duration = get_scsi_pt_duration_ms(ptvp); + if ((sl->verbose > 1) && (duration >= 0)) + fprintf(stderr, " duration=%d ms\n", duration); + + // XXX stlink fw sends broken residue, so ignore it and use the known q_len + // "usb-storage quirks=483:3744:r" + // forces residue to be ignored and calculated, but this causes aboard if + // data_len = 0 and by some other data_len values. + + const int resid = get_scsi_pt_resid(ptvp); + const int dsize = sl->q_len - resid; + + const int cat = get_scsi_pt_result_category(ptvp); + char buf[512]; + unsigned int slen; + + switch (cat) { + case SCSI_PT_RESULT_GOOD: + if (sl->verbose && (resid > 0)) + fprintf(stderr, " notice: requested %d bytes but " + "got %d bytes, ignore [broken] residue = %d\n", + sl->q_len, dsize, resid); + break; + case SCSI_PT_RESULT_STATUS: + if (sl->verbose) { + sg_get_scsi_status_str( + get_scsi_pt_status_response(ptvp), sizeof(buf), + buf); + fprintf(stderr, " scsi status: %s\n", buf); + } + return; + case SCSI_PT_RESULT_SENSE: + slen = get_scsi_pt_sense_len(ptvp); + if (sl->verbose) { + sg_get_sense_str("", sl->sense_buf, slen, (sl->verbose + > 1), sizeof(buf), buf); + fprintf(stderr, "%s", buf); + } + if (sl->verbose && (resid > 0)) { + if ((sl->verbose) || (sl->q_len > 0)) + fprintf(stderr, " requested %d bytes but " + "got %d bytes\n", sl->q_len, dsize); + } + return; + case SCSI_PT_RESULT_TRANSPORT_ERR: + if (sl->verbose) { + get_scsi_pt_transport_err_str(ptvp, sizeof(buf), buf); + // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html + // These codes potentially come from the firmware on a host adapter + // or from one of several hosts that an adapter driver controls. + // The 'host_status' field has the following values: + // [0x07] Internal error detected in the host adapter. + // This may not be fatal (and the command may have succeeded). + fprintf(stderr, " transport: %s", buf); + } + return; + case SCSI_PT_RESULT_OS_ERR: + if (sl->verbose) { + get_scsi_pt_os_err_str(ptvp, sizeof(buf), buf); + fprintf(stderr, " os: %s", buf); + } + return; + default: + fprintf(stderr, " unknown pass through result " + "category (%d)\n", cat); + } +} + +static void stlink_q(struct stlink* sl) { + fputs("CDB[", stderr); + for (int i = 0; i < CDB_SL; i++) + fprintf(stderr, " 0x%02x", (unsigned int) sl->cdb_cmd_blk[i]); + fputs("]\n", stderr); + + // Get control command descriptor of scsi structure, + // (one object per command!!) + struct sg_pt_base *ptvp = construct_scsi_pt_obj(); + if (NULL == ptvp) { + fprintf(stderr, "construct_scsi_pt_obj: out of memory\n"); + return; + } + + set_scsi_pt_cdb(ptvp, sl->cdb_cmd_blk, sizeof(sl->cdb_cmd_blk)); + + // set buffer for sense (error information) data + set_scsi_pt_sense(ptvp, sl->sense_buf, sizeof(sl->sense_buf)); + + // Set a buffer to be used for data transferred from device + if (sl->q_data_dir == Q_DATA_IN) { + //clear_buf(sl); + set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len); + } else { + set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len); + } + // Executes SCSI command (or at least forwards it to lower layers). + sl->do_scsi_pt_err = do_scsi_pt(ptvp, sl->sg_fd, SG_TIMEOUT_SEC, + sl->verbose); + + // check for scsi errors + stlink_confirm_inq(sl, ptvp); + // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp); + destruct_scsi_pt_obj(ptvp); +} + +static void stlink_print_data(struct stlink *sl) { +#if 1 /* ascii mode */ + if (sl->q_len <= 0 || sl->verbose < 2) + return; + if (sl->verbose > 2) + fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len); + + for (uint32_t i = 0; i < sl->q_len; i++) { + if (i % 16 == 0) { + if (sl->q_data_dir == Q_DATA_OUT) + fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i); + else + fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i); + } + fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]); + } + fputs("\n\n", stdout); +#else /* binary mode */ + for (uint32_t i = 0; i < sl->q_len; i++) { + write(1, (unsigned char*)&sl->q_buf[i], 1); + } +#endif +} + +// TODO thinking, cleanup +static void stlink_parse_version(struct stlink *sl) { + sl->st_vid = 0; + sl->stlink_pid = 0; + if (sl->q_len <= 0) { + fprintf(stderr, "Error: could not parse the stlink version"); + return; + } + /* stlink_print_data(sl); */ + uint32_t b0 = sl->q_buf[0]; //lsb + uint32_t b1 = sl->q_buf[1]; + uint32_t b2 = sl->q_buf[2]; + uint32_t b3 = sl->q_buf[3]; + uint32_t b4 = sl->q_buf[4]; + uint32_t b5 = sl->q_buf[5]; //msb + + // b0 b1 || b2 b3 | b4 b5 + // 4b | 6b | 6b || 2B | 2B + // stlink_v | jtag_v | swim_v || st_vid | stlink_pid + + sl->stlink_v = (b0 & 0xf0) >> 4; + sl->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6); + sl->swim_v = b1 & 0x3f; + sl->st_vid = (b3 << 8) | b2; + sl->stlink_pid = (b5 << 8) | b4; + + if (sl->verbose < 2) + return; + + fprintf(stderr, "st vid = 0x%04x (expect 0x%04x)\n", + sl->st_vid, USB_ST_VID); + fprintf(stderr, "stlink pid = 0x%04x (expect 0x%04x)\n", + sl->stlink_pid, USB_STLINK_PID); + fprintf(stderr, "stlink version = 0x%x\n", sl->stlink_v); + fprintf(stderr, "jtag version = 0x%x\n", sl->jtag_v); + fprintf(stderr, "swim version = 0x%x\n", sl->swim_v); + if (sl->jtag_v == 0) + fprintf(stderr, + " notice: the firmware doesn't support a jtag/swd interface\n"); + if (sl->swim_v == 0) + fprintf(stderr, + " notice: the firmware doesn't support a swim interface\n"); + +} + +static int stlink_mode(struct stlink *sl) { + if (sl->q_len <= 0) + return STLINK_DEV_UNKNOWN_MODE; + + /* stlink_print_data(sl); */ + + switch (sl->q_buf[0]) { + case STLINK_DEV_DFU_MODE: + fprintf(stderr, "stlink mode: dfu\n"); + return STLINK_DEV_DFU_MODE; + case STLINK_DEV_DEBUG_MODE: + fprintf(stderr, "stlink mode: debug (jtag or swd)\n"); + return STLINK_DEV_DEBUG_MODE; + case STLINK_DEV_MASS_MODE: + fprintf(stderr, "stlink mode: mass\n"); + return STLINK_DEV_MASS_MODE; + } + return STLINK_DEV_UNKNOWN_MODE; +} + +static void stlink_stat(struct stlink *sl, char *txt) { + if (sl->q_len <= 0) + return; + + /* stlink_print_data(sl); */ + + switch (sl->q_buf[0]) { + case STLINK_OK: + fprintf(stderr, " %s: ok\n", txt); + return; + case STLINK_FALSE: + fprintf(stderr, " %s: false\n", txt); + return; + default: + fprintf(stderr, " %s: unknown\n", txt); + } +} + +static void stlink_core_stat(struct stlink *sl) { + if (sl->q_len <= 0) + return; + + /* stlink_print_data(sl); */ + + switch (sl->q_buf[0]) { + case STLINK_CORE_RUNNINIG: + sl->core_stat = STLINK_CORE_RUNNINIG; + fprintf(stderr, " core status: running\n"); + return; + case STLINK_CORE_HALTED: + sl->core_stat = STLINK_CORE_HALTED; + fprintf(stderr, " core status: halted\n"); + return; + default: + sl->core_stat = STLINK_CORE_STAT_UNKNOWN; + fprintf(stderr, " core status: unknown\n"); + } +} + +void stlink_version(struct stlink *sl) { + D(sl, "\n*** stlink_version ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[0] = STLINK_GET_VERSION; + sl->q_len = 6; + sl->q_addr = 0; + stlink_q(sl); + stlink_parse_version(sl); +} + +// Get stlink mode: +// STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE +// usb dfu || usb mass || jtag or swd +int stlink_current_mode(struct stlink *sl) { + D(sl, "\n*** stlink_current_mode ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE; + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + return stlink_mode(sl); +} + +// Exit the mass mode and enter the swd debug mode. +void stlink_enter_swd_mode(struct stlink *sl) { + D(sl, "\n*** stlink_enter_swd_mode ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER; + sl->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD; + sl->q_len = 0; // >0 -> aboard + stlink_q(sl); +} + +// Exit the mass mode and enter the jtag debug mode. +// (jtag is disabled in the discovery's stlink firmware) +void stlink_enter_jtag_mode(struct stlink *sl) { + D(sl, "\n*** stlink_enter_jtag_mode ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER; + sl->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG; + sl->q_len = 0; + stlink_q(sl); +} + +// Exit the jtag or swd mode and enter the mass mode. +void stlink_exit_debug_mode(struct stlink *sl) { + D(sl, "\n*** stlink_exit_debug_mode ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT; + sl->q_len = 0; // >0 -> aboard + stlink_q(sl); +} + +// XXX kernel driver performs reset, the device temporally disappears +static void stlink_exit_dfu_mode(struct stlink *sl) { + D(sl, "\n*** stlink_exit_dfu_mode ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[0] = STLINK_DFU_COMMAND; + sl->cdb_cmd_blk[1] = STLINK_DFU_EXIT; + sl->q_len = 0; // ?? + stlink_q(sl); + /* + [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code + [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK + [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00 + [135121.844584] end_request: I/O error, dev sdb, sector 4096 + [135121.844590] Buffer I/O error on device sdb, logical block 512 + [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7 + [135130.274551] usb 6-1: device firmware changed + [135130.274618] usb 6-1: USB disconnect, address 7 + [135130.275186] VFS: busy inodes on changed media or resized disk sdb + [135130.275424] VFS: busy inodes on changed media or resized disk sdb + [135130.286758] VFS: busy inodes on changed media or resized disk sdb + [135130.292796] VFS: busy inodes on changed media or resized disk sdb + [135130.301481] VFS: busy inodes on changed media or resized disk sdb + [135130.304316] VFS: busy inodes on changed media or resized disk sdb + [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8 + [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1 + [135130.629492] scsi20 : usb-storage 6-1:1.0 + [135131.625600] scsi 20:0:0:0: Direct-Access STM32 PQ: 0 ANSI: 0 + [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0 + [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB) + [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled + [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through + [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled + [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through + [135131.640609] sdb: + [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled + [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through + [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk + [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE + [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current] + [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range + [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00 + [135131.671570] end_request: I/O error, dev sdb, sector 63872 + [135131.671575] Buffer I/O error on device sdb, logical block 7984 + [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE + [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current] + [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range + [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00 + [135131.678551] end_request: I/O error, dev sdb, sector 63872 + ... + [135131.853565] end_request: I/O error, dev sdb, sector 4096 + */ +} + +static void stlink_core_id(struct stlink *sl) { + D(sl, "\n*** stlink_core_id ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID; + sl->q_len = 4; + sl->q_addr = 0; + stlink_q(sl); + sl->core_id = read_uint32(sl->q_buf, 0); + if (sl->verbose < 2) + return; + /* stlink_print_data(sl); */ + fprintf(stderr, "core_id = 0x%08x\n", sl->core_id); +} + +// Arm-core reset -> halted state. +void stlink_reset(struct stlink *sl) { + D(sl, "\n*** stlink_reset ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS; + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + stlink_stat(sl, "core reset"); +} + +// Arm-core status: halted or running. +void stlink_status(struct stlink *sl) { + D(sl, "\n*** stlink_status ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS; + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + stlink_core_stat(sl); +} + +// Force the core into the debug mode -> halted state. +void stlink_force_debug(struct stlink *sl) { + D(sl, "\n*** stlink_force_debug ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG; + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + stlink_stat(sl, "force debug"); +} + +// Read all arm-core registers. +void stlink_read_all_regs(struct stlink *sl) { + D(sl, "\n*** stlink_read_all_regs ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS; + sl->q_len = 84; + sl->q_addr = 0; + stlink_q(sl); + /* stlink_print_data(sl); */ + + // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83 + // r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 + for (int i = 0; i < 16; i++) { + sl->reg.r[i] = read_uint32(sl->q_buf, 4 * i); + if (sl->verbose > 1) + fprintf(stderr, "r%2d = 0x%08x\n", i, sl->reg.r[i]); + } + sl->reg.xpsr = read_uint32(sl->q_buf, 64); + sl->reg.main_sp = read_uint32(sl->q_buf, 68); + sl->reg.process_sp = read_uint32(sl->q_buf, 72); + sl->reg.rw = read_uint32(sl->q_buf, 76); + sl->reg.rw2 = read_uint32(sl->q_buf, 80); + if (sl->verbose < 2) + return; + + fprintf(stderr, "xpsr = 0x%08x\n", sl->reg.xpsr); + fprintf(stderr, "main_sp = 0x%08x\n", sl->reg.main_sp); + fprintf(stderr, "process_sp = 0x%08x\n", sl->reg.process_sp); + fprintf(stderr, "rw = 0x%08x\n", sl->reg.rw); + fprintf(stderr, "rw2 = 0x%08x\n", sl->reg.rw2); +} + +// Read an arm-core register, the index must be in the range 0..20. +// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20 +// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 +void stlink_read_reg(struct stlink *sl, int r_idx) { + D(sl, "\n*** stlink_read_reg"); + fprintf(stderr, " (%d) ***\n", r_idx); + + if (r_idx > 20 || r_idx < 0) { + fprintf(stderr, "Error: register index must be in [0..20]\n"); + return; + } + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_READREG; + sl->cdb_cmd_blk[2] = r_idx; + sl->q_len = 4; + sl->q_addr = 0; + stlink_q(sl); + // 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20 + // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83 + // r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 + /* stlink_print_data(sl); */ + + uint32_t r = read_uint32(sl->q_buf, 0); + fprintf(stderr, "r_idx (%2d) = 0x%08x\n", r_idx, r); + + switch (r_idx) { + case 16: + sl->reg.xpsr = r; + break; + case 17: + sl->reg.main_sp = r; + break; + case 18: + sl->reg.process_sp = r; + break; + case 19: + sl->reg.rw = r; //XXX ?(primask, basemask etc.) + break; + case 20: + sl->reg.rw2 = r; //XXX ?(primask, basemask etc.) + break; + default: + sl->reg.r[r_idx] = r; + } +} + +// Write an arm-core register. Index: +// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20 +// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2 +void stlink_write_reg(struct stlink *sl, uint32_t reg, int idx) { + D(sl, "\n*** stlink_write_reg ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG; + // 2: reg index + // 3-6: reg content + sl->cdb_cmd_blk[2] = idx; + write_uint32(sl->cdb_cmd_blk + 3, reg); + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + stlink_stat(sl, "write reg"); +} + +// Write a register of the debug module of the core. +// XXX ?(atomic writes) +// TODO test +void stlink_write_dreg(struct stlink *sl, uint32_t reg, uint32_t addr) { + D(sl, "\n*** stlink_write_dreg ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG; + // 2-5: address of reg of the debug module + // 6-9: reg content + write_uint32(sl->cdb_cmd_blk + 2, addr); + write_uint32(sl->cdb_cmd_blk + 6, reg); + sl->q_len = 2; + sl->q_addr = addr; + stlink_q(sl); + stlink_stat(sl, "write debug reg"); +} + +// Force the core exit the debug mode. +void stlink_run(struct stlink *sl) { + D(sl, "\n*** stlink_run ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE; + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + stlink_stat(sl, "run core"); +} + +// Step the arm-core. +void stlink_step(struct stlink *sl) { + D(sl, "\n*** stlink_step ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE; + sl->q_len = 2; + sl->q_addr = 0; + stlink_q(sl); + stlink_stat(sl, "step core"); +} + +// TODO test +// see Cortex-M3 Technical Reference Manual +void stlink_set_hw_bp(struct stlink *sl, int fp_nr, uint32_t addr, int fp) { + D(sl, "\n*** stlink_set_hw_bp ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP; + // 2:The number of the flash patch used to set the breakpoint + // 3-6: Address of the breakpoint (LSB) + // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00) + sl->q_buf[2] = fp_nr; + write_uint32(sl->q_buf, addr); + sl->q_buf[7] = fp; + + sl->q_len = 2; + stlink_q(sl); + stlink_stat(sl, "set flash breakpoint"); +} + +// TODO test +void stlink_clr_hw_bp(struct stlink *sl, int fp_nr) { + D(sl, "\n*** stlink_clr_hw_bp ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP; + sl->cdb_cmd_blk[2] = fp_nr; + + sl->q_len = 2; + stlink_q(sl); + stlink_stat(sl, "clear flash breakpoint"); +} + +// Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes) +void stlink_read_mem32(struct stlink *sl, uint32_t addr, uint16_t len) { + D(sl, "\n*** stlink_read_mem32 ***\n"); + if (len % 4 != 0) { // !!! never ever: fw gives just wrong values + fprintf( + stderr, + "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", + len % 4); + return; + } + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT; + // 2-5: addr + // 6-7: len + write_uint32(sl->cdb_cmd_blk + 2, addr); + write_uint16(sl->cdb_cmd_blk + 6, len); + + // data_in 0-0x40-len + // !!! len _and_ q_len must be max 6k, + // i.e. >1024 * 6 = 6144 -> aboard) + // !!! if len < q_len: 64*k, 1024*n, n=1..5 -> aboard + // (broken residue issue) + sl->q_len = len; + sl->q_addr = addr; + stlink_q(sl); + stlink_print_data(sl); +} + +void stlink_read_mem16(struct stlink *sl, uint32_t addr, uint16_t len) { + /* todo */ + if (len % 4) len = (len + 4) & ~(0x4 - 1); + stlink_read_mem32(sl, addr, len); +} + +// Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes. +void stlink_write_mem8(struct stlink *sl, uint32_t addr, uint16_t len) { + D(sl, "\n*** stlink_write_mem8 ***\n"); + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT; + // 2-5: addr + // 6-7: len (>0x40 (64) -> aboard) + write_uint32(sl->cdb_cmd_blk + 2, addr); + write_uint16(sl->cdb_cmd_blk + 6, len); + + // data_out 0-len + sl->q_len = len; + sl->q_addr = addr; + sl->q_data_dir = Q_DATA_OUT; + stlink_q(sl); + /* stlink_print_data(sl); */ +} + +void stlink_write_mem32(struct stlink *sl, uint32_t addr, uint16_t len); +void stlink_write_mem16(struct stlink *sl, uint32_t addr, uint16_t len) { + stlink_write_mem32(sl, addr, 4); +} + +// Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes. +void stlink_write_mem32(struct stlink *sl, uint32_t addr, uint16_t len) { + D(sl, "\n*** stlink_write_mem32 ***\n"); + if (len % 4 != 0) { + fprintf( + stderr, + "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", + len % 4); + return; + } + clear_cdb(sl); + sl->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT; + // 2-5: addr + // 6-7: len "unlimited" + write_uint32(sl->cdb_cmd_blk + 2, addr); + write_uint16(sl->cdb_cmd_blk + 6, len); + + // data_out 0-0x40-...-len + sl->q_len = len; + sl->q_addr = addr; + sl->q_data_dir = Q_DATA_OUT; + stlink_q(sl); + /* stlink_print_data(sl); */ +} + +/* FPEC flash controller interface, pm0063 manual + */ + +#define FLASH_REGS_ADDR 0x40022000 +#define FLASH_REGS_SIZE 0x28 + +#define FLASH_ACR (FLASH_REGS_ADDR + 0x00) +#define FLASH_KEYR (FLASH_REGS_ADDR + 0x04) +#define FLASH_SR (FLASH_REGS_ADDR + 0x0c) +#define FLASH_CR (FLASH_REGS_ADDR + 0x10) +#define FLASH_AR (FLASH_REGS_ADDR + 0x14) +#define FLASH_OBR (FLASH_REGS_ADDR + 0x1c) +#define FLASH_WRPR (FLASH_REGS_ADDR + 0x20) + +#define FLASH_RDPTR_KEY 0x00a5 +#define FLASH_KEY1 0x45670123 +#define FLASH_KEY2 0xcdef89ab + +#define FLASH_SR_BSY 0 +#define FLASH_SR_EOP 5 + +#define FLASH_CR_PG 0 +#define FLASH_CR_PER 1 +#define FLASH_CR_MER 2 +#define FLASH_CR_STRT 6 +#define FLASH_CR_LOCK 7 + +static uint32_t read_flash_rdp(struct stlink* sl) +{ + stlink_read_mem32(sl, FLASH_WRPR, sizeof(uint32_t)); + return (*(uint32_t*)sl->q_buf) & 0xff; +} + +static void disable_flash_wrpr(struct stlink* sl) +{ + /* todo */ +} + +static inline uint32_t read_flash_wrpr(struct stlink* sl) +{ + stlink_read_mem32(sl, FLASH_WRPR, sizeof(uint32_t)); + return *(uint32_t*)sl->q_buf; +} + +static inline uint32_t read_flash_obr(struct stlink* sl) +{ + stlink_read_mem32(sl, FLASH_OBR, sizeof(uint32_t)); + return *(uint32_t*)sl->q_buf; +} + +static inline uint32_t read_flash_cr(struct stlink* sl) +{ + stlink_read_mem32(sl, FLASH_CR, sizeof(uint32_t)); + return *(uint32_t*)sl->q_buf; +} + +static inline unsigned int is_flash_locked(struct stlink* sl) +{ + /* return non zero for true */ + return read_flash_cr(sl) & (1 << FLASH_CR_LOCK); +} + +static void unlock_flash(struct stlink* sl) +{ + /* the unlock sequence consists of 2 write cycles where + 2 key values are written to the FLASH_KEYR register. + an invalid sequence results in a definitive lock of + the FPEC block until next reset. + */ + + write_uint32(sl->q_buf, FLASH_KEY1); + stlink_write_mem32(sl, FLASH_KEYR, sizeof(uint32_t)); + + write_uint32(sl->q_buf, FLASH_KEY2); + stlink_write_mem32(sl, FLASH_KEYR, sizeof(uint32_t)); +} + +static int unlock_flash_if(struct stlink* sl) +{ + /* unlock flash if already locked */ + + if (is_flash_locked(sl)) + { + unlock_flash(sl); + if (is_flash_locked(sl)) + return -1; + } + + return 0; +} + +static void lock_flash(struct stlink* sl) +{ + /* write to 1 only. reset by hw at unlock sequence */ + + const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK); + + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void set_flash_cr_pg(struct stlink* sl) +{ + const uint32_t n = 1 << FLASH_CR_PG; + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void clear_flash_cr_pg(struct stlink* sl) +{ + const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG); + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void set_flash_cr_per(struct stlink* sl) +{ + const uint32_t n = 1 << FLASH_CR_PER; + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void clear_flash_cr_per(struct stlink* sl) +{ + const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER); + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void set_flash_cr_mer(struct stlink* sl) +{ + const uint32_t n = 1 << FLASH_CR_MER; + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void clear_flash_cr_mer(struct stlink* sl) +{ + const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER); + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static void set_flash_cr_strt(struct stlink* sl) +{ + /* assume come on the flash_cr_per path */ + const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT); + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_CR, sizeof(uint32_t)); +} + +static inline uint32_t read_flash_acr(struct stlink* sl) +{ + stlink_read_mem32(sl, FLASH_ACR, sizeof(uint32_t)); + return *(uint32_t*)sl->q_buf; +} + +static inline uint32_t read_flash_sr(struct stlink* sl) +{ + stlink_read_mem32(sl, FLASH_SR, sizeof(uint32_t)); + return *(uint32_t*)sl->q_buf; +} + +static inline unsigned int is_flash_busy(struct stlink* sl) +{ + return read_flash_sr(sl) & (1 << FLASH_SR_BSY); +} + +static void wait_flash_busy(struct stlink* sl) +{ + /* todo: add some delays here */ + while (is_flash_busy(sl)) + ; +} + +static inline unsigned int is_flash_eop(struct stlink* sl) +{ + return read_flash_sr(sl) & (1 << FLASH_SR_EOP); +} + +static void clear_flash_sr_eop(struct stlink* sl) +{ + const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP); + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_SR, sizeof(uint32_t)); +} + +static void wait_flash_eop(struct stlink* sl) +{ + /* todo: add some delays here */ + while (is_flash_eop(sl) == 0) + ; +} + +static inline void write_flash_ar(struct stlink* sl, uint32_t n) +{ + write_uint32(sl->q_buf, n); + stlink_write_mem32(sl, FLASH_AR, sizeof(uint32_t)); +} + +#if 0 /* 16 bits wide by default */ +static void enable_ahb_half_mode(struct stlink* sl) +{ + /* enable half word accesses */ +} + +static void disable_ahb_half_mode(struct stlink* sl) +{ + /* disable half word accesses */ +} +#endif + +static void disable_flash_read_protection(struct stlink* sl) +{ + /* erase the option byte area */ + /* rdp = 0x00a5; */ + /* reset */ +} + +static int write_flash_mem16(struct stlink* sl, uint32_t addr, uint16_t val) +{ + /* half word writes */ + if (addr % 2) return -1; + + /* unlock if locked */ + unlock_flash_if(sl); + + /* set flash programming chosen bit */ + set_flash_cr_pg(sl); + + write_uint16(sl->q_buf, val); + stlink_write_mem16(sl, addr, 2); + + /* wait for non business */ + wait_flash_busy(sl); + + lock_flash(sl); + + /* check the programmed value back */ + stlink_read_mem16(sl, addr, 2); + if (*(const uint16_t*)sl->q_buf != val) + { + /* values differ at i * sizeof(uint16_t) */ + return -1; + } + + /* success */ + return 0; +} + +static int erase_flash_page(struct stlink* sl, uint32_t page) +{ + /* page an addr in the page to erase */ + + /* wait for ongoing op to finish */ + wait_flash_busy(sl); + + /* unlock if locked */ + unlock_flash_if(sl); + + /* set the page erase bit */ + set_flash_cr_per(sl); + + /* select the page to erase */ + write_flash_ar(sl, page); + + /* start erase operation, reset by hw with bsy bit */ + set_flash_cr_strt(sl); + + /* wait for completion */ + wait_flash_busy(sl); + + /* relock the flash */ + lock_flash(sl); + + /* todo: verify the erased page */ + + return 0; +} + +static int erase_flash_mass(struct stlink* sl) +{ + /* wait for ongoing op to finish */ + wait_flash_busy(sl); + + /* unlock if locked */ + unlock_flash_if(sl); + + /* set the mass erase bit */ + set_flash_cr_mer(sl); + + /* start erase operation, reset by hw with bsy bit */ + set_flash_cr_strt(sl); + + /* wait for completion */ + wait_flash_busy(sl); + + /* relock the flash */ + lock_flash(sl); + + /* todo: verify the erased memory */ + + return 0; +} + +static unsigned int is_core_halted(struct stlink* sl) +{ + /* return non zero if core is halted */ + stlink_status(sl); + return sl->q_buf[0] == STLINK_CORE_HALTED; +} + +/* from openocd, contrib/loaders/flash/stm32.s */ +static const uint8_t loader_code[] = +{ + 0x08, 0x4c, /* ldr r4, STM32_FLASH_BASE */ + 0x1c, 0x44, /* add r4, r3 */ + /* write_half_word: */ + 0x01, 0x23, /* movs r3, #0x01 */ + 0x23, 0x61, /* str r3, [r4, #STM32_FLASH_CR_OFFSET] */ + 0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */ + 0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */ + /* busy: */ + 0xe3, 0x68, /* ldr r3, [r4, #STM32_FLASH_SR_OFFSET] */ + 0x13, 0xf0, 0x01, 0x0f, /* tst r3, #0x01 */ + 0xfb, 0xd0, /* beq busy */ + 0x13, 0xf0, 0x14, 0x0f, /* tst r3, #0x14 */ + 0x01, 0xd1, /* bne exit */ + 0x01, 0x3a, /* subs r2, r2, #0x01 */ + 0xf0, 0xd1, /* bne write_half_word */ + /* exit: */ + 0x00, 0xbe, /* bkpt #0x00 */ + 0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */ +}; + +static uint32_t write_loader_to_sram(struct stlink* sl) +{ +#define LOADER_SRAM_ADDR 0x20000000 +#define LOADER_SRAM_SIZE sizeof(loader_code) + + static unsigned int is_written = 0; + + if (is_written == 1) goto already_written; + is_written = 1; + + memcpy(sl->q_buf, loader_code, LOADER_SRAM_SIZE); + stlink_write_mem32(sl, LOADER_SRAM_ADDR, LOADER_SRAM_SIZE); + + already_written: + return LOADER_SRAM_ADDR; +} + +static uint32_t write_buffer_to_sram +(struct stlink* sl, const uint8_t* buf, size_t size) +{ + /* write the buffer right after the loader */ +#define BUFFER_SRAM_ADDR (LOADER_SRAM_ADDR + LOADER_SRAM_SIZE) + memcpy(sl->q_buf, buf, size); + stlink_write_mem32(sl, BUFFER_SRAM_ADDR, size); + return BUFFER_SRAM_ADDR; +} + +static void run_flash_loader +(struct stlink* sl, uint32_t target, const uint8_t* buf, size_t size) +{ + const uint32_t count = size / sizeof(uint16_t); + + uint32_t loader; + uint32_t source; + + /* allocate the loader in sram */ + loader = write_loader_to_sram(sl); + + /* allocate the buffer in sram */ + source = write_buffer_to_sram(sl, buf, size); + + /* setup core */ + stlink_write_reg(sl, source, 0); /* source */ + stlink_write_reg(sl, target, 1); /* target */ + stlink_write_reg(sl, count, 2); /* count (16 bits half words) */ + stlink_write_reg(sl, 0, 3); /* flash bank 0 (input). result (output) */ + stlink_write_reg(sl, loader, 15); /* pc register */ + + /* unlock and set programming mode */ + unlock_flash_if(sl); + set_flash_cr_pg(sl); + + /* run loader */ + stlink_run(sl); + + while (is_core_halted(sl) == 0) + ; + + lock_flash(sl); + + /* not all bytes have been written */ + stlink_read_reg(sl, 2); + if (sl->reg.r[2] != 1) + fprintf(stderr, "write error: %u\n", sl->reg.r[2]); +} + + +// 1) open a sg device, switch the stlink from dfu to mass mode +// 2) wait 5s until the kernel driver stops reseting the broken device +// 3) reopen the device +// 4) the device driver is now ready for a switch to jtag/swd mode +// TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device +struct stlink* stlink_force_open(const char *dev_name, const int verbose) { + struct stlink *sl = stlink_open(dev_name, verbose); + if (sl == NULL) { + fputs("Error: could not open stlink device\n", stderr); + return NULL; + } + + stlink_version(sl); + + if (sl->st_vid != USB_ST_VID || sl->stlink_pid != USB_STLINK_PID) { + fprintf(stderr, "Error: the device %s is not a stlink\n", + dev_name); + fprintf(stderr, " VID: got %04x expect %04x \n", + sl->st_vid, USB_ST_VID); + fprintf(stderr, " PID: got %04x expect %04x \n", + sl->stlink_pid, USB_STLINK_PID); + return NULL; + } + + D(sl, "\n*** stlink_force_open ***\n"); + switch (stlink_current_mode(sl)) { + case STLINK_DEV_MASS_MODE: + return sl; + case STLINK_DEV_DEBUG_MODE: + // TODO go to mass? + return sl; + } + fprintf(stderr, "\n*** switch the stlink to mass mode ***\n"); + stlink_exit_dfu_mode(sl); + // exit the dfu mode -> the device is gone + fprintf(stderr, "\n*** reopen the stlink device ***\n"); + delay(1000); + stlink_close(sl); + delay(5000); + + sl = stlink_open(dev_name, verbose); + if (sl == NULL) { + fputs("Error: could not open stlink device\n", stderr); + return NULL; + } + // re-query device info + stlink_version(sl); + return sl; +} + +static void mark_buf(struct stlink *sl) { + clear_buf(sl); + sl->q_buf[0] = 0x12; + sl->q_buf[1] = 0x34; + sl->q_buf[2] = 0x56; + sl->q_buf[3] = 0x78; + sl->q_buf[4] = 0x90; + sl->q_buf[15] = 0x42; + sl->q_buf[16] = 0x43; + sl->q_buf[63] = 0x42; + sl->q_buf[64] = 0x43; + sl->q_buf[1024 * 6 - 1] = 0x42; //6kB + sl->q_buf[1024 * 8 - 1] = 0x42; //8kB +} + +int main(int argc, char *argv[]) { + // set scpi lib debug level: 0 for no debug info, 10 for lots + const int scsi_verbose = 2; + char *dev_name; + + switch (argc) { + case 1: + fputs( + "\nUsage: stlink-access-test /dev/sg0, sg1, ...\n" + "\n*** Notice: The stlink firmware violates the USB standard.\n" + "*** If you plug-in the discovery's stlink, wait a several\n" + "*** minutes to let the kernel driver swallow the broken device.\n" + "*** Watch:\ntail -f /var/log/messages\n" + "*** This command sequence can shorten the waiting time and fix some issues.\n" + "*** Unplug the stlink and execute once as root:\n" + "modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:lrwsro\n\n", + stderr); + return EXIT_FAILURE; + case 2: + dev_name = argv[1]; + break; + default: + return EXIT_FAILURE; + } + + fputs("*** stlink access test ***\n", stderr); + fprintf(stderr, "Using sg_lib %s : scsi_pt %s\n", sg_lib_version(), + scsi_pt_version()); + + struct stlink *sl = stlink_force_open(dev_name, scsi_verbose); + if (sl == NULL) + return EXIT_FAILURE; + + // we are in mass mode, go to swd + stlink_enter_swd_mode(sl); + stlink_current_mode(sl); + stlink_core_id(sl); + //---------------------------------------------------------------------- + + stlink_status(sl); + //stlink_force_debug(sl); + stlink_reset(sl); + stlink_status(sl); +#if 0 + // core system control block + stlink_read_mem32(sl, 0xe000ed00, 4); + fprintf(stderr, "cpu id base register: SCB_CPUID = got 0x%08x expect 0x411fc231", read_uint32(sl->q_buf, 0)); + // no MPU + stlink_read_mem32(sl, 0xe000ed90, 4); + fprintf(stderr, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0)); + + stlink_read_mem32(sl, 0xe000edf0, 4); + fprintf(stderr, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0)); + + stlink_read_mem32(sl, 0x4001100c, 4); + fprintf(stderr, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0)); +#endif +#if 0 + // happy new year 2011: let blink all the leds + // see "RM0041 Reference manual - STM32F100xx advanced ARM-based 32-bit MCUs" + +#define GPIOC 0x40011000 // port C +#define GPIOC_CRH (GPIOC + 0x04) // port configuration register high +#define GPIOC_ODR (GPIOC + 0x0c) // port output data register +#define LED_BLUE (1<<8) // pin 8 +#define LED_GREEN (1<<9) // pin 9 + stlink_read_mem32(sl, GPIOC_CRH, 4); + uint32_t io_conf = read_uint32(sl->q_buf, 0); + fprintf(stderr, "GPIOC_CRH = 0x%08x", io_conf); + + // set: general purpose output push-pull, output mode, max speed 10 MHz. + write_uint32(sl->q_buf, 0x44444411); + stlink_write_mem32(sl, GPIOC_CRH, 4); + + clear_buf(sl); + for (int i = 0; i < 100; i++) { + write_uint32(sl->q_buf, LED_BLUE | LED_GREEN); + stlink_write_mem32(sl, GPIOC_ODR, 4); + //stlink_read_mem32(sl, 0x4001100c, 4); + //fprintf(stderr, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0)); + delay(100); + + clear_buf(sl); + stlink_write_mem32(sl, GPIOC_ODR, 4); // PC lo + delay(100); + } + write_uint32(sl->q_buf, io_conf); // set old state + +#endif +#if 0 + // TODO rtfm: stlink doesn't have flash write routines + // writing to the flash area confuses the fw for the next read access + + //stlink_read_mem32(sl, 0, 1024*6); + // flash 0x08000000 128kB + fputs("++++++++++ read a flash at 0x0800 0000\n", stderr); + stlink_read_mem32(sl, 0x08000000, 1024 * 6); //max 6kB + clear_buf(sl); + stlink_read_mem32(sl, 0x08000c00, 5); + stlink_read_mem32(sl, 0x08000c00, 4); + mark_buf(sl); + stlink_write_mem32(sl, 0x08000c00, 4); + stlink_read_mem32(sl, 0x08000c00, 256); + stlink_read_mem32(sl, 0x08000c00, 256); +#endif +#if 0 + // sram 0x20000000 8kB + fputs("\n++++++++++ read/write 8bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr); + clear_buf(sl); + stlink_write_mem8(sl, 0x20000000, 16); + + mark_buf(sl); + stlink_write_mem8(sl, 0x20000000, 1); + stlink_write_mem8(sl, 0x20000001, 1); + stlink_write_mem8(sl, 0x2000000b, 3); + stlink_read_mem32(sl, 0x20000000, 16); +#endif +#if 0 + // a not aligned mem32 access doesn't work indeed + fputs("\n++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr); + clear_buf(sl); + stlink_write_mem8(sl, 0x20000000, 32); + + mark_buf(sl); + stlink_write_mem32(sl, 0x20000000, 1); + stlink_read_mem32(sl, 0x20000000, 16); + mark_buf(sl); + stlink_write_mem32(sl, 0x20000001, 1); + stlink_read_mem32(sl, 0x20000000, 16); + mark_buf(sl); + stlink_write_mem32(sl, 0x2000000b, 3); + stlink_read_mem32(sl, 0x20000000, 16); + + mark_buf(sl); + stlink_write_mem32(sl, 0x20000000, 17); + stlink_read_mem32(sl, 0x20000000, 32); +#endif +#if 0 + // sram 0x20000000 8kB + fputs("++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++\n", stderr); + mark_buf(sl); + stlink_write_mem8(sl, 0x20000000, 64); + stlink_read_mem32(sl, 0x20000000, 64); + + mark_buf(sl); + stlink_write_mem32(sl, 0x20000000, 1024 * 8); //8kB + stlink_read_mem32(sl, 0x20000000, 1024 * 6); + stlink_read_mem32(sl, 0x20000000 + 1024 * 6, 1024 * 2); +#endif +#if 0 + stlink_read_all_regs(sl); + stlink_step(sl); + fputs("++++++++++ write r0 = 0x12345678\n", stderr); + stlink_write_reg(sl, 0x12345678, 0); + stlink_read_reg(sl, 0); + stlink_read_all_regs(sl); +#endif +#if 0 + stlink_run(sl); + stlink_status(sl); + + stlink_force_debug(sl); + stlink_status(sl); +#endif +#if 0 /* read the system bootloader */ +# define SYSMEM_ADDR 0x1ffff000 +# define SYSMEM_SIZE (2 * 1024) + fputs("\n++++++++++ reading bootloader ++++++++++++++++\n\n", stderr); + stlink_read_mem32(sl, SYSMEM_ADDR, SYSMEM_SIZE); +#endif +#if 0 /* read the flash memory */ + fputs("\n+++++++ read flash memory\n\n", stderr); + /* mark_buf(sl); */ + stlink_read_mem32(sl, 0x08000000, 4); +#endif +#if 1 /* flash programming */ + fputs("\n+++++++ program flash memory\n\n", stderr); + + /* erase the containing page */ +#define FLASH_ADDR 0x08000000 + erase_flash_page(sl, FLASH_ADDR); + + /* write a half word at FLASH_ADDR */ + const uint8_t buf[] = { '*', '*', '*', '*' }; + run_flash_loader(sl, FLASH_ADDR, buf, sizeof(buf)); + + stlink_read_mem32(sl, FLASH_ADDR, 4); + +#endif + + stlink_run(sl); + stlink_status(sl); + //---------------------------------------------------------------------- + // back to mass mode, just in case ... + stlink_exit_debug_mode(sl); + stlink_current_mode(sl); + stlink_close(sl); + + //fflush(stderr); fflush(stdout); + return EXIT_SUCCESS; +} -- 2.30.2