diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h index 41c18efe5..39e41065c 100644 --- a/thermosphere/src/core_ctx.h +++ b/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]; diff --git a/thermosphere/src/debug_log.c b/thermosphere/src/debug_log.c index 776ffb57c..9caaeba29 100644 --- a/thermosphere/src/debug_log.c +++ b/thermosphere/src/debug_log.c @@ -17,8 +17,13 @@ #include #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; } diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s index 8da57b906..f51f1cdba 100644 --- a/thermosphere/src/exception_vectors.s +++ b/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: diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index 5c940adfe..d31f0dc9e 100644 --- a/thermosphere/src/main.c +++ b/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); diff --git a/thermosphere/src/semihosting.c b/thermosphere/src/semihosting.c new file mode 100644 index 000000000..7b8fe6e8d --- /dev/null +++ b/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 +#include +#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; +} \ No newline at end of file diff --git a/thermosphere/src/semihosting.h b/thermosphere/src/semihosting.h new file mode 100644 index 000000000..7eac4e2ec --- /dev/null +++ b/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 +#include /* 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); diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s index 815399666..8632ae9c4 100644 --- a/thermosphere/src/start.s +++ b/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 diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h index b678d05b0..a6f2095e7 100644 --- a/thermosphere/src/utils.h +++ b/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 (;;); }