From 524e598c16e4dd9191e6e106a30fe8e8437f1f5a Mon Sep 17 00:00:00 2001
From: Tim Deegan <tim@xen.org>
Date: Fri, 1 Jun 2012 10:20:40 +0100
Subject: [PATCH] arm: Enable VFP at boot

Signed-off-by: Tim Deegan <tim@xen.org>
Acked-by: Ian Campbell <ian.campbell@citrix.com>
Committed-by: Ian Campbell <ian.campbell@citrix.com>
---
 xen/arch/arm/Rules.mk     |  2 +-
 xen/arch/arm/setup.c      |  3 +++
 xen/arch/arm/smpboot.c    |  3 +++
 xen/include/asm-arm/vfp.h | 35 +++++++++++++++++++++++++++++++++++
 4 files changed, 42 insertions(+), 1 deletion(-)
 create mode 100644 xen/include/asm-arm/vfp.h

diff --git a/xen/arch/arm/Rules.mk b/xen/arch/arm/Rules.mk
index 14082dd0a7..c61cc67e0a 100644
--- a/xen/arch/arm/Rules.mk
+++ b/xen/arch/arm/Rules.mk
@@ -24,7 +24,7 @@ ifneq ($(call cc-option,$(CC),-fvisibility=hidden,n),n)
 CFLAGS += -DGCC_HAS_VISIBILITY_ATTRIBUTE
 endif
 
-CFLAGS += -march=armv7-a -mcpu=cortex-a15
+CFLAGS += -march=armv7-a -mcpu=cortex-a15 -mfpu=vfpv3 -mfloat-abi=softfp
 
 # Require GCC v3.4+ (to avoid issues with alignment constraints in Xen headers)
 check-$(gcc) = $(call cc-ver-check,CC,0x030400,"Xen requires at least gcc-3.4")
diff --git a/xen/arch/arm/setup.c b/xen/arch/arm/setup.c
index 33a2f7b251..0df3c1a2b7 100644
--- a/xen/arch/arm/setup.c
+++ b/xen/arch/arm/setup.c
@@ -36,6 +36,7 @@
 #include <asm/page.h>
 #include <asm/current.h>
 #include <asm/setup.h>
+#include <asm/vfp.h>
 #include "gic.h"
 
 static __attribute_used__ void init_done(void)
@@ -192,6 +193,8 @@ void __init start_xen(unsigned long boot_phys_offset,
 
     processor_id();
 
+    enable_vfp();
+
     softirq_init();
 
     tasklet_subsys_init();
diff --git a/xen/arch/arm/smpboot.c b/xen/arch/arm/smpboot.c
index 9b3f18cdf0..ea05afc2f5 100644
--- a/xen/arch/arm/smpboot.c
+++ b/xen/arch/arm/smpboot.c
@@ -26,6 +26,7 @@
 #include <xen/sched.h>
 #include <xen/smp.h>
 #include <xen/softirq.h>
+#include <asm/vfp.h>
 #include "gic.h"
 
 cpumask_t cpu_online_map;
@@ -106,6 +107,8 @@ void __cpuinit start_secondary(unsigned long boot_phys_offset,
     WRITE_CP32((uint32_t) hyp_traps_vector, HVBAR);
 
     mmu_init_secondary_cpu();
+    enable_vfp();
+
     gic_init_secondary_cpu();
     init_timer_interrupt();
     gic_route_irqs();
diff --git a/xen/include/asm-arm/vfp.h b/xen/include/asm-arm/vfp.h
new file mode 100644
index 0000000000..0bab2a8149
--- /dev/null
+++ b/xen/include/asm-arm/vfp.h
@@ -0,0 +1,35 @@
+#ifndef __ARM_VFP_H_
+#define __ARM_VFP_H_
+
+#include <xen/types.h>
+
+#define FPEXC_EN (1u << 30)
+
+/* Save and restore FP state.
+ * Ought to be using the new vmrs/vmsr names, but older binutils has a
+ * bug where it only allows them to target fpscr (and not, say, fpexc). */
+#define READ_FP(reg) ({                                 \
+    uint32_t val;                                       \
+    asm volatile ("fmrx %0, fp" #reg : "=r" (val));     \
+    val; })
+
+#define WRITE_FP(reg, val) do {                         \
+    asm volatile ("fmxr fp" #reg ", %0" : : "r" (val)); \
+} while (0)
+
+
+/* Start-of-day: Turn on VFP */
+static inline void enable_vfp(void)
+{
+    WRITE_FP(exc, READ_FP(exc) | FPEXC_EN);
+}
+
+#endif
+/*
+ * Local variables:
+ * mode: C
+ * c-set-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
-- 
2.39.5