plat: imx: add gic save/restore for imx8qm/qxp

In order to save power when AP side is suspend,
the DBLOG need to be put into low power mode.
GIC need to be power off to save power. before GIC
power off, we need to save the GIC setting, then
after resume, we need to restore the gic setting.

irqsteer need to be used to wakeup the AP side
when wakeup interrupt is pending for AP side.

Signed-off-by: Bai Ping <ping.bai@nxp.com>

8 files changed, 108 insertions, 17 deletions

diff --git a/plat/imx/imx8qm/imx8qm_bl31_setup.c b/plat/imx/imx8qm/imx8qm_bl31_setup.c
index f47ccfbb..173fe274 100644
--- a/plat/imx/imx8qm/imx8qm_bl31_setup.c
+++ b/plat/imx/imx8qm/imx8qm_bl31_setup.c
@@ -359,6 +359,9 @@ void bl31_plat_arch_setup(void)
 			MT_DEVICE | MT_RW);
 	mmap_add_region(PLAT_CCI_BASE, PLAT_CCI_BASE, 0x10000,
 			MT_DEVICE | MT_RW);
+	mmap_add_region(IMX_WUP_IRQSTR, IMX_WUP_IRQSTR, 0x10000,
+			MT_DEVICE | MT_RW);
+
 #if USE_COHERENT_MEM
 	mmap_add_region(BL31_COHERENT_RAM_BASE, BL31_COHERENT_RAM_BASE,
 		BL31_COHERENT_RAM_LIMIT - BL31_COHERENT_RAM_BASE,
diff --git a/plat/imx/imx8qm/imx8qm_psci.c b/plat/imx/imx8qm/imx8qm_psci.c
index d96a6cde..65ec8989 100644
--- a/plat/imx/imx8qm/imx8qm_psci.c
+++ b/plat/imx/imx8qm/imx8qm_psci.c
@@ -33,12 +33,15 @@
 #include <arch_helpers.h>
 #include <cci.h>
 #include <debug.h>
+#include <gicv3.h>
+#include <mmio.h>
 #include <plat_imx8.h>
 #include <psci.h>
 #include <sci/sci.h>
 
 extern sc_ipc_t ipc_handle;
-extern void mdelay(uint32_t msec);
+/* save gic dist/redist context when GIC is poewr down */
+static struct plat_gic_ctx imx_gicv3_ctx;
 
 const unsigned char imx_power_domain_tree_desc[] =
 {
@@ -61,6 +64,27 @@ static unsigned int a53_cpu_on_number __section("tzfw_coherent_mem");
 static unsigned int a72_cpu_on_number __section("tzfw_coherent_mem");
 #endif
 
+static void imx_enable_irqstr_wakeup(void)
+{
+	uint32_t irq_mask;
+	gicv3_dist_ctx_t *dist_ctx = &imx_gicv3_ctx.dist_ctx;
+
+	/* enable the irqsteer to handle wakeup irq */
+	mmio_write_32(IMX_WUP_IRQSTR, 0x1);
+	for (int i = 0; i < 15; i++) {
+		irq_mask = dist_ctx->gicd_isenabler[i];
+		mmio_write_32(IMX_WUP_IRQSTR + 0x3c - 0x4 * i, irq_mask);
+	}
+}
+
+static void imx_disable_irqstr_wakeup(void)
+{
+	/* disable the irqsteer */
+	mmio_write_32(IMX_WUP_IRQSTR, 0x0);
+	for (int i = 0; i < 16; i ++)
+		mmio_write_32(IMX_WUP_IRQSTR + 0x4 + 0x4 * i, 0x0);
+}
+
 int imx_pwr_domain_on(u_register_t mpidr)
 {
 	int ret = PSCI_E_SUCCESS;
@@ -201,17 +225,24 @@ void imx_domain_suspend(const psci_power_state_t *target_state)
 
 	plat_gic_cpuif_disable();
 
+	/* save gic context */
+	plat_gic_save(cpu_id, &imx_gicv3_ctx);
+	/* enable the irqsteer for wakeup */
+	imx_enable_irqstr_wakeup();
+
 	cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(mpidr));
 
 	/* Put GIC in LP mode. */
-	sc_pm_set_resource_power_mode(ipc_handle, SC_R_GIC, SC_PM_PW_MODE_LP);
+	sc_pm_set_resource_power_mode(ipc_handle, SC_R_GIC, SC_PM_PW_MODE_OFF);
+	/* Put IRQSTR in STBY mode */
+	sc_pm_set_resource_power_mode(ipc_handle, SC_R_IRQSTR_SCU2, SC_PM_PW_MODE_STBY);
 
 	if (cluster_id == 0) {
 		sc_pm_set_cpu_resume_addr(ipc_handle, ap_core_index[cpu_id], 0x080000000);
-		sc_pm_req_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_OFF);
+		sc_pm_req_cpu_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_OFF, SC_PM_WAKE_SRC_IRQSTEER);
 	} else {
 		sc_pm_set_cpu_resume_addr(ipc_handle, ap_core_index[cpu_id + 4], 0x080000000);
-		sc_pm_req_low_power_mode(ipc_handle, ap_core_index[cpu_id + 4], SC_PM_PW_MODE_OFF);
+		sc_pm_req_cpu_low_power_mode(ipc_handle, ap_core_index[cpu_id + 4], SC_PM_PW_MODE_OFF, SC_PM_WAKE_SRC_IRQSTEER);
 	}
 }
 
