@ -129,8 +129,25 @@ struct xcptcontext
@@ -129,8 +129,25 @@ struct xcptcontext
# ifndef __ASSEMBLY__
/* Get/set the primask register */
static inline uint8_t getprimask ( void ) __attribute__ ( ( always_inline ) ) ;
static inline uint8_t getprimask ( void )
{
uint32_t primask ;
__asm__ __volatile__
(
" \t mrs %0, primask \n "
: " =r " ( primask )
:
: " memory " ) ;
return ( uint8_t ) primask ;
}
/* Disable IRQs */
static inline void irqdisable ( void ) __attribute__ ( ( always_inline ) ) ;
static inline void irqdisable ( void )
{
__asm__ __volatile__ ( " \t cpsid i \n " ) ;
@ -138,26 +155,22 @@ static inline void irqdisable(void)
@@ -138,26 +155,22 @@ static inline void irqdisable(void)
/* Save the current primask state & disable IRQs */
static inline irqstate_t irqsave ( void ) __attribute__ ( ( always_inline ) ) ;
static inline irqstate_t irqsave ( void )
{
unsigned short primask ;
/* Return the current value of primask register (before disabling) */
/* Return the current value of primask register and set
* bit 0 of the primask register to disable interrupts
*/
uint8_t primask = getprimask ( ) ;
__asm__ __volatile__
(
" \t mrs %0, primask \n "
" \t cpsid i \n "
: " =r " ( primask )
:
: " memory " ) ;
/* Then set bit 0 of the primask register to disable interrupts */
irqdisable ( ) ;
return primask ;
}
/* Enable IRQs */
static inline void irqenable ( void ) __attribute__ ( ( always_inline ) ) ;
static inline void irqenable ( void )
{
__asm__ __volatile__ ( " \t cpsie i \n " ) ;
@ -165,61 +178,48 @@ static inline void irqenable(void)
@@ -165,61 +178,48 @@ static inline void irqenable(void)
/* Restore saved primask state */
static inline void irqrestore ( irqstate_t primask )
static inline void setprimask ( uint32_t primask ) __attribute__ ( ( always_inline ) ) ;
static inline void setprimask ( uint32_t primask )
{
/* If bit 0 of the primask is 0, then we need to restore
* interupts .
*/
__asm__ __volatile__
(
" \t tst %0, #1 \n "
" \t bne 1f \n "
" \t cpsie i \n "
" 1: \n "
" \t msr primask, %0 \n "
:
: " r " ( primask )
: " memory " ) ;
}
/* Get/set the primask register */
static inline uint8_t getprimask ( void )
static inline void irqrestore ( irqstate_t primask ) __attribute__ ( ( always_inline ) ) ;
static inline void irqrestore ( irqstate_t primask )
{
uint32_t primask ;
__asm__ __volatile__
(
" \t mrs %0, primask \n "
: " =r " ( primask )
:
: " memory " ) ;
return ( uint8_t ) primask ;
}
/* If bit 0 of the primask is 0, then we need to restore
* interrupts .
*/
static inline void setprimask ( uint32_t primask )
{
__asm__ __volatile__
(
" \t msr primask, %0 \n "
:
: " r " ( primask )
: " memory " ) ;
if ( ( primask & 1 ) = = 0 )
{
setprimask ( primask ) ;
}
}
/* Get/set the basepri register */
static inline uint8_t getbasepri ( void ) __attribute__ ( ( always_inline ) ) ;
static inline uint8_t getbasepri ( void )
{
uint32_t basepri ;
__asm__ __volatile__
(
" \t mrs %0, basepri \n "
: " =r " ( basepri )
:
: " memory " ) ;
return ( uint8_t ) basepri ;
}
static inline void setbasepri ( uint32_t basepri ) __attribute__ ( ( always_inline ) ) ;
static inline void setbasepri ( uint32_t basepri )
{
__asm__ __volatile__
@ -232,6 +232,7 @@ static inline void setbasepri(uint32_t basepri)
@@ -232,6 +232,7 @@ static inline void setbasepri(uint32_t basepri)
/* Get/set IPSR */
static inline uint32_t getipsr ( void ) __attribute__ ( ( always_inline ) ) ;
static inline uint32_t getipsr ( void )
{
uint32_t ipsr ;
@ -241,9 +242,11 @@ static inline uint32_t getipsr(void)
@@ -241,9 +242,11 @@ static inline uint32_t getipsr(void)
: " =r " ( ipsr )
:
: " memory " ) ;
return ipsr ;
}
static inline void setipsr ( uint32_t ipsr ) __attribute__ ( ( always_inline ) ) ;
static inline void setipsr ( uint32_t ipsr )
{
__asm__ __volatile__
@ -256,6 +259,7 @@ static inline void setipsr(uint32_t ipsr)
@@ -256,6 +259,7 @@ static inline void setipsr(uint32_t ipsr)
/* Get/set CONTROL */
static inline uint32_t getcontrol ( void ) __attribute__ ( ( always_inline ) ) ;
static inline uint32_t getcontrol ( void )
{
uint32_t control ;
@ -265,9 +269,11 @@ static inline uint32_t getcontrol(void)
@@ -265,9 +269,11 @@ static inline uint32_t getcontrol(void)
: " =r " ( control )
:
: " memory " ) ;
return control ;
}
static inline void setcontrol ( uint32_t control ) __attribute__ ( ( always_inline ) ) ;
static inline void setcontrol ( uint32_t control )
{
__asm__ __volatile__