From d104ff61cab63d822289bee560a05758a156c9de Mon Sep 17 00:00:00 2001 From: "Kate J. Temkin" Date: Fri, 20 Apr 2018 04:06:09 -0600 Subject: [PATCH] thermosphere: add in basic hypervisor skeleton --- thermosphere/Makefile | 69 ++ thermosphere/README.md | 6 + thermosphere/linker.ld | 55 + thermosphere/linker.specs | 7 + thermosphere/src/exceptions.c | 99 ++ thermosphere/src/exceptions.h | 176 ++++ thermosphere/src/lib/ini.c | 269 +++++ thermosphere/src/lib/ini.h | 130 +++ thermosphere/src/lib/printk.c | 25 + thermosphere/src/lib/printk.h | 2 + thermosphere/src/lib/vsprintf.c | 1676 +++++++++++++++++++++++++++++++ thermosphere/src/lib/vsprintf.h | 25 + thermosphere/src/main.c | 134 +++ thermosphere/src/regs.h | 62 ++ thermosphere/src/start.s | 254 +++++ 15 files changed, 2989 insertions(+) create mode 100644 thermosphere/Makefile create mode 100644 thermosphere/README.md create mode 100644 thermosphere/linker.ld create mode 100644 thermosphere/linker.specs create mode 100644 thermosphere/src/exceptions.c create mode 100644 thermosphere/src/exceptions.h create mode 100644 thermosphere/src/lib/ini.c create mode 100644 thermosphere/src/lib/ini.h create mode 100644 thermosphere/src/lib/printk.c create mode 100644 thermosphere/src/lib/printk.h create mode 100644 thermosphere/src/lib/vsprintf.c create mode 100644 thermosphere/src/lib/vsprintf.h create mode 100644 thermosphere/src/main.c create mode 100644 thermosphere/src/regs.h create mode 100644 thermosphere/src/start.s diff --git a/thermosphere/Makefile b/thermosphere/Makefile new file mode 100644 index 000000000..fad5c3a3b --- /dev/null +++ b/thermosphere/Makefile @@ -0,0 +1,69 @@ +rwildcard = $(foreach d, $(wildcard $1*), $(filter $(subst *, %, $2), $d) $(call rwildcard, $d/, $2)) + +ifeq ($(strip $(DEVKITPRO)),) +$(error "Please set DEVKITPRO in your environment. export DEVKITPRO=devkitPro") +endif + +include $(DEVKITPRO)/devkitA64/base_tools + +name := thermosphere + +dir_source := src +dir_build := build +dir_out := out + +ARCH := -march=armv8-a -mtune=cortex-a57 + +ASFLAGS := -g $(ARCH) + +CFLAGS = \ + $(ARCH) \ + -g \ + -O2 \ + -ffunction-sections \ + -fdata-sections \ + -mgeneral-regs-only \ + -fomit-frame-pointer \ + -std=gnu11 \ + -Werror \ + -Wall \ + -Wno-main + +LDFLAGS = -specs=linker.specs -g $(ARCH) + +objects = $(patsubst $(dir_source)/%.s, $(dir_build)/%.o, \ + $(patsubst $(dir_source)/%.c, $(dir_build)/%.o, \ + $(patsubst $(dir_source)/%.S, $(dir_build)/%.o, \ + $(call rwildcard, $(dir_source), *.s *.c *.S)))) + + +define bin2o + bin2s $< | $(AS) -o $(@) +endef + +.PHONY: all +all: $(dir_out)/$(name).bin + +.PHONY: clean +clean: + @rm -rf $(dir_build) + @rm -rf $(dir_out) + +$(dir_out)/$(name).bin: $(dir_build)/$(name).elf + @mkdir -p "$(@D)" + $(OBJCOPY) -S -O binary $< $@ + +$(dir_build)/$(name).elf: $(objects) + $(LINK.o) $(OUTPUT_OPTION) $^ + +$(dir_build)/%.bin.o: $(dir_build)/%.bin + @$(bin2o) + +$(dir_build)/%.o: $(dir_source)/%.c + @mkdir -p "$(@D)" + $(COMPILE.c) $(OUTPUT_OPTION) $< + +$(dir_build)/%.o: $(dir_source)/%.s + @mkdir -p "$(@D)" + $(COMPILE.c) -x assembler-with-cpp $(OUTPUT_OPTION) $< + diff --git a/thermosphere/README.md b/thermosphere/README.md new file mode 100644 index 000000000..6b4e9df6d --- /dev/null +++ b/thermosphere/README.md @@ -0,0 +1,6 @@ +Thermoshère +===== + +![License](https://img.shields.io/badge/License-GPLv2-blue.svg) + +Thermoshère is a hypervisor for the Nintendo Switch. diff --git a/thermosphere/linker.ld b/thermosphere/linker.ld new file mode 100644 index 000000000..33d3de24a --- /dev/null +++ b/thermosphere/linker.ld @@ -0,0 +1,55 @@ +OUTPUT_FORMAT("elf64-littleaarch64") +OUTPUT_ARCH(aarch64) +ENTRY(_start) + +SECTIONS +{ + . = 0x800D0000; + + . = ALIGN(4); + .text : { + PROVIDE(lds_thermo_start = .); + build/start.o (.text*) + *(.text*) + } + + . = ALIGN(8); + .rodata : { + *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.rodata*))) + } + + . = ALIGN(8); + .data : { + *(.data*) + } + + /* Uninitialised data */ + . = ALIGN(8); + PROVIDE(lds_bss_start = .); + .bss (NOLOAD) : { + *(.bss*) . = ALIGN(8); + } + PROVIDE(lds_bss_end = .); + + /* EL2 stack */ + . = ALIGN(16); + . += 0x10000; /* 64 KiB stack */ + el2_stack_end = .; + + /* Page align the end of binary */ + . = ALIGN(512); + PROVIDE(lds_el2_thermo_end = .); + + /* EL1 stack */ + . = ALIGN(16); + . += 0x10000; /* 64 KiB stack */ + el1_stack_end = .; + + lds_thermo_end = .; + + /DISCARD/ : { *(.dynstr*) } + /DISCARD/ : { *(.dynamic*) } + /DISCARD/ : { *(.plt*) } + /DISCARD/ : { *(.interp*) } + /DISCARD/ : { *(.gnu*) } +} diff --git a/thermosphere/linker.specs b/thermosphere/linker.specs new file mode 100644 index 000000000..17b095afa --- /dev/null +++ b/thermosphere/linker.specs @@ -0,0 +1,7 @@ +%rename link old_link + +*link: +%(old_link) -T linker.ld --nmagic --gc-sections + +*startfile: +crti%O%s crtbegin%O%s diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c new file mode 100644 index 000000000..c51a2f9e2 --- /dev/null +++ b/thermosphere/src/exceptions.c @@ -0,0 +1,99 @@ +/** + * Thermosphère: exception handler + * Handles all exceptions, including a return to EL2. + * + * Copyright (c) 2018 Kate J. Temkin + */ + +#include +#include +#include "exceptions.h" + +#include "lib/printk.h" + +/** + * Simple debug function that prints all of our saved registers. + */ +static void print_registers(struct guest_state *regs) +{ + // print x0-29 + for(int i = 0; i < 30; i += 2) { + printk("x%d:\t0x%p\t", i, regs->x[i]); + printk("x%d:\t0x%p\n", i + 1, regs->x[i + 1]); + } + + // print x30; don't bother with x31 (SP), as it's used by the stack that's + // storing this stuff; we really care about the saved SP anyways + printk("x30:\t0x%p\n", regs->x[30]); + + // Special registers. + printk("pc:\t0x%p\tcpsr:\t0x%p\n", regs->pc, regs->cpsr); + printk("sp_el1:\t0x%p\tsp_el0:\t0x%p\n", regs->sp_el1, regs->sp_el0); + printk("elr_el1:0x%p\tspsr_el1:0x%p\n", regs->elr_el1, regs->spsr_el1); + + // Note that we don't print ESR_EL2, as this isn't really part of the saved state. +} + +/** + * Placeholder function that triggers whenever a vector happens we're not + * expecting. Currently prints out some debug information. + */ +void unhandled_vector(struct guest_state *regs) +{ + printk("\nAn unexpected vector happened!\n"); + print_registers(regs); + printk("\n\n"); +} + + +/** + * Handles an HVC call. + */ +static void handle_hvc(struct guest_state *regs, int call_number) +{ + + switch(call_number) { + + + default: + printk("Got a HVC call from 64-bit code.\n"); + printk("Calling instruction was: hvc %d\n\n", call_number); + printk("Calling context (you can use these regs as hypercall args!):\n"); + print_registers(regs); + printk("\n\n"); + break; + } +} + + +/** + * Placeholder function that triggers whenever a user event triggers a + * synchronous interrupt. Currently, we really only care about 'hvc', + * so that's all we're going to handle here. + */ + +void handle_hypercall(struct guest_state *regs) +{ + // This is demonstration code. + // In the future, you'd stick your hypercall table here. + + switch (regs->esr_el2.ec) { + + case HSR_EC_HVC64: { + // Read the hypercall number. + int hvc_nr = regs->esr_el2.iss & 0xFFFF; + + // ... and handle the hypercall. + handle_hvc(regs, hvc_nr); + break; + } + default: + printk("Unexpected hypercall! ESR=%p\n", regs->esr_el2.bits); + print_registers(regs); + printk("\n\n"); + break; + + } +} + + diff --git a/thermosphere/src/exceptions.h b/thermosphere/src/exceptions.h new file mode 100644 index 000000000..e8b74928e --- /dev/null +++ b/thermosphere/src/exceptions.h @@ -0,0 +1,176 @@ +/** + * Thermosphère: exception handler + * Handles all exceptions, including a return to EL2. + * + * Copyright (c) 2018 Kate J. Temkin + */ + +#ifndef __EXCEPTION_H__ +#define __EXCEPTION_H__ + +/** + * Borrowed fom Xen (not copyrightable as these are facts). + * Description of the EL2 exception syndrome register. + */ +#define HSR_EC_UNKNOWN 0x00 +#define HSR_EC_WFI_WFE 0x01 +#define HSR_EC_CP15_32 0x03 +#define HSR_EC_CP15_64 0x04 +#define HSR_EC_CP14_32 0x05 /* Trapped MCR or MRC access to CP14 */ +#define HSR_EC_CP14_DBG 0x06 /* Trapped LDC/STC access to CP14 (only for debug registers) */ +#define HSR_EC_CP 0x07 /* HCPTR-trapped access to CP0-CP13 */ +#define HSR_EC_CP10 0x08 +#define HSR_EC_JAZELLE 0x09 +#define HSR_EC_BXJ 0x0a +#define HSR_EC_CP14_64 0x0c +#define HSR_EC_SVC32 0x11 +#define HSR_EC_HVC32 0x12 +#define HSR_EC_SMC32 0x13 +#define HSR_EC_HVC64 0x16 +#define HSR_EC_SMC64 0x17 +#define HSR_EC_SYSREG 0x18 +#define HSR_EC_INSTR_ABORT_LOWER_EL 0x20 +#define HSR_EC_INSTR_ABORT_CURR_EL 0x21 +#define HSR_EC_DATA_ABORT_LOWER_EL 0x24 +#define HSR_EC_DATA_ABORT_CURR_EL 0x25 +#define HSR_EC_BRK 0x3c + +/** + * Borrowed fom Xen (not copyrightable as these are facts). + * Description of the EL2 exception syndrome register. + */ +union esr { + uint32_t bits; + struct { + unsigned long iss:25; /* Instruction Specific Syndrome */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + }; + + /* Common to all conditional exception classes (0x0N, except 0x00). */ + struct hsr_cond { + unsigned long iss:20; /* Instruction Specific Syndrome */ + unsigned long cc:4; /* Condition Code */ + unsigned long ccvalid:1;/* CC Valid */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } cond; + + struct hsr_wfi_wfe { + unsigned long ti:1; /* Trapped instruction */ + unsigned long sbzp:19; + unsigned long cc:4; /* Condition Code */ + unsigned long ccvalid:1;/* CC Valid */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } wfi_wfe; + + /* reg, reg0, reg1 are 4 bits on AArch32, the fifth bit is sbzp. */ + struct hsr_cp32 { + unsigned long read:1; /* Direction */ + unsigned long crm:4; /* CRm */ + unsigned long reg:5; /* Rt */ + unsigned long crn:4; /* CRn */ + unsigned long op1:3; /* Op1 */ + unsigned long op2:3; /* Op2 */ + unsigned long cc:4; /* Condition Code */ + unsigned long ccvalid:1;/* CC Valid */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } cp32; /* HSR_EC_CP15_32, CP14_32, CP10 */ + + struct hsr_cp64 { + unsigned long read:1; /* Direction */ + unsigned long crm:4; /* CRm */ + unsigned long reg1:5; /* Rt1 */ + unsigned long reg2:5; /* Rt2 */ + unsigned long sbzp2:1; + unsigned long op1:4; /* Op1 */ + unsigned long cc:4; /* Condition Code */ + unsigned long ccvalid:1;/* CC Valid */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } cp64; /* HSR_EC_CP15_64, HSR_EC_CP14_64 */ + + struct hsr_cp { + unsigned long coproc:4; /* Number of coproc accessed */ + unsigned long sbz0p:1; + unsigned long tas:1; /* Trapped Advanced SIMD */ + unsigned long res0:14; + unsigned long cc:4; /* Condition Code */ + unsigned long ccvalid:1;/* CC Valid */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } cp; /* HSR_EC_CP */ + + struct hsr_sysreg { + unsigned long read:1; /* Direction */ + unsigned long crm:4; /* CRm */ + unsigned long reg:5; /* Rt */ + unsigned long crn:4; /* CRn */ + unsigned long op1:3; /* Op1 */ + unsigned long op2:3; /* Op2 */ + unsigned long op0:2; /* Op0 */ + unsigned long res0:3; + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; + } sysreg; /* HSR_EC_SYSREG */ + + struct hsr_iabt { + unsigned long ifsc:6; /* Instruction fault status code */ + unsigned long res0:1; + unsigned long s1ptw:1; /* Stage 2 fault during stage 1 translation */ + unsigned long res1:1; + unsigned long eat:1; /* External abort type */ + unsigned long res2:15; + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } iabt; /* HSR_EC_INSTR_ABORT_* */ + + struct hsr_dabt { + unsigned long dfsc:6; /* Data Fault Status Code */ + unsigned long write:1; /* Write / not Read */ + unsigned long s1ptw:1; /* Stage 2 fault during stage 1 translation */ + unsigned long cache:1; /* Cache Maintenance */ + unsigned long eat:1; /* External Abort Type */ + unsigned long sbzp0:4; + unsigned long ar:1; /* Acquire Release */ + unsigned long sf:1; /* Sixty Four bit register */ + unsigned long reg:5; /* Register */ + unsigned long sign:1; /* Sign extend */ + unsigned long size:2; /* Access Size */ + unsigned long valid:1; /* Syndrome Valid */ + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } dabt; /* HSR_EC_DATA_ABORT_* */ + + struct hsr_brk { + unsigned long comment:16; /* Comment */ + unsigned long res0:9; + unsigned long len:1; /* Instruction length */ + unsigned long ec:6; /* Exception Class */ + } brk; + +}; + +/** + * Structure that stores the saved register values on a hypercall. + */ +struct guest_state { + uint64_t pc; + uint64_t cpsr; + + uint64_t elr_el1; + uint64_t spsr_el1; + + uint64_t sp_el0; + uint64_t sp_el1; + + union esr esr_el2; + uint64_t x[31]; +} +__attribute__((packed)); + + + +#endif diff --git a/thermosphere/src/lib/ini.c b/thermosphere/src/lib/ini.c new file mode 100644 index 000000000..dcc50eb29 --- /dev/null +++ b/thermosphere/src/lib/ini.c @@ -0,0 +1,269 @@ +/* inih -- simple .INI file parser + +inih is released under the New BSD license (see LICENSE.txt). Go to the project +home page for more info: + +https://github.com/benhoyt/inih + +*/ + +#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include +#include +#include + +#include "ini.h" + +#if !INI_USE_STACK +#include +#endif + +#define MAX_SECTION 50 +#define MAX_NAME 50 + +/* Used by ini_parse_string() to keep track of string parsing state. */ +typedef struct { + const char* ptr; + size_t num_left; +} ini_parse_string_ctx; + +/* Strip whitespace chars off end of given string, in place. Return s. */ +static char* rstrip(char* s) +{ + char* p = s + strlen(s); + while (p > s && isspace((unsigned char)(*--p))) + *p = '\0'; + return s; +} + +/* Return pointer to first non-whitespace char in given string. */ +static char* lskip(const char* s) +{ + while (*s && isspace((unsigned char)(*s))) + s++; + return (char*)s; +} + +/* Return pointer to first char (of chars) or inline comment in given string, + or pointer to null at end of string if neither found. Inline comment must + be prefixed by a whitespace character to register as a comment. */ +static char* find_chars_or_comment(const char* s, const char* chars) +{ +#if INI_ALLOW_INLINE_COMMENTS + int was_space = 0; + while (*s && (!chars || !strchr(chars, *s)) && + !(was_space && strchr(INI_INLINE_COMMENT_PREFIXES, *s))) { + was_space = isspace((unsigned char)(*s)); + s++; + } +#else + while (*s && (!chars || !strchr(chars, *s))) { + s++; + } +#endif + return (char*)s; +} + +/* Version of strncpy that ensures dest (size bytes) is null-terminated. */ +static char* strncpy0(char* dest, const char* src, size_t size) +{ + strncpy(dest, src, size); + dest[size - 1] = '\0'; + return dest; +} + +/* See documentation in header file. */ +int ini_parse_stream(ini_reader reader, void* stream, ini_handler handler, + void* user) +{ + /* Uses a fair bit of stack (use heap instead if you need to) */ +#if INI_USE_STACK + char line[INI_MAX_LINE]; + int max_line = INI_MAX_LINE; +#else + char* line; + int max_line = INI_INITIAL_ALLOC; +#endif +#if INI_ALLOW_REALLOC + char* new_line; + int offset; +#endif + char section[MAX_SECTION] = ""; + char prev_name[MAX_NAME] = ""; + + char* start; + char* end; + char* name; + char* value; + int lineno = 0; + int error = 0; + +#if !INI_USE_STACK + line = (char*)malloc(INI_INITIAL_ALLOC); + if (!line) { + return -2; + } +#endif + +#if INI_HANDLER_LINENO +#define HANDLER(u, s, n, v) handler(u, s, n, v, lineno) +#else +#define HANDLER(u, s, n, v) handler(u, s, n, v) +#endif + + /* Scan through stream line by line */ + while (reader(line, max_line, stream) != NULL) { +#if INI_ALLOW_REALLOC + offset = strlen(line); + while (offset == max_line - 1 && line[offset - 1] != '\n') { + max_line *= 2; + if (max_line > INI_MAX_LINE) + max_line = INI_MAX_LINE; + new_line = realloc(line, max_line); + if (!new_line) { + free(line); + return -2; + } + line = new_line; + if (reader(line + offset, max_line - offset, stream) == NULL) + break; + if (max_line >= INI_MAX_LINE) + break; + offset += strlen(line + offset); + } +#endif + + lineno++; + + start = line; +#if INI_ALLOW_BOM + if (lineno == 1 && (unsigned char)start[0] == 0xEF && + (unsigned char)start[1] == 0xBB && + (unsigned char)start[2] == 0xBF) { + start += 3; + } +#endif + start = lskip(rstrip(start)); + + if (strchr(INI_START_COMMENT_PREFIXES, *start)) { + /* Start-of-line comment */ + } +#if INI_ALLOW_MULTILINE + else if (*prev_name && *start && start > line) { + /* Non-blank line with leading whitespace, treat as continuation + of previous name's value (as per Python configparser). */ + if (!HANDLER(user, section, prev_name, start) && !error) + error = lineno; + } +#endif + else if (*start == '[') { + /* A "[section]" line */ + end = find_chars_or_comment(start + 1, "]"); + if (*end == ']') { + *end = '\0'; + strncpy0(section, start + 1, sizeof(section)); + *prev_name = '\0'; + } + else if (!error) { + /* No ']' found on section line */ + error = lineno; + } + } + else if (*start) { + /* Not a comment, must be a name[=:]value pair */ + end = find_chars_or_comment(start, "=:"); + if (*end == '=' || *end == ':') { + *end = '\0'; + name = rstrip(start); + value = end + 1; +#if INI_ALLOW_INLINE_COMMENTS + end = find_chars_or_comment(value, NULL); + if (*end) + *end = '\0'; +#endif + value = lskip(value); + rstrip(value); + + /* Valid name[=:]value pair found, call handler */ + strncpy0(prev_name, name, sizeof(prev_name)); + if (!HANDLER(user, section, name, value) && !error) + error = lineno; + } + else if (!error) { + /* No '=' or ':' found on name[=:]value line */ + error = lineno; + } + } + +#if INI_STOP_ON_FIRST_ERROR + if (error) + break; +#endif + } + +#if !INI_USE_STACK + free(line); +#endif + + return error; +} + +/* See documentation in header file. */ +int ini_parse_file(FILE* file, ini_handler handler, void* user) +{ + return ini_parse_stream((ini_reader)fgets, file, handler, user); +} + +/* See documentation in header file. */ +int ini_parse(const char* filename, ini_handler handler, void* user) +{ + FILE* file; + int error; + + file = fopen(filename, "r"); + if (!file) + return -1; + error = ini_parse_file(file, handler, user); + fclose(file); + return error; +} + +/* An ini_reader function to read the next line from a string buffer. This + is the fgets() equivalent used by ini_parse_string(). */ +static char* ini_reader_string(char* str, int num, void* stream) { + ini_parse_string_ctx* ctx = (ini_parse_string_ctx*)stream; + const char* ctx_ptr = ctx->ptr; + size_t ctx_num_left = ctx->num_left; + char* strp = str; + char c; + + if (ctx_num_left == 0 || num < 2) + return NULL; + + while (num > 1 && ctx_num_left != 0) { + c = *ctx_ptr++; + ctx_num_left--; + *strp++ = c; + if (c == '\n') + break; + num--; + } + + *strp = '\0'; + ctx->ptr = ctx_ptr; + ctx->num_left = ctx_num_left; + return str; +} + +/* See documentation in header file. */ +int ini_parse_string(const char* string, ini_handler handler, void* user) { + ini_parse_string_ctx ctx; + + ctx.ptr = string; + ctx.num_left = strlen(string); + return ini_parse_stream((ini_reader)ini_reader_string, &ctx, handler, + user); +} diff --git a/thermosphere/src/lib/ini.h b/thermosphere/src/lib/ini.h new file mode 100644 index 000000000..f45ba40ba --- /dev/null +++ b/thermosphere/src/lib/ini.h @@ -0,0 +1,130 @@ +/* inih -- simple .INI file parser + +inih is released under the New BSD license (see LICENSE.txt). Go to the project +home page for more info: + +https://github.com/benhoyt/inih + +*/ + +#ifndef __INI_H__ +#define __INI_H__ + +/* Make this header file easier to include in C++ code */ +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* Nonzero if ini_handler callback should accept lineno parameter. */ +#ifndef INI_HANDLER_LINENO +#define INI_HANDLER_LINENO 0 +#endif + +/* Typedef for prototype of handler function. */ +#if INI_HANDLER_LINENO +typedef int (*ini_handler)(void* user, const char* section, + const char* name, const char* value, + int lineno); +#else +typedef int (*ini_handler)(void* user, const char* section, + const char* name, const char* value); +#endif + +/* Typedef for prototype of fgets-style reader function. */ +typedef char* (*ini_reader)(char* str, int num, void* stream); + +/* Parse given INI-style file. May have [section]s, name=value pairs + (whitespace stripped), and comments starting with ';' (semicolon). Section + is "" if name=value pair parsed before any section heading. name:value + pairs are also supported as a concession to Python's configparser. + + For each name=value pair parsed, call handler function with given user + pointer as well as section, name, and value (data only valid for duration + of handler call). Handler should return nonzero on success, zero on error. + + Returns 0 on success, line number of first error on parse error (doesn't + stop on first error), -1 on file open error, or -2 on memory allocation + error (only when INI_USE_STACK is zero). +*/ +int ini_parse(const char* filename, ini_handler handler, void* user); + +/* Same as ini_parse(), but takes a FILE* instead of filename. This doesn't + close the file when it's finished -- the caller must do that. */ +int ini_parse_file(FILE* file, ini_handler handler, void* user); + +/* Same as ini_parse(), but takes an ini_reader function pointer instead of + filename. Used for implementing custom or string-based I/O (see also + ini_parse_string). */ +int ini_parse_stream(ini_reader reader, void* stream, ini_handler handler, + void* user); + +/* Same as ini_parse(), but takes a zero-terminated string with the INI data +instead of a file. Useful for parsing INI data from a network socket or +already in memory. */ +int ini_parse_string(const char* string, ini_handler handler, void* user); + +/* Nonzero to allow multi-line value parsing, in the style of Python's + configparser. If allowed, ini_parse() will call the handler with the same + name for each subsequent line parsed. */ +#ifndef INI_ALLOW_MULTILINE +#define INI_ALLOW_MULTILINE 1 +#endif + +/* Nonzero to allow a UTF-8 BOM sequence (0xEF 0xBB 0xBF) at the start of + the file. See http://code.google.com/p/inih/issues/detail?id=21 */ +#ifndef INI_ALLOW_BOM +#define INI_ALLOW_BOM 1 +#endif + +/* Chars that begin a start-of-line comment. Per Python configparser, allow + both ; and # comments at the start of a line by default. */ +#ifndef INI_START_COMMENT_PREFIXES +#define INI_START_COMMENT_PREFIXES ";#" +#endif + +/* Nonzero to allow inline comments (with valid inline comment characters + specified by INI_INLINE_COMMENT_PREFIXES). Set to 0 to turn off and match + Python 3.2+ configparser behaviour. */ +#ifndef INI_ALLOW_INLINE_COMMENTS +#define INI_ALLOW_INLINE_COMMENTS 1 +#endif +#ifndef INI_INLINE_COMMENT_PREFIXES +#define INI_INLINE_COMMENT_PREFIXES ";" +#endif + +/* Nonzero to use stack for line buffer, zero to use heap (malloc/free). */ +#ifndef INI_USE_STACK +#define INI_USE_STACK 1 +#endif + +/* Maximum line length for any line in INI file (stack or heap). Note that + this must be 3 more than the longest line (due to '\r', '\n', and '\0'). */ +#ifndef INI_MAX_LINE +#define INI_MAX_LINE 200 +#endif + +/* Nonzero to allow heap line buffer to grow via realloc(), zero for a + fixed-size buffer of INI_MAX_LINE bytes. Only applies if INI_USE_STACK is + zero. */ +#ifndef INI_ALLOW_REALLOC +#define INI_ALLOW_REALLOC 0 +#endif + +/* Initial size in bytes for heap line buffer. Only applies if INI_USE_STACK + is zero. */ +#ifndef INI_INITIAL_ALLOC +#define INI_INITIAL_ALLOC 200 +#endif + +/* Stop parsing on first error (default is to keep parsing). */ +#ifndef INI_STOP_ON_FIRST_ERROR +#define INI_STOP_ON_FIRST_ERROR 0 +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __INI_H__ */ diff --git a/thermosphere/src/lib/printk.c b/thermosphere/src/lib/printk.c new file mode 100644 index 000000000..a24958dde --- /dev/null +++ b/thermosphere/src/lib/printk.c @@ -0,0 +1,25 @@ +/** + * Kernel print functions. + */ + +#include "printk.h" + +#include "vsprintf.h" + +/** + * Temporary stand-in main printk. + * + * TODO: This should print via UART, console framebuffer, and to a ring for + * consumption by Horizon + */ +void printk(char *fmt, ...) +{ + va_list list; + char buf[512]; + va_start(list, fmt); + vsnprintf(buf, sizeof(buf), fmt, list); + + /* FIXME: print via UART */ + + va_end(list); +} diff --git a/thermosphere/src/lib/printk.h b/thermosphere/src/lib/printk.h new file mode 100644 index 000000000..006985a52 --- /dev/null +++ b/thermosphere/src/lib/printk.h @@ -0,0 +1,2 @@ + +void printk(char *fmt, ...); diff --git a/thermosphere/src/lib/vsprintf.c b/thermosphere/src/lib/vsprintf.c new file mode 100644 index 000000000..d934144c9 --- /dev/null +++ b/thermosphere/src/lib/vsprintf.c @@ -0,0 +1,1676 @@ +/* + * Copyright (C) 2011 Andrei Warkentin + * + * This program is free software ; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +/* + * linux/lib/vsprintf.c + * + * Copyright (C) 1991, 1992 Linus Torvalds + */ + +/* vsprintf.c -- Lars Wirzenius & Linus Torvalds. */ +/* + * Wirzenius wrote this portably, Torvalds fucked it up :-) + */ + +/* + * Fri Jul 13 2001 Crutcher Dunnavant + * - changed to provide snprintf and vsnprintf functions + * So Feb 1 16:51:32 CET 2004 Juergen Quade + * - scnprintf and vscnprintf + */ + +#include +#include +#include +#include +#include + +#include "vsprintf.h" + +#define USHRT_MAX ((uint16_t)(~0U)) +#define SHRT_MAX ((int16_t)(USHRT_MAX>>1)) +#define SHRT_MIN ((int16_t)(-SHRT_MAX - 1)) +#define INT_MAX ((int)(~0U>>1)) +#define INT_MIN (-INT_MAX - 1) +#define UINT_MAX (~0U) +#define LONG_MAX ((long)(~0UL>>1)) +#define LONG_MIN (-LONG_MAX - 1) +#define ULONG_MAX (~0UL) + +typedef _Bool bool_t; + +#define do_div(n,base) ({\ + uint32_t __base = (base);\ + uint32_t __rem;\ + __rem = ((uint64_t)(n)) % __base;\ + (n) = ((uint64_t)(n)) / __base;\ + __rem;\ + }) + +const char hex_asc[] = "0123456789abcdef"; +#define hex_asc_lo(x) hex_asc[((x) & 0x0f)] +#define hex_asc_hi(x) hex_asc[((x) & 0xf0) >> 4] + +static inline char *pack_hex_byte(char *buf, uint8_t byte) +{ + *buf++ = hex_asc_hi(byte); + *buf++ = hex_asc_lo(byte); + return buf; +} + +/** + * skip_spaces - Removes leading whitespace from @str. + * @str: The string to be stripped. + * + * Returns a pointer to the first non-whitespace character in @str. + */ +static char *skip_spaces(const char *str) +{ + while (isspace(*str)) + ++str; + return (char *)str; +} + + +/* Works only for digits and letters, but small and fast */ +#define TOLOWER(x) ((x) | 0x20) + +static unsigned int simple_guess_base(const char *cp) +{ + if (cp[0] == '0') { + if (TOLOWER(cp[1]) == 'x' && isxdigit(cp[2])) + return 16; + else + return 8; + } else { + return 10; + } +} + +/** + * simple_strtoull - convert a string to an unsigned long long + * @cp: The start of the string + * @endp: A pointer to the end of the parsed string will be placed here + * @base: The number base to use + */ +unsigned long long simple_strtoull(const char *cp, char **endp, unsigned int base) +{ + unsigned long long result = 0; + + if (!base) + base = simple_guess_base(cp); + + if (base == 16 && cp[0] == '0' && TOLOWER(cp[1]) == 'x') + cp += 2; + + while (isxdigit(*cp)) { + unsigned int value; + + value = isdigit(*cp) ? *cp - '0' : TOLOWER(*cp) - 'a' + 10; + if (value >= base) + break; + result = result * base + value; + cp++; + } + if (endp) + *endp = (char *)cp; + + return result; +} + +/** + * simple_strtoul - convert a string to an unsigned long + * @cp: The start of the string + * @endp: A pointer to the end of the parsed string will be placed here + * @base: The number base to use + */ +unsigned long simple_strtoul(const char *cp, char **endp, unsigned int base) +{ + return simple_strtoull(cp, endp, base); +} + +/** + * simple_strtol - convert a string to a signed long + * @cp: The start of the string + * @endp: A pointer to the end of the parsed string will be placed here + * @base: The number base to use + */ +long simple_strtol(const char *cp, char **endp, unsigned int base) +{ + if (*cp == '-') + return -simple_strtoul(cp + 1, endp, base); + + return simple_strtoul(cp, endp, base); +} + +/** + * simple_strtoll - convert a string to a signed long long + * @cp: The start of the string + * @endp: A pointer to the end of the parsed string will be placed here + * @base: The number base to use + */ +long long simple_strtoll(const char *cp, char **endp, unsigned int base) +{ + if (*cp == '-') + return -simple_strtoull(cp + 1, endp, base); + + return simple_strtoull(cp, endp, base); +} + +static +int skip_atoi(const char **s) +{ + int i = 0; + + while (isdigit(**s)) + i = i*10 + *((*s)++) - '0'; + + return i; +} + +/* Decimal conversion is by far the most typical, and is used + * for /proc and /sys data. This directly impacts e.g. top performance + * with many processes running. We optimize it for speed + * using code from + * http://www.cs.uiowa.edu/~jones/bcd/decimal.html + * (with permission from the author, Douglas W. Jones). */ + +/* Formats correctly any integer in [0,99999]. + * Outputs from one to five digits depending on input. + * On i386 gcc 4.1.2 -O2: ~250 bytes of code. */ +static +char *put_dec_trunc(char *buf, unsigned q) +{ + unsigned d3, d2, d1, d0; + d1 = (q>>4) & 0xf; + d2 = (q>>8) & 0xf; + d3 = (q>>12); + + d0 = 6*(d3 + d2 + d1) + (q & 0xf); + q = (d0 * 0xcd) >> 11; + d0 = d0 - 10*q; + *buf++ = d0 + '0'; /* least significant digit */ + d1 = q + 9*d3 + 5*d2 + d1; + if (d1 != 0) { + q = (d1 * 0xcd) >> 11; + d1 = d1 - 10*q; + *buf++ = d1 + '0'; /* next digit */ + + d2 = q + 2*d2; + if ((d2 != 0) || (d3 != 0)) { + q = (d2 * 0xd) >> 7; + d2 = d2 - 10*q; + *buf++ = d2 + '0'; /* next digit */ + + d3 = q + 4*d3; + if (d3 != 0) { + q = (d3 * 0xcd) >> 11; + d3 = d3 - 10*q; + *buf++ = d3 + '0'; /* next digit */ + if (q != 0) + *buf++ = q + '0'; /* most sign. digit */ + } + } + } + + return buf; +} +/* Same with if's removed. Always emits five digits */ +static +char *put_dec_full(char *buf, unsigned q) +{ + /* BTW, if q is in [0,9999], 8-bit ints will be enough, */ + /* but anyway, gcc produces better code with full-sized ints */ + unsigned d3, d2, d1, d0; + d1 = (q>>4) & 0xf; + d2 = (q>>8) & 0xf; + d3 = (q>>12); + + /* + * Possible ways to approx. divide by 10 + * gcc -O2 replaces multiply with shifts and adds + * (x * 0xcd) >> 11: 11001101 - shorter code than * 0x67 (on i386) + * (x * 0x67) >> 10: 1100111 + * (x * 0x34) >> 9: 110100 - same + * (x * 0x1a) >> 8: 11010 - same + * (x * 0x0d) >> 7: 1101 - same, shortest code (on i386) + */ + d0 = 6*(d3 + d2 + d1) + (q & 0xf); + q = (d0 * 0xcd) >> 11; + d0 = d0 - 10*q; + *buf++ = d0 + '0'; + d1 = q + 9*d3 + 5*d2 + d1; + q = (d1 * 0xcd) >> 11; + d1 = d1 - 10*q; + *buf++ = d1 + '0'; + + d2 = q + 2*d2; + q = (d2 * 0xd) >> 7; + d2 = d2 - 10*q; + *buf++ = d2 + '0'; + + d3 = q + 4*d3; + q = (d3 * 0xcd) >> 11; /* - shorter code */ + /* q = (d3 * 0x67) >> 10; - would also work */ + d3 = d3 - 10*q; + *buf++ = d3 + '0'; + *buf++ = q + '0'; + + return buf; +} +/* No inlining helps gcc to use registers better */ +static +char *put_dec(char *buf, unsigned long long num) +{ + while (1) { + unsigned rem; + if (num < 100000) + return put_dec_trunc(buf, num); + rem = do_div(num, 100000); + buf = put_dec_full(buf, rem); + } +} + +#define ZEROPAD 1 /* pad with zero */ +#define SIGN 2 /* unsigned/signed long */ +#define PLUS 4 /* show plus */ +#define SPACE 8 /* space if plus */ +#define LEFT 16 /* left justified */ +#define SMALL 32 /* use lowercase in hex (must be 32 == 0x20) */ +#define SPECIAL 64 /* prefix hex with "0x", octal with "0" */ + +enum format_type { + FORMAT_TYPE_NONE, /* Just a string part */ + FORMAT_TYPE_WIDTH, + FORMAT_TYPE_PRECISION, + FORMAT_TYPE_CHAR, + FORMAT_TYPE_STR, + FORMAT_TYPE_PTR, + FORMAT_TYPE_PERCENT_CHAR, + FORMAT_TYPE_INVALID, + FORMAT_TYPE_LONG_LONG, + FORMAT_TYPE_ULONG, + FORMAT_TYPE_LONG, + FORMAT_TYPE_UBYTE, + FORMAT_TYPE_BYTE, + FORMAT_TYPE_USHORT, + FORMAT_TYPE_SHORT, + FORMAT_TYPE_UINT, + FORMAT_TYPE_INT, + FORMAT_TYPE_NRCHARS, + FORMAT_TYPE_SIZE_T, + FORMAT_TYPE_PTRDIFF +}; + +struct printf_spec { + uint8_t type; /* format_type enum */ + uint8_t flags; /* flags to number() */ + uint8_t base; /* number base, 8, 10 or 16 only */ + uint8_t qualifier; /* number qualifier, one of 'hHlLtzZ' */ + int16_t field_width; /* width of output field */ + int16_t precision; /* # of digits/chars */ +}; + +static +char *number(char *buf, char *end, unsigned long long num, + struct printf_spec spec) +{ + /* we are called with base 8, 10 or 16, only, thus don't need "G..." */ + static const char digits[16] = "0123456789ABCDEF"; /* "GHIJKLMNOPQRSTUVWXYZ"; */ + + char tmp[66]; + char sign; + char locase; + int need_pfx = ((spec.flags & SPECIAL) && spec.base != 10); + int i; + + /* locase = 0 or 0x20. ORing digits or letters with 'locase' + * produces same digits or (maybe lowercased) letters */ + locase = (spec.flags & SMALL); + if (spec.flags & LEFT) + spec.flags &= ~ZEROPAD; + sign = 0; + if (spec.flags & SIGN) { + if ((signed long long)num < 0) { + sign = '-'; + num = -(signed long long)num; + spec.field_width--; + } else if (spec.flags & PLUS) { + sign = '+'; + spec.field_width--; + } else if (spec.flags & SPACE) { + sign = ' '; + spec.field_width--; + } + } + if (need_pfx) { + spec.field_width--; + if (spec.base == 16) + spec.field_width--; + } + + /* generate full string in tmp[], in reverse order */ + i = 0; + if (num == 0) + tmp[i++] = '0'; + /* Generic code, for any base: + else do { + tmp[i++] = (digits[do_div(num,base)] | locase); + } while (num != 0); + */ + else if (spec.base != 10) { /* 8 or 16 */ + int mask = spec.base - 1; + int shift = 3; + + if (spec.base == 16) + shift = 4; + do { + tmp[i++] = (digits[((unsigned char)num) & mask] | locase); + num >>= shift; + } while (num); + } else { /* base 10 */ + i = put_dec(tmp, num) - tmp; + } + + /* printing 100 using %2d gives "100", not "00" */ + if (i > spec.precision) + spec.precision = i; + /* leading space padding */ + spec.field_width -= spec.precision; + if (!(spec.flags & (ZEROPAD+LEFT))) { + while (--spec.field_width >= 0) { + if (buf < end) + *buf = ' '; + ++buf; + } + } + /* sign */ + if (sign) { + if (buf < end) + *buf = sign; + ++buf; + } + /* "0x" / "0" prefix */ + if (need_pfx) { + if (buf < end) + *buf = '0'; + ++buf; + if (spec.base == 16) { + if (buf < end) + *buf = ('X' | locase); + ++buf; + } + } + /* zero or space padding */ + if (!(spec.flags & LEFT)) { + char c = (spec.flags & ZEROPAD) ? '0' : ' '; + while (--spec.field_width >= 0) { + if (buf < end) + *buf = c; + ++buf; + } + } + /* hmm even more zero padding? */ + while (i <= --spec.precision) { + if (buf < end) + *buf = '0'; + ++buf; + } + /* actual digits of result */ + while (--i >= 0) { + if (buf < end) + *buf = tmp[i]; + ++buf; + } + /* trailing space padding */ + while (--spec.field_width >= 0) { + if (buf < end) + *buf = ' '; + ++buf; + } + + return buf; +} + +static +char *string(char *buf, char *end, const char *s, struct printf_spec spec) +{ + int len, i; + + if ((unsigned long)s == 0) + s = "(null)"; + + len = strnlen(s, spec.precision); + + if (!(spec.flags & LEFT)) { + while (len < spec.field_width--) { + if (buf < end) + *buf = ' '; + ++buf; + } + } + for (i = 0; i < len; ++i) { + if (buf < end) + *buf = *s; + ++buf; ++s; + } + while (len < spec.field_width--) { + if (buf < end) + *buf = ' '; + ++buf; + } + + return buf; +} + +static +char *uuid_string(char *buf, char *end, const uint8_t *addr, + struct printf_spec spec, const char *fmt) +{ + char uuid[sizeof("xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx")]; + char *p = uuid; + int i; + static const uint8_t be[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; + static const uint8_t le[16] = {3,2,1,0,5,4,7,6,8,9,10,11,12,13,14,15}; + const uint8_t *index = be; + bool_t uc = false; + + switch (*(++fmt)) { + case 'L': + uc = true; /* fall-through */ + case 'l': + index = le; + break; + case 'B': + uc = true; + break; + } + + for (i = 0; i < 16; i++) { + p = pack_hex_byte(p, addr[index[i]]); + switch (i) { + case 3: + case 5: + case 7: + case 9: + *p++ = '-'; + break; + } + } + + *p = 0; + + if (uc) { + p = uuid; + do { + *p = toupper(*p); + } while (*(++p)); + } + + return string(buf, end, uuid, spec); +} + +int kptr_restrict = 1; + +/* + * Show a '%p' thing. A kernel extension is that the '%p' is followed + * by an extra set of alphanumeric characters that are extended format + * specifiers. + * + * Right now we handle: + * + * - 'U' For a 16 byte UUID/GUID, it prints the UUID/GUID in the form + * "xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx" + * Options for %pU are: + * b big endian lower case hex (default) + * B big endian UPPER case hex + * l little endian lower case hex + * L little endian UPPER case hex + * big endian output byte order is: + * [0][1][2][3]-[4][5]-[6][7]-[8][9]-[10][11][12][13][14][15] + * little endian output byte order is: + * [3][2][1][0]-[5][4]-[7][6]-[8][9]-[10][11][12][13][14][15] + * - 'V' For a struct va_format which contains a format string * and va_list *, + * call vsnprintf(->format, *->va_list). + * Implements a "recursive vsnprintf". + * Do not use this feature without some mechanism to verify the + * correctness of the format string and va_list arguments. + * + */ +static +char *pointer(const char *fmt, char *buf, char *end, void *ptr, + struct printf_spec spec) +{ + if (!ptr) { + /* + * Print (null) with the same width as a pointer so it makes + * tabular output look nice. + */ + if (spec.field_width == -1) + spec.field_width = 2 * sizeof(void *); + return string(buf, end, "(null)", spec); + } + + switch (*fmt) { + case 'U': + return uuid_string(buf, end, ptr, spec, fmt); + case 'V': + return buf + vsnprintf(buf, end - buf, + ((struct va_format *)ptr)->fmt, + *(((struct va_format *)ptr)->va)); + } + spec.flags |= SMALL; + if (spec.field_width == -1) { + spec.field_width = 2 * sizeof(void *); + spec.flags |= ZEROPAD; + } + spec.base = 16; + + return number(buf, end, (unsigned long) ptr, spec); +} + +/* + * Helper function to decode printf style format. + * Each call decode a token from the format and return the + * number of characters read (or likely the delta where it wants + * to go on the next call). + * The decoded token is returned through the parameters + * + * 'h', 'l', or 'L' for integer fields + * 'z' support added 23/7/1999 S.H. + * 'z' changed to 'Z' --davidm 1/25/99 + * 't' added for ptrdiff_t + * + * @fmt: the format string + * @type of the token returned + * @flags: various flags such as +, -, # tokens.. + * @field_width: overwritten width + * @base: base of the number (octal, hex, ...) + * @precision: precision of a number + * @qualifier: qualifier of a number (long, size_t, ...) + */ +static +int format_decode(const char *fmt, struct printf_spec *spec) +{ + const char *start = fmt; + + /* we finished early by reading the field width */ + if (spec->type == FORMAT_TYPE_WIDTH) { + if (spec->field_width < 0) { + spec->field_width = -spec->field_width; + spec->flags |= LEFT; + } + spec->type = FORMAT_TYPE_NONE; + goto precision; + } + + /* we finished early by reading the precision */ + if (spec->type == FORMAT_TYPE_PRECISION) { + if (spec->precision < 0) + spec->precision = 0; + + spec->type = FORMAT_TYPE_NONE; + goto qualifier; + } + + /* By default */ + spec->type = FORMAT_TYPE_NONE; + + for (; *fmt ; ++fmt) { + if (*fmt == '%') + break; + } + + /* Return the current non-format string */ + if (fmt != start || !*fmt) + return fmt - start; + + /* Process flags */ + spec->flags = 0; + + while (1) { /* this also skips first '%' */ + bool_t found = true; + + ++fmt; + + switch (*fmt) { + case '-': spec->flags |= LEFT; break; + case '+': spec->flags |= PLUS; break; + case ' ': spec->flags |= SPACE; break; + case '#': spec->flags |= SPECIAL; break; + case '0': spec->flags |= ZEROPAD; break; + default: found = false; + } + + if (!found) + break; + } + + /* get field width */ + spec->field_width = -1; + + if (isdigit(*fmt)) + spec->field_width = skip_atoi(&fmt); + else if (*fmt == '*') { + /* it's the next argument */ + spec->type = FORMAT_TYPE_WIDTH; + return ++fmt - start; + } + +precision: + /* get the precision */ + spec->precision = -1; + if (*fmt == '.') { + ++fmt; + if (isdigit(*fmt)) { + spec->precision = skip_atoi(&fmt); + if (spec->precision < 0) + spec->precision = 0; + } else if (*fmt == '*') { + /* it's the next argument */ + spec->type = FORMAT_TYPE_PRECISION; + return ++fmt - start; + } + } + +qualifier: + /* get the conversion qualifier */ + spec->qualifier = -1; + if (*fmt == 'h' || TOLOWER(*fmt) == 'l' || + TOLOWER(*fmt) == 'z' || *fmt == 't') { + spec->qualifier = *fmt++; + if (spec->qualifier == *fmt) { + if (spec->qualifier == 'l') { + spec->qualifier = 'L'; + ++fmt; + } else if (spec->qualifier == 'h') { + spec->qualifier = 'H'; + ++fmt; + } + } + } + + /* default base */ + spec->base = 10; + switch (*fmt) { + case 'c': + spec->type = FORMAT_TYPE_CHAR; + return ++fmt - start; + + case 's': + spec->type = FORMAT_TYPE_STR; + return ++fmt - start; + + case 'p': + spec->type = FORMAT_TYPE_PTR; + return fmt - start; + /* skip alnum */ + + case 'n': + spec->type = FORMAT_TYPE_NRCHARS; + return ++fmt - start; + + case '%': + spec->type = FORMAT_TYPE_PERCENT_CHAR; + return ++fmt - start; + + /* integer number formats - set up the flags and "break" */ + case 'o': + spec->base = 8; + break; + + case 'x': + spec->flags |= SMALL; + + case 'X': + spec->base = 16; + break; + + case 'd': + case 'i': + spec->flags |= SIGN; + case 'u': + break; + + default: + spec->type = FORMAT_TYPE_INVALID; + return fmt - start; + } + + if (spec->qualifier == 'L') + spec->type = FORMAT_TYPE_LONG_LONG; + else if (spec->qualifier == 'l') { + if (spec->flags & SIGN) + spec->type = FORMAT_TYPE_LONG; + else + spec->type = FORMAT_TYPE_ULONG; + } else if (TOLOWER(spec->qualifier) == 'z') { + spec->type = FORMAT_TYPE_SIZE_T; + } else if (spec->qualifier == 't') { + spec->type = FORMAT_TYPE_PTRDIFF; + } else if (spec->qualifier == 'H') { + if (spec->flags & SIGN) + spec->type = FORMAT_TYPE_BYTE; + else + spec->type = FORMAT_TYPE_UBYTE; + } else if (spec->qualifier == 'h') { + if (spec->flags & SIGN) + spec->type = FORMAT_TYPE_SHORT; + else + spec->type = FORMAT_TYPE_USHORT; + } else { + if (spec->flags & SIGN) + spec->type = FORMAT_TYPE_INT; + else + spec->type = FORMAT_TYPE_UINT; + } + + return ++fmt - start; +} + +/** + * vsnprintf - Format a string and place it in a buffer + * @buf: The buffer to place the result into + * @size: The size of the buffer, including the trailing null space + * @fmt: The format string to use + * @args: Arguments for the format string + * + * This function follows C99 vsnprintf, but has some extensions: + * %pS output the name of a text symbol with offset + * %ps output the name of a text symbol without offset + * %pF output the name of a function pointer with its offset + * %pf output the name of a function pointer without its offset + * %pB output the name of a backtrace symbol with its offset + * %pR output the address range in a struct resource with decoded flags + * %pr output the address range in a struct resource with raw flags + * %pM output a 6-byte MAC address with colons + * %pm output a 6-byte MAC address without colons + * %pI4 print an IPv4 address without leading zeros + * %pi4 print an IPv4 address with leading zeros + * %pI6 print an IPv6 address with colons + * %pi6 print an IPv6 address without colons + * %pI6c print an IPv6 address as specified by + * http://tools.ietf.org/html/draft-ietf-6man-text-addr-representation-00 + * %pU[bBlL] print a UUID/GUID in big or little endian using lower or upper + * case. + * %n is ignored + * + * The return value is the number of characters which would + * be generated for the given input, excluding the trailing + * '\0', as per ISO C99. If you want to have the exact + * number of characters written into @buf as return value + * (not including the trailing '\0'), use vscnprintf(). If the + * return is greater than or equal to @size, the resulting + * string is truncated. + * + * Call this function if you are already dealing with a va_list. + * You probably want snprintf() instead. + */ +int vsnprintf(char *buf, size_t size, const char *fmt, va_list args) +{ + unsigned long long num; + char *str, *end; + struct printf_spec spec = {0}; + + str = buf; + end = buf + size; + + /* Make sure end is always >= buf */ + if (end < buf) { + end = ((void *)-1); + size = end - buf; + } + + while (*fmt) { + const char *old_fmt = fmt; + int read = format_decode(fmt, &spec); + + fmt += read; + + switch (spec.type) { + case FORMAT_TYPE_NONE: { + int copy = read; + if (str < end) { + if (copy > end - str) + copy = end - str; + memcpy(str, old_fmt, copy); + } + str += read; + break; + } + + case FORMAT_TYPE_WIDTH: + spec.field_width = va_arg(args, int); + break; + + case FORMAT_TYPE_PRECISION: + spec.precision = va_arg(args, int); + break; + + case FORMAT_TYPE_CHAR: { + char c; + + if (!(spec.flags & LEFT)) { + while (--spec.field_width > 0) { + if (str < end) + *str = ' '; + ++str; + + } + } + c = (unsigned char) va_arg(args, int); + if (str < end) + *str = c; + ++str; + while (--spec.field_width > 0) { + if (str < end) + *str = ' '; + ++str; + } + break; + } + + case FORMAT_TYPE_STR: + str = string(str, end, va_arg(args, char *), spec); + break; + + case FORMAT_TYPE_PTR: + str = pointer(fmt+1, str, end, va_arg(args, void *), + spec); + while (isalnum(*fmt)) + fmt++; + break; + + case FORMAT_TYPE_PERCENT_CHAR: + if (str < end) + *str = '%'; + ++str; + break; + + case FORMAT_TYPE_INVALID: + if (str < end) + *str = '%'; + ++str; + break; + + case FORMAT_TYPE_NRCHARS: { + uint8_t qualifier = spec.qualifier; + + if (qualifier == 'l') { + long *ip = va_arg(args, long *); + *ip = (str - buf); + } else if (TOLOWER(qualifier) == 'z') { + size_t *ip = va_arg(args, size_t *); + *ip = (str - buf); + } else { + int *ip = va_arg(args, int *); + *ip = (str - buf); + } + break; + } + + default: + switch (spec.type) { + case FORMAT_TYPE_LONG_LONG: + num = va_arg(args, long long); + break; + case FORMAT_TYPE_ULONG: + num = va_arg(args, unsigned long); + break; + case FORMAT_TYPE_LONG: + num = va_arg(args, long); + break; + case FORMAT_TYPE_SIZE_T: + num = va_arg(args, size_t); + break; + case FORMAT_TYPE_PTRDIFF: + num = va_arg(args, ptrdiff_t); + break; + case FORMAT_TYPE_UBYTE: + num = (unsigned char) va_arg(args, int); + break; + case FORMAT_TYPE_BYTE: + num = (signed char) va_arg(args, int); + break; + case FORMAT_TYPE_USHORT: + num = (unsigned short) va_arg(args, int); + break; + case FORMAT_TYPE_SHORT: + num = (short) va_arg(args, int); + break; + case FORMAT_TYPE_INT: + num = (int) va_arg(args, int); + break; + default: + num = va_arg(args, unsigned int); + } + + str = number(str, end, num, spec); + } + } + + if (size > 0) { + if (str < end) + *str = '\0'; + else + end[-1] = '\0'; + } + + /* the trailing null byte doesn't count towards the total */ + return str-buf; + +} + +/** + * vscnprintf - Format a string and place it in a buffer + * @buf: The buffer to place the result into + * @size: The size of the buffer, including the trailing null space + * @fmt: The format string to use + * @args: Arguments for the format string + * + * The return value is the number of characters which have been written into + * the @buf not including the trailing '\0'. If @size is == 0 the function + * returns 0. + * + * Call this function if you are already dealing with a va_list. + * You probably want scnprintf() instead. + * + * See the vsnprintf() documentation for format string extensions over C99. + */ +int vscnprintf(char *buf, size_t size, const char *fmt, va_list args) +{ + int i; + + i = vsnprintf(buf, size, fmt, args); + + if (i < size) + return i; + if (size != 0) + return size - 1; + return 0; +} + +/** + * snprintf - Format a string and place it in a buffer + * @buf: The buffer to place the result into + * @size: The size of the buffer, including the trailing null space + * @fmt: The format string to use + * @...: Arguments for the format string + * + * The return value is the number of characters which would be + * generated for the given input, excluding the trailing null, + * as per ISO C99. If the return is greater than or equal to + * @size, the resulting string is truncated. + * + * See the vsnprintf() documentation for format string extensions over C99. + */ +int snprintf(char *buf, size_t size, const char *fmt, ...) +{ + va_list args; + int i; + + va_start(args, fmt); + i = vsnprintf(buf, size, fmt, args); + va_end(args); + + return i; +} + +/** + * scnprintf - Format a string and place it in a buffer + * @buf: The buffer to place the result into + * @size: The size of the buffer, including the trailing null space + * @fmt: The format string to use + * @...: Arguments for the format string + * + * The return value is the number of characters written into @buf not including + * the trailing '\0'. If @size is == 0 the function returns 0. + */ + +int scnprintf(char *buf, size_t size, const char *fmt, ...) +{ + va_list args; + int i; + + va_start(args, fmt); + i = vscnprintf(buf, size, fmt, args); + va_end(args); + + return i; +} + +/** + * vsprintf - Format a string and place it in a buffer + * @buf: The buffer to place the result into + * @fmt: The format string to use + * @args: Arguments for the format string + * + * The function returns the number of characters written + * into @buf. Use vsnprintf() or vscnprintf() in order to avoid + * buffer overflows. + * + * Call this function if you are already dealing with a va_list. + * You probably want sprintf() instead. + * + * See the vsnprintf() documentation for format string extensions over C99. + */ +int vsprintf(char *buf, const char *fmt, va_list args) +{ + return vsnprintf(buf, INT_MAX, fmt, args); +} + +/** + * sprintf - Format a string and place it in a buffer + * @buf: The buffer to place the result into + * @fmt: The format string to use + * @...: Arguments for the format string + * + * The function returns the number of characters written + * into @buf. Use snprintf() or scnprintf() in order to avoid + * buffer overflows. + * + * See the vsnprintf() documentation for format string extensions over C99. + */ +int sprintf(char *buf, const char *fmt, ...) +{ + va_list args; + int i; + + va_start(args, fmt); + i = vsnprintf(buf, INT_MAX, fmt, args); + va_end(args); + + return i; +} + +#ifdef CONFIG_BINARY_PRINTF +/* + * bprintf service: + * vbin_printf() - VA arguments to binary data + * bstr_printf() - Binary data to text string + */ + +/** + * vbin_printf - Parse a format string and place args' binary value in a buffer + * @bin_buf: The buffer to place args' binary value + * @size: The size of the buffer(by words(32bits), not characters) + * @fmt: The format string to use + * @args: Arguments for the format string + * + * The format follows C99 vsnprintf, except %n is ignored, and its argument + * is skiped. + * + * The return value is the number of words(32bits) which would be generated for + * the given input. + * + * NOTE: + * If the return value is greater than @size, the resulting bin_buf is NOT + * valid for bstr_printf(). + */ +int vbin_printf(uint32_t *bin_buf, size_t size, const char *fmt, va_list args) +{ + struct printf_spec spec = {0}; + char *str, *end; + + str = (char *)bin_buf; + end = (char *)(bin_buf + size); + +#define save_arg(type) \ +do { \ + if (sizeof(type) == 8) { \ + unsigned long long value; \ + str = PTR_ALIGN(str, sizeof(uint32_t)); \ + value = va_arg(args, unsigned long long); \ + if (str + sizeof(type) <= end) { \ + *(uint32_t *)str = *(uint32_t *)&value; \ + *(uint32_t *)(str + 4) = *((uint32_t *)&value + 1); \ + } \ + } else { \ + unsigned long value; \ + str = PTR_ALIGN(str, sizeof(type)); \ + value = va_arg(args, int); \ + if (str + sizeof(type) <= end) \ + *(typeof(type) *)str = (type)value; \ + } \ + str += sizeof(type); \ +} while (0) + + while (*fmt) { + int read = format_decode(fmt, &spec); + + fmt += read; + + switch (spec.type) { + case FORMAT_TYPE_NONE: + case FORMAT_TYPE_INVALID: + case FORMAT_TYPE_PERCENT_CHAR: + break; + + case FORMAT_TYPE_WIDTH: + case FORMAT_TYPE_PRECISION: + save_arg(int); + break; + + case FORMAT_TYPE_CHAR: + save_arg(char); + break; + + case FORMAT_TYPE_STR: { + const char *save_str = va_arg(args, char *); + size_t len; + + if ((unsigned long)save_str > (unsigned long)-PAGE_SIZE + || (unsigned long)save_str < PAGE_SIZE) + save_str = "(null)"; + len = strlen(save_str) + 1; + if (str + len < end) + memcpy(str, save_str, len); + str += len; + break; + } + + case FORMAT_TYPE_PTR: + save_arg(void *); + /* skip all alphanumeric pointer suffixes */ + while (isalnum(*fmt)) + fmt++; + break; + + case FORMAT_TYPE_NRCHARS: { + /* skip %n 's argument */ + uint8_t qualifier = spec.qualifier; + void *skip_arg; + if (qualifier == 'l') + skip_arg = va_arg(args, long *); + else if (TOLOWER(qualifier) == 'z') + skip_arg = va_arg(args, size_t *); + else + skip_arg = va_arg(args, int *); + break; + } + + default: + switch (spec.type) { + + case FORMAT_TYPE_LONG_LONG: + save_arg(long long); + break; + case FORMAT_TYPE_ULONG: + case FORMAT_TYPE_LONG: + save_arg(unsigned long); + break; + case FORMAT_TYPE_SIZE_T: + save_arg(size_t); + break; + case FORMAT_TYPE_PTRDIFF: + save_arg(ptrdiff_t); + break; + case FORMAT_TYPE_UBYTE: + case FORMAT_TYPE_BYTE: + save_arg(char); + break; + case FORMAT_TYPE_USHORT: + case FORMAT_TYPE_SHORT: + save_arg(short); + break; + default: + save_arg(int); + } + } + } + + return (uint32_t *)(PTR_ALIGN(str, sizeof(uint32_t))) - bin_buf; +#undef save_arg +} + +/** + * bstr_printf - Format a string from binary arguments and place it in a buffer + * @buf: The buffer to place the result into + * @size: The size of the buffer, including the trailing null space + * @fmt: The format string to use + * @bin_buf: Binary arguments for the format string + * + * This function like C99 vsnprintf, but the difference is that vsnprintf gets + * arguments from stack, and bstr_printf gets arguments from @bin_buf which is + * a binary buffer that generated by vbin_printf. + * + * The format follows C99 vsnprintf, but has some extensions: + * see vsnprintf comment for details. + * + * The return value is the number of characters which would + * be generated for the given input, excluding the trailing + * '\0', as per ISO C99. If you want to have the exact + * number of characters written into @buf as return value + * (not including the trailing '\0'), use vscnprintf(). If the + * return is greater than or equal to @size, the resulting + * string is truncated. + */ +int bstr_printf(char *buf, size_t size, const char *fmt, const uint32_t *bin_buf) +{ + struct printf_spec spec = {0}; + char *str, *end; + const char *args = (const char *)bin_buf; + + if (WARN_ON_ONCE((int) size < 0)) + return 0; + + str = buf; + end = buf + size; + +#define get_arg(type) \ +({ \ + typeof(type) value; \ + if (sizeof(type) == 8) { \ + args = PTR_ALIGN(args, sizeof(uint32_t)); \ + *(uint32_t *)&value = *(uint32_t *)args; \ + *((uint32_t *)&value + 1) = *(uint32_t *)(args + 4); \ + } else { \ + args = PTR_ALIGN(args, sizeof(type)); \ + value = *(typeof(type) *)args; \ + } \ + args += sizeof(type); \ + value; \ +}) + + /* Make sure end is always >= buf */ + if (end < buf) { + end = ((void *)-1); + size = end - buf; + } + + while (*fmt) { + const char *old_fmt = fmt; + int read = format_decode(fmt, &spec); + + fmt += read; + + switch (spec.type) { + case FORMAT_TYPE_NONE: { + int copy = read; + if (str < end) { + if (copy > end - str) + copy = end - str; + memcpy(str, old_fmt, copy); + } + str += read; + break; + } + + case FORMAT_TYPE_WIDTH: + spec.field_width = get_arg(int); + break; + + case FORMAT_TYPE_PRECISION: + spec.precision = get_arg(int); + break; + + case FORMAT_TYPE_CHAR: { + char c; + + if (!(spec.flags & LEFT)) { + while (--spec.field_width > 0) { + if (str < end) + *str = ' '; + ++str; + } + } + c = (unsigned char) get_arg(char); + if (str < end) + *str = c; + ++str; + while (--spec.field_width > 0) { + if (str < end) + *str = ' '; + ++str; + } + break; + } + + case FORMAT_TYPE_STR: { + const char *str_arg = args; + args += strlen(str_arg) + 1; + str = string(str, end, (char *)str_arg, spec); + break; + } + + case FORMAT_TYPE_PTR: + str = pointer(fmt+1, str, end, get_arg(void *), spec); + while (isalnum(*fmt)) + fmt++; + break; + + case FORMAT_TYPE_PERCENT_CHAR: + case FORMAT_TYPE_INVALID: + if (str < end) + *str = '%'; + ++str; + break; + + case FORMAT_TYPE_NRCHARS: + /* skip */ + break; + + default: { + unsigned long long num; + + switch (spec.type) { + + case FORMAT_TYPE_LONG_LONG: + num = get_arg(long long); + break; + case FORMAT_TYPE_ULONG: + case FORMAT_TYPE_LONG: + num = get_arg(unsigned long); + break; + case FORMAT_TYPE_SIZE_T: + num = get_arg(size_t); + break; + case FORMAT_TYPE_PTRDIFF: + num = get_arg(ptrdiff_t); + break; + case FORMAT_TYPE_UBYTE: + num = get_arg(unsigned char); + break; + case FORMAT_TYPE_BYTE: + num = get_arg(signed char); + break; + case FORMAT_TYPE_USHORT: + num = get_arg(unsigned short); + break; + case FORMAT_TYPE_SHORT: + num = get_arg(short); + break; + case FORMAT_TYPE_UINT: + num = get_arg(unsigned int); + break; + default: + num = get_arg(int); + } + + str = number(str, end, num, spec); + } /* default: */ + } /* switch(spec.type) */ + } /* while(*fmt) */ + + if (size > 0) { + if (str < end) + *str = '\0'; + else + end[-1] = '\0'; + } + +#undef get_arg + + /* the trailing null byte doesn't count towards the total */ + return str - buf; +} + +/** + * bprintf - Parse a format string and place args' binary value in a buffer + * @bin_buf: The buffer to place args' binary value + * @size: The size of the buffer(by words(32bits), not characters) + * @fmt: The format string to use + * @...: Arguments for the format string + * + * The function returns the number of words(uint32_t) written + * into @bin_buf. + */ +int bprintf(uint32_t *bin_buf, size_t size, const char *fmt, ...) +{ + va_list args; + int ret; + + va_start(args, fmt); + ret = vbin_printf(bin_buf, size, fmt, args); + va_end(args); + + return ret; +} + +#endif /* CONFIG_BINARY_PRINTF */ + +/** + * vsscanf - Unformat a buffer into a list of arguments + * @buf: input buffer + * @fmt: format of buffer + * @args: arguments + */ +int vsscanf(const char *buf, const char *fmt, va_list args) +{ + const char *str = buf; + char *next; + char digit; + int num = 0; + uint8_t qualifier; + uint8_t base; + int16_t field_width; + bool_t is_sign; + + while (*fmt && *str) { + /* skip any white space in format */ + /* white space in format matchs any amount of + * white space, including none, in the input. + */ + if (isspace(*fmt)) { + fmt = skip_spaces(++fmt); + str = skip_spaces(str); + } + + /* anything that is not a conversion must match exactly */ + if (*fmt != '%' && *fmt) { + if (*fmt++ != *str++) + break; + continue; + } + + if (!*fmt) + break; + ++fmt; + + /* skip this conversion. + * advance both strings to next white space + */ + if (*fmt == '*') { + while (!isspace(*fmt) && *fmt != '%' && *fmt) + fmt++; + while (!isspace(*str) && *str) + str++; + continue; + } + + /* get field width */ + field_width = -1; + if (isdigit(*fmt)) + field_width = skip_atoi(&fmt); + + /* get conversion qualifier */ + qualifier = -1; + if (*fmt == 'h' || TOLOWER(*fmt) == 'l' || + TOLOWER(*fmt) == 'z') { + qualifier = *fmt++; + if (qualifier == *fmt) { + if (qualifier == 'h') { + qualifier = 'H'; + fmt++; + } else if (qualifier == 'l') { + qualifier = 'L'; + fmt++; + } + } + } + + if (!*fmt || !*str) + break; + + base = 10; + is_sign = 0; + + switch (*fmt++) { + case 'c': + { + char *s = (char *)va_arg(args, char*); + if (field_width == -1) + field_width = 1; + do { + *s++ = *str++; + } while (--field_width > 0 && *str); + num++; + } + continue; + case 's': + { + char *s = (char *)va_arg(args, char *); + if (field_width == -1) + field_width = SHRT_MAX; + /* first, skip leading white space in buffer */ + str = skip_spaces(str); + + /* now copy until next white space */ + while (*str && !isspace(*str) && field_width--) + *s++ = *str++; + *s = '\0'; + num++; + } + continue; + case 'n': + /* return number of characters read so far */ + { + int *i = (int *)va_arg(args, int*); + *i = str - buf; + } + continue; + case 'o': + base = 8; + break; + case 'x': + case 'X': + base = 16; + break; + case 'i': + base = 0; + case 'd': + is_sign = 1; + case 'u': + break; + case '%': + /* looking for '%' in str */ + if (*str++ != '%') + return num; + continue; + default: + /* invalid format; stop here */ + return num; + } + + /* have some sort of integer conversion. + * first, skip white space in buffer. + */ + str = skip_spaces(str); + + digit = *str; + if (is_sign && digit == '-') + digit = *(str + 1); + + if (!digit + || (base == 16 && !isxdigit(digit)) + || (base == 10 && !isdigit(digit)) + || (base == 8 && (!isdigit(digit) || digit > '7')) + || (base == 0 && !isdigit(digit))) + break; + + switch (qualifier) { + case 'H': /* that's 'hh' in format */ + if (is_sign) { + signed char *s = (signed char *)va_arg(args, signed char *); + *s = (signed char)simple_strtol(str, &next, base); + } else { + unsigned char *s = (unsigned char *)va_arg(args, unsigned char *); + *s = (unsigned char)simple_strtoul(str, &next, base); + } + break; + case 'h': + if (is_sign) { + short *s = (short *)va_arg(args, short *); + *s = (short)simple_strtol(str, &next, base); + } else { + unsigned short *s = (unsigned short *)va_arg(args, unsigned short *); + *s = (unsigned short)simple_strtoul(str, &next, base); + } + break; + case 'l': + if (is_sign) { + long *l = (long *)va_arg(args, long *); + *l = simple_strtol(str, &next, base); + } else { + unsigned long *l = (unsigned long *)va_arg(args, unsigned long *); + *l = simple_strtoul(str, &next, base); + } + break; + case 'L': + if (is_sign) { + long long *l = (long long *)va_arg(args, long long *); + *l = simple_strtoll(str, &next, base); + } else { + unsigned long long *l = (unsigned long long *)va_arg(args, unsigned long long *); + *l = simple_strtoull(str, &next, base); + } + break; + case 'Z': + case 'z': + { + size_t *s = (size_t *)va_arg(args, size_t *); + *s = (size_t)simple_strtoul(str, &next, base); + } + break; + default: + if (is_sign) { + int *i = (int *)va_arg(args, int *); + *i = (int)simple_strtol(str, &next, base); + } else { + unsigned int *i = (unsigned int *)va_arg(args, unsigned int*); + *i = (unsigned int)simple_strtoul(str, &next, base); + } + break; + } + num++; + + if (!next) + break; + str = next; + } + + /* + * Now we've come all the way through so either the input string or the + * format ended. In the former case, there can be a %n at the current + * position in the format that needs to be filled. + */ + if (*fmt == '%' && *(fmt + 1) == 'n') { + int *p = (int *)va_arg(args, int *); + *p = str - buf; + } + + return num; +} + +/** + * sscanf - Unformat a buffer into a list of arguments + * @buf: input buffer + * @fmt: formatting of buffer + * @...: resulting arguments + */ +int sscanf(const char *buf, const char *fmt, ...) +{ + va_list args; + int i; + + va_start(args, fmt); + i = vsscanf(buf, fmt, args); + va_end(args); + + return i; +} diff --git a/thermosphere/src/lib/vsprintf.h b/thermosphere/src/lib/vsprintf.h new file mode 100644 index 000000000..9399969bd --- /dev/null +++ b/thermosphere/src/lib/vsprintf.h @@ -0,0 +1,25 @@ +/* + * Copyright (C) 2011 Andrei Warkentin + * + * This program is free software ; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +#include +#include + +#ifndef VSPRINTF_H +#define VSPRINTF_H + +struct va_format { + const char *fmt; + va_list *va; +}; + +unsigned long long simple_strtoull(const char *cp, char **endp, unsigned int base); + +int vsnprintf(char *buf, size_t size, const char *fmt, va_list args); +int sscanf(const char *buf, const char *fmt, ...); + +#endif /* VSPRINTF_H */ diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c new file mode 100644 index 000000000..008fce338 --- /dev/null +++ b/thermosphere/src/main.c @@ -0,0 +1,134 @@ +/** + * Thermosphère hypervisor -- primary setup code + * Copyright (c) 2018 Kate J. Temkin + */ + +#include +#include +#include + +#include "regs.h" +#include "lib/printk.h" + +/** + * Switches to EL1, and then calls main_el1. + * Implemented in assembly in entry.S. + */ +void switch_to_el1(); + +/** + * C entry point for execution at EL1. + */ +void main_el1(void * fdt); + +/** + * Reference to the EL2 vector table. + * Note that the type here isn't reprsentative-- we just need the address of the label. + */ +extern uint64_t el2_vector_table; + +/** + * Clear out the system's bss. + */ +void _clear_bss(void) +{ + // These symbols don't actually have a meaningful type-- instead, + // we care about the locations at which the linker /placed/ these + // symbols, which happen to be at the start and end of the BSS. + // We use chars here to make the math easy. :) + extern char lds_bss_start, lds_bss_end; + + memset(&lds_bss_start, 0, &lds_bss_end - &lds_bss_start); +} + + +/** + * Triggered on an unrecoverable condition; prints an error message + * and terminates execution. + */ +void panic(const char * message) +{ + printk("\n\n"); + printk("-----------------------------------------------------------------\n"); + printk("PANIC: %s\n", message); + printk("-----------------------------------------------------------------\n"); + + // TODO: This should probably induce a reboot, + // rather than sticking here. + while(1); +} + + + +/** + * Launch an executable kernel image. Should be the last thing called by + * Discharge, as it does not return. + * + * @param kernel The kernel to be executed. + * @param fdt The device tree to be passed to the given kernel. + */ +void launch_kernel(const void *kernel) +{ + // Construct a function pointer to our kernel, which will allow us to + // jump there immediately. Note that we don't care what this leaves on + // the stack, as either our entire stack will be ignored, or it'll + // be torn down by the target kernel anyways. + void (*target_kernel)(void) = kernel; + + printk("Launching Horizon kernel...\n"); + target_kernel(); +} + + + +/** + * Core section of the stub-- sets up the hypervisor from up in EL2. + */ +int main(int argc, void **argv) +{ + // Read the currrent execution level... + uint32_t el = get_current_el(); + + /* Say hello. */ + printk("Welcome to Atmosph\xe8re Thermosph\xe8" "re!\n"); + printk("Running at EL%d.\n", el); + + // ... and ensure we're in EL2. + if (el != 2) { + panic("Thermosph\xe8" "re must be launched from EL2!"); + } + + // Set up the vector table for EL2, so that the HVC instruction can be used + // from EL1. This allows us to return to EL2 after starting the EL1 guest. + set_vbar_el2(&el2_vector_table); + + // TODO: + // Insert any setup you want done in EL2, here. For now, EL2 is set up + // to do almost nothing-- it doesn't take control of any hardware, + // and it hasn't set up any trap-to-hypervisor features. + printk("\nSwitching to EL1...\n"); + switch_to_el1(); +} + + +/** + * Secondary section of the stub, executed once we've surrendered + * hypervisor privileges. + */ +void main_el1(void * fdt) +{ + int rc; + (void)rc; + + // Read the currrent execution level... + uint32_t el = get_current_el(); + + // Validate that we're in EL1. + printk("Now executing from EL%d!\n", el); + if(el != 1) { + panic("Executing with more privilege than we expect!"); + } + + // If we've made it here, we failed to boot, and we can't recover. + panic("We should launch Horizon, here!"); +} diff --git a/thermosphere/src/regs.h b/thermosphere/src/regs.h new file mode 100644 index 000000000..9ac2a3921 --- /dev/null +++ b/thermosphere/src/regs.h @@ -0,0 +1,62 @@ +/** + * Thermosphère: register access primitives + * Copyright (c) Kate J. Temkin + */ + +#ifndef __REGS_H__ +#define __REGS_H__ + +/** + * Access to system registers. + */ +#define WRITE_SYSREG(sysreg, val, type) \ + asm volatile ("msr "#sysreg", %0\n" : : "r"((type)(val))) +#define READ_SYSREG(sysreg, val, type) \ + asm volatile ("mrs %0, "#sysreg"\n" : "=r"((type)(val))) + +#define READ_SYSREG_32(sysreg, val) READ_SYSREG(sysreg, val, uint32_t) +#define WRITE_SYSREG_32(sysreg, val) WRITE_SYSREG(sysreg, val, uint32_t) + +#define READ_SYSREG_64(sysreg, val) READ_SYSREG(sysreg, val, uint64_t) +#define WRITE_SYSREG_64(sysreg, val) WRITE_SYSREG(sysreg, val, uint64_t) + + +/** + * Returns the system's current Execution Level (EL). + */ +inline static uint32_t get_current_el(void) { + uint32_t val; + + // Read the CurrentEl register, and extract the bits that tell us our EL. + READ_SYSREG_32(CurrentEl, val); + return val >> 2; +} + +/** + * Sets the base address of the EL2 exception table. + */ +inline static void set_vbar_el2(void * address) { + WRITE_SYSREG_64(vbar_el2, (uint64_t)address); +} + + +/** + * Sets the address to 'return to' when leaving EL2. + */ +inline static void set_elr_el2(void * address) { + WRITE_SYSREG_64(elr_el2, (uint64_t)address); +} + + +/** + * Returns the MMU status bit from the SCTLR register. + */ +inline static uint32_t get_el2_mmu_status(void) { + uint32_t val; + + // Read the CurrentEl register, and extract the bits that tell us our EL. + READ_SYSREG_32(sctlr_el2, val); + return val & 1; +} + +#endif diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s new file mode 100644 index 000000000..02a732bd4 --- /dev/null +++ b/thermosphere/src/start.s @@ -0,0 +1,254 @@ +/** + * Thermosphère hypervisor entry points + * This file contains all entry points (e.g. when launching or switching ELs). + * + * Copyright (c) Kate J. Temkin + */ + +.section ".text" +.global _start + + +/** + * Simple macro to help with generating vector table entries. + */ +.macro ventry label + .align 7 + b \label +.endm + + +/** + * Start of day code. This is the first code that executes after we're launched + * by the bootloader. We use this only to set up a C environment. + */ +_start: + + // Create a simple stack for the hypervisor, while executing in EL2. + ldr x1, =el2_stack_end + mov sp, x1 + + // Clear out our binary's bss. + stp x0, x1, [sp, #-16]! + bl _clear_bss + ldp x0, x1, [sp], #16 + + // Run the main routine. This shouldn't return. + b main + + // We shouldn't ever reach here; trap. +1: b 1b + + +/* + * Vector table for interrupts/exceptions that reach EL2. + */ +.align 11 +.global el2_vector_table; +el2_vector_table: + ventry _unhandled_vector // Synchronous EL2t + ventry _unhandled_vector // IRQ EL2t + ventry _unhandled_vector // FIQ EL2t + ventry _unhandled_vector // Error EL2t + + ventry _unhandled_vector // Synchronous EL2h + ventry _unhandled_vector // IRQ EL2h + ventry _unhandled_vector // FIQ EL2h + ventry _unhandled_vector // Error EL2h + + ventry _handle_hypercall // Synchronous 64-bit EL0/EL1 + ventry _unhandled_vector // IRQ 64-bit EL0/EL1 + ventry _unhandled_vector // FIQ 64-bit EL0/EL1 + ventry _unhandled_vector // Error 64-bit EL0/EL1 + + ventry _unhandled_vector // Synchronous 32-bit EL0/EL1 + ventry _unhandled_vector // IRQ 32-bit EL0/EL1 + ventry _unhandled_vector // FIQ 32-bit EL0/EL1 + ventry _unhandled_vector // Error 32-bit EL0/EL1 + + + +/* + * Switch down to EL1 and then execute the second half of our stub. + * Implemented in assembly, as this manipulates the stack. + * + * Obliterates the stack, but leaves the rest of memory intact. This should be + * fine, as we should be hiding the EL2 memory from the rest of the system. + * + * x0: The location of the device tree to be passed into EL0. + */ +.global switch_to_el1 +switch_to_el1: + + // Set up a post-EL1-switch return address... + ldr x2, =_post_el1_switch + msr elr_el2, x2 + + // .. and set up the CPSR after we switch to EL1. + // We overwrite the saved program status register. Note that setting + // this with the EL = EL1 is what actually causes the switch. + mov x2, #0x3c5 // EL1_SP1 | D | A | I | F + msr spsr_el2, x2 + + // Reset the stack pointer to the very end of the stack, so it's + // fresh and clean for when we jump back up into EL2. + ldr x2, =el2_stack_end + mov sp, x2 + + // ... and switch down to EL1. (This essentially asks the processor + // to switch down to EL1 and then load ELR_EL2 to the PC.) + eret + + +/* + * Entry point after the switch to EL1. + */ +.global _post_el1_switch +_post_el1_switch: + + // Create a simple stack for us to use while at EL1. + // We use this temporarily to launch Horizon. + ldr x2, =el1_stack_end + mov sp, x2 + + // Run the main routine. This shouldn't return. + b main_el1 + + // We shouldn't ever reach here; trap. +1: b 1b + + +/** + * Push and pop 'psuedo-op' macros that simplify the ARM syntax to make the below pretty. + */ +.macro push, xreg1, xreg2 + stp \xreg1, \xreg2, [sp, #-16]! +.endm +.macro pop, xreg1, xreg2 + ldp \xreg1, \xreg2, [sp], #16 +.endm + +/** + * Macro that saves registers onto the stack when entering an exception handler-- + * effectively saving the guest state. Once this method is complete, *sp will + * point to a struct guest_state. + * + * You can modify this to save whatever you'd like, but: + * 1) We can only push in pairs due to armv8 architecture quirks. + * 2) Be careful not to trounce registers until after you've saved them. + * 3) r31 is your stack pointer, and doesn't need to be saved. You'll want to + * save the lesser EL's stack pointers separately. + * 4) Make sure any changes you make are reflected both in _restore_registers_ + * and in struct guest_state, or things will break pretty badly. + */ +.macro save_registers + // General purpose registers x1 - x30 + push x29, x30 + push x27, x28 + push x25, x26 + push x23, x24 + push x21, x22 + push x19, x20 + push x17, x18 + push x15, x16 + push x13, x14 + push x11, x12 + push x9, x10 + push x7, x8 + push x5, x6 + push x3, x4 + push x1, x2 + + // x0 and the el2_esr + mrs x20, esr_el2 + push x20, x0 + + // the el1_sp and el0_sp + mrs x0, sp_el0 + mrs x1, sp_el1 + push x0, x1 + + // the el1 elr/spsr + mrs x0, elr_el1 + mrs x1, spsr_el1 + push x0, x1 + + // the el2 elr/spsr + mrs x0, elr_el2 + mrs x1, spsr_el2 + push x0, x1 + +.endm + +/** + * Macro that restores registers when returning from EL2. + * Mirrors save_registers. + */ +.macro restore_registers + // the el2 elr/spsr + pop x0, x1 + msr elr_el2, x0 + msr spsr_el2, x1 + + // the el1 elr/spsr + pop x0, x1 + msr elr_el1, x0 + msr spsr_el1, x1 + + // the el1_sp and el0_sp + pop x0, x1 + msr sp_el0, x0 + msr sp_el1, x1 + + // x0, and the el2_esr + // Note that we don't restore el2_esr, as this wouldn't + // have any meaning. + pop x20, x0 + + // General purpose registers x1 - x30 + pop x1, x2 + pop x3, x4 + pop x5, x6 + pop x7, x8 + pop x9, x10 + pop x11, x12 + pop x13, x14 + pop x15, x16 + pop x17, x18 + pop x19, x20 + pop x21, x22 + pop x23, x24 + pop x25, x26 + pop x27, x28 + pop x29, x30 +.endm + +/* + * Handler for any vector we're not equipped to handle. + */ +_unhandled_vector: + // TODO: Save interrupt state and turn off interrupts. + save_registers + + // Point x0 at our saved registers, and then call our C handler. + mov x0, sp + bl unhandled_vector + + restore_registers + eret + + +/* + * Handler for any synchronous event coming from the guest (any trap-to-EL2). + * This _stub_ only uses this to handle hypercalls-- hence the name. + */ +_handle_hypercall: + // TODO: Save interrupt state and turn off interrupts. + save_registers + + // Point x0 at our saved registers, and then call our C handler. + mov x0, sp + bl handle_hypercall + + restore_registers + eret