#include "archinc.h"
#include "armirq.h"

#include <stdio.h>
#include <stdlib.h>

#define IRQ_MASK 0x00000080
#define FIQ_MASK 0x00000040
#define INT_MASK (IRQ_MASK | FIQ_MASK)

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
static unsigned int get_cspr(void)
#else
static INLINE_KEYWORD unsigned int get_cpsr(void)
#endif
{
  unsigned long retval;
  __asm__ volatile (" mrs  %0, cpsr" : "=r" (retval) :  );
  return retval;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
static void set_cpsr(unsigned int val)
#else
static INLINE_KEYWORD void set_cpsr(unsigned int val)
#endif
{
  __asm__ volatile (" msr  cpsr, %0" :  : "r" (val)  );
}

unsigned int disableIRQ(void)
{
  unsigned int cpsr;

  cpsr = get_cpsr();
  set_cpsr(cpsr | IRQ_MASK);
  return cpsr;
}

unsigned int restoreIRQ(unsigned int oldCPSR)
{
  unsigned int cpsr;

  cpsr = get_cpsr();
  set_cpsr((cpsr & ~IRQ_MASK) | (oldCPSR & IRQ_MASK));
  return cpsr;
}

unsigned int enableIRQ(void)
{
  unsigned int cpsr;

  cpsr = get_cpsr();
  set_cpsr(cpsr & ~IRQ_MASK);
  return cpsr;
}

unsigned int disableFIQ(void)
{
  unsigned int cpsr;

  cpsr = get_cpsr();
  set_cpsr(cpsr | FIQ_MASK);
  return cpsr;
}

unsigned int restoreFIQ(unsigned int oldCPSR)
{
  unsigned int cpsr;

  cpsr = get_cpsr();
  set_cpsr((cpsr & ~FIQ_MASK) | (oldCPSR & FIQ_MASK));
  return cpsr;
}

unsigned int enableFIQ(void)
{
  unsigned int cpsr;

  cpsr = get_cpsr();
  set_cpsr(cpsr & ~FIQ_MASK);
  return cpsr;
}