From 1eefa7f4af27a201b1dba5fe79a7d8f1f7fbd920 Mon Sep 17 00:00:00 2001
From: cheng2000160 <caicheng@yingyitech.net>
Date: Fri, 21 Jun 2024 10:57:56 +0800
Subject: [PATCH] plat/rk3568: Adapts to the modification of the
 tn_bootstrap_cpu

1. use pick_bootstrap_cpu from plat/common to determine bootstrap cpu,
   not rely register MPIDR value
2. use uk_alloc() to allocate lcpu for cpu0

Signed-off-by: cheng2000160 <caicheng@yingyitech.net>
---
 Makefile.uk |  2 +-
 lcpu.c      |  5 +++++
 setup.c     |  9 +++++----
 start.S     | 18 +++++-------------
 4 files changed, 16 insertions(+), 18 deletions(-)

diff --git a/Makefile.uk b/Makefile.uk
index cd0a723..ead6bb6 100644
--- a/Makefile.uk
+++ b/Makefile.uk
@@ -48,7 +48,7 @@ LIBRK3568PLAT_SRCS-$(CONFIG_FPSIMD)              += $(UK_PLAT_COMMON_BASE)/arm/f
 ifeq ($(CONFIG_PAGING),y)
 LIBRK3568PLAT_SRCS-$(CONFIG_ARCH_ARM_64) += $(UK_PLAT_COMMON_BASE)/paging.c|isr
 endif
-
+LIBRK3568PLAT_SRCS-$(CONFIG_ARCH_ARM_64) += $(UK_PLAT_COMMON_BASE)/arm/lcpu_start.S
 LIBRK3568PLAT_SRCS-y		 += $(UK_PLAT_COMMON_BASE)/bootinfo.c|common
 LIBRK3568PLAT_SRCS-$(CONFIG_LIBFDT) += $(UK_PLAT_COMMON_BASE)/bootinfo_fdt.c|common
 LIBRK3568PLAT_SRCS-y		 += $(UK_PLAT_COMMON_BASE)/bootinfo.lds.S|common
diff --git a/lcpu.c b/lcpu.c
index 4613f84..146f47d 100644
--- a/lcpu.c
+++ b/lcpu.c
@@ -58,3 +58,8 @@ void ukplat_lcpu_irqs_handle_pending(void)
 {
 	// TODO
 }
+
+unsigned int tnplat_lcpu_physical_index(__lcpuid_p cpuid)
+{
+	return (cpuid & MPIDR_AFF1_MASK) >> MPIDR_AFF1_SHIFT;
+}
\ No newline at end of file
diff --git a/setup.c b/setup.c
index 41c8787..cae48c5 100644
--- a/setup.c
+++ b/setup.c
@@ -151,15 +151,16 @@ void _librk3568plat_entry(void *fdtp)
 	enforce_w_xor_x();
 #endif /* CONFIG_ENFORCE_W_XOR_X && CONFIG_PAGING */
 
+	/* Initialize logical boot CPU */
+	rc = lcpu_init(lcpu_alloc(get_bootstrap_cpu_physical_id()));
+	if (unlikely(rc))
+		UK_CRASH("Failed to initialize bootstrapping CPU: %d\n", rc);
+
 	/* Initialize interrupt controller */
 	rc = uk_intctlr_probe();
 	if (unlikely(rc))
 		UK_CRASH("Could not initialize the IRQ controller: %d\n", rc);
 
-	/* Initialize logical boot CPU */
-	rc = lcpu_init(lcpu_get_bsp());
-	if (unlikely(rc))
-		UK_CRASH("Failed to initialize bootstrapping CPU: %d\n", rc);
 	/* Initialize PSCI */
 	rc = get_psci_method(bi);
 	if (unlikely(rc < 0))
diff --git a/start.S b/start.S
index 35d8c4a..732adb1 100644
--- a/start.S
+++ b/start.S
@@ -34,17 +34,6 @@ _start:
 	/* preserve dtb addr */
 	mov 	x25, x0
 
-	// read cpu id, stop slave cores
-	mrs     x1, mpidr_el1
-	and     x1, x1, #3
-	cbz     x1, master
-
-// If the cpu id is > 0, hang here
-hang:  wfe
-    	b       hang
-
-// Continue if cpu id == 0
-master:
 	// disable mmu and cache
 	mrs	x2, sctlr_el1
 	mov	x3, #SCTLR_EL1_M_BIT|SCTLR_EL1_C_BIT
@@ -103,10 +92,13 @@ el1_entry:
 	/* Set the context id */
 	msr	contextidr_el1, xzr
 
+	bl	pick_bootstrap_cpu
+
+	ldr	x0, =lcpus
+	msr	tpidr_el1, x0
+
 	/* Load dtb address to x0 as a parameter */
 	mov	x0, x25
 
 	bl	_librk3568plat_entry
 
-	/* As a failsafe, we also hang the main core */
-	b	hang
-- 
Gitee