[PATCH 3/3] bsps/arm: Move HYP to SVC change to start.S

[Sebastian Huber]

This fixes the corruption of r3 by the call to
bsp_start_arm_drop_hyp_mode().

Moving the code makes it easier to review changes in start.S.

Close #3773.
---
 bsps/arm/shared/start/bsp-start-in-hyp-support.S | 76 ------------------------
 bsps/arm/shared/start/start.S                    | 42 ++++++++++++-
 c/src/lib/libbsp/arm/raspberrypi/Makefile.am     |  3 -
 3 files changed, 40 insertions(+), 81 deletions(-)
 delete mode 100644 bsps/arm/shared/start/bsp-start-in-hyp-support.S

diff --git a/bsps/arm/shared/start/bsp-start-in-hyp-support.S b/bsps/arm/shared/start/bsp-start-in-hyp-support.S
deleted file mode 100644
index 5d3d5921c5..0000000000
--- a/bsps/arm/shared/start/bsp-start-in-hyp-support.S
+++ /dev/null
@@ -1,76 +0,0 @@
-/**
- * @file
- *
- * @brief Boot and system start code.
- */
-
- /*
- * Copyright (c) 2016 Pavel Pisa <pisa at cmp.felk.cvut.cz>
- *
- * Czech Technical University in Prague
- * Zikova 1903/4
- * 166 36 Praha 6
- * Czech Republic
- *
- * The license and distribution terms for this file may be
- * found in the file LICENSE in this distribution or at
- * http://www.rtems.org/license/LICENSE.
- */
-
-#include <rtems/asm.h>
-#include <rtems/score/cpu.h>
-
-#include <bspopts.h>
-#include <bsp/irq.h>
-#include <bsp/linker-symbols.h>
-
-	.extern	bsp_start_hyp_vector_table_begin
-	.globl	bsp_start_arm_drop_hyp_mode
-	.globl	bsp_arm_drop_hyp_mode_only
-
-.arm
-
-/*
- * The routine is called from startup code and it should
- * preserve all registers except r2 and r3. r0 can be used
- * as pass though argument in some cases, a1 is used for
- * CPU stack offset during startup and r4 to r6 to preserve
- * booloader arguments
- */
-
-bsp_start_arm_drop_hyp_mode:
-	ldr	r2, bsp_start_hyp_vector_table_begin_addr
-	mcr	p15, 4, r2, c12, c0, 0
-
-	mov	r2, #0
-	mcr	p15, 4, r2, c1, c1, 0
-	mcr	p15, 4, r2, c1, c1, 2
-	mcr	p15, 4, r2, c1, c1, 3
-/*
- * HSCTLR.TE
- * optional start of hypervisor handlers in Thumb mode
- *	orr	r0, #(1 << 30)
- */
-	mcr	p15, 4, r2, c1, c0, 0	/* HSCTLR */
-	mrc	p15, 4, r2, c1, c1, 1	/* HDCR */
-	and	r2, #0x1f		/* Preserve HPMN */
-	mcr	p15, 4, r2, c1, c1, 1	/* HDCR */
-
-bsp_arm_drop_hyp_mode_only:
-	/* Prepare SVC mode for eret */
-	mrs	r2, cpsr
-	bic	r2, r2, #ARM_PSR_M_MASK
-	orr	r2, r2, #ARM_PSR_M_SVC
-	msr	spsr_cxsf, r2
-
-	adr	r2, 1f
-	.inst 0xe12ef302	/* msr ELR_hyp, r2 */
-	mov	r2, sp
-	mov	r3, lr
-	.inst 0xe160006e	/* eret */
-1:	mov	sp, r2
-	mov	lr, r3
-	bx	lr
-
-bsp_start_hyp_vector_table_begin_addr:
-	.word	bsp_start_hyp_vector_table_begin
diff --git a/bsps/arm/shared/start/start.S b/bsps/arm/shared/start/start.S
index 1f6d38f564..0e9e6a0cde 100644
--- a/bsps/arm/shared/start/start.S
+++ b/bsps/arm/shared/start/start.S
@@ -13,6 +13,13 @@
  *  Germany
  *  <rtems at embedded-brains.de>
  *
+ * Copyright (c) 2016 Pavel Pisa <pisa at cmp.felk.cvut.cz>
+ *
+ *  Czech Technical University in Prague
+ *  Zikova 1903/4
+ *  166 36 Praha 6
+ *  Czech Republic
+ *
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
  * http://www.rtems.org/license/LICENSE.
@@ -200,13 +207,44 @@ _start:
 	bne	.L_skip_hyp_svc_switch
 
 	/* Boot loader starts kernel in HYP mode, switch to SVC necessary */
+
 	ldr	r1, =bsp_stack_hyp_size
 	mov	sp, r3
 	sub	r3, r3, r1
-	bl	bsp_start_arm_drop_hyp_mode
+
+	ldr	r2, =bsp_start_hyp_vector_table_begin
+	mcr	p15, 4, r2, c12, c0, 0
+
+	mov	r2, #0
+	mcr	p15, 4, r2, c1, c1, 0
+	mcr	p15, 4, r2, c1, c1, 2
+	mcr	p15, 4, r2, c1, c1, 3
+/*
+ * HSCTLR.TE
+ * optional start of hypervisor handlers in Thumb mode
+ *	orr	r0, #(1 << 30)
+ */
+	mcr	p15, 4, r2, c1, c0, 0	/* HSCTLR */
+	mrc	p15, 4, r2, c1, c1, 1	/* HDCR */
+	and	r2, #0x1f		/* Preserve HPMN */
+	mcr	p15, 4, r2, c1, c1, 1	/* HDCR */
+
+	/* Prepare SVC mode for eret */
+	mrs	r2, cpsr
+	bic	r2, r2, #ARM_PSR_M_MASK
+	orr	r2, r2, #ARM_PSR_M_SVC
+	msr	spsr_cxsf, r2
+
+	adr	r2, .L_hyp_to_svc_return
+	.inst 0xe12ef302	/* msr ELR_hyp, r2 */
+	mov	r2, sp
+	.inst 0xe160006e	/* eret */
+.L_hyp_to_svc_return:
+	mov	sp, r2
 
 .L_skip_hyp_svc_switch:
-#endif
+#endif /* BSP_START_IN_HYP_SUPPORT */
+
 	/* Initialize stack pointer registers for the various modes */
 
 	/* Enter FIQ mode and set up the FIQ stack pointer */
diff --git a/c/src/lib/libbsp/arm/raspberrypi/Makefile.am b/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
index 14a348411d..11a22f89e3 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
+++ b/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
@@ -45,9 +45,6 @@ librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/start/sbrk.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/start/stackalloc.c
 librtemsbsp_a_SOURCES += ../../../../../../bsps/arm/shared/start/bsp-start-memcpy.S
 librtemsbsp_a_SOURCES += ../../../../../../bsps/arm/shared/cp15/arm-cp15-set-ttb-entries.c
-if BSP_START_IN_HYP_SUPPORT
-librtemsbsp_a_SOURCES += ../../../../../../bsps/arm/shared/start/bsp-start-in-hyp-support.S
-endif
 
 # Startup
 librtemsbsp_a_SOURCES += ../../../../../../bsps/arm/raspberrypi/start/bspstart.c
-- 
2.16.4