arm64: add parking protocol support

This is a first-cut effort at parking protocol support. It is
very much a work in progress (as is the spec it is based on).
This code deviates from the current spec in a number of ways
to work around current firmware issues and issues with kernels
using 64K page sizes.

caveat utilitor

Signed-off-by: Mark Salter <msalter@redhat.com>

10 files changed, 147 insertions, 2 deletions

diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 6e5939f1c3e6..428211537cd3 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -488,6 +488,10 @@ config SMP
 
 	  If you don't know what to do here, say N.
 
+config ARM_PARKING_PROTOCOL
+	def_bool y
+	depends on SMP && ACPI
+
 config SCHED_MC
 	bool "Multi-core scheduler support"
 	depends on SMP
diff --git a/arch/arm64/include/asm/acpi.h b/arch/arm64/include/asm/acpi.h
index c650c91b63ad..da6fe63afe2f 100644
--- a/arch/arm64/include/asm/acpi.h
+++ b/arch/arm64/include/asm/acpi.h
@@ -83,6 +83,7 @@ static inline bool acpi_has_cpu_in_madt(void)
 
 static inline void arch_fix_phys_package_id(int num, u32 slot) { }
 void __init acpi_init_cpus(void);
+extern phys_addr_t arm64_cpu_parking_addr[];
 
 #else
 static inline void acpi_init_cpus(void) { }
@@ -90,6 +91,6 @@ static inline void acpi_init_cpus(void) { }
 
 static inline const char *acpi_get_enable_method(int cpu)
 {
-	return acpi_psci_present() ? "psci" : NULL;
+	return acpi_psci_present() ? "psci" : "parking-protocol";
 }
 #endif /*_ASM_ACPI_H*/
diff --git a/arch/arm64/include/asm/smp.h b/arch/arm64/include/asm/smp.h
index db02be81b90a..f8b9c7a00b44 100644
--- a/arch/arm64/include/asm/smp.h
+++ b/arch/arm64/include/asm/smp.h
@@ -52,6 +52,11 @@ extern void set_smp_cross_call(void (*)(const struct cpumask *, unsigned int));
 extern void (*__smp_cross_call)(const struct cpumask *, unsigned int);
 
 /*
+ * Provide a function to signal a parked secondary CPU.
+ */
+extern void set_smp_boot_wakeup_call(void (*)(int cpu));
+
+/*
  * Called from the secondary holding pen, this is the secondary CPU entry point.
  */
 asmlinkage void secondary_start_kernel(void);
diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 426d0763c81b..c945a7675195 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -35,7 +35,7 @@ arm64-obj-$(CONFIG_KGDB)		+= kgdb.o
 arm64-obj-$(CONFIG_EFI)			+= efi.o efi-stub.o efi-entry.o
 arm64-obj-$(CONFIG_PCI)			+= pci.o
 arm64-obj-$(CONFIG_ARMV8_DEPRECATED)	+= armv8_deprecated.o
-arm64-obj-$(CONFIG_ACPI)		+= acpi.o
+arm64-obj-$(CONFIG_ACPI)		+= acpi.o smp_parking_protocol.o
 
 obj-y					+= $(arm64-obj-y) vdso/
 obj-m					+= $(arm64-obj-m)
diff --git a/arch/arm64/kernel/cpu_ops.c b/arch/arm64/kernel/cpu_ops.c
index 5ea337dd2f15..8886f734bdf5 100644
--- a/arch/arm64/kernel/cpu_ops.c
+++ b/arch/arm64/kernel/cpu_ops.c
@@ -25,6 +25,7 @@
 #include <asm/smp_plat.h>
 
 extern const struct cpu_operations smp_spin_table_ops;
+extern const struct cpu_operations smp_parking_protocol_ops;
 extern const struct cpu_operations cpu_psci_ops;
 
 const struct cpu_operations *cpu_ops[NR_CPUS];
@@ -32,6 +33,9 @@ const struct cpu_operations *cpu_ops[NR_CPUS];
 static const struct cpu_operations *supported_cpu_ops[] __initconst = {
 #ifdef CONFIG_SMP
 	&smp_spin_table_ops,
+#ifdef CONFIG_ARM_PARKING_PROTOCOL
+	&smp_parking_protocol_ops,
+#endif
 #endif
 	&cpu_psci_ops,
 	NULL,
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index 50fb4696654e..45d56f33fb8d 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -428,6 +428,8 @@ acpi_map_gic_cpu_interface(struct acpi_madt_generic_interrupt *processor)
 	/* map the logical cpu id to cpu MPIDR */
 	cpu_logical_map(cpu_count) = hwid;
 
+	arm64_cpu_parking_addr[cpu_count] = processor->parked_address;
+
 	cpu_count++;
 }
 
