Rk322x adds PSCI version, features and system suspend

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

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

core/arch/arm/plat-rockchip/cru.h

@@ -28,9 +28,19 @@
 #ifndef PLAT_ROCKCHIP_CRU_H
 #define PLAT_ROCKCHIP_CRU_H
 
+#include <common.h>
 #include <platform_config.h>
 
 #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

core/arch/arm/plat-rockchip/psci_rk322x.c

@@ -34,6 +34,7 @@
 #include <kernel/delay.h>
 #include <kernel/generic_boot.h>
 #include <kernel/misc.h>
+#include <kernel/panic.h>
 #include <mm/core_mmu.h>
 #include <mm/core_memprot.h>
 #include <platform_config.h>
@@ -43,6 +44,178 @@
 #include <tee/entry_std.h>
 #include <tee/entry_fast.h>
 
+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;
@@ -67,6 +240,26 @@ 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_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_SUSPEND:
+	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)
 {
@@ -166,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)
 {

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;