From d56185e432e8a4190e14cd11f29f4c9357642d03 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Sun, 5 Jan 2020 21:54:02 +0000
Subject: [PATCH] thermosphere: make the pending virq list ordering stable

---
 thermosphere/src/utils.h | 16 ++++++-
 thermosphere/src/vgic.c  | 94 +++++++++++++++++++++++++++++++---------
 2 files changed, 88 insertions(+), 22 deletions(-)

diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h
index 4ed7ef1e6..8f3e07693 100644
--- a/thermosphere/src/utils.h
+++ b/thermosphere/src/utils.h
@@ -36,9 +36,23 @@
 
 #define TEMPORARY       __attribute__((section(".tempbss")))
 
+#define PANIC(...)      do { DEBUG(__VA_ARGS__); panic(); } while (false)
+
 #define FOREACH_BIT(tmpmsk, var, word) for (u64 tmpmsk = (word), var = __builtin_ctzll(word); tmpmsk != 0; tmpmsk &= ~BITL(var), var = __builtin_ctzll(tmpmsk))
 
-#define PANIC(...)      do { DEBUG(__VA_ARGS__); panic(); } while (false)
+#define _DECLARE_ASM_ARITHMETIC_UNARY_HELPER(sz, regalloc, op)\
+static inline u##sz __##op##sz(u##sz n)\
+{\
+    u##sz res;\
+    __asm__ __volatile__ (#op " %" #regalloc "[res], %" #regalloc "[n]" : [res] "=r" (res) : [n] "r" (n));\
+    return res;\
+}
+
+#define _DECLARE_ASM_ARITHMETIC_UNARY_HELPER64(op)  _DECLARE_ASM_ARITHMETIC_UNARY_HELPER(64, x, op)
+#define _DECLARE_ASM_ARITHMETIC_UNARY_HELPER32(op)  _DECLARE_ASM_ARITHMETIC_UNARY_HELPER(32, w, op)
+
+_DECLARE_ASM_ARITHMETIC_UNARY_HELPER64(rbit)
+_DECLARE_ASM_ARITHMETIC_UNARY_HELPER32(rbit)
 
 static inline void __dsb_sy(void)
 {
diff --git a/thermosphere/src/vgic.c b/thermosphere/src/vgic.c
index 7cd8e6e8a..e51e52139 100644
--- a/thermosphere/src/vgic.c
+++ b/thermosphere/src/vgic.c
@@ -143,31 +143,16 @@ void vgicDebugPrintLrList(void)
     DEBUG("]\n");
 }
 
-// Note: ordered by priority
-static void vgicEnqueueVirqState(VirqStateList *list, VirqState *elem)
+static inline void vgicInsertVirqStateBefore(VirqStateList *list, VirqState *pos, VirqState *elem)
 {
-    VirqState *pos;
-
-    if (vgicIsStateQueued(elem)) {
-        PANIC("vgicEnqueueVirqState: unsanitized argument idx=%u previd=%u nextid=%u\n", (u32)vgicGetVirqStateIndex(elem), elem->listPrev, elem->listNext);
-    }
-
     ++list->size;
     // Empty list
     if (list->first == vgicGetQueueEnd()) {
         list->first = list->last = elem;
         elem->listPrev = elem->listNext = VIRQLIST_END_ID;
-        //vgicDebugPrintList(list);
         return;
     }
 
-    for (pos = list->first; pos != vgicGetQueueEnd(); pos = vgicGetNextQueuedVirqState(pos)) {
-        // Lowest priority number is higher
-        if (elem->priority <= pos->priority) {
-            break;
-        }
-    }
-
     if (pos == vgicGetQueueEnd()) {
         // Insert after last
         pos = list->last;
@@ -194,7 +179,73 @@ static void vgicEnqueueVirqState(VirqStateList *list, VirqState *elem)
             prev->listNext = idx;
         }
     }
-    //vgicDebugPrintList(list);
+}
+
+// Currently unused
+static inline void vgicInsertVirqStateAfter(VirqStateList *list, VirqState *pos, VirqState *elem)
+{
+    ++list->size;
+    // Empty list
+    if (list->first == vgicGetQueueEnd()) {
+        list->first = list->last = elem;
+        elem->listPrev = elem->listNext = VIRQLIST_END_ID;
+        return;
+    }
+
+    if (pos == vgicGetQueueEnd()) {
+        // Insert before first
+        pos = list->first;
+
+        elem->listPrev = pos->listPrev;
+        elem->listNext = vgicGetVirqStateIndex(pos);
+        pos->listPrev = vgicGetVirqStateIndex(elem);
+        list->first = elem;
+    } else {
+        // Otherwise, insert after
+        u32 idx = vgicGetVirqStateIndex(elem);
+        u32 posidx = vgicGetVirqStateIndex(pos);
+
+        u32 nextidx = pos->listPrev;
+        VirqState *next = vgicGetNextQueuedVirqState(pos);
+
+        elem->listPrev = posidx;
+        elem->listNext = nextidx;
+
+        pos->listNext = idx;
+
+        if (pos == list->last) {
+            list->last = elem;
+        } else {
+            next->listPrev = idx;
+        }
+    }
+}
+
+static inline int vgicCompareVirqState(const VirqState *a, const VirqState *b)
+{
+    // Lower priority number is higher; we sort by descending priority, ie. ascending priority number
+    // Put the interrupts that were previously in the LR before the one which don't
+    int n1 = (int)(a->priority - b->priority);
+    return n1 == 0 ? (int)b->handled - (int)a->handled : n1;
+}
+
+// Note: ordered by priority
+static void vgicEnqueueVirqState(VirqStateList *list, VirqState *elem)
+{
+    VirqState *pos;
+
+    if (vgicIsStateQueued(elem)) {
+        PANIC("vgicEnqueueVirqState: unsanitized argument idx=%u previd=%u nextid=%u\n", (u32)vgicGetVirqStateIndex(elem), elem->listPrev, elem->listNext);
+    }
+
+    for (pos = list->first; pos != vgicGetQueueEnd(); pos = vgicGetNextQueuedVirqState(pos)) {
+        // Sort predicate should be stable
+        if (vgicCompareVirqState(elem, pos) < 0) {
+            break;
+        }
+    }
+
+    vgicInsertVirqStateBefore(list, pos, elem);
 }
 
 static void vgicDequeueVirqState(VirqStateList *list, VirqState *elem)
@@ -220,7 +271,6 @@ static void vgicDequeueVirqState(VirqStateList *list, VirqState *elem)
     }
 
     elem->listPrev = elem->listNext = VIRQLIST_INVALID_ID;
-    //vgicDebugPrintList(list);
 }
 
 static inline void vgicNotifyOtherCoreList(u32 coreList)
@@ -855,12 +905,14 @@ void vgicUpdateState(void)
     u32 coreId = currentCoreCtx->coreId;
 
     // First, put back inactive interrupts into the queue, handle some SGI stuff
-    u64 usedMap = g_vgicUsedLrMap[coreId];
+    // Need to handle the LRs in reverse order to keep list stability
+    u64 usedMap = __rbit64(g_vgicUsedLrMap[coreId]);
     FOREACH_BIT (tmp, pos, usedMap) {
-        if (!vgicUpdateListRegister(&gich->lr[pos])) {
-            g_vgicUsedLrMap[coreId] &= ~BITL(pos);
+        if (!vgicUpdateListRegister(&gich->lr[63 - pos])) {
+            usedMap &= ~BITL(pos);
         }
     }
+    g_vgicUsedLrMap[coreId] = __rbit64(usedMap);
 
     // Then, clean the list up
     vgicCleanupPendingList();