arm: restore stack on return from trap.

We align the stack before calling into C code but we weren't undoing this on
return.

Collapse continue_(non)idle_domain into continue_new_vcpu.

Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
Acked-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
Committed-by: Ian Campbell <ian.campbell@citrix.com>

xen/arch/arm/domain.c

@@ -16,17 +16,6 @@
 
 DEFINE_PER_CPU(struct vcpu *, curr_vcpu);
 
-static void continue_idle_domain(struct vcpu *v)
-{
-    reset_stack_and_jump(idle_loop);
-}
-
-static void continue_nonidle_domain(struct vcpu *v)
-{
-    /* check_wakeup_from_wait(); */
-    reset_stack_and_jump(return_from_trap);
-}
-
 void idle_loop(void)
 {
     for ( ; ; )
@@ -72,9 +61,10 @@ static void continue_new_vcpu(struct vcpu *prev)
     schedule_tail(prev);
 
     if ( is_idle_vcpu(current) )
-        continue_idle_domain(current);
+        reset_stack_and_jump(idle_loop);
     else
-        continue_nonidle_domain(current);
+        /* check_wakeup_from_wait(); */
+        reset_stack_and_jump(return_to_new_vcpu);
 }
 
 void context_switch(struct vcpu *prev, struct vcpu *next)

xen/arch/arm/entry.S

@@ -72,7 +72,9 @@ DEFINE_TRAP_ENTRY(hypervisor)
 DEFINE_TRAP_ENTRY(irq)
 DEFINE_TRAP_ENTRY(fiq)
 
-ENTRY(return_from_trap)
+return_from_trap:
+	mov sp, r11
+ENTRY(return_to_new_vcpu)
 	ldr r11, [sp, #UREGS_cpsr]
 	and r11, #PSR_MODE_MASK
 	cmp r11, #PSR_MODE_HYP
@@ -82,6 +84,7 @@ ENTRY(return_to_guest)
 	mov r11, sp
 	bic sp, #7 /* Align the stack pointer */
 	bl leave_hypervisor_tail
+	mov sp, r11
 	RESTORE_ONE_BANKED(SP_usr)
 	/* LR_usr is the same physical register as lr and is restored below */
 	RESTORE_BANKED(svc)