#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));
}