@@ -222,15 +253,21 @@ void imx_domain_suspend_finish(const psci_power_state_t *target_state)
 	unsigned int cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
 
 	if (cluster_id == 0)
-		sc_pm_req_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_ON);
+		sc_pm_req_cpu_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_ON, SC_PM_WAKE_SRC_GIC);
 	else
-		sc_pm_req_low_power_mode(ipc_handle, ap_core_index[cpu_id + 4], SC_PM_PW_MODE_ON);
+		sc_pm_req_cpu_low_power_mode(ipc_handle, ap_core_index[cpu_id + 4], SC_PM_PW_MODE_ON, SC_PM_WAKE_SRC_GIC);
 
-	/* Put GIC back to high power mode. */
+	/* Put GIC/IRQSTR back to high power mode. */
 	sc_pm_set_resource_power_mode(ipc_handle, SC_R_GIC, SC_PM_PW_MODE_ON);
+	sc_pm_set_resource_power_mode(ipc_handle, SC_R_IRQSTR_SCU2, SC_PM_PW_MODE_ON);
 
 	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(mpidr));
 
+	/* restore gic context */
+	plat_gic_restore(cpu_id, &imx_gicv3_ctx);
+	/* disable the irqsteer wakeup */
+	imx_disable_irqstr_wakeup();
+
 	plat_gic_cpuif_enable();
 }
 
@@ -297,10 +334,10 @@ int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 	/* Request RUN and LP modes for DDR, system interconnect etc. */
 	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A53, SC_PM_SYS_IF_DDR, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
 	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A72, SC_PM_SYS_IF_DDR, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
-	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A53, SC_PM_SYS_IF_MU, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_LP);
-	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A72, SC_PM_SYS_IF_MU, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_LP);
-	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A53, SC_PM_SYS_IF_INTERCONNECT, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_OFF);
-	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A72, SC_PM_SYS_IF_INTERCONNECT, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_OFF);
+	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A53, SC_PM_SYS_IF_MU, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
+	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A72, SC_PM_SYS_IF_MU, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
+	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A53, SC_PM_SYS_IF_INTERCONNECT, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
+	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A72, SC_PM_SYS_IF_INTERCONNECT, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
 
 	return 0;
 }
diff --git a/plat/imx/imx8qm/include/platform_def.h b/plat/imx/imx8qm/include/platform_def.h
index 21a27b1f..54fe5394 100644
--- a/plat/imx/imx8qm/include/platform_def.h
+++ b/plat/imx/imx8qm/include/platform_def.h
@@ -72,8 +72,9 @@
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
 
 #define MAX_XLAT_TABLES			8
-#define MAX_MMAP_REGIONS		8
+#define MAX_MMAP_REGIONS		12
 
+#define IMX_WUP_IRQSTR			0x51090000
 #define IMX_BOOT_UART_BASE		0x5a060000
 #define IMX_BOOT_UART_BAUDRATE		115200
 #define IMX_BOOT_UART_CLK_IN_HZ		24000000
