From d0910c66d7c98c0448e4d9a3805986519848c255 Mon Sep 17 00:00:00 2001 From: Fabien Chouteau Date: Mon, 8 Aug 2016 19:52:43 +0200 Subject: [PATCH] Prototype of ARM semihosting support --- CMakeLists.txt | 4 +- src/gdbserver/gdb-server.c | 91 ++++++++++++++++++++++++- src/gdbserver/semihosting.c | 128 ++++++++++++++++++++++++++++++++++++ src/gdbserver/semihosting.h | 34 ++++++++++ 4 files changed, 255 insertions(+), 2 deletions(-) create mode 100644 src/gdbserver/semihosting.c create mode 100644 src/gdbserver/semihosting.h diff --git a/CMakeLists.txt b/CMakeLists.txt index cf18e0228..fc0127dda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -154,7 +154,9 @@ target_link_libraries(st-info ${PROJECT_NAME}) add_executable(st-util src/gdbserver/gdb-remote.c src/gdbserver/gdb-remote.h src/gdbserver/gdb-server.c - src/gdbserver/gdb-server.h) + src/gdbserver/gdb-server.h + src/gdbserver/semihosting.c + src/gdbserver/semihosting.h) if (WIN32 OR MSYS OR MINGW) target_link_libraries(st-util ${PROJECT_NAME} wsock32 ws2_32) else () diff --git a/src/gdbserver/gdb-server.c b/src/gdbserver/gdb-server.c index b8ee66eb5..53f4fd962 100644 --- a/src/gdbserver/gdb-server.c +++ b/src/gdbserver/gdb-server.c @@ -24,13 +24,18 @@ #include "gdb-remote.h" #include "gdb-server.h" +#include "semihosting.h" #define FLASH_BASE 0x08000000 +/* Semihosting doesn't have a short option, we define a value to identify it */ +#define SEMIHOSTING_OPTION 128 + //Allways update the FLASH_PAGE before each use, by calling stlink_calculate_pagesize #define FLASH_PAGE (sl->flash_pgsz) static stlink_t *connected_stlink = NULL; +bool semihosting = false; static const char hex[] = "0123456789abcdef"; @@ -72,6 +77,7 @@ int parse_options(int argc, char** argv, st_state_t *st) { {"listen_port", required_argument, NULL, 'p'}, {"multi", optional_argument, NULL, 'm'}, {"no-reset", optional_argument, NULL, 'n'}, + {"semihosting", no_argument, NULL, SEMIHOSTING_OPTION}, {0, 0, 0, 0}, }; const char * help_str = "%s - usage:\n\n" @@ -89,6 +95,8 @@ int parse_options(int argc, char** argv, st_state_t *st) { "\t\t\tst-util will continue listening for connections after disconnect.\n" " -n, --no-reset\n" "\t\t\tDo not reset board on connection.\n" + " --semihosting\n" + "\t\t\tEnable semihosting support.\n" "\n" "The STLINKv2 device to use can be specified in the environment\n" "variable STLINK_DEVICE on the format :.\n" @@ -145,6 +153,9 @@ int parse_options(int argc, char** argv, st_state_t *st) { case 'n': st->reset = 0; break; + case SEMIHOSTING_OPTION: + semihosting = true; + break; } } @@ -591,6 +602,16 @@ static void init_code_breakpoints(stlink_t *sl) { } } +static int has_breakpoint(stm32_addr_t addr) +{ + for(int i = 0; i < code_break_num; i++) { + if (code_breaks[i].addr == addr) { + return 1; + } + } + return 0; +} + static int update_code_breakpoint(stlink_t *sl, stm32_addr_t addr, int set) { stm32_addr_t fpb_addr; uint32_t mask; @@ -1132,6 +1153,28 @@ int serve(stlink_t *sl, st_state_t *st) { init_data_watchpoints(sl); DLOG("Rcmd: reset\n"); + } else if (!strncmp(cmd, "semihosting ", 12)) { + DLOG("Rcmd: got semihosting cmd '%s'", cmd); + char *arg = cmd + 12; + + /* Skip whitespaces */ + while (isspace(*arg)) { + arg++; + } + + if (!strncmp(arg, "enable", 6) + || !strncmp(arg, "1", 1)) + { + semihosting = true; + reply = strdup("OK"); + } else if (!strncmp(arg, "disable", 7) + || !strncmp(arg, "0", 1)) + { + semihosting = false; + reply = strdup("OK"); + } else { + DLOG("Rcmd: unknown semihosting arg: '%s'\n", arg); + } } else { DLOG("Rcmd: %s\n", cmd); } @@ -1244,7 +1287,53 @@ int serve(stlink_t *sl, st_state_t *st) { stlink_status(sl); if(sl->core_stat == STLINK_CORE_HALTED) { - break; + struct stlink_reg reg; + int ret; + stm32_addr_t pc; + stm32_addr_t addr; + int offset = 0; + uint16_t insn; + + if (!semihosting) { + break; + } + + stlink_read_all_regs (sl, ®); + + /* Read PC */ + pc = reg.r[15]; + + /* Compute aligned value */ + offset = pc % 4; + addr = pc - offset; + + /* Read instructions (address and length must be + * aligned). + */ + ret = stlink_read_mem32(sl, addr, (offset > 2 ? 8 : 4)); + + if (ret != 0) { + DLOG("Semihost: cannot read instructions at: " + "0x%08x\n", addr); + break; + } + + memcpy(&insn, &sl->q_buf[offset], sizeof(insn)); + + if (insn == 0xBEAB && !has_breakpoint(addr)) { + DLOG("Do semihosting...\n"); + + do_semihosting (sl, reg.r[0], reg.r[1], ®.r[0]); + + /* Jump over the break instruction */ + stlink_write_reg(sl, reg.r[15] + 2, 15); + + /* continue execution */ + cache_sync(sl); + stlink_run(sl); + } else { + break; + } } usleep(100000); diff --git a/src/gdbserver/semihosting.c b/src/gdbserver/semihosting.c new file mode 100644 index 000000000..74d5fa398 --- /dev/null +++ b/src/gdbserver/semihosting.c @@ -0,0 +1,128 @@ +#include +#include + +#include "semihosting.h" + +#include +#include + +static int mem_read_u8(stlink_t *sl, uint32_t addr, uint8_t *data) +{ + int offset = addr % 4; + int len = 4; + + if (sl == NULL || data == NULL) { + return -1; + } + + /* Read address and length must be aligned */ + if (stlink_read_mem32(sl, addr - offset, len) != 0) { + return -1; + } + + *data = sl->q_buf[offset]; + return 0; +} + +#ifdef UNUSED +static int mem_read_u16(stlink_t *sl, uint32_t addr, uint16_t *data) +{ + int offset = addr % 4; + int len = (offset > 2 ? 8 : 4); + + if (sl == NULL || data == NULL) { + return -1; + } + + /* Read address and length must be aligned */ + if (stlink_read_mem32(sl, addr - offset, len) != 0) { + return -1; + } + + memcpy(data, &sl->q_buf[offset], sizeof(*data)); + return 0; +} + +static int mem_read_u32(stlink_t *sl, uint32_t addr, uint32_t *data) +{ + int offset = addr % 4; + int len = (offset > 0 ? 8 : 4); + + if (sl == NULL || data == NULL) { + return -1; + } + + /* Read address and length must be aligned */ + if (stlink_read_mem32(sl, addr - offset, len) != 0) { + return -1; + } + + memcpy(data, &sl->q_buf[offset], sizeof(*data)); + return 0; +} +#endif + +static int mem_read(stlink_t *sl, uint32_t addr, uint8_t *data, uint16_t len) +{ + int offset = addr % 4; + int read_len = len + offset; + + if (sl == NULL || data == NULL) { + return -1; + } + + /* Align read size */ + if ((read_len % 4) != 0) { + read_len += 4 - (read_len % 4); + } + + /* Read address and length must be aligned */ + if (stlink_read_mem32(sl, addr - offset, read_len) != 0) { + return -1; + } + + memcpy(data, &sl->q_buf[offset], len); + return 0; +} + +#define WRITE0_BUFFER_SIZE 64 + +int do_semihosting (stlink_t *sl, uint32_t r0, uint32_t r1, uint32_t *ret) { + + if (sl == NULL || ret == NULL) { + return -1; + } + + switch (r0) { + case SYS_WRITEC: + { + uint8_t c; + if (mem_read_u8(sl, r1, &c) == 0) { + fprintf(stderr, "%c", c); + } + break; + } + case SYS_WRITE0: + { + uint8_t buf[WRITE0_BUFFER_SIZE]; + + while (true) { + if (mem_read(sl, r1, buf, WRITE0_BUFFER_SIZE) != 0 ) { + return -1; + } + for (int i = 0; i < WRITE0_BUFFER_SIZE; i++) { + if (buf[i] == 0) { + return 0; + } + fprintf(stderr, "%c", buf[i]); + } + r1 += WRITE0_BUFFER_SIZE; + } + break; + } + default: + fprintf(stderr, "semihosting: unsupported call 0x%#x\n", r0); + return -1; + } + return 0; +} diff --git a/src/gdbserver/semihosting.h b/src/gdbserver/semihosting.h new file mode 100644 index 000000000..392d31f7d --- /dev/null +++ b/src/gdbserver/semihosting.h @@ -0,0 +1,34 @@ +#ifndef _SEMIHOSTING_H_ +#define _SEMIHOSTING_H_ + +#include + +#define SYS_OPEN 0x01 +#define SYS_CLOSE 0x02 +#define SYS_WRITEC 0x03 +#define SYS_WRITE0 0x04 +#define SYS_WRITE 0x05 +#define SYS_READ 0x06 +#define SYS_READC 0x07 +#define SYS_ISERROR 0x08 +#define SYS_ISTTY 0x09 +#define SYS_SEEK 0x0A + +#define SYS_FLEN 0x0C +#define SYS_TMPNAM 0x0D +#define SYS_REMOVE 0x0E +#define SYS_RENAME 0x0E +#define SYS_CLOCK 0x10 +#define SYS_TIME 0x11 + +#define SYS_ERRNO 0x13 + +#define SYS_GET_CMD 0x15 +#define SYS_HEAPINFO 0x16 + +#define SYS_ELAPSED 0x30 +#define SYS_TICKFREQ 0x31 + +int do_semihosting (stlink_t *sl, uint32_t r0, uint32_t r1, uint32_t *ret); + +#endif /* ! _SEMIHOSTING_H_ */