Merge branch 'master' of git://git.denx.de/u-boot-video

[platform/kernel/u-boot.git] / arch / arm / cpu / armv7 / virt-v7.c

diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
index a0b8742..2cd604f 100644 (file)
--- a/arch/arm/cpu/armv7/virt-v7.c
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -1,27 +1,12 @@
 /*
  * (C) Copyright 2013
- * Andre Przywara, Linaro
+ * Andre Przywara, Linaro <andre.przywara@linaro.org>
  *
  * 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
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
+ * SPDX-License-Identifier:    GPL-2.0+
  */
 
 #include <common.h>
@@ -31,6 +16,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 +83,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;