IMX53 initialize stacks

Question

I am booting Android on an IMX53 Sabre tablet. I am trying to initialize stacks for the different processor modes. The following is my monitor initialization code:

@ Install Secure Monitor
@ -----------------------
ldr r1, =ns_image                   @ R1 is used
str r0, [r1]
ldr r0, =tz_monitor                    @ Get address of Monitors vector table
mcr p15, 0, r0, c12, c0, 1          @ Write Monitor Vector Base Address Register

@ Save Secure state
@ ------------------
ldr    r0, =S_STACK_LIMIT           @ Get address of Secure state stack
stmfd  r0!, {r4-r12}                @ Save general purpose registers
@ ADD support for SPs
mrs    r1, cpsr                     @ Also get a copy of the CPSR
stmfd  r0!, {r1, lr}                @ Save CPSR and LR


ldr r1, =STACK_ADDR

msr cpsr_c, #Mode_FIQ | I_Bit | F_Bit 
sub sp, r1, #Offset_FIQ_Stack

msr cpsr_c, #Mode_IRQ | I_Bit | F_Bit 
sub sp, r1, #Offset_IRQ_Stack

msr cpsr_c, #Mode_ABT | I_Bit | F_Bit 
sub sp, r1, #Offset_ABT_Stack

msr cpsr_c, #Mode_UND | I_Bit | F_Bit  
sub sp, r1, #Offset_UND_Stack   

msr cpsr_c, #Mode_SYS | I_Bit | F_Bit 
sub sp, r1, #Offset_SYS_Stack

msr cpsr_c, #Mode_SVC | I_Bit | F_Bit 
sub sp, r1, #Offset_SVC_Stack

msr cpsr_c, #Mode_MON | I_Bit | F_Bit 
sub sp, r1, #Offset_MON_Stack


cps    #Mode_MON                    @ Move to Monitor mode after saving Secure state


@ Save Secure state stack pointer
@ --------------------------------
ldr r1, =S_STACK_SP                 @ Get address of global
str r0, [r1]                        @ Save pointer


@ Set up initial NS state stack pointer
@ --------------------------------------
ldr r0, =NS_STACK_SP                @ Get address of global
ldr r1, =NS_STACK_LIMIT             @ Get top of Normal state stack (assuming FD model)
str r1, [r0]                        @ Save pointer


@ Set up exception return information
@ ------------------------------------
@IMPORT  ns_image

ldr lr, ns_image                    @ ns_image
msr spsr_cxsf, #Mode_SVC            @ Set SPSR to be SVC mode

@ Switch to Normal world
@ -----------------------
mrc p15, 0, r4, c1, c1, 0           @ Read Secure Configuration Register data
bic r4, #0x66
orr r4, #0x19
//orr r4, #NS_BIT                     @ Set NS bit
mcr p15, 0, r4, c1, c1, 0           @ Write Secure Configuration Register data

@ Clear general purpose registers
@ --------------------------------
mov r0,  #0
mov r1,  #0
mov r2,  #0
mov r3,  #0
mov r4,  #0
mov r5,  #0
mov r6,  #0
mov r7,  #0
mov r8,  #0
mov r9,  #0
mov r10, #0
mov r11, #0
mov r12, #0

movs pc, lr

Android booting happens fine with this, but I am not sure if the stack pointers are set up correctly, as I am not able to use them as described at IMX53 external abort. Are the stack initializations correct?

Other relevant code snippets:

.equ Mode_USR, 0x10 @ User Mode
.equ Mode_FIQ, 0x11 @ Fast Interrupt Mode
.equ Mode_IRQ, 0x12 @ Interrupt Mode
.equ Mode_SVC, 0x13 @ Supervisor Mode
.equ Mode_ABT, 0x17 @ Abort Mode
.equ Mode_UND, 0x1B @ Undefined Mode
.equ Mode_SYS, 0x1F @ System Mode
.equ Mode_MON, 0x16 @ Monitor Mode
.equ STACK_ADDR,    0xa0000000

.equ I_Bit,    0x80 @ IRQ interrupts disabled
.equ F_Bit,    0x40 @ FIQ interrupts disabled
.equ NS_BIT,   0x1



/* memory reserved (in bytes) for stacks of different mode */
.equ Len_FIQ_Stack,  64
.equ Len_IRQ_Stack,  64
.equ Len_ABT_Stack,  64
.equ Len_UND_Stack,  64
.equ Len_SVC_Stack,  64
.equ Len_USR_Stack,  64
.equ Len_MON_Stack,  64
.equ Len_SYS_Stack,  64

.equ Offset_FIQ_Stack, 0
.equ Offset_IRQ_Stack, Offset_FIQ_Stack + Len_FIQ_Stack
.equ Offset_ABT_Stack, Offset_IRQ_Stack + Len_IRQ_Stack
.equ Offset_UND_Stack, Offset_ABT_Stack + Len_ABT_Stack
.equ Offset_SVC_Stack, Offset_UND_Stack + Len_UND_Stack
.equ Offset_USR_Stack, Offset_SVC_Stack + Len_SVC_Stack
.equ Offset_MON_Stack, Offset_USR_Stack + Len_USR_Stack
.equ Offset_SYS_Stack, Offset_MON_Stack + Len_MON_Stack