1 files changed, 102 insertions, 5 deletions

diff --git a/src/cpu.c b/src/cpu.c
index 3e2d65dfb9..96e585713b 100644
--- a/src/cpu.c
+++ b/src/cpu.c
@@ -7,15 +7,106 @@
  * Date           Author       Notes
  * 2018-10-30     Bernard      The first version
  */
-
-#include <rtthread.h>
 #include <rthw.h>
+#include <rtthread.h>
 
 #ifdef RT_USING_SMP
-
 static struct rt_cpu rt_cpus[RT_CPUS_NR];
 rt_hw_spinlock_t _cpus_lock;
 
+/*
+ * disable scheduler
+ */
+static void rt_preempt_disable(void)
+{
+    register rt_base_t level;
+    struct rt_thread *current_thread;
+
+    /* disable interrupt */
+    level = rt_hw_local_irq_disable();
+
+    current_thread = rt_thread_self();
+    if (!current_thread)
+    {
+        rt_hw_local_irq_enable(level);
+        return;
+    }
+
+    /* lock scheduler for local cpu */
+    current_thread->scheduler_lock_nest ++;
+
+    /* enable interrupt */
+    rt_hw_local_irq_enable(level);
+}
+
+/*
+ * enable scheduler
+ */
+static void rt_preempt_enable(void)
+{
+    register rt_base_t level;
+    struct rt_thread *current_thread;
+
+    /* disable interrupt */
+    level = rt_hw_local_irq_disable();
+
+    current_thread = rt_thread_self();
+    if (!current_thread)
+    {
+        rt_hw_local_irq_enable(level);
+        return;
+    }
+
+    /* unlock scheduler for local cpu */
+    current_thread->scheduler_lock_nest --;
+
+    rt_schedule();
+    /* enable interrupt */
+    rt_hw_local_irq_enable(level);
+}
+
+void rt_spin_lock_init(struct rt_spinlock *lock)
+{
+    rt_hw_spin_lock_init(&lock->lock);
+}
+RTM_EXPORT(rt_spin_lock_init)
+
+void rt_spin_lock(struct rt_spinlock *lock)
+{
+    rt_preempt_disable();
+    rt_hw_spin_lock(&lock->lock);
+}
+RTM_EXPORT(rt_spin_lock)
+
+void rt_spin_unlock(struct rt_spinlock *lock)
+{
+    rt_hw_spin_unlock(&lock->lock);
+    rt_preempt_enable();
+}
+RTM_EXPORT(rt_spin_unlock)
+
+rt_base_t rt_spin_lock_irqsave(struct rt_spinlock *lock)
+{
+    unsigned long level;
+
+    rt_preempt_disable();
+
+    level = rt_hw_local_irq_disable();
+    rt_hw_spin_lock(&lock->lock);
+
+    return level;
+}
+RTM_EXPORT(rt_spin_lock_irqsave)
+
+void rt_spin_unlock_irqrestore(struct rt_spinlock *lock, rt_base_t level)
+{
+    rt_hw_spin_unlock(&lock->lock);
+    rt_hw_local_irq_enable(level);
+
+    rt_preempt_enable();
+}
+RTM_EXPORT(rt_spin_unlock_irqrestore)
+
 /**
  * This fucntion will return current cpu.
  */
@@ -42,12 +133,16 @@ rt_base_t rt_cpus_lock(void)
     pcpu = rt_cpu_self();
     if (pcpu->current_thread != RT_NULL)
     {
-        if (pcpu->current_thread->cpus_lock_nest++ == 0)
+        register rt_ubase_t lock_nest = pcpu->current_thread->cpus_lock_nest;
+
+        pcpu->current_thread->cpus_lock_nest++;
+        if (lock_nest == 0)
         {
             pcpu->current_thread->scheduler_lock_nest++;
             rt_hw_spin_lock(&_cpus_lock);
         }
     }
+
     return level;
 }
 RTM_EXPORT(rt_cpus_lock);
@@ -61,7 +156,9 @@ void rt_cpus_unlock(rt_base_t level)
 
     if (pcpu->current_thread != RT_NULL)
     {
-        if (--pcpu->current_thread->cpus_lock_nest == 0)
+        pcpu->current_thread->cpus_lock_nest--;
+
+        if (pcpu->current_thread->cpus_lock_nest == 0)
         {
             pcpu->current_thread->scheduler_lock_nest--;
             rt_hw_spin_unlock(&_cpus_lock);