18 files changed, 767 insertions, 70 deletions

diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
index 2a700e00528d..b18165ca1d38 100644
--- a/arch/arm/kvm/arm.c
+++ b/arch/arm/kvm/arm.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/cpu.h>
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/err.h>
 #include <linux/kvm_host.h>
@@ -853,6 +854,33 @@ static struct notifier_block hyp_init_cpu_nb = {
         .notifier_call = hyp_init_cpu_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static int hyp_init_cpu_pm_notifier(struct notifier_block *self,
+                                    unsigned long cmd,
+                                    void *v)
+{
+        if (cmd == CPU_PM_EXIT) {
+                cpu_init_hyp_mode(NULL);
+                return NOTIFY_OK;
+        }
+
+        return NOTIFY_DONE;
+}
+
+static struct notifier_block hyp_init_cpu_pm_nb = {
+        .notifier_call = hyp_init_cpu_pm_notifier,
+};
+
+static void __init hyp_cpu_pm_init(void)
+{
+        cpu_pm_register_notifier(&hyp_init_cpu_pm_nb);
+}
+#else
+static inline void hyp_cpu_pm_init(void)
+{
+}
+#endif
+
 /**
  * Inits Hyp-mode on all online CPUs
  */
@@ -1013,6 +1041,8 @@ int kvm_arch_init(void *opaque)
                 goto out_err;
         }
 
+        hyp_cpu_pm_init();
+
         kvm_coproc_table_init();
         return 0;
 out_err:
diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index 101683d336c9..249acb9da4e3 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -2,6 +2,7 @@ config ARM64
         def_bool y
         select ARCH_HAS_ATOMIC64_DEC_IF_POSITIVE
         select ARCH_USE_CMPXCHG_LOCKREF
+        select ARCH_HAS_TICK_BROADCAST if GENERIC_CLOCKEVENTS_BROADCAST
         select ARCH_WANT_OPTIONAL_GPIOLIB
         select ARCH_WANT_COMPAT_IPC_PARSE_VERSION
         select ARCH_WANT_FRAME_POINTERS
@@ -11,8 +12,10 @@ config ARM64
         select BUILDTIME_EXTABLE_SORT
         select CLONE_BACKWARDS
         select COMMON_CLK
+        select CPU_PM if (SUSPEND || CPU_IDLE)
         select DCACHE_WORD_ACCESS
         select GENERIC_CLOCKEVENTS
+        select GENERIC_CLOCKEVENTS_BROADCAST if SMP
         select GENERIC_IOMAP
         select GENERIC_IRQ_PROBE
         select GENERIC_IRQ_SHOW
@@ -280,6 +283,24 @@ config SYSVIPC_COMPAT
 
 endmenu
 
+menu "Power management options"
+
+source "kernel/power/Kconfig"
+
+config ARCH_SUSPEND_POSSIBLE
+        def_bool y
+
+config ARM64_CPU_SUSPEND
+        def_bool PM_SLEEP
+
+endmenu
+
+menu "CPU Power Management"
+
+source "drivers/cpuidle/Kconfig"
+
+endmenu
+
 source "net/Kconfig"
 
 source "drivers/Kconfig"
diff --git a/arch/arm64/include/asm/cpu_ops.h b/arch/arm64/include/asm/cpu_ops.h
index c4cdb5e5b73d..152413076503 100644
--- a/arch/arm64/include/asm/cpu_ops.h
+++ b/arch/arm64/include/asm/cpu_ops.h
@@ -39,6 +39,9 @@ struct device_node;
  *              from the cpu to be killed.
  * @cpu_die:    Makes a cpu leave the kernel. Must not fail. Called from the
  *              cpu being killed.
