59 lines
1.6 KiB
C
59 lines
1.6 KiB
C
|
#include <assert.h>
|
||
|
|
#include <log.h>
|
||
|
|
#include <mem.h>
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <synchronize.h>
|
||
|
|
#include <sysconfig.h>
|
||
|
|
|
||
|
|
#define MAX_CRITICAL_LEVEL 20
|
||
|
|
|
||
|
|
static volatile unsigned int critical_level[CORES] = {0};
|
||
|
|
static volatile uint64_t store[CORES][MAX_CRITICAL_LEVEL];
|
||
|
|
|
||
|
|
/* This function can be used to disable IRQ and/or FIQ interrupts for the
|
||
|
|
* current core.
|
||
|
|
*/
|
||
|
|
void gilbraltar_enter_critical(enum level level) {
|
||
|
|
assert(level == IRQ || level == FIQ);
|
||
|
|
uint64_t mpidr;
|
||
|
|
|
||
|
|
__asm__ __volatile("mrs %0, mpidr_el1" : "=r"(mpidr));
|
||
|
|
mpidr >>= 8;
|
||
|
|
|
||
|
|
unsigned int core = mpidr & (CORES - 1);
|
||
|
|
uint64_t flags;
|
||
|
|
|
||
|
|
__asm__ __volatile("mrs %0, daif" : "=r"(flags));
|
||
|
|
// if we are already on FIQ, we must not go back to IRQ here
|
||
|
|
assert(level == FIQ || !(flags & 0x40));
|
||
|
|
gilbraltar_log(DEBUG, "Disable IRQs and FIQs for %02x, flags:%08lx.\r\n",
|
||
|
|
core, flags);
|
||
|
|
__asm__ __volatile("msr DAIFSet, #3"); // disable both IRQ and FIQ
|
||
|
|
store[core][critical_level[core]++] = flags;
|
||
|
|
|
||
|
|
// if we want the IRQ level, keep the FIQ level
|
||
|
|
if (level == IRQ)
|
||
|
|
enable_fiqs();
|
||
|
|
|
||
|
|
data_mem_barrier();
|
||
|
|
gilbraltar_log(DEBUG, "Critical zone.\r\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
void gilbraltar_leave_critical() {
|
||
|
|
uint64_t mpidr;
|
||
|
|
|
||
|
|
__asm__ __volatile("mrs %0, mpidr_el1" : "=r"(mpidr));
|
||
|
|
mpidr >>= 8;
|
||
|
|
|
||
|
|
unsigned int core = mpidr & (CORES - 1);
|
||
|
|
data_mem_barrier();
|
||
|
|
disable_fiqs();
|
||
|
|
assert(critical_level[core] > 0);
|
||
|
|
uint64_t flags = store[core][--critical_level[core]];
|
||
|
|
|
||
|
|
gilbraltar_log(DEBUG, "Re-enable flags %08lx for %02x.\r\n", flags, core);
|
||
|
|
|
||
|
|
// re-set old flags
|
||
|
|
__asm__ __volatile("msr daif, %0" ::"r"(flags));
|
||
|
|
}
|