ARM: trusted_foundations: Implement L2 cache initialization callback

Implement L2 cache initialization firmware callback that should be invoked
early in boot in order to setup the required outer cache driver callbacks.

Signed-off-by: Dmitry Osipenko <digetx@gmail.com>

diff --git a/arch/arm/firmware/trusted_foundations.c b/arch/arm/firmware/trusted_foundations.c
index 689e656..3bf61a5 100644 (file)
--- a/arch/arm/firmware/trusted_foundations.c
+++ b/arch/arm/firmware/trusted_foundations.c
@@ -18,8 +18,15 @@
 #include <linux/init.h>
 #include <linux/of.h>
 #include <asm/firmware.h>
+#include <asm/hardware/cache-l2x0.h>
+#include <asm/outercache.h>
 #include <asm/trusted_foundations.h>
 
+#define TF_CACHE_MAINT         0xfffff100
+
+#define TF_CACHE_ENABLE                1
+#define TF_CACHE_DISABLE       2
+
 #define TF_SET_CPU_BOOT_ADDR_SMC 0xfffff200
 
 #define TF_CPU_PM              0xfffffffc
@@ -67,9 +74,48 @@ static int tf_prepare_idle(void)
        return 0;
 }
 
+#ifdef CONFIG_CACHE_L2X0
+static void tf_cache_write_sec(unsigned long val, unsigned int reg)
+{
+       static u32 l2x0_way_mask = 0xff;
+       static u32 l2x0_aux_ctrl = 0;
+
+       switch (reg) {
+       case L2X0_AUX_CTRL:
+               l2x0_aux_ctrl = val;
+
+               if (l2x0_aux_ctrl & BIT(16))
+                       l2x0_way_mask = 0xffff;
+               break;
+
+       case L2X0_CTRL:
+               if (val == L2X0_CTRL_EN)
+                       tf_generic_smc(TF_CACHE_MAINT, TF_CACHE_ENABLE,
+                                      l2x0_aux_ctrl);
+               else
+                       tf_generic_smc(TF_CACHE_MAINT, TF_CACHE_DISABLE,
+                                      l2x0_way_mask);
+               break;
+
+       default:
+               break;
+       }
+}
+
+static int tf_init_cache(void)
+{
+       outer_cache.write_sec = tf_cache_write_sec;
+
+       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)