+ * @cpu_suspend: Suspends a cpu and saves the required context. May fail owing
+ *               to wrong parameters or error conditions. Called from the
+ *               CPU being suspended. Must be called with IRQs disabled.
  */
 struct cpu_operations {
         const char      *name;
@@ -50,6 +53,9 @@ struct cpu_operations {
         int             (*cpu_disable)(unsigned int cpu);
         void            (*cpu_die)(unsigned int cpu);
 #endif
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+        int             (*cpu_suspend)(unsigned long);
+#endif
 };
 
 extern const struct cpu_operations *cpu_ops[NR_CPUS];
diff --git a/arch/arm64/include/asm/cputype.h b/arch/arm64/include/asm/cputype.h
index e1af1b4200d5..c404fb0df3a6 100644
--- a/arch/arm64/include/asm/cputype.h
+++ b/arch/arm64/include/asm/cputype.h
@@ -20,6 +20,16 @@
 
 #define MPIDR_HWID_BITMASK      0xff00ffffff
 
+#define MPIDR_LEVEL_BITS_SHIFT  3
+#define MPIDR_LEVEL_BITS        (1 << MPIDR_LEVEL_BITS_SHIFT)
+#define MPIDR_LEVEL_MASK        ((1 << MPIDR_LEVEL_BITS) - 1)
+
+#define MPIDR_LEVEL_SHIFT(level) \
+        (((1 << level) >> 1) << MPIDR_LEVEL_BITS_SHIFT)
+
+#define MPIDR_AFFINITY_LEVEL(mpidr, level) \
+        ((mpidr >> MPIDR_LEVEL_SHIFT(level)) & MPIDR_LEVEL_MASK)
+
 #define read_cpuid(reg) ({                                              \
         u64 __val;                                                      \
         asm("mrs        %0, " #reg : "=r" (__val));                     \
diff --git a/arch/arm64/include/asm/hardirq.h b/arch/arm64/include/asm/hardirq.h
index 990c051e7829..ae4801d77514 100644
--- a/arch/arm64/include/asm/hardirq.h
+++ b/arch/arm64/include/asm/hardirq.h
@@ -20,7 +20,7 @@
 #include <linux/threads.h>
 #include <asm/irq.h>
 
-#define NR_IPI  4
+#define NR_IPI  5
 
 typedef struct {
         unsigned int __softirq_pending;
diff --git a/arch/arm64/include/asm/proc-fns.h b/arch/arm64/include/asm/proc-fns.h
index 7cdf466fd0c5..0c657bb54597 100644
--- a/arch/arm64/include/asm/proc-fns.h
+++ b/arch/arm64/include/asm/proc-fns.h
@@ -26,11 +26,14 @@
 #include <asm/page.h>
 
 struct mm_struct;
+struct cpu_suspend_ctx;
 
 extern void cpu_cache_off(void);
 extern void cpu_do_idle(void);
 extern void cpu_do_switch_mm(unsigned long pgd_phys, struct mm_struct *mm);
 extern void cpu_reset(unsigned long addr) __attribute__((noreturn));
+extern void cpu_do_suspend(struct cpu_suspend_ctx *ptr);
+extern u64 cpu_do_resume(phys_addr_t ptr, u64 idmap_ttbr);
 
 #include <asm/memory.h>
 
diff --git a/arch/arm64/include/asm/smp_plat.h b/arch/arm64/include/asm/smp_plat.h
index ed43a0d2b1b2..59e282311b58 100644
--- a/arch/arm64/include/asm/smp_plat.h
+++ b/arch/arm64/include/asm/smp_plat.h
@@ -21,6 +21,19 @@
 
 #include <asm/types.h>
 
+struct mpidr_hash {
+        u64     mask;
+        u32     shift_aff[4];
+        u32     bits;
+};
+
+extern struct mpidr_hash mpidr_hash;
+
+static inline u32 mpidr_hash_size(void)
+{
+        return 1 << mpidr_hash.bits;
+}
+
 /*
  * Logical CPU mapping.
  */
diff --git a/arch/arm64/include/asm/suspend.h b/arch/arm64/include/asm/suspend.h
new file mode 100644
index 000000000000..e9c149c042e0
--- /dev/null
+++ b/arch/arm64/include/asm/suspend.h
@@ -0,0 +1,27 @@
+#ifndef __ASM_SUSPEND_H
+#define __ASM_SUSPEND_H
+
+#define NR_CTX_REGS 11
+
+/*
+ * struct cpu_suspend_ctx must be 16-byte aligned since it is allocated on
+ * the stack, which must be 16-byte aligned on v8
+ */
+struct cpu_suspend_ctx {
+        /*
+         * This struct must be kept in sync with
+         * cpu_do_{suspend/resume} in mm/proc.S
+         */
+        u64 ctx_regs[NR_CTX_REGS];
+        u64 sp;
+} __aligned(16);
+
+struct sleep_save_sp {
+        phys_addr_t *save_ptr_stash;
+        phys_addr_t save_ptr_stash_phys;
+};
+
+extern void cpu_resume(void);
+extern int cpu_suspend(unsigned long);
+
+#endif
diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 5ba2fd43a75b..1cd339d5037b 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -18,6 +18,7 @@ arm64-obj-$(CONFIG_SMP)			+= smp.o smp_spin_table.o
 arm64-obj-$(CONFIG_HW_PERF_EVENTS)      += perf_event.o
 arm64-obj-$(CONFIG_HAVE_HW_BREAKPOINT)+= hw_breakpoint.o
 arm64-obj-$(CONFIG_EARLY_PRINTK)        += early_printk.o
+arm64-obj-$(CONFIG_ARM64_CPU_SUSPEND)   += sleep.o suspend.o
 
 obj-y                                   += $(arm64-obj-y) vdso/
 obj-m                                   += $(arm64-obj-m)
diff --git a/arch/arm64/kernel/asm-offsets.c b/arch/arm64/kernel/asm-offsets.c
index 666e231d410b..646f888387cd 100644
--- a/arch/arm64/kernel/asm-offsets.c
+++ b/arch/arm64/kernel/asm-offsets.c
@@ -25,6 +25,8 @@
 #include <asm/thread_info.h>
 #include <asm/memory.h>
 #include <asm/cputable.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
 #include <asm/vdso_datapage.h>
 #include <linux/kbuild.h>
 
@@ -138,5 +140,14 @@ int main(void)
   DEFINE(KVM_VTTBR,             offsetof(struct kvm, arch.vttbr));
   DEFINE(KVM_VGIC_VCTRL,        offsetof(struct kvm, arch.vgic.vctrl_base));
 #endif
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+  DEFINE(CPU_SUSPEND_SZ,        sizeof(struct cpu_suspend_ctx));
+  DEFINE(CPU_CTX_SP,            offsetof(struct cpu_suspend_ctx, sp));
+  DEFINE(MPIDR_HASH_MASK,       offsetof(struct mpidr_hash, mask));
+  DEFINE(MPIDR_HASH_SHIFTS,     offsetof(struct mpidr_hash, shift_aff));
+  DEFINE(SLEEP_SAVE_SP_SZ,      sizeof(struct sleep_save_sp));
+  DEFINE(SLEEP_SAVE_SP_PHYS,    offsetof(struct sleep_save_sp, save_ptr_stash_phys));
+  DEFINE(SLEEP_SAVE_SP_VIRT,    offsetof(struct sleep_save_sp, save_ptr_stash));
+#endif
   return 0;
 }
diff --git a/arch/arm64/kernel/fpsimd.c b/arch/arm64/kernel/fpsimd.c
index bb785d23dbde..4aef42a04bdc 100644
--- a/arch/arm64/kernel/fpsimd.c
+++ b/arch/arm64/kernel/fpsimd.c
@@ -17,6 +17,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/cpu_pm.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/sched.h>
@@ -113,6 +114,39 @@ EXPORT_SYMBOL(kernel_neon_end);
 
 #endif /* CONFIG_KERNEL_MODE_NEON */
 
+#ifdef CONFIG_CPU_PM
+static int fpsimd_cpu_pm_notifier(struct notifier_block *self,
+                                  unsigned long cmd, void *v)
+{
+        switch (cmd) {
+        case CPU_PM_ENTER:
+                if (current->mm)
+                        fpsimd_save_state(&current->thread.fpsimd_state);
+                break;
+        case CPU_PM_EXIT:
+                if (current->mm)
+                        fpsimd_load_state(&current->thread.fpsimd_state);
+                break;
+        case CPU_PM_ENTER_FAILED:
+        default:
+                return NOTIFY_DONE;
+        }
+        return NOTIFY_OK;
+}
+
+static struct notifier_block fpsimd_cpu_pm_notifier_block = {
+        .notifier_call = fpsimd_cpu_pm_notifier,
+};
+
+static void fpsimd_pm_init(void)
+{
+        cpu_pm_register_notifier(&fpsimd_cpu_pm_notifier_block);
+}
+
+#else
+static inline void fpsimd_pm_init(void) { }
+#endif /* CONFIG_CPU_PM */
+
 /*
  * FP/SIMD support code initialisation.
  */
@@ -131,6 +165,8 @@ static int __init fpsimd_init(void)
         else
                 elf_hwcap |= HWCAP_ASIMD;
 
+        fpsimd_pm_init();
+
         return 0;
 }
 late_initcall(fpsimd_init);
diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index ff516f6691e4..bcaaac9e14d6 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -20,6 +20,7 @@
 
 #define pr_fmt(fmt) "hw-breakpoint: " fmt
 
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/hw_breakpoint.h>
 #include <linux/perf_event.h>
@@ -169,15 +170,68 @@ static enum debug_el debug_exception_level(int privilege)
         }
 }
 
