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.

Introuduce imx-common.c to include the common code for i.MX family,
SRC operation is use 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>

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);
+}

core/arch/arm/plat-imx/imx-regs.h

@@ -153,3 +153,7 @@
 #else
 #error "CFG_MX6 not defined"
 #endif
+
+#define IOMUXC_GPR4_OFFSET	0x10
+#define IOMUXC_GPR5_OFFSET	0x14
+#define ARM_WFI_STAT_MASK(n)	(1 << (n))

core/arch/arm/plat-imx/imx.h

@@ -0,0 +1,33 @@
+/*
+ * 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 <stdint.h>
+
+uint32_t imx_get_src_gpr(int cpu);
+void imx_set_src_gpr(int cpu, uint32_t val);

core/arch/arm/plat-imx/imx6.c

@@ -76,4 +76,3 @@ void plat_cpu_reset_late(void)
 			write32(read32(addr) | CSU_SETTING_LOCK, addr);
 	}
 }
-

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,68 @@ int psci_cpu_on(uint32_t core_idx, uint32_t entry,
 
 	return PSCI_RET_SUCCESS;
 }
+
+int psci_cpu_off(void)
+{
+	uint32_t core_id;
+	vaddr_t va = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
+
+	core_id = get_core_pos();
+
+	write32(~0, va + SRC_GPR1 + core_id * 8 + 4);
+
+	DMSG("%d\n", core_id);
+	psci_armv7_cpu_off();
+
+	imx_set_src_gpr(core_id, ~0);
+
+	asm volatile (
+		"mrs	r0, cpsr\r\n"
+		"orr	r0, r0, #(1 << 6)\r\n"
+		"orr	r0, r0, #(1 << 7)\r\n"
+		"msr	cpsr, r0\r\n"
+		"loop:\r\n"
+		"wfi\r\n"
+		" b loop\r\n");
+
+	return PSCI_RET_SUCCESS;
+}
+
+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;
+	uint32_t 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("%d %x\n", 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_t)~0)
+		;
+
+	/* 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 */
+	write32(0, va + SRC_GPR1 + cpu * 8 + 4);
+
+	imx_set_src_gpr(cpu, 0);
+
+	return PSCI_AFFINITY_LEVEL_OFF;
+}
+#endif

core/arch/arm/plat-imx/sub.mk

@@ -1,5 +1,5 @@
 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