ARM: enable cache via Trusted Foundations firmware

index 736292b42fca..665eb0758417 100644
@@ -167,11 +167,11 @@ static inline int l2x0_of_init(u32 aux_val, u32 aux_mask)
 #endif
 
 #ifdef CONFIG_CACHE_L2X0_PMU
-void l2x0_pmu_register(void __iomem *base, u32 part);
+void l2x0_pmu_register(u32 part);
 void l2x0_pmu_suspend(void);
 void l2x0_pmu_resume(void);
 #else
-static inline void l2x0_pmu_register(void __iomem *base, u32 part) {}
+static inline void l2x0_pmu_register(u32 part) {}
 static inline void l2x0_pmu_suspend(void) {}
 static inline void l2x0_pmu_resume(void) {}
 #endif

@@ -193,6 +193,8 @@ struct l2x0_regs {
 	unsigned long aux2_ctrl;
 };
 
+extern void __iomem *l2x0_base;
+extern u32 l2x0_way_mask;	/* Bitmask of active ways */
 extern struct l2x0_regs l2x0_saved_regs;
 
 #endif /* __ASSEMBLY__ */

index afe5b4c7b164..6be335ae4e2f 100644
@@ -29,11 +29,10 @@
 
 #define PMU_NR_COUNTERS 2
 
-static void __iomem *l2x0_base;
 static struct pmu *l2x0_pmu;
 static cpumask_t pmu_cpu;
 
-static const char *l2x0_name;
+static const char *__initdata l2x0_name = NULL;
 
 static ktime_t l2x0_pmu_poll_period;
 static struct hrtimer l2x0_pmu_hrtimer;

@@ -491,7 +490,7 @@ void l2x0_pmu_resume(void)
 	l2x0_pmu_enable(l2x0_pmu);
 }
 
-void __init l2x0_pmu_register(void __iomem *base, u32 part)
+void __init l2x0_pmu_register(u32 part)
 {
 	/*
 	 * Determine whether we support the PMU, and choose the name for sysfs.

@@ -516,15 +515,13 @@ void __init l2x0_pmu_register(void __iomem *base, u32 part)
 	default:
 		return;
 	}
-
-	l2x0_base = base;
 }
 
 static __init int l2x0_pmu_init(void)
 {
 	int ret;
 
-	if (!l2x0_base)
+	if (!l2x0_name)
 		return 0;
 
 	l2x0_pmu = kzalloc(sizeof(*l2x0_pmu), GFP_KERNEL);

index ea1e70ff4568..2b6a023fea3f 100644
@@ -48,13 +48,13 @@ struct l2c_init_data {
 
 #define CACHE_LINE_SIZE		32
 
-static void __iomem *l2x0_base;
 static const struct l2c_init_data *l2x0_data;
 static DEFINE_RAW_SPINLOCK(l2x0_lock);
-static u32 l2x0_way_mask;	/* Bitmask of active ways */
 static u32 l2x0_size;
 static unsigned long sync_reg_offset = L2X0_CACHE_SYNC;
 
+void __iomem *l2x0_base;
+u32 l2x0_way_mask;	/* Bitmask of active ways */
 struct l2x0_regs l2x0_saved_regs;
 
 static bool l2x0_bresp_disable;

@@ -900,7 +900,7 @@ static int __init __l2c_init(const struct l2c_init_data *data,
 	pr_info("%s: CACHE_ID 0x%08x, AUX_CTRL 0x%08x\n",
 		data->type, cache_id, aux);
 
-	l2x0_pmu_register(l2x0_base, cache_id);
+	l2x0_pmu_register(cache_id);
 
 	return 0;
 }

index 689e6565abfc..135068119453 100644
@@ -17,11 +17,19 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of.h>
+#include <asm/io.h>
 #include <asm/firmware.h>
+#include <asm/outercache.h>
+#include <asm/hardware/cache-l2x0.h>
 #include <asm/trusted_foundations.h>
 
+#define TF_CACHE_MAINT           0xfffff100
 #define TF_SET_CPU_BOOT_ADDR_SMC 0xfffff200
 
+#define TF_CACHE_INIT		1
+#define TF_CACHE_FLUSH		2
+#define TF_CACHE_REENABLE	4
+
 #define TF_CPU_PM		0xfffffffc
 #define TF_CPU_PM_S3		0xffffffe3
 #define TF_CPU_PM_S2		0xffffffe6

@@ -67,9 +75,47 @@ static int tf_prepare_idle(void)
 	return 0;
 }
 