-/*
- * Install a perf counter breakpoint.
+enum hw_breakpoint_ops {
+        HW_BREAKPOINT_INSTALL,
+        HW_BREAKPOINT_UNINSTALL,
+        HW_BREAKPOINT_RESTORE
+};
+
+/**
+ * hw_breakpoint_slot_setup - Find and setup a perf slot according to
+ *                            operations
+ *
+ * @slots: pointer to array of slots
+ * @max_slots: max number of slots
+ * @bp: perf_event to setup
+ * @ops: operation to be carried out on the slot
+ *
+ * Return:
+ *      slot index on success
+ *      -ENOSPC if no slot is available/matches
+ *      -EINVAL on wrong operations parameter
  */
-int arch_install_hw_breakpoint(struct perf_event *bp)
+static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
+                                    struct perf_event *bp,
+                                    enum hw_breakpoint_ops ops)
+{
+        int i;
+        struct perf_event **slot;
+
+        for (i = 0; i < max_slots; ++i) {
+                slot = &slots[i];
+                switch (ops) {
+                case HW_BREAKPOINT_INSTALL:
+                        if (!*slot) {
+                                *slot = bp;
+                                return i;
+                        }
+                        break;
+                case HW_BREAKPOINT_UNINSTALL:
+                        if (*slot == bp) {
+                                *slot = NULL;
+                                return i;
+                        }
+                        break;
+                case HW_BREAKPOINT_RESTORE:
+                        if (*slot == bp)
+                                return i;
+                        break;
+                default:
+                        pr_warn_once("Unhandled hw breakpoint ops %d\n", ops);
+                        return -EINVAL;
+                }
+        }
+        return -ENOSPC;
+}
+
+static int hw_breakpoint_control(struct perf_event *bp,
+                                 enum hw_breakpoint_ops ops)
 {
         struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-        struct perf_event **slot, **slots;
+        struct perf_event **slots;
         struct debug_info *debug_info = &current->thread.debug;
         int i, max_slots, ctrl_reg, val_reg, reg_enable;
+        enum debug_el dbg_el = debug_exception_level(info->ctrl.privilege);
         u32 ctrl;
 
         if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
@@ -196,67 +250,54 @@ int arch_install_hw_breakpoint(struct perf_event *bp)
                 reg_enable = !debug_info->wps_disabled;
         }
 
-        for (i = 0; i < max_slots; ++i) {
-                slot = &slots[i];
+        i = hw_breakpoint_slot_setup(slots, max_slots, bp, ops);
 
-                if (!*slot) {
-                        *slot = bp;
-                        break;
-                }
-        }
+        if (WARN_ONCE(i < 0, "Can't find any breakpoint slot"))
+                return i;
 
