Merge branch 'for-next/vexpress'

4 files changed, 102 insertions, 7 deletions

diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index dcb52bf73d..7d7a6541ac 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -104,7 +104,7 @@ int amba_device_add(struct amba_device *dev)
 {
 	u32 size;
 	void __iomem *tmp;
-	int i, ret;
+	int ret;
 	struct resource *res = NULL;
 
 	dev->dev.bus = &amba_bustype;
@@ -135,12 +135,8 @@ int amba_device_add(struct amba_device *dev)
 		 * Read pid and cid based on size of resource
 		 * they are located at end of region
 		 */
-		for (pid = 0, i = 0; i < 4; i++)
-			pid |= (readl(tmp + size - 0x20 + 4 * i) & 255) <<
-				(i * 8);
-		for (cid = 0, i = 0; i < 4; i++)
-			cid |= (readl(tmp + size - 0x10 + 4 * i) & 255) <<
-				(i * 8);
+		pid = amba_device_get_pid(tmp, size);
+		cid = amba_device_get_cid(tmp, size);
 
 		if (cid == AMBA_CID)
 			dev->periphid = pid;
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 09acdd74c5..3f27cf2430 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -1,3 +1,7 @@
+config AMBA_SP804
+	bool
+	depends on ARM_AMBA
+
 config ARM_SMP_TWD
 	bool
 	depends on ARM && CPU_V7
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 92d7c1344a..b0bc8bd7d2 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -1,3 +1,4 @@
+obj-$(CONFIG_AMBA_SP804) += amba-sp804.o
 obj-$(CONFIG_ARM_SMP_TWD) += arm_smp_twd.o
 obj-$(CONFIG_CLOCKSOURCE_BCM2835) += bcm2835.o
 obj-$(CONFIG_CLOCKSOURCE_NOMADIK) += nomadik.o
diff --git a/drivers/clocksource/amba-sp804.c b/drivers/clocksource/amba-sp804.c
new file mode 100644
index 0000000000..fedcb64839
--- /dev/null
+++ b/drivers/clocksource/amba-sp804.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2012 Jean-Christophe PLAGNIOL-VILLARD <plagnio@jcrosoft.com>
+ *
+ * Under GPL v2
+ */
+#include <common.h>
+#include <init.h>
+#include <clock.h>
+#include <io.h>
+#include <driver.h>
+#include <errno.h>
+#include <linux/amba/sp804.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <asm/hardware/arm_timer.h>
+
+static __iomem void *sp804_base;
+static struct clk *sp804_clk;
+
+static uint64_t sp804_read(void)
+{
+	return ~readl(sp804_base + TIMER_VALUE);
+}
+
+static struct clocksource sp804_clksrc = {
+	.read	= sp804_read,
+	.shift	= 20,
+	.mask	= CLOCKSOURCE_MASK(32),
+};
+
+static int sp804_probe(struct amba_device *dev, const struct amba_id *id)
+{
+	u32 tick_rate;
+	int ret;
+
+	if (sp804_base) {
+		dev_err(&dev->dev, "single instance driver\n");
+		return -EBUSY;
+	}
+
+	sp804_clk = clk_get(&dev->dev, NULL);
+	if (IS_ERR(sp804_clk)) {
+		ret = PTR_ERR(sp804_clk);
+		dev_err(&dev->dev, "clock not found: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_enable(sp804_clk);
+	if (ret < 0) {
+		dev_err(&dev->dev, "clock failed to enable: %d\n", ret);
+		clk_put(sp804_clk);
+		return ret;
+	}
+
+	sp804_base = amba_get_mem_region(dev);
+
+	tick_rate = clk_get_rate(sp804_clk);
+
+	/* setup timer 0 as free-running clocksource */
+	writel(0, sp804_base + TIMER_CTRL);
+	writel(0xffffffff, sp804_base + TIMER_LOAD);
+	writel(0xffffffff, sp804_base + TIMER_VALUE);
+	writel(TIMER_CTRL_32BIT | TIMER_CTRL_ENABLE | TIMER_CTRL_PERIODIC,
+		sp804_base + TIMER_CTRL);
+
+	sp804_clksrc.mult = clocksource_hz2mult(tick_rate, sp804_clksrc.shift);
+
+	init_clock(&sp804_clksrc);
+
+	return 0;
+}
+
+static struct amba_id sp804_ids[] = {
+	{
+		.id	= AMBA_ARM_SP804_ID,
+		.mask	= AMBA_ARM_SP804_ID_MASK,
+	},
+	{ 0, 0 },
+};
+
+struct amba_driver sp804_driver = {
+	.drv = {
+		.name = "sp804",
+	},
+	.probe		= sp804_probe,
+	.id_table	= sp804_ids,
+};
+
+static int sp804_init(void)
+{
+	return amba_driver_register(&sp804_driver);
+}
+coredevice_initcall(sp804_init);