Index: sys/arm/arm/hypervisor-stub.S =================================================================== --- sys/arm/arm/hypervisor-stub.S +++ sys/arm/arm/hypervisor-stub.S @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2015 Mihai Carabas + * 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 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 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. + */ + +#include "assym.s" +#include +#include +#include +#include + +__FBSDID("$FreeBSD$"); + +#if __ARM_ARCH >= 7 +#if defined(__ARM_ARCH_7VE__) || defined(__clang__) +.arch_extension virt +#endif + +ASENTRY_NP(hypervisor_stub_vect_install) + + /* Install hypervisor stub vectors. */ + adr r0, hypervisor_stub_vect + mcr CP15_HVBAR(r0) @ set HVBAR + + /* Disable all the traps in the hypervisor. */ + mov r0, #0 + mcr CP15_HCR(r0) @ HCR + mcr CP15_HCPTR(r0) @ HCPTR + mcr CP15_HSTR(r0) @ HSTR + mcr CP15_HSCTLR(r0) @ HSCTLR + + /* Don't disable access to perf-mon from PL0,1 */ + mrc CP15_HDCR(r0) @ HDCR + and r0, #(ARM_CP15_HDCR_HPMN) @ Preserve HPMN + mcr CP15_HDCR(r0) @ HDCR + + 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 CP15_HVBAR(r0) @ get HVBAR + mcrne CP15_HVBAR(r0) @ 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 */ +#endif /* __ARM_ARCH >= 7 */ + Index: sys/arm/arm/locore-v6.S =================================================================== --- sys/arm/arm/locore-v6.S +++ sys/arm/arm/locore-v6.S @@ -38,7 +38,6 @@ __FBSDID("$FreeBSD$"); - #if __ARM_ARCH >= 7 #if defined(__ARM_ARCH_7VE__) || defined(__clang__) /* @@ -46,27 +45,27 @@ * when enabled. llvm >= 3.6 supports it too. */ .arch_extension virt -#define MSR_ELR_HYP(regnum) msr elr_hyp, lr -#define ERET eret -#else -#define MSR_ELR_HYP(regnum) .word (0xe12ef300 | regnum) -#define ERET .word 0xe160006e #endif #endif /* __ARM_ARCH >= 7 */ /* A small statically-allocated stack used only during initarm() and AP startup. */ #define INIT_ARM_STACK_SIZE 2048 .text .align 2 #if __ARM_ARCH >= 7 -#define LEAVE_HYP \ +#define HANDLE_HYP \ /* Leave HYP mode */ ;\ mrs r0, cpsr ;\ and r0, r0, #(PSR_MODE) /* Mode is in the low 5 bits of CPSR */ ;\ teq r0, #(PSR_HYP32_MODE) /* Hyp Mode? */ ;\ bne 1f ;\ + /* Install Hypervisor Stub Exception Vector */ ;\ + bl hypervisor_stub_vect_install ;\ + mov r0, 0 ;\ + adr r1, hypmode_enabled ;\ + str r0, [r1] ;\ /* Ensure that IRQ, FIQ and Aborts will be disabled after eret */ ;\ mrs r0, cpsr ;\ bic r0, r0, #(PSR_MODE) ;\ @@ -74,14 +73,18 @@ orr r0, r0, #(PSR_I | PSR_F | PSR_A) ;\ msr spsr_cxsf, r0 ;\ /* Exit hypervisor mode */ ;\ - adr lr, 1f ;\ + adr lr, 2f ;\ MSR_ELR_HYP(14) ;\ ERET ;\ -1: +1: ;\ + mov r0, -1 ;\ + adr r1, hypmode_enabled ;\ + str r0, [r1] ;\ +2: #else -#define LEAVE_HYP +#define HANDLE_HYP #endif /* __ARM_ARCH >= 7 */ /* * On entry for FreeBSD boot ABI: * r0 - metadata pointer or 0 (boothowto on AT91's boot2) @@ -107,8 +110,9 @@ mov r10, r2 /* Save meta data */ mov r11, r3 /* Future expansion */ - LEAVE_HYP + # If HYP-MODE is active, install an exception vector stub + HANDLE_HYP /* * Check whether data cache is enabled. If it is, then we know * current tags are valid (not power-on garbage values) and there @@ -411,20 +415,23 @@ VA_TO_PA_POINTER(Lpagetable, boot_pt1) + .global _C_LABEL(hypmode_enabled) +_C_LABEL(hypmode_enabled): + .word 0 .Lstart: .word _edata /* Note that these three items are */ .word _ebss /* loaded with a single ldmia and */ .word svcstk /* must remain in order together. */ .Lmainreturned: .asciz "main() returned" .align 2 .bss svcstk: .space INIT_ARM_STACK_SIZE * MAXCPU /* * Memory for the initial pagetable. We are unable to place this in * the bss as this will be cleared after the table is loaded. @@ -444,8 +451,8 @@ /* Make sure interrupts are disabled. */ cpsid ifa - LEAVE_HYP + HANDLE_HYP /* Setup core, disable all caches. */ mrc CP15_SCTLR(r0) bic r0, #CPU_CONTROL_MMU_ENABLE Index: sys/arm/include/armreg.h =================================================================== --- sys/arm/include/armreg.h +++ sys/arm/include/armreg.h @@ -477,4 +477,7 @@ #define THUMB_INSN_SIZE 2 /* Some are 4 bytes. */ +/* ARM Hypervisor Related Defines */ +#define ARM_CP15_HDCR_HPMN 0x0000001f + #endif /* !MACHINE_ARMREG_H */ Index: sys/arm/include/asm.h =================================================================== --- sys/arm/include/asm.h +++ sys/arm/include/asm.h @@ -235,6 +235,15 @@ #define DSB dsb #define DMB dmb #define WFI wfi + +#if defined(__ARM_ARCH_7VE__) || defined(__clang__) +#define MSR_ELR_HYP(regnum) msr elr_hyp, lr +#define ERET eret +#else +#define MSR_ELR_HYP(regnum) .word (0xe12ef300 | regnum) +#define ERET .word 0xe160006e +#endif + #elif __ARM_ARCH == 6 #define ISB mcr CP15_CP15ISB #define DSB mcr CP15_CP15DSB Index: sys/arm/include/sysreg.h =================================================================== --- sys/arm/include/sysreg.h +++ sys/arm/include/sysreg.h @@ -76,47 +76,53 @@ #define CP15_ID_MMFR2(rr) p15, 0, rr, c0, c1, 6 /* Memory Model Feature Register 2 */ #define CP15_ID_MMFR3(rr) p15, 0, rr, c0, c1, 7 /* Memory Model Feature Register 3 */ +#define CP15_HCR(rr) p15, 4, rr, c1, c1, 0 /* Hyp Configuration Register */ +#define CP15_HCPTR(rr) p15, 4, rr, c1, c1, 2 /* Hyp Coprocessor Trap Register */ +#define CP15_HSTR(rr) p15, 4, rr, c1, c1, 3 /* Hyp System Trap Register */ +#define CP15_HSCTLR(rr) p15, 4, rr, c1, c0, 0 /* Hyp System Control Register */ +#define CP15_HDCR(rr) p15, 4, rr, c1, c1, 1 /* Hyp Debug Configuration Register */ + #define CP15_ID_ISAR0(rr) p15, 0, rr, c0, c2, 0 /* Instruction Set Attribute Register 0 */ #define CP15_ID_ISAR1(rr) p15, 0, rr, c0, c2, 1 /* Instruction Set Attribute Register 1 */ #define CP15_ID_ISAR2(rr) p15, 0, rr, c0, c2, 2 /* Instruction Set Attribute Register 2 */ #define CP15_ID_ISAR3(rr) p15, 0, rr, c0, c2, 3 /* Instruction Set Attribute Register 3 */ #define CP15_ID_ISAR4(rr) p15, 0, rr, c0, c2, 4 /* Instruction Set Attribute Register 4 */ #define CP15_ID_ISAR5(rr) p15, 0, rr, c0, c2, 5 /* Instruction Set Attribute Register 5 */ #define CP15_CCSIDR(rr) p15, 1, rr, c0, c0, 0 /* Cache Size ID Registers */ #define CP15_CLIDR(rr) p15, 1, rr, c0, c0, 1 /* Cache Level ID Register */ #define CP15_AIDR(rr) p15, 1, rr, c0, c0, 7 /* Auxiliary ID Register */ #define CP15_CSSELR(rr) p15, 2, rr, c0, c0, 0 /* Cache Size Selection Register */ /* * CP15 C1 registers */ #define CP15_SCTLR(rr) p15, 0, rr, c1, c0, 0 /* System Control Register */ #define CP15_ACTLR(rr) p15, 0, rr, c1, c0, 1 /* IMPLEMENTATION DEFINED Auxiliary Control Register */ #define CP15_CPACR(rr) p15, 0, rr, c1, c0, 2 /* Coprocessor Access Control Register */ #define CP15_SCR(rr) p15, 0, rr, c1, c1, 0 /* Secure Configuration Register */ #define CP15_SDER(rr) p15, 0, rr, c1, c1, 1 /* Secure Debug Enable Register */ #define CP15_NSACR(rr) p15, 0, rr, c1, c1, 2 /* Non-Secure Access Control Register */ /* * CP15 C2 registers */ #define CP15_TTBR0(rr) p15, 0, rr, c2, c0, 0 /* Translation Table Base Register 0 */ #define CP15_TTBR1(rr) p15, 0, rr, c2, c0, 1 /* Translation Table Base Register 1 */ #define CP15_TTBCR(rr) p15, 0, rr, c2, c0, 2 /* Translation Table Base Control Register */ /* * CP15 C3 registers */ #define CP15_DACR(rr) p15, 0, rr, c3, c0, 0 /* Domain Access Control Register */ /* * CP15 C5 registers */ #define CP15_DFSR(rr) p15, 0, rr, c5, c0, 0 /* Data Fault Status Register */ #if __ARM_ARCH >= 6 /* From ARMv6: */ #define CP15_IFSR(rr) p15, 0, rr, c5, c0, 1 /* Instruction Fault Status Register */ @@ -265,7 +271,8 @@ #define CP15_MVBAR(rr) p15, 0, rr, c12, c0, 1 /* Monitor Vector Base Address Register */ #define CP15_ISR(rr) p15, 0, rr, c12, c1, 0 /* Interrupt Status Register */ +#define CP15_HVBAR(rr) p15, 4, rr, c12, c0, 0 /* Hyp Vector Base Address Register*/ /* * CP15 C13 registers */ Index: sys/conf/files.arm =================================================================== --- sys/conf/files.arm +++ sys/conf/files.arm @@ -67,6 +67,7 @@ arm/arm/intr.c optional !intrng kern/subr_intr.c optional intrng arm/arm/locore.S standard no-obj +arm/arm/hypervisor-stub.S optional armv6 arm/arm/machdep.c standard arm/arm/machdep_boot.c standard arm/arm/machdep_kdb.c standard