Prototype of ARM semihosting support

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 ()

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 <USB_BUS>:<USB_ADDR>.\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, &reg);
+
+                        /* 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], &reg.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);

src/gdbserver/semihosting.c

@@ -0,0 +1,128 @@
+#include <stdio.h>
+#include <string.h>
+
+#include "semihosting.h"
+
+#include <stlink.h>
+#include <stlink/logging.h>
+
+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;
+}

src/gdbserver/semihosting.h

@@ -0,0 +1,34 @@
+#ifndef _SEMIHOSTING_H_
+#define _SEMIHOSTING_H_
+
+#include <stlink.h>
+
+#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_ */