[Kernel] Add nested mutex feature

1 files changed, 249 insertions, 75 deletions

diff --git a/src/ipc.c b/src/ipc.c
index 329fc7f4cf..67a998d5df 100644
--- a/src/ipc.c
+++ b/src/ipc.c
@@ -42,6 +42,7 @@
  * 2022-01-07     Gabriel      Moving __on_rt_xxxxx_hook to ipc.c
  * 2022-01-24     THEWON       let rt_mutex_take return thread->error when using signal
  * 2022-04-08     Stanley      Correct descriptions
+ * 2022-10-15     Bernard      add nested mutex feature
  */
 
 #include <rtthread.h>
@@ -708,6 +709,80 @@ RTM_EXPORT(rt_sem_control);
 #endif /* RT_USING_SEMAPHORE */
 
 #ifdef RT_USING_MUTEX
+rt_inline rt_uint8_t _mutex_update_priority(struct rt_mutex *mutex)
+{
+    struct rt_thread *thread;
+
+    if (!rt_list_isempty(&mutex->parent.suspend_thread))
+    {
+        thread = rt_list_entry(mutex->parent.suspend_thread.next, struct rt_thread, tlist);
+        mutex->priority = thread->current_priority;
+    }
+    else
+    {
+        mutex->priority = 0xff;
+    }
+
+    return mutex->priority;
+}
+
+rt_inline rt_uint8_t _thread_get_mutex_priority(struct rt_thread* thread)
+{
+    struct rt_mutex *mutex;
+    rt_uint8_t priority = thread->init_priority;
+
+    rt_list_for_each_entry(mutex, &(thread->taken_object_list), taken_list)
+    {
+        if (priority > mutex->priority) priority = mutex->priority;
+    }
+
+    return priority;
+}
+
+rt_inline void _thread_update_priority(struct rt_thread *thread, rt_uint8_t priority)
+{
+    RT_DEBUG_LOG(RT_DEBUG_IPC,
+            ("thread:%s priority -> %d\n", thread->name, priority));
+
+    /* change priority of the thread */
+    rt_thread_control(thread,
+                      RT_THREAD_CTRL_CHANGE_PRIORITY,
+                      &priority);
+
+    if ((thread->stat & RT_THREAD_STAT_MASK) == RT_THREAD_SUSPEND)
+    {
+        /* whether change the priority of taken mutex */
+        struct rt_object* pending_obj = thread->pending_object;
+
+        if (pending_obj && rt_object_get_type(pending_obj) == RT_Object_Class_Mutex)
+        {
+            rt_uint8_t mutex_priority;
+            struct rt_mutex* pending_mutex = (struct rt_mutex *)pending_obj;
+
+            /* re-insert thread to suspended thread list */
+            rt_list_remove(&(thread->tlist));
+            _ipc_list_suspend(&(pending_mutex->parent.suspend_thread),
+                                thread,
+                                pending_mutex->parent.parent.flag);
+
+            /* update priority */
+            _mutex_update_priority(pending_mutex);
+            /* change the priority of mutex owner thread */
+            RT_DEBUG_LOG(RT_DEBUG_IPC,
+                    ("mutex: %s priority -> %d\n", pending_mutex->parent.parent.name,
+                    pending_mutex->priority));
+
+            mutex_priority = _thread_get_mutex_priority(pending_mutex->owner);
+            if (mutex_priority != pending_mutex->owner->current_priority)
+            {
+                _thread_update_priority(pending_mutex->owner, mutex_priority);
+            }
+        }
+    }
+
+    return ;
+}
+
 /**
  * @addtogroup mutex
  */
@@ -752,10 +827,10 @@ rt_err_t rt_mutex_init(rt_mutex_t mutex, const char *name, rt_uint8_t flag)
     /* initialize ipc object */
     _ipc_object_init(&(mutex->parent));
 
-    mutex->value = 1;
-    mutex->owner = RT_NULL;
-    mutex->original_priority = 0xFF;
-    mutex->hold  = 0;
+    mutex->owner    = RT_NULL;
+    mutex->priority = 0xFF;
+    mutex->hold     = 0;
+    rt_list_init(&(mutex->taken_list));
 
     /* flag can only be RT_IPC_FLAG_PRIO. RT_IPC_FLAG_FIFO cannot solve the unbounded priority inversion problem */
     mutex->parent.parent.flag = RT_IPC_FLAG_PRIO;
