at91rm9200 startup code

Hi,

I have a simple startup code for a at91rm9200 processor. The idea was 
after the processor setup to restore the processor context saved in 
stack pointed by pCurrStack variable.
I don't know where could be a problem becuase it sometimes works but 
more often it doesn't work.

This code is downloaded by u-boot so some startup seuences are not 
necessary.

I'll be gratefull for comments.

Regards
Jacek


.equ NO_INT,   0xC0
.equ USR_MODE, 0x10
.equ FIQ_MODE, 0x11
.equ IRQ_MODE, 0x12
.equ SVC_MODE, 0x13
.equ ABT_MODE, 0x17
.equ UND_MODE, 0x1B
.equ SYS_MODE, 0x1F

.equ USR_STACK_SIZE, 0x200
.equ FIQ_STACK_SIZE, 0x200
.equ IRQ_STACK_SIZE, 0x200
.equ SVC_STACK_SIZE, 0x200
.equ ABT_STACK_SIZE, 0x200
.equ UND_STACK_SIZE, 0x200
.equ SYS_STACK_SIZE, 0x200


.globl _start
_start:	
   b       reset
	ldr	pc, ptr_undefined_instruction
	ldr	pc, ptr_software_interrupt
	ldr	pc, ptr_prefetch_abort
	ldr	pc, ptr_data_abort
	ldr	pc, ptr_not_used
	ldr	pc, ptr_irq_isr
	ldr	pc, ptr_fiq_isr

ptr_undefined_instruction:.word undefined_instruction
ptr_software_interrupt:	  .word software_interrupt
ptr_prefetch_abort:	  .word prefetch_abort
ptr_data_abort:		  .word data_abort
ptr_not_used:		  .word not_used
ptr_irq_isr: 		  .word irq_isr
ptr_fiq_isr:		  .word fiq_isr


.align 16

reset:
        /*
         * set the cpu to SVC32 mode
	      */
       mrs     r0,cpsr
       bic     r0,r0,#0x1f
       orr     r0,r0,#0x13
       msr     cpsr,r0

      /* Disable "Fast Bus Mode" - set to async */
       mrc p15,0,r0,c1,c0,0
       orr r0,r0,#0xC0000000
       mcr p15,0,r0,c1,c0,0

      /*
       * After uboot starting memories have been just remapped
       */
       ldr	r0, =_start
       ldr	r1, =0x0
       mov	r2, #16
copy_loop:
       subs	r2, r2, #1
       ldr	r3, [r0], #4
       str	r3, [r1], #4
       bne	copy_loop

setup_stacks:

       /* FIQ mode stack */
       msr   CPSR_c, #FIQ_MODE|NO_INT
       ldr   sp, =(fiq_stack - 4)

       /* IRQ mode stack */
       msr   CPSR_c, #IRQ_MODE|NO_INT
       ldr   sp, =(irq_stack - 4)


        /* Undefined mode stack */
        msr   CPSR_c, #UND_MODE|NO_INT
        ldr   sp, =(und_stack - 4)

         /* Abort mode stack */
        msr   CPSR_c, #ABT_MODE|NO_INT
        ldr   sp, =(abt_stack - 4)

         /* Supervisor mode stack */
         msr   CPSR_c, #SVC_MODE|NO_INT
         ldr   sp, =(svc_stack - 4)



         ldr     r0, =start    @ start()
         mov     lr, pc
         bx      r0


         LDR     R4, =pCurrStack
         LDR     SP, [R4]

         LDR     R4,  [SP], #4
         MSR     CPSR_cxsf,R4
         LDMFD   SP!, {R0-R12,LR,PC}^

undefined_instruction:
         b undefined_instruction
software_interrupt:
         b software_interrupt
prefetch_abort:
         b prefetch_abort
data_abort:
         b data_abort
not_used:
         b not_used



__bss_start:   .word _bss_start
__bss_end:     .word _bss_end


.section ".bss"
.align  4

         .word 0
         .space IRQ_STACK_SIZE
irq_stack:
         .word 0
         .space FIQ_STACK_SIZE
fiq_stack:
         .word 0
         .space UND_STACK_SIZE
und_stack:
         .word 0
         .space ABT_STACK_SIZE
abt_stack:
         .word 0
         .space SVC_STACK_SIZE
svc_stack:
         .word 0
         .space SYS_STACK_SIZE
sys_stack:
         .word 0