Break out monitor mode specific code

As part of general cleanup, move the monitor code into its own C and ASM
modules.

Signed-off-by: Greg Bellows <greg.bellows@linaro.org>

6 files changed, 121 insertions, 54 deletions

diff --git a/arm/Makefile.tztest b/arm/Makefile.tztest
index 971d986..3df49b1 100644
--- a/arm/Makefile.tztest
+++ b/arm/Makefile.tztest
@@ -9,6 +9,8 @@ TZNSECLOAD		= $(ARCH)/tztest_nonsecure.lds
 TZSECOBJS		= $(ARCH)/secure_init.o \
 			  	  $(ARCH)/tztest_secure_svc.o \
 			  	  $(ARCH)/secure_asm.o \
+                  $(ARCH)/monitor.o \
+                  $(ARCH)/monitor_asm.o \
                   $(ARCH)/sm_asm.o \
 			  	  $(ARCH)/sm.o
 TZNSECOBJS		= $(ARCH)/nonsecure_init.o \
diff --git a/arm/monitor.c b/arm/monitor.c
new file mode 100644
index 0000000..90b44b4
--- /dev/null
+++ b/arm/monitor.c
@@ -0,0 +1,16 @@
+
+#include "tztest.h"
+
+void monitor_init_ns_entry(uint32_t entry_point)
+{
+    struct sm_nsec_ctx *nsec_ctx;
+
+
+    /* Set-up the non-secure context so that the switch to nonsecure state
+     * resumes at initiallizing the nonsecure svc mode.
+     */
+    nsec_ctx = sm_get_nsec_ctx();
+    nsec_ctx->mon_lr = entry_point;
+    nsec_ctx->mon_spsr = CPSR_MODE_SVC | CPSR_I;
+}
+
diff --git a/arm/monitor_asm.S b/arm/monitor_asm.S
new file mode 100644
index 0000000..f6824af
--- /dev/null
+++ b/arm/monitor_asm.S
@@ -0,0 +1,50 @@
+
+#include "tztest_asm.h"
+
+.align 5
+.global monitor_vectors
+monitor_vectors:
+	b	.		/* Reset			*/
+	b	.		/* Undefined instruction	*/
+	b	monitor_smc_vect /* Secure monitor call		*/
+	b	.		/* Prefetch abort		*/
+	b	.		/* Data abort			*/
+	b	.		/* Reserved			*/
+	b	.		/* IRQ				*/
+	b	.       /* FIQ				*/
+
+monitor_smc_vect:
+    cmp r0, #SMC_YIELD
+    bge sm_smc_entry
+	srsdb sp!, #CPSR_MODE_MON
+    cmp r0, #SMC_NOOP
+    beq 1f
+	push {r0-r3}
+    mov r0, r1
+    bl tztest_dispatch_monitor
+	pop	{r0-r3}
+1:  rfefd sp!
+
+.global monitor_init
+/* monitor_init(nonsecure_entrypt) */
+monitor_init:
+    push {lr}
+
+    /* Set-up the monitor stack */
+	mrs	r10, cpsr
+    cps #CPSR_MODE_MON
+    ldr sp, =mon_stacktop
+	msr	cpsr, r10
+
+    /* Set-up mvbar */
+    ldr r10, =monitor_vectors
+    mcr p15, 0, r10, c12, c0, 1
+
+    /* Call to set the nonsecure entry point to be the save r0 value passed in
+     * by the boot code.  This will be the initial entry point on the first
+     * monitor context switch.
+     */
+    bl monitor_init_ns_entry
+
+    pop {pc}
+
diff --git a/arm/secure_asm.S b/arm/secure_asm.S
new file mode 100644
index 0000000..083f53a
--- /dev/null
+++ b/arm/secure_asm.S
@@ -0,0 +1,40 @@
+
+#include "tztest_asm.h"
+
+.global secure_dispatch_usr
+secure_dispatch_usr:
+    /* Push the LR onto the stack so we can return immediately from the svc
+     * handler on return.
+     */
+    push {lr}
+
+    /* Reset the user stack for the dispatch, so we have a clean stack */
+    cps #CPSR_MODE_SYS
+	ldr sp, =sec_usr_stacktop
+    push {r0, r1}
+    cps #CPSR_MODE_SVC
+
+    /* Return to USR mode to execute the dispatch function in r0
+     */
+    ldr r0, =dispatch_usr_wrapper
+    mov r1, #CPSR_MODE_USR
+    push {r0, r1}
+    rfefd sp!
+
+/* This is the user-side dispatch wrapper that accepts the function pointer and
+ * function arg in r0 & r1 respectively.  The wrapper is simply an anstraction
+ * layer for translating the input parameters into the user function call.
+ */
+dispatch_usr_wrapper:
+    pop {r0, r1}
+    mov r2, r0
+    mov r0, r1
+    mov pc, r2
+
+.global secure_shutdown
+secure_shutdown:
+    ldr r0, =SYSREG_BASE
+    add r0, r0, #SYSREG_CFGCTRL
+    ldr r1, =SYS_SHUTDOWN
+    str r1, [r0]
+    b secure_shutdown
diff --git a/arm/secure_init.S b/arm/secure_init.S
index 415bafe..10a480d 100644
--- a/arm/secure_init.S
+++ b/arm/secure_init.S
@@ -55,30 +55,6 @@ secure_svc_vec:
     rfefd sp!
 
 .section .text
