diff --git a/pkg/lwip/contrib/sys_arch.c b/pkg/lwip/contrib/sys_arch.c
index 4b03a522e..6eec97f3e 100644
--- a/pkg/lwip/contrib/sys_arch.c
+++ b/pkg/lwip/contrib/sys_arch.c
@@ -88,7 +88,7 @@ u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t count)
         return (u32_t)(stop / US_PER_MS);
     }
     else {
-        sema_wait_timed((sema_t *)sem, 0);
+        sema_wait((sema_t *)sem);
         return 0;
     }
 }
diff --git a/sys/include/sema.h b/sys/include/sema.h
index e06c3a158..416afbbbf 100644
--- a/sys/include/sema.h
+++ b/sys/include/sema.h
@@ -97,19 +97,44 @@ void sema_create(sema_t *sema, unsigned int value);
  */
 void sema_destroy(sema_t *sema);
 
+/**
+ * @brief Wait for a semaphore, blocking or non-blocking.
+ *
+ * @details For commit purposes you should probably use sema_wait(),
+ *          sema_wait_timed() and sema_try_wait() instead.
+ *
+ * @pre `(sema != NULL)`
+ *
+ * @param[in]  sema       A semaphore.
+ * @param[in]  block      if true, block until semaphore is available.
+ * @param[in]  timeout    if blocking, time in microseconds until the semaphore
+ *                        times out. 0 waits forever.
+ *
+ * @return  0 on success
+ * @return  -ETIMEDOUT, if the semaphore times out.
+ * @return  -ECANCELED, if the semaphore was destroyed.
+ * @return  -EAGAIN,    if the semaphore is not posted (only if block = 0)
+ */
+int _sema_wait(sema_t *sema, int block, uint64_t timeout);
+
 /**
  * @brief   Wait for a semaphore being posted.
  *
  * @pre `(sema != NULL)`
  *
  * @param[in]  sema     A semaphore.
- * @param[in]  timeout  Time in microseconds until the semaphore times out. 0 for no timeout.
+ * @param[in]  timeout  Time in microseconds until the semaphore times out.
+ *                      0 does not wait.
  *
  * @return  0 on success
  * @return  -ETIMEDOUT, if the semaphore times out.
  * @return  -ECANCELED, if the semaphore was destroyed.
+ * @return  -EAGAIN,    if the semaphore is not posted (only if timeout = 0)
  */
-int sema_wait_timed(sema_t *sema, uint64_t timeout);
+static inline int sema_wait_timed(sema_t *sema, uint64_t timeout)
+{
+    return _sema_wait(sema, (timeout != 0), timeout);
+}
 
 /**
  * @brief   Wait for a semaphore being posted (without timeout).
@@ -123,7 +148,7 @@ int sema_wait_timed(sema_t *sema, uint64_t timeout);
  */
 static inline int sema_wait(sema_t *sema)
 {
-    return sema_wait_timed(sema, 0);
+    return _sema_wait(sema, 1, 0);
 }
 
 /**
@@ -137,7 +162,10 @@ static inline int sema_wait(sema_t *sema)
  * @return  -EAGAIN, if the semaphore is not posted.
  * @return  -ECANCELED, if the semaphore was destroyed.
  */
-int sema_try_wait(sema_t *sema);
+static inline int sema_try_wait(sema_t *sema)
+{
+    return _sema_wait(sema, 0, 0);
+}
 
 /**
  * @brief   Signal semaphore.
diff --git a/sys/sema/sema.c b/sys/sema/sema.c
index 950b374ab..db7e9ea4e 100644
--- a/sys/sema/sema.c
+++ b/sys/sema/sema.c
@@ -44,25 +44,7 @@ void sema_destroy(sema_t *sema)
     mutex_unlock(&sema->mutex);
 }
 
-int sema_try_wait(sema_t *sema)
-{
-    assert(sema != NULL);
-
-    int ret = -ECANCELED;
-    if (sema->state == SEMA_OK) {
-        ret = -EAGAIN;
-        unsigned old = irq_disable();
-        if (sema->value > 0) {
-            --sema->value;
-            ret = 0;
-        }
-        irq_restore(old);
-    }
-
-    return ret;
-}
-
-int sema_wait_timed(sema_t *sema, uint64_t us)
+int _sema_wait(sema_t *sema, int block, uint64_t us)
 {
     assert(sema != NULL);
 
@@ -70,27 +52,44 @@ int sema_wait_timed(sema_t *sema, uint64_t us)
         return -ECANCELED;
     }
 
+    int did_block = block;
     unsigned old = irq_disable();
-    if (sema->value == 0) {
+    while ((sema->value == 0) && block) {
         irq_restore(old);
-        int timeout = xtimer_mutex_lock_timeout(&sema->mutex, us);
+        if (us == 0) {
+            mutex_lock(&sema->mutex);
+        }
+        else {
+            uint64_t start = xtimer_now_usec64();
+            block = !xtimer_mutex_lock_timeout(&sema->mutex, us);
+            uint64_t elapsed = xtimer_now_usec64() - start;
+
+            if (elapsed < us) {
+                us -= elapsed;
+            }
+            else {
+                block = 0;
+            }
+        }
 
         if (sema->state != SEMA_OK) {
             mutex_unlock(&sema->mutex);
             return -ECANCELED;
         }
 
-        if (timeout) {
-            return -ETIMEDOUT;
-        }
-
         old = irq_disable();
     }
 
+    if (sema->value == 0) {
+        irq_restore(old);
+        return (did_block) ? -ETIMEDOUT : -EAGAIN;
+    }
+
     unsigned int value = --sema->value;
     irq_restore(old);
 
-    if (value > 0) {
+    /* only unlock mutex if it was a blocking operation */
+    if (did_block && value > 0) {
         mutex_unlock(&sema->mutex);
     }