Index: c/src/lib/libbsp/arm/shared/include/linker-symbols.h
===================================================================
--- c/src/lib/libbsp/arm/shared/include/linker-symbols.h	(revision 8add2b6c6fe178d371b7dadf441b2a36cc55ee0c)
+++ c/src/lib/libbsp/arm/shared/include/linker-symbols.h	(revision f9c1e1172e9adb2616e761f2e3d671361c7971ba)
@@ -59,4 +59,8 @@
 LINKER_SYMBOL(bsp_stack_und_end)
 LINKER_SYMBOL(bsp_stack_und_size)
+
+LINKER_SYMBOL(bsp_stack_hyp_begin)
+LINKER_SYMBOL(bsp_stack_hyp_end)
+LINKER_SYMBOL(bsp_stack_hyp_size)
 
 LINKER_SYMBOL(bsp_stack_svc_begin)
Index: c/src/lib/libbsp/arm/shared/include/start.h
===================================================================
--- c/src/lib/libbsp/arm/shared/include/start.h	(revision 8add2b6c6fe178d371b7dadf441b2a36cc55ee0c)
+++ c/src/lib/libbsp/arm/shared/include/start.h	(revision f9c1e1172e9adb2616e761f2e3d671361c7971ba)
@@ -46,4 +46,13 @@
 #define BSP_START_DATA_SECTION __attribute__((section(".bsp_start_data")))
 
+/*
+* Many ARM boot loaders pass arguments to loaded OS kernel
+*/
+#ifdef BSP_START_HOOKS_WITH_LOADER_ARGS
+#define BSP_START_HOOKS_LOADER_ARGS int saved_psr, int saved_machid, int saved_dtb_adr
+#else
+#define BSP_START_HOOKS_LOADER_ARGS void
+#endif
+
 /**
 * @brief System start entry.
@@ -58,5 +67,5 @@
 * vectors.
 */
-void bsp_start_hook_0(void);
+void bsp_start_hook_0(BSP_START_HOOKS_LOADER_ARGS);
 
 /**
@@ -66,5 +75,5 @@
 * exception vectors but before the call to boot_card().
 */
-void bsp_start_hook_1(void);
+void bsp_start_hook_1(BSP_START_HOOKS_LOADER_ARGS);
 
 /**
Index: c/src/lib/libbsp/arm/shared/start/start.S
===================================================================
--- c/src/lib/libbsp/arm/shared/start/start.S	(revision 8add2b6c6fe178d371b7dadf441b2a36cc55ee0c)
+++ c/src/lib/libbsp/arm/shared/start/start.S	(revision f9c1e1172e9adb2616e761f2e3d671361c7971ba)
@@ -55,4 +55,9 @@
 #endif
 
+#ifdef BSP_START_IN_HYP_SUPPORT
+	.extern	bsp_start_arm_drop_hyp_mode
+	.globl	bsp_start_hyp_vector_table_begin
+#endif
+
 	/* Global symbols */
 	.globl	_start
@@ -125,4 +130,42 @@
 bsp_start_vector_table_end:
 
+#ifdef BSP_START_IN_HYP_SUPPORT
+bsp_start_hyp_vector_table_begin:
+	ldr	pc, handler_addr_hyp_reset
+	ldr	pc, handler_addr_hyp_undef
+	ldr	pc, handler_addr_hyp_swi
+	ldr	pc, handler_addr_hyp_prefetch
+	ldr	pc, handler_addr_hyp_abort
+	ldr	pc, handler_addr_hyp_hyp
+	ldr	pc, handler_addr_hyp_irq
+	ldr	pc, handler_addr_hyp_fiq
+
+handler_addr_hyp_reset:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_undef:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_swi:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_prefetch:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_abort:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_hyp:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_irq:
+	.word	_ARMV4_Exception_reserved_default
+
+handler_addr_hyp_fiq:
+	.word	_ARMV4_Exception_reserved_default
+
+bsp_start_hyp_vector_table_end:
+#endif
+
 /* Start entry */
 
@@ -131,8 +174,10 @@
 	/*
 	 * We do not save the context since we do not return to the boot
-	 * loader.
-	 */
-
-#ifdef BSP_START_NEEDS_REGISTER_INITIALIZATION
+	 * loader but preserve r1 and r2 to allow access to bootloader parameters
+	 */
+#ifndef BSP_START_NEEDS_REGISTER_INITIALIZATION
+	mov	r5, r1		/* machine type number or ~0 for DT boot */
+	mov	r6, r2		/* physical address of ATAGs or DTB */
+#else /* BSP_START_NEEDS_REGISTER_INITIALIZATION */
 	bl bsp_start_init_registers_core
 #endif
