thermosphere: add semihosting support & load a kernel using it when needed

basically host i/o

thermosphere/src/core_ctx.h

@@ -20,7 +20,8 @@
 typedef struct CoreCtx {
     u64 kernelArgument;
     uintptr_t kernelEntrypoint;
-    u32 coreId;             // @ 0x0C
+    u32 coreId;                     // @0x10
+    bool isColdBootCore;            // @0x14
 } CoreCtx;
 
 extern CoreCtx g_coreCtxs[4];

thermosphere/src/debug_log.c

@@ -17,8 +17,13 @@
 #include <stdio.h>
 #include "debug_log.h"
 #include "platform/uart.h"
+#include "semihosting.h"
 #include "utils.h"
 
+#ifndef DLOG_USE_SEMIHOSTING_WRITE0
+#define DLOG_USE_SEMIHOSTING_WRITE0 0
+#endif
+
 // NOTE: UNSAFE!
 int debugLog(const char *fmt, ...)
 {
@@ -28,6 +33,12 @@ int debugLog(const char *fmt, ...)
     int res = vsprintf(buf, fmt, args);
     va_end(args);
 
-    uartWriteData(buf, (size_t)res);
+    // Use semihosting if available (we assume qemu was launched with -semihosting), otherwise UART
+    if (DLOG_USE_SEMIHOSTING_WRITE0 && semihosting_connection_supported()) {
+        semihosting_write_string(buf);
+    } else {
+        uartWriteData(buf, (size_t)res);
+    }
+
     return res;
 }

thermosphere/src/exception_vectors.s

@@ -173,6 +173,12 @@ vector_entry serror_sp0
     .cfi_endproc
     /* To save space, insert in an unused vector segment. */
 
+.global semihosting_call
+.type   semihosting_call, %function
+semihosting_call:
+    hlt     #0xF000
+    ret
+
 .global doSmcIndirectCallImpl
 //.type   doSmcIndirectCallImpl, %function
 doSmcIndirectCallImpl:

thermosphere/src/main.c

@@ -2,16 +2,50 @@
 #include "core_ctx.h"
 #include "debug_log.h"
 #include "platform/uart.h"
+#include "semihosting.h"
 #include "traps.h"
 