+#ifdef CONFIG_CACHE_L2X0
+static void tf_write_sec(unsigned long val, unsigned reg)
+{
+	unsigned long cur = readl_relaxed(l2x0_base + reg);
+
+	pr_warn("TF: ignoring write_sec[0x%x]: 0x%08lx -> 0x%08lx\n", reg, cur, val);
+}
+
+static void tf_disable_cache(void)
+{
+	tf_generic_smc(TF_CACHE_MAINT, TF_CACHE_FLUSH, l2x0_way_mask);
+}
+
+static void tf_resume_cache(void)
+{
+	unsigned long aux_val = readl_relaxed(l2x0_base + L2X0_AUX_CTRL);
+	tf_generic_smc(TF_CACHE_MAINT, TF_CACHE_REENABLE, aux_val);
+}
+
+static void tf_configure_cache(const struct l2x0_regs *regs)
+{
+	outer_cache.disable = tf_disable_cache;
+	outer_cache.resume = tf_resume_cache;
+}
+
+static int tf_init_cache(void)
+{
+	tf_generic_smc(TF_CACHE_MAINT, TF_CACHE_INIT, 0);
+
+	outer_cache.write_sec = tf_write_sec;
+	outer_cache.configure = tf_configure_cache;
+	return 0;
+}
+#endif /* CONFIG_CACHE_L2X0 */
+
 static const struct firmware_ops trusted_foundations_ops = {
 	.set_cpu_boot_addr = tf_set_cpu_boot_addr,
 	.prepare_idle = tf_prepare_idle,
+#ifdef CONFIG_CACHE_L2X0
+	.l2x0_init = tf_init_cache,
+#endif
 };
 
 void register_trusted_foundations(struct trusted_foundations_platform_data *pd)

index 2b6a023fea3f..f1268e9b35f0 100644
@@ -30,6 +30,7 @@
 #include <asm/cp15.h>
 #include <asm/cputype.h>
 #include <asm/hardware/cache-l2x0.h>
+#include <asm/firmware.h>
 #include "cache-tauros3.h"
 #include "cache-aurora-l2.h"
 

@@ -37,6 +38,7 @@ struct l2c_init_data {
 	const char *type;
 	unsigned way_size_0;
 	unsigned num_lock;
+	void (*init)(void __iomem *, u32 *, u32 *);
 	void (*of_parse)(const struct device_node *, u32 *, u32 *);
 	void (*enable)(void __iomem *, unsigned);
 	void (*fixup)(void __iomem *, u32, struct outer_cache_fns *);

@@ -1760,6 +1762,7 @@ int __init l2x0_of_init(u32 aux_val, u32 aux_mask)
 	u32 cache_id;
 	u32 cache_level = 2;
 	bool nosync = false;
+	int err;
 
 	np = of_find_matching_node(NULL, l2x0_ids);
 	if (!np)

@@ -1792,6 +1795,11 @@ int __init l2x0_of_init(u32 aux_val, u32 aux_mask)
 
 	nosync = of_property_read_bool(np, "arm,outer-sync-disable");
 
+	/* Call firmware init */
+	err = call_firmware_op(l2x0_init);
+	if (err && err != -ENOSYS)
+		return err;
+
 	/* Read back current (default) hardware configuration */
 	if (data->save)
 		data->save(l2x0_base);

index 135068119453..e59f44fcda35 100644
@@ -30,6 +30,8 @@
 #define TF_CACHE_FLUSH		2
 #define TF_CACHE_REENABLE	4
 
+#define TF_GET_PROTOCOL_VERSION	0xfffffffb
+
 #define TF_CPU_PM		0xfffffffc
 #define TF_CPU_PM_S3		0xffffffe3
 #define TF_CPU_PM_S2		0xffffffe6

@@ -39,7 +41,7 @@
 
 static unsigned long cpu_boot_addr;
 
-static void tf_generic_smc(u32 type, u32 arg1, u32 arg2)
+static u64 tf_generic_smc(u32 type, u32 arg1, u32 arg2)
 {
 	register u32 r0 asm("r0") = type;
 	register u32 r1 asm("r1") = arg1;

@@ -55,9 +57,10 @@ static void tf_generic_smc(u32 type, u32 arg1, u32 arg2)
 		"mov	r4, #0\n\t"
 		"smc	#0\n\t"
 		"ldmfd	sp!, {r4 - r11}\n\t"
-		:
-		: "r" (r0), "r" (r1), "r" (r2)
+		: "+r" (r0), "+r" (r1)
+		: "r" (r2)
 		: "memory", "r3", "r12", "lr");
+	return ((u64)r1 << 32) | r0;
 }
 
 static int tf_set_cpu_boot_addr(int cpu, unsigned long boot_addr)

@@ -110,6 +113,11 @@ static int tf_init_cache(void)
 }
 #endif /* CONFIG_CACHE_L2X0 */
 
