Move towards a standard libstlink, with backends for libusb (stm32l discovery) and scsi
passthrough (stm32vl discovery) and a common front end.
Verified that stm32l branch works much the same, but more to go for stm32vl.
--- /dev/null
+
+VPATH=src
+
+SOURCES_LIB=stlink-common.c stlink-usb.c #stlink-sg.c
+OBJS_LIB=$(SOURCES_LIB:.c=.o)
+
+CFLAGS+=-DCONFIG_USE_LIBUSB
+#CFLAGS+=-DCONFIG_USE_LIBSG
+CFLAGS+= -std=gnu99
+CFLAGS+=-Wall -Wextra
+
+LDFLAGS=-lstlink -lusb-1.0 -L.
+
+LIBRARY=libstlink.a
+
+all: $(LIBRARY) test_usb #test_sg
+
+$(LIBRARY): $(OBJS_LIB)
+ @echo "objs are $(OBJS_LIB)"
+ $(AR) -cr $@ $^
+ @echo "done making library"
+
+
+test_sg: test_sg.o $(LIBRARY)
+ @echo "building test_sg"
+ $(CC) $(LDFLAGS) -o $@
+
+test_usb: test_usb.o $(LIBRARY)
+ @echo "building test_usb"
+ $(CC) test_usb.o $(LDFLAGS) -o $@
+ @echo "done linking"
+
+%.o: %.c
+ @echo "building $^ into $@"
+ $(CC) $(CFLAGS) -c $^ -o $@
+ @echo "done compiling"
+
+clean:
+ rm -rf $(OBJS_LIB)
+ rm -rf $(LIBRARY)
+ rm -rf test_usb*
+ rm -rf test_sg*
+
+.PHONY: clean all
+++ /dev/null
-PRG := st-util
-DEBUG := #-DDEBUG
-
-all: $(PRG)
-
-LIBS := \
- -lsgutils2
-
-OBJS += \
- stlink-hw.o gdb-remote.o gdb-server.o
-
-$(PRG): $(OBJS)
- gcc -o $(PRG) $(OBJS) $(LIBS)
-
-%.o: ../src/%.c
- gcc -O3 -g3 -Wall -Werror -c -std=gnu99 -MMD -MP \
- -fno-strict-aliasing -Wno-unused $(DEBUG) \
- -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)"\
- -o "$@" "$<"
-
-clean:
- @rm -vf *.d *.o $(PRG)
--- /dev/null
+/* -*- tab-width:8 -*- */
+
+/*
+ Copyright (C) 2011 Peter Zotov <whitequark@whitequark.org>
+ Use of this source code is governed by a BSD-style
+ license that can be found in the LICENSE file.
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <sys/poll.h>
+
+static const char hex[] = "0123456789abcdef";
+
+int gdb_send_packet(int fd, char* data) {
+ unsigned length = strlen(data) + 5;
+ char* packet = malloc(length); /* '$' data (hex) '#' cksum (hex) */
+
+ memset(packet, 0, length);
+
+ packet[0] = '$';
+
+ uint8_t cksum = 0;
+ for(int i = 0; i < strlen(data); i++) {
+ packet[i + 1] = data[i];
+ cksum += data[i];
+ }
+
+ packet[length - 4] = '#';
+ packet[length - 3] = hex[cksum >> 4];
+ packet[length - 2] = hex[cksum & 0xf];
+
+ while(1) {
+ if(write(fd, packet, length) != length) {
+ free(packet);
+ return -2;
+ }
+
+ char ack;
+ if(read(fd, &ack, 1) != 1) {
+ free(packet);
+ return -2;
+ }
+
+ if(ack == '+') {
+ free(packet);
+ return 0;
+ }
+ }
+}
+
+#define ALLOC_STEP 1024
+
+int gdb_recv_packet(int fd, char** buffer) {
+ unsigned packet_size = ALLOC_STEP + 1, packet_idx = 0;
+ uint8_t cksum = 0;
+ char recv_cksum[3] = {0};
+ char* packet_buffer = malloc(packet_size);
+ unsigned state;
+
+start:
+ state = 0;
+ /*
+ * 0: waiting $
+ * 1: data, waiting #
+ * 2: cksum 1
+ * 3: cksum 2
+ * 4: fin
+ */
+
+ char c;
+ while(state != 4) {
+ if(read(fd, &c, 1) != 1) {
+ return -2;
+ }
+
+ switch(state) {
+ case 0:
+ if(c != '$') {
+ // ignore
+ } else {
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if(c == '#') {
+ state = 2;
+ } else {
+ packet_buffer[packet_idx++] = c;
+ cksum += c;
+
+ if(packet_idx == packet_size) {
+ packet_size += ALLOC_STEP;
+ packet_buffer = realloc(packet_buffer, packet_size);
+ }
+ }
+ break;
+
+ case 2:
+ recv_cksum[0] = c;
+ state = 3;
+ break;
+
+ case 3:
+ recv_cksum[1] = c;
+ state = 4;
+ break;
+ }
+ }
+
+ uint8_t recv_cksum_int = strtoul(recv_cksum, NULL, 16);
+ if(recv_cksum_int != cksum) {
+ char nack = '-';
+ if(write(fd, &nack, 1) != 1) {
+ return -2;
+ }
+
+ goto start;
+ } else {
+ char ack = '+';
+ if(write(fd, &ack, 1) != 1) {
+ return -2;
+ }
+ }
+
+ packet_buffer[packet_idx] = 0;
+ *buffer = packet_buffer;
+
+ return packet_idx;
+}
+
+// Here we skip any characters which are not \x03, GDB interrupt.
+// As we use the mode with ACK, in a (very unlikely) situation of a packet
+// lost because of this skipping, it will be resent anyway.
+int gdb_check_for_interrupt(int fd) {
+ struct pollfd pfd;
+ pfd.fd = fd;
+ pfd.events = POLLIN;
+
+ if(poll(&pfd, 1, 0) != 0) {
+ char c;
+
+ if(read(fd, &c, 1) != 1)
+ return -2;
+
+ if(c == '\x03') // ^C
+ return 1;
+ }
+
+ return 0;
+}
+
--- /dev/null
+#ifndef _GDB_REMOTE_H_
+#define _GDB_REMOTE_H_
+
+int gdb_send_packet(int fd, char* data);
+int gdb_recv_packet(int fd, char** buffer);
+int gdb_check_for_interrupt(int fd);
+
+#endif
--- /dev/null
+/* -*- tab-width:8 -*- */
+
+/*
+ Copyright (C) 2011 Peter Zotov <whitequark@whitequark.org>
+ Use of this source code is governed by a BSD-style
+ license that can be found in the LICENSE file.
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include "gdb-remote.h"
+#include "stlink-hw.h"
+
+#define FLASH_BASE 0x08000000
+#define FLASH_PAGE (sl->flash_pgsz)
+#define FLASH_PAGE_MASK (~((1 << 10) - 1))
+#define FLASH_SIZE (FLASH_PAGE * 128)
+
+static const char hex[] = "0123456789abcdef";
+
+static const char* current_memory_map = NULL;
+
+struct chip_params {
+ uint32_t chip_id;
+ char* description;
+ uint32_t max_flash_size, flash_pagesize;
+ uint32_t sram_size;
+ uint32_t bootrom_base, bootrom_size;
+} const devices[] = {
+ { 0x412, "Low-density device",
+ 0x8000, 0x400, 0x2800, 0x1ffff000, 0x800 },
+ { 0x410, "Medium-density device",
+ 0x20000, 0x400, 0x5000, 0x1ffff000, 0x800 },
+ { 0x414, "High-density device",
+ 0x80000, 0x800, 0x10000, 0x1ffff000, 0x800 },
+ { 0x418, "Connectivity line device",
+ 0x40000, 0x800, 0x10000, 0x1fffb000, 0x4800 },
+ { 0x420, "Medium-density value line device",
+ 0x20000, 0x400, 0x2000, 0x1ffff000, 0x800 },
+ { 0x428, "High-density value line device",
+ 0x80000, 0x800, 0x8000, 0x1ffff000, 0x800 },
+ { 0x430, "XL-density device",
+ 0x100000, 0x800, 0x18000, 0x1fffe000, 0x1800 },
+ { 0 }
+};
+
+int serve(struct stlink* sl, int port);
+char* make_memory_map(const struct chip_params *params, uint32_t flash_size);
+
+int main(int argc, char** argv) {
+ if(argc != 3) {
+ fprintf(stderr, "Usage: %s <port> /dev/sgX\n", argv[0]);
+ return 1;
+ }
+
+ struct stlink *sl = stlink_quirk_open(argv[2], 0);
+ if (sl == NULL)
+ return 1;
+
+ if(stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE)
+ stlink_enter_swd_mode(sl);
+
+ uint32_t chip_id;
+
+ stlink_read_mem32(sl, 0xE0042000, 4);
+ chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) |
+ (sl->q_buf[3] << 24);
+
+ printf("Chip ID is %08x.\n", chip_id);
+
+ const struct chip_params* params = NULL;
+
+ for(int i = 0; i < sizeof(devices) / sizeof(devices[0]); i++) {
+ if(devices[i].chip_id == (chip_id & 0xFFF)) {
+ params = &devices[i];
+ break;
+ }
+ }
+
+ if(params == NULL) {
+ fprintf(stderr, "Cannot recognize the connected device!\n");
+ return 0;
+ }
+
+ printf("Device connected: %s\n", params->description);
+ printf("Device parameters: SRAM: 0x%x bytes, Flash: up to 0x%x bytes in pages of 0x%x bytes\n",
+ params->sram_size, params->max_flash_size, params->flash_pagesize);
+
+ FLASH_PAGE = params->flash_pagesize;
+
+ uint32_t flash_size;
+
+ stlink_read_mem32(sl, 0x1FFFF7E0, 4);
+ flash_size = sl->q_buf[0] | (sl->q_buf[1] << 8);
+
+ printf("Flash size is %d KiB.\n", flash_size);
+ // memory map is in 1k blocks.
+ current_memory_map = make_memory_map(params, flash_size * 0x400);
+
+ int port = atoi(argv[1]);
+
+ while(serve(sl, port) == 0);
+
+ stlink_close(sl);
+
+ return 0;
+}
+
+static const char* const memory_map_template =
+ "<?xml version=\"1.0\"?>"
+ "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
+ " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
+ "<memory-map>"
+ " <memory type=\"rom\" start=\"0x00000000\" length=\"0x%x\"/>" // code = sram, bootrom or flash; flash is bigger
+ " <memory type=\"ram\" start=\"0x20000000\" length=\"0x%x\"/>" // sram 8k
+ " <memory type=\"flash\" start=\"0x08000000\" length=\"0x%x\">"
+ " <property name=\"blocksize\">0x%x</property>"
+ " </memory>"
+ " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
+ " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
+ " <memory type=\"rom\" start=\"0x%08x\" length=\"0x%x\"/>" // bootrom
+ " <memory type=\"rom\" start=\"0x1ffff800\" length=\"0x8\"/>" // option byte area
+ "</memory-map>";
+
+char* make_memory_map(const struct chip_params *params, uint32_t flash_size) {
+ /* This will be freed in serve() */
+ char* map = malloc(4096);
+ map[0] = '\0';
+
+ snprintf(map, 4096, memory_map_template,
+ flash_size,
+ params->sram_size,
+ flash_size, params->flash_pagesize,
+ params->bootrom_base, params->bootrom_size);
+
+ return map;
+}
+
+
+/*
+ * DWT_COMP0 0xE0001020
+ * DWT_MASK0 0xE0001024
+ * DWT_FUNCTION0 0xE0001028
+ * DWT_COMP1 0xE0001030
+ * DWT_MASK1 0xE0001034
+ * DWT_FUNCTION1 0xE0001038
+ * DWT_COMP2 0xE0001040
+ * DWT_MASK2 0xE0001044
+ * DWT_FUNCTION2 0xE0001048
+ * DWT_COMP3 0xE0001050
+ * DWT_MASK3 0xE0001054
+ * DWT_FUNCTION3 0xE0001058
+ */
+
+#define DATA_WATCH_NUM 4
+
+enum watchfun { WATCHDISABLED = 0, WATCHREAD = 5, WATCHWRITE = 6, WATCHACCESS = 7 };
+
+struct code_hw_watchpoint {
+ stm32_addr_t addr;
+ uint8_t mask;
+ enum watchfun fun;
+};
+
+struct code_hw_watchpoint data_watches[DATA_WATCH_NUM];
+
+static void init_data_watchpoints(struct stlink *sl) {
+ int i;
+
+ #ifdef DEBUG
+ printf("init watchpoints\n");
+ #endif
+
+ // set trcena in debug command to turn on dwt unit
+ stlink_read_mem32(sl, 0xE000EDFC, 4);
+ sl->q_buf[3] |= 1;
+ stlink_write_mem32(sl, 0xE000EDFC, 4);
+
+ // make sure all watchpoints are cleared
+ memset(sl->q_buf, 0, 4);
+ for(int i = 0; i < DATA_WATCH_NUM; i++) {
+ data_watches[i].fun = WATCHDISABLED;
+ stlink_write_mem32(sl, 0xe0001028 + i * 16, 4);
+ }
+}
+
+static int add_data_watchpoint(struct stlink* sl, enum watchfun wf, stm32_addr_t addr, unsigned int len)
+{
+ int i = 0;
+ uint32_t mask, temp;
+
+ // computer mask
+ // find a free watchpoint
+ // configure
+
+ mask = -1;
+ i = len;
+ while(i) {
+ i >>= 1;
+ mask++;
+ }
+
+ if((mask != -1) && (mask < 16)) {
+ for(i = 0; i < DATA_WATCH_NUM; i++) {
+ // is this an empty slot ?
+ if(data_watches[i].fun == WATCHDISABLED) {
+ #ifdef DEBUG
+ printf("insert watchpoint %d addr %x wf %u mask %u len %d\n", i, addr, wf, mask, len);
+ #endif
+
+ data_watches[i].fun = wf;
+ data_watches[i].addr = addr;
+ data_watches[i].mask = mask;
+
+ // insert comparator address
+ sl->q_buf[0] = (addr & 0xff);
+ sl->q_buf[1] = ((addr >> 8) & 0xff);
+ sl->q_buf[2] = ((addr >> 16) & 0xff);
+ sl->q_buf[3] = ((addr >> 24) & 0xff);
+
+ stlink_write_mem32(sl, 0xE0001020 + i * 16, 4);
+
+ // insert mask
+ memset(sl->q_buf, 0, 4);
+ sl->q_buf[0] = mask;
+ stlink_write_mem32(sl, 0xE0001024 + i * 16, 4);
+
+ // insert function
+ memset(sl->q_buf, 0, 4);
+ sl->q_buf[0] = wf;
+ stlink_write_mem32(sl, 0xE0001028 + i * 16, 4);
+
+ // just to make sure the matched bit is clear !
+ stlink_read_mem32(sl, 0xE0001028 + i * 16, 4);
+ return 0;
+ }
+ }
+ }
+
+ #ifdef DEBUG
+ printf("failure: add watchpoints addr %x wf %u len %u\n", addr, wf, len);
+ #endif
+ return -1;
+}
+
+static int delete_data_watchpoint(struct stlink* sl, stm32_addr_t addr)
+{
+ int i;
+
+ for(i = 0 ; i < DATA_WATCH_NUM; i++) {
+ if((data_watches[i].addr == addr) && (data_watches[i].fun != WATCHDISABLED)) {
+ #ifdef DEBUG
+ printf("delete watchpoint %d addr %x\n", i, addr);
+ #endif
+
+ memset(sl->q_buf, 0, 4);
+ data_watches[i].fun = WATCHDISABLED;
+ stlink_write_mem32(sl, 0xe0001028 + i * 16, 4);
+
+ return 0;
+ }
+ }
+
+ #ifdef DEBUG
+ printf("failure: delete watchpoint addr %x\n", addr);
+ #endif
+
+ return -1;
+}
+
+#define CODE_BREAK_NUM 6
+#define CODE_BREAK_LOW 0x01
+#define CODE_BREAK_HIGH 0x02
+
+struct code_hw_breakpoint {
+ stm32_addr_t addr;
+ int type;
+};
+
+struct code_hw_breakpoint code_breaks[CODE_BREAK_NUM];
+
+static void init_code_breakpoints(struct stlink* sl) {
+ memset(sl->q_buf, 0, 4);
+ sl->q_buf[0] = 0x03; // KEY | ENABLE
+ stlink_write_mem32(sl, 0xe0002000, 4);
+
+ memset(sl->q_buf, 0, 4);
+ for(int i = 0; i < CODE_BREAK_NUM; i++) {
+ code_breaks[i].type = 0;
+ stlink_write_mem32(sl, 0xe0002008 + i * 4, 4);
+ }
+}
+
+static int update_code_breakpoint(struct stlink* sl, stm32_addr_t addr, int set) {
+ stm32_addr_t fpb_addr = addr & ~0x3;
+ int type = addr & 0x2 ? CODE_BREAK_HIGH : CODE_BREAK_LOW;
+
+ if(addr & 1) {
+ fprintf(stderr, "update_code_breakpoint: unaligned address %08x\n", addr);
+ return -1;
+ }
+
+ int id = -1;
+ for(int i = 0; i < CODE_BREAK_NUM; i++) {
+ if(fpb_addr == code_breaks[i].addr ||
+ (set && code_breaks[i].type == 0)) {
+ id = i;
+ break;
+ }
+ }
+
+ if(id == -1) {
+ if(set) return -1; // Free slot not found
+ else return 0; // Breakpoint is already removed
+ }
+
+ struct code_hw_breakpoint* brk = &code_breaks[id];
+
+ brk->addr = fpb_addr;
+
+ if(set) brk->type |= type;
+ else brk->type &= ~type;
+
+ memset(sl->q_buf, 0, 4);
+
+ if(brk->type == 0) {
+ #ifdef DEBUG
+ printf("clearing hw break %d\n", id);
+ #endif
+
+ stlink_write_mem32(sl, 0xe0002008 + id * 4, 4);
+ } else {
+ sl->q_buf[0] = ( brk->addr & 0xff) | 1;
+ sl->q_buf[1] = ((brk->addr >> 8) & 0xff);
+ sl->q_buf[2] = ((brk->addr >> 16) & 0xff);
+ sl->q_buf[3] = ((brk->addr >> 24) & 0xff) | (brk->type << 6);
+
+ #ifdef DEBUG
+ printf("setting hw break %d at %08x (%d)\n",
+ id, brk->addr, brk->type);
+ printf("reg %02x %02x %02x %02x\n",
+ sl->q_buf[3], sl->q_buf[2], sl->q_buf[1], sl->q_buf[0]);
+ #endif
+
+ stlink_write_mem32(sl, 0xe0002008 + id * 4, 4);
+ }
+
+ return 0;
+}
+
+
+struct flash_block {
+ stm32_addr_t addr;
+ unsigned length;
+ uint8_t* data;
+
+ struct flash_block* next;
+};
+
+static struct flash_block* flash_root;
+
+static int flash_add_block(stm32_addr_t addr, unsigned length,
+ struct stlink* sl) {
+ if(addr < FLASH_BASE || addr + length > FLASH_BASE + FLASH_SIZE) {
+ fprintf(stderr, "flash_add_block: incorrect bounds\n");
+ return -1;
+ }
+
+ if(addr % FLASH_PAGE != 0 || length % FLASH_PAGE != 0) {
+ fprintf(stderr, "flash_add_block: unaligned block\n");
+ return -1;
+ }
+
+ struct flash_block* new = malloc(sizeof(struct flash_block));
+ new->next = flash_root;
+
+ new->addr = addr;
+ new->length = length;
+ new->data = calloc(length, 1);
+
+ flash_root = new;
+
+ return 0;
+}
+
+static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) {
+ int fit_blocks = 0, fit_length = 0;
+
+ for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
+ /* Block: ------X------Y--------
+ * Data: a-----b
+ * a--b
+ * a-----------b
+ * Block intersects with data, if:
+ * a < Y && b > x
+ */
+
+ unsigned X = fb->addr, Y = fb->addr + fb->length;
+ unsigned a = addr, b = addr + length;
+ if(a < Y && b > X) {
+ // from start of the block
+ unsigned start = (a > X ? a : X) - X;
+ unsigned end = (b > Y ? Y : b) - X;
+
+ memcpy(fb->data + start, data, end - start);
+
+ fit_blocks++;
+ fit_length += end - start;
+ }
+ }
+
+ if(fit_blocks == 0) {
+ fprintf(stderr, "Unfit data block %08x -> %04x\n", addr, length);
+ return -1;
+ }
+
+ if(fit_length != length) {
+ fprintf(stderr, "warning: data block %08x -> %04x truncated to %04x\n",
+ addr, length, fit_length);
+ fprintf(stderr, "(this is not an error, just a GDB glitch)\n");
+ }
+
+ return 0;
+}
+
+static int flash_go(struct stlink* sl) {
+ int error = -1;
+
+ // Some kinds of clock settings do not allow writing to flash.
+ stlink_reset(sl);
+
+ for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
+ #ifdef DEBUG
+ printf("flash_do: block %08x -> %04x\n", fb->addr, fb->length);
+ #endif
+
+ unsigned length = fb->length;
+ for(stm32_addr_t page = fb->addr; page < fb->addr + fb->length; page += FLASH_PAGE) {
+ #ifdef DEBUG
+ printf("flash_do: page %08x\n", page);
+ #endif
+
+ stlink_erase_flash_page(sl, page);
+
+ if(stlink_write_flash(sl, page, fb->data + (page - fb->addr),
+ length > FLASH_PAGE ? FLASH_PAGE : length) < 0)
+ goto error;
+ }
+
+ }
+
+ stlink_reset(sl);
+
+ error = 0;
+
+error:
+ for(struct flash_block* fb = flash_root, *next; fb; fb = next) {
+ next = fb->next;
+ free(fb->data);
+ free(fb);
+ }
+
+ flash_root = NULL;
+
+ return error;
+}
+
+int serve(struct stlink* sl, int port) {
+ int sock = socket(AF_INET, SOCK_STREAM, 0);
+ if(sock < 0) {
+ perror("socket");
+ return 1;
+ }
+
+ unsigned int val = 1;
+ setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
+
+ struct sockaddr_in serv_addr = {0};
+ serv_addr.sin_family = AF_INET;
+ serv_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
+ serv_addr.sin_port = htons(port);
+
+ if(bind(sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
+ perror("bind");
+ return 1;
+ }
+
+ if(listen(sock, 5) < 0) {
+ perror("listen");
+ return 1;
+ }
+
+ stlink_force_debug(sl);
+ stlink_reset(sl);
+ init_code_breakpoints(sl);
+ init_data_watchpoints(sl);
+
+ printf("Listening at *:%d...\n", port);
+
+ int client = accept(sock, NULL, NULL);
+ if(client < 0) {
+ perror("accept");
+ return 1;
+ }
+
+ close(sock);
+
+ printf("GDB connected.\n");
+
+ /*
+ * To allow resetting the chip from GDB it is required to
+ * emulate attaching and detaching to target.
+ */
+ unsigned int attached = 1;
+
+ while(1) {
+ char* packet;
+
+ int status = gdb_recv_packet(client, &packet);
+ if(status < 0) {
+ fprintf(stderr, "cannot recv: %d\n", status);
+ return 1;
+ }
+
+ #ifdef DEBUG
+ printf("recv: %s\n", packet);
+ #endif
+
+ char* reply = NULL;
+
+ switch(packet[0]) {
+ case 'q': {
+ if(packet[1] == 'P' || packet[1] == 'C' || packet[1] == 'L') {
+ reply = strdup("");
+ break;
+ }
+
+ char *separator = strstr(packet, ":"), *params = "";
+ if(separator == NULL) {
+ separator = packet + strlen(packet);
+ } else {
+ params = separator + 1;
+ }
+
+ unsigned queryNameLength = (separator - &packet[1]);
+ char* queryName = calloc(queryNameLength + 1, 1);
+ strncpy(queryName, &packet[1], queryNameLength);
+
+ #ifdef DEBUG
+ printf("query: %s;%s\n", queryName, params);
+ #endif
+
+ if(!strcmp(queryName, "Supported")) {
+ reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
+ } else if(!strcmp(queryName, "Xfer")) {
+ char *type, *op, *annex, *s_addr, *s_length;
+ char *tok = params;
+
+ type = strsep(&tok, ":");
+ op = strsep(&tok, ":");
+ annex = strsep(&tok, ":");
+ s_addr = strsep(&tok, ",");
+ s_length = tok;
+
+ unsigned addr = strtoul(s_addr, NULL, 16),
+ length = strtoul(s_length, NULL, 16);
+
+ #ifdef DEBUG
+ printf("Xfer: type:%s;op:%s;annex:%s;addr:%d;length:%d\n",
+ type, op, annex, addr, length);
+ #endif
+
+ const char* data = NULL;
+
+ if(!strcmp(type, "memory-map") && !strcmp(op, "read"))
+ data = current_memory_map;
+
+ if(data) {
+ unsigned data_length = strlen(data);
+ if(addr + length > data_length)
+ length = data_length - addr;
+
+ if(length == 0) {
+ reply = strdup("l");
+ } else {
+ reply = calloc(length + 2, 1);
+ reply[0] = 'm';
+ strncpy(&reply[1], data, length);
+ }
+ }
+ }
+
+ if(reply == NULL)
+ reply = strdup("");
+
+ free(queryName);
+
+ break;
+ }
+
+ case 'v': {
+ char *params = NULL;
+ char *cmdName = strtok_r(packet, ":;", ¶ms);
+
+ cmdName++; // vCommand -> Command
+
+ if(!strcmp(cmdName, "FlashErase")) {
+ char *s_addr, *s_length;
+ char *tok = params;
+
+ s_addr = strsep(&tok, ",");
+ s_length = tok;
+
+ unsigned addr = strtoul(s_addr, NULL, 16),
+ length = strtoul(s_length, NULL, 16);
+
+ #ifdef DEBUG
+ printf("FlashErase: addr:%08x,len:%04x\n",
+ addr, length);
+ #endif
+
+ if(flash_add_block(addr, length, sl) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ } else if(!strcmp(cmdName, "FlashWrite")) {
+ char *s_addr, *data;
+ char *tok = params;
+
+ s_addr = strsep(&tok, ":");
+ data = tok;
+
+ unsigned addr = strtoul(s_addr, NULL, 16);
+ unsigned data_length = status - (data - packet);
+
+ // Length of decoded data cannot be more than
+ // encoded, as escapes are removed.
+ // Additional byte is reserved for alignment fix.
+ uint8_t *decoded = calloc(data_length + 1, 1);
+ unsigned dec_index = 0;
+ for(int i = 0; i < data_length; i++) {
+ if(data[i] == 0x7d) {
+ i++;
+ decoded[dec_index++] = data[i] ^ 0x20;
+ } else {
+ decoded[dec_index++] = data[i];
+ }
+ }
+
+ // Fix alignment
+ if(dec_index % 2 != 0)
+ dec_index++;
+
+ #ifdef DEBUG
+ printf("binary packet %d -> %d\n", data_length, dec_index);
+ #endif
+
+ if(flash_populate(addr, decoded, dec_index) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ } else if(!strcmp(cmdName, "FlashDone")) {
+ if(flash_go(sl) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ } else if(!strcmp(cmdName, "Kill")) {
+ attached = 0;
+
+ reply = strdup("OK");
+ }
+
+ if(reply == NULL)
+ reply = strdup("");
+
+ break;
+ }
+
+ case 'c':
+ stlink_run(sl);
+
+ while(1) {
+ int status = gdb_check_for_interrupt(client);
+ if(status < 0) {
+ fprintf(stderr, "cannot check for int: %d\n", status);
+ return 1;
+ }
+
+ if(status == 1) {
+ stlink_force_debug(sl);
+ break;
+ }
+
+ stlink_status(sl);
+ if(sl->core_stat == STLINK_CORE_HALTED) {
+ break;
+ }
+
+ usleep(100000);
+ }
+
+ reply = strdup("S05"); // TRAP
+ break;
+
+ case 's':
+ stlink_step(sl);
+
+ reply = strdup("S05"); // TRAP
+ break;
+
+ case '?':
+ if(attached) {
+ reply = strdup("S05"); // TRAP
+ } else {
+ /* Stub shall reply OK if not attached. */
+ reply = strdup("OK");
+ }
+ break;
+
+ case 'g':
+ stlink_read_all_regs(sl);
+
+ reply = calloc(8 * 16 + 1, 1);
+ for(int i = 0; i < 16; i++)
+ sprintf(&reply[i * 8], "%08x", htonl(sl->reg.r[i]));
+
+ break;
+
+ case 'p': {
+ unsigned id = strtoul(&packet[1], NULL, 16), reg = 0xDEADDEAD;
+
+ if(id < 16) {
+ stlink_read_reg(sl, id);
+ reg = htonl(sl->reg.r[id]);
+ } else if(id == 0x19) {
+ stlink_read_reg(sl, 16);
+ reg = htonl(sl->reg.xpsr);
+ } else {
+ reply = strdup("E00");
+ }
+
+ reply = calloc(8 + 1, 1);
+ sprintf(reply, "%08x", reg);
+
+ break;
+ }
+
+ case 'P': {
+ char* s_reg = &packet[1];
+ char* s_value = strstr(&packet[1], "=") + 1;
+
+ unsigned reg = strtoul(s_reg, NULL, 16);
+ unsigned value = strtoul(s_value, NULL, 16);
+
+ if(reg < 16) {
+ stlink_write_reg(sl, ntohl(value), reg);
+ } else if(reg == 0x19) {
+ stlink_write_reg(sl, ntohl(value), 16);
+ } else {
+ reply = strdup("E00");
+ }
+
+ if(!reply) {
+ reply = strdup("OK");
+ }
+
+ break;
+ }
+
+ case 'G':
+ for(int i = 0; i < 16; i++) {
+ char str[9] = {0};
+ strncpy(str, &packet[1 + i * 8], 8);
+ uint32_t reg = strtoul(str, NULL, 16);
+ stlink_write_reg(sl, ntohl(reg), i);
+ }
+
+ reply = strdup("OK");
+ break;
+
+ case 'm': {
+ char* s_start = &packet[1];
+ char* s_count = strstr(&packet[1], ",") + 1;
+
+ stm32_addr_t start = strtoul(s_start, NULL, 16);
+ unsigned count = strtoul(s_count, NULL, 16);
+
+ unsigned adj_start = start % 4;
+
+ stlink_read_mem32(sl, start - adj_start, (count % 4 == 0) ?
+ count : count + 4 - (count % 4));
+
+ reply = calloc(count * 2 + 1, 1);
+ for(int i = 0; i < count; i++) {
+ reply[i * 2 + 0] = hex[sl->q_buf[i + adj_start] >> 4];
+ reply[i * 2 + 1] = hex[sl->q_buf[i + adj_start] & 0xf];
+ }
+
+ break;
+ }
+
+ case 'M': {
+ char* s_start = &packet[1];
+ char* s_count = strstr(&packet[1], ",") + 1;
+ char* hexdata = strstr(packet, ":") + 1;
+
+ stm32_addr_t start = strtoul(s_start, NULL, 16);
+ unsigned count = strtoul(s_count, NULL, 16);
+
+ for(int i = 0; i < count; i ++) {
+ char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
+ uint8_t byte = strtoul(hex, NULL, 16);
+ sl->q_buf[i] = byte;
+ }
+
+ if((count % 4) == 0 && (start % 4) == 0) {
+ stlink_write_mem32(sl, start, count);
+ } else {
+ stlink_write_mem8(sl, start, count);
+ }
+
+ reply = strdup("OK");
+
+ break;
+ }
+
+ case 'Z': {
+ char *endptr;
+ stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
+ stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
+
+ switch (packet[1]) {
+ case '1':
+ if(update_code_breakpoint(sl, addr, 1) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ break;
+
+ case '2': // insert write watchpoint
+ case '3': // insert read watchpoint
+ case '4': // insert access watchpoint
+ {
+ enum watchfun wf;
+ if(packet[1] == '2') {
+ wf = WATCHWRITE;
+ } else if(packet[1] == '3') {
+ wf = WATCHREAD;
+ } else {
+ wf = WATCHACCESS;
+ if(add_data_watchpoint(sl, wf, addr, len) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ break;
+ }
+ }
+ }
+
+ default:
+ reply = strdup("");
+ }
+ break;
+ }
+ case 'z': {
+ char *endptr;
+ stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
+ stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
+
+ switch (packet[1]) {
+ case '1': // remove breakpoint
+ update_code_breakpoint(sl, addr, 0);
+ reply = strdup("OK");
+ break;
+
+ case '2' : // remove write watchpoint
+ case '3' : // remove read watchpoint
+ case '4' : // remove access watchpoint
+ if(delete_data_watchpoint(sl, addr) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ break;
+ }
+
+ default:
+ reply = strdup("");
+ }
+ break;
+ }
+
+ case '!': {
+ /*
+ * Enter extended mode which allows restarting.
+ * We do support that always.
+ */
+
+ reply = strdup("OK");
+
+ break;
+ }
+
+ case 'R': {
+ /* Reset the core. */
+
+ stlink_reset(sl);
+ init_code_breakpoints(sl);
+ init_data_watchpoints(sl);
+
+ attached = 1;
+
+ reply = strdup("OK");
+
+ break;
+ }
+
+ default:
+ reply = strdup("");
+ }
+
+ if(reply) {
+ #ifdef DEBUG
+ printf("send: %s\n", reply);
+ #endif
+
+ int result = gdb_send_packet(client, reply);
+ if(result != 0) {
+ fprintf(stderr, "cannot send: %d\n", result);
+ return 1;
+ }
+
+ free(reply);
+ }
+
+ free(packet);
+ }
+
+ return 0;
+}
+++ /dev/null
-/* -*- tab-width:8 -*- */
-
-/*
- Copyright (C) 2011 Peter Zotov <whitequark@whitequark.org>
- Use of this source code is governed by a BSD-style
- license that can be found in the LICENSE file.
-*/
-
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <sys/poll.h>
-
-static const char hex[] = "0123456789abcdef";
-
-int gdb_send_packet(int fd, char* data) {
- unsigned length = strlen(data) + 5;
- char* packet = malloc(length); /* '$' data (hex) '#' cksum (hex) */
-
- memset(packet, 0, length);
-
- packet[0] = '$';
-
- uint8_t cksum = 0;
- for(int i = 0; i < strlen(data); i++) {
- packet[i + 1] = data[i];
- cksum += data[i];
- }
-
- packet[length - 4] = '#';
- packet[length - 3] = hex[cksum >> 4];
- packet[length - 2] = hex[cksum & 0xf];
-
- while(1) {
- if(write(fd, packet, length) != length) {
- free(packet);
- return -2;
- }
-
- char ack;
- if(read(fd, &ack, 1) != 1) {
- free(packet);
- return -2;
- }
-
- if(ack == '+') {
- free(packet);
- return 0;
- }
- }
-}
-
-#define ALLOC_STEP 1024
-
-int gdb_recv_packet(int fd, char** buffer) {
- unsigned packet_size = ALLOC_STEP + 1, packet_idx = 0;
- uint8_t cksum = 0;
- char recv_cksum[3] = {0};
- char* packet_buffer = malloc(packet_size);
- unsigned state;
-
-start:
- state = 0;
- /*
- * 0: waiting $
- * 1: data, waiting #
- * 2: cksum 1
- * 3: cksum 2
- * 4: fin
- */
-
- char c;
- while(state != 4) {
- if(read(fd, &c, 1) != 1) {
- return -2;
- }
-
- switch(state) {
- case 0:
- if(c != '$') {
- // ignore
- } else {
- state = 1;
- }
- break;
-
- case 1:
- if(c == '#') {
- state = 2;
- } else {
- packet_buffer[packet_idx++] = c;
- cksum += c;
-
- if(packet_idx == packet_size) {
- packet_size += ALLOC_STEP;
- packet_buffer = realloc(packet_buffer, packet_size);
- }
- }
- break;
-
- case 2:
- recv_cksum[0] = c;
- state = 3;
- break;
-
- case 3:
- recv_cksum[1] = c;
- state = 4;
- break;
- }
- }
-
- uint8_t recv_cksum_int = strtoul(recv_cksum, NULL, 16);
- if(recv_cksum_int != cksum) {
- char nack = '-';
- if(write(fd, &nack, 1) != 1) {
- return -2;
- }
-
- goto start;
- } else {
- char ack = '+';
- if(write(fd, &ack, 1) != 1) {
- return -2;
- }
- }
-
- packet_buffer[packet_idx] = 0;
- *buffer = packet_buffer;
-
- return packet_idx;
-}
-
-// Here we skip any characters which are not \x03, GDB interrupt.
-// As we use the mode with ACK, in a (very unlikely) situation of a packet
-// lost because of this skipping, it will be resent anyway.
-int gdb_check_for_interrupt(int fd) {
- struct pollfd pfd;
- pfd.fd = fd;
- pfd.events = POLLIN;
-
- if(poll(&pfd, 1, 0) != 0) {
- char c;
-
- if(read(fd, &c, 1) != 1)
- return -2;
-
- if(c == '\x03') // ^C
- return 1;
- }
-
- return 0;
-}
-
+++ /dev/null
-#ifndef _GDB_REMOTE_H_
-#define _GDB_REMOTE_H_
-
-int gdb_send_packet(int fd, char* data);
-int gdb_recv_packet(int fd, char** buffer);
-int gdb_check_for_interrupt(int fd);
-
-#endif
+++ /dev/null
-/* -*- tab-width:8 -*- */
-
-/*
- Copyright (C) 2011 Peter Zotov <whitequark@whitequark.org>
- Use of this source code is governed by a BSD-style
- license that can be found in the LICENSE file.
-*/
-
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-#include <arpa/inet.h>
-#include "gdb-remote.h"
-#include "stlink-hw.h"
-
-#define FLASH_BASE 0x08000000
-#define FLASH_PAGE (sl->flash_pgsz)
-#define FLASH_PAGE_MASK (~((1 << 10) - 1))
-#define FLASH_SIZE (FLASH_PAGE * 128)
-
-static const char hex[] = "0123456789abcdef";
-
-static const char* current_memory_map = NULL;
-
-struct chip_params {
- uint32_t chip_id;
- char* description;
- uint32_t max_flash_size, flash_pagesize;
- uint32_t sram_size;
- uint32_t bootrom_base, bootrom_size;
-} const devices[] = {
- { 0x412, "Low-density device",
- 0x8000, 0x400, 0x2800, 0x1ffff000, 0x800 },
- { 0x410, "Medium-density device",
- 0x20000, 0x400, 0x5000, 0x1ffff000, 0x800 },
- { 0x414, "High-density device",
- 0x80000, 0x800, 0x10000, 0x1ffff000, 0x800 },
- { 0x418, "Connectivity line device",
- 0x40000, 0x800, 0x10000, 0x1fffb000, 0x4800 },
- { 0x420, "Medium-density value line device",
- 0x20000, 0x400, 0x2000, 0x1ffff000, 0x800 },
- { 0x428, "High-density value line device",
- 0x80000, 0x800, 0x8000, 0x1ffff000, 0x800 },
- { 0x430, "XL-density device",
- 0x100000, 0x800, 0x18000, 0x1fffe000, 0x1800 },
- { 0 }
-};
-
-int serve(struct stlink* sl, int port);
-char* make_memory_map(const struct chip_params *params, uint32_t flash_size);
-
-int main(int argc, char** argv) {
- if(argc != 3) {
- fprintf(stderr, "Usage: %s <port> /dev/sgX\n", argv[0]);
- return 1;
- }
-
- struct stlink *sl = stlink_quirk_open(argv[2], 0);
- if (sl == NULL)
- return 1;
-
- if(stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE)
- stlink_enter_swd_mode(sl);
-
- uint32_t chip_id;
-
- stlink_read_mem32(sl, 0xE0042000, 4);
- chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) |
- (sl->q_buf[3] << 24);
-
- printf("Chip ID is %08x.\n", chip_id);
-
- const struct chip_params* params = NULL;
-
- for(int i = 0; i < sizeof(devices) / sizeof(devices[0]); i++) {
- if(devices[i].chip_id == (chip_id & 0xFFF)) {
- params = &devices[i];
- break;
- }
- }
-
- if(params == NULL) {
- fprintf(stderr, "Cannot recognize the connected device!\n");
- return 0;
- }
-
- printf("Device connected: %s\n", params->description);
- printf("Device parameters: SRAM: 0x%x bytes, Flash: up to 0x%x bytes in pages of 0x%x bytes\n",
- params->sram_size, params->max_flash_size, params->flash_pagesize);
-
- FLASH_PAGE = params->flash_pagesize;
-
- uint32_t flash_size;
-
- stlink_read_mem32(sl, 0x1FFFF7E0, 4);
- flash_size = sl->q_buf[0] | (sl->q_buf[1] << 8);
-
- printf("Flash size is %d KiB.\n", flash_size);
- // memory map is in 1k blocks.
- current_memory_map = make_memory_map(params, flash_size * 0x400);
-
- int port = atoi(argv[1]);
-
- while(serve(sl, port) == 0);
-
- stlink_close(sl);
-
- return 0;
-}
-
-static const char* const memory_map_template =
- "<?xml version=\"1.0\"?>"
- "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
- " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
- "<memory-map>"
- " <memory type=\"rom\" start=\"0x00000000\" length=\"0x%x\"/>" // code = sram, bootrom or flash; flash is bigger
- " <memory type=\"ram\" start=\"0x20000000\" length=\"0x%x\"/>" // sram 8k
- " <memory type=\"flash\" start=\"0x08000000\" length=\"0x%x\">"
- " <property name=\"blocksize\">0x%x</property>"
- " </memory>"
- " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
- " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
- " <memory type=\"rom\" start=\"0x%08x\" length=\"0x%x\"/>" // bootrom
- " <memory type=\"rom\" start=\"0x1ffff800\" length=\"0x8\"/>" // option byte area
- "</memory-map>";
-
-char* make_memory_map(const struct chip_params *params, uint32_t flash_size) {
- /* This will be freed in serve() */
- char* map = malloc(4096);
- map[0] = '\0';
-
- snprintf(map, 4096, memory_map_template,
- flash_size,
- params->sram_size,
- flash_size, params->flash_pagesize,
- params->bootrom_base, params->bootrom_size);
-
- return map;
-}
-
-
-/*
- * DWT_COMP0 0xE0001020
- * DWT_MASK0 0xE0001024
- * DWT_FUNCTION0 0xE0001028
- * DWT_COMP1 0xE0001030
- * DWT_MASK1 0xE0001034
- * DWT_FUNCTION1 0xE0001038
- * DWT_COMP2 0xE0001040
- * DWT_MASK2 0xE0001044
- * DWT_FUNCTION2 0xE0001048
- * DWT_COMP3 0xE0001050
- * DWT_MASK3 0xE0001054
- * DWT_FUNCTION3 0xE0001058
- */
-
-#define DATA_WATCH_NUM 4
-
-enum watchfun { WATCHDISABLED = 0, WATCHREAD = 5, WATCHWRITE = 6, WATCHACCESS = 7 };
-
-struct code_hw_watchpoint {
- stm32_addr_t addr;
- uint8_t mask;
- enum watchfun fun;
-};
-
-struct code_hw_watchpoint data_watches[DATA_WATCH_NUM];
-
-static void init_data_watchpoints(struct stlink *sl) {
- int i;
-
- #ifdef DEBUG
- printf("init watchpoints\n");
- #endif
-
- // set trcena in debug command to turn on dwt unit
- stlink_read_mem32(sl, 0xE000EDFC, 4);
- sl->q_buf[3] |= 1;
- stlink_write_mem32(sl, 0xE000EDFC, 4);
-
- // make sure all watchpoints are cleared
- memset(sl->q_buf, 0, 4);
- for(int i = 0; i < DATA_WATCH_NUM; i++) {
- data_watches[i].fun = WATCHDISABLED;
- stlink_write_mem32(sl, 0xe0001028 + i * 16, 4);
- }
-}
-
-static int add_data_watchpoint(struct stlink* sl, enum watchfun wf, stm32_addr_t addr, unsigned int len)
-{
- int i = 0;
- uint32_t mask, temp;
-
- // computer mask
- // find a free watchpoint
- // configure
-
- mask = -1;
- i = len;
- while(i) {
- i >>= 1;
- mask++;
- }
-
- if((mask != -1) && (mask < 16)) {
- for(i = 0; i < DATA_WATCH_NUM; i++) {
- // is this an empty slot ?
- if(data_watches[i].fun == WATCHDISABLED) {
- #ifdef DEBUG
- printf("insert watchpoint %d addr %x wf %u mask %u len %d\n", i, addr, wf, mask, len);
- #endif
-
- data_watches[i].fun = wf;
- data_watches[i].addr = addr;
- data_watches[i].mask = mask;
-
- // insert comparator address
- sl->q_buf[0] = (addr & 0xff);
- sl->q_buf[1] = ((addr >> 8) & 0xff);
- sl->q_buf[2] = ((addr >> 16) & 0xff);
- sl->q_buf[3] = ((addr >> 24) & 0xff);
-
- stlink_write_mem32(sl, 0xE0001020 + i * 16, 4);
-
- // insert mask
- memset(sl->q_buf, 0, 4);
- sl->q_buf[0] = mask;
- stlink_write_mem32(sl, 0xE0001024 + i * 16, 4);
-
- // insert function
- memset(sl->q_buf, 0, 4);
- sl->q_buf[0] = wf;
- stlink_write_mem32(sl, 0xE0001028 + i * 16, 4);
-
- // just to make sure the matched bit is clear !
- stlink_read_mem32(sl, 0xE0001028 + i * 16, 4);
- return 0;
- }
- }
- }
-
- #ifdef DEBUG
- printf("failure: add watchpoints addr %x wf %u len %u\n", addr, wf, len);
- #endif
- return -1;
-}
-
-static int delete_data_watchpoint(struct stlink* sl, stm32_addr_t addr)
-{
- int i;
-
- for(i = 0 ; i < DATA_WATCH_NUM; i++) {
- if((data_watches[i].addr == addr) && (data_watches[i].fun != WATCHDISABLED)) {
- #ifdef DEBUG
- printf("delete watchpoint %d addr %x\n", i, addr);
- #endif
-
- memset(sl->q_buf, 0, 4);
- data_watches[i].fun = WATCHDISABLED;
- stlink_write_mem32(sl, 0xe0001028 + i * 16, 4);
-
- return 0;
- }
- }
-
- #ifdef DEBUG
- printf("failure: delete watchpoint addr %x\n", addr);
- #endif
-
- return -1;
-}
-
-#define CODE_BREAK_NUM 6
-#define CODE_BREAK_LOW 0x01
-#define CODE_BREAK_HIGH 0x02
-
-struct code_hw_breakpoint {
- stm32_addr_t addr;
- int type;
-};
-
-struct code_hw_breakpoint code_breaks[CODE_BREAK_NUM];
-
-static void init_code_breakpoints(struct stlink* sl) {
- memset(sl->q_buf, 0, 4);
- sl->q_buf[0] = 0x03; // KEY | ENABLE
- stlink_write_mem32(sl, 0xe0002000, 4);
-
- memset(sl->q_buf, 0, 4);
- for(int i = 0; i < CODE_BREAK_NUM; i++) {
- code_breaks[i].type = 0;
- stlink_write_mem32(sl, 0xe0002008 + i * 4, 4);
- }
-}
-
-static int update_code_breakpoint(struct stlink* sl, stm32_addr_t addr, int set) {
- stm32_addr_t fpb_addr = addr & ~0x3;
- int type = addr & 0x2 ? CODE_BREAK_HIGH : CODE_BREAK_LOW;
-
- if(addr & 1) {
- fprintf(stderr, "update_code_breakpoint: unaligned address %08x\n", addr);
- return -1;
- }
-
- int id = -1;
- for(int i = 0; i < CODE_BREAK_NUM; i++) {
- if(fpb_addr == code_breaks[i].addr ||
- (set && code_breaks[i].type == 0)) {
- id = i;
- break;
- }
- }
-
- if(id == -1) {
- if(set) return -1; // Free slot not found
- else return 0; // Breakpoint is already removed
- }
-
- struct code_hw_breakpoint* brk = &code_breaks[id];
-
- brk->addr = fpb_addr;
-
- if(set) brk->type |= type;
- else brk->type &= ~type;
-
- memset(sl->q_buf, 0, 4);
-
- if(brk->type == 0) {
- #ifdef DEBUG
- printf("clearing hw break %d\n", id);
- #endif
-
- stlink_write_mem32(sl, 0xe0002008 + id * 4, 4);
- } else {
- sl->q_buf[0] = ( brk->addr & 0xff) | 1;
- sl->q_buf[1] = ((brk->addr >> 8) & 0xff);
- sl->q_buf[2] = ((brk->addr >> 16) & 0xff);
- sl->q_buf[3] = ((brk->addr >> 24) & 0xff) | (brk->type << 6);
-
- #ifdef DEBUG
- printf("setting hw break %d at %08x (%d)\n",
- id, brk->addr, brk->type);
- printf("reg %02x %02x %02x %02x\n",
- sl->q_buf[3], sl->q_buf[2], sl->q_buf[1], sl->q_buf[0]);
- #endif
-
- stlink_write_mem32(sl, 0xe0002008 + id * 4, 4);
- }
-
- return 0;
-}
-
-
-struct flash_block {
- stm32_addr_t addr;
- unsigned length;
- uint8_t* data;
-
- struct flash_block* next;
-};
-
-static struct flash_block* flash_root;
-
-static int flash_add_block(stm32_addr_t addr, unsigned length,
- struct stlink* sl) {
- if(addr < FLASH_BASE || addr + length > FLASH_BASE + FLASH_SIZE) {
- fprintf(stderr, "flash_add_block: incorrect bounds\n");
- return -1;
- }
-
- if(addr % FLASH_PAGE != 0 || length % FLASH_PAGE != 0) {
- fprintf(stderr, "flash_add_block: unaligned block\n");
- return -1;
- }
-
- struct flash_block* new = malloc(sizeof(struct flash_block));
- new->next = flash_root;
-
- new->addr = addr;
- new->length = length;
- new->data = calloc(length, 1);
-
- flash_root = new;
-
- return 0;
-}
-
-static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) {
- int fit_blocks = 0, fit_length = 0;
-
- for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
- /* Block: ------X------Y--------
- * Data: a-----b
- * a--b
- * a-----------b
- * Block intersects with data, if:
- * a < Y && b > x
- */
-
- unsigned X = fb->addr, Y = fb->addr + fb->length;
- unsigned a = addr, b = addr + length;
- if(a < Y && b > X) {
- // from start of the block
- unsigned start = (a > X ? a : X) - X;
- unsigned end = (b > Y ? Y : b) - X;
-
- memcpy(fb->data + start, data, end - start);
-
- fit_blocks++;
- fit_length += end - start;
- }
- }
-
- if(fit_blocks == 0) {
- fprintf(stderr, "Unfit data block %08x -> %04x\n", addr, length);
- return -1;
- }
-
- if(fit_length != length) {
- fprintf(stderr, "warning: data block %08x -> %04x truncated to %04x\n",
- addr, length, fit_length);
- fprintf(stderr, "(this is not an error, just a GDB glitch)\n");
- }
-
- return 0;
-}
-
-static int flash_go(struct stlink* sl) {
- int error = -1;
-
- // Some kinds of clock settings do not allow writing to flash.
- stlink_reset(sl);
-
- for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
- #ifdef DEBUG
- printf("flash_do: block %08x -> %04x\n", fb->addr, fb->length);
- #endif
-
- unsigned length = fb->length;
- for(stm32_addr_t page = fb->addr; page < fb->addr + fb->length; page += FLASH_PAGE) {
- #ifdef DEBUG
- printf("flash_do: page %08x\n", page);
- #endif
-
- stlink_erase_flash_page(sl, page);
-
- if(stlink_write_flash(sl, page, fb->data + (page - fb->addr),
- length > FLASH_PAGE ? FLASH_PAGE : length) < 0)
- goto error;
- }
-
- }
-
- stlink_reset(sl);
-
- error = 0;
-
-error:
- for(struct flash_block* fb = flash_root, *next; fb; fb = next) {
- next = fb->next;
- free(fb->data);
- free(fb);
- }
-
- flash_root = NULL;
-
- return error;
-}
-
-int serve(struct stlink* sl, int port) {
- int sock = socket(AF_INET, SOCK_STREAM, 0);
- if(sock < 0) {
- perror("socket");
- return 1;
- }
-
- unsigned int val = 1;
- setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
-
- struct sockaddr_in serv_addr = {0};
- serv_addr.sin_family = AF_INET;
- serv_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
- serv_addr.sin_port = htons(port);
-
- if(bind(sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
- perror("bind");
- return 1;
- }
-
- if(listen(sock, 5) < 0) {
- perror("listen");
- return 1;
- }
-
- stlink_force_debug(sl);
- stlink_reset(sl);
- init_code_breakpoints(sl);
- init_data_watchpoints(sl);
-
- printf("Listening at *:%d...\n", port);
-
- int client = accept(sock, NULL, NULL);
- if(client < 0) {
- perror("accept");
- return 1;
- }
-
- close(sock);
-
- printf("GDB connected.\n");
-
- /*
- * To allow resetting the chip from GDB it is required to
- * emulate attaching and detaching to target.
- */
- unsigned int attached = 1;
-
- while(1) {
- char* packet;
-
- int status = gdb_recv_packet(client, &packet);
- if(status < 0) {
- fprintf(stderr, "cannot recv: %d\n", status);
- return 1;
- }
-
- #ifdef DEBUG
- printf("recv: %s\n", packet);
- #endif
-
- char* reply = NULL;
-
- switch(packet[0]) {
- case 'q': {
- if(packet[1] == 'P' || packet[1] == 'C' || packet[1] == 'L') {
- reply = strdup("");
- break;
- }
-
- char *separator = strstr(packet, ":"), *params = "";
- if(separator == NULL) {
- separator = packet + strlen(packet);
- } else {
- params = separator + 1;
- }
-
- unsigned queryNameLength = (separator - &packet[1]);
- char* queryName = calloc(queryNameLength + 1, 1);
- strncpy(queryName, &packet[1], queryNameLength);
-
- #ifdef DEBUG
- printf("query: %s;%s\n", queryName, params);
- #endif
-
- if(!strcmp(queryName, "Supported")) {
- reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
- } else if(!strcmp(queryName, "Xfer")) {
- char *type, *op, *annex, *s_addr, *s_length;
- char *tok = params;
-
- type = strsep(&tok, ":");
- op = strsep(&tok, ":");
- annex = strsep(&tok, ":");
- s_addr = strsep(&tok, ",");
- s_length = tok;
-
- unsigned addr = strtoul(s_addr, NULL, 16),
- length = strtoul(s_length, NULL, 16);
-
- #ifdef DEBUG
- printf("Xfer: type:%s;op:%s;annex:%s;addr:%d;length:%d\n",
- type, op, annex, addr, length);
- #endif
-
- const char* data = NULL;
-
- if(!strcmp(type, "memory-map") && !strcmp(op, "read"))
- data = current_memory_map;
-
- if(data) {
- unsigned data_length = strlen(data);
- if(addr + length > data_length)
- length = data_length - addr;
-
- if(length == 0) {
- reply = strdup("l");
- } else {
- reply = calloc(length + 2, 1);
- reply[0] = 'm';
- strncpy(&reply[1], data, length);
- }
- }
- }
-
- if(reply == NULL)
- reply = strdup("");
-
- free(queryName);
-
- break;
- }
-
- case 'v': {
- char *params = NULL;
- char *cmdName = strtok_r(packet, ":;", ¶ms);
-
- cmdName++; // vCommand -> Command
-
- if(!strcmp(cmdName, "FlashErase")) {
- char *s_addr, *s_length;
- char *tok = params;
-
- s_addr = strsep(&tok, ",");
- s_length = tok;
-
- unsigned addr = strtoul(s_addr, NULL, 16),
- length = strtoul(s_length, NULL, 16);
-
- #ifdef DEBUG
- printf("FlashErase: addr:%08x,len:%04x\n",
- addr, length);
- #endif
-
- if(flash_add_block(addr, length, sl) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- } else if(!strcmp(cmdName, "FlashWrite")) {
- char *s_addr, *data;
- char *tok = params;
-
- s_addr = strsep(&tok, ":");
- data = tok;
-
- unsigned addr = strtoul(s_addr, NULL, 16);
- unsigned data_length = status - (data - packet);
-
- // Length of decoded data cannot be more than
- // encoded, as escapes are removed.
- // Additional byte is reserved for alignment fix.
- uint8_t *decoded = calloc(data_length + 1, 1);
- unsigned dec_index = 0;
- for(int i = 0; i < data_length; i++) {
- if(data[i] == 0x7d) {
- i++;
- decoded[dec_index++] = data[i] ^ 0x20;
- } else {
- decoded[dec_index++] = data[i];
- }
- }
-
- // Fix alignment
- if(dec_index % 2 != 0)
- dec_index++;
-
- #ifdef DEBUG
- printf("binary packet %d -> %d\n", data_length, dec_index);
- #endif
-
- if(flash_populate(addr, decoded, dec_index) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- } else if(!strcmp(cmdName, "FlashDone")) {
- if(flash_go(sl) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- } else if(!strcmp(cmdName, "Kill")) {
- attached = 0;
-
- reply = strdup("OK");
- }
-
- if(reply == NULL)
- reply = strdup("");
-
- break;
- }
-
- case 'c':
- stlink_run(sl);
-
- while(1) {
- int status = gdb_check_for_interrupt(client);
- if(status < 0) {
- fprintf(stderr, "cannot check for int: %d\n", status);
- return 1;
- }
-
- if(status == 1) {
- stlink_force_debug(sl);
- break;
- }
-
- stlink_status(sl);
- if(sl->core_stat == STLINK_CORE_HALTED) {
- break;
- }
-
- usleep(100000);
- }
-
- reply = strdup("S05"); // TRAP
- break;
-
- case 's':
- stlink_step(sl);
-
- reply = strdup("S05"); // TRAP
- break;
-
- case '?':
- if(attached) {
- reply = strdup("S05"); // TRAP
- } else {
- /* Stub shall reply OK if not attached. */
- reply = strdup("OK");
- }
- break;
-
- case 'g':
- stlink_read_all_regs(sl);
-
- reply = calloc(8 * 16 + 1, 1);
- for(int i = 0; i < 16; i++)
- sprintf(&reply[i * 8], "%08x", htonl(sl->reg.r[i]));
-
- break;
-
- case 'p': {
- unsigned id = strtoul(&packet[1], NULL, 16), reg = 0xDEADDEAD;
-
- if(id < 16) {
- stlink_read_reg(sl, id);
- reg = htonl(sl->reg.r[id]);
- } else if(id == 0x19) {
- stlink_read_reg(sl, 16);
- reg = htonl(sl->reg.xpsr);
- } else {
- reply = strdup("E00");
- }
-
- reply = calloc(8 + 1, 1);
- sprintf(reply, "%08x", reg);
-
- break;
- }
-
- case 'P': {
- char* s_reg = &packet[1];
- char* s_value = strstr(&packet[1], "=") + 1;
-
- unsigned reg = strtoul(s_reg, NULL, 16);
- unsigned value = strtoul(s_value, NULL, 16);
-
- if(reg < 16) {
- stlink_write_reg(sl, ntohl(value), reg);
- } else if(reg == 0x19) {
- stlink_write_reg(sl, ntohl(value), 16);
- } else {
- reply = strdup("E00");
- }
-
- if(!reply) {
- reply = strdup("OK");
- }
-
- break;
- }
-
- case 'G':
- for(int i = 0; i < 16; i++) {
- char str[9] = {0};
- strncpy(str, &packet[1 + i * 8], 8);
- uint32_t reg = strtoul(str, NULL, 16);
- stlink_write_reg(sl, ntohl(reg), i);
- }
-
- reply = strdup("OK");
- break;
-
- case 'm': {
- char* s_start = &packet[1];
- char* s_count = strstr(&packet[1], ",") + 1;
-
- stm32_addr_t start = strtoul(s_start, NULL, 16);
- unsigned count = strtoul(s_count, NULL, 16);
-
- unsigned adj_start = start % 4;
-
- stlink_read_mem32(sl, start - adj_start, (count % 4 == 0) ?
- count : count + 4 - (count % 4));
-
- reply = calloc(count * 2 + 1, 1);
- for(int i = 0; i < count; i++) {
- reply[i * 2 + 0] = hex[sl->q_buf[i + adj_start] >> 4];
- reply[i * 2 + 1] = hex[sl->q_buf[i + adj_start] & 0xf];
- }
-
- break;
- }
-
- case 'M': {
- char* s_start = &packet[1];
- char* s_count = strstr(&packet[1], ",") + 1;
- char* hexdata = strstr(packet, ":") + 1;
-
- stm32_addr_t start = strtoul(s_start, NULL, 16);
- unsigned count = strtoul(s_count, NULL, 16);
-
- for(int i = 0; i < count; i ++) {
- char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
- uint8_t byte = strtoul(hex, NULL, 16);
- sl->q_buf[i] = byte;
- }
-
- if((count % 4) == 0 && (start % 4) == 0) {
- stlink_write_mem32(sl, start, count);
- } else {
- stlink_write_mem8(sl, start, count);
- }
-
- reply = strdup("OK");
-
- break;
- }
-
- case 'Z': {
- char *endptr;
- stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
- stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
-
- switch (packet[1]) {
- case '1':
- if(update_code_breakpoint(sl, addr, 1) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- break;
-
- case '2': // insert write watchpoint
- case '3': // insert read watchpoint
- case '4': // insert access watchpoint
- {
- enum watchfun wf;
- if(packet[1] == '2') {
- wf = WATCHWRITE;
- } else if(packet[1] == '3') {
- wf = WATCHREAD;
- } else {
- wf = WATCHACCESS;
- if(add_data_watchpoint(sl, wf, addr, len) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- break;
- }
- }
- }
-
- default:
- reply = strdup("");
- }
- break;
- }
- case 'z': {
- char *endptr;
- stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
- stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
-
- switch (packet[1]) {
- case '1': // remove breakpoint
- update_code_breakpoint(sl, addr, 0);
- reply = strdup("OK");
- break;
-
- case '2' : // remove write watchpoint
- case '3' : // remove read watchpoint
- case '4' : // remove access watchpoint
- if(delete_data_watchpoint(sl, addr) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- break;
- }
-
- default:
- reply = strdup("");
- }
- break;
- }
-
- case '!': {
- /*
- * Enter extended mode which allows restarting.
- * We do support that always.
- */
-
- reply = strdup("OK");
-
- break;
- }
-
- case 'R': {
- /* Reset the core. */
-
- stlink_reset(sl);
- init_code_breakpoints(sl);
- init_data_watchpoints(sl);
-
- attached = 1;
-
- reply = strdup("OK");
-
- break;
- }
-
- default:
- reply = strdup("");
- }
-
- if(reply) {
- #ifdef DEBUG
- printf("send: %s\n", reply);
- #endif
-
- int result = gdb_send_packet(client, reply);
- if(result != 0) {
- fprintf(stderr, "cannot send: %d\n", result);
- return 1;
- }
-
- free(reply);
- }
-
- free(packet);
- }
-
- return 0;
-}
--- /dev/null
+
+
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/mman.h>
+
+
+#include "stlink-common.h"
+
+void D(stlink_t *sl, char *txt) {
+ if (sl->verbose > 1)
+ fputs(txt, stderr);
+}
+
+void DD(stlink_t *sl, char *format, ...) {
+ if (sl->verbose > 0) {
+ va_list list;
+ va_start(list, format);
+ vfprintf(stderr, format, list);
+ va_end(list);
+ }
+}
+
+// Delegates to the backends...
+
+void stlink_close(stlink_t *sl) {
+ D(sl, "\n*** stlink_close ***\n");
+ sl->backend->close(sl);
+
+ free(sl);
+}
+
+void stlink_exit_debug_mode(stlink_t *sl) {
+ D(sl, "\n*** stlink_exit_debug_mode ***\n");
+ sl->backend->exit_debug_mode(sl);
+}
+
+void stlink_enter_swd_mode(stlink_t *sl) {
+ D(sl, "\n*** stlink_enter_swd_mode ***\n");
+ sl->backend->enter_swd_mode(sl);
+}
+
+void stlink_exit_dfu_mode(stlink_t *sl) {
+ D(sl, "\n*** stlink_exit_duf_mode ***\n");
+ sl->backend->exit_dfu_mode(sl);
+}
+
+void stlink_core_id(stlink_t *sl) {
+ D(sl, "\n*** stlink_core_id ***\n");
+ sl->backend->core_id(sl);
+ DD(sl, "core_id = 0x%08x\n", sl->core_id);
+}
+
+void stlink_reset(stlink_t *sl) {
+ D(sl, "\n*** stlink_reset ***\n");
+ sl->backend->reset(sl);
+
+}
+
+void stlink_run(stlink_t *sl) {
+ D(sl, "\n*** stlink_core_id ***\n");
+ sl->backend->run(sl);
+ DD(sl, "core_id = 0x%08x\n", sl->core_id);
+}
+
+void stlink_status(stlink_t *sl) {
+ D(sl, "\n*** stlink_core_id ***\n");
+ sl->backend->status(sl);
+ stlink_core_stat(sl);
+ DD(sl, "core_id = 0x%08x\n", sl->core_id);
+}
+
+void stlink_version(stlink_t *sl) {
+ D(sl, "*** looking up stlink version\n");
+ sl->backend->version(sl);
+}
+
+void stlink_write_mem32(stlink_t *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;
+ }
+ sl->backend->write_mem32(sl, addr, len);
+}
+
+void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
+ D(sl, "\n*** stlink_write_mem8 ***\n");
+ sl->backend->write_mem8(sl, addr, len);
+}
+
+
+
+
+
+
+// End of delegates.... Common code below here...
+
+// 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;
+}
+
+uint16_t read_uint16(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];
+ } else {
+ p[0] = c[pt + 1];
+ p[1] = c[pt];
+ }
+ return ui;
+}
+
+void stlink_core_stat(stlink_t *sl) {
+ if (sl->q_len <= 0)
+ return;
+
+ stlink_print_data(sl);
+
+ switch (sl->q_buf[0]) {
+ case STLINK_CORE_RUNNING:
+ sl->core_stat = STLINK_CORE_RUNNING;
+ DD(sl, " core status: running\n");
+ return;
+ case STLINK_CORE_HALTED:
+ sl->core_stat = STLINK_CORE_HALTED;
+ DD(sl, " core status: halted\n");
+ return;
+ default:
+ sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
+ fprintf(stderr, " core status: unknown\n");
+ }
+}
+
+void stlink_print_data(stlink_t *sl) {
+ 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 (int 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);
+}
+
+/* memory mapped file */
+
+typedef struct mapped_file {
+ uint8_t* base;
+ size_t len;
+} mapped_file_t;
+
+#define MAPPED_FILE_INITIALIZER { NULL, 0 }
+
+static int map_file(mapped_file_t* mf, const char* path) {
+ int error = -1;
+ struct stat st;
+
+ const int fd = open(path, O_RDONLY);
+ if (fd == -1) {
+ fprintf(stderr, "open(%s) == -1\n", path);
+ return -1;
+ }
+
+ if (fstat(fd, &st) == -1) {
+ fprintf(stderr, "fstat() == -1\n");
+ goto on_error;
+ }
+
+ mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
+ if (mf->base == MAP_FAILED) {
+ fprintf(stderr, "mmap() == MAP_FAILED\n");
+ goto on_error;
+ }
+
+ mf->len = st.st_size;
+
+ /* success */
+ error = 0;
+
+on_error:
+ close(fd);
+
+ return error;
+}
+
+static void unmap_file(mapped_file_t* mf) {
+ munmap((void*) mf->base, mf->len);
+ mf->base = (unsigned char*) MAP_FAILED;
+ mf->len = 0;
+}
+
+static int check_file
+(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
+ size_t off;
+
+ for (off = 0; off < mf->len; off += sl->flash_pgsz) {
+ size_t aligned_size;
+
+ /* adjust last page size */
+ size_t cmp_size = sl->flash_pgsz;
+ if ((off + sl->flash_pgsz) > mf->len)
+ cmp_size = mf->len - off;
+
+ aligned_size = cmp_size;
+ if (aligned_size & (4 - 1))
+ aligned_size = (cmp_size + 4) & ~(4 - 1);
+
+ stlink_read_mem32(sl, addr + off, aligned_size);
+
+ if (memcmp(sl->q_buf, mf->base + off, cmp_size))
+ return -1;
+ }
+
+ return 0;
+}
+
+int stlink_fwrite_sram
+(stlink_t * sl, const char* path, stm32_addr_t addr) {
+ /* write the file in sram at addr */
+
+ int error = -1;
+ size_t off;
+ mapped_file_t mf = MAPPED_FILE_INITIALIZER;
+
+ if (map_file(&mf, path) == -1) {
+ fprintf(stderr, "map_file() == -1\n");
+ return -1;
+ }
+
+ /* check addr range is inside the sram */
+ if (addr < sl->sram_base) {
+ fprintf(stderr, "addr too low\n");
+ goto on_error;
+ } else if ((addr + mf.len) < addr) {
+ fprintf(stderr, "addr overruns\n");
+ goto on_error;
+ } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
+ fprintf(stderr, "addr too high\n");
+ goto on_error;
+ } else if ((addr & 3) || (mf.len & 3)) {
+ /* todo */
+ fprintf(stderr, "unaligned addr or size\n");
+ goto on_error;
+ }
+
+ /* do the copy by 1k blocks */
+ for (off = 0; off < mf.len; off += 1024) {
+ size_t size = 1024;
+ if ((off + size) > mf.len)
+ size = mf.len - off;
+
+ memcpy(sl->q_buf, mf.base + off, size);
+
+ /* round size if needed */
+ if (size & 3)
+ size += 2;
+
+ stlink_write_mem32(sl, addr + off, size);
+ }
+
+ /* check the file ha been written */
+ if (check_file(sl, &mf, addr) == -1) {
+ fprintf(stderr, "check_file() == -1\n");
+ goto on_error;
+ }
+
+ /* success */
+ error = 0;
+
+on_error:
+ unmap_file(&mf);
+ return error;
+}
+
+int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
+ /* read size bytes from addr to file */
+
+ int error = -1;
+ size_t off;
+
+ const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
+ if (fd == -1) {
+ fprintf(stderr, "open(%s) == -1\n", path);
+ return -1;
+ }
+
+ /* do the copy by 1k blocks */
+ for (off = 0; off < size; off += 1024) {
+ size_t read_size = 1024;
+ if ((off + read_size) > size)
+ read_size = off + read_size;
+
+ /* round size if needed */
+ if (read_size & 3)
+ read_size = (read_size + 4) & ~(3);
+
+ stlink_read_mem32(sl, addr + off, read_size);
+
+ if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
+ fprintf(stderr, "write() != read_size\n");
+ goto on_error;
+ }
+ }
+
+ /* success */
+ error = 0;
+
+on_error:
+ close(fd);
+
+ return error;
+}
+
+typedef struct flash_loader {
+ stm32_addr_t loader_addr; /* loader sram adddr */
+ stm32_addr_t buf_addr; /* buffer sram address */
+} flash_loader_t;
+
+int write_buffer_to_sram
+(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
+ /* write the buffer right after the loader */
+ memcpy(sl->q_buf, buf, size);
+ stlink_write_mem8(sl, fl->buf_addr, size);
+ return 0;
+}
--- /dev/null
+/*
+ * File: stlink-common.h
+ * Bulk import from stlink-hw.h
+ *
+ * This should contain all the common top level stlink interfaces, regardless
+ * of how the backend does the work....
+ */
+
+#ifndef STLINK_COMMON_H
+#define STLINK_COMMON_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+ // 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_RUNNING 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;
+
+ typedef uint32_t stm32_addr_t;
+
+ enum transport_type {
+ TRANSPORT_TYPE_ZERO = 0,
+ TRANSPORT_TYPE_LIBSG,
+ TRANSPORT_TYPE_LIBUSB,
+ TRANSPORT_TYPE_INVALID
+ };
+
+ typedef struct _stlink stlink_t;
+
+ typedef struct _stlink_backend {
+ void (*close) (stlink_t* sl);
+ void (*exit_debug_mode) (stlink_t *sl);
+ void (*enter_swd_mode) (stlink_t *sl);
+ void (*enter_jtag_mode) (stlink_t *stl);
+ void (*exit_dfu_mode) (stlink_t *stl);
+ void (*core_id) (stlink_t *stl);
+ void (*reset) (stlink_t *stl);
+ void (*run) (stlink_t *stl);
+ void (*status) (stlink_t *stl);
+ void (*version) (stlink_t *stl);
+ void (*write_mem32) (stlink_t *sl, uint32_t addr, uint16_t len);
+ void (*write_mem8) (stlink_t *sl, uint32_t addr, uint16_t len);
+ } stlink_backend_t;
+
+ struct _stlink {
+ struct _stlink_backend *backend;
+ void *backend_data;
+
+ // Data transferred from or to device
+ unsigned char q_buf[Q_BUF_LEN];
+ int q_len;
+
+ // transport layer verboseness: 0 for no debug info, 10 for lots
+ int verbose;
+ uint32_t core_id;
+ int core_stat;
+
+
+
+ /* medium density stm32 flash settings */
+#define STM32_FLASH_BASE 0x08000000
+#define STM32_FLASH_SIZE (128 * 1024)
+#define STM32_FLASH_PGSZ 1024
+ stm32_addr_t flash_base;
+ size_t flash_size;
+ size_t flash_pgsz;
+
+ /* in flash system memory */
+#define STM32_SYSTEM_BASE 0x1ffff000
+#define STM32_SYSTEM_SIZE (2 * 1024)
+ stm32_addr_t sys_base;
+ size_t sys_size;
+
+ /* sram settings */
+#define STM32_SRAM_BASE 0x20000000
+#define STM32_SRAM_SIZE (8 * 1024)
+ stm32_addr_t sram_base;
+ size_t sram_size;
+
+ };
+
+ // some quick and dirty logging...
+ void D(stlink_t *sl, char *txt);
+ void DD(stlink_t *sl, char *format, ...);
+
+ //stlink_t* stlink_quirk_open(const char *dev_name, const int verbose);
+
+ // delegated functions...
+ void stlink_enter_swd_mode(stlink_t *sl);
+ void stlink_enter_jtag_mode(stlink_t *sl);
+ void stlink_exit_debug_mode(stlink_t *sl);
+ void stlink_exit_dfu_mode(stlink_t *sl);
+ void stlink_close(stlink_t *sl);
+ void stlink_core_id(stlink_t *sl);
+ void stlink_reset(stlink_t *sl);
+ void stlink_run(stlink_t *sl);
+ void stlink_status(stlink_t *sl);
+ void stlink_version(stlink_t *sl);
+ void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len);
+ void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len);
+
+
+ // unprocessed
+ int stlink_current_mode(stlink_t *sl);
+ void stlink_force_debug(stlink_t *sl);
+ void stlink_step(stlink_t *sl);
+ void stlink_read_all_regs(stlink_t *sl);
+ void stlink_read_reg(stlink_t *sl, int r_idx);
+ void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx);
+ void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len);
+
+ int stlink_erase_flash_page(stlink_t* sl, stm32_addr_t page);
+ int stlink_erase_flash_mass(stlink_t* sl);
+ int stlink_write_flash(stlink_t* sl, stm32_addr_t address, uint8_t* data, unsigned length);
+
+ // privates....
+ uint16_t read_uint16(const unsigned char *c, const int pt);
+ void stlink_core_stat(stlink_t *sl);
+ void stlink_print_data(stlink_t *sl);
+
+
+#include "stlink-sg.h"
+#include "stlink-usb.h"
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* STLINK_COMMON_H */
+
+++ /dev/null
-/*
- 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 <m@capitanio.org>
- The stlink related constants kindly provided by Oliver Spencer (OpenOCD)
- for use in a GPL compatible license.
-
- 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 <stdio.h>
-#include <string.h>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <sys/mman.h>
-
-// sgutils2 (apt-get install libsgutils2-dev)
-#include <scsi/sg_lib.h>
-#include <scsi/sg_pt.h>
-
-#include "stlink-hw.h"
-
-static void D(struct stlink *sl, char *txt) {
- if (sl->verbose > 1)
- fputs(txt, stderr);
-}
-
-static void DD(struct stlink *sl, char *format, ...) {
- if (sl->verbose > 0) {
- va_list list;
- va_start(list, format);
- vfprintf(stderr, format, list);
- va_end(list);
- }
-}
-
-// 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 uint16_t read_uint16(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];
- } else {
- p[0] = c[pt + 1];
- p[1] = 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) {
- DD(sl, "*** 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);
-
- /* flash memory settings */
- sl->flash_base = STM32_FLASH_BASE;
- sl->flash_size = STM32_FLASH_SIZE;
- sl->flash_pgsz = STM32_FLASH_PGSZ;
-
- /* system memory */
- sl->sys_base = STM32_SYSTEM_BASE;
- sl->sys_size = STM32_SYSTEM_SIZE;
-
- /* sram memory settings */
- sl->sram_base = STM32_SRAM_BASE;
- sl->sram_size = STM32_SRAM_SIZE;
-
- 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))
- DD(sl, " 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))
- DD(sl, " 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);
- DD(sl, " 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);
- DD(sl, "%s", buf);
- }
- if (sl->verbose && (resid > 0)) {
- if ((sl->verbose) || (sl->q_len > 0))
- DD(sl, " 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).
- DD(sl, " transport: %s", buf);
- }
- return;
- case SCSI_PT_RESULT_OS_ERR:
- if (sl->verbose) {
- get_scsi_pt_os_err_str(ptvp, sizeof(buf), buf);
- DD(sl, " os: %s", buf);
- }
- return;
- default:
- fprintf(stderr, " unknown pass through result "
- "category (%d)\n", cat);
- }
-}
-
-static void stlink_q(struct stlink* sl) {
- DD(sl, "CDB[");
- for (int i = 0; i < CDB_SL; i++)
- DD(sl, " 0x%02x", (unsigned int) sl->cdb_cmd_blk[i]);
- DD(sl, "]\n");
-
- // 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 (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);
-}
-
-// 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;
-
- DD(sl, "st vid = 0x%04x (expect 0x%04x)\n",
- sl->st_vid, USB_ST_VID);
- DD(sl, "stlink pid = 0x%04x (expect 0x%04x)\n",
- sl->stlink_pid, USB_STLINK_PID);
- DD(sl, "stlink version = 0x%x\n", sl->stlink_v);
- DD(sl, "jtag version = 0x%x\n", sl->jtag_v);
- DD(sl, "swim version = 0x%x\n", sl->swim_v);
- if (sl->jtag_v == 0)
- DD(sl,
- " notice: the firmware doesn't support a jtag/swd interface\n");
- if (sl->swim_v == 0)
- DD(sl,
- " 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:
- DD(sl, "stlink mode: dfu\n");
- return STLINK_DEV_DFU_MODE;
- case STLINK_DEV_DEBUG_MODE:
- DD(sl, "stlink mode: debug (jtag or swd)\n");
- return STLINK_DEV_DEBUG_MODE;
- case STLINK_DEV_MASS_MODE:
- DD(sl, "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:
- DD(sl, " %s: ok\n", txt);
- return;
- case STLINK_FALSE:
- DD(sl, " %s: false\n", txt);
- return;
- default:
- DD(sl, " %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_RUNNING:
- sl->core_stat = STLINK_CORE_RUNNING;
- DD(sl, " core status: running\n");
- return;
- case STLINK_CORE_HALTED:
- sl->core_stat = STLINK_CORE_HALTED;
- DD(sl, " 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
- */
-}
-
-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);
- DD(sl, "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)
- DD(sl, "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;
-
- DD(sl, "xpsr = 0x%08x\n", sl->reg.xpsr);
- DD(sl, "main_sp = 0x%08x\n", sl->reg.main_sp);
- DD(sl, "process_sp = 0x%08x\n", sl->reg.process_sp);
- DD(sl, "rw = 0x%08x\n", sl->reg.rw);
- DD(sl, "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");
- DD(sl, " (%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);
- DD(sl, "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");
-}
-
-// same as above with entrypoint.
-static unsigned int is_core_halted(struct stlink*);
-void stlink_run_at(struct stlink *sl, stm32_addr_t addr) {
- stlink_write_reg(sl, addr, 15); /* pc register */
-
- stlink_run(sl);
-
- while (is_core_halted(sl) == 0)
- usleep(3000000);
-}
-
-// 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);
-}
-
-// 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);
-}
-
-// 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 __attribute__((unused)) read_flash_rdp(struct stlink* sl)
-{
- stlink_read_mem32(sl, FLASH_WRPR, sizeof(uint32_t));
- return (*(uint32_t*)sl->q_buf) & 0xff;
-}
-
-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 __attribute__((unused)) 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 __attribute__((unused)) 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 __attribute__((unused)) 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 __attribute__((unused)) 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 __attribute__((unused)) 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 /* todo */
-static void disable_flash_read_protection(struct stlink* sl)
-{
- /* erase the option byte area */
- /* rdp = 0x00a5; */
- /* reset */
-}
-#endif /* todo */
-
-#if 0 /* not working */
-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;
-}
-#endif /* not working */
-
-int stlink_erase_flash_page(struct stlink* sl, stm32_addr_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;
-}
-
-int stlink_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;
-}
-
-static int write_loader_to_sram
-(struct stlink* sl, stm32_addr_t* addr, size_t* size)
-{
- /* 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 */
- };
-
- memcpy(sl->q_buf, loader_code, sizeof(loader_code));
- stlink_write_mem32(sl, sl->sram_base, sizeof(loader_code));
-
- *addr = sl->sram_base;
- *size = sizeof(loader_code);
-
- /* success */
- return 0;
-}
-
-typedef struct flash_loader
-{
- stm32_addr_t loader_addr; /* loader sram adddr */
- stm32_addr_t buf_addr; /* buffer sram address */
-} flash_loader_t;
-
-static int write_buffer_to_sram
-(struct stlink* sl, flash_loader_t* fl, const uint8_t* buf, size_t size)
-{
- /* write the buffer right after the loader */
- memcpy(sl->q_buf, buf, size);
- stlink_write_mem8(sl, fl->buf_addr, size);
- return 0;
-}
-
-static int init_flash_loader
-(struct stlink* sl, flash_loader_t* fl)
-{
- size_t size;
-
- /* allocate the loader in sram */
- if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1)
- {
- fprintf(stderr, "write_loader_to_sram() == -1\n");
- return -1;
- }
-
- /* allocate a one page buffer in sram right after loader */
- fl->buf_addr = fl->loader_addr + size;
-
- return 0;
-}
-
-static int run_flash_loader
-(struct stlink* sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size)
-{
- const size_t count = size / sizeof(uint16_t);
-
- if (write_buffer_to_sram(sl, fl, buf, size) == -1)
- {
- fprintf(stderr, "write_buffer_to_sram() == -1\n");
- return -1;
- }
-
- /* setup core */
- stlink_write_reg(sl, fl->buf_addr, 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) */
- stlink_write_reg(sl, fl->loader_addr, 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] != 0)
- {
- fprintf(stderr, "write error, count == %u\n", sl->reg.r[2]);
- return -1;
- }
-
- return 0;
-}
-
-/* memory mapped file */
-
-typedef struct mapped_file
-{
- uint8_t* base;
- size_t len;
-} mapped_file_t;
-
-#define MAPPED_FILE_INITIALIZER { NULL, 0 }
-
-static int map_file(mapped_file_t* mf, const char* path)
-{
- int error = -1;
- struct stat st;
-
- const int fd = open(path, O_RDONLY);
- if (fd == -1)
- {
- fprintf(stderr, "open(%s) == -1\n", path);
- return -1;
- }
-
- if (fstat(fd, &st) == -1)
- {
- fprintf(stderr, "fstat() == -1\n");
- goto on_error;
- }
-
- mf->base = (uint8_t*)mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
- if (mf->base == MAP_FAILED)
- {
- fprintf(stderr, "mmap() == MAP_FAILED\n");
- goto on_error;
- }
-
- mf->len = st.st_size;
-
- /* success */
- error = 0;
-
- on_error:
- close(fd);
-
- return error;
-}
-
-static void unmap_file(mapped_file_t* mf)
-{
- munmap((void*)mf->base, mf->len);
- mf->base = (unsigned char*)MAP_FAILED;
- mf->len = 0;
-}
-
-static int check_file
-(struct stlink* sl, mapped_file_t* mf, stm32_addr_t addr)
-{
- size_t off;
-
- for (off = 0; off < mf->len; off += sl->flash_pgsz)
- {
- size_t aligned_size;
-
- /* adjust last page size */
- size_t cmp_size = sl->flash_pgsz;
- if ((off + sl->flash_pgsz) > mf->len)
- cmp_size = mf->len - off;
-
- aligned_size = cmp_size;
- if (aligned_size & (4 - 1))
- aligned_size = (cmp_size + 4) & ~(4 - 1);
-
- stlink_read_mem32(sl, addr + off, aligned_size);
-
- if (memcmp(sl->q_buf, mf->base + off, cmp_size))
- return -1;
- }
-
- return 0;
-}
-
-static int stlink_fcheck_flash
-(struct stlink* sl, const char* path, stm32_addr_t addr)
-{
- /* check the contents of path are at addr */
-
- int res;
- mapped_file_t mf = MAPPED_FILE_INITIALIZER;
-
- if (map_file(&mf, path) == -1)
- return -1;
-
- res = check_file(sl, &mf, addr);
-
- unmap_file(&mf);
-
- return res;
-}
-
-// The stlink_fwrite_flash should not muck with mmapped files inside itself,
-// and should use this function instead. (Hell, what's the reason behind mmap
-// there?!) But, as it is not actually used anywhere, nobody cares.
-
-#define WRITE_BLOCK_SIZE 0x40
-int stlink_write_flash(struct stlink* sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
- int error = -1;
- size_t off;
- flash_loader_t fl;
-
- /* check addr range is inside the flash */
- if (addr < sl->flash_base) {
- fprintf(stderr, "addr too low\n");
- return -1;
- } else if ((addr + len) < addr) {
- fprintf(stderr, "addr overruns\n");
- return -1;
- } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
- fprintf(stderr, "addr too high\n");
- return -1;
- } else if ((addr & 1) || (len & 1)) {
- fprintf(stderr, "unaligned addr or size\n");
- return -1;
- }
-
- /* flash loader initialization */
- if (init_flash_loader(sl, &fl) == -1) {
- fprintf(stderr, "init_flash_loader() == -1\n");
- return -1;
- }
-
- /* write each page. above WRITE_BLOCK_SIZE fails? */
- for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
- /* adjust last write size */
- size_t size = WRITE_BLOCK_SIZE;
- if((off + WRITE_BLOCK_SIZE) > len)
- size = len - off;
-
- if(run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
- fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
- return -1;
- }
- }
-
- for(off = 0; off < len; off += sl->flash_pgsz) {
- size_t aligned_size;
-
- /* adjust last page size */
- size_t cmp_size = sl->flash_pgsz;
- if ((off + sl->flash_pgsz) > len)
- cmp_size = len - off;
-
- aligned_size = cmp_size;
- if (aligned_size & (4 - 1))
- aligned_size = (cmp_size + 4) & ~(4 - 1);
-
- stlink_read_mem32(sl, addr + off, aligned_size);
-
- if (memcmp(sl->q_buf, base + off, cmp_size))
- return -1;
- }
-
- return 0;
-}
-
-static int stlink_fwrite_flash
-(struct stlink* sl, const char* path, stm32_addr_t addr)
-{
- /* write the file in flash at addr */
-
- int error = -1;
- size_t off;
- mapped_file_t mf = MAPPED_FILE_INITIALIZER;
- flash_loader_t fl;
-
- if (map_file(&mf, path) == -1)
- {
- fprintf(stderr, "map_file() == -1\n");
- return -1;
- }
-
- /* check addr range is inside the flash */
- if (addr < sl->flash_base)
- {
- fprintf(stderr, "addr too low\n");
- goto on_error;
- }
- else if ((addr + mf.len) < addr)
- {
- fprintf(stderr, "addr overruns\n");
- goto on_error;
- }
- else if ((addr + mf.len) > (sl->flash_base + sl->flash_size))
- {
- fprintf(stderr, "addr too high\n");
- goto on_error;
- }
- else if ((addr & 1) || (mf.len & 1))
- {
- /* todo */
- fprintf(stderr, "unaligned addr or size\n");
- goto on_error;
- }
-
- /* erase each page. todo: mass erase faster? */
- for (off = 0; off < mf.len; off += sl->flash_pgsz)
- {
- /* addr must be an addr inside the page */
- if (stlink_erase_flash_page(sl, addr + off) == -1)
- {
- fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
- goto on_error;
- }
- }
-
- /* flash loader initialization */
- if (init_flash_loader(sl, &fl) == -1)
- {
- fprintf(stderr, "init_flash_loader() == -1\n");
- goto on_error;
- }
-
- /* write each page. above WRITE_BLOCK_SIZE fails? */
-#define WRITE_BLOCK_SIZE 0x40
- for (off = 0; off < mf.len; off += WRITE_BLOCK_SIZE)
- {
- /* adjust last write size */
- size_t size = WRITE_BLOCK_SIZE;
- if ((off + WRITE_BLOCK_SIZE) > mf.len)
- size = mf.len - off;
-
- if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1)
- {
- fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
- goto on_error;
- }
- }
-
- /* check the file ha been written */
- if (check_file(sl, &mf, addr) == -1)
- {
- fprintf(stderr, "check_file() == -1\n");
- goto on_error;
- }
-
- /* success */
- error = 0;
-
- on_error:
- unmap_file(&mf);
- return error;
-}
-
-static int stlink_fwrite_sram
-(struct stlink* sl, const char* path, stm32_addr_t addr)
-{
- /* write the file in sram at addr */
-
- int error = -1;
- size_t off;
- mapped_file_t mf = MAPPED_FILE_INITIALIZER;
-
- if (map_file(&mf, path) == -1)
- {
- fprintf(stderr, "map_file() == -1\n");
- return -1;
- }
-
- /* check addr range is inside the sram */
- if (addr < sl->sram_base)
- {
- fprintf(stderr, "addr too low\n");
- goto on_error;
- }
- else if ((addr + mf.len) < addr)
- {
- fprintf(stderr, "addr overruns\n");
- goto on_error;
- }
- else if ((addr + mf.len) > (sl->sram_base + sl->sram_size))
- {
- fprintf(stderr, "addr too high\n");
- goto on_error;
- }
- else if ((addr & 3) || (mf.len & 3))
- {
- /* todo */
- fprintf(stderr, "unaligned addr or size\n");
- goto on_error;
- }
-
- /* do the copy by 1k blocks */
- for (off = 0; off < mf.len; off += 1024)
- {
- size_t size = 1024;
- if ((off + size) > mf.len)
- size = mf.len - off;
-
- memcpy(sl->q_buf, mf.base + off, size);
-
- /* round size if needed */
- if (size & 3)
- size += 2;
-
- stlink_write_mem32(sl, addr + off, size);
- }
-
- /* check the file ha been written */
- if (check_file(sl, &mf, addr) == -1)
- {
- fprintf(stderr, "check_file() == -1\n");
- goto on_error;
- }
-
- /* success */
- error = 0;
-
- on_error:
- unmap_file(&mf);
- return error;
-}
-
-
-static int stlink_fread
-(struct stlink* sl, const char* path, stm32_addr_t addr, size_t size)
-{
- /* read size bytes from addr to file */
-
- int error = -1;
- size_t off;
-
- const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
- if (fd == -1)
- {
- fprintf(stderr, "open(%s) == -1\n", path);
- return -1;
- }
-
- /* do the copy by 1k blocks */
- for (off = 0; off < size; off += 1024)
- {
- size_t read_size = 1024;
- if ((off + read_size) > size)
- read_size = off + read_size;
-
- /* round size if needed */
- if (read_size & 3)
- read_size = (read_size + 4) & ~(3);
-
- stlink_read_mem32(sl, addr + off, read_size);
-
- if (write(fd, sl->q_buf, read_size) != (ssize_t)read_size)
- {
- fprintf(stderr, "write() != read_size\n");
- goto on_error;
- }
- }
-
- /* success */
- error = 0;
-
- on_error:
- close(fd);
-
- return error;
-}
-
-// 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_quirk_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;
- }
- DD(sl, "\n*** switch the stlink to mass mode ***\n");
- stlink_exit_dfu_mode(sl);
- // exit the dfu mode -> the device is gone
- DD(sl, "\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 __attribute__((unused)) 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
-}
-
-#if 0
-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);
- DD(sl, "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);
- DD(sl, "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);
- DD(sl, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0));
-
- stlink_read_mem32(sl, 0xe000edf0, 4);
- DD(sl, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0));
-
- stlink_read_mem32(sl, 0x4001100c, 4);
- DD(sl, "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);
- DD(sl, "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); */
- /* DD(sl, "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 1 /* read the system bootloader */
- fputs("\n++++++++++ reading bootloader ++++++++++++++++\n\n", stderr);
- stlink_fread(sl, "/tmp/barfoo", sl->sys_base, sl->sys_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 0 /* flash programming */
- fputs("\n+++++++ program flash memory\n\n", stderr);
- stlink_fwrite_flash(sl, "/tmp/foobar", 0x08000000);
-#endif
-#if 0 /* check file contents */
- fputs("\n+++++++ check flash memory\n\n", stderr);
- {
- const int res = stlink_fcheck_flash(sl, "/tmp/foobar", 0x08000000);
- printf("_____ stlink_fcheck_flash() == %d\n", res);
- }
-#endif
-#if 0
- fputs("\n+++++++ sram write and execute\n\n", stderr);
- stlink_fwrite_sram(sl, "/tmp/foobar", sl->sram_base);
- stlink_run_at(sl, sl->sram_base);
-#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;
-}
-#endif
+++ /dev/null
-#ifndef _STLINK_HW_H_
-#define _STLINK_HW_H_
-
-#include <stdint.h>
-
-// 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_RUNNING 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;
-
-typedef uint32_t stm32_addr_t;
-
-enum transport_type
-{
- TRANSPORT_TYPE_ZERO = 0,
-#if CONFIG_USE_LIBSG
- TRANSPORT_TYPE_LIBSG,
-#endif /* CONFIG_USE_LIBSG */
-#if CONFIG_USE_LIBUSB
- TRANSPORT_TYPE_LIBUSB,
-#endif /* CONFIG_USE_LIBUSB */
- TRANSPORT_TYPE_INVALID
-};
-
-struct stlink_libusb
-{
- libusb_device_handle* usb_handle;
- struct libusb_transfer* req_trans;
- struct libusb_transfer* rep_trans;
- unsigned int ep_req;
- unsigned int ep_rep;
-};
-
-struct stlink_libsg {
- 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;
-
- /* medium density stm32 flash settings */
-#define STM32_FLASH_BASE 0x08000000
-#define STM32_FLASH_SIZE (128 * 1024)
-#define STM32_FLASH_PGSZ 1024
- stm32_addr_t flash_base;
- size_t flash_size;
- size_t flash_pgsz;
-
- /* in flash system memory */
-#define STM32_SYSTEM_BASE 0x1ffff000
-#define STM32_SYSTEM_SIZE (2 * 1024)
- stm32_addr_t sys_base;
- size_t sys_size;
-
- /* sram settings */
-#define STM32_SRAM_BASE 0x20000000
-#define STM32_SRAM_SIZE (8 * 1024)
- stm32_addr_t sram_base;
- size_t sram_size;
-};
-
-struct stlink
-{
- enum transport_type tt;
- union {
-#if CONFIG_USE_LIBUSB
- struct stlink_libusb *libusb;
-#endif /* CONFIG_USE_LIBUSB */
-#if CONFIG_USE_LIBSG
- struct stlink_libsg *libsg;
-#endif /* CONFIG_USE_LIBSG */
- } transport;
-
- unsigned char q_buf[64];
- size_t q_len;
-
- /* layer independant */
- uint32_t core_id;
-};
-
-
-struct stlink* stlink_quirk_open(enum transport_type tt, const char *dev_name, const int verbose);
-int stlink_current_mode(struct stlink *sl);
-void stlink_enter_swd_mode(struct stlink *sl);
-void stlink_enter_jtag_mode(struct stlink *sl);
-void stlink_exit_debug_mode(struct stlink *sl);
-void stlink_core_id(struct stlink *sl);
-void stlink_status(struct stlink *sl);
-void stlink_force_debug(struct stlink *sl);
-void stlink_reset(struct stlink *sl);
-void stlink_run(struct stlink *sl);
-void stlink_step(struct stlink *sl);
-void stlink_read_all_regs(struct stlink *sl);
-void stlink_read_reg(struct stlink *sl, int r_idx);
-void stlink_write_reg(struct stlink *sl, uint32_t reg, int idx);
-void stlink_read_mem32(struct stlink *sl, uint32_t addr, uint16_t len);
-void stlink_write_mem8(struct stlink *sl, uint32_t addr, uint16_t len);
-void stlink_write_mem32(struct stlink *sl, uint32_t addr, uint16_t len);
-void stlink_close(struct stlink *sl);
-
-int stlink_erase_flash_page(struct stlink* sl, stm32_addr_t page);
-int stlink_erase_flash_mass(struct stlink* sl);
-int stlink_write_flash(struct stlink* sl, stm32_addr_t address, uint8_t* data, unsigned length);
-
-#endif
--- /dev/null
+/*
+ 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 <m@capitanio.org>
+ The stlink related constants kindly provided by Oliver Spencer (OpenOCD)
+ for use in a GPL compatible license.
+
+ 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 <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/mman.h>
+
+// sgutils2 (apt-get install libsgutils2-dev)
+#include <scsi/sg_lib.h>
+#include <scsi/sg_pt.h>
+
+#include "stlink-common.h"
+#include "stlink-sg.h"
+
+
+// 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);
+}
+
+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_libsg *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(stlink_t *sl) {
+ DD(sl, "*** clear_buf ***\n");
+ for (int i = 0; i < sizeof (sl->q_buf); i++)
+ sl->q_buf[i] = 0;
+
+}
+
+// close the device, free the allocated memory
+
+void _stlink_sg_close(stlink_t *sl) {
+ if (sl) {
+ struct stlink_libsg *slsg = sl->backend_data;
+ scsi_pt_close_device(slsg->sg_fd);
+ // CAUTION!? s this right?
+ free(slsg);
+ free(sl);
+ }
+}
+
+// Exit the jtag or swd mode and enter the mass mode.
+
+void _stlink_sg_exit_debug_mode(stlink_t *stl) {
+
+ if (stl) {
+ struct stlink_libsg* sl = stl->backend_data;
+ clear_cdb(sl);
+ sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
+ stl->q_len = 0; // >0 -> aboard
+ stlink_q(stl);
+ }
+}
+
+
+
+//TODO rewrite/cleanup, save the error in sl
+
+static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
+ struct stlink_libsg *sl = stl->backend_data;
+ 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 ((stl->verbose > 1) && (duration >= 0))
+ DD(stl, " 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 = stl->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 (stl->verbose && (resid > 0))
+ DD(stl, " notice: requested %d bytes but "
+ "got %d bytes, ignore [broken] residue = %d\n",
+ stl->q_len, dsize, resid);
+ break;
+ case SCSI_PT_RESULT_STATUS:
+ if (stl->verbose) {
+ sg_get_scsi_status_str(
+ get_scsi_pt_status_response(ptvp), sizeof (buf),
+ buf);
+ DD(stl, " scsi status: %s\n", buf);
+ }
+ return;
+ case SCSI_PT_RESULT_SENSE:
+ slen = get_scsi_pt_sense_len(ptvp);
+ if (stl->verbose) {
+ sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
+ > 1), sizeof (buf), buf);
+ DD(stl, "%s", buf);
+ }
+ if (stl->verbose && (resid > 0)) {
+ if ((stl->verbose) || (stl->q_len > 0))
+ DD(stl, " requested %d bytes but "
+ "got %d bytes\n", stl->q_len, dsize);
+ }
+ return;
+ case SCSI_PT_RESULT_TRANSPORT_ERR:
+ if (stl->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).
+ DD(stl, " transport: %s", buf);
+ }
+ return;
+ case SCSI_PT_RESULT_OS_ERR:
+ if (stl->verbose) {
+ get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
+ DD(stl, " os: %s", buf);
+ }
+ return;
+ default:
+ fprintf(stderr, " unknown pass through result "
+ "category (%d)\n", cat);
+ }
+}
+
+void stlink_q(stlink_t *stl) {
+ struct stlink_libsg* sl = stl->backend_data;
+ DD(stl, "CDB[");
+ for (int i = 0; i < CDB_SL; i++)
+ DD(stl, " 0x%02x", (unsigned int) sl->cdb_cmd_blk[i]);
+ DD(stl, "]\n");
+
+ // 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, stl->q_buf, stl->q_len);
+ } else {
+ set_scsi_pt_data_out(ptvp, stl->q_buf, stl->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,
+ stl->verbose);
+
+ // check for scsi errors
+ stlink_confirm_inq(stl, ptvp);
+ // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
+ destruct_scsi_pt_obj(ptvp);
+}
+
+// TODO thinking, cleanup
+
+void stlink_parse_version(stlink_t *stl) {
+ struct stlink_libsg *sl = stl->backend_data;
+ sl->st_vid = 0;
+ sl->stlink_pid = 0;
+ if (stl->q_len <= 0) {
+ fprintf(stderr, "Error: could not parse the stlink version");
+ return;
+ }
+ stlink_print_data(stl);
+ uint32_t b0 = stl->q_buf[0]; //lsb
+ uint32_t b1 = stl->q_buf[1];
+ uint32_t b2 = stl->q_buf[2];
+ uint32_t b3 = stl->q_buf[3];
+ uint32_t b4 = stl->q_buf[4];
+ uint32_t b5 = stl->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 (stl->verbose < 2)
+ return;
+
+ DD(stl, "st vid = 0x%04x (expect 0x%04x)\n",
+ sl->st_vid, USB_ST_VID);
+ DD(stl, "stlink pid = 0x%04x (expect 0x%04x)\n",
+ sl->stlink_pid, USB_STLINK_PID);
+ DD(stl, "stlink version = 0x%x\n", sl->stlink_v);
+ DD(stl, "jtag version = 0x%x\n", sl->jtag_v);
+ DD(stl, "swim version = 0x%x\n", sl->swim_v);
+ if (sl->jtag_v == 0)
+ DD(stl,
+ " notice: the firmware doesn't support a jtag/swd interface\n");
+ if (sl->swim_v == 0)
+ DD(stl,
+ " notice: the firmware doesn't support a swim interface\n");
+
+}
+
+int stlink_mode(stlink_t *stl) {
+ if (stl->q_len <= 0)
+ return STLINK_DEV_UNKNOWN_MODE;
+
+ stlink_print_data(stl);
+
+ switch (stl->q_buf[0]) {
+ case STLINK_DEV_DFU_MODE:
+ DD(stl, "stlink mode: dfu\n");
+ return STLINK_DEV_DFU_MODE;
+ case STLINK_DEV_DEBUG_MODE:
+ DD(stl, "stlink mode: debug (jtag or swd)\n");
+ return STLINK_DEV_DEBUG_MODE;
+ case STLINK_DEV_MASS_MODE:
+ DD(stl, "stlink mode: mass\n");
+ return STLINK_DEV_MASS_MODE;
+ }
+ return STLINK_DEV_UNKNOWN_MODE;
+}
+
+void stlink_stat(stlink_t *stl, char *txt) {
+ if (stl->q_len <= 0)
+ return;
+
+ stlink_print_data(stl);
+
+ switch (stl->q_buf[0]) {
+ case STLINK_OK:
+ DD(stl, " %s: ok\n", txt);
+ return;
+ case STLINK_FALSE:
+ DD(stl, " %s: false\n", txt);
+ return;
+ default:
+ DD(stl, " %s: unknown\n", txt);
+ }
+}
+
+void _stlink_sg_version(stlink_t *stl) {
+ struct stlink_libsg *sl = stl->backend_data;
+ D(stl, "\n*** stlink_version ***\n");
+ clear_cdb(sl);
+ sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
+ stl->q_len = 6;
+ sl->q_addr = 0;
+ stlink_q(stl);
+ stlink_parse_version(stl);
+}
+
+// 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(stlink_t *stl) {
+ struct stlink_libsg *sl = stl->backend_data;
+ D(stl, "\n*** stlink_current_mode ***\n");
+ clear_cdb(sl);
+ sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
+ stl->q_len = 2;
+ sl->q_addr = 0;
+ stlink_q(stl);
+ return stlink_mode(stl);
+}
+
+// Exit the mass mode and enter the swd debug mode.
+
+void _stlink_sg_enter_swd_mode(stlink_t *sl) {
+ struct stlink_libsg *sg = sl->backend_data;
+ clear_cdb(sg);
+ sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
+ sg->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_sg_enter_jtag_mode(stlink_t *sl) {
+ struct stlink_libsg *sg = sl->backend_data;
+ D(sl, "\n*** stlink_enter_jtag_mode ***\n");
+ clear_cdb(sg);
+ sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
+ sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
+ sl->q_len = 0;
+ stlink_q(sl);
+}
+
+// XXX kernel driver performs reset, the device temporally disappears
+
+void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
+ struct stlink_libsg *sg = sl->backend_data;
+ D(sl, "\n*** stlink_exit_dfu_mode ***\n");
+ clear_cdb(sg);
+ sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
+ sg->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
+ */
+}
+
+void _stlink_sg_core_id(stlink_t *sl) {
+ struct stlink_libsg *sg = sl->backend_data;
+ clear_cdb(sg);
+ sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
+ sl->q_len = 4;
+ sg->q_addr = 0;
+ stlink_q(sl);
+ sl->core_id = read_uint32(sl->q_buf, 0);
+ if (sl->verbose < 2)
+ return;
+ stlink_print_data(sl);
+}
+
+// Arm-core reset -> halted state.
+
+void _stlink_sg_reset(stlink_t *sl) {
+ struct stlink_libsg *sg = sl->backend_data;
+ D(sl, "\n*** stlink_reset ***\n");
+ clear_cdb(sg);
+ sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
+ sl->q_len = 2;
+ sg->q_addr = 0;
+ stlink_q(sl);
+ stlink_stat(sl, "core reset");
+}
+
+// Arm-core status: halted or running.
+
+void _stlink_sg_status(struct stlink_libsg *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);
+}
+
+// Force the core into the debug mode -> halted state.
+
+void stlink_force_debug(struct stlink_libsg *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_libsg *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)
+ DD(sl, "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;
+
+ DD(sl, "xpsr = 0x%08x\n", sl->reg.xpsr);
+ DD(sl, "main_sp = 0x%08x\n", sl->reg.main_sp);
+ DD(sl, "process_sp = 0x%08x\n", sl->reg.process_sp);
+ DD(sl, "rw = 0x%08x\n", sl->reg.rw);
+ DD(sl, "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_libsg *sl, int r_idx) {
+ D(sl, "\n*** stlink_read_reg");
+ DD(sl, " (%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);
+ DD(sl, "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_libsg *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_libsg *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_sg_run(stlink_t *stl) {
+ struct stlink_libsg sl = stl->backend_data;
+ D(stl, "\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");
+}
+
+// same as above with entrypoint.
+static unsigned int is_core_halted(struct stlink_libsg*);
+
+void stlink_run_at(struct stlink *sl_libsg, stm32_addr_t addr) {
+ stlink_write_reg(sl, addr, 15); /* pc register */
+
+ stlink_run(sl);
+
+ while (is_core_halted(sl) == 0)
+ usleep(3000000);
+}
+
+// Step the arm-core.
+
+void stlink_step(struct stlink_libsg *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_libsg *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_libsg *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_libsg *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);
+}
+
+// Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
+
+void _stlink_sg_write_mem8(struct stlink_libsg *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);
+}
+
+// Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
+
+void _stlink_sg_write_mem32(struct stlink_libsg *sl, uint32_t addr, uint16_t len) {
+ 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 __attribute__((unused)) read_flash_rdp(struct stlink_libsg* sl) {
+ stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
+ return (*(uint32_t*) sl->q_buf) & 0xff;
+}
+
+static inline uint32_t read_flash_wrpr(struct stlink_libsg* 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_libsg* 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_libsg* 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_libsg* sl) {
+ /* return non zero for true */
+ return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
+}
+
+static void unlock_flash(struct stlink_libsg* 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_libsg* 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_libsg* 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_libsg* 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 __attribute__((unused)) clear_flash_cr_pg(struct stlink_libsg* 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_libsg* 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 __attribute__((unused)) clear_flash_cr_per(struct stlink_libsg* 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_libsg* 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 __attribute__((unused)) clear_flash_cr_mer(struct stlink_libsg* 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_libsg* 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_libsg* 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_libsg* 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_libsg* sl) {
+ return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
+}
+
+static void wait_flash_busy(struct stlink_libsg* sl) {
+ /* todo: add some delays here */
+ while (is_flash_busy(sl))
+ ;
+}
+
+static inline unsigned int is_flash_eop(struct stlink_libsg* sl) {
+ return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
+}
+
+static void __attribute__((unused)) clear_flash_sr_eop(struct stlink_libsg* 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 __attribute__((unused)) wait_flash_eop(struct stlink_libsg* sl) {
+ /* todo: add some delays here */
+ while (is_flash_eop(sl) == 0)
+ ;
+}
+
+static inline void write_flash_ar(struct stlink_libsg* sl, uint32_t n) {
+ write_uint32(sl->q_buf, n);
+ stlink_write_mem32(sl, FLASH_AR, sizeof (uint32_t));
+}
+
+#if 0 /* todo */
+
+static void disable_flash_read_protection(struct stlink* sl) {
+ /* erase the option byte area */
+ /* rdp = 0x00a5; */
+ /* reset */
+}
+#endif /* todo */
+
+#if 0 /* not working */
+
+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;
+}
+#endif /* not working */
+
+int stlink_erase_flash_page(struct stlink_libsg* sl, stm32_addr_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;
+}
+
+int stlink_erase_flash_mass(struct stlink_libsg* 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_libsg* sl) {
+ /* return non zero if core is halted */
+ stlink_status(sl);
+ return sl->q_buf[0] == STLINK_CORE_HALTED;
+}
+
+static int write_loader_to_sram
+(struct stlink* sl, stm32_addr_t* addr, size_t* size) {
+ /* 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 */
+ };
+
+ memcpy(sl->q_buf, loader_code, sizeof (loader_code));
+ stlink_write_mem32(sl, sl->sram_base, sizeof (loader_code));
+
+ *addr = sl->sram_base;
+ *size = sizeof (loader_code);
+
+ /* success */
+ return 0;
+}
+
+
+int init_flash_loader(stlink_t *stl, flash_loader_t* fl) {
+ struct stlink_libsg* sl = stl->backend_data;
+ size_t size;
+
+ /* allocate the loader in sram */
+ if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
+ fprintf(stderr, "write_loader_to_sram() == -1\n");
+ return -1;
+ }
+
+ /* allocate a one page buffer in sram right after loader */
+ fl->buf_addr = fl->loader_addr + size;
+
+ return 0;
+}
+
+static int run_flash_loader
+(stlink_t *stl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
+ struct stlink_libsg* sl = stl->backend_data;
+ const size_t count = size / sizeof (uint16_t);
+
+ if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
+ fprintf(stderr, "write_buffer_to_sram() == -1\n");
+ return -1;
+ }
+
+ /* setup core */
+ stlink_write_reg(sl, fl->buf_addr, 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) */
+ stlink_write_reg(sl, fl->loader_addr, 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] != 0) {
+ fprintf(stderr, "write error, count == %u\n", sl->reg.r[2]);
+ return -1;
+ }
+
+ return 0;
+}
+
+static int stlink_fcheck_flash
+(struct stlink_libsg* sl, const char* path, stm32_addr_t addr) {
+ /* check the contents of path are at addr */
+
+ int res;
+ mapped_file_t mf = MAPPED_FILE_INITIALIZER;
+
+ if (map_file(&mf, path) == -1)
+ return -1;
+
+ res = check_file(sl, &mf, addr);
+
+ unmap_file(&mf);
+
+ return res;
+}
+
+// The stlink_fwrite_flash should not muck with mmapped files inside itself,
+// and should use this function instead. (Hell, what's the reason behind mmap
+// there?!) But, as it is not actually used anywhere, nobody cares.
+
+#define WRITE_BLOCK_SIZE 0x40
+
+int stlink_write_flash(struct stlink_libsg* sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
+ int error = -1;
+ size_t off;
+ flash_loader_t fl;
+
+ /* check addr range is inside the flash */
+ if (addr < sl->flash_base) {
+ fprintf(stderr, "addr too low\n");
+ return -1;
+ } else if ((addr + len) < addr) {
+ fprintf(stderr, "addr overruns\n");
+ return -1;
+ } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
+ fprintf(stderr, "addr too high\n");
+ return -1;
+ } else if ((addr & 1) || (len & 1)) {
+ fprintf(stderr, "unaligned addr or size\n");
+ return -1;
+ }
+
+ /* flash loader initialization */
+ if (init_flash_loader(sl, &fl) == -1) {
+ fprintf(stderr, "init_flash_loader() == -1\n");
+ return -1;
+ }
+
+ /* write each page. above WRITE_BLOCK_SIZE fails? */
+ for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
+ /* adjust last write size */
+ size_t size = WRITE_BLOCK_SIZE;
+ if ((off + WRITE_BLOCK_SIZE) > len)
+ size = len - off;
+
+ if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
+ fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
+ return -1;
+ }
+ }
+
+ for (off = 0; off < len; off += sl->flash_pgsz) {
+ size_t aligned_size;
+
+ /* adjust last page size */
+ size_t cmp_size = sl->flash_pgsz;
+ if ((off + sl->flash_pgsz) > len)
+ cmp_size = len - off;
+
+ aligned_size = cmp_size;
+ if (aligned_size & (4 - 1))
+ aligned_size = (cmp_size + 4) & ~(4 - 1);
+
+ stlink_read_mem32(sl, addr + off, aligned_size);
+
+ if (memcmp(sl->q_buf, base + off, cmp_size))
+ return -1;
+ }
+
+ return 0;
+}
+
+int stlink_fwrite_flash
+(stlink_t *sl, const char* path, stm32_addr_t addr) {
+ /* write the file in flash at addr */
+
+ int error = -1;
+ size_t off;
+ mapped_file_t mf = MAPPED_FILE_INITIALIZER;
+ flash_loader_t fl;
+
+ if (map_file(&mf, path) == -1) {
+ fprintf(stderr, "map_file() == -1\n");
+ return -1;
+ }
+
+ /* check addr range is inside the flash */
+ if (addr < sl->flash_base) {
+ fprintf(stderr, "addr too low\n");
+ goto on_error;
+ } else if ((addr + mf.len) < addr) {
+ fprintf(stderr, "addr overruns\n");
+ goto on_error;
+ } else if ((addr + mf.len) > (sl->flash_base + sl->flash_size)) {
+ fprintf(stderr, "addr too high\n");
+ goto on_error;
+ } else if ((addr & 1) || (mf.len & 1)) {
+ /* todo */
+ fprintf(stderr, "unaligned addr or size\n");
+ goto on_error;
+ }
+
+ /* erase each page. todo: mass erase faster? */
+ for (off = 0; off < mf.len; off += sl->flash_pgsz) {
+ /* addr must be an addr inside the page */
+ if (stlink_erase_flash_page(sl, addr + off) == -1) {
+ fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
+ goto on_error;
+ }
+ }
+
+ /* flash loader initialization */
+ if (init_flash_loader(sl, &fl) == -1) {
+ fprintf(stderr, "init_flash_loader() == -1\n");
+ goto on_error;
+ }
+
+ /* write each page. above WRITE_BLOCK_SIZE fails? */
+#define WRITE_BLOCK_SIZE 0x40
+ for (off = 0; off < mf.len; off += WRITE_BLOCK_SIZE) {
+ /* adjust last write size */
+ size_t size = WRITE_BLOCK_SIZE;
+ if ((off + WRITE_BLOCK_SIZE) > mf.len)
+ size = mf.len - off;
+
+ if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1) {
+ fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
+ goto on_error;
+ }
+ }
+
+ /* check the file ha been written */
+ if (check_file(sl, &mf, addr) == -1) {
+ fprintf(stderr, "check_file() == -1\n");
+ goto on_error;
+ }
+
+ /* success */
+ error = 0;
+
+on_error:
+ unmap_file(&mf);
+ return error;
+}
+
+// 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
+
+stlink_backend_t _stlink_sg_backend = {
+ _stlink_sg_close,
+ _stlink_sg_exit_debug_mode,
+ _stlink_sg_enter_swd_mode,
+ _stlink_sg_enter_jtag_mode,
+ _stlink_sg_exit_dfu_mode,
+ _stlink_sg_core_id,
+ _stlink_sg_reset,
+ _stlink_sg_run,
+ _stlink_sg_status,
+ _stlink_sg_version,
+ _stlink_sg_write_mem32,
+ _stlink_sg_write_mem8
+};
+
+stlink_t* 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;
+ }
+
+ stlink_t *sl = malloc(sizeof (stlink_t));
+ struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
+ if (sl == NULL || slsg == NULL) {
+ fprintf(stderr, "Couldn't malloc stlink and stlink_sg structures out of memory!\n");
+ return NULL;
+ }
+ sl->verbose = verbose;
+ sl->backend_data = slsg;
+ sl->backend = &_stlink_sg_backend;
+
+ slsg->sg_fd = sg_fd;
+ sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
+ slsg->core_id = 0;
+ slsg->q_addr = 0;
+ clear_buf(sl);
+
+ /* flash memory settings */
+ sl->flash_base = STM32_FLASH_BASE;
+ sl->flash_size = STM32_FLASH_SIZE;
+ sl->flash_pgsz = STM32_FLASH_PGSZ;
+
+ /* system memory */
+ sl->sys_base = STM32_SYSTEM_BASE;
+ sl->sys_size = STM32_SYSTEM_SIZE;
+
+ /* sram memory settings */
+ sl->sram_base = STM32_SRAM_BASE;
+ sl->sram_size = STM32_SRAM_SIZE;
+
+ return sl;
+}
+
+
+
+stlink_t* stlink_quirk_open(const char *dev_name, const int verbose) {
+
+ stlink_t *sl = stlink_open(dev_name, verbose);
+ if (sl == NULL) {
+ fputs("Error: could not open stlink device\n", stderr);
+ return NULL;
+ }
+
+ stlink_version(sl);
+ struct stlink_libsg *sg = sl->backend_data;
+
+ if (sg->st_vid != USB_ST_VID || sg->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",
+ sg->st_vid, USB_ST_VID);
+ fprintf(stderr, " PID: got %04x expect %04x \n",
+ sg->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;
+ }
+ DD(sl, "\n*** switch the stlink to mass mode ***\n");
+ _stlink_sg_exit_dfu_mode(sl);
+ // exit the dfu mode -> the device is gone
+ DD(sl, "\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 __attribute__((unused)) mark_buf(stlink_t *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
+}
+#endif
--- /dev/null
+/*
+ * File: stlink-sg.h
+ * Author: karl
+ *
+ * Created on October 1, 2011, 11:29 PM
+ */
+
+#ifndef STLINK_SG_H
+#define STLINK_SG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stlink-common.h"
+
+ // 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
+
+
+
+#if defined(CONFIG_USE_LIBUSB)
+ struct stlink_libsg {
+ int sg_fd;
+ int do_scsi_pt_err;
+
+ unsigned char cdb_cmd_blk[CDB_SL];
+
+ 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;
+ };
+#else
+ struct stlink_libsg {};
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* STLINK_SG_H */
+
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <time.h>
+#include <sys/types.h>
+#include <libusb-1.0/libusb.h>
+#include "stlink-common.h"
+#include "stlink-usb.h"
+
+/* endianess related */
+static inline unsigned int is_bigendian(void) {
+ static volatile const unsigned int i = 1;
+ return *(volatile const char*) &i == 0;
+}
+
+void _stlink_usb_close(stlink_t* sl) {
+ struct stlink_libusb * const handle = sl->backend_data;
+ // maybe we couldn't even get the usb device?
+ if (handle != NULL) {
+ if (handle->req_trans != NULL)
+ libusb_free_transfer(handle->req_trans);
+
+ if (handle->rep_trans != NULL)
+ libusb_free_transfer(handle->rep_trans);
+
+ if (handle->usb_handle != NULL)
+ libusb_close(handle->usb_handle);
+
+ libusb_exit(handle->libusb_ctx);
+ free(handle);
+ }
+}
+
+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;
+}
+
+
+struct trans_ctx {
+#define TRANS_FLAGS_IS_DONE (1 << 0)
+#define TRANS_FLAGS_HAS_ERROR (1 << 1)
+ volatile unsigned long flags;
+};
+
+static void on_trans_done(struct libusb_transfer * trans) {
+ struct trans_ctx * const ctx = trans->user_data;
+
+ if (trans->status != LIBUSB_TRANSFER_COMPLETED)
+ ctx->flags |= TRANS_FLAGS_HAS_ERROR;
+
+ ctx->flags |= TRANS_FLAGS_IS_DONE;
+}
+
+int submit_wait(struct stlink_libusb *slu, struct libusb_transfer * trans) {
+ struct timeval start;
+ struct timeval now;
+ struct timeval diff;
+ struct trans_ctx trans_ctx;
+ enum libusb_error error;
+
+ trans_ctx.flags = 0;
+
+ /* brief intrusion inside the libusb interface */
+ trans->callback = on_trans_done;
+ trans->user_data = &trans_ctx;
+
+ if ((error = libusb_submit_transfer(trans))) {
+ printf("libusb_submit_transfer(%d)\n", error);
+ return -1;
+ }
+
+ gettimeofday(&start, NULL);
+
+ while (trans_ctx.flags == 0) {
+ struct timeval timeout;
+ timeout.tv_sec = 3;
+ timeout.tv_usec = 0;
+ if (libusb_handle_events_timeout(slu->libusb_ctx, &timeout)) {
+ printf("libusb_handle_events()\n");
+ return -1;
+ }
+
+ gettimeofday(&now, NULL);
+ timersub(&now, &start, &diff);
+ if (diff.tv_sec >= 3) {
+ printf("libusb_handle_events() timeout\n");
+ return -1;
+ }
+ }
+
+ if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
+ printf("libusb_handle_events() | has_error\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+ssize_t send_recv(struct stlink_libusb* handle,
+ unsigned char* txbuf, size_t txsize,
+ unsigned char* rxbuf, size_t rxsize) {
+ /* note: txbuf and rxbuf can point to the same area */
+
+ libusb_fill_bulk_transfer(handle->req_trans, handle->usb_handle,
+ handle->ep_req,
+ txbuf, txsize,
+ NULL, NULL,
+ 0
+ );
+
+ printf("submit_wait(req)\n");
+
+ if (submit_wait(handle, handle->req_trans)) return -1;
+
+ /* send_only */
+ if (rxsize == 0) return 0;
+
+ /* read the response */
+
+ libusb_fill_bulk_transfer(handle->rep_trans, handle->usb_handle,
+ handle->ep_rep, rxbuf, rxsize, NULL, NULL, 0);
+
+ printf("submit_wait(rep)\n");
+
+ if (submit_wait(handle, handle->rep_trans)) return -1;
+
+ return handle->rep_trans->actual_length;
+}
+
+static inline int send_only
+(struct stlink_libusb* handle, unsigned char* txbuf, size_t txsize) {
+ return send_recv(handle, txbuf, txsize, NULL, 0);
+}
+
+
+// KARL - fixme, common code! (or, one per backend)
+// candidate for common code...
+
+
+static int is_stlink_device(libusb_device * dev) {
+ struct libusb_device_descriptor desc;
+
+ if (libusb_get_device_descriptor(dev, &desc))
+ return 0;
+
+ printf("device: 0x%04x, 0x%04x\n", desc.idVendor, desc.idProduct);
+
+ if (desc.idVendor != 0x0483)
+ return 0;
+
+ if (desc.idProduct != 0x3748)
+ return 0;
+
+ return 1;
+}
+
+void _stlink_usb_version(stlink_t * sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_GET_VERSION;
+ buf[1] = 0x80;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }mkdir g
+
+#if 1 /* DEBUG */
+ {
+ unsigned int i;
+ for (i = 0; i < size; ++i) printf("%02x", buf[i]);
+ printf("\n");
+ }
+#endif /* DEBUG */
+}
+
+void _stlink_usb_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
+ D(sl, "oops! no write32 support yet ;)\n");
+}
+
+void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
+ D(sl, "oops! no write8 support yet ;)\n");
+}
+
+
+
+int stlink_current_mode(stlink_t * sl) {
+ int mode = -1;
+
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+
+ buf[0] = STLINK_GET_CURRENT_MODE;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return -1;
+ }
+
+ /* mode = (int)read_uint16(buf, 0); */
+ mode = (int) buf[0];
+
+#if 1 /* DEBUG */
+ printf("mode == 0x%x\n", mode);
+#endif /* DEBUG */
+
+
+ return mode;
+}
+
+void _stlink_usb_core_id(stlink_t * sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_READCOREID;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+
+ sl->core_id = read_uint32(buf, 0);
+}
+
+void _stlink_usb_status(stlink_t * sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_GETSTATUS;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+
+ /* todo: stlink_core_stat */
+
+ // FIXME - decode into sl->core_stat
+#if 1 /* DEBUG */
+ printf("status == 0x%x\n", buf[0]);
+#endif /* DEBUG */
+
+}
+
+void _stlink_enter_swd_mode(stlink_t * sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = 0x30; /* needed byte */
+ buf[2] = STLINK_DEBUG_ENTER_JTAG;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+
+}
+
+void _stlink_usb_exit_dfu_mode(stlink_t* sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DFU_COMMAND;
+ buf[1] = STLINK_DFU_EXIT;
+
+ size = send_only(slu, buf, 16);
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+}
+
+void _stlink_usb_reset(stlink_t * sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_RESETSYS;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+}
+
+
+void stlink_step(stlink_t* sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_STEPCORE;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+}
+
+void _stlink_usb_run(stlink_t* sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_RUNCORE;
+
+ size = send_recv(slu, buf, 16, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+
+}
+
+void _stlink_usb_exit_debug_mode(stlink_t *sl) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_EXIT;
+
+ size = send_only(slu, buf, 16);
+ if (size == -1) {
+ printf("[!] send_only\n");
+ return;
+ }
+}
+
+void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
+ struct stlink_libusb * const slu = sl->backend_data;
+ unsigned char* const buf = sl->q_buf;
+ ssize_t size;
+
+ /* assume len < sizeof(sl->q_buf) */
+
+ memset(buf, 0, sizeof (sl->q_buf));
+ buf[0] = STLINK_DEBUG_COMMAND;
+ buf[1] = STLINK_DEBUG_READMEM_32BIT;
+ write_uint32(buf + 2, addr);
+ write_uint16(buf + 6, len);
+#if 0
+ /* windows usb logs show only one byte is used for length ... */
+ buf[6] = (uint8_t) len;
+#endif
+
+ size = send_recv(slu, buf, 0x10, buf, sizeof (sl->q_buf));
+ if (size == -1) {
+ printf("[!] send_recv\n");
+ return;
+ }
+
+ sl->q_len = (size_t) size;
+
+ stlink_print_data(sl);
+}
+
+
+stlink_backend_t _stlink_usb_backend = {
+ _stlink_usb_close,
+ _stlink_usb_exit_debug_mode,
+ _stlink_enter_swd_mode,
+ NULL,
+ _stlink_usb_exit_dfu_mode,
+ _stlink_usb_core_id,
+ _stlink_usb_reset,
+ _stlink_usb_run,
+ _stlink_usb_status,
+ _stlink_usb_version,
+ _stlink_usb_write_mem32,
+ _stlink_usb_write_mem8
+};
+
+stlink_t* stlink_open_usb(const char *dev_name, const int verbose) {
+ stlink_t* sl = NULL;
+ struct stlink_libusb* slu = NULL;
+
+ sl = malloc(sizeof (stlink_t));
+ slu = malloc(sizeof (struct stlink_libusb));
+ if (sl == NULL) goto on_error;
+ if (slu == NULL) goto on_error;
+
+ sl->verbose = verbose;
+
+ if (slu->libusb_ctx != NULL) {
+ fprintf(stderr, "reopening with an existing context? undefined behaviour!\n");
+ goto on_error;
+ } else {
+ if (libusb_init(&(slu->libusb_ctx))) {
+ fprintf(stderr, "failed to init libusb context, wrong version of libraries?\n");
+ goto on_error;
+ }
+ }
+
+ int error = -1;
+
+ libusb_device** devs = NULL;
+ libusb_device* dev;
+ ssize_t i;
+ ssize_t count;
+ int config;
+
+ count = libusb_get_device_list(slu->libusb_ctx, &devs);
+ if (count < 0) {
+ printf("libusb_get_device_list\n");
+ goto on_libusb_error;
+ }
+
+ for (i = 0; i < count; ++i) {
+ dev = devs[i];
+ if (is_stlink_device(dev)) break;
+ }
+ if (i == count) return NULL;
+
+ if (libusb_open(dev, &(slu->usb_handle))) {
+ printf("libusb_open()\n");
+ goto on_libusb_error;
+ }
+
+ if (libusb_get_configuration(slu->usb_handle, &config)) {
+ /* this may fail for a previous configured device */
+ printf("libusb_get_configuration()\n");
+ goto on_libusb_error;
+ }
+
+ if (config != 1) {
+ printf("setting new configuration (%d -> 1)\n", config);
+ if (libusb_set_configuration(slu->usb_handle, 1)) {
+ /* this may fail for a previous configured device */
+ printf("libusb_set_configuration()\n");
+ goto on_libusb_error;
+ }
+ }
+
+ if (libusb_claim_interface(slu->usb_handle, 0)) {
+ printf("libusb_claim_interface()\n");
+ goto on_libusb_error;
+ }
+
+ slu->req_trans = libusb_alloc_transfer(0);
+ if (slu->req_trans == NULL) {
+ printf("libusb_alloc_transfer\n");
+ goto on_libusb_error;
+ }
+
+ slu->rep_trans = libusb_alloc_transfer(0);
+ if (slu->rep_trans == NULL) {
+ printf("libusb_alloc_transfer\n");
+ goto on_libusb_error;
+ }
+
+ slu->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
+ slu->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
+
+ /* libusb_reset_device(slu->usb_handle); */
+
+ /* success */
+ error = 0;
+
+on_libusb_error:
+ if (devs != NULL) {
+ libusb_free_device_list(devs, 1);
+ fprintf(stderr, "freed libusb device list\n");
+ }
+
+ if (error == -1) {
+ stlink_close(sl);
+ return NULL;
+ }
+
+ sl->backend = &_stlink_usb_backend;
+ sl->backend_data = slu;
+ /* success */
+ return sl;
+
+on_error:
+ if (sl != NULL) free(sl);
+ if (slu != NULL) free(slu);
+ return 0;
+}
\ No newline at end of file
--- /dev/null
+/*
+ * File: stlink-usb.h
+ * Author: karl
+ *
+ * Created on October 1, 2011, 11:29 PM
+ */
+
+#ifndef STLINK_USB_H
+#define STLINK_USB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <libusb-1.0/libusb.h>
+
+#if defined(CONFIG_USE_LIBUSB)
+ struct stlink_libusb {
+ libusb_context* libusb_ctx;
+ libusb_device_handle* usb_handle;
+ struct libusb_transfer* req_trans;
+ struct libusb_transfer* rep_trans;
+ unsigned int ep_req;
+ unsigned int ep_rep;
+ };
+#else
+#error "it's all bad!"
+ struct stlink_libusb {};
+#endif
+
+
+ stlink_t* stlink_open_usb(const char *dev_name, const int verbose);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* STLINK_USB_H */
+
--- /dev/null
+/*
+ * File: test_main.c
+ *
+ * main() ripped out of old stlink-hw.c
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "stlink-common.h"
+
+#if 0
+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);
+ DD(sl, "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);
+ DD(sl, "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);
+ DD(sl, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0));
+
+ stlink_read_mem32(sl, 0xe000edf0, 4);
+ DD(sl, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0));
+
+ stlink_read_mem32(sl, 0x4001100c, 4);
+ DD(sl, "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);
+ DD(sl, "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); */
+ /* DD(sl, "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 1 /* read the system bootloader */
+ fputs("\n++++++++++ reading bootloader ++++++++++++++++\n\n", stderr);
+ stlink_fread(sl, "/tmp/barfoo", sl->sys_base, sl->sys_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 0 /* flash programming */
+ fputs("\n+++++++ program flash memory\n\n", stderr);
+ stlink_fwrite_flash(sl, "/tmp/foobar", 0x08000000);
+#endif
+#if 0 /* check file contents */
+ fputs("\n+++++++ check flash memory\n\n", stderr);
+ {
+ const int res = stlink_fcheck_flash(sl, "/tmp/foobar", 0x08000000);
+ printf("_____ stlink_fcheck_flash() == %d\n", res);
+ }
+#endif
+#if 0
+ fputs("\n+++++++ sram write and execute\n\n", stderr);
+ stlink_fwrite_sram(sl, "/tmp/foobar", sl->sram_base);
+ stlink_run_at(sl, sl->sram_base);
+#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;
+}
+#endif
\ No newline at end of file
--- /dev/null
+#include <stdio.h>
+#include "stlink-common.h"
+
+
+int main(int ac, char** av) {
+ stlink_t* sl;
+
+ sl = stlink_open_usb(NULL, 10);
+ if (sl != NULL) {
+ printf("-- version\n");
+ stlink_version(sl);
+
+ if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
+ printf("-- exit_dfu_mode\n");
+ stlink_exit_dfu_mode(sl);
+ }
+
+ printf("-- enter_swd_mode\n");
+ stlink_enter_swd_mode(sl);
+
+ printf("-- current_mode\n");
+ stlink_current_mode(sl);
+
+ /* printf("-- core_id\n"); */
+ /* stlink_core_id(sl); */
+
+ printf("-- read_sram\n");
+ static const uint32_t sram_base = 0x8000000;
+ uint32_t off;
+ for (off = 0; off < 16; off += 4)
+ stlink_read_mem32(sl, sram_base + off, 4);
+
+ printf("-- read_mem, cpuid\n");
+ stlink_read_mem32(sl, 0xe000e008, 4);
+ /* stlink_read_mem32(sl, 0xe000ed90, 4); */
+ /* stlink_read_mem32(sl, 0xe000edf0, 4); */
+ /* stlink_read_mem32(sl, 0x4001100c, 4); */
+
+ printf("-- status\n");
+ stlink_status(sl);
+
+ printf("-- reset\n");
+ stlink_reset(sl);
+
+ printf("-- status\n");
+ stlink_status(sl);
+
+ printf("-- step\n");
+ stlink_step(sl);
+ getchar();
+
+ printf("-- run\n");
+ stlink_run(sl);
+
+ printf("-- exit_debug_mode\n");
+ stlink_exit_debug_mode(sl);
+
+ stlink_close(sl);
+ }
+
+ return 0;
+}
projects / fw / stlink / commitdiff