thermosphere: fix multiple bugs

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);
 }

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
 

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

thermosphere/src/irq.h

@@ -27,6 +27,7 @@
 typedef struct IrqManager {
     RecursiveSpinlock lock;
     ArmGicV2 gic;
+    u8 priorityShift;
     u8 numPriorityLevels;
     u8 numCpuInterfaces;
     u8 numSharedInterrupts;

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;
     }

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)
 {

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