diff --git a/core/opal.c b/core/opal.c
index bb71ac37..f4142b04 100644
--- a/core/opal.c
+++ b/core/opal.c
@@ -512,6 +512,9 @@ static int64_t opal_register_os_ops(struct opal_os_ops *ops, uint64_t size)
 		vm_resurrect();
 	}
 
+	if (size >= 32)
+		os_ops.os_idle_stop = (void *) be64_to_cpu(ops->os_idle_stop);
+
 	checksum_romem();
 
 	opal_v4_os = true;
@@ -556,6 +559,93 @@ void os_vm_unmap(uint64_t ea)
 		mtmsrd(cpu->opal_call_msr, 0);
 }
 
+struct p9_sprs {
+	/* per thread sprs that get lost in shallow states */
+	u64 amr;
+	u64 iamr;
+	u64 amor;
+	u64 uamor;
+};
+
+/*
+ * Opal function to handle idle stop in kernel.
+ */
+static int64_t opal_cpu_idle(uint64_t srr1_addr, uint64_t psscr)
+{
+	int pvr;
+	u64 mmcra;
+	u64 mmcr0 = 0;
+	struct p9_sprs sprs = {};
+	u64 *le_srr1 = (u64 *)be64_to_cpu(srr1_addr);
+
+	if (!os_ops.os_idle_stop)
+		return OPAL_UNSUPPORTED;
+
+	if (proc_gen != proc_gen_p9)
+		return OPAL_UNSUPPORTED;
+
+	/* Deep states are not supported for opal fallback */
+	if ((psscr & OPAL_PM_PSSCR_RL_MASK) >= 4)
+		return OPAL_UNSUPPORTED;
+
+	pvr = mfspr(SPR_PVR);
+	if (!(psscr & (OPAL_PM_PSSCR_EC|OPAL_PM_PSSCR_ESL))) {
+		*le_srr1 = os_ops.os_idle_stop(srr1_addr, psscr);
+		goto out;
+	}
+
+	/* EC=ESL=1 case */
+	if (PVR_VERS_MAJ(pvr) == 2 && (PVR_VERS_MIN(pvr) == 0))
+		/*
+		 * POWER9 DD2 can incorrectly set PMAO when waking up
+		 * after a state-loss idle. Saving and restoring MMCR0
+		 * over idle is a workaround.
+		 */
+		mmcr0   = mfspr(SPR_MMCR0);
+
+	/* Save sprs lost in shallow state */
+	sprs.amr	= mfspr(SPR_AMR);
+	sprs.iamr	= mfspr(SPR_IAMR);
+	sprs.amor	= mfspr(SPR_AMOR);
+	sprs.uamor	= mfspr(SPR_UAMOR);
+
+	*le_srr1 = os_ops.os_idle_stop(srr1_addr, psscr);
+
+	if ((*le_srr1 & SPR_SRR1_PM_WAKE_MASK) != SPR_SRR1_PM_WAKE_NOLOSS) {
+
+		mtspr(SPR_AMR,		sprs.amr);
+		mtspr(SPR_IAMR,		sprs.iamr);
+		mtspr(SPR_AMOR,		sprs.amor);
+		mtspr(SPR_UAMOR,	sprs.uamor);
+		/*
+		 * Workaround for POWER9 DD2.0, if we lost resources, the ERAT
+		 * might have been corrupted and needs flushing. We also need
+		 * to reload MMCR0 (see mmcr0 comment above).
+		 */
+		if (PVR_VERS_MAJ(pvr) == 2 && (PVR_VERS_MIN(pvr) == 0)) {
+			/* Handle PPC_ISA_3_0_INVALIDATE_ERAT */
+			asm volatile (".long 0x7c1003e4" : : : "memory");
+			mtspr(SPR_MMCR0, mmcr0);
+		}
+
+		/*
+		 * DD2.2 and earlier need to set then clear bit 60 in MMCRA
+		 * to ensure the PMU starts running.
+		 */
+		if (PVR_VERS_MAJ(pvr) == 2 && (PVR_VERS_MIN(pvr) <= 2)) {
+			mmcra = mfspr(SPR_MMCRA);
+			mmcra |= PPC_BIT(60);
+			mtspr(SPR_MMCRA, mmcra);
+			mmcra &= ~PPC_BIT(60);
+			mtspr(SPR_MMCRA, mmcra);
+		}
+	}
+
+out:
+	return OPAL_SUCCESS;
+}
+opal_call(OPAL_CPU_IDLE, opal_cpu_idle, 2);
+
 void add_opal_node(void)
 {
 	uint64_t base, entry, size;
diff --git a/include/opal-api.h b/include/opal-api.h
index 3c630f02..08429d72 100644
--- a/include/opal-api.h
+++ b/include/opal-api.h
@@ -232,7 +232,8 @@
 #define OPAL_REPORT_TRAP			183
 #define OPAL_FIND_VM_AREA			184
 #define OPAL_REGISTER_OS_OPS			185
-#define OPAL_LAST				185
+#define OPAL_CPU_IDLE				186
+#define OPAL_LAST				186
 
 #define QUIESCE_HOLD			1 /* Spin all calls at entry */
 #define QUIESCE_REJECT			2 /* Fail all calls with OPAL_BUSY */
@@ -1280,7 +1281,7 @@ struct opal_os_ops {
         __be64  os_printf;      /* void printf(int32_t level, const char *str) */
         __be64  os_vm_map;      /* void os_vm_map(uint64_t ea, uint64_t pa, uint64_t flags) */
         __be64  os_vm_unmap;    /* static void os_vm_unmap(uint64_t ea) */
-
+	__be64	os_idle_stop;	/* void os_idle_stop(uint64_t srr1_addr, uint64_t psscr) */
 };
 
 #endif /* __ASSEMBLY__ */
diff --git a/include/opal-internal.h b/include/opal-internal.h
index af521663..63f1c10c 100644
--- a/include/opal-internal.h
+++ b/include/opal-internal.h
@@ -22,6 +22,7 @@ struct os_ops {
         void (*os_printf)(uint32_t log_level, const char *str);
         int64_t (*os_vm_map)(uint64_t ea, uint64_t pa, uint64_t flags);
         void (*os_vm_unmap)(uint64_t ea);
+	int64_t (*os_idle_stop)(uint64_t srr1_addr, uint64_t psscr);
 };
 
 extern bool opal_v4_os;
@@ -30,6 +31,7 @@ extern struct os_ops os_ops;
 extern void os_printf(uint32_t log_level, const char *str);
 extern int64_t os_vm_map(uint64_t ea, uint64_t pa, uint64_t flags);
 extern void os_vm_unmap(uint64_t ea);
+extern int64_t os_idle_stop(uint64_t srr1_addr, uint64_t psscr);
 
 #ifdef __CHECKER__
 #define __opal_func_test_arg(__func, __nargs) 0
diff --git a/include/processor.h b/include/processor.h
index 9d197ffc..860f3fb3 100644
--- a/include/processor.h
+++ b/include/processor.h
@@ -77,12 +77,15 @@
 #define SPR_HID4	0x3f4
 #define SPR_HID5	0x3f6
 #define SPR_PIR		0x3ff	/* RO: Processor Identification */
+#define SPR_MMCR0	795
+#define SPR_MMCRA	0x312
 
 /* Bits in SRR1 */
 
 #define SPR_SRR1_PM_WAKE_MASK	0x3c0000	/* PM wake reason for P8/9 */
 #define SPR_SRR1_PM_WAKE_SRESET	0x100000
 #define SPR_SRR1_PM_WAKE_MCE	0x3c0000	/* Use reserved value for MCE */
+#define SPR_SRR1_PM_WAKE_NOLOSS	0x100000
 
 /* Bits in DSISR */
 