+static u32 tf_get_protocol_version(void)
+{
+	return tf_generic_smc(TF_GET_PROTOCOL_VERSION, 0, 0) >> 48;
+}
+
 static const struct firmware_ops trusted_foundations_ops = {
 	.set_cpu_boot_addr = tf_set_cpu_boot_addr,
 	.prepare_idle = tf_prepare_idle,

@@ -120,6 +128,12 @@ static const struct firmware_ops trusted_foundations_ops = {
 
 void register_trusted_foundations(struct trusted_foundations_platform_data *pd)
 {
+	unsigned int protocol = tf_get_protocol_version();
+
+	pr_info("TF: version %u.%u, protocol %u.%u\n",
+		pd->version_major, pd->version_minor,
+		protocol >> 8, protocol & 0xFF);
+
 	/*
 	 * we are not using version information for now since currently
 	 * supported SMCs are compatible with all TF releases

@@ -145,5 +159,6 @@ void of_register_trusted_foundations(void)
 				   &pdata.version_minor);
 	if (err != 0)
 		panic("Trusted Foundation: missing version-minor property\n");
+
 	register_trusted_foundations(&pdata);
 }

index e59f44fcda35..148d890da0b6 100644
@@ -141,7 +141,7 @@ void register_trusted_foundations(struct trusted_foundations_platform_data *pd)
 	register_firmware_ops(&trusted_foundations_ops);
 }
 
-void of_register_trusted_foundations(void)
+int of_register_trusted_foundations(void)
 {
 	struct device_node *node;
 	struct trusted_foundations_platform_data pdata;

@@ -149,7 +149,7 @@ void of_register_trusted_foundations(void)
 
 	node = of_find_compatible_node(NULL, NULL, "tlm,trusted-foundations");
 	if (!node)
-		return;
+		return -ENOENT;
 
 	err = of_property_read_u32(node, "tlm,version-major",
 				   &pdata.version_major);

@@ -161,4 +161,6 @@ void of_register_trusted_foundations(void)
 		panic("Trusted Foundation: missing version-minor property\n");
 
 	register_trusted_foundations(&pdata);
+
+	return 0;
 }

index 00748350cf72..80104a4e8476 100644
@@ -40,7 +40,7 @@ struct trusted_foundations_platform_data {
 #if IS_ENABLED(CONFIG_TRUSTED_FOUNDATIONS)
 
 void register_trusted_foundations(struct trusted_foundations_platform_data *pd);
-void of_register_trusted_foundations(void);
+int of_register_trusted_foundations(void);
 
 #else /* CONFIG_TRUSTED_FOUNDATIONS */
 

@@ -59,7 +59,7 @@ static inline void register_trusted_foundations(
 	cpu_idle_poll_ctrl(true);
 }
 
-static inline void of_register_trusted_foundations(void)
+static inline int of_register_trusted_foundations(void)
 {
 	/*
 	 * If we find the target should enable TF but does not support it,

@@ -67,6 +67,8 @@ static inline void of_register_trusted_foundations(void)
 	 */
 	if (of_find_compatible_node(NULL, NULL, "tlm,trusted-foundations"))
 		register_trusted_foundations(NULL);
+
+	return -ENOENT;
 }
 #endif /* CONFIG_TRUSTED_FOUNDATIONS */
 

index dc558892753c..1c3cb62255fe 100644
@@ -18,6 +18,7 @@
 #include <linux/cpumask.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/of.h>
 
 #include <soc/tegra/fuse.h>
 

@@ -87,7 +88,7 @@ static void __init tegra_cpu_reset_handler_enable(void)
 	}
 }
 
-void __init tegra_cpu_reset_handler_init(void)
+void __init tegra_cpu_reset_handler_init(bool tf_present)
 {
 
 #ifdef CONFIG_SMP

@@ -104,5 +105,7 @@ void __init tegra_cpu_reset_handler_init(void)
 		__pa_symbol((void *)tegra_resume);
 #endif
 
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_TF_PRESENT] = tf_present;
+
 	tegra_cpu_reset_handler_enable();
 }

index 9c479c7925b8..5e42dd1e9841 100644
@@ -25,7 +25,8 @@
 #define TEGRA_RESET_STARTUP_SECONDARY	3
 #define TEGRA_RESET_STARTUP_LP2		4
 #define TEGRA_RESET_STARTUP_LP1		5
-#define TEGRA_RESET_DATA_SIZE		6
+#define TEGRA_RESET_TF_PRESENT		6
+#define TEGRA_RESET_DATA_SIZE		7
 
 #ifndef __ASSEMBLY__
 

@@ -60,7 +61,7 @@ void __tegra_cpu_reset_handler_end(void);
 		(__tegra_cpu_reset_handler_end - \
 		 __tegra_cpu_reset_handler_start)
 
-void __init tegra_cpu_reset_handler_init(void);
+void __init tegra_cpu_reset_handler_init(bool tf_present);
 
 #endif
 #endif

index f9587be48235..0281de82387e 100644
@@ -72,8 +72,10 @@ u32 tegra_uart_config[3] = {
 
 static void __init tegra_init_early(void)
 {
-	of_register_trusted_foundations();
-	tegra_cpu_reset_handler_init();
+	bool tf_present;
+
+	tf_present = of_register_trusted_foundations() == 0;
+	tegra_cpu_reset_handler_init(tf_present);
 }
 
 static void __init tegra_dt_init_irq(void)