From 1d40eb880a89810206a70219f0106f4f4c85d216 Mon Sep 17 00:00:00 2001 From: Joseph Chen Date: Mon, 10 Jul 2017 18:12:57 +0800 Subject: [PATCH 1/4] core: arm: sm: add PSCI system suspend Add __weak property for the function, developers could have their own implementation. Signed-off-by: Joseph Chen Acked-by: Jerome Forissier Acked-by: Etienne Carriere --- core/arch/arm/include/sm/psci.h | 1 + core/arch/arm/sm/psci.c | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/core/arch/arm/include/sm/psci.h b/core/arch/arm/include/sm/psci.h index a5601560a21..9b8eb4c1d41 100644 --- a/core/arch/arm/include/sm/psci.h +++ b/core/arch/arm/include/sm/psci.h @@ -55,6 +55,7 @@ void psci_system_off(void); void psci_system_reset(void); int psci_features(uint32_t psci_fid); int psci_node_hw_state(uint32_t cpu_id, uint32_t power_level); +int psci_system_suspend(uintptr_t entry, uint32_t context_id); int psci_stat_residency(uint32_t cpu_id, uint32_t power_state); int psci_stat_count(uint32_t cpu_id, uint32_t power_state); void tee_psci_handler(struct thread_smc_args *args); diff --git a/core/arch/arm/sm/psci.c b/core/arch/arm/sm/psci.c index 32a897c229c..b50dc3bab33 100644 --- a/core/arch/arm/sm/psci.c +++ b/core/arch/arm/sm/psci.c @@ -99,6 +99,12 @@ __weak int psci_node_hw_state(uint32_t cpu_id __unused, return PSCI_RET_NOT_SUPPORTED; } +__weak int psci_system_suspend(uintptr_t entry __unused, + uint32_t context_id __unused) +{ + return PSCI_RET_NOT_SUPPORTED; +} + __weak int psci_stat_residency(uint32_t cpu_id __unused, uint32_t power_state __unused) { @@ -159,6 +165,9 @@ void tee_psci_handler(struct thread_smc_args *args) case PSCI_NODE_HW_STATE: args->a0 = psci_node_hw_state(a1, a2); break; + case PSCI_SYSTEM_SUSPEND: + args->a0 = psci_system_suspend(a1, a2); + break; default: args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION; break; From 00d47cf014f85a91c266a8d695366ba17cae0e9f Mon Sep 17 00:00:00 2001 From: Joseph Chen Date: Wed, 26 Jul 2017 20:20:45 +0800 Subject: [PATCH 2/4] plat-rockchip: rk322x: add PSCI version Improve PSCI version to PSCI_VERSION_1_0. Signed-off-by: Joseph Chen Acked-by: Jerome Forissier Acked-by: Etienne Carriere --- core/arch/arm/plat-rockchip/psci_rk322x.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/arch/arm/plat-rockchip/psci_rk322x.c b/core/arch/arm/plat-rockchip/psci_rk322x.c index 55068c545d4..231c0e8526c 100644 --- a/core/arch/arm/plat-rockchip/psci_rk322x.c +++ b/core/arch/arm/plat-rockchip/psci_rk322x.c @@ -67,6 +67,11 @@ static bool core_held_in_reset(uint32_t core) return val & CORE_HELD_IN_RESET(core); } +uint32_t psci_version(void) +{ + return PSCI_VERSION_1_0; +} + int psci_cpu_on(uint32_t core_idx, uint32_t entry, uint32_t context_id __unused) { From 7922149f3708e1cc18e3a57e87bac7f30af17ae4 Mon Sep 17 00:00:00 2001 From: Joseph Chen Date: Wed, 26 Jul 2017 20:23:26 +0800 Subject: [PATCH 3/4] plat-rockchip: rk322x: add PSCI features Add currently implemented PSCI functions. Signed-off-by: Joseph Chen Acked-by: Jerome Forissier Acked-by: Etienne Carriere --- core/arch/arm/plat-rockchip/psci_rk322x.c | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/core/arch/arm/plat-rockchip/psci_rk322x.c b/core/arch/arm/plat-rockchip/psci_rk322x.c index 231c0e8526c..b50a0cd26c1 100644 --- a/core/arch/arm/plat-rockchip/psci_rk322x.c +++ b/core/arch/arm/plat-rockchip/psci_rk322x.c @@ -72,6 +72,20 @@ uint32_t psci_version(void) return PSCI_VERSION_1_0; } +int psci_features(uint32_t psci_fid) +{ + switch (psci_fid) { + case PSCI_PSCI_FEATURES: + case PSCI_VERSION: + case PSCI_CPU_ON: + case PSCI_CPU_OFF: + case PSCI_SYSTEM_RESET: + return PSCI_RET_SUCCESS; + default: + return PSCI_RET_NOT_SUPPORTED; + } +} + int psci_cpu_on(uint32_t core_idx, uint32_t entry, uint32_t context_id __unused) { From 0892b0ad3b04d57b05fbf340734323386b31d89a Mon Sep 17 00:00:00 2001 From: Joseph Chen Date: Wed, 26 Jul 2017 20:27:02 +0800 Subject: [PATCH 4/4] plat-rockchip: rk322x: add PSCI system suspend Support gating clks and power down PLLs. Signed-off-by: Joseph Chen Acked-by: Jerome Forissier Acked-by: Etienne Carriere --- core/arch/arm/plat-rockchip/common.h | 13 ++ core/arch/arm/plat-rockchip/cru.h | 26 +++ core/arch/arm/plat-rockchip/psci_rk322x.c | 192 ++++++++++++++++++++++ 3 files changed, 231 insertions(+) diff --git a/core/arch/arm/plat-rockchip/common.h b/core/arch/arm/plat-rockchip/common.h index 18b7a2eea68..6ee74ce3a9b 100644 --- a/core/arch/arm/plat-rockchip/common.h +++ b/core/arch/arm/plat-rockchip/common.h @@ -33,4 +33,17 @@ #define LOCK_ADDR_OFFSET 4 #define BOOT_ADDR_OFFSET 8 +/* + * Some register has write-mask bits, it means if you want to set the bits, + * you need set the write-mask bits at the same time, the write-mask bits is + * in high 16-bits. The following macro definition helps you access register + * efficiently. + */ +#define REG_MSK_SHIFT 16 +#define WMSK_BIT(nr) BIT((nr) + REG_MSK_SHIFT) +#define BIT_WITH_WMSK(nr) (BIT(nr) | WMSK_BIT(nr)) +#define BITS_WMSK(msk, shift) SHIFT_U32(msk, (shift) + REG_MSK_SHIFT) +#define BITS_WITH_WMASK(bits, msk, shift) \ + (SHIFT_U32(bits, shift) | BITS_WMSK(msk, shift)) + #endif diff --git a/core/arch/arm/plat-rockchip/cru.h b/core/arch/arm/plat-rockchip/cru.h index 624b4dc9a98..62f0b1c9daf 100644 --- a/core/arch/arm/plat-rockchip/cru.h +++ b/core/arch/arm/plat-rockchip/cru.h @@ -28,9 +28,19 @@ #ifndef PLAT_ROCKCHIP_CRU_H #define PLAT_ROCKCHIP_CRU_H +#include #include #if defined(PLATFORM_FLAVOR_rk322x) + +enum plls_id { + APLL_ID, + DPLL_ID, + CPLL_ID, + GPLL_ID, + PLL_END, +}; + #define CRU_SOFTRST_CON(i) (0x110 + ((i) * 4)) #define CRU_MODE_CON 0x040 #define CRU_GLBRST_CFG_BASE 0x140 @@ -44,6 +54,22 @@ #define CORE_SOFT_RELEASE(core) SHIFT_U32(0x100000, (core)) #define CORE_HELD_IN_RESET(core) SHIFT_U32(0x000010, (core)) #define NONBOOT_CORES_SOFT_RESET 0x00e000e0 + +#define CRU_CLKGATE_CON_CNT 16 +#define CRU_CLKSEL_CON(i) (0x044 + ((i) * 4)) +#define CRU_CLKGATE_CON(i) (0x0d0 + ((i) * 4)) +#define CRU_PLL_CON0(pll) ((pll) * 0x0c + 0x0) +#define CRU_PLL_CON1(pll) ((pll) * 0x0c + 0x4) +#define CRU_PLL_CON2(pll) ((pll) * 0x0c + 0x8) + +#define PLL_LOCK BIT(10) +#define PLL_POWER_UP BITS_WITH_WMASK(0, 1, 13) +#define PLL_POWER_DOWN BITS_WITH_WMASK(1, 1, 13) + +#define PLL_MODE_BIT(pll) ((pll) * 4) +#define PLL_MODE_MSK(pll) BIT(PLL_MODE_BIT(pll)) +#define PLL_SLOW_MODE(pll) BITS_WITH_WMASK(0, 1, PLL_MODE_BIT(pll)) +#define PLL_NORM_MODE(pll) BITS_WITH_WMASK(1, 1, PLL_MODE_BIT(pll)) #endif #endif diff --git a/core/arch/arm/plat-rockchip/psci_rk322x.c b/core/arch/arm/plat-rockchip/psci_rk322x.c index b50a0cd26c1..d5b03bbdbfe 100644 --- a/core/arch/arm/plat-rockchip/psci_rk322x.c +++ b/core/arch/arm/plat-rockchip/psci_rk322x.c @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,178 @@ #include #include +struct dram_data { + uint32_t cru_mode_con; + uint32_t cru_clksel0; + uint32_t cru_clksel1; + uint32_t cru_clksel10; + uint32_t cru_clksel21; + uint32_t cru_clkgate[CRU_CLKGATE_CON_CNT]; +}; + +static struct dram_data dram_d; + +static const uint32_t clks_gating_table[CRU_CLKGATE_CON_CNT] = { + /* gate: 0-3 */ + 0xefb8, + 0x0ff7, + 0xfff4, + 0x887f, + /* gate: 4-7 */ + 0x0030, + 0x00f8, + 0x07e0, + 0xc000, + /* gate: 8-11 */ + 0xff84, + 0xb047, + 0x1ca0, + 0x57ff, + /* gate: 12-15 */ + 0x0000, + 0x00ff, + 0x1cc0, + 0x000f, +}; + +static void clks_disable(void) +{ + uint32_t i; + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + for (i = 0; i < CRU_CLKGATE_CON_CNT; i++) { + dram_d.cru_clkgate[i] = read32(va_base + CRU_CLKGATE_CON(i)); + write32(BITS_WITH_WMASK(clks_gating_table[i], 0xffff, 0), + va_base + CRU_CLKGATE_CON(i)); + } +} + +static void clks_restore(void) +{ + uint32_t i; + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + for (i = 0; i < CRU_CLKGATE_CON_CNT; i++) + write32(BITS_WITH_WMASK(dram_d.cru_clkgate[i], 0xffff, 0), + va_base + CRU_CLKGATE_CON(i)); +} + +static void pll_power_down(uint32_t pll) +{ + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + write32(PLL_SLOW_MODE(pll), va_base + CRU_MODE_CON); + write32(PLL_POWER_DOWN, va_base + CRU_PLL_CON1(pll)); +} + +static void pll_power_up(uint32_t pll) +{ + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + write32(PLL_POWER_UP, va_base + CRU_PLL_CON1(pll)); +} + +static void pll_wait_lock(uint32_t pll) +{ + uint32_t loop = 0; + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + while (!(read32(va_base + CRU_PLL_CON1(pll)) & PLL_LOCK) && + (loop < 500)) { + udelay(2); + loop++; + } + + if (!(read32(va_base + CRU_PLL_CON1(pll)) & PLL_LOCK)) { + EMSG("PLL can't lock, index = %" PRIu32, pll); + panic(); + } +} + +/* + * Select clock from external 24MHz OSC(slow mode) and power down plls, + * then set frequency division of relevant bus to 24MHz. + */ +static void plls_power_down(void) +{ + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + dram_d.cru_clksel0 = read32(va_base + CRU_CLKSEL_CON(0)); + dram_d.cru_clksel1 = read32(va_base + CRU_CLKSEL_CON(1)); + dram_d.cru_clksel10 = read32(va_base + CRU_CLKSEL_CON(10)); + dram_d.cru_clksel21 = read32(va_base + CRU_CLKSEL_CON(21)); + dram_d.cru_mode_con = read32(va_base + CRU_MODE_CON); + + pll_power_down(GPLL_ID); + pll_power_down(CPLL_ID); + pll_power_down(APLL_ID); + + /* core */ + write32(BITS_WITH_WMASK(0, 0x1f, 0), va_base + CRU_CLKSEL_CON(0)); + write32(BITS_WITH_WMASK(0, 0xf, 0) | BITS_WITH_WMASK(0, 0x7, 4), + va_base + CRU_CLKSEL_CON(1)); + + /* peri aclk, hclk, pclk */ + write32(BITS_WITH_WMASK(0, 0x1f, 0) | BITS_WITH_WMASK(0, 0x3, 8) | + BITS_WITH_WMASK(0, 0x7, 12), + va_base + CRU_CLKSEL_CON(10)); + + /* pdbus */ + write32(BITS_WITH_WMASK(0, 0x1f, 8), va_base + CRU_CLKSEL_CON(0)); + write32(BITS_WITH_WMASK(0, 0x3, 8) | BITS_WITH_WMASK(0, 0x7, 12), + va_base + CRU_CLKSEL_CON(1)); + + /* hdmi cec 32k */ + write32(BITS_WITH_WMASK(732, 0x3fff, 0) | BITS_WITH_WMASK(2, 0x3, 14), + va_base + CRU_CLKSEL_CON(21)); +} + +static void plls_restore(void) +{ + vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); + + /* power up plls */ + pll_power_up(APLL_ID); + pll_power_up(GPLL_ID); + pll_power_up(CPLL_ID); + + udelay(200); + + /* wait lock*/ + pll_wait_lock(APLL_ID); + pll_wait_lock(GPLL_ID); + pll_wait_lock(CPLL_ID); + + /* hdmi cec 32k */ + write32(dram_d.cru_clksel21 | BITS_WMSK(0x3fff, 0) | BITS_WMSK(0x3, 14), + va_base + CRU_CLKSEL_CON(21)); + + /* pdbus */ + write32(dram_d.cru_clksel0 | BITS_WMSK(0x1f, 8), + va_base + CRU_CLKSEL_CON(0)); + write32(dram_d.cru_clksel1 | BITS_WMSK(0x3, 8) | BITS_WMSK(0x7, 12), + va_base + CRU_CLKSEL_CON(1)); + + /* peri aclk, hclk, pclk */ + write32(dram_d.cru_clksel10 | BITS_WMSK(0x1f, 0) | BITS_WMSK(0x3, 8) | + BITS_WMSK(0x7, 12), + va_base + CRU_CLKSEL_CON(10)); + + /* core */ + write32(dram_d.cru_clksel0 | BITS_WMSK(0x1f, 0), + va_base + CRU_CLKSEL_CON(0)); + write32(dram_d.cru_clksel1 | BITS_WMSK(0xf, 0) | BITS_WMSK(0x7, 4), + va_base + CRU_CLKSEL_CON(1)); + + /* resume plls mode */ + write32(dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(APLL_ID)), + va_base + CRU_MODE_CON); + write32(dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(CPLL_ID)), + va_base + CRU_MODE_CON); + write32(dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(GPLL_ID)), + va_base + CRU_MODE_CON); +} + static bool wait_core_wfe_i(uint32_t core) { uint32_t wfei_mask, loop = 0; @@ -79,6 +252,7 @@ int psci_features(uint32_t psci_fid) case PSCI_VERSION: case PSCI_CPU_ON: case PSCI_CPU_OFF: + case PSCI_SYSTEM_SUSPEND: case PSCI_SYSTEM_RESET: return PSCI_RET_SUCCESS; default: @@ -185,6 +359,24 @@ void psci_system_reset(void) dsb(); } +int psci_system_suspend(uintptr_t entry __unused, + uint32_t context_id __unused) +{ + DMSG("system suspend"); + + clks_disable(); + plls_power_down(); + + cache_op_inner(DCACHE_CLEAN_INV, NULL, 0); + + wfi(); + + plls_restore(); + clks_restore(); + + return PSCI_RET_SUCCESS; +} + /* When SMP bootup, we release cores one by one */ static TEE_Result reset_nonboot_cores(void) {