pm: pm_lock support recursive_lock

Signed-off-by: ligd <liguiding1@xiaomi.com>

drivers/power/pm.h

@@ -78,6 +78,8 @@ struct pm_domain_s
    */
 
   sem_t sem;
+  pid_t holder;
+  unsigned int count;
 };
 
 /* This structure encapsulates all of the global data used by the PM system */

drivers/power/pm_initialize.c

@@ -26,6 +26,7 @@
 
 #include <nuttx/power/pm.h>
 #include <nuttx/semaphore.h>
+#include <nuttx/sched.h>
 
 #include "pm.h"
 
@@ -90,6 +91,9 @@ void pm_initialize(void)
       pm_set_governor(i, gov);
 
       nxsem_init(&g_pmglobals.domain[i].sem, 0, 1);
+
+      g_pmglobals.domain[i].holder = INVALID_PROCESS_ID;
+      g_pmglobals.domain[i].count  = 0;
     }
 }
 

drivers/power/pm_lock.c

@@ -27,11 +27,68 @@
 #include <nuttx/arch.h>
 #include <nuttx/irq.h>
 #include <nuttx/power/pm.h>
+#include <assert.h>
 #include <sched.h>
 #include "pm.h"
 
 #if defined(CONFIG_PM)
 
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int pm_recursive_lock(struct pm_domain_s *pm)
+{
+  pid_t me = gettid();
+  int ret = OK;
+
+  /* Does this thread already hold the semaphore? */
+
+  if (pm->holder == me)
+    {
+      /* Yes.. just increment the reference count */
+
+      pm->count++;
+    }
+  else
+    {
+      /* No.. take the semaphore (perhaps waiting) */
+
+      ret = nxsem_wait_uninterruptible(&pm->sem);
+      if (ret >= 0)
+        {
+          /* Now this thread holds the semaphore */
+
+          pm->holder = me;
+          pm->count  = 1;
+        }
+    }
+
+  return ret;
+}
+
+static void pm_recursive_unlock(struct pm_domain_s *pm)
+{
+  DEBUGASSERT(pm->holder == getpid() && pm->count > 0);
+
+  /* If the count would go to zero, then release the semaphore */
+
+  if (pm->count == 1)
+    {
+      /* We no longer hold the semaphore */
+
+      pm->holder = INVALID_PROCESS_ID;
+      pm->count  = 0;
+      nxsem_post(&pm->sem);
+    }
+  else
+    {
+      /* We still hold the semaphore. Just decrement the count */
+
+      pm->count--;
+    }
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -51,7 +108,7 @@ irqstate_t pm_lock(int domain)
 {
   if (!up_interrupt_context() && !sched_idletask())
     {
-      nxsem_wait_uninterruptible(&g_pmglobals.domain[domain].sem);
+      pm_recursive_lock(&g_pmglobals.domain[domain]);
     }
 
   return enter_critical_section();
@@ -74,7 +131,7 @@ void pm_unlock(int domain, irqstate_t flags)
 
   if (!up_interrupt_context() && !sched_idletask())
     {
-      nxsem_post(&g_pmglobals.domain[domain].sem);
+      pm_recursive_unlock(&g_pmglobals.domain[domain]);
     }
 }