From 755e9312b51f8a03f38d9287e1e2bfbae285cfbe Mon Sep 17 00:00:00 2001
From: Juha Niskanen <juha.niskanen@haltian.com>
Date: Mon, 10 Apr 2017 07:18:16 -0600
Subject: [PATCH] pthread: use cancel cleanup handlers in rwlock

---
 libc/pthread/pthread_rwlock_rdlock.c | 17 ++++++++++++++++-
 libc/pthread/pthread_rwlock_wrlock.c | 27 +++++++++++++++++++++++----
 2 files changed, 39 insertions(+), 5 deletions(-)

diff --git a/libc/pthread/pthread_rwlock_rdlock.c b/libc/pthread/pthread_rwlock_rdlock.c
index 9fd461a758..19979f79bb 100644
--- a/libc/pthread/pthread_rwlock_rdlock.c
+++ b/libc/pthread/pthread_rwlock_rdlock.c
@@ -1,5 +1,5 @@
 /****************************************************************************
- * libc/pthread/pthread_rwlockread.c
+ * libc/pthread/pthread_rwlock_rdlock.c
  *
  *   Copyright (C) 2017 Mark Schulte. All rights reserved.
  *   Author: Mark Schulte <mark@mjs.pw>
@@ -50,6 +50,15 @@
  * Private Functions
  ****************************************************************************/
 
+#ifdef CONFIG_PTHREAD_CLEANUP
+static void rdlock_cleanup(FAR void *arg)
+{
+  FAR pthread_rwlock_t *rw_lock = (FAR pthread_rwlock_t *)arg;
+
+  pthread_mutex_unlock(&rw_lock->lock);
+}
+#endif
+
 static int tryrdlock(FAR pthread_rwlock_t *rw_lock)
 {
   int err;
@@ -116,6 +125,9 @@ int pthread_rwlock_timedrdlock(FAR pthread_rwlock_t *rw_lock,
       return err;
     }
 
+#ifdef CONFIG_PTHREAD_CLEANUP
+  pthread_cleanup_push(&rdlock_cleanup, rw_lock);
+#endif
   while ((err = tryrdlock(rw_lock)) == EBUSY)
     {
       if (ts != NULL)
@@ -132,6 +144,9 @@ int pthread_rwlock_timedrdlock(FAR pthread_rwlock_t *rw_lock,
           break;
         }
     }
+#ifdef CONFIG_PTHREAD_CLEANUP
+  pthread_cleanup_pop(0);
+#endif
 
   pthread_mutex_unlock(&rw_lock->lock);
   return err;
diff --git a/libc/pthread/pthread_rwlock_wrlock.c b/libc/pthread/pthread_rwlock_wrlock.c
index ecda4cb25b..fcda35cb4f 100644
--- a/libc/pthread/pthread_rwlock_wrlock.c
+++ b/libc/pthread/pthread_rwlock_wrlock.c
@@ -1,5 +1,5 @@
 /****************************************************************************
- * libc/pthread/pthread_rwlockwrite.c
+ * libc/pthread/pthread_rwlock_wrlock.c
  *
  *   Copyright (C) 2017 Mark Schulte. All rights reserved.
  *   Author: Mark Schulte <mark@mjs.pw>
@@ -46,15 +46,29 @@
 
 #include <nuttx/semaphore.h>
 
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+#ifdef CONFIG_PTHREAD_CLEANUP
+static void wrlock_cleanup(FAR void *arg)
+{
+  FAR pthread_rwlock_t *rw_lock = (FAR pthread_rwlock_t *)arg;
+
+  rw_lock->num_writers--;
+  pthread_mutex_unlock(&rw_lock->lock);
+}
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
 /****************************************************************************
- * Name: pthread_rwlock_rdlock
+ * Name: pthread_rwlock_wrlock
  *
  * Description:
- *   Locks a read/write lock for reading
+ *   Locks a read/write lock for writing
  *
  * Parameters:
  *   None
@@ -106,6 +120,9 @@ int pthread_rwlock_timedwrlock(FAR pthread_rwlock_t *rw_lock,
 
   rw_lock->num_writers++;
 
+#ifdef CONFIG_PTHREAD_CLEANUP
+  pthread_cleanup_push(&wrlock_cleanup, rw_lock);
+#endif
   while (rw_lock->write_in_progress || rw_lock->num_readers > 0)
     {
       if (ts != NULL)
@@ -122,12 +139,14 @@ int pthread_rwlock_timedwrlock(FAR pthread_rwlock_t *rw_lock,
           break;
         }
     }
+#ifdef CONFIG_PTHREAD_CLEANUP
+  pthread_cleanup_pop(0);
+#endif
 
   if (err == 0)
     {
       rw_lock->write_in_progress = true;
     }
-
   else
     {
       /* In case of error, notify any blocked readers. */