diff --git a/plat/imx/imx8qm/platform.mk b/plat/imx/imx8qm/platform.mk
index 954e7575..8a29aef5 100644
--- a/plat/imx/imx8qm/platform.mk
+++ b/plat/imx/imx8qm/platform.mk
@@ -33,6 +33,8 @@ PLAT_INCLUDES		:=	-Iplat/imx/imx8qm/include		\
 				-Iplat/imx/common/include		\
 
 PLAT_GIC_SOURCES	:=	drivers/arm/gic/v3/gicv3_helpers.c	\
+				drivers/arm/gic/v3/arm_gicv3_common.c   \
+				drivers/arm/gic/v3/gic500.c             \
 				drivers/arm/gic/v3/gicv3_main.c		\
 				drivers/arm/gic/common/gic_common.c	\
 				plat/common/plat_gicv3.c		\
diff --git a/plat/imx/imx8qxp/imx8qxp_bl31_setup.c b/plat/imx/imx8qxp/imx8qxp_bl31_setup.c
index 0044d113..d4c4ddcd 100644
--- a/plat/imx/imx8qxp/imx8qxp_bl31_setup.c
+++ b/plat/imx/imx8qxp/imx8qxp_bl31_setup.c
@@ -346,6 +346,9 @@ void bl31_plat_arch_setup(void)
 			MT_DEVICE | MT_RW);
 //	mmap_add_region(IMX_GPT0_BASE, IMX_GPT0_BASE, 0x10000,
 //			MT_DEVICE | MT_RW);
+	mmap_add_region(IMX_WUP_IRQSTR, IMX_WUP_IRQSTR, 0x10000,
+			MT_DEVICE | MT_RW);
+
 #if USE_COHERENT_MEM
 	mmap_add_region(BL31_COHERENT_RAM_BASE, BL31_COHERENT_RAM_BASE,
 		BL31_COHERENT_RAM_LIMIT - BL31_COHERENT_RAM_BASE,
diff --git a/plat/imx/imx8qxp/imx8qxp_psci.c b/plat/imx/imx8qxp/imx8qxp_psci.c
index 64e00780..1c00c61b 100644
--- a/plat/imx/imx8qxp/imx8qxp_psci.c
+++ b/plat/imx/imx8qxp/imx8qxp_psci.c
@@ -32,6 +32,8 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <debug.h>
+#include <gicv3.h>
+#include <mmio.h>
 #include <plat_imx8.h>
 #include <psci.h>
 #include <sci/sci.h>
@@ -39,6 +41,9 @@
 extern sc_ipc_t ipc_handle;
 extern void mdelay(uint32_t msec);
 
+/* save gic dist/redist context when GIC is power down */
+static struct plat_gic_ctx imx_gicv3_ctx;
+
 const unsigned char imx_power_domain_tree_desc[] =
 {
 	/* number of root nodes */
@@ -52,6 +57,27 @@ const static int ap_core_index[PLATFORM_CORE_COUNT] = {
 	SC_R_A35_0, SC_R_A35_1, SC_R_A35_2, SC_R_A35_3
 };
 
+static void imx_enable_irqstr_wakeup(void)
+{
+	uint32_t irq_mask;
+	gicv3_dist_ctx_t *dist_ctx = &imx_gicv3_ctx.dist_ctx;
+
+	/* enable the irqsteer to handle wakeup irq */
+	mmio_write_32(IMX_WUP_IRQSTR, 0x1);
+	for (int i = 0; i < 15; i++) {
+		irq_mask = dist_ctx->gicd_isenabler[i];
+		mmio_write_32(IMX_WUP_IRQSTR + 0x3c - 0x4 * i, irq_mask);
+	}
+}
+
+static void imx_disable_irqstr_wakeup(void)
+{
+	/* disable the irqsteer */
+	mmio_write_32(IMX_WUP_IRQSTR, 0x0);
+	for (int i = 0; i < 16; i ++)
+		mmio_write_32(IMX_WUP_IRQSTR + 0x4 + 0x4 * i, 0x0);
+}
+
 plat_local_state_t plat_get_target_pwr_state(unsigned int lvl,
 				const plat_local_state_t *target_state,
 				unsigned int ncpu)
@@ -167,11 +193,18 @@ void imx_domain_suspend(const psci_power_state_t *target_state)
 
 	plat_gic_cpuif_disable();
 
-	/* Put GIC in LP mode. */
-	sc_pm_set_resource_power_mode(ipc_handle, SC_R_GIC, SC_PM_PW_MODE_LP);
+	/* save gic context */
+	plat_gic_save(cpu_id, &imx_gicv3_ctx);
+	/* enable the irqsteer for wakeup */
+	imx_enable_irqstr_wakeup();
+
+	/* Put GIC in OFF mode. */
+	sc_pm_set_resource_power_mode(ipc_handle, SC_R_GIC, SC_PM_PW_MODE_OFF);
+	/* put IRQSTR in STBY mode */
+	sc_pm_set_resource_power_mode(ipc_handle, SC_R_IRQSTR_SCU2, SC_PM_PW_MODE_STBY);
 
 	sc_pm_set_cpu_resume_addr(ipc_handle, ap_core_index[cpu_id], 0x080000000);
-	sc_pm_req_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_OFF);
+	sc_pm_req_cpu_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_OFF, SC_PM_WAKE_SRC_IRQSTEER);
 }
 
 void imx_domain_suspend_finish(const psci_power_state_t *target_state)
