From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Martin Blumenstingl <martin.blumenstingl@googlemail.com>
Date: Mon, 3 May 2021 08:36:16 +0200
Subject: ARM: meson: platsmp: Add support for SoCs running on TrustZone
 firmware

When the SoC is running on the TrustZone firmware we cannot modify the
SMP related registers. Add a new set of SMP ops which use firmware calls
to set the startup (function) address and core control (on/off).

Signed-off-by: Martin Blumenstingl <martin.blumenstingl@googlemail.com>
---
 arch/arm/mach-meson/platsmp.c | 33 ++++++++++
 1 file changed, 33 insertions(+)

diff --git a/arch/arm/mach-meson/platsmp.c b/arch/arm/mach-meson/platsmp.c
index 111111111111..222222222222 100644
--- a/arch/arm/mach-meson/platsmp.c
+++ b/arch/arm/mach-meson/platsmp.c
@@ -16,6 +16,7 @@
 
 #include <asm/cacheflush.h>
 #include <asm/cp15.h>
+#include <asm/firmware.h>
 #include <asm/smp_scu.h>
 #include <asm/smp_plat.h>
 
@@ -291,6 +292,31 @@ static int meson8b_smp_boot_secondary(unsigned int cpu,
 	return 0;
 }
 
+static int meson8_smp_trustzone_firmware_boot_secondary(unsigned int cpu,
+							struct task_struct *idle)
+{
+	unsigned int addr = __pa_symbol(secondary_startup);
+	int ret;
+
+	ret = call_firmware_op(set_cpu_boot_addr, cpu, addr);
+	if (ret) {
+		pr_err("Failed to set aux core boot address for CPU%u using TrustZone secure firmware\n",
+			cpu);
+		return ret;
+	}
+
+	ret = call_firmware_op(cpu_boot, cpu);
+	if (ret) {
+		pr_err("Failed to modify core control for CPU%u using TrustZone secure firmware\n",
+			cpu);
+		return ret;
+	}
+
+	udelay(10);
+
+	return 0;
+}
+
 #ifdef CONFIG_HOTPLUG_CPU
 static void meson8_smp_cpu_die(unsigned int cpu)
 {
@@ -428,5 +454,12 @@ static struct smp_operations meson8b_smp_ops __initdata = {
 #endif
 };
 
+static struct smp_operations meson8_smp_trustzone_firmware_ops __initdata = {
+	.smp_boot_secondary	= meson8_smp_trustzone_firmware_boot_secondary,
+};
+
 CPU_METHOD_OF_DECLARE(meson8_smp, "amlogic,meson8-smp", &meson8_smp_ops);
 CPU_METHOD_OF_DECLARE(meson8b_smp, "amlogic,meson8b-smp", &meson8b_smp_ops);
+CPU_METHOD_OF_DECLARE(meson8_trustzone_firmware_smp,
+		      "amlogic,meson8-trustzone-firmware-smp",
+		      &meson8_smp_trustzone_firmware_ops);
-- 
Armbian