[PATCH v3 7/8] xen/arm: introduce vexpress_syscfg

[Stefano Stabellini <stefano.stabellini@eu.citrix.com>]

Introduce a Versatile Express specific function to read/write
motherboard settings.

Signed-off-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
---
 xen/arch/arm/Makefile                   |    1 +
 xen/arch/arm/platform_vexpress.c        |   97 +++++++++++++++++++++++++++++++
 xen/include/asm-arm/platform_vexpress.h |   23 +++++++
 3 files changed, 121 insertions(+), 0 deletions(-)
 create mode 100644 xen/arch/arm/platform_vexpress.c

diff --git a/xen/arch/arm/Makefile b/xen/arch/arm/Makefile
index 4c61b04..24689c5 100644
--- a/xen/arch/arm/Makefile
+++ b/xen/arch/arm/Makefile
@@ -18,6 +18,7 @@ obj-y += p2m.o
 obj-y += percpu.o
 obj-y += guestcopy.o
 obj-y += physdev.o
+obj-y += platform_vexpress.o
 obj-y += setup.o
 obj-y += time.o
 obj-y += smpboot.o
diff --git a/xen/arch/arm/platform_vexpress.c b/xen/arch/arm/platform_vexpress.c
new file mode 100644
index 0000000..41e3806
--- /dev/null
+++ b/xen/arch/arm/platform_vexpress.c
@@ -0,0 +1,97 @@
+/*
+ * xen/arch/arm/platform_vexpress.c
+ *
+ * Versatile Express specific settings
+ *
+ * Stefano Stabellini <stefano.stabellini@eu.citrix.com>
+ * Copyright (c) 2012 Citrix Systems.
+ *
+ * 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.
+ */
+
+#include <asm/platform_vexpress.h>
+#include <xen/mm.h>
+
+#define DCC_SHIFT      26
+#define FUNCTION_SHIFT 20
+#define SITE_SHIFT     16
+#define POSITION_SHIFT 12
+#define DEVICE_SHIFT   0
+
+int vexpress_syscfg(int write, int function, int device, uint32_t *data)
+{
+    uint32_t *syscfg = (uint32_t *) FIXMAP_ADDR(FIXMAP_MISC);
+    uint32_t stat;
+    int dcc = 0; /* DCC to access */
+    int site = 0; /* motherboard */
+    int position = 0; /* motherboard */
+
+    set_fixmap(FIXMAP_MISC, V2M_SYS_MMIO_BASE >> PAGE_SHIFT, DEV_SHARED);
+
+    if ( syscfg[V2M_SYS_CFGCTRL] & V2M_SYS_CFG_START )
+        return -1;
+
+    /* clear the complete bit in the V2M_SYS_CFGSTAT status register */
+    syscfg[V2M_SYS_CFGSTAT] = 0;
+
+    if ( write )
+    {
+        /* write data */
+        syscfg[V2M_SYS_CFGDATA] = *data;
+
+        /* set control register */
+        syscfg[V2M_SYS_CFGCTRL] = V2M_SYS_CFG_START | V2M_SYS_CFG_WRITE |
+            (dcc << DCC_SHIFT) | (function << FUNCTION_SHIFT) |
+            (site << SITE_SHIFT) | (position << POSITION_SHIFT) |
+            (device << DEVICE_SHIFT);
+
+        /* wait for complete flag to be set */
+        do {
+            stat = syscfg[V2M_SYS_CFGSTAT];
+            dsb();
+        } while ( !(stat & V2M_SYS_CFG_COMPLETE) );
+
+        /* check error status and return error flag if set */
+        if ( stat & V2M_SYS_CFG_ERROR )
+            return -1;
+    } else {
+        /* set control register */
+        syscfg[V2M_SYS_CFGCTRL] = V2M_SYS_CFG_START | (dcc << DCC_SHIFT) |
+            (function << FUNCTION_SHIFT) | (site << SITE_SHIFT) |
+            (position << POSITION_SHIFT) | (device << DEVICE_SHIFT);
+
+        /* wait for complete flag to be set */
+        do {
+            stat = syscfg[V2M_SYS_CFGSTAT];
+            dsb();
+        } while ( !(stat & V2M_SYS_CFG_COMPLETE) );
+
+        /* check error status flag and return error flag if set */
+        if ( stat & V2M_SYS_CFG_ERROR )
+            return -1;
+        else
+            /* read data */
+            *data = syscfg[V2M_SYS_CFGDATA];
+    }
+
+    clear_fixmap(FIXMAP_MISC);
+
+    return 0;
+}
+
+/*
+ * Local variables:
+ * mode: C
+ * c-set-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
diff --git a/xen/include/asm-arm/platform_vexpress.h b/xen/include/asm-arm/platform_vexpress.h
index 3556af3..407602d 100644
--- a/xen/include/asm-arm/platform_vexpress.h
+++ b/xen/include/asm-arm/platform_vexpress.h
@@ -6,6 +6,29 @@
 #define V2M_SYS_FLAGSSET      (0x30)
 #define V2M_SYS_FLAGSCLR      (0x34)
 
+#define V2M_SYS_CFGDATA       (0x00A0/4)
+#define V2M_SYS_CFGCTRL       (0x00A4/4)
+#define V2M_SYS_CFGSTAT       (0x00A8/4)
+
+#define V2M_SYS_CFG_START     (1<<31)
+#define V2M_SYS_CFG_WRITE     (1<<30)
+#define V2M_SYS_CFG_ERROR     (1<<1)
+#define V2M_SYS_CFG_COMPLETE  (1<<0)
+
+#define V2M_SYS_CFG_OSC_FUNC  1
+#define V2M_SYS_CFG_OSC0      0
+#define V2M_SYS_CFG_OSC1      1
+#define V2M_SYS_CFG_OSC2      2
+#define V2M_SYS_CFG_OSC3      3
+#define V2M_SYS_CFG_OSC4      4
+#define V2M_SYS_CFG_OSC5      5
+
+#ifndef __ASSEMBLY__
+#include <xen/inttypes.h>
+
+int vexpress_syscfg(int write, int function, int device, uint32_t *data);
+#endif
+
 #endif /* __ASM_ARM_PLATFORM_H */
 /*
  * Local variables:
-- 
1.7.2.5