run standalone software in user mode

Hello,

I am using a standalone software which is current run in SYSTEM mode. But I would like to run it in USER mode instead, in order to be in non-privileged mode and be able to test my MMU fault handler (in particular permission faults).

In the linker script I have declared the USER stack within the stack section, and defined its size as 1024B.

_STACK_SIZE = DEFINED(_STACK_SIZE) ? _STACK_SIZE : 0x2000;
_ABORT_STACK_SIZE = DEFINED(_ABORT_STACK_SIZE) ? _ABORT_STACK_SIZE : 1024;
_SUPERVISOR_STACK_SIZE = DEFINED(_SUPERVISOR_STACK_SIZE) ? _SUPERVISOR_STACK_SIZE : 2048;
_IRQ_STACK_SIZE = DEFINED(_IRQ_STACK_SIZE) ? _IRQ_STACK_SIZE : 1024;
_FIQ_STACK_SIZE = DEFINED(_FIQ_STACK_SIZE) ? _FIQ_STACK_SIZE : 1024;
_UNDEF_STACK_SIZE = DEFINED(_UNDEF_STACK_SIZE) ? _UNDEF_STACK_SIZE : 1024;
_USR_STACK_SIZE = DEFINED(_USR_STACK_SIZE) ? _USR_STACK_SIZE : 1024;

.stack (NOLOAD) : {
   . = ALIGN(16);
   _stack_end = .;
   . += _STACK_SIZE;
   . = ALIGN(16);
   _stack = .;
   __stack = _stack;
   . = ALIGN(16);
   _irq_stack_end = .;
   . += _IRQ_STACK_SIZE;
   . = ALIGN(16);
   __irq_stack = .;
   _supervisor_stack_end = .;
   . += _SUPERVISOR_STACK_SIZE;
   . = ALIGN(16);
   __supervisor_stack = .;
   _abort_stack_end = .;
   . += _ABORT_STACK_SIZE;
   . = ALIGN(16);
   __abort_stack = .;
   _fiq_stack_end = .;
   . += _FIQ_STACK_SIZE;
   . = ALIGN(16);
   __fiq_stack = .;
   _undef_stack_end = .;
   . += _UNDEF_STACK_SIZE;
   . = ALIGN(16);
   __undef_stack = .;
   _usr_stack_end = .;
   . += _USR_STACK_SIZE;
   . = ALIGN(16);
   __usr_stack = .;
} > ps7_ddr_0

Then in the __boot assembly code I added the code to initialize the USER stack and switch to USER mode (after SYS mode init):

	mrs	r0, cpsr			/* get the current PSR */
	mvn	r1, #0x1f			/* set up the system stack pointer */
	and	r2, r1, r0
	orr	r2, r2, #0x1F			/* SYS mode */
	msr	cpsr, r2
	ldr	r13,=SYS_stack			/* SYS stack pointer */
	bic r2, r2, #(0x1 << 9)     		/* Set EE bit to little-endian */
	msr spsr_fsxc,r2

	mrs	r0, cpsr			/* get the current PSR */
	mvn	r1, #0x1f			/* set up the user stack pointer */
	and	r2, r1, r0
	orr	r2, r2, #0x10			/* USER mode */
	msr	cpsr, r2
	ldr	r13,=USR_stack			/* USER stack pointer */

But when I try to read the current CPSR mode when the standalone software is executing, I find out it is still in SYSTEM mode (0x1f)...

Could anyone tell me if the stack is setup properly? 

Thank you a lot.