ARM: vexpress: add TC2 CPU idle PM

TC2 test-chip integrates power management circuitry and firmware that
allows to remove voltage from both (A7 and A15) clusters when they are
idle or more generically when the system is forced into shutdown mode.

All CPUs in a cluster share the same voltage source so they cannot be
shutdown independently. In order to take advantage of TC2 power
management capabilities this patch implements a multi-cluster aware
CPU idle implementation. It is based on coupled C-state concept provided
by this code:

http://lists.infradead.org/pipermail/linux-arm-kernel/2012-April/097084.html

CPUs that are part of the same cluster are coupled using the mask
provided by the MPIDR at boot. Once all CPUs hit the coupled barrier the
primary CPU in the cluster (the one with MPIDR[7:0] == 0) waits for
secondaries to clean their L1 and enter wfi. Then it cleans all cache
levels, exits cluster coherency and starts the procedure to shutdown the
respective cluster. All wake-up IRQ sources are enabled by default.

Deep shutdown states for both clusters are not enabled by default.

To enable them:

A15 cluster

echo 0 > /sys/devices/system/debug/idle_debug/enable_idle

A7 cluster

echo 1 > /sys/devices/system/debug/idle_debug/enable_idle

Tested thoroughly using lookbusy to modulate system load and trigger idle
states entry/exit.

5 files changed, 357 insertions, 0 deletions

diff --git a/arch/arm/mach-vexpress/Kconfig b/arch/arm/mach-vexpress/Kconfig
index cf8730d35e7..95d1c2b743a 100644
--- a/arch/arm/mach-vexpress/Kconfig
+++ b/arch/arm/mach-vexpress/Kconfig
@@ -12,6 +12,20 @@ config ARCH_VEXPRESS_CORTEX_A5_A9_ERRATA
 	  build a working kernel, you must also enable relevant core
 	  tile support or Flattened Device Tree based support options.
 
+config ARCH_VEXPRESS_TC2_PM
+	bool "Power Management Support for TC2 test-chip"
+	depends on CPU_IDLE && PM
+	select ARM_CPU_SUSPEND
+	select ARCH_NEEDS_CPU_IDLE_COUPLED
+	select ARM_SPC
+	select ARM_CCI
+	help
+	  Provides code that enables CPU idle power management on the
+	  TC2 testchip. It enables the CPU idle driver so that the kernel
+	  can enter cluster power down states provided by the power
+	  controller. Code is built on top of coupled C-state idle code
+	  since all CPUs need to be idle to enter cluster shutdown.
+
 config ARCH_VEXPRESS_CA9X4
 	bool "Versatile Express Cortex-A9x4 tile"
 	select ARCH_VEXPRESS_CORTEX_A5_A9_ERRATA
diff --git a/arch/arm/mach-vexpress/Makefile b/arch/arm/mach-vexpress/Makefile
index 90551b9780a..5067ef487bd 100644
--- a/arch/arm/mach-vexpress/Makefile
+++ b/arch/arm/mach-vexpress/Makefile
@@ -6,3 +6,4 @@ obj-y					:= v2m.o
 obj-$(CONFIG_ARCH_VEXPRESS_CA9X4)	+= ct-ca9x4.o
 obj-$(CONFIG_SMP)			+= platsmp.o
 obj-$(CONFIG_HOTPLUG_CPU)		+= hotplug.o
