path: root/src/st-util/semihosting.c

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>

#include <stlink.h>
#include "semihosting.h"

#include <logging.h>
#include <read_write.h>

static int32_t mem_read_u8(stlink_t *sl, uint32_t addr, uint8_t *data) {
    int32_t offset = addr % 4;
    int32_t 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 int32_t mem_read_u16(stlink_t *sl, uint32_t addr, uint16_t *data) {
    int32_t offset = addr % 4;
    int32_t 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 int32_t mem_read_u32(stlink_t *sl, uint32_t addr, uint32_t *data) {
    int32_t offset = addr % 4;
    int32_t 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 int32_t mem_read(stlink_t *sl, uint32_t addr, void *data, uint16_t len) {
    int32_t offset = addr % 4;
    int32_t 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); }

    // 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);
}

static int32_t mem_write(stlink_t *sl, uint32_t addr, void *data, uint16_t len) {
    /* Note: this function can write more than it is asked to!
     * If addr is not an even 32 bit boundary, or len is not a multiple of 4.
     * If only 32 bit values can be written to the target, then this function should read
     * the target memory at the start and end of the buffer where it will write more that
     * the requested bytes. (perhaps reading the whole area is faster??).
     * If 16 and 8 bit writes are available, then they could be used instead.
     * Just return when the length is zero avoiding unneeded work. */
    if (len == 0) { return (0); }

    int32_t offset = addr % 4;
    int32_t write_len = len + offset;

    if (sl == NULL || data == NULL) { return (-1); }

    // align read size
    if ((write_len % 4) != 0) { write_len += 4 - (write_len % 4); }

    memcpy(&sl->q_buf[offset], data, len);

    // address and length must be aligned
    if (stlink_write_mem32(sl, addr - offset, write_len) != 0) { return (-1); }

    return (0);
}

/* For the SYS_WRITE0 call, we don't know the size of the null-terminated buffer
 * in the target memory. Instead of reading one byte at a time, we read by
 * chunks of WRITE0_BUFFER_SIZE bytes.
 */
#define WRITE0_BUFFER_SIZE 64

/* Define a maximum size for buffers transmitted by semihosting. There is no
 * limit in the ARM specification but this is a safety net.
 * We remove 4 byte from Q_BUF_LEN to handle alignment correction.
 */
#define MAX_BUFFER_SIZE (Q_BUF_LEN - 4)

/* Flags for Open syscall */

#ifndef O_BINARY
#define O_BINARY 0
#endif

static int32_t open_mode_flags[12] = {
    O_RDONLY,
    O_RDONLY | O_BINARY,
    O_RDWR,
    O_RDWR   | O_BINARY,
    O_WRONLY | O_CREAT | O_TRUNC,
    O_WRONLY | O_CREAT | O_TRUNC  | O_BINARY,
    O_RDWR   | O_CREAT | O_TRUNC,
    O_RDWR   | O_CREAT | O_TRUNC  | O_BINARY,
    O_WRONLY | O_CREAT | O_APPEND,
    O_WRONLY | O_CREAT | O_APPEND | O_BINARY,
    O_RDWR   | O_CREAT | O_APPEND,
    O_RDWR   | O_CREAT | O_APPEND | O_BINARY
};

static int32_t saved_errno = 0;

int32_t do_semihosting (stlink_t *sl, uint32_t r0, uint32_t r1, uint32_t *ret) {

    if (sl == NULL || ret == NULL) { return (-1); }

    DLOG("Do semihosting R0=0x%08x R1=0x%08x\n", r0, r1);

    switch (r0) {
    case SEMIHOST_SYS_OPEN:
    {
        uint32_t args[3];
        uint32_t name_address;
        uint32_t mode;
        uint32_t name_len;
        char     *name;

        if (mem_read(sl, r1, args, sizeof(args)) != 0) {
            DLOG("Semihosting SYS_OPEN error: cannot read args from target memory\n");
            *ret = -1;
            return (-1);
        }

        name_address = args[0];
        mode         = args[1];
        name_len     = args[2];

        if (mode > 12) {
            /* Invalid mode */
            DLOG("Semihosting SYS_OPEN error: invalid mode %d\n", mode);
            *ret = -1;
            return (-1);
        }

        /* Add the trailing zero that is not counted in the length argument (see
         * ARM semihosting specification)
         */
        name_len += 1;

        if (name_len > MAX_BUFFER_SIZE) {
            DLOG("Semihosting SYS_OPEN error: name buffer size is too big %d\n", name_len);
            *ret = -1;
            return (-1);
        }

        name = malloc(name_len);

        if (name == NULL) {
            DLOG("Semihosting SYS_OPEN error: cannot allocate name buffer\n");
            *ret = -1;
            return (-1);
        }

        if (mem_read(sl, name_address, name, name_len) != 0) {
            free(name);
            *ret = -1;
            DLOG("Semihosting SYS_OPEN error: cannot read name from target memory\n");
            return (-1);
        }

        DLOG("Semihosting: open('%s', (SH open mode)%d, 0644)\n", name, mode);

        *ret = (uint32_t)open(name, open_mode_flags[mode], 0644);
        saved_errno = errno;

        DLOG("Semihosting: return %d\n", *ret);

        free(name);
        break;
    }
    case SEMIHOST_SYS_CLOSE:
    {
        uint32_t args[1];
        int32_t fd;

        if (mem_read(sl, r1, args, sizeof(args)) != 0) {
            DLOG("Semihosting SYS_CLOSE error: cannot read args from target memory\n");
            *ret = -1;
            return (-1);
        }

        fd = (int32_t)args[0];

        DLOG("Semihosting: close(%d)\n", fd);

        *ret = (uint32_t)close(fd);
        saved_errno = errno;

        DLOG("Semihosting: return %d\n", *ret);
        break;
    }
    case SEMIHOST_SYS_WRITE:
    {
        uint32_t args[3];
        uint32_t buffer_address;
        int32_t fd;
        uint32_t buffer_len;
        void    *buffer;

        if (mem_read(sl, r1, args, sizeof(args)) != 0) {
            DLOG("Semihosting SYS_WRITE error: cannot read args from target memory\n");
            *ret = -1;
            return (-1);
        }

        fd             = (int32_t)args[0];
        buffer_address = args[1];
        buffer_len     = args[2];

        if (buffer_len > MAX_BUFFER_SIZE) {
            DLOG("Semihosting SYS_WRITE error: buffer size is too big %d\n",
                 buffer_len);
            *ret = buffer_len;
            return (-1);
        }

        buffer = malloc(buffer_len);

        if (buffer == NULL) {
            DLOG("Semihosting SYS_WRITE error: cannot allocate buffer\n");
            *ret = buffer_len;
            return (-1);
        }

        if (mem_read(sl, buffer_address, buffer, buffer_len) != 0) {
            DLOG("Semihosting SYS_WRITE error: cannot read buffer from target memory\n");
            free(buffer);
            *ret = buffer_len;
            return (-1);
        }

        DLOG("Semihosting: write(%d, target_addr:0x%08x, %u)\n", fd, buffer_address, buffer_len);

        *ret = (uint32_t)write(fd, buffer, buffer_len);
        saved_errno = errno;

        if (*ret == (uint32_t)-1) {
            *ret = buffer_len;
        } else {
            *ret -= buffer_len;
        }

        DLOG("Semihosting: return %d\n", *ret);
        free(buffer);
        break;
    }
    case SEMIHOST_SYS_READ:
    {
        uint32_t args[3];
        uint32_t buffer_address;
        int32_t fd;
        uint32_t buffer_len;
        void    *buffer;
        ssize_t read_result;

        if (mem_read(sl, r1, args, sizeof(args)) != 0) {
            DLOG("Semihosting SYS_READ error: cannot read args from target memory\n");
            *ret = -1;
            return (-1);
        }

        fd             = (int32_t)args[0];
        buffer_address = args[1];
        buffer_len     = args[2];

        if (buffer_len > MAX_BUFFER_SIZE) {
            DLOG("Semihosting SYS_READ error: buffer size is too big %d\n", buffer_len);
            *ret = buffer_len;
            return (-1);
        }

        buffer = malloc(buffer_len);

        if (buffer == NULL) {
            DLOG("Semihosting SYS_READ error: cannot allocatebuffer\n");
            *ret = buffer_len;
            return (-1);
        }

        DLOG("Semihosting: read(%d, target_addr:0x%08x, %u)\n", fd, buffer_address,
             buffer_len);

        read_result = read(fd, buffer, buffer_len);
        saved_errno = errno;

        if (read_result == -1) {
            *ret = buffer_len;
        } else {
            if (mem_write(sl, buffer_address, buffer, read_result) != 0) {
                DLOG("Semihosting SYS_READ error: cannot write buffer to target memory\n");
                free(buffer);
                *ret = buffer_len;
                return (-1);
            } else {
                *ret = buffer_len - (uint32_t)read_result;
            }
        }

        DLOG("Semihosting: return %d\n", *ret);
        free(buffer);
        break;
    }
    case SEMIHOST_SYS_ERRNO:
    {
        *ret = (uint32_t)saved_errno;
        DLOG("Semihosting: Errno return %d\n", *ret);
        break;
    }
    case SEMIHOST_SYS_REMOVE:
    {
        uint32_t args[2];
        uint32_t name_address;
        uint32_t name_len;
        char     *name;

        if (mem_read(sl, r1, args, sizeof(args)) != 0) {
            DLOG("Semihosting SYS_REMOVE error: cannot read args from target memory\n");
            *ret = -1;
            return (-1);
        }

        name_address = args[0];
        name_len     = args[1];

        /* Add the trailing zero that is not counted in the length argument (see
         * ARM semihosting specification)
         */
        name_len += 1;

        if (name_len > MAX_BUFFER_SIZE) {
            DLOG("Semihosting SYS_REMOVE error: name buffer size is too big %d\n",
                 name_len);
            *ret = -1;
            return (-1);
        }

        name = malloc(name_len);

        if (name == NULL) {
            DLOG("Semihosting SYS_REMOVE error: cannot allocate name buffer\n");
            *ret = -1;
            return (-1);
        }

        if (mem_read(sl, name_address, name, name_len) != 0) {
            free(name);
            *ret = -1;
            DLOG("Semihosting SYS_REMOVE error: cannot read name from target memory\n");
            return (-1);
        }

        DLOG("Semihosting: unlink('%s')\n", name);
        *ret = (uint32_t)unlink(name);
        saved_errno = errno;
        DLOG("Semihosting: return %d\n", *ret);
        free(name);
        break;
    }
    case SEMIHOST_SYS_SEEK:
    {
        uint32_t args[2];
        int32_t fd;
        off_t offset;

        if (mem_read(sl, r1, args, sizeof(args)) != 0) {
            DLOG("Semihosting SYS_SEEK error: cannot read args from target memory\n");
            *ret = -1;
            return (-1);
        }

        fd = (int32_t)args[0];
        offset = (off_t)args[1];

        DLOG("Semihosting: lseek(%d, %d, SEEK_SET)\n", fd, (int32_t)offset);
        *ret = (uint32_t)lseek(fd, offset, SEEK_SET);
        saved_errno = errno;

        if (*ret != (uint32_t)-1) { *ret = 0; /* Success */ }

        DLOG("Semihosting: return %d\n", *ret);
        break;
    }
    case SEMIHOST_SYS_WRITEC:
    {
        uint8_t c;

        if (mem_read_u8(sl, r1, &c) == 0) {
            fprintf(stderr, "%c", c);
        } else {
            DLOG("Semihosting WRITEC: cannot read target memory at 0x%08x\n", r1);
        }

        break;
    }
    case SEMIHOST_SYS_READC:
    {
        uint8_t c = getchar();
        *ret = c;
        break;
    }
    case SEMIHOST_SYS_WRITE0:
    {
        uint8_t buf[WRITE0_BUFFER_SIZE];

        while (true) {
            if (mem_read(sl, r1, buf, WRITE0_BUFFER_SIZE) != 0) {
                DLOG("Semihosting WRITE0: cannot read target memory at 0x%08x\n", r1);
                return (-1);
            }

            for (int32_t 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 %#x\n", r0);
        return (-1);
    }
    return (0);
}