diff --git a/lcpu.c b/lcpu.c
index 4613f84f19f4469cabce462fedd76495249712ba..ab12123d01791f6eb75b76d74d6f87ed204e691f 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_AFF3_SHIFT;
+}
\ No newline at end of file
diff --git a/setup.c b/setup.c
index 41c8787ac8b2fb7683c0cec6b9d723aa47286616..9c1bc8592cf6038f5e2364d9045ad9428ca3fe71 100644
--- a/setup.c
+++ b/setup.c
@@ -22,6 +22,7 @@
 #include <arm/smccc.h>
 #include <stdio.h>
 #include <rk3568/gpio.h>
+#include <arm/arm64/cpu.h>
 
 smccc_conduit_fn_t smccc_psci_call;
 
@@ -151,15 +152,24 @@ 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());
+#ifdef CONFIG_HAVE_SMP
+	rc = lcpu_mp_init(CONFIG_UKPLAT_LCPU_RUN_IRQ,
+					CONFIG_UKPLAT_LCPU_WAKEUP_IRQ,
+					fdtp);
 	if (unlikely(rc))
-		UK_CRASH("Failed to initialize bootstrapping CPU: %d\n", rc);
+		UK_CRASH("SMP initialization failed: %d.\n", rc);
+#endif /* CONFIG_HAVE_SMP */
+
 	/* Initialize PSCI */
 	rc = get_psci_method(bi);
 	if (unlikely(rc < 0))
diff --git a/start.S b/start.S
index 35d8c4a881f8d814abf4a787f99a934a00ed94c8..e6e5c01e13a4921fcee6df33f4394751fc1e747d 100644
--- a/start.S
+++ b/start.S
@@ -18,6 +18,7 @@
 #include <uk/arch/lcpu.h>
 #include <uk/arch/limits.h>
 #include <arm/arm64/pagetable.S>
+#include <uk/plat/offset.h>
 
 #define BOOTSTACK_SIZE	4096
 
@@ -27,24 +28,14 @@
 lcpu_bootstack:
 
 .section ".text.boot"
+.extern bootstrap_cpu_record
 
 .global _start
 _start:
 
 	/* preserve dtb addr */
-	mov 	x25, x0
+	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,6 +94,44 @@ el1_entry:
 	/* Set the context id */
 	msr	contextidr_el1, xzr
 
+	/* Only one cpu can continue executing as the primary core */
+	ldr	x1, =bootstrap_cpu_record
+	ldr	x3, =BOOTSTRAP_CPU_FLAG_OFFSET
+	add	x4, x1, x3
+
+try_acquire_flag:
+	ldaxr	w2, [x4]
+	cbz	w2, set_bootstrap_flag
+	clrex
+
+hang:
+	/* If the cpus did not acquire the flag, hang here */
+	wfe
+
+set_bootstrap_flag:
+	mov	w3, #1
+	/* Try to store 1 to the flag with release semantics */
+	stlxr	w2, w3, [x4]
+	/* Fail to set flag to 1, try again */
+	cbnz	x2, try_acquire_flag
+
+	/* some core alreay acquire the flag, store its hardware ID
+	 * But before that, convert it into format like aff3/aff2/aff1/aff0
+	 */
+	mrs	x2, mpidr_el1
+	mov	w3, w2
+	and	x3, x3, #MPIDR_AFF2_MASK|MPIDR_AFF1_MASK|MPIDR_AFF0_MASK
+	and	x2, x2, #MPIDR_AFF3_MASK
+	orr	x3, x3, x2, LSR #MPIDR_AFF3_SHIFT
+	ldr	x2, =BOOTSTRAP_CPU_PHYSICAL_ID_OFFSET
+	add	x4, x1, x2
+	str	x3, [x4]
+	/* If lcpus pointer not supported before lcpu_init() in setup.c,
+	 * uk_pr_debug() will crash, so we support it manually for bootstrap cpu here.
+	 */
+	ldr	x0, =lcpus
+	msr	tpidr_el1, x0
+
 	/* Load dtb address to x0 as a parameter */
 	mov	x0, x25