diff --git a/thermosphere/src/exceptions.h b/thermosphere/src/exceptions.h index 66a1d3b41..99862b981 100644 --- a/thermosphere/src/exceptions.h +++ b/thermosphere/src/exceptions.h @@ -102,6 +102,19 @@ static inline void spsrSetT32ItFlags(u64 *spsr, u32 itFlags) *spsr |= ((itFlags >> 2) & 0x3F) << 10; } +static inline u64 readFrameRegisterZ(ExceptionStackFrame *frame, u32 id) +{ + return id == 31 ? 0 /* xzr */ : frame->x[id]; +} + +static inline void writeFrameRegisterZ(ExceptionStackFrame *frame, u32 id, u64 val) +{ + if (id != 31) { + // If not xzr + frame->x[id] = val; + } +} + bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode); void skipFaultingInstruction(ExceptionStackFrame *frame, u32 size); void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl); diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index ac13d9acd..a4992e184 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/thermosphere/src/sysreg_traps.c @@ -56,7 +56,7 @@ void doSystemRegisterRead(ExceptionStackFrame *frame, u32 iss, u32 reg) } doSystemRegisterRwImpl(&val, iss | 1); - frame->x[reg] = val; + writeFrameRegisterZ(frame, reg, val); skipFaultingInstruction(frame, 4); } @@ -66,7 +66,7 @@ void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg) u64 val = 0; iss &= ~((0x1F << 5) | 1); - val = frame->x[reg]; + val = readFrameRegisterZ(frame, reg); bool reevalSoftwareBreakpoints = false; @@ -126,7 +126,7 @@ void handleSysregAccessA32Stub(ExceptionStackFrame *frame, ExceptionSyndromeRegi // A32 stub: Skip instruction, read 0 if necessary (there are debug regs at EL0) if (esr.iss & 1 && evaluateMcrMrcCondition(frame->spsr_el2, (esr.iss >> 20) & 0xF, (esr.iss & BIT(24)) != 0)) { - frame->x[(esr.iss >> 5) & 0x1F] = 0; + writeFrameRegisterZ(frame, (esr.iss >> 5) & 0x1F, 0); } skipFaultingInstruction(frame, esr.il == 0 ? 2 : 4); } diff --git a/thermosphere/src/vgic.c b/thermosphere/src/vgic.c index 359fec71f..73c53a377 100644 --- a/thermosphere/src/vgic.c +++ b/thermosphere/src/vgic.c @@ -309,6 +309,7 @@ static void vgicSetInterruptPriorityByte(u16 id, u8 priority) // Nothing to do... return; } + state->priority = priority; u32 targets = g_irqManager.gic.gicd->itargetsr[id]; if (targets != 0 && vgicIsVirqPending(state)) { @@ -373,7 +374,7 @@ static inline u32 vgicGetInterruptConfigByte(u16 id, u32 config) static void vgicSetSgiPendingState(u16 id, u32 coreId, u32 srcCoreId) { u8 old = g_virqSgiPendingSources[coreId][id]; - g_virqSgiPendingSources[coreId][id] = old | srcCoreId; + g_virqSgiPendingSources[coreId][id] = old | BIT(srcCoreId); if (old == 0) { // SGI is now pending & possibly needs to be serviced VirqState *state = vgicGetVirqState(coreId, id); @@ -389,7 +390,7 @@ static void vgicClearSgiPendingState(u16 id, u32 srcCoreId) // Only for the current core, therefore no need to signal physical SGI, etc., etc. u32 coreId = currentCoreCtx->coreId; u8 old = g_virqSgiPendingSources[coreId][id]; - u8 new_ = old & ~(u8)srcCoreId; + u8 new_ = old & ~BIT((u8)srcCoreId); g_virqSgiPendingSources[coreId][id] = new_; if (old != 0 && new_ == 0) { VirqState *state = vgicGetVirqState(coreId, id); @@ -438,10 +439,10 @@ 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] & MASKL(8 * sz)); + u32 val = (u32)(readFrameRegisterZ(frame, 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); + //DEBUG("gicd write off 0x%03llx sz %lx val %x w%d\n", offset, sz, val, (int)dabtIss.srt); switch (offset) { case GICDOFF(typer): @@ -612,7 +613,7 @@ static void handleVgicMmioRead(ExceptionStackFrame *frame, DataAbortIss dabtIss, break; } - frame->x[dabtIss.srt] = val; + writeFrameRegisterZ(frame, dabtIss.srt, val); } static void vgicCleanupPendingList(void) @@ -826,7 +827,9 @@ void vgicUpdateState(void) } // Raise vIRQ when applicable. We only need to check for the highest priority - if (vgicIsInterruptRaisable(newHiPrio)) { + // TRM: "The GIC always masks an interrupt that has the largest supported priority field value. + // This provides an additional means of preventing an interrupt being signaled to any processor" + if (newHiPrio < 0x1F && vgicIsInterruptRaisable(newHiPrio)) { u32 hcr = GET_SYSREG(hcr_el2); SET_SYSREG(hcr_el2, hcr | HCR_VI); } @@ -891,7 +894,7 @@ void handleVgicdMmio(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t of handleVgicMmioRead(frame, dabtIss, offset); } - // TODO gic main loop + vgicUpdateState(); recursiveSpinlockUnlock(&g_irqManager.lock); } @@ -911,6 +914,7 @@ void vgicInit(void) for (u32 i = 0; i < 512 - 32 + 32 * 4; i++) { g_virqStates[i].listNext = g_virqStates[i].listPrev = MAX_NUM_INTERRUPTS; + g_virqStates[i].priority = 0x1F; } }