diff --git a/thermosphere/src/caches.h b/thermosphere/src/caches.h index 2590b354c..284ce1f59 100644 --- a/thermosphere/src/caches.h +++ b/thermosphere/src/caches.h @@ -19,6 +19,12 @@ #include "utils.h" #include "sysreg.h" +static inline u32 cacheGetInstructionCachePolicy(void) +{ + u32 ctr = (u32)GET_SYSREG(ctr_el0); + return (ctr >> 14) & 3; +} + static inline u32 cacheGetSmallestInstructionCacheLineSize(void) { u32 ctr = (u32)GET_SYSREG(ctr_el0); diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s index d595b1b71..b9eb2e39d 100644 --- a/thermosphere/src/exception_vectors.s +++ b/thermosphere/src/exception_vectors.s @@ -140,7 +140,7 @@ vector_entry _synchSp0 msr elr_el2, x18 // Note: non-broadcasting TLB maintenance op tlbi alle2 - dsb ish + dsb nsh isb eret check_vector_size _synchSp0 @@ -271,11 +271,30 @@ doSmcIndirectCallImplSize: /* Current EL, SPx */ -EXCEPTION_HANDLER_START _synchSpx, EXCEPTION_TYPE_HOST_CRASH +vector_entry _synchSpx + // Ignore crash if x18 is 0, when we're copying memory from the guest (w/ irq masked) + cbz x18, _synchSpxIgnoreCrash + + PIVOT_STACK_FOR_CRASH + SAVE_MOST_REGISTERS + + mov x0, sp + mov w1, #0 + + bl exceptionEntryPostprocess + mov x0, sp mrs x1, esr_el2 bl handleSameElSyncException -EXCEPTION_HANDLER_END _synchSpx, EXCEPTION_TYPE_HOST_CRASH + + b . + +_synchSpxIgnoreCrash: + mrs x18, elr_el2 + add x18, x18, #4 + msr elr_el2, x18 + eret +check_vector_size _synchSpx EXCEPTION_HANDLER_START _irqSpx, EXCEPTION_TYPE_HOST mov x0, sp diff --git a/thermosphere/src/execute_function.c b/thermosphere/src/execute_function.c index dcf019ef5..d326c3548 100644 --- a/thermosphere/src/execute_function.c +++ b/thermosphere/src/execute_function.c @@ -25,7 +25,7 @@ void executeFunctionOnCores(ExecutedFunction fun, void *args, bool sync, u32 cor currentCoreCtx->executedFunction = fun; currentCoreCtx->executedFunctionArgs = args; currentCoreCtx->executedFunctionSync = sync; - + __compiler_barrier(); generateSgiForList(ThermosphereSgi_ExecuteFunction, coreList); } @@ -46,4 +46,4 @@ void executeFunctionInterruptHandler(u32 srcCore) if (ctx->executedFunctionSync) { barrierWait(&ctx->executedFunctionBarrier); } -} \ No newline at end of file +} diff --git a/thermosphere/src/fpu_regs_load_store.s b/thermosphere/src/fpu_regs_load_store.s index 52feb0868..0959c1cb9 100644 --- a/thermosphere/src/fpu_regs_load_store.s +++ b/thermosphere/src/fpu_regs_load_store.s @@ -42,13 +42,13 @@ FUNCTION fpuLoadRegistersFromStorage msr fpsr, x1 msr fpcr, x2 dsb ish - isb ish + isb ret END_FUNCTION FUNCTION fpuStoreRegistersToStorage dsb ish - isb ish + isb LDSTORE_QREGS stp mrs x1, fpsr mrs x2, fpcr diff --git a/thermosphere/src/guest_memory.c b/thermosphere/src/guest_memory.c new file mode 100644 index 000000000..69c7d3a17 --- /dev/null +++ b/thermosphere/src/guest_memory.c @@ -0,0 +1,292 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "guest_memory.h" +#include "memory_map.h" +#include "mmu.h" +#include "spinlock.h" +#include "core_ctx.h" +#include "sysreg.h" +#include "vgic.h" +#include "irq.h" +#include "caches.h" + +static size_t guestReadWriteGicd(size_t offset, size_t size, void *readBuf, const void *writeBuf) +{ + recursiveSpinlockLock(&g_irqManager.lock); + + if (readBuf != NULL) { + size_t readOffset = 0; + size_t rem = size; + while (rem > 0) { + if ((offset + readOffset) % 4 == 0 && rem >= 4) { + // All accesses of this kind are valid + *(u32 *)((uintptr_t)readBuf + readOffset) = vgicReadGicdRegister(offset + readOffset, 4); + readOffset += 4; + rem -= 4; + } else if ((offset + readOffset) % 2 == 0 && rem >= 2) { + // All accesses of this kind would be translated to ldrh and are thus invalid. Abort. + size = readOffset; + goto end; + } else if (vgicValidateGicdRegisterAccess(offset + readOffset, 1)) { + // Valid byte access + *(u8 *)((uintptr_t)readBuf + readOffset) = vgicReadGicdRegister(offset + readOffset, 1); + readOffset += 1; + rem -= 1; + } else { + // Invalid byte access + size = readOffset; + goto end; + } + } + } + + if (writeBuf != NULL) { + size_t writeOffset = 0; + size_t rem = size; + while (rem > 0) { + if ((offset + writeOffset) % 4 == 0 && rem >= 4) { + // All accesses of this kind are valid + vgicWriteGicdRegister(*(u32 *)((uintptr_t)writeBuf + writeOffset), offset + writeOffset, 4); + writeOffset += 4; + rem -= 4; + } else if ((offset + writeOffset) % 2 == 0 && rem >= 2) { + // All accesses of this kind would be translated to ldrh and are thus invalid. Abort. + size = writeOffset; + goto end; + } else if (vgicValidateGicdRegisterAccess(offset + writeOffset, 1)) { + // Valid byte access + vgicWriteGicdRegister(*(u32 *)((uintptr_t)writeBuf + writeOffset), offset + writeOffset, 1); + writeOffset += 1; + rem -= 1; + } else { + // Invalid byte access + size = writeOffset; + goto end; + } + } + } + +end: + recursiveSpinlockUnlock(&g_irqManager.lock); + return size; +} + +static size_t guestReadWriteDeviceMemory(void *addr, size_t size, void *readBuf, const void *writeBuf) +{ + // We might trigger bus errors... ignore the exception and return early if that's the case + + CoreCtx *curCtxBackup = currentCoreCtx; + __compiler_barrier(); + currentCoreCtx = NULL; + __compiler_barrier(); + + uintptr_t addri = (uintptr_t)addr; + + if (readBuf != NULL) { + size_t readOffset = 0; + size_t rem = size; + while (rem > 0 && (__compiler_barrier(), currentCoreCtx == NULL)) { + if ((addri + readOffset) % 4 == 0 && rem >= 4) { + *(vu32 *)((uintptr_t)readBuf + readOffset) = *(vu32 *)(addri + readOffset); + readOffset += 4; + rem -= 4; + } else if (readOffset % 2 == 0 && rem >= 2) { + *(vu16 *)((uintptr_t)readBuf + readOffset) = *(vu16 *)(addri + readOffset); + readOffset += 2; + rem -= 2; + } else { + *(vu8 *)((uintptr_t)readBuf + readOffset) = *(vu8 *)(addri + readOffset); + readOffset += 1; + rem -= 1; + } + } + if (rem != 0) { + size = readOffset; + goto end; + } + } + + if (writeBuf != NULL) { + size_t writeOffset = 0; + size_t rem = size; + while (rem > 0 && (__compiler_barrier(), currentCoreCtx == NULL)) { + if ((addri + writeOffset) % 4 == 0 && rem >= 4) { + *(vu32 *)(addri + writeOffset) = *(vu32 *)((uintptr_t)writeBuf + writeOffset); + writeOffset += 4; + rem -= 4; + } else if (writeOffset % 2 == 0 && rem >= 2) { + *(vu16 *)(addri + writeOffset) = *(vu16 *)((uintptr_t)writeBuf + writeOffset); + writeOffset += 2; + rem -= 2; + } else { + *(vu8 *)(addri + writeOffset) = *(vu8 *)((uintptr_t)writeBuf + writeOffset); + writeOffset += 1; + rem -= 1; + } + } + if (rem != 0) { + size = writeOffset; + goto end; + } + } + +end: + __compiler_barrier(); + currentCoreCtx = curCtxBackup; + __compiler_barrier(); + return size; +} + +static size_t guestReadWriteNormalMemory(void *addr, size_t size, void *readBuf, const void *writeBuf) +{ + if (readBuf != NULL) { + memcpy(readBuf, addr, size); + } + + if (writeBuf != NULL) { + memcpy(addr, writeBuf, size); + + // We may have written to executable memory or to translation tables... + // & the page may have various aliases. + // We need to ensure cache & TLB coherency. + cacheCleanDataCacheRangePoU(addr, size); + u32 policy = cacheGetInstructionCachePolicy(); + if (policy == 1 || policy == 2) { + // AVIVT, VIVT + cacheInvalidateInstructionCache(); + } else { + // VPIPT, PIPT + // Ez coherency, just do range operations... + cacheInvalidateInstructionCacheRangePoU(addr, size); + } + __tlb_invalidate_el1(); + __dsb(); + __isb(); + } + + return size; +} + +static size_t guestReadWriteMemoryPage(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf) +{ + u64 irqFlags = maskIrq(); + size_t offset = addr & 0xFFFull; + + // Translate the VA, stages 1&2 + __asm__ __volatile__ ("at s12e1r, %0" :: "r"(addr) : "memory"); + u64 par = GET_SYSREG(par_el1); + if (par & PAR_F) { + // The translation failed. Why? + if (par & PAR_S) { + // Stage 2 fault. Could be an attempt to access the GICD, let's see what the IPA is... + __asm__ __volatile__ ("at s1e1r, %0" :: "r"(addr) : "memory"); + par = GET_SYSREG(par_el1); + if ((par & PAR_F) != 0 || (par & PAR_PA_MASK) != MEMORY_MAP_VA_GICD) { + // The guest doesn't have access to it... + // Read as 0, write ignored + if (readBuf != NULL) { + memset(readBuf, 0, size); + } + } else { + // GICD mmio + size = guestReadWriteGicd(offset, size, readBuf, writeBuf); + } + } else { + // Oops, couldn't read/write anything (stage 1 fault) + size = 0; + } + } else { + /* + Translation didn't fail. + + To avoid "B2.8 Mismatched memory attributes" we must use the same effective + attributes & shareability as the guest. + + Note that par_el1 reports the effective shareablity of device and noncacheable memory as inner shareable. + In fact, the VMSAv8-64 section in the Armv8 ARM reads: + "The shareability field is only relevant if the memory is a Normal Cacheable memory type. All Device and Normal + Non-cacheable memory regions are always treated as Outer Shareable, regardless of the translation table + shareability attributes." + + There's one corner case where we can't avoid it: another core is running, + changes the attributes (other than permissions) of the page, and issues + a broadcasting TLB maintenance instructions and/or accesses the page with the altered + attribute itself. We don't handle this corner case -- just don't read/write that kind of memory... + */ + u64 memAttribs = (par >> PAR_ATTR_SHIFT) & PAR_ATTR_MASK; + u32 shrb = (par >> PAR_SH_SHIFT) & PAR_SH_MASK; + uintptr_t pa = par & PAR_PA_MASK; + uintptr_t va = MEMORY_MAP_VA_GUEST_MEM + 0x2000 * currentCoreCtx->coreId; + + u64 mair = GET_SYSREG(mair_el2); + mair |= memAttribs << (8 * MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT); + SET_SYSREG(mair_el2, mair); + __isb(); + + u64 attribs = MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_SH(shrb) | MMU_PTE_BLOCK_MEMTYPE(MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT); + mmu_map_page((uintptr_t *)MEMORY_MAP_VA_TTBL, va, pa, attribs); + // Note: no need to broadcast here + __tlb_invalidate_el2_page_local(pa); + __dsb_local(); + + void *vaddr = (void *)(va + offset); + if (memAttribs & 0xF0) { + // Normal memory, or unpredictable + size = guestReadWriteNormalMemory(vaddr, size, readBuf, writeBuf); + } else { + // Device memory, or unpredictable + size = guestReadWriteDeviceMemory(vaddr, size, readBuf, writeBuf); + } + + __dsb_local(); + __isb(); + mmu_unmap_page((uintptr_t *)MEMORY_MAP_VA_TTBL, va); + // Note: no need to broadcast here + __tlb_invalidate_el2_page_local(pa); + __dsb_local(); + + mair &= ~(0xFFul << (8 * MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT)); + SET_SYSREG(mair_el2, mair); + __isb(); + } + + restoreInterruptFlags(irqFlags); + return size; +} + +size_t guestReadWriteMemory(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf) +{ + uintptr_t curAddr = addr; + size_t remainingAmount = size; + u8 *rb8 = (u8 *)readBuf; + const u8 *wb8 = (const u8*)writeBuf; + while (remainingAmount > 0) { + size_t expectedAmount = ((curAddr & ~0xFFFul) + 0x1000) - curAddr; + expectedAmount = expectedAmount > remainingAmount ? remainingAmount : expectedAmount; + size_t actualAmount = guestReadWriteMemoryPage(curAddr, expectedAmount, rb8, wb8); + curAddr += actualAmount; + rb8 += actualAmount; + wb8 += actualAmount; + remainingAmount -= actualAmount; + if (actualAmount != expectedAmount) { + break; + } + } + return curAddr - addr; +} diff --git a/thermosphere/src/guest_memory.h b/thermosphere/src/guest_memory.h new file mode 100644 index 000000000..4b1ee34a1 --- /dev/null +++ b/thermosphere/src/guest_memory.h @@ -0,0 +1,31 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "utils.h" + +size_t guestReadWriteMemory(uintptr_t addr, size_t size, void *readBuf, const void *writeBuf); + +static inline size_t guestReadMemory(uintptr_t addr, size_t size, void *buf) +{ + return guestReadWriteMemory(addr, size, buf, NULL); +} + +static inline size_t guestWriteMemory(uintptr_t addr, size_t size, void *buf) +{ + return guestReadWriteMemory(addr, size, NULL, buf); +} diff --git a/thermosphere/src/initSystem.c b/thermosphere/src/initSystem.c index ded60bbf9..3ff65a8f2 100644 --- a/thermosphere/src/initSystem.c +++ b/thermosphere/src/initSystem.c @@ -41,7 +41,7 @@ static void initSysregs(void) SET_SYSREG(cntp_ctl_el0, 0x00000000); SET_SYSREG(cntv_ctl_el0, 0x00000000); - __dsb(); + __dsb_local(); __isb(); } diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index a09ca738d..723aeda1f 100644 --- a/thermosphere/src/main.c +++ b/thermosphere/src/main.c @@ -14,6 +14,7 @@ #include "timer.h" #include "irq.h" #include "transport_interface.h" +#include "guest_memory.h" #include "memory_map.h" #include "mmu.h" @@ -32,8 +33,8 @@ static void loadKernelViaSemihosting(void) MMU_PTE_BLOCK_XN | MMU_PTE_BLOCK_INNER_SHAREBLE | MMU_PTE_BLOCK_MEMTYPE(MEMORY_MAP_MEMTYPE_NORMAL_UNCACHEABLE) ); - __tlb_invalidate_el2(); - __dsb(); + __tlb_invalidate_el2_local(); + __dsb_local(); DEBUG("Loading kernel via semihosted file I/O... "); handle = semihosting_file_open("test_kernel.bin", FOPEN_MODE_RB); @@ -49,8 +50,8 @@ static void loadKernelViaSemihosting(void) semihosting_file_close(handle); mmu_unmap_range(1, mmuTable, 0x40000000, 0x40000000); - __tlb_invalidate_el2(); - __dsb(); + __tlb_invalidate_el2_local(); + __dsb_local(); currentCoreCtx->kernelEntrypoint = buf; } diff --git a/thermosphere/src/memory_map.c b/thermosphere/src/memory_map.c index 07cac2407..d69a98b58 100644 --- a/thermosphere/src/memory_map.c +++ b/thermosphere/src/memory_map.c @@ -143,20 +143,20 @@ void memoryMapEnableMmu(const LoadImageLayout *layout) SET_SYSREG(ttbr0_el2, mmuTable); SET_SYSREG(tcr_el2, tcr); SET_SYSREG(mair_el2, mair); - __dsb(); + __dsb_local(); __isb(); // TLB invalidation // Whether this does anything before MMU is enabled is impldef, apparently __tlb_invalidate_el2_local(); - __dsb(); + __dsb_local(); __isb(); // Enable MMU & enable caching. We will crash. u64 sctlr = GET_SYSREG(sctlr_el2); sctlr |= SCTLR_ELx_I | SCTLR_ELx_C | SCTLR_ELx_M; SET_SYSREG(sctlr_el2, sctlr); - __dsb(); + __dsb_local(); __isb(); } diff --git a/thermosphere/src/memory_map.h b/thermosphere/src/memory_map.h index ec8c448e7..d61f615a1 100644 --- a/thermosphere/src/memory_map.h +++ b/thermosphere/src/memory_map.h @@ -22,6 +22,7 @@ #define MEMORY_MAP_MEMTYPE_NORMAL 1ul #define MEMORY_MAP_MEMTYPE_DEVICE_NGNRE 2ul #define MEMORY_MAP_MEMTYPE_NORMAL_UNCACHEABLE 3ul +#define MEMORY_MAP_MEMTYPE_NORMAL_GUEST_SLOT 4ul #define MEMORY_MAP_VA_SPACE_SIZE 39ul diff --git a/thermosphere/src/mmu.h b/thermosphere/src/mmu.h index 3e8fcc87a..1e1bbcc50 100644 --- a/thermosphere/src/mmu.h +++ b/thermosphere/src/mmu.h @@ -72,6 +72,7 @@ */ #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) @@ -194,4 +195,4 @@ static inline void mmu_unmap_range(unsigned int level, uintptr_t *tbl, uintptr_t for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) { mmu_unmap(level, tbl, base_addr + offset); } -} \ No newline at end of file +} diff --git a/thermosphere/src/platform/qemu/devices.c b/thermosphere/src/platform/qemu/devices.c index 20c766507..ff560ed69 100644 --- a/thermosphere/src/platform/qemu/devices.c +++ b/thermosphere/src/platform/qemu/devices.c @@ -26,6 +26,6 @@ void devicesMapAllExtra(void) // Don't broadcast, since it's only ran once per boot by only one core, before the others are started... __tlb_invalidate_el2_local(); - __dsb(); + __dsb_local(); __isb(); } diff --git a/thermosphere/src/platform/stage2.c b/thermosphere/src/platform/stage2.c index c69c46dcb..8e95d2dad 100644 --- a/thermosphere/src/platform/stage2.c +++ b/thermosphere/src/platform/stage2.c @@ -39,18 +39,18 @@ void stage2ConfigureAndEnable(void) // Stage2 regs config SET_SYSREG(vttbr_el2, vttbr); SET_SYSREG(vtcr_el2, vtcr); - __dsb(); + __dsb_local(); __isb(); // Enable stage 2 u64 hcr = GET_SYSREG(hcr_el2); hcr |= HCR_VM; SET_SYSREG(hcr_el2, hcr); - __dsb(); + __dsb_local(); __isb(); // TLB invalidation __tlb_invalidate_el1_stage12_local(); - __dsb(); + __dsb_local(); __isb(); } diff --git a/thermosphere/src/platform/tegra/devices.c b/thermosphere/src/platform/tegra/devices.c index 96997e61d..228704bb6 100644 --- a/thermosphere/src/platform/tegra/devices.c +++ b/thermosphere/src/platform/tegra/devices.c @@ -33,6 +33,6 @@ void devicesMapAllExtra(void) // Don't broadcast, since it's only ran once per boot by only one core, before the others are started... __tlb_invalidate_el2_local(); - __dsb(); + __dsb_local(); __isb(); } diff --git a/thermosphere/src/smc.c b/thermosphere/src/smc.c index 152f8b629..9226e52a6 100644 --- a/thermosphere/src/smc.c +++ b/thermosphere/src/smc.c @@ -16,6 +16,8 @@ void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId) cacheHandleSelfModifyingCodePoU(codebuf, doSmcIndirectCallImplSize/4); + __dsb_sy(); + __isb(); ((void (*)(ExceptionStackFrame *))codebuf)(frame); } diff --git a/thermosphere/src/software_breakpoints.c b/thermosphere/src/software_breakpoints.c index 6c5cc9e6c..191dbe645 100644 --- a/thermosphere/src/software_breakpoints.c +++ b/thermosphere/src/software_breakpoints.c @@ -62,10 +62,10 @@ static inline bool doApplySoftwareBreakpoint(size_t id) u32 brkInst = 0xF2000000 | bp->uid; - if (readEl1Memory(&bp->savedInstruction, bp->address, 4) && writeEl1Memory(bp->address, &brkInst, 4)) { + /*if (readEl1Memory(&bp->savedInstruction, bp->address, 4) && writeEl1Memory(bp->address, &brkInst, 4)) { bp->applied = true; return true; - } + }*/ return false; } @@ -91,10 +91,10 @@ static inline bool doRevertSoftwareBreakpoint(size_t id) return true; } - if (writeEl1Memory(bp->address, &bp->savedInstruction, 4)) { + /*if (writeEl1Memory(bp->address, &bp->savedInstruction, 4)) { bp->applied = false; return true; - } + }*/ return false; } diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s index 0751c97fe..ae9249177 100644 --- a/thermosphere/src/start.s +++ b/thermosphere/src/start.s @@ -52,6 +52,10 @@ _startCommon: // "Boot core only" stuff: bl cacheClearSharedDataCachesOnBoot + ic iallu + dsb nsh + isb + // Temporarily use temp end region as stack, then create the translation table // The stack top is also equal to the mmu table address... adr x0, g_loadImageLayout @@ -89,9 +93,6 @@ _postMmuEnableReturnAddr: stp x18, xzr, [sp, #-0x10]! sub sp, sp, #EXCEP_STACK_FRAME_SIZE - dsb sy - isb - mov x0, sp mov x1, x20 bl thermosphereMain diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h index 242e05f21..43617ef8d 100644 --- a/thermosphere/src/sysreg.h +++ b/thermosphere/src/sysreg.h @@ -412,13 +412,28 @@ #define MDSCR_SS BITL(0) // Common CNTHCTL_EL2 flags -#define CNTHCTL_EVNTI_MASK 0xFll +#define CNTHCTL_EVNTI_MASK 0xFul #define CNTHCTL_EVNTI_SHIFT 4 #define CNTHCTL_EVNTDIR BITL(3) #define CNTHCTL_EVNTEN BITL(2) #define CNTHCTL_EL1PCEN BITL(1) #define CNTHCTL_EL1PCTEN BITL(0) +// PAR_EL1 +#define PAR_F BITL(0) +// Successful translation: +#define PAR_ATTR_SHIFT 56 +#define PAR_ATTR_MASK 0xFFul +#define PAR_PA_MASK MASK2L(51, 12) // bits 51-48 RES0 if not implemented +#define PAR_NS BITL(9) +#define PAR_SH_SHIFT 7 +#define PAR_SH_MASK 3ul +// Faulting translation: +#define PAR_S BITL(9) +#define PAR_PTW BITL(8) +#define PAR_FST_SHIFT 1 +#define PAR_FST_MASK 0x3Ful + #define ENCODE_SYSREG_FIELDS_MOV(op0, op1, crn, crm, op2) (((op0) << 19) | ((op1) << 16) | ((crn) << 12) | ((crm) << 8) | ((op2) << 5)) #define ENCODE_SYSREG_MOV(name) EVAL(ENCODE_SYSREG_FIELDS_MOV CAT(TUP_, name)) #define MAKE_MSR(name, Rt) (0xD5000000 | ENCODE_SYSREG_MOV(name) | ((Rt) & 0x1F)) diff --git a/thermosphere/src/utils.c b/thermosphere/src/utils.c index ebdfd45f0..823e1b6b9 100644 --- a/thermosphere/src/utils.c +++ b/thermosphere/src/utils.c @@ -27,44 +27,3 @@ __attribute__((noinline)) bool overlaps(u64 as, u64 ae, u64 bs, u64 be) return true; return false; } - -// TODO: put that elsewhere -bool readEl1Memory(void *dst, uintptr_t addr, size_t size) -{ - // Note: what if we read uncached regions/not shared? - bool valid; - - u64 flags = maskIrq(); - uintptr_t pa = get_physical_address_el1_stage12(&valid, addr); - restoreInterruptFlags(flags); - - if (!valid) { - return false; - } - - memcpy(dst, (const void *)pa, size); - - return true; -} - -bool writeEl1Memory(uintptr_t addr, const void *src, size_t size) -{ - bool valid; - - u64 flags = maskIrq(); - uintptr_t pa = get_physical_address_el1_stage12(&valid, addr); - restoreInterruptFlags(flags); - - if (!valid) { - return false; - } - - memcpy((void *)pa, src, size); - cacheHandleSelfModifyingCodePoU((const void *)pa, size); - - __tlb_invalidate_el1_stage12_local(); //FIXME FIXME FIXME - __dsb_sy(); - __isb(); - - return true; -} diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h index 01803ad9e..7ea9f4bf0 100644 --- a/thermosphere/src/utils.h +++ b/thermosphere/src/utils.h @@ -62,6 +62,11 @@ typedef enum ReadWriteDirection { DIRECTION_READWRITE = DIRECTION_READ | DIRECTION_WRITE, } ReadWriteDirection; +static inline void __compiler_barrier(void) +{ + __asm__ __volatile__ ("" ::: "memory"); +} + static inline void __wfi(void) { __asm__ __volatile__ ("wfi" ::: "memory"); @@ -100,6 +105,11 @@ static inline void __dsb(void) __asm__ __volatile__ ("dsb ish" ::: "memory"); } +static inline void __dsb_local(void) +{ + __asm__ __volatile__ ("dsb nsh" ::: "memory"); +} + static inline void __dmb_sy(void) { __asm__ __volatile__ ("dmb sy" ::: "memory"); @@ -125,11 +135,27 @@ static inline void __tlb_invalidate_el2_local(void) __asm__ __volatile__ ("tlbi alle2" ::: "memory"); } +static inline void __tlb_invalidate_el2_page(uintptr_t addr) +{ + __asm__ __volatile__ ("tlbi vae2is, %0" :: "r"(addr) : "memory"); +} + +static inline void __tlb_invalidate_el2_page_local(uintptr_t addr) +{ + __asm__ __volatile__ ("tlbi vae2is, %0" :: "r"(addr) : "memory"); +} + + static inline void __tlb_invalidate_el1_stage12_local(void) { __asm__ __volatile__ ("tlbi alle1" ::: "memory"); } +static inline void __tlb_invalidate_el1(void) +{ + __asm__ __volatile__ ("tlbi vmalle1is" ::: "memory"); +} + bool overlaps(u64 as, u64 ae, u64 bs, u64 be); // Assumes addr is valid, must be called with interrupts masked @@ -138,23 +164,11 @@ static inline uintptr_t va2pa(const void *el2_vaddr) { // For debug purposes only uintptr_t PAR; uintptr_t va = (uintptr_t)el2_vaddr; - __asm__ __volatile__ ("at s1e2r, %0" :: "r"(va)); - __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR)); + __asm__ __volatile__ ("at s1e2r, %0" :: "r"(va) : "memory"); + __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR) :: "memory"); return (PAR & MASK2L(47, 12)) | (va & MASKL(12)); } -static inline uintptr_t get_physical_address_el1_stage12(bool *valid, uintptr_t el1_vaddr) { - // NOTE: interrupt must be disabled when calling this func - uintptr_t PAR; - __asm__ __volatile__ ("at s12e1r, %0" :: "r"(el1_vaddr)); // note: we don't care whether it's writable in EL1&0 translation regime - __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR)); - *valid = (PAR & 1) == 0ull; - return (PAR & 1) ? 0ull : (PAR & MASK2L(47, 12)) | ((uintptr_t)el1_vaddr & MASKL(12)); -} - -bool readEl1Memory(void *dst, uintptr_t addr, size_t size); -bool writeEl1Memory(uintptr_t addr, const void *src, size_t size); - static inline void panic(void) { #ifndef PLATFORM_QEMU __builtin_trap(); diff --git a/thermosphere/src/vgic.c b/thermosphere/src/vgic.c index f203abcab..6be16bf46 100644 --- a/thermosphere/src/vgic.c +++ b/thermosphere/src/vgic.c @@ -526,12 +526,23 @@ static inline u32 vgicGetPeripheralId2Register(void) return 2 << 4; } -static void handleVgicMmioWrite(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset) +bool vgicValidateGicdRegisterAccess(size_t offset, size_t sz) { - size_t sz = BITL(dabtIss.sas); - u32 val = (u32)(readFrameRegisterZ(frame, dabtIss.srt) & MASKL(8 * sz)); - uintptr_t addr = (uintptr_t)gicd + offset; + // ipriorityr, itargetsr, *pendsgir are byte-accessible + if ( + !(offset >= GICDOFF(ipriorityr) && offset < GICDOFF(ipriorityr) + GIC_IRQID_MAX) && + !(offset >= GICDOFF(itargetsr) && offset < GICDOFF(itargetsr) + GIC_IRQID_MAX) && + !(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 vgicWriteGicdRegister(u32 val, size_t offset, size_t sz) +{ //DEBUG("gicd write off 0x%03llx sz %lx val %x w%d\n", offset, sz, val, (int)dabtIss.srt); switch (offset) { @@ -607,19 +618,15 @@ static void handleVgicMmioWrite(ExceptionStackFrame *frame, DataAbortIss dabtIss break; default: - dumpUnhandledDataAbort(dabtIss, addr, "GICD reserved/implementation-defined register"); + DEBUG("Write to GICD reserved/implementation-defined register offset=0x%03lx value=0x%08lx", offset, val); break; } } -static void handleVgicMmioRead(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset) +u32 vgicReadGicdRegister(size_t offset, size_t sz) { - size_t sz = BITL(dabtIss.sas); - uintptr_t addr = (uintptr_t)gicd + offset; - - u32 val = 0; - //DEBUG("gicd read off 0x%03llx sz %lx\n", offset, sz); + u32 val = 0; switch (offset) { case GICDOFF(icfgr) ... GICDOFF(icfgr) + 31/4: @@ -682,7 +689,7 @@ static void handleVgicMmioRead(ExceptionStackFrame *frame, DataAbortIss dabtIss, case GICDOFF(sgir): // Write-only register - dumpUnhandledDataAbort(dabtIss, addr, "GICD read to write-only register GCID_SGIR"); + DEBUG("Read from write-only register GCID_SGIR\n"); break; case GICDOFF(icpidr2): @@ -690,10 +697,24 @@ static void handleVgicMmioRead(ExceptionStackFrame *frame, DataAbortIss dabtIss, break; default: - dumpUnhandledDataAbort(dabtIss, addr, "GICD reserved/implementation-defined register"); + DEBUG("Read from GICD reserved/implementation-defined register offset=0x%03lx\n", offset); break; } + return val; +} + +static void handleVgicMmioWrite(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset) +{ + size_t sz = BITL(dabtIss.sas); + u32 val = (u32)(readFrameRegisterZ(frame, dabtIss.srt) & MASKL(8 * sz)); + vgicWriteGicdRegister(val, offset, sz); +} + +static void handleVgicMmioRead(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset) +{ + size_t sz = BITL(dabtIss.sas); + u32 val = vgicReadGicdRegister(offset, sz); writeFrameRegisterZ(frame, dabtIss.srt, val); } @@ -993,28 +1014,10 @@ void handleVgicdMmio(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t of { size_t sz = BITL(dabtIss.sas); uintptr_t addr = (uintptr_t)gicd + offset; - bool oops = true; + bool oops = !vgicValidateGicdRegisterAccess(offset, sz); - // ipriorityr, itargetsr, *pendsgir are byte-accessible - if ( - !(offset >= GICDOFF(ipriorityr) && offset < GICDOFF(ipriorityr) + GIC_IRQID_MAX) && - !(offset >= GICDOFF(itargetsr) && offset < GICDOFF(itargetsr) + GIC_IRQID_MAX) && - !(offset >= GICDOFF(cpendsgir) && offset < GICDOFF(cpendsgir) + 16) && - !(offset >= GICDOFF(spendsgir) && offset < GICDOFF(spendsgir) + 16) - ) { - if ((offset & 3) != 0 || sz != 4) { - dumpUnhandledDataAbort(dabtIss, addr, "GICD non-word aligned MMIO"); - } else { - oops = false; - } - } else { - if (sz != 1 && sz != 4) { - dumpUnhandledDataAbort(dabtIss, addr, "GICD 16 or 64-bit access"); - } else if (sz == 4 && (offset & 3) != 0) { - dumpUnhandledDataAbort(dabtIss, addr, "GICD misaligned MMIO"); - } else { - oops = false; - } + if (oops) { + dumpUnhandledDataAbort(dabtIss, addr, "invalid GICD register access"); } recursiveSpinlockLock(&g_irqManager.lock); diff --git a/thermosphere/src/vgic.h b/thermosphere/src/vgic.h index f816ac74e..57402e25c 100644 --- a/thermosphere/src/vgic.h +++ b/thermosphere/src/vgic.h @@ -19,6 +19,10 @@ #include "types.h" #include "data_abort.h" +bool vgicValidateGicdRegisterAccess(size_t offset, size_t sz); +void vgicWriteGicdRegister(u32 val, size_t offset, size_t sz); +u32 vgicReadGicdRegister(size_t offset, size_t sz); + void handleVgicdMmio(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset); void vgicInit(void);