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