From 9b92c71a211767ecc343566da064d3ff130126ec Mon Sep 17 00:00:00 2001
From: TXuian <1163589503@qq.com>
Date: Tue, 12 Mar 2024 16:31:21 +0800
Subject: [PATCH] Start supporting smp.

---
 .../preboot_for_imx6q-sabrelite/boot.S          |  7 +++----
 .../cortex-a9/imx6q-sabrelite/trap_common.c     |  6 ++++++
 .../cortex-a9/zynq7000-zc702/trap_common.c      |  6 ++++++
 .../XiZi_AIoT/hardkernel/intr/trap_common.h     |  1 +
 Ubiquitous/XiZi_AIoT/softkernel/main.c          | 17 +++++++++++++----
 5 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/preboot_for_imx6q-sabrelite/boot.S b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/preboot_for_imx6q-sabrelite/boot.S
index d3f824508..2f5ab4867 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/preboot_for_imx6q-sabrelite/boot.S
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/preboot_for_imx6q-sabrelite/boot.S
@@ -84,11 +84,10 @@ _boot_start:
     @ check cpu id - cpu0 is primary cpu
     cmp     r5, 	#0
     beq     primary_cpu_init
-    bx      r4     @ for secondary cpus, jump to argument function pointer passed in by ROM
-    
-    @ control should not return from the secondary cpu entry point
-    b       .
+    bl      bootmain     @ for secondary cpus, jump to argument function pointer passed in by ROM
 
+    bl      .
+    
 primary_cpu_init:
    	/* init .bss */
     /* clear the .bss section (zero init) */
diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/imx6q-sabrelite/trap_common.c b/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/imx6q-sabrelite/trap_common.c
index 3e1acbc0d..8cb244967 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/imx6q-sabrelite/trap_common.c
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/imx6q-sabrelite/trap_common.c
@@ -213,8 +213,14 @@ static int _is_interruptable(void)
     return !(val & DIS_INT);
 }
 
+int _cur_cpu_id()
+{
+    return cpu_get_current();
+}
+
 static struct XiziTrapDriver xizi_trap_driver = {
     .sys_irq_init = _sys_irq_init,
+    .cur_cpu_id = _cur_cpu_id,
 
     .cpu_irq_enable = _cpu_irq_enable,
     .cpu_irq_disable = _cpu_irq_disable,
diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/zynq7000-zc702/trap_common.c b/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/zynq7000-zc702/trap_common.c
index 4d06f20f4..de89cbd21 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/zynq7000-zc702/trap_common.c
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/zynq7000-zc702/trap_common.c
@@ -215,8 +215,14 @@ static int _is_interruptable(void)
     return !(val & DIS_INT);
 }
 
+int _cur_cpu_id()
+{
+    return cpu_get_current();
+}
+
 static struct XiziTrapDriver xizi_trap_driver = {
     .sys_irq_init = _sys_irq_init,
+    .cur_cpu_id = _cur_cpu_id,
 
     .cpu_irq_enable = _cpu_irq_enable,
     .cpu_irq_disable = _cpu_irq_disable,
diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/intr/trap_common.h b/Ubiquitous/XiZi_AIoT/hardkernel/intr/trap_common.h
index 327f856b1..19c6831e0 100644
--- a/Ubiquitous/XiZi_AIoT/hardkernel/intr/trap_common.h
+++ b/Ubiquitous/XiZi_AIoT/hardkernel/intr/trap_common.h
@@ -60,6 +60,7 @@ struct XiziTrapDriver {
     uint32_t curr_int[NR_CPU];
 
     void (*sys_irq_init)();
+    int (*cur_cpu_id)();
 
     void (*cpu_irq_enable)();
     void (*cpu_irq_disable)();
diff --git a/Ubiquitous/XiZi_AIoT/softkernel/main.c b/Ubiquitous/XiZi_AIoT/softkernel/main.c
index d283b4b27..95dc72341 100644
--- a/Ubiquitous/XiZi_AIoT/softkernel/main.c
+++ b/Ubiquitous/XiZi_AIoT/softkernel/main.c
@@ -53,11 +53,20 @@ int main(void)
     if (!hardkernel_init(&hardkernel_tag)) {
         return -1;
     }
-    /* init softkernel */
-    if (!softkernel_init(&hardkernel_tag, &softkernel_tag)) {
-        return -1;
+
+    struct TraceTag main_intr_tag;
+    AchieveResourceTag(&main_intr_tag, &hardkernel_tag, "intr-ac-resource");
+    struct XiziTrapDriver* p_intr_driver = (struct XiziTrapDriver*)AchieveResource(&main_intr_tag);
+    int cpu_id = p_intr_driver->cur_cpu_id();
+    if (cpu_id == 0) {
+        /* init softkernel */
+        if (!softkernel_init(&hardkernel_tag, &softkernel_tag)) {
+            return -1;
+        }
+        show_xizi_bar();
+
+        int cpu_count = NR_CPU;
     }
-    show_xizi_bar();
 
     /* start first task */
     char* init_task_param[2] = { "/app/init", 0 };