@@ -785,13 +860,19 @@ RTM_EXPORT(rt_mutex_init);
  */
 rt_err_t rt_mutex_detach(rt_mutex_t mutex)
 {
+    rt_ubase_t level;
+
     /* parameter check */
     RT_ASSERT(mutex != RT_NULL);
     RT_ASSERT(rt_object_get_type(&mutex->parent.parent) == RT_Object_Class_Mutex);
     RT_ASSERT(rt_object_is_systemobject(&mutex->parent.parent));
 
+    level = rt_hw_interrupt_disable();
     /* wakeup all suspended threads */
     _ipc_list_resume_all(&(mutex->parent.suspend_thread));
+    /* remove mutex from thread's taken list */
+    rt_list_remove(&mutex->taken_list);
+    rt_hw_interrupt_enable(level);
 
     /* detach mutex object */
     rt_object_detach(&(mutex->parent.parent));
@@ -800,6 +881,48 @@ rt_err_t rt_mutex_detach(rt_mutex_t mutex)
 }
 RTM_EXPORT(rt_mutex_detach);
 
+/* drop a thread from the suspend list of mutex */
+void rt_mutex_drop_thread(rt_mutex_t mutex, rt_thread_t thread)
+{
+    rt_uint8_t priority;
+    rt_bool_t need_update = RT_FALSE;
+
+    rt_list_remove(&(thread->tlist));
+
+    /* should change the priority of mutex owner thread */
+    if (mutex->owner->current_priority == thread->current_priority)
+        need_update = RT_TRUE;
+
+    /* update the priority of mutex */
+    if (!rt_list_isempty(&mutex->parent.suspend_thread))
+    {
+        /* more thread suspended in the list */
+        struct rt_thread *th;
+
+        th = rt_list_entry(mutex->parent.suspend_thread.next,
+                                struct rt_thread,
+                                tlist);
+        /* update the priority of mutex */
+        mutex->priority = th->current_priority;
+    }
+    else
+    {
+        /* set mutex priority to maximal priority */
+        mutex->priority = 0xff;
+    }
+
+    /* try to change the priority of mutex owner thread */
+    if (need_update)
+    {
+        /* get the maximal priority of mutex in thread */
+        priority = _thread_get_mutex_priority(mutex->owner);
+        if (priority != mutex->owner->current_priority)
+        {
+            _thread_update_priority(mutex->owner, priority);
+        }
+    }
+}
+
 #ifdef RT_USING_HEAP
 /**
  * @brief    This function will create a mutex object.
@@ -836,10 +959,10 @@ rt_mutex_t rt_mutex_create(const char *name, rt_uint8_t flag)
     /* initialize ipc object */
     _ipc_object_init(&(mutex->parent));
 
-    mutex->value              = 1;
-    mutex->owner              = RT_NULL;
-    mutex->original_priority  = 0xFF;
-    mutex->hold               = 0;
+    mutex->owner    = RT_NULL;
+    mutex->priority = 0xFF;
+    mutex->hold     = 0;
+    rt_list_init(&(mutex->taken_list));
 
     /* flag can only be RT_IPC_FLAG_PRIO. RT_IPC_FLAG_FIFO cannot solve the unbounded priority inversion problem */
     mutex->parent.parent.flag = RT_IPC_FLAG_PRIO;
@@ -869,6 +992,8 @@ RTM_EXPORT(rt_mutex_create);
  */
 rt_err_t rt_mutex_delete(rt_mutex_t mutex)
 {
+	rt_ubase_t level;
+
     /* parameter check */
     RT_ASSERT(mutex != RT_NULL);
     RT_ASSERT(rt_object_get_type(&mutex->parent.parent) == RT_Object_Class_Mutex);
@@ -876,8 +1001,12 @@ rt_err_t rt_mutex_delete(rt_mutex_t mutex)
 
     RT_DEBUG_NOT_IN_INTERRUPT;
 
+    level = rt_hw_interrupt_disable();
     /* wakeup all suspended threads */
     _ipc_list_resume_all(&(mutex->parent.suspend_thread));
+    /* remove mutex from thread's taken list */
+    rt_list_remove(&mutex->taken_list);
+    rt_hw_interrupt_enable(level);
 
     /* delete mutex object */
     rt_object_delete(&(mutex->parent.parent));
@@ -933,8 +1062,8 @@ rt_err_t rt_mutex_take(rt_mutex_t mutex, rt_int32_t timeout)
     RT_OBJECT_HOOK_CALL(rt_object_trytake_hook, (&(mutex->parent.parent)));
 
     RT_DEBUG_LOG(RT_DEBUG_IPC,
-                 ("mutex_take: current thread %s, mutex value: %d, hold: %d\n",
-                  thread->name, mutex->value, mutex->hold));
+                 ("mutex_take: current thread %s, hold: %d\n",
+                  thread->name, mutex->hold));
 
     /* reset thread error */
     thread->error = RT_EOK;
