5 files changed, 40 insertions, 7 deletions

diff --git a/arch/arm/firmware/Kconfig b/arch/arm/firmware/Kconfig
index bb00ccf00d66..ad396af68e47 100644
--- a/arch/arm/firmware/Kconfig
+++ b/arch/arm/firmware/Kconfig
@@ -11,6 +11,7 @@ menu "Firmware options"
 config TRUSTED_FOUNDATIONS
         bool "Trusted Foundations secure monitor support"
         depends on ARCH_SUPPORTS_TRUSTED_FOUNDATIONS
+        default y
         help
           Some devices (including most Tegra-based consumer devices on the
           market) are booted with the Trusted Foundations secure monitor
@@ -20,7 +21,7 @@ config TRUSTED_FOUNDATIONS
           This option allows the kernel to invoke the secure monitor whenever
           required on devices using Trusted Foundations. See
           arch/arm/include/asm/trusted_foundations.h or the
-          tl,trusted-foundations device tree binding documentation for details
+          tlm,trusted-foundations device tree binding documentation for details
           on how to use it.
 
           Say n if you don't know what this is about.
diff --git a/arch/arm/firmware/trusted_foundations.c b/arch/arm/firmware/trusted_foundations.c
index ef1e3d8f4af0..3fb1b5a1dce9 100644
--- a/arch/arm/firmware/trusted_foundations.c
+++ b/arch/arm/firmware/trusted_foundations.c
@@ -22,6 +22,15 @@
 
 #define TF_SET_CPU_BOOT_ADDR_SMC 0xfffff200
 
+#define TF_CPU_PM               0xfffffffc
+#define TF_CPU_PM_S3            0xffffffe3
+#define TF_CPU_PM_S2            0xffffffe6
+#define TF_CPU_PM_S2_NO_MC_CLK  0xffffffe5
+#define TF_CPU_PM_S1            0xffffffe4
+#define TF_CPU_PM_S1_NOFLUSH_L2 0xffffffe7
+
+static unsigned long cpu_boot_addr;
+
 static void __naked tf_generic_smc(u32 type, u32 arg1, u32 arg2)
 {
         asm volatile(
@@ -41,13 +50,22 @@ static void __naked tf_generic_smc(u32 type, u32 arg1, u32 arg2)
 
 static int tf_set_cpu_boot_addr(int cpu, unsigned long boot_addr)
 {
-        tf_generic_smc(TF_SET_CPU_BOOT_ADDR_SMC, boot_addr, 0);
+        cpu_boot_addr = boot_addr;
+        tf_generic_smc(TF_SET_CPU_BOOT_ADDR_SMC, cpu_boot_addr, 0);
+
+        return 0;
+}
+
+static int tf_prepare_idle(void)
+{
+        tf_generic_smc(TF_CPU_PM, TF_CPU_PM_S1_NOFLUSH_L2, cpu_boot_addr);
 
         return 0;
 }
 
 static const struct firmware_ops trusted_foundations_ops = {
         .set_cpu_boot_addr = tf_set_cpu_boot_addr,
+        .prepare_idle = tf_prepare_idle,
 };
 
 void register_trusted_foundations(struct trusted_foundations_platform_data *pd)
diff --git a/arch/arm/include/asm/firmware.h b/arch/arm/include/asm/firmware.h
index 15631300c238..2c9f10df7568 100644
--- a/arch/arm/include/asm/firmware.h
+++ b/arch/arm/include/asm/firmware.h
@@ -22,6 +22,10 @@
  */
 struct firmware_ops {
         /*
+         * Inform the firmware we intend to enter CPU idle mode
+         */
+        int (*prepare_idle)(void);
+        /*
          * Enters CPU idle mode
          */
         int (*do_idle)(void);
diff --git a/arch/arm/include/asm/trusted_foundations.h b/arch/arm/include/asm/trusted_foundations.h
index 3bd36e2c5f2e..b5f7705abcb0 100644
--- a/arch/arm/include/asm/trusted_foundations.h
+++ b/arch/arm/include/asm/trusted_foundations.h
@@ -30,6 +30,8 @@
 #include <linux/printk.h>
 #include <linux/bug.h>
 #include <linux/of.h>
+#include <linux/cpu.h>
+#include <linux/smp.h>
 
 struct trusted_foundations_platform_data {
         unsigned int version_major;
@@ -47,10 +49,13 @@ static inline void register_trusted_foundations(
                                    struct trusted_foundations_platform_data *pd)
 {
         /*
-         * If we try to register TF, this means the system needs it to continue.
-         * Its absence if thus a fatal error.
+         * If the system requires TF and we cannot provide it, continue booting
+         * but disable features that cannot be provided.
          */
-        panic("No support for Trusted Foundations, stopping...\n");
+        pr_err("No support for Trusted Foundations, continuing in degraded mode.\n");
+        pr_err("Secondary processors as well as CPU PM will be disabled.\n");
+        setup_max_cpus = 0;
+        cpu_idle_poll_ctrl(true);
 }
 
 static inline void of_register_trusted_foundations(void)
@@ -59,7 +64,7 @@ static inline void of_register_trusted_foundations(void)
          * If we find the target should enable TF but does not support it,
          * fail as the system won't be able to do much anyway
          */
-        if (of_find_compatible_node(NULL, NULL, "tl,trusted-foundations"))
+        if (of_find_compatible_node(NULL, NULL, "tlm,trusted-foundations"))
                 register_trusted_foundations(NULL);
 }
 #endif /* CONFIG_TRUSTED_FOUNDATIONS */
diff --git a/arch/arm/mach-tegra/cpuidle-tegra114.c b/arch/arm/mach-tegra/cpuidle-tegra114.c
index e0b87300243d..b5fb7c110c64 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra114.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra114.c
@@ -19,6 +19,7 @@
 #include <linux/cpuidle.h>
 #include <linux/cpu_pm.h>
 #include <linux/clockchips.h>
+#include <asm/firmware.h>
 
 #include <asm/cpuidle.h>
 #include <asm/suspend.h>
@@ -45,7 +46,11 @@ static int tegra114_idle_power_down(struct cpuidle_device *dev,
 
         clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
 
-        cpu_suspend(0, tegra30_sleep_cpu_secondary_finish);
+        call_firmware_op(prepare_idle);
+
+        /* Do suspend by ourselves if the firmware does not implement it */
+        if (call_firmware_op(do_idle) == -ENOSYS)
+                cpu_suspend(0, tegra30_sleep_cpu_secondary_finish);
 
         clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);