smp_ipi

5 files changed, 158 insertions, 1 deletions

diff --git a/components/smp/SConscript b/components/smp/SConscript
new file mode 100644
index 0000000000..9b87df0e9f
--- /dev/null
+++ b/components/smp/SConscript
@@ -0,0 +1,10 @@
+from building import *
+
+cwd  = GetCurrentDir()
+src = []
+if GetDepend("RT_USING_SMP"):
+    src += Glob('*.c')
+CPPPATH = [cwd]
+group = DefineGroup('mprotect', src, depend = [''], CPPPATH = CPPPATH)
+
+Return('group')
diff --git a/components/smp/smp.c b/components/smp/smp.c
new file mode 100644
index 0000000000..0fab13f9df
--- /dev/null
+++ b/components/smp/smp.c
@@ -0,0 +1,110 @@
+#include "smp.h"
+struct smp_call **global_work;
+
+rt_err_t smp_call_handler(struct smp_event * event)
+{
+    switch(event->event_id)
+    {
+    case SMP_CALL_EVENT_FUNC:
+        event->func(event->data);
+        break;
+    default:
+        rt_kprintf("error event id\n");
+        return -RT_ERROR;
+        break;
+    }
+    return RT_EOK;
+}
+void rt_smp_call_ipi_handler(int vector, void *param)
+{
+    int err;
+    struct smp_call *work,*tmp;
+    int cur_cpu = rt_hw_cpu_id();
+    rt_spin_lock(&global_work[cur_cpu]->lock);
+    rt_list_for_each_entry_safe(work,tmp,&global_work[cur_cpu]->node,node)
+    {
+        if(work->event)
+        {
+            err = smp_call_handler(work->event);
+            if(err)
+                break;
+            rt_list_remove(&work->node);
+            rt_free(work);
+        }
+            
+    }
+    rt_spin_unlock(&global_work[cur_cpu]->lock);
+    
+}
+
+void rt_smp_call_func_cond(int cpu_mask, smp_func func, void *data)
+{
+    rt_bool_t run_cur_cpu = RT_TRUE;
+    rt_bool_t need_call = RT_TRUE;
+    int cur_cpu = rt_hw_cpu_id();
+    int cpuid = 1 << cur_cpu;
+    int tmp_id = 0;
+    int tmp_mask = cpu_mask;
+
+    if(cpuid & ~cpu_mask)
+        run_cur_cpu = RT_FALSE;
+
+    if(run_cur_cpu)
+        func(data);
+
+    if(!(cpu_mask & cpuid))
+        need_call = RT_FALSE;
+    else
+        cpu_mask = cpu_mask & (~cpuid);
+
+    if(need_call)
+    {
+        while(tmp_mask)
+        {
+            if((tmp_mask & 1) && tmp_id < RT_CPUS_NR)
+            {
+                struct smp_event *event = rt_calloc(1, sizeof(struct smp_event));
+                event->event_id = SMP_CALL_EVENT_FUNC;
+                event->func = func;
+                event->data = data;
+                event->cpu_mask = cpu_mask;
+                struct smp_call *work = rt_calloc(1, sizeof(struct smp_call));
+                if(work == RT_NULL)
+                    break;
+                
+                work->event = event;
+                rt_spin_lock(&global_work[tmp_id]->lock);
+                rt_list_insert_before(&global_work[tmp_id]->node, &work->node);
+                rt_spin_unlock(&global_work[tmp_id]->lock);
+            }
+            tmp_id++;
+            tmp_mask = tmp_mask >> 1;
+        }
+        rt_hw_ipi_send(RT_IPI_FUNC, cpu_mask);
+    }
+}
+
+void smp_init(void)
+{
+    struct smp_call **work_list = (struct smp_call **)rt_malloc(sizeof(struct smp_call *));
+    for(int i = 0; i < RT_CPUS_NR; i++)
+    {        
+        work_list[i] = rt_calloc(1, sizeof(struct smp_call));
+        if(work_list[i] == RT_NULL)
+            break;
+        rt_list_init(&work_list[i]->node);
+        rt_spin_lock_init(&work_list[i]->lock);
+    }        
+    global_work = work_list;
+}
+
+void test_call(void *data)
+{
+    rt_kprintf("call cpu id  = %d \n",rt_hw_cpu_id());
+}
+void test()
+{
+    int cpu_mask = 0xf;
+    rt_smp_call_func_cond(cpu_mask,test_call, RT_NULL);
+
+}
\ No newline at end of file
diff --git a/components/smp/smp.h b/components/smp/smp.h
new file mode 100644
index 0000000000..63a7b96712
--- /dev/null
+++ b/components/smp/smp.h
@@ -0,0 +1,30 @@
+#ifndef __SMP_IPI_H__
+#define __SMP_IPI_H__
+#include <rtthread.h>
+typedef void (*smp_func)(void *data);
+
+#define SMP_CALL_EVENT_FUNC 0x1
+
+struct smp_call
+{
+    struct rt_spinlock lock;
+    struct rt_list_node node;
+    struct smp_event *event;
+
+};
+
+struct smp_event
+{
+    int cpu_mask;
+    int event_id;
+    void *data;
+    smp_func func;
+
+};
+
+void test();
+void rt_smp_call_func_cond(int cpu_mask,smp_func func, void *data);
+void rt_smp_call_ipi_handler(int vector, void *param);
+void smp_init(void);
+
+#endif
\ No newline at end of file
diff --git a/include/rtdef.h b/include/rtdef.h
index 17dbc329b0..2d22d42524 100644
--- a/include/rtdef.h
+++ b/include/rtdef.h
@@ -672,6 +672,10 @@ typedef struct rt_cpu_usage_stats *rt_cpu_usage_stats_t;
 #define RT_STOP_IPI                     1
 #endif /* RT_STOP_IPI */
 
+#ifndef RT_IPI_FUNC
+#define RT_IPI_FUNC                     2
+#endif
+
 #define _SCHEDULER_CONTEXT(fileds) fileds
 
 /**
diff --git a/libcpu/aarch64/common/setup.c b/libcpu/aarch64/common/setup.c
index 304e8565fb..9d6c16b639 100644
--- a/libcpu/aarch64/common/setup.c
+++ b/libcpu/aarch64/common/setup.c
@@ -13,7 +13,7 @@
 #define DBG_TAG "cpu.aa64"
 #define DBG_LVL DBG_INFO
 #include <rtdbg.h>
-
+#include <smp.h>
 #include <cpu.h>
 #include <mmu.h>
 #include <cpuport.h>
@@ -302,11 +302,14 @@ void rt_hw_common_setup(void)
     rt_thread_idle_sethook(rt_hw_idle_wfi);
 
 #ifdef RT_USING_SMP
+    smp_init();
     /* Install the IPI handle */
     rt_hw_ipi_handler_install(RT_SCHEDULE_IPI, rt_scheduler_ipi_handler);
     rt_hw_ipi_handler_install(RT_STOP_IPI, rt_scheduler_ipi_handler);
+    rt_hw_ipi_handler_install(RT_IPI_FUNC, rt_smp_call_ipi_handler);
     rt_hw_interrupt_umask(RT_SCHEDULE_IPI);
     rt_hw_interrupt_umask(RT_STOP_IPI);
+    rt_hw_interrupt_umask(RT_IPI_FUNC);
 #endif
 }