/******************************************************************************
 *
 * $RCSfile: $
 * $Revision: $
 *
 * This module provides the interface routines controlling the various
 * interrupt modes present on the ARM processor.
 * Copyright 2004, R O SoftWare
 * No guarantees, warrantees, or promises, implied or otherwise.
 * May be used for hobby or commercial purposes provided copyright
 * notice remains intact.
 *
 *****************************************************************************/
#include "armISR.h"

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

/******************************************************************************
 *
 * Function Name: __get_cpsr()
 *
 * Description:
 *    This function gets the value of the CPSR.
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    current value of CPSR
 *
 *****************************************************************************/
static inline unsigned __get_cpsr(void)
{
  unsigned long retval;
  asm volatile (" mrs  %0, cpsr" : "=r" (retval) : /* no inputs */  );
  return retval;
}

/******************************************************************************
 *
 * Function Name: __set_cpsr()
 *
 * Description:
 *    This function sets the CPSR to the supplied value.
 *
 * Calling Sequence: 
 *    val - new setting for CPSR
 *
 * Returns:
 *    void
 *
 *****************************************************************************/
static inline void __set_cpsr(unsigned val)
{
  asm volatile (" msr  cpsr, %0" : /* no outputs */ : "r" (val)  );
}

/******************************************************************************
 *
 * Function Name: disableIRQ()
 *
 * Description:
 *    This function sets the IRQ disable bit in the status register and
 *    returns the previous value.
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned disableIRQ(void)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr(_cpsr | IRQ_MASK);
  while (!(__get_cpsr() & IRQ_MASK));

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: enableIRQ()
 *
 * Description:
 *    This function clears the IRQ disable bit in the status register
 *    and returns the previous value.
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned enableIRQ(void)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr(_cpsr & ~IRQ_MASK);
  while (__get_cpsr() & IRQ_MASK);

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: restoreIRQ()
 *
 * Description:
 *    This function restores the IRQ disable bit in the status register
 *    to the value contained within passed oldCPSR
 *
 * Calling Sequence: 
 *    new value for the CPSR
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned restoreIRQ(unsigned oldCPSR)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr((_cpsr & ~IRQ_MASK) | (oldCPSR & IRQ_MASK));
  while ((__get_cpsr() ^ oldCPSR) & IRQ_MASK);

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: disableFIQ()
 *
 * Description:
 *    This function sets the FIQ disable bit in the status register
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned disableFIQ(void)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr(_cpsr | FIQ_MASK);
  while (!(__get_cpsr() & FIQ_MASK));

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: enableFIQ()
 *
 * Description:
 *    This function clears the FIQ disable bit in the status register
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned enableFIQ(void)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr(_cpsr & ~FIQ_MASK);
  while (__get_cpsr() & FIQ_MASK);

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: restoreFIQ()
 *
 * Description:
 *    This function restores the FIQ disable bit in the status register
 *    to the value contained within passed oldCPSR
 *
 * Calling Sequence: 
 *    new value for the CPSR
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned restoreFIQ(unsigned oldCPSR)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr((_cpsr & ~FIQ_MASK) | (oldCPSR & FIQ_MASK));
  while ((__get_cpsr() ^ oldCPSR) & FIQ_MASK);

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: disableINTs()
 *
 * Description:
 *    This function sets the IRQ & FIQ disable bits in the status register
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned disableINTs(void)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr(_cpsr | INTs_MASK);
  while ((__get_cpsr() ^ INTs_MASK) & INTs_MASK);

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: enableINTs()
 *
 * Description:
 *    This function clears the IRQ & FIQ disable bits in the status register
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned enableINTs(void)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr(_cpsr & ~INTs_MASK);
  while ((__get_cpsr() ^ ~INTs_MASK) & INTs_MASK);

  return _cpsr;
}

/******************************************************************************
 *
 * Function Name: restoreINTs()
 *
 * Description:
 *    This function restores the IRQ & FIQ disable bits in the status register
 *    to the value contained within passed oldCPSR
 *
 * Calling Sequence: 
 *    new value for the CPSR
 *
 * Returns:
 *    previous value of CPSR
 *
 *****************************************************************************/
unsigned restoreINTs(unsigned oldCPSR)
{
  unsigned _cpsr;

  _cpsr = __get_cpsr();

  do
    __set_cpsr((_cpsr & ~INTs_MASK) | (oldCPSR & INTs_MASK));
  while ((__get_cpsr() ^ oldCPSR) & INTs_MASK);

  return _cpsr;
}