+obj-$(CONFIG_ARCH_VEXPRESS_TC2_PM)	+= cpuidle-tc2.o hotplug-asm.o tc2-sleep.o
diff --git a/arch/arm/mach-vexpress/cpuidle-tc2.c b/arch/arm/mach-vexpress/cpuidle-tc2.c
new file mode 100644
index 00000000000..bda97dd7c65
--- /dev/null
+++ b/arch/arm/mach-vexpress/cpuidle-tc2.c
@@ -0,0 +1,251 @@
+/*
+ * TC2 CPU idle driver.
+ *
+ * Copyright (C) 2012 ARM Ltd.
+ * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
+ *
+ * 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/arm-cci.h>
+#include <linux/bitmap.h>
+#include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/clockchips.h>
+#include <linux/debugfs.h>
+#include <linux/hrtimer.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/tick.h>
+#include <linux/vexpress.h>
+#include <asm/cpuidle.h>
+#include <asm/cputype.h>
+#include <asm/idmap.h>
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+
+#include <mach/motherboard.h>
+
+static int tc2_enter_coupled(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv, int idx);
+
+static struct cpuidle_state tc2_cpuidle_set[] __initdata = {
+	[0] = ARM_CPUIDLE_WFI_STATE,
+	[1] = {
+		.enter			= tc2_enter_coupled,
+		.exit_latency		= 300,
+		.target_residency	= 1000,
+		.flags			= CPUIDLE_FLAG_TIME_VALID |
+							CPUIDLE_FLAG_COUPLED,
+		.name			= "C1",
+		.desc			= "ARM power down",
+	},
+};
+
+struct cpuidle_driver tc2_idle_driver = {
+	.name = "tc2_idle",
+	.owner = THIS_MODULE,
+	.en_core_tk_irqen = 1,
+	.safe_state_index = 0
+};
+
+static DEFINE_PER_CPU(struct cpuidle_device, tc2_idle_dev);
+
+#define NR_CLUSTERS 2
+static cpumask_t cluster_mask = CPU_MASK_NONE;
+
+extern void disable_clean_inv_dcache(int);
+static atomic_t abort_barrier[NR_CLUSTERS];
+
+extern void tc2_cpu_resume(void);
+extern void disable_snoops(void);
+
+int tc2_coupled_finisher(unsigned long arg)
+{
+	unsigned int mpidr = read_cpuid_mpidr();
+	unsigned int cpu = smp_processor_id();
+	unsigned int cluster = (mpidr >> 8) & 0xf;
+	unsigned int weight = cpumask_weight(topology_core_cpumask(cpu));
+	u8 wfi_weight = 0;
+
+	cpuidle_coupled_parallel_barrier((struct cpuidle_device *)arg,
+					&abort_barrier[cluster]);
+	if (mpidr & 0xf) {
+		disable_clean_inv_dcache(0);
+		wfi();
+		/* not reached */
+	}
+
+	while (wfi_weight != (weight - 1)) {
+		wfi_weight = vexpress_spc_wfi_cpustat(cluster);
+		wfi_weight = hweight8(wfi_weight);
+	}
+
+	vexpress_spc_powerdown_enable(cluster, 1);
+	disable_clean_inv_dcache(1);
+	disable_cci(cluster);
+	disable_snoops();
+	return 1;
+}
+
+/*
+ * tc2_enter_coupled - Programs CPU to enter the specified state
+ * @dev: cpuidle device
+ * @drv: The target state to be programmed
+ * @idx: state index
+ *
+ * Called from the CPUidle framework to program the device to the
+ * specified target state selected by the governor.
+ */
+static int tc2_enter_coupled(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv, int idx)
+{
+	struct timespec ts_preidle, ts_postidle, ts_idle;
+	int ret;
+	int cluster = (read_cpuid_mpidr() >> 8) & 0xf;
+	/* Used to keep track of the total time in idle */
+	getnstimeofday(&ts_preidle);
+
+	if (!cpu_isset(cluster, cluster_mask))
+			goto shallow_out;
+
+	BUG_ON(!irqs_disabled());
+
+	cpu_pm_enter();
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
+
+	ret = cpu_suspend((unsigned long) dev, tc2_coupled_finisher);
+
+	if (ret)
+		BUG();
+
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);
+
+	cpu_pm_exit();
+
+shallow_out:
+	getnstimeofday(&ts_postidle);
+	ts_idle = timespec_sub(ts_postidle, ts_preidle);
+
+	dev->last_residency = ts_idle.tv_nsec / NSEC_PER_USEC +
+					ts_idle.tv_sec * USEC_PER_SEC;
+	return idx;
+}
+
+static int idle_mask_show(struct seq_file *f, void *p)
+{
+	char buf[256];
+	bitmap_scnlistprintf(buf, 256, cpumask_bits(&cluster_mask),
+							NR_CLUSTERS);
+
+	seq_printf(f, "%s\n", buf);
+
+	return 0;
+}
+
+static int idle_mask_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, idle_mask_show, inode->i_private);
+}
+
+static const struct file_operations cpuidle_fops = {
+	.open		= idle_mask_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int idle_debug_set(void *data, u64 val)
+{
+	int i;
+
+	if ((val > NR_CLUSTERS || val < 0) && val != 0xff) {
+		pr_warning("Wrong parameter passed\n");
+		return -EINVAL;
+	}
+	cpuidle_pause_and_lock();
+	if (val == 0xff) {
+		cpumask_clear(&cluster_mask);
+		return 0;
+	}
+
+	for (i = 0; i < NR_CLUSTERS; i++)
+		if (val == i)
+			cpumask_set_cpu(i, &cluster_mask);
+	cpuidle_resume_and_unlock();
+	return 0;
+}
+DEFINE_SIMPLE_ATTRIBUTE(idle_debug_fops, NULL, idle_debug_set, "%llu\n");
+
+/*
+ * tc2_idle_init
+ *
+ * Registers the TC2 specific cpuidle driver with the cpuidle
+ * framework with the valid set of states.
+ */
+int __init tc2_idle_init(void)
+{
+	struct cpuidle_device *dev;
+	int i, cpu_id;
+	struct dentry *idle_debug, *file_debug;
+	struct cpuidle_driver *drv = &tc2_idle_driver;
+
+	drv->state_count = (sizeof(tc2_cpuidle_set) /
+				       sizeof(struct cpuidle_state));
+
+	for (i = 0; i < drv->state_count; i++) {
+		memcpy(&drv->states[i], &tc2_cpuidle_set[i],
+				sizeof(struct cpuidle_state));
+	}
+
+	cpuidle_register_driver(drv);
+
+	for_each_cpu(cpu_id, cpu_online_mask) {
+		pr_err("CPUidle for CPU%d registered\n", cpu_id);
+		dev = &per_cpu(tc2_idle_dev, cpu_id);
+		dev->cpu = cpu_id;
+		dev->safe_state_index = 0;
+
+		cpumask_copy(&dev->coupled_cpus,
+				topology_core_cpumask(cpu_id));
+		dev->state_count = drv->state_count;
+
+		if (cpuidle_register_device(dev)) {
+			printk(KERN_ERR "%s: Cpuidle register device failed\n",
+			       __func__);
+			return -EIO;
+		}
+	}
+
+	idle_debug = debugfs_create_dir("idle_debug", NULL);
+
+	if (IS_ERR_OR_NULL(idle_debug)) {
+		printk(KERN_INFO "Error in creating idle debugfs directory\n");
+		return 0;
+	}
+
+	file_debug = debugfs_create_file("enable_idle", S_IRUGO | S_IWGRP,
+				   idle_debug, NULL, &idle_debug_fops);
+
+	if (IS_ERR_OR_NULL(file_debug)) {
+		printk(KERN_INFO "Error in creating enable_idle file\n");
+		return 0;
+	}
+
+	file_debug = debugfs_create_file("enable_mask", S_IRUGO | S_IWGRP,
+					idle_debug, NULL, &cpuidle_fops);
+
+	if (IS_ERR_OR_NULL(file_debug))
+		printk(KERN_INFO "Error in creating enable_mask file\n");
+
+	/* enable all wake-up IRQs by default */
+	vexpress_spc_set_wake_intr(0x7ff);
+	v2m_flags_set(virt_to_phys(tc2_cpu_resume));
+
+	return 0;
+}
+
+late_initcall(tc2_idle_init);
diff --git a/arch/arm/mach-vexpress/hotplug-asm.S b/arch/arm/mach-vexpress/hotplug-asm.S
new file mode 100644
index 00000000000..c51d2310e39
--- /dev/null
+++ b/arch/arm/mach-vexpress/hotplug-asm.S
@@ -0,0 +1,31 @@
+#include <linux/linkage.h>
+#include <asm/asm-offsets.h>
+
+	.text
+ENTRY(disable_clean_inv_dcache)
+ ARM(	stmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	stmfd	sp!, {r4-r7, r9-r11, lr}	)
+
+	mrc	p15, 0, r3, c1, c0, 0
+	bic	r3, #4			@ clear C bit
+	mcr	p15, 0, r3, c1, c0, 0
+	dsb
+	isb
+	cmp	r0, #0
+	mrc	p15, 1, r0, c0, c0, 1
+	andeq	r0, r0, #0xe00000
+	moveq	r0, r0, lsr #21
+	andne	r0, r0, #0x7000000
+	movne	r0, r0, lsr #24
+	movs	r0, r0
+	blne	v7_flush_dcache_level
+	clrex
+	mrc	p15, 0, r3, c1, c0, 1
+	bic	r3, #0x40		@ clear SMP bit
+	mcr	p15, 0, r3, c1, c0, 1
+	isb
+	dsb
+ ARM(	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}	)
+ THUMB(	ldmfd	sp!, {r4-r7, r9-r11, lr}	)
+	mov	pc, lr
+ENDPROC(disable_clean_inv_dcache)
diff --git a/arch/arm/mach-vexpress/tc2-sleep.S b/arch/arm/mach-vexpress/tc2-sleep.S
new file mode 100644
index 00000000000..cc6b84144fa
--- /dev/null
+++ b/arch/arm/mach-vexpress/tc2-sleep.S
@@ -0,0 +1,60 @@
+#include <linux/linkage.h>
+
+ENTRY(tc2_cpu_resume)
+	mrc	p15, 0, r0, c0, c0, 5
+	ands	r0, r0, #0xff00
+	adr	r0, value
+	addne	r0, r0, #16
+	ldmia	r0, {r1, r2, r3, r4}	@ CCI address, SCC snoop control & val
+	mvn	r3, r3			@ undo actions done at shutdown
+	ldr	r0, [r2]
+	and	r5, r0, r3
+	str	r5, [r2]
+	mov	r0, #3			@ enable CCI for the cluster
+	str	r0, [r1]
+	adr	r1, cci_ctrl
+	ldr	r1, [r1]
+loop:
+	ldr	r0, [r1]
+	ands	r0, r0, #1
+	bne	loop
+	mov	r0, #0			@ disable power down enable
+	str	r0, [r4]
+	b	cpu_resume
+ENDPROC(tc2_cpu_resume)
+
+ENTRY(disable_snoops)
+	mrc	p15, 0, r0, c0, c0, 5
+	ands	r0, r0, #0xff00
+	adr	r0, vvalue
+	addne	r0, r0, #8
+	ldmia	r0, {r2, r3}	@ CCI address, SCC snoop control & val
+	ldr	r1, scc_ptr
+	ldr	r1, [r1]
+	add	r2, r1, r2
+	ldr	r0, [r2]
+	orr	r0, r0, r3
+	dsb
+	isb
+	str	r0, [r2]
+	wfi
+ENDPROC(disable_snoops)
+
+cci_ctrl:
+	.long	0x2c09000c
+value:
+	.long 	0x2c094000
+	.long 	0x7fff0404
+	.long 	0x180
+	.long 	0x7fff0b30
+	.long 	0x2c095000
+	.long 	0x7fff0504
+	.long 	0x2000
+	.long 	0x7fff0b34
+vvalue:
+	.long 	0x404
+	.long 	0x180
+	.long 	0x504
+	.long 	0x2000
+scc_ptr:
+	.long vscc