From 3e45797b2790492e2863441e24246536a33f1efd Mon Sep 17 00:00:00 2001
From: XiaoDong Huang <derrick.huang@rock-chips.com>
Date: Mon, 17 Jun 2024 10:55:27 +0800
Subject: [PATCH] feat(rk3588): enable crypto function

The CPU crypto is not default on when power up, need to enable it by
software.

Signed-off-by: XiaoDong Huang <derrick.huang@rock-chips.com>
Change-Id: Ifee2eab55d9c13cef5f15926fb80016845e2a66d
---

diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index dfb14e9..6d1e11e 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -16,6 +16,11 @@
 	.globl	bl31_entrypoint
 	.globl	bl31_warm_entrypoint
 
+#ifdef PLAT_RK_BL31_ENTRYPOINT
+	.globl	plat_rockchip_bl31_entrypoint
+	.globl	plat_rockchip_bl31_entrypoint_set_sp
+#endif
+
 	/* -----------------------------------------------------
 	 * bl31_entrypoint() is the cold boot entrypoint,
 	 * executed only by the primary cpu.
@@ -23,6 +28,10 @@
 	 */
 
 func bl31_entrypoint
+#ifdef PLAT_RK_BL31_ENTRYPOINT
+	bl	plat_rockchip_bl31_entrypoint_set_sp
+	bl	plat_rockchip_bl31_entrypoint
+#endif
 	/* ---------------------------------------------------------------
 	 * Stash the previous bootloader arguments x0 - x3 for later use.
 	 * ---------------------------------------------------------------
diff --git a/plat/rockchip/rk3588/drivers/pmu/pmu.c b/plat/rockchip/rk3588/drivers/pmu/pmu.c
index 83d6cad..a7c1c47 100644
--- a/plat/rockchip/rk3588/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3588/drivers/pmu/pmu.c
@@ -136,6 +136,22 @@
 
 static __pmusramfunc void ddr_resume(void)
 {
+	/* check the crypto function had been enabled or not */
+	if (mmio_read_32(DSUSGRF_BASE + DSU_SGRF_SOC_CON(4)) & BIT(4)) {
+		/* enable the crypto function */
+		mmio_write_32(DSUSGRF_BASE + DSU_SGRF_SOC_CON(4), BITS_WITH_WMASK(0, 0x1, 4));
+		dsb();
+		isb();
+
+		__asm__ volatile ("mov	x0, #3\n"
+				  "dsb	sy\n"
+				  "msr	rmr_el3, x0\n"
+				  "1:\n"
+				  "isb\n"
+				  "wfi\n"
+				  "b 1b\n");
+	}
+
 	dsu_restore_early();
 }
 
@@ -1437,3 +1453,66 @@
 
 	pm_reg_rgns_init();
 }
+
+void bl31_entrypoint(void);
+
+static uint64_t boot_cpu_save[4];
+
+void plat_rockchip_bl31_entrypoint_set_sp(uint64_t reg0, uint64_t reg1,
+					  uint64_t reg2, uint64_t reg3)
+{
+	__asm__ volatile("mov x10, %0\n" : : "r" (reg0) : );
+
+	reg0 = PSRAM_SP_TOP;
+	__asm__ volatile("mov sp, %0\n" : : "r" (reg0) : );
+
+	__asm__ volatile("mov %0, x10\n" : "=r" (reg0) : :);
+}
+
+void plat_rockchip_bl31_entrypoint(uint64_t reg0, uint64_t reg1,
+				   uint64_t reg2, uint64_t reg3)
+{
+	uint32_t tmp = mmio_read_32(DSUSGRF_BASE + DSU_SGRF_SOC_CON(4));
+
+	/* check the crypto function had been enabled or not */
+	if (tmp & BIT(4)) {
+		/* save x0~x3 */
+		boot_cpu_save[0] = reg0;
+		boot_cpu_save[1] = reg1;
+		boot_cpu_save[2] = reg2;
+		boot_cpu_save[3] = reg3;
+
+		/* enable the crypto function */
+		mmio_write_32(DSUSGRF_BASE + DSU_SGRF_SOC_CON(4), BITS_WITH_WMASK(0, 0x1, 4));
+
+		/* remap pmusram to 0xffff0000 */
+		mmio_write_32(PMU0SGRF_BASE + PMU0_SGRF_SOC_CON(2), 0x00030001);
+		dsb();
+		psram_sleep_cfg->pm_flag = PM_WARM_BOOT_BIT;
+		cpuson_flags[0] = PMU_CPU_HOTPLUG;
+		cpuson_entry_point[0] = (uintptr_t)bl31_entrypoint;
+		dsb();
+
+		/* to enable the crypto function, must reset the core0 */
+		__asm__ volatile ("mov	x0, #3\n"
+				  "dsb	sy\n"
+				  "msr	rmr_el3, x0\n"
+				  "1:\n"
+				  "isb\n"
+				  "wfi\n"
+				  "b 1b\n");
+	} else {
+		/* remap bootrom to 0xffff0000 */
+		mmio_write_32(PMU0SGRF_BASE + PMU0_SGRF_SOC_CON(2), 0x00030000);
+
+		/*
+		 * the crypto function has been enabled,
+		 * restore the x0~x3.
+		 */
+		__asm__ volatile ("ldr	x0, [%0]\n"
+				  "ldr	x1, [%0 , 0x8]\n"
+				  "ldr	x2, [%0 , 0x10]\n"
+				  "ldr	x3, [%0 , 0x18]\n"
+				  : : "r" (&boot_cpu_save[0]));
+	}
+}
diff --git a/plat/rockchip/rk3588/platform.mk b/plat/rockchip/rk3588/platform.mk
index 07eda40..3d9dc59 100644
--- a/plat/rockchip/rk3588/platform.mk
+++ b/plat/rockchip/rk3588/platform.mk
@@ -96,3 +96,4 @@
 
 $(eval $(call add_define,PLAT_EXTRA_LD_SCRIPT))
 $(eval $(call add_define,PLAT_SKIP_DFS_TLB_DCACHE_MAINTENANCE))
+$(eval $(call add_define,PLAT_RK_BL31_ENTRYPOINT))