armv8-switch-el2-to-el1.s

/*-
 * Copyright (c) 2012-2014 Andrew Turner
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

	.text
	.globl	start
start:
	mrs	x1, CurrentEL
	lsr	x1, x1, #2
	cmp	x1, #0x2
	b.eq	1f
	ret
1:
	/* Configure the Hypervisor */
	//ldr	x2, =((1 << 29) | (1 << 31))	/* Remain in 64-bit */
	ldr	x2, =(1 << 29)			/* Switch to 32-bit */
	msr	hcr_el2, x2

	/* Load the Virtualization Process ID Register */
	mrs	x2, midr_el1
	msr	vpidr_el2, x2

	/* Load the Virtualization Multiprocess ID Register */
	mrs	x2, mpidr_el1
	msr	vmpidr_el2, x2

	/* Set the bits that need to be 1 in sctlr_el1 */
	ldr	x2, .Lsctlr_res1
	msr	sctlr_el1, x2

	/* Don't trap to EL2 for exceptions */
	mov	x2, #0x000033ff
	msr	cptr_el2, x2

	/* Don't trap to EL2 for CP15 traps */
	msr	hstr_el2, xzr

	/* Enable access to the physical timers at EL1 */
	mrs	x2, cnthctl_el2
	orr	x2, x2, #(1 | 2)
	msr	cnthctl_el2, x2

	/* Set the counter offset to a known value */
	msr	cntvoff_el2, xzr

	/* Hypervisor trap functions */
	adr	x2, hyp_vectors
	msr	vbar_el2, x2

	//mov	x2, #(0x40 | 0x80 | 0x100 | 0x200 | 0x5)	/* Remain in 64-bit */
	mov	x2, #(0x40 | 0x80 | 0x100 | 0x10 | 0x3)		/* Switch to 32-bit */
	msr	spsr_el2, x2

	/* Configure GICv3 CPU interface */
	mrs	x2, id_aa64pfr0_el1
	/* Extract GIC bits from the register */
	ubfx	x2, x2, #24, #4
	/* GIC[3:0] == 0001 - GIC CPU interface via special regs. supported */
	cmp	x2, #0x1
	b.ne	2f

	//mrs	x2, icc_sre_el2
	mrs	x2, s3_4_c12_c9_5
	orr	x2, x2, #(1 << 3)	/* Enable access from insecure EL1 */
	orr	x2, x2, #1		/* Enable system registers */
	//msr	icc_sre_el2, x2
	msr	s3_4_c12_c9_5, x2

2:
	//msr	elr_el2, x30
	//mov	x2, #0x11000000
	ldr	x2, =0x11000100
	msr	elr_el2, x2
	isb

	eret

	.align	3
.Lsctlr_res1:
	.quad	0x30d00800

	.align	11
hyp_vectors:
	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b

	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b

	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b

	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b
	.align 7; 1: b 1b