From 27c61ea33dc53e6b1b9f088e1a297de9ca075e84 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Micha=C5=82=20Narajowski?= <michal.narajowski@codecoup.pl>
Date: Thu, 7 Feb 2019 16:55:01 +0100
Subject: [PATCH] porting/linux: Fix semaphore and mutex pend implementations

---
 porting/npl/linux/src/os_mutex.c | 31 +++++++++++++++-----------
 porting/npl/linux/src/os_sem.c   | 37 +++++++++++++-------------------
 2 files changed, 34 insertions(+), 34 deletions(-)

diff --git a/porting/npl/linux/src/os_mutex.c b/porting/npl/linux/src/os_mutex.c
index 5c6a996fd..43263ef39 100644
--- a/porting/npl/linux/src/os_mutex.c
+++ b/porting/npl/linux/src/os_mutex.c
@@ -17,14 +17,12 @@
  * under the License.
  */
 
-#include <assert.h>
-#include <stdint.h>
-#include <string.h>
+#include <errno.h>
+#include <pthread.h>
+
 #include "os/os.h"
 #include "nimble/nimble_npl.h"
 
-#include <pthread.h>
-
 ble_npl_error_t
 ble_npl_mutex_init(struct ble_npl_mutex *mu)
 {
@@ -56,19 +54,28 @@ ble_npl_mutex_release(struct ble_npl_mutex *mu)
 ble_npl_error_t
 ble_npl_mutex_pend(struct ble_npl_mutex *mu, uint32_t timeout)
 {
+    int err;
+
     if (!mu) {
         return BLE_NPL_INVALID_PARAM;
     }
 
-    assert(&mu->lock);
+    if (timeout == BLE_NPL_WAIT_FOREVER) {
+        err = pthread_mutex_lock(&mu->lock);
+    } else {
+        err = clock_gettime(CLOCK_REALTIME, &mu->wait);
+        if (err) {
+            return BLE_NPL_ERROR;
+        }
 
-    mu->wait.tv_sec  = timeout / 1000;
-    mu->wait.tv_nsec = (timeout % 1000) * 1000000;
-    mu->wait.tv_nsec %= 1000000000;
+        mu->wait.tv_sec  += timeout / 1000;
+        mu->wait.tv_nsec += (timeout % 1000) * 1000000;
 
-    if (pthread_mutex_timedlock(&mu->lock, &mu->wait)) {
-        return BLE_NPL_TIMEOUT;
+        err = pthread_mutex_timedlock(&mu->lock, &mu->wait);
+        if (err == ETIMEDOUT) {
+            return BLE_NPL_TIMEOUT;
+        }
     }
 
-    return BLE_NPL_OK;
+    return (err) ? BLE_NPL_ERROR : BLE_NPL_OK;
 }
diff --git a/porting/npl/linux/src/os_sem.c b/porting/npl/linux/src/os_sem.c
index bec0039fa..704c856ef 100644
--- a/porting/npl/linux/src/os_sem.c
+++ b/porting/npl/linux/src/os_sem.c
@@ -18,15 +18,11 @@
  */
 
 #include <assert.h>
-#include <stdint.h>
-#include <string.h>
-#include "os/os.h"
-#include "nimble/nimble_npl.h"
-
 #include <errno.h>
-#include <pthread.h>
 #include <semaphore.h>
 
+#include "os/os.h"
+#include "nimble/nimble_npl.h"
 
 ble_npl_error_t
 ble_npl_sem_init(struct ble_npl_sem *sem, uint16_t tokens)
@@ -64,24 +60,21 @@ ble_npl_sem_pend(struct ble_npl_sem *sem, uint32_t timeout)
         return BLE_NPL_INVALID_PARAM;
     }
 
-    err = clock_gettime(CLOCK_REALTIME, &wait);
-    if (err) {
-        return BLE_NPL_ERROR;
-    }
-
-    wait.tv_sec  += timeout / 1000;
-    wait.tv_nsec += (timeout % 1000) * 1000000;
-    wait.tv_nsec %= 1000000000;
-
     if (timeout == BLE_NPL_WAIT_FOREVER) {
         err = sem_wait(&sem->lock);
-    }
-    else
-    {
-        if (sem_timedwait(&sem->lock, &wait)) {
-	        assert(errno == ETIMEDOUT);
-	        return BLE_NPL_TIMEOUT;
-	    }
+    } else {
+        err = clock_gettime(CLOCK_REALTIME, &wait);
+        if (err) {
+            return BLE_NPL_ERROR;
+        }
+
+        wait.tv_sec  += timeout / 1000;
+        wait.tv_nsec += (timeout % 1000) * 1000000;
+
+        err = sem_timedwait(&sem->lock, &wait);
+        if (err && errno == ETIMEDOUT) {
+            return BLE_NPL_TIMEOUT;
+        }
     }
 
     return (err) ? BLE_NPL_ERROR : BLE_NPL_OK;