From 0c092fe69021c5fc7c9f84e5540d888797a3a023 Mon Sep 17 00:00:00 2001
From: =?utf8?q?=E9=BB=84=E6=B6=9B?= <huangtao@rock-chips.com>
Date: Thu, 16 Jan 2014 12:15:17 +0800
Subject: [PATCH] ARM: rockchip: add cortex-a9 cpuidle support

---
 arch/arm/mach-rockchip/Makefile  |  1 +
 arch/arm/mach-rockchip/cpu.h     |  2 +
 arch/arm/mach-rockchip/cpuidle.c | 64 ++++++++++++++++++++++++++++++++
 3 files changed, 67 insertions(+)
 create mode 100644 arch/arm/mach-rockchip/cpuidle.c

diff --git a/arch/arm/mach-rockchip/Makefile b/arch/arm/mach-rockchip/Makefile
index 83ae96d69a37..edb2865ff494 100644
--- a/arch/arm/mach-rockchip/Makefile
+++ b/arch/arm/mach-rockchip/Makefile
@@ -1,5 +1,6 @@
 obj-y += common.o
 obj-y += cpu.o
 obj-y += rk3188.o
+obj-$(CONFIG_CPU_IDLE) += cpuidle.o
 obj-$(CONFIG_SMP) += platsmp.o
 obj-$(CONFIG_FIQ_DEBUGGER) += rk_fiq_debugger.o
diff --git a/arch/arm/mach-rockchip/cpu.h b/arch/arm/mach-rockchip/cpu.h
index 06cef95ed3d2..b9820044846e 100644
--- a/arch/arm/mach-rockchip/cpu.h
+++ b/arch/arm/mach-rockchip/cpu.h
@@ -3,6 +3,8 @@
 
 extern unsigned long rockchip_soc_id;
 
+static inline bool cpu_is_rockchip(void) { return rockchip_soc_id; }
+
 #define ROCKCHIP_CPU_MASK       0xffff0000
 #define ROCKCHIP_CPU_RK2928     0x29280000
 #define ROCKCHIP_CPU_RK3026     0x30260000
diff --git a/arch/arm/mach-rockchip/cpuidle.c b/arch/arm/mach-rockchip/cpuidle.c
new file mode 100644
index 000000000000..9788b3d6b5a5
--- /dev/null
+++ b/arch/arm/mach-rockchip/cpuidle.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2012-2014 ROCKCHIP, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/cpuidle.h>
+#include <linux/export.h>
+#include <linux/suspend.h>
+#include <linux/err.h>
+#include <linux/irqchip/arm-gic.h>
+#include <asm/cpuidle.h>
+#include <asm/cputype.h>
+#include <asm/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include "cpu.h"
+
+static void __iomem *gic_cpu_base;
+
+static int rockchip_ca9_cpuidle_enter(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int index)
+{
+	do {
+		cpu_do_idle();
+	} while (readl_relaxed(gic_cpu_base + GIC_CPU_HIGHPRI) == 0x3FF);
+	return 0;
+}
+
+static struct cpuidle_driver rockchip_ca9_cpuidle_driver = {
+	.name = "rockchip_ca9_cpuidle",
+	.owner = THIS_MODULE,
+	.states[0] = ARM_CPUIDLE_WFI_STATE,
+	.state_count = 1,
+};
+
+static int __init rockchip_ca9_cpuidle_init(void)
+{
+	struct device_node *np;
+	int ret;
+
+	if (!cpu_is_rockchip())
+		return -ENODEV;
+	if (read_cpuid_part_number() != ARM_CPU_PART_CORTEX_A9)
+		return -ENODEV;
+	np = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-gic");
+	if (!np)
+		return -ENODEV;
+	gic_cpu_base = of_iomap(np, 1);
+	if (!gic_cpu_base) {
+		pr_err("%s: failed to map gic cpu registers\n", __func__);
+		return -EINVAL;
+	}
+	rockchip_ca9_cpuidle_driver.states[0].enter = rockchip_ca9_cpuidle_enter;
+	ret = cpuidle_register(&rockchip_ca9_cpuidle_driver, NULL);
+	if (ret)
+		pr_err("%s: failed to register cpuidle driver: %d\n", __func__, ret);
+
+	return ret;
+}
+
+device_initcall(rockchip_ca9_cpuidle_init);
-- 
2.34.1