From fe0662a75d735804c001df164d8c5d815021ecb8 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Thu, 26 Dec 2019 00:33:38 +0000
Subject: [PATCH] vgic: fix multiple bugs

---
 thermosphere/src/gicv2.h |  2 +-
 thermosphere/src/vgic.c  | 44 +++++++++++++++++++++++++++++++++++-----
 2 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/thermosphere/src/gicv2.h b/thermosphere/src/gicv2.h
index a8de05706..21de180ac 100644
--- a/thermosphere/src/gicv2.h
+++ b/thermosphere/src/gicv2.h
@@ -99,7 +99,7 @@ typedef struct ArmGicV2MaintenanceIntStatRegister {
 } ArmGicV2MaintenanceIntStatRegister;
 
 typedef struct ArmGicV2ListRegister {
-    u32 virtualId       : 9;
+    u32 virtualId       : 10;
     u32 physicalId      : 10; // note: different encoding if hw = 0 (can't represent it in struct)
     u32 sbz2            : 3;
     u32 priority        : 5;
diff --git a/thermosphere/src/vgic.c b/thermosphere/src/vgic.c
index 73c53a377..c65401839 100644
--- a/thermosphere/src/vgic.c
+++ b/thermosphere/src/vgic.c
@@ -735,7 +735,7 @@ static inline volatile ArmGicV2ListRegister *vgicGetFreeListRegister(void)
 
 static void vgicPushListRegisters(VirqState *chosen[], size_t num)
 {
-    for (size_t i = 0; i < num; num++) {
+    for (size_t i = 0; i < num; i++) {
         VirqState *state = chosen[i];
         u16 irqId = vgicGetVirqStateInterruptId(state);
 
@@ -820,19 +820,29 @@ void vgicUpdateState(void)
 
     // Choose interrupts...
     newHiPrio = vgicChoosePendingInterrupts(&numChosen, chosen, numFreeLr);
+    (void)newHiPrio;
 
     // ...and push them
     for (size_t i = 0; i < numChosen; i++) {
         vgicPushListRegisters(chosen, numChosen);
     }
-
+/*
     // Raise vIRQ when applicable. We only need to check for the highest priority
     // 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)) {
+    if (false && newHiPrio < 0x1F && vgicIsInterruptRaisable(newHiPrio)) {
+        DEBUG("enablegrp0 %d\n", (int)gich->vmcr.enableGrp0);
+        DEBUG("enablegrp1 %d\n", (int)gich->vmcr.enableGrp1);
+        gich->hcr.npie = true;
         u32 hcr = GET_SYSREG(hcr_el2);
         SET_SYSREG(hcr_el2, hcr | HCR_VI);
+    } else {
+        //DEBUG("unraising\n");
+        gich->hcr.npie = false;
+        u32 hcr = GET_SYSREG(hcr_el2);
+        SET_SYSREG(hcr_el2, hcr & ~HCR_VI);
     }
+*/
 
     // Enable underflow interrupt when appropriate to do so
     if (vgicGetNumberOfFreeListRegisters() != g_irqManager.numListRegisters) {
@@ -844,13 +854,28 @@ void vgicUpdateState(void)
 
 void vgicMaintenanceInterruptHandler(void)
 {
-    ArmGicV2MaintenanceIntStatRegister misr = g_irqManager.gic.gich->misr;
+    volatile ArmGicV2VirtualInterfaceController *gich = g_irqManager.gic.gich;
 
+    ArmGicV2MaintenanceIntStatRegister misr = g_irqManager.gic.gich->misr;
+    DEBUG("maintenance\n");
     // 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) {
         g_irqManager.gic.gicv->ctlr &= BIT(9) | BIT(0);
     }
 
+    if (misr.vgrp0e) {
+        gich->hcr.vgrp0eie = false;
+        gich->hcr.vgrp0die = true;
+    } else if (misr.vgrp0d) {
+        gich->hcr.vgrp0eie = true;
+        gich->hcr.vgrp0die = false;
+    }
+
+    if (misr.vgrp1e) {
+        // Nothing to do since we unset the bit asap
+    }
+
     if (misr.lrenp) {
         DEBUG("VGIC: List Register Entry Not Present maintenance interrupt!");
         panic();
@@ -858,6 +883,7 @@ void vgicMaintenanceInterruptHandler(void)
 
     // The rest should be handled by the main loop...
 }
+
 void handleVgicdMmio(ExceptionStackFrame *frame, DataAbortIss dabtIss, size_t offset)
 {
     size_t sz = BITL(dabtIss.sas);
@@ -918,5 +944,13 @@ void vgicInit(void)
         }
     }
 
-    g_irqManager.gic.gich->hcr.en = true;
+    // Deassert vIRQ line, just in case
+    // Enable a few maintenance interrupts
+    ArmGicV2HypervisorControlRegister hcr = {
+        .vgrp1eie = true,
+        .vgrp0eie = true,
+        .lrenpie = true,
+        .en = true,
+    };
+    g_irqManager.gic.gich->hcr = hcr;
 }