diff --git a/Makefile b/Makefile
index 0ae82bd..05b64cc 100644
--- a/Makefile
+++ b/Makefile
@@ -4,7 +4,7 @@
 
 .SECONDARY:
 
-CFLAGS+= -I. -Os  -DSTM32F4 -std=c99
+CFLAGS+= -I. -O1  -DSTM32F4 -std=c99
 #CFLAGS+= -mthumb
 #CFLAGS+= -march=armv7e-m
 CFLAGS+= -mfloat-abi=hard
diff --git a/main.c b/main.c
index d50c57d..8b54559 100644
--- a/main.c
+++ b/main.c
@@ -19,6 +19,7 @@
 #include "scheduler.h"
 #include "usartu.h"
 #include "semoper.h"
+#include "semaphore.h"
 
 
 void delay(uint32_t n) {
@@ -59,35 +60,43 @@ static void systick_setup(void) {
 }
 
 int g_uptime;
+sem_t g_sem;
 
 void task1(void) {
     while (true) {
+        sem_wait(&g_sem);
         printf("task 1 %d\r\n", g_uptime);
+        sem_post(&g_sem);
         delay(3000);
     };
 }
 
 void task2(void) {
     while (true) {
+        sem_wait(&g_sem);
         printf("task 2 %d\r\n", g_uptime);
-        delay(3100);
+        sem_post(&g_sem);
+        delay(3000);
     };
 }
 
 void task3(void) {
     while (true) {
+        sem_wait(&g_sem);
         printf("task 3 %d\r\n", g_uptime);
-        delay(3200);
+        sem_post(&g_sem);
+        delay(3000);
     };
 }
 
 void task4(void) {
     static volatile int32_t t4;
-
     while (true) {
         atom_inc32(&t4, (int32_t)1);
+        sem_wait(&g_sem);
         printf("task 4 %d %lu\r\n", g_uptime, t4);
-        delay(3300);
+        sem_post(&g_sem);
+        delay(3000);
     };
 }
 
@@ -99,7 +108,7 @@ void sys_tick_handler(void) {
 
 int main(void) {
     g_uptime = 0;
-
+    sem_init(&g_sem, 1);
     static scheduler_t g_scheduler;
 
     clock_setup();
diff --git a/scheduler.c b/scheduler.c
index 8b4087b..c0aae0d 100644
--- a/scheduler.c
+++ b/scheduler.c
@@ -83,42 +83,47 @@ static void scheduler_switch(scheduler_t *scheduler) {
 void __attribute__((naked)) pend_sv_handler(void) {
 
     const uint32_t RETURN_ON_PSP = RETURN_ON_PSP_THREAD;
-
-    __asm__("cpsid if":::"memory");
     uint32_t lr;
-    __asm__("mov %0, lr\n":"=r"(lr));
+
+    __asm__(
+            "cpsid  if      \n"
+            "mov    %0, lr  \n"
+            :"=r"(lr)
+    );
 
     if (lr & EXC_RETURN_MODE_THREAD) {
         uint32_t psp;
         __asm__(
-            "mrs %0, psp\n"
-            "stmdb %0!, {r4-r11}\n"
-            "msr psp, %0\n" : "=r"(psp)
+            "mrs    %0, psp       \n"
+            "stmdb  %0!, {r4-r11} \n"
+            "msr    psp, %0       \n"
+            : "=r"(psp)
         );
         m_scheduler->tasks[m_scheduler->current_task].sp = psp;
     } else {
         __asm__(
-           "stmdb sp!, {r4-r11}\n");
+           "stmdb   sp!, {r4-r11} \n"
+        );
     }
 
     scheduler_switch(m_scheduler);
 
     uint32_t psp = (uint32_t)m_scheduler->tasks[m_scheduler->current_task].sp;
-    __asm__(
-            "ldmfd %0!, {r4-r11}\n"
-            "msr psp, %0\n"::"r"(psp)
+    __asm__ (
+            "ldmfd  %0!, {r4-r11} \n"
+            "msr    psp, %0       \n"
+            "cpsie  if            \n"
+            "bx     %1            \n"
+            :: "r"(psp), "r"(RETURN_ON_PSP)
     );
-
-    __asm__("cpsie if" ::: "memory");
-    __asm__("bx %0\n"::"r"(RETURN_ON_PSP));
 }
 
 void scheduler_yield(void) {
     SCB_ICSR |= SCB_ICSR_PENDSVSET;
     __asm__(
-        "nop\n"
-        "nop\n"
-        "nop\n"
-        "nop\n"
+        "nop        \n"
+        "nop        \n"
+        "nop        \n"
+        "nop        \n"
     );
 }
diff --git a/semaphore.c b/semaphore.c
index df8ff1e..f83d9bf 100644
--- a/semaphore.c
+++ b/semaphore.c
@@ -10,13 +10,15 @@ void sem_init(sem_t* sem, int32_t value) {
 }
 
 int32_t sem_wait(sem_t* sem) {
-    //while(sem->value > 0);
+    //while(sem->value <= 0);
     //sem->value--;
-    while (atom_dec32(&(sem->value), (int32_t)0) > 0);
+    //return sem->value;
+    while (atom_dec32(&(sem->value), (int32_t)0) <= 0);
     return atom_dec32(&(sem->value), (int32_t)1);
 }
 
 int32_t sem_post(sem_t* sem) {
     //sem->value++;
+    //return sem->value;
     return atom_inc32(&(sem->value), (int32_t)1);
 }
diff --git a/semoper.S b/semoper.S
index 52d185d..cfe6668 100644
--- a/semoper.S
+++ b/semoper.S
@@ -7,8 +7,7 @@
 .type   atom_inc32, %function
 
 atom_inc32:
-1:
-        ldrex           r2, [r0]
+1:      ldrex           r2, [r0]
         add             r2, r2, r1
         strex           r3, r2, [r0]
         teq             r3, #0
@@ -21,8 +20,7 @@ atom_inc32:
 .type   atom_dec32, %function
 
 atom_dec32:
-1:
-        ldrex           r2, [r0]
+1:      ldrex           r2, [r0]
         sub             r2, r2, r1
         strex           r3, r2, [r0]
         teq             r3, #0