Add smp support to RT-Thread 4.0

1 files changed, 568 insertions, 106 deletions

diff --git a/src/scheduler.c b/src/scheduler.c
index fd068a6671..e177bde6b7 100644
--- a/src/scheduler.c
+++ b/src/scheduler.c
@@ -21,28 +21,36 @@
  * 2010-12-13     Bernard      add defunct list initialization even if not use heap.
  * 2011-05-10     Bernard      clean scheduler debug log.
  * 2013-12-21     Grissiom     add rt_critical_level
+ * 2018-11-22     Jesven       remove the current task from ready queue
+ *                             add per cpu ready queue
+ *                             add _get_highest_priority_thread to find highest priority task
+ *                             rt_schedule_insert_thread won't insert current task to ready queue
+ *                             in smp version, rt_hw_context_switch_interrupt maybe switch to
+ *                               new task directly
+ *
  */
 
 #include <rtthread.h>
 #include <rthw.h>
 
-static rt_int16_t rt_scheduler_lock_nest;
-extern volatile rt_uint8_t rt_interrupt_nest;
+#ifdef RT_USING_SMP
+rt_hw_spinlock_t _rt_critical_lock;
+#endif /*RT_USING_SMP*/
 
 rt_list_t rt_thread_priority_table[RT_THREAD_PRIORITY_MAX];
-struct rt_thread *rt_current_thread;
-
-rt_uint8_t rt_current_priority;
-
+rt_uint32_t rt_thread_ready_priority_group;
 #if RT_THREAD_PRIORITY_MAX > 32
 /* Maximum priority level, 256 */
-rt_uint32_t rt_thread_ready_priority_group;
 rt_uint8_t rt_thread_ready_table[32];
-#else
-/* Maximum priority level, 32 */
-rt_uint32_t rt_thread_ready_priority_group;
 #endif
 
+#ifndef RT_USING_SMP
+extern volatile rt_uint8_t rt_interrupt_nest;
+static rt_int16_t rt_scheduler_lock_nest;
+struct rt_thread *rt_current_thread;
+rt_uint8_t rt_current_priority;
+#endif /*RT_USING_SMP*/
+
 rt_list_t rt_thread_defunct;
 
 #ifdef RT_USING_HOOK
@@ -91,19 +99,84 @@ static void _rt_scheduler_stack_check(struct rt_thread *thread)
         level = rt_hw_interrupt_disable();
         while (level);
     }
-#if defined(ARCH_CPU_STACK_GROWS_UPWARD)
-    else if ((rt_ubase_t)thread->sp > ((rt_ubase_t)thread->stack_addr + thread->stack_size))
+    else if ((rt_ubase_t)thread->sp <= ((rt_ubase_t)thread->stack_addr + 32))
     {
-        rt_kprintf("warning: %s stack is close to the top of stack address.\n",
+        rt_kprintf("warning: %s stack is close to end of stack address.\n",
                    thread->name);
     }
+}
+#endif
+
+/*
+ * get the highest priority thread in ready queue
+ */
+#ifdef RT_USING_SMP
+static struct rt_thread* _get_highest_priority_thread(rt_ubase_t *highest_prio)
+{
+    register struct rt_thread *highest_priority_thread;
+    register rt_ubase_t highest_ready_priority, local_highest_ready_priority;
+    struct rt_cpu* pcpu = rt_cpu_self();
+
+#if RT_THREAD_PRIORITY_MAX > 32
+    register rt_ubase_t number;
+
+    if (rt_thread_ready_priority_group == 0 && pcpu->priority_group == 0)
+    {
+        *highest_prio = pcpu->current_thread->current_priority;
+        /* only local IDLE is readly */
+        return pcpu->current_thread;
+    }
+
+    number = __rt_ffs(rt_thread_ready_priority_group) - 1;
+    highest_ready_priority = (number << 3) + __rt_ffs(rt_thread_ready_table[number]) - 1;
+    number = __rt_ffs(pcpu->priority_group) - 1;
+    local_highest_ready_priority = (number << 3) + __rt_ffs(pcpu->ready_table[number]) - 1;
 #else
-    else if ((rt_ubase_t)thread->sp <= ((rt_ubase_t)thread->stack_addr + 32))
+    highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1;
+    local_highest_ready_priority = __rt_ffs(pcpu->priority_group) - 1;
+#endif
+
+    /* get highest ready priority thread */
+    if (highest_ready_priority < local_highest_ready_priority)
     {
-        rt_kprintf("warning: %s stack is close to the bottom of stack address.\n",
-                   thread->name);
+        *highest_prio = highest_ready_priority;
+        highest_priority_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next,
+                                  struct rt_thread,
+                                  tlist);
+    }
+    else
+    {
+        *highest_prio = local_highest_ready_priority;
+        highest_priority_thread = rt_list_entry(pcpu->priority_table[local_highest_ready_priority].next,
+                                  struct rt_thread,
+                                  tlist);
     }
