diff --git a/atomic.S b/atomic.S
index cc67731..782a3e8 100644
--- a/atomic.S
+++ b/atomic.S
@@ -20,10 +20,24 @@ atomic_inc32:
 .type   atomic_dec32, %function
 
 atomic_dec32:
-2:      ldrex           r2, [r0]
+1:      ldrex           r2, [r0]
         sub             r2, r2, r1
         strex           r3, r2, [r0]
         teq             r3, #0
-        bne             2b
+        bne             1b
+        mov             r0, r2
+        bx              lr
+
+.globl  atomic_dec32le0
+.type   atomic_dec32le0, %function
+
+atomic_dec32le0:
+1:      ldrex           r2, [r0]
+        cmp             r2, #0
+        ble             1b
+        sub             r2, r2, r1
+        strex           r3, r2, [r0]
+        teq             r3, #0
+        bne             1b
         mov             r0, r2
         bx              lr
diff --git a/atomic.h b/atomic.h
index 2cd54c3..66258d2 100644
--- a/atomic.h
+++ b/atomic.h
@@ -10,4 +10,6 @@
 int32_t atomic_inc32(volatile int32_t *addr, int32_t value);
 int32_t atomic_dec32(volatile int32_t *addr, int32_t value);
 
+
+int32_t atomic_dec32le0(volatile int32_t *addr, int32_t value);
 #endif
diff --git a/mutex.c b/mutex.c
index 05a84a5..e9c4a06 100644
--- a/mutex.c
+++ b/mutex.c
@@ -13,8 +13,7 @@ int32_t mutex_lock(mutex_t* mutex) {
     //while(mutex->value <= 0);
     //mutex->value--;
     //return mutex->value;
-    while (atomic_dec32(&(mutex->value), (int32_t)0) <= 0);
-    return atomic_dec32(&(mutex->value), (int32_t)1);
+    return atomic_dec32le0(&(mutex->value), (int32_t)1);
 }
 
 int32_t mutex_unlock(mutex_t* mutex) {
diff --git a/scheduler.c b/scheduler.c
index c0aae0d..8c426dc 100644
--- a/scheduler.c
+++ b/scheduler.c
@@ -73,7 +73,7 @@ void scheduler_task(scheduler_t *scheduler, int task_no, void (*entry)(void)) {
 
 static void scheduler_switch(scheduler_t *scheduler) {
     do {
-        scheduler->current_task = (scheduler->current_task + 1) % SCHEDULER_NUM_TASKS;
+        scheduler->current_task = (scheduler->current_task) % SCHEDULER_NUM_TASKS;
     } while (!scheduler->tasks[scheduler->current_task].runnable);
 }
 
diff --git a/semaphore.c b/semaphore.c
index 5cd116b..ba22e6a 100644
--- a/semaphore.c
+++ b/semaphore.c
@@ -13,8 +13,8 @@ int32_t sem_wait(sem_t* sem) {
     //while(sem->value <= 0);
     //sem->value--;
     //return sem->value;
-    while (atomic_dec32(&(sem->value), (int32_t)0) <= 0);
-    return atomic_dec32(&(sem->value), (int32_t)1);
+    //while (atomic_dec32(&(sem->value), (int32_t)0) <= 0);
+    return atomic_dec32le0(&(sem->value), (int32_t)1);
 }
 
 int32_t sem_post(sem_t* sem) {