@@ -148,4 +193,22 @@
 #endif
 
+	mrs	r4, cpsr	/* save original procesor status value  */
+#ifdef BSP_START_IN_HYP_SUPPORT
+	orr	r0, r4, #(ARM_PSR_I | ARM_PSR_F)
+	msr	cpsr, r4
+
+	and	r0, r4, #ARM_PSR_M_MASK
+	cmp	r0, #ARM_PSR_M_HYP
+	bne	bsp_start_skip_hyp_svc_switch
+
+	/* Boot loader stats kernel in HYP mode, switch to SVC necessary */
+	ldr	sp, =bsp_stack_hyp_end
+#ifdef RTEMS_SMP
+	add	sp, r1
+#endif
+	bl	bsp_start_arm_drop_hyp_mode
+
+bsp_start_skip_hyp_svc_switch:
+#endif
 	/*
 	 * Set SVC mode, disable interrupts and enable ARM instructions.
@@ -248,4 +311,8 @@
 	SWITCH_FROM_ARM_TO_THUMB	r0
 
+	mov	r0, r4		/* original cpsr value */
+	mov	r1, r5		/* machine type number or ~0 for DT boot */
+	mov	r2, r6		/* physical address of ATAGs or DTB */
+
 	b	bsp_start_hook_0
 
@@ -258,4 +325,6 @@
 	 * vectors and the pointers to the default exception handlers.
 	 */
+
+	stmdb	sp!, {r4, r5, r6}
 
 	ldr	r0, =bsp_vector_table_begin
@@ -270,5 +339,7 @@
 bsp_vector_table_copy_done:
 
-	SWITCH_FROM_ARM_TO_THUMB	r0
+	ldmia	sp!, {r0, r1, r2}
+
+	SWITCH_FROM_ARM_TO_THUMB	r3
 
 	/* Branch to start hook 1 */
Index: c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S
===================================================================
--- c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S	(revision f9c1e1172e9adb2616e761f2e3d671361c7971ba)
+++ c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S	(revision f9c1e1172e9adb2616e761f2e3d671361c7971ba)
@@ -0,0 +1,77 @@
+/**
+ * @file
+ *
+ * @brief Boot and system start code.
+ */
+
+ /*
+ * Copyright (c) 2016 Pavel Pisa <pisa@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/system.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
Index: c/src/lib/libbsp/arm/shared/startup/linkcmds.base
===================================================================
--- c/src/lib/libbsp/arm/shared/startup/linkcmds.base	(revision 8add2b6c6fe178d371b7dadf441b2a36cc55ee0c)
+++ c/src/lib/libbsp/arm/shared/startup/linkcmds.base	(revision f9c1e1172e9adb2616e761f2e3d671361c7971ba)
@@ -54,8 +54,11 @@
 bsp_stack_und_size = ALIGN (bsp_stack_und_size, bsp_stack_align);
 
+bsp_stack_hyp_size = DEFINED (bsp_stack_hyp_size) ? bsp_stack_hyp_size : 0;
+bsp_stack_hyp_size = ALIGN (bsp_stack_hyp_size, bsp_stack_align);
+
 bsp_stack_main_size = DEFINED (bsp_stack_main_size) ? bsp_stack_main_size : 0;
 bsp_stack_main_size = ALIGN (bsp_stack_main_size, bsp_stack_align);
 
-bsp_stack_all_size = bsp_stack_abt_size + bsp_stack_fiq_size + bsp_stack_irq_size + bsp_stack_svc_size + bsp_stack_und_size + bsp_stack_main_size;
+bsp_stack_all_size = bsp_stack_abt_size + bsp_stack_fiq_size + bsp_stack_irq_size + bsp_stack_svc_size + bsp_stack_und_size + bsp_stack_hyp_size + bsp_stack_main_size;
 
 bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 1;
@@ -312,4 +315,8 @@
 		. = . + bsp_stack_und_size;
 		bsp_stack_und_end = .;
+
+		bsp_stack_hyp_begin = .;
+		. = . + bsp_stack_hyp_size;
+		bsp_stack_hyp_end = .;
 
 		bsp_stack_abt_begin = .;