-.align 5
-monitor_vectors:
-	b	.		/* Reset			*/
-	b	.		/* Undefined instruction	*/
-	b	monitor_smc_vect /* Secure monitor call		*/
-	b	.		/* Prefetch abort		*/
-	b	.		/* Data abort			*/
-	b	.		/* Reserved			*/
-	b	.		/* IRQ				*/
-	b	.       /* FIQ				*/
-
-monitor_smc_vect:
-    cmp r0, #SMC_YIELD
-    bge sm_smc_entry
-	srsdb sp!, #CPSR_MODE_MON
-    cmp r0, #SMC_NOOP
-    beq 1f
-	push {r0-r3}
-    mov r0, r1
-    bl tztest_dispatch_monitor
-	pop	{r0-r3}
-1:  rfefd sp!
-
-    .globl secure_init
 secure_init:
     /* We enter with R0 pointing to the nonsecure entry point.  Put it in R11
      * for now to avoid overwriting it on calls. */
@@ -86,17 +62,17 @@ secure_init:
 
     /* Disable interrupts for now */
     mrs r10, cpsr
-    orr r10, r10, #0xc0     @ Mask IRQ and FIQ
+    orr r10, r10, #0xc0     /* Mask IRQ and FIQ */
     msr cpsr, r10
 
     /* During startup, before the MMU is enabled we'll set-up the vecs in low
      * mem.
      */
     mrc p15, 0, r10, c1, c0, 0
-    bic r10, r10, #0x2000           @ SCTLR.V = 0
+    bic r10, r10, #0x2000           /* SCTLR.V = 0 */
     mcr p15, 0, r10, c1, c0, 0
     ldr r10, =secure_vectors
-    mcr p15, 0, r10, c12, c0, 0     @ VBAR = secure_vectors
+    mcr p15, 0, r10, c12, c0, 0     /* VBAR = secure_vectors */
 
     isb
 
@@ -167,26 +143,22 @@ secure_remap_vecs:
 #endif
 
 secure_init_monitor:
-    /* Set-up the monitor stack */
-	mrs	r10, cpsr
-    cps #CPSR_MODE_MON
-    ldr sp, =mon_stacktop
-	msr	cpsr, r10
-
-    /* Set-up mvbar */
-    ldr r10, =monitor_vectors
-    mcr p15, 0, r10, c12, c0, 1
-
-    /* Call to set the nonsecure entry point to be the save r0 value passed in
-     * by the boot code.  This will be the initial entry point on the first
-     * monitor context switch.
+    /* Call monitor mode initialization and pass to it the non-secure execution
+     * entry point.
      */
     mov r0, r11
-    bl tztest_secure_svc_init_monitor
+    bl monitor_init
 
+    /* Once monitor mode is set-up, we yield to non-secure execution.  The
+     * non-secure entrypoint was established in monitor init.
+     */
     mov r0, #SMC_YIELD
     smc #0
 
+    /* The first return to the secure world will set us off into our
+     * secure-side monitor loop.  The only way out is to issue an EXIT SMC call
+     * to the secure world.
+     */
     bl tztest_secure_svc_loop
 
 	/* If we get here we are on the way out, poweroff the device */
diff --git a/arm/tztest_secure_svc.c b/arm/tztest_secure_svc.c
index 69f036b..2d7eddd 100644
--- a/arm/tztest_secure_svc.c
+++ b/arm/tztest_secure_svc.c
@@ -195,19 +195,6 @@ void tztest_secure_pagetable_init()
     pagetable_init_common(sec_l1_page_table);
 }
 
-void tztest_secure_svc_init_monitor(uint32_t entry_point)
-{
-    struct sm_nsec_ctx *nsec_ctx;
-
-
-    /* Set-up the non-secure context so that the switch to nonsecure state
-     * resumes at initiallizing the nonsecure svc mode.
-     */
-    nsec_ctx = sm_get_nsec_ctx();
-    nsec_ctx->mon_lr = entry_point;
-    nsec_ctx->mon_spsr = CPSR_MODE_SVC | CPSR_I;
-}
-
 void tztest_dispatch_monitor(tztest_smc_desc_t *desc)
 {
     uint32_t (*func)(uint32_t) = desc->dispatch.func;