+
+    return highest_priority_thread;
+}
+#else
+static struct rt_thread* _get_highest_priority_thread(rt_ubase_t *highest_prio)
+{
+    register struct rt_thread *highest_priority_thread;
+    register rt_ubase_t highest_ready_priority;
+
+#if RT_THREAD_PRIORITY_MAX > 32
+    register rt_ubase_t number;
+
+    number = __rt_ffs(rt_thread_ready_priority_group) - 1;
+    highest_ready_priority = (number << 3) + __rt_ffs(rt_thread_ready_table[number]) - 1;
+#else
+    highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1;
 #endif
+
+    /* get highest ready priority thread */
+    highest_priority_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next,
+                              struct rt_thread,
+                              tlist);
+
+    *highest_prio = highest_ready_priority;
+
+    return highest_priority_thread;
 }
 #endif
 
@@ -113,9 +186,14 @@ static void _rt_scheduler_stack_check(struct rt_thread *thread)
  */
 void rt_system_scheduler_init(void)
 {
+#ifdef RT_USING_SMP
+    int cpu;
+#endif /*RT_USING_SMP*/
     register rt_base_t offset;
 
+#ifndef RT_USING_SMP
     rt_scheduler_lock_nest = 0;
+#endif /*RT_USING_SMP*/
 
     RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("start scheduler: max priority 0x%02x\n",
                                       RT_THREAD_PRIORITY_MAX));
@@ -124,9 +202,25 @@ void rt_system_scheduler_init(void)
     {
         rt_list_init(&rt_thread_priority_table[offset]);
     }
+#ifdef RT_USING_SMP
+    for (cpu = 0; cpu < RT_CPUS_NR; cpu++)
+    {
+        struct rt_cpu *pcpu =  rt_cpu_index(cpu);
+        for (offset = 0; offset < RT_THREAD_PRIORITY_MAX; offset ++)
+        {
+            rt_list_init(&pcpu->priority_table[offset]);
+        }
 
-    rt_current_priority = RT_THREAD_PRIORITY_MAX - 1;
-    rt_current_thread = RT_NULL;
+        pcpu->irq_switch_flag = 0;
+        pcpu->current_priority = RT_THREAD_PRIORITY_MAX - 1;
+        pcpu->current_thread = RT_NULL;
+        pcpu->priority_group = 0;
+
+#if RT_THREAD_PRIORITY_MAX > 32
+        rt_memset(pcpu->ready_table, 0, sizeof(pcpu->ready_table));
+#endif
+    }
+#endif /*RT_USING_SMP*/
 
     /* initialize ready priority group */
     rt_thread_ready_priority_group = 0;
