thermosphere: fix watchpoint creation, fix wp&bp allocation

We don't need to forcefully clear the line level

exosphere/src/bootup.c

@@ -315,7 +315,7 @@ void setup_current_core_state(void) {
     uint64_t temp_reg;
 
     /* Setup system registers. */
-    SET_SYSREG(spsr_el3, 0b1111 << 6 | 0b0101); /* use EL2h+DAIF set initially, may be overwritten later. Not in official code */
+    SET_SYSREG(spsr_el3, 0b1111 << 6 | 0b1001); /* use EL2h+DAIF set initially, may be overwritten later. Not in official code */
 
     SET_SYSREG(actlr_el3, 0x73ull);
     SET_SYSREG(actlr_el2, 0x73ull);

fusee/fusee-secondary/src/package2.c

@@ -35,9 +35,10 @@
 
 static void package2_decrypt(package2_header_t *package2);
 static size_t package2_get_src_section(void **section, package2_header_t *package2, unsigned int id);
-static size_t package2_get_thermosphere(void **thermosphere);
+static size_t package2_get_thermosphere(const void **thermosphere);
 static ini1_header_t *package2_rebuild_ini1(ini1_header_t *ini1, uint32_t target_firmware, void *emummc, size_t emummc_size);
-static void package2_append_section(unsigned int id, package2_header_t *package2, void *data, size_t size);
+static void package2_append_section(unsigned int id, package2_header_t *package2, const void *data, size_t size);
+static void package2_fixup_thermosphere_and_entrypoint(package2_header_t *package2);
 static void package2_fixup_header_and_section_hashes(package2_header_t *package2, size_t size);
 
 static inline size_t align_to_4(size_t s) {
@@ -50,7 +51,7 @@ void package2_rebuild_and_copy(package2_header_t *package2, uint32_t target_firm
     void *kernel;
     size_t kernel_size;
     bool is_sd_kernel = false;
-    void *thermosphere;
+    const void *thermosphere;
     size_t thermosphere_size;
     ini1_header_t *orig_ini1, *rebuilt_ini1;
 
@@ -67,6 +68,8 @@ void package2_rebuild_and_copy(package2_header_t *package2, uint32_t target_firm
         fatal_error(u8"Error: Package2 has no unused section for Thermosphère!\n");
     }
 
+    package2->metadata.section_offsets[PACKAGE2_SECTION_UNUSED] = 0; /* base of DRAM */
+
     /* Load Kernel from SD, if possible. */
     {
         size_t sd_kernel_size = get_file_size("atmosphere/kernel.bin");
@@ -125,6 +128,9 @@ void package2_rebuild_and_copy(package2_header_t *package2, uint32_t target_firm
     package2_append_section(PACKAGE2_SECTION_INI1, rebuilt_package2, rebuilt_ini1, rebuilt_ini1->size);
     package2_append_section(PACKAGE2_SECTION_UNUSED, rebuilt_package2, thermosphere, thermosphere_size);
 
+    /* Swap entrypoint if Thermosphère is present */
+    package2_fixup_thermosphere_and_entrypoint(rebuilt_package2);
+
     /* Fix all necessary data in the header to accomodate for the new patches. */
     package2_fixup_header_and_section_hashes(rebuilt_package2, rebuilt_package2_size);
 
@@ -309,12 +315,9 @@ static size_t package2_get_src_section(void **section, package2_header_t *packag
     return package2->metadata.section_sizes[id];
 }
 
-static size_t package2_get_thermosphere(void **thermosphere) {
-    /*extern const uint8_t thermosphere_bin[];
-    extern const uint32_t thermosphere_bin_size;*/
-    /* TODO: enable when tested. */
-    (*thermosphere) = NULL;
-    return 0;
+static size_t package2_get_thermosphere(const void **thermosphere) {
+    (*thermosphere) = thermosphere_bin;
+    return thermosphere_bin_size;
 }
 
 static ini1_header_t *package2_rebuild_ini1(ini1_header_t *ini1, uint32_t target_firmware, void *emummc, size_t emummc_size) {
@@ -335,7 +338,7 @@ static ini1_header_t *package2_rebuild_ini1(ini1_header_t *ini1, uint32_t target
     return merged;
 }
 
-static void package2_append_section(unsigned int id, package2_header_t *package2, void *data, size_t size) {
+static void package2_append_section(unsigned int id, package2_header_t *package2, const void *data, size_t size) {
     /* This function must be called in ascending order of id. */
     /* We assume that the loading address doesn't need to be changed. */
     uint8_t *dst = package2->data;
@@ -347,6 +350,22 @@ static void package2_append_section(unsigned int id, package2_header_t *package2
     package2->metadata.section_sizes[id] = align_to_4(size);
 }
 
+static void package2_fixup_thermosphere_and_entrypoint(package2_header_t *package2) {
+    /* Return if Thermosphère is not present */
+    if (package2->metadata.section_sizes[PACKAGE2_SECTION_UNUSED] == 0) {
+        return;
+    }
+
+    uint8_t *dst = package2->data;
+    for (unsigned int i = 0; i < PACKAGE2_SECTION_UNUSED; i++) {
+        dst += package2->metadata.section_sizes[i];
+    }
+
+    /* Swap kernel entrypoint with Thermosphère */
+    *(uint64_t *)(dst + 8) = DRAM_BASE_PHYSICAL + package2->metadata.entrypoint;
+    package2->metadata.entrypoint = 0;
+}
+
 static void package2_fixup_header_and_section_hashes(package2_header_t *package2, size_t size) {
     uint8_t *data = package2->data;
 

fusee/fusee-secondary/src/package2.h

@@ -55,6 +55,7 @@
 #define PACKAGE2_MINVER_910_CURRENT 0xE
 
 #define NX_BOOTLOADER_PACKAGE2_LOAD_ADDRESS ((void *)(0xA9800000ull))
+#define DRAM_BASE_PHYSICAL (0x80000000)
 
 typedef struct {
     union {

thermosphere/.gitignore

@@ -1 +0,0 @@
-out

thermosphere/Makefile

@@ -10,12 +10,36 @@ TOPDIR ?= $(CURDIR)
 include $(DEVKITPRO)/devkitA64/base_rules
 
 AMSBRANCH := $(shell git symbolic-ref --short HEAD)
+AMSHASH = $(shell git rev-parse --short=16 HEAD)
 AMSREV := $(AMSBRANCH)-$(shell git rev-parse --short HEAD)
 
 ifneq (, $(strip $(shell git status --porcelain 2>/dev/null)))
     AMSREV := $(AMSREV)-dirty
 endif
 
+ifeq ($(PLATFORM), qemu)
+
+export PLATFORM	:= qemu
+
+PLATFORM_SOURCES	:= src/platform/qemu
+PLATFORM_DEFINES	:= -DPLATFORM_QEMU -DMAX_CORE=4 -DMAX_BCR=6 -DMAX_WCR=4
+
+else ifeq ($(PLATFORM), tegra-t210-arm-tf)
+
+export PLATFORM := tegra-t210-arm-tf
+
+PLATFORM_SOURCES	:= src/platform/tegra
+PLATFORM_DEFINES	:= -DPLATFORM_TEGRA -DPLATFORM_TEGRA_T210_ARM_TF -DMAX_CORE=4 -DMAX_BCR=6 -DMAX_WCR=4
+
+else
+
+export PLATFORM	:= tegra-t210-nintendo
+
+PLATFORM_SOURCES	:= src/platform/tegra
+PLATFORM_DEFINES	:= -DPLATFORM_TEGRA -D DPLATFORM_TEGRA_T210_NINTENDO -DMAX_CORE=4 -DMAX_BCR=6 -DMAX_WCR=4
+
+endif
+
 #---------------------------------------------------------------------------------
 # TARGET is the name of the output
 # BUILD is the directory where object files & intermediate files will be placed
@@ -25,37 +49,41 @@ endif
 #---------------------------------------------------------------------------------
 TARGET		:=	$(notdir $(CURDIR))
 BUILD		:=	build
-SOURCES		:=	src src/lib
+SOURCES		:=	src src/platform src/gdb $(PLATFORM_SOURCES)
 DATA		:=	data
 INCLUDES	:=	include ../common/include
 
 #---------------------------------------------------------------------------------
 # options for code generation
 #---------------------------------------------------------------------------------
-ARCH	:=	-march=armv8-a -mtune=cortex-a57
-DEFINES :=  -DATMOSPHERE_GIT_BRANCH=\"$(AMSBRANCH)\" -DATMOSPHERE_GIT_REV=\"$(AMSREV)\"
-
+# Note: -ffixed-x18 and -mgeneral-regs-only are very important and must be enabled
+ARCH	:=	-march=armv8-a -mtune=cortex-a57 -mgeneral-regs-only -ffixed-x18 -Wno-psabi
+DEFINES	:=	-D__CCPLEX__ -DATMOSPHERE_GIT_BRANCH=\"$(AMSBRANCH)\" -DATMOSPHERE_GIT_REV=\"$(AMSREV)\"\
+			-DATMOSPHERE_RELEASE_VERSION_HASH="0x$(AMSHASH)" $(PLATFORM_DEFINES)
 CFLAGS	:= \
 	-g \
-		-O2 \
+	-fmacro-prefix-map=$(TOPDIR)/src/= \
+	-Os \
 	-ffunction-sections \
 	-fdata-sections \
-		-mgeneral-regs-only \
 	-fomit-frame-pointer \
+	-fno-asynchronous-unwind-tables \
+	-fstrict-volatile-bitfields \
+	-fno-unwind-tables \
 	-std=gnu11 \
-		-Werror \
 	-Wall \
+	-Werror \
 	-Wno-main \
 	$(ARCH) $(DEFINES)
 
-CFLAGS	+=	$(INCLUDE) -D__CCPLEX__
+CFLAGS	+=	$(INCLUDE) 
 
 CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions -std=gnu++11
 
-ASFLAGS	:=	-g $(ARCH)
-LDFLAGS	=	-specs=$(TOPDIR)/linker.specs -g $(ARCH) -Wl,-Map,$(notdir $*.map)
+ASFLAGS	:=	-g $(ARCH) $(DEFINES)
+LDFLAGS	=	-specs=$(TOPDIR)/linker.specs -nostartfiles -nostdlib -g $(ARCH) -Wl,-Map,$(notdir $*.map)
 
-LIBS	:=
+LIBS	:=	-lgcc
 
 #---------------------------------------------------------------------------------
 # list of directories containing libraries, this must be the top level containing
@@ -109,11 +137,29 @@ export INCLUDE	:=	$(foreach dir,$(INCLUDES),-I$(CURDIR)/$(dir)) \
 
 export LIBPATHS	:=	$(foreach dir,$(LIBDIRS),-L$(dir)/lib)
 
-.PHONY: $(BUILD) clean all
+.PHONY: $(BUILD) clean all qemu qemudbg
 
 #---------------------------------------------------------------------------------
 all: $(BUILD)
 
+ifeq ($(PLATFORM), qemu)
+
+export QEMU := qemu-system-aarch64
+#export QEMU := ~/qemu/aarch64-softmmu/qemu-system-aarch64
+
+QEMUFLAGS :=	-nographic -machine virt,virtualization=on,accel=tcg,gic-version=2 -cpu cortex-a57 -smp 4 -m 1024\
+				-kernel thermosphere.elf -d unimp,guest_errors -semihosting-config enable,target=native\
+				-chardev socket,id=uart,port=2222,host=0.0.0.0,server,nowait -chardev stdio,id=test -serial chardev:uart\
+				-monitor tcp:localhost:3333,server,nowait
+
+qemu: all
+	@$(QEMU) $(QEMUFLAGS)
+
+qemudbg: all
+	@$(QEMU) $(QEMUFLAGS) -s -S
+
+endif
+
 $(BUILD):
 	@[ -d $@ ] || mkdir -p $@
 	@$(MAKE) --no-print-directory -C $(BUILD) -f $(CURDIR)/Makefile
@@ -151,7 +197,7 @@ $(OFILES_SRC)	: $(HFILES_BIN)
 #---------------------------------------------------------------------------------
 # you need a rule like this for each extension you use as binary data
 #---------------------------------------------------------------------------------
-%.bin.o	:	%.bin
+%.bin.o	%_bin.h:	%.bin
 #---------------------------------------------------------------------------------
 	@echo $(notdir $<)
 	@$(bin2o)

thermosphere/README.md

@@ -1,6 +0,0 @@
-Thermosphère
-=====
-
-![License](https://img.shields.io/badge/License-GPLv2-blue.svg)
-
-Thermosphère is a hypervisor for the Nintendo Switch.

thermosphere/linker.ld

@@ -1,55 +1,209 @@
-OUTPUT_FORMAT("elf64-littleaarch64")
 OUTPUT_ARCH(aarch64)
 ENTRY(_start)
 
+PHDRS
+{
+    main PT_LOAD;
+}
+
+MEMORY
+{
+    mainVa  : ORIGIN = 0x7FFFE10000, LENGTH = 2M - 64K
+}
+
 SECTIONS
 {
-    . = 0x800D0000;
+    __start_pa__        = ABSOLUTE(ORIGIN(main));
+    __temp_pa__         = ABSOLUTE(ORIGIN(temp));
+    __max_image_size__  = ABSOLUTE(LENGTH(main));
+    __max_temp_size__   = ABSOLUTE(LENGTH(temp) - 0x1000);
+
+    .text :
+    {
+        . = ALIGN(8);
+        __start__ = ABSOLUTE(.);
+        KEEP(*(.crt0*));
+        *(.text.unlikely .text.*_unlikely .text.unlikely.*)
+        *(.text.exit .text.exit.*)
+        *(.text.startup .text.startup.*)
+        *(.text.hot .text.hot.*)
+        *(.text .stub .text.* .gnu.linkonce.t.*)
+        . = ALIGN(0x800);
+        __vectors_start__ = ABSOLUTE(.);
+        KEEP(*(.vectors*));
+        __vectors_end__   = ABSOLUTE(.);
+        ASSERT(__vectors_end__ - __vectors_start__ <= 0x800, "Exception vectors section should be max 0x800 in size!");
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .init :
+    {
+        KEEP( *(.init) )
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .plt :
+    {
+        *(.plt)
+        *(.iplt)
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+
+    .fini :
+    {
+        KEEP( *(.fini) )
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .rodata :
+    {
+        *(.rodata .rodata.* .gnu.linkonce.r.*)
+        SORT(CONSTRUCTORS)
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .got            : { __got_start__ = ABSOLUTE(.); *(.got) *(.igot) } >mainVa AT>main :main
+    .got.plt        : { *(.got.plt)  *(.igot.plt)  __got_end__ = ABSOLUTE(.);} >mainVa AT>main :main
+
+    .preinit_array :
+    {
+        . = ALIGN(8);
+        PROVIDE (__preinit_array_start = ABSOLUTE(.));
+        KEEP (*(.preinit_array))
+        PROVIDE (__preinit_array_end = ABSOLUTE(.));
+        ASSERT(__preinit_array_end == __preinit_array_start, ".preinit_array not empty!");
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .init_array :
+    {
+        PROVIDE (__init_array_start = ABSOLUTE(.));
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        PROVIDE (__init_array_end = ABSOLUTE(.));
+        ASSERT(__init_array_end == __init_array_start, ".init_array not empty!");
+    } >mainVa AT>main :main
+
+    .fini_array :
+    {
+        . = ALIGN(8);
+        PROVIDE (__fini_array_start = ABSOLUTE(.));
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        PROVIDE (__fini_array_end = ABSOLUTE(.));
+        . = ALIGN(8);
+        ASSERT(__fini_array_end == __fini_array_start, ".fini_array not empty!");
+    } >mainVa AT>main :main
+
+    .ctors :
+    {
+        . = ALIGN(8);
+        KEEP (*crtbegin.o(.ctors)) /* MUST be first -- GCC requires it */
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*(.ctors))
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .dtors ALIGN(8) :
+    {
+        . = ALIGN(8);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*(.dtors))
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .data ALIGN(8) :
+    {
+        *(.data .data.* .gnu.linkonce.d.*)
+        CONSTRUCTORS
+        . = ALIGN(8);
+    } >mainVa AT>main :main
+
+    .dynamic           : { *(.dynamic) } >mainVa AT>main :main
+    .interp            : { *(.interp) } >mainVa AT>main :main
+    .note.gnu.build-id : { *(.note.gnu.build-id) } >mainVa AT>main :main
+    .hash              : { *(.hash) } >mainVa AT>main :main
+    .gnu.hash          : { *(.gnu.hash) } >mainVa AT>main :main
+    .gnu.version       : { *(.gnu.version) } >mainVa AT>main :main
+    .gnu.version_d     : { *(.gnu.version_d) } >mainVa AT>main :main
+    .gnu.version_r     : { *(.gnu.version_r) } >mainVa AT>main :main
+    .dynsym            : { *(.dynsym) } >mainVa AT>main :main
+    .dynstr            : { *(.dynstr) } >mainVa AT>main :main
+    .rela.dyn          : { *(.rela.*); __main_end__ = ABSOLUTE(.);} >mainVa AT>main :main
+
+    .bss (NOLOAD) :
+    {
+        __bss_start__  = ABSOLUTE(.);
+        *(.dynbss)
+        *(.bss .bss.* .gnu.linkonce.b.*)
+        *(COMMON)
+    } >mainVa :NONE
+
+    .tempbss (NOLOAD) :
+    {
+        . = ALIGN(0x1000);
+        __real_bss_end__ = ABSOLUTE(.);
+        __image_size__ = ABSOLUTE(__real_bss_end__ - __start__);
+        /*ASSERT(__image_size__ <= __max_image_size__, "Image too big!");*/
+        *(.tempbss .tempbss.*)
+        . = ALIGN(0x1000);
+        __bss_end__ = ABSOLUTE(.);
+        __temp_size__ = ABSOLUTE(__bss_end__ - __real_bss_end__);
+        ASSERT(__temp_size__ <= __max_temp_size__, "tempbss too big!");
+    } >mainVa :NONE
+
 
-    . = ALIGN(4);
-    .text : {
-    PROVIDE(lds_thermo_start = .);
-        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*) }
+
+    /* Shit we keep in the elf but otherwise discard */
+    .eh_frame_hdr     (NOLOAD) : { *(.eh_frame_hdr) *(.eh_frame_entry .eh_frame_entry.*) } >mainVa :NONE
+    .eh_frame         (NOLOAD) : { KEEP (*(.eh_frame)) *(.eh_frame.*) } >mainVa :NONE
+    .gcc_except_table (NOLOAD) : { *(.gcc_except_table .gcc_except_table.*) } >mainVa :NONE
+    .gnu_extab        (NOLOAD) : { *(.gnu_extab*) } >mainVa :NONE
+    .exception_ranges (NOLOAD) : { *(.exception_ranges .exception_ranges*) } >mainVa :NONE
+
+    /* ==================
+       ==== Metadata ====
+       ================== */
+
+    /* Discard sections that difficult post-processing */
+    /DISCARD/ : { *(.group .comment .note) }
+
+    /* Stabs debugging sections. */
+    .stab          0 : { *(.stab) }
+    .stabstr       0 : { *(.stabstr) }
+    .stab.excl     0 : { *(.stab.excl) }
+    .stab.exclstr  0 : { *(.stab.exclstr) }
+    .stab.index    0 : { *(.stab.index) }
+    .stab.indexstr 0 : { *(.stab.indexstr) }
+
+    /* DWARF debug sections.
+       Symbols in the DWARF debugging sections are relative to the beginning
+       of the section so we begin them at 0. */
+
+    /* DWARF 1 */
+    .debug          0 : { *(.debug) }
+    .line           0 : { *(.line) }
+
+    /* GNU DWARF 1 extensions */
+    .debug_srcinfo  0 : { *(.debug_srcinfo) }
+    .debug_sfnames  0 : { *(.debug_sfnames) }
+
+    /* DWARF 1.1 and DWARF 2 */
+    .debug_aranges  0 : { *(.debug_aranges) }
+    .debug_pubnames 0 : { *(.debug_pubnames) }
+
+    /* DWARF 2 */
+    .debug_info     0 : { *(.debug_info) }
+    .debug_abbrev   0 : { *(.debug_abbrev) }
+    .debug_line     0 : { *(.debug_line) }
+    .debug_frame    0 : { *(.debug_frame) }
+    .debug_str      0 : { *(.debug_str) }
+    .debug_loc      0 : { *(.debug_loc) }
+    .debug_macinfo  0 : { *(.debug_macinfo) }
 }

thermosphere/linker.specs

@@ -1,7 +1,4 @@
 %rename link                old_link
 
 *link:
-%(old_link) -T %:getenv(TOPDIR /linker.ld) --nmagic --gc-sections
-
-*startfile:
-crti%O%s crtbegin%O%s
+%(old_link) -T %:getenv(TOPDIR /%:getenv(PLATFORM .mem)) -T %:getenv(TOPDIR /linker.ld) -no-pie --nmagic --gc-sections

thermosphere/qemu.mem

@@ -0,0 +1,6 @@
+MEMORY
+{
+    NULL  : ORIGIN = 0,           LENGTH = 0x1000
+    main  : ORIGIN = 0x60000000,  LENGTH = 64M   /* QEMU's memory map changes dynamically? */
+    temp  : ORIGIN = 0x64000000,  LENGTH = 64M
+}

thermosphere/src/asm_macros.s

@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define EXCEP_STACK_FRAME_SIZE      0x140
+
+#define CORECTX_CRASH_STACK_OFFSET  0x000
+#define CORECTX_GUEST_FRAME_OFFSET  0x040
+#define CORECTX_SCRATCH_OFFSET      0x048
+
+
+.macro FUNCTION name
+    .section        .text.\name, "ax", %progbits
+    .global         \name
+    .type           \name, %function
+    .func           \name
+    .cfi_sections   .debug_frame
+    .cfi_startproc
+    \name:
+.endm
+
+.macro END_FUNCTION
+    .cfi_endproc
+    .endfunc
+.endm

thermosphere/src/barrier.c

@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "barrier.h"
+#include "core_ctx.h"
+#include "utils.h"
+
+
+void barrierInit(Barrier *barrier, u32 coreList)
+{
+    atomic_store(&barrier->val, coreList);
+}
+
+void barrierInitAllButSelf(Barrier *barrier)
+{
+    barrierInit(barrier, getActiveCoreMask() & ~(BIT(currentCoreCtx->coreId)));
+}
+
+void barrierInitAll(Barrier *barrier)
+{
+    barrierInit(barrier, getActiveCoreMask());
+}
+
+void barrierWait(Barrier *barrier)
+{
+    atomic_fetch_and(&barrier->val, ~(BIT(currentCoreCtx->coreId)));
+    __sev();
+    do {
+        __wfe();
+    } while (atomic_load(&barrier->val) != 0);
+}

thermosphere/src/barrier.h

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdatomic.h>
+#include "types.h"
+
+typedef struct Barrier {
+    atomic_uint val;
+} Barrier;
+
+void barrierInit(Barrier *barrier, u32 coreList);
+void barrierInitAllButSelf(Barrier *barrier);
+void barrierInitAll(Barrier *barrier);
+
+void barrierWait(Barrier *barrier);

thermosphere/src/breakpoints.c

@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+#include "breakpoints.h"
+#include "breakpoints_watchpoints_load.h"
+#include "utils.h"
+#include "core_ctx.h"
+
+BreakpointManager g_breakpointManager = {0};
+
+// Init the structure (already in BSS, so already zero-initialized) and load the registers
+void initBreakpoints(void)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+
+    if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) {
+        size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
+        g_breakpointManager.maxBreakpoints = (u32)num;
+        g_breakpointManager.freeBitmap = BIT(num) - 1;
+        g_breakpointManager.usedBitmap = 0;
+    }
+
+    loadBreakpointRegs(g_breakpointManager.breakpoints, g_breakpointManager.maxBreakpoints);
+
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
+}
+
+static void commitAndBroadcastBreakpointHandler(void *p)
+{
+    (void)p;
+    u64 flags = maskIrq();
+    loadBreakpointRegs(g_breakpointManager.breakpoints, g_breakpointManager.maxBreakpoints);
+    restoreInterruptFlags(flags);
+}
+
+static inline void commitAndBroadcastBreakpoints(void)
+{
+    __dmb();
+    executeFunctionOnAllCores(commitAndBroadcastBreakpointHandler, NULL, true);
+}
+
+static DebugRegisterPair *allocateBreakpoint(void)
+{
+    u32 pos = __builtin_ffs(g_breakpointManager.freeBitmap);
+    if (pos == 0) {
+        return NULL;
+    } else {
+        g_breakpointManager.freeBitmap &= ~BIT(pos - 1);
+        g_breakpointManager.usedBitmap |= BIT(pos - 1);
+        return &g_breakpointManager.breakpoints[pos - 1];
+    }
+}
+
+static void freeBreakpoint(u32 pos)
+{
+    memset(&g_breakpointManager.breakpoints[pos], 0, sizeof(DebugRegisterPair));
+    g_breakpointManager.freeBitmap |= BIT(pos);
+    g_breakpointManager.usedBitmap &= ~BIT(pos);
+}
+
+static DebugRegisterPair *findBreakpoint(uintptr_t addr)
+{
+    FOREACH_BIT (tmp, i, g_breakpointManager.usedBitmap) {
+        if (g_breakpointManager.breakpoints[i - 1].vr == addr) {
+            return &g_breakpointManager.breakpoints[i - 1];
+        }
+    }
+
+    return NULL;
+}
+
+// Note: A32/T32/T16 support intentionnally left out
+// Note: addresses are supposed to be well-formed regarding the sign extension bits
+
+int addBreakpoint(uintptr_t addr)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+
+    // Reject misaligned addresses
+    if (addr & 3) {
+        recursiveSpinlockUnlock(&g_breakpointManager.lock);
+        return -EINVAL;
+    }
+
+    // Oops
+    if (g_breakpointManager.freeBitmap == 0) {
+        return -EBUSY;
+    }
+
+    // Breakpoint already added
+    if (findBreakpoint(addr) != NULL) {
+        recursiveSpinlockUnlock(&g_breakpointManager.lock);
+        return -EEXIST;
+    }
+
+    DebugRegisterPair *regs = allocateBreakpoint();
+    regs->cr.bt = BreakpointType_AddressMatch;
+    regs->cr.linked = false;
+
+    // NS EL1&0 only
+    regs->cr.hmc = DebugHmc_LowerEl;
+    regs->cr.ssc = DebugSsc_NonSecure;
+    regs->cr.pmc = DebugPmc_El1And0;
+
+    regs->cr.bas = 0xF;
+    regs->cr.enabled = true;
+
+    regs->vr = addr;
+
+    commitAndBroadcastBreakpoints();
+
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
+
+    return 0;
+}
+
+int removeBreakpoint(uintptr_t addr)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+
+    DebugRegisterPair *regs = findBreakpoint(addr);
+
+    if (findBreakpoint(addr) == NULL) {
+        recursiveSpinlockUnlock(&g_breakpointManager.lock);
+        return -ENOENT;
+    }
+
+    freeBreakpoint(regs - &g_breakpointManager.breakpoints[0]);
+
+    commitAndBroadcastBreakpoints();
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
+
+    return 0;
+}
+
+int removeAllBreakpoints(void)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+    g_breakpointManager.freeBitmap |= g_breakpointManager.usedBitmap;
+    g_breakpointManager.usedBitmap = 0;
+    memset(g_breakpointManager.breakpoints, 0, sizeof(g_breakpointManager.breakpoints));
+
+    commitAndBroadcastBreakpoints();
+
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
+
+    return 0;
+}

thermosphere/src/breakpoints.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "breakpoints_watchpoints_common.h"
+#include "spinlock.h"
+
+#define _REENT_ONLY
+#include <errno.h>
+
+/// Structure to synchronize and keep track of breakpoints
+typedef struct BreakpointManager {
+    DebugRegisterPair breakpoints[MAX_BCR];
+    RecursiveSpinlock lock;
+    u32 maxBreakpoints;
+    u16 freeBitmap;
+    u16 usedBitmap;
+} BreakpointManager;
+
+extern BreakpointManager g_breakpointManager;
+
+void initBreakpoints(void);
+int addBreakpoint(uintptr_t addr);
+int removeBreakpoint(uintptr_t addr);
+int removeAllBreakpoints(void);

thermosphere/src/breakpoints_watchpoints_common.h

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "types.h"
+#include "sysreg.h"
+
+/// BT[3:1] or res0. BT[0]/WT[0] is "is linked"
+typedef enum BreakpointType {
+    BreakpointType_AddressMatch = 0,
+    BreakpointType_VheContextIdMatch = 1,
+    BreakpointType_ContextIdMatch = 3,
+    BreakpointType_VmidMatch = 4,
+    BreakpointType_VmidContextIdMatch = 5,
+    BreakpointType_VmidVheContextIdMatch = 6,
+    BreakpointType_FullVheContextIdMatch = 7,
+} BreakpointType;
+
+// Note: some SSC HMC PMC combinations are invalid
+// Refer to "Table D2-9 Summary of breakpoint HMC, SSC, and PMC encodings"
+
+/// Debug Security State Control
+typedef enum DebugSsc {
+    DebugSsc_Both                  = 0,
+    DebugSsc_NonSecure             = 1,
+    DebugSsc_Secure                = 2,
+    DebugSsc_SecureIfLowerOrBoth   = 3,
+} DebugSsc;
+
+/// Debug Higher Mode Control
+typedef enum DebugHmc {
+    DebugHmc_LowerEl   = 0,
+    DebugHmc_HigherEl  = 1,
+} DebugHmc;
+
+/// Debug Privilege Mode Control (called PAC for watchpoints)
+typedef enum DebugPmc {
+    DebugPmc_NeitherEl1Nor0    = 0,
+    DebugPmc_El1               = 1,
+    DebugPmc_El0               = 2,
+    DebugPmc_El1And0           = 3,
+} DebugPmc;
+
+typedef enum WatchpointLoadStoreControl {
+    WatchpointLoadStoreControl_Load         = 1,
+    WatchpointLoadStoreControl_Store        = 2,
+    WatchpointLoadStoreControl_LoadStore    = 3,
+} WatchpointLoadStoreControl;
+
+// bas only 4 bits for breakpoints, other bits res0.
+// lsc, mask only for watchpoints, res0 for breakpoints
+// bt only from breakpoints, res0 for watchpoints
+typedef struct DebugControlRegister {
+    bool enabled                        : 1;
+    DebugPmc pmc                        : 2;
+    WatchpointLoadStoreControl lsc      : 2;
+    u32 bas                             : 8;
+    DebugHmc hmc                        : 1;
+    DebugSsc ssc                        : 2;
+    u32 lbn                             : 4;
+    bool linked                         : 1;
+    BreakpointType bt                   : 3;
+    u32 mask                            : 5;
+    u64 res0                            : 35;
+} DebugControlRegister;
+
+typedef struct DebugRegisterPair {
+    DebugControlRegister cr;
+    u64 vr;
+} DebugRegisterPair;
+
+static inline void enableBreakpointsAndWatchpoints(void)
+{
+    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_MDE);
+}

thermosphere/src/breakpoints_watchpoints_load.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include "breakpoints_watchpoints_common.h"
+
+void loadBreakpointRegs(const DebugRegisterPair *regs, size_t num);
+void loadWatchpointRegs(const DebugRegisterPair *regs, size_t num);

thermosphere/src/breakpoints_watchpoints_load.s

@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "asm_macros.s"
+
+.altmacro
+
+.macro LOAD_DBG_REG_PAIRS what, id
+    ldp     x2, x3, [x0, #-0x10]!
+    msr     dbg\what\()cr\id\()_el1, x2
+    msr     dbg\what\()vr\id\()_el1, x3
+    .if     \id != 0
+        LOAD_DBG_REG_PAIRS      \what, %(\id - 1)
+    .endif
+.endm
+
+// Precondition: x1 <= 16
+FUNCTION loadBreakpointRegs
+    // x1 = number
+    dmb     ish
+
+    adr     x16, 1f
+    add     x0, x0, #(MAX_BCR * 8)
+    mov     x4, #(MAX_BCR * 12)
+    sub     x4, x4, x1,lsl #3
+    sub     x4, x4, x1,lsl #2
+    add     x16, x16, x4
+    br      x16
+
+    1:
+    LOAD_DBG_REG_PAIRS b, %(MAX_BCR - 1)
+
+    dsb     ish
+    isb
+    ret
+END_FUNCTION
+
+// Precondition: x1 <= 16
+FUNCTION loadWatchpointRegs
+    // x1 = number
+    dmb     ish
+
+    adr     x16, 1f
+    add     x0, x0, #(MAX_WCR * 8)
+    mov     x4, #(MAX_WCR * 12)
+    sub     x4, x4, x1,lsl #3
+    sub     x4, x4, x1,lsl #2
+    add     x16, x16, x4
+    br      x16
+
+    1:
+    LOAD_DBG_REG_PAIRS w, %(MAX_WCR - 1)
+
+    dsb     ish
+    isb
+    ret
+END_FUNCTION

thermosphere/src/caches.c

@@ -0,0 +1,166 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "caches.h"
+#include "preprocessor.h"
+#include "core_ctx.h"
+
+#define DEFINE_CACHE_RANGE_FUNC(isn, name, cache, post)\
+void name(const void *addr, size_t size)\
+{\
+    u32 lineCacheSize = cacheGetSmallest##cache##CacheLineSize();\
+    uintptr_t begin = (uintptr_t)addr & ~(lineCacheSize - 1);\
+    uintptr_t end = ((uintptr_t)addr + size + lineCacheSize - 1) & ~(lineCacheSize - 1);\
+    for (uintptr_t pos = begin; pos < end; pos += lineCacheSize) {\
+        __asm__ __volatile__ (isn ", %0" :: "r"(pos) : "memory");\
+    }\
+    post;\
+}
+
+static inline ALINLINE void cacheSelectByLevel(bool instructionCache, u32 level)
+{
+    u32 ibit  = instructionCache ? 1 : 0;
+    u32 lbits = (level & 7) << 1;
+    SET_SYSREG(csselr_el1, lbits | ibit);
+    __isb();
+}
+
+static inline ALINLINE void cacheInvalidateDataCacheLevel(u32 level)
+{
+    cacheSelectByLevel(false, level);
+    u32 ccsidr = (u32)GET_SYSREG(ccsidr_el1);
+    u32 numWays = 1 + ((ccsidr >> 3) & 0x3FF);
+    u32 numSets = 1 + ((ccsidr >> 13) & 0x7FFF);
+    u32 wayShift = __builtin_clz(numWays);
+    u32 setShift = (ccsidr & 7) + 4;
+    u32 lbits = (level & 7) << 1;
+
+    for (u32 way = 0; way < numWays; way++) {
+        for (u32 set = 0; set < numSets; set++) {
+            u64 val = ((u64)way << wayShift) | ((u64)set << setShift) | lbits;
+            __asm__ __volatile__ ("dc isw, %0" :: "r"(val) : "memory");
+        }
+    }
+}
+
+static inline ALINLINE void cacheCleanInvalidateDataCacheLevel(u32 level)
+{
+    cacheSelectByLevel(false, level);
+    u32 ccsidr = (u32)GET_SYSREG(ccsidr_el1);
+    u32 numWays = 1 + ((ccsidr >> 3) & 0x3FF);
+    u32 numSets = 1 + ((ccsidr >> 13) & 0x7FFF);
+    u32 wayShift = __builtin_clz(numWays);
+    u32 setShift = (ccsidr & 7) + 4;
+    u32 lbits = (level & 7) << 1;
+
+    for (u32 way = 0; way < numWays; way++) {
+        for (u32 set = 0; set < numSets; set++) {
+            u64 val = ((u64)way << wayShift) | ((u64)set << setShift) | lbits;
+            __asm__ __volatile__ ("dc cisw, %0" :: "r"(val) : "memory");
+        }
+    }
+
+    __dsb_sy();
+    __isb();
+}
+
+static inline ALINLINE void cacheInvalidateDataCacheLevels(u32 from, u32 to)
+{
+    // Let's hope it doesn't generate a stack frame...
+    for (u32 level = from; level < to; level++) {
+        cacheInvalidateDataCacheLevel(level);
+    }
+
+    __dsb_sy();
+    __isb();
+}
+
+DEFINE_CACHE_RANGE_FUNC("dc civac", cacheCleanInvalidateDataCacheRange, Data, __dsb())
+DEFINE_CACHE_RANGE_FUNC("dc cvau", cacheCleanDataCacheRangePoU, Data, __dsb())
+DEFINE_CACHE_RANGE_FUNC("ic ivau", cacheInvalidateInstructionCacheRangePoU, Instruction, __dsb(); __isb())
+
+void cacheHandleSelfModifyingCodePoU(const void *addr, size_t size)
+{
+    // See docs for ctr_el0.{dic, idc}. It's unclear when these bits have been added, but they're
+    // RES0 if not implemented, so that's fine
+    u32 ctr = (u32)GET_SYSREG(ctr_el0);
+    if (!(ctr & BIT(28))) {
+        cacheCleanDataCacheRangePoU(addr, size);
+    }
+    if (!(ctr & BIT(29))) {
+        cacheInvalidateInstructionCacheRangePoU(addr, size);
+    }
+}
+
+void cacheClearSharedDataCachesOnBoot(void)
+{
+    u32 clidr = (u32)GET_SYSREG(clidr_el1);
+    u32 louis = (clidr >> 21) & 7;
+    u32 loc   = (clidr >> 24) & 7;
+    cacheInvalidateDataCacheLevels(louis, loc);
+}
+
+void cacheClearLocalDataCacheOnBoot(void)
+{
+    u32 clidr = (u32)GET_SYSREG(clidr_el1);
+    u32 louis = (clidr >> 21) & 7;
+    cacheInvalidateDataCacheLevels(0, louis);
+}
+
+
+/* Ok so:
+    - cache set/way ops can't really be virtualized
+    - since we have only one guest OS & don't care about security (for space limitations),
+    we do the following:
+        - ignore all cache s/w ops applying before the Level Of Unification Inner Shareable (L1, typically).
+        These clearly break coherency and should only be done once, on power on/off/suspend/resume only. And we already
+        do it ourselves...
+        - allow ops after the LoUIS, but do it ourselves and ignore the next (numSets*numWay - 1) requests. This is because
+        we have to handle Nintendo's dodgy code
+        - ignore "invalidate only" ops by the guest. Should only be done on power on/resume and we already did it ourselves...
+        - transform "clean only" into "clean and invalidate"
+*/
+void cacheHandleTrappedSetWayOperation(bool invalidateOnly)
+{
+    DEBUG("hello");
+    if (invalidateOnly) {
+        return;
+    }
+
+    u32 clidr = (u32)GET_SYSREG(clidr_el1);
+    u32 louis = (clidr >> 21) & 7;
+
+    u32 csselr = (u32)GET_SYSREG(csselr_el1);
+    u32 level = (csselr >> 1) & 7;
+    if (csselr & BIT(0)) {
+        // Icache, ignore
+        return;
+    } else if (level < louis) {
+        return;
+    }
+
+
+    u32 ccsidr = (u32)GET_SYSREG(ccsidr_el1);
+    u32 numWays = 1 + ((ccsidr >> 3) & 0x3FF);
+    u32 numSets = 1 + ((ccsidr >> 13) & 0x7FFF);
+    if (currentCoreCtx->setWayCounter++ == 0) {
+        cacheCleanInvalidateDataCacheLevel(level);
+    }
+
+    if (currentCoreCtx->setWayCounter >= numSets * numWays) {
+        currentCoreCtx->setWayCounter = 0;
+    }
+}

thermosphere/src/caches.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+#include "sysreg.h"
+
+static inline u32 cacheGetInstructionCachePolicy(void)
+{
+    u32 ctr = (u32)GET_SYSREG(ctr_el0);
+    return (ctr >> 14) & 3;
+}
+
+static inline u32 cacheGetSmallestInstructionCacheLineSize(void)
+{
+    u32 ctr = (u32)GET_SYSREG(ctr_el0);
+    u32 shift = ctr & 0xF;
+    // "log2 of the number of words"...
+    return 4 << shift;
+}
+
+static inline u32 cacheGetSmallestDataCacheLineSize(void)
+{
+    u32 ctr = (u32)GET_SYSREG(ctr_el0);
+    u32 shift = (ctr >> 16) & 0xF;
+    // "log2 of the number of words"...
+    return 4 << shift;
+}
+
+static inline void cacheInvalidateInstructionCache(void)
+{
+    __asm__ __volatile__ ("ic ialluis" ::: "memory");
+    __isb();
+}
+
+static inline void cacheInvalidateInstructionCacheLocal(void)
+{
+    __asm__ __volatile__ ("ic iallu" ::: "memory");
+    __isb();
+}
+
+void cacheCleanInvalidateDataCacheRange(const void *addr, size_t size);
+void cacheCleanDataCacheRangePoU(const void *addr, size_t size);
+
+void cacheInvalidateInstructionCacheRangePoU(const void *addr, size_t size);
+
+void cacheHandleSelfModifyingCodePoU(const void *addr, size_t size);
+
+void cacheClearSharedDataCachesOnBoot(void);
+void cacheClearLocalDataCacheOnBoot(void);
+
+void cacheHandleTrappedSetWayOperation(bool invalidateOnly);

thermosphere/src/core_ctx.c

@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "core_ctx.h"
+#include "memory_map.h"
+
+// start.s
+extern uintptr_t g_initialKernelEntrypoint;
+
+static atomic_uint g_activeCoreMask = 0;
+
+// Prevents it from being put in BSS
+CoreCtx g_coreCtxs[4] = {
+    { .coreId = 0 },
+    { .coreId = 1 },
+    { .coreId = 2 },
+    { .coreId = 3 },
+};
+
+void coreCtxInit(u32 coreId, bool isBootCore, u64 argument)
+{
+    size_t crashStackSize = 0x1000 / 4;
+    currentCoreCtx = &g_coreCtxs[coreId];
+    currentCoreCtx->isBootCore = isBootCore;
+    currentCoreCtx->kernelArgument = argument;
+    currentCoreCtx->crashStack = (u8 *)(MEMORY_MAP_VA_CRASH_STACKS_TOP - crashStackSize * coreId);
+    if (isBootCore && currentCoreCtx->kernelEntrypoint == 0) {
+        currentCoreCtx->kernelEntrypoint = g_initialKernelEntrypoint;
+    }
+}
+
+void setCurrentCoreActive(void)
+{
+    atomic_fetch_or(&g_activeCoreMask, BIT(currentCoreCtx->coreId));
+}
+
+u32 getActiveCoreMask(void)
+{
+    return (u32)atomic_load(&g_activeCoreMask);
+}

thermosphere/src/core_ctx.h

@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include <stdatomic.h>
+#include <assert.h>
+#include "utils.h"
+#include "barrier.h"
+#include "execute_function.h"
+
+struct ExceptionStackFrame;
+typedef struct ALIGN(64) CoreCtx {
+    // Most likely only just read (assume cache line size of at most 64 bytes):
+
+    u8 *crashStack;                                         // @0x00
+    u64 kernelArgument;                                     // @0x08
+    uintptr_t kernelEntrypoint;                             // @0x10
+    u32 coreId;                                             // @0x18
+    u8 gicInterfaceMask;                                    // @0x1C. Equal to BIT(coreId) anyway
+    bool isBootCore;                                        // @0x1D
+    bool warmboot;                                          // @0x1E
+
+    // Debug features
+    bool wasPaused;                                         // @0x1F
+    uintptr_t steppingRangeStartAddr;                       // @0x20
+    uintptr_t steppingRangeEndAddr;                         // @0x28
+
+    // Most likely written to:
+
+    ALIGN(64) struct ExceptionStackFrame *guestFrame;       // @0x40
+    u64 scratch;                                            // @0x48
+
+    // Timer stuff
+    u64 totalTimeInHypervisor;                              // @0x50. cntvoff_el2 is updated to that value.
+    u64 emulPtimerCval;                                     // @0x58. When setting cntp_cval_el0 and on interrupt
+
+    // "Execute function"
+    ExecutedFunction executedFunction;                      // @0x60
+    void *executedFunctionArgs;                             // @0x68
+    Barrier executedFunctionBarrier;                        // @0x70
+    u32 executedFunctionSrcCore;                            // @0x74
+    bool executedFunctionSync;                              // @0x78. Receiver fills it
+
+    // Cache stuff
+    u32 setWayCounter;                                      // @0x7C
+} CoreCtx;
+
+static_assert(offsetof(CoreCtx, warmboot) == 0x1E, "Wrong definition for CoreCtx");
+static_assert(offsetof(CoreCtx, emulPtimerCval) == 0x58, "Wrong definition for CoreCtx");
+static_assert(offsetof(CoreCtx, executedFunctionSync) == 0x78, "Wrong definition for CoreCtx");
+static_assert(offsetof(CoreCtx, setWayCounter) == 0x7C, "Wrong definition for CoreCtx");
+
+extern CoreCtx g_coreCtxs[4];
+register CoreCtx *currentCoreCtx asm("x18");
+
+void coreCtxInit(u32 coreId, bool isBootCore, u64 argument);
+
+void setCurrentCoreActive(void);
+u32 getActiveCoreMask(void);

thermosphere/src/data_abort.c

@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include "data_abort.h"
+#include "sysreg.h"
+#include "debug_log.h"
+#include "irq.h"
+#include "vgic.h"
+
+// Lower el
+
+void dumpUnhandledDataAbort(DataAbortIss dabtIss, u64 far, const char *msg)
+{
+    char s1[64], s2[32], s3[64] = "";
+    (void)s1; (void)s2; (void)s3;
+    sprintf(s1, "Unhandled%s %s", msg , dabtIss.wnr ? "write" : "read");
+    if (dabtIss.fnv) {
+        sprintf(s2, "<unk>");
+    } else {
+        sprintf(s2, "%016lx", far);
+    }
+
+    if (dabtIss.isv) {
+        sprintf(s3, ", size %u Rt=%u", BIT(dabtIss.sas), dabtIss.srt);
+    }
+
+    DEBUG("%s at %s%s\n", s1, s2, s3);
+}
+
+void handleLowerElDataAbortException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    DataAbortIss dabtIss;
+    u32 iss = esr.iss;
+    memcpy(&dabtIss, &iss, 4);
+
+    u64 far = GET_SYSREG(far_el2);
+    u64 farpg = far & ~0xFFFull;
+
+    if (!dabtIss.isv || dabtIss.fnv) {
+        dumpUnhandledDataAbort(dabtIss, far, "");
+    }
+
+    if (farpg == MEMORY_MAP_PA_GICD) {
+        handleVgicdMmio(frame, dabtIss, far & 0xFFF);
+    } else if (farpg == MEMORY_MAP_PA_GICH) {
+        dumpUnhandledDataAbort(dabtIss, far, " GICH");
+    } else {
+        dumpUnhandledDataAbort(dabtIss, far, "");
+    }
+
+    // Skip instruction anyway
+    skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4);
+}

thermosphere/src/data_abort.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "exceptions.h"
+
+typedef struct DataAbortIss {
+    u32 dfsc        : 6; // Fault status code
+
+    u32 wnr         : 1; // Write, not Read
+    u32 s1ptw       : 1; // Stage1 page table walk fault
+    u32 cm          : 1; // Cache maintenance
+    u32 ea          : 1; // External abort
+    u32 fnv         : 1; // FAR not Valid
+    u32 set         : 2; // Synchronous error type
+    u32 vncr        : 1; // vncr_el2 trap
+
+    u32 ar          : 1; // Acquire/release. Bit 14
+    u32 sf          : 1; // 64-bit register used
+    u32 srt         : 5; // Syndrome register transfer (register used)
+    u32 sse         : 1; // Syndrome sign extend
+    u32 sas         : 2; // Syndrome access size. Bit 23
+
+    u32 isv         : 1; // Instruction syndrome valid (ISS[23:14] valid)
+} DataAbortIss;
+
+void dumpUnhandledDataAbort(DataAbortIss dabtIss, u64 far, const char *msg);
+void handleLowerElDataAbortException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);

thermosphere/src/debug_log.c

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include "debug_log.h"
+#include "platform/uart.h"
+#include "semihosting.h"
+#include "utils.h"
+#include "transport_interface.h"
+#include "platform/uart.h"
+
+#ifndef DLOG_USE_SEMIHOSTING_WRITE0
+#define DLOG_USE_SEMIHOSTING_WRITE0 1
+#endif
+
+static TransportInterface *g_debugLogTransportInterface;
+
+void debugLogInit(void)
+{
+    if (!DLOG_USE_SEMIHOSTING_WRITE0) {
+        transportInterfaceCreate(TRANSPORT_INTERFACE_TYPE_UART, DEFAULT_UART, DEFAULT_UART_FLAGS, NULL, NULL, NULL);
+    }
+}
+
+void debugLogRaw(const char *str)
+{
+    // Use semihosting if available (we assume qemu was launched with -semihosting), otherwise UART
+    if (DLOG_USE_SEMIHOSTING_WRITE0 && semihosting_connection_supported()) {
+        semihosting_write_string(str);
+    } else {
+        transportInterfaceWriteData(g_debugLogTransportInterface, str, strlen(str));
+    }
+}
+
+// NOTE: UNSAFE!
+int debugLog(const char *fmt, ...)
+{
+    char buf[128];
+    va_list args;
+    va_start(args, fmt);
+    int res = vsprintf(buf, fmt, args);
+    va_end(args);
+
+    debugLogRaw(buf);
+
+    return res;
+}

thermosphere/src/debug_log.h

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include <stdarg.h>
+
+#ifndef NDEBUG
+#define DEBUG(...)      debugLog(__VA_ARGS__)
+#define DEBUGRAW(str)   debugLogRaw(str)
+#else
+#define DEBUG(...)
+#define DEBUGRAW(str)
+#endif
+
+void debugLogInit(void);
+void debugLogRaw(const char *str);
+int debugLog(const char *fmt, ...);

thermosphere/src/debug_manager.c

@@ -0,0 +1,252 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdatomic.h>
+#include <stdarg.h>
+#include <string.h>
+
+#include "debug_manager.h"
+#include "core_ctx.h"
+#include "irq.h"
+#include "spinlock.h"
+#include "single_step.h"
+
+#include "gdb/debug.h"
+
+GDBContext g_gdbContext = { 0 };
+
+typedef struct DebugManager {
+    DebugEventInfo              debugEventInfos[MAX_CORE];
+    uintptr_t                   steppingRangeStartAddrs[MAX_CORE];
+    uintptr_t                   steppingRangeEndAddrs[MAX_CORE];
+
+    ALIGN(64) atomic_uint       pausedCoreList;
+    atomic_uint                 singleStepCoreList;
+    atomic_uint                 eventsSentList;
+    Barrier                     pauseBarrier;
+    atomic_bool                 reportingEnabled;
+} DebugManager;
+
+static DebugManager g_debugManager = { 0 };
+
+static void debugManagerDoPauseCores(u32 coreList)
+{
+    __builtin_prefetch(&g_debugManager.pausedCoreList, 1, 0);
+
+    u32 desiredList = coreList;
+    u32 remainingList = coreList;
+    u32 readList = atomic_load(&g_debugManager.pausedCoreList);
+    do {
+        desiredList |= readList;
+        remainingList &= ~readList;
+    } while (!atomic_compare_exchange_weak(&g_debugManager.pausedCoreList, &readList, desiredList));
+
+    if (remainingList & ~BIT(currentCoreCtx->coreId)) {
+        // We need to notify other cores...
+        u32 otherCores = remainingList & ~BIT(currentCoreCtx->coreId);
+        barrierInit(&g_debugManager.pauseBarrier, otherCores | BIT(currentCoreCtx->coreId));
+        generateSgiForList(ThermosphereSgi_DebugPause, otherCores);
+        barrierWait(&g_debugManager.pauseBarrier);
+    }
+
+    if (remainingList & BIT(currentCoreCtx->coreId)) {
+        currentCoreCtx->wasPaused = true;
+    }
+
+    __sev();
+}
+
+void debugManagerPauseSgiHandler(void)
+{
+    currentCoreCtx->wasPaused = true;
+    barrierWait(&g_debugManager.pauseBarrier);
+}
+
+void debugManagerInit(TransportInterfaceType gdbIfaceType, u32 gdbIfaceId, u32 gdbIfaceFlags)
+{
+    memset(&g_debugManager, 0, sizeof(DebugManager));
+    GDB_InitializeContext(&g_gdbContext, gdbIfaceType, gdbIfaceId, gdbIfaceFlags);
+    GDB_MigrateRxIrq(&g_gdbContext, currentCoreCtx->coreId);
+}
+
+bool debugManagerHandlePause(void)
+{
+    u32 coreId = currentCoreCtx->coreId;
+    __builtin_prefetch(&g_debugManager.pausedCoreList, 0, 3);
+
+    if (atomic_load(&g_debugManager.pausedCoreList) & BIT(coreId)) {
+        unmaskIrq();
+        do {
+            __wfe();
+        } while (atomic_load(&g_debugManager.pausedCoreList) & BIT(coreId));
+        maskIrq();
+
+        if (!g_debugManager.debugEventInfos[coreId].handled) {
+            // Do we still have an unhandled debug event?
+            GDB_TrySignalDebugEvent(&g_gdbContext, &g_debugManager.debugEventInfos[coreId]);
+            return false;
+        }
+    }
+
+    currentCoreCtx->wasPaused = false;
+
+    // Single-step: if inactive and requested, start single step; cancel if active and not requested
+    u32 ssReqd = (atomic_load(&g_debugManager.singleStepCoreList) & BIT(currentCoreCtx->coreId)) != 0;
+    SingleStepState singleStepState = singleStepGetNextState(currentCoreCtx->guestFrame);
+    if (ssReqd) {
+        currentCoreCtx->steppingRangeStartAddr = g_debugManager.steppingRangeStartAddrs[coreId];
+        currentCoreCtx->steppingRangeEndAddr = g_debugManager.steppingRangeEndAddrs[coreId];
+        if(singleStepState == SingleStepState_Inactive) {
+            singleStepSetNextState(currentCoreCtx->guestFrame, SingleStepState_ActiveNotPending);
+        }
+    } else if (!ssReqd && singleStepState != SingleStepState_Inactive) {
+        singleStepSetNextState(currentCoreCtx->guestFrame, SingleStepState_Inactive);
+    }
+
+    return true;
+}
+
+void debugManagerSetReportingEnabled(bool enabled)
+{
+    atomic_store(&g_debugManager.reportingEnabled, enabled);
+}
+
+bool debugManagerHasDebugEvent(u32 coreId)
+{
+    bool isPaused = debugManagerIsCorePaused(coreId);
+    return isPaused && g_debugManager.debugEventInfos[coreId].type != DBGEVENT_NONE;
+}
+
+void debugManagerPauseCores(u32 coreList)
+{
+    u64 flags = maskIrq();
+    debugManagerDoPauseCores(coreList);
+    restoreInterruptFlags(flags);
+}
+
+void debugManagerSetSingleStepCoreList(u32 coreList)
+{
+    atomic_store(&g_debugManager.singleStepCoreList, coreList);
+}
+
+void debugManagerUnpauseCores(u32 coreList)
+{
+    FOREACH_BIT (tmp, coreId, coreList) {
+        if (&g_debugManager.debugEventInfos[coreId].handled) {
+            // Discard already handled debug events
+            g_debugManager.debugEventInfos[coreId].type = DBGEVENT_NONE;
+        }
+    }
+
+    atomic_fetch_and(&g_debugManager.pausedCoreList, ~coreList);
+
+    __sev();
+}
+
+void debugManagerSetSteppingRange(u32 coreId, uintptr_t startAddr, uintptr_t endAddr)
+{
+    g_debugManager.steppingRangeStartAddrs[coreId] = startAddr;
+    g_debugManager.steppingRangeEndAddrs[coreId] = endAddr;
+}
+
+u32 debugManagerGetPausedCoreList(void)
+{
+    return atomic_load(&g_debugManager.pausedCoreList);
+}
+
+DebugEventInfo *debugManagerGetDebugEvent(u32 coreId)
+{
+    return &g_debugManager.debugEventInfos[coreId];
+}
+
+void debugManagerReportEvent(DebugEventType type, ...)
+{
+    u64 flags = maskIrq();
+    bool reportingEnabled = atomic_load(&g_debugManager.reportingEnabled);
+    if (!reportingEnabled && type != DBGEVENT_DEBUGGER_BREAK) {
+        restoreInterruptFlags(flags);
+        return;
+    }
+
+    u32 coreId = currentCoreCtx->coreId;
+
+    DebugEventInfo *info = &g_debugManager.debugEventInfos[coreId];
+    memset(info, 0 , sizeof(DebugEventInfo));
+
+    info->type = type;
+    info->coreId = coreId;
+    info->frame = currentCoreCtx->guestFrame;
+
+    va_list args;
+    va_start(args, type);
+
+    switch (type) {
+        case DBGEVENT_OUTPUT_STRING:
+            info->outputString.address = va_arg(args, uintptr_t);
+            info->outputString.size = va_arg(args, size_t);
+            break;
+        default:
+            break;
+    }
+
+    va_end(args);
+
+    // Now, pause ourselves and try to signal we have a debug event
+    debugManagerDoPauseCores(BIT(coreId));
+
+    if (reportingEnabled) {
+        exceptionEnterInterruptibleHypervisorCode();
+        unmaskIrq();
+
+        GDB_TrySignalDebugEvent(&g_gdbContext, info);
+    }
+
+    restoreInterruptFlags(flags);
+}
+
+void debugManagerBreakCores(u32 coreList)
+{
+    u32 coreId = currentCoreCtx->coreId;
+    if (coreList & ~BIT(coreId)) {
+        generateSgiForList(ThermosphereSgi_ReportDebuggerBreak, coreList & ~BIT(coreId));
+    }
+    if ((coreList & BIT(coreId)) && !debugManagerHasDebugEvent(coreId)) {
+        debugManagerReportEvent(DBGEVENT_DEBUGGER_BREAK);
+    }
+
+    // Wait for all cores
+    __sevl();
+    do {
+        __wfe();
+    } while ((atomic_load(&g_debugManager.pausedCoreList) & coreList) != coreList);
+}
+
+void debugManagerContinueCores(u32 coreList)
+{
+    u32 coreId = currentCoreCtx->coreId;
+    if (coreList & ~BIT(coreId)) {
+        generateSgiForList(ThermosphereSgi_DebuggerContinue, coreList & ~BIT(coreId));
+    }
+    if (coreList & BIT(coreId) && debugManagerIsCorePaused(coreId)) {
+        debugManagerUnpauseCores(BIT(coreId));
+    }
+
+    // Wait for all cores
+    __sevl();
+    do {
+        __wfe();
+    } while ((atomic_load(&g_debugManager.pausedCoreList) & coreList) != 0);
+}

thermosphere/src/debug_manager.h

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "exceptions.h"
+#include "gdb/context.h"
+#include "transport_interface.h"
+
+extern GDBContext g_gdbContext;
+
+typedef enum DebugEventType {
+    DBGEVENT_NONE = 0,
+    DBGEVENT_DEBUGGER_BREAK,
+    DBGEVENT_EXCEPTION,
+    DBGEVENT_CORE_ON,
+    DBGEVENT_CORE_OFF,
+    DBGEVENT_EXIT,
+    DBGEVENT_OUTPUT_STRING,
+} DebugEventType;
+
+typedef struct OutputStringDebugEventInfo {
+    uintptr_t address;
+    size_t size;
+} OutputStringDebugEventInfo;
+
+typedef struct DebugEventInfo {
+    DebugEventType type;
+    bool preprocessed;
+    bool handled;
+    u32 coreId;
+    ExceptionStackFrame *frame;
+    union {
+        OutputStringDebugEventInfo outputString;
+    };
+} DebugEventInfo;
+
+void debugManagerPauseSgiHandler(void);
+
+void debugManagerInit(TransportInterfaceType gdbIfaceType, u32 gdbIfaceId, u32 gdbIfaceFlags);
+
+void debugManagerSetReportingEnabled(bool enabled);
+
+// Hypervisor interrupts will be serviced during the pause-wait
+bool debugManagerHandlePause(void);
+
+DebugEventInfo *debugManagerGetDebugEvent(u32 coreId);
+bool debugManagerHasDebugEvent(u32 coreId);
+
+// Note: these functions are not reentrant EXCEPT debugPauseCores(1 << currentCoreId)
+// "Pause" makes sure all cores reaches the pause function before proceeding.
+// "Unpause" doesn't synchronize, it just makes sure the core resumes & that "pause" can be called again.
+void debugManagerPauseCores(u32 coreList);
+void debugManagerUnpauseCores(u32 coreList);
+void debugManagerSetSingleStepCoreList(u32 coreList);
+
+void debugManagerSetSteppingRange(u32 coreId, uintptr_t startAddr, uintptr_t endAddr);
+
+u32 debugManagerGetPausedCoreList(void);
+
+
+void debugManagerReportEvent(DebugEventType type, ...);
+
+void debugManagerBreakCores(u32 coreList);
+void debugManagerContinueCores(u32 coreList);
+
+static inline bool debugManagerIsCorePaused(u32 coreId)
+{
+    return (debugManagerGetPausedCoreList() & BIT(coreId)) != 0;
+}

thermosphere/src/exception_vectors.s

@@ -0,0 +1,344 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "asm_macros.s"
+
+/* Some macros taken from https://github.com/ARM-software/arm-trusted-firmware/blob/master/include/common/aarch64/asm_macros.S */
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Declare the exception vector table, enforcing it is aligned on a
+ * 2KB boundary, as required by the ARMv8 architecture.
+ * Use zero bytes as the fill value to be stored in the padding bytes
+ * so that it inserts illegal AArch64 instructions. This increases
+ * security, robustness and potentially facilitates debugging.
+ */
+.macro vector_base  label, section_name=.vectors
+.section \section_name, "ax"
+.align 11, 0
+\label:
+.endm
+
+/*
+ * Create an entry in the exception vector table, enforcing it is
+ * aligned on a 128-byte boundary, as required by the ARMv8 architecture.
+ * Use zero bytes as the fill value to be stored in the padding bytes
+ * so that it inserts illegal AArch64 instructions. This increases
+ * security, robustness and potentially facilitates debugging.
+ */
+.macro vector_entry  label, section_name=.vectors
+.cfi_sections .debug_frame
+.section \section_name, "ax"
+.align 7, 0
+.type \label, %function
+.func \label
+.cfi_startproc
+\label:
+.endm
+
+/*
+ * This macro verifies that the given vector doesnt exceed the
+ * architectural limit of 32 instructions. This is meant to be placed
+ * immediately after the last instruction in the vector. It takes the
+ * vector entry as the parameter
+ */
+.macro check_vector_size since
+    .endfunc
+    .cfi_endproc
+    .if (. - \since) > (32 * 4)
+        .error "Vector exceeds 32 instructions"
+    .endif
+.endm
+
+.macro  SAVE_MOST_REGISTERS
+    sub     sp, sp, #EXCEP_STACK_FRAME_SIZE
+
+    stp     x28, x29, [sp, #-0x20]
+    stp     x30, xzr, [sp, #-0x10]
+    mrs     x28, far_el2
+    mrs     x29, cntpct_el0
+    bl      _saveMostRegisters
+.endm
+
+.macro PIVOT_STACK_FOR_CRASH
+    // Note: x18 assumed uncorrupted
+    // Note: replace sp_el0 with crashing sp
+    str     x16, [x18, #CORECTX_SCRATCH_OFFSET]
+    mov     x16, sp
+    msr     sp_el0, x16
+    ldr     x16, [x18, #CORECTX_CRASH_STACK_OFFSET]
+    mov     sp, x16
+    ldr     x16, [x18, #CORECTX_SCRATCH_OFFSET]
+.endm
+
+#define     EXCEPTION_TYPE_HOST            0
+#define     EXCEPTION_TYPE_GUEST           1
+#define     EXCEPTION_TYPE_HOST_CRASH      2
+
+.macro  EXCEPTION_HANDLER_START name, type
+vector_entry \name
+    .if \type == EXCEPTION_TYPE_HOST_CRASH
+        PIVOT_STACK_FOR_CRASH
+    .endif
+
+    SAVE_MOST_REGISTERS
+
+    mov         x0, sp
+
+    .if \type == EXCEPTION_TYPE_GUEST
+        ldp     x18, xzr, [sp, #EXCEP_STACK_FRAME_SIZE]
+        prfm    pldl1keep, [x18]
+        prfm    pstl1keep, [x18, #0x40]
+        str     x0, [x18, #CORECTX_GUEST_FRAME_OFFSET]
+        mov     w1, #1
+    .else
+        mov     w1, #0
+    .endif
+
+    bl          exceptionEntryPostprocess
+.endm
+
+.macro EXCEPTION_HANDLER_END name, type
+    .if \type != EXCEPTION_TYPE_HOST_CRASH
+        mov     x0, sp
+        bl      exceptionReturnPreprocess
+        b       _restoreAllRegisters
+    .else
+        b       .
+    .endif
+check_vector_size \name
+.endm
+
+.macro  UNKNOWN_EXCEPTION name
+vector_entry \name
+    bl      _unknownException
+check_vector_size \name
+.endm
+
+/* Actual Vectors for Thermosphere. */
+.global     g_thermosphereVectors
+vector_base g_thermosphereVectors
+
+/* Current EL, SP0 */
+vector_entry        _synchSp0
+    // Used when we enable the MMU
+    msr     elr_el2, x18
+    // Note: non-broadcasting TLB maintenance op
+    tlbi    alle2
+    dsb     nsh
+    isb
+    eret
+check_vector_size   _synchSp0
+
+_unknownException:
+    pivot_stack_for_crash
+    mov     x0, x30
+    adr     x1, g_thermosphereVectors + 4
+    sub     x0, x0, x1
+    bl      handleUnknownException
+    b       .
+
+UNKNOWN_EXCEPTION       _irqSp0
+
+/* To save space, insert in an unused vector segment. */
+_saveMostRegisters:
+    stp     x0, x1, [sp, #0x00]
+    stp     x2, x3, [sp, #0x10]
+    stp     x4, x5, [sp, #0x20]
+    stp     x6, x7, [sp, #0x30]
+    stp     x8, x9, [sp, #0x40]
+    stp     x10, x11, [sp, #0x50]
+    stp     x12, x13, [sp, #0x60]
+    stp     x14, x15, [sp, #0x70]
+    stp     x16, x17, [sp, #0x80]
+    stp     x18, x19, [sp, #0x90]
+    stp     x20, x21, [sp, #0xA0]
+    stp     x22, x23, [sp, #0xB0]
+    stp     x24, x25, [sp, #0xC0]
+    stp     x26, x27, [sp, #0xD0]
+
+    mrs     x20, sp_el1
+    mrs     x21, sp_el0
+    mrs     x22, elr_el2
+    mrs     x23, spsr_el2
+    mrs     x24, esr_el2
+    mov     x25, x28                // far_el2
+    mov     x26, x29                // cntpct_el0
+
+    // See SAVE_MOST_REGISTERS macro
+    ldp     x28, x29, [sp, #-0x20]
+    ldp     x19, xzr, [sp, #-0x10]
+
+    stp     x28, x29, [sp, #0xE0]
+    stp     x19, x20, [sp, #0xF0]
+    stp     x21, x22, [sp, #0x100]
+    stp     x23, x24, [sp, #0x110]
+    stp     x25, x26, [sp, #0x120]
+
+    ret
+
+UNKNOWN_EXCEPTION       _fiqSp0
+
+/* To save space, insert in an unused vector segment. */
+
+// Accessed by start.s
+.global     _restoreAllRegisters
+.type       _restoreAllRegisters, %function
+_restoreAllRegisters:
+    ldp     x30, x20, [sp, #0xF0]
+    ldp     x21, x22, [sp, #0x100]
+    ldp     x23, xzr, [sp, #0x110]
+
+    msr     sp_el1, x20
+    msr     sp_el0, x21
+    msr     elr_el2, x22
+    msr     spsr_el2, x23
+
+    ldp     x0, x1, [sp, #0x00]
+    ldp     x2, x3, [sp, #0x10]
+    ldp     x4, x5, [sp, #0x20]
+    ldp     x6, x7, [sp, #0x30]
+    ldp     x8, x9, [sp, #0x40]
+    ldp     x10, x11, [sp, #0x50]
+    ldp     x12, x13, [sp, #0x60]
+    ldp     x14, x15, [sp, #0x70]
+    ldp     x16, x17, [sp, #0x80]
+    ldp     x18, x19, [sp, #0x90]
+    ldp     x20, x21, [sp, #0xA0]
+    ldp     x22, x23, [sp, #0xB0]
+    ldp     x24, x25, [sp, #0xC0]
+    ldp     x26, x27, [sp, #0xD0]
+    ldp     x28, x29, [sp, #0xE0]
+
+    add     sp, sp, #EXCEP_STACK_FRAME_SIZE
+    eret
+
+UNKNOWN_EXCEPTION       _serrorSp0
+
+/* To save space, insert in an unused vector segment. */
+.global semihosting_call
+.type   semihosting_call, %function
+.func   semihosting_call
+.cfi_startproc
+semihosting_call:
+    hlt     #0xF000
+    ret
+.cfi_endproc
+.endfunc
+
+.global doSmcIndirectCallImpl
+doSmcIndirectCallImpl:
+    stp     x19, x20, [sp, #-0x10]!
+    mov     x19, x0
+
+    ldp     x0, x1, [x19, #0x00]
+    ldp     x2, x3, [x19, #0x10]
+    ldp     x4, x5, [x19, #0x20]
+    ldp     x6, x7, [x19, #0x30]
+
+    _doSmcIndirectCallImplSmcInstruction:
+    smc     #0
+
+    stp     x0, x1, [x19, #0x00]
+    stp     x2, x3, [x19, #0x10]
+    stp     x4, x5, [x19, #0x20]
+    stp     x6, x7, [x19, #0x30]
+
+    ldp     x19, x20, [sp], #0x10
+    ret
+
+_doSmcIndirectCallImplEnd:
+.global doSmcIndirectCallImplSmcInstructionOffset
+doSmcIndirectCallImplSmcInstructionOffset:
+    .word _doSmcIndirectCallImplSmcInstruction - doSmcIndirectCallImpl
+.global doSmcIndirectCallImplSize
+doSmcIndirectCallImplSize:
+    .word _doSmcIndirectCallImplEnd - doSmcIndirectCallImpl
+
+/* Current EL, SPx */
+
+vector_entry                _synchSpx
+    // Ignore crash if x18 is 0, when we're copying memory from the guest (w/ irq masked)
+    cbz     x18, _synchSpxIgnoreCrash
+
+    PIVOT_STACK_FOR_CRASH
+    SAVE_MOST_REGISTERS
+
+    mov     x0, sp
+    mov     w1, #0
+
+    bl      exceptionEntryPostprocess
+
+    mov     x0, sp
+    mrs     x1, esr_el2
+    bl      handleSameElSyncException
+
+    b       .
+
+_synchSpxIgnoreCrash:
+    mrs     x18, elr_el2
+    add     x18, x18, #4
+    msr     elr_el2, x18
+    eret
+check_vector_size           _synchSpx
+
+EXCEPTION_HANDLER_START     _irqSpx, EXCEPTION_TYPE_HOST
+    mov     x0, sp
+    mov     w1, wzr
+    mov     w2, wzr
+    bl      handleIrqException
+EXCEPTION_HANDLER_END       _irqSpx, EXCEPTION_TYPE_HOST
+
+UNKNOWN_EXCEPTION           _fiqSpx
+UNKNOWN_EXCEPTION           _serrorSpx
+
+/* Lower EL, A64 */
+
+EXCEPTION_HANDLER_START     _synchA64, EXCEPTION_TYPE_GUEST
+    mov     x0, sp
+    bl      handleLowerElSyncException
+EXCEPTION_HANDLER_END       _synchA64, EXCEPTION_TYPE_GUEST
+
+EXCEPTION_HANDLER_START     _irqA64, EXCEPTION_TYPE_GUEST
+    mov     x0, sp
+    mov     w1, #1
+    mov     w2, #0
+    bl      handleIrqException
+EXCEPTION_HANDLER_END        _irqA64, EXCEPTION_TYPE_GUEST
+
+UNKNOWN_EXCEPTION           _fiqA64
+UNKNOWN_EXCEPTION           _serrorA64
+
+/* Lower EL, A32 */
+
+EXCEPTION_HANDLER_START     _synchA32, EXCEPTION_TYPE_GUEST
+    mov     x0, sp
+    bl      handleLowerElSyncException
+EXCEPTION_HANDLER_END       _synchA32, EXCEPTION_TYPE_GUEST
+
+EXCEPTION_HANDLER_START     _irqA32, EXCEPTION_TYPE_GUEST
+    mov     x0, sp
+    mov     w1, #1
+    mov     w2, #1
+    bl      handleIrqException
+EXCEPTION_HANDLER_END        _irqA32, EXCEPTION_TYPE_GUEST
+
+UNKNOWN_EXCEPTION           _fiqA32
+UNKNOWN_EXCEPTION           _serrorA32

thermosphere/src/exceptions.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020 Atmosphère-NX
+ * Copyright (c) 2019 Atmosphère-NX
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -14,95 +14,200 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdint.h>
-#include <stddef.h>
 #include "exceptions.h"
 
-#include "lib/printk.h"
+#include "hvc.h"
+#include "traps.h"
+#include "sysreg_traps.h"
+#include "smc.h"
+#include "core_ctx.h"
+#include "single_step.h"
+#include "data_abort.h"
+#include "spinlock.h"
+#include "debug_manager.h"
+#include "timer.h"
+#include "memory_map.h"
 
-/**
- * Simple debug function that prints all of our saved registers.
- */
-static void print_registers(struct guest_state *regs)
+#include "fpu.h"
+
+bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode)
 {
-    // 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]);
+    if (conditionCode == 14) {
+        // AL
+        return true;
+    } else if (conditionCode == 15) {
+        // Invalid encoding
+        return false;
     }
 
-    // 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]);
+    // NZCV
+    bool n = (spsr & BIT(31)) != 0;
+    bool z = (spsr & BIT(30)) != 0;
+    bool c = (spsr & BIT(29)) != 0;
+    bool v = (spsr & BIT(28)) != 0;
 
-    // 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);
+    bool tableHalf[] = {
+        // EQ, CS, MI, VS, HI, GE, GT
+        z, c, n, v, c && !z, n == v, !z && n == v,
+    };
 
-    // Note that we don't print ESR_EL2, as this isn't really part of the saved state.
+    return (conditionCode & 1) == 0 ? tableHalf[conditionCode / 2] : !tableHalf[conditionCode / 2];
 }
 
-/**
- * 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)
+void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl)
 {
-    printk("\nAn unexpected vector happened!\n");
-    print_registers(regs);
-    printk("\n\n");
+#ifndef NDEBUG
+    uintptr_t stackTop = memoryMapGetStackTop(currentCoreCtx->coreId);
+
+    for (u32 i = 0; i < 30; i += 2) {
+        DEBUG("x%u\t\t%016llx\t\tx%u\t\t%016llx\n", i, frame->x[i], i + 1, frame->x[i + 1]);
     }
 
+    DEBUG("x30\t\t%016llx\n\n", frame->x[30]);
+    DEBUG("elr_el2\t\t%016llx\n", frame->elr_el2);
+    DEBUG("spsr_el2\t%016llx\n", frame->spsr_el2);
+    DEBUG("far_el2\t\t%016llx\n", frame->far_el2);
+    if (sameEl) {
+        DEBUG("sp_el2\t\t%016llx\n", frame->sp_el2);
+    } else {
+        DEBUG("sp_el0\t\t%016llx\n", frame->sp_el0);
+    }
+    DEBUG("sp_el1\t\t%016llx\n", frame->sp_el1);
+    DEBUG("cntpct_el0\t%016llx\n", frame->cntpct_el0);
+    if (frame == currentCoreCtx->guestFrame) {
+        DEBUG("cntp_ctl_el0\t%016llx\n", frame->cntp_ctl_el0);
+        DEBUG("cntv_ctl_el0\t%016llx\n", frame->cntv_ctl_el0);
+    } else if ((frame->sp_el2 & ~0xFFFul) + 0x1000 == stackTop) {
+        // Try to dump the stack (comment if this crashes)
+        u64 *sp = (u64 *)frame->sp_el2;
+        u64 *spEnd = sp + 0x20;
+        u64 *spMax = (u64 *)((frame->sp_el2 + 0xFFF) & ~0xFFFul);
+        DEBUG("Stack trace:\n");
+        while (sp < spEnd && sp < spMax) {
+            DEBUG("\t%016lx\n", *sp++);
+        }
+    } else {
+        DEBUG("Stack overflow/double fault detected!\n");
+    }
+#else
+    (void)frame;
+    (void)sameEl;
+#endif
+}
 
-/**
- * Handles an HVC call.
- */
-static void handle_hvc(struct guest_state *regs, int call_number)
+static void advanceItState(ExceptionStackFrame *frame)
 {
+    // Just in case EL0 is executing A32 (& not sure if fully supported)
+    if (!spsrIsThumb(frame->spsr_el2) || spsrGetT32ItFlags(frame->spsr_el2) == 0) {
+        return;
+    }
 
-    switch(call_number) {
+    u32 it = spsrGetT32ItFlags(frame->spsr_el2);
 
+    // Last instruction of the block => wipe, otherwise advance
+    spsrSetT32ItFlags(&frame->spsr_el2, (it & 7) == 0 ? 0 : (it & 0xE0) | ((it << 1) & 0x1F));
+}
 
+void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size)
+{
+    advanceItState(frame);
+    frame->elr_el2 += size;
+}
+
+void exceptionEnterInterruptibleHypervisorCode(void)
+{
+    // We don't want the guest to spam us with its timer interrupts. Disable the timers.
+    SET_SYSREG(cntp_ctl_el0, 0);
+    SET_SYSREG(cntv_ctl_el0, 0);
+}
+
+// Called on exception entry (avoids overflowing a vector section)
+void exceptionEntryPostprocess(ExceptionStackFrame *frame, bool isLowerEl)
+{
+    if (frame == currentCoreCtx->guestFrame) {
+        frame->cntp_ctl_el0 = GET_SYSREG(cntp_ctl_el0);
+        frame->cntv_ctl_el0 = GET_SYSREG(cntv_ctl_el0);
+    }
+}
+
+// Called on exception return (avoids overflowing a vector section)
+void exceptionReturnPreprocess(ExceptionStackFrame *frame)
+{
+    if (frame == currentCoreCtx->guestFrame) {
+        if (currentCoreCtx->wasPaused) { 
+            // Were we paused & are we about to return to the guest?
+            exceptionEnterInterruptibleHypervisorCode();
+            while (!debugManagerHandlePause());
+            fpuCleanInvalidateRegisterCache();
+        }
+
+        // Update virtual counter
+        u64 ticksNow = timerGetSystemTick();
+        currentCoreCtx->totalTimeInHypervisor += ticksNow - frame->cntpct_el0;
+        SET_SYSREG(cntvoff_el2, currentCoreCtx->totalTimeInHypervisor);
+
+        if (frame == currentCoreCtx->guestFrame) {
+            // Restore interrupt mask
+            SET_SYSREG(cntp_ctl_el0, frame->cntp_ctl_el0);
+            SET_SYSREG(cntv_ctl_el0, frame->cntv_ctl_el0);
+        }
+    }
+}
+
+void handleLowerElSyncException(ExceptionStackFrame *frame)
+{
+    ExceptionSyndromeRegister esr = frame->esr_el2;
+    switch (esr.ec) {
+        case Exception_CP15RTTrap:
+            handleMcrMrcCP15Trap(frame, esr);
+            break;
+        case Exception_CP15RRTTrap:
+            handleMcrrMrrcCP15Trap(frame, esr);
+            break;
+        case Exception_CP14RTTrap:
+        case Exception_CP14DTTrap:
+        case Exception_CP14RRTTrap:
+            // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0)
+            handleA32CP14Trap(frame, esr);
+            break;
+        case Exception_HypervisorCallA64:
+            handleHypercall(frame, esr);
+            break;
+        case Exception_MonitorCallA64:
+            handleSmcTrap(frame, esr);
+            break;
+        case Exception_SystemRegisterTrap:
+            handleMsrMrsTrap(frame, esr);
+            break;
+        case Exception_DataAbortLowerEl:
+            // Basically, stage2 translation faults
+            handleLowerElDataAbortException(frame, esr);
+            break;
+        case Exception_SoftwareStepLowerEl:
+            handleSingleStep(frame, esr);
+            break;
+        case Exception_BreakpointLowerEl:
+        case Exception_WatchpointLowerEl:
+        case Exception_SoftwareBreakpointA64:
+        case Exception_SoftwareBreakpointA32:
+            debugManagerReportEvent(DBGEVENT_EXCEPTION);
+            break;
         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");
+            DEBUG("Lower EL sync exception, EC = 0x%02llx IL=%llu ISS=0x%06llx\n", (u64)esr.ec, esr.il, esr.iss);
+            dumpStackFrame(frame, false);
             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)
+void handleSameElSyncException(ExceptionStackFrame *frame)
 {
-    // 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;
-
-    }
+    ExceptionSyndromeRegister esr = frame->esr_el2;
+    (void)esr;
+    DEBUG("Same EL sync exception on core %x, EC = 0x%02llx IL=%llu ISS=0x%06llx\n", currentCoreCtx->coreId, (u64)esr.ec, esr.il, esr.iss);
+    dumpStackFrame(frame, true);
 }
 
-
+void handleUnknownException(u32 offset)
+{
+    DEBUG("Unknown exception on core %x! (offset 0x%03lx)\n", offset, currentCoreCtx->coreId);
+}

thermosphere/src/exceptions.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020 Atmosphère-NX
+ * Copyright (c) 2019 Atmosphère-NX
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -14,172 +14,146 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef __EXCEPTION_H__
-#define __EXCEPTION_H__
+#pragma once
+#include <assert.h>
+#include "utils.h"
+#include "core_ctx.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
+// Adapted from https://developer.arm.com/docs/ddi0596/a/a64-shared-pseudocode-functions/shared-exceptions-pseudocode
+typedef enum ExceptionClass {
+    Exception_Uncategorized = 0x0,
+    Exception_WFxTrap = 0x1,
+    Exception_CP15RTTrap = 0x3,
+    Exception_CP15RRTTrap = 0x4,
+    Exception_CP14RTTrap = 0x5,
+    Exception_CP14DTTrap = 0x6,
+    Exception_AdvSIMDFPAccessTrap = 0x7,
+    Exception_FPIDTrap = 0x8,
+    Exception_PACTrap = 0x9,
+    Exception_CP14RRTTrap = 0xC,
+    Exception_BranchTargetException = 0xD, // No official enum field name from Arm yet
+    Exception_IllegalState = 0xE,
+    Exception_SupervisorCallA32 = 0x11,
+    Exception_HypervisorCallA32 = 0x12,
+    Exception_MonitorCallA32 = 0x13,
+    Exception_SupervisorCallA64 = 0x15,
+    Exception_HypervisorCallA64 = 0x16,
+    Exception_MonitorCallA64 = 0x17,
+    Exception_SystemRegisterTrap = 0x18,
+    Exception_SVEAccessTrap = 0x19,
+    Exception_ERetTrap = 0x1A,
+    Exception_El3_ImplementationDefined = 0x1F,
+    Exception_InstructionAbortLowerEl = 0x20,
+    Exception_InstructionAbortSameEl = 0x21,
+    Exception_PCAlignment = 0x22,
+    Exception_DataAbortLowerEl = 0x24,
+    Exception_DataAbortSameEl = 0x25,
+    Exception_SPAlignment = 0x26,
+    Exception_FPTrappedExceptionA32 = 0x28,
+    Exception_FPTrappedExceptionA64 = 0x2C,
+    Exception_SError = 0x2F,
+    Exception_BreakpointLowerEl = 0x30,
+    Exception_BreakpointSameEl = 0x31,
+    Exception_SoftwareStepLowerEl = 0x32,
+    Exception_SoftwareStepSameEl = 0x33,
+    Exception_WatchpointLowerEl = 0x34,
+    Exception_WatchpointSameEl = 0x35,
+    Exception_SoftwareBreakpointA32 = 0x38,
+    Exception_VectorCatchA32 = 0x3A,
+    Exception_SoftwareBreakpointA64 = 0x3C,
+} ExceptionClass;
 
-/**
- * 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 */
+typedef struct ExceptionSyndromeRegister {
+    u32 iss             : 25;   // Instruction Specific Syndrome
+    u32 il              :  1;   // Instruction Length (16 or 32-bit)
+    ExceptionClass ec   :  6;   // Exception Class
+    u32 res0            : 32;
+} ExceptionSyndromeRegister;
+
+typedef struct ExceptionStackFrame {
+    u64 x[31]; // x0 .. x30
+    u64 sp_el1;
+    union {
+        u64 sp_el2;
+        u64 sp_el0;
     };
+    u64 elr_el2;
+    u64 spsr_el2;
+    ExceptionSyndromeRegister esr_el2;
+    u64 far_el2;
+    u64 cntpct_el0;
+    u64 cntp_ctl_el0;
+    u64 cntv_ctl_el0;
+} ExceptionStackFrame;
 
-    /* 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;
+static_assert(offsetof(ExceptionStackFrame, far_el2) == 0x120, "Wrong definition for ExceptionStackFrame");
+static_assert(sizeof(ExceptionStackFrame) == 0x140, "Wrong size for ExceptionStackFrame");
 
-    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];
+static inline bool spsrIsA32(u64 spsr)
+{
+    return (spsr & 0x10) != 0;
 }
-__attribute__((packed));
 
+static inline bool spsrIsThumb(u64 spsr)
+{
+    return spsrIsA32(spsr) && (spsr & 0x20) != 0;
+}
 
+static inline u32 spsrGetT32ItFlags(u64 spsr)
+{
+    return (((spsr >> 10) & 0x3F) << 2) | ((spsr >> 25) & 3);
+}
 
-#endif
+static inline void spsrSetT32ItFlags(u64 *spsr, u32 itFlags)
+{
+    static const u32 itMask = (0x3F << 10) | (3 << 25);
+    *spsr &= ~itMask;
+    *spsr |= (itFlags & 3) << 25;
+    *spsr |= ((itFlags >> 2) & 0x3F) << 10;
+}
+
+static inline u64 readFrameRegister(ExceptionStackFrame *frame, u32 id)
+{
+    return frame->x[id];
+}
+
+static inline u64 readFrameRegisterZ(ExceptionStackFrame *frame, u32 id)
+{
+    return id == 31 ? 0 /* xzr */ : frame->x[id];
+}
+
+static inline void writeFrameRegister(ExceptionStackFrame *frame, u32 id, u64 val)
+{
+    frame->x[id] = val;
+}
+
+static inline void writeFrameRegisterZ(ExceptionStackFrame *frame, u32 id, u64 val)
+{
+    if (id != 31) {
+        // If not xzr
+        frame->x[id] = val;
+    }
+}
+
+static inline u64 *exceptionGetSpPtr(ExceptionStackFrame *frame)
+{
+    // Note: the return value is more or less meaningless if we took an exception from A32...
+    // We try our best to reflect which privilege level the exception was took from, nonetheless
+
+    bool spEl0;
+    u64 m = frame->spsr_el2 & 0xF;
+    if (spsrIsA32(frame->spsr_el2)) {
+        spEl0 = m == 0;
+    } else {
+        u64 el = m >> 2;
+        spEl0 = el == 2 || el == 0 || (m & 1) == 0; // note: frame->sp_el2 is aliased to frame->sp_el0
+    }
+
+    return spEl0 ? &frame->sp_el0 : &frame->sp_el1;
+}
+
+bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode);
+void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size);
+void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl);
+
+void exceptionEnterInterruptibleHypervisorCode(void);

thermosphere/src/execute_function.c

@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "execute_function.h"
+#include "utils.h"
+#include "core_ctx.h"
+#include "irq.h"
+
+void executeFunctionOnCores(ExecutedFunction fun, void *args, bool sync, u32 coreList)
+{
+    barrierInit(&currentCoreCtx->executedFunctionBarrier, coreList);
+    currentCoreCtx->executedFunction = fun;
+    currentCoreCtx->executedFunctionArgs = args;
+    currentCoreCtx->executedFunctionSync = sync;
+    __compiler_barrier();
+    generateSgiForList(ThermosphereSgi_ExecuteFunction, coreList);
+}
+
+void executeFunctionOnAllCores(ExecutedFunction fun, void *args, bool sync)
+{
+    executeFunctionOnCores(fun, args, sync, getActiveCoreMask());
+}
+
+void executeFunctionOnAllCoresButSelf(ExecutedFunction fun, void *args, bool sync)
+{
+    executeFunctionOnCores(fun, args, sync, getActiveCoreMask() & ~(BIT(currentCoreCtx->coreId)));
+}
+
+void executeFunctionInterruptHandler(u32 srcCore)
+{
+    CoreCtx *ctx = &g_coreCtxs[srcCore];
+    currentCoreCtx->executedFunctionSrcCore = srcCore;
+    ctx->executedFunction(ctx->executedFunctionArgs);
+    if (ctx->executedFunctionSync) {
+        barrierWait(&ctx->executedFunctionBarrier);
+    }
+}

thermosphere/src/execute_function.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#include "types.h"
+
+typedef void (*ExecutedFunction)(void *args);
+
+void executeFunctionOnCores(ExecutedFunction fun, void *args, bool sync, u32 coreList);
+void executeFunctionOnAllCores(ExecutedFunction fun, void *args, bool sync);
+void executeFunctionOnAllCoresButSelf(ExecutedFunction fun, void *args, bool sync);
+
+void executeFunctionInterruptHandler(u32 srcCore);

thermosphere/src/fmt.c

@@ -0,0 +1,310 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* Adapted from Luma3DS with permission. */
+
+/* File : barebones/ee_printf.c
+	This file contains an implementation of ee_printf that only requires a method to output a char to a UART without pulling in library code.
+This code is based on a file that contains the following:
+ Copyright (C) 2002 Michael Ringgaard. All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+ 1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+ 3. Neither the name of the project nor the names of its contributors
+    may be used to endorse or promote products derived from this software
+    without specific prior written permission.
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ SUCH DAMAGE.
+*/
+
+//TuxSH's changes: add support for 64-bit numbers, remove floating-point code
+
+#include <string.h>
+#include <stdarg.h>
+#include "types.h"
+
+#define ZEROPAD   (1<<0) //Pad with zero
+#define SIGN      (1<<1) //Unsigned/signed long
+#define PLUS      (1<<2) //Show plus
+#define SPACE     (1<<3) //Spacer
+#define LEFT      (1<<4) //Left justified
+#define HEX_PREP  (1<<5) //0x
+#define UPPERCASE (1<<6) //'ABCDEF'
+
+#define IS_DIGIT(c) ((c) >= '0' && (c) <= '9')
+
+static s32 skipAtoi(const char **s)
+{
+    s32 i = 0;
+
+    while(IS_DIGIT(**s)) i = i * 10 + *((*s)++) - '0';
+
+    return i;
+}
+
+static char *processNumber(char *str, s64 num, bool isHex, s32 size, s32 precision, u32 type)
+{
+    char sign = 0;
+
+    if(type & SIGN)
+    {
+        if(num < 0)
+        {
+            sign = '-';
+            num = -num;
+            size--;
+        }
+        else if(type & PLUS)
+        {
+            sign = '+';
+            size--;
+        }
+        else if(type & SPACE)
+        {
+            sign = ' ';
+            size--;
+        }
+    }
+
+    static const char *lowerDigits = "0123456789abcdef",
+                      *upperDigits = "0123456789ABCDEF";
+
+    s32 i = 0;
+    char tmp[20];
+    const char *dig = (type & UPPERCASE) ? upperDigits : lowerDigits;
+
+    if(num == 0)
+    {
+        if(precision != 0) tmp[i++] = '0';
+        type &= ~HEX_PREP;
+    }
+    else
+    {
+        while(num != 0)
+        {
+            u64 base = isHex ? 16ULL : 10ULL;
+            tmp[i++] = dig[(u64)num % base];
+            num = (s64)((u64)num / base);
+        }
+    }
+
+    if(type & LEFT || precision != -1) type &= ~ZEROPAD;
+    if(type & HEX_PREP && isHex) size -= 2;
+    if(i > precision) precision = i;
+    size -= precision;
+    if(!(type & (ZEROPAD | LEFT))) while(size-- > 0) *str++ = ' ';
+    if(sign) *str++ = sign;
+
+    if(type & HEX_PREP && isHex)
+    {
+        *str++ = '0';
+        *str++ = 'x';
+    }
+
+    if(type & ZEROPAD) while(size-- > 0) *str++ = '0';
+    while(i < precision--) *str++ = '0';
+    while(i-- > 0) *str++ = tmp[i];
+    while(size-- > 0) *str++ = ' ';
+
+    return str;
+}
+
+int vsprintf(char *buf, const char *fmt, va_list args)
+{
+    char *str;
+
+    for(str = buf; *fmt; fmt++)
+    {
+        if(*fmt != '%')
+        {
+            *str++ = *fmt;
+            continue;
+        }
+
+        //Process flags
+        u32 flags = 0; //Flags to number()
+        bool loop = true;
+
+        while(loop)
+        {
+            switch(*++fmt)
+            {
+                case '-': flags |= LEFT; break;
+                case '+': flags |= PLUS; break;
+                case ' ': flags |= SPACE; break;
+                case '#': flags |= HEX_PREP; break;
+                case '0': flags |= ZEROPAD; break;
+                default: loop = false; break;
+            }
+        }
+
+        //Get field width
+        s32 fieldWidth = -1; //Width of output field
+        if(IS_DIGIT(*fmt)) fieldWidth = skipAtoi(&fmt);
+        else if(*fmt == '*')
+        {
+            fmt++;
+
+            fieldWidth = va_arg(args, s32);
+
+            if(fieldWidth < 0)
+            {
+                fieldWidth = -fieldWidth;
+                flags |= LEFT;
+            }
+        }
+
+        //Get the precision
+        s32 precision = -1; //Min. # of digits for integers; max number of chars for from string
+        if(*fmt == '.')
+        {
+            fmt++;
+
+            if(IS_DIGIT(*fmt)) precision = skipAtoi(&fmt);
+            else if(*fmt == '*')
+            {
+                fmt++;
+                precision = va_arg(args, s32);
+            }
+
+            if(precision < 0) precision = 0;
+        }
+
+        //Get the conversion qualifier
+        u32 integerType = 0;
+        if(*fmt == 'l')
+        {
+            if(*++fmt == 'l')
+            {
+                fmt++;
+                integerType = 1;
+            }
+            else
+            {
+                integerType = sizeof(unsigned long) == 8 ? 1 : 0;
+            }
+        }
+        else if(*fmt == 'h')
+        {
+            if(*++fmt == 'h')
+            {
+                fmt++;
+                integerType = 3;
+            }
+            else integerType = 2;
+        }
+
+        bool isHex;
+
+        switch(*fmt)
+        {
+            case 'c':
+                if(!(flags & LEFT)) while(--fieldWidth > 0) *str++ = ' ';
+                *str++ = (u8)va_arg(args, s32);
+                while(--fieldWidth > 0) *str++ = ' ';
+                continue;
+
+            case 's':
+            {
+                char *s = va_arg(args, char *);
+                if(!s) s = "<NULL>";
+                u32 len = (precision != -1) ? strnlen(s, precision) : strlen(s);
+                if(!(flags & LEFT)) while((s32)len < fieldWidth--) *str++ = ' ';
+                for(u32 i = 0; i < len; i++) *str++ = *s++;
+                while((s32)len < fieldWidth--) *str++ = ' ';
+                continue;
+            }
+
+            case 'p':
+                if(fieldWidth == -1)
+                {
+                    fieldWidth = 8;
+                    flags |= ZEROPAD;
+                }
+                str = processNumber(str, va_arg(args, u32), true, fieldWidth, precision, flags);
+                continue;
+
+            //Integer number formats - set up the flags and "break"
+            case 'X':
+                flags |= UPPERCASE;
+                //Falls through
+            case 'x':
+                isHex = true;
+                break;
+
+            case 'd':
+            case 'i':
+                flags |= SIGN;
+                //Falls through
+            case 'u':
+                isHex = false;
+                break;
+
+            default:
+                if(*fmt != '%') *str++ = '%';
+                if(*fmt) *str++ = *fmt;
+                else fmt--;
+                continue;
+        }
+
+        s64 num;
+
+        if(flags & SIGN)
+        {
+            if(integerType == 1) num = va_arg(args, s64);
+            else num = va_arg(args, s32);
+
+            if(integerType == 2) num = (s16)num;
+            else if(integerType == 3) num = (s8)num;
+        }
+        else
+        {
+            if(integerType == 1) num = va_arg(args, u64);
+            else num = va_arg(args, u32);
+
+            if(integerType == 2) num = (u16)num;
+            else if(integerType == 3) num = (u8)num;
+        }
+
+        str = processNumber(str, num, isHex, fieldWidth, precision, flags);
+    }
+
+    *str = 0;
+    return str - buf;
+}
+
+int sprintf(char *buf, const char *fmt, ...)
+{
+    va_list args;
+    va_start(args, fmt);
+    int res = vsprintf(buf, fmt, args);
+    va_end(args);
+    return res;
+}

thermosphere/src/fpu.c

@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "fpu.h"
+#include "core_ctx.h"
+
+static FpuRegisterCache TEMPORARY g_fpuRegisterCache = { 0 };
+
+// fpu_regs_load_store.s
+void fpuLoadRegistersFromCache(const FpuRegisterCache *cache);
+void fpuStoreRegistersToCache(FpuRegisterCache *cache);
+
+FpuRegisterCache *fpuGetRegisterCache(void)
+{
+    g_fpuRegisterCache.coreId = currentCoreCtx->coreId;
+    return &g_fpuRegisterCache;
+}
+
+FpuRegisterCache *fpuReadRegisters(void)
+{
+    FpuRegisterCache *cache = &g_fpuRegisterCache;
+    if (!cache->valid) {
+        fpuStoreRegistersToCache(cache);
+        cache->valid = true;
+    }
+    return cache;
+}
+
+void fpuCommitRegisters(void)
+{
+    FpuRegisterCache *cache = &g_fpuRegisterCache;
+    cache->dirty = true;
+
+    // Because the caller rewrote the entire cache in the event it didn't read it before:
+    cache->valid = true;
+}
+
+void fpuCleanInvalidateRegisterCache(void)
+{
+    FpuRegisterCache *cache = &g_fpuRegisterCache;
+    if (cache->dirty && cache->coreId == currentCoreCtx->coreId) {
+        fpuLoadRegistersFromCache(cache);
+        cache->dirty = false;
+    }
+
+    cache->valid = false;
+}

thermosphere/src/fpu.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+#include "spinlock.h"
+typedef struct FpuRegisterCache {
+    u128                q[32];
+    u64                 fpsr;
+    u64                 fpcr;
+    u32                 coreId;
+    bool                valid;
+    bool                dirty;
+} FpuRegisterCache;
+
+// Only for the current core:
+
+FpuRegisterCache *fpuGetRegisterCache(void);
+FpuRegisterCache *fpuReadRegisters(void);
+void fpuCommitRegisters(void);
+void fpuCleanInvalidateRegisterCache(void);

thermosphere/src/fpu_regs_load_store.s

@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "asm_macros.s"
+
+.macro      LDSTORE_QREGS, op
+    \op     q0, q1, [x0], 0x20
+    \op     q2, q3, [x0], 0x20
+    \op     q4, q5, [x0], 0x20
+    \op     q6, q7, [x0], 0x20
+    \op     q8, q9, [x0], 0x20
+    \op     q10, q11, [x0], 0x20
+    \op     q12, q13, [x0], 0x20
+    \op     q14, q15, [x0], 0x20
+    \op     q16, q17, [x0], 0x20
+    \op     q18, q19, [x0], 0x20
+    \op     q20, q21, [x0], 0x20
+    \op     q22, q23, [x0], 0x20
+    \op     q24, q25, [x0], 0x20
+    \op     q26, q27, [x0], 0x20
+    \op     q28, q29, [x0], 0x20
+    \op     q30, q31, [x0], 0x20
+.endm
+
+FUNCTION fpuLoadRegistersFromCache
+    dmb     ish
+    LDSTORE_QREGS ldp
+    ldp     x1, x2, [x0]
+    msr     fpsr, x1
+    msr     fpcr, x2
+    dsb     ish
+    isb
+    ret
+END_FUNCTION
+
+FUNCTION fpuStoreRegistersToCache
+    dsb     ish
+    isb
+    LDSTORE_QREGS stp
+    mrs     x1, fpsr
+    mrs     x2, fpcr
+    stp     x1, x2, [x0]
+    dmb     ish
+    ret
+END_FUNCTION

thermosphere/src/gdb/context.c

@@ -0,0 +1,253 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+// Lots of code from:
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+
+#include "context.h"
+
+#include "net.h"
+
+#include "debug.h"
+#include "query.h"
+#include "verbose.h"
+#include "thread.h"
+#include "debug.h"
+#include "regs.h"
+#include "mem.h"
+#include "hio.h"
+#include "stop_points.h"
+
+#include "../breakpoints.h"
+#include "../software_breakpoints.h"
+#include "../watchpoints.h"
+#include "../fpu.h"
+
+static TEMPORARY char g_gdbWorkBuffer[GDB_WORK_BUF_LEN];
+static TEMPORARY char g_gdbBuffer[GDB_BUF_LEN + 4 + 1];
+
+static const struct{
+    char command;
+    GDBCommandHandler handler;
+} gdbCommandHandlers[] = {
+    { '?', GDB_HANDLER(GetStopReason) },
+    //{ '!', GDB_HANDLER(EnableExtendedMode) }, // note: stubbed
+    //{ 'c', GDB_HANDLER(ContinueOrStepDeprecated) },
+    //{ 'C', GDB_HANDLER(ContinueOrStepDeprecated) },
+    { 'D', GDB_HANDLER(Detach) },
+    { 'F', GDB_HANDLER(HioReply) },
+    { 'g', GDB_HANDLER(ReadRegisters) },
+    { 'G', GDB_HANDLER(WriteRegisters) },
+    { 'H', GDB_HANDLER(SetThreadId) },
+    { 'k', GDB_HANDLER(Kill) },
+    { 'm', GDB_HANDLER(ReadMemory) },
+    { 'M', GDB_HANDLER(WriteMemory) },
+    { 'p', GDB_HANDLER(ReadRegister) },
+    { 'P', GDB_HANDLER(WriteRegister) },
+    { 'q', GDB_HANDLER(ReadQuery) },
+    { 'Q', GDB_HANDLER(WriteQuery) },
+    //{ 's', GDB_HANDLER(ContinueOrStepDeprecated) },
+    //{ 'S', GDB_HANDLER(ContinueOrStepDeprecated) },
+    { 'T', GDB_HANDLER(IsThreadAlive) },
+    { 'v', GDB_HANDLER(VerboseCommand) },
+    { 'X', GDB_HANDLER(WriteMemoryRaw) },
+    { 'z', GDB_HANDLER(ToggleStopPoint) },
+    { 'Z', GDB_HANDLER(ToggleStopPoint) },
+};
+
+static inline GDBCommandHandler GDB_GetCommandHandler(char command)
+{
+    static const u32 nbHandlers = sizeof(gdbCommandHandlers) / sizeof(gdbCommandHandlers[0]);
+
+    size_t i;
+    for (i = 0; i < nbHandlers && gdbCommandHandlers[i].command != command; i++);
+
+    return i < nbHandlers ? gdbCommandHandlers[i].handler : GDB_HANDLER(Unsupported);
+}
+
+static int GDB_ProcessPacket(GDBContext *ctx, size_t len)
+{
+    int ret;
+
+    ENSURE(ctx->state != GDB_STATE_DISCONNECTED);
+
+    // Handle the packet...
+    if (ctx->buffer[0] == '\x03') {
+        GDB_BreakAllCores(ctx);
+        ret = 0;
+    } else {
+        GDBCommandHandler handler = GDB_GetCommandHandler(ctx->buffer[1]);
+        ctx->commandData = ctx->buffer + 2;
+        ret = handler(ctx);
+    }
+
+
+    // State changes...
+    if (ctx->state == GDB_STATE_DETACHING) {
+        return -1;
+    }
+
+    return ret;
+}
+
+static size_t GDB_ReceiveDataCallback(TransportInterface *iface, void *ctxVoid)
+{
+    return (size_t)GDB_ReceivePacket((GDBContext *)ctxVoid);
+}
+
+static void GDB_Disconnect(GDBContext *ctx)
+{
+    GDB_DetachFromContext(ctx);
+
+    ctx->flags = 0;
+    ctx->state = GDB_STATE_DISCONNECTED;
+
+    ctx->selectedThreadId = 0;
+    ctx->selectedThreadIdForContinuing = 0;
+    ctx->sentDebugEventCoreList = 0;
+    ctx->acknowledgedDebugEventCoreList = 0;
+
+    ctx->attachedCoreList = 0;
+    ctx->sendOwnDebugEventDisallowed = false;
+    ctx->catchThreadEvents = false;
+    ctx->lastSentPacketSize = 0;
+    ctx->lastDebugEvent = NULL;
+
+    ctx->processEnded = false;
+    ctx->processExited = false;
+
+    ctx->noAckSent = false;
+
+    ctx->currentHioRequestTargetAddr = 0;
+    memset(&ctx->currentHioRequest, 0, sizeof(PackedGdbHioRequest));
+
+    ctx->targetXmlLen = 0;
+}
+
+static void GDB_ProcessDataCallback(TransportInterface *iface, void *ctxVoid, size_t sz)
+{
+    int r = (int)sz;
+    GDBContext *ctx = (GDBContext *)ctxVoid;
+
+    if (r == -1) {
+        // Not sure if GDB has something to forcefully close connections over UART...
+        char c = '\x04'; // ctrl-D
+        transportInterfaceWriteData(iface, &c, 1);
+        GDB_Disconnect(ctx);
+    }
+
+    r = GDB_ProcessPacket(ctx, sz);
+    if (r == -1) {
+        GDB_Disconnect(ctx);
+    }
+}
+
+void GDB_InitializeContext(GDBContext *ctx, TransportInterfaceType ifaceType, u32 ifaceId, u32 ifaceFlags)
+{
+    memset(ctx, 0, sizeof(GDBContext));
+    ctx->workBuffer = g_gdbWorkBuffer;
+    ctx->buffer = g_gdbBuffer;
+    ctx->transportInterface = transportInterfaceCreate(
+        ifaceType,
+        ifaceId,
+        ifaceFlags,
+        GDB_ReceiveDataCallback,
+        GDB_ProcessDataCallback,
+        ctx
+    );
+}
+
+void GDB_AttachToContext(GDBContext *ctx)
+{
+    // TODO: move the debug traps enable here?
+
+    ctx->attachedCoreList = getActiveCoreMask();
+
+    // We're in full-stop mode at this point
+    // Break cores, but don't send the debug event (it will be fetched with '?')
+    // Initialize lastDebugEvent
+
+    debugManagerSetReportingEnabled(true);
+    ctx->sendOwnDebugEventDisallowed = true;
+
+    GDB_BreakAllCores(ctx);
+
+    DebugEventInfo *info = debugManagerGetDebugEvent(currentCoreCtx->coreId);
+    info->preprocessed = true;
+    info->handled = true;
+    ctx->lastDebugEvent = info;
+
+    ctx->state = GDB_STATE_ATTACHED;
+
+    ctx->sendOwnDebugEventDisallowed = false;
+}
+
+void GDB_DetachFromContext(GDBContext *ctx)
+{
+    removeAllWatchpoints();
+    removeAllBreakpoints();
+    removeAllSoftwareBreakpoints(true);
+
+    // Reports to gdb are prevented because of "detaching" state?
+
+    // TODO: disable debug traps
+
+    if(ctx->flags & GDB_FLAG_TERMINATE) {
+        // TODO: redefine what it means for thermosphère, if anything.
+        ctx->processEnded = true;
+        ctx->processExited = false;
+    }
+
+    ctx->currentHioRequestTargetAddr = 0;
+    memset(&ctx->currentHioRequest, 0, sizeof(PackedGdbHioRequest));
+
+    debugManagerSetReportingEnabled(false);
+    debugManagerContinueCores(getActiveCoreMask());
+}
+
+void GDB_AcquireContext(GDBContext *ctx)
+{
+    transportInterfaceAcquire(ctx->transportInterface);
+}
+
+void GDB_ReleaseContext(GDBContext *ctx)
+{
+    transportInterfaceRelease(ctx->transportInterface);
+}
+
+void GDB_MigrateRxIrq(GDBContext *ctx, u32 coreId)
+{
+    fpuCleanInvalidateRegisterCache();
+    transportInterfaceSetInterruptAffinity(ctx->transportInterface, BIT(coreId));
+}
+
+GDB_DECLARE_HANDLER(Unsupported)
+{
+    return GDB_ReplyEmpty(ctx);
+}
+
+GDB_DECLARE_HANDLER(EnableExtendedMode)
+{
+    // We don't support it for now...
+    return GDB_HandleUnsupported(ctx);
+}

thermosphere/src/gdb/context.h

@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+// Lots of code from:
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "defines.h"
+#include "../transport_interface.h"
+
+typedef struct PackedGdbHioRequest
+{
+    char magic[4]; // "GDB\x00"
+    u32 version;
+
+    // Request
+    char functionName[16+1];
+    char paramFormat[8+1];
+
+    u64 parameters[8];
+    size_t stringLengths[8];
+
+    // Return
+    s64 retval;
+    int gdbErrno;
+    bool ctrlC;
+} PackedGdbHioRequest;
+
+enum {
+    GDB_FLAG_NOACK              = BIT(0),
+    GDB_FLAG_CONTINUING         = BIT(1),
+    GDB_FLAG_TERMINATE          = BIT(2),
+    GDB_FLAG_ATTACHED_AT_START  = BIT(3),
+    GDB_FLAG_NONSTOP            = BIT(4),
+};
+
+typedef enum GDBState
+{
+    GDB_STATE_DISCONNECTED,
+    GDB_STATE_CONNECTED,
+    GDB_STATE_ATTACHED,
+    GDB_STATE_DETACHING,
+} GDBState;
+
+struct DebugEventInfo;
+
+typedef struct GDBContext {
+    // No need for a lock, it's in the transport interface layer...
+
+    TransportInterface *transportInterface;
+    u32 flags;
+    GDBState state;
+    bool noAckSent;
+
+    u32 attachedCoreList;
+
+    int selectedThreadId;
+    int selectedThreadIdForContinuing;
+
+    u32 sentDebugEventCoreList;
+    u32 acknowledgedDebugEventCoreList;
+
+    bool sendOwnDebugEventDisallowed;
+
+    bool catchThreadEvents;
+    bool processEnded, processExited;
+
+    const struct DebugEventInfo *lastDebugEvent;
+
+    uintptr_t currentHioRequestTargetAddr;
+    PackedGdbHioRequest currentHioRequest;
+
+    size_t targetXmlLen;
+
+    char *commandData, *commandEnd;
+    size_t lastSentPacketSize;
+    char *buffer;
+    char *workBuffer;
+} GDBContext;
+
+typedef int (*GDBCommandHandler)(GDBContext *ctx);
+
+void GDB_InitializeContext(GDBContext *ctx, TransportInterfaceType ifaceType, u32 ifaceId, u32 ifaceFlags);
+
+void GDB_AttachToContext(GDBContext *ctx);
+void GDB_DetachFromContext(GDBContext *ctx);
+
+void GDB_AcquireContext(GDBContext *ctx);
+void GDB_ReleaseContext(GDBContext *ctx);
+void GDB_MigrateRxIrq(GDBContext *ctx, u32 coreId);
+
+GDB_DECLARE_HANDLER(Unsupported);
+GDB_DECLARE_HANDLER(EnableExtendedMode);
+
+static inline bool GDB_IsAttached(GDBContext *ctx)
+{
+    return ctx->state == GDB_STATE_ATTACHED;
+}
+
+static inline bool GDB_IsNonStop(GDBContext *ctx)
+{
+    return (ctx->flags & GDB_FLAG_NONSTOP) != 0;
+}

thermosphere/src/gdb/debug.c

@@ -0,0 +1,556 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#define _GNU_SOURCE // for strchrnul
+#include <stdio.h>
+#include <string.h>
+#include "../debug_manager.h"
+#include "../watchpoints.h"
+
+#include "debug.h"
+#include "net.h"
+
+#include "context.h"
+#include "verbose.h"
+#include "thread.h"
+#include "mem.h"
+#include "hio.h"
+
+#include <stdlib.h>
+#include <signal.h>
+
+static bool GDB_PreprocessDebugEvent(GDBContext *ctx, DebugEventInfo *info)
+{
+    u64 irqFlags = maskIrq();
+    bool shouldSignal;
+
+    switch (info->type) {
+        case DBGEVENT_CORE_ON: {
+            shouldSignal = ctx->catchThreadEvents;
+            if (!info->preprocessed) {
+                ctx->attachedCoreList |= BIT(info->coreId);
+            }
+            break;
+        }
+        case DBGEVENT_CORE_OFF: {
+            if (!info->preprocessed) {
+                u32 newLst = ctx->attachedCoreList & ~BIT(info->coreId);
+                if (ctx->selectedThreadId == info->coreId && newLst != 0) {
+                    ctx->selectedThreadId = __builtin_ctz(newLst);
+                    GDB_MigrateRxIrq(ctx, ctx->selectedThreadId);
+                }
+                ctx->attachedCoreList = newLst;
+                shouldSignal = ctx->catchThreadEvents || newLst == 0;
+            } else {
+                shouldSignal = ctx->catchThreadEvents || ctx->attachedCoreList == 0;
+            }
+            break;
+        }
+
+        default:
+            shouldSignal = true;
+            break;
+    }
+
+    info->preprocessed = true;
+    restoreInterruptFlags(irqFlags);
+    return shouldSignal; 
+}
+
+static inline void GDB_MarkDebugEventAcked(GDBContext *ctx, const DebugEventInfo *info)
+{
+    ctx->acknowledgedDebugEventCoreList |= BIT(info->coreId);
+}
+
+static int GDB_ParseExceptionFrame(char *out, const DebugEventInfo *info, int sig)
+{
+    u32 coreId = info->coreId;
+    ExceptionStackFrame *frame = info->frame;
+
+    int n = sprintf(out, "T%02xthread:%x;core:%x;", sig, 1 + coreId, coreId);
+
+    // Dump the GPRs & sp & pc & cpsr (cpsr is 32-bit in the xml desc)
+    // For performance reasons, we don't include the FPU registers here
+    for (u32 i = 0; i < 31; i++) {
+        n += sprintf(out + n, "%x:%016lx;", i, __builtin_bswap64(readFrameRegister(frame, i)));
+    }
+
+    n += sprintf(
+        out + n,
+        "1f:%016lx;20:%016lx;21:%08x;",
+        __builtin_bswap64(*exceptionGetSpPtr(frame)),
+        __builtin_bswap64(frame->elr_el2),
+        __builtin_bswap32((u32)frame->spsr_el2)
+    );
+
+    return n;
+}
+
+int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotification)
+{
+    char *buf = ctx->buffer + 1;
+    int n;
+    bool invalid = false;
+
+    buf[0] = 0;
+
+    if (asNotification) {
+        strcpy(buf, "Stopped:");
+    }
+
+    n = strlen(buf);
+
+    // Even if the info is invalid:
+    ctx->lastDebugEvent = info;
+    ctx->sentDebugEventCoreList |= BIT(info->coreId);
+
+    switch(info->type) {
+        case DBGEVENT_DEBUGGER_BREAK: {
+            n += GDB_ParseExceptionFrame(buf + n, info, 0);
+            break;
+        }
+    
+        case DBGEVENT_CORE_ON: {
+            if (ctx->catchThreadEvents) {
+                n += GDB_ParseExceptionFrame(buf + n, info, SIGTRAP);
+                strcat(buf, "create:;");
+            } else {
+                invalid = true;
+            }
+            break;
+        }
+
+        case DBGEVENT_CORE_OFF: {
+            if (ctx->attachedCoreList == 0) {
+                // All cores have exited, must report an exit
+                ctx->processExited = true;
+                ctx->processEnded = true;
+                strcat(buf, "W00");
+            } else if(ctx->catchThreadEvents) {
+                sprintf(buf, "w0;%x", info->coreId + 1);
+            } else {
+                invalid = true;
+            }
+            break;
+        }
+
+        case DBGEVENT_EXIT: {
+            // exited (no error / unhandled exception), SIGTERM (process terminated) * 2
+            static const char *processExitReplies[] = { "W00", "X0f" };
+            strcat(buf, processExitReplies[ctx->processExited ? 0 : 1]);
+            break;
+        }
+
+        case DBGEVENT_EXCEPTION: {
+            ExceptionClass ec = info->frame->esr_el2.ec;
+
+            // Aside from stage 2 translation faults and other pre-handled exceptions, 
+            // the only notable exceptions we get are stop point/single step events from the debugee (basically classes 0x3x)
+            switch(ec) {
+                case Exception_BreakpointLowerEl: {
+                    n += GDB_ParseExceptionFrame(buf + n, info, SIGTRAP);
+                    strcat(buf, "hwbreak:;");
+                    break;
+                }
+
+                case Exception_WatchpointLowerEl: {
+                    static const char *kinds[] = { "", "r", "", "a" };
+                    // Note: exception info doesn't provide us with the access size. Use 1.
+                    bool wnr = (info->frame->esr_el2.iss & BIT(6)) != 0;
+                    WatchpointLoadStoreControl dr = wnr ? WatchpointLoadStoreControl_Store : WatchpointLoadStoreControl_Load;
+                    DebugControlRegister cr = retrieveWatchpointConfig(info->frame->far_el2, dr);
+                    if (!cr.enabled) {
+                        DEBUG("GDB: oops, unhandled watchpoint for core id %u, far=%016lx\n", info->coreId, info->frame->far_el2);
+                    } else {
+                        n += GDB_ParseExceptionFrame(buf + n, info, SIGTRAP);
+                        sprintf(buf + n, "%swatch:%016lx;", kinds[cr.lsc], info->frame->far_el2);
+                    }
+                    break;
+                }
+
+                case Exception_SoftwareStepLowerEl: {
+                    n += GDB_ParseExceptionFrame(buf + n, info, SIGTRAP);
+                    break;
+                }
+
+                // Note: we don't really support 32-bit sw breakpoints, we'll still report them
+                // if the guest has inserted some of them manually...
+                case Exception_SoftwareBreakpointA64:
+                case Exception_SoftwareBreakpointA32: {
+                    n += GDB_ParseExceptionFrame(buf + n, info, SIGTRAP);
+                    strcat(buf, "swbreak:;");
+                    break;
+                }
+                
+                default: {
+                    invalid = true;
+                    DEBUG("GDB: oops, unhandled exception for core id %u\n", info->coreId);
+                    break;
+                }
+            }
+            break;
+        }
+
+        case DBGEVENT_OUTPUT_STRING: {
+            if (!GDB_IsNonStop(ctx)) {
+                uintptr_t addr = info->outputString.address;
+                size_t remaining = info->outputString.size;
+                size_t sent = 0;
+                size_t total = 0;
+                while (remaining > 0) {
+                    size_t pending = (GDB_BUF_LEN - 1) / 2;
+                    pending = pending < remaining ? pending : remaining;
+
+                    int res = GDB_SendMemory(ctx, "O", 1, addr + sent, pending);
+                    if(res < 0 || res != 5 + 2 * pending)
+                        break;
+
+                    sent += pending;
+                    remaining -= pending;
+                    total += res;
+                }
+
+                return (int)total;
+            } else {
+                invalid = true;
+                break;
+            }
+        }
+
+        // TODO: HIO
+
+        default: {
+            invalid = true;
+            DEBUG("GDB: unknown exception type %u, core id %u\n", (u32)info->type, info->coreId);
+            break;
+        }
+    }
+
+    if (invalid) {
+        return 0;
+    } else if (asNotification) {
+        return GDB_SendNotificationPacket(ctx, buf, strlen(buf));
+    } else {
+        return GDB_SendPacket(ctx, buf, strlen(buf));
+    }
+}
+
+/*
+    Non-stop mode:
+        -> %Stop:<info>
+        <- $vStopped
+        -> $<info>
+        <- vStopped, etc.
+        -> $OK
+    If we're the first to try to send a notification, send it.
+    Otherwise don't, the core which will handle the GDB packets then will see the changes.
+
+    GDB can also send the "?" packet. This aborts the current notfication/vStopped sequence,
+    and asks to resend the events for each stopped core, no matter if already sent before.
+
+    Full-stop mode (default):
+
+    If we lose the race, we have to wait until we're continued to send the remaining events...
+*/
+
+int GDB_TrySignalDebugEvent(GDBContext *ctx, DebugEventInfo *info)
+{
+    int ret = 0;
+
+    // Acquire the gdb lock/disable rx irq. We most likely block here.
+    GDB_AcquireContext(ctx);
+
+    // Need to put it here otherwise core on/off would never be seen
+    bool shouldSignal = GDB_PreprocessDebugEvent(ctx, info);
+
+    // Are we still paused & has the packet not been handled & are we allowed to send on our own?
+
+    if (shouldSignal && !ctx->sendOwnDebugEventDisallowed && !info->handled && debugManagerIsCorePaused(info->coreId)) {
+        bool nonStop = GDB_IsNonStop(ctx);
+        info->handled = true;
+
+        // Full-stop mode: stop other cores
+        if (!nonStop) {
+            debugManagerPauseCores(ctx->attachedCoreList & ~BIT(info->coreId));
+        }
+
+        ctx->sendOwnDebugEventDisallowed = true;
+        ret = GDB_SendStopReply(ctx, info, nonStop);
+    }
+
+    if (!shouldSignal) {
+        debugManagerContinueCores(BIT(currentCoreCtx->coreId));
+    }
+
+    GDB_ReleaseContext(ctx);
+
+    return ret;
+}
+
+void GDB_BreakAllCores(GDBContext *ctx)
+{
+    if (GDB_IsNonStop(ctx)) {
+        debugManagerBreakCores(ctx->attachedCoreList);
+    } else {
+        // Break all cores too, but mark everything but the first has handled
+        debugManagerBreakCores(ctx->attachedCoreList);
+        u32 rem = ctx->attachedCoreList & ~BIT(currentCoreCtx->coreId);
+        FOREACH_BIT (tmp, coreId, rem) {
+            DebugEventInfo *info = debugManagerGetDebugEvent(coreId);
+            info->handled = true;
+            info->preprocessed = true;
+        }
+    }
+}
+
+GDB_DECLARE_VERBOSE_HANDLER(Stopped)
+{
+    u32 coreList = debugManagerGetPausedCoreList() & ctx->attachedCoreList;
+    u32 remaining = coreList & ~ctx->sentDebugEventCoreList;
+
+    // Ack
+    if (ctx->lastDebugEvent != NULL) {
+        GDB_MarkDebugEventAcked(ctx, ctx->lastDebugEvent);
+    }
+
+    for (;;) {
+        if (remaining != 0) {
+            // Send one more debug event (marking it as handled)
+            u32 coreId = __builtin_ctz(remaining);
+            DebugEventInfo *info = debugManagerGetDebugEvent(coreId);
+
+            if (GDB_PreprocessDebugEvent(ctx, info)) {
+                ctx->sendOwnDebugEventDisallowed = true;
+                return GDB_SendStopReply(ctx, info, false);
+            } else {
+                remaining &= ~BIT(coreId);
+            }
+        } else {
+            // vStopped sequenced finished
+            ctx->sendOwnDebugEventDisallowed = false;
+            return GDB_ReplyOk(ctx);
+        }
+    }
+}
+
+GDB_DECLARE_HANDLER(GetStopReason)
+{
+    if (!GDB_IsNonStop(ctx)) {
+        // Full-stop:
+        return GDB_SendStopReply(ctx, ctx->lastDebugEvent, false);
+    } else {
+        // Non-stop, start new vStopped sequence
+        ctx->sentDebugEventCoreList = 0;
+        ctx->acknowledgedDebugEventCoreList = 0;
+        ctx->lastDebugEvent = NULL;
+        ctx->sendOwnDebugEventDisallowed = true;
+        return GDB_HandleVerboseStopped(ctx);
+    }
+}
+
+GDB_DECLARE_HANDLER(Detach)
+{
+    ctx->state = GDB_STATE_DETACHING;
+    return GDB_ReplyOk(ctx);
+}
+
+GDB_DECLARE_HANDLER(Kill)
+{
+    ctx->state = GDB_STATE_DETACHING;
+    ctx->flags |= GDB_FLAG_TERMINATE;
+
+    return 0;
+}
+
+GDB_DECLARE_VERBOSE_HANDLER(CtrlC)
+{
+    int ret = GDB_ReplyOk(ctx);
+    GDB_BreakAllCores(ctx);
+    return ret;
+}
+
+GDB_DECLARE_HANDLER(ContinueOrStepDeprecated)
+{
+    char *addrStart = NULL;
+
+    char cmd = ctx->commandData[-1];
+
+    // This deprecated command should not be permitted in non-stop mode
+    /*if (GDB_IsNonStop(ctx)) {
+        return GDB_ReplyErrno(ctx, EPERM);
+    }*/
+
+    if(cmd == 'C' || cmd == 'S') {
+        // Check the presence of the two-digit signature, even if we ignore it.
+        u8 sg;
+        if (GDB_DecodeHex(&sg, ctx->commandData, 1) != 1) {
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+
+        // Check: [;addr] or [nothing]
+        if (ctx->commandData[2] != 0 && ctx->commandData[2] != ';') {
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+
+        if(ctx->commandData[2] == ';') {
+            addrStart = ctx->commandData + 3;
+        }
+    }
+    else {
+        // 'c', 's'
+        if (ctx->commandData[0] != 0) {
+            addrStart = ctx->commandData;
+        }
+    }
+
+    // Only support the simplest form, with no address
+    // Only degenerate clients will use ;addr, anyway (and the packets are deprecated in favor
+    // of vCont anyway)
+
+    if (addrStart != NULL) {
+        return GDB_ReplyErrno(ctx, ENOSYS);
+    }
+
+    u32 coreList = ctx->selectedThreadIdForContinuing == -1 ? ctx->attachedCoreList : BIT(ctx->selectedThreadIdForContinuing);
+    u32 ssMask = (cmd == 's' || cmd == 'S') ? coreList : 0;
+
+    FOREACH_BIT (tmp, coreId, ssMask) {
+        debugManagerSetSteppingRange(coreId, 0, 0);
+    }
+
+    u32 mask = ctx->acknowledgedDebugEventCoreList;
+    debugManagerSetSingleStepCoreList(ssMask & mask);
+    debugManagerUnpauseCores(coreList & mask);
+    return 0;
+}
+
+GDB_DECLARE_VERBOSE_HANDLER(Continue)
+{
+    u32 parsedCoreList = 0;
+    u32 continueCoreList = 0;
+    u32 stepCoreList = 0;
+    u32 stopCoreList = 0;
+
+    char *cmd = ctx->commandData;
+
+    while (cmd != NULL) {
+        char *nextCmd;
+        char *threadIdPart;
+        int threadId;
+        u32 curMask = 0;
+        const char *cmdEnd;
+
+        // It it always fine if we set the single-stepping range to 0,0 by default
+        // Because the fields we set are the shadow fields copied to the real fields after debug unpause
+        uintptr_t ssStartAddr = 0;
+        uintptr_t ssEndAddr = 0;
+
+        // Locate next command, replace delimiter by NUL
+        nextCmd = strchr(cmd, ';');
+        if (nextCmd != NULL && *nextCmd == ';') {
+            *nextCmd++ = 0;
+        }
+
+        // Locate thread-id part, parse thread id
+        threadIdPart = strchr(cmd, ':');
+        if (threadIdPart != NULL) {
+            *threadIdPart++ = 0;
+        }
+        if (threadIdPart == NULL || strcmp(threadIdPart, "-1") == 0) {
+            // Default action...
+            threadId = -1;
+            curMask = ctx->attachedCoreList;
+        } else {
+            unsigned long id;
+            if(GDB_ParseHexIntegerList(&id, threadIdPart, 1, 0) == NULL) {
+                return GDB_ReplyErrno(ctx, EILSEQ);
+            } else if (id >= MAX_CORE + 1) {
+                return GDB_ReplyErrno(ctx, EINVAL);
+            }
+
+            threadId = id == 0 ? (int)currentCoreCtx->coreId : (int)id;
+            curMask = BIT(threadId - 1) & ctx->attachedCoreList;
+        }
+
+        // Parse the command itself
+        // Note that we may already have handled that thread in a previous command
+        curMask &= ~parsedCoreList;
+        switch (cmd[0]) {
+            case 'S':
+            case 'C': {
+                // Check the presence of the two-digit signature, even if we ignore it.
+                u8 sg;
+                if (GDB_DecodeHex(&sg, cmd + 1, 1) != 1) {
+                    return GDB_ReplyErrno(ctx, EILSEQ);
+                }
+                stepCoreList |= cmd[0] == 'S' ? curMask : 0;
+                continueCoreList |= curMask;
+                cmdEnd = cmd + 3;
+                break;
+            }
+            case 's':
+                stepCoreList |= curMask;
+                continueCoreList |= curMask;
+                cmdEnd = cmd + 1;
+                break;
+            case 'c':
+                continueCoreList |= curMask;
+                cmdEnd = cmd + 1;
+                break;
+            case 't':
+                stopCoreList |= curMask;
+                cmdEnd = cmd + 1;
+                break;
+            case 'r': {
+                // Range step
+                unsigned long tmp[2];
+                cmdEnd = GDB_ParseHexIntegerList(tmp, cmd + 1, 2, 0);
+                if (cmdEnd == NULL) {
+                    return GDB_ReplyErrno(ctx, EILSEQ);
+                }
+
+                ssStartAddr = tmp[0];
+                ssEndAddr = tmp[1];
+                stepCoreList |= curMask;
+                continueCoreList |= curMask;
+                break;
+            }
+
+            default:
+                return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+
+        if (*cmdEnd != 0) {
+            // We've got garbage data...
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+
+        FOREACH_BIT (tmp, t, curMask) {
+            // Set/unset stepping range for all threads affected by this command
+            debugManagerSetSteppingRange(t, ssStartAddr, ssEndAddr);
+        }
+
+        parsedCoreList |= curMask;
+        cmd = nextCmd;
+    }
+
+    // "Note: In non-stop mode, a thread is considered running until GDB acknowledges 
+    // an asynchronous stop notification for it with the ‘vStopped’ packet (see Remote Non-Stop)."
+    u32 mask;
+    if (GDB_IsNonStop(ctx)) {
+        mask = ctx->acknowledgedDebugEventCoreList;
+    } else {
+        mask = ctx->attachedCoreList;
+        ctx->sendOwnDebugEventDisallowed = (continueCoreList & mask) == 0;
+    }
+
+    debugManagerSetSingleStepCoreList(stepCoreList & mask);
+    debugManagerBreakCores(stopCoreList & ~mask);
+    debugManagerContinueCores(continueCoreList & mask);
+
+    return 0;
+}

thermosphere/src/gdb/debug.h

@@ -0,0 +1,28 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+#include "../core_ctx.h"
+#include "../debug_manager.h"
+
+int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotification);
+int GDB_TrySignalDebugEvent(GDBContext *ctx, DebugEventInfo *info);
+
+void GDB_BreakAllCores(GDBContext *ctx);
+
+GDB_DECLARE_VERBOSE_HANDLER(Stopped);
+
+GDB_DECLARE_HANDLER(Detach);
+GDB_DECLARE_HANDLER(Kill);
+GDB_DECLARE_VERBOSE_HANDLER(CtrlC);
+GDB_DECLARE_HANDLER(ContinueOrStepDeprecated);
+GDB_DECLARE_VERBOSE_HANDLER(Continue);
+GDB_DECLARE_HANDLER(GetStopReason);
+
+//void GDB_BreakProcessAndSinkDebugEvents(GDBContext *ctx, DebugFlags flags);

thermosphere/src/gdb/defines.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+// Some code from:
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "../utils.h"
+
+// 512+24 is the ideal size as IDA will try to read exactly 0x100 bytes at a time.
+// IDA seems to want additional bytes as well.
+// 1024 is fine enough to put all regs in the 'T' stop reply packets
+// Add 4 to this for the actual allocated size, for $#<checksum>, see below.
+#define GDB_BUF_LEN 0x800
+#define GDB_WORK_BUF_LEN 0x1000
+
+#define GDB_HANDLER(name)           GDB_Handle##name
+#define GDB_QUERY_HANDLER(name)     GDB_HANDLER(Query##name)
+#define GDB_VERBOSE_HANDLER(name)   GDB_HANDLER(Verbose##name)
+
+#define GDB_DECLARE_HANDLER(name)           int GDB_HANDLER(name)(GDBContext *ctx)
+#define GDB_DECLARE_QUERY_HANDLER(name)     GDB_DECLARE_HANDLER(Query##name)
+#define GDB_DECLARE_VERBOSE_HANDLER(name)   GDB_DECLARE_HANDLER(Verbose##name)

thermosphere/src/gdb/hio.c

@@ -0,0 +1,133 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+
+#include "hio.h"
+#include "net.h"
+#include "mem.h"
+#include "debug.h"
+/*
+bool GDB_FetchPackedHioRequest(GDBContext *ctx, u32 addr)
+{
+    u32 total = GDB_ReadTargetMemory(&ctx->currentHioRequest, ctx, addr, sizeof(PackedGdbHioRequest));
+    if (total != sizeof(PackedGdbHioRequest) || memcmp(&ctx->currentHioRequest.magic, "GDB\x00", 4) != 0)
+    {
+        memset(&ctx->currentHioRequest, 0, sizeof(PackedGdbHioRequest));
+        ctx->currentHioRequestTargetAddr = 0;
+        return false;
+    }
+    else
+    {
+        ctx->currentHioRequestTargetAddr = addr;
+        return true;
+    }
+}
+
+bool GDB_IsHioInProgress(GDBContext *ctx)
+{
+    return ctx->currentHioRequestTargetAddr != 0;
+}
+
+int GDB_SendCurrentHioRequest(GDBContext *ctx)
+{
+    char buf[256+1];
+    char tmp[32+1];
+    u32 nStr = 0;
+
+    sprintf(buf, "F%s", ctx->currentHioRequest.functionName);
+
+    for (u32 i = 0; i < 8 && ctx->currentHioRequest.paramFormat[i] != 0; i++)
+    {
+        switch (ctx->currentHioRequest.paramFormat[i])
+        {
+            case 'i':
+            case 'I':
+            case 'p':
+                sprintf(tmp, ",%lx", (u32)ctx->currentHioRequest.parameters[i]);
+                break;
+            case 'l':
+            case 'L':
+                sprintf(tmp, ",%llx", ctx->currentHioRequest.parameters[i]);
+                break;
+            case 's':
+                sprintf(tmp, ",%lx/%x", (u32)ctx->currentHioRequest.parameters[i], ctx->currentHioRequest.stringLengths[nStr++]);
+                break;
+            default:
+                tmp[0] = 0;
+                break;
+        }
+        strcat(buf, tmp);
+    }
+
+    return GDB_SendPacket(ctx, buf, strlen(buf));
+}*/
+
+GDB_DECLARE_HANDLER(HioReply)
+{
+    return 0;
+/*    if (!GDB_IsHioInProgress(ctx))
+        return GDB_ReplyErrno(ctx, EPERM);
+    
+    // Reply in the form of Fretcode,errno,Ctrl-C flag;call-specific attachment
+    // "Call specific attachement" is always empty, though.
+
+    const char *pos = ctx->commandData;
+    u64 retval;
+
+    if (*pos == 0 || *pos == ',')
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    else if (*pos == '-')
+    {
+        pos++;
+        ctx->currentHioRequest.retval = -1ll;
+    }
+    else if (*pos == '+')
+    {
+        pos++;
+        ctx->currentHioRequest.retval = 1;
+    }
+    else
+        ctx->currentHioRequest.retval = 1;
+
+    pos = GDB_ParseHexIntegerList64(&retval, pos, 1, ',');
+
+    if (pos == NULL)
+        return GDB_ReplyErrno(ctx, EILSEQ);
+
+    ctx->currentHioRequest.retval *= retval;
+    ctx->currentHioRequest.gdbErrno = 0;
+    ctx->currentHioRequest.ctrlC = false;
+
+    if (*pos != 0)
+    {
+        u32 errno_;
+        // GDB protocol technically allows errno to have a +/- prefix but this will never happen.
+        pos = GDB_ParseHexIntegerList(&errno_, ++pos, 1, ',');
+        ctx->currentHioRequest.gdbErrno = (int)errno_;
+        if (pos == NULL)
+            return GDB_ReplyErrno(ctx, EILSEQ);
+
+        if (*pos != 0)
+        {
+            if (*pos != 'C')
+                return GDB_ReplyErrno(ctx, EILSEQ);
+
+            ctx->currentHioRequest.ctrlC = true;
+        }
+    }
+
+    memset(ctx->currentHioRequest.paramFormat, 0, sizeof(ctx->currentHioRequest.paramFormat));
+
+    u32 total = GDB_WriteTargetMemory(ctx, &ctx->currentHioRequest, ctx->currentHioRequestTargetAddr, sizeof(PackedGdbHioRequest));
+
+    memset(&ctx->currentHioRequest, 0, sizeof(PackedGdbHioRequest));
+    ctx->currentHioRequestTargetAddr = 0;
+
+    GDB_ContinueExecution(ctx);
+    return total == sizeof(PackedGdbHioRequest) ? 0 : GDB_ReplyErrno(ctx, EFAULT);*/
+}

thermosphere/src/gdb/hio.h

@@ -0,0 +1,16 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+bool GDB_FetchPackedHioRequest(GDBContext *ctx, u32 addr);
+bool GDB_IsHioInProgress(GDBContext *ctx);
+int GDB_SendCurrentHioRequest(GDBContext *ctx);
+
+GDB_DECLARE_HANDLER(HioReply);

thermosphere/src/gdb/mem.c

@@ -0,0 +1,182 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+#include "mem.h"
+#include "net.h"
+#include "../guest_memory.h"
+#include "../pattern_utils.h"
+
+int GDB_SendMemory(GDBContext *ctx, const char *prefix, size_t prefixLen, size_t addr, size_t len)
+{
+    char *buf = ctx->buffer + 1;
+    char *membuf = ctx->workBuffer;
+
+    if(prefix != NULL) {
+        memmove(buf, prefix, prefixLen);
+    }
+    else {
+        prefixLen = 0;
+    }
+
+    if(prefixLen + 2 * len > GDB_BUF_LEN) {
+        // gdb shouldn't send requests which responses don't fit in a packet
+        return prefix == NULL ? GDB_ReplyErrno(ctx, ENOMEM) : -1;
+    }
+
+    size_t total = guestReadMemory(addr, len, membuf);
+
+    if (total == 0) {
+        return prefix == NULL ? GDB_ReplyErrno(ctx, EFAULT) : -EFAULT;
+    } else {
+        GDB_EncodeHex(buf + prefixLen, membuf, total);
+        return GDB_SendPacket(ctx, buf, prefixLen + 2 * total);
+    }
+}
+
+int GDB_WriteMemory(GDBContext *ctx, const void *buf, uintptr_t addr, size_t len)
+{
+    size_t total = guestWriteMemory(addr, len, buf);
+    return total == len ? GDB_ReplyOk(ctx) : GDB_ReplyErrno(ctx, EFAULT);
+}
+
+u32 GDB_SearchMemory(bool *found, GDBContext *ctx, uintptr_t addr, size_t len, const void *pattern, size_t patternLen)
+{
+    // Note: need to ensure GDB_WORK_BUF_LEN is at least 0x1000 bytes in size
+
+    u8 *buf = (u8 *)ctx->workBuffer;
+    size_t maxNbPages = GDB_WORK_BUF_LEN / 0x1000;
+    uintptr_t curAddr = addr;
+
+    while (curAddr < addr + len) {
+        size_t nbPages;
+        uintptr_t addrBase = curAddr & ~0xFFF;
+        size_t addrDispl = curAddr & 0xFFF;
+
+        for (nbPages = 0; nbPages < maxNbPages; nbPages++) {
+            if (guestReadMemory(addrBase + nbPages * 0x1000, 0x1000, buf + nbPages * 0x1000) != 0x1000) {
+                break;
+            }
+        }
+
+        u8 *pos = NULL;
+        if(addrDispl + patternLen <= 0x1000 * nbPages) {
+            pos = memsearch(buf + addrDispl, pattern, 0x1000 * nbPages - addrDispl, patternLen);
+        }
+
+        if(pos != NULL) {
+            *found = true;
+            return addrBase + (pos - buf);
+        }
+
+        curAddr = addrBase + 0x1000;
+    }
+
+    *found = false;
+    return 0;
+}
+
+GDB_DECLARE_HANDLER(ReadMemory)
+{
+    unsigned long lst[2];
+    if (GDB_ParseHexIntegerList(lst, ctx->commandData, 2, 0) == NULL) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    uintptr_t addr = lst[0];
+    size_t len = lst[1];
+
+    return GDB_SendMemory(ctx, NULL, 0, addr, len);
+}
+
+GDB_DECLARE_HANDLER(WriteMemory)
+{
+    unsigned long lst[2];
+    const char *dataStart = GDB_ParseHexIntegerList(lst, ctx->commandData, 2, ':');
+    if (dataStart == NULL || *dataStart != ':') {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    dataStart++;
+    uintptr_t addr = lst[0];
+    size_t len = lst[1];
+
+    if(dataStart + 2 * len >= ctx->buffer + 4 + GDB_BUF_LEN) {
+        // Data len field doesn't match what we got...
+        return GDB_ReplyErrno(ctx, ENOMEM);
+    }
+
+    size_t n = GDB_DecodeHex(ctx->workBuffer, dataStart, len);
+
+    if(n != len) {
+        // Decoding error...
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    return GDB_WriteMemory(ctx, ctx->workBuffer, addr, len);
+}
+
+GDB_DECLARE_HANDLER(WriteMemoryRaw)
+{
+    unsigned long lst[2];
+    const char *dataStart = GDB_ParseHexIntegerList(lst, ctx->commandData, 2, ':');
+    if (dataStart == NULL || *dataStart != ':') {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    dataStart++;
+    uintptr_t addr = lst[0];
+    size_t len = lst[1];
+
+    if(dataStart + 2 * len >= ctx->buffer + 4 + GDB_BUF_LEN) {
+        // Data len field doesn't match what we got...
+        return GDB_ReplyErrno(ctx, ENOMEM);
+    }
+
+    // Note: could be done in place in ctx->buffer...
+    size_t n = GDB_UnescapeBinaryData(ctx->workBuffer, dataStart, len);
+
+    if(n != len) {
+        // Decoding error...
+        return GDB_ReplyErrno(ctx, n);
+    }
+
+    return GDB_WriteMemory(ctx, ctx->workBuffer, addr, len);
+}
+
+GDB_DECLARE_QUERY_HANDLER(SearchMemory)
+{
+    unsigned long lst[2];
+    const char *patternStart;
+    size_t patternLen;
+    bool found;
+    u32 foundAddr;
+
+    if (strncmp(ctx->commandData, "memory:", 7) != 0) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    ctx->commandData += 7;
+    patternStart = GDB_ParseIntegerList(lst, ctx->commandData, 2, ';', ';', 16, false);
+    if (patternStart == NULL || *patternStart != ';') {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    uintptr_t addr = lst[0];
+    size_t len = lst[1];
+
+    patternStart++;
+    patternLen = ctx->commandEnd - patternStart;
+
+    // Unescape pattern in place
+    char *pattern = (char *)patternStart;
+    patternLen = GDB_UnescapeBinaryData(pattern, patternStart, patternLen);
+
+    foundAddr = GDB_SearchMemory(&found, ctx, addr, len, patternStart, patternLen);
+
+    return found ? GDB_SendFormattedPacket(ctx, "1,%x", foundAddr) : GDB_SendPacket(ctx, "0", 1);
+}

thermosphere/src/gdb/mem.h

@@ -0,0 +1,19 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+int GDB_SendMemory(GDBContext *ctx, const char *prefix, size_t prefixLen, uintptr_t addr, size_t len);
+int GDB_WriteMemory(GDBContext *ctx, const void *buf, uintptr_t addr, size_t len);
+u32 GDB_SearchMemory(bool *found, GDBContext *ctx, size_t addr, size_t len, const void *pattern, size_t patternLen);
+
+GDB_DECLARE_HANDLER(ReadMemory);
+GDB_DECLARE_HANDLER(WriteMemory);
+GDB_DECLARE_HANDLER(WriteMemoryRaw);
+GDB_DECLARE_QUERY_HANDLER(SearchMemory);

thermosphere/src/gdb/net.c

@@ -0,0 +1,371 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "net.h"
+
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include "../pattern_utils.h"
+
+u8 GDB_ComputeChecksum(const char *packetData, size_t len)
+{
+    unsigned long cksum = 0;
+    for(size_t i = 0; i < len; i++) {
+        cksum += packetData[i];
+    }
+
+    return (u8)cksum;
+}
+
+size_t GDB_EncodeHex(char *dst, const void *src, size_t len)
+{
+    static const char *alphabet = "0123456789abcdef";
+    const u8 *src8 = (const u8 *)src;
+
+    for (size_t i = 0; i < len; i++) {
+        dst[2 * i] = alphabet[(src8[i] & 0xf0) >> 4];
+        dst[2 * i + 1] = alphabet[src8[i] & 0x0f];
+    }
+
+    return 2 * len;
+}
+
+static inline u32 GDB_DecodeHexDigit(char src, bool *ok)
+{
+    *ok = true;
+    switch (src) {
+        case '0' ... '9': return  0 + (src - '0');
+        case 'a' ... 'f': return 10 + (src - 'a');
+        case 'A' ... 'F': return 10 + (src - 'A');
+        default:
+            *ok = false;
+            return 0;
+    }
+}
+
+size_t GDB_DecodeHex(void *dst, const char *src, size_t len) {
+    size_t i = 0;
+    u8 *dst8 = (u8 *)dst;
+    for (i = 0; i < len && src[2 * i] != 0 && src[2 * i + 1] != 0; i++) {
+        bool ok1, ok2;
+        dst8[i]  = GDB_DecodeHexDigit(src[2 * i], &ok1) << 4;
+        dst8[i] |= GDB_DecodeHexDigit(src[2 * i + 1], &ok2);
+        if (!ok1 || !ok2) {
+            return i;
+        }
+    }
+
+    return i;
+}
+
+size_t GDB_EscapeBinaryData(size_t *encodedCount, void *dst, const void *src, size_t len, size_t maxLen)
+{
+    u8 *dst8 = (u8 *)dst;
+    const u8 *src8 = (const u8 *)src;
+
+    maxLen = maxLen >= len ? len : maxLen;
+
+    while ((uintptr_t)dst8 < (uintptr_t)dst + maxLen) {
+        if (*src8 == '$' || *src8 == '#' || *src8 == '}' || *src8 == '*') {
+            if ((uintptr_t)dst8 + 1 >= (uintptr_t)dst + maxLen) {
+                break;
+            }
+            *dst8++ = '}';
+            *dst8++ = *src8++ ^ 0x20;
+        }
+        else {
+            *dst8++ = *src8++;
+        }
+    }
+
+    *encodedCount = dst8 - (u8 *)dst;
+    return src8 - (u8 *)src;
+}
+
+size_t GDB_UnescapeBinaryData(void *dst, const void *src, size_t len)
+{
+    u8 *dst8 = (u8 *)dst;
+    const u8 *src8 = (const u8 *)src;
+
+    while ((uintptr_t)src8 < (uintptr_t)src + len) {
+        if (*src8 == '}') {
+            src8++;
+            *dst8++ = *src8++ ^ 0x20;
+        } else {
+            *dst8++ = *src8++;
+        }
+    }
+
+    return dst8 - (u8 *)dst;
+}
+
+const char *GDB_ParseIntegerList(unsigned long *dst, const char *src, size_t nb, char sep, char lastSep, u32 base, bool allowPrefix)
+{
+    const char *pos = src;
+    const char *endpos;
+    bool ok;
+
+    for (size_t i = 0; i < nb; i++) {
+        unsigned long n = xstrtoul(pos, (char **)&endpos, (int) base, allowPrefix, &ok);
+        if(!ok || endpos == pos) {
+            return NULL;
+        }
+
+        if (i != nb - 1) {
+            if (*endpos != sep) {
+                return NULL;
+            }
+            pos = endpos + 1;
+        } else {
+            if (*endpos != lastSep && *endpos != 0) {
+                return NULL;
+            }
+            pos = endpos;
+        }
+
+        dst[i] = n;
+    }
+
+    return pos;
+}
+
+const char *GDB_ParseHexIntegerList(unsigned long *dst, const char *src, size_t nb, char lastSep)
+{
+    return GDB_ParseIntegerList(dst, src, nb, ',', lastSep, 16, false);
+}
+
+static int GDB_SendNackIfPossible(GDBContext *ctx) {
+    if (ctx->flags & GDB_FLAG_NOACK) {
+        return -1;
+    } else {
+        char hdr = '-';
+        transportInterfaceWriteData(ctx->transportInterface, &hdr, 1);
+        return 1;
+    }
+}
+
+int GDB_ReceivePacket(GDBContext *ctx)
+{
+    char hdr;
+    bool ctrlC = false;
+    TransportInterface *iface = ctx->transportInterface;
+
+    // Read the first character...
+    transportInterfaceReadData(iface, &hdr, 1);
+
+    // Check if the ack/nack packets are not allowed
+    if ((hdr == '+' || hdr == '-') && (ctx->flags & GDB_FLAG_NOACK) != 0) {
+        DEBUG("Received a packed with an invalid header from GDB, hdr=%c\n", hdr);
+        return -1;
+    } 
+
+    switch (hdr) {
+        case '+': {
+            // Ack, don't do anything else except maybe NoAckMode state transition
+            if (ctx->noAckSent) {
+                ctx->flags |= GDB_FLAG_NOACK;
+                ctx->noAckSent = false;
+            }
+            return 0;
+        }
+        case '-':
+            // Nack, return the previous packet
+            transportInterfaceWriteData(iface, ctx->buffer, ctx->lastSentPacketSize);
+            return ctx->lastSentPacketSize;
+        case '$':
+            // Normal packet, handled below
+            break;
+        case '\x03':
+            // Normal packet (Control-C), handled below
+            ctrlC = true;
+            break;
+        default:
+            // Oops, send a nack
+            DEBUG("Received a packed with an invalid header from GDB, hdr=%c\n", hdr);
+            return GDB_SendNackIfPossible(ctx);
+    }
+
+    // We didn't get a nack past this point, read the remaining data if any
+
+    ctx->buffer[0] = hdr;
+    if (ctrlC) {
+        // Will never normally happen, but ok
+        if (ctx->state < GDB_STATE_ATTACHED) {
+            DEBUG("Received connection from GDB, now attaching...\n");
+            GDB_AttachToContext(ctx);
+            ctx->state = GDB_STATE_ATTACHED;
+        }
+        return 1;
+    }
+
+    size_t delimPos = transportInterfaceReadDataUntil(iface, ctx->buffer + 1, 4 + GDB_BUF_LEN - 1, '#');
+    if (ctx->buffer[delimPos] != '#') {
+        // The packet is malformed, send a nack
+        return GDB_SendNackIfPossible(ctx);
+    }
+
+    // Read the checksum
+    size_t checksumPos = delimPos + 1;
+    u8 checksum;
+    transportInterfaceReadData(iface, ctx->buffer + checksumPos, 2);
+
+    if (GDB_DecodeHex(&checksum, ctx->buffer + checksumPos, 1) != 1) {
+        // Malformed checksum
+        return GDB_SendNackIfPossible(ctx);
+    } else if (GDB_ComputeChecksum(ctx->buffer + 1, delimPos - 1) != checksum) {
+        // Invalid checksum
+        return GDB_SendNackIfPossible(ctx);
+    }
+
+    // Ok, send ack (if possible)
+    if (!(ctx->flags & GDB_FLAG_NOACK)) {
+        hdr = '+';
+        transportInterfaceWriteData(iface, &hdr, 1);
+    }
+
+    // State transitions...
+    if (ctx->state < GDB_STATE_ATTACHED) {
+        DEBUG("Received connection from GDB, now attaching...\n");
+        GDB_AttachToContext(ctx);
+        ctx->state = GDB_STATE_ATTACHED;
+    }
+
+    // Debug
+    ctx->buffer[checksumPos + 2] = '\0';
+    DEBUGRAW("->");
+    DEBUGRAW(ctx->buffer);
+    DEBUGRAW("\n");
+
+    // Set helper attributes, change '#' to NUL
+    ctx->commandData = ctx->buffer + 2;
+    ctx->commandEnd = ctx->buffer + delimPos;
+    ctx->buffer[delimPos] = '\0';
+
+    return (int)(delimPos + 2);
+}
+
+static int GDB_DoSendPacket(GDBContext *ctx, size_t len)
+{
+    transportInterfaceWriteData(ctx->transportInterface, ctx->buffer, len);
+    ctx->lastSentPacketSize = len;
+
+    // Debugging:
+    ctx->buffer[len] = 0;
+    DEBUGRAW("<-");
+    DEBUGRAW(ctx->buffer);
+    DEBUGRAW("\n");
+
+    return (int)len;
+}
+
+int GDB_SendPacket(GDBContext *ctx, const char *packetData, size_t len)
+{
+    if (packetData != ctx->buffer + 1) {
+        memmove(ctx->buffer + 1, packetData, len);
+    }
+
+    ctx->buffer[0] = '$';
+
+    char *checksumLoc = ctx->buffer + len + 1;
+    *checksumLoc++ = '#';
+
+    hexItoa(GDB_ComputeChecksum(ctx->buffer + 1, len), checksumLoc, 2, false);
+    return GDB_DoSendPacket(ctx, 4 + len);
+}
+
+int GDB_SendFormattedPacket(GDBContext *ctx, const char *packetDataFmt, ...)
+{
+    va_list args;
+
+    va_start(args, packetDataFmt);
+    int n = vsprintf(ctx->buffer + 1, packetDataFmt, args);
+    va_end(args);
+
+    if (n < 0) {
+        return -1;
+    }
+
+    ctx->buffer[0] = '$';
+    char *checksumLoc = ctx->buffer + n + 1;
+    *checksumLoc++ = '#';
+
+    hexItoa(GDB_ComputeChecksum(ctx->buffer + 1, n), checksumLoc, 2, false);
+    return GDB_DoSendPacket(ctx, 4 + n);
+}
+
+int GDB_SendHexPacket(GDBContext *ctx, const void *packetData, size_t len)
+{
+    if(4 + 2 * len > GDB_BUF_LEN)
+        return -1;
+
+    ctx->buffer[0] = '$';
+    GDB_EncodeHex(ctx->buffer + 1, packetData, len);
+
+    char *checksumLoc = ctx->buffer + 2 * len + 1;
+    *checksumLoc++ = '#';
+
+    hexItoa(GDB_ComputeChecksum(ctx->buffer + 1, 2 * len), checksumLoc, 2, false);
+    return GDB_DoSendPacket(ctx, 4 + 2 * len);
+}
+
+int GDB_SendNotificationPacket(GDBContext *ctx, const char *packetData, size_t len)
+{
+    if (packetData != ctx->buffer + 1) {
+        memmove(ctx->buffer + 1, packetData, len);
+    }
+
+    ctx->buffer[0] = '%';
+
+    char *checksumLoc = ctx->buffer + len + 1;
+    *checksumLoc++ = '#';
+
+    hexItoa(GDB_ComputeChecksum(ctx->buffer + 1, len), checksumLoc, 2, false);
+    return GDB_DoSendPacket(ctx, 4 + len);
+}
+
+int GDB_SendStreamData(GDBContext *ctx, const char *streamData, size_t offset, size_t length, size_t totalSize, bool forceEmptyLast)
+{
+    // GDB_BUF_LEN does not include the usual %#<1-byte checksum>
+    if(length > GDB_BUF_LEN - 1) {
+        length = GDB_BUF_LEN - 1;
+    }
+
+    char letter;
+
+    if ((forceEmptyLast && offset >= totalSize) || (!forceEmptyLast && offset + length >= totalSize)) {
+        length = offset >= totalSize ? 0 : totalSize - offset;
+        letter = 'l';
+    } else {
+        letter = 'm';
+    }
+
+    // Note: ctx->buffer[0] = '$'
+    if (streamData + offset != ctx->buffer + 2) {
+        memmove(ctx->buffer + 2, streamData + offset, length);
+    }
+    ctx->buffer[1] = letter; 
+    return GDB_SendPacket(ctx, ctx->buffer + 1, 1 + length);
+}
+
+int GDB_ReplyEmpty(GDBContext *ctx)
+{
+    return GDB_SendPacket(ctx, "", 0);
+}
+
+int GDB_ReplyOk(GDBContext *ctx)
+{
+    return GDB_SendPacket(ctx, "OK", 2);
+}
+
+int GDB_ReplyErrno(GDBContext *ctx, int no)
+{
+    char buf[] = "E01";
+    hexItoa(no & 0xFF, buf + 1, 2, false);
+    return GDB_SendPacket(ctx, buf, 3);
+}

thermosphere/src/gdb/net.h

@@ -0,0 +1,31 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+#define _REENT_ONLY
+#include <errno.h>
+
+u8 GDB_ComputeChecksum(const char *packetData, size_t len);
+size_t GDB_EncodeHex(char *dst, const void *src, size_t len);
+size_t GDB_DecodeHex(void *dst, const char *src, size_t len);
+size_t GDB_EscapeBinaryData(size_t *encodedCount, void *dst, const void *src, size_t len, size_t maxLen);
+size_t GDB_UnescapeBinaryData(void *dst, const void *src, size_t len);
+
+const char *GDB_ParseIntegerList(unsigned long *dst, const char *src, size_t nb, char sep, char lastSep, u32 base, bool allowPrefix);
+const char *GDB_ParseHexIntegerList(unsigned long *dst, const char *src, size_t nb, char lastSep);
+
+int GDB_ReceivePacket(GDBContext *ctx);
+int GDB_SendPacket(GDBContext *ctx, const char *packetData, size_t len);
+int GDB_SendFormattedPacket(GDBContext *ctx, const char *packetDataFmt, ...);
+int GDB_SendHexPacket(GDBContext *ctx, const void *packetData, size_t len);
+int GDB_SendNotificationPacket(GDBContext *ctx, const char *packetData, size_t len);
+int GDB_SendStreamData(GDBContext *ctx, const char *streamData, size_t offset, size_t length, size_t totalSize, bool forceEmptyLast);
+int GDB_ReplyEmpty(GDBContext *ctx);
+int GDB_ReplyOk(GDBContext *ctx);
+int GDB_ReplyErrno(GDBContext *ctx, int no);

thermosphere/src/gdb/query.c

@@ -0,0 +1,108 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+
+#include "../utils.h"
+
+#include "query.h"
+#include "xfer.h"
+#include "thread.h"
+#include "mem.h"
+#include "net.h"
+#include "remote_command.h"
+
+typedef enum GDBQueryDirection {
+    GDB_QUERY_DIRECTION_READ,
+    GDB_QUERY_DIRECTION_WRITE
+} GDBQueryDirection;
+
+#define GDB_QUERY_HANDLER_LIST_ITEM_3(name, name2, direction)   { name, GDB_QUERY_HANDLER(name2), GDB_QUERY_DIRECTION_##direction }
+#define GDB_QUERY_HANDLER_LIST_ITEM(name, direction)            GDB_QUERY_HANDLER_LIST_ITEM_3(STRINGIZE(name), name, direction)
+
+static const struct {
+    const char *name;
+    GDBCommandHandler handler;
+    GDBQueryDirection direction;
+} gdbQueryHandlers[] =
+{
+    GDB_QUERY_HANDLER_LIST_ITEM(Supported, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM(Xfer, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM(StartNoAckMode, WRITE),
+    GDB_QUERY_HANDLER_LIST_ITEM(Attached, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM(fThreadInfo, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM(sThreadInfo, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM(ThreadEvents, WRITE),
+    GDB_QUERY_HANDLER_LIST_ITEM(ThreadExtraInfo, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM_3("C", CurrentThreadId, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM_3("Search", SearchMemory, READ),
+    GDB_QUERY_HANDLER_LIST_ITEM(Rcmd, READ),
+};
+
+static int GDB_HandleQuery(GDBContext *ctx, GDBQueryDirection direction)
+{
+    char *nameBegin = ctx->commandData; // w/o leading 'q'/'Q'
+    if(*nameBegin == 0)
+        return GDB_ReplyErrno(ctx, EILSEQ);
+
+    char *nameEnd;
+    char *queryData = NULL;
+
+    for(nameEnd = nameBegin; *nameEnd != 0 && *nameEnd != ':' && *nameEnd != ','; nameEnd++);
+    if(*nameEnd != 0)
+    {
+        *nameEnd = 0;
+        queryData = nameEnd + 1;
+    }
+    else
+        queryData = nameEnd;
+
+    for(u32 i = 0; i < sizeof(gdbQueryHandlers) / sizeof(gdbQueryHandlers[0]); i++)
+    {
+        if(strcmp(gdbQueryHandlers[i].name, nameBegin) == 0 && gdbQueryHandlers[i].direction == direction)
+        {
+            ctx->commandData = queryData;
+            return gdbQueryHandlers[i].handler(ctx);
+        }
+    }
+
+    return GDB_HandleUnsupported(ctx); // No handler found!
+}
+
+int GDB_HandleReadQuery(GDBContext *ctx)
+{
+    return GDB_HandleQuery(ctx, GDB_QUERY_DIRECTION_READ);
+}
+
+int GDB_HandleWriteQuery(GDBContext *ctx)
+{
+    return GDB_HandleQuery(ctx, GDB_QUERY_DIRECTION_WRITE);
+}
+
+GDB_DECLARE_QUERY_HANDLER(Supported)
+{
+    // TODO!
+    return GDB_SendFormattedPacket(ctx,
+        "PacketSize=%x;"
+        "qXfer:features:read+;"
+        "QStartNoAckMode+;QThreadEvents+"
+        "vContSupported+;swbreak+;hwbreak+",
+
+        GDB_BUF_LEN // should have been sizeof(ctx->buffer) but GDB memory functions are bugged
+    );
+}
+
+GDB_DECLARE_QUERY_HANDLER(StartNoAckMode)
+{
+    ctx->noAckSent = true;
+    return GDB_ReplyOk(ctx);
+}
+
+GDB_DECLARE_QUERY_HANDLER(Attached)
+{
+    return GDB_SendPacket(ctx, "1", 1);
+}

thermosphere/src/gdb/query.h

@@ -0,0 +1,17 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+int GDB_HandleReadQuery(GDBContext *ctx);
+int GDB_HandleWriteQuery(GDBContext *ctx);
+
+GDB_DECLARE_QUERY_HANDLER(Supported);
+GDB_DECLARE_QUERY_HANDLER(StartNoAckMode);
+GDB_DECLARE_QUERY_HANDLER(Attached);

thermosphere/src/gdb/regs.c

@@ -0,0 +1,212 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+#include <assert.h>
+
+#include "../exceptions.h"
+#include "../fpu.h"
+
+#include "regs.h"
+#include "net.h"
+
+// GDB treats cpsr, fpsr, fpcr as 32-bit integers...
+
+GDB_DECLARE_HANDLER(ReadRegisters)
+{
+    ENSURE(ctx->selectedThreadId == 1 + currentCoreCtx->coreId);
+
+    ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
+    FpuRegisterCache *fpuRegCache = fpuReadRegisters();
+
+    char *buf = ctx->buffer + 1;
+    size_t n = 0;
+
+    struct {
+        u64 sp;
+        u64 pc;
+        u32 cpsr;
+    } cpuSprs = {
+        .sp = *exceptionGetSpPtr(frame),
+        .pc = frame->elr_el2,
+        .cpsr = (u32)frame->spsr_el2,
+    };
+
+    u32 fpuSprs[2] = {
+        (u32)fpuRegCache->fpsr,
+        (u32)fpuRegCache->fpcr,
+    };
+
+
+    n += GDB_EncodeHex(buf + n, frame->x, sizeof(frame->x));
+    n += GDB_EncodeHex(buf + n, &cpuSprs, 8+8+4);
+    n += GDB_EncodeHex(buf + n, fpuRegCache->q, sizeof(fpuRegCache->q));
+    n += GDB_EncodeHex(buf + n, fpuSprs, sizeof(fpuSprs));
+
+    return GDB_SendPacket(ctx, buf, n);
+}
+
+GDB_DECLARE_HANDLER(WriteRegisters)
+{
+    ENSURE(ctx->selectedThreadId == 1 + currentCoreCtx->coreId);
+
+    ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
+    FpuRegisterCache *fpuRegCache = fpuGetRegisterCache();
+
+    char *buf = ctx->commandData;
+    char *tmp = ctx->workBuffer;
+
+    size_t n = 0;
+    size_t m = 0;
+
+    struct {
+        u64 sp;
+        u64 pc;
+        u32 cpsr;
+    } cpuSprs;
+
+    u32 fpuSprs[2];
+
+    struct {
+        void *dst;
+        size_t sz;
+    } info[4] = {
+        { frame->x,         sizeof(frame->x)        },
+        { &cpuSprs,         8+8+4                   },
+        { fpuRegCache->q,   sizeof(fpuRegCache->q)  },
+        { fpuSprs,          sizeof(fpuSprs)         },
+    };
+
+    // Parse & return on error
+    for (u32 i = 0; i < 4; i++) {
+        if (GDB_DecodeHex(tmp + m, buf + n, info[i].sz) != info[i].sz) {
+            return GDB_ReplyErrno(ctx, EPERM);
+        }
+        n += 2 * info[i].sz;
+        m += info[i].sz;
+    }
+
+    // Copy. Note: we don't check if cpsr (spsr_el2) was modified to return to EL2...
+    m = 0;
+    for (u32 i = 0; i < 4; i++) {
+        memcpy(info[i].dst, tmp + m, info[i].sz);
+        m += info[i].sz;
+    }
+    *exceptionGetSpPtr(frame) = cpuSprs.sp;
+    frame->elr_el2 = cpuSprs.pc;
+    frame->spsr_el2 = cpuSprs.cpsr;
+    fpuRegCache->fpsr = fpuSprs[0];
+    fpuRegCache->fpcr = fpuSprs[1];
+    fpuCommitRegisters();
+
+    return GDB_ReplyOk(ctx);
+}
+
+static void GDB_GetRegisterPointerAndSize(size_t *outSz, void **outPtr, unsigned long id, ExceptionStackFrame *frame, FpuRegisterCache *fpuRegCache)
+{
+    switch (id) {
+        case 0 ... 30:
+            *outPtr = &frame->x[id];
+            *outSz = 8;
+            break;
+        case 31:
+            *outPtr = exceptionGetSpPtr(frame);
+            *outSz = 8;
+            break;
+        case 32:
+            *outPtr = &frame->spsr_el2;
+            *outSz = 4;
+            break;
+        case 33 ... 64:
+            *outPtr = &fpuRegCache->q[id - 33];
+            *outSz = 16;
+        case 65:
+            *outPtr = &fpuRegCache->fpsr;
+            *outSz = 4;
+        case 66:
+            *outPtr = &fpuRegCache->fpcr;
+            *outSz = 4;
+        default:
+            __builtin_unreachable();
+            return;
+    }
+}
+
+GDB_DECLARE_HANDLER(ReadRegister)
+{
+    ENSURE(ctx->selectedThreadId == 1 + currentCoreCtx->coreId);
+
+    ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
+    FpuRegisterCache *fpuRegCache = NULL;
+
+    unsigned long gdbRegNum;
+
+    if (GDB_ParseHexIntegerList(&gdbRegNum, ctx->commandData, 1, 0) == NULL) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    // Check the register number
+    if (gdbRegNum >= 31 + 3 + 32 + 2) {
+        return GDB_ReplyErrno(ctx, EINVAL);
+    }
+
+    if (gdbRegNum > 31 + 3) {
+        // FPU register -- must read the FPU registers first
+        fpuRegCache = fpuReadRegisters();
+    }
+
+    size_t sz;
+    void *regPtr;
+    GDB_GetRegisterPointerAndSize(&sz, &regPtr, gdbRegNum, frame, fpuRegCache);
+
+    return GDB_SendHexPacket(ctx, regPtr, sz);
+}
+
+GDB_DECLARE_HANDLER(WriteRegister)
+{
+    ENSURE(ctx->selectedThreadId == 1 + currentCoreCtx->coreId);
+
+    ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
+    FpuRegisterCache *fpuRegCache = fpuGetRegisterCache();
+
+    char *tmp = ctx->workBuffer;
+    unsigned long gdbRegNum;
+
+    const char *valueStart = GDB_ParseHexIntegerList(&gdbRegNum, ctx->commandData, 1, '=');
+    if(valueStart == NULL || *valueStart != '=') {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+    valueStart++;
+
+    // Check the register number
+    if (gdbRegNum >= 31 + 3 + 32 + 2) {
+        return GDB_ReplyErrno(ctx, EINVAL);
+    }
+
+    size_t sz;
+    void *regPtr;
+    GDB_GetRegisterPointerAndSize(&sz, &regPtr, gdbRegNum, frame, fpuRegCache);
+
+    // Check if we got 2 hex digits per byte
+    if (strlen(valueStart) != 2 * sz) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    // Decode, check for errors
+    if (GDB_DecodeHex(tmp, valueStart, sz) != sz) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    memcpy(regPtr, tmp, sz);
+
+    if (gdbRegNum > 31 + 3) {
+        // FPU register -- must commit the FPU registers
+        fpuCommitRegisters();
+    }
+
+    return GDB_ReplyOk(ctx);
+}

thermosphere/src/gdb/regs.h

@@ -0,0 +1,15 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+GDB_DECLARE_HANDLER(ReadRegisters);
+GDB_DECLARE_HANDLER(WriteRegisters);
+GDB_DECLARE_HANDLER(ReadRegister);
+GDB_DECLARE_HANDLER(WriteRegister);

thermosphere/src/gdb/remote_command.c

@@ -0,0 +1,51 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+
+#include "remote_command.h"
+#include "net.h"
+struct
+{
+    const char *name;
+    GDBCommandHandler handler;
+} remoteCommandHandlers[] = {
+};
+
+static const char *GDB_SkipSpaces(const char *pos)
+{
+    const char *nextpos;
+    for (nextpos = pos; *nextpos != 0 && ((*nextpos >= 9 && *nextpos <= 13) || *nextpos == ' '); nextpos++);
+    return nextpos;
+}
+
+GDB_DECLARE_QUERY_HANDLER(Rcmd)
+{
+    char commandData[GDB_BUF_LEN / 2 + 1];
+    char *endpos;
+    const char *errstr = "Unrecognized command.\n";
+    size_t len = strlen(ctx->commandData);
+
+    if(len == 0 || (len % 2) == 1 || GDB_DecodeHex(commandData, ctx->commandData, len / 2) != len / 2) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+    commandData[len / 2] = 0;
+
+    for (endpos = commandData; !(*endpos >= 9 && *endpos <= 13) && *endpos != ' ' && *endpos != 0; endpos++);
+
+    char *nextpos = (char *)GDB_SkipSpaces(endpos);
+    *endpos = 0;
+
+    for (size_t i = 0; i < sizeof(remoteCommandHandlers) / sizeof(remoteCommandHandlers[0]); i++) {
+        if (strcmp(commandData, remoteCommandHandlers[i].name) == 0) {
+            ctx->commandData = nextpos;
+            return remoteCommandHandlers[i].handler(ctx);
+        }
+    }
+
+    return GDB_SendHexPacket(ctx, errstr, strlen(errstr));
+}

thermosphere/src/gdb/remote_command.h

@@ -0,0 +1,15 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+#define GDB_REMOTE_COMMAND_HANDLER(name)         GDB_HANDLER(RemoteCommand##name)
+#define GDB_DECLARE_REMOTE_COMMAND_HANDLER(name) GDB_DECLARE_HANDLER(RemoteCommand##name)
+
+GDB_DECLARE_QUERY_HANDLER(Rcmd);

thermosphere/src/gdb/stop_points.c

@@ -0,0 +1,70 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+
+#include "context.h"
+#include "net.h"
+
+#include "../breakpoints.h"
+#include "../software_breakpoints.h"
+#include "../watchpoints.h"
+
+GDB_DECLARE_HANDLER(ToggleStopPoint)
+{
+    bool add = ctx->commandData[-1] == 'Z';
+    unsigned long lst[3];
+
+    const char *pos = GDB_ParseHexIntegerList(lst, ctx->commandData, 3, ';');
+    if (pos == NULL) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+    bool persist = *pos != 0 && strncmp(pos, ";cmds:1", 7) == 0;
+
+    // In theory we should reject leading zeroes in "kind". Oh well...
+    unsigned long kind = lst[0];
+    uintptr_t addr = lst[1];
+    size_t size = lst[2];
+
+    int res;
+    static const WatchpointLoadStoreControl kinds[3] = {
+        WatchpointLoadStoreControl_Store,
+        WatchpointLoadStoreControl_Load,
+        WatchpointLoadStoreControl_LoadStore,
+    };
+
+    switch(kind) {
+        // Software breakpoint
+        case 0: {
+            if(size != 4) {
+                return GDB_ReplyErrno(ctx, EINVAL);
+            }
+            res = add ? addSoftwareBreakpoint(addr, persist) : removeSoftwareBreakpoint(addr, false);
+            return res == 0 ? GDB_ReplyOk(ctx) : GDB_ReplyErrno(ctx, -res);
+        }
+
+        // Hardware breakpoint
+        case 1: {
+            if(size != 4) {
+                return GDB_ReplyErrno(ctx, EINVAL);
+            }
+            res = add ? addBreakpoint(addr) : removeBreakpoint(addr);
+            return res == 0 ? GDB_ReplyOk(ctx) : GDB_ReplyErrno(ctx, -res);
+        }
+
+        // Watchpoints
+        case 2:
+        case 3:
+        case 4: {
+            res = add ? addWatchpoint(addr, size, kinds[kind - 2]) : removeWatchpoint(addr, size, kinds[kind - 2]);
+            return res == 0 ? GDB_ReplyOk(ctx) : GDB_ReplyErrno(ctx, -res);
+        }
+        default: {
+            return GDB_ReplyEmpty(ctx);
+        }
+    }
+}

thermosphere/src/gdb/stop_points.h

@@ -0,0 +1,12 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+GDB_DECLARE_HANDLER(ToggleStopPoint);

thermosphere/src/gdb/thread.c

@@ -0,0 +1,119 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <stdio.h>
+#include <string.h>
+
+#include "thread.h"
+#include "net.h"
+#include "../core_ctx.h"
+
+GDB_DECLARE_HANDLER(SetThreadId)
+{
+    // Id = 0 means any thread
+    if (ctx->commandData[0] == 'g') {
+        if(strcmp(ctx->commandData + 1, "-1") == 0) {
+            return GDB_ReplyErrno(ctx, EINVAL);
+        }
+
+        unsigned long id;
+        if (GDB_ParseHexIntegerList(&id, ctx->commandData + 1, 1, 0) == NULL) {
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }  else if (id >= MAX_CORE + 1) {
+                return GDB_ReplyErrno(ctx, EINVAL);
+        }
+
+        ctx->selectedThreadId = id == 0 ? (int)currentCoreCtx->coreId + 1 : (int)id;
+        GDB_MigrateRxIrq(ctx, (u32)(ctx->selectedThreadId - 1));
+        return GDB_ReplyOk(ctx);
+    } else if (ctx->commandData[0] == 'c') {
+        if(strcmp(ctx->commandData + 1, "-1") == 0) {
+            ctx->selectedThreadIdForContinuing = -1;
+        } else {
+            unsigned long id;
+            if (GDB_ParseHexIntegerList(&id, ctx->commandData + 1, 1, 0) == NULL) {
+                return GDB_ReplyErrno(ctx, EILSEQ);
+            } else if (id >= MAX_CORE + 1) {
+                return GDB_ReplyErrno(ctx, EINVAL);
+            }
+            ctx->selectedThreadIdForContinuing = id == 0 ? (int)currentCoreCtx->coreId + 1 : (int)id;
+        }
+
+        return GDB_ReplyOk(ctx);
+    }
+    else
+        return GDB_ReplyErrno(ctx, EPERM);
+}
+
+GDB_DECLARE_HANDLER(IsThreadAlive)
+{
+    unsigned long threadId;
+
+    if (GDB_ParseHexIntegerList(&threadId, ctx->commandData, 1, 0) == NULL || threadId < 1) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    u32 coreMask = ctx->attachedCoreList;
+    return (coreMask & BIT(threadId - 1)) != 0 ? GDB_ReplyOk(ctx) : GDB_ReplyErrno(ctx, ESRCH);
+}
+
+GDB_DECLARE_QUERY_HANDLER(CurrentThreadId)
+{
+    return GDB_SendFormattedPacket(ctx, "QC%x", 1 + currentCoreCtx->coreId);
+}
+
+GDB_DECLARE_QUERY_HANDLER(fThreadInfo)
+{
+    // We have made our GDB packet big enough to list all the thread ids (coreIds + 1 for each coreId)
+    char *buf = ctx->buffer + 1;
+    size_t n = 0;
+
+    u32 coreMask = ctx->attachedCoreList;
+
+    FOREACH_BIT (tmp, coreId, coreMask) {
+        n += sprintf(buf + n, "%lx,", 1 + coreId);
+    }
+
+    // Remove trailing comma
+    buf[--n] = 0;
+
+    return GDB_SendStreamData(ctx, buf, 0, n, n, true);
+}
+
+GDB_DECLARE_QUERY_HANDLER(sThreadInfo)
+{
+    // We have made our GDB packet big enough to list all the thread ids (coreIds + 1 for each coreId) in fThreadInfo
+    // Note: we assume GDB doesn't accept notifications during the sequence transfer...
+    return GDB_SendPacket(ctx, "l", 1);
+}
+
+GDB_DECLARE_QUERY_HANDLER(ThreadEvents)
+{
+    switch (ctx->commandData[0]) {
+        case '0':
+            ctx->catchThreadEvents = false;
+            return GDB_ReplyOk(ctx);
+        case '1':
+            ctx->catchThreadEvents = true;
+            return GDB_ReplyOk(ctx);
+        default:
+            return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+}
+
+GDB_DECLARE_QUERY_HANDLER(ThreadExtraInfo)
+{
+    unsigned long id;
+    int n;
+
+    if(GDB_ParseHexIntegerList(&id, ctx->commandData, 1, 0) == NULL)
+        return GDB_ReplyErrno(ctx, EILSEQ);
+
+    n = sprintf(ctx->workBuffer, "TODO");
+
+    return GDB_SendHexPacket(ctx, ctx->workBuffer, n);
+}

thermosphere/src/gdb/thread.h

@@ -0,0 +1,20 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+GDB_DECLARE_HANDLER(SetThreadId);
+GDB_DECLARE_HANDLER(IsThreadAlive);
+
+GDB_DECLARE_QUERY_HANDLER(CurrentThreadId);
+GDB_DECLARE_QUERY_HANDLER(fThreadInfo);
+GDB_DECLARE_QUERY_HANDLER(sThreadInfo);
+GDB_DECLARE_QUERY_HANDLER(ThreadEvents);
+GDB_DECLARE_QUERY_HANDLER(ThreadExtraInfo);
+GDB_DECLARE_QUERY_HANDLER(GetTLSAddr);

thermosphere/src/gdb/verbose.c

@@ -0,0 +1,61 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+
+#include "verbose.h"
+#include "net.h"
+#include "debug.h"
+
+static const struct {
+    const char *name;
+    char trailingCharacter;
+    GDBCommandHandler handler;
+} gdbVerboseCommandHandlers[] = {
+    { "Cont?",          '\0',   GDB_VERBOSE_HANDLER(ContinueSupported) },
+    { "Cont",           ';',    GDB_VERBOSE_HANDLER(Continue) },
+    { "CtrlC",           '\0',  GDB_VERBOSE_HANDLER(CtrlC) },
+    { "MustReplyEmpty", '\0',   GDB_HANDLER(Unsupported) },
+    { "Stopped",        '\0',   GDB_VERBOSE_HANDLER(Stopped) },
+};
+
+GDB_DECLARE_HANDLER(VerboseCommand)
+{
+    char *nameBegin = ctx->commandData; // w/o leading 'v'
+    if (*nameBegin == 0) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    char *nameEnd;
+    char *vData = NULL;
+
+    for (nameEnd = nameBegin; *nameEnd != 0 && *nameEnd != ';' && *nameEnd != ':'; nameEnd++);
+    char oldNameEnd = *nameEnd;
+    if (*nameEnd != 0) {
+        *nameEnd = 0;
+        vData = nameEnd + 1;
+    }
+
+    for (size_t i = 0; i < sizeof(gdbVerboseCommandHandlers) / sizeof(gdbVerboseCommandHandlers[0]); i++) {
+        if (strcmp(gdbVerboseCommandHandlers[i].name, nameBegin) == 0) {
+            ctx->commandData = vData;
+            if (oldNameEnd == gdbVerboseCommandHandlers[i].trailingCharacter) {
+                return gdbVerboseCommandHandlers[i].handler(ctx);
+            } else {
+                return GDB_ReplyErrno(ctx, EILSEQ);
+            }
+        }
+    }
+
+    return GDB_HandleUnsupported(ctx); // No handler found!
+}
+
+GDB_DECLARE_VERBOSE_HANDLER(ContinueSupported)
+{
+    const char *supported = "vCont;c;C;s;S;t;r";
+    return GDB_SendPacket(ctx, supported, strlen(supported));
+}

thermosphere/src/gdb/verbose.h

@@ -0,0 +1,13 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+GDB_DECLARE_HANDLER(VerboseCommand);
+GDB_DECLARE_VERBOSE_HANDLER(ContinueSupported);

thermosphere/src/gdb/xfer.c

@@ -0,0 +1,162 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include <string.h>
+#include <stdio.h>
+
+#include "../utils.h"
+
+#include "xfer.h"
+#include "net.h"
+
+
+struct {
+    const char *name;
+    int (*handler)(GDBContext *ctx, bool write, const char *annex, size_t offset, size_t length);
+} xferCommandHandlers[] = {
+    { "features", GDB_XFER_HANDLER(Features) },
+};
+
+static void GDB_GenerateTargetXml(char *buf)
+{
+    int pos;
+    const char *hdr = "<?xml version=\"1.0\"?><!DOCTYPE feature SYSTEM \"gdb-target.dtd\"><target>";
+    const char *cpuDescBegin = "<feature name=\"org.gnu.gdb.aarch64.core\">";
+    const char *cpuDescEnd =
+    "<reg name=\"sp\" bitsize=\"64\" type=\"data_ptr\"/>"
+    "<reg name=\"pc\" bitsize=\"64\" type=\"code_ptr\"/>"
+    "<reg name=\"cpsr\" bitsize=\"32\"/></feature>";
+
+    const char *fpuDescBegin =
+    "<feature name=\"org.gnu.gdb.aarch64.fpu\"><vector id=\"v2d\" type=\"ieee_double\" count=\"2\"/>"
+    "<vector id=\"v2u\" type=\"uint64\" count=\"2\"/><vector id=\"v2i\" type=\"int64\" count=\"2\"/>"
+    "<vector id=\"v4f\" type=\"ieee_single\" count=\"4\"/><vector id=\"v4u\" type=\"uint32\" count=\"4\"/>"
+    "<vector id=\"v4i\" type=\"int32\" count=\"4\"/><vector id=\"v8u\" type=\"uint16\" count=\"8\"/>"
+    "<vector id=\"v8i\" type=\"int16\" count=\"8\"/><vector id=\"v16u\" type=\"uint8\" count=\"16\"/>"
+    "<vector id=\"v16i\" type=\"int8\" count=\"16\"/><vector id=\"v1u\" type=\"uint128\" count=\"1\"/>"
+    "<vector id=\"v1i\" type=\"int128\" count=\"1\"/><union id=\"vnd\"><field name=\"f\" type=\"v2d\"/>"
+    "<field name=\"u\" type=\"v2u\"/><field name=\"s\" type=\"v2i\"/></union><union id=\"vns\">"
+    "<field name=\"f\" type=\"v4f\"/><field name=\"u\" type=\"v4u\"/><field name=\"s\" type=\"v4i\"/></union>"
+    "<union id=\"vnh\"><field name=\"u\" type=\"v8u\"/><field name=\"s\" type=\"v8i\"/></union><union id=\"vnb\">"
+    "<field name=\"u\" type=\"v16u\"/><field name=\"s\" type=\"v16i\"/></union><union id=\"vnq\">"
+    "<field name=\"u\" type=\"v1u\"/><field name=\"s\" type=\"v1i\"/></union><union id=\"aarch64v\">"
+    "<field name=\"d\" type=\"vnd\"/><field name=\"s\" type=\"vns\"/><field name=\"h\" type=\"vnh\"/>"
+    "<field name=\"b\" type=\"vnb\"/><field name=\"q\" type=\"vnq\"/></union>";
+
+    const char *fpuDescEnd = "<reg name=\"fpsr\" bitsize=\"32\"/>\r\n<reg name=\"fpcr\" bitsize=\"32\"/>\r\n</feature>";
+    const char *footer = "</target>";
+
+    strcpy(buf, hdr);
+
+    // CPU registers
+    strcat(buf, cpuDescBegin);
+    pos = (int)strlen(buf);
+    for (u32 i = 0; i < 31; i++) {
+        pos += sprintf(buf + pos, "<reg name=\"x%u\" bitsize=\"64\"/>", i);
+    }
+    strcat(buf, cpuDescEnd);
+
+    strcat(buf, fpuDescBegin);
+    pos = (int)strlen(buf);
+    for (u32 i = 0; i < 32; i++) {
+        pos += sprintf(buf + pos, "<reg name=\"v%u\" bitsize=\"128\" type=\"aarch64v\"/>", i);
+    }
+    strcat(buf, fpuDescEnd);
+
+    strcat(buf, footer);
+
+    DEBUG("target.xml length is 0x%x\n", strlen(buf));
+}
+
+GDB_DECLARE_XFER_HANDLER(Features)
+{
+    if(strcmp(annex, "target.xml") != 0 || write) {
+        return GDB_ReplyEmpty(ctx);
+    }
+
+    // Generate the target xml on-demand
+    // This is a bit whack, we rightfully assume that GDB won't sent any other command during the stream transfer
+    if (ctx->targetXmlLen == 0) {
+        GDB_GenerateTargetXml(ctx->workBuffer);
+        ctx->targetXmlLen = strlen(ctx->workBuffer);
+    }
+
+    int n = GDB_SendStreamData(ctx, ctx->workBuffer, offset, length, ctx->targetXmlLen, false);
+
+    // Transfer ended
+    if(offset + length >= ctx->targetXmlLen) {
+        ctx->targetXmlLen = 0;
+    }
+
+    return n;
+}
+
+GDB_DECLARE_QUERY_HANDLER(Xfer)
+{
+    const char *objectStart = ctx->commandData;
+    char *objectEnd = (char*)strchr(objectStart, ':');
+    if (objectEnd == NULL) {
+        return -1;
+    }
+    *objectEnd = 0;
+
+    char *opStart = objectEnd + 1;
+    char *opEnd = (char*)strchr(opStart, ':');
+    if(opEnd == NULL) {
+        return -1;
+    }
+    *opEnd = 0;
+
+    char *annexStart = opEnd + 1;
+    char *annexEnd = (char*)strchr(annexStart, ':');
+    if(annexEnd == NULL) {
+        return -1;
+    }
+    *annexEnd = 0;
+
+    const char *offStart = annexEnd + 1;
+    size_t offset, length;
+
+    bool write;
+    const char *pos;
+    if (strcmp(opStart, "read") == 0) {
+        unsigned long lst[2];
+        if(GDB_ParseHexIntegerList(lst, offStart, 2, 0) == NULL) {
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+
+        offset = lst[0];
+        length = lst[1];
+        write = false;
+    } else if (strcmp(opStart, "write") == 0) {
+        pos = GDB_ParseHexIntegerList(&offset, offStart, 1, ':');
+        if (pos == NULL || *pos++ != ':') {
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+
+        size_t len = strlen(pos);
+        if (len == 0 || (len % 2) != 0) {
+            return GDB_ReplyErrno(ctx, EILSEQ);
+        }
+        length = len / 2;
+        write = true;
+    } else {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
+
+    for (size_t i = 0; i < sizeof(xferCommandHandlers) / sizeof(xferCommandHandlers[0]); i++) {
+        if (strcmp(objectStart, xferCommandHandlers[i].name) == 0) {
+            if(write) {
+                ctx->commandData = (char *)pos;
+            }
+
+            return xferCommandHandlers[i].handler(ctx, write, annexStart, offset, length);
+        }
+    }
+
+    return GDB_HandleUnsupported(ctx);
+}

thermosphere/src/gdb/xfer.h

@@ -0,0 +1,17 @@
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#pragma once
+
+#include "context.h"
+
+#define GDB_XFER_HANDLER(name)                  GDB_HANDLER(Xfer##name)
+#define GDB_DECLARE_XFER_HANDLER(name)          int GDB_XFER_HANDLER(name)(GDBContext *ctx, bool write, const char *annex, size_t offset, size_t length)
+
+GDB_DECLARE_XFER_HANDLER(Features);
+
+GDB_DECLARE_QUERY_HANDLER(Xfer);

thermosphere/src/gicv2.h

@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "types.h"
+
+#define GIC_IRQID_MAX                   1019
+#define GIC_IRQID_RESERVED_START        1020
+
+#define GIC_IRQID_SPURIOUS_GRPNEEDACK   (GIC_IRQID_RESERVED_START + 2)
+#define GIC_IRQID_SPURIOUS              (GIC_IRQID_RESERVED_START + 3)
+#define GICV_PRIO_LEVELS                32
+#define GICV_IDLE_PRIORITY              0xF8 // sometimes 0xFF
+
+typedef struct ArmGicV2Distributor {
+    u32 ctlr;
+    u32 typer;
+    u32 iidr;
+    u8 _0x0c[0x80 - 0x0C];
+    // Note: in reality only 512 interrupts max. are defined (nor "reserved") on Gicv2
+    u32 igroupr[1024 / 32];
+    u32 isenabler[1024 / 32];
+    u32 icenabler[1024 / 32];
+    u32 ispendr[1024 / 32];
+    u32 icpendr[1024 / 32];
+    u32 isactiver[1024 / 32];
+    u32 icactiver[1024 / 32];
+    u8 ipriorityr[1024]; // can be accessed as u8 or u32
+    u8 itargetsr[1024]; // can be accessed as u8 or u32
+    u32 icfgr[1024 / 16];
+    u8 impldef_d00[0xF00 - 0xD00];
+    u32 sgir;
+    u8 _0xf04[0xF10 - 0xF04];
+    u8 cpendsgir[16];
+    u8 spendsgir[16];
+    u8 _0xf30[0xFE8 - 0xF30];
+    u32 icpidr2;
+    u8 _0xfec[0x1000 - 0xFEC];
+} ArmGicV2Distributor;
+
+typedef struct ArmGicV2Controller {
+    u32 ctlr;
+    u32 pmr;
+    u32 bpr;
+    u32 iar;
+    u32 eoir;
+    u32 rpr;
+    u32 hppir;
+    u32 abpr;
+    u32 aiar;
+    u32 aeoir;
+    u32 ahppir;
+    u8 _0x2c[0x40 - 0x2C];
+    u8 impldef_40[0xD0 - 0x40];
+    u32 apr[4];
+    u32 nsapr[4];
+    u8 _0xf0[0xFC - 0xF0];
+    u32 iidr;
+    u8 _0x100[0x1000 - 0x100];
+    u32 dir;
+    u8 _0x1004[0x2000 - 0x1004];
+} ArmGicV2Controller;
+
+typedef struct ArmGicV2HypervisorControlRegister {
+    u32 en         : 1;
+    u32 uie        : 1;
+    u32 lrenpie    : 1;
+    u32 npie       : 1;
+    u32 vgrp0eie   : 1;
+    u32 vgrp0die   : 1;
+    u32 vgrp1eie   : 1;
+    u32 vgrp1die   : 1;
+    u32 _8         : 19;
+    u32 eoiCount   : 5;
+} ArmGicV2HypervisorControlRegister;
+
+typedef struct ArmGicV2MaintenanceIntStatRegister {
+    u32 eoi     : 1;
+    u32 u       : 1;
+    u32 lrenp   : 1;
+    u32 np      : 1;
+    u32 vgrp0e  : 1;
+    u32 vgrp0d  : 1;
+    u32 vgrp1e  : 1;
+    u32 vgrp1d  : 1;
+    u32 _8      : 24;
+} ArmGicV2MaintenanceIntStatRegister;
+
+typedef struct ArmGicV2ListRegister {
+    u32 virtualId       : 10;
+    u32 physicalId      : 10; // note: different encoding if hw = 0 (can't represent it in struct)
+    u32 sbz2            : 3;
+    u32 priority        : 5;
+    u32 pending         : 1;
+    u32 active          : 1;
+    u32 grp1            : 1;
+    u32 hw              : 1;
+} ArmGicV2ListRegister;
+
+typedef struct ArmGicV2VmControlRegister {
+    u32 enableGrp0      : 1;
+    u32 enableGrp1      : 1;
+    u32 ackCtl          : 1;
+    u32 fiqEn           : 1;
+    u32 cbpr            : 1;
+    u32 _5              : 4;
+    u32 eoiMode         : 1;
+    u32 _10             : 8;
+    u32 abpr            : 3;
+    u32 bpr             : 3;
+    u32 _24             : 3;
+    u32 pmr             : 5;
+} ArmGicV2VmControlRegister;
+
+typedef struct ArmGicV2VirtualInterfaceController {
+    ArmGicV2HypervisorControlRegister hcr;
+    u32 vtr;
+    ArmGicV2VmControlRegister vmcr;
+    u8 _0x0c[0x10 - 0xC];
+    ArmGicV2MaintenanceIntStatRegister misr;
+    u8 _0x14[0x20 - 0x14];
+    u32 eisr0;
+    u32 eisr1;
+    u8 _0x28[0x30 - 0x28];
+    u32 elsr0;
+    u32 elsr1;
+    u8 _0x38[0xF0 - 0x38];
+    u32 apr;
+    u8 _0xf4[0x100 - 0xF4];
+    ArmGicV2ListRegister lr[64];
+} ArmGicV2VirtualInterfaceController;

thermosphere/src/guest_memory.c

@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+
+#include "guest_memory.h"
+#include "memory_map.h"
+#include "mmu.h"
+#include "spinlock.h"
+#include "core_ctx.h"
+#include "sysreg.h"
+#include "vgic.h"
+#include "irq.h"
+#include "caches.h"
+
+static size_t guestReadWriteGicd(size_t offset, size_t size, void *readBuf, const void *writeBuf)
+{
+    recursiveSpinlockLock(&g_irqManager.lock);
+
+    if (readBuf != NULL) {
+        size_t readOffset = 0;
+        size_t rem = size;
+        while (rem > 0) {
+            if ((offset + readOffset) % 4 == 0 && rem >= 4) {
+                // All accesses of this kind are valid
+                *(u32 *)((uintptr_t)readBuf + readOffset) = vgicReadGicdRegister(offset + readOffset, 4);
+                readOffset += 4;
+                rem -= 4;
+            } else if ((offset + readOffset) % 2 == 0 && rem >= 2) {
+                // All accesses of this kind would be translated to ldrh and are thus invalid. Abort.
+                size = readOffset;
+                goto end;
+            } else if (vgicValidateGicdRegisterAccess(offset + readOffset, 1)) {
+                // Valid byte access
+                *(u8 *)((uintptr_t)readBuf + readOffset) = vgicReadGicdRegister(offset + readOffset, 1);
+                readOffset += 1;
+                rem -= 1;
+            } else {
+                // Invalid byte access
+                size = readOffset;
+                goto end;
+            }
+        }
+    }
+
+    if (writeBuf != NULL) {
+        size_t writeOffset = 0;
+        size_t rem = size;
+        while (rem > 0) {
+            if ((offset + writeOffset) % 4 == 0 && rem >= 4) {
+                // All accesses of this kind are valid
+                vgicWriteGicdRegister(*(u32 *)((uintptr_t)writeBuf + writeOffset), offset + writeOffset, 4);
+                writeOffset += 4;
+                rem -= 4;
+            } else if ((offset + writeOffset) % 2 == 0 && rem >= 2) {
+                // All accesses of this kind would be translated to ldrh and are thus invalid. Abort.
+                size = writeOffset;
+                goto end;
+            } else if (vgicValidateGicdRegisterAccess(offset + writeOffset, 1)) {
+                // Valid byte access
+                vgicWriteGicdRegister(*(u32 *)((uintptr_t)writeBuf + writeOffset), offset + writeOffset, 1);
+                writeOffset += 1;
+                rem -= 1;
+            } else {
+                // Invalid byte access
+                size = writeOffset;
+                goto end;
+            }
+        }
+    }
+
+end:
+    recursiveSpinlockUnlock(&g_irqManager.lock);
+    return size;
+}
+
+static size_t guestReadWriteDeviceMemory(void *addr, size_t size, void *readBuf, const void *writeBuf)
+{
+    // We might trigger bus errors... ignore the exception and return early if that's the case
+
+    CoreCtx *curCtxBackup = currentCoreCtx;
+    __compiler_barrier();
+    currentCoreCtx = NULL;
+    __compiler_barrier();
+
+    uintptr_t addri = (uintptr_t)addr;
+
+    if (readBuf != NULL) {
+        size_t readOffset = 0;
+        size_t rem = size;
+        while (rem > 0 && (__compiler_barrier(), currentCoreCtx == NULL)) {
+            if ((addri + readOffset) % 4 == 0 && rem >= 4) {
+                *(vu32 *)((uintptr_t)readBuf + readOffset) = *(vu32 *)(addri + readOffset);
+                readOffset += 4;
+                rem -= 4;
+            } else if (readOffset % 2 == 0 && rem >= 2) {
+                *(vu16 *)((uintptr_t)readBuf + readOffset) = *(vu16 *)(addri + readOffset);
+                readOffset += 2;
+                rem -= 2;
+            } else {
+                *(vu8 *)((uintptr_t)readBuf + readOffset) = *(vu8 *)(addri + readOffset);
+                readOffset += 1;
+                rem -= 1;
+            }
+        }
+        if (rem != 0) {
+            size = readOffset;
+            goto end;
+        }
+    }
+
+    if (writeBuf != NULL) {
+        size_t writeOffset = 0;
+        size_t rem = size;
+        while (rem > 0 && (__compiler_barrier(), currentCoreCtx == NULL)) {
+            if ((addri + writeOffset) % 4 == 0 && rem >= 4) {
+                *(vu32 *)(addri + writeOffset) = *(vu32 *)((uintptr_t)writeBuf + writeOffset);
+                writeOffset += 4;
+                rem -= 4;
+            } else if (writeOffset % 2 == 0 && rem >= 2) {
+                *(vu16 *)(addri + writeOffset) = *(vu16 *)((uintptr_t)writeBuf + writeOffset);
+                writeOffset += 2;
+                rem -= 2;
+            } else {
+                *(vu8 *)(addri + writeOffset) = *(vu8 *)((uintptr_t)writeBuf + writeOffset);
+                writeOffset += 1;
+                rem -= 1;
+            }
+        }
+        if (rem != 0) {
+            size = writeOffset;
+            goto end;
+        }
+    }
+
+end:
+    __compiler_barrier();
+    currentCoreCtx = curCtxBackup;
+    __compiler_barrier();
+    return size;
+}
+
+static size_t guestReadWriteNormalMemory(void *addr, size_t size, void *readBuf, const void *writeBuf)
+{
+    if (readBuf != NULL) {
+        memcpy(readBuf, addr, size);
+    }
+
+    if (writeBuf != NULL) {
+        memcpy(addr, writeBuf, size);
+
+        // We may have written to executable memory or to translation tables...
+        // & the page may have various aliases.
+        // We need to ensure cache & TLB coherency.
+        cacheCleanDataCacheRangePoU(addr, size);
+        u32 policy = cacheGetInstructionCachePolicy();
+        if (policy == 1 || policy == 2) {
+            // AVIVT, VIVT
+            cacheInvalidateInstructionCache();
+        } else {
+            // VPIPT, PIPT
+            // Ez coherency, just do range operations...
+            cacheInvalidateInstructionCacheRangePoU(addr, size);
+        }
+        __tlb_invalidate_el1();
+        __dsb();
+        __isb();
+    }
+
+    return size;
+}
+
+static size_t guestReadWriteMemoryPage(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf)
+{
+    u64 irqFlags = maskIrq();
+    size_t offset = addr & 0xFFFull;
+
+    // Translate the VA, stages 1&2
+    __asm__ __volatile__ ("at s12e1r, %0" :: "r"(addr) : "memory");
+    u64 par = GET_SYSREG(par_el1);
+    if (par & PAR_F) {
+        // The translation failed. Why?
+        if (par & PAR_S) {
+            // Stage 2 fault. Could be an attempt to access the GICD, let's see what the IPA is...
+            __asm__ __volatile__ ("at s1e1r, %0" :: "r"(addr) : "memory");
+            par = GET_SYSREG(par_el1);
+            if ((par & PAR_F) != 0 || (par & PAR_PA_MASK) != MEMORY_MAP_VA_GICD) {
+                // The guest doesn't have access to it...
+                // Read as 0, write ignored
+                if (readBuf != NULL) {
+                    memset(readBuf, 0, size);
+                }
+            } else {
+                // GICD mmio
+                size = guestReadWriteGicd(offset, size, readBuf, writeBuf);
+            }
+        } else {
+            // Oops, couldn't read/write anything (stage 1 fault)
+            size = 0;
+        }
+    } else {
+        /*
+            Translation didn't fail.
+
+            To avoid "B2.8 Mismatched memory attributes" we must use the same effective
+            attributes & shareability as the guest.
+
+            Note that par_el1 reports the effective shareablity of device and noncacheable memory as inner shareable.
+            In fact, the VMSAv8-64 section in the Armv8 ARM reads:
+                "The shareability field is only relevant if the memory is a Normal Cacheable memory type. All Device and Normal
+                Non-cacheable memory regions are always treated as Outer Shareable, regardless of the translation table
+                shareability attributes."
+
+            There's one corner case where we can't avoid it: another core is running,
+            changes the attributes (other than permissions) of the page, and issues
+            a broadcasting TLB maintenance instructions and/or accesses the page with the altered
+            attribute itself. We don't handle this corner case -- just don't read/write that kind of memory...
+        */
+        u64 memAttribs = (par >> PAR_ATTR_SHIFT) & PAR_ATTR_MASK;
+        u32 shrb = (par >> PAR_SH_SHIFT) & PAR_SH_MASK;
+        uintptr_t pa = par & PAR_PA_MASK;
+        uintptr_t va = MEMORY_MAP_VA_GUEST_MEM + 0x2000 * currentCoreCtx->coreId;
+
+        u64 mair = GET_SYSREG(mair_el2);
+        mair |= memAttribs << (8 * MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT);
+        SET_SYSREG(mair_el2, mair);
+        __isb();
+
+        u64 attribs = MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_SH(shrb) | MMU_PTE_BLOCK_MEMTYPE(MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT);
+        mmu_map_page((uintptr_t *)MEMORY_MAP_VA_TTBL, va, pa, attribs);
+        // Note: no need to broadcast here
+        __tlb_invalidate_el2_page_local(pa);
+        __dsb_local();
+
+        void *vaddr = (void *)(va + offset);
+        if (memAttribs & 0xF0) {
+            // Normal memory, or unpredictable
+            size = guestReadWriteNormalMemory(vaddr, size, readBuf, writeBuf);
+        } else {
+            // Device memory, or unpredictable
+            size = guestReadWriteDeviceMemory(vaddr, size, readBuf, writeBuf);
+        }
+
+        __dsb_local();
+        __isb();
+        mmu_unmap_page((uintptr_t *)MEMORY_MAP_VA_TTBL, va);
+        // Note: no need to broadcast here
+        __tlb_invalidate_el2_page_local(pa);
+        __dsb_local();
+
+        mair &= ~(0xFFul << (8 * MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT));
+        SET_SYSREG(mair_el2, mair);
+        __isb();
+    }
+
+    restoreInterruptFlags(irqFlags);
+    return size;
+}
+
+size_t guestReadWriteMemory(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf)
+{
+    uintptr_t curAddr = addr;
+    size_t remainingAmount = size;
+    u8 *rb8 = (u8 *)readBuf;
+    const u8 *wb8 = (const u8*)writeBuf;
+    while (remainingAmount > 0) {
+        size_t expectedAmount = ((curAddr & ~0xFFFul) + 0x1000) - curAddr;
+        expectedAmount = expectedAmount > remainingAmount ? remainingAmount : expectedAmount;
+        size_t actualAmount = guestReadWriteMemoryPage(curAddr, expectedAmount, rb8, wb8);
+        curAddr += actualAmount;
+        rb8 += actualAmount;
+        wb8 += actualAmount;
+        remainingAmount -= actualAmount;
+        if (actualAmount != expectedAmount) {
+            break;
+        }
+    }
+    return curAddr - addr;
+}

thermosphere/src/guest_memory.h

@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+
+size_t guestReadWriteMemory(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf);
+
+static inline size_t guestReadMemory(uintptr_t addr, size_t size, void *buf)
+{
+    return guestReadWriteMemory(addr, size, buf, NULL);
+}
+
+static inline size_t guestWriteMemory(uintptr_t addr, size_t size, const void *buf)
+{
+    return guestReadWriteMemory(addr, size, NULL, buf);
+}

thermosphere/src/guest_timers.h

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "exceptions.h"
+#include "sysreg.h"
+
+static inline u64 computeCntvct(const ExceptionStackFrame *frame)
+{
+    return frame->cntpct_el0 - currentCoreCtx->totalTimeInHypervisor;
+}
+
+static inline void writeEmulatedPhysicalCompareValue(ExceptionStackFrame *frame, u64 val)
+{
+    // We lied about the value of cntpct, so we need to compute the time delta
+    // the guest actually intended to use...
+    u64 vct = computeCntvct(frame);
+    currentCoreCtx->emulPtimerCval = val;
+    SET_SYSREG(cntp_cval_el0, frame->cntpct_el0 + (val - vct));
+}

thermosphere/src/hvc.c

@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "hvc.h"
+#include "debug_log.h"
+
+void handleHypercall(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
+{
+    u32 id = esr.iss;
+    switch (id) {
+        default:
+            DEBUG("Unhandled hypercall: 0x%x.\n");
+            dumpStackFrame(frame, false);
+            break;
+    }
+}

thermosphere/src/hvc.h

@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+#include "exceptions.h"
+
+void handleHypercall(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);

thermosphere/src/initSystem.c

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+#include "core_ctx.h"
+#include "platform/stage2.h"
+#include "platform/devices.h"
+#include "sysreg.h"
+#include "utils.h"
+
+// BSS includes real bss and tmp bss
+extern u8 __bss_start__[], __real_bss_end__[], __bss_end__[];
+
+static void initSysregs(void)
+{
+    // Set system to sane defaults, aarch64 for el1, mmu&caches initially disabled for EL1, etc.
+    SET_SYSREG(hcr_el2,         0x80000000);
+    SET_SYSREG(dacr32_el2,      0xFFFFFFFF);    // unused
+    SET_SYSREG(sctlr_el1,       0x00C50838);
+
+    SET_SYSREG(mdcr_el2,        0x00000000);
+    SET_SYSREG(mdscr_el1,       0x00000000);
+
+    // Timer stuff
+    SET_SYSREG(cntvoff_el2,     0x00000000);
+    SET_SYSREG(cnthctl_el2,     0x00000003);    // Don't trap anything for now; event streams disabled
+    SET_SYSREG(cntkctl_el1,     0x00000003);    // Don't trap anything for now; event streams disabled
+    SET_SYSREG(cntp_ctl_el0,    0x00000000);
+    SET_SYSREG(cntv_ctl_el0,    0x00000000);
+
+    __dsb_local();
+    __isb();
+}
+
+void initSystem(u32 coreId, bool isBootCore, u64 argument)
+{
+    coreCtxInit(coreId, isBootCore, argument);
+    initSysregs();
+
+    if (isBootCore) {
+        if (!currentCoreCtx->warmboot) {
+            memset(__bss_start__, 0, __real_bss_end__ - __bss_start__);
+        }
+
+        memset(__real_bss_end__, 0, __bss_end__ - __real_bss_end__);
+    }
+
+    stage2ConfigureAndEnable();
+    if (isBootCore) {
+        devicesMapAllExtra();
+    }
+}

thermosphere/src/irq.c

@@ -0,0 +1,291 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "irq.h"
+#include "core_ctx.h"
+#include "debug_log.h"
+#include "vgic.h"
+#include "timer.h"
+#include "guest_timers.h"
+#include "transport_interface.h"
+#include "debug_manager.h"
+
+IrqManager g_irqManager = {0};
+
+static void initGic(void)
+{
+    // Reinits the GICD and GICC (for non-secure mode, obviously)
+    if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) {
+        // Disable interrupt handling & global interrupt distribution
+        gicd->ctlr = 0;
+
+        // Get some info
+        g_irqManager.numSharedInterrupts = 32 * (gicd->typer & 0x1F); // number of interrupt lines / 32
+
+        // unimplemented priority bits (lowest significant) are RAZ/WI
+        gicd->ipriorityr[0] = 0xFF;
+        g_irqManager.priorityShift = 8 - __builtin_popcount(gicd->ipriorityr[0]);
+        g_irqManager.numPriorityLevels = (u8)BIT(__builtin_popcount(gicd->ipriorityr[0]));
+
+        g_irqManager.numCpuInterfaces = (u8)(1 + ((gicd->typer >> 5) & 7));
+        g_irqManager.numListRegisters = (u8)(1 + (gich->vtr & 0x3F));
+    }
+
+    // Only one core will reset the GIC state for the shared peripheral interrupts
+
+    u32 numInterrupts = 32;
+    if (currentCoreCtx->isBootCore) {
+        numInterrupts += g_irqManager.numSharedInterrupts;
+    }
+
+    // Filter all interrupts
+    gicc->pmr = 0;
+
+    // Disable interrupt preemption
+    gicc->bpr = 7;
+
+    // Note: the GICD I...n regs are banked for private interrupts
+
+    // Disable all interrupts, clear active status, clear pending status
+    for (u32 i = 0; i < numInterrupts / 32; i++) {
+        gicd->icenabler[i] = 0xFFFFFFFF;
+        gicd->icactiver[i] = 0xFFFFFFFF;
+        gicd->icpendr[i] = 0xFFFFFFFF;
+    }
+
+    // Set priorities to lowest
+    for (u32 i = 0; i < numInterrupts; i++) {
+        gicd->ipriorityr[i] = 0xFF;
+    }
+
+    // Reset icfgr, itargetsr for shared peripheral interrupts
+    for (u32 i = 32 / 16; i < numInterrupts / 16; i++) {
+        gicd->icfgr[i] = 0x55555555;
+    }
+
+    for (u32 i = 32; i < numInterrupts; i++) {
+        gicd->itargetsr[i] = 0;
+    }
+
+    // Now, reenable interrupts
+
+    // Enable the distributor
+    if (currentCoreCtx->isBootCore) {
+        gicd->ctlr = 1;
+    }
+
+    // Enable the CPU interface. Set EOIModeNS=1 (split prio drop & deactivate priority)
+    gicc->ctlr = BIT(9) | 1;
+
+    // Disable interrupt filtering
+    gicc->pmr = 0xFF;
+
+    currentCoreCtx->gicInterfaceMask = gicd->itargetsr[0];
+}
+
+static inline bool checkRescheduleEmulatedPtimer(ExceptionStackFrame *frame)
+{
+    // Evaluate if the timer has really expired in the PoV of the guest kernel.
+    // If not, reschedule (add missed time delta) it & exit early
+    u64 cval = currentCoreCtx->emulPtimerCval;
+    u64 vct  = computeCntvct(frame);
+
+    if (cval > vct) {
+        // It has not: reschedule the timer
+        // Note: this isn't 100% precise esp. on QEMU so it may take a few tries...
+        writeEmulatedPhysicalCompareValue(frame, cval);
+        return false;
+    }
+
+    return true;
+}
+
+
+static inline bool checkGuestTimerInterrupts(ExceptionStackFrame *frame, u16 irqId)
+{
+    // A thing that might have happened is losing the race vs disabling the guest interrupts
+    // Another thing is that the virtual timer might have fired before us updating voff when executing a top half?
+    if (irqId == TIMER_IRQID(NS_VIRT_TIMER)) {
+        u64 cval = GET_SYSREG(cntp_cval_el0);
+        return cval <= computeCntvct(frame);
+    } else if (irqId == TIMER_IRQID(NS_PHYS_TIMER)) {
+        return checkRescheduleEmulatedPtimer(frame);
+    } else {
+        return true;
+    }
+}
+
+static void doConfigureInterrupt(u16 id, u8 prio, bool isLevelSensitive)
+{
+    gicd->icenabler[id / 32] = BIT(id % 32);
+
+    if (id >= 32) {
+        u32 cfgr = gicd->icfgr[id / 16];
+        cfgr &= ~(3 << IRQ_CFGR_SHIFT(id));
+        cfgr |= (!isLevelSensitive ? 3 : 1) << IRQ_CFGR_SHIFT(id);
+        gicd->icfgr[id / 16] = cfgr;
+        gicd->itargetsr[id]  = 0xFF; // all cpu interfaces
+    }
+    gicd->icpendr[id / 32]      = BIT(id % 32);
+    gicd->ipriorityr[id]        = (prio << g_irqManager.priorityShift) & 0xFF;
+    gicd->isenabler[id / 32]    = BIT(id % 32);
+}
+
+void initIrq(void)
+{
+    u64 flags = recursiveSpinlockLockMaskIrq(&g_irqManager.lock);
+
+    initGic();
+    vgicInit();
+
+    // Configure the interrupts we use here
+    for (u32 i = 0; i < ThermosphereSgi_Max; i++) {
+        doConfigureInterrupt(i, IRQ_PRIORITY_HOST, false);
+    }
+
+    doConfigureInterrupt(GIC_IRQID_MAINTENANCE, IRQ_PRIORITY_HOST, true);
+
+    recursiveSpinlockUnlockRestoreIrq(&g_irqManager.lock, flags);
+}
+
+void configureInterrupt(u16 id, u8 prio, bool isLevelSensitive)
+{
+    u64 flags = recursiveSpinlockLockMaskIrq(&g_irqManager.lock);
+    doConfigureInterrupt(id, prio, isLevelSensitive);
+    recursiveSpinlockUnlockRestoreIrq(&g_irqManager.lock, flags);
+}
+
+void irqSetAffinity(u16 id, u8 affinity)
+{
+    u64 flags = recursiveSpinlockLockMaskIrq(&g_irqManager.lock);
+    gicd->itargetsr[id] = affinity;
+    recursiveSpinlockUnlockRestoreIrq(&g_irqManager.lock, flags);
+}
+
+bool irqIsGuest(u16 id)
+{
+    if (id >= 32 + g_irqManager.numSharedInterrupts) {
+        DEBUG("vgic: %u not supported by physical distributor\n", (u32)id);
+        return false;
+    }
+
+    bool ret = true;
+    ret = ret && id != GIC_IRQID_MAINTENANCE;
+    ret = ret && id != GIC_IRQID_NS_PHYS_HYP_TIMER;
+
+    // If the following interrupts don't exist, that's fine, they're defined as GIC_IRQID_SPURIOUS in that case
+    // (for which the function isn't called, anyway)
+    ret = ret && id != GIC_IRQID_NS_VIRT_HYP_TIMER;
+    ret = ret && id != GIC_IRQID_SEC_PHYS_HYP_TIMER;
+    ret = ret && id != GIC_IRQID_SEC_VIRT_HYP_TIMER;
+
+    ret = ret && transportInterfaceFindByIrqId(id) == NULL;
+    return ret;
+}
+
+void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
+{
+    (void)isLowerEl;
+    (void)isA32;
+
+    // Acknowledge the interrupt. Interrupt goes from pending to active.
+    u32 iar = gicc->iar;
+    u32 irqId = iar & 0x3FF;
+    u32 srcCore = (iar >> 10) & 7;
+
+    //DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId);
+
+    if (irqId == GIC_IRQID_SPURIOUS) {
+        // Spurious interrupt received
+        return;
+    } else if (!checkGuestTimerInterrupts(frame, irqId)) {
+        // Deactivate the interrupt, return early
+        gicc->eoir = iar;
+        gicc->dir  = iar;
+        return;
+    }
+
+    bool isGuestInterrupt = false;
+    bool isMaintenanceInterrupt = false;
+    bool isPaused = false;
+    bool hasDebugEvent = false;
+
+    switch (irqId) {
+        case ThermosphereSgi_ExecuteFunction:
+            executeFunctionInterruptHandler(srcCore);
+            break;
+        case ThermosphereSgi_VgicUpdate:
+            // Nothing in particular to do here
+            break;
+        case ThermosphereSgi_DebugPause:
+            debugManagerPauseSgiHandler();
+            break;
+        case ThermosphereSgi_ReportDebuggerBreak:
+        case ThermosphereSgi_DebuggerContinue:
+            // See bottom halves
+            // Because exceptions (other debug events) are handling w/ interrupts off, if
+            // we get there, there's no race condition possible with debugManagerReportEvent
+            break;
+        case GIC_IRQID_MAINTENANCE:
+            isMaintenanceInterrupt = true;
+            break;
+        case TIMER_IRQID(CURRENT_TIMER):
+            timerInterruptHandler();
+            break;
+        default:
+            isGuestInterrupt = irqId >= 16;
+            break;
+    }
+
+    TransportInterface *transportIface = irqId >= 32 ? transportInterfaceIrqHandlerTopHalf(irqId) : NULL;
+
+    // Priority drop
+    gicc->eoir = iar;
+
+    isGuestInterrupt = isGuestInterrupt && transportIface == NULL && irqIsGuest(irqId);
+
+    recursiveSpinlockLock(&g_irqManager.lock);
+
+    if (!isGuestInterrupt) {
+        if (isMaintenanceInterrupt) {
+            vgicMaintenanceInterruptHandler();
+        }
+        // Deactivate the interrupt
+        gicc->dir = iar;
+    } else {
+        vgicEnqueuePhysicalIrq(irqId);
+    }
+
+    // Update vgic state
+    vgicUpdateState();
+
+    recursiveSpinlockUnlock(&g_irqManager.lock);
+
+    isPaused = debugManagerIsCorePaused(currentCoreCtx->coreId);
+    hasDebugEvent = debugManagerHasDebugEvent(currentCoreCtx->coreId);
+    if (irqId == ThermosphereSgi_ReportDebuggerBreak) DEBUG("debug event=%d\n", (int)debugManagerGetDebugEvent(currentCoreCtx->coreId)->type);
+    // Bottom half part
+    if (transportIface != NULL) {
+        exceptionEnterInterruptibleHypervisorCode();
+        unmaskIrq();
+        transportInterfaceIrqHandlerBottomHalf(transportIface);
+    } else if (irqId == ThermosphereSgi_ReportDebuggerBreak && !hasDebugEvent) {
+        debugManagerReportEvent(DBGEVENT_DEBUGGER_BREAK);
+    } else if (irqId == ThermosphereSgi_DebuggerContinue && isPaused) {
+        debugManagerUnpauseCores(BIT(currentCoreCtx->coreId));
+    }
+
+}

thermosphere/src/irq.h

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "gicv2.h"
+#include "spinlock.h"
+#include "exceptions.h"
+#include "utils.h"
+#include "platform/interrupt_config.h"
+#include "memory_map.h"
+
+#define IRQ_PRIORITY_HOST       0
+#define IRQ_PRIORITY_GUEST      1
+
+#define IRQ_CFGR_SHIFT(id)      (2*((id) % 16))
+
+typedef struct IrqManager {
+    RecursiveSpinlock lock;
+    u16 numSharedInterrupts;
+    u8 priorityShift;
+    u8 numPriorityLevels;
+    u8 numCpuInterfaces;
+    u8 numListRegisters;
+} IrqManager;
+
+typedef enum ThermosphereSgi {
+    ThermosphereSgi_ExecuteFunction = 0,
+    ThermosphereSgi_VgicUpdate,
+    ThermosphereSgi_DebugPause,
+    ThermosphereSgi_ReportDebuggerBreak,
+    ThermosphereSgi_DebuggerContinue,
+
+    ThermosphereSgi_Max,
+} ThermosphereSgi;
+
+static volatile ArmGicV2Distributor *const gicd = (volatile ArmGicV2Distributor *)MEMORY_MAP_VA_GICD;
+static volatile ArmGicV2Controller *const gicc = (volatile ArmGicV2Controller *)MEMORY_MAP_VA_GICC;
+static volatile ArmGicV2VirtualInterfaceController *const gich = (volatile ArmGicV2VirtualInterfaceController *)MEMORY_MAP_VA_GICH;
+
+extern IrqManager g_irqManager;
+
+void initIrq(void);
+void configureInterrupt(u16 id, u8 prio, bool isLevelSensitive);
+bool irqIsGuest(u16 id);
+void irqSetAffinity(u16 id, u8 affinityMask);
+
+static inline void generateSgiForAllOthers(ThermosphereSgi id)
+{
+    gicd->sgir = (1 << 24) | ((u32)id & 0xF);
+}
+
+static inline void generateSgiForSelf(ThermosphereSgi id)
+{
+    gicd->sgir = (2 << 24) | ((u32)id & 0xF);
+}
+
+static inline void generateSgiForList(ThermosphereSgi id, u32 list)
+{
+    gicd->sgir = (0 << 24) | (list << 16) | ((u32)id & 0xF);
+}
+
+static inline void generateSgiForAll(ThermosphereSgi id)
+{
+    generateSgiForList(id, MASK(g_irqManager.numCpuInterfaces));
+}

thermosphere/src/lib/ini.c

@@ -1,269 +0,0 @@
-/* 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 <stdio.h>
-#include <ctype.h>
-#include <string.h>
-
-#include "ini.h"
-
-#if !INI_USE_STACK
-#include <stdlib.h>
-#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 - 1);
-    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);
-}

thermosphere/src/lib/ini.h

@@ -1,130 +0,0 @@
-/* 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 <stdio.h>
-
-/* 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__ */

thermosphere/src/lib/vsprintf.h

@@ -1,25 +0,0 @@
-/*
- *  Copyright (C) 2011 Andrei Warkentin <andrey.warkentin@gmail.com>
- *
- * 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 <stdarg.h>
-#include <stdlib.h>
-
-#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 */

thermosphere/src/main.c

@@ -1,145 +1,143 @@
-/*
- * Copyright (c) 2018-2020 Atmosphère-NX
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <stdint.h>
-#include <stddef.h>
 #include <string.h>
 
-#include "regs.h"
-#include "lib/printk.h"
+#include "utils.h"
+#include "core_ctx.h"
+#include "debug_log.h"
+#include "platform/uart.h"
+#include "semihosting.h"
+#include "traps.h"
+#include "sysreg.h"
+#include "exceptions.h"
+#include "single_step.h"
+#include "breakpoints.h"
+#include "watchpoints.h"
+#include "timer.h"
+#include "irq.h"
+#include "transport_interface.h"
+#include "guest_memory.h"
+#include "fpu.h"
 
-/**
- * Switches to EL1, and then calls main_el1.
- * Implemented in assembly in entry.S.
- */
-void switch_to_el1();
+#include "memory_map.h"
+#include "mmu.h"
 
-/**
- * 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)
+static void loadKernelViaSemihosting(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;
+    // Note: !! hardcoded addresses !!
+    size_t len = 1<<20; // max len
+    uintptr_t buf = 0x60000000 + (1<<20);
 
-    memset(&lds_bss_start, 0, &lds_bss_end - &lds_bss_start);
+    long handle = -1, ret;
+
+    u64 *mmuTable = (u64 *)MEMORY_MAP_VA_TTBL;
+    mmu_map_block_range(
+        1, mmuTable, 0x40000000, 0x40000000, 0x40000000,
+        MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_INNER_SHAREBLE | MMU_PTE_BLOCK_MEMTYPE(MEMORY_MAP_MEMTYPE_NORMAL_UNCACHEABLE)
+    );
+
+    __tlb_invalidate_el2_local();
+    __dsb_local();
+
+    DEBUG("Loading kernel via semihosted file I/O... ");
+    handle = semihosting_file_open("test_kernel.bin", FOPEN_MODE_RB);
+    ENSURE2(handle >= 0, "failed to open file (%ld)!\n", handle);
+
+    ret = semihosting_file_read(handle, &len, buf);
+    ENSURE2(ret >= 0, "failed to read file (%ld)!\n", ret);
+
+    DEBUG("OK!\n");
+    semihosting_file_close(handle);
+
+    mmu_unmap_range(1, mmuTable, 0x40000000, 0x40000000);
+    __tlb_invalidate_el2_local();
+    __dsb_local();
+
+    currentCoreCtx->kernelEntrypoint = buf;
 }
 
 
-/**
- * Triggered on an unrecoverable condition; prints an error message
- * and terminates execution.
- */
-void panic(const char * message)
+
+#include "platform/uart.h"
+#include "debug_manager.h"
+typedef struct TestCtx {
+    char buf[512+1];
+} TestCtx;
+
+static TestCtx g_testCtx;
+
+size_t testReceiveCallback(TransportInterface *iface, void *p)
 {
-    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);
+    TestCtx *ctx = (TestCtx *)p;
+    debugManagerPauseCores(BIT(0));
+    return transportInterfaceReadDataUntil(iface, ctx->buf, 512, '\r');
 }
 
-
-
-/**
- * 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)
+void testProcessDataCallback(TransportInterface *iface, void *p, size_t sz)
 {
-    // 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();
+    (void)iface;
+    (void)sz;
+    debugManagerUnpauseCores(BIT(0));
+    TestCtx *ctx = (TestCtx *)p;
+    (void)ctx;
+    DEBUG("EL2 [core %u]: you typed: %s\n", currentCoreCtx->coreId, ctx->buf);
 }
 
-
-
-/**
- * Core section of the stub-- sets up the hypervisor from up in EL2.
- */
-int main(int argc, void **argv)
+void test(void)
 {
-    // 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!");
+    TransportInterface *iface = transportInterfaceCreate(
+        TRANSPORT_INTERFACE_TYPE_UART,
+        DEFAULT_UART,
+        DEFAULT_UART_FLAGS,
+        testReceiveCallback,
+        testProcessDataCallback,
+        &g_testCtx
+    );
+    transportInterfaceSetInterruptAffinity(iface, BIT(1));
 }
 
-    // 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)
+void thermosphereMain(ExceptionStackFrame *frame, u64 pct)
 {
-    int rc;
-    (void)rc;
+    initIrq();
 
-    // 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 (currentCoreCtx->isBootCore) {
+        transportInterfaceInitLayer();
+        debugLogInit();
+        //test();
+        debugManagerInit(TRANSPORT_INTERFACE_TYPE_UART, DEFAULT_UART, DEFAULT_UART_FLAGS);
+        DEBUG("EL2: core %u reached main first!\n", currentCoreCtx->coreId);
     }
 
-    // If we've made it here, we failed to boot, and we can't recover.
-    panic("We should launch Horizon here!");
+    enableTraps();
+    enableBreakpointsAndWatchpoints();
+    timerInit();
+    initBreakpoints();
+    initWatchpoints();
+
+    if (currentCoreCtx->isBootCore) {
+        if (currentCoreCtx->kernelEntrypoint == 0) {
+            ENSURE2(semihosting_connection_supported(), "Kernel not loaded!\n");
+            loadKernelViaSemihosting();
+        }
+    }
+    else {
+        DEBUG("EL2: core %u reached main!\n", currentCoreCtx->coreId);
+    }
+
+    setCurrentCoreActive();
+
+    // Set up exception frame: init regs to 0, set spsr, elr, etc.
+    memset(frame, 0, sizeof(ExceptionStackFrame));
+    frame->spsr_el2     = (0xF << 6) | (1 << 2) | 1; // EL1h+DAIF
+    frame->elr_el2      = currentCoreCtx->kernelEntrypoint;
+    frame->x[0]         = currentCoreCtx->kernelArgument;
+    frame->cntpct_el0   = pct;
+
+    // Initialize FPU registers -- no need to memset, the regcaches are in .tempbss
+    fpuCommitRegisters();
+    fpuCleanInvalidateRegisterCache();
+
+    if (!currentCoreCtx->isBootCore) {
+        debugManagerReportEvent(DBGEVENT_CORE_ON);
+    } else {
+        debugManagerReportEvent(DBGEVENT_DEBUGGER_BREAK);
+    }
 }

thermosphere/src/memory_map.c

@@ -0,0 +1,166 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "memory_map.h"
+#include "mmu.h"
+#include "sysreg.h"
+#include "platform/interrupt_config.h"
+
+#define ATTRIB_MEMTYPE_NORMAL MMU_PTE_BLOCK_MEMTYPE(MEMORY_MAP_MEMTYPE_NORMAL)
+#define ATTRIB_MEMTYPE_DEVICE MMU_PTE_BLOCK_MEMTYPE(MEMORY_MAP_MEMTYPE_DEVICE_NGNRE)
+
+static uintptr_t g_currentPlatformMmioPage = MEMORY_MAP_VA_MMIO_PLAT_BASE;
+
+void memoryMapSetupMmu(const LoadImageLayout *layout, u64 *mmuTable)
+{
+    static const u64 normalAttribs   =                    MMU_PTE_BLOCK_INNER_SHAREBLE | ATTRIB_MEMTYPE_NORMAL;
+    static const u64 deviceAttribs   = MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_INNER_SHAREBLE | ATTRIB_MEMTYPE_DEVICE;
+
+    // mmuTable is currently a PA
+    mmu_init_table(mmuTable, 0x200);
+
+    /*
+        Map the table into itself at the entry which index has all bits set.
+        This is called "recursive page tables" and means (assuming 39-bit addr space) that:
+            - the table will reuse itself as L2 table for the 0x7FC0000000+ range
+            - the table will reuse itself as L3 table for the 0x7FFFE00000+ range
+            - the table itself will be accessible at 0x7FFFFFF000
+    */
+    mmuTable[0x1FF] = (uintptr_t)mmuTable | MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_AF | MMU_PTE_TYPE_TABLE;
+
+    /*
+        Layout in physmem:
+        Location1
+            Image (code and data incl. BSS)
+        Location2
+            tempbss
+            MMU table (taken from temp physmem)
+
+        Layout in vmem:
+        Location1
+            Image
+            padding
+            tempbss
+        Location2
+            Crash stacks
+            {guard page, stack} * numCores
+        Location3 (all L1, L2, L3 bits set):
+            MMU table
+    */
+
+    // Map our code & data (.text/other code, .rodata, .data, .bss) at the bottom of our L3 range, all RWX
+    // Note that the end of "image" is page-aligned
+    // See LD script for more details
+    uintptr_t curVa = MEMORY_MAP_VA_IMAGE;
+    uintptr_t curPa = layout->startPa;
+
+    // Do not map the MMU table in that mapping:
+    mmu_map_page_range(mmuTable, curVa, curPa, layout->imageSize, normalAttribs);
+
+    curVa += layout->imageSize;
+    curPa = layout->tempPa;
+    mmu_map_page_range(mmuTable, curVa, curPa, layout->tempSize , normalAttribs);
+    curPa += layout->tempSize;
+
+    // Map the remaining temporary data as stacks, aligned 0x1000
+
+    // Crash stacks, total size is fixed:
+    curVa = MEMORY_MAP_VA_CRASH_STACKS_BOTTOM;
+    mmu_map_page_range(mmuTable, curVa, curPa, MEMORY_MAP_VA_CRASH_STACKS_SIZE, normalAttribs);
+    curPa += MEMORY_MAP_VA_CRASH_STACKS_SIZE;
+
+    // Regular stacks
+    size_t sizePerStack = 0x1000;
+    curVa = MEMORY_MAP_VA_STACKS_TOP - sizePerStack;
+    for (u32 i = 0; i < 4; i++) {
+        mmu_map_page_range(mmuTable, curVa, curPa, sizePerStack, normalAttribs);
+        curVa -= 2 * sizePerStack;
+        curPa += sizePerStack;
+    }
+
+    // MMIO
+    mmu_map_page(mmuTable, MEMORY_MAP_VA_GICD, MEMORY_MAP_PA_GICD, deviceAttribs);
+    mmu_map_page_range(mmuTable, MEMORY_MAP_VA_GICC, MEMORY_MAP_PA_GICC, 0x2000, deviceAttribs);
+    mmu_map_page(mmuTable, MEMORY_MAP_VA_GICH, MEMORY_MAP_PA_GICH, deviceAttribs);
+}
+
+void memoryMapEnableMmu(const LoadImageLayout *layout)
+{
+    uintptr_t mmuTable = layout->tempPa + layout->maxTempSize;
+
+    u32 ps = GET_SYSREG(id_aa64mmfr0_el1) & 0xF;
+    /*
+        - PA size: from ID_AA64MMFR0_EL1
+        - Granule size: 4KB
+        - Shareability attribute for memory associated with translation table walks using TTBR0_EL2: Inner Shareable
+        - Outer cacheability attribute for memory associated with translation table walks using TTBR0_EL2: Normal memory, Outer Write-Back Read-Allocate Write-Allocate Cacheable
+        - Inner cacheability attribute for memory associated with translation table walks using TTBR0_EL2: Normal memory, Inner Write-Back Read-Allocate Write-Allocate Cacheable
+        - T0SZ = MEMORY_MAP_VA_SPACE_SIZE = 39
+    */
+    u64 tcr = TCR_EL2_RSVD | TCR_PS(ps) | TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA | TCR_T0SZ(MEMORY_MAP_VA_SPACE_SIZE);
+
+
+    /*
+        - Attribute 0: Device-nGnRnE memory
+        - Attribute 1: Normal memory, Inner and Outer Write-Back Read-Allocate Write-Allocate Non-transient
+        - Attribute 2: Device-nGnRE memory
+        - Attribute 3: Normal memory, Inner and Outer Noncacheable
+        - Other attributes: Device-nGnRnE memory
+    */
+    u64 mair = 0x44FF0400;
+
+    // Set VBAR because we *will* crash (instruction abort because of the value of pc) when enabling the MMU
+    SET_SYSREG(vbar_el2, layout->vbar);
+
+    // MMU regs config
+    SET_SYSREG(ttbr0_el2, mmuTable);
+    SET_SYSREG(tcr_el2, tcr);
+    SET_SYSREG(mair_el2, mair);
+    __dsb_local();
+    __isb();
+
+    // TLB invalidation
+    // Whether this does anything before MMU is enabled is impldef, apparently
+    __tlb_invalidate_el2_local();
+    __dsb_local();
+    __isb();
+
+    // Enable MMU & enable caching. We will crash.
+    u64 sctlr = GET_SYSREG(sctlr_el2);
+    sctlr |= SCTLR_ELx_I | SCTLR_ELx_C | SCTLR_ELx_M;
+    SET_SYSREG(sctlr_el2, sctlr);
+    __dsb_local();
+    __isb();
+}
+
+uintptr_t memoryMapGetStackTop(u32 coreId)
+{
+    return MEMORY_MAP_VA_STACKS_TOP - 0x2000 * coreId;
+}
+
+uintptr_t memoryMapPlatformMmio(uintptr_t pa, size_t size)
+{
+    uintptr_t va = g_currentPlatformMmioPage;
+    static const u64 deviceAttribs = MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_INNER_SHAREBLE | ATTRIB_MEMTYPE_DEVICE;
+    u64 *mmuTable = (u64 *)MEMORY_MAP_VA_TTBL;
+
+    size = (size + 0xFFF) & ~0xFFFul;
+    mmu_map_page_range(mmuTable, va, pa, size, deviceAttribs);
+
+    g_currentPlatformMmioPage += size;
+
+    return va;
+}

thermosphere/src/memory_map.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+
+#define MEMORY_MAP_MEMTYPE_DEVICE_NGNRNE                0ul
+#define MEMORY_MAP_MEMTYPE_NORMAL                       1ul
+#define MEMORY_MAP_MEMTYPE_DEVICE_NGNRE                 2ul
+#define MEMORY_MAP_MEMTYPE_NORMAL_UNCACHEABLE           3ul
+#define MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT            4ul
+
+#define MEMORY_MAP_VA_SPACE_SIZE                        39ul
+
+// The following few definitions depend on the value of MEMORY_MAP_VA_SPACE_SIZE:
+#define MEMORY_MAP_SELF_L2_VA_RANGE                     0x7FC0000000ul // = 511 << 31
+#define MEMORY_MAP_SELF_L3_VA_RANGE                     0x7FFFE00000ul // = 511 << 31 | 511 << 21
+#define MEMORY_MAP_VA_TTBL                              0x7FFFFFF000ul // = 511 << 31 | 511 << 21 | 511 << 12
+#define MEMORY_MAP_VA_MAX                               0x7FFFFFFFFFul // = all 39 bits set
+
+#define MEMORY_MAP_VA_CRASH_STACKS_SIZE                 0x1000ul
+#define MEMORY_MAP_VA_IMAGE                             (MEMORY_MAP_SELF_L3_VA_RANGE + 0x10000)
+#define MEMORY_MAP_VA_CRASH_STACKS_BOTTOM               (MEMORY_MAP_SELF_L3_VA_RANGE + 0x40000)
+#define MEMORY_MAP_VA_CRASH_STACKS_TOP                  (MEMORY_MAP_SELF_L3_VA_RANGE + 0x41000)
+#define MEMORY_MAP_VA_GUEST_MEM                         (MEMORY_MAP_SELF_L3_VA_RANGE + 0x50000)
+#define MEMORY_MAP_VA_STACKS_TOP                        (MEMORY_MAP_SELF_L3_VA_RANGE + 0x80000)
+
+#define MEMORY_MAP_VA_MMIO_BASE                         MEMORY_MAP_VA_STACKS_TOP
+#define MEMORY_MAP_VA_GICD                              MEMORY_MAP_VA_MMIO_BASE
+#define MEMORY_MAP_VA_GICC                              (MEMORY_MAP_VA_MMIO_BASE + 0x1000)
+#define MEMORY_MAP_VA_GICH                              (MEMORY_MAP_VA_MMIO_BASE + 0x3000)
+
+#define MEMORY_MAP_VA_MMIO_PLAT_BASE                    (MEMORY_MAP_VA_MMIO_BASE + 0x90000)
+
+
+typedef struct LoadImageLayout {
+    uintptr_t startPa;
+    size_t imageSize; // "image" includes "real" BSS but not tempbss
+
+    uintptr_t tempPa;
+    size_t maxTempSize;
+    size_t tempSize;
+
+    uintptr_t vbar;
+} LoadImageLayout;
+
+extern LoadImageLayout g_loadImageLayout;
+
+uintptr_t memoryMapGetStackTop(u32 coreId);
+
+// Non-reentrant
+uintptr_t memoryMapPlatformMmio(uintptr_t pa, size_t size);

thermosphere/src/mmu.h

@@ -0,0 +1,198 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+
+
+#ifndef MMU_GRANULE_TYPE
+#define MMU_GRANULE_TYPE    0 /* 0: 4KB, 1: 64KB, 2: 16KB. The Switch always uses a 4KB granule size. */
+#endif
+
+#if MMU_GRANULE_TYPE == 0
+#define MMU_Lx_SHIFT(x) (12 + 9 * (3 - (x)))
+#define MMU_Lx_MASK(x)  MASKL(9)
+#elif MMU_GRANULE_TYPE == 1
+/* 64 KB, no L0 here */
+#define MMU_Lx_SHIFT(x) (16 + 13 * (3 - (x)))
+#define MMU_Lx_MASK(x)  ((x) == 1 ? MASKL(5) : MASKL(13))
+#elif MMU_GRANULE_TYPE == 2
+#define MMU_Lx_SHIFT(x) (14 + 11 * (3 - (x)))
+#define MMU_Lx_MASK(x)  ((x) == 0 ? 1 : MASKL(11))
+#endif
+
+/*
+ * The following defines are adapted from uboot:
+ *
+ * (C) Copyright 2013
+ * David Feng <fenghua@phytium.com.cn>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+/* Memory attributes, see set_memory_registers_enable_mmu */
+#define MMU_MT_NORMAL           0ull
+#define MMU_MT_DEVICE_NGNRE     1ull
+#define MMU_MT_DEVICE_NGNRNE    2ull /* not used, also the same as Attr4-7 */
+
+/*
+ * Hardware page table definitions.
+ *
+ */
+
+#define MMU_PTE_TYPE_MASK       3ull
+#define MMU_PTE_TYPE_FAULT      0ull
+#define MMU_PTE_TYPE_TABLE      3ull
+#define MMU_PTE_TYPE_BLOCK      1ull
+
+/* L3 only */
+#define MMU_PTE_TYPE_PAGE       3ull
+
+#define MMU_PTE_TABLE_PXN       BITL(59)
+#define MMU_PTE_TABLE_XN        BITL(60)
+#define MMU_PTE_TABLE_AP        BITL(61)
+#define MMU_PTE_TABLE_NS        BITL(63)
+
+/*
+ * Block
+ */
+#define MMU_PTE_BLOCK_MEMTYPE(x)        ((uint64_t)((x) << 2))
+#define MMU_PTE_BLOCK_NS                BITL(5)
+#define MMU_PTE_BLOCK_SH(x)             ((x) << 8)
+#define MMU_PTE_BLOCK_NON_SHAREABLE     (0ull << 8)
+#define MMU_PTE_BLOCK_OUTER_SHAREABLE   (2ull << 8)
+#define MMU_PTE_BLOCK_INNER_SHAREBLE    (3ull << 8)
+#define MMU_PTE_BLOCK_AF                BITL(10)
+#define MMU_PTE_BLOCK_NG                BITL(11)
+#define MMU_PTE_BLOCK_PXN               BITL(53)
+#define MMU_PTE_BLOCK_UXN               BITL(54)
+#define MMU_PTE_BLOCK_XN                MMU_PTE_BLOCK_UXN
+
+/*
+ * AP[2:1]
+ */
+#define MMU_AP_PRIV_RW                  (0ull << 6)
+#define MMU_AP_RW                       (1ull << 6)
+#define MMU_AP_PRIV_RO                  (2ull << 6)
+#define MMU_AP_RO                       (3ull << 6)
+
+/*
+ * S2AP[1:0] (for stage2 translations; secmon doesn't use it)
+ */
+#define MMU_S2AP_NONE                   (0ull << 6)
+#define MMU_S2AP_RO                     (1ull << 6)
+#define MMU_S2AP_WO                     (2ull << 6)
+#define MMU_S2AP_RW                     (3ull << 6)
+
+/*
+ * AttrIndx[2:0]
+ */
+#define MMU_PMD_ATTRINDX(t)     ((uint64_t)((t) << 2))
+#define MMU_PMD_ATTRINDX_MASK   (7ull << 2)
+
+/*
+ * MemAttr[3:0] (stage2)
+ * 
+ */
+#define MMU_MEMATTR(x)                              ((x) << 2)
+#define MMU_MEMATTR_DEVICE_NGNRE                    MMU_MEMATTR(2)
+#define MMU_MEMATTR_NORMAL_CACHEABLE_OR_UNCHANGED   MMU_MEMATTR(0xF)
+
+/*
+ * TCR flags.
+ */
+#define TCR_T0SZ(x)         ((64 - (x)) << 0)
+#define VTCR_SL0(x)         ((x) << 6)
+#define TCR_IRGN_NC         (0 << 8)
+#define TCR_IRGN_WBWA       (1 << 8)
+#define TCR_IRGN_WT         (2 << 8)
+#define TCR_IRGN_WBNWA      (3 << 8)
+#define TCR_IRGN_MASK       (3 << 8)
+#define TCR_ORGN_NC         (0 << 10)
+#define TCR_ORGN_WBWA       (1 << 10)
+#define TCR_ORGN_WT         (2 << 10)
+#define TCR_ORGN_WBNWA      (3 << 10)
+#define TCR_ORGN_MASK       (3 << 10)
+#define TCR_NOT_SHARED      (0 << 12)
+#define TCR_SHARED_OUTER    (2 << 12)
+#define TCR_SHARED_INNER    (3 << 12)
+#define TCR_TG0_4K          (0 << 14)
+#define TCR_TG0_64K         (1 << 14)
+#define TCR_TG0_16K         (2 << 14)
+#define TCR_PS(x)           ((x) << 16)
+#define TCR_EPD1_DISABLE    BIT(23)
+
+#define TCR_EL1_RSVD        BIT(31)
+#define TCR_EL2_RSVD        (BIT(31) | BIT(23))
+#define VTCR_EL2_RSVD       BIT(31)
+#define TCR_EL3_RSVD        (BIT(31) | BIT(23))
+
+static inline void mmu_init_table(uintptr_t *tbl, size_t num_entries) {
+    for(size_t i = 0; i < num_entries; i++) {
+        tbl[i] = MMU_PTE_TYPE_FAULT;
+    }
+}
+
+/*
+    All the functions below assume base_addr is valid.
+    They do not invalidate the TLB, which must be done separately.
+*/
+
+static inline unsigned int mmu_compute_index(unsigned int level, uintptr_t base_addr) {
+    return (base_addr >> MMU_Lx_SHIFT(level)) & MMU_Lx_MASK(level);
+}
+
+static inline void mmu_map_table(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t *next_lvl_tbl_pa, uint64_t attrs) {
+    tbl[mmu_compute_index(level, base_addr)] = (uintptr_t)next_lvl_tbl_pa | attrs | MMU_PTE_TYPE_TABLE;
+}
+
+static inline void mmu_map_block(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, uint64_t attrs) {
+    tbl[mmu_compute_index(level, base_addr)] = phys_addr | attrs | MMU_PTE_BLOCK_AF | MMU_PTE_TYPE_BLOCK;
+}
+
+static inline void mmu_map_page(uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, uint64_t attrs) {
+    tbl[mmu_compute_index(3, base_addr)] = phys_addr | attrs | MMU_PTE_BLOCK_AF | MMU_PTE_TYPE_PAGE;
+}
+
+static inline void mmu_unmap(unsigned int level, uintptr_t *tbl, uintptr_t base_addr) {
+    tbl[mmu_compute_index(level, base_addr)] = MMU_PTE_TYPE_FAULT;
+}
+
+static inline void mmu_unmap_page(uintptr_t *tbl, uintptr_t base_addr) {
+    tbl[mmu_compute_index(3, base_addr)] = MMU_PTE_TYPE_FAULT;
+}
+
+static inline void mmu_map_block_range(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, size_t size, uint64_t attrs) {
+    size = ((size + (BITL(MMU_Lx_SHIFT(level)) - 1)) >> MMU_Lx_SHIFT(level)) << MMU_Lx_SHIFT(level);
+    for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) {
+        mmu_map_block(level, tbl, base_addr + offset, phys_addr + offset, attrs);
+    }
+}
+
+static inline void mmu_map_page_range(uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, size_t size, uint64_t attrs) {
+    size = ((size + (BITL(MMU_Lx_SHIFT(3)) - 1)) >> MMU_Lx_SHIFT(3)) << MMU_Lx_SHIFT(3);
+    for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(3))) {
+        mmu_map_page(tbl, base_addr + offset, phys_addr + offset, attrs);
+    }
+}
+
+static inline void mmu_unmap_range(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, size_t size) {
+    size = ((size + (BITL(MMU_Lx_SHIFT(level)) - 1)) >> MMU_Lx_SHIFT(level)) << MMU_Lx_SHIFT(level);
+    for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) {
+        mmu_unmap(level, tbl, base_addr + offset);
+    }
+}

thermosphere/src/pattern_utils.c

@@ -0,0 +1,352 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+#include "pattern_utils.h"
+
+
+u32 hexItoa(u64 number, char *out, u32 digits, bool uppercase) 
+{
+    static const char hexDigits[] = "0123456789ABCDEF";
+    static const char hexDigitsLowercase[] = "0123456789abcdef";
+    u32 i = 0;
+
+    while (number > 0) {
+        out[digits - 1 - i++] = uppercase ? hexDigits[number & 0xF] : hexDigitsLowercase[number & 0xF];
+        number >>= 4;
+    }
+
+    while (i < digits) out[digits - 1 - i++] = '0';
+    return digits;
+}
+
+//Boyer-Moore Horspool algorithm, adapted from http://www-igm.univ-mlv.fr/~lecroq/string/node18.html#SECTION00180
+//u32 size to limit stack usage
+u8 *memsearch(u8 *startPos, const void *pattern, u32 size, u32 patternSize)
+{
+    const u8 *patternc = (const u8 *)pattern;
+    u32 table[256];
+
+    //Preprocessing
+    for(u32 i = 0; i < 256; i++)
+        table[i] = patternSize;
+    for(u32 i = 0; i < patternSize - 1; i++)
+        table[patternc[i]] = patternSize - i - 1;
+
+    //Searching
+    u32 j = 0;
+    while(j <= size - patternSize)
+    {
+        u8 c = startPos[j + patternSize - 1];
+        if(patternc[patternSize - 1] == c && memcmp(pattern, startPos + j, patternSize - 1) == 0)
+            return startPos + j;
+        j += table[c];
+    }
+
+    return NULL;
+}
+
+// Copied from newlib, without the reent stuff + some other stuff
+
+/*
+ * Copyright (c) 1990 Regents of the University of California.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *	This product includes software developed by the University of
+ *	California, Berkeley and its contributors.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+unsigned int xstrtoui(const char *nptr, char **endptr, int base, bool allowPrefix, bool *ok)
+{
+    register const unsigned char *s = (const unsigned char *)nptr;
+    register unsigned int acc;
+    register int c;
+    register unsigned int cutoff;
+    register int neg = 0, any, cutlim;
+
+    if(ok != NULL)
+        *ok = true;
+    /*
+     * See strtol for comments as to the logic used.
+     */
+    do {
+        c = *s++;
+    } while ((c >= 9 && c <= 13) || c == ' ');
+    if (c == '-') {
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+        neg = 1;
+        c = *s++;
+    } else if (c == '+'){
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+
+        c = *s++;
+    }
+    if ((base == 0 || base == 16) &&
+        c == '0' && (*s == 'x' || *s == 'X')) {
+
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+        c = s[1];
+        s += 2;
+        base = 16;
+    }
+    if (base == 0) {
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+
+        base = c == '0' ? 8 : 10;
+    }
+    cutoff = (unsigned int)(-1) / (unsigned int)base;
+    cutlim = (unsigned int)(-1) % (unsigned int)base;
+    for (acc = 0, any = 0;; c = *s++) {
+        if (c >= '0' && c <= '9')
+            c -= '0';
+        else if ((c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z'))
+            c -= c >= 'A' && c <= 'Z' ? 'A' - 10 : 'a' - 10;
+        else
+            break;
+        if (c >= base)
+            break;
+               if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
+            any = -1;
+        else {
+            any = 1;
+            acc *= base;
+            acc += c;
+        }
+    }
+    if (any < 0) {
+        acc = (unsigned int)-1;
+        if(ok != NULL)
+            *ok = false;
+//        rptr->_errno = ERANGE;
+    } else if (neg)
+        acc = -acc;
+    if (endptr != 0)
+        *endptr = (char *) (any ? (char *)s - 1 : nptr);
+    return (acc);
+}
+
+// Copied from newlib, without the reent stuff + some other stuff
+unsigned long int xstrtoul(const char *nptr, char **endptr, int base, bool allowPrefix, bool *ok)
+{
+    register const unsigned char *s = (const unsigned char *)nptr;
+    register unsigned long acc;
+    register int c;
+    register unsigned long cutoff;
+    register int neg = 0, any, cutlim;
+
+    if(ok != NULL)
+        *ok = true;
+    /*
+     * See strtol for comments as to the logic used.
+     */
+    do {
+        c = *s++;
+    } while ((c >= 9 && c <= 13) || c == ' ');
+    if (c == '-') {
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+        neg = 1;
+        c = *s++;
+    } else if (c == '+'){
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+
+        c = *s++;
+    }
+    if ((base == 0 || base == 16) &&
+        c == '0' && (*s == 'x' || *s == 'X')) {
+
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+        c = s[1];
+        s += 2;
+        base = 16;
+    }
+    if (base == 0) {
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+
+        base = c == '0' ? 8 : 10;
+    }
+    cutoff = (unsigned long)(-1) / (unsigned long)base;
+    cutlim = (unsigned long)(-1) % (unsigned long)base;
+    for (acc = 0, any = 0;; c = *s++) {
+        if (c >= '0' && c <= '9')
+            c -= '0';
+        else if ((c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z'))
+            c -= c >= 'A' && c <= 'Z' ? 'A' - 10 : 'a' - 10;
+        else
+            break;
+        if (c >= base)
+            break;
+               if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
+            any = -1;
+        else {
+            any = 1;
+            acc *= base;
+            acc += c;
+        }
+    }
+    if (any < 0) {
+        acc = (unsigned long)-1;
+        if(ok != NULL)
+            *ok = false;
+//        rptr->_errno = ERANGE;
+    } else if (neg)
+        acc = -acc;
+    if (endptr != 0)
+        *endptr = (char *) (any ? (char *)s - 1 : nptr);
+    return (acc);
+}
+
+// Copied from newlib, without the reent stuff + some other stuff
+unsigned long long int xstrtoull(const char *nptr, char **endptr, int base, bool allowPrefix, bool *ok)
+{
+    register const unsigned char *s = (const unsigned char *)nptr;
+    register unsigned long long acc;
+    register int c;
+    register unsigned long long cutoff;
+    register int neg = 0, any, cutlim;
+
+    if(ok != NULL)
+        *ok = true;
+    /*
+     * See strtol for comments as to the logic used.
+     */
+    do {
+        c = *s++;
+    } while ((c >= 9 && c <= 13) || c == ' ');
+    if (c == '-') {
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+        neg = 1;
+        c = *s++;
+    } else if (c == '+'){
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+
+        c = *s++;
+    }
+    if ((base == 0 || base == 16) &&
+        c == '0' && (*s == 'x' || *s == 'X')) {
+
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+        c = s[1];
+        s += 2;
+        base = 16;
+    }
+    if (base == 0) {
+        if(!allowPrefix) {
+            if(ok != NULL)
+                *ok = false;
+            return 0;
+        }
+
+        base = c == '0' ? 8 : 10;
+    }
+    cutoff = (unsigned long long)(-1ull) / (unsigned long long)base;
+    cutlim = (unsigned long long)(-1ull) % (unsigned long long)base;
+    for (acc = 0, any = 0;; c = *s++) {
+        if (c >= '0' && c <= '9')
+            c -= '0';
+        else if ((c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z'))
+            c -= c >= 'A' && c <= 'Z' ? 'A' - 10 : 'a' - 10;
+        else
+            break;
+        if (c >= base)
+            break;
+               if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
+            any = -1;
+        else {
+            any = 1;
+            acc *= base;
+            acc += c;
+        }
+    }
+    if (any < 0) {
+        acc = (unsigned long long)-1ull;
+        if(ok != NULL)
+            *ok = false;
+//        rptr->_errno = ERANGE;
+    } else if (neg)
+        acc = -acc;
+    if (endptr != 0)
+        *endptr = (char *) (any ? (char *)s - 1 : nptr);
+    return (acc);
+}

thermosphere/src/pattern_utils.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+
+u32 hexItoa(u64 number, char *out, u32 digits, bool uppercase);
+
+// u32 size to limit stack usage
+// Not sure if we need this function because we can only map one guest page at a time...
+u8 *memsearch(u8 *startPos, const void *pattern, u32 size, u32 patternSize);
+unsigned int xstrtoui(const char *nptr, char **endptr, int base, bool allowPrefix, bool *ok);
+unsigned long int xstrtoul(const char *nptr, char **endptr, int base, bool allowPrefix, bool *ok);
+unsigned long long int xstrtoull(const char *nptr, char **endptr, int base, bool allowPrefix, bool *ok);

thermosphere/src/platform/devices.h

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef PLATFORM_TEGRA
+
+#include "tegra/devices.h"
+
+#elif defined(PLATFORM_QEMU)
+
+#include "qemu/devices.h"
+
+#endif

thermosphere/src/platform/interrupt_config.h

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef PLATFORM_TEGRA
+
+#include "tegra/interrupt_config.h"
+
+#elif defined(PLATFORM_QEMU)
+
+#include "qemu/interrupt_config.h"
+
+#endif

thermosphere/src/platform/qemu/devices.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020 Atmosphère-NX
+ * Copyright (c) 2019 Atmosphère-NX
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -14,23 +14,18 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "printk.h"
-#include "vsprintf.h"
+#include "devices.h"
+#include "../../memory_map.h"
+#include "../../utils.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, ...)
+#include "uart.h"
+
+void devicesMapAllExtra(void)
 {
-    va_list list;
-    char buf[512];
-    va_start(list, fmt);
-    vsnprintf(buf, sizeof(buf), fmt, list);
+    uartSetRegisterBase(memoryMapPlatformMmio(MEMORY_MAP_PA_UART, 0x1000));
 
-    /* FIXME: print via UART */
-
-    va_end(list);
+    // Don't broadcast, since it's only ran once per boot by only one core, before the others are started...
+    __tlb_invalidate_el2_local();
+    __dsb_local();
+    __isb();
 }

thermosphere/src/platform/qemu/devices.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020 Atmosphère-NX
+ * Copyright (c) 2019 Atmosphère-NX
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -14,4 +14,6 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-void printk(char *fmt, ...);
+#pragma once
+
+void devicesMapAllExtra(void);

thermosphere/src/platform/qemu/interrupt_config.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "../../gicv2.h"
+
+#define MEMORY_MAP_PA_GICD              0x08000000ull
+#define MEMORY_MAP_PA_GICC              0x08010000ull
+#define MEMORY_MAP_PA_GICH              0x08030000ull
+#define MEMORY_MAP_PA_GICV              0x08040000ull
+
+#define GIC_IRQID_PMU                   23
+#define GIC_IRQID_MAINTENANCE           25
+#define GIC_IRQID_NS_PHYS_HYP_TIMER     26
+#define GIC_IRQID_NS_VIRT_TIMER         27
+#define GIC_IRQID_SEC_PHYS_TIMER        29
+#define GIC_IRQID_NS_PHYS_TIMER         30
+
+
+#define GIC_IRQID_NS_VIRT_HYP_TIMER     GIC_IRQID_SPURIOUS // SBSA: 28. Unimplemented
+#define GIC_IRQID_SEC_PHYS_HYP_TIMER    GIC_IRQID_SPURIOUS // SBSA: 20. Unimplemented
+#define GIC_IRQID_SEC_VIRT_HYP_TIMER    GIC_IRQID_SPURIOUS // SBSA: 19. Unimplemented
+
+#define GIC_IRQID_UART                  (32 + 1)

thermosphere/src/platform/qemu/stage2_config.c

@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "stage2_config.h"
+#include "interrupt_config.h"
+#include "../../memory_map.h"
+#include "../../utils.h"
+#include "../../mmu.h"
+#include "../../core_ctx.h"
+
+// QEMU presently advertises 44-bit PAs we'll only use 39 of them to avoid level 0 tables.
+#define ADDRSPACESZ    39
+#define ADDRSPACESZ2   ADDRSPACESZ
+
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl[BIT(ADDRSPACESZ2 - 30)] = {0};
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl_l2_mmio_0_0[512] = {0};
+static TEMPORARY ALIGN(0x1000) u64 g_vttbl_l3_0[512] = {0};
+
+static inline void identityMapL1(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(1, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
+static inline void identityMapL2(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(2, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
+static inline void identityMapL3(u64 *tbl, uintptr_t addr, size_t size, u64 attribs)
+{
+    mmu_map_block_range(3, tbl, addr, addr, size, attribs | MMU_PTE_BLOCK_INNER_SHAREBLE);
+}
+
+uintptr_t stage2Configure(u32 *addrSpaceSize)
+{
+    *addrSpaceSize = ADDRSPACESZ2;
+    static const u64 devattrs = 0 | MMU_S2AP_RW | MMU_MEMATTR_DEVICE_NGNRE;
+    static const u64 unchanged = MMU_S2AP_RW | MMU_MEMATTR_NORMAL_CACHEABLE_OR_UNCHANGED;
+
+    uintptr_t g_vttblPaddr = va2pa(g_vttbl);
+
+    if (currentCoreCtx->isBootCore) {
+        uintptr_t *l2pa = (uintptr_t *)va2pa(g_vttbl_l2_mmio_0_0);
+        uintptr_t *l3pa = (uintptr_t *)va2pa(g_vttbl_l3_0);
+
+        identityMapL1(g_vttbl, 0, 4ull << 30, unchanged);
+        identityMapL1(g_vttbl, 0x40000000ull, (BITL(ADDRSPACESZ2 - 30) - 1ull) << 30, unchanged);
+        mmu_map_table(1, g_vttbl, 0x00000000ull, l2pa, 0);
+
+        identityMapL2(g_vttbl_l2_mmio_0_0, 0x08000000ull, BITL(30), unchanged);
+        mmu_map_table(2, g_vttbl_l2_mmio_0_0, 0x08000000ull, l3pa, 0);
+
+        identityMapL3(g_vttbl_l3_0, 0x08000000ull, BITL(21), unchanged);
+
+        // GICD -> trapped, GICv2 CPU -> vCPU interface, GICH -> trapped (deny access)
+        mmu_unmap_range(3, g_vttbl_l3_0, MEMORY_MAP_PA_GICD, 0x10000ull);
+        mmu_unmap_range(3, g_vttbl_l3_0, MEMORY_MAP_PA_GICH, 0x10000ull);
+        mmu_map_page_range(g_vttbl_l3_0, MEMORY_MAP_PA_GICC, MEMORY_MAP_PA_GICV, 0x10000ull, devattrs);
+    }
+
+    return g_vttblPaddr;
+}

thermosphere/src/platform/qemu/stage2_config.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "../../types.h"
+
+uintptr_t stage2Configure(u32 *addrSpaceSize);

thermosphere/src/platform/qemu/uart.c

@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "uart.h"
+#include "../../irq.h"
+
+// Adapted from TF asm code
+// AMBA PL101 driver
+
+/*
+ * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+//115200
+
+static uintptr_t g_uartRegBase;
+
+static inline volatile PL011UartRegisters *uartGetRegisters(UartDevice dev)
+{
+    switch (dev) {
+        case UART_A:
+            return (volatile PL011UartRegisters *)g_uartRegBase;
+        default:
+            return NULL;
+    }
+}
+
+void uartSetRegisterBase(uintptr_t regBase)
+{
+    g_uartRegBase = regBase;
+}
+
+void uartInit(UartDevice dev, u32 baudRate, u32 flags)
+{
+    /* The TRM (DDI0183) reads:
+        Program the control registers as follows:
+        1. Disable the UART.
+        2. Wait for the end of transmission or reception of the current character.
+        3. Flush the transmit FIFO by disabling bit 4 (FEN) in the line control register
+        (UARTCLR_H).
+        4. Reprogram the control register.
+        5. Enable the UART.
+    */
+    (void)flags;
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+
+    // First, disable the UART. Flush the receive FIFO, wait for tx to complete, and disable both FIFOs.
+    uart->cr &= ~PL011_UARTCR_UARTEN;
+    while (!(uart->fr & PL011_UARTFR_RXFE)) {
+        uart->dr;
+    }
+    while (uart->fr & PL011_UARTFR_BUSY);
+    // This flushes the transmit FIFO:
+    uart->lcr_h &= ~PL011_UARTLCR_H_FEN;
+
+    // Set baudrate, Divisor =  (Uart clock * 4) / baudrate; stored in IBRD|FBRD
+    u32 divisor = (4 * UART_CLK_IN_HZ) / baudRate;
+    uart->ibrd = divisor >> 6;
+    uart->fbrd = divisor & 0x3F;
+
+    // Select FIFO fill levels for interrupts
+    uart->ifls = PL011_IFLS_RX4_8 | PL011_IFLS_TX4_8;
+
+    // FIFO Enabled / No Parity / 8 Data bit / One Stop Bit
+    uart->lcr_h = PL011_UARTLCR_H_FEN | PL011_UARTLCR_H_WLEN_8;
+
+    // Select the interrupts we want to have
+    // RX timeout and TX/RX fill interrupts
+    uart->imsc = PL011_RTI | PL011_RXI | PL011_RXI;
+
+    // Clear any pending errors
+    uart->ecr = 0;
+
+    // Clear all interrupts
+    uart->icr = PL011_ALL_INTERRUPTS;
+
+    // Register the interrupt ID
+    configureInterrupt(uartGetIrqId(dev), IRQ_PRIORITY_HOST, true);
+
+    // Enable tx, rx, and uart overall
+    uart->cr = PL011_UARTCR_RXE | PL011_UARTCR_TXE | PL011_UARTCR_UARTEN;
+}
+
+void uartWriteData(UartDevice dev, const void *buffer, size_t size)
+{
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+
+    const u8 *buf8 = (const u8 *)buffer;
+    for (size_t i = 0; i < size; i++) {
+        while (uart->fr & PL011_UARTFR_TXFF); // while TX FIFO full
+        uart->dr = buf8[i]; 
+    }
+}
+
+void uartReadData(UartDevice dev, void *buffer, size_t size)
+{
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+
+    u8 *buf8 = (u8 *)buffer;
+    size_t i;
+
+    for (i = 0; i < size; i++) {
+        while (uart->fr & PL011_UARTFR_RXFE);
+        buf8[i] = uart->dr;
+    }
+}
+
+size_t uartReadDataMax(UartDevice dev, void *buffer, size_t maxSize)
+{
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+
+    u8 *buf8 = (u8 *)buffer;
+    size_t count = 0;
+
+    for (size_t i = 0; i < maxSize && !(uart->fr & PL011_UARTFR_RXFE); i++) {
+        buf8[i] = uart->dr;
+        ++count;
+    }
+
+    return count;
+}
+
+size_t uartReadDataUntil(UartDevice dev, char *buffer, size_t maxSize, char delimiter)
+{
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+
+    size_t count = 0;
+
+    for (size_t i = 0; i < maxSize; i++) {
+        while (uart->fr & PL011_UARTFR_RXFE);
+        buffer[i] = uart->dr;
+        ++count;
+        if (buffer[i] == delimiter) {
+            break;
+        }
+    }
+
+    return count;
+}
+
+ReadWriteDirection uartGetInterruptDirection(UartDevice dev)
+{
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+    u32 ret = 0;
+
+    u32 istatus = uart->mis;
+    if (istatus & (PL011_RTI | PL011_RXI)) {
+        ret |= DIRECTION_READ;
+    }
+    if (istatus & PL011_TXI) {
+        ret |= DIRECTION_WRITE;
+    }
+
+    return (ReadWriteDirection)ret;
+}
+
+void uartSetInterruptStatus(UartDevice dev, ReadWriteDirection direction, bool enable)
+{
+    volatile PL011UartRegisters *uart = uartGetRegisters(dev);
+
+    u32 mask = 0;
+    if (direction & DIRECTION_READ) {
+        mask |= PL011_RTI | PL011_RXI;
+    }
+    if (direction & DIRECTION_WRITE) {
+        mask |= PL011_TXI;
+    }
+
+    if (enable) {
+        uart->imsc |= mask;
+    } else {
+        //uart->icr = mask;
+        uart->imsc &= ~mask;
+    }
+}

thermosphere/src/platform/qemu/uart.h

@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "../../utils.h"
+#include "interrupt_config.h"
+
+#define MEMORY_MAP_PA_UART      0x09000000ull
+
+// AMBA PL011 driver
+// Originally from
+/*
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+typedef enum UartDevice {
+    UART_A = 0,
+
+    UART_MAX,
+} UartDevice;
+
+typedef struct PL011UartRegisters {
+    u32 dr;
+    union {
+        u32 sr;
+        u32 ecr;
+    };
+    u32 _0x08, _0x0C, _0x10, _0x14;
+    u32 fr;
+    u32 _0x1C;
+    u32 ilpr;
+    u32 ibrd;
+    u32 fbrd;
+    u32 lcr_h;
+    u32 cr;
+    u32 ifls;
+    u32 imsc;
+    u32 ris;
+    u32 mis;
+    u32 icr;
+    u32 dmacr;
+} PL011UartRegisters;
+
+// Data status bits
+#define PL011_DATA_ERROR_MASK       0x0F00
+
+// Status reg bits
+#define PL011_STATUS_ERROR_MASK     0x0F
+
+// Errors
+#define PL011_OE                    BIT(3)  // Overrun error
+#define PL011_BE                    BIT(2)  // Break error
+#define PL011_PE                    BIT(1)  // Parity error
+#define PL011_FE                    BIT(0)  // Framing error
+
+// Interrupts
+#define PL011_OEI                   BIT(10) // Overrun error interrupt
+#define PL011_BEI                   BIT(9)  // Break error interrupt
+#define PL011_PEI                   BIT(8)  // Parity error interrupt
+#define PL011_FEI                   BIT(7)  // Framing error interrupt
+#define PL011_RTI                   BIT(6)  // Receive timeout interrupt
+#define PL011_TXI                   BIT(5)  // Transmit interrupt
+#define PL011_RXI                   BIT(4)  // Receive interrupt
+#define PL011_DSRMI                 BIT(3)  // DSR modem interrupt
+#define PL011_DCDMI                 BIT(2)  // DCD modem interrupt
+#define PL011_CTSMI                 BIT(1)  // CTS modem interrupt
+#define PL011_RIMI                  BIT(0)  // RI modem interrupt
+#define PL011_ALL_INTERRUPTS        MASK(11)
+
+// Flag reg bits
+#define PL011_UARTFR_RI             BIT(8)  // Ring indicator
+#define PL011_UARTFR_TXFE           BIT(7)  // Transmit FIFO empty
+#define PL011_UARTFR_RXFF           BIT(6)  // Receive  FIFO full
+#define PL011_UARTFR_TXFF           BIT(5)  // Transmit FIFO full
+#define PL011_UARTFR_RXFE           BIT(4)  // Receive  FIFO empty
+#define PL011_UARTFR_BUSY           BIT(3)  // UART busy
+#define PL011_UARTFR_DCD            BIT(2)  // Data carrier detect
+#define PL011_UARTFR_DSR            BIT(1)  // Data set ready
+#define PL011_UARTFR_CTS            BIT(0)  // Clear to send
+
+// Control reg bits
+#define PL011_UARTCR_CTSEN          BIT(15) // CTS hardware flow control enable
+#define PL011_UARTCR_RTSEN          BIT(14) // RTS hardware flow control enable
+#define PL011_UARTCR_RTS            BIT(11) // Request to send
+#define PL011_UARTCR_DTR            BIT(10) // Data transmit ready.
+#define PL011_UARTCR_RXE            BIT(9)  // Receive enable
+#define PL011_UARTCR_TXE            BIT(8)  // Transmit enable
+#define PL011_UARTCR_LBE            BIT(7)  // Loopback enable
+#define PL011_UARTCR_UARTEN         BIT(0)  // UART Enable
+
+// Line Control Register Bits
+#define PL011_UARTLCR_H_SPS         BIT(7)  // Stick parity select
+#define PL011_UARTLCR_H_WLEN_8      (3 << 5)
+#define PL011_UARTLCR_H_WLEN_7      (2 << 5)
+#define PL011_UARTLCR_H_WLEN_6      BIT(5)
+#define PL011_UARTLCR_H_WLEN_5      (0 << 5)
+#define PL011_UARTLCR_H_FEN         BIT(4)  // FIFOs Enable
+#define PL011_UARTLCR_H_STP2        BIT(3)  // Two stop bits select
+#define PL011_UARTLCR_H_EPS         BIT(2)  // Even parity select
+#define PL011_UARTLCR_H_PEN         BIT(1)  // Parity Enable
+#define PL011_UARTLCR_H_BRK         BIT(0)  // Send break
+
+// FIFO level select register
+#define PL011_IFLS_RX1_8            (0 << 3)
+#define PL011_IFLS_RX2_8            (1 << 3)
+#define PL011_IFLS_RX4_8            (2 << 3)
+#define PL011_IFLS_RX6_8            (3 << 3)
+#define PL011_IFLS_RX7_8            (4 << 3)
+#define PL011_IFLS_TX1_8            (0 << 0)
+#define PL011_IFLS_TX2_8            (1 << 0)
+#define PL011_IFLS_TX4_8            (2 << 0)
+#define PL011_IFLS_TX6_8            (3 << 0)
+#define PL011_IFLS_TX7_8            (4 << 0)
+
+#define UART_CLK_IN_HZ              1
+
+void uartSetRegisterBase(uintptr_t regBase);
+void uartInit(UartDevice dev, u32 baudRate, u32 flags);
+void uartWriteData(UartDevice dev, const void *buffer, size_t size);
+void uartReadData(UartDevice dev, void *buffer, size_t size);
+size_t uartReadDataMax(UartDevice dev, void *buffer, size_t maxSize);
+size_t uartReadDataUntil(UartDevice dev, char *buffer, size_t maxSize, char delimiter);
+
+ReadWriteDirection uartGetInterruptDirection(UartDevice dev);
+void uartSetInterruptStatus(UartDevice dev, ReadWriteDirection direction, bool enable);
+
+static inline u16 uartGetIrqId(UartDevice dev)
+{
+    switch (dev) {
+        case UART_A:
+            return GIC_IRQID_UART;
+        default:
+            return GIC_IRQID_SPURIOUS;
+    }
+}

thermosphere/src/platform/stage2.c

@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "../utils.h"
+#include "../sysreg.h"
+#include "../mmu.h"
+#include "stage2.h"
+
+void stage2ConfigureAndEnable(void)
+{
+    u32 addrSpaceSize;
+    uintptr_t vttbr = stage2Configure(&addrSpaceSize);
+
+    u32 ps = GET_SYSREG(id_aa64mmfr0_el1) & 0xF;
+    /*
+        - PA size: from ID_AA64MMFR0_EL1
+        - Granule size: 4KB
+        - Shareability attribute for memory associated with translation table walks using VTTBR_EL2: Inner Shareable
+        - Outer cacheability attribute for memory associated with translation table walks using VTTBR_EL2: Normal memory, Outer Write-Back Read-Allocate Write-Allocate Cacheable
+        - Inner cacheability attribute for memory associated with translation table walks using VTTBR_EL2: Normal memory, Inner Write-Back Read-Allocate Write-Allocate Cacheable
+        - SL0 = start at level 1
+        - T0SZ = from configureMemoryMap
+    */
+    u64 vtcr = VTCR_EL2_RSVD | TCR_PS(ps) | TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA | VTCR_SL0(1) | TCR_T0SZ(addrSpaceSize);
+
+    // Stage2 regs config
+    SET_SYSREG(vttbr_el2, vttbr);
+    SET_SYSREG(vtcr_el2, vtcr);
+    __dsb_local();
+    __isb();
+
+    // Enable stage 2
+    u64 hcr = GET_SYSREG(hcr_el2);
+    hcr |= HCR_VM;
+    SET_SYSREG(hcr_el2, hcr);
+    __dsb_local();
+    __isb();
+
+    // TLB invalidation
+    __tlb_invalidate_el1_stage12_local();
+    __dsb_local();
+    __isb();
+}

thermosphere/src/platform/stage2.h

@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef PLATFORM_TEGRA
+
+#include "tegra/stage2_config.h"
+
+#elif defined(PLATFORM_QEMU)
+
+#include "qemu/stage2_config.h"
+
+#endif
+
+void stage2ConfigureAndEnable(void);

thermosphere/src/platform/tegra/car.c

@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+#include "car.h"
+#include "../../utils.h"
+
+static uintptr_t g_carRegs;
+
+static inline volatile tegra_car_t *car_get_regs(void)
+{
+    return (volatile tegra_car_t *)g_carRegs;
+}
+
+static inline volatile uint32_t *car_reg_at(uint32_t offset)
+{
+    return (volatile uint32_t *)(g_carRegs + offset);
+}
+
+static inline uint32_t get_clk_source_reg(CarDevice dev) {
+    switch (dev) {
+        case CARDEVICE_UARTA: return 0x178;
+        case CARDEVICE_UARTB: return 0x17C;
+        case CARDEVICE_UARTC: return 0x1A0;
+        case CARDEVICE_UARTD: return 0x1C0;
+        case CARDEVICE_I2C1: return 0x124;
+        case CARDEVICE_I2C5: return 0x128;
+        case CARDEVICE_TZRAM: return 0;
+        case CARDEVICE_SE: return 0x42C;
+        case CARDEVICE_HOST1X: return 0x180;
+        case CARDEVICE_TSEC: return 0x1F4;
+        case CARDEVICE_SOR_SAFE: return 0;
+        case CARDEVICE_SOR0: return 0;
+        case CARDEVICE_SOR1: return 0x410;
+        case CARDEVICE_KFUSE: return 0;
+        case CARDEVICE_CL_DVFS: return 0;
+        case CARDEVICE_CORESIGHT: return 0x1D4;
+        case CARDEVICE_ACTMON: return 0x3E8;
+        case CARDEVICE_BPMP: return 0;
+        default: return 0;//generic_panic();
+    }
+} 
+
+static inline uint32_t get_clk_source_val(CarDevice dev) {
+    switch (dev) {
+        case CARDEVICE_UARTA: return 0;
+        case CARDEVICE_UARTB: return 0;
+        case CARDEVICE_UARTC: return 0;
+        case CARDEVICE_UARTD: return 0;
+        case CARDEVICE_I2C1: return 6;
+        case CARDEVICE_I2C5: return 6;
+        case CARDEVICE_TZRAM: return 0;
+        case CARDEVICE_SE: return 0;
+        case CARDEVICE_HOST1X: return 4;
+        case CARDEVICE_TSEC: return 0;
+        case CARDEVICE_SOR_SAFE: return 0;
+        case CARDEVICE_SOR0: return 0;
+        case CARDEVICE_SOR1: return 0;
+        case CARDEVICE_KFUSE: return 0;
+        case CARDEVICE_CL_DVFS: return 0;
+        case CARDEVICE_CORESIGHT: return 0;
+        case CARDEVICE_ACTMON: return 6;
+        case CARDEVICE_BPMP: return 0;
+        default: return 0;//generic_panic();
+    }
+}
+
+static inline uint32_t get_clk_source_div(CarDevice dev) {
+    switch (dev) {
+        case CARDEVICE_UARTA: return 0;
+        case CARDEVICE_UARTB: return 0;
+        case CARDEVICE_UARTC: return 0;
+        case CARDEVICE_UARTD: return 0;
+        case CARDEVICE_I2C1: return 0;
+        case CARDEVICE_I2C5: return 0;
+        case CARDEVICE_TZRAM: return 0;
+        case CARDEVICE_SE: return 0;
+        case CARDEVICE_HOST1X: return 3;
+        case CARDEVICE_TSEC: return 2;
+        case CARDEVICE_SOR_SAFE: return 0;
+        case CARDEVICE_SOR0: return 0;
+        case CARDEVICE_SOR1: return 2;
+        case CARDEVICE_KFUSE: return 0;
+        case CARDEVICE_CL_DVFS: return 0;
+        case CARDEVICE_CORESIGHT: return 4;
+        case CARDEVICE_ACTMON: return 0;
+        case CARDEVICE_BPMP: return 0;
+        default: return 0;//generic_panic();
+    }
+}
+
+static uint32_t g_clk_reg_offsets[NUM_CAR_BANKS] = {0x010, 0x014, 0x018, 0x360, 0x364, 0x280, 0x298};
+static uint32_t g_rst_reg_offsets[NUM_CAR_BANKS] = {0x004, 0x008, 0x00C, 0x358, 0x35C, 0x28C, 0x2A4}; 
+
+void car_set_regs(uintptr_t regs)
+{
+    g_carRegs = regs;
+}
+
+void clk_enable(CarDevice dev) {
+    uint32_t clk_source_reg;
+    if ((clk_source_reg = get_clk_source_reg(dev))) {
+        *car_reg_at(clk_source_reg) = (get_clk_source_val(dev) << 29) | get_clk_source_div(dev);
+    }
+    *car_reg_at(g_clk_reg_offsets[dev >> 5]) |= BIT(dev & 0x1F);
+}
+
+void clk_disable(CarDevice dev) {
+    *car_reg_at(g_clk_reg_offsets[dev >> 5]) &= ~(BIT(dev & 0x1F));
+}
+
+void rst_enable(CarDevice dev) {
+    *car_reg_at(g_rst_reg_offsets[dev >> 5]) |= BIT(dev & 0x1F);
+}
+
+void rst_disable(CarDevice dev) {
+    *car_reg_at(g_rst_reg_offsets[dev >> 5]) &= ~(BIT(dev & 0x1F));
+}
+
+void clkrst_enable(CarDevice dev) {
+    clk_enable(dev);
+    rst_disable(dev);
+}
+
+void clkrst_disable(CarDevice dev) {
+    rst_enable(dev);
+    clk_disable(dev);
+}
+
+void clkrst_reboot(CarDevice dev) {
+    clkrst_disable(dev);
+    if (dev == CARDEVICE_KFUSE) {
+        /* Workaround for KFUSE clock. */
+        /*clk_enable(dev);
+        udelay(100);
+        rst_disable(dev);
+        udelay(200);*/
+    } else {
+        clkrst_enable(dev);
+    }
+}
+
+void clkrst_enable_fuse_regs(bool enable) {
+    volatile tegra_car_t *car = car_get_regs();
+    car->misc_clk_enb = ((car->misc_clk_enb & 0xEFFFFFFF) | ((enable & 1) << 28));
+}

thermosphere/src/platform/tegra/car.h

@@ -0,0 +1,498 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+#pragma once
+#include "../../utils.h"
+
+#define MEMORY_MAP_PA_CAR           0x60006000ul
+
+#define CLK_L_SDMMC1    (1 << 14)
+#define CLK_L_SDMMC2    (1 << 9)
+#define CLK_U_SDMMC3    (1 << 5)
+#define CLK_L_SDMMC4    (1 << 15)
+
+#define CLK_SOURCE_MASK             (0b111 << 29)
+#define CLK_SOURCE_FIRST            (0b000 << 29)
+#define CLK_DIVIDER_MASK            (0xff << 0)
+#define CLK_DIVIDER_UNITY           (0x00 << 0)
+
+#define NUM_CAR_BANKS 7
+
+/* Clock and reset devices. */
+typedef enum {
+    CARDEVICE_UARTA = ((0 << 5) | 0x6),
+    CARDEVICE_UARTB = ((0 << 5) | 0x7),
+    CARDEVICE_UARTC = ((1 << 5) | 0x17),
+    CARDEVICE_UARTD = ((2 << 5) | 0x1),
+    CARDEVICE_I2C1 = ((0 << 5) | 0xC),
+    CARDEVICE_I2C5 = ((1 << 5) | 0xF),
+    CARDEVICE_TZRAM = ((3 << 5) | 0x1E),
+    CARDEVICE_SE = ((3 << 5) | 0x1F),
+    CARDEVICE_HOST1X = ((0 << 5) | 0x1C),
+    CARDEVICE_TSEC = ((2 << 5) | 0x13),
+    CARDEVICE_SOR_SAFE = ((6 << 5) | 0x1E),
+    CARDEVICE_SOR0 = ((5 << 5) | 0x16),
+    CARDEVICE_SOR1 = ((5 << 5) | 0x17),
+    CARDEVICE_KFUSE = ((1 << 5) | 0x8),
+    CARDEVICE_CL_DVFS = ((4 << 5) | 0x1B),
+    CARDEVICE_CORESIGHT = ((2 << 5) | 0x9),
+    CARDEVICE_ACTMON = ((3 << 5) | 0x17),
+    CARDEVICE_BPMP = ((0 << 5) | 0x1)
+} CarDevice;
+
+/* Clock/Reset Controller (CLK_RST_CONTROLLER_) regs */
+typedef struct {
+    uint32_t rst_src;           /* _RST_SOURCE_0,   0x00 */
+
+    /* _RST_DEVICES_L/H/U_0     0x4-0xc */
+    uint32_t rst_dev_l;
+    uint32_t rst_dev_h;
+    uint32_t rst_dev_u;
+
+    /* _CLK_OUT_ENB_L/H/U_0     0x10-0x18 */
+    uint32_t clk_out_enb_l;
+    uint32_t clk_out_enb_h;
+    uint32_t clk_out_enb_u;
+
+    uint32_t _0x1C;
+    uint32_t cclk_brst_pol;     /* _CCLK_BURST_POLICY_0,    0x20 */
+    uint32_t super_cclk_div;    /* _SUPER_CCLK_DIVIDER_0,   0x24 */
+    uint32_t sclk_brst_pol;     /* _SCLK_BURST_POLICY_0,    0x28 */
+    uint32_t super_sclk_div;    /* _SUPER_SCLK_DIVIDER_0,   0x2c */
+    uint32_t clk_sys_rate;      /* _CLK_SYSTEM_RATE_0,      0x30 */
+    uint32_t prog_dly_clk;      /* _PROG_DLY_CLK_0,         0x34 */
+    uint32_t aud_sync_clk_rate; /* _AUDIO_SYNC_CLK_RATE_0,  0x38 */
+    uint32_t _0x3C;
+    uint32_t cop_clk_skip_plcy; /* _COP_CLK_SKIP_POLICY_0,  0x40 */
+    uint32_t clk_mask_arm;      /* _CLK_MASK_ARM_0,         0x44 */
+    uint32_t misc_clk_enb;      /* _MISC_CLK_ENB_0,         0x48 */
+    uint32_t clk_cpu_cmplx;     /* _CLK_CPU_CMPLX_0,        0x4c */
+    uint32_t osc_ctrl;          /* _OSC_CTRL_0,             0x50 */
+    uint32_t pll_lfsr;          /* _PLL_LFSR_0,             0x54 */
+    uint32_t osc_freq_det;      /* _OSC_FREQ_DET_0,         0x58 */
+    uint32_t osc_freq_det_stat; /* _OSC_FREQ_DET_STATUS_0,  0x5c */
+    uint32_t _0x60[2];
+    uint32_t plle_ss_cntl;      /* _PLLE_SS_CNTL_0,         0x68 */
+    uint32_t plle_misc1;        /* _PLLE_MISC1_0,           0x6c */
+    uint32_t _0x70[4];
+
+    /* PLLC 0x80-0x8c */
+    uint32_t pllc_base;
+    uint32_t pllc_out;
+    uint32_t pllc_misc0;
+    uint32_t pllc_misc1;
+    
+    /* PLLM 0x90-0x9c */
+    uint32_t pllm_base;
+    uint32_t pllm_out;
+    uint32_t pllm_misc1;
+    uint32_t pllm_misc2;
+    
+    /* PLLP 0xa0-0xac */
+    uint32_t pllp_base;
+    uint32_t pllp_outa;
+    uint32_t pllp_outb;
+    uint32_t pllp_misc;
+    
+    /* PLLA 0xb0-0xbc */
+    uint32_t plla_base;
+    uint32_t plla_out;
+    uint32_t plla_misc0;
+    uint32_t plla_misc1;
+    
+    /* PLLU 0xc0-0xcc */
+    uint32_t pllu_base;
+    uint32_t pllu_out;
+    uint32_t pllu_misc1;
+    uint32_t pllu_misc2;
+    
+    /* PLLD 0xd0-0xdc */
+    uint32_t plld_base;
+    uint32_t plld_out;
+    uint32_t plld_misc1;
+    uint32_t plld_misc2;
+
+    /* PLLX 0xe0-0xe4 */
+    uint32_t pllx_base;
+    uint32_t pllx_misc;
+    
+    /* PLLE 0xe8-0xf4 */
+    uint32_t plle_base;
+    uint32_t plle_misc;
+    uint32_t plle_ss_cntl1;
+    uint32_t plle_ss_cntl2;
+    
+    uint32_t lvl2_clk_gate_ovra;            /* _LVL2_CLK_GATE_OVRA_0,   0xf8 */
+    uint32_t lvl2_clk_gate_ovrb;            /* _LVL2_CLK_GATE_OVRB_0,   0xfc */
+
+    uint32_t clk_source_i2s2;               /* _CLK_SOURCE_I2S2_0,      0x100 */
+    uint32_t clk_source_i2s3;               /* _CLK_SOURCE_I2S3_0,      0x104 */
+    uint32_t clk_source_spdif_out;          /* _CLK_SOURCE_SPDIF_OUT_0, 0x108 */
+    uint32_t clk_source_spdif_in;           /* _CLK_SOURCE_SPDIF_IN_0,  0x10c */
+    uint32_t clk_source_pwm;                /* _CLK_SOURCE_PWM_0,       0x110 */
+    uint32_t _0x114;
+    uint32_t clk_source_spi2;               /* _CLK_SOURCE_SPI2_0,      0x118 */
+    uint32_t clk_source_spi3;               /* _CLK_SOURCE_SPI3_0,      0x11c */
+    uint32_t _0x120;
+    uint32_t clk_source_i2c1;               /* _CLK_SOURCE_I2C1_0,      0x124 */
+    uint32_t clk_source_i2c5;               /* _CLK_SOURCE_I2C5_0,      0x128 */
+    uint32_t _0x12c[2];
+    uint32_t clk_source_spi1;               /* _CLK_SOURCE_SPI1_0,      0x134 */
+    uint32_t clk_source_disp1;              /* _CLK_SOURCE_DISP1_0,     0x138 */
+    uint32_t clk_source_disp2;              /* _CLK_SOURCE_DISP2_0,     0x13c */
+    uint32_t _0x140;
+    uint32_t clk_source_isp;                /* _CLK_SOURCE_ISP_0,       0x144 */
+    uint32_t clk_source_vi;                 /* _CLK_SOURCE_VI_0,        0x148 */
+    uint32_t _0x14c;
+    uint32_t clk_source_sdmmc1;             /* _CLK_SOURCE_SDMMC1_0,    0x150 */
+    uint32_t clk_source_sdmmc2;             /* _CLK_SOURCE_SDMMC2_0,    0x154 */
+    uint32_t _0x158[3];
+    uint32_t clk_source_sdmmc4;             /* _CLK_SOURCE_SDMMC4_0,    0x164 */
+    uint32_t _0x168[4];
+    uint32_t clk_source_uarta;              /* _CLK_SOURCE_UARTA_0,     0x178 */
+    uint32_t clk_source_uartb;              /* _CLK_SOURCE_UARTB_0,     0x17c */
+    uint32_t clk_source_host1x;             /* _CLK_SOURCE_HOST1X_0,    0x180 */
+    uint32_t _0x184[5];
+    uint32_t clk_source_i2c2;               /* _CLK_SOURCE_I2C2_0,      0x198 */
+    uint32_t clk_source_emc;                /* _CLK_SOURCE_EMC_0,       0x19c */
+    uint32_t clk_source_uartc;              /* _CLK_SOURCE_UARTC_0,     0x1a0 */
+    uint32_t _0x1a4;
+    uint32_t clk_source_vi_sensor;          /* _CLK_SOURCE_VI_SENSOR_0, 0x1a8 */
+    uint32_t _0x1ac[2];
+    uint32_t clk_source_spi4;               /* _CLK_SOURCE_SPI4_0,      0x1b4 */
+    uint32_t clk_source_i2c3;               /* _CLK_SOURCE_I2C3_0,      0x1b8 */
+    uint32_t clk_source_sdmmc3;             /* _CLK_SOURCE_SDMMC3_0,    0x1bc */
+    uint32_t clk_source_uartd;              /* _CLK_SOURCE_UARTD_0,     0x1c0 */
+    uint32_t _0x1c4[2];
+    uint32_t clk_source_owr;                /* _CLK_SOURCE_OWR_0,       0x1cc */
+    uint32_t _0x1d0;
+    uint32_t clk_source_csite;              /* _CLK_SOURCE_CSITE_0,     0x1d4 */
+    uint32_t clk_source_i2s1;               /* _CLK_SOURCE_I2S1_0,      0x1d8 */
+    uint32_t clk_source_dtv;                /* _CLK_SOURCE_DTV_0,       0x1dc */
+    uint32_t _0x1e0[5];
+    uint32_t clk_source_tsec;               /* _CLK_SOURCE_TSEC_0,      0x1f4 */
+    uint32_t _0x1f8;
+    
+    uint32_t clk_spare2;                    /* _CLK_SPARE2_0,       0x1fc */
+    uint32_t _0x200[32];
+
+    uint32_t clk_out_enb_x;                 /* _CLK_OUT_ENB_X_0,    0x280 */
+    uint32_t clk_enb_x_set;                 /* _CLK_ENB_X_SET_0,    0x284 */
+    uint32_t clk_enb_x_clr;                 /* _CLK_ENB_X_CLR_0,    0x288 */
+
+    uint32_t rst_devices_x;                 /* _RST_DEVICES_X_0,    0x28c */
+    uint32_t rst_dev_x_set;                 /* _RST_DEV_X_SET_0,    0x290 */
+    uint32_t rst_dev_x_clr;                 /* _RST_DEV_X_CLR_0,    0x294 */
+
+    uint32_t clk_out_enb_y;                 /* _CLK_OUT_ENB_Y_0,    0x298 */
+    uint32_t clk_enb_y_set;                 /* _CLK_ENB_Y_SET_0,    0x29c */
+    uint32_t clk_enb_y_clr;                 /* _CLK_ENB_Y_CLR_0,    0x2a0 */
+
+    uint32_t rst_devices_y;                 /* _RST_DEVICES_Y_0,    0x2a4 */
+    uint32_t rst_dev_y_set;                 /* _RST_DEV_Y_SET_0,    0x2a8 */
+    uint32_t rst_dev_y_clr;                 /* _RST_DEV_Y_CLR_0,    0x2ac */
+
+    uint32_t _0x2b0[17];
+    uint32_t dfll_base;                     /* _DFLL_BASE_0,        0x2f4 */
+    uint32_t _0x2f8[2];
+
+    /* _RST_DEV_L/H/U_SET_0 0x300-0x314 */
+    uint32_t rst_dev_l_set;
+    uint32_t rst_dev_l_clr;
+    uint32_t rst_dev_h_set;
+    uint32_t rst_dev_h_clr;
+    uint32_t rst_dev_u_set;
+    uint32_t rst_dev_u_clr;
+
+    uint32_t _0x318[2];
+
+    /* _CLK_ENB_L/H/U_CLR_0 0x320-0x334 */
+    uint32_t clk_enb_l_set;
+    uint32_t clk_enb_l_clr;
+    uint32_t clk_enb_h_set;
+    uint32_t clk_enb_h_clr;
+    uint32_t clk_enb_u_set;
+    uint32_t clk_enb_u_clr;
+
+    uint32_t _0x338;
+    uint32_t ccplex_pg_sm_ovrd;         /* _CCPLEX_PG_SM_OVRD_0,    0x33c */
+    uint32_t rst_cpu_cmplx_set;         /* _RST_CPU_CMPLX_SET_0,    0x340 */
+    uint32_t rst_cpu_cmplx_clr;         /* _RST_CPU_CMPLX_CLR_0,    0x344 */
+
+    /* Additional (T30) registers */
+    uint32_t clk_cpu_cmplx_set;         /* _CLK_CPU_CMPLX_SET_0,    0x348 */
+    uint32_t clk_cpu_cmplx_clr;         /* _CLK_CPU_CMPLX_SET_0,    0x34c */
+
+    uint32_t _0x350[2];
+    uint32_t rst_dev_v;                 /* _RST_DEVICES_V_0,        0x358 */
+    uint32_t rst_dev_w;                 /* _RST_DEVICES_W_0,        0x35c */
+    uint32_t clk_out_enb_v;             /* _CLK_OUT_ENB_V_0,        0x360 */
+    uint32_t clk_out_enb_w;             /* _CLK_OUT_ENB_W_0,        0x364 */
+    uint32_t cclkg_brst_pol;            /* _CCLKG_BURST_POLICY_0,   0x368 */
+    uint32_t super_cclkg_div;           /* _SUPER_CCLKG_DIVIDER_0,  0x36c */
+    uint32_t cclklp_brst_pol;           /* _CCLKLP_BURST_POLICY_0,  0x370 */
+    uint32_t super_cclkp_div;           /* _SUPER_CCLKLP_DIVIDER_0, 0x374 */
+    uint32_t clk_cpug_cmplx;            /* _CLK_CPUG_CMPLX_0,       0x378 */
+    uint32_t clk_cpulp_cmplx;           /* _CLK_CPULP_CMPLX_0,      0x37c */
+    uint32_t cpu_softrst_ctrl;          /* _CPU_SOFTRST_CTRL_0,     0x380 */
+    uint32_t cpu_softrst_ctrl1;         /* _CPU_SOFTRST_CTRL1_0,    0x384 */
+    uint32_t cpu_softrst_ctrl2;         /* _CPU_SOFTRST_CTRL2_0,    0x388 */
+    uint32_t _0x38c[5];
+    uint32_t lvl2_clk_gate_ovrc;        /* _LVL2_CLK_GATE_OVRC,     0x3a0 */
+    uint32_t lvl2_clk_gate_ovrd;        /* _LVL2_CLK_GATE_OVRD,     0x3a4 */
+    uint32_t _0x3a8[2];
+    
+    uint32_t _0x3b0;
+    uint32_t clk_source_mselect;        /* _CLK_SOURCE_MSELECT_0,       0x3b4 */
+    uint32_t clk_source_tsensor;        /* _CLK_SOURCE_TSENSOR_0,       0x3b8 */
+    uint32_t clk_source_i2s4;           /* _CLK_SOURCE_I2S4_0,          0x3bc */
+    uint32_t clk_source_i2s5;           /* _CLK_SOURCE_I2S5_0,          0x3c0 */
+    uint32_t clk_source_i2c4;           /* _CLK_SOURCE_I2C4_0,          0x3c4 */
+    uint32_t _0x3c8[2];
+    uint32_t clk_source_ahub;           /* _CLK_SOURCE_AHUB_0,          0x3d0 */
+    uint32_t _0x3d4[4];
+    uint32_t clk_source_hda2codec_2x;   /* _CLK_SOURCE_HDA2CODEC_2X_0,  0x3e4 */
+    uint32_t clk_source_actmon;         /* _CLK_SOURCE_ACTMON_0,        0x3e8 */
+    uint32_t clk_source_extperiph1;     /* _CLK_SOURCE_EXTPERIPH1_0,    0x3ec */
+    uint32_t clk_source_extperiph2;     /* _CLK_SOURCE_EXTPERIPH2_0,    0x3f0 */
+    uint32_t clk_source_extperiph3;     /* _CLK_SOURCE_EXTPERIPH3_0,    0x3f4 */
+    uint32_t _0x3f8;
+    uint32_t clk_source_i2c_slow;       /* _CLK_SOURCE_I2C_SLOW_0,      0x3fc */
+    uint32_t clk_source_sys;            /* _CLK_SOURCE_SYS_0,           0x400 */
+    uint32_t clk_source_ispb;           /* _CLK_SOURCE_ISPB_0,          0x404 */
+    uint32_t _0x408[2];
+    uint32_t clk_source_sor1;           /* _CLK_SOURCE_SOR1_0,          0x410 */
+    uint32_t clk_source_sor0;           /* _CLK_SOURCE_SOR0_0,          0x414 */
+    uint32_t _0x418[2];
+    uint32_t clk_source_sata_oob;       /* _CLK_SOURCE_SATA_OOB_0,      0x420 */
+    uint32_t clk_source_sata;           /* _CLK_SOURCE_SATA_0,          0x424 */
+    uint32_t clk_source_hda;            /* _CLK_SOURCE_HDA_0,           0x428 */
+    uint32_t _0x42c;
+
+    /* _RST_DEV_V/W_SET_0 0x430-0x43c */
+    uint32_t rst_dev_v_set;
+    uint32_t rst_dev_v_clr;
+    uint32_t rst_dev_w_set;
+    uint32_t rst_dev_w_clr;
+
+    /* _CLK_ENB_V/W_CLR_0 0x440-0x44c */
+    uint32_t clk_enb_v_set;
+    uint32_t clk_enb_v_clr;
+    uint32_t clk_enb_w_set;
+    uint32_t clk_enb_w_clr;
+
+    /* Additional (T114+) registers */
+    uint32_t rst_cpug_cmplx_set;    /* _RST_CPUG_CMPLX_SET_0,   0x450 */
+    uint32_t rst_cpug_cmplx_clr;    /* _RST_CPUG_CMPLX_CLR_0,   0x454 */
+    uint32_t rst_cpulp_cmplx_set;   /* _RST_CPULP_CMPLX_SET_0,  0x458 */
+    uint32_t rst_cpulp_cmplx_clr;   /* _RST_CPULP_CMPLX_CLR_0,  0x45c */
+    uint32_t clk_cpug_cmplx_set;    /* _CLK_CPUG_CMPLX_SET_0,   0x460 */
+    uint32_t clk_cpug_cmplx_clr;    /* _CLK_CPUG_CMPLX_CLR_0,   0x464 */
+    uint32_t clk_cpulp_cmplx_set;   /* _CLK_CPULP_CMPLX_SET_0,  0x468 */
+    uint32_t clk_cpulp_cmplx_clr;   /* _CLK_CPULP_CMPLX_CLR_0,  0x46c */
+    uint32_t cpu_cmplx_status;      /* _CPU_CMPLX_STATUS_0,     0x470 */
+    uint32_t _0x474;
+    uint32_t intstatus;             /* _INTSTATUS_0,            0x478 */
+    uint32_t intmask;               /* _INTMASK_0,              0x47c */
+    uint32_t utmip_pll_cfg0;        /* _UTMIP_PLL_CFG0_0,       0x480 */
+    uint32_t utmip_pll_cfg1;        /* _UTMIP_PLL_CFG1_0,       0x484 */
+    uint32_t utmip_pll_cfg2;        /* _UTMIP_PLL_CFG2_0,       0x488 */
+
+    uint32_t plle_aux;              /* _PLLE_AUX_0,             0x48c */
+    uint32_t sata_pll_cfg0;         /* _SATA_PLL_CFG0_0,        0x490 */
+    uint32_t sata_pll_cfg1;         /* _SATA_PLL_CFG1_0,        0x494 */
+    uint32_t pcie_pll_cfg0;         /* _PCIE_PLL_CFG0_0,        0x498 */
+
+    uint32_t prog_audio_dly_clk;    /* _PROG_AUDIO_DLY_CLK_0,   0x49c */
+    uint32_t audio_sync_clk_i2s0;   /* _AUDIO_SYNC_CLK_I2S0_0,  0x4a0 */
+    uint32_t audio_sync_clk_i2s1;   /* _AUDIO_SYNC_CLK_I2S1_0,  0x4a4 */
+    uint32_t audio_sync_clk_i2s2;   /* _AUDIO_SYNC_CLK_I2S2_0,  0x4a8 */
+    uint32_t audio_sync_clk_i2s3;   /* _AUDIO_SYNC_CLK_I2S3_0,  0x4ac */
+    uint32_t audio_sync_clk_i2s4;   /* _AUDIO_SYNC_CLK_I2S4_0,  0x4b0 */
+    uint32_t audio_sync_clk_spdif;  /* _AUDIO_SYNC_CLK_SPDIF_0, 0x4b4 */
+
+    uint32_t plld2_base;            /* _PLLD2_BASE_0, 0x4b8 */
+    uint32_t plld2_misc;            /* _PLLD2_MISC_0, 0x4bc */
+    uint32_t utmip_pll_cfg3;        /* _UTMIP_PLL_CFG3_0, 0x4c0 */
+    uint32_t pllrefe_base;          /* _PLLREFE_BASE_0, 0x4c4 */
+    uint32_t pllrefe_misc;          /* _PLLREFE_MISC_0, 0x4c8 */
+    uint32_t pllrefe_out;           /* _PLLREFE_OUT_0, 0x4cc */
+    uint32_t cpu_finetrim_byp;      /* _CPU_FINETRIM_BYP_0, 0x4d0 */
+    uint32_t cpu_finetrim_select;   /* _CPU_FINETRIM_SELECT_0, 0x4d4 */
+    uint32_t cpu_finetrim_dr;       /* _CPU_FINETRIM_DR_0, 0x4d8 */
+    uint32_t cpu_finetrim_df;       /* _CPU_FINETRIM_DF_0, 0x4dc */
+    uint32_t cpu_finetrim_f;        /* _CPU_FINETRIM_F_0, 0x4e0 */
+    uint32_t cpu_finetrim_r;        /* _CPU_FINETRIM_R_0, 0x4e4 */
+    uint32_t pllc2_base;            /* _PLLC2_BASE_0, 0x4e8 */
+    uint32_t pllc2_misc0;           /* _PLLC2_MISC_0_0, 0x4ec */
+    uint32_t pllc2_misc1;           /* _PLLC2_MISC_1_0, 0x4f0 */
+    uint32_t pllc2_misc2;           /* _PLLC2_MISC_2_0, 0x4f4 */
+    uint32_t pllc2_misc3;           /* _PLLC2_MISC_3_0, 0x4f8 */
+    uint32_t pllc3_base;            /* _PLLC3_BASE_0, 0x4fc */
+    uint32_t pllc3_misc0;           /* _PLLC3_MISC_0_0, 0x500 */
+    uint32_t pllc3_misc1;           /* _PLLC3_MISC_1_0, 0x504 */
+    uint32_t pllc3_misc2;           /* _PLLC3_MISC_2_0, 0x508 */
+    uint32_t pllc3_misc3;           /* _PLLC3_MISC_3_0, 0x50c */
+    uint32_t pllx_misc1;            /* _PLLX_MISC_1_0, 0x510 */
+    uint32_t pllx_misc2;            /* _PLLX_MISC_2_0, 0x514 */
+    uint32_t pllx_misc3;            /* _PLLX_MISC_3_0, 0x518 */
+    uint32_t xusbio_pll_cfg0;       /* _XUSBIO_PLL_CFG0_0, 0x51c */
+    uint32_t xusbio_pll_cfg1;       /* _XUSBIO_PLL_CFG0_1, 0x520 */
+    uint32_t plle_aux1;             /* _PLLE_AUX1_0, 0x524 */
+    uint32_t pllp_reshift;          /* _PLLP_RESHIFT_0, 0x528 */
+    uint32_t utmipll_hw_pwrdn_cfg0; /* _UTMIPLL_HW_PWRDN_CFG0_0, 0x52c */
+    uint32_t pllu_hw_pwrdn_cfg0;    /* _PLLU_HW_PWRDN_CFG0_0, 0x530 */
+    uint32_t xusb_pll_cfg0;         /* _XUSB_PLL_CFG0_0, 0x534 */
+    uint32_t _0x538;
+    uint32_t clk_cpu_misc;          /* _CLK_CPU_MISC_0, 0x53c */
+    uint32_t clk_cpug_misc;         /* _CLK_CPUG_MISC_0, 0x540 */
+    uint32_t clk_cpulp_misc;        /* _CLK_CPULP_MISC_0, 0x544 */
+    uint32_t pllx_hw_ctrl_cfg;      /* _PLLX_HW_CTRL_CFG_0, 0x548 */
+    uint32_t pllx_sw_ramp_cfg;      /* _PLLX_SW_RAMP_CFG_0, 0x54c */
+    uint32_t pllx_hw_ctrl_status;   /* _PLLX_HW_CTRL_STATUS_0, 0x550 */
+    uint32_t lvl2_clk_gate_ovre;    /* _LVL2_CLK_GATE_OVRE, 0x554 */
+    uint32_t super_gr3d_clk_div;    /* _SUPER_GR3D_CLK_DIVIDER_0, 0x558 */
+    uint32_t spare_reg0;            /* _SPARE_REG0_0, 0x55c */
+    uint32_t audio_sync_clk_dmic1;  /* _AUDIO_SYNC_CLK_DMIC1_0, 0x560 */
+    uint32_t audio_sync_clk_dmic2;  /* _AUDIO_SYNC_CLK_DMIC2_0, 0x564 */
+    
+    uint32_t _0x568[2];
+    uint32_t plld2_ss_cfg;          /* _PLLD2_SS_CFG,           0x570 */
+    uint32_t plld2_ss_ctrl1;        /* _PLLD2_SS_CTRL1_0,       0x574 */
+    uint32_t plld2_ss_ctrl2;        /* _PLLD2_SS_CTRL2_0,       0x578 */
+    uint32_t _0x57c[5];
+    
+    uint32_t plldp_base;            /* _PLLDP_BASE, 0x590*/
+    uint32_t plldp_misc;            /* _PLLDP_MISC, 0x594 */
+    uint32_t plldp_ss_cfg;          /* _PLLDP_SS_CFG, 0x598 */
+    uint32_t plldp_ss_ctrl1;        /* _PLLDP_SS_CTRL1_0, 0x59c */
+    uint32_t plldp_ss_ctrl2;        /* _PLLDP_SS_CTRL2_0, 0x5a0 */
+    uint32_t pllc4_base;            /* _PLLC4_BASE_0, 0x5a4 */
+    uint32_t pllc4_misc;            /* _PLLC4_MISC_0, 0x5a8 */
+    uint32_t _0x5ac[6];
+    uint32_t clk_spare0;            /* _CLK_SPARE0_0, 0x5c4 */
+    uint32_t clk_spare1;            /* _CLK_SPARE1_0, 0x5c8 */
+    uint32_t gpu_isob_ctrl;         /* _GPU_ISOB_CTRL_0, 0x5cc */
+    uint32_t pllc_misc2;            /* _PLLC_MISC_2_0, 0x5d0 */
+    uint32_t pllc_misc3;            /* _PLLC_MISC_3_0, 0x5d4 */
+    uint32_t plla_misc2;            /* _PLLA_MISC2_0, 0x5d8 */
+    uint32_t _0x5dc[2];
+    uint32_t pllc4_out;             /* _PLLC4_OUT_0, 0x5e4 */
+    uint32_t pllmb_base;            /* _PLLMB_BASE_0, 0x5e8 */
+    uint32_t pllmb_misc1;           /* _PLLMB_MISC1_0, 0x5ec */
+    uint32_t pllx_misc4;            /* _PLLX_MISC_4_0, 0x5f0 */
+    uint32_t pllx_misc5;            /* _PLLX_MISC_5_0, 0x5f4 */
+    uint32_t _0x5f8[2];
+    
+    uint32_t clk_source_xusb_core_host;             /* _CLK_SOURCE_XUSB_CORE_HOST_0,    0x600 */
+    uint32_t clk_source_xusb_falcon;                /* _CLK_SOURCE_XUSB_FALCON_0,    0x604 */
+    uint32_t clk_source_xusb_fs;                    /* _CLK_SOURCE_XUSB_FS_0,    0x608 */
+    uint32_t clk_source_xusb_core_dev;              /* _CLK_SOURCE_XUSB_CORE_DEV_0,    0x60c */
+    uint32_t clk_source_xusb_ss;                    /* _CLK_SOURCE_XUSB_SS_0,    0x610 */
+    uint32_t clk_source_cilab;                      /* _CLK_SOURCE_CILAB_0,    0x614 */
+    uint32_t clk_source_cilcd;                      /* _CLK_SOURCE_CILCD_0,    0x618 */
+    uint32_t clk_source_cilef;                      /* _CLK_SOURCE_CILEF_0,    0x61c */
+    uint32_t clk_source_dsia_lp;                    /* _CLK_SOURCE_DSIA_LP_0,    0x620 */
+    uint32_t clk_source_dsib_lp;                    /* _CLK_SOURCE_DSIB_LP_0,    0x624 */
+    uint32_t clk_source_entropy;                    /* _CLK_SOURCE_ENTROPY_0,    0x628 */
+    uint32_t clk_source_dvfs_ref;                   /* _CLK_SOURCE_DVFS_REF_0,    0x62c */
+    uint32_t clk_source_dvfs_soc;                   /* _CLK_SOURCE_DVFS_SOC_0,    0x630 */
+    uint32_t _0x634[3];
+    uint32_t clk_source_emc_latency;                /* _CLK_SOURCE_EMC_LATENCY_0,    0x640 */
+    uint32_t clk_source_soc_therm;                  /* _CLK_SOURCE_SOC_THERM_0,    0x644 */
+    uint32_t _0x648;
+    uint32_t clk_source_dmic1;                      /* _CLK_SOURCE_DMIC1_0,    0x64c */
+    uint32_t clk_source_dmic2;                      /* _CLK_SOURCE_DMIC2_0,    0x650 */
+    uint32_t _0x654;
+    uint32_t clk_source_vi_sensor2;                 /* _CLK_SOURCE_VI_SENSOR2_0,    0x658 */
+    uint32_t clk_source_i2c6;                       /* _CLK_SOURCE_I2C6_0,    0x65c */
+    uint32_t clk_source_mipibif;                    /* _CLK_SOURCE_MIPIBIF_0,    0x660 */
+    uint32_t clk_source_emc_dll;                    /* _CLK_SOURCE_EMC_DLL_0,    0x664 */
+    uint32_t _0x668;
+    uint32_t clk_source_uart_fst_mipi_cal;          /* _CLK_SOURCE_UART_FST_MIPI_CAL_0,    0x66c */
+    uint32_t _0x670[2];
+    uint32_t clk_source_vic;                        /* _CLK_SOURCE_VIC_0,    0x678 */
+    
+    uint32_t pllp_outc;                             /* _PLLP_OUTC_0, 0x67c */
+    uint32_t pllp_misc1;                            /* _PLLP_MISC1_0, 0x680 */
+    uint32_t _0x684[2];
+    uint32_t emc_div_clk_shaper_ctrl;               /* _EMC_DIV_CLK_SHAPER_CTRL_0, 0x68c */
+    uint32_t emc_pllc_shaper_ctrl;                  /* _EMC_PLLC_SHAPER_CTRL_0, 0x690 */
+
+    uint32_t clk_source_sdmmc_legacy_tm;            /* _CLK_SOURCE_SDMMC_LEGACY_TM_0,    0x694 */
+    uint32_t clk_source_nvdec;                      /* _CLK_SOURCE_NVDEC_0,    0x698 */
+    uint32_t clk_source_nvjpg;                      /* _CLK_SOURCE_NVJPG_0,    0x69c */
+    uint32_t clk_source_nvenc;                      /* _CLK_SOURCE_NVENC_0,    0x6a0 */
+    
+    uint32_t plla1_base;                            /* _PLLA1_BASE_0, 0x6a4 */
+    uint32_t plla1_misc0;                           /* _PLLA1_MISC_0_0, 0x6a8 */
+    uint32_t plla1_misc1;                           /* _PLLA1_MISC_1_0, 0x6ac */
+    uint32_t plla1_misc2;                           /* _PLLA1_MISC_2_0, 0x6b0 */
+    uint32_t plla1_misc3;                           /* _PLLA1_MISC_3_0, 0x6b4 */
+    uint32_t audio_sync_clk_dmic3;                  /* _AUDIO_SYNC_CLK_DMIC3_0, 0x6b8 */
+    
+    uint32_t clk_source_dmic3;                      /* _CLK_SOURCE_DMIC3_0,    0x6bc */
+    uint32_t clk_source_ape;                        /* _CLK_SOURCE_APE_0,    0x6c0 */
+    uint32_t clk_source_qspi;                       /* _CLK_SOURCE_QSPI_0,    0x6c4 */
+    uint32_t clk_source_vi_i2c;                     /* _CLK_SOURCE_VI_I2C_0,    0x6c8 */
+    uint32_t clk_source_usb2_hsic_trk;              /* _CLK_SOURCE_USB2_HSIC_TRK_0,    0x6cc */
+    uint32_t clk_source_pex_sata_usb_rx_byp;        /* _CLK_SOURCE_PEX_SATA_USB_RX_BYP_0,    0x6d0 */
+    uint32_t clk_source_maud;                       /* _CLK_SOURCE_MAUD_0,    0x6d4 */
+    uint32_t clk_source_tsecb;                      /* _CLK_SOURCE_TSECB_0,    0x6d8 */
+    
+    uint32_t clk_cpug_misc1;                        /* _CLK_CPUG_MISC1_0, 0x6dc */
+    uint32_t aclk_burst_policy;                     /* _ACLK_BURST_POLICY_0, 0x6e0 */
+    uint32_t super_aclk_divider;                    /* _SUPER_ACLK_DIVIDER_0, 0x6e4 */
+    
+    uint32_t nvenc_super_clk_divider;               /* _NVENC_SUPER_CLK_DIVIDER_0, 0x6e8 */
+    uint32_t vi_super_clk_divider;                  /* _VI_SUPER_CLK_DIVIDER_0, 0x6ec */
+    uint32_t vic_super_clk_divider;                 /* _VIC_SUPER_CLK_DIVIDER_0, 0x6f0 */
+    uint32_t nvdec_super_clk_divider;               /* _NVDEC_SUPER_CLK_DIVIDER_0, 0x6f4 */
+    uint32_t isp_super_clk_divider;                 /* _ISP_SUPER_CLK_DIVIDER_0, 0x6f8 */
+    uint32_t ispb_super_clk_divider;                /* _ISPB_SUPER_CLK_DIVIDER_0, 0x6fc */
+    uint32_t nvjpg_super_clk_divider;               /* _NVJPG_SUPER_CLK_DIVIDER_0, 0x700 */
+    uint32_t se_super_clk_divider;                  /* _SE_SUPER_CLK_DIVIDER_0, 0x704 */
+    uint32_t tsec_super_clk_divider;                /* _TSEC_SUPER_CLK_DIVIDER_0, 0x708 */
+    uint32_t tsecb_super_clk_divider;               /* _TSECB_SUPER_CLK_DIVIDER_0, 0x70c */
+    
+    uint32_t clk_source_uartape;                    /* _CLK_SOURCE_UARTAPE_0, 0x710 */
+    uint32_t clk_cpug_misc2;                        /* _CLK_CPUG_MISC2_0, 0x714 */
+    uint32_t clk_source_dbgapb;                     /* _CLK_SOURCE_DBGAPB_0, 0x718 */
+    uint32_t clk_ccplex_cc4_ret_clk_enb;            /* _CLK_CCPLEX_CC4_RET_CLK_ENB_0, 0x71c */
+    uint32_t actmon_cpu_clk;                        /* _ACTMON_CPU_CLK_0, 0x720 */
+    uint32_t clk_source_emc_safe;                   /* _CLK_SOURCE_EMC_SAFE_0, 0x724 */
+    uint32_t sdmmc2_pllc4_out0_shaper_ctrl;         /* _SDMMC2_PLLC4_OUT0_SHAPER_CTRL_0, 0x728 */
+    uint32_t sdmmc2_pllc4_out1_shaper_ctrl;         /* _SDMMC2_PLLC4_OUT1_SHAPER_CTRL_0, 0x72c */
+    uint32_t sdmmc2_pllc4_out2_shaper_ctrl;         /* _SDMMC2_PLLC4_OUT2_SHAPER_CTRL_0, 0x730 */
+    uint32_t sdmmc2_div_clk_shaper_ctrl;            /* _SDMMC2_DIV_CLK_SHAPER_CTRL_0, 0x734 */
+    uint32_t sdmmc4_pllc4_out0_shaper_ctrl;         /* _SDMMC4_PLLC4_OUT0_SHAPER_CTRL_0, 0x738 */
+    uint32_t sdmmc4_pllc4_out1_shaper_ctrl;         /* _SDMMC4_PLLC4_OUT1_SHAPER_CTRL_0, 0x73c */
+    uint32_t sdmmc4_pllc4_out2_shaper_ctrl;         /* _SDMMC4_PLLC4_OUT2_SHAPER_CTRL_0, 0x740 */
+    uint32_t sdmmc4_div_clk_shaper_ctrl;            /* _SDMMC4_DIV_CLK_SHAPER_CTRL_0, 0x744 */    
+} tegra_car_t;
+
+void car_set_regs(uintptr_t regs);
+
+void clk_enable(CarDevice dev);
+void clk_disable(CarDevice dev);
+void rst_enable(CarDevice dev);
+void rst_disable(CarDevice dev);
+
+void clkrst_enable(CarDevice dev);
+void clkrst_disable(CarDevice dev);
+void clkrst_reboot(CarDevice dev);
+
+void clkrst_enable_fuse_regs(bool enable);

thermosphere/src/platform/tegra/devices.c

@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "devices.h"
+#include "../../memory_map.h"
+#include "../../utils.h"
+
+#include "uart.h"
+#include "car.h"
+#include "gpio.h"
+#include "pinmux.h"
+
+void devicesMapAllExtra(void)
+{
+    uartSetRegisterBase(memoryMapPlatformMmio(MEMORY_MAP_PA_UART, 0x1000));
+
+    car_set_regs(memoryMapPlatformMmio(MEMORY_MAP_PA_CAR, 0x1000));
+    gpio_set_regs(memoryMapPlatformMmio(MEMORY_MAP_PA_GPIO, 0x1000));
+    pinmux_set_regs(memoryMapPlatformMmio(MEMORY_MAP_PA_PINMUX, 0x1000));
+
+    // Don't broadcast, since it's only ran once per boot by only one core, before the others are started...
+    __tlb_invalidate_el2_local();
+    __dsb_local();
+    __isb();
+}

thermosphere/src/platform/tegra/devices.h

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void devicesMapAllExtra(void);

thermosphere/src/platform/tegra/gpio.c

@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+#include <stdint.h>
+
+#include "gpio.h"
+#include "../../utils.h"
+
+static uintptr_t g_gpioRegs;
+
+static inline volatile tegra_gpio_t *gpio_get_regs(void)
+{
+    return (volatile tegra_gpio_t *)g_gpioRegs;
+}
+
+static volatile tegra_gpio_bank_t *gpio_get_bank(uint32_t pin) {
+    volatile tegra_gpio_t *gpio = gpio_get_regs();
+    uint32_t bank_number = (pin >> GPIO_BANK_SHIFT);
+
+    return &gpio->bank[bank_number];
+}
+
+static volatile uint32_t gpio_get_port(uint32_t pin) {
+    return ((pin >> GPIO_PORT_SHIFT) & GPIO_PORT_MASK);
+}
+
+static volatile uint32_t gpio_get_mask(uint32_t pin) {
+    uint32_t pin_number = (pin & GPIO_PIN_MASK);
+    return (1 << pin_number);
+}
+
+static void gpio_simple_register_set(uint32_t pin, bool should_be_set, uint32_t offset) {
+    /* Retrieve the register set that corresponds to the given pin and offset. */
+    uintptr_t cluster_addr = (uintptr_t)gpio_get_bank(pin) + offset;
+    uint32_t *cluster = (uint32_t *)cluster_addr;
+
+    /* Figure out the offset into the cluster, and the mask to be used. */
+    uint32_t port = gpio_get_port(pin);
+    uint32_t mask = gpio_get_mask(pin);
+
+    /* Set or clear the bit, as appropriate. */
+    if (should_be_set)
+        cluster[port] |= mask;
+    else
+        cluster[port] &= ~mask;
+    
+    /* Dummy read. */
+    (void)cluster[port];
+}
+
+static bool gpio_simple_register_get(uint32_t pin, uint32_t offset) {
+    /* Retrieve the register set that corresponds to the given pin and offset. */
+    uintptr_t cluster_addr = (uintptr_t)gpio_get_bank(pin) + offset;
+    uint32_t *cluster = (uint32_t *)cluster_addr;
+
+    /* Figure out the offset into the cluster, and the mask to be used. */
+    uint32_t port = gpio_get_port(pin);
+    uint32_t mask = gpio_get_mask(pin);
+
+    /* Convert the given value to a boolean. */
+    return !!(cluster[port] & mask);
+}
+
+void gpio_set_regs(uintptr_t regs)
+{
+    g_gpioRegs = regs;
+}
+
+void gpio_configure_mode(uint32_t pin, uint32_t mode) {
+    gpio_simple_register_set(pin, mode == GPIO_MODE_GPIO, offsetof(tegra_gpio_bank_t, config));
+}
+
+void gpio_configure_direction(uint32_t pin, uint32_t dir) {
+    gpio_simple_register_set(pin, dir == GPIO_DIRECTION_OUTPUT, offsetof(tegra_gpio_bank_t, direction)); 
+}
+
+void gpio_write(uint32_t pin, uint32_t value) {
+    gpio_simple_register_set(pin, value == GPIO_LEVEL_HIGH, offsetof(tegra_gpio_bank_t, out)); 
+}
+
+uint32_t gpio_read(uint32_t pin) {
+    return gpio_simple_register_get(pin, offsetof(tegra_gpio_bank_t, in)); 
+}

thermosphere/src/platform/tegra/gpio.h

@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+#pragma once
+#include <stdint.h>
+
+#define MEMORY_MAP_PA_GPIO      0x6000D000ul
+
+#define TEGRA_GPIO_PORTS        4
+#define TEGRA_GPIO_BANKS        8
+#define GPIO_BANK_SHIFT         5
+#define GPIO_PORT_SHIFT         3
+#define GPIO_PORT_MASK          0x03
+#define GPIO_PIN_MASK           0x07
+
+typedef enum {
+    TEGRA_GPIO_PORT_A = 0,
+    TEGRA_GPIO_PORT_B = 1,
+    TEGRA_GPIO_PORT_C = 2,
+    TEGRA_GPIO_PORT_D = 3,
+    TEGRA_GPIO_PORT_E = 4,
+    TEGRA_GPIO_PORT_F = 5,
+    TEGRA_GPIO_PORT_G = 6,
+    TEGRA_GPIO_PORT_H = 7,
+    TEGRA_GPIO_PORT_I = 8,
+    TEGRA_GPIO_PORT_J = 9,
+    TEGRA_GPIO_PORT_K = 10,
+    TEGRA_GPIO_PORT_L = 11,
+    TEGRA_GPIO_PORT_M = 12,
+    TEGRA_GPIO_PORT_N = 13,
+    TEGRA_GPIO_PORT_O = 14,
+    TEGRA_GPIO_PORT_P = 15,
+    TEGRA_GPIO_PORT_Q = 16,
+    TEGRA_GPIO_PORT_R = 17,
+    TEGRA_GPIO_PORT_S = 18,
+    TEGRA_GPIO_PORT_T = 19,
+    TEGRA_GPIO_PORT_U = 20,
+    TEGRA_GPIO_PORT_V = 21,
+    TEGRA_GPIO_PORT_W = 22,
+    TEGRA_GPIO_PORT_X = 23,
+    TEGRA_GPIO_PORT_Y = 24,
+    TEGRA_GPIO_PORT_Z = 25,
+    TEGRA_GPIO_PORT_AA = 26,
+    TEGRA_GPIO_PORT_BB = 27,
+    TEGRA_GPIO_PORT_CC = 28,
+    TEGRA_GPIO_PORT_DD = 29,
+    TEGRA_GPIO_PORT_EE = 30,
+    TEGRA_GPIO_PORT_FF = 31,
+} tegra_gpio_port;
+
+typedef struct {
+    uint32_t config[TEGRA_GPIO_PORTS];
+    uint32_t direction[TEGRA_GPIO_PORTS];
+    uint32_t out[TEGRA_GPIO_PORTS];
+    uint32_t in[TEGRA_GPIO_PORTS];
+    uint32_t int_status[TEGRA_GPIO_PORTS];
+    uint32_t int_enable[TEGRA_GPIO_PORTS];
+    uint32_t int_level[TEGRA_GPIO_PORTS];
+    uint32_t int_clear[TEGRA_GPIO_PORTS];
+    uint32_t masked_config[TEGRA_GPIO_PORTS];
+    uint32_t masked_dir_out[TEGRA_GPIO_PORTS];
+    uint32_t masked_out[TEGRA_GPIO_PORTS];
+    uint32_t masked_in[TEGRA_GPIO_PORTS];
+    uint32_t masked_int_status[TEGRA_GPIO_PORTS];
+    uint32_t masked_int_enable[TEGRA_GPIO_PORTS];
+    uint32_t masked_int_level[TEGRA_GPIO_PORTS];
+    uint32_t masked_int_clear[TEGRA_GPIO_PORTS];
+} tegra_gpio_bank_t;
+
+typedef struct {
+    tegra_gpio_bank_t bank[TEGRA_GPIO_BANKS];
+} tegra_gpio_t;
+
+#define TEGRA_GPIO(port, offset) \
+    ((TEGRA_GPIO_PORT_##port * 8) + offset)
+
+/* Mode select */
+#define GPIO_MODE_SFIO          0
+#define GPIO_MODE_GPIO          1
+
+/* Direction */
+#define GPIO_DIRECTION_INPUT    0
+#define GPIO_DIRECTION_OUTPUT   1
+
+/* Level */
+#define GPIO_LEVEL_LOW          0
+#define GPIO_LEVEL_HIGH         1
+
+/* Named GPIOs */
+#define GPIO_BUTTON_VOL_DOWN            TEGRA_GPIO(X, 7)
+#define GPIO_BUTTON_VOL_UP              TEGRA_GPIO(X, 6)
+#define GPIO_MICROSD_CARD_DETECT        TEGRA_GPIO(Z, 1)
+#define GPIO_MICROSD_WRITE_PROTECT      TEGRA_GPIO(Z, 4)
+#define GPIO_MICROSD_SUPPLY_ENABLE      TEGRA_GPIO(E, 4)
+#define GPIO_LCD_BL_P5V                 TEGRA_GPIO(I, 0)
+#define GPIO_LCD_BL_N5V                 TEGRA_GPIO(I, 1)
+#define GPIO_LCD_BL_PWM                 TEGRA_GPIO(V, 0)
+#define GPIO_LCD_BL_EN                  TEGRA_GPIO(V, 1)
+#define GPIO_LCD_BL_RST                 TEGRA_GPIO(V, 2)
+
+void gpio_set_regs(uintptr_t regs);
+void gpio_configure_mode(uint32_t pin, uint32_t mode);
+void gpio_configure_direction(uint32_t pin, uint32_t dir);
+void gpio_write(uint32_t pin, uint32_t value);
+uint32_t gpio_read(uint32_t pin);

thermosphere/src/platform/tegra/interrupt_config.h

@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "../../gicv2.h"
+
+#define MEMORY_MAP_PA_GICD              0x50041000ull
+#define MEMORY_MAP_PA_GICC              0x50042000ull
+#define MEMORY_MAP_PA_GICH              0x50044000ull
+#define MEMORY_MAP_PA_GICV              0x50046000ull
+
+#define GIC_IRQID_MAINTENANCE           25
+#define GIC_IRQID_NS_PHYS_HYP_TIMER     26
+#define GIC_IRQID_NS_VIRT_TIMER         27
+#define GIC_IRQID_LEGACY_NFIQ           28
+#define GIC_IRQID_SEC_PHYS_TIMER        29
+#define GIC_IRQID_NS_PHYS_TIMER         30
+#define GIC_IRQID_LEGACY_NIRQ           31
+
+#define GIC_IRQID_NS_VIRT_HYP_TIMER     GIC_IRQID_SPURIOUS // SBSA: 28. Unimplemented
+#define GIC_IRQID_SEC_PHYS_HYP_TIMER    GIC_IRQID_SPURIOUS // SBSA: 20. Unimplemented
+#define GIC_IRQID_SEC_VIRT_HYP_TIMER    GIC_IRQID_SPURIOUS // SBSA: 19. Unimplemented
+
+#define GIC_IRQID_UARTA                 (32 + 36)
+#define GIC_IRQID_UARTB                 (32 + 37)
+#define GIC_IRQID_UARTC                 (32 + 46)
+#define GIC_IRQID_UARTD                 (32 + 90)

thermosphere/src/platform/tegra/pinmux.c

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2018-2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "pinmux.h"
+
+uintptr_t g_pinmuxRegs;