@@ -954,26 +1083,15 @@ rt_err_t rt_mutex_take(rt_mutex_t mutex, rt_int32_t timeout)
     }
     else
     {
-        /* The value of mutex is 1 in initial status. Therefore, if the
-         * value is great than 0, it indicates the mutex is avaible.
-         */
-        if (mutex->value > 0)
+        /* whether the mutex has owner thread. */
+        if (mutex->owner == RT_NULL)
         {
-            /* mutex is available */
-            mutex->value --;
-
             /* set mutex owner and original priority */
-            mutex->owner             = thread;
-            mutex->original_priority = thread->current_priority;
-            if(mutex->hold < RT_MUTEX_HOLD_MAX)
-            {
-                mutex->hold ++;
-            }
-            else
-            {
-                rt_hw_interrupt_enable(level); /* enable interrupt */
-                return -RT_EFULL; /* value overflowed */
-            }
+            mutex->owner    = thread;
+            mutex->priority = 0xff;
+            mutex->hold     = 1;
+            /* insert mutex to thread's taken object list */
+            rt_list_insert_after(&thread->taken_object_list, &mutex->taken_list);
         }
         else
         {
@@ -990,23 +1108,28 @@ rt_err_t rt_mutex_take(rt_mutex_t mutex, rt_int32_t timeout)
             }
             else
             {
+                rt_uint8_t priority = thread->current_priority;
+
                 /* mutex is unavailable, push to suspend list */
                 RT_DEBUG_LOG(RT_DEBUG_IPC, ("mutex_take: suspend thread: %s\n",
                                             thread->name));
 
-                /* change the owner thread priority of mutex */
-                if (thread->current_priority < mutex->owner->current_priority)
-                {
-                    /* change the owner thread priority */
-                    rt_thread_control(mutex->owner,
-                                      RT_THREAD_CTRL_CHANGE_PRIORITY,
-                                      &thread->current_priority);
-                }
-
                 /* suspend current thread */
                 _ipc_list_suspend(&(mutex->parent.suspend_thread),
                                     thread,
                                     mutex->parent.parent.flag);
+                /* set pending object in thread to this mutex */
+                thread->pending_object = &(mutex->parent.parent);
+
+                /* update the priority level of mutex */
+                if (priority < mutex->priority)
+                {
+                    mutex->priority = priority;
+                    if (mutex->priority < mutex->owner->current_priority)
+                    {
+                        _thread_update_priority(mutex->owner, priority);
+                    }
+                }
 
                 /* has waiting time, start thread timer */
                 if (timeout > 0)
@@ -1028,16 +1151,57 @@ rt_err_t rt_mutex_take(rt_mutex_t mutex, rt_int32_t timeout)
                 /* do schedule */
                 rt_schedule();
 
-                if (thread->error != RT_EOK)
+                /* disable interrupt */
+                level = rt_hw_interrupt_disable();
+
+                if (thread->error == RT_EOK)
                 {
-                    /* return error */
-                    return thread->error;
+                    /* get mutex successfully */
                 }
                 else
                 {
-                    /* the mutex is taken successfully. */
-                    /* disable interrupt */
-                    level = rt_hw_interrupt_disable();
+                    /* the mutex has not been taken and thread has detach from the pending list. */
+
+                    rt_bool_t need_update = RT_FALSE;
+
+                    /* should change the priority of mutex owner thread */
+                    if (mutex->owner->current_priority == thread->current_priority)
+                        need_update = RT_TRUE;
+
+                    /* update the priority of mutex */
+                    if (!rt_list_isempty(&mutex->parent.suspend_thread))
+                    {
+                        /* more thread suspended in the list */
+                        struct rt_thread *th;
+
+                        th = rt_list_entry(mutex->parent.suspend_thread.next,
+                                                struct rt_thread,
+                                                tlist);
+                        /* update the priority of mutex */
+                        mutex->priority = th->current_priority;
+                    }
+                    else
+                    {
+                        /* set mutex priority to maximal priority */
+                        mutex->priority = 0xff;
+                    }
+
+                    /* try to change the priority of mutex owner thread */
+                    if (need_update)
+                    {
+                        /* get the maximal priority of mutex in thread */
+                        priority = _thread_get_mutex_priority(mutex->owner);
+                        if (priority != mutex->owner->current_priority)
+                        {
+                            _thread_update_priority(mutex->owner, priority);
+                        }
+                    }
+
+                    /* enable interrupt */
+                    rt_hw_interrupt_enable(level);
+
+                    /* return error */
+                    return thread->error;
                 }
             }
         }