@@ -148,26 +242,24 @@ void rt_system_scheduler_init(void)
 void rt_system_scheduler_start(void)
 {
     register struct rt_thread *to_thread;
-    register rt_ubase_t highest_ready_priority;
+    rt_ubase_t highest_ready_priority;
 
-#if RT_THREAD_PRIORITY_MAX > 32
-    register rt_ubase_t number;
+    to_thread = _get_highest_priority_thread(&highest_ready_priority);
 
-    number = __rt_ffs(rt_thread_ready_priority_group) - 1;
-    highest_ready_priority = (number << 3) + __rt_ffs(rt_thread_ready_table[number]) - 1;
+#ifdef RT_USING_SMP
+    to_thread->oncpu = rt_hw_cpu_id();
 #else
-    highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1;
-#endif
-
-    /* get switch to thread */
-    to_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next,
-                              struct rt_thread,
-                              tlist);
-
     rt_current_thread = to_thread;
+#endif /*RT_USING_SMP*/
+
+    rt_schedule_remove_thread(to_thread);
 
     /* switch to new thread */
-    rt_hw_context_switch_to((rt_ubase_t)&to_thread->sp);
+#ifdef RT_USING_SMP
+    rt_hw_context_switch_to((rt_uint32_t)&to_thread->sp, to_thread);
+#else
+    rt_hw_context_switch_to((rt_uint32_t)&to_thread->sp);
+#endif /*RT_USING_SMP*/
 
     /* never come back */
 }
@@ -178,6 +270,114 @@ void rt_system_scheduler_start(void)
 
 /**@{*/
 
