added atomic_dec32le0 for sem/mutex lock

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

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

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) {

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);
 }
 

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) {