diff --git a/arch/arm64/kernel/smp_parking_protocol.c b/arch/arm64/kernel/smp_parking_protocol.c
new file mode 100644
index 000000000000..829ed3668190
--- /dev/null
+++ b/arch/arm64/kernel/smp_parking_protocol.c
@@ -0,0 +1,107 @@
+/*
+ * Parking Protocol SMP initialisation
+ *
+ * Based largely on spin-table method.
+ *
+ * Copyright (C) 2013 ARM Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/smp.h>
+#include <linux/types.h>
+#include <linux/acpi.h>
+
+#include <asm/cacheflush.h>
+#include <asm/cpu_ops.h>
+#include <asm/cputype.h>
+#include <asm/smp_plat.h>
+#include <asm/system_misc.h>
+
+phys_addr_t arm64_cpu_parking_addr[NR_CPUS];
+
+static void (*__smp_boot_wakeup)(int cpu);
+
+void set_smp_boot_wakeup_call(void (*fn)(int cpu))
+{
+	__smp_boot_wakeup = fn;
+}
+
+static int smp_parking_protocol_cpu_init(unsigned int cpu)
+{
+	/*
+	 * Determine the mailbox address.
+	 */
+	if (arm64_cpu_parking_addr[cpu]) {
+		pr_info("%s: ACPI parked addr=%llx\n",
+			__func__, arm64_cpu_parking_addr[cpu]);
+		return 0;
+	}
+
+	pr_err("CPU %d: missing or invalid parking protocol mailbox\n", cpu);
+
+	return -1;
+}
+
+static int smp_parking_protocol_cpu_prepare(unsigned int cpu)
+{
+	return 0;
+}
+
+struct parking_protocol_mailbox {
+	__le32 cpu_id;
+	__le32 reserved;
+	__le64 entry_point;
+};
+
+static int smp_parking_protocol_cpu_boot(unsigned int cpu)
+{
+	struct parking_protocol_mailbox __iomem *mailbox;
+
+	if (!arm64_cpu_parking_addr[cpu] || !__smp_boot_wakeup)
+		return -ENODEV;
+
+	/*
+	 * The mailbox may or may not be inside the linear mapping.
+	 * As ioremap_cache will either give us a new mapping or reuse the
+	 * existing linear mapping, we can use it to cover both cases. In
+	 * either case the memory will be MT_NORMAL.
+	 */
+	mailbox = ioremap_cache(arm64_cpu_parking_addr[cpu], sizeof(*mailbox));
+	if (!mailbox)
+		return -ENOMEM;
+
+	/*
+	 * We write the entry point and cpu id as LE regardless of the
+	 * native endianess of the kernel. Therefore, any boot-loaders
+	 * that read this address need to convert this address to the
+	 * Boot-Loader's endianess before jumping.
+	 */
+	writeq(__pa(secondary_entry), &mailbox->entry_point);
+	writel(cpu, &mailbox->cpu_id);
+	__flush_dcache_area(mailbox, sizeof(*mailbox));
+	__smp_boot_wakeup(cpu);
+
+	iounmap(mailbox);
+
+	return 0;
+}
+
+const struct cpu_operations smp_parking_protocol_ops = {
+	.name		= "parking-protocol",
+	.cpu_init	= smp_parking_protocol_cpu_init,
+	.cpu_prepare	= smp_parking_protocol_cpu_prepare,
+	.cpu_boot	= smp_parking_protocol_cpu_boot,
+};
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index c52f7ba205b4..8f11b52ce6f7 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -579,9 +579,19 @@ static void gic_raise_softirq(const struct cpumask *mask, unsigned int irq)
 	isb();
 }
 
+#ifdef CONFIG_ARM_PARKING_PROTOCOL
+static void gic_wakeup_parked_cpu(int cpu)
+{
+	gic_raise_softirq(cpumask_of(cpu), 0);
+}
+#endif
+
 static void gic_smp_init(void)
 {
 	set_smp_cross_call(gic_raise_softirq);
+#ifdef CONFIG_ARM_PARKING_PROTOCOL
+	set_smp_boot_wakeup_call(gic_wakeup_parked_cpu);
+#endif
 	register_cpu_notifier(&gic_cpu_notifier);
 }
 
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 4dd88264dff5..37e932c679e1 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -644,6 +644,13 @@ static void gic_raise_softirq(const struct cpumask *mask, unsigned int irq)
 
 	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
 }
+
+#ifdef CONFIG_ARM_PARKING_PROTOCOL
+static void gic_wakeup_parked_cpu(int cpu)
+{
+	gic_raise_softirq(cpumask_of(cpu), GIC_DIST_SOFTINT_NSATT);
+}
+#endif
 #endif
 
 #ifdef CONFIG_BL_SWITCHER
@@ -984,6 +991,9 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 #ifdef CONFIG_SMP
 		set_smp_cross_call(gic_raise_softirq);
 		register_cpu_notifier(&gic_cpu_notifier);
+#ifdef CONFIG_ARM_PARKING_PROTOCOL
+		set_smp_boot_wakeup_call(gic_wakeup_parked_cpu);
+#endif
 #endif
 		set_handle_irq(gic_handle_irq);
 	}
diff --git a/include/linux/irqchip/arm-gic.h b/include/linux/irqchip/arm-gic.h
index 9de976b4f9a7..b933b7226f1c 100644
--- a/include/linux/irqchip/arm-gic.h
+++ b/include/linux/irqchip/arm-gic.h
@@ -55,6 +55,8 @@
 					(GICD_INT_DEF_PRI << 8) |\
 					GICD_INT_DEF_PRI)
 
+#define GIC_DIST_SOFTINT_NSATT		0x8000
+
 #define GICH_HCR			0x0
 #define GICH_VTR			0x4
 #define GICH_VMCR			0x8