+
+#ifdef RT_USING_SMP
+/**
+ * This function will handle IPI interrupt and do a scheduling in system;
+ * 
+ * @param vector, the number of IPI interrupt for system scheduling
+ * @param param, use RT_NULL
+ * 
+ * NOTE: this function should be invoke or register as ISR in BSP.
+ */
+void rt_scheduler_ipi_handler(int vector, void *param)
+{
+    rt_schedule();
+}
+
+/**
+ * This function will perform one scheduling. It will select one thread
+ * with the highest priority level in global ready queue or local ready queue, 
+ * then switch to it.
+ */
+void rt_schedule(void)
+{
+    rt_base_t level;
+    struct rt_thread *to_thread;
+    struct rt_thread *current_thread;
+    struct rt_cpu    *pcpu;
+    int cpu_id;
+
+    /* disable interrupt */
+    level = rt_hw_interrupt_disable();
+
+    cpu_id = rt_hw_cpu_id();
+    pcpu   = rt_cpu_index(cpu_id);
+    current_thread = pcpu->current_thread;
+
+    /* whether do switch in interrupt */
+    if (pcpu->irq_nest)
+    {
+        pcpu->irq_switch_flag = 1;
+    }
+    else if (current_thread->scheduler_lock_nest == 1) /* whether lock scheduler */
+    {
+        rt_ubase_t highest_ready_priority;
+
+        if (rt_thread_ready_priority_group != 0 || pcpu->priority_group != 0)
+        {
+            to_thread = _get_highest_priority_thread(&highest_ready_priority);
+            current_thread->oncpu = RT_CPU_DETACHED;
+            if ((current_thread->stat & RT_THREAD_STAT_MASK) == RT_THREAD_READY)
+            {
+                if (current_thread->current_priority < highest_ready_priority)
+                {
+                    to_thread = current_thread;
+                }
+                else
+                {
+                    rt_schedule_insert_thread(current_thread);
+                }
+            }
+            to_thread->oncpu = cpu_id;
+            if (to_thread != current_thread)
+            {
+                /* if the destination thread is not the same as current thread */
+                pcpu->current_priority = (rt_uint8_t)highest_ready_priority;
+
+                RT_OBJECT_HOOK_CALL(rt_scheduler_hook, (current_thread, to_thread));
+
+                rt_schedule_remove_thread(to_thread);
+
+                /* switch to new thread */
+                RT_DEBUG_LOG(RT_DEBUG_SCHEDULER,
+                        ("[%d]switch to priority#%d "
+                         "thread:%.*s(sp:0x%08x), "
+                         "from thread:%.*s(sp: 0x%08x)\n",
+                         pcpu->irq_nest, highest_ready_priority,
+                         RT_NAME_MAX, to_thread->name, to_thread->sp,
+                         RT_NAME_MAX, current_thread->name, current_thread->sp));
+
+#ifdef RT_USING_OVERFLOW_CHECK
+                _rt_scheduler_stack_check(to_thread);
+#endif
+
+                {
+                    extern void rt_thread_handle_sig(rt_bool_t clean_state);
+
+                    rt_hw_context_switch((rt_ubase_t)&current_thread->sp,
+                                         (rt_ubase_t)&to_thread->sp, to_thread);
+
+                    /* enable interrupt */
+                    rt_hw_interrupt_enable(level);
+
+#ifdef RT_USING_SIGNALS
+                    /* check signal status */
+                    rt_thread_handle_sig(RT_TRUE);
+#endif
+                    goto __exit;
+                }
+            }
+        }
+    }
+
+    /* enable interrupt */
+    rt_hw_interrupt_enable(level);
+
+__exit:
+    return ;
+}
+#else
 /**
  * This function will perform one schedule. It will select one thread
  * with the highest priority level, then switch to it.
@@ -194,81 +394,169 @@ void rt_schedule(void)
     /* check the scheduler is enabled or not */
     if (rt_scheduler_lock_nest == 0)
     {
-        register rt_ubase_t highest_ready_priority;
+        rt_ubase_t highest_ready_priority;
 
-#if RT_THREAD_PRIORITY_MAX <= 32
-        highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1;
-#else
-        register rt_ubase_t number;
+        if (rt_thread_ready_priority_group != 0)
+        {
+            int need_insert_from_thread = 0;
 
-        number = __rt_ffs(rt_thread_ready_priority_group) - 1;
-        highest_ready_priority = (number << 3) + __rt_ffs(rt_thread_ready_table[number]) - 1;
-#endif
+            to_thread = _get_highest_priority_thread(&highest_ready_priority);
 
-        /* get switch to thread */
-        to_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next,
-                                  struct rt_thread,
-                                  tlist);
+            if ((rt_current_thread->stat & RT_THREAD_STAT_MASK) == RT_THREAD_READY)
+            {
+                if (rt_current_thread->current_priority < highest_ready_priority)
+                {
+                    to_thread = rt_current_thread;
+                }
+                else
+                {
+                    need_insert_from_thread = 1;
+                }
+            }
 
-        /* if the destination thread is not the same as current thread */
-        if (to_thread != rt_current_thread)
-        {
-            rt_current_priority = (rt_uint8_t)highest_ready_priority;
-            from_thread         = rt_current_thread;
-            rt_current_thread   = to_thread;
+            if (to_thread != rt_current_thread)
+            {
+                /* if the destination thread is not the same as current thread */
+                rt_current_priority = (rt_uint8_t)highest_ready_priority;
+                from_thread         = rt_current_thread;
+                rt_current_thread   = to_thread;
+
+                RT_OBJECT_HOOK_CALL(rt_scheduler_hook, (from_thread, to_thread));
 
-            RT_OBJECT_HOOK_CALL(rt_scheduler_hook, (from_thread, to_thread));
+                if (need_insert_from_thread)
+                {
+                    rt_schedule_insert_thread(from_thread);
+                }
 
-            /* switch to new thread */
-            RT_DEBUG_LOG(RT_DEBUG_SCHEDULER,
-                         ("[%d]switch to priority#%d "
-                          "thread:%.*s(sp:0x%08x), "
-                          "from thread:%.*s(sp: 0x%08x)\n",
-                          rt_interrupt_nest, highest_ready_priority,
-                          RT_NAME_MAX, to_thread->name, to_thread->sp,
-                          RT_NAME_MAX, from_thread->name, from_thread->sp));
+                rt_schedule_remove_thread(to_thread);
+
+                /* switch to new thread */
+                RT_DEBUG_LOG(RT_DEBUG_SCHEDULER,
+                        ("[%d]switch to priority#%d "
+                         "thread:%.*s(sp:0x%p), "
+                         "from thread:%.*s(sp: 0x%p)\n",
+                         rt_interrupt_nest, highest_ready_priority,
+                         RT_NAME_MAX, to_thread->name, to_thread->sp,
+                         RT_NAME_MAX, from_thread->name, from_thread->sp));
 
 #ifdef RT_USING_OVERFLOW_CHECK
-            _rt_scheduler_stack_check(to_thread);
+                _rt_scheduler_stack_check(to_thread);
 #endif
 
-            if (rt_interrupt_nest == 0)
-            {
-                extern void rt_thread_handle_sig(rt_bool_t clean_state);
+                if (rt_interrupt_nest == 0)
+                {
+                    extern void rt_thread_handle_sig(rt_bool_t clean_state);
 
-                rt_hw_context_switch((rt_ubase_t)&from_thread->sp,
-                                     (rt_ubase_t)&to_thread->sp);
+                    rt_hw_context_switch((rt_ubase_t)&from_thread->sp,
+                            (rt_ubase_t)&to_thread->sp);
 
-                /* enable interrupt */
-                rt_hw_interrupt_enable(level);
+                    /* enable interrupt */
+                    rt_hw_interrupt_enable(level);
 
 #ifdef RT_USING_SIGNALS
-                /* check signal status */
-                rt_thread_handle_sig(RT_TRUE);
+                    /* check signal status */
+                    rt_thread_handle_sig(RT_TRUE);
 #endif
+                    goto __exit;
+                }
+                else
+                {
+                    RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("switch in interrupt\n"));
+
+                    rt_hw_context_switch_interrupt((rt_ubase_t)&from_thread->sp,
+                            (rt_ubase_t)&to_thread->sp);
+                }
             }
             else
             {
-                RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("switch in interrupt\n"));
-
-                rt_hw_context_switch_interrupt((rt_ubase_t)&from_thread->sp,
-                                               (rt_ubase_t)&to_thread->sp);
-                /* enable interrupt */
-                rt_hw_interrupt_enable(level);
+                rt_schedule_remove_thread(rt_current_thread);
             }
         }
