spacewrench wrote on Monday, February 18, 2008:
I figured out how to implement the interrupt disable/enable, save/restore context, and yield routines in inline assembly for both ARM and Thumb. It was an interesting exercise, but now that I think about it, the Thumb version is probably no faster than an ordinary function call, since the inline code has to branch at least twice (once to enter ARM mode and once to return to Thumb).
Code follows:
/*
* portmacro.h for Game Boy Advance
*/
#ifndef PORTMACRO_H
#define PORTMACRO_H 1
/*
*-----------------------------------------------------------
* Port specific definitions.
*
* The settings in this file configure FreeRTOS correctly for the
* given hardware and compiler.
*
* These settings should not need to be altered.
*-----------------------------------------------------------
*/
/* Type definitions. */
#define portCHAR char
#define portFLOAT float
#define portDOUBLE double
#define portLONG long
#define portSHORT short
#define portSTACK_TYPE unsigned portLONG
#define portBASE_TYPE portLONG
#if (configUSE_16_BIT_TICKS == 1)
typedef unsigned portSHORT portTickType;
#else
typedef unsigned portLONG portTickType;
#endif
#define portMAX_DELAY ((portTickType)~0)
#define portSTACK_GROWTH ( -1 )
#define portTICK_RATE_MS ((portTickType)(1000/configTICK_RATE_HZ))
#define portBYTE_ALIGNMENT 4
#define portNOP() asm volatile ("nop")
/*
* These defines permit non-standard task signatures (not necessary for GBA)
*/
#define portTASK_FUNCTION_PROTO( vFunction, pvParameters )
void vFunction( void *pvParameters )
#define portTASK_FUNCTION( vFunction, pvParameters )
void vFunction( void *pvParameters )
extern volatile unsigned portLONG ulCriticalNesting;
#define portNO_CRITICAL_NESTING ((unsigned portLONG)0)
extern void vPortStartFirstTask( void );
inline void vTaskSwitchContext( void );
extern void vPortTick( void );
/*
* FreeRTOS requires a few port-specific services. These are declared
* or defined below. Since the GBA is an ARM-T device, we provide two
* versions: one for ARM mode code and one for Thumb code. The Thumb
* versions switch to ARM mode briefly to execute the necessary system
* instructions.
*/
#ifndef THUMB
static inline int portDISABLE_INTERRUPTS( void ) {
int retval;
asm volatile ( "mrs %0, CPSR\n"
"orr r1, %0, #0xc0\n"
"msr CPSR_c, r1"
: "=r" (retval) : : "r1" );
return retval;
}
static inline int portENABLE_INTERRUPTS( void ) {
int retval;
asm volatile ( "mrs %0, CPSR\n"
"bic r1, %0, #0xc0\n"
"msr CPSR_c, r1"
: "=r" (retval) : : "r1" );
return retval;
}
/*
* This will only work in a non-USR/SYS mode!
*/
static inline void portSAVE_CONTEXT( void ) {
asm volatile (
“stmdb sp!, {r0}\n” // Save R0 on this mode’s stack
“mov r0 , sp\n” // Use R0 to point current stack
“sub sp , sp, #4\n” // Open space on current stack
“stmdb r0 , {sp}^\n” // Store usr SP on current stack
“ldmia sp!, {r0}\n” // R0 points to USR stack
“stmdb r0!, {lr}\n” // Put return address in place
“stmdb r0 , {r1-r14}^\n” // USR regs onto USR stack
“sub r0 , r0, #(14 * 4)\n” // Writeback is unpredictable
“ldmia sp!, {r3}\n” // Get usr R0
“mrs r2 , spsr\n” // Put final few things in context
“ldr r1 , 1f\n” // get ulCriticalNesting
“ldr r1 , [r1]\n”
“stmdb r0!, {r1-r3}\n” // Complete context saved
"ldr r1 , 2f\n" // Save usrSP in pxCurrentTCB
"ldr r1 , [r1]\n"
"str r0 , [r1]\n"
"b 3f\n"
"1:.word ulCriticalNesting\n"
"2:.word pxCurrentTCB\n"
"3:\n"
);
}
/*
* This will only work in a non-USR/SYS mode!
*/
static inline void portRESTORE_CONTEXT( void ) {
asm volatile (
"ldr lr , 1f\n" // LR points to saved context
"ldr lr , [lr]\n"
"ldr lr , [lr]\n"
"ldmia lr!, {r0, r1}\n" // Recover ulCriticalNesting & SPSR
"ldr r2 , 2f\n"
"str r0 , [r2]\n"
"msr spsr, r1\n"
"ldmia lr , {r0-r14}^\n" // Restore user regs
"ldr lr , [lr, #60]\n"
"movs pc , lr\n" // "return" to new context
"1: .word pxCurrentTCB\n"
"2: .word ulCriticalNesting\n"
);
}
static inline void portYIELD( void ) {
asm volatile (
"mrs r0, CPSR\n"
"msr CPSR_c, #0x13 | 0x80 | 0x40\n" // SVC mode, no interrupts
"msr SPSR, r0\n"
"bl 1f\n"
"movs pc, lr\n" // <-- RESTORE_CONTEXT "returns" to here
"1:\n"
:::"r0") ;
portSAVE_CONTEXT( );
vTaskSwitchContext( );
portRESTORE_CONTEXT( );
}
#else /* defined(THUMB) */
/*
* These are Thumb versions of the above functions. They just
* switch to ARM mode briefly when necessary, then back to Thumb.
*/
static inline int portDISABLE_INTERRUPTS( void ) {
int retval;
asm volatile ( "ldr %0, =1f\n"
"bx %0\n"
".arm\n"
"1:mrs %0, CPSR\n"
"orr r1, %0, #0xc0\n"
"msr CPSR_c, r1\n"
"ldr r1, =2f + 1\n"
"bx r1\n"
".thumb\n"
"2:\n"
: "=r" (retval) : : "r1" );
return retval;
}
static inline int portENABLE_INTERRUPTS( void ) {
int retval;
asm volatile ( "ldr %0, =1f\n"
"bx %0\n"
".arm\n"
"1:mrs %0, CPSR\n"
"bic r1, %0, #0xc0\n"
"msr CPSR_c, r1\n"
"ldr r1, =2f + 1\n"
"bx r1\n"
".thumb\n"
"2:\n"
: "=r" (retval) : : "r1" );
return retval;
}
/*
* This will only work in a non-USR/SYS mode!
*/
static inline void portSAVE_CONTEXT( void ) {
asm volatile (
“push {r0}\n” // Save R0 on this mode’s stack
“ldr r0, =1f\n” // Switch to ARM mode
“bx r0\n”
“.arm\n”
“1:\n” // We’re in ARM mode and R0 is stacked…get on with it
"mov r0 , sp\n" // Use R0 to point current stack
"sub sp , sp, #4\n" // Open space on current stack
"stmdb r0 , {sp}^\n" // Store usr SP on current stack
"ldmia sp!, {r0}\n" // R0 points to USR stack
"stmdb r0!, {lr}\n" // Put return address in place
"stmdb r0 , {r1-r14}^\n" // USR regs onto USR stack
"sub r0 , r0, #(14 * 4)\n" // Writeback is unpredictable
"ldmia sp!, {r3}\n" // Get usr R0
"mrs r2 , spsr\n" // Put final few things in context
"ldr r1 , 1f\n" // get ulCriticalNesting
"ldr r1 , [r1]\n"
"stmdb r0!, {r1-r3}\n" // Complete context saved
"ldr r1 , 2f\n" // Save usrSP in pxCurrentTCB
"ldr r1 , [r1]\n"
"str r0 , [r1]\n"
"ldr r0, =3f + 1\n"
"bx r0\n"
"1:.word ulCriticalNesting\n"
"2:.word pxCurrentTCB\n"
".thumb\n"
"3:\n"
);
}
/*
* This will only work in a non-USR/SYS mode! We’re in Thumb mode
* on entry and change to ARM mode to do our business, but we
* NEVER CHANGE BACK! Instead, we return to the context we’re
* restoring, which may or may not be Thumb.
*/
static inline void portRESTORE_CONTEXT( void ) {
asm volatile (
“ldr r0 , =0f\n”
“bx r0\n”
“.arm\n”
“0:ldr lr , 1f\n” // LR points to saved context
“ldr lr , [lr]\n”
“ldr lr , [lr]\n”
"ldmia lr!, {r0, r1}\n" // Recover ulCriticalNesting & SPSR
"ldr r2 , 2f\n"
"str r0 , [r2]\n"
"msr spsr, r1\n"
"ldmia lr , {r0-r14}^\n" // Restore user regs
"ldr lr , [lr, #60]\n"
"movs pc , lr\n" // "return" to new context
"1: .word pxCurrentTCB\n"
"2: .word ulCriticalNesting\n"
);
}
/*
* Yield is a little tricky – we want to return in Thumb mode,
* but must switch to ARM mode to change CPSR. However, GCC
* thinks we’re in Thumb mode always, so must change back to Thumb
* before calling SAVE_CONTEXT & friends.
*/
static inline void portYIELD( void ) {
asm volatile (
“ldr r0 , =1f\n”
“bx r0\n”
“.arm\n”
“1:mrs r0, CPSR\n”
“msr CPSR_c, #0x13 | 0x80 | 0x40\n” // SVC mode, no interrupts
“orr r0, r0, #0x20\n” // Set T bit in faked SPSR
“msr SPSR, r0\n”
“bl 2f\n” // Put addr of next instr in LR
“.thumb\n”
“movs pc, lr\n” // <-- RESTORE_CONTEXT will
// “return” here in Thumb mode
“.arm\n”
“2:ldr r0, =3f + 1\n” // Switch back to Thumb
“bx r0\n”
“.thumb\n”
“3:\n”
:::“r0”) ;
portSAVE_CONTEXT( );
vTaskSwitchContext( );
portRESTORE_CONTEXT( );
}
#endif /* !defined(THUMB) */
static inline void portENTER_CRITICAL( void ) {
portDISABLE_INTERRUPTS( );
ulCriticalNesting++;
}
static inline void portEXIT_CRITICAL( void ) {
if (ulCriticalNesting > portNO_CRITICAL_NESTING) {
ulCriticalNesting–;
if (ulCriticalNesting == portNO_CRITICAL_NESTING)
portENABLE_INTERRUPTS( );
}
}
#endif /* PORTMACRO_H */