From ef0df95a3682d331dd331ae6a809c7b6b1568b6b Mon Sep 17 00:00:00 2001
From: lr <1234@qq.com>
Date: Mon, 8 Jul 2024 17:42:59 +0800
Subject: [PATCH] boot cpu0 and 1 successfully

---
 .../hardkernel/arch/arm/armv8-a/cortex-a55/core.h    |  2 +-
 .../arm/armv8-a/cortex-a55/preboot_for_3568/boot.S   | 12 ++++++------
 .../arm/armv8-a/cortex-a55/preboot_for_3568/smp.c    |  2 +-
 .../hardkernel/mmu/arm/armv8-a/cortex-a55/bootmmu.c  |  6 +++++-
 4 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/core.h b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/core.h
index c9667450f..d3259cab0 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/core.h
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/core.h
@@ -73,7 +73,7 @@ Modification:
 
 #include "cortex_a55.h"
 
-#define NR_CPU 4 // maximum number of CPUs
+#define NR_CPU 2 // maximum number of CPUs
 
 __attribute__((always_inline)) static inline uint64_t EL0_mode() // Set ARM mode to EL0
 {
diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S
index 198a893e5..524e19b6c 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/boot.S
@@ -36,7 +36,8 @@ _boot_start:
 
     // get cpu id, and subtract the offset from the stacks base address
     mrs     x2,     mpidr_el1
-    and     x2,     x2, #0x3
+    and     x2,     x2, #0xFFF
+    lsr     x2,     x2, #8
     mov     x5,     x2
     mul     x3,     x2, x1
     sub     x0,     x0, x3
@@ -45,11 +46,10 @@ _boot_start:
     mov     x2,         #ARM_MODE_EL1_h | DIS_INT  
     msr     spsr_el1,   x2
 
-    // bl el2_setup
-
     // check cpu id - cpu0 is primary cpu
     mrs     x2,     mpidr_el1
-    and     x2,     x2, #0x3
+    and     x2,     x2, #0xFFF
+    lsr     x2,     x2, #8
     mov     x5,     x2
     cmp     x5, 	#0
     beq     primary_cpu_init
@@ -66,7 +66,7 @@ primary_cpu_init:
 1:  
     cmp     x1,	x2
     stp     x3,     x3, [x1], #16
-    b.lt     1b    
+    b.lt     1b  
 
 	bl bootmain
 
@@ -117,4 +117,4 @@ el2_setup:
     eret
     .endfunc
 
-    .end
+    .end  
\ No newline at end of file
diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/smp.c b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/smp.c
index 69774f78c..7b73bd73a 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/smp.c
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv8-a/cortex-a55/preboot_for_3568/smp.c
@@ -75,7 +75,7 @@ static struct arm_smccc_res __invoke_sip_fn_smc(unsigned long function_id,
 void cpu_start_secondary(uint8_t cpu_id)
 {
     //psci_call(PSCI_CPUON, cpu_id, (uintptr_t)&_boot_start, 0);
-    __invoke_sip_fn_smc(PSCI_CPUON, cpu_id, (uintptr_t)&__print, 0);
+    __invoke_sip_fn_smc(PSCI_CPUON, cpu_id, (uintptr_t)0xa00000, 0);
 }
 
 
diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/mmu/arm/armv8-a/cortex-a55/bootmmu.c b/Ubiquitous/XiZi_AIoT/hardkernel/mmu/arm/armv8-a/cortex-a55/bootmmu.c
index 2e818362e..f05989867 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/mmu/arm/armv8-a/cortex-a55/bootmmu.c
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/mmu/arm/armv8-a/cortex-a55/bootmmu.c
@@ -253,7 +253,11 @@ extern void main(void);
 static bool _bss_inited = false;
 void bootmain()
 {
-    // __print();
+    __print();
+    // if(_bss_inited){
+    //     unsigned int* p = (unsigned int*)0xffffffff;
+    //     *p = 0;
+    // }
     build_boot_pgdir();
     load_boot_pgdir();
     __asm__ __volatile__("add sp, sp, %0" ::"r"(KERN_OFFSET));