ARM: extend non-secure switch to also go into HYP mode

For the KVM and XEN hypervisors to be usable, we need to enter the
kernel in HYP mode. Now that we already are in non-secure state,
HYP mode switching is within short reach.

While doing the non-secure switch, we have to enable the HVC
instruction and setup the HYP mode HVBAR (while still secure).

The actual switch is done by dropping back from a HYP mode handler
without actually leaving HYP mode, so we introduce a new handler
routine in our new secure exception vector table.

In the assembly switching routine we save and restore the banked LR
and SP registers around the hypercall to do the actual HYP mode
switch.

The C routine first checks whether we are in HYP mode already and
also whether the virtualization extensions are available. It also
checks whether the HYP mode switch was finally successful.
The bootm command part only calls the new function after the
non-secure switch.

Signed-off-by: Andre Przywara <andre.przywara@linaro.org>

1 files changed, 37 insertions, 0 deletions

diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
index a0b8742464..6de7fe7813 100644
--- a/arch/arm/cpu/armv7/virt-v7.c
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -3,6 +3,7 @@
  * Andre Przywara, Linaro
  *
  * Routines to transition ARMv7 processors from secure into non-secure state
+ * and from non-secure SVC into HYP mode
  * needed to enable ARMv7 virtualization for current hypervisors
  *
  * See file CREDITS for list of people who contributed to this
@@ -31,6 +32,14 @@
 
 unsigned long gic_dist_addr;
 
+static unsigned int read_cpsr(void)
+{
+	unsigned int reg;
+
+	asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
+	return reg;
+}
+
 static unsigned int read_id_pfr1(void)
 {
 	unsigned int reg;
@@ -90,6 +99,34 @@ void __weak smp_kick_all_cpus(void)
 	kick_secondary_cpus_gic(gic_dist_addr);
 }
 
+int armv7_switch_hyp(void)
+{
+	unsigned int reg;
+
+	/* check whether we are in HYP mode already */
+	if ((read_cpsr() & 0x1f) == 0x1a) {
+		debug("CPU already in HYP mode\n");
+		return 0;
+	}
+
+	/* check whether the CPU supports the virtualization extensions */
+	reg = read_id_pfr1();
+	if ((reg & CPUID_ARM_VIRT_MASK) != 1 << CPUID_ARM_VIRT_SHIFT) {
+		printf("HYP mode: Virtualization extensions not implemented.\n");
+		return -1;
+	}
+
+	/* call the HYP switching code on this CPU also */
+	_switch_to_hyp();
+
+	if ((read_cpsr() & 0x1F) != 0x1a) {
+		printf("HYP mode: switch not successful.\n");
+		return -1;
+	}
+
+	return 0;
+}
+
 int armv7_switch_nonsec(void)
 {
 	unsigned int reg;