@@ -1109,8 +1273,8 @@ rt_err_t rt_mutex_release(rt_mutex_t mutex)
     level = rt_hw_interrupt_disable();
 
     RT_DEBUG_LOG(RT_DEBUG_IPC,
-                 ("mutex_release:current thread %s, mutex value: %d, hold: %d\n",
-                  thread->name, mutex->value, mutex->hold));
+                 ("mutex_release:current thread %s, hold: %d\n",
+                  thread->name, mutex->hold));
 
     RT_OBJECT_HOOK_CALL(rt_object_put_hook, (&(mutex->parent.parent)));
 
@@ -1130,60 +1294,70 @@ rt_err_t rt_mutex_release(rt_mutex_t mutex)
     /* if no hold */
     if (mutex->hold == 0)
     {
-        /* change the owner thread to original priority */
-        if (mutex->original_priority != mutex->owner->current_priority)
+        /* remove mutex from thread's taken list */
+        rt_list_remove(&mutex->taken_list);
+
+        /* whether change the thread priority */
+        if (thread->current_priority == mutex->priority)
         {
-            rt_thread_control(mutex->owner,
+            rt_uint8_t priority = 0xff;
+
+            /* get the highest priority in the taken list of thread */
+            priority = _thread_get_mutex_priority(thread);
+
+            rt_thread_control(thread,
                               RT_THREAD_CTRL_CHANGE_PRIORITY,
-                              &(mutex->original_priority));
+                              &priority);
+
+            need_schedule = RT_TRUE;
         }
 
         /* wakeup suspended thread */
         if (!rt_list_isempty(&mutex->parent.suspend_thread))
         {
-            /* get suspended thread */
-            thread = rt_list_entry(mutex->parent.suspend_thread.next,
+            /* get the first suspended thread */
+            struct rt_thread *next_thread = rt_list_entry(mutex->parent.suspend_thread.next,
                                    struct rt_thread,
                                    tlist);
 
             RT_DEBUG_LOG(RT_DEBUG_IPC, ("mutex_release: resume thread: %s\n",
-                                        thread->name));
+                    next_thread->name));
+
+            /* remove the thread from the suspended list of mutex */
+            rt_list_remove(&(next_thread->tlist));
 
-            /* set new owner and priority */
-            mutex->owner             = thread;
-            mutex->original_priority = thread->current_priority;
+            /* set new owner and put mutex into taken list of thread */
+            mutex->owner = next_thread;
+            mutex->hold  = 1;
+            rt_list_insert_after(&next_thread->taken_object_list, &mutex->taken_list);
+            /* cleanup pending object */
+            next_thread->pending_object = RT_NULL;
+
+            /* resume thread */
+            rt_thread_resume(next_thread);
 
-            if(mutex->hold < RT_MUTEX_HOLD_MAX)
+            /* update mutex priority */
+            if (!rt_list_isempty(&(mutex->parent.suspend_thread)))
             {
-                mutex->hold ++;
+                struct rt_thread *th;
+
+                th = rt_list_entry(mutex->parent.suspend_thread.next,
+                        struct rt_thread,
+                        tlist);
+                mutex->priority = th->current_priority;
             }
             else
             {
-                rt_hw_interrupt_enable(level); /* enable interrupt */
-                return -RT_EFULL; /* value overflowed */
+                mutex->priority = 0xff;
             }
 
-            /* resume thread */
-            _ipc_list_resume(&(mutex->parent.suspend_thread));
-
             need_schedule = RT_TRUE;
         }
         else
         {
-            if(mutex->value < RT_MUTEX_VALUE_MAX)
-            {
-                /* increase value */
-                mutex->value ++;
-            }
-            else
-            {
-                rt_hw_interrupt_enable(level); /* enable interrupt */
-                return -RT_EFULL; /* value overflowed */
-            }
-
             /* clear owner */
-            mutex->owner             = RT_NULL;
-            mutex->original_priority = 0xff;
+            mutex->owner    = RT_NULL;
+            mutex->priority = 0xff;
         }
     }