-        if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-                return -ENOSPC;
-
-        /* Ensure debug monitors are enabled at the correct exception level.  */
-        enable_debug_monitors(debug_exception_level(info->ctrl.privilege));
-
-        /* Setup the address register. */
-        write_wb_reg(val_reg, i, info->address);
+        switch (ops) {
+        case HW_BREAKPOINT_INSTALL:
+                /*
+                 * Ensure debug monitors are enabled at the correct exception
+                 * level.
+                 */
+                enable_debug_monitors(dbg_el);
+                /* Fall through */
+        case HW_BREAKPOINT_RESTORE:
+                /* Setup the address register. */
+                write_wb_reg(val_reg, i, info->address);
+
+                /* Setup the control register. */
+                ctrl = encode_ctrl_reg(info->ctrl);
+                write_wb_reg(ctrl_reg, i,
+                             reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+                break;
+        case HW_BREAKPOINT_UNINSTALL:
+                /* Reset the control register. */
+                write_wb_reg(ctrl_reg, i, 0);
 
-        /* Setup the control register. */
-        ctrl = encode_ctrl_reg(info->ctrl);
-        write_wb_reg(ctrl_reg, i, reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+                /*
+                 * Release the debug monitors for the correct exception
+                 * level.
+                 */
+                disable_debug_monitors(dbg_el);
+                break;
+        }
 
         return 0;
 }
 
-void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+/*
+ * Install a perf counter breakpoint.
+ */
+int arch_install_hw_breakpoint(struct perf_event *bp)
 {
-        struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-        struct perf_event **slot, **slots;
-        int i, max_slots, base;
-
-        if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
-                /* Breakpoint */
-                base = AARCH64_DBG_REG_BCR;
-                slots = this_cpu_ptr(bp_on_reg);
-                max_slots = core_num_brps;
-        } else {
-                /* Watchpoint */
-                base = AARCH64_DBG_REG_WCR;
-                slots = this_cpu_ptr(wp_on_reg);
-                max_slots = core_num_wrps;
-        }
-
-        /* Remove the breakpoint. */
-        for (i = 0; i < max_slots; ++i) {
-                slot = &slots[i];
-
-                if (*slot == bp) {
-                        *slot = NULL;
-                        break;
-                }
-        }
-
-        if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-                return;
-
-        /* Reset the control register. */
-        write_wb_reg(base, i, 0);
+        return hw_breakpoint_control(bp, HW_BREAKPOINT_INSTALL);
+}
 
-        /* Release the debug monitors for the correct exception level.  */
-        disable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+{
+        hw_breakpoint_control(bp, HW_BREAKPOINT_UNINSTALL);
 }
 
 static int get_hbp_len(u8 hbp_len)
@@ -806,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
 /*
  * CPU initialisation.
  */
