x86/kprobes: Fix crash when probing CS CALL instructionsgrafted

When using eBPF to probe CS CALL instructions within a function,
a crash can be triggered.

The eBPF tool probes offset 257 of the __hrtimer_run_queues()
function:

<__hrtimer_run_queues+249>:  nopl   0x0(%rax,%rax,1)
<__hrtimer_run_queues+254>:  mov    %r14,%rdi
<__hrtimer_run_queues+257>:  cs call <__x86_indirect_thunk_r12>
<__hrtimer_run_queues+263>:  mov    %eax,%r12d
<__hrtimer_run_queues+266>:  xchg   %ax,%ax
<__hrtimer_run_queues+268>:  mov    %r13,%rdi

Which triggers this crash:

  BUG: unable to handle page fault for address: 00000000000f41c9
  #PF: supervisor write access in kernel mode
  #PF: error_code(0x0002) - not-present page
  PGD 0 P4D 0
  Oops: 0002 [#1] SMP NOPTI
  CPU: 1 PID: 0 Comm: swapper/1 Kdump: loaded Tainted: P
  RIP: 0010:__hrtimer_run_queues+0x106/0x230

Note that __hrtimer_run_queues+0x106 is __hrtimer_run_queues+262, which is
at the 6th byte of the above CS CALL instruction. Since the CS CALL
instruction occupies 6 bytes, the exception occurred in the middle of that
call instruction.

The root cause is that when using eBPF tools to probe in the middle of a
function, a kprobe with INT3 is used as the underlying implementation.

During single-step emulation of the original CALL instruction,
int3_emulate_call() assumes that the probed CALL instruction is 5 bytes
long. However, the actual CS-prefixed CALL instruction occupies 6 bytes,
so it constructs an incorrect exception return address. When the CPU
returns from the kprobe handler, the next instruction to be executed is at
the address of the last byte of that CS CALL instruction. Coincidentally,
starting from that address, the CPU fetches and decodes a completely
different instruction, which ultimately triggers a kernel crash.

Fix the issue by using the actual instruction length obtained from
the instruction decoder when constructing the exception return
address, rather than relying on the hardcoded CALL_INSN_SIZE macro.

[ mingo: Refined the changelog ]

Fixes: 6256e668b7af ("x86/kprobes: Use int3 instead of debug trap for single-step")
Suggested-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Signed-off-by: Jinke Han <jinkehan@didiglobal.com>
Signed-off-by: Ingo Molnar <mingo@kernel.org>
Reviewed-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Acked-by: Yafang Shao <laoar.shao@gmail.com>
Acked-by: Borislav Petkov <bp@alien8.de>
Cc: Peter Zijlstra <peterz@infradead.org>
Link: https://patch.msgid.link/20260908073742.GA10517@didi-ThinkCentre-M920t-N000

16 files changed, 4525 insertions, 0 deletions

diff --git a/arch/arm/common/Kconfig b/arch/arm/common/Kconfig
new file mode 100644
index 000000000..d2fdb1796
--- /dev/null
+++ b/arch/arm/common/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0
+config SA1111
+	bool
+	select ZONE_DMA if ARCH_SA1100
+
+config KRAIT_L2_ACCESSORS
+	bool
+
+config SHARP_LOCOMO
+	bool
+
+config SHARP_PARAM
+	bool
+
+config SHARP_SCOOP
+	bool
diff --git a/arch/arm/common/Makefile b/arch/arm/common/Makefile
new file mode 100644
index 000000000..973338107
--- /dev/null
+++ b/arch/arm/common/Makefile
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the linux kernel.
+#
+
+obj-y				+= firmware.o
+
+obj-$(CONFIG_SA1111)		+= sa1111.o
+obj-$(CONFIG_KRAIT_L2_ACCESSORS) += krait-l2-accessors.o
+obj-$(CONFIG_SHARP_LOCOMO)	+= locomo.o
+obj-$(CONFIG_SHARP_PARAM)	+= sharpsl_param.o
+obj-$(CONFIG_SHARP_SCOOP)	+= scoop.o
+obj-$(CONFIG_CPU_V7)		+= secure_cntvoff.o
+obj-$(CONFIG_MCPM)		+= mcpm_head.o mcpm_entry.o mcpm_platsmp.o vlock.o
+CFLAGS_REMOVE_mcpm_entry.o	= -pg
+obj-$(CONFIG_BL_SWITCHER)	+= bL_switcher.o
+obj-$(CONFIG_BL_SWITCHER_DUMMY_IF) += bL_switcher_dummy_if.o
diff --git a/arch/arm/common/bL_switcher.c b/arch/arm/common/bL_switcher.c
new file mode 100644
index 000000000..d1e82a318
--- /dev/null
+++ b/arch/arm/common/bL_switcher.c
@@ -0,0 +1,801 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * arch/arm/common/bL_switcher.c -- big.LITTLE cluster switcher core driver
+ *
+ * Created by:	Nicolas Pitre, March 2012
+ * Copyright:	(C) 2012-2013  Linaro Limited
+ */
+
+#include <linux/atomic.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched/signal.h>
+#include <uapi/linux/sched/types.h>
+#include <linux/interrupt.h>
+#include <linux/cpu_pm.h>
+#include <linux/cpu.h>
+#include <linux/cpumask.h>
+#include <linux/kthread.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/clockchips.h>
+#include <linux/hrtimer.h>
+#include <linux/tick.h>
+#include <linux/notifier.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/smp.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include <linux/sysfs.h>
+#include <linux/irqchip/arm-gic.h>
+#include <linux/moduleparam.h>
+
+#include <asm/smp_plat.h>
+#include <asm/cputype.h>
+#include <asm/suspend.h>
+#include <asm/mcpm.h>
+#include <asm/bL_switcher.h>
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/power_cpu_migrate.h>
+
+
+/*
+ * Use our own MPIDR accessors as the generic ones in asm/cputype.h have
+ * __attribute_const__ and we don't want the compiler to assume any
+ * constness here as the value _does_ change along some code paths.
+ */
+
+static int read_mpidr(void)
+{
+	unsigned int id;
+	asm volatile ("mrc p15, 0, %0, c0, c0, 5" : "=r" (id));
+	return id & MPIDR_HWID_BITMASK;
+}
+
+/*
+ * bL switcher core code.
+ */
+
+static void bL_do_switch(void *_arg)
+{
+	unsigned ib_mpidr, ib_cpu, ib_cluster;
+	long volatile handshake, **handshake_ptr = _arg;
+
+	pr_debug("%s\n", __func__);
+
+	ib_mpidr = cpu_logical_map(smp_processor_id());
+	ib_cpu = MPIDR_AFFINITY_LEVEL(ib_mpidr, 0);
+	ib_cluster = MPIDR_AFFINITY_LEVEL(ib_mpidr, 1);
+
+	/* Advertise our handshake location */
+	if (handshake_ptr) {
+		handshake = 0;
+		*handshake_ptr = &handshake;
+	} else
+		handshake = -1;
+
+	/*
+	 * Our state has been saved at this point.  Let's release our
+	 * inbound CPU.
+	 */
+	mcpm_set_entry_vector(ib_cpu, ib_cluster, cpu_resume);
+	sev();
+
+	/*
+	 * From this point, we must assume that our counterpart CPU might
+	 * have taken over in its parallel world already, as if execution
+	 * just returned from cpu_suspend().  It is therefore important to
+	 * be very careful not to make any change the other guy is not
+	 * expecting.  This is why we need stack isolation.
+	 *
+	 * Fancy under cover tasks could be performed here.  For now
+	 * we have none.
+	 */
+
+	/*
+	 * Let's wait until our inbound is alive.
+	 */
+	while (!handshake) {
+		wfe();
+		smp_mb();
+	}
+
+	/* Let's put ourself down. */
+	mcpm_cpu_power_down();
+
+	/* should never get here */
+	BUG();
+}
+
+/*
+ * Stack isolation.  To ensure 'current' remains valid, we just use another
+ * piece of our thread's stack space which should be fairly lightly used.
+ * The selected area starts just above the thread_info structure located
+ * at the very bottom of the stack, aligned to a cache line, and indexed
+ * with the cluster number.
+ */
+#define STACK_SIZE 512
+extern void call_with_stack(void (*fn)(void *), void *arg, void *sp);
+static int bL_switchpoint(unsigned long _arg)
+{
+	unsigned int mpidr = read_mpidr();
+	unsigned int clusterid = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	void *stack = current_thread_info() + 1;
+	stack = PTR_ALIGN(stack, L1_CACHE_BYTES);
+	stack += clusterid * STACK_SIZE + STACK_SIZE;
+	call_with_stack(bL_do_switch, (void *)_arg, stack);
+	BUG();
+}
+
+/*
+ * Generic switcher interface
+ */
+
+static unsigned int bL_gic_id[MAX_CPUS_PER_CLUSTER][MAX_NR_CLUSTERS];
+static int bL_switcher_cpu_pairing[NR_CPUS];
+
+/*
+ * bL_switch_to - Switch to a specific cluster for the current CPU
+ * @new_cluster_id: the ID of the cluster to switch to.
+ *
+ * This function must be called on the CPU to be switched.
+ * Returns 0 on success, else a negative status code.
+ */
+static int bL_switch_to(unsigned int new_cluster_id)
+{
+	unsigned int mpidr, this_cpu, that_cpu;
+	unsigned int ob_mpidr, ob_cpu, ob_cluster, ib_mpidr, ib_cpu, ib_cluster;
+	struct completion inbound_alive;
+	long volatile *handshake_ptr;
+	int ipi_nr, ret;
+
+	this_cpu = smp_processor_id();
+	ob_mpidr = read_mpidr();
+	ob_cpu = MPIDR_AFFINITY_LEVEL(ob_mpidr, 0);
+	ob_cluster = MPIDR_AFFINITY_LEVEL(ob_mpidr, 1);
+	BUG_ON(cpu_logical_map(this_cpu) != ob_mpidr);
+
+	if (new_cluster_id == ob_cluster)
+		return 0;
+
+	that_cpu = bL_switcher_cpu_pairing[this_cpu];
+	ib_mpidr = cpu_logical_map(that_cpu);
+	ib_cpu = MPIDR_AFFINITY_LEVEL(ib_mpidr, 0);
+	ib_cluster = MPIDR_AFFINITY_LEVEL(ib_mpidr, 1);
+
+	pr_debug("before switch: CPU %d MPIDR %#x -> %#x\n",
+		 this_cpu, ob_mpidr, ib_mpidr);
+
+	this_cpu = smp_processor_id();
+
+	/* Close the gate for our entry vectors */
+	mcpm_set_entry_vector(ob_cpu, ob_cluster, NULL);
+	mcpm_set_entry_vector(ib_cpu, ib_cluster, NULL);
+
+	/* Install our "inbound alive" notifier. */
+	init_completion(&inbound_alive);
+	ipi_nr = register_ipi_completion(&inbound_alive, this_cpu);
+	ipi_nr |= ((1 << 16) << bL_gic_id[ob_cpu][ob_cluster]);
+	mcpm_set_early_poke(ib_cpu, ib_cluster, gic_get_sgir_physaddr(), ipi_nr);
+
+	/*
+	 * Let's wake up the inbound CPU now in case it requires some delay
+	 * to come online, but leave it gated in our entry vector code.
+	 */
+	ret = mcpm_cpu_power_up(ib_cpu, ib_cluster);
+	if (ret) {
+		pr_err("%s: mcpm_cpu_power_up() returned %d\n", __func__, ret);
+		return ret;
+	}
+
+	/*
+	 * Raise a SGI on the inbound CPU to make sure it doesn't stall
+	 * in a possible WFI, such as in bL_power_down().
+	 */
+	gic_send_sgi(bL_gic_id[ib_cpu][ib_cluster], 0);
+
+	/*
+	 * Wait for the inbound to come up.  This allows for other
+	 * tasks to be scheduled in the mean time.
+	 */
+	wait_for_completion(&inbound_alive);
+	mcpm_set_early_poke(ib_cpu, ib_cluster, 0, 0);
+
+	/*
+	 * From this point we are entering the switch critical zone
+	 * and can't take any interrupts anymore.
+	 */
+	local_irq_disable();
+	local_fiq_disable();
+	trace_cpu_migrate_begin(ktime_get_real_ns(), ob_mpidr);
+
+	/* redirect GIC's SGIs to our counterpart */
+	gic_migrate_target(bL_gic_id[ib_cpu][ib_cluster]);
+
+	tick_suspend_local();
+
+	ret = cpu_pm_enter();
+
+	/* we can not tolerate errors at this point */
+	if (ret)
+		panic("%s: cpu_pm_enter() returned %d\n", __func__, ret);
+
+	/* Swap the physical CPUs in the logical map for this logical CPU. */
+	cpu_logical_map(this_cpu) = ib_mpidr;
+	cpu_logical_map(that_cpu) = ob_mpidr;
+
+	/* Let's do the actual CPU switch. */
+	ret = cpu_suspend((unsigned long)&handshake_ptr, bL_switchpoint);
+	if (ret > 0)
+		panic("%s: cpu_suspend() returned %d\n", __func__, ret);
+
+	/* We are executing on the inbound CPU at this point */
+	mpidr = read_mpidr();
+	pr_debug("after switch: CPU %d MPIDR %#x\n", this_cpu, mpidr);
+	BUG_ON(mpidr != ib_mpidr);
+
+	mcpm_cpu_powered_up();
+
+	ret = cpu_pm_exit();
+
+	tick_resume_local();
+
+	trace_cpu_migrate_finish(ktime_get_real_ns(), ib_mpidr);
+	local_fiq_enable();
+	local_irq_enable();
+
+	*handshake_ptr = 1;
+	dsb_sev();
+
+	if (ret)
+		pr_err("%s exiting with error %d\n", __func__, ret);
+	return ret;
+}
+
+struct bL_thread {
+	spinlock_t lock;
+	struct task_struct *task;
+	wait_queue_head_t wq;
+	int wanted_cluster;
+	struct completion started;
+	bL_switch_completion_handler completer;
+	void *completer_cookie;
+};
+
+static struct bL_thread bL_threads[NR_CPUS];
+
+static int bL_switcher_thread(void *arg)
+{
+	struct bL_thread *t = arg;
+	int cluster;
+	bL_switch_completion_handler completer;
+	void *completer_cookie;
+
+	sched_set_fifo_low(current);
+	complete(&t->started);
+
+	do {
+		if (signal_pending(current))
+			flush_signals(current);
+		wait_event_interruptible(t->wq,
+				t->wanted_cluster != -1 ||
+				kthread_should_stop());
+
+		spin_lock(&t->lock);
+		cluster = t->wanted_cluster;
+		completer = t->completer;
+		completer_cookie = t->completer_cookie;
+		t->wanted_cluster = -1;
+		t->completer = NULL;
+		spin_unlock(&t->lock);
+
+		if (cluster != -1) {
+			bL_switch_to(cluster);
+
+			if (completer)
+				completer(completer_cookie);
+		}
+	} while (!kthread_should_stop());
+
+	return 0;
+}
+
+static struct task_struct *bL_switcher_thread_create(int cpu, void *arg)
+{
+	struct task_struct *task;
+
+	task = kthread_run_on_cpu(bL_switcher_thread, arg,
+				  cpu, "kswitcher_%d");
+	if (IS_ERR(task))
+		pr_err("%s failed for CPU %d\n", __func__, cpu);
+
+	return task;
+}
+
+/*
+ * bL_switch_request_cb - Switch to a specific cluster for the given CPU,
+ *      with completion notification via a callback
+ *
+ * @cpu: the CPU to switch
+ * @new_cluster_id: the ID of the cluster to switch to.
+ * @completer: switch completion callback.  if non-NULL,
+ *	@completer(@completer_cookie) will be called on completion of
+ *	the switch, in non-atomic context.
+ * @completer_cookie: opaque context argument for @completer.
+ *
+ * This function causes a cluster switch on the given CPU by waking up
+ * the appropriate switcher thread.  This function may or may not return
+ * before the switch has occurred.
+ *
+ * If a @completer callback function is supplied, it will be called when
+ * the switch is complete.  This can be used to determine asynchronously
+ * when the switch is complete, regardless of when bL_switch_request()
+ * returns.  When @completer is supplied, no new switch request is permitted
+ * for the affected CPU until after the switch is complete, and @completer
+ * has returned.
+ */
+int bL_switch_request_cb(unsigned int cpu, unsigned int new_cluster_id,
+			 bL_switch_completion_handler completer,
+			 void *completer_cookie)
+{
+	struct bL_thread *t;
+
+	if (cpu >= ARRAY_SIZE(bL_threads)) {
+		pr_err("%s: cpu %d out of bounds\n", __func__, cpu);
+		return -EINVAL;
+	}
+
+	t = &bL_threads[cpu];
+
+	if (IS_ERR(t->task))
+		return PTR_ERR(t->task);
+	if (!t->task)
+		return -ESRCH;
+
+	spin_lock(&t->lock);
+	if (t->completer) {
+		spin_unlock(&t->lock);
+		return -EBUSY;
+	}
+	t->completer = completer;
+	t->completer_cookie = completer_cookie;
+	t->wanted_cluster = new_cluster_id;
+	spin_unlock(&t->lock);
+	wake_up(&t->wq);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(bL_switch_request_cb);
+
+/*
+ * Activation and configuration code.
+ */
+
+static DEFINE_MUTEX(bL_switcher_activation_lock);
+static BLOCKING_NOTIFIER_HEAD(bL_activation_notifier);
+static unsigned int bL_switcher_active;
+static unsigned int bL_switcher_cpu_original_cluster[NR_CPUS];
+static cpumask_t bL_switcher_removed_logical_cpus;
+
+int bL_switcher_register_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&bL_activation_notifier, nb);
+}
+EXPORT_SYMBOL_GPL(bL_switcher_register_notifier);
+
+int bL_switcher_unregister_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&bL_activation_notifier, nb);
+}
+EXPORT_SYMBOL_GPL(bL_switcher_unregister_notifier);
+
+static int bL_activation_notify(unsigned long val)
+{
+	int ret;
+
+	ret = blocking_notifier_call_chain(&bL_activation_notifier, val, NULL);
+	if (ret & NOTIFY_STOP_MASK)
+		pr_err("%s: notifier chain failed with status 0x%x\n",
+			__func__, ret);
+	return notifier_to_errno(ret);
+}
+
+static void bL_switcher_restore_cpus(void)
+{
+	int i;
+
+	for_each_cpu(i, &bL_switcher_removed_logical_cpus) {
+		struct device *cpu_dev = get_cpu_device(i);
+		int ret = device_online(cpu_dev);
+		if (ret)
+			dev_err(cpu_dev, "switcher: unable to restore CPU\n");
+	}
+}
+
+static int bL_switcher_halve_cpus(void)
+{
+	int i, j, cluster_0, gic_id, ret;
+	unsigned int cpu, cluster, mask;
+	cpumask_t available_cpus;
+
+	/* First pass to validate what we have */
+	mask = 0;
+	for_each_online_cpu(i) {
+		cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(i), 0);
+		cluster = MPIDR_AFFINITY_LEVEL(cpu_logical_map(i), 1);
+		if (cluster >= 2) {
+			pr_err("%s: only dual cluster systems are supported\n", __func__);
+			return -EINVAL;
+		}
+		if (WARN_ON(cpu >= MAX_CPUS_PER_CLUSTER))
+			return -EINVAL;
+		mask |= (1 << cluster);
+	}
+	if (mask != 3) {
+		pr_err("%s: no CPU pairing possible\n", __func__);
+		return -EINVAL;
+	}
+
+	/*
+	 * Now let's do the pairing.  We match each CPU with another CPU
+	 * from a different cluster.  To get a uniform scheduling behavior
+	 * without fiddling with CPU topology and compute capacity data,
+	 * we'll use logical CPUs initially belonging to the same cluster.
+	 */
+	memset(bL_switcher_cpu_pairing, -1, sizeof(bL_switcher_cpu_pairing));
+	cpumask_copy(&available_cpus, cpu_online_mask);
+	cluster_0 = -1;
+	for_each_cpu(i, &available_cpus) {
+		int match = -1;
+		cluster = MPIDR_AFFINITY_LEVEL(cpu_logical_map(i), 1);
+		if (cluster_0 == -1)
+			cluster_0 = cluster;
+		if (cluster != cluster_0)
+			continue;
+		cpumask_clear_cpu(i, &available_cpus);
+		for_each_cpu(j, &available_cpus) {
+			cluster = MPIDR_AFFINITY_LEVEL(cpu_logical_map(j), 1);
+			/*
+			 * Let's remember the last match to create "odd"
+			 * pairings on purpose in order for other code not
+			 * to assume any relation between physical and
+			 * logical CPU numbers.
+			 */
+			if (cluster != cluster_0)
+				match = j;
+		}
+		if (match != -1) {
+			bL_switcher_cpu_pairing[i] = match;
+			cpumask_clear_cpu(match, &available_cpus);
+			pr_info("CPU%d paired with CPU%d\n", i, match);
+		}
+	}
+
+	/*
+	 * Now we disable the unwanted CPUs i.e. everything that has no
+	 * pairing information (that includes the pairing counterparts).
+	 */
+	cpumask_clear(&bL_switcher_removed_logical_cpus);
+	for_each_online_cpu(i) {
+		cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(i), 0);
+		cluster = MPIDR_AFFINITY_LEVEL(cpu_logical_map(i), 1);
+
+		/* Let's take note of the GIC ID for this CPU */
+		gic_id = gic_get_cpu_id(i);
+		if (gic_id < 0) {
+			pr_err("%s: bad GIC ID for CPU %d\n", __func__, i);
+			bL_switcher_restore_cpus();
+			return -EINVAL;
+		}
+		bL_gic_id[cpu][cluster] = gic_id;
+		pr_info("GIC ID for CPU %u cluster %u is %u\n",
+			cpu, cluster, gic_id);
+
+		if (bL_switcher_cpu_pairing[i] != -1) {
+			bL_switcher_cpu_original_cluster[i] = cluster;
+			continue;
+		}
+
+		ret = device_offline(get_cpu_device(i));
+		if (ret) {
+			bL_switcher_restore_cpus();
+			return ret;
+		}
+		cpumask_set_cpu(i, &bL_switcher_removed_logical_cpus);
+	}
+
+	return 0;
+}
+
+/* Determine the logical CPU a given physical CPU is grouped on. */
+int bL_switcher_get_logical_index(u32 mpidr)
+{
+	int cpu;
+
+	if (!bL_switcher_active)
+		return -EUNATCH;
+
+	mpidr &= MPIDR_HWID_BITMASK;
+	for_each_online_cpu(cpu) {
+		int pairing = bL_switcher_cpu_pairing[cpu];
+		if (pairing == -1)
+			continue;
+		if ((mpidr == cpu_logical_map(cpu)) ||
+		    (mpidr == cpu_logical_map(pairing)))
+			return cpu;
+	}
+	return -EINVAL;
+}
+
+static void bL_switcher_trace_trigger_cpu(void *__always_unused info)
+{
+	trace_cpu_migrate_current(ktime_get_real_ns(), read_mpidr());
+}
+
+int bL_switcher_trace_trigger(void)
+{
+	preempt_disable();
+
+	bL_switcher_trace_trigger_cpu(NULL);
+	smp_call_function(bL_switcher_trace_trigger_cpu, NULL, true);
+
+	preempt_enable();
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(bL_switcher_trace_trigger);
+
+static int bL_switcher_enable(void)
+{
+	int cpu, ret;
+
+	mutex_lock(&bL_switcher_activation_lock);
+	lock_device_hotplug();
+	if (bL_switcher_active) {
+		unlock_device_hotplug();
+		mutex_unlock(&bL_switcher_activation_lock);
+		return 0;
+	}
+
+	pr_info("big.LITTLE switcher initializing\n");
+
+	ret = bL_activation_notify(BL_NOTIFY_PRE_ENABLE);
+	if (ret)
+		goto error;
+
+	ret = bL_switcher_halve_cpus();
+	if (ret)
+		goto error;
+
+	bL_switcher_trace_trigger();
+
+	for_each_online_cpu(cpu) {
+		struct bL_thread *t = &bL_threads[cpu];
+		spin_lock_init(&t->lock);
+		init_waitqueue_head(&t->wq);
+		init_completion(&t->started);
+		t->wanted_cluster = -1;
+		t->task = bL_switcher_thread_create(cpu, t);
+	}
+
+	bL_switcher_active = 1;
+	bL_activation_notify(BL_NOTIFY_POST_ENABLE);
+	pr_info("big.LITTLE switcher initialized\n");
+	goto out;
+
+error:
+	pr_warn("big.LITTLE switcher initialization failed\n");
+	bL_activation_notify(BL_NOTIFY_POST_DISABLE);
+
+out:
+	unlock_device_hotplug();
+	mutex_unlock(&bL_switcher_activation_lock);
+	return ret;
+}
+
+#ifdef CONFIG_SYSFS
+
+static void bL_switcher_disable(void)
+{
+	unsigned int cpu, cluster;
+	struct bL_thread *t;
+	struct task_struct *task;
+
+	mutex_lock(&bL_switcher_activation_lock);
+	lock_device_hotplug();
+
+	if (!bL_switcher_active)
+		goto out;
+
+	if (bL_activation_notify(BL_NOTIFY_PRE_DISABLE) != 0) {
+		bL_activation_notify(BL_NOTIFY_POST_ENABLE);
+		goto out;
+	}
+
+	bL_switcher_active = 0;
+
+	/*
+	 * To deactivate the switcher, we must shut down the switcher
+	 * threads to prevent any other requests from being accepted.
+	 * Then, if the final cluster for given logical CPU is not the
+	 * same as the original one, we'll recreate a switcher thread
+	 * just for the purpose of switching the CPU back without any
+	 * possibility for interference from external requests.
+	 */
+	for_each_online_cpu(cpu) {
+		t = &bL_threads[cpu];
+		task = t->task;
+		t->task = NULL;
+		if (!task || IS_ERR(task))
+			continue;
+		kthread_stop(task);
+		/* no more switch may happen on this CPU at this point */
+		cluster = MPIDR_AFFINITY_LEVEL(cpu_logical_map(cpu), 1);
+		if (cluster == bL_switcher_cpu_original_cluster[cpu])
+			continue;
+		init_completion(&t->started);
+		t->wanted_cluster = bL_switcher_cpu_original_cluster[cpu];
+		task = bL_switcher_thread_create(cpu, t);
+		if (!IS_ERR(task)) {
+			wait_for_completion(&t->started);
+			kthread_stop(task);
+			cluster = MPIDR_AFFINITY_LEVEL(cpu_logical_map(cpu), 1);
+			if (cluster == bL_switcher_cpu_original_cluster[cpu])
+				continue;
+		}
+		/* If execution gets here, we're in trouble. */
+		pr_crit("%s: unable to restore original cluster for CPU %d\n",
+			__func__, cpu);
+		pr_crit("%s: CPU %d can't be restored\n",
+			__func__, bL_switcher_cpu_pairing[cpu]);
+		cpumask_clear_cpu(bL_switcher_cpu_pairing[cpu],
+				  &bL_switcher_removed_logical_cpus);
+	}
+
+	bL_switcher_restore_cpus();
+	bL_switcher_trace_trigger();
+
+	bL_activation_notify(BL_NOTIFY_POST_DISABLE);
+
+out:
+	unlock_device_hotplug();
+	mutex_unlock(&bL_switcher_activation_lock);
+}
+
+static ssize_t bL_switcher_active_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%u\n", bL_switcher_active);
+}
+
+static ssize_t bL_switcher_active_store(struct kobject *kobj,
+		struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	int ret;
+
+	switch (buf[0]) {
+	case '0':
+		bL_switcher_disable();
+		ret = 0;
+		break;
+	case '1':
+		ret = bL_switcher_enable();
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return (ret >= 0) ? count : ret;
+}
+
+static ssize_t bL_switcher_trace_trigger_store(struct kobject *kobj,
+		struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	int ret = bL_switcher_trace_trigger();
+
+	return ret ? ret : count;
+}
+
+static struct kobj_attribute bL_switcher_active_attr =
+	__ATTR(active, 0644, bL_switcher_active_show, bL_switcher_active_store);
+
+static struct kobj_attribute bL_switcher_trace_trigger_attr =
+	__ATTR(trace_trigger, 0200, NULL, bL_switcher_trace_trigger_store);
+
+static struct attribute *bL_switcher_attrs[] = {
+	&bL_switcher_active_attr.attr,
+	&bL_switcher_trace_trigger_attr.attr,
+	NULL,
+};
+
+static struct attribute_group bL_switcher_attr_group = {
+	.attrs = bL_switcher_attrs,
+};
+
+static struct kobject *bL_switcher_kobj;
+
+static int __init bL_switcher_sysfs_init(void)
+{
+	int ret;
+
+	bL_switcher_kobj = kobject_create_and_add("bL_switcher", kernel_kobj);
+	if (!bL_switcher_kobj)
+		return -ENOMEM;
+	ret = sysfs_create_group(bL_switcher_kobj, &bL_switcher_attr_group);
+	if (ret)
+		kobject_put(bL_switcher_kobj);
+	return ret;
+}
+
+#endif  /* CONFIG_SYSFS */
+
+bool bL_switcher_get_enabled(void)
+{
+	mutex_lock(&bL_switcher_activation_lock);
+
+	return bL_switcher_active;
+}
+EXPORT_SYMBOL_GPL(bL_switcher_get_enabled);
+
+void bL_switcher_put_enabled(void)
+{
+	mutex_unlock(&bL_switcher_activation_lock);
+}
+EXPORT_SYMBOL_GPL(bL_switcher_put_enabled);
+
+/*
+ * Veto any CPU hotplug operation on those CPUs we've removed
+ * while the switcher is active.
+ * We're just not ready to deal with that given the trickery involved.
+ */
+static int bL_switcher_cpu_pre(unsigned int cpu)
+{
+	int pairing;
+
+	if (!bL_switcher_active)
+		return 0;
+
+	pairing = bL_switcher_cpu_pairing[cpu];
+
+	if (pairing == -1)
+		return -EINVAL;
+	return 0;
+}
+
+static bool no_bL_switcher;
+core_param(no_bL_switcher, no_bL_switcher, bool, 0644);
+
+static int __init bL_switcher_init(void)
+{
+	int ret;
+
+	if (!mcpm_is_available())
+		return -ENODEV;
+
+	cpuhp_setup_state_nocalls(CPUHP_ARM_BL_PREPARE, "arm/bl:prepare",
+				  bL_switcher_cpu_pre, NULL);
+	ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN, "arm/bl:predown",
+					NULL, bL_switcher_cpu_pre);
+	if (ret < 0) {
+		cpuhp_remove_state_nocalls(CPUHP_ARM_BL_PREPARE);
+		pr_err("bL_switcher: Failed to allocate a hotplug state\n");
+		return ret;
+	}
+	if (!no_bL_switcher) {
+		ret = bL_switcher_enable();
+		if (ret)
+			return ret;
+	}
+
+#ifdef CONFIG_SYSFS
+	ret = bL_switcher_sysfs_init();
+	if (ret)
+		pr_err("%s: unable to create sysfs entry\n", __func__);
+#endif
+
+	return 0;
+}
+
+late_initcall(bL_switcher_init);
diff --git a/arch/arm/common/bL_switcher_dummy_if.c b/arch/arm/common/bL_switcher_dummy_if.c
new file mode 100644
index 000000000..cabc0659b
--- /dev/null
+++ b/arch/arm/common/bL_switcher_dummy_if.c
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * arch/arm/common/bL_switcher_dummy_if.c -- b.L switcher dummy interface
+ *
+ * Created by:	Nicolas Pitre, November 2012
+ * Copyright:	(C) 2012-2013  Linaro Limited
+ *
+ * Dummy interface to user space for debugging purpose only.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <asm/bL_switcher.h>
+
+static ssize_t bL_switcher_write(struct file *file, const char __user *buf,
+			size_t len, loff_t *pos)
+{
+	unsigned char val[3];
+	unsigned int cpu, cluster;
+	int ret;
+
+	pr_debug("%s\n", __func__);
+
+	if (len < 3)
+		return -EINVAL;
+
+	if (copy_from_user(val, buf, 3))
+		return -EFAULT;
+
+	/* format: <cpu#>,<cluster#> */
+	if (val[0] < '0' || val[0] > '9' ||
+	    val[1] != ',' ||
+	    val[2] < '0' || val[2] > '1')
+		return -EINVAL;
+
+	cpu = val[0] - '0';
+	cluster = val[2] - '0';
+	ret = bL_switch_request(cpu, cluster);
+
+	return ret ? : len;
+}
+
+static const struct file_operations bL_switcher_fops = {
+	.write		= bL_switcher_write,
+	.owner	= THIS_MODULE,
+};
+
+static struct miscdevice bL_switcher_device = {
+	MISC_DYNAMIC_MINOR,
+	"b.L_switcher",
+	&bL_switcher_fops
+};
+module_misc_device(bL_switcher_device);
+
+MODULE_AUTHOR("Nicolas Pitre <nico@linaro.org>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("big.LITTLE switcher dummy user interface");
diff --git a/arch/arm/common/firmware.c b/arch/arm/common/firmware.c
new file mode 100644
index 000000000..c1f8f581b
--- /dev/null
+++ b/arch/arm/common/firmware.c
@@ -0,0 +1,15 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2012 Samsung Electronics.
+ * Kyungmin Park <kyungmin.park@samsung.com>
+ * Tomasz Figa <t.figa@samsung.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/suspend.h>
+
+#include <asm/firmware.h>
+
+static const struct firmware_ops default_firmware_ops;
+
+const struct firmware_ops *firmware_ops = &default_firmware_ops;
diff --git a/arch/arm/common/krait-l2-accessors.c b/arch/arm/common/krait-l2-accessors.c
new file mode 100644
index 000000000..9a97ddade
--- /dev/null
+++ b/arch/arm/common/krait-l2-accessors.c
@@ -0,0 +1,48 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2018, The Linux Foundation. All rights reserved.
+
+#include <linux/spinlock.h>
+#include <linux/export.h>
+
+#include <asm/barrier.h>
+#include <asm/krait-l2-accessors.h>
+
+static DEFINE_RAW_SPINLOCK(krait_l2_lock);
+
+void krait_set_l2_indirect_reg(u32 addr, u32 val)
+{
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&krait_l2_lock, flags);
+	/*
+	 * Select the L2 window by poking l2cpselr, then write to the window
+	 * via l2cpdr.
+	 */
+	asm volatile ("mcr p15, 3, %0, c15, c0, 6 @ l2cpselr" : : "r" (addr));
+	isb();
+	asm volatile ("mcr p15, 3, %0, c15, c0, 7 @ l2cpdr" : : "r" (val));
+	isb();
+
+	raw_spin_unlock_irqrestore(&krait_l2_lock, flags);
+}
+EXPORT_SYMBOL(krait_set_l2_indirect_reg);
+
+u32 krait_get_l2_indirect_reg(u32 addr)
+{
+	u32 val;
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&krait_l2_lock, flags);
+	/*
+	 * Select the L2 window by poking l2cpselr, then read from the window
+	 * via l2cpdr.
+	 */
+	asm volatile ("mcr p15, 3, %0, c15, c0, 6 @ l2cpselr" : : "r" (addr));
+	isb();
+	asm volatile ("mrc p15, 3, %0, c15, c0, 7 @ l2cpdr" : "=r" (val));
+
+	raw_spin_unlock_irqrestore(&krait_l2_lock, flags);
+
+	return val;
+}
+EXPORT_SYMBOL(krait_get_l2_indirect_reg);
diff --git a/arch/arm/common/locomo.c b/arch/arm/common/locomo.c
new file mode 100644
index 000000000..55e360452
--- /dev/null
+++ b/arch/arm/common/locomo.c
@@ -0,0 +1,886 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * linux/arch/arm/common/locomo.c
+ *
+ * Sharp LoCoMo support
+ *
+ * This file contains all generic LoCoMo support.
+ *
+ * All initialization functions provided here are intended to be called
+ * from machine specific code with proper arguments when required.
+ *
+ * Based on sa1111.c
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+
+#include <asm/irq.h>
+#include <asm/mach/irq.h>
+
+#include <asm/hardware/locomo.h>
+
+/* LoCoMo Interrupts */
+#define IRQ_LOCOMO_KEY		(0)
+#define IRQ_LOCOMO_GPIO		(1)
+#define IRQ_LOCOMO_LT		(2)
+#define IRQ_LOCOMO_SPI		(3)
+
+/* M62332 output channel selection */
+#define M62332_EVR_CH	1	/* M62332 volume channel number  */
+				/*   0 : CH.1 , 1 : CH. 2        */
+/* DAC send data */
+#define	M62332_SLAVE_ADDR	0x4e	/* Slave address  */
+#define	M62332_W_BIT		0x00	/* W bit (0 only) */
+#define	M62332_SUB_ADDR		0x00	/* Sub address    */
+#define	M62332_A_BIT		0x00	/* A bit (0 only) */
+
+/* DAC setup and hold times (expressed in us) */
+#define DAC_BUS_FREE_TIME	5	/*   4.7 us */
+#define DAC_START_SETUP_TIME	5	/*   4.7 us */
+#define DAC_STOP_SETUP_TIME	4	/*   4.0 us */
+#define DAC_START_HOLD_TIME	5	/*   4.7 us */
+#define DAC_SCL_LOW_HOLD_TIME	5	/*   4.7 us */
+#define DAC_SCL_HIGH_HOLD_TIME	4	/*   4.0 us */
+#define DAC_DATA_SETUP_TIME	1	/*   250 ns */
+#define DAC_DATA_HOLD_TIME	1	/*   300 ns */
+#define DAC_LOW_SETUP_TIME	1	/*   300 ns */
+#define DAC_HIGH_SETUP_TIME	1	/*  1000 ns */
+
+/* the following is the overall data for the locomo chip */
+struct locomo {
+	struct device *dev;
+	unsigned long phys;
+	unsigned int irq;
+	int irq_base;
+	spinlock_t lock;
+	void __iomem *base;
+#ifdef CONFIG_PM
+	void *saved_state;
+#endif
+};
+
+static const struct bus_type locomo_bus_type;
+
+struct locomo_dev_info {
+	unsigned long	offset;
+	unsigned long	length;
+	unsigned int	devid;
+	unsigned int	irq[1];
+	const char *	name;
+};
+
+/* All the locomo devices.  If offset is non-zero, the mapbase for the
+ * locomo_dev will be set to the chip base plus offset.  If offset is
+ * zero, then the mapbase for the locomo_dev will be set to zero.  An
+ * offset of zero means the device only uses GPIOs or other helper
+ * functions inside this file */
+static struct locomo_dev_info locomo_devices[] = {
+	{
+		.devid 		= LOCOMO_DEVID_KEYBOARD,
+		.irq		= { IRQ_LOCOMO_KEY },
+		.name		= "locomo-keyboard",
+		.offset		= LOCOMO_KEYBOARD,
+		.length		= 16,
+	},
+	{
+		.devid		= LOCOMO_DEVID_FRONTLIGHT,
+		.irq		= {},
+		.name		= "locomo-frontlight",
+		.offset		= LOCOMO_FRONTLIGHT,
+		.length		= 8,
+
+	},
+	{
+		.devid		= LOCOMO_DEVID_BACKLIGHT,
+		.irq		= {},
+		.name		= "locomo-backlight",
+		.offset		= LOCOMO_BACKLIGHT,
+		.length		= 8,
+	},
+	{
+		.devid		= LOCOMO_DEVID_AUDIO,
+		.irq		= {},
+		.name		= "locomo-audio",
+		.offset		= LOCOMO_AUDIO,
+		.length		= 4,
+	},
+	{
+		.devid		= LOCOMO_DEVID_LED,
+		.irq 		= {},
+		.name		= "locomo-led",
+		.offset		= LOCOMO_LED,
+		.length		= 8,
+	},
+	{
+		.devid		= LOCOMO_DEVID_UART,
+		.irq		= {},
+		.name		= "locomo-uart",
+		.offset		= 0,
+		.length		= 0,
+	},
+	{
+		.devid		= LOCOMO_DEVID_SPI,
+		.irq		= {},
+		.name		= "locomo-spi",
+		.offset		= LOCOMO_SPI,
+		.length		= 0x30,
+	},
+};
+
+static void locomo_handler(struct irq_desc *desc)
+{
+	struct locomo *lchip = irq_desc_get_handler_data(desc);
+	int req, i;
+
+	/* Acknowledge the parent IRQ */
+	desc->irq_data.chip->irq_ack(&desc->irq_data);
+
+	/* check why this interrupt was generated */
+	req = locomo_readl(lchip->base + LOCOMO_ICR) & 0x0f00;
+
+	if (req) {
+		unsigned int irq;
+
+		/* generate the next interrupt(s) */
+		irq = lchip->irq_base;
+		for (i = 0; i <= 3; i++, irq++) {
+			if (req & (0x0100 << i)) {
+				generic_handle_irq(irq);
+			}
+
+		}
+	}
+}
+
+static void locomo_ack_irq(struct irq_data *d)
+{
+}
+
+static void locomo_mask_irq(struct irq_data *d)
+{
+	struct locomo *lchip = irq_data_get_irq_chip_data(d);
+	unsigned int r;
+	r = locomo_readl(lchip->base + LOCOMO_ICR);
+	r &= ~(0x0010 << (d->irq - lchip->irq_base));
+	locomo_writel(r, lchip->base + LOCOMO_ICR);
+}
+
+static void locomo_unmask_irq(struct irq_data *d)
+{
+	struct locomo *lchip = irq_data_get_irq_chip_data(d);
+	unsigned int r;
+	r = locomo_readl(lchip->base + LOCOMO_ICR);
+	r |= (0x0010 << (d->irq - lchip->irq_base));
+	locomo_writel(r, lchip->base + LOCOMO_ICR);
+}
+
+static struct irq_chip locomo_chip = {
+	.name		= "LOCOMO",
+	.irq_ack	= locomo_ack_irq,
+	.irq_mask	= locomo_mask_irq,
+	.irq_unmask	= locomo_unmask_irq,
+};
+
+static void locomo_setup_irq(struct locomo *lchip)
+{
+	int irq = lchip->irq_base;
+
+	/*
+	 * Install handler for IRQ_LOCOMO_HW.
+	 */
+	irq_set_irq_type(lchip->irq, IRQ_TYPE_EDGE_FALLING);
+	irq_set_chained_handler_and_data(lchip->irq, locomo_handler, lchip);
+
+	/* Install handlers for IRQ_LOCOMO_* */
+	for ( ; irq <= lchip->irq_base + 3; irq++) {
+		irq_set_chip_and_handler(irq, &locomo_chip, handle_level_irq);
+		irq_set_chip_data(irq, lchip);
+		irq_clear_status_flags(irq, IRQ_NOREQUEST | IRQ_NOPROBE);
+	}
+}
+
+
+static void locomo_dev_release(struct device *_dev)
+{
+	struct locomo_dev *dev = LOCOMO_DEV(_dev);
+
+	kfree(dev);
+}
+
+static int
+locomo_init_one_child(struct locomo *lchip, struct locomo_dev_info *info)
+{
+	struct locomo_dev *dev;
+	int ret;
+
+	dev = kzalloc_obj(struct locomo_dev);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/*
+	 * If the parent device has a DMA mask associated with it,
+	 * propagate it down to the children.
+	 */
+	if (lchip->dev->dma_mask) {
+		dev->dma_mask = *lchip->dev->dma_mask;
+		dev->dev.dma_mask = &dev->dma_mask;
+	}
+
+	dev_set_name(&dev->dev, "%s", info->name);
+	dev->devid	 = info->devid;
+	dev->dev.parent  = lchip->dev;
+	dev->dev.bus     = &locomo_bus_type;
+	dev->dev.release = locomo_dev_release;
+	dev->dev.coherent_dma_mask = lchip->dev->coherent_dma_mask;
+
+	if (info->offset)
+		dev->mapbase = lchip->base + info->offset;
+	else
+		dev->mapbase = 0;
+	dev->length = info->length;
+
+	dev->irq[0] = (lchip->irq_base == NO_IRQ) ?
+			NO_IRQ : lchip->irq_base + info->irq[0];
+
+	ret = device_register(&dev->dev);
+	if (ret) {
+ out:
+		kfree(dev);
+	}
+	return ret;
+}
+
+#ifdef CONFIG_PM
+
+struct locomo_save_data {
+	u16	LCM_GPO;
+	u16	LCM_SPICT;
+	u16	LCM_GPE;
+	u16	LCM_ASD;
+	u16	LCM_SPIMD;
+};
+
+static int locomo_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct locomo *lchip = platform_get_drvdata(dev);
+	struct locomo_save_data *save;
+	unsigned long flags;
+
+	save = kmalloc_obj(struct locomo_save_data);
+	if (!save)
+		return -ENOMEM;
+
+	lchip->saved_state = save;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+
+	save->LCM_GPO     = locomo_readl(lchip->base + LOCOMO_GPO);	/* GPIO */
+	locomo_writel(0x00, lchip->base + LOCOMO_GPO);
+	save->LCM_SPICT   = locomo_readl(lchip->base + LOCOMO_SPI + LOCOMO_SPICT);	/* SPI */
+	locomo_writel(0x40, lchip->base + LOCOMO_SPI + LOCOMO_SPICT);
+	save->LCM_GPE     = locomo_readl(lchip->base + LOCOMO_GPE);	/* GPIO */
+	locomo_writel(0x00, lchip->base + LOCOMO_GPE);
+	save->LCM_ASD     = locomo_readl(lchip->base + LOCOMO_ASD);	/* ADSTART */
+	locomo_writel(0x00, lchip->base + LOCOMO_ASD);
+	save->LCM_SPIMD   = locomo_readl(lchip->base + LOCOMO_SPI + LOCOMO_SPIMD);	/* SPI */
+	locomo_writel(0x3C14, lchip->base + LOCOMO_SPI + LOCOMO_SPIMD);
+
+	locomo_writel(0x00, lchip->base + LOCOMO_PAIF);
+	locomo_writel(0x00, lchip->base + LOCOMO_DAC);
+	locomo_writel(0x00, lchip->base + LOCOMO_BACKLIGHT + LOCOMO_TC);
+
+	if ((locomo_readl(lchip->base + LOCOMO_LED + LOCOMO_LPT0) & 0x88) && (locomo_readl(lchip->base + LOCOMO_LED + LOCOMO_LPT1) & 0x88))
+		locomo_writel(0x00, lchip->base + LOCOMO_C32K); 	/* CLK32 off */
+	else
+		/* 18MHz already enabled, so no wait */
+		locomo_writel(0xc1, lchip->base + LOCOMO_C32K); 	/* CLK32 on */
+
+	locomo_writel(0x00, lchip->base + LOCOMO_TADC);		/* 18MHz clock off*/
+	locomo_writel(0x00, lchip->base + LOCOMO_AUDIO + LOCOMO_ACC);			/* 22MHz/24MHz clock off */
+	locomo_writel(0x00, lchip->base + LOCOMO_FRONTLIGHT + LOCOMO_ALS);			/* FL */
+
+	spin_unlock_irqrestore(&lchip->lock, flags);
+
+	return 0;
+}
+
+static int locomo_resume(struct platform_device *dev)
+{
+	struct locomo *lchip = platform_get_drvdata(dev);
+	struct locomo_save_data *save;
+	unsigned long r;
+	unsigned long flags;
+
+	save = lchip->saved_state;
+	if (!save)
+		return 0;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+
+	locomo_writel(save->LCM_GPO, lchip->base + LOCOMO_GPO);
+	locomo_writel(save->LCM_SPICT, lchip->base + LOCOMO_SPI + LOCOMO_SPICT);
+	locomo_writel(save->LCM_GPE, lchip->base + LOCOMO_GPE);
+	locomo_writel(save->LCM_ASD, lchip->base + LOCOMO_ASD);
+	locomo_writel(save->LCM_SPIMD, lchip->base + LOCOMO_SPI + LOCOMO_SPIMD);
+
+	locomo_writel(0x00, lchip->base + LOCOMO_C32K);
+	locomo_writel(0x90, lchip->base + LOCOMO_TADC);
+
+	locomo_writel(0, lchip->base + LOCOMO_KEYBOARD + LOCOMO_KSC);
+	r = locomo_readl(lchip->base + LOCOMO_KEYBOARD + LOCOMO_KIC);
+	r &= 0xFEFF;
+	locomo_writel(r, lchip->base + LOCOMO_KEYBOARD + LOCOMO_KIC);
+	locomo_writel(0x1, lchip->base + LOCOMO_KEYBOARD + LOCOMO_KCMD);
+
+	spin_unlock_irqrestore(&lchip->lock, flags);
+
+	lchip->saved_state = NULL;
+	kfree(save);
+
+	return 0;
+}
+#endif
+
+static int
+__locomo_probe(struct device *me, struct resource *mem, int irq)
+{
+	struct locomo_platform_data *pdata = me->platform_data;
+	struct locomo *lchip;
+	unsigned long r;
+	int i, ret = -ENODEV;
+
+	lchip = kzalloc_obj(struct locomo);
+	if (!lchip)
+		return -ENOMEM;
+
+	spin_lock_init(&lchip->lock);
+
+	lchip->dev = me;
+	dev_set_drvdata(lchip->dev, lchip);
+
+	lchip->phys = mem->start;
+	lchip->irq = irq;
+	lchip->irq_base = (pdata) ? pdata->irq_base : NO_IRQ;
+
+	/*
+	 * Map the whole region.  This also maps the
+	 * registers for our children.
+	 */
+	lchip->base = ioremap(mem->start, PAGE_SIZE);
+	if (!lchip->base) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* locomo initialize */
+	locomo_writel(0, lchip->base + LOCOMO_ICR);
+	/* KEYBOARD */
+	locomo_writel(0, lchip->base + LOCOMO_KEYBOARD + LOCOMO_KIC);
+
+	/* GPIO */
+	locomo_writel(0, lchip->base + LOCOMO_GPO);
+	locomo_writel((LOCOMO_GPIO(1) | LOCOMO_GPIO(2) | LOCOMO_GPIO(13) | LOCOMO_GPIO(14))
+			, lchip->base + LOCOMO_GPE);
+	locomo_writel((LOCOMO_GPIO(1) | LOCOMO_GPIO(2) | LOCOMO_GPIO(13) | LOCOMO_GPIO(14))
+			, lchip->base + LOCOMO_GPD);
+	locomo_writel(0, lchip->base + LOCOMO_GIE);
+
+	/* Frontlight */
+	locomo_writel(0, lchip->base + LOCOMO_FRONTLIGHT + LOCOMO_ALS);
+	locomo_writel(0, lchip->base + LOCOMO_FRONTLIGHT + LOCOMO_ALD);
+
+	/* Longtime timer */
+	locomo_writel(0, lchip->base + LOCOMO_LTINT);
+	/* SPI */
+	locomo_writel(0, lchip->base + LOCOMO_SPI + LOCOMO_SPIIE);
+
+	locomo_writel(6 + 8 + 320 + 30 - 10, lchip->base + LOCOMO_ASD);
+	r = locomo_readl(lchip->base + LOCOMO_ASD);
+	r |= 0x8000;
+	locomo_writel(r, lchip->base + LOCOMO_ASD);
+
+	locomo_writel(6 + 8 + 320 + 30 - 10 - 128 + 4, lchip->base + LOCOMO_HSD);
+	r = locomo_readl(lchip->base + LOCOMO_HSD);
+	r |= 0x8000;
+	locomo_writel(r, lchip->base + LOCOMO_HSD);
+
+	locomo_writel(128 / 8, lchip->base + LOCOMO_HSC);
+
+	/* XON */
+	locomo_writel(0x80, lchip->base + LOCOMO_TADC);
+	udelay(1000);
+	/* CLK9MEN */
+	r = locomo_readl(lchip->base + LOCOMO_TADC);
+	r |= 0x10;
+	locomo_writel(r, lchip->base + LOCOMO_TADC);
+	udelay(100);
+
+	/* init DAC */
+	r = locomo_readl(lchip->base + LOCOMO_DAC);
+	r |= LOCOMO_DAC_SCLOEB | LOCOMO_DAC_SDAOEB;
+	locomo_writel(r, lchip->base + LOCOMO_DAC);
+
+	r = locomo_readl(lchip->base + LOCOMO_VER);
+	printk(KERN_INFO "LoCoMo Chip: %lu%lu\n", (r >> 8), (r & 0xff));
+
+	/*
+	 * The interrupt controller must be initialised before any
+	 * other device to ensure that the interrupts are available.
+	 */
+	if (lchip->irq != NO_IRQ && lchip->irq_base != NO_IRQ)
+		locomo_setup_irq(lchip);
+
+	for (i = 0; i < ARRAY_SIZE(locomo_devices); i++)
+		locomo_init_one_child(lchip, &locomo_devices[i]);
+	return 0;
+
+ out:
+	kfree(lchip);
+	return ret;
+}
+
+static int locomo_remove_child(struct device *dev, void *data)
+{
+	device_unregister(dev);
+	return 0;
+} 
+
+static void __locomo_remove(struct locomo *lchip)
+{
+	device_for_each_child(lchip->dev, NULL, locomo_remove_child);
+
+	if (lchip->irq != NO_IRQ) {
+		irq_set_chained_handler_and_data(lchip->irq, NULL, NULL);
+	}
+
+	iounmap(lchip->base);
+	kfree(lchip);
+}
+
+/**
+ *	locomo_probe - probe for a single LoCoMo chip.
+ *	@dev: platform device
+ *
+ *	Probe for a LoCoMo chip.  This must be called
+ *	before any other locomo-specific code.
+ *
+ *	Returns:
+ *	* %-EINVAL	- device's IORESOURCE_MEM not found
+ *	* %-ENXIO	- could not allocate an IRQ for the device
+ *	* %-ENODEV	- device not found.
+ *	* %-EBUSY	- physical address already marked in-use.
+ *	* %-ENOMEM	- could not allocate or iomap memory.
+ *	* %0		- successful.
+ */
+static int locomo_probe(struct platform_device *dev)
+{
+	struct resource *mem;
+	int irq;
+
+	mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -EINVAL;
+	irq = platform_get_irq(dev, 0);
+	if (irq < 0)
+		return -ENXIO;
+
+	return __locomo_probe(&dev->dev, mem, irq);
+}
+
+static void locomo_remove(struct platform_device *dev)
+{
+	struct locomo *lchip = platform_get_drvdata(dev);
+
+	if (lchip) {
+		__locomo_remove(lchip);
+		platform_set_drvdata(dev, NULL);
+	}
+}
+
+/*
+ *	Not sure if this should be on the system bus or not yet.
+ *	We really want some way to register a system device at
+ *	the per-machine level, and then have this driver pick
+ *	up the registered devices.
+ */
+static struct platform_driver locomo_device_driver = {
+	.probe		= locomo_probe,
+	.remove		= locomo_remove,
+#ifdef CONFIG_PM
+	.suspend	= locomo_suspend,
+	.resume		= locomo_resume,
+#endif
+	.driver		= {
+		.name	= "locomo",
+	},
+};
+
+/*
+ *	Get the parent device driver (us) structure
+ *	from a child function device
+ */
+static inline struct locomo *locomo_chip_driver(struct locomo_dev *ldev)
+{
+	return (struct locomo *)dev_get_drvdata(ldev->dev.parent);
+}
+
+void locomo_gpio_set_dir(struct device *dev, unsigned int bits, unsigned int dir)
+{
+	struct locomo *lchip = dev_get_drvdata(dev);
+	unsigned long flags;
+	unsigned int r;
+
+	if (!lchip)
+		return;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+
+	r = locomo_readl(lchip->base + LOCOMO_GPD);
+	if (dir)
+		r |= bits;
+	else
+		r &= ~bits;
+	locomo_writel(r, lchip->base + LOCOMO_GPD);
+
+	r = locomo_readl(lchip->base + LOCOMO_GPE);
+	if (dir)
+		r |= bits;
+	else
+		r &= ~bits;
+	locomo_writel(r, lchip->base + LOCOMO_GPE);
+
+	spin_unlock_irqrestore(&lchip->lock, flags);
+}
+EXPORT_SYMBOL(locomo_gpio_set_dir);
+
+int locomo_gpio_read_level(struct device *dev, unsigned int bits)
+{
+	struct locomo *lchip = dev_get_drvdata(dev);
+	unsigned long flags;
+	unsigned int ret;
+
+	if (!lchip)
+		return -ENODEV;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+	ret = locomo_readl(lchip->base + LOCOMO_GPL);
+	spin_unlock_irqrestore(&lchip->lock, flags);
+
+	ret &= bits;
+	return ret;
+}
+EXPORT_SYMBOL(locomo_gpio_read_level);
+
+int locomo_gpio_read_output(struct device *dev, unsigned int bits)
+{
+	struct locomo *lchip = dev_get_drvdata(dev);
+	unsigned long flags;
+	unsigned int ret;
+
+	if (!lchip)
+		return -ENODEV;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+	ret = locomo_readl(lchip->base + LOCOMO_GPO);
+	spin_unlock_irqrestore(&lchip->lock, flags);
+
+	ret &= bits;
+	return ret;
+}
+EXPORT_SYMBOL(locomo_gpio_read_output);
+
+void locomo_gpio_write(struct device *dev, unsigned int bits, unsigned int set)
+{
+	struct locomo *lchip = dev_get_drvdata(dev);
+	unsigned long flags;
+	unsigned int r;
+
+	if (!lchip)
+		return;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+
+	r = locomo_readl(lchip->base + LOCOMO_GPO);
+	if (set)
+		r |= bits;
+	else
+		r &= ~bits;
+	locomo_writel(r, lchip->base + LOCOMO_GPO);
+
+	spin_unlock_irqrestore(&lchip->lock, flags);
+}
+EXPORT_SYMBOL(locomo_gpio_write);
+
+static void locomo_m62332_sendbit(void *mapbase, int bit)
+{
+	unsigned int r;
+
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SCLOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	udelay(DAC_DATA_HOLD_TIME);	/* 300 nsec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SCLOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	udelay(DAC_SCL_LOW_HOLD_TIME);	/* 4.7 usec */
+
+	if (bit & 1) {
+		r = locomo_readl(mapbase + LOCOMO_DAC);
+		r |=  LOCOMO_DAC_SDAOEB;
+		locomo_writel(r, mapbase + LOCOMO_DAC);
+		udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	} else {
+		r = locomo_readl(mapbase + LOCOMO_DAC);
+		r &=  ~(LOCOMO_DAC_SDAOEB);
+		locomo_writel(r, mapbase + LOCOMO_DAC);
+		udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	}
+
+	udelay(DAC_DATA_SETUP_TIME);	/* 250 nsec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/*  4.0 usec */
+}
+
+void locomo_m62332_senddata(struct locomo_dev *ldev, unsigned int dac_data, int channel)
+{
+	struct locomo *lchip = locomo_chip_driver(ldev);
+	int i;
+	unsigned char data;
+	unsigned int r;
+	void *mapbase = lchip->base;
+	unsigned long flags;
+
+	spin_lock_irqsave(&lchip->lock, flags);
+
+	/* Start */
+	udelay(DAC_BUS_FREE_TIME);	/* 5.0 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB | LOCOMO_DAC_SDAOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/* 4.0 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SDAOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_START_HOLD_TIME);	/* 5.0 usec */
+	udelay(DAC_DATA_HOLD_TIME);	/* 300 nsec */
+
+	/* Send slave address and W bit (LSB is W bit) */
+	data = (M62332_SLAVE_ADDR << 1) | M62332_W_BIT;
+	for (i = 1; i <= 8; i++) {
+		locomo_m62332_sendbit(mapbase, data >> (8 - i));
+	}
+
+	/* Check A bit */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SCLOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	udelay(DAC_SCL_LOW_HOLD_TIME);	/* 4.7 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SDAOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/* 4.7 usec */
+	if (locomo_readl(mapbase + LOCOMO_DAC) & LOCOMO_DAC_SDAOEB) {	/* High is error */
+		printk(KERN_WARNING "locomo: m62332_senddata Error 1\n");
+		goto out;
+	}
+
+	/* Send Sub address (LSB is channel select) */
+	/*    channel = 0 : ch1 select              */
+	/*            = 1 : ch2 select              */
+	data = M62332_SUB_ADDR + channel;
+	for (i = 1; i <= 8; i++) {
+		locomo_m62332_sendbit(mapbase, data >> (8 - i));
+	}
+
+	/* Check A bit */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SCLOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	udelay(DAC_SCL_LOW_HOLD_TIME);	/* 4.7 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SDAOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/* 4.7 usec */
+	if (locomo_readl(mapbase + LOCOMO_DAC) & LOCOMO_DAC_SDAOEB) {	/* High is error */
+		printk(KERN_WARNING "locomo: m62332_senddata Error 2\n");
+		goto out;
+	}
+
+	/* Send DAC data */
+	for (i = 1; i <= 8; i++) {
+		locomo_m62332_sendbit(mapbase, dac_data >> (8 - i));
+	}
+
+	/* Check A bit */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SCLOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	udelay(DAC_SCL_LOW_HOLD_TIME);	/* 4.7 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SDAOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/* 4.7 usec */
+	if (locomo_readl(mapbase + LOCOMO_DAC) & LOCOMO_DAC_SDAOEB) {	/* High is error */
+		printk(KERN_WARNING "locomo: m62332_senddata Error 3\n");
+	}
+
+out:
+	/* stop */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r &=  ~(LOCOMO_DAC_SCLOEB);
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 300 nsec */
+	udelay(DAC_SCL_LOW_HOLD_TIME);	/* 4.7 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/* 4 usec */
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SDAOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_HIGH_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_HIGH_HOLD_TIME);	/* 4 usec */
+
+	r = locomo_readl(mapbase + LOCOMO_DAC);
+	r |=  LOCOMO_DAC_SCLOEB | LOCOMO_DAC_SDAOEB;
+	locomo_writel(r, mapbase + LOCOMO_DAC);
+	udelay(DAC_LOW_SETUP_TIME);	/* 1000 nsec */
+	udelay(DAC_SCL_LOW_HOLD_TIME);	/* 4.7 usec */
+
+	spin_unlock_irqrestore(&lchip->lock, flags);
+}
+EXPORT_SYMBOL(locomo_m62332_senddata);
+
+/*
+ *	Frontlight control
+ */
+
+void locomo_frontlight_set(struct locomo_dev *dev, int duty, int vr, int bpwf)
+{
+	unsigned long flags;
+	struct locomo *lchip = locomo_chip_driver(dev);
+
+	if (vr)
+		locomo_gpio_write(dev->dev.parent, LOCOMO_GPIO_FL_VR, 1);
+	else
+		locomo_gpio_write(dev->dev.parent, LOCOMO_GPIO_FL_VR, 0);
+
+	spin_lock_irqsave(&lchip->lock, flags);
+	locomo_writel(bpwf, lchip->base + LOCOMO_FRONTLIGHT + LOCOMO_ALS);
+	udelay(100);
+	locomo_writel(duty, lchip->base + LOCOMO_FRONTLIGHT + LOCOMO_ALD);
+	locomo_writel(bpwf | LOCOMO_ALC_EN, lchip->base + LOCOMO_FRONTLIGHT + LOCOMO_ALS);
+	spin_unlock_irqrestore(&lchip->lock, flags);
+}
+EXPORT_SYMBOL(locomo_frontlight_set);
+
+/*
+ *	LoCoMo "Register Access Bus."
+ *
+ *	We model this as a regular bus type, and hang devices directly
+ *	off this.
+ */
+static int locomo_match(struct device *_dev, const struct device_driver *_drv)
+{
+	struct locomo_dev *dev = LOCOMO_DEV(_dev);
+	const struct locomo_driver *drv = LOCOMO_DRV(_drv);
+
+	return dev->devid == drv->devid;
+}
+
+static int locomo_bus_probe(struct device *dev)
+{
+	struct locomo_dev *ldev = LOCOMO_DEV(dev);
+	struct locomo_driver *drv = LOCOMO_DRV(dev->driver);
+	int ret = -ENODEV;
+
+	if (drv->probe)
+		ret = drv->probe(ldev);
+	return ret;
+}
+
+static void locomo_bus_remove(struct device *dev)
+{
+	struct locomo_dev *ldev = LOCOMO_DEV(dev);
+	struct locomo_driver *drv = LOCOMO_DRV(dev->driver);
+
+	if (drv->remove)
+		drv->remove(ldev);
+}
+
+static const struct bus_type locomo_bus_type = {
+	.name		= "locomo-bus",
+	.match		= locomo_match,
+	.probe		= locomo_bus_probe,
+	.remove		= locomo_bus_remove,
+};
+
+int locomo_driver_register(struct locomo_driver *driver)
+{
+	driver->drv.bus = &locomo_bus_type;
+	return driver_register(&driver->drv);
+}
+EXPORT_SYMBOL(locomo_driver_register);
+
+void locomo_driver_unregister(struct locomo_driver *driver)
+{
+	driver_unregister(&driver->drv);
+}
+EXPORT_SYMBOL(locomo_driver_unregister);
+
+static int __init locomo_init(void)
+{
+	int ret = bus_register(&locomo_bus_type);
+	if (ret == 0)
+		platform_driver_register(&locomo_device_driver);
+	return ret;
+}
+
+static void __exit locomo_exit(void)
+{
+	platform_driver_unregister(&locomo_device_driver);
+	bus_unregister(&locomo_bus_type);
+}
+
+module_init(locomo_init);
+module_exit(locomo_exit);
+
+MODULE_DESCRIPTION("Sharp LoCoMo core driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("John Lenz <lenz@cs.wisc.edu>");
diff --git a/arch/arm/common/mcpm_entry.c b/arch/arm/common/mcpm_entry.c
new file mode 100644
index 000000000..e013ff116
--- /dev/null
+++ b/arch/arm/common/mcpm_entry.c
@@ -0,0 +1,456 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * arch/arm/common/mcpm_entry.c -- entry point for multi-cluster PM
+ *
+ * Created by:  Nicolas Pitre, March 2012
+ * Copyright:   (C) 2012-2013  Linaro Limited
+ */
+
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/irqflags.h>
+#include <linux/cpu_pm.h>
+
+#include <asm/mcpm.h>
+#include <asm/cacheflush.h>
+#include <asm/idmap.h>
+#include <asm/cputype.h>
+#include <asm/suspend.h>
+
+/*
+ * The public API for this code is documented in arch/arm/include/asm/mcpm.h.
+ * For a comprehensive description of the main algorithm used here, please
+ * see Documentation/arch/arm/cluster-pm-race-avoidance.rst.
+ */
+
+struct sync_struct mcpm_sync;
+
+/*
+ * __mcpm_cpu_going_down: Indicates that the cpu is being torn down.
+ *    This must be called at the point of committing to teardown of a CPU.
+ *    The CPU cache (SCTRL.C bit) is expected to still be active.
+ */
+static void __mcpm_cpu_going_down(unsigned int cpu, unsigned int cluster)
+{
+	mcpm_sync.clusters[cluster].cpus[cpu].cpu = CPU_GOING_DOWN;
+	sync_cache_w(&mcpm_sync.clusters[cluster].cpus[cpu].cpu);
+}
+
+/*
+ * __mcpm_cpu_down: Indicates that cpu teardown is complete and that the
+ *    cluster can be torn down without disrupting this CPU.
+ *    To avoid deadlocks, this must be called before a CPU is powered down.
+ *    The CPU cache (SCTRL.C bit) is expected to be off.
+ *    However L2 cache might or might not be active.
+ */
+static void __mcpm_cpu_down(unsigned int cpu, unsigned int cluster)
+{
+	dmb();
+	mcpm_sync.clusters[cluster].cpus[cpu].cpu = CPU_DOWN;
+	sync_cache_w(&mcpm_sync.clusters[cluster].cpus[cpu].cpu);
+	sev();
+}
+
+/*
+ * __mcpm_outbound_leave_critical: Leave the cluster teardown critical section.
+ * @state: the final state of the cluster:
+ *     CLUSTER_UP: no destructive teardown was done and the cluster has been
+ *         restored to the previous state (CPU cache still active); or
+ *     CLUSTER_DOWN: the cluster has been torn-down, ready for power-off
+ *         (CPU cache disabled, L2 cache either enabled or disabled).
+ */
+static void __mcpm_outbound_leave_critical(unsigned int cluster, int state)
+{
+	dmb();
+	mcpm_sync.clusters[cluster].cluster = state;
+	sync_cache_w(&mcpm_sync.clusters[cluster].cluster);
+	sev();
+}
+
+/*
+ * __mcpm_outbound_enter_critical: Enter the cluster teardown critical section.
+ * This function should be called by the last man, after local CPU teardown
+ * is complete.  CPU cache expected to be active.
+ *
+ * Returns:
+ *     false: the critical section was not entered because an inbound CPU was
+ *         observed, or the cluster is already being set up;
+ *     true: the critical section was entered: it is now safe to tear down the
+ *         cluster.
+ */
+static bool __mcpm_outbound_enter_critical(unsigned int cpu, unsigned int cluster)
+{
+	unsigned int i;
+	struct mcpm_sync_struct *c = &mcpm_sync.clusters[cluster];
+
+	/* Warn inbound CPUs that the cluster is being torn down: */
+	c->cluster = CLUSTER_GOING_DOWN;
+	sync_cache_w(&c->cluster);
+
+	/* Back out if the inbound cluster is already in the critical region: */
+	sync_cache_r(&c->inbound);
+	if (c->inbound == INBOUND_COMING_UP)
+		goto abort;
+
+	/*
+	 * Wait for all CPUs to get out of the GOING_DOWN state, so that local
+	 * teardown is complete on each CPU before tearing down the cluster.
+	 *
+	 * If any CPU has been woken up again from the DOWN state, then we
+	 * shouldn't be taking the cluster down at all: abort in that case.
+	 */
+	sync_cache_r(&c->cpus);
+	for (i = 0; i < MAX_CPUS_PER_CLUSTER; i++) {
+		int cpustate;
+
+		if (i == cpu)
+			continue;
+
+		while (1) {
+			cpustate = c->cpus[i].cpu;
+			if (cpustate != CPU_GOING_DOWN)
+				break;
+
+			wfe();
+			sync_cache_r(&c->cpus[i].cpu);
+		}
+
+		switch (cpustate) {
+		case CPU_DOWN:
+			continue;
+
+		default:
+			goto abort;
+		}
+	}
+
+	return true;
+
+abort:
+	__mcpm_outbound_leave_critical(cluster, CLUSTER_UP);
+	return false;
+}
+
+static int __mcpm_cluster_state(unsigned int cluster)
+{
+	sync_cache_r(&mcpm_sync.clusters[cluster].cluster);
+	return mcpm_sync.clusters[cluster].cluster;
+}
+
+extern unsigned long mcpm_entry_vectors[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER];
+
+void mcpm_set_entry_vector(unsigned cpu, unsigned cluster, void *ptr)
+{
+	unsigned long val = ptr ? __pa_symbol(ptr) : 0;
+	mcpm_entry_vectors[cluster][cpu] = val;
+	sync_cache_w(&mcpm_entry_vectors[cluster][cpu]);
+}
+
+extern unsigned long mcpm_entry_early_pokes[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER][2];
+
+void mcpm_set_early_poke(unsigned cpu, unsigned cluster,
+			 unsigned long poke_phys_addr, unsigned long poke_val)
+{
+	unsigned long *poke = &mcpm_entry_early_pokes[cluster][cpu][0];
+	poke[0] = poke_phys_addr;
+	poke[1] = poke_val;
+	__sync_cache_range_w(poke, 2 * sizeof(*poke));
+}
+
+static const struct mcpm_platform_ops *platform_ops;
+
+int __init mcpm_platform_register(const struct mcpm_platform_ops *ops)
+{
+	if (platform_ops)
+		return -EBUSY;
+	platform_ops = ops;
+	return 0;
+}
+
+bool mcpm_is_available(void)
+{
+	return (platform_ops) ? true : false;
+}
+EXPORT_SYMBOL_GPL(mcpm_is_available);
+
+/*
+ * We can't use regular spinlocks. In the switcher case, it is possible
+ * for an outbound CPU to call power_down() after its inbound counterpart
+ * is already live using the same logical CPU number which trips lockdep
+ * debugging.
+ */
+static arch_spinlock_t mcpm_lock = __ARCH_SPIN_LOCK_UNLOCKED;
+
+static int mcpm_cpu_use_count[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER];
+
+static inline bool mcpm_cluster_unused(unsigned int cluster)
+{
+	int i, cnt;
+	for (i = 0, cnt = 0; i < MAX_CPUS_PER_CLUSTER; i++)
+		cnt |= mcpm_cpu_use_count[cluster][i];
+	return !cnt;
+}
+
+int mcpm_cpu_power_up(unsigned int cpu, unsigned int cluster)
+{
+	bool cpu_is_down, cluster_is_down;
+	int ret = 0;
+
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	if (!platform_ops)
+		return -EUNATCH; /* try not to shadow power_up errors */
+	might_sleep();
+
+	/*
+	 * Since this is called with IRQs enabled, and no arch_spin_lock_irq
+	 * variant exists, we need to disable IRQs manually here.
+	 */
+	local_irq_disable();
+	arch_spin_lock(&mcpm_lock);
+
+	cpu_is_down = !mcpm_cpu_use_count[cluster][cpu];
+	cluster_is_down = mcpm_cluster_unused(cluster);
+
+	mcpm_cpu_use_count[cluster][cpu]++;
+	/*
+	 * The only possible values are:
+	 * 0 = CPU down
+	 * 1 = CPU (still) up
+	 * 2 = CPU requested to be up before it had a chance
+	 *     to actually make itself down.
+	 * Any other value is a bug.
+	 */
+	BUG_ON(mcpm_cpu_use_count[cluster][cpu] != 1 &&
+	       mcpm_cpu_use_count[cluster][cpu] != 2);
+
+	if (cluster_is_down)
+		ret = platform_ops->cluster_powerup(cluster);
+	if (cpu_is_down && !ret)
+		ret = platform_ops->cpu_powerup(cpu, cluster);
+
+	arch_spin_unlock(&mcpm_lock);
+	local_irq_enable();
+	return ret;
+}
+
+typedef typeof(cpu_reset) phys_reset_t;
+
+void mcpm_cpu_power_down(void)
+{
+	unsigned int mpidr, cpu, cluster;
+	bool cpu_going_down, last_man;
+	phys_reset_t phys_reset;
+
+	mpidr = read_cpuid_mpidr();
+	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	if (WARN_ON_ONCE(!platform_ops))
+	       return;
+	BUG_ON(!irqs_disabled());
+
+	setup_mm_for_reboot();
+
+	__mcpm_cpu_going_down(cpu, cluster);
+	arch_spin_lock(&mcpm_lock);
+	BUG_ON(__mcpm_cluster_state(cluster) != CLUSTER_UP);
+
+	mcpm_cpu_use_count[cluster][cpu]--;
+	BUG_ON(mcpm_cpu_use_count[cluster][cpu] != 0 &&
+	       mcpm_cpu_use_count[cluster][cpu] != 1);
+	cpu_going_down = !mcpm_cpu_use_count[cluster][cpu];
+	last_man = mcpm_cluster_unused(cluster);
+
+	if (last_man && __mcpm_outbound_enter_critical(cpu, cluster)) {
+		platform_ops->cpu_powerdown_prepare(cpu, cluster);
+		platform_ops->cluster_powerdown_prepare(cluster);
+		arch_spin_unlock(&mcpm_lock);
+		platform_ops->cluster_cache_disable();
+		__mcpm_outbound_leave_critical(cluster, CLUSTER_DOWN);
+	} else {
+		if (cpu_going_down)
+			platform_ops->cpu_powerdown_prepare(cpu, cluster);
+		arch_spin_unlock(&mcpm_lock);
+		/*
+		 * If cpu_going_down is false here, that means a power_up
+		 * request raced ahead of us.  Even if we do not want to
+		 * shut this CPU down, the caller still expects execution
+		 * to return through the system resume entry path, like
+		 * when the WFI is aborted due to a new IRQ or the like..
+		 * So let's continue with cache cleaning in all cases.
+		 */
+		platform_ops->cpu_cache_disable();
+	}
+
+	__mcpm_cpu_down(cpu, cluster);
+
+	/* Now we are prepared for power-down, do it: */
+	if (cpu_going_down)
+		wfi();
+
+	/*
+	 * It is possible for a power_up request to happen concurrently
+	 * with a power_down request for the same CPU. In this case the
+	 * CPU might not be able to actually enter a powered down state
+	 * with the WFI instruction if the power_up request has removed
+	 * the required reset condition.  We must perform a re-entry in
+	 * the kernel as if the power_up method just had deasserted reset
+	 * on the CPU.
+	 */
+	phys_reset = (phys_reset_t)(unsigned long)__pa_symbol(cpu_reset);
+	phys_reset(__pa_symbol(mcpm_entry_point), false);
+
+	/* should never get here */
+	BUG();
+}
+
+int mcpm_wait_for_cpu_powerdown(unsigned int cpu, unsigned int cluster)
+{
+	int ret;
+
+	if (WARN_ON_ONCE(!platform_ops || !platform_ops->wait_for_powerdown))
+		return -EUNATCH;
+
+	ret = platform_ops->wait_for_powerdown(cpu, cluster);
+	if (ret)
+		pr_warn("%s: cpu %u, cluster %u failed to power down (%d)\n",
+			__func__, cpu, cluster, ret);
+
+	return ret;
+}
+
+void mcpm_cpu_suspend(void)
+{
+	if (WARN_ON_ONCE(!platform_ops))
+		return;
+
+	/* Some platforms might have to enable special resume modes, etc. */
+	if (platform_ops->cpu_suspend_prepare) {
+		unsigned int mpidr = read_cpuid_mpidr();
+		unsigned int cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+		unsigned int cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1); 
+		arch_spin_lock(&mcpm_lock);
+		platform_ops->cpu_suspend_prepare(cpu, cluster);
+		arch_spin_unlock(&mcpm_lock);
+	}
+	mcpm_cpu_power_down();
+}
+
+int mcpm_cpu_powered_up(void)
+{
+	unsigned int mpidr, cpu, cluster;
+	bool cpu_was_down, first_man;
+	unsigned long flags;
+
+	if (!platform_ops)
+		return -EUNATCH;
+
+	mpidr = read_cpuid_mpidr();
+	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	local_irq_save(flags);
+	arch_spin_lock(&mcpm_lock);
+
+	cpu_was_down = !mcpm_cpu_use_count[cluster][cpu];
+	first_man = mcpm_cluster_unused(cluster);
+
+	if (first_man && platform_ops->cluster_is_up)
+		platform_ops->cluster_is_up(cluster);
+	if (cpu_was_down)
+		mcpm_cpu_use_count[cluster][cpu] = 1;
+	if (platform_ops->cpu_is_up)
+		platform_ops->cpu_is_up(cpu, cluster);
+
+	arch_spin_unlock(&mcpm_lock);
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+#ifdef CONFIG_ARM_CPU_SUSPEND
+
+static int __init nocache_trampoline(unsigned long _arg)
+{
+	void (*cache_disable)(void) = (void *)_arg;
+	unsigned int mpidr = read_cpuid_mpidr();
+	unsigned int cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	unsigned int cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	phys_reset_t phys_reset;
+
+	mcpm_set_entry_vector(cpu, cluster, cpu_resume_no_hyp);
+	setup_mm_for_reboot();
+
+	__mcpm_cpu_going_down(cpu, cluster);
+	BUG_ON(!__mcpm_outbound_enter_critical(cpu, cluster));
+	cache_disable();
+	__mcpm_outbound_leave_critical(cluster, CLUSTER_DOWN);
+	__mcpm_cpu_down(cpu, cluster);
+
+	phys_reset = (phys_reset_t)(unsigned long)__pa_symbol(cpu_reset);
+	phys_reset(__pa_symbol(mcpm_entry_point), false);
+	BUG();
+}
+
+int __init mcpm_loopback(void (*cache_disable)(void))
+{
+	int ret;
+
+	/*
+	 * We're going to soft-restart the current CPU through the
+	 * low-level MCPM code by leveraging the suspend/resume
+	 * infrastructure. Let's play it safe by using cpu_pm_enter()
+	 * in case the CPU init code path resets the VFP or similar.
+	 */
+	local_irq_disable();
+	local_fiq_disable();
+	ret = cpu_pm_enter();
+	if (!ret) {
+		ret = cpu_suspend((unsigned long)cache_disable, nocache_trampoline);
+		cpu_pm_exit();
+	}
+	local_fiq_enable();
+	local_irq_enable();
+	if (ret)
+		pr_err("%s returned %d\n", __func__, ret);
+	return ret;
+}
+
+#endif
+
+extern unsigned long mcpm_power_up_setup_phys;
+
+int __init mcpm_sync_init(
+	void (*power_up_setup)(unsigned int affinity_level))
+{
+	unsigned int i, j, mpidr, this_cluster;
+
+	BUILD_BUG_ON(MCPM_SYNC_CLUSTER_SIZE * MAX_NR_CLUSTERS != sizeof mcpm_sync);
+	BUG_ON((unsigned long)&mcpm_sync & (__CACHE_WRITEBACK_GRANULE - 1));
+
+	/*
+	 * Set initial CPU and cluster states.
+	 * Only one cluster is assumed to be active at this point.
+	 */
+	for (i = 0; i < MAX_NR_CLUSTERS; i++) {
+		mcpm_sync.clusters[i].cluster = CLUSTER_DOWN;
+		mcpm_sync.clusters[i].inbound = INBOUND_NOT_COMING_UP;
+		for (j = 0; j < MAX_CPUS_PER_CLUSTER; j++)
+			mcpm_sync.clusters[i].cpus[j].cpu = CPU_DOWN;
+	}
+	mpidr = read_cpuid_mpidr();
+	this_cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	for_each_online_cpu(i) {
+		mcpm_cpu_use_count[this_cluster][i] = 1;
+		mcpm_sync.clusters[this_cluster].cpus[i].cpu = CPU_UP;
+	}
+	mcpm_sync.clusters[this_cluster].cluster = CLUSTER_UP;
+	sync_cache_w(&mcpm_sync);
+
+	if (power_up_setup) {
+		mcpm_power_up_setup_phys = __pa_symbol(power_up_setup);
+		sync_cache_w(&mcpm_power_up_setup_phys);
+	}
+
+	return 0;
+}
diff --git a/arch/arm/common/mcpm_head.S b/arch/arm/common/mcpm_head.S
new file mode 100644
index 000000000..f590e803c
--- /dev/null
+++ b/arch/arm/common/mcpm_head.S
@@ -0,0 +1,231 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * arch/arm/common/mcpm_head.S -- kernel entry point for multi-cluster PM
+ *
+ * Created by:  Nicolas Pitre, March 2012
+ * Copyright:   (C) 2012-2013  Linaro Limited
+ *
+ * Refer to Documentation/arch/arm/cluster-pm-race-avoidance.rst
+ * for details of the synchronisation algorithms used here.
+ */
+
+#include <linux/linkage.h>
+#include <asm/mcpm.h>
+#include <asm/assembler.h>
+
+#include "vlock.h"
+
+.arch armv7-a
+
+.if MCPM_SYNC_CLUSTER_CPUS
+.error "cpus must be the first member of struct mcpm_sync_struct"
+.endif
+
+	.macro	pr_dbg	string
+#if defined(CONFIG_DEBUG_LL) && defined(DEBUG)
+	b	1901f
+1902:	.asciz	"CPU"
+1903:	.asciz	" cluster"
+1904:	.asciz	": \string"
+	.align
+1901:	adr	r0, 1902b
+	bl	printascii
+	mov	r0, r9
+	bl	printhex2
+	adr	r0, 1903b
+	bl	printascii
+	mov	r0, r10
+	bl	printhex2
+	adr	r0, 1904b
+	bl	printascii
+#endif
+	.endm
+
+	.arm
+	.align
+
+ENTRY(mcpm_entry_point)
+
+ ARM_BE8(setend        be)
+ THUMB(	badr	r12, 1f		)
+ THUMB(	bx	r12		)
+ THUMB(	.thumb			)
+1:
+	mrc	p15, 0, r0, c0, c0, 5		@ MPIDR
+	ubfx	r9, r0, #0, #8			@ r9 = cpu
+	ubfx	r10, r0, #8, #8			@ r10 = cluster
+	mov	r3, #MAX_CPUS_PER_CLUSTER
+	mla	r4, r3, r10, r9			@ r4 = canonical CPU index
+	cmp	r4, #(MAX_CPUS_PER_CLUSTER * MAX_NR_CLUSTERS)
+	blo	2f
+
+	/* We didn't expect this CPU.  Try to cheaply make it quiet. */
+1:	wfi
+	wfe
+	b	1b
+
+2:	pr_dbg	"kernel mcpm_entry_point\n"
+
+	/*
+	 * MMU is off so we need to get to various variables in a
+	 * position independent way.
+	 */
+	adr	r5, 3f
+	ldmia	r5, {r0, r6, r7, r8, r11}
+	add	r0, r5, r0			@ r0 = mcpm_entry_early_pokes
+	add	r6, r5, r6			@ r6 = mcpm_entry_vectors
+	ldr	r7, [r5, r7]			@ r7 = mcpm_power_up_setup_phys
+	add	r8, r5, r8			@ r8 = mcpm_sync
+	add	r11, r5, r11			@ r11 = first_man_locks
+
+	@ Perform an early poke, if any
+	add	r0, r0, r4, lsl #3
+	ldmia	r0, {r0, r1}
+	teq	r0, #0
+	strne	r1, [r0]
+
+	mov	r0, #MCPM_SYNC_CLUSTER_SIZE
+	mla	r8, r0, r10, r8			@ r8 = sync cluster base
+
+	@ Signal that this CPU is coming UP:
+	mov	r0, #CPU_COMING_UP
+	mov	r5, #MCPM_SYNC_CPU_SIZE
+	mla	r5, r9, r5, r8			@ r5 = sync cpu address
+	strb	r0, [r5]
+
+	@ At this point, the cluster cannot unexpectedly enter the GOING_DOWN
+	@ state, because there is at least one active CPU (this CPU).
+
+	mov	r0, #VLOCK_SIZE
+	mla	r11, r0, r10, r11		@ r11 = cluster first man lock
+	mov	r0, r11
+	mov	r1, r9				@ cpu
+	bl	vlock_trylock			@ implies DMB
+
+	cmp	r0, #0				@ failed to get the lock?
+	bne	mcpm_setup_wait		@ wait for cluster setup if so
+
+	ldrb	r0, [r8, #MCPM_SYNC_CLUSTER_CLUSTER]
+	cmp	r0, #CLUSTER_UP			@ cluster already up?
+	bne	mcpm_setup			@ if not, set up the cluster
+
+	@ Otherwise, release the first man lock and skip setup:
+	mov	r0, r11
+	bl	vlock_unlock
+	b	mcpm_setup_complete
+
+mcpm_setup:
+	@ Control dependency implies strb not observable before previous ldrb.
+
+	@ Signal that the cluster is being brought up:
+	mov	r0, #INBOUND_COMING_UP
+	strb	r0, [r8, #MCPM_SYNC_CLUSTER_INBOUND]
+	dmb
+
+	@ Any CPU trying to take the cluster into CLUSTER_GOING_DOWN from this
+	@ point onwards will observe INBOUND_COMING_UP and abort.
+
+	@ Wait for any previously-pending cluster teardown operations to abort
+	@ or complete:
+mcpm_teardown_wait:
+	ldrb	r0, [r8, #MCPM_SYNC_CLUSTER_CLUSTER]
+	cmp	r0, #CLUSTER_GOING_DOWN
+	bne	first_man_setup
+	wfe
+	b	mcpm_teardown_wait
+
+first_man_setup:
+	dmb
+
+	@ If the outbound gave up before teardown started, skip cluster setup:
+
+	cmp	r0, #CLUSTER_UP
+	beq	mcpm_setup_leave
+
+	@ power_up_setup is now responsible for setting up the cluster:
+
+	cmp	r7, #0
+	mov	r0, #1		@ second (cluster) affinity level
+	blxne	r7		@ Call power_up_setup if defined
+	dmb
+
+	mov	r0, #CLUSTER_UP
+	strb	r0, [r8, #MCPM_SYNC_CLUSTER_CLUSTER]
+	dmb
+
+mcpm_setup_leave:
+	@ Leave the cluster setup critical section:
+
+	mov	r0, #INBOUND_NOT_COMING_UP
+	strb	r0, [r8, #MCPM_SYNC_CLUSTER_INBOUND]
+	dsb	st
+	sev
+
+	mov	r0, r11
+	bl	vlock_unlock	@ implies DMB
+	b	mcpm_setup_complete
+
+	@ In the contended case, non-first men wait here for cluster setup
+	@ to complete:
+mcpm_setup_wait:
+	ldrb	r0, [r8, #MCPM_SYNC_CLUSTER_CLUSTER]
+	cmp	r0, #CLUSTER_UP
+	wfene
+	bne	mcpm_setup_wait
+	dmb
+
+mcpm_setup_complete:
+	@ If a platform-specific CPU setup hook is needed, it is
+	@ called from here.
+
+	cmp	r7, #0
+	mov	r0, #0		@ first (CPU) affinity level
+	blxne	r7		@ Call power_up_setup if defined
+	dmb
+
+	@ Mark the CPU as up:
+
+	mov	r0, #CPU_UP
+	strb	r0, [r5]
+
+	@ Observability order of CPU_UP and opening of the gate does not matter.
+
+mcpm_entry_gated:
+	ldr	r5, [r6, r4, lsl #2]		@ r5 = CPU entry vector
+	cmp	r5, #0
+	wfeeq
+	beq	mcpm_entry_gated
+	dmb
+
+	pr_dbg	"released\n"
+	bx	r5
+
+	.align	2
+
+3:	.word	mcpm_entry_early_pokes - .
+	.word	mcpm_entry_vectors - 3b
+	.word	mcpm_power_up_setup_phys - 3b
+	.word	mcpm_sync - 3b
+	.word	first_man_locks - 3b
+
+ENDPROC(mcpm_entry_point)
+
+	.bss
+
+	.align	CACHE_WRITEBACK_ORDER
+	.type	first_man_locks, #object
+first_man_locks:
+	.space	VLOCK_SIZE * MAX_NR_CLUSTERS
+	.align	CACHE_WRITEBACK_ORDER
+
+	.type	mcpm_entry_vectors, #object
+ENTRY(mcpm_entry_vectors)
+	.space	4 * MAX_NR_CLUSTERS * MAX_CPUS_PER_CLUSTER
+
+	.type	mcpm_entry_early_pokes, #object
+ENTRY(mcpm_entry_early_pokes)
+	.space	8 * MAX_NR_CLUSTERS * MAX_CPUS_PER_CLUSTER
+
+	.type	mcpm_power_up_setup_phys, #object
+ENTRY(mcpm_power_up_setup_phys)
+	.space  4		@ set by mcpm_sync_init()
diff --git a/arch/arm/common/mcpm_platsmp.c b/arch/arm/common/mcpm_platsmp.c
new file mode 100644
index 000000000..3e172f4b0
--- /dev/null
+++ b/arch/arm/common/mcpm_platsmp.c
@@ -0,0 +1,96 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * linux/arch/arm/mach-vexpress/mcpm_platsmp.c
+ *
+ * Created by:  Nicolas Pitre, November 2012
+ * Copyright:   (C) 2012-2013  Linaro Limited
+ *
+ * Code to handle secondary CPU bringup and hotplug for the cluster power API.
+ */
+
+#include <linux/init.h>
+#include <linux/smp.h>
+#include <linux/spinlock.h>
+
+#include <asm/mcpm.h>
+#include <asm/smp.h>
+#include <asm/smp_plat.h>
+
+static void cpu_to_pcpu(unsigned int cpu,
+			unsigned int *pcpu, unsigned int *pcluster)
+{
+	unsigned int mpidr;
+
+	mpidr = cpu_logical_map(cpu);
+	*pcpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	*pcluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+}
+
+static int mcpm_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	unsigned int pcpu, pcluster, ret;
+	extern void secondary_startup(void);
+
+	cpu_to_pcpu(cpu, &pcpu, &pcluster);
+
+	pr_debug("%s: logical CPU %d is physical CPU %d cluster %d\n",
+		 __func__, cpu, pcpu, pcluster);
+
+	mcpm_set_entry_vector(pcpu, pcluster, NULL);
+	ret = mcpm_cpu_power_up(pcpu, pcluster);
+	if (ret)
+		return ret;
+	mcpm_set_entry_vector(pcpu, pcluster, secondary_startup);
+	arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+	dsb_sev();
+	return 0;
+}
+
+static void mcpm_secondary_init(unsigned int cpu)
+{
+	mcpm_cpu_powered_up();
+}
+
+#ifdef CONFIG_HOTPLUG_CPU
+
+static int mcpm_cpu_kill(unsigned int cpu)
+{
+	unsigned int pcpu, pcluster;
+
+	cpu_to_pcpu(cpu, &pcpu, &pcluster);
+
+	return !mcpm_wait_for_cpu_powerdown(pcpu, pcluster);
+}
+
+static bool mcpm_cpu_can_disable(unsigned int cpu)
+{
+	/* We assume all CPUs may be shut down. */
+	return true;
+}
+
+static void mcpm_cpu_die(unsigned int cpu)
+{
+	unsigned int mpidr, pcpu, pcluster;
+	mpidr = read_cpuid_mpidr();
+	pcpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	pcluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	mcpm_set_entry_vector(pcpu, pcluster, NULL);
+	mcpm_cpu_power_down();
+}
+
+#endif
+
+static const struct smp_operations mcpm_smp_ops __initconst = {
+	.smp_boot_secondary	= mcpm_boot_secondary,
+	.smp_secondary_init	= mcpm_secondary_init,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_kill		= mcpm_cpu_kill,
+	.cpu_can_disable	= mcpm_cpu_can_disable,
+	.cpu_die		= mcpm_cpu_die,
+#endif
+};
+
+void __init mcpm_smp_set_ops(void)
+{
+	smp_set_ops(&mcpm_smp_ops);
+}
diff --git a/arch/arm/common/sa1111.c b/arch/arm/common/sa1111.c
new file mode 100644
index 000000000..a0f854cf4
--- /dev/null
+++ b/arch/arm/common/sa1111.c
@@ -0,0 +1,1413 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * linux/arch/arm/common/sa1111.c
+ *
+ * SA1111 support
+ *
+ * Original code by John Dorsey
+ *
+ * This file contains all generic SA1111 support.
+ *
+ * All initialization functions provided here are intended to be called
+ * from machine specific code with proper arguments when required.
+ */
+#include <linux/module.h>
+#include <linux/gpio/driver.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/dma-map-ops.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+
+#include <asm/mach/irq.h>
+#include <asm/mach-types.h>
+#include <linux/sizes.h>
+
+#include <asm/hardware/sa1111.h>
+
+#ifdef CONFIG_ARCH_SA1100
+#include <mach/hardware.h>
+#endif
+
+/* SA1111 IRQs */
+#define IRQ_GPAIN0		(0)
+#define IRQ_GPAIN1		(1)
+#define IRQ_GPAIN2		(2)
+#define IRQ_GPAIN3		(3)
+#define IRQ_GPBIN0		(4)
+#define IRQ_GPBIN1		(5)
+#define IRQ_GPBIN2		(6)
+#define IRQ_GPBIN3		(7)
+#define IRQ_GPBIN4		(8)
+#define IRQ_GPBIN5		(9)
+#define IRQ_GPCIN0		(10)
+#define IRQ_GPCIN1		(11)
+#define IRQ_GPCIN2		(12)
+#define IRQ_GPCIN3		(13)
+#define IRQ_GPCIN4		(14)
+#define IRQ_GPCIN5		(15)
+#define IRQ_GPCIN6		(16)
+#define IRQ_GPCIN7		(17)
+#define IRQ_MSTXINT		(18)
+#define IRQ_MSRXINT		(19)
+#define IRQ_MSSTOPERRINT	(20)
+#define IRQ_TPTXINT		(21)
+#define IRQ_TPRXINT		(22)
+#define IRQ_TPSTOPERRINT	(23)
+#define SSPXMTINT		(24)
+#define SSPRCVINT		(25)
+#define SSPROR			(26)
+#define AUDXMTDMADONEA		(32)
+#define AUDRCVDMADONEA		(33)
+#define AUDXMTDMADONEB		(34)
+#define AUDRCVDMADONEB		(35)
+#define AUDTFSR			(36)
+#define AUDRFSR			(37)
+#define AUDTUR			(38)
+#define AUDROR			(39)
+#define AUDDTS			(40)
+#define AUDRDD			(41)
+#define AUDSTO			(42)
+#define IRQ_USBPWR		(43)
+#define IRQ_HCIM		(44)
+#define IRQ_HCIBUFFACC		(45)
+#define IRQ_HCIRMTWKP		(46)
+#define IRQ_NHCIMFCIR		(47)
+#define IRQ_USB_PORT_RESUME	(48)
+#define IRQ_S0_READY_NINT	(49)
+#define IRQ_S1_READY_NINT	(50)
+#define IRQ_S0_CD_VALID		(51)
+#define IRQ_S1_CD_VALID		(52)
+#define IRQ_S0_BVD1_STSCHG	(53)
+#define IRQ_S1_BVD1_STSCHG	(54)
+#define SA1111_IRQ_NR		(55)
+
+extern void sa1110_mb_enable(void);
+extern void sa1110_mb_disable(void);
+
+/*
+ * We keep the following data for the overall SA1111.  Note that the
+ * struct device and struct resource are "fake"; they should be supplied
+ * by the bus above us.  However, in the interests of getting all SA1111
+ * drivers converted over to the device model, we provide this as an
+ * anchor point for all the other drivers.
+ */
+struct sa1111 {
+	struct device	*dev;
+	struct clk	*clk;
+	unsigned long	phys;
+	int		irq;
+	int		irq_base;	/* base for cascaded on-chip IRQs */
+	spinlock_t	lock;
+	void __iomem	*base;
+	struct sa1111_platform_data *pdata;
+	struct irq_domain *irqdomain;
+	struct gpio_chip gc;
+#ifdef CONFIG_PM
+	void		*saved_state;
+#endif
+};
+
+/*
+ * We _really_ need to eliminate this.  Its only users
+ * are the PWM and DMA checking code.
+ */
+static struct sa1111 *g_sa1111;
+
+struct sa1111_dev_info {
+	unsigned long	offset;
+	unsigned long	skpcr_mask;
+	bool		dma;
+	unsigned int	devid;
+	unsigned int	hwirq[6];
+};
+
+static struct sa1111_dev_info sa1111_devices[] = {
+	{
+		.offset		= SA1111_USB,
+		.skpcr_mask	= SKPCR_UCLKEN,
+		.dma		= true,
+		.devid		= SA1111_DEVID_USB,
+		.hwirq = {
+			IRQ_USBPWR,
+			IRQ_HCIM,
+			IRQ_HCIBUFFACC,
+			IRQ_HCIRMTWKP,
+			IRQ_NHCIMFCIR,
+			IRQ_USB_PORT_RESUME
+		},
+	},
+	{
+		.offset		= 0x0600,
+		.skpcr_mask	= SKPCR_I2SCLKEN | SKPCR_L3CLKEN,
+		.dma		= true,
+		.devid		= SA1111_DEVID_SAC,
+		.hwirq = {
+			AUDXMTDMADONEA,
+			AUDXMTDMADONEB,
+			AUDRCVDMADONEA,
+			AUDRCVDMADONEB
+		},
+	},
+	{
+		.offset		= 0x0800,
+		.skpcr_mask	= SKPCR_SCLKEN,
+		.devid		= SA1111_DEVID_SSP,
+	},
+	{
+		.offset		= SA1111_KBD,
+		.skpcr_mask	= SKPCR_PTCLKEN,
+		.devid		= SA1111_DEVID_PS2_KBD,
+		.hwirq = {
+			IRQ_TPRXINT,
+			IRQ_TPTXINT
+		},
+	},
+	{
+		.offset		= SA1111_MSE,
+		.skpcr_mask	= SKPCR_PMCLKEN,
+		.devid		= SA1111_DEVID_PS2_MSE,
+		.hwirq = {
+			IRQ_MSRXINT,
+			IRQ_MSTXINT
+		},
+	},
+	{
+		.offset		= 0x1800,
+		.skpcr_mask	= 0,
+		.devid		= SA1111_DEVID_PCMCIA,
+		.hwirq = {
+			IRQ_S0_READY_NINT,
+			IRQ_S0_CD_VALID,
+			IRQ_S0_BVD1_STSCHG,
+			IRQ_S1_READY_NINT,
+			IRQ_S1_CD_VALID,
+			IRQ_S1_BVD1_STSCHG,
+		},
+	},
+};
+
+static int sa1111_map_irq(struct sa1111 *sachip, irq_hw_number_t hwirq)
+{
+	return irq_create_mapping(sachip->irqdomain, hwirq);
+}
+
+/*
+ * SA1111 interrupt support.  Since clearing an IRQ while there are
+ * active IRQs causes the interrupt output to pulse, the upper levels
+ * will call us again if there are more interrupts to process.
+ */
+static void sa1111_irq_handler(struct irq_desc *desc)
+{
+	unsigned int stat0, stat1, i;
+	struct sa1111 *sachip = irq_desc_get_handler_data(desc);
+	struct irq_domain *irqdomain;
+	void __iomem *mapbase = sachip->base + SA1111_INTC;
+
+	stat0 = readl_relaxed(mapbase + SA1111_INTSTATCLR0);
+	stat1 = readl_relaxed(mapbase + SA1111_INTSTATCLR1);
+
+	writel_relaxed(stat0, mapbase + SA1111_INTSTATCLR0);
+
+	desc->irq_data.chip->irq_ack(&desc->irq_data);
+
+	writel_relaxed(stat1, mapbase + SA1111_INTSTATCLR1);
+
+	if (stat0 == 0 && stat1 == 0) {
+		handle_bad_irq(desc);
+		return;
+	}
+
+	irqdomain = sachip->irqdomain;
+
+	for (i = 0; stat0; i++, stat0 >>= 1)
+		if (stat0 & 1)
+			generic_handle_domain_irq(irqdomain, i);
+
+	for (i = 32; stat1; i++, stat1 >>= 1)
+		if (stat1 & 1)
+			generic_handle_domain_irq(irqdomain, i);
+
+	/* For level-based interrupts */
+	desc->irq_data.chip->irq_unmask(&desc->irq_data);
+}
+
+static u32 sa1111_irqmask(struct irq_data *d)
+{
+	return BIT(irqd_to_hwirq(d) & 31);
+}
+
+static int sa1111_irqbank(struct irq_data *d)
+{
+	return (irqd_to_hwirq(d) / 32) * 4;
+}
+
+static void sa1111_ack_irq(struct irq_data *d)
+{
+}
+
+static void sa1111_mask_irq(struct irq_data *d)
+{
+	struct sa1111 *sachip = irq_data_get_irq_chip_data(d);
+	void __iomem *mapbase = sachip->base + SA1111_INTC + sa1111_irqbank(d);
+	u32 ie;
+
+	ie = readl_relaxed(mapbase + SA1111_INTEN0);
+	ie &= ~sa1111_irqmask(d);
+	writel(ie, mapbase + SA1111_INTEN0);
+}
+
+static void sa1111_unmask_irq(struct irq_data *d)
+{
+	struct sa1111 *sachip = irq_data_get_irq_chip_data(d);
+	void __iomem *mapbase = sachip->base + SA1111_INTC + sa1111_irqbank(d);
+	u32 ie;
+
+	ie = readl_relaxed(mapbase + SA1111_INTEN0);
+	ie |= sa1111_irqmask(d);
+	writel_relaxed(ie, mapbase + SA1111_INTEN0);
+}
+
+/*
+ * Attempt to re-trigger the interrupt.  The SA1111 contains a register
+ * (INTSET) which claims to do this.  However, in practice no amount of
+ * manipulation of INTEN and INTSET guarantees that the interrupt will
+ * be triggered.  In fact, its very difficult, if not impossible to get
+ * INTSET to re-trigger the interrupt.
+ */
+static int sa1111_retrigger_irq(struct irq_data *d)
+{
+	struct sa1111 *sachip = irq_data_get_irq_chip_data(d);
+	void __iomem *mapbase = sachip->base + SA1111_INTC + sa1111_irqbank(d);
+	u32 ip, mask = sa1111_irqmask(d);
+	int i;
+
+	ip = readl_relaxed(mapbase + SA1111_INTPOL0);
+	for (i = 0; i < 8; i++) {
+		writel_relaxed(ip ^ mask, mapbase + SA1111_INTPOL0);
+		writel_relaxed(ip, mapbase + SA1111_INTPOL0);
+		if (readl_relaxed(mapbase + SA1111_INTSTATCLR0) & mask)
+			break;
+	}
+
+	if (i == 8) {
+		pr_err("Danger Will Robinson: failed to re-trigger IRQ%d\n",
+		       d->irq);
+		return 0;
+	}
+
+	return 1;
+}
+
+static int sa1111_type_irq(struct irq_data *d, unsigned int flags)
+{
+	struct sa1111 *sachip = irq_data_get_irq_chip_data(d);
+	void __iomem *mapbase = sachip->base + SA1111_INTC + sa1111_irqbank(d);
+	u32 ip, mask = sa1111_irqmask(d);
+
+	if (flags == IRQ_TYPE_PROBE)
+		return 0;
+
+	if ((!(flags & IRQ_TYPE_EDGE_RISING) ^ !(flags & IRQ_TYPE_EDGE_FALLING)) == 0)
+		return -EINVAL;
+
+	ip = readl_relaxed(mapbase + SA1111_INTPOL0);
+	if (flags & IRQ_TYPE_EDGE_RISING)
+		ip &= ~mask;
+	else
+		ip |= mask;
+	writel_relaxed(ip, mapbase + SA1111_INTPOL0);
+	writel_relaxed(ip, mapbase + SA1111_WAKEPOL0);
+
+	return 0;
+}
+
+static int sa1111_wake_irq(struct irq_data *d, unsigned int on)
+{
+	struct sa1111 *sachip = irq_data_get_irq_chip_data(d);
+	void __iomem *mapbase = sachip->base + SA1111_INTC + sa1111_irqbank(d);
+	u32 we, mask = sa1111_irqmask(d);
+
+	we = readl_relaxed(mapbase + SA1111_WAKEEN0);
+	if (on)
+		we |= mask;
+	else
+		we &= ~mask;
+	writel_relaxed(we, mapbase + SA1111_WAKEEN0);
+
+	return 0;
+}
+
+static struct irq_chip sa1111_irq_chip = {
+	.name		= "SA1111",
+	.irq_ack	= sa1111_ack_irq,
+	.irq_mask	= sa1111_mask_irq,
+	.irq_unmask	= sa1111_unmask_irq,
+	.irq_retrigger	= sa1111_retrigger_irq,
+	.irq_set_type	= sa1111_type_irq,
+	.irq_set_wake	= sa1111_wake_irq,
+};
+
+static int sa1111_irqdomain_map(struct irq_domain *d, unsigned int irq,
+	irq_hw_number_t hwirq)
+{
+	struct sa1111 *sachip = d->host_data;
+
+	/* Disallow unavailable interrupts */
+	if (hwirq > SSPROR && hwirq < AUDXMTDMADONEA)
+		return -EINVAL;
+
+	irq_set_chip_data(irq, sachip);
+	irq_set_chip_and_handler(irq, &sa1111_irq_chip, handle_edge_irq);
+	irq_clear_status_flags(irq, IRQ_NOREQUEST | IRQ_NOPROBE);
+
+	return 0;
+}
+
+static const struct irq_domain_ops sa1111_irqdomain_ops = {
+	.map = sa1111_irqdomain_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
+static int sa1111_setup_irq(struct sa1111 *sachip, unsigned irq_base)
+{
+	void __iomem *irqbase = sachip->base + SA1111_INTC;
+	int ret;
+
+	/*
+	 * We're guaranteed that this region hasn't been taken.
+	 */
+	request_mem_region(sachip->phys + SA1111_INTC, 512, "irq");
+
+	ret = irq_alloc_descs(-1, irq_base, SA1111_IRQ_NR, -1);
+	if (ret <= 0) {
+		dev_err(sachip->dev, "unable to allocate %u irqs: %d\n",
+			SA1111_IRQ_NR, ret);
+		if (ret == 0)
+			ret = -EINVAL;
+		return ret;
+	}
+
+	sachip->irq_base = ret;
+
+	/* disable all IRQs */
+	writel_relaxed(0, irqbase + SA1111_INTEN0);
+	writel_relaxed(0, irqbase + SA1111_INTEN1);
+	writel_relaxed(0, irqbase + SA1111_WAKEEN0);
+	writel_relaxed(0, irqbase + SA1111_WAKEEN1);
+
+	/*
+	 * detect on rising edge.  Note: Feb 2001 Errata for SA1111
+	 * specifies that S0ReadyInt and S1ReadyInt should be '1'.
+	 */
+	writel_relaxed(0, irqbase + SA1111_INTPOL0);
+	writel_relaxed(BIT(IRQ_S0_READY_NINT & 31) |
+		       BIT(IRQ_S1_READY_NINT & 31),
+		       irqbase + SA1111_INTPOL1);
+
+	/* clear all IRQs */
+	writel_relaxed(~0, irqbase + SA1111_INTSTATCLR0);
+	writel_relaxed(~0, irqbase + SA1111_INTSTATCLR1);
+
+	sachip->irqdomain = irq_domain_create_linear(NULL, SA1111_IRQ_NR,
+						     &sa1111_irqdomain_ops,
+						     sachip);
+	if (!sachip->irqdomain) {
+		irq_free_descs(sachip->irq_base, SA1111_IRQ_NR);
+		return -ENOMEM;
+	}
+
+	irq_domain_associate_many(sachip->irqdomain,
+				  sachip->irq_base + IRQ_GPAIN0,
+				  IRQ_GPAIN0, SSPROR + 1 - IRQ_GPAIN0);
+	irq_domain_associate_many(sachip->irqdomain,
+				  sachip->irq_base + AUDXMTDMADONEA,
+				  AUDXMTDMADONEA,
+				  IRQ_S1_BVD1_STSCHG + 1 - AUDXMTDMADONEA);
+
+	/*
+	 * Register SA1111 interrupt
+	 */
+	irq_set_irq_type(sachip->irq, IRQ_TYPE_EDGE_RISING);
+	irq_set_chained_handler_and_data(sachip->irq, sa1111_irq_handler,
+					 sachip);
+
+	dev_info(sachip->dev, "Providing IRQ%u-%u\n",
+		sachip->irq_base, sachip->irq_base + SA1111_IRQ_NR - 1);
+
+	return 0;
+}
+
+static void sa1111_remove_irq(struct sa1111 *sachip)
+{
+	struct irq_domain *domain = sachip->irqdomain;
+	void __iomem *irqbase = sachip->base + SA1111_INTC;
+	int i;
+
+	/* disable all IRQs */
+	writel_relaxed(0, irqbase + SA1111_INTEN0);
+	writel_relaxed(0, irqbase + SA1111_INTEN1);
+	writel_relaxed(0, irqbase + SA1111_WAKEEN0);
+	writel_relaxed(0, irqbase + SA1111_WAKEEN1);
+
+	irq_set_chained_handler_and_data(sachip->irq, NULL, NULL);
+	for (i = 0; i < SA1111_IRQ_NR; i++)
+		irq_dispose_mapping(irq_find_mapping(domain, i));
+	irq_domain_remove(domain);
+
+	release_mem_region(sachip->phys + SA1111_INTC, 512);
+}
+
+enum {
+	SA1111_GPIO_PXDDR = (SA1111_GPIO_PADDR - SA1111_GPIO_PADDR),
+	SA1111_GPIO_PXDRR = (SA1111_GPIO_PADRR - SA1111_GPIO_PADDR),
+	SA1111_GPIO_PXDWR = (SA1111_GPIO_PADWR - SA1111_GPIO_PADDR),
+	SA1111_GPIO_PXSDR = (SA1111_GPIO_PASDR - SA1111_GPIO_PADDR),
+	SA1111_GPIO_PXSSR = (SA1111_GPIO_PASSR - SA1111_GPIO_PADDR),
+};
+
+static struct sa1111 *gc_to_sa1111(struct gpio_chip *gc)
+{
+	return container_of(gc, struct sa1111, gc);
+}
+
+static void __iomem *sa1111_gpio_map_reg(struct sa1111 *sachip, unsigned offset)
+{
+	void __iomem *reg = sachip->base + SA1111_GPIO;
+
+	if (offset < 4)
+		return reg + SA1111_GPIO_PADDR;
+	if (offset < 10)
+		return reg + SA1111_GPIO_PBDDR;
+	if (offset < 18)
+		return reg + SA1111_GPIO_PCDDR;
+	return NULL;
+}
+
+static u32 sa1111_gpio_map_bit(unsigned offset)
+{
+	if (offset < 4)
+		return BIT(offset);
+	if (offset < 10)
+		return BIT(offset - 4);
+	if (offset < 18)
+		return BIT(offset - 10);
+	return 0;
+}
+
+static void sa1111_gpio_modify(void __iomem *reg, u32 mask, u32 set)
+{
+	u32 val;
+
+	val = readl_relaxed(reg);
+	val &= ~mask;
+	val |= mask & set;
+	writel_relaxed(val, reg);
+}
+
+static int sa1111_gpio_get_direction(struct gpio_chip *gc, unsigned offset)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+	void __iomem *reg = sa1111_gpio_map_reg(sachip, offset);
+	u32 mask = sa1111_gpio_map_bit(offset);
+
+	return !!(readl_relaxed(reg + SA1111_GPIO_PXDDR) & mask);
+}
+
+static int sa1111_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+	unsigned long flags;
+	void __iomem *reg = sa1111_gpio_map_reg(sachip, offset);
+	u32 mask = sa1111_gpio_map_bit(offset);
+
+	spin_lock_irqsave(&sachip->lock, flags);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXDDR, mask, mask);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXSDR, mask, mask);
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+	return 0;
+}
+
+static int sa1111_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
+	int value)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+	unsigned long flags;
+	void __iomem *reg = sa1111_gpio_map_reg(sachip, offset);
+	u32 mask = sa1111_gpio_map_bit(offset);
+
+	spin_lock_irqsave(&sachip->lock, flags);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXDWR, mask, value ? mask : 0);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXSSR, mask, value ? mask : 0);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXDDR, mask, 0);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXSDR, mask, 0);
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+	return 0;
+}
+
+static int sa1111_gpio_get(struct gpio_chip *gc, unsigned offset)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+	void __iomem *reg = sa1111_gpio_map_reg(sachip, offset);
+	u32 mask = sa1111_gpio_map_bit(offset);
+
+	return !!(readl_relaxed(reg + SA1111_GPIO_PXDRR) & mask);
+}
+
+static int sa1111_gpio_set(struct gpio_chip *gc, unsigned int offset, int value)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+	unsigned long flags;
+	void __iomem *reg = sa1111_gpio_map_reg(sachip, offset);
+	u32 mask = sa1111_gpio_map_bit(offset);
+
+	spin_lock_irqsave(&sachip->lock, flags);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXDWR, mask, value ? mask : 0);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PXSSR, mask, value ? mask : 0);
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+	return 0;
+}
+
+static int sa1111_gpio_set_multiple(struct gpio_chip *gc, unsigned long *mask,
+				    unsigned long *bits)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+	unsigned long flags;
+	void __iomem *reg = sachip->base + SA1111_GPIO;
+	u32 msk, val;
+
+	msk = *mask;
+	val = *bits;
+
+	spin_lock_irqsave(&sachip->lock, flags);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PADWR, msk & 15, val);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PASSR, msk & 15, val);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PBDWR, (msk >> 4) & 255, val >> 4);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PBSSR, (msk >> 4) & 255, val >> 4);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PCDWR, (msk >> 12) & 255, val >> 12);
+	sa1111_gpio_modify(reg + SA1111_GPIO_PCSSR, (msk >> 12) & 255, val >> 12);
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+	return 0;
+}
+
+static int sa1111_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct sa1111 *sachip = gc_to_sa1111(gc);
+
+	return sa1111_map_irq(sachip, offset);
+}
+
+static int sa1111_setup_gpios(struct sa1111 *sachip)
+{
+	sachip->gc.label = "sa1111";
+	sachip->gc.parent = sachip->dev;
+	sachip->gc.owner = THIS_MODULE;
+	sachip->gc.get_direction = sa1111_gpio_get_direction;
+	sachip->gc.direction_input = sa1111_gpio_direction_input;
+	sachip->gc.direction_output = sa1111_gpio_direction_output;
+	sachip->gc.get = sa1111_gpio_get;
+	sachip->gc.set = sa1111_gpio_set;
+	sachip->gc.set_multiple = sa1111_gpio_set_multiple;
+	sachip->gc.to_irq = sa1111_gpio_to_irq;
+	sachip->gc.base = -1;
+	sachip->gc.ngpio = 18;
+
+	return devm_gpiochip_add_data(sachip->dev, &sachip->gc, sachip);
+}
+
+/*
+ * Bring the SA1111 out of reset.  This requires a set procedure:
+ *  1. nRESET asserted (by hardware)
+ *  2. CLK turned on from SA1110
+ *  3. nRESET deasserted
+ *  4. VCO turned on, PLL_BYPASS turned off
+ *  5. Wait lock time, then assert RCLKEn
+ *  7. PCR set to allow clocking of individual functions
+ *
+ * Until we've done this, the only registers we can access are:
+ *   SBI_SKCR
+ *   SBI_SMCR
+ *   SBI_SKID
+ */
+static void sa1111_wake(struct sa1111 *sachip)
+{
+	unsigned long flags, r;
+
+	spin_lock_irqsave(&sachip->lock, flags);
+
+	clk_enable(sachip->clk);
+
+	/*
+	 * Turn VCO on, and disable PLL Bypass.
+	 */
+	r = readl_relaxed(sachip->base + SA1111_SKCR);
+	r &= ~SKCR_VCO_OFF;
+	writel_relaxed(r, sachip->base + SA1111_SKCR);
+	r |= SKCR_PLL_BYPASS | SKCR_OE_EN;
+	writel_relaxed(r, sachip->base + SA1111_SKCR);
+
+	/*
+	 * Wait lock time.  SA1111 manual _doesn't_
+	 * specify a figure for this!  We choose 100us.
+	 */
+	udelay(100);
+
+	/*
+	 * Enable RCLK.  We also ensure that RDYEN is set.
+	 */
+	r |= SKCR_RCLKEN | SKCR_RDYEN;
+	writel_relaxed(r, sachip->base + SA1111_SKCR);
+
+	/*
+	 * Wait 14 RCLK cycles for the chip to finish coming out
+	 * of reset. (RCLK=24MHz).  This is 590ns.
+	 */
+	udelay(1);
+
+	/*
+	 * Ensure all clocks are initially off.
+	 */
+	writel_relaxed(0, sachip->base + SA1111_SKPCR);
+
+	spin_unlock_irqrestore(&sachip->lock, flags);
+}
+
+#ifdef CONFIG_ARCH_SA1100
+
+static u32 sa1111_dma_mask[] = {
+	~0,
+	~(1 << 20),
+	~(1 << 23),
+	~(1 << 24),
+	~(1 << 25),
+	~(1 << 20),
+	~(1 << 20),
+	0,
+};
+
+/*
+ * Configure the SA1111 shared memory controller.
+ */
+static void
+sa1111_configure_smc(struct sa1111 *sachip, int sdram, unsigned int drac,
+		     unsigned int cas_latency)
+{
+	unsigned int smcr = SMCR_DTIM | SMCR_MBGE | FInsrt(drac, SMCR_DRAC);
+
+	if (cas_latency == 3)
+		smcr |= SMCR_CLAT;
+
+	writel_relaxed(smcr, sachip->base + SA1111_SMCR);
+
+	/*
+	 * Now clear the bits in the DMA mask to work around the SA1111
+	 * DMA erratum (Intel StrongARM SA-1111 Microprocessor Companion
+	 * Chip Specification Update, June 2000, Erratum #7).
+	 */
+	if (sachip->dev->dma_mask)
+		*sachip->dev->dma_mask &= sa1111_dma_mask[drac >> 2];
+
+	sachip->dev->coherent_dma_mask &= sa1111_dma_mask[drac >> 2];
+}
+#endif
+
+static void sa1111_dev_release(struct device *_dev)
+{
+	struct sa1111_dev *dev = to_sa1111_device(_dev);
+
+	kfree(dev);
+}
+
+static int
+sa1111_init_one_child(struct sa1111 *sachip, struct resource *parent,
+		      struct sa1111_dev_info *info)
+{
+	struct sa1111_dev *dev;
+	unsigned i;
+	int ret;
+
+	dev = kzalloc_obj(struct sa1111_dev);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	device_initialize(&dev->dev);
+	dev_set_name(&dev->dev, "%4.4lx", info->offset);
+	dev->devid	 = info->devid;
+	dev->dev.parent  = sachip->dev;
+	dev->dev.bus     = &sa1111_bus_type;
+	dev->dev.release = sa1111_dev_release;
+	dev->res.start   = sachip->phys + info->offset;
+	dev->res.end     = dev->res.start + 511;
+	dev->res.name    = dev_name(&dev->dev);
+	dev->res.flags   = IORESOURCE_MEM;
+	dev->mapbase     = sachip->base + info->offset;
+	dev->skpcr_mask  = info->skpcr_mask;
+
+	for (i = 0; i < ARRAY_SIZE(info->hwirq); i++)
+		dev->hwirq[i] = info->hwirq[i];
+
+	/*
+	 * If the parent device has a DMA mask associated with it, and
+	 * this child supports DMA, propagate it down to the children.
+	 */
+	if (info->dma && sachip->dev->dma_mask) {
+		dev->dma_mask = *sachip->dev->dma_mask;
+		dev->dev.dma_mask = &dev->dma_mask;
+		dev->dev.coherent_dma_mask = sachip->dev->coherent_dma_mask;
+	}
+
+	ret = request_resource(parent, &dev->res);
+	if (ret) {
+		dev_err(sachip->dev, "failed to allocate resource for %s\n",
+			dev->res.name);
+		goto err_resource;
+	}
+
+	ret = device_add(&dev->dev);
+	if (ret)
+		goto err_add;
+	return 0;
+
+ err_add:
+	release_resource(&dev->res);
+ err_resource:
+	put_device(&dev->dev);
+ err_alloc:
+	return ret;
+}
+
+static int __sa1111_probe(struct device *me, struct resource *mem, int irq)
+{
+	struct sa1111_platform_data *pd = me->platform_data;
+	struct sa1111 *sachip;
+	unsigned long id;
+	unsigned int has_devs;
+	int i, ret = -ENODEV;
+
+	if (!pd)
+		return -EINVAL;
+
+	sachip = devm_kzalloc(me, sizeof(struct sa1111), GFP_KERNEL);
+	if (!sachip)
+		return -ENOMEM;
+
+	sachip->clk = devm_clk_get(me, "SA1111_CLK");
+	if (IS_ERR(sachip->clk))
+		return PTR_ERR(sachip->clk);
+
+	ret = clk_prepare(sachip->clk);
+	if (ret)
+		return ret;
+
+	spin_lock_init(&sachip->lock);
+
+	sachip->dev = me;
+	dev_set_drvdata(sachip->dev, sachip);
+
+	sachip->pdata = pd;
+	sachip->phys = mem->start;
+	sachip->irq = irq;
+
+	/*
+	 * Map the whole region.  This also maps the
+	 * registers for our children.
+	 */
+	sachip->base = ioremap(mem->start, PAGE_SIZE * 2);
+	if (!sachip->base) {
+		ret = -ENOMEM;
+		goto err_clk_unprep;
+	}
+
+	/*
+	 * Probe for the chip.  Only touch the SBI registers.
+	 */
+	id = readl_relaxed(sachip->base + SA1111_SKID);
+	if ((id & SKID_ID_MASK) != SKID_SA1111_ID) {
+		printk(KERN_DEBUG "SA1111 not detected: ID = %08lx\n", id);
+		ret = -ENODEV;
+		goto err_unmap;
+	}
+
+	pr_info("SA1111 Microprocessor Companion Chip: silicon revision %lx, metal revision %lx\n",
+		(id & SKID_SIREV_MASK) >> 4, id & SKID_MTREV_MASK);
+
+	/*
+	 * We found it.  Wake the chip up, and initialise.
+	 */
+	sa1111_wake(sachip);
+
+	/*
+	 * The interrupt controller must be initialised before any
+	 * other device to ensure that the interrupts are available.
+	 */
+	ret = sa1111_setup_irq(sachip, pd->irq_base);
+	if (ret)
+		goto err_clk;
+
+	/* Setup the GPIOs - should really be done after the IRQ setup */
+	ret = sa1111_setup_gpios(sachip);
+	if (ret)
+		goto err_irq;
+
+#ifdef CONFIG_ARCH_SA1100
+	{
+	unsigned int val;
+
+	/*
+	 * The SDRAM configuration of the SA1110 and the SA1111 must
+	 * match.  This is very important to ensure that SA1111 accesses
+	 * don't corrupt the SDRAM.  Note that this ungates the SA1111's
+	 * MBGNT signal, so we must have called sa1110_mb_disable()
+	 * beforehand.
+	 */
+	sa1111_configure_smc(sachip, 1,
+			     FExtr(MDCNFG, MDCNFG_SA1110_DRAC0),
+			     FExtr(MDCNFG, MDCNFG_SA1110_TDL0));
+
+	/*
+	 * We only need to turn on DCLK whenever we want to use the
+	 * DMA.  It can otherwise be held firmly in the off position.
+	 * (currently, we always enable it.)
+	 */
+	val = readl_relaxed(sachip->base + SA1111_SKPCR);
+	writel_relaxed(val | SKPCR_DCLKEN, sachip->base + SA1111_SKPCR);
+
+	/*
+	 * Enable the SA1110 memory bus request and grant signals.
+	 */
+	sa1110_mb_enable();
+	}
+#endif
+
+	g_sa1111 = sachip;
+
+	has_devs = ~0;
+	if (pd)
+		has_devs &= ~pd->disable_devs;
+
+	for (i = 0; i < ARRAY_SIZE(sa1111_devices); i++)
+		if (sa1111_devices[i].devid & has_devs)
+			sa1111_init_one_child(sachip, mem, &sa1111_devices[i]);
+
+	return 0;
+
+ err_irq:
+	sa1111_remove_irq(sachip);
+ err_clk:
+	clk_disable(sachip->clk);
+ err_unmap:
+	iounmap(sachip->base);
+ err_clk_unprep:
+	clk_unprepare(sachip->clk);
+	return ret;
+}
+
+static int sa1111_remove_one(struct device *dev, void *data)
+{
+	struct sa1111_dev *sadev = to_sa1111_device(dev);
+	if (dev->bus != &sa1111_bus_type)
+		return 0;
+	device_del(&sadev->dev);
+	release_resource(&sadev->res);
+	put_device(&sadev->dev);
+	return 0;
+}
+
+static void __sa1111_remove(struct sa1111 *sachip)
+{
+	device_for_each_child(sachip->dev, NULL, sa1111_remove_one);
+
+	sa1111_remove_irq(sachip);
+
+	clk_disable(sachip->clk);
+	clk_unprepare(sachip->clk);
+
+	iounmap(sachip->base);
+}
+
+struct sa1111_save_data {
+	unsigned int	skcr;
+	unsigned int	skpcr;
+	unsigned int	skcdr;
+	unsigned char	skaud;
+	unsigned char	skpwm0;
+	unsigned char	skpwm1;
+
+	/*
+	 * Interrupt controller
+	 */
+	unsigned int	intpol0;
+	unsigned int	intpol1;
+	unsigned int	inten0;
+	unsigned int	inten1;
+	unsigned int	wakepol0;
+	unsigned int	wakepol1;
+	unsigned int	wakeen0;
+	unsigned int	wakeen1;
+};
+
+#ifdef CONFIG_PM
+
+static int sa1111_suspend_noirq(struct device *dev)
+{
+	struct sa1111 *sachip = dev_get_drvdata(dev);
+	struct sa1111_save_data *save;
+	unsigned long flags;
+	unsigned int val;
+	void __iomem *base;
+
+	save = kmalloc_obj(struct sa1111_save_data);
+	if (!save)
+		return -ENOMEM;
+	sachip->saved_state = save;
+
+	spin_lock_irqsave(&sachip->lock, flags);
+
+	/*
+	 * Save state.
+	 */
+	base = sachip->base;
+	save->skcr     = readl_relaxed(base + SA1111_SKCR);
+	save->skpcr    = readl_relaxed(base + SA1111_SKPCR);
+	save->skcdr    = readl_relaxed(base + SA1111_SKCDR);
+	save->skaud    = readl_relaxed(base + SA1111_SKAUD);
+	save->skpwm0   = readl_relaxed(base + SA1111_SKPWM0);
+	save->skpwm1   = readl_relaxed(base + SA1111_SKPWM1);
+
+	writel_relaxed(0, sachip->base + SA1111_SKPWM0);
+	writel_relaxed(0, sachip->base + SA1111_SKPWM1);
+
+	base = sachip->base + SA1111_INTC;
+	save->intpol0  = readl_relaxed(base + SA1111_INTPOL0);
+	save->intpol1  = readl_relaxed(base + SA1111_INTPOL1);
+	save->inten0   = readl_relaxed(base + SA1111_INTEN0);
+	save->inten1   = readl_relaxed(base + SA1111_INTEN1);
+	save->wakepol0 = readl_relaxed(base + SA1111_WAKEPOL0);
+	save->wakepol1 = readl_relaxed(base + SA1111_WAKEPOL1);
+	save->wakeen0  = readl_relaxed(base + SA1111_WAKEEN0);
+	save->wakeen1  = readl_relaxed(base + SA1111_WAKEEN1);
+
+	/*
+	 * Disable.
+	 */
+	val = readl_relaxed(sachip->base + SA1111_SKCR);
+	writel_relaxed(val | SKCR_SLEEP, sachip->base + SA1111_SKCR);
+
+	clk_disable(sachip->clk);
+
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+#ifdef CONFIG_ARCH_SA1100
+	sa1110_mb_disable();
+#endif
+
+	return 0;
+}
+
+/*
+ *	sa1111_resume - Restore the SA1111 device state.
+ *	@dev: device to restore
+ *
+ *	Restore the general state of the SA1111; clock control and
+ *	interrupt controller.  Other parts of the SA1111 must be
+ *	restored by their respective drivers, and must be called
+ *	via LDM after this function.
+ */
+static int sa1111_resume_noirq(struct device *dev)
+{
+	struct sa1111 *sachip = dev_get_drvdata(dev);
+	struct sa1111_save_data *save;
+	unsigned long flags, id;
+	void __iomem *base;
+
+	save = sachip->saved_state;
+	if (!save)
+		return 0;
+
+	/*
+	 * Ensure that the SA1111 is still here.
+	 * FIXME: shouldn't do this here.
+	 */
+	id = readl_relaxed(sachip->base + SA1111_SKID);
+	if ((id & SKID_ID_MASK) != SKID_SA1111_ID) {
+		__sa1111_remove(sachip);
+		dev_set_drvdata(dev, NULL);
+		kfree(save);
+		return 0;
+	}
+
+	/*
+	 * First of all, wake up the chip.
+	 */
+	sa1111_wake(sachip);
+
+#ifdef CONFIG_ARCH_SA1100
+	/* Enable the memory bus request/grant signals */
+	sa1110_mb_enable();
+#endif
+
+	/*
+	 * Only lock for write ops. Also, sa1111_wake must be called with
+	 * released spinlock!
+	 */
+	spin_lock_irqsave(&sachip->lock, flags);
+
+	writel_relaxed(0, sachip->base + SA1111_INTC + SA1111_INTEN0);
+	writel_relaxed(0, sachip->base + SA1111_INTC + SA1111_INTEN1);
+
+	base = sachip->base;
+	writel_relaxed(save->skcr,     base + SA1111_SKCR);
+	writel_relaxed(save->skpcr,    base + SA1111_SKPCR);
+	writel_relaxed(save->skcdr,    base + SA1111_SKCDR);
+	writel_relaxed(save->skaud,    base + SA1111_SKAUD);
+	writel_relaxed(save->skpwm0,   base + SA1111_SKPWM0);
+	writel_relaxed(save->skpwm1,   base + SA1111_SKPWM1);
+
+	base = sachip->base + SA1111_INTC;
+	writel_relaxed(save->intpol0,  base + SA1111_INTPOL0);
+	writel_relaxed(save->intpol1,  base + SA1111_INTPOL1);
+	writel_relaxed(save->inten0,   base + SA1111_INTEN0);
+	writel_relaxed(save->inten1,   base + SA1111_INTEN1);
+	writel_relaxed(save->wakepol0, base + SA1111_WAKEPOL0);
+	writel_relaxed(save->wakepol1, base + SA1111_WAKEPOL1);
+	writel_relaxed(save->wakeen0,  base + SA1111_WAKEEN0);
+	writel_relaxed(save->wakeen1,  base + SA1111_WAKEEN1);
+
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+	sachip->saved_state = NULL;
+	kfree(save);
+
+	return 0;
+}
+
+#else
+#define sa1111_suspend_noirq NULL
+#define sa1111_resume_noirq  NULL
+#endif
+
+/**
+ *	sa1111_probe - probe for a single SA1111 chip.
+ *	@pdev: platform device.
+ *
+ *	Probe for a SA1111 chip.  This must be called
+ *	before any other SA1111-specific code.
+ *
+ *	Returns:
+ *	* %-ENODEV	- device not found.
+ *	* %-ENOMEM	- memory allocation failure.
+ *	* %-EBUSY	- physical address already marked in-use.
+ *	* %-EINVAL	- no platform data passed
+ *	* %0		- successful.
+ */
+static int sa1111_probe(struct platform_device *pdev)
+{
+	struct resource *mem;
+	int irq;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -EINVAL;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	return __sa1111_probe(&pdev->dev, mem, irq);
+}
+
+static void sa1111_remove(struct platform_device *pdev)
+{
+	struct sa1111 *sachip = platform_get_drvdata(pdev);
+
+	if (sachip) {
+#ifdef CONFIG_PM
+		kfree(sachip->saved_state);
+		sachip->saved_state = NULL;
+#endif
+		__sa1111_remove(sachip);
+		platform_set_drvdata(pdev, NULL);
+	}
+}
+
+static struct dev_pm_ops sa1111_pm_ops = {
+	.suspend_noirq = sa1111_suspend_noirq,
+	.resume_noirq = sa1111_resume_noirq,
+};
+
+/*
+ *	Not sure if this should be on the system bus or not yet.
+ *	We really want some way to register a system device at
+ *	the per-machine level, and then have this driver pick
+ *	up the registered devices.
+ *
+ *	We also need to handle the SDRAM configuration for
+ *	PXA250/SA1110 machine classes.
+ */
+static struct platform_driver sa1111_device_driver = {
+	.probe		= sa1111_probe,
+	.remove		= sa1111_remove,
+	.driver		= {
+		.name	= "sa1111",
+		.pm	= &sa1111_pm_ops,
+	},
+};
+
+/*
+ *	Get the parent device driver (us) structure
+ *	from a child function device
+ */
+static inline struct sa1111 *sa1111_chip_driver(struct sa1111_dev *sadev)
+{
+	return (struct sa1111 *)dev_get_drvdata(sadev->dev.parent);
+}
+
+/*
+ * The bits in the opdiv field are non-linear.
+ */
+static unsigned char opdiv_table[] = { 1, 4, 2, 8 };
+
+static unsigned int __sa1111_pll_clock(struct sa1111 *sachip)
+{
+	unsigned int skcdr, fbdiv, ipdiv, opdiv;
+
+	skcdr = readl_relaxed(sachip->base + SA1111_SKCDR);
+
+	fbdiv = (skcdr & 0x007f) + 2;
+	ipdiv = ((skcdr & 0x0f80) >> 7) + 2;
+	opdiv = opdiv_table[(skcdr & 0x3000) >> 12];
+
+	return 3686400 * fbdiv / (ipdiv * opdiv);
+}
+
+/**
+ *	sa1111_pll_clock - return the current PLL clock frequency.
+ *	@sadev: SA1111 function block
+ *
+ *	BUG: we should look at SKCR.  We also blindly believe that
+ *	the chip is being fed with the 3.6864MHz clock.
+ *
+ *	Returns the PLL clock in Hz.
+ */
+unsigned int sa1111_pll_clock(struct sa1111_dev *sadev)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+
+	return __sa1111_pll_clock(sachip);
+}
+EXPORT_SYMBOL(sa1111_pll_clock);
+
+/**
+ *	sa1111_select_audio_mode - select I2S or AC link mode
+ *	@sadev: SA1111 function block
+ *	@mode: One of %SA1111_AUDIO_ACLINK or %SA1111_AUDIO_I2S
+ *
+ *	Frob the SKCR to select AC Link mode or I2S mode for
+ *	the audio block.
+ */
+void sa1111_select_audio_mode(struct sa1111_dev *sadev, int mode)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+	unsigned long flags;
+	unsigned int val;
+
+	spin_lock_irqsave(&sachip->lock, flags);
+
+	val = readl_relaxed(sachip->base + SA1111_SKCR);
+	if (mode == SA1111_AUDIO_I2S) {
+		val &= ~SKCR_SELAC;
+	} else {
+		val |= SKCR_SELAC;
+	}
+	writel_relaxed(val, sachip->base + SA1111_SKCR);
+
+	spin_unlock_irqrestore(&sachip->lock, flags);
+}
+EXPORT_SYMBOL(sa1111_select_audio_mode);
+
+/**
+ *	sa1111_set_audio_rate - set the audio sample rate
+ *	@sadev: SA1111 SAC function block
+ *	@rate: sample rate to select
+ */
+int sa1111_set_audio_rate(struct sa1111_dev *sadev, int rate)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+	unsigned int div;
+
+	if (sadev->devid != SA1111_DEVID_SAC)
+		return -EINVAL;
+
+	div = (__sa1111_pll_clock(sachip) / 256 + rate / 2) / rate;
+	if (div == 0)
+		div = 1;
+	if (div > 128)
+		div = 128;
+
+	writel_relaxed(div - 1, sachip->base + SA1111_SKAUD);
+
+	return 0;
+}
+EXPORT_SYMBOL(sa1111_set_audio_rate);
+
+/**
+ *	sa1111_get_audio_rate - get the audio sample rate
+ *	@sadev: SA1111 SAC function block device
+ */
+int sa1111_get_audio_rate(struct sa1111_dev *sadev)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+	unsigned long div;
+
+	if (sadev->devid != SA1111_DEVID_SAC)
+		return -EINVAL;
+
+	div = readl_relaxed(sachip->base + SA1111_SKAUD) + 1;
+
+	return __sa1111_pll_clock(sachip) / (256 * div);
+}
+EXPORT_SYMBOL(sa1111_get_audio_rate);
+
+/*
+ * Individual device operations.
+ */
+
+/**
+ *	sa1111_enable_device - enable an on-chip SA1111 function block
+ *	@sadev: SA1111 function block device to enable
+ */
+int sa1111_enable_device(struct sa1111_dev *sadev)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+	unsigned long flags;
+	unsigned int val;
+	int ret = 0;
+
+	if (sachip->pdata && sachip->pdata->enable)
+		ret = sachip->pdata->enable(sachip->pdata->data, sadev->devid);
+
+	if (ret == 0) {
+		spin_lock_irqsave(&sachip->lock, flags);
+		val = readl_relaxed(sachip->base + SA1111_SKPCR);
+		writel_relaxed(val | sadev->skpcr_mask, sachip->base + SA1111_SKPCR);
+		spin_unlock_irqrestore(&sachip->lock, flags);
+	}
+	return ret;
+}
+EXPORT_SYMBOL(sa1111_enable_device);
+
+/**
+ *	sa1111_disable_device - disable an on-chip SA1111 function block
+ *	@sadev: SA1111 function block device to disable
+ */
+void sa1111_disable_device(struct sa1111_dev *sadev)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+	unsigned long flags;
+	unsigned int val;
+
+	spin_lock_irqsave(&sachip->lock, flags);
+	val = readl_relaxed(sachip->base + SA1111_SKPCR);
+	writel_relaxed(val & ~sadev->skpcr_mask, sachip->base + SA1111_SKPCR);
+	spin_unlock_irqrestore(&sachip->lock, flags);
+
+	if (sachip->pdata && sachip->pdata->disable)
+		sachip->pdata->disable(sachip->pdata->data, sadev->devid);
+}
+EXPORT_SYMBOL(sa1111_disable_device);
+
+int sa1111_get_irq(struct sa1111_dev *sadev, unsigned num)
+{
+	struct sa1111 *sachip = sa1111_chip_driver(sadev);
+	if (num >= ARRAY_SIZE(sadev->hwirq))
+		return -EINVAL;
+	return sa1111_map_irq(sachip, sadev->hwirq[num]);
+}
+EXPORT_SYMBOL_GPL(sa1111_get_irq);
+
+/*
+ *	SA1111 "Register Access Bus."
+ *
+ *	We model this as a regular bus type, and hang devices directly
+ *	off this.
+ */
+static int sa1111_match(struct device *_dev, const struct device_driver *_drv)
+{
+	struct sa1111_dev *dev = to_sa1111_device(_dev);
+	const struct sa1111_driver *drv = SA1111_DRV(_drv);
+
+	return !!(dev->devid & drv->devid);
+}
+
+static int sa1111_bus_probe(struct device *dev)
+{
+	struct sa1111_dev *sadev = to_sa1111_device(dev);
+	struct sa1111_driver *drv = SA1111_DRV(dev->driver);
+	int ret = -ENODEV;
+
+	if (drv->probe)
+		ret = drv->probe(sadev);
+	return ret;
+}
+
+static void sa1111_bus_remove(struct device *dev)
+{
+	struct sa1111_dev *sadev = to_sa1111_device(dev);
+	struct sa1111_driver *drv = SA1111_DRV(dev->driver);
+
+	if (drv->remove)
+		drv->remove(sadev);
+}
+
+const struct bus_type sa1111_bus_type = {
+	.name		= "sa1111-rab",
+	.match		= sa1111_match,
+	.probe		= sa1111_bus_probe,
+	.remove		= sa1111_bus_remove,
+};
+EXPORT_SYMBOL(sa1111_bus_type);
+
+int sa1111_driver_register(struct sa1111_driver *driver)
+{
+	driver->drv.bus = &sa1111_bus_type;
+	return driver_register(&driver->drv);
+}
+EXPORT_SYMBOL(sa1111_driver_register);
+
+void sa1111_driver_unregister(struct sa1111_driver *driver)
+{
+	driver_unregister(&driver->drv);
+}
+EXPORT_SYMBOL(sa1111_driver_unregister);
+
+static int __init sa1111_init(void)
+{
+	int ret = bus_register(&sa1111_bus_type);
+	if (ret == 0)
+		platform_driver_register(&sa1111_device_driver);
+	return ret;
+}
+
+static void __exit sa1111_exit(void)
+{
+	platform_driver_unregister(&sa1111_device_driver);
+	bus_unregister(&sa1111_bus_type);
+}
+
+subsys_initcall(sa1111_init);
+module_exit(sa1111_exit);
+
+MODULE_DESCRIPTION("Intel Corporation SA1111 core driver");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/common/scoop.c b/arch/arm/common/scoop.c
new file mode 100644
index 000000000..904749947
--- /dev/null
+++ b/arch/arm/common/scoop.c
@@ -0,0 +1,269 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Support code for the SCOOP interface found on various Sharp PDAs
+ *
+ * Copyright (c) 2004 Richard Purdie
+ *
+ *	Based on code written by Sharp/Lineo for 2.4 kernels
+ */
+
+#include <linux/device.h>
+#include <linux/gpio/driver.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/export.h>
+#include <linux/io.h>
+#include <asm/hardware/scoop.h>
+
+/* PCMCIA to Scoop linkage
+
+   There is no easy way to link multiple scoop devices into one
+   single entity for the pxa2xx_pcmcia device so this structure
+   is used which is setup by the platform code.
+
+   This file is never modular so this symbol is always
+   accessile to the board support files.
+*/
+struct scoop_pcmcia_config *platform_scoop_config;
+EXPORT_SYMBOL(platform_scoop_config);
+
+struct  scoop_dev {
+	void __iomem *base;
+	struct gpio_chip gpio;
+	spinlock_t scoop_lock;
+	unsigned short suspend_clr;
+	unsigned short suspend_set;
+	u32 scoop_gpwr;
+};
+
+void reset_scoop(struct device *dev)
+{
+	struct scoop_dev *sdev = dev_get_drvdata(dev);
+
+	iowrite16(0x0100, sdev->base + SCOOP_MCR);  /* 00 */
+	iowrite16(0x0000, sdev->base + SCOOP_CDR);  /* 04 */
+	iowrite16(0x0000, sdev->base + SCOOP_CCR);  /* 10 */
+	iowrite16(0x0000, sdev->base + SCOOP_IMR);  /* 18 */
+	iowrite16(0x00FF, sdev->base + SCOOP_IRM);  /* 14 */
+	iowrite16(0x0000, sdev->base + SCOOP_ISR);  /* 1C */
+	iowrite16(0x0000, sdev->base + SCOOP_IRM);
+}
+
+static void __scoop_gpio_set(struct scoop_dev *sdev,
+			unsigned offset, int value)
+{
+	unsigned short gpwr;
+
+	gpwr = ioread16(sdev->base + SCOOP_GPWR);
+	if (value)
+		gpwr |= 1 << (offset + 1);
+	else
+		gpwr &= ~(1 << (offset + 1));
+	iowrite16(gpwr, sdev->base + SCOOP_GPWR);
+}
+
+static int scoop_gpio_set(struct gpio_chip *chip, unsigned int offset,
+			  int value)
+{
+	struct scoop_dev *sdev = gpiochip_get_data(chip);
+	unsigned long flags;
+
+	spin_lock_irqsave(&sdev->scoop_lock, flags);
+
+	__scoop_gpio_set(sdev, offset, value);
+
+	spin_unlock_irqrestore(&sdev->scoop_lock, flags);
+
+	return 0;
+}
+
+static int scoop_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct scoop_dev *sdev = gpiochip_get_data(chip);
+
+	/* XXX: I'm unsure, but it seems so */
+	return !!(ioread16(sdev->base + SCOOP_GPRR) & (1 << (offset + 1)));
+}
+
+static int scoop_gpio_direction_input(struct gpio_chip *chip,
+			unsigned offset)
+{
+	struct scoop_dev *sdev = gpiochip_get_data(chip);
+	unsigned long flags;
+	unsigned short gpcr;
+
+	spin_lock_irqsave(&sdev->scoop_lock, flags);
+
+	gpcr = ioread16(sdev->base + SCOOP_GPCR);
+	gpcr &= ~(1 << (offset + 1));
+	iowrite16(gpcr, sdev->base + SCOOP_GPCR);
+
+	spin_unlock_irqrestore(&sdev->scoop_lock, flags);
+
+	return 0;
+}
+
+static int scoop_gpio_direction_output(struct gpio_chip *chip,
+			unsigned offset, int value)
+{
+	struct scoop_dev *sdev = gpiochip_get_data(chip);
+	unsigned long flags;
+	unsigned short gpcr;
+
+	spin_lock_irqsave(&sdev->scoop_lock, flags);
+
+	__scoop_gpio_set(sdev, offset, value);
+
+	gpcr = ioread16(sdev->base + SCOOP_GPCR);
+	gpcr |= 1 << (offset + 1);
+	iowrite16(gpcr, sdev->base + SCOOP_GPCR);
+
+	spin_unlock_irqrestore(&sdev->scoop_lock, flags);
+
+	return 0;
+}
+
+unsigned short read_scoop_reg(struct device *dev, unsigned short reg)
+{
+	struct scoop_dev *sdev = dev_get_drvdata(dev);
+	return ioread16(sdev->base + reg);
+}
+
+void write_scoop_reg(struct device *dev, unsigned short reg, unsigned short data)
+{
+	struct scoop_dev *sdev = dev_get_drvdata(dev);
+	iowrite16(data, sdev->base + reg);
+}
+
+EXPORT_SYMBOL(reset_scoop);
+EXPORT_SYMBOL(read_scoop_reg);
+EXPORT_SYMBOL(write_scoop_reg);
+
+#ifdef CONFIG_PM
+static void check_scoop_reg(struct scoop_dev *sdev)
+{
+	unsigned short mcr;
+
+	mcr = ioread16(sdev->base + SCOOP_MCR);
+	if ((mcr & 0x100) == 0)
+		iowrite16(0x0101, sdev->base + SCOOP_MCR);
+}
+
+static int scoop_suspend(struct platform_device *dev, pm_message_t state)
+{
+	struct scoop_dev *sdev = platform_get_drvdata(dev);
+
+	check_scoop_reg(sdev);
+	sdev->scoop_gpwr = ioread16(sdev->base + SCOOP_GPWR);
+	iowrite16((sdev->scoop_gpwr & ~sdev->suspend_clr) | sdev->suspend_set, sdev->base + SCOOP_GPWR);
+
+	return 0;
+}
+
+static int scoop_resume(struct platform_device *dev)
+{
+	struct scoop_dev *sdev = platform_get_drvdata(dev);
+
+	check_scoop_reg(sdev);
+	iowrite16(sdev->scoop_gpwr, sdev->base + SCOOP_GPWR);
+
+	return 0;
+}
+#else
+#define scoop_suspend	NULL
+#define scoop_resume	NULL
+#endif
+
+static int scoop_probe(struct platform_device *pdev)
+{
+	struct scoop_dev *devptr;
+	struct scoop_config *inf;
+	struct resource *mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	int ret;
+
+	if (!mem)
+		return -EINVAL;
+
+	devptr = kzalloc_obj(struct scoop_dev);
+	if (!devptr)
+		return -ENOMEM;
+
+	spin_lock_init(&devptr->scoop_lock);
+
+	inf = pdev->dev.platform_data;
+	devptr->base = ioremap(mem->start, resource_size(mem));
+
+	if (!devptr->base) {
+		ret = -ENOMEM;
+		goto err_ioremap;
+	}
+
+	platform_set_drvdata(pdev, devptr);
+
+	printk("Sharp Scoop Device found at 0x%08x -> 0x%8p\n",(unsigned int)mem->start, devptr->base);
+
+	iowrite16(0x0140, devptr->base + SCOOP_MCR);
+	reset_scoop(&pdev->dev);
+	iowrite16(0x0000, devptr->base + SCOOP_CPR);
+	iowrite16(inf->io_dir & 0xffff, devptr->base + SCOOP_GPCR);
+	iowrite16(inf->io_out & 0xffff, devptr->base + SCOOP_GPWR);
+
+	devptr->suspend_clr = inf->suspend_clr;
+	devptr->suspend_set = inf->suspend_set;
+
+	devptr->gpio.base = -1;
+
+	if (inf->gpio_base != 0) {
+		devptr->gpio.label = dev_name(&pdev->dev);
+		devptr->gpio.base = inf->gpio_base;
+		devptr->gpio.ngpio = 12; /* PA11 = 0, PA12 = 1, etc. up to PA22 = 11 */
+		devptr->gpio.set = scoop_gpio_set;
+		devptr->gpio.get = scoop_gpio_get;
+		devptr->gpio.direction_input = scoop_gpio_direction_input;
+		devptr->gpio.direction_output = scoop_gpio_direction_output;
+
+		ret = gpiochip_add_data(&devptr->gpio, devptr);
+		if (ret)
+			goto err_gpio;
+	}
+
+	return 0;
+
+err_gpio:
+	platform_set_drvdata(pdev, NULL);
+err_ioremap:
+	iounmap(devptr->base);
+	kfree(devptr);
+
+	return ret;
+}
+
+static void scoop_remove(struct platform_device *pdev)
+{
+	struct scoop_dev *sdev = platform_get_drvdata(pdev);
+
+	if (sdev->gpio.base != -1)
+		gpiochip_remove(&sdev->gpio);
+
+	platform_set_drvdata(pdev, NULL);
+	iounmap(sdev->base);
+	kfree(sdev);
+}
+
+static struct platform_driver scoop_driver = {
+	.probe		= scoop_probe,
+	.remove		= scoop_remove,
+	.suspend	= scoop_suspend,
+	.resume		= scoop_resume,
+	.driver		= {
+		.name	= "sharp-scoop",
+	},
+};
+
+static int __init scoop_init(void)
+{
+	return platform_driver_register(&scoop_driver);
+}
+
+subsys_initcall(scoop_init);
diff --git a/arch/arm/common/secure_cntvoff.S b/arch/arm/common/secure_cntvoff.S
new file mode 100644
index 000000000..53fc7bdb6
--- /dev/null
+++ b/arch/arm/common/secure_cntvoff.S
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ *
+ * Initialization of CNTVOFF register from secure mode
+ *
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+ENTRY(secure_cntvoff_init)
+	.arch	armv7-a
+	/*
+	 * CNTVOFF has to be initialized either from non-secure Hypervisor
+	 * mode or secure Monitor mode with SCR.NS==1. If TrustZone is enabled
+	 * then it should be handled by the secure code. The CPU must implement
+	 * the virtualization extensions.
+	 */
+	cps	#MON_MODE
+	mrc	p15, 0, r1, c1, c1, 0		/* Get Secure Config */
+	orr	r0, r1, #1
+	mcr	p15, 0, r0, c1, c1, 0		/* Set Non Secure bit */
+	isb
+	mov	r0, #0
+	mcrr	p15, 4, r0, r0, c14		/* CNTVOFF = 0 */
+	isb
+	mcr	p15, 0, r1, c1, c1, 0		/* Set Secure bit */
+	isb
+	cps	#SVC_MODE
+	ret	lr
+ENDPROC(secure_cntvoff_init)
diff --git a/arch/arm/common/sharpsl_param.c b/arch/arm/common/sharpsl_param.c
new file mode 100644
index 000000000..1ca26c063
--- /dev/null
+++ b/arch/arm/common/sharpsl_param.c
@@ -0,0 +1,63 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Hardware parameter area specific to Sharp SL series devices
+ *
+ * Copyright (c) 2005 Richard Purdie
+ *
+ * Based on Sharp's 2.4 kernel patches
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <asm/mach/sharpsl_param.h>
+#include <asm/page.h>
+
+/*
+ * Certain hardware parameters determined at the time of device manufacture,
+ * typically including LCD parameters are loaded by the bootloader at the
+ * address PARAM_BASE. As the kernel will overwrite them, we need to store
+ * them early in the boot process, then pass them to the appropriate drivers.
+ * Not all devices use all parameters but the format is common to all.
+ */
+#ifdef CONFIG_ARCH_SA1100
+#define PARAM_BASE	0xe8ffc000
+#define param_start(x)	(void *)(x)
+#else
+#define PARAM_BASE	0xa0000a00
+#define param_start(x)	__va(x)
+#endif
+#define MAGIC_CHG(a,b,c,d) ( ( d << 24 ) | ( c << 16 )  | ( b << 8 ) | a )
+
+#define COMADJ_MAGIC	MAGIC_CHG('C','M','A','D')
+#define UUID_MAGIC	MAGIC_CHG('U','U','I','D')
+#define TOUCH_MAGIC	MAGIC_CHG('T','U','C','H')
+#define AD_MAGIC	MAGIC_CHG('B','V','A','D')
+#define PHAD_MAGIC	MAGIC_CHG('P','H','A','D')
+
+struct sharpsl_param_info sharpsl_param;
+EXPORT_SYMBOL(sharpsl_param);
+
+void sharpsl_save_param(void)
+{
+	struct sharpsl_param_info *params = param_start(PARAM_BASE);
+
+	memcpy(&sharpsl_param, params, sizeof(*params));
+
+	if (sharpsl_param.comadj_keyword != COMADJ_MAGIC)
+		sharpsl_param.comadj=-1;
+
+	if (sharpsl_param.phad_keyword != PHAD_MAGIC)
+		sharpsl_param.phadadj=-1;
+
+	if (sharpsl_param.uuid_keyword != UUID_MAGIC)
+		sharpsl_param.uuid[0]=-1;
+
+	if (sharpsl_param.touch_keyword != TOUCH_MAGIC)
+		sharpsl_param.touch_xp=-1;
+
+	if (sharpsl_param.adadj_keyword != AD_MAGIC)
+		sharpsl_param.adadj=-1;
+}
+
+
diff --git a/arch/arm/common/vlock.S b/arch/arm/common/vlock.S
new file mode 100644
index 000000000..c5eaed5a7
--- /dev/null
+++ b/arch/arm/common/vlock.S
@@ -0,0 +1,101 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vlock.S - simple voting lock implementation for ARM
+ *
+ * Created by:	Dave Martin, 2012-08-16
+ * Copyright:	(C) 2012-2013  Linaro Limited
+ *
+ * This algorithm is described in more detail in
+ * Documentation/arch/arm/vlocks.rst.
+ */
+
+#include <linux/linkage.h>
+#include "vlock.h"
+
+.arch armv7-a
+
+/* Select different code if voting flags  can fit in a single word. */
+#if VLOCK_VOTING_SIZE > 4
+#define FEW(x...)
+#define MANY(x...) x
+#else
+#define FEW(x...) x
+#define MANY(x...)
+#endif
+
+@ voting lock for first-man coordination
+
+.macro voting_begin rbase:req, rcpu:req, rscratch:req
+	mov	\rscratch, #1
+	strb	\rscratch, [\rbase, \rcpu]
+	dmb
+.endm
+
+.macro voting_end rbase:req, rcpu:req, rscratch:req
+	dmb
+	mov	\rscratch, #0
+	strb	\rscratch, [\rbase, \rcpu]
+	dsb	st
+	sev
+.endm
+
+/*
+ * The vlock structure must reside in Strongly-Ordered or Device memory.
+ * This implementation deliberately eliminates most of the barriers which
+ * would be required for other memory types, and assumes that independent
+ * writes to neighbouring locations within a cacheline do not interfere
+ * with one another.
+ */
+
+@ r0: lock structure base
+@ r1: CPU ID (0-based index within cluster)
+ENTRY(vlock_trylock)
+	add	r1, r1, #VLOCK_VOTING_OFFSET
+
+	voting_begin	r0, r1, r2
+
+	ldrb	r2, [r0, #VLOCK_OWNER_OFFSET]	@ check whether lock is held
+	cmp	r2, #VLOCK_OWNER_NONE
+	bne	trylock_fail			@ fail if so
+
+	@ Control dependency implies strb not observable before previous ldrb.
+
+	strb	r1, [r0, #VLOCK_OWNER_OFFSET]	@ submit my vote
+
+	voting_end	r0, r1, r2		@ implies DMB
+
+	@ Wait for the current round of voting to finish:
+
+ MANY(	mov	r3, #VLOCK_VOTING_OFFSET			)
+0:
+ MANY(	ldr	r2, [r0, r3]					)
+ FEW(	ldr	r2, [r0, #VLOCK_VOTING_OFFSET]			)
+	cmp	r2, #0
+	wfene
+	bne	0b
+ MANY(	add	r3, r3, #4					)
+ MANY(	cmp	r3, #VLOCK_VOTING_OFFSET + VLOCK_VOTING_SIZE	)
+ MANY(	bne	0b						)
+
+	@ Check who won:
+
+	dmb
+	ldrb	r2, [r0, #VLOCK_OWNER_OFFSET]
+	eor	r0, r1, r2			@ zero if I won, else nonzero
+	bx	lr
+
+trylock_fail:
+	voting_end	r0, r1, r2
+	mov	r0, #1				@ nonzero indicates that I lost
+	bx	lr
+ENDPROC(vlock_trylock)
+
+@ r0: lock structure base
+ENTRY(vlock_unlock)
+	dmb
+	mov	r1, #VLOCK_OWNER_NONE
+	strb	r1, [r0, #VLOCK_OWNER_OFFSET]
+	dsb	st
+	sev
+	bx	lr
+ENDPROC(vlock_unlock)
diff --git a/arch/arm/common/vlock.h b/arch/arm/common/vlock.h
new file mode 100644
index 000000000..422429781
--- /dev/null
+++ b/arch/arm/common/vlock.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vlock.h - simple voting lock implementation
+ *
+ * Created by:	Dave Martin, 2012-08-16
+ * Copyright:	(C) 2012-2013  Linaro Limited
+ */
+
+#ifndef __VLOCK_H
+#define __VLOCK_H
+
+#include <asm/mcpm.h>
+
+/* Offsets and sizes are rounded to a word (4 bytes) */
+#define VLOCK_OWNER_OFFSET	0
+#define VLOCK_VOTING_OFFSET	4
+#define VLOCK_VOTING_SIZE	((MAX_CPUS_PER_CLUSTER + 3) / 4 * 4)
+#define VLOCK_SIZE		(VLOCK_VOTING_OFFSET + VLOCK_VOTING_SIZE)
+#define VLOCK_OWNER_NONE	0
+
+#endif /* ! __VLOCK_H */