-        else
-        {
-            /* enable interrupt */
-            rt_hw_interrupt_enable(level);
-        }
     }
-    else
+
+    /* enable interrupt */
+    rt_hw_interrupt_enable(level);
+
+__exit:
+    return;
+}
+#endif /*RT_USING_SMP*/
+
+/**
+ * This function checks if a scheduling is needed after IRQ context. If yes,
+ * it will select one thread with the highest priority level, and then switch
+ * to it.
+ */
+#ifdef RT_USING_SMP
+void rt_scheduler_do_irq_switch(void *context)
+{
+    int cpu_id;
+    rt_base_t level;
+    struct rt_cpu* pcpu;
+    struct rt_thread *to_thread;
+    struct rt_thread *current_thread;
+
+    level = rt_hw_interrupt_disable();
+
+    cpu_id = rt_hw_cpu_id();
+    pcpu   = rt_cpu_index(cpu_id);
+    current_thread = pcpu->current_thread;
+
+    if (pcpu->irq_switch_flag == 0)
     {
-        /* enable interrupt */
         rt_hw_interrupt_enable(level);
+        return;
+    }
+
+    if (current_thread->scheduler_lock_nest == 1 && pcpu->irq_nest == 0)
+    {
+        rt_ubase_t highest_ready_priority;
+
+        /* clear irq switch flag */
+        pcpu->irq_switch_flag = 0;
+
+        if (rt_thread_ready_priority_group != 0 || pcpu->priority_group != 0)
+        {
+            to_thread = _get_highest_priority_thread(&highest_ready_priority);
+            current_thread->oncpu = RT_CPU_DETACHED;
+            if ((current_thread->stat & RT_THREAD_STAT_MASK) == RT_THREAD_READY)
+            {
+                if (current_thread->current_priority < highest_ready_priority)
+                {
+                    to_thread = current_thread;
+                }
+                else
+                {
+                    rt_schedule_insert_thread(current_thread);
+                }
+            }
+            to_thread->oncpu = cpu_id;
+            if (to_thread != current_thread)
+            {
+                /* if the destination thread is not the same as current thread */
+
+                pcpu->current_priority = (rt_uint8_t)highest_ready_priority;
+
+                RT_OBJECT_HOOK_CALL(rt_scheduler_hook, (current_thread, to_thread));
+
+                rt_schedule_remove_thread(to_thread);
+
+#ifdef RT_USING_OVERFLOW_CHECK
+                _rt_scheduler_stack_check(to_thread);
+#endif
+                RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("switch in interrupt\n"));
+
+                current_thread->cpus_lock_nest--;
+                current_thread->scheduler_lock_nest--;
+
+                rt_hw_context_switch_interrupt(context, (rt_ubase_t)&current_thread->sp,
+                        (rt_ubase_t)&to_thread->sp, to_thread);
+            }
+        }
     }
