From e3b6d64f1b7a699805b1cf176a557400549fb90c Mon Sep 17 00:00:00 2001 From: TuxSH <1922548+TuxSH@users.noreply.github.com> Date: Mon, 23 Dec 2019 20:12:02 +0000 Subject: [PATCH] thermosphere: fix multiple bugs --- thermosphere/src/data_abort.c | 6 +++++- thermosphere/src/data_abort.h | 2 +- thermosphere/src/irq.c | 9 +++++---- thermosphere/src/irq.h | 1 + thermosphere/src/spinlock.h | 2 +- thermosphere/src/utils.h | 3 ++- thermosphere/src/vgic.c | 13 ++++++++++++- 7 files changed, 27 insertions(+), 9 deletions(-) diff --git a/thermosphere/src/data_abort.c b/thermosphere/src/data_abort.c index b2f3d93b5..62a832f33 100644 --- a/thermosphere/src/data_abort.c +++ b/thermosphere/src/data_abort.c @@ -44,7 +44,8 @@ void dumpUnhandledDataAbort(DataAbortIss dabtIss, u64 far, const char *msg) void handleLowerElDataAbortException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) { DataAbortIss dabtIss; - memcpy(&dabtIss, &esr, 4); + u32 iss = esr.iss; + memcpy(&dabtIss, &iss, 4); u64 far = GET_SYSREG(far_el2); u64 farpg = far & ~0xFFFull; @@ -60,4 +61,7 @@ void handleLowerElDataAbortException(ExceptionStackFrame *frame, ExceptionSyndro } else { dumpUnhandledDataAbort(dabtIss, far, "(fallback)"); } + + // Skip instruction anyway? + skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4); } diff --git a/thermosphere/src/data_abort.h b/thermosphere/src/data_abort.h index e5d0e5cf5..fc16bcd3b 100644 --- a/thermosphere/src/data_abort.h +++ b/thermosphere/src/data_abort.h @@ -31,7 +31,7 @@ typedef struct DataAbortIss { u32 ar : 1; // Acquire/release. Bit 14 u32 sf : 1; // 64-bit register used - u32 srt : 1; // Syndrome register transfer (register used) + u32 srt : 5; // Syndrome register transfer (register used) u32 sse : 1; // Syndrome sign extend u32 sas : 2; // Syndrome access size. Bit 23 diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c index 8ab4f1e5c..f56040848 100644 --- a/thermosphere/src/irq.c +++ b/thermosphere/src/irq.c @@ -36,6 +36,7 @@ static void initGic(void) // unimplemented priority bits (lowest significant) are RAZ/WI g_irqManager.gic.gicd->ipriorityr[0] = 0xFF; + g_irqManager.priorityShift = 8 - __builtin_popcount(g_irqManager.gic.gicd->ipriorityr[0]); g_irqManager.numPriorityLevels = (u8)BIT(__builtin_popcount(g_irqManager.gic.gicd->ipriorityr[0])); g_irqManager.numCpuInterfaces = (u8)(1 + ((g_irqManager.gic.gicd->typer >> 5) & 7)); @@ -112,7 +113,7 @@ static void configureInterrupt(u16 id, u8 prio, bool isLevelSensitive) } gicd->icpendr[id / 32] |= BIT(id % 32); - gicd->ipriorityr[id] = prio; + gicd->ipriorityr[id] = (prio << g_irqManager.priorityShift) & 0xFF; gicd->isenabler[id / 32] |= BIT(id % 32); } @@ -125,10 +126,10 @@ void initIrq(void) // Configure the interrupts we use here for (u32 i = 0; i < ThermosphereSgi_Max; i++) { - configureInterrupt(i, 0, false); + configureInterrupt(i, IRQ_PRIORITY_HOST, false); } - configureInterrupt(GIC_IRQID_MAINTENANCE, 0, true); + configureInterrupt(GIC_IRQID_MAINTENANCE, IRQ_PRIORITY_HOST, true); recursiveSpinlockUnlockRestoreIrq(&g_irqManager.lock, flags); } @@ -149,7 +150,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32) u32 irqId = iar & 0x3FF; u32 srcCore = (iar >> 12) & 7; - DEBUG("Received irq %x\n", irqId); + DEBUG("EL2: Received irq %x\n", irqId); if (irqId == GIC_IRQID_SPURIOUS) { // Spurious interrupt received diff --git a/thermosphere/src/irq.h b/thermosphere/src/irq.h index 1d9fb4813..5e157d7b8 100644 --- a/thermosphere/src/irq.h +++ b/thermosphere/src/irq.h @@ -27,6 +27,7 @@ typedef struct IrqManager { RecursiveSpinlock lock; ArmGicV2 gic; + u8 priorityShift; u8 numPriorityLevels; u8 numCpuInterfaces; u8 numSharedInterrupts; diff --git a/thermosphere/src/spinlock.h b/thermosphere/src/spinlock.h index 8fea94718..bb9c5ab41 100644 --- a/thermosphere/src/spinlock.h +++ b/thermosphere/src/spinlock.h @@ -70,7 +70,7 @@ static inline void recursiveSpinlockLock(RecursiveSpinlock *lock) if (lock->tag != currentCoreCtx->coreId + 1) { spinlockLock(&lock->lock); lock->tag = currentCoreCtx->coreId + 1; - lock->count = 0; + lock->count = 1; } else { ++lock->count; } diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h index e66e2ae83..6bff75e53 100644 --- a/thermosphere/src/utils.h +++ b/thermosphere/src/utils.h @@ -18,6 +18,7 @@ #include "types.h" #include "preprocessor.h" +#include "debug_log.h" #define BIT(n) (1u << (n)) #define BITL(n) (1ull << (n)) @@ -35,7 +36,7 @@ #define TEMPORARY __attribute__((section(".tempbss"))) -#define FOREACH_BIT(tmpmsk, var, word) for (u64 tmpmsk = (word), var = __builtin_ctzll(word); tmpmsk != 0; var = __builtin_ctzll(tmpmsk), tmpmsk &= ~BITL(var)) +#define FOREACH_BIT(tmpmsk, var, word) for (u64 tmpmsk = (word), var = __builtin_ctzll(word); tmpmsk != 0; tmpmsk &= ~BITL(var), var = __builtin_ctzll(tmpmsk)) static inline void __dsb_sy(void) { diff --git a/thermosphere/src/vgic.c b/thermosphere/src/vgic.c index c171f43a9..206070819 100644 --- a/thermosphere/src/vgic.c +++ b/thermosphere/src/vgic.c @@ -297,6 +297,11 @@ static void vgicSetInterruptPriorityByte(u16 id, u8 priority) priority >>= 3; priority &= 0x1F; + if (id >= 16) { + // Ensure we have the correct priority on the physical distributor... + g_irqManager.gic.gicd->ipriorityr[id] = IRQ_PRIORITY_GUEST << g_irqManager.priorityShift; + } + VirqState *state = vgicGetVirqState(currentCoreCtx->coreId, id); if (priority == state->priority) { // Nothing to do... @@ -415,6 +420,8 @@ static void vgicSendSgi(u16 id, u32 filter, u32 coreList) return; } + coreList &= getActiveCoreMask(); + FOREACH_BIT(tmp, dstCore, coreList) { vgicSetSgiPendingState(id, dstCore, currentCoreCtx->coreId); } @@ -429,9 +436,11 @@ static inline u32 vgicGetPeripheralId2Register(void) static void handleVgicMmioWrite(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset) { size_t sz = BITL(dabtIss.sas); - u32 val = (u32)frame->x[dabtIss.srt]; + u32 val = (u32)(frame->x[dabtIss.srt] & MASKL(8 * sz)); uintptr_t addr = (uintptr_t)g_irqManager.gic.gicd + offset; + //DEBUG("gicd write off 0x%03llx sz %lx val %x\n", offset, sz, val); + switch (offset) { case GICDOFF(typer): case GICDOFF(iidr): @@ -527,6 +536,8 @@ static void handleVgicMmioRead(ExceptionStackFrame *frame, DataAbortIss dabtIss, u32 val = 0; + //DEBUG("gicd read off 0x%03llx sz %lx\n", offset, sz); + switch (offset) { case GICDOFF(icfgr) ... GICDOFF(icfgr) + 31/4: // RAZ because of an implementation-defined choice