thermosphere: minor refactor

Thanks to @CTCaer

See dumped values from HOS: ccaa47ee04/kernel-dts/icosa-platforms/tegra210-icosa-pinmux.dtsi

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");
@@ -143,6 +146,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);
 
@@ -327,12 +333,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) {
+static size_t package2_get_thermosphere(const void **thermosphere) {
-    /*extern const uint8_t thermosphere_bin[];
+    (*thermosphere) = thermosphere_bin;
-    extern const uint32_t thermosphere_bin_size;*/
+    return thermosphere_bin_size;
-    /* TODO: enable when tested. */
-    (*thermosphere) = NULL;
-    return 0;
 }
 
 static ini1_header_t *package2_rebuild_ini1(ini1_header_t *ini1, uint32_t target_firmware, void *emummc, size_t emummc_size) {
@@ -353,7 +356,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;
@@ -365,6 +368,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

@@ -59,6 +59,7 @@
 #define PACKAGE2_MINVER_1100_CURRENT 0x10
 
 #define NX_BOOTLOADER_PACKAGE2_LOAD_ADDRESS ((void *)(0xA9800000ull))
+#define DRAM_BASE_PHYSICAL (0x80000000)
 
 typedef struct {
     union {

thermosphere/.gitignore

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

thermosphere/Makefile

@@ -9,13 +9,39 @@ endif
 TOPDIR ?= $(CURDIR)
 include $(DEVKITPRO)/devkitA64/base_rules
 
+export AMSLIBSDIR := $(TOPDIR)/../libraries
+
 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,43 +51,62 @@ endif
 #---------------------------------------------------------------------------------
 TARGET		:=	$(notdir $(CURDIR))
 BUILD		:=	build
-SOURCES		:=	src src/lib
+SOURCES		:=	src src/libc src/platform src/gdb $(PLATFORM_SOURCES)
 DATA		:=	data
-INCLUDES	:=	include ../common/include
+INCLUDES	:=
 
 #---------------------------------------------------------------------------------
 # options for code generation
 #---------------------------------------------------------------------------------
-ARCH	:=	-march=armv8-a -mtune=cortex-a57
+# Note: -ffixed-x18 and -mgeneral-regs-only are very important and must be enabled
-DEFINES :=  -DATMOSPHERE_GIT_BRANCH=\"$(AMSBRANCH)\" -DATMOSPHERE_GIT_REV=\"$(AMSREV)\"
+ARCH	:=	-march=armv8-a -mtune=cortex-a57 -mgeneral-regs-only -ffixed-x18 -Wno-psabi
-
+DEFINES	:=	$(PLATFORM_DEFINES)
 CFLAGS	:= \
-		-g \
+	-g \
-		-O2 \
+	-fmacro-prefix-map=$(TOPDIR)/src/= \
-		-ffunction-sections \
+	-Os \
-		-fdata-sections \
+	-ffunction-sections \
-		-mgeneral-regs-only \
+	-fdata-sections \
-		-fomit-frame-pointer \
+	-fomit-frame-pointer \
-		-std=gnu11 \
+	-fno-asynchronous-unwind-tables \
-		-Werror \
+	-fno-unwind-tables \
-		-Wall \
+	-fno-stack-protector \
-		-Wno-main \
+	-fstrict-volatile-bitfields \
-		$(ARCH) $(DEFINES)
+	-Wall \
+	-Werror \
+	-Wno-main \
+	$(ARCH) $(DEFINES)
 
-CFLAGS	+=	$(INCLUDE) -D__CCPLEX__
+export CXXWRAPS := -Wl,--wrap,__cxa_pure_virtual \
+	-Wl,--wrap,__cxa_throw \
+	-Wl,--wrap,__cxa_rethrow \
+	-Wl,--wrap,__cxa_allocate_exception \
+	-Wl,--wrap,__cxa_free_exception \
+	-Wl,--wrap,__cxa_begin_catch \
+	-Wl,--wrap,__cxa_end_catch \
+	-Wl,--wrap,__cxa_call_unexpected \
+	-Wl,--wrap,__cxa_call_terminate \
+	-Wl,--wrap,__gxx_personality_v0 \
+	-Wl,--wrap,_Unwind_Resume \
+	-Wl,--wrap,_Unwind_Resume \
+	-Wl,--wrap,_ZSt19__throw_logic_errorPKc \
+	-Wl,--wrap,_ZSt20__throw_length_errorPKc \
+	-Wl,--wrap,_ZNSt11logic_errorC2EPKc
 
-CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions -std=gnu++11
+CFLAGS	+=	$(INCLUDE) 
+CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions -std=gnu++2a
+CFLAGS	+=  -std=gnu11
 
-ASFLAGS	:=	-g $(ARCH)
+ASFLAGS	:=	-g $(ARCH) $(DEFINES)
-LDFLAGS	=	-specs=$(TOPDIR)/linker.specs -g $(ARCH) -Wl,-Map,$(notdir $*.map)
+LDFLAGS	=	-specs=$(TOPDIR)/linker.specs -nostartfiles -nostdlib -g $(ARCH) $(CXXWRAPS) -Wl,-Map,$(notdir $*.map)
 
-LIBS	:=
+LIBS	:=	-lgcc
 
 #---------------------------------------------------------------------------------
 # list of directories containing libraries, this must be the top level containing
 # include and lib
 #---------------------------------------------------------------------------------
-LIBDIRS	:=
+LIBDIRS	:= $(AMSLIBSDIR)/libvapours
 
 
 #---------------------------------------------------------------------------------
@@ -100,7 +145,7 @@ endif
 
 export OFILES_BIN	:=	$(addsuffix .o,$(BINFILES))
 export OFILES_SRC	:=	$(CPPFILES:.cpp=.o) $(CFILES:.c=.o) $(SFILES:.s=.o)
-export OFILES 	:=	$(OFILES_BIN) $(OFILES_SRC)
+export OFILES 		:=	$(OFILES_BIN) $(OFILES_SRC)
 export HFILES_BIN	:=	$(addsuffix .h,$(subst .,_,$(BINFILES)))
 
 export INCLUDE	:=	$(foreach dir,$(INCLUDES),-I$(CURDIR)/$(dir)) \
@@ -109,11 +154,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 +214,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);
+    /* Shit we keep in the elf but otherwise discard */
-    .data : {
+    .eh_frame_hdr     (NOLOAD) : { *(.eh_frame_hdr) *(.eh_frame_entry .eh_frame_entry.*) } >mainVa :NONE
-        *(.data*)
+    .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
 
-    /* Uninitialised data */
+    /* ==================
-    . = ALIGN(8);
+       ==== Metadata ====
-    PROVIDE(lds_bss_start = .);
+       ================== */
-    .bss (NOLOAD) : {
-        *(.bss*) . = ALIGN(8);
-    }
-    PROVIDE(lds_bss_end = .);
 
-    /* EL2 stack */
+    /* Discard sections that difficult post-processing */
-    . = ALIGN(16);
+    /DISCARD/ : { *(.group .comment .note) }
-    . += 0x10000; /* 64 KiB stack */
-    el2_stack_end = .;
 
-    /* Page align the end of binary */
+    /* Stabs debugging sections. */
-    . = ALIGN(512);
+    .stab          0 : { *(.stab) }
-    PROVIDE(lds_el2_thermo_end = .);
+    .stabstr       0 : { *(.stabstr) }
+    .stab.excl     0 : { *(.stab.excl) }
+    .stab.exclstr  0 : { *(.stab.exclstr) }
+    .stab.index    0 : { *(.stab.index) }
+    .stab.indexstr 0 : { *(.stab.indexstr) }
 
-    /* EL1 stack */
+    /* DWARF debug sections.
-    . = ALIGN(16);
+       Symbols in the DWARF debugging sections are relative to the beginning
-    . += 0x10000; /* 64 KiB stack */
+       of the section so we begin them at 0. */
-    el1_stack_end = .;
 
-    lds_thermo_end = .;
+    /* DWARF 1 */
+    .debug          0 : { *(.debug) }
+    .line           0 : { *(.line) }
 
-    /DISCARD/ : { *(.dynstr*) }
+    /* GNU DWARF 1 extensions */
-    /DISCARD/ : { *(.dynamic*) }
+    .debug_srcinfo  0 : { *(.debug_srcinfo) }
-    /DISCARD/ : { *(.plt*) }
+    .debug_sfnames  0 : { *(.debug_sfnames) }
-    /DISCARD/ : { *(.interp*) }
+
-    /DISCARD/ : { *(.gnu*) }
+    /* 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
+%(old_link) -T %:getenv(TOPDIR /%:getenv(PLATFORM .mem)) -T %:getenv(TOPDIR /linker.ld) -no-pie --nmagic --gc-sections
-
-*startfile:
-crti%O%s crtbegin%O%s

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/abort.cpp

@@ -0,0 +1,54 @@
+/*
+ * 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 "defines.hpp"
+
+extern "C" {
+
+    /* Redefine abort to trigger these handlers. */
+    void abort();
+
+    /* Redefine C++ exception handlers. Requires wrap linker flag. */
+    #define WRAP_ABORT_FUNC(func) void NORETURN __wrap_##func(void) { abort(); __builtin_unreachable(); }
+    WRAP_ABORT_FUNC(__cxa_pure_virtual)
+    WRAP_ABORT_FUNC(__cxa_throw)
+    WRAP_ABORT_FUNC(__cxa_rethrow)
+    WRAP_ABORT_FUNC(__cxa_allocate_exception)
+    WRAP_ABORT_FUNC(__cxa_free_exception)
+    WRAP_ABORT_FUNC(__cxa_begin_catch)
+    WRAP_ABORT_FUNC(__cxa_end_catch)
+    WRAP_ABORT_FUNC(__cxa_call_unexpected)
+    WRAP_ABORT_FUNC(__cxa_call_terminate)
+    WRAP_ABORT_FUNC(__gxx_personality_v0)
+    WRAP_ABORT_FUNC(_ZSt19__throw_logic_errorPKc)
+    WRAP_ABORT_FUNC(_ZSt20__throw_length_errorPKc)
+    WRAP_ABORT_FUNC(_ZNSt11logic_errorC2EPKc)
+
+    /* TODO: We may wish to consider intentionally not defining an _Unwind_Resume wrapper. */
+    /* This would mean that a failure to wrap all exception functions is a linker error. */
+    WRAP_ABORT_FUNC(_Unwind_Resume)
+    #undef WRAP_ABORT_FUNC
+
+}
+
+/* Custom abort handler, so that std::abort will trigger these. */
+void abort()
+{
+#ifndef PLATFORM_QEMU
+    __builtin_trap();
+#endif
+    for (;;);
+}

thermosphere/src/asm_macros.s

@@ -0,0 +1,32 @@
+/*
+ * 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
+
+.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/cpu/hvisor_cpu_caches.cpp

@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_cpu_caches.hpp"
+
+#define DEFINE_CACHE_RANGE_FUNC(isn, name, cache, post)\
+void name(const void *addr, size_t size)\
+{\
+    u32 lineCacheSize = GetSmallest##cache##CacheLineSize();\
+    uintptr_t begin = reinterpret_cast<uintptr_t>(addr) & ~(lineCacheSize - 1);\
+    uintptr_t end = (reinterpret_cast<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;\
+}
+
+namespace {
+
+    ALWAYS_INLINE void SelectCacheLevel(bool instructionCache, u32 level)
+    {
+        u32 ibit  = instructionCache ? 1 : 0;
+        u32 lbits = (level & 7) << 1;
+        THERMOSPHERE_SET_SYSREG(csselr_el1, lbits | ibit);
+        ams::hvisor::cpu::isb();
+    }
+
+    [[gnu::optimize("O2")]] ALWAYS_INLINE void InvalidateDataCacheLevel(u32 level)
+    {
+        SelectCacheLevel(false, level);
+        u32 ccsidr = static_cast<u32>(THERMOSPHERE_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");
+            }
+        }
+    }
+
+    ALWAYS_INLINE void CleanInvalidateDataCacheLevel(u32 level)
+    {
+        SelectCacheLevel(false, level);
+        u32 ccsidr = static_cast<u32>(THERMOSPHERE_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");
+            }
+        }
+    }
+
+    [[gnu::optimize("O2")]] ALWAYS_INLINE void InvalidateDataCacheLevels(u32 from, u32 to)
+    {
+        // Let's hope it doesn't generate a stack frame...
+        for (u32 level = from; level < to; level++) {
+            InvalidateDataCacheLevel(level);
+        }
+
+        ams::hvisor::cpu::dsbSy();
+        ams::hvisor::cpu::isb();
+    }
+
+}
+namespace ams::hvisor::cpu {
+
+    DEFINE_CACHE_RANGE_FUNC("dc civac", CleanInvalidateDataCacheRange, Data, dsbSy())
+    DEFINE_CACHE_RANGE_FUNC("dc cvau", CleanDataCacheRangePoU, Data, dsb())
+    DEFINE_CACHE_RANGE_FUNC("ic ivau", InvalidateInstructionCacheRangePoU, Instruction, dsb(); isb())
+
+    void HandleSelfModifyingCodePoU(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 = static_cast<u32>(THERMOSPHERE_GET_SYSREG(ctr_el0));
+        if (!(ctr & BIT(28))) {
+            CleanDataCacheRangePoU(addr, size);
+        }
+        if (!(ctr & BIT(29))) {
+            InvalidateInstructionCacheRangePoU(addr, size);
+        } else {
+            // Make sure we have at least a dsb/isb
+            dsb();
+            isb();
+        }
+    }
+
+    [[gnu::optimize("O2")]] void ClearSharedDataCachesOnBoot(void)
+    {
+        u32 clidr = static_cast<u32>(THERMOSPHERE_GET_SYSREG(clidr_el1));
+        u32 louis = (clidr >> 21) & 7;
+        u32 loc   = (clidr >> 24) & 7;
+        InvalidateDataCacheLevels(louis, loc);
+    }
+
+    [[gnu::optimize("O2")]] void ClearLocalDataCacheOnBoot(void)
+    {
+        u32 clidr = static_cast<u32>(THERMOSPHERE_GET_SYSREG(clidr_el1));
+        u32 louis = (clidr >> 21) & 7;
+        InvalidateDataCacheLevels(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 (check if SetWay == 0)
+            - transform all s/w cache ops into clean and invalidate
+     */
+    void HandleTrappedSetWayOperation(u32 val)
+    {
+        u32 clidr = static_cast<u32>(THERMOSPHERE_GET_SYSREG(clidr_el1));
+        u32 louis = (clidr >> 21) & 7;
+
+        u32 level = val >> 1 & 7;
+        u32 setway = val >> 3; 
+        if (level < louis) {
+            return;
+        }
+
+        if (setway == 0) {
+            CleanInvalidateDataCacheLevel(level);
+            dsbSy();
+            isb();
+        }
+    }
+
+}

thermosphere/src/cpu/hvisor_cpu_caches.hpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_cpu_instructions.hpp"
+#include "hvisor_cpu_sysreg_general.hpp"
+
+namespace ams::hvisor::cpu {
+
+    inline u32 GetInstructionCachePolicy(void)
+    {
+        u32 ctr = static_cast<u32>(THERMOSPHERE_GET_SYSREG(ctr_el0));
+        return (ctr >> 14) & 3;
+    }
+
+    inline u32 GetSmallestInstructionCacheLineSize(void)
+    {
+        u32 ctr = static_cast<u32>(THERMOSPHERE_GET_SYSREG(ctr_el0));
+        u32 shift = ctr & 0xF;
+        // "log2 of the number of words"...
+        return 4 << shift;
+    }
+
+    inline u32 GetSmallestDataCacheLineSize(void)
+    {
+        u32 ctr = static_cast<u32>(THERMOSPHERE_GET_SYSREG(ctr_el0));
+        u32 shift = (ctr >> 16) & 0xF;
+        // "log2 of the number of words"...
+        return 4 << shift;
+    }
+
+    ALWAYS_INLINE void InvalidateInstructionCache(void)
+    {
+        __asm__ __volatile__ ("ic ialluis" ::: "memory");
+        cpu::isb();
+    }
+
+    ALWAYS_INLINE void InvalidateInstructionCacheLocal(void)
+    {
+        __asm__ __volatile__ ("ic iallu" ::: "memory");
+        cpu::isb();
+    }
+
+    void CleanInvalidateDataCacheRange(const void *addr, size_t size);
+    void CleanDataCacheRangePoU(const void *addr, size_t size);
+
+    void InvalidateInstructionCacheRangePoU(const void *addr, size_t size);
+
+    void HandleSelfModifyingCodePoU(const void *addr, size_t size);
+
+    void ClearSharedDataCachesOnBoot(void);
+    void ClearLocalDataCacheOnBoot(void);
+
+    // Dunno where else to put that
+    void HandleTrappedSetWayOperation(u32 val);
+
+}

thermosphere/src/cpu/hvisor_cpu_debug_register_pair.hpp

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../defines.hpp"
+
+
+namespace ams::hvisor::cpu {
+
+    // TODO GCC 10, use enum class.
+    // Would be nice if gcc didn't take 9+ years to fix a trivial bug ("too small to fit")
+    struct DebugRegisterPair {
+        // For breakpoints only
+        /// BT[3:1] or res0. BT[0]/WT[0] is "is linked"
+        enum BreakpointType : u32 {
+            AddressMatch = 0,
+            VheContextIdMatch = 1,
+            ContextIdMatch = 3,
+            VmidMatch = 4,
+            VmidContextIdMatch = 5,
+            VmidVheContextIdMatch = 6,
+            FullVheContextIdMatch = 7,
+        };
+
+        // Note: some SSC HMC PMC combinations are invalid
+        // Refer to "Table D2-9 Summary of breakpoint HMC, SSC, and PMC encodings"
+
+        /// Security State Control
+        enum SecurityStateControl : u32 {
+            Both                  = 0,
+            NonSecure             = 1,
+            Secure                = 2,
+            SecureIfLowerOrBoth   = 3,
+        };
+
+        /// Higher Mode Control
+        enum HigherModeControl : u32 {
+            LowerEl   = 0,
+            HigherEl  = 1,
+        };
+
+        /// Privilege Mode Control (called PAC for watchpoints)
+        enum PrivilegeModeControl : u32 {
+            NeitherEl1Nor0    = 0,
+            El1               = 1,
+            El0               = 2,
+            El1And0           = 3,
+        };
+
+        // Watchpoints only
+        enum LoadStoreControl : u32 {
+            NotAWatchpoint  = 0,
+            Load            = 1,
+            Store           = 2,
+            LoadStore       = 3,
+        };
+
+        // bas only 4 bits for breakpoints, other bits res0.
+        // lsc, mask only for watchpoints, res0 for breakpoints
+        // bt only from breakpoints, res0 for watchpoints
+        struct ControlRegister {
+            union {
+                struct {
+                    bool enabled                        : 1;
+                    PrivilegeModeControl pmc            : 2;
+                    LoadStoreControl lsc                : 2;
+                    u32 bas                             : 8;
+                    HigherModeControl hmc               : 1;
+                    SecurityStateControl ssc            : 2;
+                    u32 lbn                             : 4;
+                    bool linked                         : 1;
+                    BreakpointType bt                   : 3;
+                    u32 mask                            : 5;
+                    u64 res0                            : 35;
+                };
+                u64 raw;
+            };
+        };
+
+        ControlRegister cr;
+        u64             vr;
+
+        constexpr void SetDefaults()
+        {
+            cr.linked = false;
+
+            // NS EL1&0 only
+            cr.hmc = LowerEl;
+            cr.ssc = NonSecure;
+            cr.pmc = El1And0;
+        }
+    };
+
+    static_assert(std::is_standard_layout_v<DebugRegisterPair>);
+    static_assert(std::is_trivial_v<DebugRegisterPair>);
+}

thermosphere/src/cpu/hvisor_cpu_exception_sysregs.hpp

@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+
+#pragma once
+
+#include "../defines.hpp"
+#include "hvisor_cpu_sysreg_general.hpp"
+
+namespace ams::hvisor::cpu {
+
+    // FIXME GCC 10
+
+    struct ExceptionSyndromeRegister {
+        enum ExceptionClass : u32 {
+            Uncategorized = 0x0,
+            WFxTrap = 0x1,
+            CP15RTTrap = 0x3,
+            CP15RRTTrap = 0x4,
+            CP14RTTrap = 0x5,
+            CP14DTTrap = 0x6,
+            AdvSIMDFPAccessTrap = 0x7,
+            FPIDTrap = 0x8,
+            PACTrap = 0x9,
+            CP14RRTTrap = 0xC,
+            BranchTargetException = 0xD, // No official enum field name from Arm yet
+            IllegalState = 0xE,
+            SupervisorCallA32 = 0x11,
+            HypervisorCallA32 = 0x12,
+            MonitorCallA32 = 0x13,
+            SupervisorCallA64 = 0x15,
+            HypervisorCallA64 = 0x16,
+            MonitorCallA64 = 0x17,
+            SystemRegisterTrap = 0x18,
+            SVEAccessTrap = 0x19,
+            ERetTrap = 0x1A,
+            El3_ImplementationDefined = 0x1F,
+            InstructionAbortLowerEl = 0x20,
+            InstructionAbortSameEl = 0x21,
+            PCAlignment = 0x22,
+            DataAbortLowerEl = 0x24,
+            DataAbortSameEl = 0x25,
+            SPAlignment = 0x26,
+            FPTrappedExceptionA32 = 0x28,
+            FPTrappedExceptionA64 = 0x2C,
+            SError = 0x2F,
+            BreakpointLowerEl = 0x30,
+            BreakpointSameEl = 0x31,
+            SoftwareStepLowerEl = 0x32,
+            SoftwareStepSameEl = 0x33,
+            WatchpointLowerEl = 0x34,
+            WatchpointSameEl = 0x35,
+            SoftwareBreakpointA32 = 0x38,
+            VectorCatchA32 = 0x3A,
+            SoftwareBreakpointA64 = 0x3C,
+        };
+
+        u32 iss             : 25;   // Instruction Specific Syndrome
+        u32 il              :  1;   // Instruction Length (16 or 32-bit)
+        ExceptionClass ec   :  6;   // Exception Class
+        u32 res0            : 32;
+
+        constexpr size_t GetInstructionLength()
+        {
+            return il == 0 ? 2 : 4;
+        }
+    };
+
+
+    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)
+
+        constexpr bool HasValidFar()
+        {
+            return isv && !fnv;
+        }
+        constexpr size_t GetAccessSize()
+        {
+            return BITL(sas);
+        }
+    };
+
+
+    static_assert(std::is_standard_layout_v<ExceptionSyndromeRegister>);
+    static_assert(std::is_standard_layout_v<DataAbortIss>);
+    static_assert(std::is_trivial_v<ExceptionSyndromeRegister>);
+    static_assert(std::is_trivial_v<DataAbortIss>);
+
+}

thermosphere/src/cpu/hvisor_cpu_instructions.hpp

@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../defines.hpp"
+
+#define _ASM_ARITHMETIC_UNARY_HELPER(sz, regalloc, op) ({\
+    u##sz res;\
+    __asm__ __volatile__ (STRINGIZE(op) " %" STRINGIZE(regalloc) "[res], %" STRINGIZE(regalloc) "[val]" : [res] "=r" (res) : [val] "r" (val));\
+    res;\
+})
+
+#define DECLARE_SINGLE_ASM_INSN2(name, what) ALWAYS_INLINE void name() { __asm__ __volatile__ (what ::: "memory"); }
+#define DECLARE_SINGLE_ASM_INSN(name) ALWAYS_INLINE void name() { __asm__ __volatile__ (STRINGIZE(name) ::: "memory"); }
+
+namespace ams::hvisor::cpu {
+
+
+    template<typename T>
+    ALWAYS_INLINE static T rbit(T val)
+    {
+        static_assert(std::is_integral_v<T> && (sizeof(T) == 8 || sizeof(T) == 4));
+        if constexpr (sizeof(T) == 8) {
+            return _ASM_ARITHMETIC_UNARY_HELPER(64, x, rbit);
+        } else {
+            return _ASM_ARITHMETIC_UNARY_HELPER(32, w, rbit);
+        }
+    }
+
+    DECLARE_SINGLE_ASM_INSN(wfi)
+    DECLARE_SINGLE_ASM_INSN(wfe)
+    DECLARE_SINGLE_ASM_INSN(sevl)
+    DECLARE_SINGLE_ASM_INSN(sev)
+    DECLARE_SINGLE_ASM_INSN2(dmb, "dmb ish")
+    DECLARE_SINGLE_ASM_INSN2(dmbSy, "dmb sy")
+    DECLARE_SINGLE_ASM_INSN2(dsb, "dsb ish")
+    DECLARE_SINGLE_ASM_INSN2(dsbSy, "dsb sy")
+    DECLARE_SINGLE_ASM_INSN(isb)
+
+    DECLARE_SINGLE_ASM_INSN2(TlbInvalidateEl2Local, "tlbi alle2")
+    DECLARE_SINGLE_ASM_INSN2(TlbInvalidateEl2, "tlbi alle2is")
+    DECLARE_SINGLE_ASM_INSN2(TlbInvalidateEl1, "tlbi vmalle1is")
+    DECLARE_SINGLE_ASM_INSN2(TlbInvalidateEl1Stage12, "tlbi alle1is")
+    DECLARE_SINGLE_ASM_INSN2(TlbInvalidateEl1Stage12Local, "tlbi alle1")
+
+    ALWAYS_INLINE void TlbInvalidateEl2Page(uintptr_t addr)
+    {
+        __asm__ __volatile__ ("tlbi vae2is, %0" :: "r"(addr) : "memory");
+    }
+
+}
+
+#undef DECLARE_SINGLE_ASM_INSN
+#undef DECLARE_SINGLE_ASM_INSN2
+#undef _ASM_ARITHMETIC_UNARY_HELPER

thermosphere/src/cpu/hvisor_cpu_interrupt_mask_guard.hpp

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_cpu_sysreg_general.hpp"
+
+namespace ams::hvisor::cpu {
+
+    ALWAYS_INLINE u64 MaskIrq()
+    {
+        u64 daif = THERMOSPHERE_GET_SYSREG(daif);
+        THERMOSPHERE_SET_SYSREG_IMM(daifset, BIT(1));
+        return daif;
+    }
+
+    ALWAYS_INLINE u64 UnmaskIrq()
+    {
+        u64 daif = THERMOSPHERE_GET_SYSREG(daif);
+        THERMOSPHERE_SET_SYSREG_IMM(daifclr, BIT(1));
+        return daif;
+    }
+
+    ALWAYS_INLINE void RestoreInterruptFlags(u64 flags)
+    {
+        THERMOSPHERE_SET_SYSREG(daif, flags);
+    }
+
+    class InterruptMaskGuard final {
+        NON_COPYABLE(InterruptMaskGuard);
+        NON_MOVEABLE(InterruptMaskGuard);
+        private:
+            u64 m_flags;
+        public:
+            ALWAYS_INLINE InterruptMaskGuard() : m_flags(MaskIrq()) {}
+            ALWAYS_INLINE ~InterruptMaskGuard()
+            {
+                RestoreInterruptFlags(m_flags);
+            }
+    };
+}

thermosphere/src/cpu/hvisor_cpu_mmu.hpp

@@ -0,0 +1,198 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+
+#pragma once
+
+#include "hvisor_cpu_sysreg_general.hpp"
+
+namespace ams::hvisor::cpu {
+
+    // Assumes addr is valid, must be called with interrupts masked
+    inline uintptr_t Va2Pa(const void *vaddrEl2) {
+        uintptr_t va = reinterpret_cast<uintptr_t>(vaddrEl2);
+        __asm__ __volatile__("at s1e2r, %0" :: "r"(va) : "memory");
+        return (THERMOSPHERE_GET_SYSREG(par_el1) & MASK2L(47, 12)) | (va & MASKL(12));
+    }
+
+    enum MmuPteType : u64 {
+        MMU_ENTRY_FAULT  = 0,
+        MMU_ENTRY_BLOCK  = 1,
+        MMU_ENTRY_TABLE  = 3,
+
+        // L3 (this definition allows for recursive page tables)
+        MMU_ENTRY_PAGE   = 3,
+    };
+
+    // Multi-byte attributes...
+    constexpr u64 MMU_ATTRINDX(u64 idx)     { return (idx & 8) << 2; }
+    constexpr u64 MMU_MEMATTR(u64 attr)     { return (attr & 0xF) << 2; }
+    constexpr u64 MMU_SH(u64 sh)            { return (sh & 3) << 8; }
+
+    // Attributes. They are defined in a way that allows recursive page tables (assuming PBHA isn't used)
+    enum MmuPteAttributes : u64 {
+        // Stage 1 Table only, the rest is block/page only
+        MMU_NS_TABLE        = BITL(62),
+        MMU_AP_TABLE        = BITL(61),
+        MMU_XN_TABLE        = BITL(60),
+        MMU_PXN_TABLE       = BITL(59),
+
+        MMU_UXN             = BITL(54), // EL1&0 only
+        MMU_PXN             = BITL(53), // EL1&0 only
+        MMU_XN              = MMU_UXN,
+        MMU_XN0             = MMU_PXN,  // Armv8.2, stage 2 only
+        MMU_CONTIGUOUS      = BITL(52),
+        MMU_DBM             = BITL(51), // stage 1 only
+        MMU_GP              = BITL(50), // undocumented
+
+        // ARMv8.4-TTRem only
+        MMU_NT              = BITL(16),
+
+        // EL1&0 only
+        MMU_NG              = BITL(11),
+
+        MMU_AF              = BITL(10),
+
+        // SH[1:0]
+        MMU_NON_SHAREABLE   = MMU_SH(0),
+        MMU_OUTER_SHAREABLE = MMU_SH(2),
+        MMU_INNER_SHAREABLE = MMU_SH(3),
+
+        // AP[2:1], stage 1 only. AP[0] does not exist.
+        MMU_AP_PRIV_RW      = 0 << 6,
+        MMU_AP_RW           = 1 << 6,
+        MMU_AP_PRIV_RO      = 2 << 6,
+        MMU_AP_RO           = 3 << 6,
+
+        // S2AP[1:0], stage 2 only
+        MMU_S2AP_NONE       = 0 << 6,
+        MMU_S2AP_RO         = 1 << 6,
+        MMU_S2AP_WO         = 2 << 6,
+        MMU_S2AP_RW         = 3 << 6,
+
+        // NS, stage 1 only
+        MMU_NS              = BITL(5),
+
+        // See above...
+
+        // MemAttr[3:0], stage 2 only (convenience defs). When combining, strongest memory type applies
+        MMU_MEMATTR_DEVICE_NGNRE    = MMU_MEMATTR(2),
+        MMU_MEMATTR_UNCHANGED       = MMU_MEMATTR(0xF),
+
+        // Other useful defines for stage 2:
+        MMU_SAME_SHAREABILITY       = MMU_NON_SHAREABLE,
+    };
+
+    template<u32 Level, u32 AddressSpaceSize, bool IsMmuEnabled = false, TranslationGranuleSize GranuleSize = TranslationGranule_4K>
+    class MmuTableBuilder final {
+        private:
+            static constexpr u32 tgBitSize = GetTranslationGranuleBitSize(GranuleSize);
+
+            // tgBitSize - 3 = log2(tg / sizeof(u64))
+            static constexpr u32 levelShift = tgBitSize + (tgBitSize - 3) * (3 - Level);
+            static constexpr u32 levelBitSize = std::min(AddressSpaceSize - levelShift, tgBitSize - 3);
+            static constexpr u64 levelMask = MASKL(levelBitSize);
+            static constexpr size_t ComputeIndex(uintptr_t va)
+            {
+                return (va >> levelShift) & levelMask;
+            }
+
+        private:
+            u64 *m_pageTable = nullptr;
+
+        public:
+            using NextLevelBuilder = MmuTableBuilder<Level + 1, AddressSpaceSize, IsMmuEnabled, GranuleSize>;
+            static_assert(Level <= 3, "Invalid translation table level");
+            static_assert(AddressSpaceSize <= 48);
+            static_assert(AddressSpaceSize > levelShift, "Address space size mismatch with translation level");
+            static constexpr size_t blockSize = BITL(levelShift);
+            static constexpr size_t tableSize = BITL(levelBitSize);
+
+        public:
+            constexpr MmuTableBuilder(u64 *pageTable = nullptr) : m_pageTable{pageTable} {}
+
+            constexpr MmuTableBuilder &InitializeTable()
+            {
+                std::memset(m_pageTable, 0, 8 * tableSize);
+                // Fails to optimize before GCC 10: std::fill_n(m_pageTable, tableSize, MMU_ENTRY_FAULT);
+                return *this;
+            }
+
+            // Precondition: va and pa bits in range
+            constexpr NextLevelBuilder MapTable(uintptr_t va, uintptr_t pa, u64 *table, u64 attribs = 0) const
+            {
+                static_assert(Level < 3, "Level 3 is the last level of translation");
+
+                m_pageTable[ComputeIndex(va)] = pa | attribs | MMU_ENTRY_TABLE;
+                return NextLevelBuilder{table};
+            }
+
+            NextLevelBuilder MapTable(uintptr_t va, u64 *table, u64 attribs = 0) const
+            {
+                if constexpr (IsMmuEnabled) {
+                    return MapTable(va, Va2Pa(table), table, attribs);
+                } else {
+                    return MapTable(va, reinterpret_cast<uintptr_t>(table), table, attribs);
+                }
+            }
+
+            constexpr MmuTableBuilder &Unmap(uintptr_t va)
+            {
+                m_pageTable[ComputeIndex(va)] = MMU_ENTRY_FAULT;
+                return *this;
+            }
+
+            // Precondition: guardSize == 0 if Level == 0
+            constexpr MmuTableBuilder &UnmapRange(uintptr_t va, size_t size, size_t guardSize = 0)
+            {
+                for (size_t off = 0, offVa = 0; off < size; off += blockSize, offVa += blockSize + guardSize) {
+                    Unmap(va + offVa);
+                }
+                return *this;
+            }
+
+            // Precondition: va and pa bits in range
+            constexpr MmuTableBuilder &MapBlock(uintptr_t va, uintptr_t pa, u64 attribs)
+            {
+                static_assert(Level > 0, "Can only map L1 tables at L0");
+
+                constexpr u64 entryType = Level == 3 ? MMU_ENTRY_PAGE : MMU_ENTRY_BLOCK;
+                m_pageTable[ComputeIndex(va)] = pa | attribs | MMU_AF | entryType;
+                return *this;
+            }
+
+            constexpr MmuTableBuilder &MapBlock(uintptr_t pa, u64 attribs)
+            {
+                return MapBlock(pa, pa, attribs);
+            }
+
+            // Precondition: size and guardSize are multiples of blockSize
+            constexpr MmuTableBuilder &MapBlockRange(uintptr_t va, uintptr_t pa, size_t size, u64 attribs, size_t guardSize = 0)
+            {
+                for (size_t off = 0, offVa = 0; off < size; off += blockSize, offVa += blockSize + guardSize) {
+                    MapBlock(va + offVa, pa + off, attribs);
+                    UnmapRange(va + offVa + blockSize, guardSize, 0);
+                }
+                return *this;
+            }
+
+            constexpr MmuTableBuilder &MapBlockRange(uintptr_t pa, size_t size, u64 attribs)
+            {
+                return MapBlockRange(pa, pa, attribs, size, 0);
+            }
+    };
+
+}

thermosphere/src/cpu/hvisor_cpu_sysreg_general.hpp

@@ -0,0 +1,495 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+
+#pragma once
+
+#include "../preprocessor.h"
+#include "../defines.hpp"
+
+#define THERMOSPHERE_GET_SYSREG(r) ({\
+    u64 __val;                                                                      \
+    __asm__ __volatile__("mrs %0, " STRINGIZE(r) : "=r" (__val) :: "memory");       \
+    __val;                                                                          \
+})
+
+#define THERMOSPHERE_SET_SYSREG(reg, val)\
+    do {\
+        u64 temp_reg = (val);\
+        __asm__ __volatile__ ("msr " STRINGIZE(reg) ", %0" :: "r"(temp_reg) : "memory");\
+    } while(false)
+
+#define THERMOSPHERE_SET_SYSREG_IMM(reg, imm)\
+    do {\
+        __asm__ __volatile__ ("msr " STRINGIZE(reg) ", %0" :: "I"(imm) : "memory", "cc");\
+    } while(false)
+
+
+namespace ams::hvisor::cpu {
+
+    using SysregEncoding = std::array<u8, 5>;
+
+    constexpr u32 EncodeSysregIss(SysregEncoding reg)
+    {
+        auto [op0, op1, crn, crm, op2] = reg;
+        return op0 << 20 | op2 << 17 | op1 << 14 | crn << 10 | crm << 1;
+    }
+
+    constexpr u32 MakeMsrFromEncoding(SysregEncoding reg, u32 Rt)
+    {
+        auto [op0, op1, crn, crm, op2] = reg;
+        u32 enc = op0 << 19 | op1 << 16 | crn << 12 | crm << 8 | op2 << 5;
+        return 0xD5000000u | enc | (Rt & 0x1Fu);
+    }
+
+    constexpr u32 MakeMrsFromEncoding(SysregEncoding reg, u32 Rt)
+    {
+        auto [op0, op1, crn, crm, op2] = reg;
+        u32 enc = op0 << 19 | op1 << 16 | crn << 12 | crm << 8 | op2 << 5;
+        return 0xD5200000u | enc | (Rt & 0x1Fu);
+    }
+
+    // The list mostly includes EL1 registers as these are the one we're trapping
+
+    constexpr SysregEncoding dbgbvrN_el1(u8 n)           { return {2, 0, 0, n, 4}; }
+    constexpr SysregEncoding dbgbcrN_el1(u8 n)           { return {2, 0, 0, n, 5}; }
+    constexpr SysregEncoding dbgwvrN_el1(u8 n)           { return {2, 0, 0, n, 6}; }
+    constexpr SysregEncoding dbgwcrN_el1(u8 n)           { return {2, 0, 0, n, 7}; }
+
+    constexpr inline SysregEncoding dc_isw               = {1, 0, 7, 6, 2};
+    constexpr inline SysregEncoding dc_csw               = {1, 0, 7, 10, 2};
+    constexpr inline SysregEncoding dc_cisw              = {1, 0, 7, 14, 2};
+
+    constexpr inline SysregEncoding osdtrrx_el1          = {2, 0, 0, 0, 2};
+    constexpr inline SysregEncoding mdccint_el1          = {2, 0, 0, 2, 0};
+    constexpr inline SysregEncoding mdscr_el1            = {2, 0, 0, 2, 2};
+    constexpr inline SysregEncoding osdtrtx_el1          = {2, 0, 0, 3, 2};
+    constexpr inline SysregEncoding oseccr_el1           = {2, 0, 0, 6, 2};
+
+    constexpr inline SysregEncoding mdrar_el1            = {2, 0, 1, 0, 0};
+    constexpr inline SysregEncoding oslar_el1            = {2, 0, 1, 0, 4};
+    constexpr inline SysregEncoding oslsr_el1            = {2, 0, 1, 1, 4};
+    constexpr inline SysregEncoding osdlr_el1            = {2, 0, 1, 3, 4};
+    constexpr inline SysregEncoding dbgprcr_el1          = {2, 0, 1, 4, 4};
+    constexpr inline SysregEncoding dbgclaimset_el1      = {2, 0, 7, 8, 6};
+    constexpr inline SysregEncoding dbgclaimclr_el1      = {2, 0, 7, 9, 6};
+    constexpr inline SysregEncoding dbgauthstatus_el1    = {2, 0, 7, 14, 6};
+    constexpr inline SysregEncoding mdccsr_el0           = {2, 3, 0, 1, 0};
+    constexpr inline SysregEncoding dbgdtr_el0           = {2, 3, 0, 4, 0};
+    constexpr inline SysregEncoding dbgdtrrx_el0         = {2, 3, 0, 5, 0};
+    constexpr inline SysregEncoding dbgdtrtx_el0         = {2, 3, 0, 5, 0};
+    constexpr inline SysregEncoding dbgvcr32_el2         = {2, 4, 0, 7, 0};
+
+    constexpr inline SysregEncoding midr_el1             = {3, 0, 0, 0, 0};
+    constexpr inline SysregEncoding mpidr_el1            = {3, 0, 0, 0, 5};
+    constexpr inline SysregEncoding revidr_el1           = {3, 0, 0, 0, 6};
+
+    constexpr inline SysregEncoding id_pfr0_el1          = {3, 0, 0, 1, 0};
+    constexpr inline SysregEncoding id_pfr1_el1          = {3, 0, 0, 1, 1};
+    constexpr inline SysregEncoding id_dfr0_el1          = {3, 0, 0, 1, 2};
+    constexpr inline SysregEncoding id_afr0_el1          = {3, 0, 0, 1, 3};
+    constexpr inline SysregEncoding id_mmfr0_el1         = {3, 0, 0, 1, 4};
+    constexpr inline SysregEncoding id_mmfr1_el1         = {3, 0, 0, 1, 5};
+    constexpr inline SysregEncoding id_mmfr2_el1         = {3, 0, 0, 1, 6};
+    constexpr inline SysregEncoding id_mmfr3_el1         = {3, 0, 0, 1, 7};
+
+    constexpr inline SysregEncoding id_isar0_el1         = {3, 0, 0, 2, 0};
+    constexpr inline SysregEncoding id_isar1_el1         = {3, 0, 0, 2, 1};
+    constexpr inline SysregEncoding id_isar2_el1         = {3, 0, 0, 2, 2};
+    constexpr inline SysregEncoding id_isar3_el1         = {3, 0, 0, 2, 3};
+    constexpr inline SysregEncoding id_isar4_el1         = {3, 0, 0, 2, 4};
+    constexpr inline SysregEncoding id_isar5_el1         = {3, 0, 0, 2, 5};
+    constexpr inline SysregEncoding id_mmfr4_el1         = {3, 0, 0, 2, 6};
+
+    constexpr inline SysregEncoding mvfr0_el1            = {3, 0, 0, 3, 0};
+    constexpr inline SysregEncoding mvfr1_el1            = {3, 0, 0, 3, 1};
+    constexpr inline SysregEncoding mvfr2_el1            = {3, 0, 0, 3, 2};
+
+    constexpr inline SysregEncoding id_aa64pfr0_el1      = {3, 0, 0, 4, 0};
+    constexpr inline SysregEncoding id_aa64pfr1_el1      = {3, 0, 0, 4, 1};
+    constexpr inline SysregEncoding id_aa64zfr0_el1      = {3, 0, 0, 4, 4};
+
+    constexpr inline SysregEncoding id_aa64dfr0_el1      = {3, 0, 0, 5, 0};
+    constexpr inline SysregEncoding id_aa64dfr1_el1      = {3, 0, 0, 5, 1};
+
+    constexpr inline SysregEncoding id_aa64afr0_el1      = {3, 0, 0, 5, 4};
+    constexpr inline SysregEncoding id_aa64afr1_el1      = {3, 0, 0, 5, 5};
+
+    constexpr inline SysregEncoding id_aa64isar0_el1     = {3, 0, 0, 6, 0};
+    constexpr inline SysregEncoding id_aa64isar1_el1     = {3, 0, 0, 6, 1};
+
+    constexpr inline SysregEncoding id_aa64mmfr0_el1     = {3, 0, 0, 7, 0};
+    constexpr inline SysregEncoding id_aa64mmfr1_el1     = {3, 0, 0, 7, 1};
+    constexpr inline SysregEncoding id_aa64mmfr2_el1     = {3, 0, 0, 7, 2};
+
+    constexpr inline SysregEncoding sctlr_el1            = {3, 0, 1, 0, 0};
+    constexpr inline SysregEncoding actlr_el1            = {3, 0, 1, 0, 1};
+    constexpr inline SysregEncoding cpacr_el1            = {3, 0, 1, 0, 2};
+
+    constexpr inline SysregEncoding zcr_el1              = {3, 0, 1, 2, 0};
+
+    constexpr inline SysregEncoding ttbr0_el1            = {3, 0, 2, 0, 0};
+    constexpr inline SysregEncoding ttbr1_el1            = {3, 0, 2, 0, 1};
+    constexpr inline SysregEncoding tcr_el1              = {3, 0, 2, 0, 2};
+
+    constexpr inline SysregEncoding apiakeylo_el1        = {3, 0, 2, 1, 0};
+    constexpr inline SysregEncoding apiakeyhi_el1        = {3, 0, 2, 1, 1};
+    constexpr inline SysregEncoding apibkeylo_el1        = {3, 0, 2, 1, 2};
+    constexpr inline SysregEncoding apibkeyhi_el1        = {3, 0, 2, 1, 3};
+
+    constexpr inline SysregEncoding apdakeylo_el1        = {3, 0, 2, 2, 0};
+    constexpr inline SysregEncoding apdakeyhi_el1        = {3, 0, 2, 2, 1};
+    constexpr inline SysregEncoding apdbkeylo_el1        = {3, 0, 2, 2, 2};
+    constexpr inline SysregEncoding apdbkeyhi_el1        = {3, 0, 2, 2, 3};
+
+    constexpr inline SysregEncoding apgakeylo_el1        = {3, 0, 2, 3, 0};
+    constexpr inline SysregEncoding apgakeyhi_el1        = {3, 0, 2, 3, 1};
+
+    constexpr inline SysregEncoding afsr0_el1            = {3, 0, 5, 1, 0};
+    constexpr inline SysregEncoding afsr1_el1            = {3, 0, 5, 1, 1};
+    constexpr inline SysregEncoding esr_el1              = {3, 0, 5, 2, 0};
+
+    constexpr inline SysregEncoding erridr_el1           = {3, 0, 5, 3, 0};
+    constexpr inline SysregEncoding errselr_el1          = {3, 0, 5, 3, 1};
+    constexpr inline SysregEncoding erxfr_el1            = {3, 0, 5, 4, 0};
+    constexpr inline SysregEncoding erxctlr_el1          = {3, 0, 5, 4, 1};
+    constexpr inline SysregEncoding erxstatus_el1        = {3, 0, 5, 4, 2};
+    constexpr inline SysregEncoding erxaddr_el1          = {3, 0, 5, 4, 3};
+    constexpr inline SysregEncoding erxmisc0_el1         = {3, 0, 5, 5, 0};
+    constexpr inline SysregEncoding erxmisc1_el1         = {3, 0, 5, 5, 1};
+
+    constexpr inline SysregEncoding far_el1              = {3, 0, 6, 0, 0};
+    constexpr inline SysregEncoding par_el1              = {3, 0, 7, 4, 0};
+
+
+    constexpr inline SysregEncoding pmsidr_el1           = {3, 0, 9, 9, 7};
+    constexpr inline SysregEncoding pmbidr_el1           = {3, 0, 9, 10, 7};
+    constexpr inline SysregEncoding pmscr_el1            = {3, 0, 9, 9, 0};
+    constexpr inline SysregEncoding pmscr_el2            = {3, 4, 9, 9, 0};
+    constexpr inline SysregEncoding pmsicr_el1           = {3, 0, 9, 9, 2};
+    constexpr inline SysregEncoding pmsirr_el1           = {3, 0, 9, 9, 3};
+    constexpr inline SysregEncoding pmsfcr_el1           = {3, 0, 9, 9, 4};
+
+
+    constexpr inline SysregEncoding pmsevfr_el1          = {3, 0, 9, 9, 5};
+    constexpr inline SysregEncoding pmslatfr_el1         = {3, 0, 9, 9, 6};
+    constexpr inline SysregEncoding pmblimitr_el1        = {3, 0, 9, 10, 0};
+
+
+    constexpr inline SysregEncoding pmbptr_el1           = {3, 0, 9, 10, 1};
+
+    constexpr inline SysregEncoding pmbsr_el1            = {3, 0, 9, 10, 3};
+
+
+    constexpr inline SysregEncoding pmintenset_el1       = {3, 0, 9, 14, 1};
+    constexpr inline SysregEncoding pmintenclr_el1       = {3, 0, 9, 14, 2};
+
+    constexpr inline SysregEncoding mair_el1             = {3, 0, 10, 2, 0};
+    constexpr inline SysregEncoding amair_el1            = {3, 0, 10, 3, 0};
+
+    constexpr inline SysregEncoding lorsa_el1            = {3, 0, 10, 4, 0};
+    constexpr inline SysregEncoding lorea_el1            = {3, 0, 10, 4, 1};
+    constexpr inline SysregEncoding lorn_el1             = {3, 0, 10, 4, 2};
+    constexpr inline SysregEncoding lorc_el1             = {3, 0, 10, 4, 3};
+    constexpr inline SysregEncoding lorid_el1            = {3, 0, 10, 4, 7};
+
+    constexpr inline SysregEncoding vbar_el1             = {3, 0, 12, 0, 0};
+    constexpr inline SysregEncoding disr_el1             = {3, 0, 12, 1, 1};
+
+    constexpr inline SysregEncoding contextidr_el1       = {3, 0, 13, 0, 1};
+    constexpr inline SysregEncoding tpidr_el1            = {3, 0, 13, 0, 4};
+
+    constexpr inline SysregEncoding cntkctl_el1          = {3, 0, 14, 1, 0};
+
+    constexpr inline SysregEncoding ccsidr_el1           = {3, 1, 0, 0, 0};
+    constexpr inline SysregEncoding clidr_el1            = {3, 1, 0, 0, 1};
+    constexpr inline SysregEncoding aidr_el1             = {3, 1, 0, 0, 7};
+
+    constexpr inline SysregEncoding csselr_el1           = {3, 2, 0, 0, 0};
+
+    constexpr inline SysregEncoding ctr_el0              = {3, 3, 0, 0, 1};
+    constexpr inline SysregEncoding dczid_el0            = {3, 3, 0, 0, 7};
+
+    constexpr inline SysregEncoding pmcr_el0             = {3, 3, 9, 12, 0};
+    constexpr inline SysregEncoding pmcntenset_el0       = {3, 3, 9, 12, 1};
+    constexpr inline SysregEncoding pmcntenclr_el0       = {3, 3, 9, 12, 2};
+    constexpr inline SysregEncoding pmovsclr_el0         = {3, 3, 9, 12, 3};
+    constexpr inline SysregEncoding pmswinc_el0          = {3, 3, 9, 12, 4};
+    constexpr inline SysregEncoding pmselr_el0           = {3, 3, 9, 12, 5};
+    constexpr inline SysregEncoding pmceid0_el0          = {3, 3, 9, 12, 6};
+    constexpr inline SysregEncoding pmceid1_el0          = {3, 3, 9, 12, 7};
+    constexpr inline SysregEncoding pmccntr_el0          = {3, 3, 9, 13, 0};
+    constexpr inline SysregEncoding pmxevtyper_el0       = {3, 3, 9, 13, 1};
+    constexpr inline SysregEncoding pmxevcntr_el0        = {3, 3, 9, 13, 2};
+    constexpr inline SysregEncoding pmuserenr_el0        = {3, 3, 9, 14, 0};
+    constexpr inline SysregEncoding pmovsset_el0         = {3, 3, 9, 14, 3};
+
+    constexpr inline SysregEncoding tpidr_el0            = {3, 3, 13, 0, 2};
+    constexpr inline SysregEncoding tpidrro_el0          = {3, 3, 13, 0, 3};
+
+    constexpr inline SysregEncoding cntfrq_el0           = {3, 3, 14, 0, 0};
+    constexpr inline SysregEncoding cntpct_el0           = {3, 3, 14, 0, 1};
+    constexpr inline SysregEncoding cntvct_el0           = {3, 3, 14, 0, 2};
+
+    constexpr inline SysregEncoding cntp_tval_el0        = {3, 3, 14, 2, 0};
+    constexpr inline SysregEncoding cntp_ctl_el0         = {3, 3, 14, 2, 1};
+    constexpr inline SysregEncoding cntp_cval_el0        = {3, 3, 14, 2, 2};
+
+    constexpr inline SysregEncoding cntv_tval_el0        = {3, 3, 14, 3, 0};
+    constexpr inline SysregEncoding cntv_ctl_el0         = {3, 3, 14, 3, 1};
+    constexpr inline SysregEncoding cntv_cval_el0        = {3, 3, 14, 3, 2};
+
+    constexpr inline SysregEncoding cntvoff_el2          = {3, 4, 14, 0, 3};
+    constexpr inline SysregEncoding cnthctl_el2          = {3, 4, 14, 1, 0};
+    constexpr inline SysregEncoding cnthp_cval_el2       = {3, 4, 14, 2, 2};
+
+    constexpr inline SysregEncoding pmccfiltr_el0        = {3, 3, 14, 15, 7};
+
+    constexpr inline SysregEncoding zcr_el2              = {3, 4, 1, 2, 0};
+
+    constexpr inline SysregEncoding dacr32_el2           = {3, 4, 3, 0, 0};
+    constexpr inline SysregEncoding ifsr32_el2           = {3, 4, 5, 0, 1};
+    constexpr inline SysregEncoding vsesr_el2            = {3, 4, 5, 2, 3};
+    constexpr inline SysregEncoding fpexc32_el2          = {3, 4, 5, 3, 0};
+
+    constexpr inline SysregEncoding zcr_el12             = {3, 5, 1, 2, 0};
+
+    enum SctlrFlags {
+        SCTLR_ELx_DSSBS         = BITL(44),
+        SCTLR_ELx_ENIA          = BITL(31),
+        SCTLR_ELx_ENIB          = BITL(30),
+        SCTLR_ELx_ENDA          = BITL(27),
+        SCTLR_ELx_EE            = BITL(25),
+        SCTLR_ELx_IESB          = BITL(21),
+        SCTLR_ELx_WXN           = BITL(19),
+        SCTLR_ELx_ENDB          = BITL(13),
+        SCTLR_ELx_I             = BITL(12),
+        SCTLR_ELx_SA            = BITL(3),
+        SCTLR_ELx_C             = BITL(2),
+        SCTLR_ELx_A             = BITL(1),
+        SCTLR_ELx_M             = BITL(0),
+
+        SCTLR_EL1_UCI           = BITL(26),
+        SCTLR_EL1_E0E           = BITL(24),
+        SCTLR_EL1_SPAN          = BITL(23),
+        SCTLR_EL1_NTWE          = BITL(18),
+        SCTLR_EL1_NTWI          = BITL(16),
+        SCTLR_EL1_UCT           = BITL(15),
+        SCTLR_EL1_DZE           = BITL(14),
+        SCTLR_EL1_UMA           = BITL(9),
+        SCTLR_EL1_SED           = BITL(8),
+        SCTLR_EL1_ITD           = BITL(7),
+        SCTLR_EL1_CP15BEN       = BITL(5),
+        SCTLR_EL1_SA0           = BITL(4),
+
+        SCTLR_EL2_RES1          = util::CombineBits<u64>(29, 28, 23, 22, 18, 16, 11, 5, 4),
+        SCTLR_EL2_RES0          = (0xFFFFEFFFull << 32) | util::CombineBits<u64>(
+            31, 30, 27, 26, 24, 20, 17, 15, 14, 13, 10, 9, 8, 7, 6
+        ),
+
+        SCTLR_EL1_RES1          = util::CombineBits<u64>(29, 28, 22, 20, 11),
+        SCTLR_EL1_RES0          = (0xFFFFEFFFull << 32) | util::CombineBits<u64>(31, 30, 27, 17, 13, 10, 6),
+    };
+
+
+    // HCR Flags
+    enum HcrFlags {
+        HCR_FWB     = BITL(46),
+        HCR_API     = BITL(41),
+        HCR_APK     = BITL(40),
+        HCR_TEA     = BITL(37),
+        HCR_TERR    = BITL(36),
+        HCR_TLOR    = BITL(35),
+        HCR_E2H     = BITL(34),
+        HCR_ID      = BITL(33),
+        HCR_CD      = BITL(32),
+        HCR_RW      = BITL(31),
+        HCR_TRVM    = BITL(30),
+        HCR_HCD     = BITL(29),
+        HCR_TDZ     = BITL(28),
+        HCR_TGE     = BITL(27),
+        HCR_TVM     = BITL(26),
+        HCR_TTLB    = BITL(25),
+        HCR_TPU     = BITL(24),
+        HCR_TPC     = BITL(23),
+        HCR_TSW     = BITL(22),
+        HCR_TAC     = BITL(21),
+        HCR_TIDCP   = BITL(20),
+        HCR_TSC     = BITL(19),
+        HCR_TID3    = BITL(18),
+        HCR_TID2    = BITL(17),
+        HCR_TID1    = BITL(16),
+        HCR_TID0    = BITL(15),
+        HCR_TWE     = BITL(14),
+        HCR_TWI     = BITL(13),
+        HCR_DC      = BITL(12),
+        HCR_BSU     = (3ul << 10),
+        HCR_BSU_IS  = BITL(10),
+        HCR_FB      = BITL(9),
+        HCR_VSE     = BITL(8),
+        HCR_VI      = BITL(7),
+        HCR_VF      = BITL(6),
+        HCR_AMO     = BITL(5),
+        HCR_IMO     = BITL(4),
+        HCR_FMO     = BITL(3),
+        HCR_PTW     = BITL(2),
+        HCR_SWI     = BITL(1),
+        HCR_VM      = BITL(0),
+    };
+
+    // CPTR flags
+    enum CptrFlags {
+        CPTR_TCPAC      = BITL(31),
+        CPTR_TAM        = BITL(30),
+        CPTR_TTA        = BITL(20),
+        CPTR_TFP        = BITL(10),
+        CPTR_TZ         = BITL(8), // (EL2)
+        CPTR_EZ         = BITL(8), // (EL3)
+        CPTR_RES1       = 0x000032FFul,
+    };
+
+    // MDCR flags (EL2)
+    enum MdcrEl2Flags {
+        MDCR_EL2_TPMS           = BITL(14),
+        MDCR_EL2_E2PB_MASK      = 3ul,
+        MDCR_EL2_E2PB_SHIFT     = 12,
+        MDCR_EL2_TDRA           = BITL(11),
+        MDCR_EL2_TDOSA          = BITL(10),
+        MDCR_EL2_TDA            = BITL(9),
+        MDCR_EL2_TDE            = BITL(8),
+        MDCR_EL2_HPME           = BITL(7),
+        MDCR_EL2_TPM            = BITL(6),
+        MDCR_EL2_TPMCR          = BITL(5),
+        MDCR_EL2_HPMN_MASK      = 0x1Ful,
+    };
+
+    // Some MDSCR flags
+    enum MdscrFlags {
+        MDSCR_MDE   = BITL(15),
+        MDSCR_KDE   = BITL(13),
+        MDSCR_SS    = BITL(0),
+    };
+
+    // Some CNTHCTL flags + shifts
+    enum CnthctlFlags {
+        CNTHCTL_EVNTI_MASK      = 0xFul,
+        CNTHCTL_EVNTI_SHIFT     = 4,
+
+        CNTHCTL_EVNTDIR         = BITL(3),
+        CNTHCTL_EVNTEN          = BITL(2),
+        CNTHCTL_EL1PCEN         = BITL(1),
+        CNTHCTL_EL1PCTEN        = BITL(0),
+    };
+
+    // PAR_EL1 flags, shifts, masks
+    enum ParFlags {
+        PAR_F               = BITL(0),
+
+        // Successful translation:
+        PAR_ATTR_SHIFT      = 56,
+        PAR_ATTR_MASK       = 0xFFul,
+        PAR_PA_MASK         = MASK2L(51, 12),// bits 51-48 RES0 if not implemented
+        PAR_NS              = BITL(9),
+        PAR_SH_SHIFT        = 7,
+        PAR_SH_MASK         = 3ul,
+
+        // Faulting translation:
+        PAR_S               = BITL(9),
+        PAR_PTW             = BITL(8),
+        PAR_FST_SHIFT       = 1,
+        PAR_FST_MASK        = 0x3Ful,
+    };
+
+    // Some (S)PSR flags, masks, shifts
+    enum PsrFlags {
+        PSR_AA32_IT10_SHIFT = 25,
+        PSR_AA32_IT10_MASK  = 3ul,
+
+        PSR_SS              = BITL(21),
+
+        PSR_AA32_IT72_SHIFT = 10,
+        PSR_AA32_IT72_MASK  = 0x3Ful,
+
+        PSR_DAIF_SHIFT      = 6,
+        PSR_D               = BITL(9),
+        PSR_A               = BITL(8),
+        PSR_I               = BITL(7),
+        PSR_F               = BITL(6),
+
+        PSR_AA32_THUMB      = BITL(5),
+        PSR_MODE32          = BITL(4),
+
+        PSR_EL_SHIFT        = 2,
+        PSR_EL_MASK         = 3ul,
+
+        PSR_SP_ELX          = BITL(0),
+    };
+
+    // cnt*_ctl flags
+    enum CntCtlFlags {
+        CNTCTL_ISTATUS      = BITL(2),
+        CNTCTL_IMASK        = BITL(1),
+        CNTCTL_ENABLE       = BITL(0),
+    };
+
+    // TCR_ELx flags
+    enum TcrFlags {
+        TCR_IRGN_NC         = (0 << 8),
+        TCR_IRGN_WBWA       = (1 << 8),
+        TCR_IRGN_WT         = (2 << 8),
+        TCR_IRGN_WBNWA      = (3 << 8),
+        TCR_IRGN_MASK       = (3 << 8),
+        TCR_ORGN_NC         = (0 << 10),
+        TCR_ORGN_WBWA       = (1 << 10),
+        TCR_ORGN_WT         = (2 << 10),
+        TCR_ORGN_WBNWA      = (3 << 10),
+        TCR_ORGN_MASK       = (3 << 10),
+        TCR_NOT_SHARED      = (0 << 12),
+        TCR_SHARED_OUTER    = (2 << 12),
+        TCR_SHARED_INNER    = (3 << 12),
+        TCR_EPD1_DISABLE    = BITL(23),
+
+        TCR_EL1_RSVD        = BITL(31),
+        TCR_EL2_RSVD        = (BITL(31) | BITL(23)),
+        VTCR_EL2_RSVD       = BITL(31),
+        TCR_EL3_RSVD        = (BITL(31) | BITL(23)),
+    };
+
+    // Could have used enum class here, but can't start identifiers with a digit...
+    enum TranslationGranuleSize : u64 {
+        TranslationGranule_4K   = 0,
+        TranslationGranule_64K  = 1,
+        TranslationGranule_16K  = 2,
+    };
+
+    constexpr size_t GetTranslationGranuleBitSize(TranslationGranuleSize granuleSize)
+    {
+        switch (granuleSize) {
+            case TranslationGranule_4K:     return 12;
+            case TranslationGranule_64K:    return 16;
+            case TranslationGranule_16K:    return 14;
+            default:                        return 0;
+        }
+    }
+
+    constexpr u64 TCR_TG0(TranslationGranuleSize granuleSize)
+    {
+        return (granuleSize & 3) << 14;
+    }
+    constexpr u64 TCR_T0SZ(size_t addressSpaceSize) { return (64ul - (addressSpaceSize & 0x3F)) << 0; }
+    constexpr u64 TCR_PS(u64 n)                     { return (n & 7) << 16; }
+    constexpr u64 VTCR_SL0(u64 n)                   { return (n & 3) << 6; }
+ }

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,263 @@
+/*
+ * 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);
+}
+
+/*     u64 mdcr = GET_SYSREG(mdcr_el2);
+
+    // Trap Debug Exceptions, and accesses to debug registers.
+    mdcr |= MDCR_EL2_TDE;
+
+    // Implied from TDE
+    mdcr |= MDCR_EL2_TDRA | MDCR_EL2_TDOSA | MDCR_EL2_TDA;
+
+    SET_SYSREG(mdcr_el2, mdcr);
+*/

thermosphere/src/debug_manager.h

@@ -0,0 +1,84 @@
+/*
+ * 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"
+struct ExceptionStackFrame;
+//#include "gdb/hvisor_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;
+    struct 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/defines.hpp

@@ -0,0 +1,54 @@
+/*
+ * 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 <vapours/defines.hpp>
+#include <vapours/util/util_bitutil.hpp>
+
+#include <atomic>
+#include <utility>
+#include <optional>
+#include <functional>
+#include <tuple>
+#include <array>
+
+#include "preprocessor.h"
+#include "debug_log.h"
+
+#define SINGLETON(cl) \
+    NON_COPYABLE(cl);\
+    NON_MOVEABLE(cl);\
+    private:\
+        static cl instance;\
+    public:\
+        static cl &GetInstance() { return instance; }
+
+#define SINGLETON_WITH_ATTRS(cl, attrs) \
+    NON_COPYABLE(cl);\
+    NON_MOVEABLE(cl);\
+    private:\
+        attrs static cl instance;\
+    public:\
+        static cl &GetInstance() { return instance; }
+
+//FIXME
+#ifndef ENSURE
+#define ENSURE(...)
+#endif
+
+//FIXME
+#ifndef ENSURE2
+#define ENSURE2(...)
+#endif

thermosphere/src/drivers/arm/hvisor_drivers_arm_pl011.cpp

@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_drivers_arm_pl011.hpp"
+
+namespace ams::hvisor::drivers::arm {
+
+    void PL011::Initialize(u32 baudRate, u32 clkRate) const
+    {
+        /* 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.
+         */
+        // First, disable the UART. Flush the receive FIFO, wait for tx to complete, and disable both FIFOs.
+        m_regs->cr &= ~CR_UARTEN;
+        while (!(m_regs->fr & FR_RXFE)) {
+            m_regs->dr;
+        }
+        while (m_regs->fr & FR_BUSY);
+        // This flushes the transmit FIFO:
+        m_regs->lcr_h &= ~LCR_H_FEN;
+
+        // Divisor = clkRate / (16 * baudRate). Integer part (16 bits) in IBRD, 6 fractional bits in FBRD (fixed point)
+        // This means the encoded divisor is 2^6 * divisor = 4*clkRate / baudRate
+        u32 rawDivisor = (4 * clkRate) / baudRate;
+        m_regs->ibrd = (rawDivisor >> 6) & 0xFFFF;
+        m_regs->fbrd = rawDivisor & 0x3F;
+
+        // Select FIFO fill levels for interrupts
+        m_regs->ifls = IFLS_RX4_8 | IFLS_TX4_8;
+
+        // FIFO Enabled / No Parity / 8 Data bit / One Stop Bit
+        m_regs->lcr_h = LCR_H_FEN | LCR_H_WLEN_8;
+
+        // Select the interrupts we want to have
+        // RX timeout and TX/RX fill interrupts
+        m_regs->imsc = RTI | RXI | RXI;
+
+        // Clear any pending errors
+        m_regs->ecr = 0;
+
+        // Clear all interrupts
+        m_regs->icr = ALL_INTERRUPTS_MASK;
+
+        // Enable tx, rx, and uart overall
+        m_regs->cr = CR_RXE | CR_TXE | CR_UARTEN;
+    }
+
+    void PL011::WriteData(const void *buffer, size_t size) const
+    {
+        const u8 *buf8 = reinterpret_cast<const u8 *>(buffer);
+        for (size_t i = 0; i < size; i++) {
+            while (m_regs->fr & FR_TXFF); // while TX FIFO full
+            m_regs->dr = buf8[i]; 
+        }
+
+    }
+
+    void PL011::ReadData(void *buffer, size_t size) const
+    {
+        u8 *buf8 = reinterpret_cast<u8 *>(buffer);
+        size_t i;
+
+        for (i = 0; i < size; i++) {
+            while (m_regs->fr & FR_RXFE);
+            buf8[i] = m_regs->dr;
+        }
+
+    }
+
+    size_t PL011::ReadDataMax(void *buffer, size_t maxSize) const
+    {
+        u8 *buf8 = reinterpret_cast<u8 *>(buffer);
+        size_t count = 0;
+
+        for (size_t i = 0; i < maxSize && !(m_regs->fr & FR_RXFE); i++) {
+            buf8[i] = m_regs->dr;
+            ++count;
+        }
+
+        return count;
+
+    }
+
+    size_t PL011::ReadDataUntil(char *buffer, size_t maxSize, char delimiter) const
+    {
+        size_t count = 0;
+
+        for (size_t i = 0; i < maxSize; i++) {
+            while (m_regs->fr & FR_RXFE);
+            buffer[i] = m_regs->dr;
+            ++count;
+            if (buffer[i] == delimiter) {
+                break;
+            }
+        }
+
+        return count;
+    }
+
+    void PL011::SetRxInterruptEnabled(bool enabled) const
+    {
+        constexpr u32 mask = RTI | RXI;
+
+        // We don't support any other interrupt here.
+        m_regs->imsc = enabled ? mask : 0;
+    }
+
+}

thermosphere/src/drivers/arm/hvisor_drivers_arm_pl011.hpp

@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../../defines.hpp"
+
+// AMBA PL011 driver
+// Originally from
+/*
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+namespace ams::hvisor::drivers::arm {
+
+    class PL011 final {
+        private:
+            struct Registers {
+                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;
+            };
+            static_assert(std::is_standard_layout_v<Registers>);
+            static_assert(std::is_trivial_v<Registers>);
+
+            enum Mask : u32 {
+                DATA_ERROR_MASK     = 0x0F00,           // Data status bits
+                PL011_STATUS_ERROR_MASK = 0x0F,     // Status reg bits
+            };
+
+            enum Error : u32 {
+                OE                  = BIT(3),  // Overrun error
+                BE                  = BIT(2),  // Break error
+                PE                  = BIT(1),  // Parity error
+                FE                  = BIT(0),  // Framing error
+            };
+
+            enum Interrupt : u32 {
+                OEI                 = BIT(10), // Overrun error interrupt
+                BEI                 = BIT(9),  // Break error interrupt
+                PEI                 = BIT(8),  // Parity error interrupt
+                FEI                 = BIT(7),  // Framing error interrupt
+                RTI                 = BIT(6),  // Receive timeout interrupt
+                TXI                 = BIT(5),  // Transmit interrupt
+                RXI                 = BIT(4),  // Receive interrupt
+                DSRMI               = BIT(3),  // DSR modem interrupt
+                DCDMI               = BIT(2),  // DCD modem interrupt
+                CTSMI               = BIT(1),  // CTS modem interrupt
+                RIMI                = BIT(0),  // RI modem interrupt
+                ALL_INTERRUPTS_MASK = MASK(11),
+            };
+
+            // Flag reg bits
+            enum FrFlags : u32 {
+                FR_RI           = BIT(8),  // Ring indicator
+                FR_TXFE         = BIT(7),  // Transmit FIFO empty
+                FR_RXFF         = BIT(6),  // Receive  FIFO full
+                FR_TXFF         = BIT(5),  // Transmit FIFO full
+                FR_RXFE         = BIT(4),  // Receive  FIFO empty
+                FR_BUSY         = BIT(3),  // UART busy
+                FR_DCD          = BIT(2),  // Data carrier detect
+                FR_DSR          = BIT(1),  // Data set ready
+                FR_CTS          = BIT(0),  // Clear to send
+            };
+
+            // Control reg bits
+            enum CrFlags : u32 {
+                CR_CTSEN        = BIT(15), // CTS hardware flow control enable
+                CR_RTSEN        = BIT(14), // RTS hardware flow control enable
+                CR_RTS          = BIT(11), // Request to send
+                CR_DTR          = BIT(10), // Data transmit ready.
+                CR_RXE          = BIT(9),  // Receive enable
+                CR_TXE          = BIT(8),  // Transmit enable
+                CR_LBE          = BIT(7),  // Loopback enable
+                CR_UARTEN       = BIT(0),  // UART Enable
+            };
+
+            // Line Control Register Bits
+            enum LcrFlags : u32 {
+                LCR_H_SPS       = BIT(7),  // Stick parity select
+                LCR_H_WLEN_8    = (3 << 5),
+                LCR_H_WLEN_7    = (2 << 5),
+                LCR_H_WLEN_6    = BIT(5),
+                LCR_H_WLEN_5    = (0 << 5),
+                LCR_H_FEN       = BIT(4),  // FIFOs Enable
+                LCR_H_STP2      = BIT(3),  // Two stop bits select
+                LCR_H_EPS       = BIT(2),  // Even parity select
+                LCR_H_PEN       = BIT(1),  // Parity Enable
+                LCR_H_BRK       = BIT(0),  // Send break
+            };
+
+            // FIFO level select register
+            enum IflsLevels : u32 {
+                IFLS_RX1_8          = (0 << 3),
+                IFLS_RX2_8          = (1 << 3),
+                IFLS_RX4_8          = (2 << 3),
+                IFLS_RX6_8          = (3 << 3),
+                IFLS_RX7_8          = (4 << 3),
+                IFLS_TX1_8          = (0 << 0),
+                IFLS_TX2_8          = (1 << 0),
+                IFLS_TX4_8          = (2 << 0),
+                IFLS_TX6_8          = (3 << 0),
+                IFLS_TX7_8          = (4 << 0),
+            };
+
+        private:
+            // TODO friend
+            volatile Registers *m_regs = nullptr;
+
+        private:
+            void Initialize(u32 baudRate, u32 clkRate) const;
+
+        public:
+            void WriteData(const void *buffer, size_t size) const;
+            void ReadData(void *buffer, size_t size) const;
+            size_t ReadDataMax(void *buffer, size_t maxSize) const;
+            size_t ReadDataUntil(char *buffer, size_t maxSize, char delimiter) const;
+
+            void SetRxInterruptEnabled(bool enabled) const;
+    };
+}

thermosphere/src/drivers/tegra/hvisor_drivers_tegra_uart.cpp

@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_drivers_tegra_uart.hpp"
+#include "../../hvisor_generic_timer.hpp"
+
+#include <chrono>
+
+using namespace ams::hvisor;
+
+namespace {
+
+    inline void WaitCycles(u32 baudRate, u32 num)
+    {
+        u32 t = (num * 1000000 + 16 * baudRate - 1) / (16 * baudRate);
+        GenericTimer::GetInstance().Wait(std::chrono::microseconds{t});
+    }
+
+    inline void WaitSyms(u32 baudRate, u32 num)
+    {
+        u32 t = (num * 1000000 + baudRate - 1) / baudRate;
+        GenericTimer::GetInstance().Wait(std::chrono::microseconds{t});
+    }
+
+}
+
+namespace ams::hvisor::drivers::tegra {
+
+    void Uart::Initialize(u32 baudRate, u32 clkRate, bool invertTx) const
+    {
+        // Calculate baud rate, round to nearest (clkRate / (16 * baudRate))
+        u32 divisor = (8 * baudRate + clkRate) / (16 * baudRate);
+
+        m_regs->lcr &= ~LCR_DLAB;                               // Disable DLAB.
+        m_regs->ier = 0;                                        // Disable all interrupts.
+        m_regs->mcr = 0;
+
+        // Setup UART in FIFO mode
+        m_regs->lcr = LCR_DLAB | LCR_WD_LENGTH_8;               // Enable DLAB and set word length 8.
+        m_regs->dll = divisor & 0xFF;                           // Divisor latch LSB.
+        m_regs->dlh = (divisor >> 8) & 0xFF;                    // Divisor latch MSB.
+        m_regs->lcr &= ~LCR_DLAB;                               // Disable DLAB.
+        m_regs->spr;                                            // Dummy read.
+        WaitSyms(baudRate, 3);                                  // Wait for 3 symbols at the new baudrate.
+
+        // Enable FIFO with default settings.
+        m_regs->fcr = FCR_FCR_EN_FIFO;
+        m_regs->irda_csr = invertTx ? IRDA_CSR_INVERT_TXD : 0;  // Invert TX if needed
+        m_regs->spr;                                            // Dummy read as mandated by TRM.
+        WaitCycles(baudRate, 3);                                // Wait for 3 baud cycles, as mandated by TRM (erratum).
+
+        // Flush FIFO.
+        WaitIdle(STATUS_TX_IDLE);                               // Make sure there's no data being written in TX FIFO (TRM).
+        m_regs->fcr |= FCR_RX_CLR | FCR_TX_CLR;                 // Clear TX and RX FIFOs.
+        WaitCycles(baudRate, 32);                               // Wait for 32 baud cycles (TRM, erratum).
+
+        // Wait for idle state (TRM).
+        WaitIdle(STATUS_TX_IDLE | STATUS_RX_IDLE);
+    }
+
+    void Uart::WriteData(const void *buffer, size_t size) const
+    {
+        const u8 *buf8 = reinterpret_cast<const u8 *>(buffer);
+        for (size_t i = 0; i < size; i++) {
+            while (!(m_regs->lsr & LSR_THRE)); // Wait until it's possible to send data.
+            m_regs->thr = buf8[i]; 
+        }
+    }
+
+    void Uart::ReadData(void *buffer, size_t size) const
+    {
+        u8 *buf8 = reinterpret_cast<u8 *>(buffer);
+        for (size_t i = 0; i < size; i++) {
+            while (!(m_regs->lsr & LSR_RDR)) // Wait until it's possible to receive data.
+            buf8[i] = m_regs->rbr;
+        }
+    }
+
+    size_t Uart::ReadDataMax(void *buffer, size_t maxSize) const
+    {
+        u8 *buf8 = reinterpret_cast<u8 *>(buffer);
+        size_t count = 0;
+
+        for (size_t i = 0; i < maxSize && (m_regs->lsr & LSR_RDR); i++) {
+            buf8[i] = m_regs->rbr;
+            ++count;
+        }
+
+        return count;
+    }
+
+    size_t Uart::ReadDataUntil(char *buffer, size_t maxSize, char delimiter) const
+    {
+        size_t count = 0;
+
+        for (size_t i = 0; i < maxSize && (m_regs->lsr & LSR_RDR); i++) {
+            while (!(m_regs->lsr & LSR_RDR)) // Wait until it's possible to receive data.
+
+            buffer[i] = m_regs->rbr;
+            ++count;
+            if (buffer[i] == delimiter) {
+                break;
+            }
+        }
+
+        return count;
+    }
+
+    void Uart::SetRxInterruptEnabled(bool enabled) const
+    {
+        constexpr u32 mask = IER_IE_RX_TIMEOUT | IER_IE_RHR;
+
+        // We don't support any other interrupt here.
+        m_regs->ier = enabled ? mask : 0;
+    }
+}

thermosphere/src/drivers/tegra/hvisor_drivers_tegra_uart.hpp

@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../../defines.hpp"
+
+namespace ams::hvisor::drivers::tegra {
+
+    class Uart final {
+        private:
+            struct Registers {
+                union {
+                    // UART_THR_DLAB_0
+                    u32 thr;
+                    u32 rbr;
+                    u32 dll;
+                };
+                union {
+                    // UART_IER_DLAB_0
+                    u32 ier;
+                    u32 dlh;
+                };
+                union {
+                    // UART_IIR_FCR_0
+                    u32 iir;
+                    u32 fcr;
+                };
+                u32 lcr;
+                u32 mcr;
+                u32 lsr;
+                u32 msr;
+                u32 spr;
+                u32 irda_csr;
+                u32 rx_fifo_cfg;
+                u32 mie;
+                u32 vendor_status;
+                u8 _0x30[0x0C];
+                u32 asr;
+            };
+            static_assert(std::is_standard_layout_v<Registers>);
+            static_assert(std::is_trivial_v<Registers>);
+
+            // 36.3.12 UART_VENDOR_STATUS_0_0
+            enum VendorStatusFlags : u32 {
+                STATUS_TX_FIFO_COUNTER   = 0b111111 << 24, // reflects number of current entries in TX FIFO
+                STATUS_RX_FIFO_COUNTER   = 0b111111 << 16, // reflects number of current entries in RX FIFO
+
+                /*
+                    This bit is set to 1 when write data is issued to the TX FIFO when
+                    it is already full and gets cleared on register read (sticky bit until read):
+                        0 = NO_OVERRUN
+                        1 = OVERRUN
+                 */
+                STATUS_TX_OVERRUN        = BIT(3),
+
+                /*
+                    This bit is set to 1 when a read is issued to an empty FIFO and
+                    gets cleared on register read (sticky bit until read):
+                        0 = NO_UNDERRUN
+                        1 = UNDERRUN
+                 */
+                STATUS_RX_UNDERRUN       = BIT(2),
+                STATUS_RX_IDLE           = BIT(1),
+                STATUS_TX_IDLE           = BIT(0),
+            };
+
+            // 36.3.6 UART_LSR_0
+            enum LsrFlags : u32 {
+                LSR_RX_FIFO_EMPTY = BIT(9), // Receiver FIFO empty status
+                LSR_TX_FIFO_FULL  = BIT(8), // Transmitter FIFO full status
+                LSR_FIFOE         = BIT(7), // Receive FIFO Error
+                LSR_TMTY          = BIT(6), // Transmit Shift Register empty status
+                LSR_THRE          = BIT(5), // Transmit Holding Register is Empty -- OK to write data
+                LSR_BRK           = BIT(4), // BREAK condition detected on line
+                LSR_FERR          = BIT(3), // Framing Error
+                LSR_PERR          = BIT(2), // Parity Error
+                LSR_OVRF          = BIT(1), // Receiver Overrun Error
+                LSR_RDR           = BIT(0), // Receiver Data Ready
+            };
+
+            // 36.3.4 UART_LCR_0
+            enum LcrFlags : u32 {
+                /*
+                    STOP:
+                        0 = Transmit 1 stop bit
+                        1 = Transmit 2 stop bits (receiver always checks for 1 stop bit)
+                 */
+                LCR_DLAB        = BIT(7), // Divisor Latch Access Bit (set to allow programming of the DLH, DLM Divisors)
+                LCR_SET_B       = BIT(6), // Set BREAK condition -- Transmitter sends all zeroes to indicate BREAK
+                LCR_SET_P       = BIT(5), // Set (force) parity to value in LCR[4]
+                LCR_EVEN        = BIT(4), // Even parity format. There will always be an even number of 1s in the binary representation (PAR = 1)
+                LCR_PAR         = BIT(3), // Parity enabled
+                LCR_STOP        = BIT(2),
+                
+                LCR_WD_LENGTH_5 = 0 << 0, // word length 5
+                LCR_WD_LENGTH_6 = 1 << 0, // word length 6
+                LCR_WD_LENGTH_7 = 2 << 0, // word length 7
+                LCR_WD_LENGTH_8 = 3 << 0, // word length 8
+            };
+
+            // 36.3.3 UART_IIR_FCR_0
+            enum FcrFlags : u32{
+                // RX_TRIG
+                FCR_RX_TRIG_MASK                  = 3 << 6,
+                FCR_RX_TRIG_FIFO_COUNT_GREATER_1  = 0 << 6,
+                FCR_RX_TRIG_FIFO_COUNT_GREATER_4  = 1 << 6,
+                FCR_RX_TRIG_FIFO_COUNT_GREATER_8  = 2 << 6,
+                FCR_RX_TRIG_FIFO_COUNT_GREATER_16 = 3 << 6,
+
+                // TX_TRIG
+                FCR_TX_TRIG_MASK                  = 3 << 4,
+                FCR_TX_TRIG_FIFO_COUNT_GREATER_16 = 0 << 4,
+                FCR_TX_TRIG_FIFO_COUNT_GREATER_8  = 1 << 4,
+                FCR_TX_TRIG_FIFO_COUNT_GREATER_4  = 2 << 4,
+                FCR_TX_TRIG_FIFO_COUNT_GREATER_1  = 3 << 4,
+
+                /*
+                    DMA:
+                        0 = DMA_MODE_0
+                        1 = DMA_MODE_1
+                 */
+                FCR_DMA         = BIT(3),
+
+                /*
+                    RX/TX_CLR:
+                        Clears the contents of the receive (resp. transmit) FIFO and resets
+                        its counter logic to 0.
+                        The receive (resp. transmit) shift register is not cleared or altered.
+                        This bit returns to 0 after clearing the FIFOs.
+                 */
+                FCR_TX_CLR      = BIT(2), // See above
+                FCR_RX_CLR      = BIT(1), // See above
+                FCR_FCR_EN_FIFO = BIT(0), // Enable the transmit and receive FIFOs. This bit should be enabled
+            };
+
+            // 36.3.2 UART_IER_DLAB_0_0
+            enum IerFlags : u32 {
+                IER_IE_EORD         = BIT(5), // Interrupt enable for Interrupt Enable for End of Received Data
+                IER_IE_RX_TIMEOUT   = BIT(4), // Interrupt enable for RX FIFO timeout
+                IER_IE_MSI          = BIT(3), // Interrupt enable for Modem Status Interrupt
+                IER_IE_RXS          = BIT(2), // Interrupt enable for Receiver Line Status Interrupt
+                IER_IE_THR          = BIT(1), // Interrupt enable for Transmitter Holding Register Empty interrupt
+                IER_IE_RHR          = BIT(0), // Interrupt enable for Received Data Interrupt
+            };
+
+            // 6.3.3 UART_IIR_FCR_0
+            enum IirFlags : u32 {
+                IIR_EN_FIFO_MASK    = 3 << 6,
+                IIR_MODE_16450      = 0 << 6,
+                IIR_MODE_16550      = 1 << 6,
+
+                IIR_IS_PRI2         = BIT(3), // Encoded Interrupt ID Refer to IIR[3:0] table
+                IIR_IS_PRI1         = BIT(2), // Encoded Interrupt ID Refer to IIR[3:0] table
+                IIR_IS_PRI0         = BIT(1), // Encoded Interrupt ID Refer to IIR[3:0] table [36.3.3]
+                IIR_IS_STA          = BIT(0), // Interrupt Pending if ZERO
+            };
+
+            // 36.3.9 UART_IRDA_CSR_0
+            enum IrdaCsrFlags : u32{
+                IRDA_CSR_SIR_A                     = BIT(7),
+
+                IRDA_CSR_PWT_A_BAUD_PULSE_3_14     = 0 << 6,
+                IRDA_CSR_PWT_A_BAUD_PULSE_4_14     = 1 << 6,
+
+                IRDA_CSR_INVERT_RTS                = BIT(3),
+                IRDA_CSR_INVERT_CTS                = BIT(2),
+                IRDA_CSR_INVERT_TXD                = BIT(1),
+                IRDA_CSR_INVERT_RXD                = BIT(0),
+            };
+
+        private:
+            // TODO friend
+            volatile Registers *m_regs = nullptr;
+
+        private:
+            void Initialize(u32 baudRate, u32 clkRate, bool invertTx) const;
+            void WaitIdle(u32 status) const
+            {
+                if (status & STATUS_TX_IDLE) {
+                    while (!(m_regs->lsr & LSR_TMTY));
+                }
+
+                if (status & STATUS_RX_IDLE) {
+                    while (m_regs->lsr & LSR_RDR);
+                }
+            }
+
+        public:
+            void WriteData(const void *buffer, size_t size) const;
+            void ReadData(void *buffer, size_t size) const;
+            size_t ReadDataMax(void *buffer, size_t maxSize) const;
+            size_t ReadDataUntil(char *buffer, size_t maxSize, char delimiter) const;
+
+            void SetRxInterruptEnabled(bool enabled) const;
+    };
+}

thermosphere/src/drivers/tegra/t210/hvisor_drivers_tegra_t210_car.hpp

@@ -0,0 +1,654 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../../../defines.hpp"
+
+namespace ams::hvisor::drivers::tegra::t210 {
+
+    class Car final {
+        private:
+            struct Registers {
+                u32 rst_src;                                // _RST_SOURCE_0,                           0x000
+
+                // _RST_DEVICES_L/H/U_0 0x4-0xc
+                u32 rst_dev_l;
+                u32 rst_dev_h;
+                u32 rst_dev_u;
+
+                // _CLK_OUT_ENB_L/H/U_0 0x10-0x18
+                u32 clk_out_enb_l;
+                u32 clk_out_enb_h;
+                u32 clk_out_enb_u;
+
+                u32 _0x1C;
+                u32 cclk_brst_pol;                          // _CCLK_BURST_POLICY_0,                    0x020
+                u32 super_cclk_div;                         // _SUPER_CCLK_DIVIDER_0,                   0x024
+                u32 sclk_brst_pol;                          // _SCLK_BURST_POLICY_0,                    0x028
+                u32 super_sclk_div;                         // _SUPER_SCLK_DIVIDER_0,                   0x02c
+                u32 clk_sys_rate;                           // _CLK_SYSTEM_RATE_0,                      0x030
+                u32 prog_dly_clk;                           // _PROG_DLY_CLK_0,                         0x034
+                u32 aud_sync_clk_rate;                      // _AUDIO_SYNC_CLK_RATE_0,                  0x038
+                u32 _0x3C;
+                u32 cop_clk_skip_plcy;                      // _COP_CLK_SKIP_POLICY_0,                  0x040
+                u32 clk_mask_arm;                           // _CLK_MASK_ARM_0,                         0x044
+                u32 misc_clk_enb;                           // _MISC_CLK_ENB_0,                         0x048
+                u32 clk_cpu_cmplx;                          // _CLK_CPU_CMPLX_0,                        0x04c
+                u32 osc_ctrl;                               // _OSC_CTRL_0,                             0x050
+                u32 pll_lfsr;                               // _PLL_LFSR_0,                             0x054
+                u32 osc_freq_det;                           // _OSC_FREQ_DET_0,                         0x058
+                u32 osc_freq_det_stat;                      // _OSC_FREQ_DET_STATUS_0,                  0x05c
+                u32 _0x60[2];
+                u32 plle_ss_cntl;                           // _PLLE_SS_CNTL_0,                         0x068
+                u32 plle_misc1;                             // _PLLE_MISC1_0,                           0x06c
+                u32 _0x70[4];
+
+                // PLLC 0x80-0x8c
+                u32 pllc_base;
+                u32 pllc_out;
+                u32 pllc_misc0;
+                u32 pllc_misc1;
+
+                // PLLM 0x90-0x9c
+                u32 pllm_base;
+                u32 pllm_out;
+                u32 pllm_misc1;
+                u32 pllm_misc2;
+
+                // PLLP 0xa0-0xac
+                u32 pllp_base;
+                u32 pllp_outa;
+                u32 pllp_outb;
+                u32 pllp_misc;
+
+                // PLLA 0xb0-0xbc
+                u32 plla_base;
+                u32 plla_out;
+                u32 plla_misc0;
+                u32 plla_misc1;
+
+                // PLLU 0xc0-0xcc
+                u32 pllu_base;
+                u32 pllu_out;
+                u32 pllu_misc1;
+                u32 pllu_misc2;
+
+                // PLLD 0xd0-0xdc
+                u32 plld_base;
+                u32 plld_out;
+                u32 plld_misc1;
+                u32 plld_misc2;
+
+                // PLLX 0xe0-0xe4
+                u32 pllx_base;
+                u32 pllx_misc;
+
+                // PLLE 0xe8-0xf4
+                u32 plle_base;
+                u32 plle_misc;
+                u32 plle_ss_cntl1;
+                u32 plle_ss_cntl2;
+
+                u32 lvl2_clk_gate_ovra;                     // _LVL2_CLK_GATE_OVRA_0,                   0x0f8
+                u32 lvl2_clk_gate_ovrb;                     // _LVL2_CLK_GATE_OVRB_0,                   0x0fc
+
+                u32 clk_source_i2s2;                        // _CLK_SOURCE_I2S2_0,                      0x100
+                u32 clk_source_i2s3;                        // _CLK_SOURCE_I2S3_0,                      0x104
+                u32 clk_source_spdif_out;                   // _CLK_SOURCE_SPDIF_OUT_0,                 0x108
+                u32 clk_source_spdif_in;                    // _CLK_SOURCE_SPDIF_IN_0,                  0x10c
+                u32 clk_source_pwm;                         // _CLK_SOURCE_PWM_0,                       0x110
+                u32 _0x114;
+                u32 clk_source_spi2;                        // _CLK_SOURCE_SPI2_0,                      0x118
+                u32 clk_source_spi3;                        // _CLK_SOURCE_SPI3_0,                      0x11c
+                u32 _0x120;
+                u32 clk_source_i2c1;                        // _CLK_SOURCE_I2C1_0,                      0x124
+                u32 clk_source_i2c5;                        // _CLK_SOURCE_I2C5_0,                      0x128
+                u32 _0x12c[2];
+                u32 clk_source_spi1;                        // _CLK_SOURCE_SPI1_0,                      0x134
+                u32 clk_source_disp1;                       // _CLK_SOURCE_DISP1_0,                     0x138
+                u32 clk_source_disp2;                       // _CLK_SOURCE_DISP2_0,                     0x13c
+                u32 _0x140;
+                u32 clk_source_isp;                         // _CLK_SOURCE_ISP_0,                       0x144
+                u32 clk_source_vi;                          // _CLK_SOURCE_VI_0,                        0x148
+                u32 _0x14c;
+                u32 clk_source_sdmmc1;                      // _CLK_SOURCE_SDMMC1_0,                    0x150
+                u32 clk_source_sdmmc2;                      // _CLK_SOURCE_SDMMC2_0,                    0x154
+                u32 _0x158[3];
+                u32 clk_source_sdmmc4;                      // _CLK_SOURCE_SDMMC4_0,                    0x164
+                u32 _0x168[4];
+                u32 clk_source_uarta;                       // _CLK_SOURCE_UARTA_0,                     0x178
+                u32 clk_source_uartb;                       // _CLK_SOURCE_UARTB_0,                     0x17c
+                u32 clk_source_host1x;                      // _CLK_SOURCE_HOST1X_0,                    0x180
+                u32 _0x184[5];
+                u32 clk_source_i2c2;                        // _CLK_SOURCE_I2C2_0,                      0x198
+                u32 clk_source_emc;                         // _CLK_SOURCE_EMC_0,                       0x19c
+                u32 clk_source_uartc;                       // _CLK_SOURCE_UARTC_0,                     0x1a0
+                u32 _0x1a4;
+                u32 clk_source_vi_sensor;                   // _CLK_SOURCE_VI_SENSOR_0,                 0x1a8
+                u32 _0x1ac[2];
+                u32 clk_source_spi4;                        // _CLK_SOURCE_SPI4_0,                      0x1b4
+                u32 clk_source_i2c3;                        // _CLK_SOURCE_I2C3_0,                      0x1b8
+                u32 clk_source_sdmmc3;                      // _CLK_SOURCE_SDMMC3_0,                    0x1bc
+                u32 clk_source_uartd;                       // _CLK_SOURCE_UARTD_0,                     0x1c0
+                u32 _0x1c4[2];
+                u32 clk_source_owr;                         // _CLK_SOURCE_OWR_0,                       0x1cc
+                u32 _0x1d0;
+                u32 clk_source_csite;                       // _CLK_SOURCE_CSITE_0,                     0x1d4
+                u32 clk_source_i2s1;                        // _CLK_SOURCE_I2S1_0,                      0x1d8
+                u32 clk_source_dtv;                         // _CLK_SOURCE_DTV_0,                       0x1dc
+                u32 _0x1e0[5];
+                u32 clk_source_tsec;                        // _CLK_SOURCE_TSEC_0,                      0x1f4
+                u32 _0x1f8;
+
+                u32 clk_spare2;                             // _CLK_SPARE2_0,                           0x1fc
+                u32 _0x200[32];
+
+                u32 clk_out_enb_x;                          // _CLK_OUT_ENB_X_0,                        0x280
+                u32 clk_enb_x_set;                          // _CLK_ENB_X_SET_0,                        0x284
+                u32 clk_enb_x_clr;                          // _CLK_ENB_X_CLR_0,                        0x288
+
+                u32 rst_devices_x;                          // _RST_DEVICES_X_0,                        0x28c
+                u32 rst_dev_x_set;                          // _RST_DEV_X_SET_0,                        0x290
+                u32 rst_dev_x_clr;                          // _RST_DEV_X_CLR_0,                        0x294
+
+                u32 clk_out_enb_y;                          // _CLK_OUT_ENB_Y_0,                        0x298
+                u32 clk_enb_y_set;                          // _CLK_ENB_Y_SET_0,                        0x29c
+                u32 clk_enb_y_clr;                          // _CLK_ENB_Y_CLR_0,                        0x2a0
+
+                u32 rst_devices_y;                          // _RST_DEVICES_Y_0,                        0x2a4
+                u32 rst_dev_y_set;                          // _RST_DEV_Y_SET_0,                        0x2a8
+                u32 rst_dev_y_clr;                          // _RST_DEV_Y_CLR_0,                        0x2ac
+
+                u32 _0x2b0[17];
+                u32 dfll_base;                              // _DFLL_BASE_0,                            0x2f4
+                u32 _0x2f8[2];
+
+                // _RST_DEV_L/H/U_SET_0 0x300-0x314
+                u32 rst_dev_l_set;
+                u32 rst_dev_l_clr;
+                u32 rst_dev_h_set;
+                u32 rst_dev_h_clr;
+                u32 rst_dev_u_set;
+                u32 rst_dev_u_clr;
+
+                u32 _0x318[2];
+
+                // _CLK_ENB_L/H/U_CLR_0 0x320-0x334
+                u32 clk_enb_l_set;
+                u32 clk_enb_l_clr;
+                u32 clk_enb_h_set;
+                u32 clk_enb_h_clr;
+                u32 clk_enb_u_set;
+                u32 clk_enb_u_clr;
+
+                u32 _0x338;
+                u32 ccplex_pg_sm_ovrd;                      // _CCPLEX_PG_SM_OVRD_0,                    0x33c
+                u32 rst_cpu_cmplx_set;                      // _RST_CPU_CMPLX_SET_0,                    0x340
+                u32 rst_cpu_cmplx_clr;                      // _RST_CPU_CMPLX_CLR_0,                    0x344
+
+                // Additional (T30) registers
+                u32 clk_cpu_cmplx_set;                      // _CLK_CPU_CMPLX_SET_0,                    0x348
+                u32 clk_cpu_cmplx_clr;                      // _CLK_CPU_CMPLX_SET_0,                    0x34c
+
+                u32 _0x350[2];
+                u32 rst_dev_v;                              // _RST_DEVICES_V_0,                        0x358
+                u32 rst_dev_w;                              // _RST_DEVICES_W_0,                        0x35c
+                u32 clk_out_enb_v;                          // _CLK_OUT_ENB_V_0,                        0x360
+                u32 clk_out_enb_w;                          // _CLK_OUT_ENB_W_0,                        0x364
+                u32 cclkg_brst_pol;                         // _CCLKG_BURST_POLICY_0,                   0x368
+                u32 super_cclkg_div;                        // _SUPER_CCLKG_DIVIDER_0,                  0x36c
+                u32 cclklp_brst_pol;                        // _CCLKLP_BURST_POLICY_0,                  0x370
+                u32 super_cclkp_div;                        // _SUPER_CCLKLP_DIVIDER_0,                 0x374
+                u32 clk_cpug_cmplx;                         // _CLK_CPUG_CMPLX_0,                       0x378
+                u32 clk_cpulp_cmplx;                        // _CLK_CPULP_CMPLX_0,                      0x37c
+                u32 cpu_softrst_ctrl;                       // _CPU_SOFTRST_CTRL_0,                     0x380
+                u32 cpu_softrst_ctrl1;                      // _CPU_SOFTRST_CTRL1_0,                    0x384
+                u32 cpu_softrst_ctrl2;                      // _CPU_SOFTRST_CTRL2_0,                    0x388
+                u32 _0x38c[5];
+                u32 lvl2_clk_gate_ovrc;                     // _LVL2_CLK_GATE_OVRC,                     0x3a0
+                u32 lvl2_clk_gate_ovrd;                     // _LVL2_CLK_GATE_OVRD,                     0x3a4
+                u32 _0x3a8[2];
+
+                u32 _0x3b0;
+                u32 clk_source_mselect;                     // _CLK_SOURCE_MSELECT_0,                   0x3b4
+                u32 clk_source_tsensor;                     // _CLK_SOURCE_TSENSOR_0,                   0x3b8
+                u32 clk_source_i2s4;                        // _CLK_SOURCE_I2S4_0,                      0x3bc
+                u32 clk_source_i2s5;                        // _CLK_SOURCE_I2S5_0,                      0x3c0
+                u32 clk_source_i2c4;                        // _CLK_SOURCE_I2C4_0,                      0x3c4
+                u32 _0x3c8[2];
+                u32 clk_source_ahub;                        // _CLK_SOURCE_AHUB_0,                      0x3d0
+                u32 _0x3d4[4];
+                u32 clk_source_hda2codec_2x;                // _CLK_SOURCE_HDA2CODEC_2X_0,              0x3e4
+                u32 clk_source_actmon;                      // _CLK_SOURCE_ACTMON_0,                    0x3e8
+                u32 clk_source_extperiph1;                  // _CLK_SOURCE_EXTPERIPH1_0,                0x3ec
+                u32 clk_source_extperiph2;                  // _CLK_SOURCE_EXTPERIPH2_0,                0x3f0
+                u32 clk_source_extperiph3;                  // _CLK_SOURCE_EXTPERIPH3_0,                0x3f4
+                u32 _0x3f8;
+                u32 clk_source_i2c_slow;                    // _CLK_SOURCE_I2C_SLOW_0,                  0x3fc
+                u32 clk_source_sys;                         // _CLK_SOURCE_SYS_0,                       0x400
+                u32 clk_source_ispb;                        // _CLK_SOURCE_ISPB_0,                      0x404
+                u32 _0x408[2];
+                u32 clk_source_sor1;                        // _CLK_SOURCE_SOR1_0,                      0x410
+                u32 clk_source_sor0;                        // _CLK_SOURCE_SOR0_0,                      0x414
+                u32 _0x418[2];
+                u32 clk_source_sata_oob;                    // _CLK_SOURCE_SATA_OOB_0,                  0x420
+                u32 clk_source_sata;                        // _CLK_SOURCE_SATA_0,                      0x424
+                u32 clk_source_hda;                         // _CLK_SOURCE_HDA_0,                       0x428
+                u32 _0x42c;
+
+                // _RST_DEV_V/W_SET_0 0x430-0x43c
+                u32 rst_dev_v_set;
+                u32 rst_dev_v_clr;
+                u32 rst_dev_w_set;
+                u32 rst_dev_w_clr;
+
+                // _CLK_ENB_V/W_CLR_0 0x440-0x44c
+                u32 clk_enb_v_set;
+                u32 clk_enb_v_clr;
+                u32 clk_enb_w_set;
+                u32 clk_enb_w_clr;
+
+                // Additional (T114+) registers
+                u32 rst_cpug_cmplx_set;                     // _RST_CPUG_CMPLX_SET_0,                   0x450
+                u32 rst_cpug_cmplx_clr;                     // _RST_CPUG_CMPLX_CLR_0,                   0x454
+                u32 rst_cpulp_cmplx_set;                    // _RST_CPULP_CMPLX_SET_0,                  0x458
+                u32 rst_cpulp_cmplx_clr;                    // _RST_CPULP_CMPLX_CLR_0,                  0x45c
+                u32 clk_cpug_cmplx_set;                     // _CLK_CPUG_CMPLX_SET_0,                   0x460
+                u32 clk_cpug_cmplx_clr;                     // _CLK_CPUG_CMPLX_CLR_0,                   0x464
+                u32 clk_cpulp_cmplx_set;                    // _CLK_CPULP_CMPLX_SET_0,                  0x468
+                u32 clk_cpulp_cmplx_clr;                    // _CLK_CPULP_CMPLX_CLR_0,                  0x46c
+                u32 cpu_cmplx_status;                       // _CPU_CMPLX_STATUS_0,                     0x470
+                u32 _0x474;                                 
+                u32 intstatus;                              // _INTSTATUS_0,                            0x478
+                u32 intmask;                                // _INTMASK_0,                              0x47c
+                u32 utmip_pll_cfg0;                         // _UTMIP_PLL_CFG0_0,                       0x480
+                u32 utmip_pll_cfg1;                         // _UTMIP_PLL_CFG1_0,                       0x484
+                u32 utmip_pll_cfg2;                         // _UTMIP_PLL_CFG2_0,                       0x488
+
+                u32 plle_aux;                               // _PLLE_AUX_0,                             0x48c
+                u32 sata_pll_cfg0;                          // _SATA_PLL_CFG0_0,                        0x490
+                u32 sata_pll_cfg1;                          // _SATA_PLL_CFG1_0,                        0x494
+                u32 pcie_pll_cfg0;                          // _PCIE_PLL_CFG0_0,                        0x498
+
+                u32 prog_audio_dly_clk;                     // _PROG_AUDIO_DLY_CLK_0,                   0x49c
+                u32 audio_sync_clk_i2s0;                    // _AUDIO_SYNC_CLK_I2S0_0,                  0x4a0
+                u32 audio_sync_clk_i2s1;                    // _AUDIO_SYNC_CLK_I2S1_0,                  0x4a4
+                u32 audio_sync_clk_i2s2;                    // _AUDIO_SYNC_CLK_I2S2_0,                  0x4a8
+                u32 audio_sync_clk_i2s3;                    // _AUDIO_SYNC_CLK_I2S3_0,                  0x4ac
+                u32 audio_sync_clk_i2s4;                    // _AUDIO_SYNC_CLK_I2S4_0,                  0x4b0
+                u32 audio_sync_clk_spdif;                   // _AUDIO_SYNC_CLK_SPDIF_0,                 0x4b4
+
+                u32 plld2_base;                             // _PLLD2_BASE_0,                           0x4b8
+                u32 plld2_misc;                             // _PLLD2_MISC_0,                           0x4bc
+                u32 utmip_pll_cfg3;                         // _UTMIP_PLL_CFG3_0,                       0x4c0
+                u32 pllrefe_base;                           // _PLLREFE_BASE_0,                         0x4c4
+                u32 pllrefe_misc;                           // _PLLREFE_MISC_0,                         0x4c8
+                u32 pllrefe_out;                            // _PLLREFE_OUT_0,                          0x4cc
+                u32 cpu_finetrim_byp;                       // _CPU_FINETRIM_BYP_0,                     0x4d0
+                u32 cpu_finetrim_select;                    // _CPU_FINETRIM_SELECT_0,                  0x4d4
+                u32 cpu_finetrim_dr;                        // _CPU_FINETRIM_DR_0,                      0x4d8
+                u32 cpu_finetrim_df;                        // _CPU_FINETRIM_DF_0,                      0x4dc
+                u32 cpu_finetrim_f;                         // _CPU_FINETRIM_F_0,                       0x4e0
+                u32 cpu_finetrim_r;                         // _CPU_FINETRIM_R_0,                       0x4e4
+                u32 pllc2_base;                             // _PLLC2_BASE_0,                           0x4e8
+                u32 pllc2_misc0;                            // _PLLC2_MISC_0_0,                         0x4ec
+                u32 pllc2_misc1;                            // _PLLC2_MISC_1_0,                         0x4f0
+                u32 pllc2_misc2;                            // _PLLC2_MISC_2_0,                         0x4f4
+                u32 pllc2_misc3;                            // _PLLC2_MISC_3_0,                         0x4f8
+                u32 pllc3_base;                             // _PLLC3_BASE_0,                           0x4fc
+                u32 pllc3_misc0;                            // _PLLC3_MISC_0_0,                         0x500
+                u32 pllc3_misc1;                            // _PLLC3_MISC_1_0,                         0x504
+                u32 pllc3_misc2;                            // _PLLC3_MISC_2_0,                         0x508
+                u32 pllc3_misc3;                            // _PLLC3_MISC_3_0,                         0x50c
+                u32 pllx_misc1;                             // _PLLX_MISC_1_0,                          0x510
+                u32 pllx_misc2;                             // _PLLX_MISC_2_0,                          0x514
+                u32 pllx_misc3;                             // _PLLX_MISC_3_0,                          0x518
+                u32 xusbio_pll_cfg0;                        // _XUSBIO_PLL_CFG0_0,                      0x51c
+                u32 xusbio_pll_cfg1;                        // _XUSBIO_PLL_CFG0_1,                      0x520
+                u32 plle_aux1;                              // _PLLE_AUX1_0,                            0x524
+                u32 pllp_reshift;                           // _PLLP_RESHIFT_0,                         0x528
+                u32 utmipll_hw_pwrdn_cfg0;                  // _UTMIPLL_HW_PWRDN_CFG0_0,                0x52c
+                u32 pllu_hw_pwrdn_cfg0;                     // _PLLU_HW_PWRDN_CFG0_0,                   0x530
+                u32 xusb_pll_cfg0;                          // _XUSB_PLL_CFG0_0,                        0x534
+                u32 _0x538;
+                u32 clk_cpu_misc;                           // _CLK_CPU_MISC_0,                         0x53c
+                u32 clk_cpug_misc;                          // _CLK_CPUG_MISC_0,                        0x540
+                u32 clk_cpulp_misc;                         // _CLK_CPULP_MISC_0,                       0x544
+                u32 pllx_hw_ctrl_cfg;                       // _PLLX_HW_CTRL_CFG_0,                     0x548
+                u32 pllx_sw_ramp_cfg;                       // _PLLX_SW_RAMP_CFG_0,                     0x54c
+                u32 pllx_hw_ctrl_status;                    // _PLLX_HW_CTRL_STATUS_0,                  0x550
+                u32 lvl2_clk_gate_ovre;                     // _LVL2_CLK_GATE_OVRE,                     0x554
+                u32 super_gr3d_clk_div;                     // _SUPER_GR3D_CLK_DIVIDER_0,               0x558
+                u32 spare_reg0;                             // _SPARE_REG0_0,                           0x55c
+                u32 audio_sync_clk_dmic1;                   // _AUDIO_SYNC_CLK_DMIC1_0,                 0x560
+                u32 audio_sync_clk_dmic2;                   // _AUDIO_SYNC_CLK_DMIC2_0,                 0x564
+
+                u32 _0x568[2];
+                u32 plld2_ss_cfg;                           // _PLLD2_SS_CFG,                           0x570
+                u32 plld2_ss_ctrl1;                         // _PLLD2_SS_CTRL1_0,                       0x574
+                u32 plld2_ss_ctrl2;                         // _PLLD2_SS_CTRL2_0,                       0x578
+                u32 _0x57c[5];
+
+                u32 plldp_base;                             // _PLLDP_BASE,                             0x590
+                u32 plldp_misc;                             // _PLLDP_MISC,                             0x594
+                u32 plldp_ss_cfg;                           // _PLLDP_SS_CFG,                           0x598
+                u32 plldp_ss_ctrl1;                         // _PLLDP_SS_CTRL1_0,                       0x59c
+                u32 plldp_ss_ctrl2;                         // _PLLDP_SS_CTRL2_0,                       0x5a0
+                u32 pllc4_base;                             // _PLLC4_BASE_0,                           0x5a4
+                u32 pllc4_misc;                             // _PLLC4_MISC_0,                           0x5a8
+                u32 _0x5ac[6];
+                u32 clk_spare0;                             // _CLK_SPARE0_0,                           0x5c4
+                u32 clk_spare1;                             // _CLK_SPARE1_0,                           0x5c8
+                u32 gpu_isob_ctrl;                          // _GPU_ISOB_CTRL_0,                        0x5cc
+                u32 pllc_misc2;                             // _PLLC_MISC_2_0,                          0x5d0
+                u32 pllc_misc3;                             // _PLLC_MISC_3_0,                          0x5d4
+                u32 plla_misc2;                             // _PLLA_MISC2_0,                           0x5d8
+                u32 _0x5dc[2];
+                u32 pllc4_out;                              // _PLLC4_OUT_0,                            0x5e4
+                u32 pllmb_base;                             // _PLLMB_BASE_0,                           0x5e8
+                u32 pllmb_misc1;                            // _PLLMB_MISC1_0,                          0x5ec
+                u32 pllx_misc4;                             // _PLLX_MISC_4_0,                          0x5f0
+                u32 pllx_misc5;                             // _PLLX_MISC_5_0,                          0x5f4
+                u32 _0x5f8[2];
+
+                u32 clk_source_xusb_core_host;              // _CLK_SOURCE_XUSB_CORE_HOST_0,            0x600
+                u32 clk_source_xusb_falcon;                 // _CLK_SOURCE_XUSB_FALCON_0,               0x604
+                u32 clk_source_xusb_fs;                     // _CLK_SOURCE_XUSB_FS_0,                   0x608
+                u32 clk_source_xusb_core_dev;               // _CLK_SOURCE_XUSB_CORE_DEV_0,             0x60c
+                u32 clk_source_xusb_ss;                     // _CLK_SOURCE_XUSB_SS_0,                   0x610
+                u32 clk_source_cilab;                       // _CLK_SOURCE_CILAB_0,                     0x614
+                u32 clk_source_cilcd;                       // _CLK_SOURCE_CILCD_0,                     0x618
+                u32 clk_source_cilef;                       // _CLK_SOURCE_CILEF_0,                     0x61c
+                u32 clk_source_dsia_lp;                     // _CLK_SOURCE_DSIA_LP_0,                   0x620
+                u32 clk_source_dsib_lp;                     // _CLK_SOURCE_DSIB_LP_0,                   0x624
+                u32 clk_source_entropy;                     // _CLK_SOURCE_ENTROPY_0,                   0x628
+                u32 clk_source_dvfs_ref;                    // _CLK_SOURCE_DVFS_REF_0,                  0x62c
+                u32 clk_source_dvfs_soc;                    // _CLK_SOURCE_DVFS_SOC_0,                  0x630
+                u32 _0x634[3];
+                u32 clk_source_emc_latency;                 // _CLK_SOURCE_EMC_LATENCY_0,               0x640
+                u32 clk_source_soc_therm;                   // _CLK_SOURCE_SOC_THERM_0,                 0x644
+                u32 _0x648;
+                u32 clk_source_dmic1;                       // _CLK_SOURCE_DMIC1_0,                     0x64c
+                u32 clk_source_dmic2;                       // _CLK_SOURCE_DMIC2_0,                     0x650
+                u32 _0x654;
+                u32 clk_source_vi_sensor2;                  // _CLK_SOURCE_VI_SENSOR2_0,                0x658
+                u32 clk_source_i2c6;                        // _CLK_SOURCE_I2C6_0,                      0x65c
+                u32 clk_source_mipibif;                     // _CLK_SOURCE_MIPIBIF_0,                   0x660
+                u32 clk_source_emc_dll;                     // _CLK_SOURCE_EMC_DLL_0,                   0x664
+                u32 _0x668;
+                u32 clk_source_uart_fst_mipi_cal;           // _CLK_SOURCE_UART_FST_MIPI_CAL_0,         0x66c
+                u32 _0x670[2];
+                u32 clk_source_vic;                         // _CLK_SOURCE_VIC_0,                       0x678
+
+                u32 pllp_outc;                              // _PLLP_OUTC_0,                            0x67c
+                u32 pllp_misc1;                             // _PLLP_MISC1_0,                           0x680
+                u32 _0x684[2];
+                u32 emc_div_clk_shaper_ctrl;                // _EMC_DIV_CLK_SHAPER_CTRL_0,              0x68c
+                u32 emc_pllc_shaper_ctrl;                   // _EMC_PLLC_SHAPER_CTRL_0,                 0x690
+
+                u32 clk_source_sdmmc_legacy_tm;             // _CLK_SOURCE_SDMMC_LEGACY_TM_0,           0x694
+                u32 clk_source_nvdec;                       // _CLK_SOURCE_NVDEC_0,                     0x698
+                u32 clk_source_nvjpg;                       // _CLK_SOURCE_NVJPG_0,                     0x69c
+                u32 clk_source_nvenc;                       // _CLK_SOURCE_NVENC_0,                     0x6a0
+
+                u32 plla1_base;                             // _PLLA1_BASE_0,                           0x6a4
+                u32 plla1_misc0;                            // _PLLA1_MISC_0_0,                         0x6a8
+                u32 plla1_misc1;                            // _PLLA1_MISC_1_0,                         0x6ac
+                u32 plla1_misc2;                            // _PLLA1_MISC_2_0,                         0x6b0
+                u32 plla1_misc3;                            // _PLLA1_MISC_3_0,                         0x6b4
+                u32 audio_sync_clk_dmic3;                   // _AUDIO_SYNC_CLK_DMIC3_0,                 0x6b8
+
+                u32 clk_source_dmic3;                       // _CLK_SOURCE_DMIC3_0,                     0x6bc
+                u32 clk_source_ape;                         // _CLK_SOURCE_APE_0,                       0x6c0
+                u32 clk_source_qspi;                        // _CLK_SOURCE_QSPI_0,                      0x6c4
+                u32 clk_source_vi_i2c;                      // _CLK_SOURCE_VI_I2C_0,                    0x6c8
+                u32 clk_source_usb2_hsic_trk;               // _CLK_SOURCE_USB2_HSIC_TRK_0,             0x6cc
+                u32 clk_source_pex_sata_usb_rx_byp;         // _CLK_SOURCE_PEX_SATA_USB_RX_BYP_0,       0x6d0
+                u32 clk_source_maud;                        // _CLK_SOURCE_MAUD_0,                      0x6d4
+                u32 clk_source_tsecb;                       // _CLK_SOURCE_TSECB_0,                     0x6d8
+
+                u32 clk_cpug_misc1;                         // _CLK_CPUG_MISC1_0, 0x6dc
+                u32 aclk_burst_policy;                      // _ACLK_BURST_POLICY_0,                    0x6e0
+                u32 super_aclk_divider;                     // _SUPER_ACLK_DIVIDER_0,                   0x6e4
+
+                u32 nvenc_super_clk_divider;                // _NVENC_SUPER_CLK_DIVIDER_0,              0x6e8
+                u32 vi_super_clk_divider;                   // _VI_SUPER_CLK_DIVIDER_0,                 0x6ec
+                u32 vic_super_clk_divider;                  // _VIC_SUPER_CLK_DIVIDER_0,                0x6f0
+                u32 nvdec_super_clk_divider;                // _NVDEC_SUPER_CLK_DIVIDER_0,              0x6f4
+                u32 isp_super_clk_divider;                  // _ISP_SUPER_CLK_DIVIDER_0,                0x6f8
+                u32 ispb_super_clk_divider;                 // _ISPB_SUPER_CLK_DIVIDER_0,               0x6fc
+                u32 nvjpg_super_clk_divider;                // _NVJPG_SUPER_CLK_DIVIDER_0,              0x700
+                u32 se_super_clk_divider;                   // _SE_SUPER_CLK_DIVIDER_0,                 0x704
+                u32 tsec_super_clk_divider;                 // _TSEC_SUPER_CLK_DIVIDER_0,               0x708
+                u32 tsecb_super_clk_divider;                // _TSECB_SUPER_CLK_DIVIDER_0,              0x70c
+
+                u32 clk_source_uartape;                     // _CLK_SOURCE_UARTAPE_0,                   0x710
+                u32 clk_cpug_misc2;                         // _CLK_CPUG_MISC2_0,                       0x714
+                u32 clk_source_dbgapb;                      // _CLK_SOURCE_DBGAPB_0,                    0x718
+                u32 clk_ccplex_cc4_ret_clk_enb;             // _CLK_CCPLEX_CC4_RET_CLK_ENB_0,           0x71c
+                u32 actmon_cpu_clk;                         // _ACTMON_CPU_CLK_0,                       0x720
+                u32 clk_source_emc_safe;                    // _CLK_SOURCE_EMC_SAFE_0,                  0x724
+                u32 sdmmc2_pllc4_out0_shaper_ctrl;          // _SDMMC2_PLLC4_OUT0_SHAPER_CTRL_0,        0x728
+                u32 sdmmc2_pllc4_out1_shaper_ctrl;          // _SDMMC2_PLLC4_OUT1_SHAPER_CTRL_0,        0x72c
+                u32 sdmmc2_pllc4_out2_shaper_ctrl;          // _SDMMC2_PLLC4_OUT2_SHAPER_CTRL_0,        0x730
+                u32 sdmmc2_div_clk_shaper_ctrl;             // _SDMMC2_DIV_CLK_SHAPER_CTRL_0,           0x734
+                u32 sdmmc4_pllc4_out0_shaper_ctrl;          // _SDMMC4_PLLC4_OUT0_SHAPER_CTRL_0,        0x738
+                u32 sdmmc4_pllc4_out1_shaper_ctrl;          // _SDMMC4_PLLC4_OUT1_SHAPER_CTRL_0,        0x73c
+                u32 sdmmc4_pllc4_out2_shaper_ctrl;          // _SDMMC4_PLLC4_OUT2_SHAPER_CTRL_0,        0x740
+                u32 sdmmc4_div_clk_shaper_ctrl;             // _SDMMC4_DIV_CLK_SHAPER_CTRL_0,           0x744
+            };
+            static_assert(std::is_standard_layout_v<Registers>);
+            static_assert(std::is_trivial_v<Registers>);
+
+        private:
+            static constexpr u32 clkRegOffsets[] = { 0x010, 0x014, 0x018, 0x360, 0x364, 0x280, 0x298 };
+            static constexpr u32 rstRegOffsets[] = { 0x004, 0x008, 0x00C, 0x358, 0x35C, 0x28C, 0x2A4 };
+
+        private:
+            volatile Registers *m_regs = nullptr;
+            // TODO friend
+
+        private:
+            vu32 *RegisterAt(u32 offset) const
+            {
+                return reinterpret_cast<vu32 *>(reinterpret_cast<uintptr_t>(m_regs) + offset);
+            }
+
+        public:
+            union Device {
+                struct {
+                    u32 bank        : 3;
+                    u32 bitPos      : 5;
+                    u32 regOffset   : 12;
+                    u32 value       : 3;
+                    u32             : 0;
+                    u32 divisor     : 16;
+                };
+                u64 raw;
+            };
+            static_assert(std::is_standard_layout_v<Device>);
+            static_assert(std::is_trivial_v<Device>);
+            static_assert(sizeof(Device) == 8);
+
+            static constexpr Device uartA = {{
+                .bank = 5,
+                .bitPos = 6,
+                .regOffset = 0x178,
+                .value = 0,
+                .divisor = 0,
+            }};
+            static constexpr Device uartB = {{
+                .bank = 0,
+                .bitPos = 7,
+                .regOffset = 0x17C,
+                .value = 0,
+                .divisor = 0,
+            }};
+            static constexpr Device uartC = {{
+                .bank = 1,
+                .bitPos = 23,
+                .regOffset = 0x1A0,
+                .value = 0,
+                .divisor = 0,
+            }};
+            static constexpr Device uartD = {{
+                .bank = 2,
+                .bitPos = 1,
+                .regOffset = 0x1C0,
+                .value = 0,
+                .divisor = 0,
+            }};
+            static constexpr Device i2c1 = {{
+                .bank = 0,
+                .bitPos = 12,
+                .regOffset = 0x124,
+                .value = 6,
+                .divisor = 0,
+            }};
+            static constexpr Device i2c5 = {{
+                .bank = 1,
+                .bitPos = 15,
+                .regOffset = 0x128,
+                .value = 6,
+                .divisor = 0,
+            }};
+            static constexpr Device tzram = {{
+                .bank = 3,
+                .bitPos = 30,
+            }};
+            static constexpr Device se = {{
+                .bank = 3,
+                .bitPos = 31,
+                .regOffset = 0x42C,
+                .value = 0,
+                .divisor = 0,
+            }};
+            static constexpr Device host1x = {{
+                .bank = 0,
+                .bitPos = 28,
+                .regOffset = 0x180,
+                .value = 4,
+                .divisor = 3,
+            }};
+            static constexpr Device tsec = {{
+                .bank = 2,
+                .bitPos = 19,
+                .regOffset = 0x1F4,
+                .value = 0,
+                .divisor = 2,
+            }};
+            static constexpr Device sorSafe = {{
+                .bank = 6,
+                .bitPos = 30,
+            }};
+            static constexpr Device sor0 = {{
+                .bank = 5,
+                .bitPos = 22,
+            }};
+            static constexpr Device sor1 = {{
+                .bank = 5,
+                .bitPos = 30,
+                .regOffset = 0x410,
+                .value = 0,
+                .divisor = 2,
+            }};
+            static constexpr Device kfuse = {{
+                .bank = 1,
+                .bitPos = 8,
+            }};
+            static constexpr Device clDvfs = {{
+                .bank = 4,
+                .bitPos = 27,
+            }};
+            static constexpr Device bpmp = {{
+                .bank = 0,
+                .bitPos = 1,
+            }};
+            static constexpr Device actmon = {{
+                .bank = 3,
+                .bitPos = 23,
+                .regOffset = 0x3E8,
+                .value = 6,
+                .divisor = 0,
+            }};
+            static constexpr Device coresight = {{
+                .bank = 2,
+                .bitPos = 9,
+                .regOffset = 0x1D4,
+                .value = 0,
+                .divisor = 4,
+            }};
+
+        public:
+            void EnableClock(Device dev) const
+            {
+                // Configure default PLL and divisor
+                if (dev.regOffset != 0) {
+                    *RegisterAt(dev.regOffset) = dev.value << 29 | dev.divisor;
+                }
+
+                // Enable the clock
+                *RegisterAt(clkRegOffsets[dev.bank]) |= BIT(dev.bitPos);
+            }
+
+            void DisableClock(Device dev) const
+            {
+                *RegisterAt(clkRegOffsets[dev.bank]) &= ~BIT(dev.bitPos);
+            }
+
+            void EnableReset(Device dev) const
+            {
+                *RegisterAt(rstRegOffsets[dev.bank]) |= BIT(dev.bitPos);
+            }
+
+            void DisableReset(Device dev) const
+            {
+                *RegisterAt(rstRegOffsets[dev.bank]) &= ~BIT(dev.bitPos);
+            }
+
+            void Enable(Device dev) const
+            {
+                EnableClock(dev);
+                DisableReset(dev);
+            }
+
+            void Disable(Device dev) const
+            {
+                EnableReset(dev);
+                DisableClock(dev);
+            }
+
+            void Reboot(Device dev) const {
+                Disable(dev);
+                // KFUSE needs a workaround
+                /*if (dev.raw == kfuse.raw) {
+                    EnableClock(dev);
+                    // Wait 100us
+                    DisableReset(dev);
+                    // Wait 200us
+                }*/
+                Enable(dev);
+            }
+
+            void SetFuseRegsEnabled(bool enabled) const
+            {
+                u32 mask = enabled ? BIT(28) : 0;
+                m_regs->misc_clk_enb = (m_regs->misc_clk_enb & ~BIT(28)) | mask;
+            }
+    };
+
+}

thermosphere/src/drivers/tegra/t210/hvisor_drivers_tegra_t210_gpio.hpp

@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../../../defines.hpp"
+
+namespace ams::hvisor::drivers::tegra::t210 {
+
+    class Gpio final {
+        private:
+            static constexpr size_t numBanks = 8;
+            static constexpr size_t portsPerBank = 4;
+            struct Bank {
+                u32 cnf[portsPerBank];
+                u32 oe[portsPerBank];
+                u32 out[portsPerBank];
+                u32 in[portsPerBank];
+                u32 int_sta[portsPerBank];
+                u32 int_enb[portsPerBank];
+                u32 int_lvl[portsPerBank];
+                u32 int_clr[portsPerBank];
+                u32 msk_cnf[portsPerBank];
+                u32 msk_oe[portsPerBank];
+                u32 msk_out[portsPerBank];
+                u32 db_ctrl_p[portsPerBank];
+                u32 msk_int_sta[portsPerBank];
+                u32 msk_int_enb[portsPerBank];
+                u32 msk_int_lvl[portsPerBank];
+                u32 db_cnt_p[portsPerBank];
+            };
+
+            struct Registers {
+                Bank bank[numBanks];
+            };
+            static_assert(std::is_standard_layout_v<Registers>);
+            static_assert(std::is_trivial_v<Registers>);
+
+        public:
+            enum Port {
+                PORT_A = 0,
+                PORT_B = 1,
+                PORT_C = 2,
+                PORT_D = 3,
+                PORT_E = 4,
+                PORT_F = 5,
+                PORT_G = 6,
+                PORT_H = 7,
+                PORT_I = 8,
+                PORT_J = 9,
+                PORT_K = 10,
+                PORT_L = 11,
+                PORT_M = 12,
+                PORT_N = 13,
+                PORT_O = 14,
+                PORT_P = 15,
+                PORT_Q = 16,
+                PORT_R = 17,
+                PORT_S = 18,
+                PORT_T = 19,
+                PORT_U = 20,
+                PORT_V = 21,
+                PORT_W = 22,
+                PORT_X = 23,
+                PORT_Y = 24,
+                PORT_Z = 25,
+                PORT_AA = 26,
+                PORT_BB = 27,
+                PORT_CC = 28,
+                PORT_DD = 29,
+                PORT_EE = 30,
+                PORT_FF = 31,
+            };
+
+            enum class Mode {
+                Sfio = 0,
+                Gpio = 1,
+            };
+
+            enum class Direction {
+                Tristate    = 0,   // Input
+                Driven      = 1,     // Output
+
+                Input       = Tristate,
+                Output      = Driven, 
+            };
+
+            enum class Level {
+                Low = 0,
+                High = 1,
+            };
+
+        private:
+            // For msk_* fields
+            static constexpr u32 MakeMaskedWriteValue(u32 pos, bool value)
+            {
+                return BIT(8 + pos) | ((value ? 1 : 0) << pos);
+            }
+
+            static constexpr u32 MakeMaskedWriteValueContiguous(u32 pos, size_t n, bool value)
+            {
+                u32 msk = MASK2(pos + n - 1, pos);
+                return (msk << 8) | (value ? msk : 0);
+            }
+
+        private:
+            struct Pin {
+                Port port;
+                u8 pos;
+            };
+            static_assert(std::is_standard_layout_v<Pin>);
+            static_assert(std::is_trivial_v<Pin>);
+            static_assert(sizeof(Pin) <= 8);
+
+
+        public:
+            static constexpr Pin uart3Tx             = {PORT_D, 1};
+            static constexpr Pin uart3Rx             = {PORT_D, 2};
+            static constexpr Pin uart3Rts            = {PORT_D, 3};
+            static constexpr Pin uart3Cts            = {PORT_D, 4};
+            static constexpr Pin uart2Tx             = {PORT_G, 0};
+            static constexpr Pin uart2Rx             = {PORT_G, 1};
+            static constexpr Pin uart2Rts            = {PORT_G, 2};
+            static constexpr Pin uart2Cts            = {PORT_G, 3};
+            static constexpr Pin uart4Tx             = {PORT_I, 4};
+            static constexpr Pin uart4Rx             = {PORT_I, 5};
+            static constexpr Pin uart4Rts            = {PORT_I, 6};
+            static constexpr Pin uart4Cts            = {PORT_I, 7};
+            static constexpr Pin uart1Tx             = {PORT_U, 0};
+            static constexpr Pin uart1Rx             = {PORT_U, 1};
+            static constexpr Pin uart1Rts            = {PORT_U, 2};
+            static constexpr Pin uart1Cts            = {PORT_U, 3};
+
+            static constexpr Pin volUp               = {PORT_X, 6};
+            static constexpr Pin volDown             = {PORT_X, 7};
+            static constexpr Pin microSdCardDetect   = {PORT_Z, 1};
+            static constexpr Pin microSdWriteProtect = {PORT_Z, 4};
+            static constexpr Pin microSdSupplyEnable = {PORT_E, 4};
+            static constexpr Pin lcdBlP5v            = {PORT_I, 0};
+            static constexpr Pin lcdBlN5v            = {PORT_I, 1};
+            static constexpr Pin lcdBlPwm            = {PORT_V, 0};
+            static constexpr Pin lcdBlEn             = {PORT_V, 1};
+            static constexpr Pin lcdBlRst            = {PORT_V, 2};
+
+        private:
+            volatile Registers *m_regs = nullptr;
+
+        public:
+            void SetMode(Pin pin, Mode mode) const
+            {
+                m_regs->bank[pin.port / portsPerBank].msk_cnf[pin.port % portsPerBank] = MakeMaskedWriteValue(pin.pos, mode == Mode::Gpio);
+            }
+
+            void SetModeContiguous(Pin pin, size_t n, Mode mode) const
+            {
+                m_regs->bank[pin.port / portsPerBank].msk_cnf[pin.port % portsPerBank] = MakeMaskedWriteValueContiguous(pin.pos, n, mode == Mode::Gpio);
+            }
+
+            // Only valid for GPIO (not SFIO)
+            void SetDirection(Pin pin, Direction direction) const
+            {
+                m_regs->bank[pin.port / portsPerBank].msk_oe[pin.port % portsPerBank] = MakeMaskedWriteValue(pin.pos, direction == Direction::Output);
+            }
+
+            // Only valid for GPIO (not SFIO)
+            void SetDirectionContiguous(Pin pin, size_t n, Direction direction) const
+            {
+                m_regs->bank[pin.port / portsPerBank].msk_oe[pin.port % portsPerBank] = MakeMaskedWriteValueContiguous(pin.pos, n, direction == Direction::Output);
+            }
+
+            // Only valid for GPIO (not SFIO)
+            void Write(Pin pin, Level level) const
+            {
+                m_regs->bank[pin.port / portsPerBank].msk_out[pin.port % portsPerBank] = MakeMaskedWriteValue(pin.pos, level == Level::High);
+            }
+
+            // Only valid for GPIO (not SFIO)
+            Level Read(Pin pin) const
+            {
+                return static_cast<Level>((m_regs->bank[pin.port / portsPerBank].in[pin.port % portsPerBank] >> pin.pos) & 1);
+            }
+
+            void ConfigureUartPins()
+            {
+                constexpr Pin uartPins[] = {uart1Tx, uart2Tx, uart3Tx, uart4Tx};
+
+                // Set SFIO to all the 4 contiguous pins (tx, rx, rts, cts)
+                for (Pin pin : uartPins) {
+                    SetModeContiguous(pin, 4, Mode::Sfio);
+                }
+            }
+    };
+
+}

thermosphere/src/drivers/tegra/t210/hvisor_drivers_tegra_t210_pinmux.hpp

@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../../../defines.hpp"
+
+namespace ams::hvisor::drivers::tegra::t210 {
+
+    class Pinmux final {
+        private:
+            struct Registers {
+                u32 sdmmc1_clk;
+                u32 sdmmc1_cmd;
+                u32 sdmmc1_dat3;
+                u32 sdmmc1_dat2;
+                u32 sdmmc1_dat1;
+                u32 sdmmc1_dat0;
+                u32 _r18;
+                u32 sdmmc3_clk;
+                u32 sdmmc3_cmd;
+                u32 sdmmc3_dat0;
+                u32 sdmmc3_dat1;
+                u32 sdmmc3_dat2;
+                u32 sdmmc3_dat3;
+                u32 _r34;
+                u32 pex_l0_rst_n;
+                u32 pex_l0_clkreq_n;
+                u32 pex_wake_n;
+                u32 pex_l1_rst_n;
+                u32 pex_l1_clkreq_n;
+                u32 sata_led_active;
+                u32 spi1_mosi;
+                u32 spi1_miso;
+                u32 spi1_sck;
+                u32 spi1_cs0;
+                u32 spi1_cs1;
+                u32 spi2_mosi;
+                u32 spi2_miso;
+                u32 spi2_sck;
+                u32 spi2_cs0;
+                u32 spi2_cs1;
+                u32 spi4_mosi;
+                u32 spi4_miso;
+                u32 spi4_sck;
+                u32 spi4_cs0;
+                u32 qspi_sck;
+                u32 qspi_cs_n;
+                u32 qspi_io0;
+                u32 qspi_io1;
+                u32 qspi_io2;
+                u32 qspi_io3;
+                u32 _ra0;
+                u32 dmic1_clk;
+                u32 dmic1_dat;
+                u32 dmic2_clk;
+                u32 dmic2_dat;
+                u32 dmic3_clk;
+                u32 dmic3_dat;
+                u32 gen1_i2c_scl;
+                u32 gen1_i2c_sda;
+                u32 gen2_i2c_scl;
+                u32 gen2_i2c_sda;
+                u32 gen3_i2c_scl;
+                u32 gen3_i2c_sda;
+                u32 cam_i2c_scl;
+                u32 cam_i2c_sda;
+                u32 pwr_i2c_scl;
+                u32 pwr_i2c_sda;
+                u32 uart1_tx;
+                u32 uart1_rx;
+                u32 uart1_rts;
+                u32 uart1_cts;
+                u32 uart2_tx;
+                u32 uart2_rx;
+                u32 uart2_rts;
+                u32 uart2_cts;
+                u32 uart3_tx;
+                u32 uart3_rx;
+                u32 uart3_rts;
+                u32 uart3_cts;
+                u32 uart4_tx;
+                u32 uart4_rx;
+                u32 uart4_rts;
+                u32 uart4_cts;
+                u32 dap1_fs;
+                u32 dap1_din;
+                u32 dap1_dout;
+                u32 dap1_sclk;
+                u32 dap2_fs;
+                u32 dap2_din;
+                u32 dap2_dout;
+                u32 dap2_sclk;
+                u32 dap4_fs;
+                u32 dap4_din;
+                u32 dap4_dout;
+                u32 dap4_sclk;
+                u32 cam1_mclk;
+                u32 cam2_mclk;
+                u32 jtag_rtck;
+                u32 clk_32k_in;
+                u32 clk_32k_out;
+                u32 batt_bcl;
+                u32 clk_req;
+                u32 cpu_pwr_req;
+                u32 pwr_int_n;
+                u32 shutdown;
+                u32 core_pwr_req;
+                u32 aud_mclk;
+                u32 dvfs_pwm;
+                u32 dvfs_clk;
+                u32 gpio_x1_aud;
+                u32 gpio_x3_aud;
+                u32 pcc7;
+                u32 hdmi_cec;
+                u32 hdmi_int_dp_hpd;
+                u32 spdif_out;
+                u32 spdif_in;
+                u32 usb_vbus_en0;
+                u32 usb_vbus_en1;
+                u32 dp_hpd0;
+                u32 wifi_en;
+                u32 wifi_rst;
+                u32 wifi_wake_ap;
+                u32 ap_wake_bt;
+                u32 bt_rst;
+                u32 bt_wake_ap;
+                u32 ap_wake_nfc;
+                u32 nfc_en;
+                u32 nfc_int;
+                u32 gps_en;
+                u32 gps_rst;
+                u32 cam_rst;
+                u32 cam_af_en;
+                u32 cam_flash_en;
+                u32 cam1_pwdn;
+                u32 cam2_pwdn;
+                u32 cam1_strobe;
+                u32 lcd_te;
+                u32 lcd_bl_pwm;
+                u32 lcd_bl_en;
+                u32 lcd_rst;
+                u32 lcd_gpio1;
+                u32 lcd_gpio2;
+                u32 ap_ready;
+                u32 touch_rst;
+                u32 touch_clk;
+                u32 modem_wake_ap;
+                u32 touch_int;
+                u32 motion_int;
+                u32 als_prox_int;
+                u32 temp_alert;
+                u32 button_power_on;
+                u32 button_vol_up;
+                u32 button_vol_down;
+                u32 button_slide_sw;
+                u32 button_home;
+                u32 pa6;
+                u32 pe6;
+                u32 pe7;
+                u32 ph6;
+                u32 pk0;
+                u32 pk1;
+                u32 pk2;
+                u32 pk3;
+                u32 pk4;
+                u32 pk5;
+                u32 pk6;
+                u32 pk7;
+                u32 pl0;
+                u32 pl1;
+                u32 pz0;
+                u32 pz1;
+                u32 pz2;
+                u32 pz3;
+                u32 pz4;
+                u32 pz5;
+            };
+            static_assert(std::is_standard_layout_v<Registers>);
+            static_assert(std::is_trivial_v<Registers>);
+
+            enum Flags : u32 {
+                PREEMP_ENABLED          = BIT(15),
+
+                DRIVE_1X                = 0 << 13,
+                DRIVE_2X                = 1 << 13,
+                DRIVE_3X                = 2 << 13,
+                DRIVE_4X                = 3 << 13,
+
+                SCHMT_ENABLED           = BIT(12),
+                OD_ENABLED              = BIT(11),
+                IO_HV_ENABLED           = BIT(10),
+                HSM_ENABLED             = BIT(9),
+                LPDR_ENABLED            = BIT(8),
+                LOCKED                  = BIT(7),
+                INPUT                   = BIT(6),
+                PARKED                  = BIT(5),
+                TRISTATE                = BIT(4),
+
+                PULL_NONE               = 0 << 2,
+                PULL_DOWN               = 1 << 2,
+                PULL_UP                 = 2 << 2,
+
+                SELECT_FUNCTION0        = 0 << 0,
+                SELECT_FUNCTION1        = 1 << 0,
+                SELECT_FUNCTION2        = 2 << 0,
+                SELECT_FUNCTION3        = 3 << 0,
+            };
+
+        private:
+            // TODO friend
+            volatile Registers *m_regs = nullptr;
+
+        public:
+            void ConfigureUartPins() const
+            {
+                m_regs->uart1_tx    = 0     | 0             | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart1_rx    = INPUT | TRISTATE      | PULL_UP   | SELECT_FUNCTION0;
+                m_regs->uart1_rts   = 0     | 0             | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart1_cts   = INPUT | TRISTATE      | PULL_DOWN | SELECT_FUNCTION0;
+                m_regs->uart2_tx    = 0     | 0             | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart2_rx    = INPUT | TRISTATE      | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart2_rts   = 0     | 0             | PULL_DOWN | SELECT_FUNCTION0;
+                m_regs->uart2_cts   = INPUT | TRISTATE      | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart3_tx    = 0     | 0             | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart3_rx    = INPUT | TRISTATE      | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart3_rts   = 0     | 0             | PULL_DOWN | SELECT_FUNCTION0;
+                m_regs->uart3_cts   = INPUT | TRISTATE      | PULL_NONE | SELECT_FUNCTION0;
+                m_regs->uart4_tx    = 0     | 0             | PULL_DOWN | SELECT_FUNCTION0;
+                m_regs->uart4_rx    = INPUT | TRISTATE      | PULL_DOWN | SELECT_FUNCTION0;
+                m_regs->uart4_cts   = 0     | 0             | PULL_DOWN | SELECT_FUNCTION0;
+                m_regs->uart4_rts   = INPUT | TRISTATE      | PULL_DOWN | SELECT_FUNCTION0;
+            }
+    };
+}

thermosphere/src/exception_vectors.s

@@ -0,0 +1,375 @@
+/*
+ * 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: replace sp_el1 with crashing sp (for convenience)
+    // (sp_el2 is not accessible at el2)
+    msr     spsel, #0
+    stp     x0, x1, [sp, #-0x10]
+    mov     x0, sp
+    msr     spsel, #1
+    mov     x1, sp
+    mov     sp, x0
+    msr     sp_el1, x1
+    ldp     x0, x1, [sp, #-0x10]
+.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, x19, [sp, #EXCEP_STACK_FRAME_SIZE]
+        msr     sp_el0, x19
+        prfm    pstl1keep, [x18]
+        mov     w1, #1
+    .else
+        mov     w1, #0
+    .endif
+    // ams::hvisor::ExceptionEntryPostprocess(ams::hvisor::ExceptionStackFrame*, bool)
+    bl          _ZN3ams6hvisor25ExceptionEntryPostprocessEPNS0_19ExceptionStackFrameEb
+.endm
+
+.macro EXCEPTION_HANDLER_END name, type
+    .if \type != EXCEPTION_TYPE_HOST_CRASH
+        mov     x0, sp
+        // ams::hvisor::ExceptionReturnPreprocess(ams::hvisor::ExceptionStackFrame*)
+        bl      _ZN3ams6hvisor25ExceptionReturnPreprocessEPNS0_19ExceptionStackFrameE
+        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
+    // Safecpy
+    cbz     x18, _handleSafecpy
+
+    // Used when we enable the MMU
+    msr     elr_el2, x18
+    // Note: non-broadcasting TLB maintenance op
+    tlbi    alle2
+    dsb     ish
+    isb
+    eret
+
+    _handleSafecpy:
+    // Set x16 to 1
+    mov     x16, #1
+    eret
+
+check_vector_size   _synchSp0
+
+_unknownException:
+    PIVOT_STACK_FOR_CRASH
+    mov     x0, x30
+    adr     x1, g_thermosphereVectors + 4
+    sub     x0, x0, x1
+    // ams::hvisor::HandleUnknownException(unsigned int)
+    bl      _ZN3ams6hvisor22HandleUnknownExceptionEj
+    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.
+
+// ams::hvisor::traps::CallSmc0(ams::hvisor::ExceptionStackFrame*):
+.global _ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE
+.type   _ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE, %function
+.func   _ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE
+.cfi_startproc
+.cfi_sections   .debug_frame
+// ams::hvisor::callSmcTemplate[]
+.global _ZN3ams6hvisor5traps15callSmcTemplateE
+_ZN3ams6hvisor5traps15callSmcTemplateE:
+_ZN3ams6hvisor5traps8CallSmc0EPNS0_19ExceptionStackFrameE:
+    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]
+
+    _callSmcTemplateSmcInstruction:
+    smc     #0
+
+    // Note that NN's secure monitor can return results in x4-x7, this differs from Arm's spec.
+    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
+
+_callSmcTemplateEnd:
+.cfi_endproc
+.endfunc
+
+// ams::hvisor::traps::callSmcTemplateInstructionOffset
+.global _ZN3ams6hvisor5traps32callSmcTemplateInstructionOffsetE
+_ZN3ams6hvisor5traps32callSmcTemplateInstructionOffsetE:
+    .word _callSmcTemplateSmcInstruction - _ZN3ams6hvisor5traps15callSmcTemplateE
+// ams::hvisor::traps::callSmcTemplateSize
+.global _ZN3ams6hvisor5traps19callSmcTemplateSizeE
+_ZN3ams6hvisor5traps19callSmcTemplateSizeE:
+    .word _callSmcTemplateEnd - _ZN3ams6hvisor5traps15callSmcTemplateE
+
+// ams::hvisor::traps::CallSmc1(ams::hvisor::ExceptionStackFrame*):
+.global _ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE
+.type   _ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE, %function
+.func   _ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE
+.cfi_startproc
+.cfi_sections   .debug_frame
+_ZN3ams6hvisor5traps8CallSmc1EPNS0_19ExceptionStackFrameE:
+    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]
+
+    smc     #1
+
+    // Note that NN's secure monitor can return results in x4-x7, this differs from Arm's spec.
+    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
+.cfi_endproc
+.endfunc
+
+/* Current EL, SPx */
+
+EXCEPTION_HANDLER_START     _synchSpx, EXCEPTION_TYPE_HOST
+    mov     x0, sp
+    // ams::hvisor::HandleSameElSyncException(ams::hvisor::ExceptionStackFrame*):
+    bl      _ZN3ams6hvisor25HandleSameElSyncExceptionEPNS0_19ExceptionStackFrameE
+EXCEPTION_HANDLER_END     _synchSpx
+
+EXCEPTION_HANDLER_START     _irqSpx, EXCEPTION_TYPE_HOST
+    mov     x0, sp
+    mov     w1, #0
+    mov     w2, #0
+    // ams::hvisor::IrqManager::HandleInterrupt(ams::hvisor::ExceptionStackFrame*):
+    bl      _ZN3ams6hvisor10IrqManager15HandleInterruptEPNS0_19ExceptionStackFrameE
+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
+    // ams::hvisor::HandleLowerElSyncException(ams::hvisor::ExceptionStackFrame*)
+    bl      _ZN3ams6hvisor26HandleLowerElSyncExceptionEPNS0_19ExceptionStackFrameE
+EXCEPTION_HANDLER_END       _synchA64, EXCEPTION_TYPE_GUEST
+
+EXCEPTION_HANDLER_START     _irqA64, EXCEPTION_TYPE_GUEST
+    mov     x0, sp
+    mov     w1, #1
+    mov     w2, #0
+    // ams::hvisor::IrqManager::HandleInterrupt(ams::hvisor::ExceptionStackFrame*):
+    bl      _ZN3ams6hvisor10IrqManager15HandleInterruptEPNS0_19ExceptionStackFrameE
+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
+    // ams::hvisor::HandleLowerElSyncException(ams::hvisor::ExceptionStackFrame*)
+    bl      _ZN3ams6hvisor26HandleLowerElSyncExceptionEPNS0_19ExceptionStackFrameE
+EXCEPTION_HANDLER_END       _synchA32, EXCEPTION_TYPE_GUEST
+
+EXCEPTION_HANDLER_START     _irqA32, EXCEPTION_TYPE_GUEST
+    mov     x0, sp
+    mov     w1, #1
+    mov     w2, #1
+    // ams::hvisor::IrqManager::HandleInterrupt(ams::hvisor::ExceptionStackFrame*):
+    bl      _ZN3ams6hvisor10IrqManager15HandleInterruptEPNS0_19ExceptionStackFrameE
+EXCEPTION_HANDLER_END        _irqA32, EXCEPTION_TYPE_GUEST
+
+UNKNOWN_EXCEPTION           _fiqA32
+UNKNOWN_EXCEPTION           _serrorA32

thermosphere/src/exceptions.c

@@ -1,108 +0,0 @@
-/*
- * 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 "exceptions.h"
-
-#include "lib/printk.h"
-
-/**
- * Simple debug function that prints all of our saved registers.
- */
-static void print_registers(struct guest_state *regs)
-{
-    // print x0-29
-    for(int i = 0; i < 30; i += 2) {
-        printk("x%d:\t0x%p\t", i,     regs->x[i]);
-        printk("x%d:\t0x%p\n", i + 1, regs->x[i + 1]);
-    }
-
-    // print x30; don't bother with x31 (SP), as it's used by the stack that's
-    // storing this stuff; we really care about the saved SP anyways
-    printk("x30:\t0x%p\n", regs->x[30]);
-
-    // Special registers.
-    printk("pc:\t0x%p\tcpsr:\t0x%p\n", regs->pc, regs->cpsr);
-    printk("sp_el1:\t0x%p\tsp_el0:\t0x%p\n", regs->sp_el1, regs->sp_el0);
-    printk("elr_el1:0x%p\tspsr_el1:0x%p\n", regs->elr_el1, regs->spsr_el1);
-
-    // Note that we don't print ESR_EL2, as this isn't really part of the saved state.
-}
-
-/**
- * Placeholder function that triggers whenever a vector happens we're not
- * expecting. Currently prints out some debug information.
- */
-void unhandled_vector(struct guest_state *regs)
-{
-    printk("\nAn unexpected vector happened!\n");
-    print_registers(regs);
-    printk("\n\n");
-}
-
-
-/**
- * Handles an HVC call.
- */
-static void handle_hvc(struct guest_state *regs, int call_number)
-{
-
-    switch(call_number) {
-
-
-    default:
-        printk("Got a HVC call from 64-bit code.\n");
-        printk("Calling instruction was: hvc %d\n\n", call_number);
-        printk("Calling context (you can use these regs as hypercall args!):\n");
-        print_registers(regs);
-        printk("\n\n");
-        break;
-    }
-}
-
-
-/**
- * Placeholder function that triggers whenever a user event triggers a
- * synchronous interrupt. Currently, we really only care about 'hvc',
- * so that's all we're going to handle here.
- */
-
-void handle_hypercall(struct guest_state *regs)
-{
-    // This is demonstration code.
-    // In the future, you'd stick your hypercall table here.
-
-    switch (regs->esr_el2.ec) {
-
-    case HSR_EC_HVC64: {
-        // Read the hypercall number.
-        int hvc_nr = regs->esr_el2.iss & 0xFFFF;
-
-        // ... and handle the hypercall.
-        handle_hvc(regs, hvc_nr);
-        break;
-    }
-    default:
-        printk("Unexpected hypercall! ESR=%p\n", regs->esr_el2.bits);
-        print_registers(regs);
-        printk("\n\n");
-        break;
-
-    }
-}
-
-

thermosphere/src/exceptions.h

@@ -1,185 +0,0 @@
-/*
- * 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/>.
- */
-
-#ifndef __EXCEPTION_H__
-#define __EXCEPTION_H__
-
-/**
- * Borrowed fom Xen (not copyrightable as these are facts).
- * Description of the EL2 exception syndrome register.
- */
-#define HSR_EC_UNKNOWN              0x00
-#define HSR_EC_WFI_WFE              0x01
-#define HSR_EC_CP15_32              0x03
-#define HSR_EC_CP15_64              0x04
-#define HSR_EC_CP14_32              0x05        /* Trapped MCR or MRC access to CP14 */
-#define HSR_EC_CP14_DBG             0x06        /* Trapped LDC/STC access to CP14 (only for debug registers) */
-#define HSR_EC_CP                   0x07        /* HCPTR-trapped access to CP0-CP13 */
-#define HSR_EC_CP10                 0x08
-#define HSR_EC_JAZELLE              0x09
-#define HSR_EC_BXJ                  0x0a
-#define HSR_EC_CP14_64              0x0c
-#define HSR_EC_SVC32                0x11
-#define HSR_EC_HVC32                0x12
-#define HSR_EC_SMC32                0x13
-#define HSR_EC_HVC64                0x16
-#define HSR_EC_SMC64                0x17
-#define HSR_EC_SYSREG               0x18
-#define HSR_EC_INSTR_ABORT_LOWER_EL 0x20
-#define HSR_EC_INSTR_ABORT_CURR_EL  0x21
-#define HSR_EC_DATA_ABORT_LOWER_EL  0x24
-#define HSR_EC_DATA_ABORT_CURR_EL   0x25
-#define HSR_EC_BRK                  0x3c
-
-/**
- * Borrowed fom Xen (not copyrightable as these are facts).
- * Description of the EL2 exception syndrome register.
- */
-union esr {
-    uint32_t bits;
-    struct {
-        unsigned long iss:25;  /* Instruction Specific Syndrome */
-        unsigned long len:1;   /* Instruction length */
-        unsigned long ec:6;    /* Exception Class */
-    };
-
-    /* Common to all conditional exception classes (0x0N, except 0x00). */
-    struct hsr_cond {
-        unsigned long iss:20;  /* Instruction Specific Syndrome */
-        unsigned long cc:4;    /* Condition Code */
-        unsigned long ccvalid:1;/* CC Valid */
-        unsigned long len:1;   /* Instruction length */
-        unsigned long ec:6;    /* Exception Class */
-    } cond;
-
-    struct hsr_wfi_wfe {
-        unsigned long ti:1;    /* Trapped instruction */
-        unsigned long sbzp:19;
-        unsigned long cc:4;    /* Condition Code */
-        unsigned long ccvalid:1;/* CC Valid */
-        unsigned long len:1;   /* Instruction length */
-        unsigned long ec:6;    /* Exception Class */
-    } wfi_wfe;
-
-    /* reg, reg0, reg1 are 4 bits on AArch32, the fifth bit is sbzp. */
-    struct hsr_cp32 {
-        unsigned long read:1;  /* Direction */
-        unsigned long crm:4;   /* CRm */
-        unsigned long reg:5;   /* Rt */
-        unsigned long crn:4;   /* CRn */
-        unsigned long op1:3;   /* Op1 */
-        unsigned long op2:3;   /* Op2 */
-        unsigned long cc:4;    /* Condition Code */
-        unsigned long ccvalid:1;/* CC Valid */
-        unsigned long len:1;   /* Instruction length */
-        unsigned long ec:6;    /* Exception Class */
-    } cp32; /* HSR_EC_CP15_32, CP14_32, CP10 */
-
-    struct hsr_cp64 {
-        unsigned long read:1;   /* Direction */
-        unsigned long crm:4;    /* CRm */
-        unsigned long reg1:5;   /* Rt1 */
-        unsigned long reg2:5;   /* Rt2 */
-        unsigned long sbzp2:1;
-        unsigned long op1:4;    /* Op1 */
-        unsigned long cc:4;     /* Condition Code */
-        unsigned long ccvalid:1;/* CC Valid */
-        unsigned long len:1;    /* Instruction length */
-        unsigned long ec:6;     /* Exception Class */
-    } cp64; /* HSR_EC_CP15_64, HSR_EC_CP14_64 */
-
-     struct hsr_cp {
-        unsigned long coproc:4; /* Number of coproc accessed */
-        unsigned long sbz0p:1;
-        unsigned long tas:1;    /* Trapped Advanced SIMD */
-        unsigned long res0:14;
-        unsigned long cc:4;     /* Condition Code */
-        unsigned long ccvalid:1;/* CC Valid */
-        unsigned long len:1;    /* Instruction length */
-        unsigned long ec:6;     /* Exception Class */
-    } cp; /* HSR_EC_CP */
-
-    struct hsr_sysreg {
-        unsigned long read:1;   /* Direction */
-        unsigned long crm:4;    /* CRm */
-        unsigned long reg:5;    /* Rt */
-        unsigned long crn:4;    /* CRn */
-        unsigned long op1:3;    /* Op1 */
-        unsigned long op2:3;    /* Op2 */
-        unsigned long op0:2;    /* Op0 */
-        unsigned long res0:3;
-        unsigned long len:1;    /* Instruction length */
-        unsigned long ec:6;
-    } sysreg; /* HSR_EC_SYSREG */
-
-    struct hsr_iabt {
-        unsigned long ifsc:6;  /* Instruction fault status code */
-        unsigned long res0:1;
-        unsigned long s1ptw:1; /* Stage 2 fault during stage 1 translation */
-        unsigned long res1:1;
-        unsigned long eat:1;   /* External abort type */
-        unsigned long res2:15;
-        unsigned long len:1;   /* Instruction length */
-        unsigned long ec:6;    /* Exception Class */
-    } iabt; /* HSR_EC_INSTR_ABORT_* */
-
-    struct hsr_dabt {
-        unsigned long dfsc:6;  /* Data Fault Status Code */
-        unsigned long write:1; /* Write / not Read */
-        unsigned long s1ptw:1; /* Stage 2 fault during stage 1 translation */
-        unsigned long cache:1; /* Cache Maintenance */
-        unsigned long eat:1;   /* External Abort Type */
-        unsigned long sbzp0:4;
-        unsigned long ar:1;    /* Acquire Release */
-        unsigned long sf:1;    /* Sixty Four bit register */
-        unsigned long reg:5;   /* Register */
-        unsigned long sign:1;  /* Sign extend */
-        unsigned long size:2;  /* Access Size */
-        unsigned long valid:1; /* Syndrome Valid */
-        unsigned long len:1;   /* Instruction length */
-        unsigned long ec:6;    /* Exception Class */
-    } dabt; /* HSR_EC_DATA_ABORT_* */
-
-    struct hsr_brk {
-        unsigned long comment:16;   /* Comment */
-        unsigned long res0:9;
-        unsigned long len:1;        /* Instruction length */
-        unsigned long ec:6;         /* Exception Class */
-    } brk;
-
-};
-
-/**
- * Structure that stores the saved register values on a hypercall.
- */
-struct guest_state {
-    uint64_t pc;
-    uint64_t cpsr;
-
-    uint64_t elr_el1;
-    uint64_t spsr_el1;
-
-    uint64_t sp_el0;
-    uint64_t sp_el1;
-
-    union esr esr_el2;
-    uint64_t x[31];
-}
-__attribute__((packed));
-
-
-
-#endif

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(ReadRegister(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,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 "gdb_context.hpp"
+#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);

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,14 @@
+/*
+*   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);

thermosphere/src/gdb/hvisor_gdb_comms.cpp

@@ -0,0 +1,226 @@
+/*
+ * Copyright (c) 2019-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 <cstdio>
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+namespace {
+
+    void WriteAck(TransportInterface *iface)
+    {
+        char c = '+';
+        transportInterfaceWriteData(iface, &c, 1);
+    }
+
+    int WriteNack(TransportInterface *iface)
+    {
+        char c = '-';
+        transportInterfaceWriteData(iface, &c, 1);
+        return 1;
+    }
+
+}
+
+namespace ams::hvisor::gdb {
+
+    int Context::ReceivePacket()
+    {
+        char hdr;
+        bool ctrlC = false;
+        TransportInterface *iface = m_transportInterface;
+
+        // Read the first character...
+        transportInterfaceReadData(iface, &hdr, 1);
+
+        switch (hdr) {
+            case '+': {
+                // Ack, don't do anything else except maybe NoAckMode state transition
+                if (m_noAckSent) {
+                    m_noAck = true;
+                    m_noAckSent = false;
+                }
+                return 0;
+            }
+            case '-':
+                // Nack, return the previous packet
+                transportInterfaceWriteData(iface, m_buffer, m_lastSentPacketSize);
+                return m_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 WriteNack(iface);
+        }
+
+        // We didn't get a nack past this point, read the remaining data if any
+
+        m_buffer[0] = hdr;
+        if (ctrlC) {
+            // Will never normally happen, but ok
+            if (m_state < State::Attached) {
+                DEBUG("Received connection from GDB, now attaching...\n");
+                Attach();
+                m_state = State::Attached;
+            }
+            return 1;
+        }
+
+        size_t delimPos = transportInterfaceReadDataUntil(iface, m_buffer + 1, 4 + GDB_BUF_LEN - 1, '#');
+        if (m_buffer[delimPos] != '#' || delimPos == 1) {
+            // The packet is malformed, send a nack. Refuse empty packets
+            return WriteNack(iface);
+        }
+
+        m_commandLetter = m_buffer[1];
+        m_commandData = std::string_view{m_buffer + 1, delimPos};
+
+        // Read the checksum
+        size_t checksumPos = delimPos + 1;
+        transportInterfaceReadData(iface, m_buffer + checksumPos, 2);
+
+        auto checksumOpt = DecodeHexByte(std::string_view{m_buffer + checksumPos, 2});
+
+        if (!checksumOpt || *checksumOpt != ComputeChecksum(m_commandData)) {
+            // Malformed or invalid checksum
+            return WriteNack(iface);
+        } else if (!m_noAck) {
+            WriteAck(iface);
+        }
+
+        // Remove command letter
+        m_commandData.remove_prefix(1);
+
+        // State transitions...
+        if (m_state < State::Attached) {
+            DEBUG("Received connection from GDB, now attaching...\n");
+            Attach();
+             m_state = State::Attached;
+        }
+
+        // Debug
+        /*m_buffer[checksumPos + 2] = '\0';
+        DEBUGRAW("->");
+        DEBUGRAW(m_buffer);
+        DEBUGRAW("\n");*/
+
+        return static_cast<int>(delimPos + 2);
+    }
+
+    int Context::DoSendPacket(size_t len)
+    {
+        transportInterfaceWriteData(m_transportInterface, m_buffer, len);
+        m_lastSentPacketSize = len;
+
+        // Debugging:
+        /*m_buffer[len] = 0;
+        DEBUGRAW("<-");
+        DEBUGRAW(ctx->buffer);
+        DEBUGRAW("\n");*/
+
+        return static_cast<int>(len);
+    }
+
+    int Context::SendPacket(std::string_view packetData, char hdr)
+    {
+        u8 checksum = ComputeChecksum(packetData);
+        if (packetData.data() != m_buffer + 1) {
+            std::memmove(m_buffer + 1, packetData.data(), packetData.size());
+        }
+
+        size_t checksumPos = 1 + packetData.size() + 1;
+        m_buffer[0] = '$';
+        m_buffer[checksumPos - 1] = '#';
+        EncodeHex(m_buffer + checksumPos, &checksum, 1);
+
+        return DoSendPacket(4 + packetData.size());
+    }
+
+    int Context::SendFormattedPacket(const char *packetDataFmt, ...)
+    {
+        va_list args;
+
+        va_start(args, packetDataFmt);
+        int n = vsprintf(m_buffer + 1, packetDataFmt, args);
+        va_end(args);
+
+        if (n < 0) {
+            return -1;
+        } else {
+            return SendPacket(std::string_view{m_buffer + 1, n});
+        }
+    }
+
+    int Context::SendHexPacket(const void *packetData, size_t len)
+    {
+        if (4 + 2 * len < GDB_BUF_LEN) {
+            return -1;
+        }
+
+        EncodeHex(m_buffer + 1, packetData, len);
+        return SendPacket(std::string_view{m_buffer + 1, 2 * len});
+    }
+
+    int Context::SendStreamData(std::string_view streamData, size_t offset, size_t length, bool forceEmptyLast)
+    {
+        size_t totalSize = streamData.size();
+
+        // GDB_BUF_LEN does not include the usual $#<1-byte checksum>
+        length = std::min(length, GDB_BUF_LEN - 1ul);
+
+        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.data() + offset != m_buffer + 2) {
+            memmove(m_buffer + 2, streamData.data() + offset, length);
+        }
+
+        m_buffer[1] = letter; 
+        return SendPacket(std::string_view{m_buffer + 1, 1 + length});
+    }
+
+    int Context::ReplyOk()
+    {
+        return SendPacket("OK");
+    }
+
+    int Context::ReplyEmpty()
+    {
+        return SendPacket("");
+    }
+
+    int Context::ReplyErrno(int no)
+    {
+        u8 no8 = static_cast<u8>(no);
+        char resp[] = "E00";
+        EncodeHex(resp + 1, &no8, 1);
+        return SendPacket(resp);
+    }
+}

thermosphere/src/gdb/hvisor_gdb_context.cpp

@@ -0,0 +1,257 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+// 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 "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+#include "../hvisor_hw_breakpoint_manager.hpp"
+#include "../hvisor_sw_breakpoint_manager.hpp"
+#include "../hvisor_watchpoint_manager.hpp"
+
+#include "../hvisor_fpu_register_cache.hpp"
+
+#include "../debug_manager.h"
+
+namespace {
+
+    TEMPORARY char g_gdbWorkBuffer[GDB_WORK_BUF_LEN];
+    TEMPORARY char g_gdbBuffer[GDB_BUF_LEN + 4 + 1];
+
+}
+
+namespace ams::hvisor::gdb {
+
+    void Context::Disconnect()
+    {
+        Detach();
+        auto *iface = m_transportInterface;
+
+        *this = {};
+        m_transportInterface = iface;
+    }
+
+    void Context::Initialize(TransportInterfaceType ifaceType, u32 ifaceId, u32 ifaceFlags)
+    {
+        m_workBuffer = g_gdbWorkBuffer;
+        m_buffer = g_gdbBuffer;
+        /*m_transportInterface = transportInterfaceCreate(
+            ifaceType,
+            ifaceId,
+            ifaceFlags,
+            GDB_ReceiveDataCallback,
+            GDB_ProcessDataCallback,
+            ctx
+        );*/
+    }
+
+    void Context::Attach()
+    {
+        // TODO: move the debug traps enable here?
+
+        m_attachedCoreList = CoreContext::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);
+        m_sendOwnDebugEventDisallowed = true;
+
+        BreakAllCores();
+
+        DebugEventInfo *info = debugManagerGetDebugEvent(currentCoreCtx->GetCoreId());
+        info->preprocessed = true;
+        info->handled = true;
+        m_lastDebugEvent = info;
+
+        m_state = State::Attached;
+
+        m_sendOwnDebugEventDisallowed = false;
+    }
+
+    void Context::Detach()
+    {
+        WatchpointManager::GetInstance().RemoveAll();
+        HwBreakpointManager::GetInstance().RemoveAll();
+        SwBreakpointManager::GetInstance().RemoveAll(true);
+
+        // Reports to gdb are prevented because of "detaching" state?
+
+        // TODO: disable debug traps
+
+        m_currentHioRequestTargetAddr = 0;
+        memset(&m_currentHioRequest, 0, sizeof(PackedGdbHioRequest));
+
+        debugManagerSetReportingEnabled(false);
+        debugManagerContinueCores(CoreContext::GetActiveCoreMask());
+    }
+
+    void Context::MigrateRxIrq(u32 coreId) const
+    {
+        FpuRegisterCache::GetInstance().CleanInvalidate();
+        //transportInterfaceSetInterruptAffinity(ctx->transportInterface, BIT(coreId));
+    }
+
+    GDB_DEFINE_HANDLER(Unsupported)
+    {
+        return ReplyEmpty();
+    }
+
+#define COMMAND_CASE(letter, method) case letter: return GDB_HANDLER(method)();
+
+    int Context::ProcessPacket()
+    {
+        m_commandLetter = m_commandData[0];
+        m_commandData.remove_prefix(1);
+
+        switch (m_commandLetter) {
+            COMMAND_CASE('?', GetStopReason)
+            //COMMAND_CASE('c', ContinueOrStepDeprecated)
+            //COMMAND_CASE('C', ContinueOrStepDeprecated)
+            COMMAND_CASE('D', Detach)
+            COMMAND_CASE('F', HioReply)
+            COMMAND_CASE('g', ReadRegisters)
+            COMMAND_CASE('G', WriteRegisters)
+            COMMAND_CASE('H', SetThreadId)
+            COMMAND_CASE('k', Kill)
+            COMMAND_CASE('m', ReadMemory)
+            COMMAND_CASE('M', WriteMemory)
+            COMMAND_CASE('p', ReadRegister)
+            COMMAND_CASE('P', WriteRegister)
+            COMMAND_CASE('q', Query)
+            COMMAND_CASE('Q', Query)
+            //COMMAND_CASE('s', ContinueOrStepDeprecated)
+            //COMMAND_CASE('S', ContinueOrStepDeprecated)
+            COMMAND_CASE('T', IsThreadAlive)
+            COMMAND_CASE('v', VerboseCommand)
+            COMMAND_CASE('X', WriteMemoryRaw)
+            COMMAND_CASE('z', ToggleStopPoint)
+            COMMAND_CASE('Z', ToggleStopPoint)
+
+            default:
+                return HandleUnsupported();
+        }
+    }
+
+#undef COMMAND_CASE
+
+    /*
+    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_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_AcquireContext(GDBContext *ctx)
+    {
+        transportInterfaceAcquire(ctx->transportInterface);
+    }
+
+    void GDB_ReleaseContext(GDBContext *ctx)
+    {
+        transportInterfaceRelease(ctx->transportInterface);
+    }
+*/
+}

thermosphere/src/gdb/hvisor_gdb_context.hpp

@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+// 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.hpp"
+#include "../transport_interface.h"
+
+#include <string_view>
+
+#define _REENT_ONLY
+#include <cerrno>
+
+#define DECLARE_HANDLER(name)           int Handle##name()
+#define DECLARE_QUERY_HANDLER(name)     DECLARE_HANDLER(Query##name)
+#define DECLARE_VERBOSE_HANDLER(name)   DECLARE_HANDLER(Verbose##name)
+#define DECLARE_REMOTE_HANDLER(name)    DECLARE_HANDLER(Remote##name)
+#define DECLARE_XFER_HANDLER(name)      int HandleXfer##name(bool write, std::string_view annex, size_t offset, size_t length)
+
+struct DebugEventInfo;
+
+namespace ams::hvisor::gdb {
+
+    struct PackedGdbHioRequest {
+        // TODO revamp
+        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;
+    };
+
+    class Context final {
+        private:
+            enum class State {
+                Disconnected = 0,
+                Connected,
+                Attached,
+                Detaching
+            };
+
+        private:
+            // No need for a lock, it's in the transport interface layer...
+            TransportInterface *m_transportInterface = nullptr;
+            State m_state = State::Disconnected;
+            bool m_noAckSent = false;
+            bool m_noAck = false;
+            bool m_nonStop = false;
+
+            u32 m_attachedCoreList = 0;
+
+            int m_selectedCoreId = 0;
+            int m_selectedCoreIdForContinuing = 0;
+
+            u32 m_sentDebugEventCoreList = 0;
+            u32 m_acknowledgedDebugEventCoreList = 0;
+
+            bool m_sendOwnDebugEventDisallowed = 0;
+
+            bool m_catchThreadEvents = false;
+            bool m_processEnded = false;
+            bool m_processExited = false;
+
+            const struct DebugEventInfo *m_lastDebugEvent = nullptr;
+            uintptr_t m_currentHioRequestTargetAddr = 0ul;
+            PackedGdbHioRequest m_currentHioRequest{};
+
+            std::string_view m_targetXml{};
+
+            char m_commandLetter = '\0';
+            std::string_view m_commandData{};
+            size_t m_lastSentPacketSize = 0ul;
+            char *m_buffer = nullptr;
+            char *m_workBuffer = nullptr;
+
+        private:
+            Context(const Context &) = default;
+            Context &operator=(const Context &) = default;
+
+            Context(Context &&) = default;
+            Context &operator=(Context &&) = default;
+
+        private:
+            void MigrateRxIrq(u32 coreId) const;
+            int ProcessPacket();
+            void Disconnect();
+
+            // Debug
+            void BreakAllCores();
+
+            // Comms
+            int ReceivePacket();
+            int DoSendPacket(size_t len);
+            int SendPacket(std::string_view packetData, char hdr = '$');
+            int SendFormattedPacket(const char *packetDataFmt, ...);
+            int SendHexPacket(const void *packetData, size_t len);
+            int SendStreamData(std::string_view streamData, size_t offset, size_t length, bool forceEmptyLast);
+            int ReplyOk();
+            int ReplyEmpty();
+            int ReplyErrno(int no);
+
+            // Memory
+            int SendMemory(uintptr_t addr, size_t len, std::string_view prefix = {});
+            int WriteMemoryImpl(size_t (*decoder)(void *, const void *, size_t));
+
+            // Helpers
+            constexpr char *GetInPlaceOutputBuffer() const
+            {
+                return m_buffer + 1;
+            }
+
+            constexpr char *GetWorkBuffer() const
+            {
+                return m_workBuffer;
+            }
+        private:
+            // Meta
+            DECLARE_HANDLER(Unsupported);
+            DECLARE_HANDLER(Query);
+            DECLARE_QUERY_HANDLER(Xfer);
+            DECLARE_HANDLER(VerboseCommand);
+
+            // General queries
+            DECLARE_QUERY_HANDLER(Supported);
+            DECLARE_QUERY_HANDLER(StartNoAckMode);
+            DECLARE_QUERY_HANDLER(Attached);
+
+            // XML Transfer
+            DECLARE_XFER_HANDLER(Features);
+
+            // Resuming features enumeration
+            DECLARE_VERBOSE_HANDLER(ContinueSupported);
+
+            // "Threads"
+            // Capitalization in "GetTLSAddr" is intended.
+            DECLARE_HANDLER(SetThreadId);
+            DECLARE_HANDLER(IsThreadAlive);
+            DECLARE_QUERY_HANDLER(CurrentThreadId);
+            DECLARE_QUERY_HANDLER(fThreadInfo);
+            DECLARE_QUERY_HANDLER(sThreadInfo);
+            DECLARE_QUERY_HANDLER(ThreadEvents);
+            DECLARE_QUERY_HANDLER(ThreadExtraInfo);
+            DECLARE_QUERY_HANDLER(GetTLSAddr);
+
+            // Debug
+            DECLARE_VERBOSE_HANDLER(Stopped);
+            DECLARE_HANDLER(Detach);
+            DECLARE_HANDLER(Kill);
+            DECLARE_VERBOSE_HANDLER(CtrlC);
+            DECLARE_HANDLER(ContinueOrStepDeprecated);
+            DECLARE_VERBOSE_HANDLER(Continue);
+            DECLARE_HANDLER(GetStopReason);
+
+            // Stop points
+            DECLARE_HANDLER(ToggleStopPoint);
+
+            // Memory
+            DECLARE_HANDLER(ReadMemory);
+            DECLARE_HANDLER(WriteMemory);
+            DECLARE_HANDLER(WriteMemoryRaw);
+            DECLARE_QUERY_HANDLER(SearchMemory);
+
+            // Registers
+            DECLARE_HANDLER(ReadRegisters);
+            DECLARE_HANDLER(WriteRegisters);
+            DECLARE_HANDLER(ReadRegister);
+            DECLARE_HANDLER(WriteRegister);
+
+            // Hio
+            DECLARE_HANDLER(HioReply);
+
+            // Custom commands
+            DECLARE_QUERY_HANDLER(Rcmd);
+
+        public:
+            Context() = default;
+
+            void Initialize(TransportInterfaceType ifaceType, u32 ifaceId, u32 ifaceFlags);
+            void Attach();
+            void Detach();
+
+            void lock();
+            void unlock();
+            /* TODO: parent
+            void Acquire();
+            void Release();
+            */
+
+            constexpr bool IsAttached() const
+            {
+                return m_state == State::Attached;
+            }
+    };
+}
+
+#undef DECLARE_HANDLER
+#undef DECLARE_QUERY_HANDLER
+#undef DECLARE_VERBOSE_HANDLER
+#undef DECLARE_REMOTE_HANDLER
+#undef DECLARE_XFER_HANDLER

thermosphere/src/gdb/hvisor_gdb_defines_internal.hpp

@@ -0,0 +1,49 @@
+/*
+ * 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 "hvisor_gdb_context.hpp"
+
+// 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)                           Handle##name
+#define GDB_QUERY_HANDLER(name)                     GDB_HANDLER(Query##name)
+#define GDB_VERBOSE_HANDLER(name)                   GDB_HANDLER(Verbose##name)
+#define GDB_REMOTE_COMMAND_HANDLER(name)            GDB_HANDLER(RemoteCommand##name)
+#define GDB_XFER_HANDLER(name)                      GDB_HANDLER(Xfer##name)
+
+#define GDB_DEFINE_HANDLER(name)                    int Context::GDB_HANDLER(name)()
+#define GDB_DEFINE_QUERY_HANDLER(name)              GDB_DEFINE_HANDLER(Query##name)
+#define GDB_DEFINE_VERBOSE_HANDLER(name)            GDB_DEFINE_HANDLER(Verbose##name)
+#define GDB_DEFINE_REMOTE_COMMAND_HANDLER(name)     GDB_DEFINE_HANDLER(RemoteCommand##name)
+#define GDB_DEFINE_XFER_HANDLER(name)\
+    int Context::GDB_XFER_HANDLER(name)(bool write, std::string_view annex, size_t offset, size_t length)
+
+#define GDB_CHECK_NO_CMD_DATA()                      do { if (!m_commandData.empty()) return ReplyErrno(EILSEQ); } while (false)

thermosphere/src/gdb/hvisor_gdb_mem.cpp

@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+#include "../hvisor_guest_memory.hpp"
+
+namespace ams::hvisor::gdb {
+
+    int Context::SendMemory(uintptr_t addr, size_t len, std::string_view prefix)
+    {
+        char *buf = GetInPlaceOutputBuffer();
+        char *membuf = GetWorkBuffer();
+
+        size_t prefixLen = prefix.size();
+
+        if(prefixLen + 2 * len > GDB_BUF_LEN) {
+            // gdb shouldn't send requests which responses don't fit in a packet
+            return prefixLen == 0 ? ReplyErrno(ENOMEM) : -1;
+        }
+
+        size_t total = GuestReadMemory(addr, len, membuf);
+
+        if (total == 0) {
+            return prefixLen == 0 ? ReplyErrno(EFAULT) : -EFAULT;
+        } else {
+            std::copy(prefix.begin(), prefix.end(), buf);
+            EncodeHex(buf + prefixLen, membuf, total);
+            return SendPacket(std::string_view{buf, prefixLen + 2 * total});
+        }
+    }
+
+    int Context::WriteMemoryImpl(size_t (*decoder)(void *, const void *, size_t))
+    {
+        char *workbuf = GetWorkBuffer();
+
+        auto [nread, addr, len] = ParseHexIntegerList<2>(m_commandData, ':');
+        if (nread == 0) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        m_commandData.remove_prefix(nread);
+        if (len > m_commandData.length() / 2) {
+            // Data len field doesn't match what we got...
+            return ReplyErrno(ENOMEM);
+        }
+
+        size_t n = decoder(workbuf, m_commandData.data(), m_commandData.size());
+
+        if(n != len) {
+            // Decoding error...
+            return ReplyErrno(EILSEQ);
+        }
+
+        size_t total = GuestWriteMemory(addr, len, workbuf);
+        return total == len ? ReplyOk() : ReplyErrno(EFAULT);
+    }
+
+
+    GDB_DEFINE_HANDLER(ReadMemory)
+    {
+        auto [nparsed, addr, len] = ParseHexIntegerList<2>(m_commandData);
+        if (nparsed == 0) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        return SendMemory(addr, len);
+    }
+
+    GDB_DEFINE_HANDLER(WriteMemory)
+    {
+        return WriteMemoryImpl(DecodeHex);
+    }
+
+    GDB_DEFINE_HANDLER(WriteMemoryRaw)
+    {
+        return WriteMemoryImpl(UnescapeBinaryData);
+    }
+
+}

thermosphere/src/gdb/hvisor_gdb_packet_data.cpp

@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_gdb_packet_data.hpp"
+
+namespace ams::hvisor::gdb {
+
+    u8 ComputeChecksum(std::string_view packetData)
+    {
+        return std::accumulate(packetData.cbegin(), packetData.cend(), u8{0u});
+    }
+
+    size_t EncodeHex(char *dst, const void *src, size_t len)
+    {
+        static const char *alphabet = "0123456789abcdef";
+        const u8 *src8 = reinterpret_cast<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;
+    }
+
+    size_t DecodeHex(void *dst, std::string_view data)
+    {
+        size_t i = 0;
+        u8 *dst8 = reinterpret_cast<u8 *>(dst);
+        for (i = 0; i < data.size() / 2; i++) {
+            auto bOpt = DecodeHexByte(data);
+            if (!bOpt) {
+                return i;
+            }
+
+            dst8[i] = *bOpt;
+            data.remove_prefix(2);
+        }
+
+        return i;
+    }
+
+    size_t DecodeHex(void *dst, const void *src, size_t len)
+    {
+        return DecodeHex(dst, std::string_view{reinterpret_cast<const char *>(src), len});
+    }
+
+    size_t EscapeBinaryData(size_t *encodedCount, void *dst, const void *src, size_t len, size_t maxLen)
+    {
+        u8 *dst8 = reinterpret_cast<u8 *>(dst);
+        const u8 *src8 = reinterpret_cast<const u8 *>(src);
+        len = std::min(len, maxLen);
+
+        u8 *dstMax = dst8 + len;
+
+        while (dst8 < dstMax) {
+            if (*src8 == '$' || *src8 == '#' || *src8 == '}' || *src8 == '*') {
+                if (dst8 + 1 >= dstMax) {
+                    break;
+                }
+                *dst8++ = '}';
+                *dst8++ = *src8++ ^ 0x20;
+            }
+            else {
+                *dst8++ = *src8++;
+            }
+        }
+
+        *encodedCount = dst8 - reinterpret_cast<u8 *>(dst);
+        return src8 - reinterpret_cast<const u8 *>(src);
+    }
+
+    size_t UnescapeBinaryData(void *dst, const void *src, size_t len)
+    {
+        u8 *dst8 = reinterpret_cast<u8 *>(dst);
+        const u8 *src8 = reinterpret_cast<const u8 *>(src);
+        const u8 *srcEnd = src8 + len;
+
+        while (src8 < srcEnd) {
+            if (*src8 == '}') {
+                src8++;
+                *dst8++ = *src8++ ^ 0x20;
+            } else {
+                *dst8++ = *src8++;
+            }
+        }
+
+        return dst8 - reinterpret_cast<u8 *>(dst);
+    }
+
+}

thermosphere/src/gdb/hvisor_gdb_packet_data.hpp

@@ -0,0 +1,199 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "../defines.hpp"
+#include <string_view>
+
+namespace ams::hvisor::gdb {
+
+    constexpr unsigned long DecodeHexDigit(char src)
+    {
+        switch (src) {
+            case '0' ... '9': return  0 + (src - '0');
+            case 'a' ... 'f': return 10 + (src - 'a');
+            case 'A' ... 'F': return 10 + (src - 'A');
+            default:
+                return 16;
+        }
+    }
+
+    constexpr auto ParseInteger(std::string_view str, u32 base = 0, bool allowPrefix = true)
+    {
+        unsigned long res = 0;
+        long mult = 1;
+        auto errval = std::tuple{0ul, 0ul};
+
+        size_t total = 0;
+
+        if ((base == 0 && !allowPrefix) || base > 16 || str.empty()) {
+            return errval;
+        }
+
+        // Check for +, -
+        if (str[0] == '+') {
+            if (!allowPrefix) {
+                return errval;
+            }
+            str.remove_prefix(1);
+            ++total;
+        } else if (str[0] == '-') {
+            if (!allowPrefix) {
+                return errval;
+            }
+            str.remove_prefix(1);
+            mult = -1;
+            ++total;
+        }
+
+        if (str.empty()) {
+            // Oops
+            return errval;
+        }
+
+        // Now, check for 0x or leading 0
+        if (str.size() >= 2 && str[0] == '0' && str[1] == 'x') {
+            if (!allowPrefix || (base != 16 && base != 0)) {
+                return errval;
+            } else {
+                str.remove_prefix(2);
+                base = 16;
+                total += 2;
+            }
+        } else if (base == 0 && str[0] == '0') {
+            base = 8;
+        } else if (base == 0) {
+            base = 10;
+        }
+
+        if (str.empty()) {
+            // Oops
+            return errval;
+        }
+
+        auto it = str.begin();
+        for (; it != str.end(); ++it) {
+            unsigned long v = DecodeHexDigit(*it);
+            if (v >= base) {
+                break;
+            }
+
+            res *= base;
+            res += v;
+            ++total;
+        }
+
+        return std::tuple{total, res * mult};
+    }
+
+    template<size_t N>
+    constexpr auto ParseIntegerList(std::string_view str, u32 base, bool allowPrefix, char sep, char lastSep = '\0')
+    {
+        // First element is parsed size
+        std::array<unsigned long, 1+N> res{ 0 };
+
+        size_t total = 0;
+        for (size_t i = 0; i < N && !str.empty(); i++) {
+            auto [nread, val] = ParseInteger(str, base, allowPrefix);
+
+            // Parse failure
+            if (nread == 0) {
+                return res;
+            }
+
+            str.remove_prefix(nread);
+
+            // Check separators
+            if (i != N - 1) {
+                if (str.empty() || str[0] != sep) {
+                    return res;
+                }
+                str.remove_prefix(1);
+                ++total;
+            } else if (i == N - 1) {
+                if ((lastSep == '\0') && !str.empty()) {
+                    return res;
+                } else if (lastSep != '\0') {
+                    if (str.empty() || str[0] != lastSep) {
+                        return res;
+                    }
+                    str.remove_prefix(1);
+                    ++total;
+                }
+            }
+
+            total += nread;
+            res[1 + i] = val;
+        }
+
+        res[0] = total;
+        return res;
+    }
+
+    template<size_t N>
+    constexpr auto ParseHexIntegerList(std::string_view str, char lastSep = '\0')
+    {
+        return ParseIntegerList<N>(str, 16, false, ',', lastSep);
+    }
+
+    template<size_t N>
+    constexpr auto SplitString(std::string_view data, char delim)
+    {
+        static_assert(N != 0);
+
+        std::array<std::string_view, N> res = {};
+        size_t delimPos = 0;
+
+        for (size_t i = 0; i < N - 1; i++) {
+            delimPos = data.find(delim);
+            if (delimPos == std::string_view::npos) {
+                return res;
+            }
+
+            res[i] = std::string_view{data.data(), delimPos};
+            data.remove_prefix(delimPos + 1);
+        }
+        res[N - 1] = data;
+
+        return res;
+    }
+
+    constexpr std::optional<u8> DecodeHexByte(std::string_view data)
+    {
+        if (data.size() < 2) {
+            return {};
+        }
+
+        auto v1 = DecodeHexDigit(data[0]);
+        auto v2 = DecodeHexDigit(data[1]);
+
+        if (v1 >= 16 || v2 >= 16) {
+            return {};
+        }
+
+        return (v1 << 4) | v2;
+    }
+
+    u8 ComputeChecksum(std::string_view packetData);
+    size_t EncodeHex(char *dst, const void *src, size_t len);
+    size_t DecodeHex(void *dst, std::string_view data);
+    size_t DecodeHex(void *dst, const void *src, size_t len);
+
+    size_t EscapeBinaryData(size_t *encodedCount, void *dst, const void *src, size_t len, size_t maxLen);
+    size_t UnescapeBinaryData(void *dst, const void *src, size_t len);
+
+}

thermosphere/src/gdb/hvisor_gdb_query.cpp

@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+namespace ams::hvisor::gdb {
+
+    GDB_DEFINE_QUERY_HANDLER(Supported)
+    {
+        // Ignore what gdb sent...
+        return SendFormattedPacket(
+            "PacketSize=%x;"
+            "qXfer:features:read+;"
+            "QStartNoAckMode+;QThreadEvents+"
+            "vContSupported+;swbreak+;hwbreak+",
+
+            GDB_BUF_LEN
+        );
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(StartNoAckMode)
+    {
+        GDB_CHECK_NO_CMD_DATA();
+
+        m_noAckSent = true;
+        return ReplyOk();
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(Attached)
+    {
+        GDB_CHECK_NO_CMD_DATA();
+
+        return SendPacket("1");
+    }
+
+#define QUERY_CMD_CASE2(name, fun)  if (cmdName==name) { return GDB_QUERY_HANDLER(fun)(); } else
+#define QUERY_CMD_CASE(fun)         QUERY_CMD_CASE2(STRINGIZE(fun), fun)
+
+    GDB_DEFINE_HANDLER(Query)
+    {
+        // Extract name
+        char delim = ':';
+
+        size_t delimPos = m_commandData.find_first_of(":,");
+        std::string_view cmdName = m_commandData;
+        if (delimPos != std::string_view::npos) {
+            delim = m_commandData[delimPos];
+            cmdName.remove_suffix(cmdName.size() - delimPos);
+            m_commandData.remove_prefix(delimPos + 1);
+        }
+
+        // Only 2 commands are delimited by a comma, all with lowercase 'q' prefix
+        // We don't handle qP nor qL
+        if (delim != ':') {
+            if (m_commandLetter != 'q') {
+                return ReplyErrno(EILSEQ);
+            } else if (cmdName != "Rcmd" && cmdName != "ThreadExtraInfo") {
+                return ReplyErrno(EILSEQ);
+            }
+        }
+
+        if (m_commandLetter == 'q') {
+            QUERY_CMD_CASE(Supported)
+            QUERY_CMD_CASE(Xfer)
+            QUERY_CMD_CASE(Attached)
+            QUERY_CMD_CASE(fThreadInfo)
+            QUERY_CMD_CASE(sThreadInfo)
+            QUERY_CMD_CASE(ThreadExtraInfo)
+            QUERY_CMD_CASE2("C", CurrentThreadId)
+            QUERY_CMD_CASE(Rcmd)
+            /*default :*/{
+                return HandleUnsupported();
+            }
+        } else {
+            QUERY_CMD_CASE(StartNoAckMode)
+            QUERY_CMD_CASE(ThreadEvents)
+            /*default :*/{
+                return HandleUnsupported();
+            }
+        }
+    }
+
+    #undef QUERY_CMD_CASE
+    #undef QUERY_CMD_CASE2
+
+}

thermosphere/src/gdb/hvisor_gdb_regs.cpp

@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+#include "../hvisor_exception_stack_frame.hpp"
+#include "../hvisor_fpu_register_cache.hpp"
+
+namespace {
+
+    auto GetRegisterPointerAndSize(unsigned long id, ams::hvisor::ExceptionStackFrame *frame, ams::hvisor::FpuRegisterCache::Storage &fpuRegStorage)
+    {
+        void *outPtr = nullptr;
+        size_t outSz = 0;
+
+        switch (id) {
+            case 0 ... 30:
+                outPtr = &frame->x[id];
+                outSz = 8;
+                break;
+            case 31:
+                outPtr = &frame->GetSpRef();
+                outSz = 8;
+                break;
+            case 32:
+                outPtr = &frame->spsr_el2;
+                outSz = 4;
+                break;
+            case 33 ... 64:
+                outPtr = &fpuRegStorage.q[id - 33];
+                outSz = 16;
+                break;
+            case 65:
+                outPtr = &fpuRegStorage.fpsr;
+                outSz = 4;
+                break;
+            case 66:
+                outPtr = &fpuRegStorage.fpcr;
+                outSz = 4;
+                break;
+            default:
+                __builtin_unreachable();
+                break;
+        }
+
+        return std::tuple{outPtr, outSz};
+    }
+
+}
+
+namespace ams::hvisor::gdb {
+
+    // Note: GDB treats cpsr, fpsr, fpcr as 32-bit integers...
+    GDB_DEFINE_HANDLER(ReadRegisters)
+    {
+        ENSURE(m_selectedCoreId == currentCoreCtx->GetCoreId());
+        GDB_CHECK_NO_CMD_DATA();
+
+        ExceptionStackFrame *frame = currentCoreCtx->GetGuestFrame();
+        auto &fpuRegStorage = FpuRegisterCache::GetInstance().ReadRegisters();
+
+        char *buf = GetInPlaceOutputBuffer();
+
+        size_t n = 0;
+
+        struct {
+            u64 sp;
+            u64 pc;
+            u32 cpsr;
+        } cpuSprs = {
+            .sp = frame->GetSpRef(),
+            .pc = frame->elr_el2,
+            .cpsr = static_cast<u32>(frame->spsr_el2),
+        };
+
+        u32 fpuSprs[2] = {
+            static_cast<u32>(fpuRegStorage.fpsr),
+            static_cast<u32>(fpuRegStorage.fpcr),
+        };
+
+        n += EncodeHex(buf + n, frame->x, sizeof(frame->x));
+        n += EncodeHex(buf + n, &cpuSprs, 8+8+4);
+        n += EncodeHex(buf + n, fpuRegStorage.q, sizeof(fpuRegStorage.q));
+        n += EncodeHex(buf + n, fpuSprs, sizeof(fpuSprs));
+
+        return SendPacket(std::string_view{buf, n});
+    }
+
+    GDB_DEFINE_HANDLER(WriteRegisters)
+    {
+        ENSURE(m_selectedCoreId == currentCoreCtx->GetCoreId());
+
+        ExceptionStackFrame *frame = currentCoreCtx->GetGuestFrame();
+        auto &fpuRegStorage = FpuRegisterCache::GetInstance().ReadRegisters();
+
+        char *tmp = GetWorkBuffer();
+
+        size_t n = 0;
+
+        struct {
+            u64 sp;
+            u64 pc;
+            u32 cpsr;
+        } cpuSprs;
+
+        u32 fpuSprs[2];
+
+        struct {
+            void *dst;
+            size_t sz;
+        } infos[4] = {
+            { frame->x,         sizeof(frame->x)        },
+            { &cpuSprs,         8+8+4                   },
+            { fpuRegStorage.q,  sizeof(fpuRegStorage.q)  },
+            { fpuSprs,          sizeof(fpuSprs)         },
+        };
+
+        // Parse & return on error
+        for (const auto &info: infos) {
+            // Fuck std::string_view.substr throwing exceptions
+            if (DecodeHex(tmp + n, m_commandData.data(), info.sz) != info.sz) {
+                return ReplyErrno(EILSEQ);
+            }
+            m_commandData.remove_prefix(2 * info.sz);
+            n += info.sz;
+        }
+
+        // Copy. Note: we don't check if cpsr (spsr_el2) was modified to return to EL2...
+        n = 0;
+        for (const auto &info: infos) {
+            std::copy(tmp + n, tmp + n + info.sz, info.dst);
+            n += info.sz;
+        }
+
+        frame->GetSpRef() = cpuSprs.sp;
+        frame->elr_el2 = cpuSprs.pc;
+        frame->spsr_el2 = cpuSprs.cpsr;
+        fpuRegStorage.fpsr = fpuSprs[0];
+        fpuRegStorage.fpcr = fpuSprs[1];
+        FpuRegisterCache::GetInstance().CommitRegisters();
+
+        return ReplyOk();
+    }
+
+    GDB_DEFINE_HANDLER(ReadRegister)
+    {
+        ENSURE(m_selectedCoreId == currentCoreCtx->GetCoreId());
+
+        ExceptionStackFrame *frame = currentCoreCtx->GetGuestFrame();
+        FpuRegisterCache::Storage *fpuRegStorage = nullptr;
+
+        auto [nread, gdbRegNum] = ParseHexIntegerList<1>(m_commandData);
+        if (nread == 0) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        // Check the register number
+        if (gdbRegNum >= 31 + 3 + 32 + 2) {
+            return ReplyErrno(EINVAL);
+        }
+
+        if (gdbRegNum > 31 + 3) {
+            // FPU register -- must read the FPU registers first
+            fpuRegStorage = &FpuRegisterCache::GetInstance().ReadRegisters();
+        }
+
+        return std::apply(SendHexPacket, GetRegisterPointerAndSize(gdbRegNum, frame, *fpuRegStorage));
+    }
+
+    GDB_DEFINE_HANDLER(WriteRegister)
+    {
+        ENSURE(m_selectedCoreId == currentCoreCtx->GetCoreId());
+
+        char *tmp = GetWorkBuffer();
+        ExceptionStackFrame *frame = currentCoreCtx->GetGuestFrame();
+        auto &fpuRegStorage = FpuRegisterCache::GetInstance().GetStorageRef();
+
+        auto [nread, gdbRegNum] = ParseHexIntegerList<1>(m_commandData, '=');
+        if (nread == 0) {
+            return ReplyErrno(EILSEQ);
+        }
+        m_commandData.remove_prefix(nread);
+
+        // Check the register number
+        if (gdbRegNum >= 31 + 3 + 32 + 2) {
+            return ReplyErrno(EINVAL);
+        }
+
+        auto [regPtr, sz] = GetRegisterPointerAndSize(gdbRegNum, frame, fpuRegStorage);
+
+        // Decode, check for errors
+        if (m_commandData.size() != 2 * sz || DecodeHex(tmp, m_commandData) != sz) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        std::copy(tmp, tmp + sz, regPtr);
+
+        if (gdbRegNum > 31 + 3) {
+            // FPU register -- must commit the FPU registers
+            FpuRegisterCache::GetInstance().CommitRegisters();
+        }
+
+        return ReplyOk();
+    }
+
+}

thermosphere/src/gdb/hvisor_gdb_remote_command.cpp

@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+namespace {
+
+    constexpr std::string_view SkipSpaces(std::string_view str)
+    {
+        size_t n = str.find_first_not_of("\t\v\n\f\r ");
+        if (n == std::string_view::npos) {
+            return {};
+        } else {
+            str.remove_prefix(n);
+            return str;
+        }
+    }
+
+}
+
+namespace ams::hvisor::gdb {
+
+    GDB_DEFINE_QUERY_HANDLER(Rcmd)
+    {
+        char *buf = GetInPlaceOutputBuffer();
+        size_t encodedLen = m_commandData.size();
+        if (encodedLen == 0 || encodedLen % 2 != 0) {
+            ReplyErrno(EILSEQ);
+        }
+
+        // Decode in place
+        if (DecodeHex(buf, m_commandData) != encodedLen / 2) {
+            ReplyErrno(EILSEQ);
+        }
+
+        // Extract command name, data
+        m_commandData = std::string_view{buf, encodedLen / 2};
+        size_t nameSize = m_commandData.find_first_of("\t\v\n\f\r ");
+        std::string_view commandName = m_commandData;
+        if (nameSize != std::string_view::npos) {
+            commandName.remove_suffix(commandName.size() - nameSize);
+            m_commandData.remove_prefix(nameSize);
+            m_commandData = SkipSpaces(m_commandData);
+        } else {
+            m_commandData = std::string_view{};
+        }
+
+        // Nothing implemented yet
+        (void)commandName;
+
+        return SendHexPacket("Unrecognized command.\n");
+    }
+
+}

thermosphere/src/gdb/hvisor_gdb_stop_points.cpp

@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+#include "../hvisor_hw_breakpoint_manager.hpp"
+#include "../hvisor_sw_breakpoint_manager.hpp"
+#include "../hvisor_watchpoint_manager.hpp"
+
+
+namespace ams::hvisor::gdb {
+
+    GDB_DEFINE_HANDLER(ToggleStopPoint)
+    {
+        bool add = m_commandLetter == 'Z';
+
+        auto [nread, kind, addr, size] = ParseHexIntegerList<3>(m_commandData, ';');
+        if (nread == 0) {
+            return ReplyErrno(EILSEQ);
+        }
+        m_commandData.remove_prefix(nread);
+    
+        // We don't support cond_list
+        bool persist = m_commandData == "cmds:1";
+
+        // In theory we should reject leading zeroes in "kind". Oh well...
+
+        int res;
+        static const cpu::DebugRegisterPair::LoadStoreControl kinds[3] = {
+            cpu::DebugRegisterPair::Store,
+            cpu::DebugRegisterPair::Load,
+            cpu::DebugRegisterPair::LoadStore,
+        };
+
+        auto &hwBpMgr = HwBreakpointManager::GetInstance();
+        auto &swBpMgr = SwBreakpointManager::GetInstance();
+        auto &wpMgr = WatchpointManager::GetInstance();
+
+        switch(kind) {
+            // Software breakpoint
+            case 0: {
+                if(size != 4) {
+                    return ReplyErrno(EINVAL);
+                }
+                res = add ? swBpMgr.Add(addr, persist) : swBpMgr.Remove(addr, false);
+                return res == 0 ? ReplyOk() : ReplyErrno(-res);
+            }
+
+            // Hardware breakpoint
+            case 1: {
+                if(size != 4) {
+                    return ReplyErrno(EINVAL);
+                }
+                res = add ? hwBpMgr.Add(addr) : hwBpMgr.Remove(addr);
+                return res == 0 ? ReplyOk() : ReplyErrno(-res);
+            }
+
+            // Watchpoints
+            case 2:
+            case 3:
+            case 4: {
+                res = add ? wpMgr.Add(addr, size, kinds[kind - 2]) : wpMgr.Remove(addr, size, kinds[kind - 2]);
+                return res == 0 ? ReplyOk() : ReplyErrno(-res);
+            }
+            default: {
+                return ReplyEmpty();
+            }
+        }
+    }
+
+}

thermosphere/src/gdb/hvisor_gdb_thread.cpp

@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2019-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 <cstdio>
+
+#include "hvisor_gdb_thread.hpp"
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+#include "../hvisor_core_context.hpp"
+
+namespace ams::hvisor::gdb {
+
+    int ConvertTidToCoreId(unsigned long tid)
+    {
+        switch (tid) {
+            case ULONG_MAX:
+                return -1;
+            case 0:
+                return currentCoreCtx->GetCoreId();
+            default:
+                return currentCoreCtx->GetCoreId() - 1;
+        }
+    }
+
+    std::optional<int> ParseConvertExactlyOneTid(std::string_view str)
+    {
+        if (str.size() == 2 && str[0] == '-' && str[1] == '1') {
+            return -1;
+        } else  {
+            auto [n, tid] = ParseHexIntegerList<1>(str);
+            if (n != 0 && tid < MAX_CORE + 1) {
+                return ConvertTidToCoreId(tid);
+            } else {
+                return {};
+            }
+        }
+    }
+
+    // Hg<tid>, Hc<tid>
+    GDB_DEFINE_HANDLER(SetThreadId)
+    {
+        if (!m_commandData.starts_with('g') && !m_commandData.starts_with('c')) {
+            return ReplyErrno(EINVAL);
+        }
+
+        char kind = m_commandData[0];
+        m_commandData.remove_prefix(1);
+
+        auto coreIdOpt = ParseConvertExactlyOneTid(m_commandData);
+        if (!coreIdOpt) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        int coreId = *coreIdOpt;
+        if (kind == 'g') {
+            if (coreId = -1) {
+                return ReplyErrno(EINVAL);
+            }
+            m_selectedCoreId = coreId;
+            MigrateRxIrq(m_selectedCoreId);
+        } else {
+            m_selectedCoreIdForContinuing = coreId;
+        }
+
+        return ReplyOk();
+    }
+
+    GDB_DEFINE_HANDLER(IsThreadAlive)
+    {
+        int coreId = ParseConvertExactlyOneTid(m_commandData).value_or(-1);
+        if (coreId < 0) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        // Is the core off?
+        if (m_attachedCoreList & BIT(coreId)) {
+            return ReplyOk();
+        } else {
+            return ReplyErrno(ESRCH);
+        }
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(CurrentThreadId)
+    {
+        GDB_CHECK_NO_CMD_DATA();
+        return SendFormattedPacket("QC%x", 1 + currentCoreCtx->GetCoreId());
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(fThreadInfo)
+    {
+        GDB_CHECK_NO_CMD_DATA();
+
+        // We have made our GDB packet big enough to list all the thread ids (coreIds + 1 for each coreId)
+        char *buf = GetInPlaceOutputBuffer();
+        size_t n = 0;
+
+        for (int coreId: util::BitsOf{m_attachedCoreList}) {
+            n += sprintf(buf + n, "%lx,", 1u + coreId);
+        }
+
+        // Remove trailing comma
+        --n;
+
+        return SendStreamData(std::string_view{buf, n}, 0, n, true);
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(sThreadInfo)
+    {
+        GDB_CHECK_NO_CMD_DATA();
+
+        // 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 SendPacket("l");
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(ThreadEvents)
+    {
+        if (m_commandData.size() != 1) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        switch (m_commandData[0]) {
+            case '0':
+                m_catchThreadEvents = false;
+                return ReplyOk();
+            case '1':
+                m_catchThreadEvents = true;
+                return ReplyOk();
+            default:
+                return ReplyErrno(EILSEQ);
+        }
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(ThreadExtraInfo)
+    {
+        int coreId = ParseConvertExactlyOneTid(m_commandData).value_or(-1);
+        if (coreId < 0) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        size_t n = sprintf(m_workBuffer, "TODO");
+
+        return SendHexPacket(m_workBuffer, n);
+    }
+}

thermosphere/src/gdb/hvisor_gdb_thread.hpp

@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_gdb_context.hpp"
+
+namespace ams::hvisor::gdb {
+
+    int ConvertTidToCoreId(unsigned long tid);
+    std::optional<int> ParseConvertExactlyOneTid(std::string_view str);
+
+}

thermosphere/src/gdb/hvisor_gdb_vebose.cpp

@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+namespace ams::hvisor::gdb {
+
+    GDB_DEFINE_HANDLER(VerboseCommand)
+    {
+        // Extract name
+        char delim = ':';
+
+        size_t delimPos = m_commandData.find_first_of(";:");
+        std::string_view cmdName = m_commandData;
+        if (delimPos != std::string_view::npos) {
+            delim = m_commandData[delimPos];
+            cmdName.remove_suffix(cmdName.size() - delimPos);
+            m_commandData.remove_prefix(delimPos + 1);
+        }
+
+        if (cmdName == "Cont?") {
+            GDB_VERBOSE_HANDLER(ContinueSupported)();
+        } else if (cmdName == "Cont") {
+            GDB_VERBOSE_HANDLER(Continue)();
+        } else if (cmdName == "CtrlC") {
+            GDB_VERBOSE_HANDLER(CtrlC)();
+        } else if (cmdName == "MustReplyEmpty") {
+            return HandleUnsupported();
+        } else if (cmdName == "Stopped") {
+            return GDB_VERBOSE_HANDLER(Stopped)();
+        } else {
+            return HandleUnsupported(); // No handler found!
+        }
+    }
+
+    GDB_DEFINE_VERBOSE_HANDLER(ContinueSupported)
+    {
+        return SendPacket("vCont;c;C;s;S;t;r");
+    }
+
+}

thermosphere/src/gdb/hvisor_gdb_xfer.cpp

@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+/*
+*   This file is part of Luma3DS.
+*   Copyright (C) 2016-2019 Aurora Wright, TuxSH
+*
+*   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
+*/
+
+#include "hvisor_gdb_defines_internal.hpp"
+#include "hvisor_gdb_packet_data.hpp"
+
+namespace {
+
+    std::string_view 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>";
+
+        std::strcpy(buf, hdr);
+
+        // CPU registers
+        std::strcat(buf, cpuDescBegin);
+        pos = static_cast<int>(std::strlen(buf));
+        for (u32 i = 0; i < 31; i++) {
+            pos += std::sprintf(buf + pos, "<reg name=\"x%u\" bitsize=\"64\"/>", i);
+        }
+        std::strcat(buf, cpuDescEnd);
+
+        std::strcat(buf, fpuDescBegin);
+        pos = static_cast<int>(std::strlen(buf));
+        for (u32 i = 0; i < 32; i++) {
+            pos += std::sprintf(buf + pos, "<reg name=\"v%u\" bitsize=\"128\" type=\"aarch64v\"/>", i);
+        }
+        std::strcat(buf, fpuDescEnd);
+
+        std::strcat(buf, footer);
+        return std::string_view{buf};
+    }
+
+}
+
+namespace ams::hvisor::gdb {
+
+    GDB_DEFINE_XFER_HANDLER(Features)
+    {
+        if (write || annex != "target.xml") {
+            return ReplyEmpty();
+        }
+
+        // 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 (m_targetXml.empty()) {
+            m_targetXml = GenerateTargetXml(m_workBuffer);
+        }
+
+        int n = SendStreamData(m_targetXml, offset, length, false);
+
+        // Transfer ended
+        if(offset + length >= m_targetXml.size()) {
+            m_targetXml = {};
+        }
+
+        return n;
+    }
+
+    GDB_DEFINE_QUERY_HANDLER(Xfer)
+    {
+        // e.g. qXfer:features:read:annex:offset,length
+
+        // Split
+        auto [cmd, directionStr, annex, offsetlen] = SplitString<4>(m_commandData, ':');
+        if (offsetlen.empty()) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        // Check direction
+        bool isWrite;
+        if (directionStr == "read") {
+            isWrite = false;
+        } else if (directionStr == "write") {
+            isWrite = true;
+        } else {
+            return ReplyErrno(EILSEQ);
+        }
+
+        // Get offset and length
+        auto [nread, off, len] = ParseHexIntegerList<2>(offsetlen, isWrite ? ':' : '\0');
+        if (nread == 0) {
+            return ReplyErrno(EILSEQ);
+        }
+
+        // Get data/nothing
+        m_commandData = offsetlen;
+        m_commandData.remove_prefix(nread);
+
+        // Run command
+        if (cmd == "features") {
+            return GDB_XFER_HANDLER(Features)(isWrite, annex, off, len);
+        } else {
+            return HandleUnsupported();
+        }
+    }
+
+}

thermosphere/src/hvisor_core_context.cpp

@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_core_context.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+
+namespace ams::hvisor {
+
+    std::array<CoreContext, MAX_CORE> CoreContext::instances{};
+    std::atomic<u32> CoreContext::activeCoreMask{};
+    bool CoreContext::coldboot = true;
+
+    void CoreContext::InitializeCoreInstance(u32 coreId, bool isBootCore, u64 argument)
+    {
+        CoreContext &instance = instances[coreId];
+        instance.m_coreId = coreId;
+        instance.m_bootCore = isBootCore;
+        instance.m_kernelArgument = argument;
+        if (isBootCore && instance.m_kernelEntrypoint == 0) {
+            instance.m_kernelEntrypoint = initialKernelEntrypoint;
+        }
+        currentCoreCtx = &instance;
+        cpu::dmb();
+    }
+}

thermosphere/src/hvisor_core_context.hpp

@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+#include "defines.hpp"
+
+namespace ams::hvisor {
+
+    struct ExceptionStackFrame;
+    class CoreContext;
+
+    register CoreContext *currentCoreCtx asm("x18");
+
+    class alignas(64) CoreContext final {
+        // This should be 64-byte big
+        NON_COPYABLE(CoreContext);
+        NON_MOVEABLE(CoreContext);
+        private:
+            static std::array<CoreContext, MAX_CORE> instances;
+            static std::atomic<u32> activeCoreMask;
+            static bool coldboot; // "coldboot" to be 'true' on init & thus not in BSS
+
+            // start.s
+            static uintptr_t initialKernelEntrypoint;
+
+        private:
+            ExceptionStackFrame *m_guestFrame = nullptr;
+
+            u64 m_kernelArgument = 0;
+            uintptr_t m_kernelEntrypoint = 0;
+        
+            u32 m_coreId = 0;
+            bool m_bootCore = false;
+
+            // Debug features
+            bool m_wasPaused = false;
+            uintptr_t m_steppingRangeStartAddr = 0;
+            uintptr_t m_steppingRangeEndAddr = 0;
+
+            // Timer stuff
+            u64 m_totalTimeInHypervisor = 0;
+            u64 m_emulPtimerCval = 0;
+
+        private:
+            constexpr CoreContext() = default;
+
+        public:
+            static void InitializeCoreInstance(u32 coreId, bool isBootCore, u64 argument);
+
+            static CoreContext &GetInstanceFor(u32 coreId)                  { return instances[coreId]; }
+            static u32 GetActiveCoreMask()                                  { return activeCoreMask.load(); }
+            static u32 SetCurrentCoreActive()
+            {
+                activeCoreMask |= BIT(currentCoreCtx->m_coreId);
+            }
+            static bool IsColdboot()                                        { return coldboot; }
+
+        public:
+            constexpr ExceptionStackFrame *GetGuestFrame() const            { return m_guestFrame; }
+            constexpr void SetGuestFrame(ExceptionStackFrame *frame)        { m_guestFrame = frame; }
+
+            constexpr u64 GetKernelArgument() const                         { return m_kernelArgument; }
+
+            constexpr u64 GetKernelEntrypoint() const                       { return m_kernelEntrypoint; }
+
+            constexpr u32 GetCoreId() const                                 { return m_coreId; }
+            constexpr bool IsBootCore() const                               { return m_bootCore; }
+
+            constexpr u64 SetKernelEntrypoint(uintptr_t ep, bool warmboot = false)
+            {
+                if (warmboot) {
+                    // No race possible, only possible transition is 1->0 and we only really check IsColdboot() at init time
+                    // And CPU_SUSPEND should only be called with only one core left.
+                    coldboot = false;
+                }
+                m_kernelEntrypoint = ep;
+            }
+
+            constexpr bool WasPaused() const                                { return m_wasPaused; }
+            constexpr void SetPausedFlag(bool wasPaused)                    { m_wasPaused = wasPaused; }
+
+            constexpr auto GetSteppingRange() const
+            {
+                return std::tuple{m_steppingRangeStartAddr, m_steppingRangeEndAddr};
+            }
+            constexpr void SetSteppingRange(uintptr_t startAddr, uintptr_t endAddr)
+            {
+                m_steppingRangeStartAddr = startAddr;
+                m_steppingRangeEndAddr = endAddr;
+            }
+
+            constexpr u64 GetTotalTimeInHypervisor() const                  { return m_totalTimeInHypervisor; }
+            constexpr void IncrementTotalTimeInHypervisor(u64 timeDelta)    { m_totalTimeInHypervisor += timeDelta; }
+
+            constexpr u64 GetEmulPtimerCval() const                         { return m_emulPtimerCval; }
+            constexpr void SetEmulPtimerCval(u64 cval)                      { m_emulPtimerCval = cval; }
+    };
+}

thermosphere/src/hvisor_exception_dispatcher.cpp

@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_exception_dispatcher.hpp"
+#include "hvisor_irq_manager.hpp"
+#include "hvisor_fpu_register_cache.hpp"
+#include "hvisor_guest_timers.hpp"
+#include "hvisor_generic_timer.hpp"
+#include "hvisor_memory_map.hpp"
+
+#include "traps/hvisor_traps_data_abort.hpp"
+#include "traps/hvisor_traps_hvc.hpp"
+#include "traps/hvisor_traps_single_step.hpp"
+#include "traps/hvisor_traps_smc.hpp"
+#include "traps/hvisor_traps_sysreg.hpp"
+
+#include "debug_manager.h"
+
+namespace ams::hvisor {
+
+    void EnableGeneralTraps(void)
+    {
+        u64 hcr = THERMOSPHERE_GET_SYSREG(hcr_el2);
+
+        // Trap SMC instructions
+        hcr |= cpu::HCR_TSC;
+
+        // Trap set/way isns
+        hcr |= cpu::HCR_TSW;
+
+        // Reroute physical IRQs to EL2
+        hcr |= cpu::HCR_IMO;
+
+        // Make sure HVC is enabled
+        hcr &= ~cpu::HCR_HCD;
+
+        THERMOSPHERE_SET_SYSREG(hcr_el2, hcr);
+
+        EnableGuestTimerTraps();
+    }
+
+    void DumpStackFrame(ExceptionStackFrame *frame, bool sameEl)
+    {
+#ifndef NDEBUG
+        uintptr_t stackTop = MemoryMap::GetStackTopVa(currentCoreCtx->GetCoreId());
+
+        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_el1\t\t%016llx\n", frame->sp_el1);
+        }
+        DEBUG("sp_el0\t\t%016llx\n", frame->sp_el0);
+        DEBUG("cntpct_el0\t%016llx\n", frame->cntpct_el0);
+        if (frame == currentCoreCtx->GetGuestFrame()) {
+            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 = reinterpret_cast<u64 *>(frame->sp_el2);
+            u64 *spEnd = sp + 0x20;
+            u64 *spMax = reinterpret_cast<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
+    }
+
+    void ExceptionEntryPostprocess(ExceptionStackFrame *frame, bool isLowerEl)
+    {
+        if (isLowerEl) {
+            currentCoreCtx->SetGuestFrame(frame);
+            frame->cntp_ctl_el0 = THERMOSPHERE_GET_SYSREG(cntp_ctl_el0);
+            frame->cntv_ctl_el0 = THERMOSPHERE_GET_SYSREG(cntv_ctl_el0);
+        }
+    }
+
+    void ExceptionReturnPreprocess(ExceptionStackFrame *frame)
+    {
+        if (frame == currentCoreCtx->GetGuestFrame()) {
+            if (currentCoreCtx->WasPaused()) { 
+                // Were we paused & are we about to return to the guest?
+                IrqManager::EnterInterruptibleHypervisorCode();
+                while (!debugManagerHandlePause());
+                FpuRegisterCache::GetInstance().CleanInvalidate;
+            }
+
+            // Update virtual counter
+            u64 ticksNow = GenericTimer::GetSystemTick();
+            currentCoreCtx->IncrementTotalTimeInHypervisor(ticksNow - frame->cntpct_el0);
+            UpdateVirtualOffsetSysreg();
+
+            // Restore timer interrupt config
+            THERMOSPHERE_SET_SYSREG(cntp_ctl_el0, frame->cntp_ctl_el0);
+            THERMOSPHERE_SET_SYSREG(cntv_ctl_el0, frame->cntv_ctl_el0);
+        }
+    }
+
+    void HandleLowerElSyncException(ExceptionStackFrame *frame)
+    {
+        auto esr = frame->esr_el2;
+        switch (esr.ec) {
+            case cpu::ExceptionSyndromeRegister::CP15RTTrap:
+                traps::HandleMcrMrcCP15Trap(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::CP15RRTTrap:
+                traps::HandleMcrrMrrcCP15Trap(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::CP14RTTrap:
+            case cpu::ExceptionSyndromeRegister::CP14DTTrap:
+            case cpu::ExceptionSyndromeRegister::CP14RRTTrap:
+                // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0)
+                traps::HandleA32CP14Trap(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::HypervisorCallA64:
+                traps::HandleHvc(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::MonitorCallA64:
+                traps::HandleSmc(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::SystemRegisterTrap:
+                traps::HandleMsrMrsTrap(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::DataAbortLowerEl:
+                // Basically, stage2 translation faults
+                traps::HandleLowerElDataAbort(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::SoftwareStepLowerEl:
+                traps::HandleSingleStep(frame, esr);
+                break;
+            case cpu::ExceptionSyndromeRegister::BreakpointLowerEl:
+            case cpu::ExceptionSyndromeRegister::WatchpointLowerEl:
+            case cpu::ExceptionSyndromeRegister::SoftwareBreakpointA64:
+            case cpu::ExceptionSyndromeRegister::SoftwareBreakpointA32:
+                debugManagerReportEvent(DBGEVENT_EXCEPTION);
+                break;
+            default:
+                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;
+        }
+    }
+
+    void HandleSameElSyncException(ExceptionStackFrame *frame)
+    {
+        auto esr = frame->esr_el2;
+        DEBUG("Same EL sync exception on core %x, EC = 0x%02x IL=%llu ISS=0x%06llx\n", currentCoreCtx->GetCoreId(), esr.ec, esr.il, esr.iss);
+        DumpStackFrame(frame, true);
+    }
+
+    void HandleUnknownException(u32 offset)
+    {
+        DEBUG("Unknown exception on core %x! (offset 0x%03lx)\n", currentCoreCtx->GetCoreId(), offset);
+    }
+
+}

thermosphere/src/hvisor_exception_dispatcher.hpp

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_exception_stack_frame.hpp"
+#include "cpu/hvisor_cpu_exception_sysregs.hpp"
+
+namespace ams::hvisor {
+
+    void EnableGeneralTraps(void);
+
+    void DumpStackFrame(const ExceptionStackFrame *frame, bool sameEl);
+
+    // Called on exception entry (avoids overflowing a vector section)
+    void ExceptionEntryPostprocess(ExceptionStackFrame *frame, bool isLowerEl);
+
+    // Called on exception return (avoids overflowing a vector section)
+    void ExceptionReturnPreprocess(ExceptionStackFrame *frame);
+
+    void HandleLowerElSyncException(ExceptionStackFrame *frame);
+
+    void HandleSameElSyncException(ExceptionStackFrame *frame);
+
+    void HandleUnknownException(u32 offset);
+
+}

thermosphere/src/hvisor_exception_stack_frame.hpp

@@ -0,0 +1,144 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "cpu/hvisor_cpu_exception_sysregs.hpp"
+
+namespace ams::hvisor {
+
+    struct alignas(16) ExceptionStackFrame {
+        u64 x[31]; // x0 .. x30
+        union {
+            u64 sp_el1;
+            u64 sp_el2;
+        };
+        u64 sp_el0;
+        u64 elr_el2;
+        u64 spsr_el2;
+        cpu::ExceptionSyndromeRegister esr_el2;
+        u64 far_el2;
+        u64 cntpct_el0;
+        u64 cntp_ctl_el0;
+        u64 cntv_ctl_el0;
+
+        constexpr bool IsA32() const    { return (spsr_el2 & cpu::PSR_MODE32) != 0; }
+        constexpr bool IsThumb() const  { return IsA32() && (spsr_el2 & cpu::PSR_AA32_THUMB) != 0; }
+
+        constexpr u32 GetT32ItFlags() const
+        {
+            u64 it10 = (spsr_el2 >> cpu::PSR_AA32_IT10_MASK) & cpu::PSR_AA32_IT10_MASK;
+            u64 it72 = (spsr_el2 >> cpu::PSR_AA32_IT72_MASK) & cpu::PSR_AA32_IT72_MASK;
+            return it72 << 2 | it10;
+        }
+        constexpr void SetT32ItFlags(u32 flags)
+        {
+            spsr_el2 &= ~(cpu::PSR_AA32_IT72_MASK << cpu::PSR_AA32_IT72_SHIFT);
+            spsr_el2 &= ~(cpu::PSR_AA32_IT10_MASK << cpu::PSR_AA32_IT10_SHIFT);
+
+            u64 it10 = flags & cpu::PSR_AA32_IT10_MASK;
+            u64 it72 = (flags >> 2) & cpu::PSR_AA32_IT72_MASK;
+
+            spsr_el2 |= it72 << cpu::PSR_AA32_IT72_SHIFT;
+            spsr_el2 |= it10 << cpu::PSR_AA32_IT10_SHIFT;
+        }
+
+        constexpr bool EvaluateConditionCode(u32 conditionCode) const
+        {
+            u64 spsr = spsr_el2;
+            if (conditionCode == 14) {
+                // AL
+                return true;
+            } else if (conditionCode == 15) {
+                // Invalid encoding
+                return false;
+            }
+
+            // 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;
+
+            bool tableHalf[] = {
+                // EQ, CS, MI, VS, HI, GE, GT
+                z, c, n, v, c && !z, n == v, !z && n == v,
+            };
+
+            return (conditionCode & 1) == 0 ? tableHalf[conditionCode / 2] : !tableHalf[conditionCode / 2];
+        }
+
+        constexpr void AdvanceItState()
+        {
+            u32 it = GetT32ItFlags();
+
+            // Just in case EL0 is executing A32 (& not sure if fully supported)
+            if (!IsThumb() || it == 0) {
+                return;
+            }
+
+            // Last instruction of the block => wipe, otherwise advance
+            SetT32ItFlags((it & 7) == 0 ? 0 : (it & 0xE0) | ((it << 1) & 0x1F));
+        }
+
+        constexpr void SkipInstruction(size_t size)
+        {
+            AdvanceItState();
+            elr_el2 += size;
+        }
+
+        template<typename T = u64>
+        constexpr T ReadRegister(u32 id) const
+        {
+            static_assert(std::is_integral_v<T> && std::is_unsigned_v<T>);
+            return id == 31 ? static_cast<T>(0u) /* xzr */ : static_cast<T>(x[id]);
+        }
+        constexpr void WriteRegister(u32 id, u64 val)
+        {
+            if (id != 31) {
+                // If not xzr
+                x[id] = val;
+            }
+        }
+
+        constexpr u64 &GetSpRef()
+        {
+            // 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 = false;
+            u64 m = spsr_el2 & 0xF;
+            if (IsA32()) {
+                spEl0 = m == 0;
+            } else {
+                u64 el = m >> 2;
+                spEl0 = el == 0 || (m & 1) == 0; // note: frame->sp_el2 is aliased to frame->sp_el1
+            }
+
+            return spEl0 ? sp_el0 : sp_el1;
+        }
+
+    };
+
+    static_assert(offsetof(ExceptionStackFrame, far_el2) == 0x120, "Wrong definition for ExceptionStackFrame");
+    static_assert(sizeof(ExceptionStackFrame) == 0x140, "Wrong size for ExceptionStackFrame");
+
+    static_assert(std::is_standard_layout_v<ExceptionStackFrame>);
+    static_assert(std::is_trivial_v<ExceptionStackFrame>);
+}
+
+/*void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl);
+void exceptionEnterInterruptibleHypervisorCode(void);*/

thermosphere/src/hvisor_fpu_register_cache.hpp

@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+#include "hvisor_core_context.hpp"
+
+namespace ams::hvisor {
+
+    class FpuRegisterCache final {
+        SINGLETON_WITH_ATTRS(FpuRegisterCache, TEMPORARY);
+        public:
+            struct Storage {
+                u128 q[32];
+                u64 fpsr;
+                u64 fpcr;
+            };
+            static_assert(std::is_standard_layout_v<Storage>);
+
+        private:
+            static void ReloadRegisters(const Storage *storage);
+            static void DumpRegisters(Storage *storage);
+
+        private:
+            Storage m_storage{};
+            u32 m_coreId = 0;
+            bool m_valid = false;
+            bool m_dirty = false;
+
+        public:
+            constexpr void TakeOwnership()
+            {
+                if (m_coreId != currentCoreCtx->GetCoreId()) {
+                    m_valid = false;
+                    m_dirty = false;
+                }
+                m_coreId = currentCoreCtx->GetCoreId();
+            }
+
+            Storage &GetStorageRef()
+            {
+                return m_storage;
+            }
+
+            Storage &ReadRegisters()
+            {
+                if (!m_valid) {
+                    DumpRegisters(&m_storage);
+                    m_valid = true;
+                }
+                return m_storage;
+            }
+
+            constexpr void CommitRegisters()
+            {
+                m_dirty = true;
+                // Because the caller rewrote the entire cache in the event it didn't read it before:
+                m_valid = true;
+            }
+
+            void CleanInvalidate()
+            {
+                if (m_dirty && m_coreId == currentCoreCtx->GetCoreId()) {
+                    ReloadRegisters(&m_storage);
+                    m_dirty = false;
+                }
+                m_valid = false;
+            }
+        public:
+            constexpr FpuRegisterCache() = default;
+    };
+
+}

thermosphere/src/hvisor_fpu_register_cache_impl.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 _ZN3ams3hyp16FpuRegisterCache15ReloadRegistersEPKNS1_7StorageE
+    dmb     ish
+    LDSTORE_QREGS ldp
+    ldp     x1, x2, [x0]
+    msr     fpsr, x1
+    msr     fpcr, x2
+    dsb     ish
+    isb
+    ret
+END_FUNCTION
+
+FUNCTION _ZN3ams3hyp16FpuRegisterCache13DumpRegistersEPNS1_7StorageE
+    dsb     ish
+    isb
+    LDSTORE_QREGS stp
+    mrs     x1, fpsr
+    mrs     x2, fpcr
+    stp     x1, x2, [x0]
+    dmb     ish
+    ret
+END_FUNCTION

thermosphere/src/hvisor_generic_timer.cpp

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_generic_timer.hpp"
+#include "hvisor_irq_manager.hpp"
+#include "hvisor_core_context.hpp"
+
+#include "cpu/hvisor_cpu_interrupt_mask_guard.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+
+#include <mutex>
+
+namespace ams::hvisor {
+
+    void GenericTimer::Initialize()
+    {
+        Configure(false, false);
+        if (currentCoreCtx->IsBootCore()) {
+            m_timerFreq = THERMOSPHERE_GET_SYSREG(cntfrq_el0);
+        }
+
+        IrqManager::GetInstance().Register(*this, irqId, true);
+    }
+
+    std::optional<bool> GenericTimer::InterruptTopHalfHandler(u32 irqId, u32)
+    {
+        if (irqId != GenericTimer::irqId) {
+            return std::nullopt;
+        }
+
+        // Mask the timer interrupt until reprogrammed
+        Configure(false, false);
+
+        return false;
+    }
+
+    void GenericTimer::WaitTicks(s64 ticks)
+    {
+        IrqManager::EnterInterruptibleHypervisorCode();
+        auto flags = cpu::UnmaskIrq();
+        SetTimeoutTicks(ticks);
+        do {
+            cpu::wfi();
+        } while (!GetInterruptStatus());
+        cpu::RestoreInterruptFlags(flags);
+    }
+
+}

thermosphere/src/hvisor_generic_timer.hpp

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+#include "defines.hpp"
+
+#include "hvisor_i_interrupt_task.hpp"
+#include "cpu/hvisor_cpu_sysreg_general.hpp"
+
+#include "preprocessor.h"
+#include "platform/interrupt_config.h"
+
+#include <chrono>
+
+namespace ams::hvisor {
+
+    class GenericTimer final : public IInterruptTask {
+        SINGLETON(GenericTimer);
+        private:
+            static constexpr u32 irqId = GIC_IRQID_NS_PHYS_HYP_TIMER;
+
+        private:
+            static void Configure(bool enabled, bool interruptMasked)
+            {
+                u64 ebit = enabled          ? cpu::CNTCTL_ENABLE : 0;
+                u64 mbit = interruptMasked  ? cpu::CNTCTL_IMASK: 0;
+                THERMOSPHERE_SET_SYSREG(cnthp_ctl_el2, mbit | ebit);
+            }
+
+            static bool GetInterruptStatus()
+            {
+                return (THERMOSPHERE_GET_SYSREG(cnthp_ctl_el2) & cpu::CNTCTL_ISTATUS) != 0;
+            }
+
+        private:
+            u64 m_timerFreq = 0;
+
+        private:
+            constexpr GenericTimer() = default;
+
+        public:
+            static s64 GetSystemTick()
+            {
+                return static_cast<s64>(THERMOSPHERE_GET_SYSREG(cntpct_el0));
+            }
+
+            static void SetTimeoutTicks(s64 ticks)
+            {
+                THERMOSPHERE_SET_SYSREG(cnthp_cval_el2, GetSystemTick() + ticks);
+                Configure(true, false);
+            }
+
+            static void WaitTicks(s64 ticks);
+
+        public:
+            void Initialize();
+            std::optional<bool> InterruptTopHalfHandler(u32 irqId, u32) final;
+
+            constexpr u64 GetTimerFrequency() const { return m_timerFreq; }
+
+            template<typename SecondRatio = std::ratio<1>>
+            auto GetSystemTime() const
+            {
+                auto tick = GetSystemTick();
+                return (tick * SecondRatio::den) / (m_timerFreq * SecondRatio::num);
+            }
+
+            std::chrono::nanoseconds GetSystemTimeNs() const
+            {
+                return std::chrono::nanoseconds{GetSystemTime<std::nano>()};
+            }
+
+            template<typename Duration>
+            void SetTimeout(Duration d) const
+            {
+                using SecondRatio = typename Duration::period;
+                auto v = (d.count() * m_timerFreq * SecondRatio::num) / SecondRatio::den;
+                SetTimeoutTicks(v);
+            }
+
+            template<typename Duration>
+            void Wait(Duration d) const
+            {
+                using SecondRatio = typename Duration::period;
+                auto v = (d.count() * m_timerFreq * SecondRatio::num) / SecondRatio::den;
+                WaitTicks(v);
+            }
+    };
+
+}

thermosphere/src/hvisor_gicv2.hpp

@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+
+namespace ams::hvisor {
+
+    struct GicV2Distributor {
+        static constexpr u32 maxIrqId = 1019;
+        static constexpr u32 spuriousGrpNeedAckIrqId = 1022;
+        static constexpr u32 spuriousIrqId = 1023;
+
+        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];
+
+        enum SgirTargetListFilter : u32 {
+            ForwardToTargetList = 0,
+            ForwardToAllOthers  = 1,
+            ForwardToSelf       = 2,
+        };
+
+        static constexpr int GetCfgrShift(u32 id) {
+            return 2 * (id % 16);
+        }
+    };
+
+    struct GicV2Controller {
+        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];
+    };
+
+    // GICH
+    struct GicV2VirtualInterfaceController {
+        union HypervisorControlRegister {
+            struct {
+                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;
+            };
+            u32 raw;
+        };
+
+        union VmControlRegister {
+            struct {
+                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;
+            };
+            u32 raw;
+        };
+
+        union MaintenanceIntStatRegister {
+            struct {
+                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;
+            };
+            u32 raw;
+        };
+
+        union ListRegister {
+            struct {
+                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;
+            };
+            u32 raw;
+        };
+
+        HypervisorControlRegister hcr;
+        u32 vtr;
+        VmControlRegister vmcr;
+        u8 _0x0c[0x10 - 0xC];
+        MaintenanceIntStatRegister 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];
+        ListRegister lr[64];
+    };
+
+    struct GicV2VirtualInterface : public GicV2Controller {
+        // Allowed because no non-static members
+        static constexpr u32 numPriorityLevels = 32;
+        static constexpr u8  idlePriorityLevel = 0xF8;
+    };
+
+    static_assert(std::is_standard_layout_v<GicV2Distributor>);
+    static_assert(std::is_standard_layout_v<GicV2Controller>);
+    static_assert(std::is_standard_layout_v<GicV2VirtualInterfaceController>);
+    static_assert(std::is_standard_layout_v<GicV2VirtualInterface>);
+
+    static_assert(std::is_trivial_v<GicV2Distributor>);
+    static_assert(std::is_trivial_v<GicV2Controller>);
+    static_assert(std::is_trivial_v<GicV2VirtualInterfaceController>);
+    static_assert(std::is_trivial_v<GicV2VirtualInterface>);
+}

thermosphere/src/hvisor_guest_memory.cpp

@@ -0,0 +1,240 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_virtual_gic.hpp"
+#include "hvisor_safe_io_copy.hpp"
+#include "cpu/hvisor_cpu_caches.hpp"
+#include "cpu/hvisor_cpu_interrupt_mask_guard.hpp"
+
+using namespace ams::hvisor;
+using namespace ams::hvisor::cpu;
+
+namespace {
+
+    template<typename T>
+    T ReadBufferValue(const void *buf, size_t off)
+    {
+        static_assert(std::is_unsigned_v<T> && sizeof(T) <= 4);
+        T ret;
+        std::memcpy(&ret, reinterpret_cast<const u8 *>(buf) + off, sizeof(T));
+        return ret;
+    }
+
+    template<typename T>
+    void WriteBufferValue(void *buf, size_t off, T val)
+    {
+        static_assert(std::is_unsigned_v<T> && sizeof(T) <= 4);
+        std::memcpy(reinterpret_cast<u8 *>(buf) + off, T, sizeof(T));
+    }
+
+    size_t GuestReadWriteGicd(size_t offset, size_t size, void *readBuf, const void *writeBuf)
+    {
+        auto &vgic = VirtualGic::GetInstance();
+        if (readBuf != nullptr) {
+            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
+                    WriteBufferValue<u32>(readBuf, readOffset, vgic.ReadGicdRegister(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.
+                    return readOffset;
+                } else if (VirtualGic::ValidateGicdRegisterAccess(offset + readOffset, 1)) {
+                    // Valid byte access
+                    WriteBufferValue<u8>(readBuf, readOffset, vgic.ReadGicdRegister(offset + readOffset, 1));
+                    readOffset += 1;
+                    rem -= 1;
+                } else {
+                    // Invalid byte access
+                    return readOffset;
+                }
+            }
+        }
+
+        if (writeBuf != nullptr) {
+            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
+                    vgic.WriteGicdRegister(ReadBufferValue<u32>(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.
+                    return writeOffset;
+                } else if (VirtualGic::ValidateGicdRegisterAccess(offset + writeOffset, 1)) {
+                    // Valid byte access
+                    vgic.WriteGicdRegister(ReadBufferValue<u8>(writeBuf, writeOffset), offset + writeOffset, 1);
+                    writeOffset += 1;
+                    rem -= 1;
+                } else {
+                    // Invalid byte access
+                    return writeOffset;
+                }
+            }
+        }
+
+        return size;
+    }
+
+    size_t GuestReadWriteDeviceMemory(void *addr, size_t size, void *readBuf, const void *writeBuf)
+    {
+        if (readBuf != nullptr) {
+            size_t sz = SafeIoCopy(readBuf, addr, size);
+            if (sz < size) {
+                return sz;
+            }
+        }
+
+        if (writeBuf != nullptr) {
+            size_t sz = SafeIoCopy(addr, writeBuf, size);
+            if (sz < size) {
+                return sz;
+            }
+        }
+
+        // Translation tables must be on Normal memory & Device memory isn't cacheable, so we don't have
+        // that kind of thing to handle...
+
+        return size;
+    }
+
+    size_t GuestReadWriteNormalMemory(void *addr, size_t size, void *readBuf, const void *writeBuf)
+    {
+        if (readBuf != nullptr) {
+            std::memcpy(readBuf, addr, size);
+        }
+
+        if (writeBuf != nullptr) {
+            std::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.
+            CleanDataCacheRangePoU(addr, size);
+            u32 policy = GetInstructionCachePolicy();
+            if (policy == 1 || policy == 2) {
+                // AVIVT, VIVT
+                InvalidateInstructionCache();
+            } else {
+                // VPIPT, PIPT
+                // Ez coherency, just do range operations...
+                InvalidateInstructionCacheRangePoU(addr, size);
+            }
+            TlbInvalidateEl1();
+            dsb();
+            isb();
+        }
+
+        return size;
+    }
+
+    size_t GuestReadWriteMemoryPage(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf)
+    {
+        InterruptMaskGuard ig{};
+        size_t offset = addr & 0xFFFul;
+
+        // Translate the VA, stages 1&2
+        __asm__ __volatile__ ("at s12e1r, %0" :: "r"(addr) : "memory");
+        u64 par = THERMOSPHERE_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 = THERMOSPHERE_GET_SYSREG(par_el1);
+                if ((par & PAR_F) != 0 || (par & PAR_PA_MASK) != VirtualGic::gicdPhysicalAddress) {
+                    // The guest doesn't have access to it...
+                    // Read as 0, write ignored
+                    if (readBuf != NULL) {
+                        std::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;
+            u64 shrb = (par >> PAR_SH_SHIFT) & PAR_SH_MASK;
+            uintptr_t pa = par & PAR_PA_MASK;
+
+            uintptr_t va = MemoryMap::MapGuestPage(pa, memAttribs, shrb);
+            void *vaddr = reinterpret_cast<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);
+            }
+
+            MemoryMap::UnmapGuestPage();
+        }
+
+        return size;
+    }
+
+}
+
+namespace ams::hvisor {
+
+    size_t GuestReadWriteMemory(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf)
+    {
+        uintptr_t curAddr = addr;
+        size_t remainingAmount = size;
+        u8 *rb8 = reinterpret_cast<u8 *>(readBuf);
+        const u8 *wb8 = reinterpret_cast<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/hvisor_guest_memory.hpp

@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+
+namespace ams::hvisor {
+
+    size_t GuestReadWriteMemory(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf);
+
+    inline size_t GuestReadMemory(uintptr_t addr, size_t size, void *buf)
+    {
+        return GuestReadWriteMemory(addr, size, buf, NULL);
+    }
+
+    inline size_t GuestWriteMemory(uintptr_t addr, size_t size, const void *buf)
+    {
+        return GuestReadWriteMemory(addr, size, NULL, buf);
+    }
+
+}

thermosphere/src/hvisor_guest_timers.hpp

@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_core_context.hpp"
+#include "hvisor_exception_stack_frame.hpp"
+#include "cpu/hvisor_cpu_sysreg_general.hpp"
+
+namespace ams::hvisor {
+
+    inline u64 ComputeCntvct(const ExceptionStackFrame *frame)
+    {
+        return frame->cntpct_el0 - currentCoreCtx->GetTotalTimeInHypervisor();
+    }
+
+    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->SetEmulPtimerCval(val);
+        THERMOSPHERE_SET_SYSREG(cntp_cval_el0, frame->cntpct_el0 + (val - vct));
+    }
+
+    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->GetEmulPtimerCval();
+        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;
+    }
+
+    ALWAYS_INLINE void EnableGuestTimerTraps(void)
+    {
+        // Disable event streams, trap everything
+        u64 cnthctl = 0;
+        THERMOSPHERE_SET_SYSREG(cnthctl_el2, cnthctl);
+    }
+
+    ALWAYS_INLINE void UpdateVirtualOffsetSysreg(void)
+    {
+        THERMOSPHERE_SET_SYSREG(cntvoff_el2, currentCoreCtx->GetTotalTimeInHypervisor());
+    }
+}

thermosphere/src/hvisor_hw_breakpoint_manager.cpp

@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_hw_breakpoint_manager.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+
+#define _REENT_ONLY
+#include <cerrno>
+
+// Can't use two THERMOSPHERE_SAVE_SYSREG as it prevents ldp from being generated
+#define SAVE_BREAKPOINT(i, _)\
+    __asm__ __volatile__ (\
+        "msr " STRINGIZE(dbgbvr##i##_el1) ", %0\n"\
+        "msr " STRINGIZE(dbgbcr##i##_el1) ", %1"\
+        :\
+        : "r"(m_stopPoints[i].vr), "r"(m_stopPoints[i].cr.raw)\
+        : "memory"\
+    );
+
+namespace ams::hvisor {
+
+    HwBreakpointManager HwBreakpointManager::instance{};
+
+    void HwBreakpointManager::Reload() const
+    {
+        cpu::dmb();
+        EVAL(REPEAT(MAX_BCR, SAVE_BREAKPOINT, ~));
+        cpu::dsb();
+        cpu::isb();
+    }
+
+    bool HwBreakpointManager::FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t, cpu::DebugRegisterPair::LoadStoreControl) const
+    {
+        return pair.vr == addr;
+    }
+
+    // Note: A32/T32/T16 support intentionnally left out
+    // Note: addresses are supposed to be well-formed regarding the sign extension bits
+    int HwBreakpointManager::Add(uintptr_t addr)
+    {
+        // Reject misaligned addresses
+        if (addr & 3) {
+            return -EINVAL;
+        }
+
+        cpu::DebugRegisterPair bp{};
+        bp.cr.bt = cpu::DebugRegisterPair::AddressMatch;
+        bp.cr.bas = 0xF; // mandated
+        bp.vr = addr;
+
+        return AddImpl(addr, 0, bp);
+    }
+
+    int HwBreakpointManager::Remove(uintptr_t addr)
+    {
+        // Reject misaligned addresses
+        if (addr & 3) {
+            return -EINVAL;
+        }
+
+        return RemoveImpl(addr, 0, cpu::DebugRegisterPair::NotAWatchpoint);
+    }
+}

thermosphere/src/hvisor_hw_breakpoint_manager.hpp

@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_hw_stop_point_manager.hpp"
+
+namespace ams::hvisor {
+
+    class HwBreakpointManager final : public HwStopPointManager {
+        SINGLETON(HwBreakpointManager);
+        private:
+            bool FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t, cpu::DebugRegisterPair::LoadStoreControl) const final;
+            void Reload() const final;
+
+        private:
+            constexpr HwBreakpointManager() : HwStopPointManager(MAX_BCR, IrqManager::ReloadHwBreakpointsSgi) {}
+
+        public:
+            int Add(uintptr_t addr);
+            int Remove(uintptr_t addr);
+    };
+}

thermosphere/src/hvisor_hw_stop_point_manager.cpp

@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_hw_stop_point_manager.hpp"
+#include "hvisor_core_context.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+#include "cpu/hvisor_cpu_interrupt_mask_guard.hpp"
+#include <mutex>
+
+#define _REENT_ONLY
+#include <cerrno>
+
+namespace ams::hvisor {
+
+    void HwStopPointManager::DoReloadOnAllCores() const
+    {
+        cpu::InterruptMaskGuard mg{};
+        cpu::dmb();
+        Reload();
+        m_reloadBarrier.Reset(CoreContext::GetActiveCoreMask());
+        IrqManager::GenerateSgiForAllOthers(m_irqId);
+        m_reloadBarrier.Join();
+    }
+
+    cpu::DebugRegisterPair *HwStopPointManager::Allocate()
+    {
+        size_t pos = __builtin_ffs(m_freeBitmap);
+        if (pos == 0) {
+            return nullptr;
+        } else {
+            m_freeBitmap &= ~BIT(pos - 1);
+            m_usedBitmap |= BIT(pos - 1);
+            return &m_stopPoints[pos - 1];
+        }
+    }
+
+    void HwStopPointManager::Free(size_t pos)
+    {
+        m_stopPoints[pos] = {};
+        m_freeBitmap |= BIT(pos);
+        m_usedBitmap &= ~BIT(pos);
+    }
+
+    const cpu::DebugRegisterPair *HwStopPointManager::Find(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl dir) const
+    {
+        for (auto bit: util::BitsOf{m_usedBitmap}) {
+            auto *p = &m_stopPoints[bit];
+            if (FindPredicate(*p, addr, size, dir)) {
+                return p;
+            }
+        }
+
+        return nullptr;
+    }
+
+    int HwStopPointManager::AddImpl(uintptr_t addr, size_t size, cpu::DebugRegisterPair preconfiguredPair)
+    {
+        std::scoped_lock lk{m_lock};
+        auto lsc = preconfiguredPair.cr.lsc;
+
+        if (m_freeBitmap == 0) {
+            // Oops
+            return -EBUSY;
+        }
+
+        if (Find(addr, size, lsc) != nullptr) {
+            // Already exists
+            return -EEXIST;
+        }
+
+        auto *regs = Allocate();
+        regs->SetDefaults();
+
+        // Apply preconfig
+        regs->cr.raw |= preconfiguredPair.cr.raw;
+        regs->vr = preconfiguredPair.vr;
+        regs->cr.enabled = true;
+
+        DoReloadOnAllCores();
+        return 0;
+    }
+
+    int HwStopPointManager::RemoveImpl(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl dir)
+    {
+        std::scoped_lock lk{m_lock};
+        const auto *p = Find(addr, size, dir);
+        if (p == nullptr) {
+            return -ENOENT;
+        }
+
+        Free(p - m_stopPoints.data());
+        return 0;
+    }
+
+    void HwStopPointManager::RemoveAll()
+    {
+        std::scoped_lock lk{m_lock};
+        m_freeBitmap |= m_usedBitmap;
+        m_usedBitmap = 0;
+        std::fill(m_stopPoints.begin(), m_stopPoints.end(), cpu::DebugRegisterPair{});
+        DoReloadOnAllCores();
+    }
+
+    std::optional<bool> HwStopPointManager::InterruptTopHalfHandler(u32 irqId, u32)
+    {
+        if (irqId != m_irqId) {
+            return {};
+        }
+
+        Reload();
+        return false;
+    }
+}

thermosphere/src/hvisor_hw_stop_point_manager.hpp

@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+#include "cpu/hvisor_cpu_debug_register_pair.hpp"
+#include "hvisor_irq_manager.hpp"
+
+namespace ams::hvisor {
+
+    class HwStopPointManager : public IInterruptTask {
+        NON_COPYABLE(HwStopPointManager);
+        NON_MOVEABLE(HwStopPointManager);
+        protected:
+            static constexpr size_t maxStopPoints = std::max(MAX_BCR, MAX_WCR);
+
+        protected:
+            mutable RecursiveSpinlock m_lock{};
+            mutable Barrier m_reloadBarrier{};
+
+            u16 m_freeBitmap;
+            u16 m_usedBitmap = 0;
+            std::array<cpu::DebugRegisterPair, maxStopPoints> m_stopPoints{};
+            IrqManager::ThermosphereSgi m_irqId;
+
+        protected:
+            void DoReloadOnAllCores() const;
+            cpu::DebugRegisterPair *Allocate();
+            void Free(size_t pos);
+            const cpu::DebugRegisterPair *Find(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl dir) const;
+
+            virtual bool FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction) const = 0;
+            virtual void Reload() const = 0;
+
+            int AddImpl(uintptr_t addr, size_t size, cpu::DebugRegisterPair preconfiguredPair);
+            int RemoveImpl(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction);
+
+        protected:
+            constexpr HwStopPointManager(size_t numStopPoints, IrqManager::ThermosphereSgi irqId) :
+                m_freeBitmap(MASK(numStopPoints)), m_irqId(irqId)
+            {
+            }
+
+        public:
+            void RemoveAll();
+            std::optional<bool> InterruptTopHalfHandler(u32 irqId, u32) final;
+
+            void ReloadOnAllCores() const
+            {
+                m_lock.lock();
+                DoReloadOnAllCores();
+                m_lock.unlock();
+            }
+
+            void Initialize()
+            {
+                IrqManager::GetInstance().Register(*this, m_irqId, false);
+            }
+    };
+}

thermosphere/src/hvisor_i_interrupt_task.hpp

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+#include <vapours/util/util_intrusive_list.hpp>
+
+namespace ams::hvisor {
+
+    class IInterruptTask : public util::IntrusiveListBaseNode<IInterruptTask> {
+        NON_COPYABLE(IInterruptTask);
+        NON_MOVEABLE(IInterruptTask);
+        protected:
+            constexpr IInterruptTask() = default;
+        public:
+            virtual std::optional<bool> InterruptTopHalfHandler(u32 irqId, u32 srcCore) = 0;
+            virtual void InterruptBottomHalfHandler(u32 irqId, u32 srcCore) {}
+    };
+
+}

thermosphere/src/hvisor_irq_manager.cpp

@@ -0,0 +1,223 @@
+/*
+ * Copyright (c) 2019-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 <mutex>
+
+#include "hvisor_irq_manager.hpp"
+#include "hvisor_virtual_gic.hpp"
+#include "hvisor_core_context.hpp"
+#include "hvisor_guest_timers.hpp"
+
+#include "cpu/hvisor_cpu_interrupt_mask_guard.hpp"
+#include "platform/interrupt_config.h"
+#include "transport_interface.h"
+#include "timer.h"
+
+//#include "debug_manager.h"
+
+namespace {
+
+    inline bool CheckGuestTimerInterrupts(ams::hvisor::ExceptionStackFrame *frame, u32 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 = THERMOSPHERE_GET_SYSREG(cntp_cval_el0);
+            return cval <= ams::hvisor::ComputeCntvct(frame);
+        } else if (irqId == TIMER_IRQID(NS_PHYS_TIMER)) {
+            return ams::hvisor::CheckRescheduleEmulatedPtimer(frame);
+        } else {
+            return true;
+        }
+    }
+
+}
+
+namespace ams::hvisor {
+
+    bool IrqManager::IsGuestInterrupt(u32 id)
+    {
+        // We don't care about the interrupts we don't use
+        // Special interrupts id (eg. spurious interrupt id 1023) are also reserved to us
+        // because the virtual interface hw itself will generate it for the guest.
+
+        bool ret = id <= GicV2Distributor::maxIrqId && id != GIC_IRQID_MAINTENANCE && id != GIC_IRQID_NS_PHYS_HYP_TIMER;
+        ret = ret && transportInterfaceFindByIrqId(id) == NULL;
+        return ret;
+    }
+
+    void IrqManager::InitializeGic()
+    {
+        // Reinits the GICD and GICC (for non-secure mode, obviously)
+        if (currentCoreCtx->IsBootCore() && currentCoreCtx->IsColdboot()) {
+            // Disable interrupt handling & global interrupt distribution
+            gicd->ctlr = 0;
+
+            // Get some info
+            m_numSharedInterrupts = 32 * (gicd->typer & 0x1F); // number of interrupt lines / 32
+
+            // unimplemented priority bits (lowest significant) are RAZ/WI
+            gicd->ipriorityr[0] = 0xFF;
+            m_priorityShift = 8 - __builtin_popcount(gicd->ipriorityr[0]);
+            m_numPriorityLevels = static_cast<u8>(BIT(__builtin_popcount(gicd->ipriorityr[0])));
+
+            m_numCpuInterfaces = static_cast<u8>(1 + ((gicd->typer >> 5) & 7));
+        }
+
+        // Only one core will reset the GIC state for the shared peripheral interrupts
+
+        u32 numInterrupts = 32;
+        if (currentCoreCtx->IsBootCore()) {
+            numInterrupts += m_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;
+    }
+
+    void IrqManager::DoConfigureInterrupt(u32 id, u8 prio, bool isLevelSensitive)
+    {
+        ClearInterruptEnabled(id);
+        ClearInterruptPending(id);
+        if (id >= 32) {
+            SetInterruptMode(id, !isLevelSensitive);
+            SetInterruptTargets(id, 0xFF); // all possible processors
+        }
+        SetInterruptPriority(id, prio);
+        SetInterruptEnabled(id);
+    }
+
+    void IrqManager::Initialize()
+    {
+        cpu::InterruptMaskGuard mg{};
+        std::scoped_lock lk{m_lock};
+
+        InitializeGic();
+        DoConfigureInterrupt(GIC_IRQID_MAINTENANCE, hostPriority, true);
+
+        VirtualGic::GetInstance().Initialize();
+    }
+
+    void IrqManager::Register(IInterruptTask &task, u32 id, bool isLevelSensitive, u8 prio)
+    {
+        cpu::InterruptMaskGuard mg{};
+        std::scoped_lock lk{m_lock};
+
+        DoConfigureInterrupt(id, prio, isLevelSensitive);
+        if (!task.IsLinked()) {
+            m_interruptTaskList.push_back(task);
+        }
+    }
+
+    void IrqManager::SetInterruptAffinity(u32 id, u8 affinity)
+    {
+        cpu::InterruptMaskGuard mg{};
+        std::scoped_lock lk{m_lock};
+
+        SetInterruptTargets(id, affinity);
+    }
+
+    void IrqManager::HandleInterrupt(ExceptionStackFrame *frame)
+    {
+        // Acknowledge the interrupt. Interrupt goes from pending to active.
+        u32 iar = AcknowledgeIrq();
+        u32 irqId = iar & 0x3FF;
+        u32 srcCore = (iar >> 10) & 7;
+        IInterruptTask *taskForBottomHalf;
+
+        //DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId);
+        if (irqId == GicV2Distributor::spuriousIrqId) {
+            // Spurious interrupt received
+            return;
+        } else if (!CheckGuestTimerInterrupts(frame, irqId)) {
+            // Deactivate the interrupt, return ASAP
+            DropCurrentInterruptPriority(iar);
+            DeactivateCurrentInterrupt(iar);
+            return;
+        } else {
+            // Everything else
+            std::scoped_lock lk{instance.m_lock};
+            VirtualGic &vgic = VirtualGic::GetInstance();
+
+            if (irqId >= 16 && IsGuestInterrupt(irqId)) {
+                // Guest interrupts
+                taskForBottomHalf = nullptr;
+                DropCurrentInterruptPriority(iar);
+                vgic.EnqueuePhysicalIrq(irqId);
+            } else {
+                // Host interrupts
+                // Try all handlers and see which one fits
+                for (IInterruptTask &task: instance.m_interruptTaskList) {
+                    auto b = task.InterruptTopHalfHandler(irqId, srcCore);
+                    if (b) {
+                        taskForBottomHalf = *b ? &task : nullptr;
+                        break;
+                    }
+                }
+                DropCurrentInterruptPriority(iar);
+                DeactivateCurrentInterrupt(iar);
+            }
+
+            vgic.UpdateState();
+        }
+
+
+        if (taskForBottomHalf != nullptr) {
+            // Unmasking the irq signal is left at the discretion of the bottom half handler
+            EnterInterruptibleHypervisorCode();
+            taskForBottomHalf->InterruptBottomHalfHandler(irqId, srcCore);
+        }
+    }
+}

thermosphere/src/hvisor_irq_manager.hpp

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_gicv2.hpp"
+#include "hvisor_synchronization.hpp"
+#include "hvisor_i_interrupt_task.hpp"
+#include "hvisor_exception_stack_frame.hpp"
+#include "hvisor_memory_map.hpp"
+#include "cpu/hvisor_cpu_sysreg_general.hpp"
+
+namespace ams::hvisor {
+
+    class IrqManager final {
+        SINGLETON(IrqManager);
+        friend class VirtualGic;
+        private:
+            static constexpr u8 hostPriority = 0;
+            static constexpr u8 guestPriority = 1;
+
+            static inline volatile auto *const gicd = reinterpret_cast<volatile GicV2Distributor *>(MemoryMap::gicdVa); 
+            static inline volatile auto *const gicc = reinterpret_cast<volatile GicV2Controller *>(MemoryMap::giccVa); 
+            static inline volatile auto *const gich = reinterpret_cast<volatile GicV2VirtualInterfaceController *>(MemoryMap::gichVa);
+
+            static bool IsGuestInterrupt(u32 id);
+
+            static u32 GetTypeRegister()                                { return gicd->typer; }
+            static void SetInterruptEnabled(u32 id)                     { gicd->isenabler[id / 32] = BIT(id % 32); }
+            static void ClearInterruptEnabled(u32 id)                   { gicd->icenabler[id / 32] = BIT(id % 32); }
+            static bool IsInterruptPending(u32 id)                      { return (gicd->ispendr[id / 32] & BIT(id % 32)) != 0;}
+            static void ClearInterruptPending(u32 id)                   { gicd->icpendr[id / 32] = BIT(id % 32); }
+            static void ClearInterruptActive(u32 id)                    { gicd->icactiver[id / 32] = BIT(id % 32); }
+            static void SetInterruptShiftedPriority(u32 id, u8 prio)    { gicd->ipriorityr[id] = prio; }
+            static void SetInterruptTargets(u32 id, u8 targetList)      { gicd->itargetsr[id] = targetList; }
+            static bool IsInterruptEdgeTriggered(u32 id)
+            {
+                return ((gicd->icfgr[id / 16] >> GicV2Distributor::GetCfgrShift(id)) & 2) != 0;
+            }
+            static void SetInterruptMode(u32 id, bool isEdgeTriggered)
+            {
+                u32 cfgw = gicd->icfgr[id / 16];
+                cfgw &= ~(2 << GicV2Distributor::GetCfgrShift(id));
+                cfgw |= (isEdgeTriggered ? 2 : 0) << GicV2Distributor::GetCfgrShift(id);
+                gicd->icfgr[id / 16] = cfgw;
+            }
+
+            static u32 AcknowledgeIrq()                         { return gicc->iar; }
+            static void DropCurrentInterruptPriority(u32 iar)   { gicc->eoir = iar; }
+            static void DeactivateCurrentInterrupt(u32 iar)     { gicc->dir = iar; }
+
+        private:
+            using InterruptTaskList = util::IntrusiveListBaseTraits<IInterruptTask>::ListType;
+
+            mutable RecursiveSpinlock m_lock{};
+            InterruptTaskList m_interruptTaskList{};
+            u32 m_numSharedInterrupts = 0;
+            u8 m_priorityShift = 0;
+            u8 m_numPriorityLevels = 0;
+            u8 m_numCpuInterfaces = 0;
+
+        private:
+            void SetInterruptPriority(u32 id, u8 prio) { SetInterruptShiftedPriority(id, prio << m_priorityShift); }
+
+            void InitializeGic();
+            void DoConfigureInterrupt(u32 id, u8 prio, bool isLevelSensitive);
+
+        private:
+            constexpr IrqManager() = default;
+
+        public:
+            enum ThermosphereSgi : u32 {
+                VgicUpdateSgi = 0,
+
+                ReloadHwBreakpointsSgi,
+                ReloadWatchpointsSgi,
+                ApplyRevertSwBreakpointSgi,
+
+                DebugPauseSgi,
+                ReportDebuggerBreakSgi,
+                DebuggerContinueSgi,
+
+                MaxSgi,
+            };
+
+            static void GenerateSgiForList(ThermosphereSgi id, u32 coreList)
+            {
+                gicd->sgir = GicV2Distributor::ForwardToTargetList << 24 | coreList << 16 | id;
+            }
+            static void GenerateSgiForAllOthers(ThermosphereSgi id)
+            {
+                gicd->sgir = GicV2Distributor::ForwardToAllOthers << 24 | id;
+            }
+
+            static void EnterInterruptibleHypervisorCode()
+            {
+                // We don't want the guest to spam us with its timer interrupts. Disable the timers.
+                THERMOSPHERE_SET_SYSREG(cntp_ctl_el0, 0);
+                THERMOSPHERE_SET_SYSREG(cntv_ctl_el0, 0);
+            }
+
+            static void HandleInterrupt(ExceptionStackFrame *frame);
+
+        public:
+            void Initialize();
+            void Register(IInterruptTask &task, u32 id, bool isLevelSensitive, u8 prio = IrqManager::hostPriority);
+            void SetInterruptAffinity(u32 id, u8 affinityMask);
+    };
+
+}

thermosphere/src/hvisor_memory_map.cpp

@@ -0,0 +1,197 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_memory_map.hpp"
+#include "hvisor_core_context.hpp"
+
+#include "cpu/hvisor_cpu_mmu.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+
+namespace ams::hvisor {
+
+    uintptr_t MemoryMap::currentPlatformMmioPage = MemoryMap::mmioPlatBaseVa;
+
+    void MemoryMap::SetupMmu(const MemoryMap::LoadImageLayout *layout)
+    {
+        using namespace cpu;
+
+        constexpr u64 normalAttribs =          MMU_INNER_SHAREABLE | MMU_ATTRINDX(Memtype_Normal);
+        constexpr u64 deviceAttribs = MMU_XN | MMU_INNER_SHAREABLE | MMU_ATTRINDX(Memtype_Device_nGnRE);
+
+        /*
+            Layout in physmem:
+            Location1
+                Image (code and data incl. BSS), which size is page-aligned
+            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
+
+            We 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
+         */
+
+        using Builder = MmuTableBuilder<3, addressSpaceSize>;
+        uintptr_t mmuTablePa = layout->tempPa + layout->maxTempSize;
+
+        uintptr_t tempVa = imageVa + layout->imageSize;
+        uintptr_t crashStacksPa = layout->tempPa + layout->tempSize;
+        uintptr_t stacksPa = crashStacksPa + crashStacksSize;
+
+        Builder{reinterpret_cast<u64 *>(mmuTablePa)}
+            .InitializeTable()
+            // Image & tempbss & crash stacks
+            .MapBlockRange(imageVa, layout->startPa, layout->imageSize, normalAttribs)
+            .MapBlockRange(tempVa, layout->tempPa, layout->tempSize, normalAttribs)
+            .MapBlockRange(crashStacksBottomVa, crashStacksPa, crashStacksSize, normalAttribs)
+            // Stacks, each with a guard page
+            .MapBlockRange(stacksBottomVa, stacksPa, 0x1000ul * MAX_CORE, normalAttribs, 0x1000)
+            // GICD, GICC, GICH
+            .MapBlock(gicdVa, MEMORY_MAP_PA_GICD, deviceAttribs)
+            .MapBlockRange(giccVa, MEMORY_MAP_PA_GICC, 0x2000, deviceAttribs)
+            .MapBlock(gichVa, MEMORY_MAP_PA_GICH, deviceAttribs)
+            // Recursive page mapping
+            .MapBlock(ttblVa, mmuTablePa, normalAttribs)
+        ;
+    }
+
+    std::array<uintptr_t, 2> MemoryMap::EnableMmuGetStacks(const MemoryMap::LoadImageLayout *layout, u32 coreId)
+    {
+        using namespace cpu;
+        uintptr_t mmuTablePa = layout->tempPa + layout->maxTempSize;
+
+        u32 ps = THERMOSPHERE_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 = 39
+        */
+        u64 tcr = TCR_EL2_RSVD | TCR_PS(ps) | TCR_TG0(TranslationGranule_4K) | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA | TCR_T0SZ(addressSpaceSize);
+
+
+        /*
+            - 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
+        */
+        constexpr u64 mair = 0x44FF0400;
+
+        // Set VBAR because we *will* crash (instruction abort because of the value of pc) when enabling the MMU
+        THERMOSPHERE_SET_SYSREG(vbar_el2, layout->vbar);
+
+        // MMU regs config
+        THERMOSPHERE_SET_SYSREG(ttbr0_el2, mmuTablePa);
+        THERMOSPHERE_SET_SYSREG(tcr_el2, tcr);
+        THERMOSPHERE_SET_SYSREG(mair_el2, mair);
+        dsb();
+        isb();
+
+        // TLB invalidation
+        // Whether this does anything before MMU is enabled is impldef, apparently
+        TlbInvalidateEl2Local();
+        dsb();
+        isb();
+
+        // Enable MMU & enable caching. We will crash.
+        u64 sctlr = THERMOSPHERE_GET_SYSREG(sctlr_el2);
+        sctlr |= SCTLR_ELx_I | SCTLR_ELx_C | SCTLR_ELx_M;
+        THERMOSPHERE_SET_SYSREG(sctlr_el2, sctlr);
+        dsb();
+        isb();
+
+        // crashStackTop is fragile, check if crashStacksSize is suitable for MAX_CORE
+        uintptr_t stackTop = stacksBottomVa + 0x2000 * coreId + 0x1000;
+        uintptr_t crashStackTop = crashStacksBottomVa + (crashStacksSize / MAX_CORE) * (1 + coreId);
+        return std::array{stackTop, crashStackTop};
+    }
+
+    uintptr_t MemoryMap::MapPlatformMmio(uintptr_t pa, size_t size)
+    {
+        using namespace cpu;
+        using Builder = MmuTableBuilder<3, addressSpaceSize, true>;
+        constexpr u64 deviceAttribs = MMU_XN | MMU_INNER_SHAREABLE | MMU_ATTRINDX(Memtype_Device_nGnRE);
+
+        uintptr_t va = currentPlatformMmioPage;
+        size = (size + 0xFFF) & ~0xFFFul;
+        Builder{reinterpret_cast<u64 *>(ttblVa)}.MapBlockRange(currentPlatformMmioPage, va, size, deviceAttribs);
+
+        currentPlatformMmioPage += size;
+        return va;
+    }
+
+    uintptr_t MemoryMap::MapGuestPage(uintptr_t pa, u64 memAttribs, u64 shareability)
+    {
+        using namespace cpu;
+        using Builder = MmuTableBuilder<3, addressSpaceSize, true>;
+
+        u64 attribs = MMU_XN | MMU_SH(shareability) | MMU_ATTRINDX(Memtype_Guest_Slot);
+        uintptr_t va = guestMemVa + 0x2000 * currentCoreCtx->GetCoreId(); // one guard page
+
+        // Update mair_el2
+        u64 mair = THERMOSPHERE_GET_SYSREG(mair_el2);
+        mair |= memAttribs << (8 * Memtype_Guest_Slot);
+        THERMOSPHERE_SET_SYSREG(mair_el2, mair);
+        isb();
+
+        Builder{reinterpret_cast<u64 *>(ttblVa)}.MapBlock(va, pa, attribs);
+        TlbInvalidateEl2Page(va);
+        dsb();
+        isb();
+    }
+
+    void MemoryMap::UnmapGuestPage()
+    {
+        using namespace cpu;
+        using Builder = MmuTableBuilder<3, addressSpaceSize, true>;
+        uintptr_t va = guestMemVa + 0x2000 * currentCoreCtx->GetCoreId();
+
+        dsb();
+        isb();
+
+        Builder{reinterpret_cast<u64 *>(ttblVa)}.Unmap(va);
+        TlbInvalidateEl2Page(va);
+        dsb();
+        isb();
+
+        // Update mair_el2
+        u64 mair = THERMOSPHERE_GET_SYSREG(mair_el2);
+        mair &= ~(0xFF << (8 * Memtype_Guest_Slot));
+        THERMOSPHERE_SET_SYSREG(mair_el2, mair);
+        isb();
+    }
+}

thermosphere/src/hvisor_memory_map.hpp

@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+
+namespace ams::hvisor {
+
+    class MemoryMap final {
+        NON_COPYABLE(MemoryMap);
+        NON_MOVEABLE(MemoryMap);
+        private:
+            // Maps to AttrIndx[2:0]
+            enum MemType {
+                Memtype_Device_nGnRnE           = 0,
+                Memtype_Normal                  = 1,
+                Memtype_Device_nGnRE            = 2,
+                Memtype_Normal_Uncacheable      = 3,
+                Memtype_Guest_Slot              = 4,
+            };
+
+            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;
+            };
+            static_assert(std::is_standard_layout_v<LoadImageLayout>);
+            static_assert(std::is_trivial_v<LoadImageLayout>);
+        private:
+            static LoadImageLayout imageLayout;
+            static uintptr_t currentPlatformMmioPage;
+
+        public:
+            static constexpr u32 addressSpaceSize = 39;
+
+            // The following come from the fact we're using a recursive page table:
+            static constexpr uintptr_t selfL2VaRange        = 0x7FC0000000ul;   // = 511 << 31
+            static constexpr uintptr_t selfL3VaRange        = 0x7FFFE00000ul;   // = 511 << 31 | 511 << 21
+            static constexpr uintptr_t ttblVa               = 0x7FFFFFF000ul;   // = 511 << 31 | 511 << 21 | 511 << 12
+            static constexpr uintptr_t maxVa                = 0x7FFFFFFFFFul;   // = all 39 bits set
+
+            static constexpr size_t crashStacksSize         = 0x1000ul;
+
+            // Do not use the first 0x10000 to allow for L1/L2 mappings...
+            static constexpr uintptr_t imageVa              = selfL3VaRange + 0x10000;
+            static constexpr uintptr_t crashStacksBottomVa  = selfL3VaRange + 0x40000;
+            static constexpr uintptr_t crashStacksTopVa     = crashStacksBottomVa + crashStacksSize;
+            static constexpr uintptr_t guestMemVa           = selfL3VaRange + 0x50000;
+            static constexpr uintptr_t stacksBottomVa       = selfL3VaRange + 0x60000;
+
+            static constexpr uintptr_t mmioBaseVa           = selfL3VaRange + 0x80000;
+            static constexpr uintptr_t gicdVa               = mmioBaseVa + 0x0000;
+            static constexpr uintptr_t giccVa               = mmioBaseVa + 0x1000;
+            static constexpr uintptr_t gichVa               = mmioBaseVa + 0x3000;
+
+            static constexpr uintptr_t mmioPlatBaseVa       = selfL3VaRange + 0x90000;
+
+            static uintptr_t GetStartPa() { return imageLayout.startPa; }
+
+            // Called before MMU is enabled. EnableMmu must not use a stack frame
+            static void SetupMmu(const LoadImageLayout *layout);
+            static std::array<uintptr_t, 2> EnableMmuGetStacks(const LoadImageLayout *layout, u32 coreId);
+
+            static constexpr uintptr_t GetStackTopVa(u32 coreId)
+            {
+                return stacksBottomVa + 0x2000 * coreId + 0x1000;
+            }
+
+            // Caller is expected to invalidate TLB + barrier at some point
+            static uintptr_t MapPlatformMmio(uintptr_t pa, size_t size);
+
+            // Caller is expected to disable interrupts, etc, etc.
+            static uintptr_t MapGuestPage(uintptr_t pa, u64 memAttribs, u64 shareability);
+            static void UnmapGuestPage();
+
+        public:
+            constexpr MemoryMap() = delete;
+    };
+
+}

thermosphere/src/hvisor_safe_io_copy.hpp

@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+
+namespace ams::hvisor {
+
+    // Caller needs to disable interrupts
+    size_t SafeIoCopy(void *dst, const void *src, size_t size);
+
+}

thermosphere/src/hvisor_safe_io_copy.s

@@ -0,0 +1,84 @@
+/*
+ * 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"
+
+// ams::hvisor::SafeIoCopy(void*, void const*, unsigned long)
+FUNCTION _ZN3ams6hvisor10SafeIoCopyEPvPKvm
+    // Caller needs to mask interrupts
+    // See _synchSp0 exception handler
+    msr     spsel, #0
+    mov     x9, x18
+    mov     x18, #0
+    mov     x16, #0
+
+    // x0-x2 parameters
+    // x3: remainder
+    // x4: offset
+
+    cbz     x2, 2f
+    mov     x3, x2
+    mov     x4, #0
+
+    mov     x5, #0
+    // while (remainder > 0) { offset += increment; test alignment etc. } return offset;
+    1:
+        // Dispatcher
+        add     x5, x1, x4
+        add     x6, x0, x4
+        orr     x7, x5, x6
+        tst     x7, #3
+        // ((src + off)|(dst + off)) & 3 == 0 ? remainder > 3 : eq
+        ccmp    x3, #3, #0, eq
+        bhi     3f
+        // same thing but for 2-byte alignment
+        cmp     x3, #1
+        cset    w8, hi
+        bics    wzr, w8, w5
+        bne     4f
+
+        // 8-bit load, if the load and/or store crashes, x16 = 1 (same thing for the other load/stores)
+        ldrb    w5, [x5]
+        strb    w5, [x6]
+        cbnz    x16, 2f
+        add     x4, x4, #1
+        subs    x3, x3, #1
+        bne     1b
+    2:
+        // Return
+        msr     spsel, #1
+        mov     x18, x9
+        mov     x0, x4
+        ret
+    3:
+        // 32-bit load
+        ldr     w5, [x5]
+        str     w5, [x6]
+        cbnz    x16, 2b
+        add     x4, x4, #4
+        subs    x3, x3, #4
+        bne     1b
+        b       2b
+    4:
+        // 16-bit load
+        ldrh    w5, [x5]
+        strh    w5, [x6]
+        cbnz    x16, 2b
+        add     x4, x4, #2
+        subs    x3, x3, #2
+        bne     1b
+        b       2b
+END_FUNCTION

thermosphere/src/hvisor_sw_breakpoint_manager.cpp

@@ -0,0 +1,182 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_sw_breakpoint_manager.hpp"
+#include "hvisor_core_context.hpp"
+#include "hvisor_guest_memory.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+#include "cpu/hvisor_cpu_interrupt_mask_guard.hpp"
+
+#include <mutex>
+
+#define _REENT_ONLY
+#include <cerrno>
+
+/*
+    Consider the following:
+        - Breakpoints are based on VA
+        - Translation tables may change
+        - Translation tables may differ from core to core
+
+    We also define sw breakpoints on invalid addresses (for one or more cores) UNPREDICTABLE.
+*/
+
+namespace ams::hvisor {
+
+    SwBreakpointManager SwBreakpointManager::instance{};
+
+    size_t SwBreakpointManager::FindClosest(uintptr_t addr) const
+    {
+        auto endit = m_breakpoints.cbegin() + m_numBreakpoints;
+        auto it = std::lower_bound(
+            m_breakpoints.cbegin(),
+            endit,
+            addr,
+            [] (const Breakpoint &a, const Breakpoint &b) {
+                return a.address < b.address;
+            }
+        );
+
+        return it == endit ? m_numBreakpoints : static_cast<size_t>(it - m_breakpoints.cbegin());
+    }
+
+    bool SwBreakpointManager::DoApply(size_t id)
+    {
+        Breakpoint &bp = m_breakpoints[id];
+        u32 brkInst = 0xD4200000 | (bp.uid << 5);
+
+        size_t sz = GuestReadWriteMemory(bp.address, 4, &bp.savedInstruction, &brkInst);
+        bp.applied = sz == 4;
+        return sz == 4;
+    }
+
+    bool SwBreakpointManager::DoRevert(size_t id)
+    {
+        Breakpoint &bp = m_breakpoints[id];
+        size_t sz = GuestWriteMemory(bp.address, 4, &bp.savedInstruction);
+        bp.applied = sz != 4;
+        return sz == 4;
+    }
+
+    std::optional<bool> SwBreakpointManager::InterruptTopHalfHandler(u32 irqId, u32)
+    {
+        if (irqId != IrqManager::ApplyRevertSwBreakpointSgi) {
+            return {};
+        }
+
+        m_applyBarrier.Join();
+        return false;
+    }
+
+    bool SwBreakpointManager::ApplyOrRevert(size_t id, bool apply)
+    {
+        cpu::InterruptMaskGuard mg{};
+        m_applyBarrier.Reset(CoreContext::GetActiveCoreMask());
+        IrqManager::GenerateSgiForAllOthers(IrqManager::ApplyRevertSwBreakpointSgi);
+        if (apply) {
+            DoApply(id);
+        } else {
+            DoRevert(id);
+        }
+
+        m_applyBarrier.Join();
+    }
+
+    // TODO apply revert handlers
+
+    int SwBreakpointManager::Add(uintptr_t addr, bool persistent)
+    {
+        if ((addr & 3) != 0) {
+            return -EINVAL;
+        }
+
+        std::scoped_lock lk{m_lock};
+
+        if (m_numBreakpoints == MAX_SW_BREAKPOINTS) {
+            return -EBUSY;
+        }
+
+        size_t id = FindClosest(addr);
+        if (id != m_numBreakpoints && m_breakpoints[id].uid != 0) {
+            return -EEXIST;
+        }
+
+        // Insert
+        for(size_t i = m_numBreakpoints; i > id && i != 0; i--) {
+            m_breakpoints[i] = m_breakpoints[i - 1];
+        }
+        ++m_numBreakpoints;
+
+        Breakpoint &bp = m_breakpoints[id];
+        bp.address = addr;
+        bp.persistent = persistent;
+        bp.applied = false;
+        bp.uid = static_cast<u16>(0x2000 + m_bpUniqueCounter++);
+
+        return ApplyOrRevert(id, true) ? 0 : -EFAULT;
+    }
+
+    int SwBreakpointManager::Remove(uintptr_t addr, bool keepPersistent)
+    {
+        if ((addr & 3) != 0) {
+            return -EINVAL;
+        }
+
+        std::scoped_lock lk{m_lock};
+
+        if (m_numBreakpoints == MAX_SW_BREAKPOINTS) {
+            return -EBUSY;
+        }
+
+        size_t id = FindClosest(addr);
+        if (id == m_numBreakpoints || m_breakpoints[id].uid == 0) {
+            return -ENOENT;
+        }
+
+        Breakpoint &bp = m_breakpoints[id];
+        bool ok = true;
+        if (!keepPersistent || !bp.persistent) {
+            ok = ApplyOrRevert(id, false);
+        }
+
+        for(size_t i = id; i < m_numBreakpoints - 1; i++) {
+            m_breakpoints[i] = m_breakpoints[i + 1];
+        }
+
+        m_breakpoints[--m_numBreakpoints] = {};
+
+        return ok ? 0 : -EFAULT;
+    }
+
+    int SwBreakpointManager::RemoveAll(bool keepPersistent)
+    {
+        std::scoped_lock lk{m_lock};
+
+        bool ok = true;
+        for (size_t id = 0; id < m_numBreakpoints; id++) {
+            Breakpoint &bp = m_breakpoints[id];
+            if (!keepPersistent || !bp.persistent) {
+                ok = ok && ApplyOrRevert(id, false);
+            }
+        }
+
+        std::fill_n(m_breakpoints.begin(), m_breakpoints.end(), Breakpoint{});
+        m_numBreakpoints = 0;
+        m_bpUniqueCounter = 0;
+
+        return ok ? 0 : -EFAULT;
+    }
+}

thermosphere/src/hvisor_sw_breakpoint_manager.hpp

@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+#include "hvisor_irq_manager.hpp"
+
+#define MAX_SW_BREAKPOINTS  16
+
+namespace ams::hvisor {
+
+    class SwBreakpointManager : public IInterruptTask {
+        SINGLETON(SwBreakpointManager);
+        private:
+            struct Breakpoint {
+                uintptr_t address;
+                u32 savedInstruction;
+                u16 uid;
+                bool persistent;
+                bool applied;
+            };
+
+        private:
+            mutable RecursiveSpinlock m_lock{};
+            mutable Barrier m_applyBarrier{};
+
+            u32 m_bpUniqueCounter = 0;
+            size_t m_numBreakpoints = 0;
+            std::array<Breakpoint, MAX_SW_BREAKPOINTS> m_breakpoints{};
+
+        private:
+            size_t FindClosest(uintptr_t addr) const;
+
+            bool DoApply(size_t id);
+            bool DoRevert(size_t id);
+
+            bool ApplyOrRevert(size_t id, bool apply);
+
+        private:
+            constexpr SwBreakpointManager() = default;
+
+        public:
+            int Add(uintptr_t addr, bool persistent);
+            int Remove(uintptr_t addr, bool keepPersistent);
+            int RemoveAll(bool keepPersistent);
+
+            std::optional<bool> InterruptTopHalfHandler(u32 irqId, u32) final;
+            void Initialize()
+            {
+                IrqManager::GetInstance().Register(*this, IrqManager::ApplyRevertSwBreakpointSgi, false);
+            }
+    };
+}

thermosphere/src/hvisor_synchronization.cpp

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_synchronization.hpp"
+#include "hvisor_core_context.hpp"
+
+namespace ams::hvisor {
+
+    void Spinlock::lock()
+    {
+        u32 tmp1;
+        const u32 tmp2 = 1;
+        __asm__ __volatile__(
+            "prfm   pstl1keep, %[val]                   \n"
+            "sevl                                       \n"
+            "1:                                         \n"
+            "   wfe                                     \n"
+            "   2:                                      \n"
+            "       ldaxr   %[tmp1], %[val]             \n"
+            "       cbnz    %[tmp1], 1b                 \n"
+            "       stxr    %[tmp1], %[tmp2], %[val]    \n"
+            "       cbnz    %[tmp1], 2b                 \n"
+            : [tmp1] "=&r"(tmp1), [val] "+Q" (m_val)
+            : [tmp2] "r"(tmp2)
+            : "cc", "memory"
+        );
+    }
+
+    void Spinlock::unlock() noexcept
+    {
+        __asm__ __volatile__("stlr wzr, %[val]" : [val] "=Q" (m_val) :: "memory");
+    }
+
+    void Barrier::Join()
+    {
+        const u32 mask = BIT(currentCoreCtx->GetCoreId());
+        u32 newval, tmp;
+        __asm__ __volatile__(
+            "prfm   pstl1keep, %[val]                   \n"
+            
+            /* Fetch-and */
+            "1:                                         \n"
+            "   ldaxr   %[newval], %[val]               \n"
+            "   bic     %[newval], %[newval], %[mask]   \n"
+            "   stlxr   %[tmp], %[newval], %[val]       \n"
+            "   cbnz    %[tmp], 1b                      \n"
+
+            /* Check if now/already 0, wait if not */
+            "cbz    %[newval], 3f                       \n"
+            
+            /* Event will be signaled if the stlxr succeeds for another core... */
+            "2:                                         \n"
+            "   wfe                                     \n"
+            "   ldaxr   %[newval], %[val]               \n"
+            "   cbnz    %[newval], 2b                   \n"
+            "3:                                         \n"
+            : [newval] "=&r"(newval), [tmp] "=&r" (tmp), [val] "+Q" (m_val)
+            : [mask] "r"(mask)
+            : "cc", "memory"
+        );
+    }
+
+    void RecursiveSpinlock::lock()
+    {
+        u32 tag = currentCoreCtx->GetCoreId() + 1;
+        if (AMS_LIKELY(tag != m_tag)) {
+            m_spinlock.lock();
+            m_tag = tag;
+            m_count = 1;
+        } else {
+            ++m_count;
+        }
+    }
+
+    void RecursiveSpinlock::unlock() noexcept
+    {
+        if (AMS_LIKELY(--m_count == 0)) {
+            m_tag = 0;
+            m_spinlock.unlock();
+        }
+    }
+}

thermosphere/src/hvisor_synchronization.hpp

@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+
+namespace ams::hvisor {
+
+    class Spinlock final {
+        NON_COPYABLE(Spinlock);
+        NON_MOVEABLE(Spinlock);
+        private:
+            u32 m_val = 0;
+        public:
+            constexpr Spinlock() = default;
+            void lock();
+            void unlock() noexcept;
+    };
+
+    class Barrier final {
+        NON_COPYABLE(Barrier);
+        NON_MOVEABLE(Barrier);
+        private:
+            u32 m_val = 0;
+        public:
+            constexpr Barrier() = default;
+            void Join();
+
+            constexpr void Reset(u32 val)
+            {
+                m_val = val;
+            }
+    };
+
+    class RecursiveSpinlock final {
+        NON_COPYABLE(RecursiveSpinlock);
+        NON_MOVEABLE(RecursiveSpinlock);
+        private:
+            Spinlock m_spinlock{};
+            u32 m_tag = 0;
+            u32 m_count = 0;
+        public:
+            constexpr RecursiveSpinlock() = default;
+            void lock();
+            void unlock() noexcept;
+    };
+
+}

thermosphere/src/hvisor_virtual_gic.cpp

@@ -0,0 +1,747 @@
+/*
+ * Copyright (c) 2019-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 <mutex>
+#include "hvisor_virtual_gic.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+
+#include "platform/interrupt_config.h" // TODO remove
+
+#define GICDOFF(field)          (offsetof(GicV2Distributor, field))
+
+namespace ams::hvisor {
+
+
+    VirtualGic::VirqQueue::iterator VirtualGic::VirqQueue::insert(VirtualGic::VirqQueue::iterator pos, VirtualGic::VirqState &elem)
+    {
+        // Insert before
+        ENSURE(!elem.IsQueued());
+
+        // Empty list
+        if (begin() == end()) {
+            m_first = m_last = &elem;
+            elem.listPrev = elem.listNext = virqListEndIndex;
+            return begin();
+        }
+
+        if (pos == end()) {
+            // Insert after last
+            VirqState &prev = back();
+            elem.listPrev = GetStateIndex(prev);
+            elem.listNext = prev.listNext;
+            prev.listNext = GetStateIndex(elem);
+            m_last = &elem;
+        } else {
+            u32 idx = GetStateIndex(elem);
+            u32 posidx = GetStateIndex(*pos);
+            u32 previdx = elem.listPrev;
+
+            elem.listNext = posidx;
+            elem.listPrev = previdx;
+
+            pos->listPrev = idx;
+
+            if (pos == begin()) {
+                m_first = &elem;
+            } else {
+                --pos;
+                pos->listNext = idx;
+            }
+        }
+
+        return iterator{&elem, m_storage};
+    }
+
+    VirtualGic::VirqQueue::iterator VirtualGic::VirqQueue::insert(VirtualGic::VirqState &elem)
+    {
+        // Insert in a stable sorted way
+        // Lower priority number is higher; we sort by descending priority, ie. ascending priority number
+        // Put the interrupts that were previously in the LR before the one which don't
+        return insert(
+            std::find_if(begin(), end(), [&a = elem](const VirqState &b) {
+                return a.priority == b.priority ? a.handled && !b.handled : a.priority < b.priority;
+            }),
+            elem
+        );
+    }
+
+    void VirtualGic::SetInterruptEnabledState(u32 id)
+    {
+        VirqState &state = GetVirqState(id);
+
+        if (id < 16 || !IrqManager::IsGuestInterrupt(id) || state.enabled) {
+            // Nothing to do...
+            // Also, ignore for SGIs
+            return;
+        }
+
+        // Similar effects to setting the target list to non-0 when it was 0...
+        if (state.IsPending()) {
+            NotifyOtherCoreList(state.targetList);
+        }
+
+        state.enabled = true;
+        IrqManager::SetInterruptEnabled(id);
+    }
+
+    void VirtualGic::ClearInterruptEnabledState(u32 id)
+    {
+        VirqState &state = GetVirqState(id);
+
+        if (id < 16 || !IrqManager::IsGuestInterrupt(id) || !state.enabled) {
+            // Nothing to do...
+            // Also, ignore for SGIs
+            return;
+        }
+
+        // Similar effects to setting the target list to 0, we may need to notify the core
+        // handling the interrupt if it's pending
+        if (state.handled) {
+            NotifyOtherCoreList(BIT(state.coreId));
+        }
+
+        state.enabled = false;
+        IrqManager::ClearInterruptEnabled(id);
+    }
+
+    void VirtualGic::SetInterruptPriorityByte(u32 id, u8 priority)
+    {
+        if (!IrqManager::IsGuestInterrupt(id)) {
+            return;
+        }
+
+        // 32 priority levels max, bits [7:3]
+        priority >>= priorityShift;
+
+        if (id >= 16) {
+            // Ensure we have the correct priority on the physical distributor...
+            IrqManager::GetInstance().SetInterruptPriority(id, IrqManager::guestPriority);
+        }
+
+        VirqState &state = GetVirqState(id);
+        if (priority == state.priority) {
+            // Nothing to do...
+            return;
+        }
+
+        state.priority = priority;
+        u32 targets = state.targetList;
+        if (targets != 0 && state.IsPending()) {
+            NotifyOtherCoreList(targets);
+        }
+    }
+
+    void VirtualGic::SetInterruptTargets(u32 id, u8 coreList)
+    {
+        // Ignored for SGIs and PPIs, and non-guest interrupts
+        if (id < 32 || !IrqManager::IsGuestInterrupt(id)) {
+            return;
+        }
+
+        // Interrupt not pending (inactive or active-only): nothing much to do (see reference manual)
+        // Otherwise, we may need to migrate the interrupt.
+        // In our model, while a physical interrupt can be pending on multiple cores, we decide that a pending SPI
+        // can only be handled on a single core (either it's in a LR, or in the global list), therefore we need to
+        // send a signal to (oldList XOR newList) to either put the interrupt back in the global list or potentially handle it
+
+        // Note that we take into account that the interrupt may be disabled.
+        VirqState &state = GetVirqState(id);
+        if (state.IsPending()) {
+            u8 oldList = state.targetList;
+            u8 diffList = (oldList ^ coreList) & CoreContext::GetActiveCoreMask();
+            if (diffList != 0) {
+                NotifyOtherCoreList(diffList);
+            }
+        }
+
+        state.targetList = coreList;
+        IrqManager::SetInterruptTargets(id, state.targetList);
+    }
+
+    void VirtualGic::SetInterruptConfigBits(u32 id, u32 config)
+    {
+        // Ignored for SGIs, implementation defined for PPIs
+        if (id < 32 || !IrqManager::IsGuestInterrupt(id)) {
+            return;
+        }
+
+        VirqState &state = GetVirqState(id);
+
+        // Expose bit(2n) as nonprogrammable to the guest no matter what the physical distributor actually behaves
+        bool newEdgeTriggered = ((config & 2) << GicV2Distributor::GetCfgrShift(id)) != 0;
+
+        if (state.edgeTriggered != newEdgeTriggered) {
+            state.edgeTriggered = newEdgeTriggered;
+            IrqManager::SetInterruptMode(id, newEdgeTriggered);
+        }
+    }
+
+    void VirtualGic::SetSgiPendingState(u32 id, u32 coreId, u32 srcCoreId)
+    {
+        VirqState &state = GetVirqState(coreId, id);
+        m_incomingSgiPendingSources[coreId][id] |= BIT(srcCoreId);
+        if (!state.handled && !state.IsQueued()) {
+            // The SGI was inactive on the target core...
+            state.SetPending();
+            state.srcCoreId = srcCoreId;
+            m_incomingSgiPendingSources[coreId][id] &= ~BIT(srcCoreId);
+            m_virqPendingQueue.insert(state);
+            NotifyOtherCoreList(BIT(coreId));
+        }
+    }
+
+    void VirtualGic::SendSgi(u32 id, GicV2Distributor::SgirTargetListFilter filter, u32 coreList)
+    {
+        switch (filter) {
+            case GicV2Distributor::ForwardToTargetList:
+                // Forward to coreList
+                break;
+            case GicV2Distributor::ForwardToAllOthers:
+                // Forward to all but current core
+                coreList = ~BIT(currentCoreCtx->GetCoreId());
+                break;
+            case GicV2Distributor::ForwardToSelf:
+                // Forward to current core only
+                coreList = BIT(currentCoreCtx->GetCoreId());
+                break;
+            default:
+                DEBUG("Emulated GCID_SGIR: invalid TargetListFilter value!\n");
+                return;
+        }
+
+        coreList &= CoreContext::GetActiveCoreMask();
+        for (u32 dstCore: util::BitsOf{coreList}) {
+            SetSgiPendingState(id, dstCore, currentCoreCtx->GetCoreId());
+        }
+    }
+
+    bool VirtualGic::ValidateGicdRegisterAccess(size_t offset, size_t sz)
+    {
+        // ipriorityr, itargetsr, *pendsgir are byte-accessible
+        // Report a fault on accessing fields for 
+        if (
+            !(offset >= GICDOFF(ipriorityr) && offset < GICDOFF(ipriorityr) + GicV2Distributor::maxIrqId) &&
+            !(offset >= GICDOFF(itargetsr)  && offset < GICDOFF(itargetsr) + GicV2Distributor::maxIrqId) &&
+            !(offset >= GICDOFF(cpendsgir)  && offset < GICDOFF(cpendsgir) + 16) &&
+            !(offset >= GICDOFF(spendsgir)  && offset < GICDOFF(spendsgir) + 16)
+        ) {
+            return (offset & 3) == 0 && sz == 4;
+        } else {
+            return sz == 1 || (sz == 4 && ((offset & 3) != 0));
+        }
+    }
+
+    void VirtualGic::WriteGicdRegister(u32 val, size_t offset, size_t sz)
+    {
+        static constexpr auto maxIrqId = GicV2Distributor::maxIrqId;
+        std::scoped_lock lk{IrqManager::GetInstance().m_lock};
+
+        switch (offset) {
+            case GICDOFF(typer):
+            case GICDOFF(iidr):
+            case GICDOFF(icpidr2):
+            case GICDOFF(itargetsr) ... GICDOFF(itargetsr) + 31:
+                // Write ignored (read-only registers)
+                break;
+            case GICDOFF(icfgr) ... GICDOFF(icfgr) + 31/4:
+                // Write ignored because of an implementation-defined choice
+                break;
+            case GICDOFF(igroupr) ... GICDOFF(igroupr) + maxIrqId/8:
+                // Write ignored because we don't implement Group 1 here
+                break;
+            case GICDOFF(ispendr) ... GICDOFF(ispendr) + maxIrqId/8:
+            case GICDOFF(icpendr) ... GICDOFF(icpendr) + maxIrqId/8:
+            case GICDOFF(isactiver) ... GICDOFF(isactiver) + maxIrqId/8:
+            case GICDOFF(icactiver) ... GICDOFF(icactiver) + maxIrqId/8:
+            case GICDOFF(cpendsgir) ... GICDOFF(cpendsgir) + 15:
+            case GICDOFF(spendsgir) ... GICDOFF(spendsgir) + 15:
+                // Write ignored, not implemented (at least not yet, TODO)
+                break;
+
+            case GICDOFF(ctlr): {
+                SetDistributorControlRegister(val);
+                break;
+            }
+
+            case GICDOFF(isenabler) ... GICDOFF(isenabler) + maxIrqId/8: {
+                u32 base = 8 * static_cast<u32>(offset - GICDOFF(isenabler));
+                for(u32 pos: util::BitsOf{val}) {
+                    SetInterruptEnabledState(base + pos);
+                }
+                break;
+            }
+            case GICDOFF(icenabler) ... GICDOFF(icenabler) + maxIrqId/8: {
+                u32 base = 8 * static_cast<u32>(offset - GICDOFF(icenabler));
+                for(u32 pos: util::BitsOf{val}) {
+                    SetInterruptEnabledState(base + pos);
+                }
+                break;
+            }
+
+            case GICDOFF(ipriorityr) ... GICDOFF(ipriorityr) + maxIrqId: {
+                u32 base = static_cast<u32>(offset - GICDOFF(ipriorityr));
+                for (u32 i = 0; i < static_cast<u32>(sz); i++) {
+                    SetInterruptPriorityByte(base + i, static_cast<u8>(val));
+                    val >>= 8;
+                }
+                break;
+            }
+
+            case GICDOFF(itargetsr) + 32 ... GICDOFF(itargetsr) + maxIrqId: {
+                u32 base = static_cast<u32>(offset - GICDOFF(itargetsr));
+                for (u32 i = 0; i < static_cast<u32>(sz); i++) {
+                    SetInterruptTargets(base + i, static_cast<u8>(val));
+                    val >>= 8;
+                }
+                break;
+            }
+
+            case GICDOFF(icfgr) + 32/4 ... GICDOFF(icfgr) + maxIrqId/4: {
+                u32 base = 4 * static_cast<u32>(offset & 0xFF);
+                for (u32 i = 0; i < 16; i++) {
+                    SetInterruptConfigBits(base + i, val & 3);
+                    val >>= 2;
+                }
+                break;
+            }
+
+            case GICDOFF(sgir): {
+                SendSgi(val & 0xF, static_cast<GicV2Distributor::SgirTargetListFilter>((val >> 24) & 3), (val >> 16) & 0xFF);
+                break;
+            }
+
+            default:
+                DEBUG("Write to GICD reserved/implementation-defined register offset=0x%03lx value=0x%08lx", offset, val);
+                break;
+        }
+
+        UpdateState();
+    }
+
+
+    u32 VirtualGic::ReadGicdRegister(size_t offset, size_t sz)
+    {
+        static constexpr auto maxIrqId = GicV2Distributor::maxIrqId;
+        std::scoped_lock lk{IrqManager::GetInstance().m_lock};
+
+        //DEBUG("gicd read off 0x%03llx sz %lx\n", offset, sz);
+        u32 val = 0;
+
+        switch (offset) {
+            case GICDOFF(icfgr) ... GICDOFF(icfgr) + 31/4:
+                // RAZ because of an implementation-defined choice
+                break;
+            case GICDOFF(igroupr) ... GICDOFF(igroupr) + maxIrqId/8:
+                // RAZ because we don't implement Group 1 here
+                break;
+            case GICDOFF(ispendr) ... GICDOFF(ispendr) + maxIrqId/8:
+            case GICDOFF(icpendr) ... GICDOFF(icpendr) + maxIrqId/8:
+            case GICDOFF(isactiver) ... GICDOFF(isactiver) + maxIrqId/8:
+            case GICDOFF(icactiver) ... GICDOFF(icactiver) + maxIrqId/8:
+            case GICDOFF(cpendsgir) ... GICDOFF(cpendsgir) + 15:
+            case GICDOFF(spendsgir) ... GICDOFF(spendsgir) + 15:
+                // RAZ, not implemented (at least not yet, TODO)
+                break;
+
+            case GICDOFF(ctlr): {
+                val = GetDistributorControlRegister();
+                break;
+            }
+            case GICDOFF(typer): {
+                val = GetDistributorTypeRegister();
+                break;
+            }
+            case GICDOFF(iidr): {
+                val = GetDistributorImplementerIdentificationRegister();
+                break;
+            }
+
+            case GICDOFF(isenabler) ... GICDOFF(isenabler) + maxIrqId/8:
+            case GICDOFF(icenabler) ... GICDOFF(icenabler) + maxIrqId/8: {
+                u32 base = 8 * static_cast<u32>(offset & 0x7F);
+                for (u32 i = 0; i < 32; i++) {
+                    val |= GetInterruptEnabledState(base + i) ? BIT(i) : 0;
+                }
+                break;
+            }
+
+            case GICDOFF(ipriorityr) ... GICDOFF(ipriorityr) + maxIrqId: {
+                u32 base = static_cast<u32>(offset - GICDOFF(ipriorityr));
+                for (u32 i = 0; i < sz; i++) {
+                    val |= GetInterruptPriorityByte(base + i) << (8 * i);
+                }
+                break;
+            }
+
+            case GICDOFF(itargetsr) ... GICDOFF(itargetsr) + maxIrqId: {
+                u32 base = static_cast<u32>(offset - GICDOFF(itargetsr));
+                for (u32 i = 0; i < sz; i++) {
+                    val |= GetInterruptTargets(base + i) << (8 * i);
+                }
+                break;
+            }
+
+            case GICDOFF(icfgr) + 32/4 ... GICDOFF(icfgr) + maxIrqId/4: {
+                u32 base = 4 * static_cast<u32>(offset & 0xFF);
+                for (u32 i = 0; i < 16; i++) {
+                    val |= GetInterruptConfigBits(base + i) << (2 * i);
+                }
+                break;
+            }
+
+            case GICDOFF(sgir):
+                // Write-only register
+                DEBUG("Read from write-only register GCID_SGIR\n");
+                break;
+
+            case GICDOFF(icpidr2): {
+                val = GetPeripheralId2Register();
+                break;
+            }
+
+            default:
+                DEBUG("Read from GICD reserved/implementation-defined register offset=0x%03lx\n", offset);
+                break;
+        }
+
+        UpdateState();
+        return val;
+    }
+
+    void VirtualGic::ResampleVirqLevel(VirtualGic::VirqState &state)
+    {
+        /*
+            For hardware interrupts, we have kept the interrupt active on the physical GICD
+            For level-sensitive interrupts, we need to check if they're also still physically pending (resampling).
+            If not, there's nothing to service anymore, and therefore we have to deactivate them, so that
+            we're notified when they become pending again.
+         */
+
+        if (state.edgeTriggered || !state.IsPending()) {
+            // Nothing to do for edge-triggered interrupts and non-pending interrupts
+            return;
+        }
+
+        u32 irqId = state.irqId;
+
+        // Can't do anything for level-sensitive PPIs from other cores either
+        if (irqId < 32 && state.coreId != currentCoreCtx->coreId) {
+            return;
+        }
+
+        bool lineLevel = IrqManager::IsInterruptPending(irqId);
+        if (!lineLevel) {
+            IrqManager::ClearInterruptActive(irqId);
+            state.ClearPendingLine();
+        }
+    }
+
+    void VirtualGic::CleanupPendingQueue()
+    {
+        // SGIs are pruned elsewhere
+
+        // Resample line level for level-sensitive interrupts
+        for (VirqState &state: m_virqPendingQueue) {
+            ResampleVirqLevel(state);
+        }
+
+        // Cleanup the list
+        m_virqPendingQueue.erase_if([](const VirqState &state) { return !state.IsPending(); });
+    }
+
+    size_t VirtualGic::ChoosePendingInterrupts(VirtualGic::VirqState *chosen[], size_t maxNum)
+    {
+        size_t numChosen = 0;
+        auto pred = [](const VirqState &state) {
+            if (state.irqId < 32 && state.coreId != currentCoreCtx->GetCoreId()) {
+                // We can't handle SGIs/PPIs of other cores.
+                return false;
+            }
+
+            return state.enabled  && (state.irqId < 32 || (state.targetList & BIT(currentCoreCtx->GetCoreId())) != 0);
+        };
+
+        for (VirqState &state: m_virqPendingQueue) {
+            if (pred(state)) {
+                chosen[numChosen++] = &state;
+            }
+        }
+
+        for (size_t i = 0; i < numChosen; i++) {
+            chosen[i]->handled = true;
+            chosen[i]->coreId = currentCoreCtx->GetCoreId();
+            m_virqPendingQueue.erase(*chosen[i]);
+        }
+    }
+
+    void VirtualGic::PushListRegisters(VirqState *chosen[], size_t num)
+    {
+        for (size_t i = 0; i < num; i++) {
+            VirqState &state = *chosen[i];
+            u32 irqId = state.irqId;
+
+            GicV2VirtualInterfaceController::ListRegister lr = {0};
+            lr.grp1 = false; // group0
+            lr.priority = state.priority;
+            lr.virtualId = irqId;
+
+            // We only add new pending interrupts here...
+            lr.pending = true;
+            lr.active = false;
+
+            // We don't support guests setting the pending latch, so the logic is probably simpler...
+
+            if (irqId < 16) {
+                // SGI
+                lr.physicalId = BIT(9) /* EOI notification bit */ | state.srcCoreId;
+                // ^ IDK how kvm gets away with not setting the EOI notif bits in some cases,
+                // what they do seems to be prone to drop interrupts, etc.
+
+                lr.hw = false; // software
+            } else {
+                // Actual physical interrupt
+                lr.hw = true;
+                lr.physicalId = irqId;
+            }
+
+            volatile auto *freeLr = AllocateListRegister();
+            ENSURE(freeLr != nullptr);
+            freeLr->raw = lr.raw;
+        }
+    }
+
+    bool VirtualGic::UpdateListRegister(volatile GicV2VirtualInterfaceController::ListRegister *lr)
+    {
+        GicV2VirtualInterfaceController::ListRegister lrCopy = { .raw = lr->raw };
+
+        u32 irqId = lrCopy.virtualId;
+
+        // Note: this give priority to multi-SGIs than can be immediately handled
+
+        // Update the state
+        VirqState &state = GetVirqState(irqId);
+        ENSURE(state.handled);
+
+        u32 srcCoreId = state.coreId;
+        u32 coreId = currentCoreCtx->GetCoreId();
+
+        state.active = lrCopy.active;
+
+        if (lrCopy.active) {
+            // We don't dequeue active interrupts
+
+            if (irqId < 16) {
+                // We can allow SGIs to be marked active-pending if it's been made pending from the same source again
+                // For hw interrupts, the active-pending state is tracked in the real GICD
+                if (m_incomingSgiPendingSources[coreId][irqId] & BIT(srcCoreId)) {
+                    lrCopy.pending = true;
+                    m_incomingSgiPendingSources[coreId][irqId] &= ~BIT(srcCoreId);
+                }
+            }
+
+            // If the vIRQ goes from pending to active, it has been acknowledged: clear line level and pending latch
+            // SGIs are always edge-triggered, so line level doesn't matter & that's why we handle them above to simplify the code
+            if (!lrCopy.pending) {
+                state.ClearPendingOnAck();
+            }
+            lr->raw = lrCopy.raw;
+            return true;
+        } else if (lrCopy.pending) {
+            // New interrupts might have come, pending status might have been changed, etc.
+            // We need to put the interrupt back in the pending list (which we clean up afterwards)
+            state.handled = false;
+            m_virqPendingQueue.insert(state);
+            lr->raw = 0;
+            return false;
+        } else {
+            // Interrupt is inactive. This means it has been acked and handled.
+            // SGIs are always edge-triggered, so line level doesn't matter & that's why we handle them above to simplify the code
+
+            if (irqId < 16) {
+                // Special case for multi-SGIs if they can be immediately handled
+                if (m_incomingSgiPendingSources[coreId][irqId] != 0) {
+                    srcCoreId = __builtin_ctz(m_incomingSgiPendingSources[coreId][irqId]);
+                    state.srcCoreId = srcCoreId;
+                    m_incomingSgiPendingSources[coreId][irqId] &= ~BIT(srcCoreId);
+                    lrCopy.physicalId = BIT(9) /* EOI notification bit */ | srcCoreId;
+
+                    lrCopy.pending = true;
+                    lr->raw = lrCopy.raw;
+                }
+            }
+
+            if (!lrCopy.pending) {
+                // Inactive interrupt, cleanup
+                state.ClearPendingOnAck();
+                state.handled = false;
+                lr->raw = 0;
+                return false;
+            } else {
+                return true;
+            }
+        }
+    }
+
+    void VirtualGic::UpdateState()
+    {
+        GicV2VirtualInterfaceController::HypervisorControlRegister hcr = { .raw = gich->hcr.raw };
+        u32 coreId = currentCoreCtx->GetCoreId();
+
+        // First, put back inactive interrupts into the queue, handle some SGI stuff
+        // Need to handle the LRs in reverse order to keep list stability
+        u64 usedMap = cpu::rbit(m_usedLrMap[coreId]);
+        for (auto pos: util::BitsOf{usedMap}) {
+            if (!UpdateListRegister(&gich->lr[63 - pos])) {
+                usedMap &= ~BITL(pos);
+            }
+        }
+        m_usedLrMap[coreId] = cpu::rbit(usedMap);
+
+        // Then, clean the list up
+        CleanupPendingQueue();
+
+        size_t numFreeLr = GetNumberOfFreeListRegisters();
+        VirqState *chosen[64];
+
+        // Choose interrupts...
+        size_t numChosen = ChoosePendingInterrupts(chosen, numFreeLr);
+
+        // ...and push them
+        PushListRegisters(chosen, numChosen);
+
+        // Enable underflow interrupt when appropriate to do so
+        hcr.uie = m_numListRegisters - GetNumberOfFreeListRegisters() > 1;
+        gich->hcr.raw = hcr.raw;
+    }
+
+    std::optional<bool> VirtualGic::InterruptTopHalfHandler(u32 irqId, u32)
+    {
+        if (irqId == IrqManager::VgicUpdateSgi) {
+            // This SGI is just there to trigger the state update
+            return false;
+        } else if (irqId != GIC_IRQID_MAINTENANCE) {
+            return {};
+        }
+
+        // Maintenance interrupt handler:
+        GicV2VirtualInterfaceController::MaintenanceIntStatRegister misr = { .raw = gich->misr.raw };
+
+        // Force GICV_CTRL to behave like ns-GICC_CTLR, with group 1 being replaced by group 0
+        // Ensure we aren't spammed by maintenance interrupts, either.
+        if (misr.vgrp0e || misr.vgrp0d || misr.vgrp1e || misr.vgrp1d) {
+            GicV2VirtualInterfaceController::VmControlRegister vmcr = { .raw = gich->vmcr.raw };
+            vmcr.cbpr = 0;
+            vmcr.fiqEn = 0;
+            vmcr.ackCtl = 0;
+            vmcr.enableGrp1 = 0;
+            gich->vmcr.raw = vmcr.raw;
+        }
+
+        if (misr.vgrp0e) {
+            DEBUG("EL2 [core %d]: Group 0 enabled maintenance interrupt\n", (int)currentCoreCtx->GetCoreId());
+            gich->hcr.vgrp0eie = false;
+            gich->hcr.vgrp0die = true;
+        } else if (misr.vgrp0d) {
+            DEBUG("EL2 [core %d]: Group 0 disabled maintenance interrupt\n", (int)currentCoreCtx->GetCoreId());
+            gich->hcr.vgrp0eie = true;
+            gich->hcr.vgrp0die = false;
+        }
+
+        // Already handled the following 2 above:
+        if (misr.vgrp1e) {
+            DEBUG("EL2 [core %d]: Group 1 enabled maintenance interrupt\n", (int)currentCoreCtx->GetCoreId());
+        }
+        if (misr.vgrp1d) {
+            DEBUG("EL2 [core %d]: Group 1 disabled maintenance interrupt\n", (int)currentCoreCtx->GetCoreId());
+        }
+
+        if (misr.eoi) {
+            //DEBUG("EL2 [core %d]: SGI EOI maintenance interrupt\n", currentCoreCtx->GetCoreId());
+        }
+
+        if (misr.u) {
+            //DEBUG("EL2 [core %d]: Underflow maintenance interrupt\n", currentCoreCtx->GetCoreId());
+        }
+
+        ENSURE2(!misr.lrenp, "List Register Entry Not Present maintenance interrupt!\n");
+
+        // The rest should be handled by the main loop...
+        return false;
+    }
+
+    void VirtualGic::EnqueuePhysicalIrq(u32 id)
+    {
+        VirqState &state = GetVirqState(id);
+        state.SetPending();
+        m_virqPendingQueue.insert(state);
+    }
+
+    void VirtualGic::Initialize()
+    {
+        if (currentCoreCtx->IsBootCore()) {
+            m_virqPendingQueue.Initialize(m_virqStates.data());
+            m_numListRegisters = static_cast<u8>(1 + (gich->vtr & 0x3F));
+
+            // All fields are reset to 0 on reset and deep sleep exit
+
+            for (VirqState &state: m_virqStates) {
+                state.listPrev = state.listNext = virqListInvalidIndex;
+                state.priority = lowestPriority;
+            }
+
+            // SPIs (+ reserved interrupts just in case)
+            for (u32 i = 32; i < 1024; i++) {
+                GetVirqState(0, i).irqId = i;
+            }
+
+            // SGIs, PPIs
+            for (u32 coreId = 0; coreId < MAX_CORE; coreId++) {
+                for (u32 i = 0; i < 32; i++) {
+                    VirqState &state = GetVirqState(coreId, i);
+                    state.coreId = coreId;
+                    state.irqId = i;
+                    if (i < 16) {
+                        state.edgeTriggered = true;
+                        state.enabled = true;
+                    } else {
+                        state.edgeTriggered = IrqManager::IsInterruptEdgeTriggered(i);
+                    }
+                }
+            }
+
+            // All guest interrupts are initially configured as disabled
+            // All guest SPIs are initially configured as level-sensitive with no targets
+        }
+
+        auto &mgr = IrqManager::GetInstance();
+
+        mgr.Register(*this, GIC_IRQID_MAINTENANCE, true);
+        mgr.Register(*this, IrqManager::VgicUpdateSgi, false);
+
+        // Clear the list registers (they reset to 0, though)
+        for (u8 i = 0; i < m_numListRegisters; i++) {
+            gich->lr[i].raw = 0;
+        }
+
+        // Enable a few maintenance interrupts. Enable the virtual interface.
+        GicV2VirtualInterfaceController::HypervisorControlRegister hcr = {};
+        hcr.vgrp1eie = true, 
+        hcr.vgrp0eie = true,
+        hcr.lrenpie = true,
+        hcr.en = true,
+        gich->hcr.raw = hcr.raw;
+    }
+}

thermosphere/src/hvisor_virtual_gic.hpp

@@ -0,0 +1,389 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "defines.hpp"
+#include "hvisor_core_context.hpp"
+#include "cpu/hvisor_cpu_exception_sysregs.hpp"
+#include "hvisor_irq_manager.hpp"
+
+namespace ams::hvisor {
+
+    class VirtualGic final : public IInterruptTask {
+        SINGLETON_WITH_ATTRS(VirtualGic, TEMPORARY);
+
+        private:
+            // For convenience, although they're already defined in irq manager header:
+            static inline volatile auto *const gicd = IrqManager::gicd;
+            static inline volatile auto *const gich = IrqManager::gich;
+
+            // Architectural properties
+            static constexpr u32 priorityShift = 3;
+            static constexpr u32 numPriorityLevels = BIT(8 - priorityShift);
+            static constexpr u32 lowestPriority = numPriorityLevels - 1;
+
+            // List managament constants
+            static constexpr u32 spiEndIndex = GicV2Distributor::maxIrqId + 1 - 32;
+            static constexpr u32 maxNumIntStates = spiEndIndex + MAX_CORE * 32;
+            static constexpr u32 virqListEndIndex = maxNumIntStates;
+            static constexpr u32 virqListInvalidIndex = virqListEndIndex + 1;
+
+        private:
+            struct VirqState {
+                u32 listPrev        : 11;
+                u32 listNext        : 11;
+                u32 irqId           : 10;
+                u32 priority        : 5;
+                bool pending        : 1;
+                bool active         : 1;
+                bool handled        : 1;
+                bool pendingLatch   : 1;
+                bool edgeTriggered  : 1;
+                u32 coreId          : 3;
+                u32 targetList      : 8;
+                u32 srcCoreId       : 3;
+                bool enabled        : 1;
+                u64                 : 0;
+
+                constexpr bool IsPending() const
+                {
+                    return pendingLatch || (!edgeTriggered && pending);
+                }
+                constexpr void SetPending()
+                {
+                    if (!edgeTriggered) {
+                        pending = true;
+                    } else {
+                        pendingLatch = true;
+                    }
+                }
+                constexpr bool ClearPendingLine()
+                {
+                    // Don't clear pending latch status
+                    pending = false;
+                }
+                constexpr bool ClearPendingOnAck()
+                {
+                    // On ack, both pending line status and latch are cleared
+                    pending = false;
+                    pendingLatch = false;
+                }
+                constexpr bool IsQueued() const
+                {
+                    return listPrev != virqListInvalidIndex && listNext != virqListInvalidIndex;
+                }
+            };
+
+
+            class VirqQueue final {
+                private:
+                    VirqState *m_first = nullptr;
+                    VirqState *m_last = nullptr;
+                    VirqState *m_storage = nullptr;
+                public:
+                    template<bool isConst>
+                    class Iterator {
+                        friend class Iterator<true>;
+                        friend class VirqQueue;
+                        private:
+                            VirqState *m_node = nullptr;
+                            VirqState *m_storage = nullptr;
+
+                        private:
+                            explicit constexpr Iterator(VirqState *node, VirqState *storage) : m_node{node}, m_storage{storage} {}
+
+                        public:
+                            // allow implicit const->non-const
+                            constexpr Iterator(const Iterator<false> &other) : m_node{other.m_storage}, m_storage{other.m_storage} {}
+                            constexpr Iterator() = default;
+
+                        public:
+                            using iterator_category = std::bidirectional_iterator_tag;
+                            using value_type = VirqState;
+                            using difference_type = ptrdiff_t;
+                            using pointer = typename std::conditional<isConst, const VirqState *, VirqState *>::type;
+                            using reference = typename std::conditional<isConst, const VirqState &, VirqState &>::type;
+
+                            constexpr bool operator==(const Iterator &other) const { return m_node == other.m_node; }
+                            constexpr bool operator!=(const Iterator &other) const { return !(*this == other); }
+                            constexpr reference operator*() { return *m_node; }
+                            constexpr pointer operator->() { return m_node; }
+
+                            constexpr Iterator &operator++()
+                            {
+                                m_node = &m_storage[m_node->listNext];
+                                return *this;
+                            }
+
+                            constexpr Iterator &operator--()
+                            {
+                                m_node = &m_storage[m_node->listPrev];
+                                return *this;
+                            }
+
+                            constexpr Iterator &operator++(int)
+                            {
+                                const Iterator v{*this};
+                                ++(*this);
+                                return v;
+                            }
+
+                            constexpr Iterator &operator--(int)
+                            {
+                                const Iterator v{*this};
+                                --(*this);
+                                return v;
+                            }
+                    };
+
+                    private:
+                        constexpr u32 GetStateIndex(VirqState &elem) { return static_cast<u32>(&elem - &m_storage[0]); }
+                    public:
+                        using pointer = VirqState *;
+                        using const_pointer = const VirqState *;
+                        using reference = VirqState &;
+                        using const_reference = const VirqState &;
+                        using value_type = VirqState;
+                        using size_type = size_t;
+                        using difference_type = ptrdiff_t;
+                        using iterator = Iterator<false>;
+                        using const_iterator = Iterator<true>;
+
+                        constexpr void Initialize(VirqState *storage) { m_storage = storage; }
+
+                        constexpr VirqState &front() { return *m_first; };
+                        constexpr const VirqState &front() const { return *m_first; };
+
+                        constexpr VirqState &back() { return *m_last; };
+                        constexpr const VirqState &back() const { return *m_last; };
+
+                        constexpr const_iterator cbegin() const { return const_iterator{m_first, m_storage}; }
+                        constexpr const_iterator cend() const { return const_iterator{&m_storage[virqListEndIndex], m_storage}; }
+
+                        constexpr const_iterator begin() const { return cbegin(); }
+                        constexpr const_iterator end() const { return cend(); }
+
+                        constexpr iterator begin() { return iterator{m_first, m_storage}; }
+                        constexpr iterator end() { return iterator{&m_storage[virqListEndIndex], m_storage}; }
+
+                        iterator insert(iterator pos, VirqState &elem);
+                        iterator insert(VirqState &elem);
+
+                        constexpr iterator erase(iterator startPos, iterator endPos)
+                        {
+                            VirqState &prev = m_storage[startPos->listPrev];
+                            VirqState &next = *endPos;
+                            u32 nextPos = GetStateIndex(*endPos);
+
+                            ENSURE(startPos->IsQueued());
+                            if (startPos->listPrev != virqListEndIndex) {
+                                prev.listNext = nextPos;
+                            } else {
+                                m_first = &next;
+                            }
+
+                            if (nextPos != virqListEndIndex) {
+                                next.listPrev = startPos->listPrev;
+                            } else {
+                                m_last = &prev;
+                            }
+
+                            for (iterator it = startPos; it != endPos; ++it) {
+                                it->listPrev = it->listNext = virqListInvalidIndex;
+                            }
+                        }
+
+                        constexpr iterator erase(iterator pos) { return erase(pos, std::next(pos)); }
+                        constexpr iterator erase(VirqState &pos) { return erase(iterator{&pos, m_storage}); }
+
+                        template<typename Pred>
+                        void erase_if(Pred p)
+                        {
+                            for (iterator it = begin(); l = end(); i != l) {
+                                if(p(*it)) {
+                                    it = erase(it);
+                                } else {
+                                    ++it;
+                                }
+                            }
+                        }
+
+                        constexpr void Initialize(VirqState *storage)
+                        {
+                            m_storage = storage;
+                            m_first = m_last = &(*end());
+                        }
+            };
+
+
+            private:
+                static void NotifyOtherCoreList(u32 coreList)
+                {
+                    coreList &= ~BIT(currentCoreCtx->GetCoreId());
+                    if (coreList != 0) {
+                        IrqManager::GenerateSgiForList(IrqManager::VgicUpdateSgi, coreList);
+                    }
+                }
+
+                static void NotifyAllOtherCores()
+                {
+                    IrqManager::GenerateSgiForAllOthers(IrqManager::VgicUpdateSgi);
+                }
+
+                static u64 GetEmptyListStatusRegister()
+                {
+                    return static_cast<u64>(gich->elsr1) << 32 | static_cast<u64>(gich->elsr0);
+                }
+
+                static u64 GetNumberOfFreeListRegisters()
+                {
+                    return __builtin_popcountll(GetEmptyListStatusRegister());
+                }
+
+            private:
+                std::array<VirqState, maxNumIntStates> m_virqStates{};
+                std::array<std::array<u8, 32>, MAX_CORE> m_incomingSgiPendingSources{};
+                std::array<u64, MAX_CORE> m_usedLrMap{};
+
+                VirqQueue m_virqPendingQueue{};
+                bool m_distributorEnabled = false;
+
+                u8 m_numListRegisters = 0;
+            private:
+                constexpr VirqState &GetVirqState(u32 coreId, u32 id)
+                {
+                    if (id >= 32) {
+                        return m_virqStates[id - 32];
+                    } else if (id <= GicV2Distributor::maxIrqId) {
+                        return m_virqStates[spiEndIndex + 32 * coreId + id];
+                    }
+                }
+
+                VirqState &GetVirqState(u32 id) { return GetVirqState(currentCoreCtx->GetCoreId(), id); }
+
+                void SetDistributorControlRegister(u32 value)
+                {
+                    // We implement a virtual distributor/interface w/o security extensions.
+                    // Moreover, we forward all interrupts as Group 0 so that non-secure code that assumes GICv2
+                    // *with* security extensions (and thus all interrupts fw as group 1 there) still works (bit are in the same positions).
+
+                    // We don't implement Group 1 interrupts, either (so that's similar to GICv1).
+                    bool old = m_distributorEnabled;
+                    m_distributorEnabled = (value & 1) != 0;
+
+                    // Enable bit is actually just a global enable bit for all irq forwarding, other functions of the GICD aren't affected by it
+                    if (old != m_distributorEnabled) {
+                        NotifyAllOtherCores();
+                    }
+                }
+
+                u32 GetDistributorControlRegister(void)
+                {
+                    return m_distributorEnabled ? 1 : 0;
+                }
+
+                u32 GetDistributorTypeRegister(void)
+                {
+                    // See above comment.
+                    // Therefore, LSPI = 0, SecurityExtn = 0, rest = from physical distributor
+                    return IrqManager::GetTypeRegister() & 0x7F;
+                }
+
+                u32 GetDistributorImplementerIdentificationRegister(void)
+                {
+                    u32 iidr = 'A' << 24; // Product Id: Atmosphère (?)
+                    iidr |= 2 << 16;    // Major revision 2 (GICv2)
+                    iidr |= 0 << 12;    // Minor revision 0
+                    iidr |= 0x43B;      // Implementer: Arm (value copied from physical GICD)
+                    return iidr;
+                }
+
+                bool GetInterruptEnabledState(u32 id)
+                {
+                    // SGIs are always enabled
+                    return id < 16 || (IrqManager::IsGuestInterrupt(id) && GetVirqState(currentCoreCtx->GetCoreId(), id).enabled);
+                }
+
+                u8 GetInterruptPriorityByte(u32 id)
+                {
+                    return IrqManager::IsGuestInterrupt(id) ? GetVirqState(currentCoreCtx->GetCoreId(), id).priority << priorityShift : 0;
+                }
+
+                u8 GetInterruptTargets(u16 id)
+                {
+                    return id < 32 || (IrqManager::IsGuestInterrupt(id) && GetVirqState(currentCoreCtx->GetCoreId(), id).targetList);
+                }
+
+                u32 GetInterruptConfigBits(u16 id)
+                {
+                    u32 oneNModel = id < 32 || !IrqManager::IsGuestInterrupt(id) ? 0 : 1;
+                    return (IrqManager::IsGuestInterrupt(id) && GetVirqState(id).edgeTriggered) ? 2 | oneNModel : oneNModel;
+                }
+
+                u32 GetPeripheralId2Register(void)
+                {
+                    return 2u << 4;
+                }
+
+                void SetInterruptEnabledState(u32 id);
+                void ClearInterruptEnabledState(u32 id);
+                void SetInterruptPriorityByte(u32 id, u8 priority);
+                void SetInterruptTargets(u32 id, u8 coreList);
+                void SetInterruptConfigBits(u32 id, u32 config);
+                void SetSgiPendingState(u32 id, u32 coreId, u32 srcCoreId);
+                void SendSgi(u32 id, GicV2Distributor::SgirTargetListFilter filter, u32 coreList);
+
+                void ResampleVirqLevel(VirqState &state);
+                void CleanupPendingQueue();
+                size_t ChoosePendingInterrupts(VirqState *chosen[], size_t maxNum);
+
+                volatile GicV2VirtualInterfaceController::ListRegister *AllocateListRegister(void)
+                {
+                    u32 ff = __builtin_ffsll(GetEmptyListStatusRegister());
+                    if (ff == 0) {
+                        return nullptr;
+                    } else {
+                        m_usedLrMap[currentCoreCtx->GetCoreId()] |= BITL(ff - 1);
+                        return &gich->lr[ff - 1];
+                    }
+                }
+
+                void PushListRegisters(VirqState *chosen[], size_t num);
+                bool UpdateListRegister(volatile GicV2VirtualInterfaceController::ListRegister *lr);
+
+            private:
+                constexpr VirtualGic() = default;
+
+            public:
+                // For convenience (when trapping lower-el data aborts):
+                static constexpr uintptr_t gicdPhysicalAddress = 0; // fixme pls MEMORY_MAP_PA_GICD;
+            public:
+                static bool ValidateGicdRegisterAccess(size_t offset, size_t sz);
+            public:
+                void WriteGicdRegister(u32 val, size_t offset, size_t sz);
+                u32 ReadGicdRegister(size_t offset, size_t sz);
+
+                // Must be called by irqManager only...
+                // not sure if I should have made IrqManager a friend of this class
+                void UpdateState();
+
+                std::optional<bool> InterruptTopHalfHandler(u32 irqId, u32) final;
+
+                void EnqueuePhysicalIrq(u32 id);
+                void Initialize();
+    };
+}

thermosphere/src/hvisor_watchpoint_manager.cpp

@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2019-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 "hvisor_watchpoint_manager.hpp"
+#include "cpu/hvisor_cpu_instructions.hpp"
+#include <mutex>
+
+#define _REENT_ONLY
+#include <cerrno>
+
+// Can't use two THERMOSPHERE_SAVE_SYSREG as it prevents ldp from being generated
+#define SAVE_WATCHPOINT(i, _)\
+    __asm__ __volatile__ (\
+        "msr " STRINGIZE(dbgwvr##i##_el1) ", %0\n"\
+        "msr " STRINGIZE(dbgwcr##i##_el1) ", %1"\
+        :\
+        : "r"(m_stopPoints[i].vr), "r"(m_stopPoints[i].cr.raw)\
+        : "memory"\
+    );
+
+namespace {
+
+    constexpr bool IsRangeMaskWatchpoint(uintptr_t addr, size_t size)
+    {
+        // size needs to be a power of 2, at least 8 (we'll only allow 16+ though), addr needs to be aligned.
+        bool ret = (size & (size - 1)) == 0 && size >= 16 && (addr & (size - 1)) == 0;
+        return ret;
+    }
+
+    constexpr bool CheckWatchpointAddressAndSizeParams(uintptr_t addr, size_t size)
+    {
+        if (size == 0) {
+            return false;
+        } else if (size > 8) {
+            return IsRangeMaskWatchpoint(addr, size);
+        } else {
+            return ((addr + size) & ~7ul) == (addr & ~7ul);
+        }
+    }
+
+}
+
+namespace ams::hvisor {
+
+    WatchpointManager WatchpointManager::instance{};
+
+    void WatchpointManager::Reload() const
+    {
+        cpu::dmb();
+        EVAL(REPEAT(MAX_WCR, SAVE_WATCHPOINT, ~));
+        cpu::dsb();
+        cpu::isb();
+    }
+
+    bool WatchpointManager::FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction) const
+    {
+        size_t off;
+        size_t sz;
+        size_t nmask;
+    
+        if (pair.cr.mask != 0) {
+            off = 0;
+            sz = MASK(pair.cr.mask);
+            nmask = ~sz;
+        } else {
+            off = __builtin_ffs(pair.cr.bas) - 1;
+            sz = __builtin_popcount(pair.cr.bas);
+            nmask = ~7ul;
+        }
+
+        if (size != 0) {
+            // Strict watchpoint check
+            if (addr == pair.vr + off && direction == pair.cr.lsc && sz == size) {
+                return true;
+            }
+        } else {
+            // Return first wp that could have triggered the exception
+            if ((addr & nmask) == pair.vr && (direction & pair.cr.lsc) != 0) {
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    cpu::DebugRegisterPair WatchpointManager::RetrieveWatchpointConfig(uintptr_t addr, cpu::DebugRegisterPair::LoadStoreControl direction) const
+    {
+        std::scoped_lock lk{m_lock};
+        const cpu::DebugRegisterPair *p = Find(addr, 0, direction);
+        return p != nullptr ? *p : cpu::DebugRegisterPair{};
+    }
+
+    int WatchpointManager::Add(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction)
+    {
+        if (!CheckWatchpointAddressAndSizeParams(addr, size)) {
+            return -EINVAL;
+        }
+
+        cpu::DebugRegisterPair wp{};
+        wp.cr.lsc = direction;
+        if (IsRangeMaskWatchpoint(addr, size)) {
+            wp.vr = addr;
+            wp.cr.bas = 0xFF; // TRM-mandated
+            wp.cr.mask = static_cast<u32>(__builtin_ffsl(size) - 1);
+        } else {
+            size_t off = addr & 7ull;
+            wp.vr = addr & ~7ul;
+            wp.cr.bas = MASK2(off + size - 1, off);
+        }
+
+        return AddImpl(addr, size, wp);
+    }
+
+    int WatchpointManager::Remove(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction)
+    {
+        if (!CheckWatchpointAddressAndSizeParams(addr, size)) {
+            return -EINVAL;
+        }
+
+        return RemoveImpl(addr, size, direction);
+    }
+}

thermosphere/src/hvisor_watchpoint_manager.hpp

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2019-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/>.
+ */
+
+#pragma once
+
+#include "hvisor_hw_stop_point_manager.hpp"
+
+namespace ams::hvisor {
+
+    class WatchpointManager final : public HwStopPointManager {
+        SINGLETON(WatchpointManager);
+        private:
+            bool FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction) const final;
+            void Reload() const final;
+
+        private:
+            constexpr WatchpointManager() : HwStopPointManager(MAX_WCR, IrqManager::ReloadWatchpointsSgi) {}
+
+        public:
+            virtual void ReloadOnAllCores() const;
+            static void ReloadOnAllCoresSgiHandler();
+
+            cpu::DebugRegisterPair RetrieveWatchpointConfig(uintptr_t addr, cpu::DebugRegisterPair::LoadStoreControl direction) const;
+            int Add(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction);
+            int Remove(uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction);
+    };
+}

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/lib/ini.c

@@ -1,272 +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)
-{
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wstringop-truncation"
-    strncpy(dest, src, size - 1);
-#pragma GCC diagnostic pop
-    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/libc/fmt.c

@@ -0,0 +1,320 @@
+/*
+ * 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
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <string.h>
+#include <stdarg.h>
+#include <stddef.h>
+#include <stdbool.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 int skipAtoi(const char **s)
+{
+    int i = 0;
+
+    while(IS_DIGIT(**s)) i = i * 10 + *((*s)++) - '0';
+
+    return i;
+}
+
+static char *processNumber(char *str, long long num, bool isHex, int size, int precision, unsigned int 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";
+
+    int 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)
+        {
+            unsigned long long int base = isHex ? 16ULL : 10ULL;
+            tmp[i++] = dig[(unsigned long long int)num % base];
+            num = (long long)((unsigned long long int)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
+        unsigned int 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
+        int fieldWidth = -1; //Width of output field
+        if(IS_DIGIT(*fmt)) fieldWidth = skipAtoi(&fmt);
+        else if(*fmt == '*')
+        {
+            fmt++;
+
+            fieldWidth = va_arg(args, int);
+
+            if(fieldWidth < 0)
+            {
+                fieldWidth = -fieldWidth;
+                flags |= LEFT;
+            }
+        }
+
+        //Get the precision
+        int 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, int);
+            }
+
+            if(precision < 0) precision = 0;
+        }
+
+        //Get the conversion qualifier
+        unsigned int 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++ = (unsigned char)va_arg(args, int);
+                while(--fieldWidth > 0) *str++ = ' ';
+                continue;
+
+            case 's':
+            {
+                char *s = va_arg(args, char *);
+                if(!s) s = "<NULL>";
+                unsigned int len = (precision != -1) ? strnlen(s, precision) : strlen(s);
+                if(!(flags & LEFT)) while((int)len < fieldWidth--) *str++ = ' ';
+                for(unsigned int i = 0; i < len; i++) *str++ = *s++;
+                while((int)len < fieldWidth--) *str++ = ' ';
+                continue;
+            }
+
+            case 'p':
+                if(fieldWidth == -1)
+                {
+                    fieldWidth = 8;
+                    flags |= ZEROPAD;
+                }
+                str = processNumber(str, va_arg(args, unsigned int), 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;
+        }
+
+        long long num;
+
+        if(flags & SIGN)
+        {
+            if(integerType == 1) num = va_arg(args, signed long long int);
+            else num = va_arg(args, signed int);
+
+            if(integerType == 2) num = (signed short)num;
+            else if(integerType == 3) num = (signed char)num;
+        }
+        else
+        {
+            if(integerType == 1) num = va_arg(args, unsigned long long int);
+            else num = va_arg(args, unsigned int);
+
+            if(integerType == 2) num = (unsigned short)num;
+            else if(integerType == 3) num = (unsigned char)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;
+}
+
+
+#ifdef __cplusplus
+}
+#endif

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 "utils.h"
-#include "lib/printk.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"
 
-/**
+#include "memory_map.h"
- * Switches to EL1, and then calls main_el1.
+#include "mmu.h"
- * Implemented in assembly in entry.S.
- */
-void switch_to_el1();
 
-/**
+static void loadKernelViaSemihosting(void)
- * C entry point for execution at EL1.
- */
-void main_el1(void * fdt);
-
-/**
- * Reference to the EL2 vector table.
- * Note that the type here isn't reprsentative-- we just need the address of the label.
- */
-extern uint64_t el2_vector_table;
-
-/**
- * Clear out the system's bss.
- */
-void _clear_bss(void)
 {
-    // These symbols don't actually have a meaningful type-- instead,
+    // Note: !! hardcoded addresses !!
-    // we care about the locations at which the linker /placed/ these
+    size_t len = 1<<20; // max len
-    // symbols, which happen to be at the start and end of the BSS.
+    uintptr_t buf = 0x60000000 + (1<<20);
-    // We use chars here to make the math easy. :)
-    extern char lds_bss_start, lds_bss_end;
 
-    memset(&lds_bss_start, 0, &lds_bss_end - &lds_bss_start);
+    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();
- * Triggered on an unrecoverable condition; prints an error message
+    __dsb_local();
- * and terminates execution.
- */
-void panic(const char * message)
-{
-    printk("\n\n");
-    printk("-----------------------------------------------------------------\n");
-    printk("PANIC: %s\n", message);
-    printk("-----------------------------------------------------------------\n");
 
-    // TODO: This should probably induce a reboot,
+    DEBUG("Loading kernel via semihosted file I/O... ");
-    // rather than sticking here.
+    handle = semihosting_file_open("test_kernel.bin", FOPEN_MODE_RB);
-    while(1);
+    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;
 }
 
 
 
-/**
+#include "platform/uart.h"
- * Launch an executable kernel image. Should be the last thing called by
+#include "debug_manager.h"
- * Discharge, as it does not return.
+typedef struct TestCtx {
- *
+    char buf[512+1];
- * @param kernel The kernel to be executed.
+} TestCtx;
- * @param fdt The device tree to be passed to the given kernel.
- */
-void launch_kernel(const void *kernel)
-{
-    // Construct a function pointer to our kernel, which will allow us to
-    // jump there immediately. Note that we don't care what this leaves on
-    // the stack, as either our entire stack will be ignored, or it'll
-    // be torn down by the target kernel anyways.
-    void (*target_kernel)(void) = kernel;
 
-    printk("Launching Horizon kernel...\n");
+static TestCtx g_testCtx;
-    target_kernel();
+
+size_t testReceiveCallback(TransportInterface *iface, void *p)
+{
+    TestCtx *ctx = (TestCtx *)p;
+    debugManagerPauseCores(BIT(0));
+    return transportInterfaceReadDataUntil(iface, ctx->buf, 512, '\r');
 }
 
-
+void testProcessDataCallback(TransportInterface *iface, void *p, size_t sz)
-
-/**
- * Core section of the stub-- sets up the hypervisor from up in EL2.
- */
-int main(int argc, void **argv)
 {
-    // Read the currrent execution level...
+    (void)iface;
-    uint32_t el = get_current_el();
+    (void)sz;
+    debugManagerUnpauseCores(BIT(0));
+    TestCtx *ctx = (TestCtx *)p;
+    (void)ctx;
+    DEBUG("EL2 [core %u]: you typed: %s\n", currentCoreCtx->coreId, ctx->buf);
+}
 
-    /* Say hello. */
+void test(void)
-    printk("Welcome to Atmosph\xe8re Thermosph\xe8" "re!\n");
+{
-    printk("Running at EL%d.\n", el);
+    TransportInterface *iface = transportInterfaceCreate(
+        TRANSPORT_INTERFACE_TYPE_UART,
+        DEFAULT_UART,
+        DEFAULT_UART_FLAGS,
+        testReceiveCallback,
+        testProcessDataCallback,
+        &g_testCtx
+    );
+    transportInterfaceSetInterruptAffinity(iface, BIT(1));
+}
 
-    // ... and ensure we're in EL2.
+void thermosphereMain(ExceptionStackFrame *frame, u64 pct)
-    if (el != 2) {
+{
-        panic("Thermosph\xe8" "re must be launched from EL2!");
+    initIrq();
+
+    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);
     }
 
-    // Set up the vector table for EL2, so that the HVC instruction can be used
+    enableTraps();
-    // from EL1. This allows us to return to EL2 after starting the EL1 guest.
+    enableBreakpointsAndWatchpoints();
-    set_vbar_el2(&el2_vector_table);
+    timerInit();
+    initBreakpoints();
+    initWatchpoints();
 
-    // TODO:
+    if (currentCoreCtx->isBootCore) {
-    // Insert any setup you want done in EL2, here. For now, EL2 is set up
+        if (currentCoreCtx->kernelEntrypoint == 0) {
-    // to do almost nothing-- it doesn't take control of any hardware,
+            ENSURE2(semihosting_connection_supported(), "Kernel not loaded!\n");
-    // and it hasn't set up any trap-to-hypervisor features.
+            loadKernelViaSemihosting();
-    printk("\nSwitching to EL1...\n");
+        }
-    switch_to_el1();
+    }
-}
+    else {
-
+        DEBUG("EL2: core %u reached main!\n", currentCoreCtx->coreId);
-
-/**
- * Secondary section of the stub, executed once we've surrendered
- * hypervisor privileges.
- */
-void main_el1(void * fdt)
-{
-    int rc;
-    (void)rc;
-
-    // Read the currrent execution level...
-    uint32_t el = get_current_el();
-
-    // Validate that we're in EL1.
-    printk("Now executing from EL%d!\n", el);
-    if(el != 1) {
-        panic("Executing with more privilege than we expect!");
     }
 
-    // If we've made it here, we failed to boot, and we can't recover.
+    setCurrentCoreActive();
-    panic("We should launch Horizon here!");
+
+    // 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/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/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 "devices.h"
-#include "vsprintf.h"
+#include "../../memory_map.h"
+#include "../../utils.h"
 
-/**
+#include "uart.h"
- * Temporary stand-in main printk.
+
- *
+void devicesMapAllExtra(void)
- * TODO: This should print via UART, console framebuffer, and to a ring for
- * consumption by Horizon
- */
-void printk(char *fmt, ...)
 {
-    va_list list;
+    uartSetRegisterBase(memoryMapPlatformMmio(MEMORY_MAP_PA_UART, 0x1000));
-    char buf[512];
-    va_start(list, fmt);
-    vsnprintf(buf, sizeof(buf), fmt, list);
 
-    /* FIXME: print via UART */
+    // Don't broadcast, since it's only ran once per boot by only one core, before the others are started...
-
+    __tlb_invalidate_el2_local();
-    va_end(list);
+    __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,36 @@
+/*
+ * 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
+
+#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     1023 // SBSA: 28. Unimplemented
+#define GIC_IRQID_SEC_PHYS_HYP_TIMER    1023 // SBSA: 20. Unimplemented
+#define GIC_IRQID_SEC_VIRT_HYP_TIMER    1023 // 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);