core: arm: imx support psci off and affinity

Support psci off and affinity.
To i.MX6, CPU could not offline itself, so needs to use core0 to
offline other cores.

Introduce imx-common.c to include the common code for i.MX family,
SRC operation is used by i.MX6/7, so move them to imx-common.c

Use CFG_BOOT_SECONDARY_REQUEST to wrap the psci_cpu_on/off/affinity
functions, these functions are only needed by SMP systems.To i.MX6UL,
they are not needed.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
Acked-by: Jens Wiklander <jens.wiklander@linaro.org>

7 files changed, 159 insertions, 2 deletions

diff --git a/core/arch/arm/include/arm32.h b/core/arch/arm/include/arm32.h
index c37ab647..b73d853f 100644
--- a/core/arch/arm/include/arm32.h
+++ b/core/arch/arm/include/arm32.h
@@ -658,6 +658,11 @@ static inline uint64_t read_pmu_ccnt(void)
 	asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r"(val));
 	return val;
 }
+
+static inline void wfi(void)
+{
+	asm volatile("wfi");
+}
 #endif /*ASM*/
 
 #endif /*ARM32_H*/
diff --git a/core/arch/arm/plat-imx/imx-common.c b/core/arch/arm/plat-imx/imx-common.c
new file mode 100644
index 00000000..953d7d1a
--- /dev/null
+++ b/core/arch/arm/plat-imx/imx-common.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * Copyright 2017 NXP
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <console.h>
+#include <io.h>
+#include <imx.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+
+uint32_t imx_get_src_gpr(int cpu)
+{
+	vaddr_t va = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
+
+	return read32(va + SRC_GPR1 + cpu * 8 + 4);
+}
+
+void imx_set_src_gpr(int cpu, uint32_t val)
+{
+	vaddr_t va = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
+
+	write32(val, va + SRC_GPR1 + cpu * 8 + 4);
+}
diff --git a/core/arch/arm/plat-imx/imx-regs.h b/core/arch/arm/plat-imx/imx-regs.h
index 909c3d53..2b180761 100644
--- a/core/arch/arm/plat-imx/imx-regs.h
+++ b/core/arch/arm/plat-imx/imx-regs.h
@@ -155,4 +155,9 @@
 #else
 #error "CFG_MX6 not defined"
 #endif
+
+#define IOMUXC_GPR4_OFFSET	0x10
+#define IOMUXC_GPR5_OFFSET	0x14
+#define ARM_WFI_STAT_MASK(n)	BIT(n)
+
 #endif
diff --git a/core/arch/arm/plat-imx/imx.h b/core/arch/arm/plat-imx/imx.h
new file mode 100644
index 00000000..144ac1d7
--- /dev/null
+++ b/core/arch/arm/plat-imx/imx.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright 2017 NXP
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef PLAT_IMX_IMX_H
+#define PLAT_IMX_IMX_H
+
+#include <stdint.h>
+
+uint32_t imx_get_src_gpr(int cpu);
+void imx_set_src_gpr(int cpu, uint32_t val);
+#endif
diff --git a/core/arch/arm/plat-imx/imx6.c b/core/arch/arm/plat-imx/imx6.c
index 58a1a257..cfac00c8 100644
--- a/core/arch/arm/plat-imx/imx6.c
+++ b/core/arch/arm/plat-imx/imx6.c
@@ -76,4 +76,3 @@ void plat_cpu_reset_late(void)
 			write32(read32(addr) | CSU_SETTING_LOCK, addr);
 	}
 }
-
diff --git a/core/arch/arm/plat-imx/psci.c b/core/arch/arm/plat-imx/psci.c
index ff36ba0b..a0a70e0b 100644
--- a/core/arch/arm/plat-imx/psci.c
+++ b/core/arch/arm/plat-imx/psci.c
@@ -29,7 +29,10 @@
 #include <console.h>
 #include <drivers/imx_uart.h>
 #include <io.h>
+#include <imx.h>
+#include <imx-regs.h>
 #include <kernel/generic_boot.h>
+#include <kernel/misc.h>
 #include <kernel/panic.h>
 #include <kernel/pm_stubs.h>
 #include <mm/core_mmu.h>
@@ -41,6 +44,7 @@
 #include <tee/entry_std.h>
 #include <tee/entry_fast.h>
 
+#ifdef CFG_BOOT_SECONDARY_REQUEST
 int psci_cpu_on(uint32_t core_idx, uint32_t entry,
 		uint32_t context_id __attribute__((unused)))
 {
@@ -67,3 +71,60 @@ int psci_cpu_on(uint32_t core_idx, uint32_t entry,
 
 	return PSCI_RET_SUCCESS;
 }
+
+int psci_cpu_off(void)
+{
+	uint32_t core_id;
+
+	core_id = get_core_pos();
+
+	DMSG("core_id: %" PRIu32, core_id);
+
+	psci_armv7_cpu_off();
+
+	imx_set_src_gpr(core_id, UINT32_MAX);
+
+	thread_mask_exceptions(THREAD_EXCP_ALL);
+
+	while (true)
+		wfi();
+
+	return PSCI_RET_INTERNAL_FAILURE;
+}
+
+int psci_affinity_info(uint32_t affinity,
+		       uint32_t lowest_affnity_level __unused)
+{
+	vaddr_t va = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
+	vaddr_t gpr5 = core_mmu_get_va(IOMUXC_BASE, MEM_AREA_IO_SEC) +
+				       IOMUXC_GPR5_OFFSET;
+	uint32_t cpu, val;
+	bool wfi;
+
+	cpu = affinity;
+
+	wfi = read32(gpr5) & ARM_WFI_STAT_MASK(cpu);
+
+	if ((imx_get_src_gpr(cpu) == 0) || !wfi)
+		return PSCI_AFFINITY_LEVEL_ON;
+
+	DMSG("cpu: %" PRIu32 "GPR: %" PRIx32, cpu, imx_get_src_gpr(cpu));
+	/*
+	 * Wait secondary cpus ready to be killed
+	 * TODO: Change to non dead loop
+	 */
+	while (read32(va + SRC_GPR1 + cpu * 8 + 4) != UINT32_MAX)
+		;
+
+	/* Kill cpu */
+	val = read32(va + SRC_SCR);
+	val &= ~BIT32(SRC_SCR_CORE1_ENABLE_OFFSET + cpu - 1);
+	val |=  BIT32(SRC_SCR_CORE1_RST_OFFSET + cpu - 1);
+	write32(val, va + SRC_SCR);
+
+	/* Clean arg */
+	imx_set_src_gpr(cpu, 0);
+
+	return PSCI_AFFINITY_LEVEL_OFF;
+}
+#endif
diff --git a/core/arch/arm/plat-imx/sub.mk b/core/arch/arm/plat-imx/sub.mk
index 55087532..3eb68064 100644
--- a/core/arch/arm/plat-imx/sub.mk
+++ b/core/arch/arm/plat-imx/sub.mk
@@ -1,8 +1,9 @@
 global-incdirs-y += .
-srcs-y += main.c
+srcs-y += main.c imx-common.c
 
 srcs-$(CFG_PL310) += imx_pl310.c
 srcs-$(CFG_PSCI_ARM32) += psci.c
+cflags-psci.c-y += -Wno-suggest-attribute=noreturn
 
 ifneq (,$(filter y, $(CFG_MX6Q) $(CFG_MX6D) $(CFG_MX6DL)))
 srcs-y += a9_plat_init.S imx6.c