From 25b16576e804e65d49cf00d9677e95e7864262e2 Mon Sep 17 00:00:00 2001
From: ligd <liguiding1@xiaomi.com>
Date: Fri, 6 May 2022 12:56:23 +0800
Subject: [PATCH] pm: pm_lock support recursive_lock

Signed-off-by: ligd <liguiding1@xiaomi.com>
---
 drivers/power/pm.h            |  2 ++
 drivers/power/pm_initialize.c |  4 +++
 drivers/power/pm_lock.c       | 61 +++++++++++++++++++++++++++++++++--
 3 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/drivers/power/pm.h b/drivers/power/pm.h
index 7c4e0668b73..0ae9a934738 100644
--- a/drivers/power/pm.h
+++ b/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 */
diff --git a/drivers/power/pm_initialize.c b/drivers/power/pm_initialize.c
index a2b376dc08a..f1d9cf798ca 100644
--- a/drivers/power/pm_initialize.c
+++ b/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;
     }
 }
 
diff --git a/drivers/power/pm_lock.c b/drivers/power/pm_lock.c
index fe496ea3208..7e87fe56a7d 100644
--- a/drivers/power/pm_lock.c
+++ b/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]);
     }
 }