#include "assym.s"
#include <sys/syscall.h>
#include <machine/asm.h>
#include <machine/asmacros.h>
#include <machine/armreg.h>
#include <machine/sysreg.h>
#include <machine/cpuconf.h>

ASENTRY_NP(hypervisor_stub_vect_install)

	/* If we are not in SVC mode than return */
	mrs	r0, cpsr
	and	r0, r0, #(PSR_MODE)
	cmp	r0, #(PSR_HYP32_MODE)
	mov	r0, #-1
	movne	pc, lr

	/* Install hypervisor stub vectors. */
	ldr	r0, =hypervisor_stub_vect
	mcr	p15, 4, r0, c12, c0, 0	@ set HVBAR

	/* Disable all the traps in the hypervisor. */
	mov	r0, #0
	mcr	p15, 4, r0, c1, c1, 0	@ HCR
	mcr	p15, 4, r0, c1, c1, 2	@ HCPTR
	mcr	p15, 4, r0, c1, c1, 3	@ HSTR
	mcr	p15, 4, r0, c1, c0, 0	@ HSCTLR

	/* Don't disable access to perf-mon from PL0,1 */ 
	mrc	p15, 4, r0, c1, c1, 1	@ HDCR
	and	r0, #0x1f		@ Preserve HPMN
	mcr	p15, 4, r0, c1, c1, 1	@ HDCR

	mov	r0, #0
	mov	pc, lr
END(hypervisor_stub_vect_install)

ASENTRY_NP(hypervisor_stub_trap)
	/*
	 * If the first parameter is -1 than return the
	 * exception vector (HVBAR), otherwise set it to
	 * the value of it.
	 */
	cmp	r0, #-1
	mrceq	p15, 4, r0, c12, c0, 0	@ get HVBAR
	mcrne	p15, 4, r0, c12, c0, 0	@ set HVBAR
	eret
END(hypervisor_stub_trap)

	.globl hypervisor_stub_vect
	.align 5
_C_LABEL(hypervisor_stub_vect):
	.word 0 /* Reset */
	.word 0 /* undev */
	.word 0 /* SMC */
	.word 0 /* PABT */
	.word 0 /* DABT */
	b	hypervisor_stub_trap /* HYP-Mode */
	.word 0 /* FIQ */
	.word 0 /* IRQ */