+extern const u8 __start__[];
+
+static void loadKernelViaSemihosting(void)
+{
+    size_t len = 1<<20; // max len
+    uintptr_t buf = (uintptr_t)__start__ + (1<<20);
+    long handle = -1, ret;
+
+    DEBUG("Loading kernel via semihosting file I/O... ");
+    handle = semihosting_file_open("test_kernel.bin", FOPEN_MODE_RB);
+    if (handle < 0) {
+        DEBUG("failed to open file (%ld)!\n", handle);
+        panic();
+    }
+
+    if ((ret = semihosting_file_read(handle, &len, buf)) < 0) {
+        DEBUG("failed to read file (%ld)!\n", ret);
+        panic();
+    }
+
+    DEBUG("OK!");
+    semihosting_file_close(handle);
+    currentCoreCtx->kernelEntrypoint = buf;
+}
+
 int main(void)
 {
     enableTraps();
 
-    if (currentCoreCtx->coreId == 0) {
+    if (currentCoreCtx->isColdBootCore) {
         uartInit(115200);
         DEBUG("Hello from Thermosphere!\n");
-        __builtin_trap();
+        if (currentCoreCtx->kernelEntrypoint == 0) {
+            if (semihosting_connection_supported()) {
+                loadKernelViaSemihosting();
+                //panic();
+            } else {
+                DEBUG("Kernel not loaded!\n");
+                panic();
+            }
+        }
     }
     else {
         DEBUG("Core %u booted\n", currentCoreCtx->coreId);

thermosphere/src/semihosting.c

@@ -0,0 +1,209 @@
+// Adapted from Arm TF
+
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <string.h>
+#include "semihosting.h"
+
+long semihosting_call(unsigned long operation,
+            void *system_block_address);
+
+typedef struct {
+    const char *file_name;
+    unsigned long mode;
+    size_t name_length;
+} smh_file_open_block_t;
+
+typedef struct {
+    long handle;
+    uintptr_t buffer;
+    size_t length;
+} smh_file_read_write_block_t;
+
+typedef struct {
+    long handle;
+    ssize_t location;
+} smh_file_seek_block_t;
+
+typedef struct {
+    char *command_line;
+    size_t command_length;
+} smh_system_block_t;
+
+long semihosting_file_open(const char *file_name, size_t mode)
+{
+    smh_file_open_block_t open_block;
+
+    open_block.file_name = file_name;
+    open_block.mode = mode;
+    open_block.name_length = strlen(file_name);
+
+    return semihosting_call(SEMIHOSTING_SYS_OPEN,
+                (void *) &open_block);
+}
+
+long semihosting_file_seek(long file_handle, ssize_t offset)
+{
+    smh_file_seek_block_t seek_block;
+    long result;
+
+    seek_block.handle = file_handle;
+    seek_block.location = offset;
+
+    result = semihosting_call(SEMIHOSTING_SYS_SEEK,
+                  (void *) &seek_block);
+
+    if (result)
+        result = semihosting_call(SEMIHOSTING_SYS_ERRNO, 0);
+
+    return result;
+}
+
+long semihosting_file_read(long file_handle, size_t *length, uintptr_t buffer)
+{
+    smh_file_read_write_block_t read_block;
+    long result = -EINVAL;
+
+    if ((length == NULL) || (buffer == (uintptr_t)NULL))
+        return result;
+
+    read_block.handle = file_handle;
+    read_block.buffer = buffer;
+    read_block.length = *length;
+
+    result = semihosting_call(SEMIHOSTING_SYS_READ,
+                  (void *) &read_block);
+
+    if (result == *length) {
+        return -EINVAL;
+    } else if (result < *length) {
+        *length -= result;
+        return 0;
+    } else
+        return result;
+}
+
+long semihosting_file_write(long file_handle,
+                size_t *length,
+                const uintptr_t buffer)
+{
+    smh_file_read_write_block_t write_block;
+    long result = -EINVAL;
+
+    if ((length == NULL) || (buffer == (uintptr_t)NULL))
+        return -EINVAL;
+
+    write_block.handle = file_handle;
+    write_block.buffer = (uintptr_t)buffer; /* cast away const */
+    write_block.length = *length;
+
+    result = semihosting_call(SEMIHOSTING_SYS_WRITE,
+                   (void *) &write_block);
+
+    *length = result;
+
+    return (result == 0) ? 0 : -EINVAL;
+}
+
+long semihosting_file_close(long file_handle)
+{
+    return semihosting_call(SEMIHOSTING_SYS_CLOSE,
+                (void *) &file_handle);
+}
+
+long semihosting_file_length(long file_handle)
+{
+    return semihosting_call(SEMIHOSTING_SYS_FLEN,
+                (void *) &file_handle);
+}
+
+char semihosting_read_char(void)
+{
+    return semihosting_call(SEMIHOSTING_SYS_READC, NULL);
+}
+
+void semihosting_write_char(char character)
+{
+    semihosting_call(SEMIHOSTING_SYS_WRITEC, (void *) &character);
+}
+
+void semihosting_write_string(const char *string)
+{
+    semihosting_call(SEMIHOSTING_SYS_WRITE0, (void *) string);
+}
+
+long semihosting_system(char *command_line)
+{
+    smh_system_block_t system_block;
+
+    system_block.command_line = command_line;
+    system_block.command_length = strlen(command_line);
+
+    return semihosting_call(SEMIHOSTING_SYS_SYSTEM,
+                (void *) &system_block);
+}
+
+long semihosting_get_flen(const char *file_name)
+{
+    long file_handle;
+    size_t length;
+
+    //assert(semihosting_connection_supported());
+
+    file_handle = semihosting_file_open(file_name, FOPEN_MODE_RB);
+    if (file_handle == -1)
+        return file_handle;
+
+    /* Find the length of the file */
+    length = semihosting_file_length(file_handle);
+
+    return semihosting_file_close(file_handle) ? -1 : length;
+}
+
+long semihosting_download_file(const char *file_name,
+                  size_t buf_size,
+                  uintptr_t buf)
+{
+    long ret = -EINVAL;
+    size_t length;
+    long file_handle;
+
+    /* Null pointer check */
+    if (!buf)
+        return ret;
+
+    //assert(semihosting_connection_supported());
+
+    file_handle = semihosting_file_open(file_name, FOPEN_MODE_RB);
+    if (file_handle == -1)
+        return ret;
+
+    /* Find the actual length of the file */
+    length = semihosting_file_length(file_handle);
+    if (length == -1)
+        goto semihosting_fail;
+
+    /* Signal error if we do not have enough space for the file */
+    if (length > buf_size)
+        goto semihosting_fail;
+
+    /*
+     * A successful read will return 0 in which case we pass back
+     * the actual number of bytes read. Else we pass a negative
+     * value indicating an error.
+     */
+    ret = semihosting_file_read(file_handle, &length, buf);
+    if (ret)
+        goto semihosting_fail;
+    else
+        ret = length;
+
+semihosting_fail:
+    semihosting_file_close(file_handle);
+    return ret;
+}

thermosphere/src/semihosting.h

@@ -0,0 +1,65 @@
+#pragma once
+
+// Adapted from Arm TF
+
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <stdio.h> /* For ssize_t */
+
+
+#define SEMIHOSTING_SYS_OPEN            0x01
+#define SEMIHOSTING_SYS_CLOSE           0x02
+#define SEMIHOSTING_SYS_WRITE0          0x04
+#define SEMIHOSTING_SYS_WRITEC          0x03
+#define SEMIHOSTING_SYS_WRITE           0x05
+#define SEMIHOSTING_SYS_READ            0x06
+#define SEMIHOSTING_SYS_READC           0x07
+#define SEMIHOSTING_SYS_SEEK            0x0A
+#define SEMIHOSTING_SYS_FLEN            0x0C
+#define SEMIHOSTING_SYS_REMOVE          0x0E
+#define SEMIHOSTING_SYS_SYSTEM          0x12
+#define SEMIHOSTING_SYS_ERRNO           0x13
+
+#define FOPEN_MODE_R            0x0
+#define FOPEN_MODE_RB           0x1
+#define FOPEN_MODE_RPLUS        0x2
+#define FOPEN_MODE_RPLUSB       0x3
+#define FOPEN_MODE_W            0x4
+#define FOPEN_MODE_WB           0x5
+#define FOPEN_MODE_WPLUS        0x6
+#define FOPEN_MODE_WPLUSB       0x7
+#define FOPEN_MODE_A            0x8
+#define FOPEN_MODE_AB           0x9
+#define FOPEN_MODE_APLUS        0xa
+#define FOPEN_MODE_APLUSB       0xb
+
+static inline long semihosting_connection_supported(void)
+{
+#ifdef PLATFORM_QEMU
+    return 1;
+#else
+    return 0;
+#endif
+}
+
+long semihosting_file_open(const char *file_name, size_t mode);
+long semihosting_file_seek(long file_handle, ssize_t offset);
+long semihosting_file_read(long file_handle, size_t *length, uintptr_t buffer);
+long semihosting_file_write(long file_handle,
+                size_t *length,
+                const uintptr_t buffer);
+long semihosting_file_close(long file_handle);
+long semihosting_file_length(long file_handle);
+long semihosting_system(char *command_line);
+long semihosting_get_flen(const char *file_name);
+long semihosting_download_file(const char *file_name,
+                   size_t buf_size,
+                   uintptr_t buf);
+void semihosting_write_char(char character);
+void semihosting_write_string(const char *string);
+char semihosting_read_char(void);

thermosphere/src/start.s

@@ -77,6 +77,8 @@ _startCommon:
     add     x18, x18, x10, lsl #3
     stp     x18, xzr, [sp, #-0x10]!
 
+    strb    w19, [x18, #0x14]           // isColdbootCore
+
     // Store entrypoint if first core
     cbz     x19, _store_arg
     ldr     x8, _initialKernelEntrypoint

thermosphere/src/utils.h

@@ -105,5 +105,6 @@ static inline bool check_32bit_additive_overflow(u32 a, u32 b) {
 }
 
 static inline void panic(void) {
+    __builtin_trap();
     for (;;);
 }