-static void reset_ctrl_regs(void *unused)
+static void hw_breakpoint_reset(void *unused)
 {
         int i;
-
-        for (i = 0; i < core_num_brps; ++i) {
-                write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
-                write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+        struct perf_event **slots;
+        /*
+         * When a CPU goes through cold-boot, it does not have any installed
+         * slot, so it is safe to share the same function for restoring and
+         * resetting breakpoints; when a CPU is hotplugged in, it goes
+         * through the slots, which are all empty, hence it just resets control
+         * and value for debug registers.
+         * When this function is triggered on warm-boot through a CPU PM
+         * notifier some slots might be initialized; if so they are
+         * reprogrammed according to the debug slots content.
+         */
+        for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
+                if (slots[i]) {
+                        hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+                } else {
+                        write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
+                        write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+                }
         }
 
-        for (i = 0; i < core_num_wrps; ++i) {
-                write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
-                write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+        for (slots = this_cpu_ptr(wp_on_reg), i = 0; i < core_num_wrps; ++i) {
+                if (slots[i]) {
+                        hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+                } else {
+                        write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
+                        write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+                }
         }
 }
 
@@ -827,7 +886,7 @@ static int hw_breakpoint_reset_notify(struct notifier_block *self,
 {
         int cpu = (long)hcpu;
         if (action == CPU_ONLINE)
-                smp_call_function_single(cpu, reset_ctrl_regs, NULL, 1);
+                smp_call_function_single(cpu, hw_breakpoint_reset, NULL, 1);
         return NOTIFY_OK;
 }
 
@@ -835,6 +894,33 @@ static struct notifier_block hw_breakpoint_reset_nb = {
         .notifier_call = hw_breakpoint_reset_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static int hw_breakpoint_cpu_pm_notify(struct notifier_block *self,
+                                       unsigned long action,
+                                       void *v)
+{
+        if (action == CPU_PM_EXIT) {
+                hw_breakpoint_reset(NULL);
+                return NOTIFY_OK;
+        }
+
+        return NOTIFY_DONE;
+}
+
+static struct notifier_block hw_breakpoint_cpu_pm_nb = {
+        .notifier_call = hw_breakpoint_cpu_pm_notify,
+};
+
+static void __init hw_breakpoint_pm_init(void)
+{
+        cpu_pm_register_notifier(&hw_breakpoint_cpu_pm_nb);
+}
+#else
+static inline void hw_breakpoint_pm_init(void)
+{
+}
+#endif
+
 /*
  * One-time initialisation.
  */
@@ -850,8 +936,8 @@ static int __init arch_hw_breakpoint_init(void)
          * Reset the breakpoint resources. We assume that a halting
          * debugger will leave the world in a nice state for us.
          */
-        smp_call_function(reset_ctrl_regs, NULL, 1);
-        reset_ctrl_regs(NULL);
+        smp_call_function(hw_breakpoint_reset, NULL, 1);
+        hw_breakpoint_reset(NULL);
 
         /* Register debug fault handlers. */
         hook_debug_fault_code(DBG_ESR_EVT_HWBP, breakpoint_handler, SIGTRAP,
@@ -861,6 +947,7 @@ static int __init arch_hw_breakpoint_init(void)
 
         /* Register hotplug notifier. */
         register_cpu_notifier(&hw_breakpoint_reset_nb);
+        hw_breakpoint_pm_init();
 
         return 0;
 }
diff --git a/arch/arm64/kernel/process.c b/arch/arm64/kernel/process.c
index 0adb8f0f4549..248a15db37f2 100644
--- a/arch/arm64/kernel/process.c
+++ b/arch/arm64/kernel/process.c
@@ -33,6 +33,7 @@
 #include <linux/kallsyms.h>
 #include <linux/init.h>
 #include <linux/cpu.h>
+#include <linux/cpuidle.h>
 #include <linux/elfcore.h>
 #include <linux/pm.h>
 #include <linux/tick.h>
@@ -98,8 +99,10 @@ void arch_cpu_idle(void)
          * This should do all the clock switching and wait for interrupt
          * tricks
          */
-        cpu_do_idle();
-        local_irq_enable();
+        if (cpuidle_idle_call()) {
+                cpu_do_idle();
+                local_irq_enable();
+        }
 }
 
 #ifdef CONFIG_HOTPLUG_CPU
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index bb33fff09ba2..c8e9effe52e1 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -123,6 +123,75 @@ bool arch_match_cpu_phys_id(int cpu, u64 phys_id)
         return phys_id == cpu_logical_map(cpu);
 }
 
+struct mpidr_hash mpidr_hash;
+#ifdef CONFIG_SMP
+/**
+ * smp_build_mpidr_hash - Pre-compute shifts required at each affinity
+ *                        level in order to build a linear index from an
+ *                        MPIDR value. Resulting algorithm is a collision
+ *                        free hash carried out through shifting and ORing
+ */
+static void __init smp_build_mpidr_hash(void)
+{
+        u32 i, affinity, fs[4], bits[4], ls;
+        u64 mask = 0;
+        /*
+         * Pre-scan the list of MPIDRS and filter out bits that do
+         * not contribute to affinity levels, ie they never toggle.
+         */
+        for_each_possible_cpu(i)
+                mask |= (cpu_logical_map(i) ^ cpu_logical_map(0));
+        pr_debug("mask of set bits %#llx\n", mask);
+        /*
+         * Find and stash the last and first bit set at all affinity levels to
+         * check how many bits are required to represent them.
+         */
+        for (i = 0; i < 4; i++) {
+                affinity = MPIDR_AFFINITY_LEVEL(mask, i);
+                /*
+                 * Find the MSB bit and LSB bits position
+                 * to determine how many bits are required
+                 * to express the affinity level.
+                 */
+                ls = fls(affinity);
+                fs[i] = affinity ? ffs(affinity) - 1 : 0;
+                bits[i] = ls - fs[i];
+        }
+        /*
+         * An index can be created from the MPIDR_EL1 by isolating the
+         * significant bits at each affinity level and by shifting
+         * them in order to compress the 32 bits values space to a
+         * compressed set of values. This is equivalent to hashing
+         * the MPIDR_EL1 through shifting and ORing. It is a collision free
+         * hash though not minimal since some levels might contain a number
+         * of CPUs that is not an exact power of 2 and their bit
+         * representation might contain holes, eg MPIDR_EL1[7:0] = {0x2, 0x80}.
+         */
+        mpidr_hash.shift_aff[0] = MPIDR_LEVEL_SHIFT(0) + fs[0];
+        mpidr_hash.shift_aff[1] = MPIDR_LEVEL_SHIFT(1) + fs[1] - bits[0];
+        mpidr_hash.shift_aff[2] = MPIDR_LEVEL_SHIFT(2) + fs[2] -
+                                                (bits[1] + bits[0]);
+        mpidr_hash.shift_aff[3] = MPIDR_LEVEL_SHIFT(3) +
+                                  fs[3] - (bits[2] + bits[1] + bits[0]);
+        mpidr_hash.mask = mask;
+        mpidr_hash.bits = bits[3] + bits[2] + bits[1] + bits[0];
+        pr_debug("MPIDR hash: aff0[%u] aff1[%u] aff2[%u] aff3[%u] mask[%#llx] bits[%u]\n",
+                mpidr_hash.shift_aff[0],
+                mpidr_hash.shift_aff[1],
+                mpidr_hash.shift_aff[2],
+                mpidr_hash.shift_aff[3],
+                mpidr_hash.mask,
+                mpidr_hash.bits);
+        /*
+         * 4x is an arbitrary value used to warn on a hash table much bigger
+         * than expected on most systems.
+         */
+        if (mpidr_hash_size() > 4 * num_possible_cpus())
+                pr_warn("Large number of MPIDR hash buckets detected\n");
+        __flush_dcache_area(&mpidr_hash, sizeof(struct mpidr_hash));
+}
+#endif
+
 static void __init setup_processor(void)
 {
         struct cpu_info *cpu_info;
@@ -273,6 +342,7 @@ void __init setup_arch(char **cmdline_p)
         cpu_read_bootcpu_ops();
 #ifdef CONFIG_SMP
         smp_init_cpus();
+        smp_build_mpidr_hash();
 #endif
 
 #ifdef CONFIG_VT
diff --git a/arch/arm64/kernel/sleep.S b/arch/arm64/kernel/sleep.S
new file mode 100644
index 000000000000..b1925729c692
--- /dev/null
+++ b/arch/arm64/kernel/sleep.S
@@ -0,0 +1,184 @@
+#include <linux/errno.h>
+#include <linux/linkage.h>
+#include <asm/asm-offsets.h>
+#include <asm/assembler.h>
+
+        .text
+/*
+ * Implementation of MPIDR_EL1 hash algorithm through shifting
+ * and OR'ing.
+ *
+ * @dst: register containing hash result
+ * @rs0: register containing affinity level 0 bit shift
+ * @rs1: register containing affinity level 1 bit shift
+ * @rs2: register containing affinity level 2 bit shift
+ * @rs3: register containing affinity level 3 bit shift
+ * @mpidr: register containing MPIDR_EL1 value
+ * @mask: register containing MPIDR mask
+ *
+ * Pseudo C-code:
+ *
+ *u32 dst;
+ *
+ *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 rs3, u64 mpidr, u64 mask) {
+ *      u32 aff0, aff1, aff2, aff3;
+ *      u64 mpidr_masked = mpidr & mask;
+ *      aff0 = mpidr_masked & 0xff;
+ *      aff1 = mpidr_masked & 0xff00;
+ *      aff2 = mpidr_masked & 0xff0000;
+ *      aff2 = mpidr_masked & 0xff00000000;
+ *      dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2 | aff3 >> rs3);
+ *}
+ * Input registers: rs0, rs1, rs2, rs3, mpidr, mask
+ * Output register: dst
+ * Note: input and output registers must be disjoint register sets
+         (eg: a macro instance with mpidr = x1 and dst = x1 is invalid)
+ */
+        .macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask
+        and     \mpidr, \mpidr, \mask           // mask out MPIDR bits
+        and     \dst, \mpidr, #0xff             // mask=aff0
+        lsr     \dst ,\dst, \rs0                // dst=aff0>>rs0
+        and     \mask, \mpidr, #0xff00          // mask = aff1
+        lsr     \mask ,\mask, \rs1
+        orr     \dst, \dst, \mask               // dst|=(aff1>>rs1)
+        and     \mask, \mpidr, #0xff0000        // mask = aff2
+        lsr     \mask ,\mask, \rs2
+        orr     \dst, \dst, \mask               // dst|=(aff2>>rs2)
+        and     \mask, \mpidr, #0xff00000000    // mask = aff3
+        lsr     \mask ,\mask, \rs3
+        orr     \dst, \dst, \mask               // dst|=(aff3>>rs3)
+        .endm
+/*
+ * Save CPU state for a suspend.  This saves callee registers, and allocates
+ * space on the kernel stack to save the CPU specific registers + some
+ * other data for resume.
+ *
+ *  x0 = suspend finisher argument
+ */
+ENTRY(__cpu_suspend)
+        stp     x29, lr, [sp, #-96]!
+        stp     x19, x20, [sp,#16]
+        stp     x21, x22, [sp,#32]
+        stp     x23, x24, [sp,#48]
+        stp     x25, x26, [sp,#64]
+        stp     x27, x28, [sp,#80]
+        mov     x2, sp
+        sub     sp, sp, #CPU_SUSPEND_SZ // allocate cpu_suspend_ctx
+        mov     x1, sp
+        /*
+         * x1 now points to struct cpu_suspend_ctx allocated on the stack
+         */
+        str     x2, [x1, #CPU_CTX_SP]
+        ldr     x2, =sleep_save_sp
+        ldr     x2, [x2, #SLEEP_SAVE_SP_VIRT]
+#ifdef CONFIG_SMP
+        mrs     x7, mpidr_el1
+        ldr     x9, =mpidr_hash
+        ldr     x10, [x9, #MPIDR_HASH_MASK]
+        /*
+         * Following code relies on the struct mpidr_hash
+         * members size.
+         */
+        ldp     w3, w4, [x9, #MPIDR_HASH_SHIFTS]
+        ldp     w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
+        compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
+        add     x2, x2, x8, lsl #3
+#endif
+        bl      __cpu_suspend_finisher
+        /*
+         * Never gets here, unless suspend fails.
+         * Successful cpu_suspend should return from cpu_resume, returning
+         * through this code path is considered an error
+         * If the return value is set to 0 force x0 = -EOPNOTSUPP
+         * to make sure a proper error condition is propagated
+         */
+        cmp     x0, #0
+        mov     x3, #-EOPNOTSUPP
+        csel    x0, x3, x0, eq
+        add     sp, sp, #CPU_SUSPEND_SZ // rewind stack pointer
+        ldp     x19, x20, [sp, #16]
+        ldp     x21, x22, [sp, #32]
+        ldp     x23, x24, [sp, #48]
+        ldp     x25, x26, [sp, #64]
+        ldp     x27, x28, [sp, #80]
+        ldp     x29, lr, [sp], #96
+        ret
+ENDPROC(__cpu_suspend)
+        .ltorg
+
+/*
+ * x0 must contain the sctlr value retrieved from restored context
+ */
+ENTRY(cpu_resume_mmu)
+        ldr     x3, =cpu_resume_after_mmu
+        msr     sctlr_el1, x0           // restore sctlr_el1
+        isb
+        br      x3                      // global jump to virtual address
+ENDPROC(cpu_resume_mmu)
+cpu_resume_after_mmu:
+        mov     x0, #0                  // return zero on success
+        ldp     x19, x20, [sp, #16]
+        ldp     x21, x22, [sp, #32]
+        ldp     x23, x24, [sp, #48]
+        ldp     x25, x26, [sp, #64]
+        ldp     x27, x28, [sp, #80]
+        ldp     x29, lr, [sp], #96
+        ret
+ENDPROC(cpu_resume_after_mmu)
+
+        .data
+ENTRY(cpu_resume)
+        bl      el2_setup               // if in EL2 drop to EL1 cleanly
+#ifdef CONFIG_SMP
+        mrs     x1, mpidr_el1
+        adr     x4, mpidr_hash_ptr
+        ldr     x5, [x4]
+        add     x8, x4, x5              // x8 = struct mpidr_hash phys address
+        /* retrieve mpidr_hash members to compute the hash */
+        ldr     x2, [x8, #MPIDR_HASH_MASK]
+        ldp     w3, w4, [x8, #MPIDR_HASH_SHIFTS]
+        ldp     w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
+        compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
+        /* x7 contains hash index, let's use it to grab context pointer */
+#else
+        mov     x7, xzr
+#endif
+        adr     x0, sleep_save_sp
+        ldr     x0, [x0, #SLEEP_SAVE_SP_PHYS]
+        ldr     x0, [x0, x7, lsl #3]
+        /* load sp from context */
+        ldr     x2, [x0, #CPU_CTX_SP]
+        adr     x1, sleep_idmap_phys
+        /* load physical address of identity map page table in x1 */
+        ldr     x1, [x1]
+        mov     sp, x2
+        /*
+         * cpu_do_resume expects x0 to contain context physical address
+         * pointer and x1 to contain physical address of 1:1 page tables
+         */
+        bl      cpu_do_resume           // PC relative jump, MMU off
+        b       cpu_resume_mmu          // Resume MMU, never returns
+ENDPROC(cpu_resume)
+
+        .align 3
+mpidr_hash_ptr:
+        /*
+         * offset of mpidr_hash symbol from current location
+         * used to obtain run-time mpidr_hash address with MMU off
+         */
+        .quad   mpidr_hash - .
+/*
+ * physical address of identity mapped page tables
+ */
+        .type   sleep_idmap_phys, #object
+ENTRY(sleep_idmap_phys)
+        .quad   0
+/*
+ * struct sleep_save_sp {
+ *      phys_addr_t *save_ptr_stash;
+ *      phys_addr_t save_ptr_stash_phys;
+ * };
+ */
+        .type   sleep_save_sp, #object
+ENTRY(sleep_save_sp)
+        .space  SLEEP_SAVE_SP_SZ        // struct sleep_save_sp
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index b5d2031c12c6..1b7617ab499b 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -61,6 +61,7 @@ enum ipi_msg_type {
         IPI_CALL_FUNC,
         IPI_CALL_FUNC_SINGLE,
         IPI_CPU_STOP,
+        IPI_TIMER,
 };
 
 /*
@@ -449,6 +450,7 @@ static const char *ipi_types[NR_IPI] = {
         S(IPI_CALL_FUNC, "Function call interrupts"),
         S(IPI_CALL_FUNC_SINGLE, "Single function call interrupts"),
         S(IPI_CPU_STOP, "CPU stop interrupts"),
+        S(IPI_TIMER, "Timer broadcast interrupts"),
 };
 
 void show_ipi_list(struct seq_file *p, int prec)
@@ -534,6 +536,14 @@ void handle_IPI(int ipinr, struct pt_regs *regs)
                 irq_exit();
                 break;
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+        case IPI_TIMER:
+                irq_enter();
+                tick_receive_broadcast();
+                irq_exit();
+                break;
+#endif
+
         default:
                 pr_crit("CPU%u: Unknown IPI message 0x%x\n", cpu, ipinr);
                 break;
@@ -546,6 +556,13 @@ void smp_send_reschedule(int cpu)
         smp_cross_call(cpumask_of(cpu), IPI_RESCHEDULE);
 }
 
+#ifdef CONFIG_GENERIC_CLOCKEVENTS_BROADCAST
+void tick_broadcast(const struct cpumask *mask)
+{
+        smp_cross_call(mask, IPI_TIMER);
+}
+#endif
+
 void smp_send_stop(void)
 {
         unsigned long timeout;
diff --git a/arch/arm64/kernel/suspend.c b/arch/arm64/kernel/suspend.c
new file mode 100644
index 000000000000..e074b1c32723
--- /dev/null
+++ b/arch/arm64/kernel/suspend.c
@@ -0,0 +1,109 @@
+#include <linux/slab.h>
+#include <asm/cacheflush.h>
+#include <asm/cpu_ops.h>
+#include <asm/debug-monitors.h>
+#include <asm/pgtable.h>
+#include <asm/memory.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
+#include <asm/tlbflush.h>
+
+extern int __cpu_suspend(unsigned long);
+/*
+ * This is called by __cpu_suspend() to save the state, and do whatever
+ * flushing is required to ensure that when the CPU goes to sleep we have
+ * the necessary data available when the caches are not searched.
+ *
+ * @arg: Argument to pass to suspend operations
+ * @ptr: CPU context virtual address
+ * @save_ptr: address of the location where the context physical address
+ *            must be saved
+ */
+int __cpu_suspend_finisher(unsigned long arg, struct cpu_suspend_ctx *ptr,
+                           phys_addr_t *save_ptr)
+{
+        int cpu = smp_processor_id();
+
+        *save_ptr = virt_to_phys(ptr);
+
+        cpu_do_suspend(ptr);
+        /*
+         * Only flush the context that must be retrieved with the MMU
+         * off. VA primitives ensure the flush is applied to all
+         * cache levels so context is pushed to DRAM.
+         */
+        __flush_dcache_area(ptr, sizeof(*ptr));
+        __flush_dcache_area(save_ptr, sizeof(*save_ptr));
+
+        return cpu_ops[cpu]->cpu_suspend(arg);
+}
+
+/**
+ * cpu_suspend
+ *
+ * @arg: argument to pass to the finisher function
+ */
+int cpu_suspend(unsigned long arg)
+{
+        struct mm_struct *mm = current->active_mm;
+        int ret, cpu = smp_processor_id();
+        unsigned long flags;
+
+        /*
+         * If cpu_ops have not been registered or suspend
+         * has not been initialized, cpu_suspend call fails early.
+         */
+        if (!cpu_ops[cpu] || !cpu_ops[cpu]->cpu_suspend)
+                return -EOPNOTSUPP;
+
+        /*
+         * From this point debug exceptions are disabled to prevent
+         * updates to mdscr register (saved and restored along with
+         * general purpose registers) from kernel debuggers.
+         */
+        local_dbg_save(flags);
+
+        /*
+         * mm context saved on the stack, it will be restored when
+         * the cpu comes out of reset through the identity mapped
+         * page tables, so that the thread address space is properly
+         * set-up on function return.
+         */
+        ret = __cpu_suspend(arg);
+        if (ret == 0) {
+                cpu_switch_mm(mm->pgd, mm);
+                flush_tlb_all();
+        }
+
+        /*
+         * Restore pstate flags. OS lock and mdscr have been already
+         * restored, so from this point onwards, debugging is fully
+         * renabled if it was enabled when core started shutdown.
+         */
+        local_dbg_restore(flags);
+
+        return ret;
+}
+
+extern struct sleep_save_sp sleep_save_sp;
+extern phys_addr_t sleep_idmap_phys;
+
+static int cpu_suspend_init(void)
+{
+        void *ctx_ptr;
+
+        /* ctx_ptr is an array of physical addresses */
+        ctx_ptr = kcalloc(mpidr_hash_size(), sizeof(phys_addr_t), GFP_KERNEL);
+
+        if (WARN_ON(!ctx_ptr))
+                return -ENOMEM;
+
+        sleep_save_sp.save_ptr_stash = ctx_ptr;
+        sleep_save_sp.save_ptr_stash_phys = virt_to_phys(ctx_ptr);
+        sleep_idmap_phys = virt_to_phys(idmap_pg_dir);
+        __flush_dcache_area(&sleep_save_sp, sizeof(struct sleep_save_sp));
+        __flush_dcache_area(&sleep_idmap_phys, sizeof(sleep_idmap_phys));
+
+        return 0;
+}
+early_initcall(cpu_suspend_init);
diff --git a/arch/arm64/mm/proc.S b/arch/arm64/mm/proc.S
index 0f7fec52c7f8..bed1f1de1caf 100644
--- a/arch/arm64/mm/proc.S
+++ b/arch/arm64/mm/proc.S
@@ -80,6 +80,75 @@ ENTRY(cpu_do_idle)
         ret
 ENDPROC(cpu_do_idle)
 
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+/**
+ * cpu_do_suspend - save CPU registers context
+ *
+ * x0: virtual address of context pointer
+ */
+ENTRY(cpu_do_suspend)
+        mrs     x2, tpidr_el0
+        mrs     x3, tpidrro_el0
+        mrs     x4, contextidr_el1
+        mrs     x5, mair_el1
+        mrs     x6, cpacr_el1
+        mrs     x7, ttbr1_el1
+        mrs     x8, tcr_el1
+        mrs     x9, vbar_el1
+        mrs     x10, mdscr_el1
+        mrs     x11, oslsr_el1
+        mrs     x12, sctlr_el1
+        stp     x2, x3, [x0]
+        stp     x4, x5, [x0, #16]
+        stp     x6, x7, [x0, #32]
+        stp     x8, x9, [x0, #48]
+        stp     x10, x11, [x0, #64]
+        str     x12, [x0, #80]
+        ret
+ENDPROC(cpu_do_suspend)
+
+/**
+ * cpu_do_resume - restore CPU register context
+ *
+ * x0: Physical address of context pointer
+ * x1: ttbr0_el1 to be restored
+ *
+ * Returns:
+ *      sctlr_el1 value in x0
+ */
+ENTRY(cpu_do_resume)
+        /*
+         * Invalidate local tlb entries before turning on MMU
+         */
+        tlbi    vmalle1
+        ldp     x2, x3, [x0]
+        ldp     x4, x5, [x0, #16]
+        ldp     x6, x7, [x0, #32]
+        ldp     x8, x9, [x0, #48]
+        ldp     x10, x11, [x0, #64]
+        ldr     x12, [x0, #80]
+        msr     tpidr_el0, x2
+        msr     tpidrro_el0, x3
+        msr     contextidr_el1, x4
+        msr     mair_el1, x5
+        msr     cpacr_el1, x6
+        msr     ttbr0_el1, x1
+        msr     ttbr1_el1, x7
+        msr     tcr_el1, x8
+        msr     vbar_el1, x9
+        msr     mdscr_el1, x10
+        /*
+         * Restore oslsr_el1 by writing oslar_el1
+         */
+        ubfx    x11, x11, #1, #1
+        msr     oslar_el1, x11
+        mov     x0, x12
+        dsb     nsh             // Make sure local tlb invalidation completed
+        isb
+        ret
+ENDPROC(cpu_do_resume)
+#endif
+
 /*
  *      cpu_switch_mm(pgd_phys, tsk)
  *