@@ -180,9 +213,18 @@ void imx_domain_suspend_finish(const psci_power_state_t *target_state)
 	unsigned int cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
 
 	sc_pm_req_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_ON);
+	sc_pm_req_cpu_low_power_mode(ipc_handle, ap_core_index[cpu_id], SC_PM_PW_MODE_ON, SC_PM_WAKE_SRC_GIC);
 
 	/* Put GIC back to high power mode. */
 	sc_pm_set_resource_power_mode(ipc_handle, SC_R_GIC, SC_PM_PW_MODE_ON);
+	/* Put IRQSTEER back to high power mode */
+	sc_pm_set_resource_power_mode(ipc_handle, SC_R_IRQSTR_SCU2, SC_PM_PW_MODE_ON);
+
+	/* restore gic context */
+	plat_gic_restore(cpu_id, &imx_gicv3_ctx);
+	/* disable the irqsteer wakeup */
+	imx_disable_irqstr_wakeup();
+
 	plat_gic_cpuif_enable();
 }
 
@@ -238,8 +280,8 @@ int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 
 	/* Request RUN and LP modes for DDR, system interconnect etc. */
 	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A35, SC_PM_SYS_IF_DDR, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
-	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A35, SC_PM_SYS_IF_MU, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_LP);
-	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A35, SC_PM_SYS_IF_INTERCONNECT, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_OFF);
+	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A35, SC_PM_SYS_IF_MU, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
+	sc_pm_req_sys_if_power_mode(ipc_handle, SC_R_A35, SC_PM_SYS_IF_INTERCONNECT, SC_PM_PW_MODE_ON, SC_PM_PW_MODE_STBY);
 
 	return 0;
 }
diff --git a/plat/imx/imx8qxp/include/platform_def.h b/plat/imx/imx8qxp/include/platform_def.h
index 7955e60f..57a351da 100644
--- a/plat/imx/imx8qxp/include/platform_def.h
+++ b/plat/imx/imx8qxp/include/platform_def.h
@@ -66,6 +66,7 @@
 #define MAX_XLAT_TABLES			8
 #define MAX_MMAP_REGIONS		8
 
+#define IMX_WUP_IRQSTR			0x51090000
 #define IMX_BOOT_UART_BASE		0x5a060000
 #define IMX_BOOT_UART_BAUDRATE		115200
 #define IMX_BOOT_UART_CLK_IN_HZ		24000000
diff --git a/plat/imx/imx8qxp/platform.mk b/plat/imx/imx8qxp/platform.mk
index bf0a0a84..47ced36c 100644
--- a/plat/imx/imx8qxp/platform.mk
+++ b/plat/imx/imx8qxp/platform.mk
@@ -33,6 +33,8 @@ PLAT_INCLUDES		:=	-Iplat/imx/imx8qxp/include		\
 				-Iplat/imx/common/include		\
 
 PLAT_GIC_SOURCES	:=	drivers/arm/gic/v3/gicv3_helpers.c	\
+				drivers/arm/gic/v3/arm_gicv3_common.c	\
+				drivers/arm/gic/v3/gic500.c		\
 				drivers/arm/gic/v3/gicv3_main.c		\
 				drivers/arm/gic/common/gic_common.c	\
 				plat/common/plat_gicv3.c		\