From 5390a66bdf021e75ec17456b8b5934d0ae9fbc6c Mon Sep 17 00:00:00 2001 From: Peng Fan Date: Fri, 2 Jun 2017 16:07:41 +0800 Subject: [PATCH] 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 --- core/arch/arm/plat-imx/imx-common.c | 50 +++++++++++++++++++++ core/arch/arm/plat-imx/imx-regs.h | 4 ++ core/arch/arm/plat-imx/imx.h | 33 ++++++++++++++ core/arch/arm/plat-imx/imx6.c | 1 - core/arch/arm/plat-imx/psci.c | 69 +++++++++++++++++++++++++++++ core/arch/arm/plat-imx/sub.mk | 2 +- 6 files changed, 157 insertions(+), 2 deletions(-) create mode 100644 core/arch/arm/plat-imx/imx-common.c create mode 100644 core/arch/arm/plat-imx/imx.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 00000000000..953d7d1ab27 --- /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 + * + * 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 +#include +#include +#include +#include +#include + +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 ed88157cb6e..2938d9075cb 100644 --- a/core/arch/arm/plat-imx/imx-regs.h +++ b/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)) diff --git a/core/arch/arm/plat-imx/imx.h b/core/arch/arm/plat-imx/imx.h new file mode 100644 index 00000000000..86757235b27 --- /dev/null +++ b/core/arch/arm/plat-imx/imx.h @@ -0,0 +1,33 @@ +/* + * Copyright 2017 NXP + * All rights reserved. + * + * Peng Fan + * + * 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 + +uint32_t imx_get_src_gpr(int cpu); +void imx_set_src_gpr(int cpu, uint32_t val); diff --git a/core/arch/arm/plat-imx/imx6.c b/core/arch/arm/plat-imx/imx6.c index 58a1a257c13..cfac00c8d7d 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 ff36ba0b1be..4e0af210038 100644 --- a/core/arch/arm/plat-imx/psci.c +++ b/core/arch/arm/plat-imx/psci.c @@ -29,7 +29,10 @@ #include #include #include +#include +#include #include +#include #include #include #include @@ -41,6 +44,7 @@ #include #include +#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 diff --git a/core/arch/arm/plat-imx/sub.mk b/core/arch/arm/plat-imx/sub.mk index 55087532f28..dba76d7b95e 100644 --- a/core/arch/arm/plat-imx/sub.mk +++ b/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