+    rt_hw_interrupt_enable(level);
 }
+#endif /*RT_USING_SMP*/
 
 /*
  * This function will insert a thread to system ready queue. The state of
@@ -277,6 +565,70 @@ void rt_schedule(void)
  * @param thread the thread to be inserted
  * @note Please do not invoke this function in user application.
  */
+#ifdef RT_USING_SMP
+void rt_schedule_insert_thread(struct rt_thread *thread)
+{
+    int cpu_id;
+    int bind_cpu;
+    rt_uint32_t cpu_mask;
+    register rt_base_t level;
+
+    RT_ASSERT(thread != RT_NULL);
+
+    /* disable interrupt */
+    level = rt_hw_interrupt_disable();
+
+    /* change stat */
+    thread->stat = RT_THREAD_READY | (thread->stat & ~RT_THREAD_STAT_MASK);
+
+    if (thread->oncpu != RT_CPU_DETACHED)
+    {
+        goto __exit;
+    }
+
+    cpu_id   = rt_hw_cpu_id();
+    bind_cpu = thread->bind_cpu ;
+
+    /* insert thread to ready list */
+    if (bind_cpu == RT_CPUS_NR)
+    {
+#if RT_THREAD_PRIORITY_MAX > 32
+        rt_thread_ready_table[thread->number] |= thread->high_mask;
+#endif
+        rt_thread_ready_priority_group |= thread->number_mask;
+
+        rt_list_insert_before(&(rt_thread_priority_table[thread->current_priority]),
+                              &(thread->tlist));
+        cpu_mask = RT_CPU_MASK ^ (1 << cpu_id);
+        rt_hw_ipi_send(RT_SCHEDULE_IPI_IRQ, cpu_mask);
+    }
+    else
+    {
+        struct rt_cpu *pcpu = rt_cpu_index(bind_cpu);
+
+#if RT_THREAD_PRIORITY_MAX > 32
+        pcpu->ready_table[thread->number] |= thread->high_mask;
+#endif
+        pcpu->priority_group |= thread->number_mask;
+
+        rt_list_insert_before(&(rt_cpu_index(bind_cpu)->priority_table[thread->current_priority]),
+                              &(thread->tlist));
+
+        if (cpu_id != bind_cpu)
+        {
+            cpu_mask = 1 << bind_cpu;
+            rt_hw_ipi_send(RT_SCHEDULE_IPI_IRQ, cpu_mask);
+        }
+    }
+
+    RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("insert thread[%.*s], the priority: %d\n",
+                                      RT_NAME_MAX, thread->name, thread->current_priority));
+
+__exit:
+    /* enable interrupt */
+    rt_hw_interrupt_enable(level);
+}
+#else
 void rt_schedule_insert_thread(struct rt_thread *thread)
 {
     register rt_base_t temp;
@@ -289,32 +641,29 @@ void rt_schedule_insert_thread(struct rt_thread *thread)
     /* change stat */
     thread->stat = RT_THREAD_READY | (thread->stat & ~RT_THREAD_STAT_MASK);
 
+    if (thread == rt_current_thread)
+    {
+        goto __exit;
+    }
+
     /* insert thread to ready list */
     rt_list_insert_before(&(rt_thread_priority_table[thread->current_priority]),
                           &(thread->tlist));
 
-    /* set priority mask */
-#if RT_THREAD_PRIORITY_MAX <= 32
     RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("insert thread[%.*s], the priority: %d\n",
                                       RT_NAME_MAX, thread->name, thread->current_priority));
