1 files changed, 3 insertions, 1 deletions
diff --git a/src/chsem.c b/src/chsem.c
index 21f80c284..085f4014b 100644
--- a/src/chsem.c
+++ b/src/chsem.c
@@ -174,8 +174,10 @@ msg_t chSemWaitTimeoutS(Semaphore *sp, systime_t time) {
   chDbgCheck(sp != NULL, "chSemWaitTimeoutS");
 
   if (--sp->s_cnt < 0) {
-    if (TIME_IMMEDIATE == time)
+    if (TIME_IMMEDIATE == time) {
+      sp->s_cnt++;
       return RDY_TIMEOUT;
+    }
     sem_insert(currp, &sp->s_queue);
     currp->p_wtsemp = sp;
     return chSchGoSleepTimeoutS(PRWTSEM, time);