IMX53 initialize stacks - linux-kernel

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