-#else
-    RT_DEBUG_LOG(RT_DEBUG_SCHEDULER,
-                 ("insert thread[%.*s], the priority: %d 0x%x %d\n",
-                  RT_NAME_MAX,
-                  thread->name,
-                  thread->number,
-                  thread->number_mask,
-                  thread->high_mask));
-#endif
 
+    /* set priority mask */
 #if RT_THREAD_PRIORITY_MAX > 32
     rt_thread_ready_table[thread->number] |= thread->high_mask;
 #endif
     rt_thread_ready_priority_group |= thread->number_mask;
 
+__exit:
     /* enable interrupt */
     rt_hw_interrupt_enable(temp);
 }
+#endif /*RT_USING_SMP*/
 
 /*
  * This function will remove a thread from system ready queue.
@@ -323,28 +672,71 @@ void rt_schedule_insert_thread(struct rt_thread *thread)
  *
  * @note Please do not invoke this function in user application.
  */
+#ifdef RT_USING_SMP
 void rt_schedule_remove_thread(struct rt_thread *thread)
 {
-    register rt_base_t temp;
+    register rt_base_t level;
 
     RT_ASSERT(thread != RT_NULL);
 
     /* disable interrupt */
-    temp = rt_hw_interrupt_disable();
+    level = rt_hw_interrupt_disable();
 
-#if RT_THREAD_PRIORITY_MAX <= 32
     RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("remove thread[%.*s], the priority: %d\n",
                                       RT_NAME_MAX, thread->name,
                                       thread->current_priority));
+
+    /* remove thread from ready list */
+    rt_list_remove(&(thread->tlist));
+    if (thread->bind_cpu == RT_CPUS_NR)
+    {
+        if (rt_list_isempty(&(rt_thread_priority_table[thread->current_priority])))
+        {
+#if RT_THREAD_PRIORITY_MAX > 32
+            rt_thread_ready_table[thread->number] &= ~thread->high_mask;
+            if (rt_thread_ready_table[thread->number] == 0)
+            {
+                rt_thread_ready_priority_group &= ~thread->number_mask;
+            }
+#else
+            rt_thread_ready_priority_group &= ~thread->number_mask;
+#endif
+        }
+    }
+    else
+    {
+        struct rt_cpu *pcpu = rt_cpu_index(thread->bind_cpu);
+
+        if (rt_list_isempty(&(pcpu->priority_table[thread->current_priority])))
+        {
+#if RT_THREAD_PRIORITY_MAX > 32
+            pcpu->ready_table[thread->number] &= ~thread->high_mask;
+            if (rt_thread_ready_table[thread->number] == 0)
+            {
+                pcpu->priority_group &= ~thread->number_mask;
+            }
 #else
-    RT_DEBUG_LOG(RT_DEBUG_SCHEDULER,
-                 ("remove thread[%.*s], the priority: %d 0x%x %d\n",
-                  RT_NAME_MAX,
-                  thread->name,
-                  thread->number,
-                  thread->number_mask,
-                  thread->high_mask));
+            pcpu->priority_group &= ~thread->number_mask;
 #endif
+        }
+    }
+
+    /* enable interrupt */
+    rt_hw_interrupt_enable(level);
+}
+#else
+void rt_schedule_remove_thread(struct rt_thread *thread)
+{
+    register rt_base_t level;
+
+    RT_ASSERT(thread != RT_NULL);
+
+    /* disable interrupt */
+    level = rt_hw_interrupt_disable();
+
+    RT_DEBUG_LOG(RT_DEBUG_SCHEDULER, ("remove thread[%.*s], the priority: %d\n",
+                                      RT_NAME_MAX, thread->name,
+                                      thread->current_priority));
 
     /* remove thread from ready list */
     rt_list_remove(&(thread->tlist));
@@ -362,12 +754,41 @@ void rt_schedule_remove_thread(struct rt_thread *thread)
     }
 
     /* enable interrupt */
