diff --git a/thermosphere/src/barrier.c b/thermosphere/src/barrier.c new file mode 100644 index 000000000..36cead568 --- /dev/null +++ b/thermosphere/src/barrier.c @@ -0,0 +1,41 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include "barrier.h" +#include "core_ctx.h" +#include "utils.h" + + +void barrierInit(Barrier *barrier, u32 coreList) +{ + atomic_store(&barrier->val, coreList); +} + +void barrierInitAllButSelf(Barrier *barrier) +{ + barrierInit(barrier, getActiveCoreMask() & ~(BIT(currentCoreCtx->coreId))); +} + +void barrierInitAll(Barrier *barrier) +{ + barrierInit(barrier, getActiveCoreMask()); +} + +void barrierWait(Barrier *barrier) +{ + while (atomic_fetch_and(&barrier->val, ~(BIT(currentCoreCtx->coreId))) != 0); +} diff --git a/thermosphere/src/barrier.h b/thermosphere/src/barrier.h new file mode 100644 index 000000000..cf818f8a3 --- /dev/null +++ b/thermosphere/src/barrier.h @@ -0,0 +1,30 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include "types.h" + +typedef struct Barrier { + atomic_uint val; +} Barrier; + +void barrierInit(Barrier *barrier, u32 coreList); +void barrierInitAllButSelf(Barrier *barrier); +void barrierInitAll(Barrier *barrier); + +void barrierWait(Barrier *barrier); \ No newline at end of file diff --git a/thermosphere/src/core_ctx.c b/thermosphere/src/core_ctx.c index 60f2bcb67..c4d417a13 100644 --- a/thermosphere/src/core_ctx.c +++ b/thermosphere/src/core_ctx.c @@ -15,12 +15,15 @@ */ #include "core_ctx.h" +#include "utils.h" // start.s extern uintptr_t g_initialKernelEntrypoint; extern u8 __stacks_top__[], __crash_stacks_top__[]; +static atomic_uint g_activeCoreMask = 0; + // Prevents it from being put in BSS CoreCtx g_coreCtxs[4] = { { .coreId = 0 }, @@ -40,3 +43,13 @@ void coreCtxInit(u32 coreId, bool isBootCore, u64 argument) currentCoreCtx->kernelEntrypoint = g_initialKernelEntrypoint; } } + +void setCurrentCoreActive(void) +{ + atomic_fetch_or(&g_activeCoreMask, BIT(currentCoreCtx->coreId)); +} + +u32 getActiveCoreMask(void) +{ + return (u32)atomic_load(&g_activeCoreMask); +} diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h index d37fd83a1..8da5fac3a 100644 --- a/thermosphere/src/core_ctx.h +++ b/thermosphere/src/core_ctx.h @@ -16,6 +16,7 @@ #pragma once #include "utils.h" +#include "barrier.h" typedef struct CoreCtx { u64 kernelArgument; // @0x00 @@ -26,9 +27,16 @@ typedef struct CoreCtx { u8 gicInterfaceId; // @0x24 bool isBootCore; // @0x25 bool warmboot; // @0x26 + + // "Execute function" + void *asyncFunctionArgs; // @0x28 + Barrier asyncFunctionBarrier; // @0x30 } CoreCtx; extern CoreCtx g_coreCtxs[4]; register CoreCtx *currentCoreCtx asm("x18"); void coreCtxInit(u32 coreId, bool isBootCore, u64 argument); + +void setCurrentCoreActive(void); +u32 getActiveCoreMask(void); diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c index 8e936f1b1..48a09cd0f 100644 --- a/thermosphere/src/irq.c +++ b/thermosphere/src/irq.c @@ -37,9 +37,7 @@ static void initGic(void) g_irqManager.gic.gicd->ipriorityr[0] = 0xFF; g_irqManager.numPriorityLevels = (u8)BIT(__builtin_popcount(g_irqManager.gic.gicd->ipriorityr[0])); - // same thing for CPU interfaces targets (save for ITARGETSR registers corresponding to SGIs) - g_irqManager.gic.gicd->itargetsr[32] = 0xFF; - g_irqManager.numCpuInterfaces = (u8)__builtin_popcount(g_irqManager.gic.gicd->itargetsr[32]); + g_irqManager.numCpuInterfaces = (u8)(1 + ((g_irqManager.gic.gicd->typer >> 5) & 7)); } volatile ArmGicV2Controller *gicc = g_irqManager.gic.gicc; @@ -60,12 +58,11 @@ static void initGic(void) // Note: the GICD I...n regs are banked for private interrupts - // Disable all interrupts, clear active status, clear pending status, also clear secure regs igroupr to be sure + // Disable all interrupts, clear active status, clear pending status for (u32 i = 0; i < numInterrupts / 32; i++) { gicd->icenabler[i] = 0xFFFFFFFF; gicd->icactiver[i] = 0xFFFFFFFF; gicd->icpendr[i] = 0xFFFFFFFF; - gicd->igroupr[i] = 0x00000000; } // Set priorities to lowest diff --git a/thermosphere/src/irq.h b/thermosphere/src/irq.h index 0f21ebb7b..2578c677d 100644 --- a/thermosphere/src/irq.h +++ b/thermosphere/src/irq.h @@ -30,27 +30,33 @@ typedef struct IrqManager { // Note: we don't store interrupt handlers since we will handle some SGI + uart interrupt(s)... } IrqManager; +typedef enum ThermosphereSgi { + ThermosphereSgi_ExecuteFunction = 0, + + ThermosphereSgi_Max = 1, +} ThermosphereSgi; + extern IrqManager g_irqManager; void initIrq(void); void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32); -static inline void generateSgiForAllOthers(u32 id) +static inline void generateSgiForAllOthers(ThermosphereSgi id) { - g_irqManager.gic.gicd->sgir = (1 << 24) | (id & 0xF); + g_irqManager.gic.gicd->sgir = (1 << 24) | ((u32)id & 0xF); } -static inline void generateSgiForSelf(u32 id) +static inline void generateSgiForSelf(ThermosphereSgi id) { - g_irqManager.gic.gicd->sgir = (2 << 24) | (id & 0xF); + g_irqManager.gic.gicd->sgir = (2 << 24) | ((u32)id & 0xF); } -static inline void generateSgiForList(u32 id, u32 list) +static inline void generateSgiForList(ThermosphereSgi id, u32 list) { - g_irqManager.gic.gicd->sgir = (0 << 24) | (list << 16) | (id & 0xF); + g_irqManager.gic.gicd->sgir = (0 << 24) | (list << 16) | ((u32)id & 0xF); } -static inline void generateSgiForAll(u32 id) +static inline void generateSgiForAll(ThermosphereSgi id) { generateSgiForList(id, MASK(g_irqManager.numCpuInterfaces)); } diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index c1b9b3895..1db76a20a 100644 --- a/thermosphere/src/main.c +++ b/thermosphere/src/main.c @@ -75,10 +75,12 @@ void main(ExceptionStackFrame *frame) frame->elr_el2 = currentCoreCtx->kernelEntrypoint; frame->x[0] = currentCoreCtx->kernelArgument; + setCurrentCoreActive(); + // Test singleStepSetNextState(frame, SingleStepState_ActivePending); - // Test + /*// Test unmaskIrq(); - generateSgiForAll(0); + generateSgiForAll(0);*/ }