#define FIQ_STACK_SIZE 8*3*4
#define IRQ_STACK_SIZE 8*3*4

#define ARM_MODE_ABT 0x17
#define ARM_MODE_FIQ 0x11
#define ARM_MODE_IRQ 0x12
#define ARM_MODE_SVC 0x13

#define I_BIT 0x80
#define F_BIT 0x40

//------------------------------------------------------------------------------
// Startup routine
//------------------------------------------------------------------------------

.align 4
.arm

/* Exception vectors
*******************/
.section .vectors, "a", %progbits

resetVector:
ldr pc, =resetHandler /* Reset */
undefVector:
b undefVector /* Undefined instruction */
swiVector:
b swiVector /* Software interrupt */
prefetchAbortVector:
b prefetchAbortVector /* Prefetch abort */
dataAbortVector:
b dataAbortVector /* Data abort */
reservedVector:
b reservedVector /* Reserved for future use */
irqVector:
b irqHandler /* Interrupt */
fiqVector:
// b fiqHandler /* Fast interrupt */
//------------------------------------------------------------------------------
/// Handles a fast interrupt request by branching to the address defined in the
/// AIC.
//------------------------------------------------------------------------------
fiqHandler:

/* Save interrupt context on the stack to allow nesting */
sub lr, lr, #4
stmfd sp!, {lr}
mrs lr, SPSR
stmfd sp!, {r0, lr}

/* Write in the FVR to support Protect Mode */
ldr lr, =AT91C_BASE_AIC
ldr r0, [lr, #AIC_FVR]
str lr, [lr, #AIC_FVR]

/* Branch to interrupt handler in Supervisor mode */
msr CPSR_c, #ARM_MODE_SVC | F_BIT | I_BIT
stmfd sp!, {r1-r3, r4, r12, lr}
blx r0

/* Restore scratch/used registers and LR from User Stack */
/* Disable Interrupt and switch back in FIQ mode */
ldmia sp!, {r1-r3, r4, r12, lr}
msr CPSR_c, #ARM_MODE_FIQ | F_BIT | I_BIT

/* Acknowledge interrupt */
ldr lr, =AT91C_BASE_AIC
str lr, [lr, #AIC_EOICR]

/* Restore interrupt context and branch back to calling code */
ldmia sp!, {r0, lr}
msr SPSR_cxsf, lr
ldmia sp!, {pc}^

//------------------------------------------------------------------------------
/// Handles incoming interrupt requests by branching to the corresponding
/// handler, as defined in the AIC. Supports interrupt nesting.
//------------------------------------------------------------------------------
irqHandler:

/* Save interrupt context on the stack to allow nesting */
sub lr, lr, #4
stmfd sp!, {lr}
mrs lr, SPSR
stmfd sp!, {r0, lr}

/* Write in the IVR to support Protect Mode */
ldr lr, =AT91C_BASE_AIC
ldr r0, [lr, #AIC_IVR]
str lr, [lr, #AIC_IVR]

/* Branch to interrupt handler in Supervisor mode */
msr CPSR_c, #ARM_MODE_SVC
stmfd sp!, {r1-r3, r4, r12, lr}
blx r0

/* Restore scratch/used registers and LR from User Stack */
/* Disable Interrupt and switch back in IRQ mode */
ldmia sp!, {r1-r3, r4, r12, lr}
msr CPSR_c, #ARM_MODE_IRQ | F_BIT | I_BIT

/* Acknowledge interrupt */
ldr lr, =AT91C_BASE_AIC
str lr, [lr, #AIC_EOICR]

/* Restore interrupt context and branch back to calling code */
ldmia sp!, {r0, lr}
msr SPSR_cxsf, lr
ldmia sp!, {pc}^

//------------------------------------------------------------------------------
/// Initializes the chip and branches to the main() function.
//------------------------------------------------------------------------------
.section .text
.global entry

entry:
resetHandler:

/* Useless instruction for referencing the .vectors section */
ldr r0, =resetVector

/* Set pc to actual code location (i.e. not in remap zone) */
ldr pc, =1f

/* Initialize the prerelocate segment */
1:
ldr r0, =_efixed
ldr r1, =_sprerelocate
ldr r2, =_eprerelocate
1:
cmp r1, r2
ldrcc r3, [r0], #4
strcc r3, [r1], #4
bcc 1b

/* Perform low-level initialization of the chip using LowLevelInit() */
ldr sp, =_sstack
stmfd sp!, {r0}
ldr r0, =LowLevelInit
blx r0

/* Initialize the postrelocate segment */

ldmfd sp!, {r0}
ldr r1, =_spostrelocate
ldr r2, =_epostrelocate
1:
cmp r1, r2
ldrcc r3, [r0], #4
strcc r3, [r1], #4
bcc 1b

/* Clear the zero segment */
ldr r0, =_szero
ldr r1, =_ezero
mov r2, #0
1:
cmp r0, r1
strcc r2, [r0], #4
bcc 1b

/* Setup stacks
**************/
/* FIQ mode */
msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
ldr sp, =_sstack
sub r4, sp, #FIQ_STACK_SIZE

/* IRQ mode */
msr CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT
mov sp, r4
sub r4, sp, #IRQ_STACK_SIZE

/* Supervisor mode (interrupts enabled) */
msr CPSR_c, #ARM_MODE_SVC
mov sp, r4

/* Branch to main()
******************/
ldr r0, =main
blx r0

/* Loop indefinitely when program is finished */
1:
b 1b

OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm")
OUTPUT_ARCH(arm)
ENTRY(entry)

MEMORY
{
sram (W!RX) : ORIGIN = 0x300000, LENGTH = 0x10000
sdram (W!RX) : ORIGIN = 0x20000000, LENGTH = 32M
sdram_nocache (W!RX) : ORIGIN = 0x22000000, LENGTH = 32M
}

SECTIONS
{
.fixed :
{
. = ALIGN(4);
_sfixed = .;
*(.text*)
*(.glue_7)
*(.glue_7t)
*(.rodata*)
*(.data)
. = ALIGN(4);
_efixed = .;
} > sdram

.prerelocate : AT (_efixed)
{
. = ALIGN(4);
_sprerelocate = .;
. = ALIGN(4);
_eprerelocate = .;
}

.postrelocate : AT (_efixed + SIZEOF(.prerelocate))
{
. = ALIGN(4);
_spostrelocate = .;
*(.vectors)
*(.ramfunc)
. = ALIGN(4);

_epostrelocate = .;
} > sram

.bss (NOLOAD) :
{
_szero = .;
*(.bss)
_ezero = .;
} > sdram

.nocache (NOLOAD) :
{
*(.nocache)
} > sdram_nocache

_sstack = 0x22000000;
}
end = .;

Abort:[D] PC:00016AF0 Op:E5D00020 CPSR:6000001F
   LR:00014BF4 SP:400059C0
SP[0]:00000022->00014BF4->00000016->0001381C->
SP[4]:40000EBC->40000E70->00000004->40002D4C
R0:00430001 R1:00000000 R2:00027190 R3:00000000  R4:40002D4C  R5:00026D40
R6:00000000 R7:40001698 R8:00000001 R9:00000004 R10:00000FFF R11:400034D4
R12:00000000

{
    U8 Id;
    U8 Type;
    ...
    U32 Payload[ 64 ];
}

U32 *p = pCmd->Payload;

*p++ = xxx;
*p++ = xxx;
...
*p++ = xxx;

typedef struct tagPACKET
{
     struct tagPACKET *pNext;
     ....
} TPACKET, *PPACKET;