-    rt_hw_interrupt_enable(temp);
+    rt_hw_interrupt_enable(level);
 }
+#endif /*RT_USING_SMP*/
 
 /**
  * This function will lock the thread scheduler.
  */
+#ifdef RT_USING_SMP
+void rt_enter_critical(void)
+{
+    register rt_base_t level;
+    struct rt_thread *current_thread;
+
+    /* disable interrupt */
+    level = rt_hw_local_irq_disable();
+
+    current_thread = rt_cpu_self()->current_thread;
+    /*
+     * the maximal number of nest is RT_UINT16_MAX, which is big
+     * enough and does not check here
+     */
+
+    /* lock scheduler for all cpus */
+    if (current_thread->scheduler_lock_nest == !!current_thread->cpus_lock_nest)
+    {
+        rt_hw_spin_lock(&_rt_critical_lock);
+    }
+
+    /* lock scheduler for local cpu */
+    current_thread->scheduler_lock_nest ++;
+
+    /* enable interrupt */
+    rt_hw_local_irq_enable(level);
+}
+#else
 void rt_enter_critical(void)
 {
     register rt_base_t level;
@@ -384,11 +805,45 @@ void rt_enter_critical(void)
     /* enable interrupt */
     rt_hw_interrupt_enable(level);
 }
+#endif /*RT_USING_SMP*/
 RTM_EXPORT(rt_enter_critical);
 
 /**
  * This function will unlock the thread scheduler.
  */
+#ifdef RT_USING_SMP
+void rt_exit_critical(void)
+{
+    register rt_base_t level;
+    struct rt_thread *current_thread;
+
+    /* disable interrupt */
+    level = rt_hw_local_irq_disable();
+
+    current_thread = rt_cpu_self()->current_thread;
+
+    current_thread->scheduler_lock_nest --;
+
+    if (current_thread->scheduler_lock_nest == !!current_thread->cpus_lock_nest)
+    {
+        rt_hw_spin_unlock(&_rt_critical_lock);
+    }
+
+    if (current_thread->scheduler_lock_nest <= 0)
+    {
+        current_thread->scheduler_lock_nest = 0;
+        /* enable interrupt */
+        rt_hw_local_irq_enable(level);
+
+        rt_schedule();
+    }
+    else
+    {
+        /* enable interrupt */
+        rt_hw_local_irq_enable(level);
+    }
+}
+#else
 void rt_exit_critical(void)
 {
     register rt_base_t level;
@@ -412,6 +867,7 @@ void rt_exit_critical(void)
         rt_hw_interrupt_enable(level);
     }
 }
+#endif /*RT_USING_SMP*/
 RTM_EXPORT(rt_exit_critical);
 
 /**
@@ -421,8 +877,14 @@ RTM_EXPORT(rt_exit_critical);
  */
 rt_uint16_t rt_critical_level(void)
 {
-    return rt_scheduler_lock_nest;
+#ifdef RT_USING_SMP
+    struct rt_thread *current_thread = rt_cpu_self()->current_thread;
+
+    return current_thread->scheduler_lock_nest;
+#else
+	return rt_scheduler_lock_nest;
+#endif /*RT_USING_SMP*/
 }
 RTM_EXPORT(rt_critical_level);
-/**@}*/
 
+/**@}*/