diff --git a/include/keyboard.h b/include/keyboard.h
index 58b4881352b937a4d2b8a92139876a0f8412a054..eeab17a458d0bd5c5aa0d439c79c5b4e1f523f88 100644
--- a/include/keyboard.h
+++ b/include/keyboard.h
@@ -37,9 +37,7 @@ char keyboard_map(unsigned char code);
 
 void init_queue();
 
-void putc(unsigned char c);
-
-void putc(unsigned char c);
+void add_to_queue(unsigned char c);
 
 char getc();
 
diff --git a/src/keyboard.c b/src/keyboard.c
index 63bc4bfc4c811b406b7ef283d8d3e84035751dfb..03ce2fe989c13211aad9cf5bc603bbbc3475c4b4 100644
--- a/src/keyboard.c
+++ b/src/keyboard.c
@@ -1,4 +1,8 @@
 #include "keyboard.h"
+#include "sem.h"
+#include "utils.h"
+
+sem_t keyboard_sem;
 
 void init_keymapping()
 {
@@ -86,15 +90,16 @@ void init_queue()
     keyboard_queue.first_free = 0;
 }
 
-void putc(unsigned char c)
+void add_to_queue(unsigned char c)
 {
     irq_disable();
 
     if (c != NONE && keyboard_queue.cpt < QUEUE_SIZE)
     {
-        keyboard_queue.cpt++;
-        keyboard_queue.first_free = (keyboard_queue.first_free + 1) % QUEUE_SIZE;
         keyboard_queue.array[keyboard_queue.first_free] = c;
+        keyboard_queue.first_free = (keyboard_queue.first_free + 1) % QUEUE_SIZE;
+        keyboard_queue.cpt++;
+        // sem_up(&keyboard_sem);
     }
 
     irq_enable();
@@ -104,9 +109,15 @@ char getc()
 {
     irq_disable();
 
-    char idx = (keyboard_queue.first_free + QUEUE_SIZE - keyboard_queue.cpt) % QUEUE_SIZE;
-    keyboard_queue.cpt--;
+    char c = NONE;
+    if (keyboard_queue.cpt)
+    {
+        // sem_down(&keyboard_sem);
+        int idx = (keyboard_queue.first_free + QUEUE_SIZE - keyboard_queue.cpt) % QUEUE_SIZE;
+        c = keyboard_queue.array[idx];
+        keyboard_queue.cpt--;
+    }
 
     irq_enable();
-    return keyboard_queue.array[idx];
+    return c;
 }
\ No newline at end of file
diff --git a/src/main.c b/src/main.c
index f597cf8d0cbb7f7a7865eb2fadb71c3da19de127..7d13d13285ec7cb0c25a7d11bca8ea918ce47c7e 100644
--- a/src/main.c
+++ b/src/main.c
@@ -8,6 +8,7 @@
 
 extern int cursor_x;
 extern int cursor_y;
+extern sem_t keyboard_sem;
 
 int counter = 0;
 
@@ -26,6 +27,21 @@ void timer_it(int_regs_t *r)
   yield();
 }
 
+void keyboard_it(int_regs_t *r)
+{
+  irq_disable();
+
+  // puts("keyboard_it\n");
+  if (_inb(0x64) == (0x1D))
+  {
+    _outb(0x64, 0x1C);
+    char c = keyboard_map(_inb(0x60));
+    add_to_queue(c);
+  }
+
+  irq_enable();
+}
+
 void counter_handler(void *args)
 {
   while (1)
@@ -39,51 +55,20 @@ void counter_handler(void *args)
 
     cursor_x = tmp_cursor[0];
     cursor_y = tmp_cursor[1];
-
     irq_enable();
   }
 }
 
 void keyboard_handler(void *args)
 {
-  init_keymapping();
   while (1)
   {
     irq_disable();
-    if (_inb(0x64) == (0x1D))
-    {
-      _outb(0x64, 0x1C);
-      unsigned char s = _inb(0x60);
-      char c = keyboard_map(s);
-      if (c)
-        putc(c);
-      // puthex(s);
-    }
-    irq_enable();
-  }
-}
+    char c = getc();
+    if (c != NONE)
+      putc(c);
 
-void ping(void *arg)
-{
-  while (1)
-  {
-    puts("PING\n");
-    for (int i = 0; i < 100000000; i++)
-      ;
-    sem_up(&spong);
-    sem_down(&sping);
-  }
-}
-
-void pong(void *arg)
-{
-  while (1)
-  {
-    puts("PONG\n");
-    for (int i = 0; i < 100000000; i++)
-      ;
-    sem_up(&sping);
-    sem_down(&spong);
+    irq_enable();
   }
 }
 
@@ -103,8 +88,8 @@ void main(unsigned int *mboot_info)
 
   puts("\n\n");
 
-  idt_setup_handler(0, empty_irq);
-  idt_setup_handler(1, empty_irq);
+  idt_setup_handler(0, timer_it);
+  idt_setup_handler(1, keyboard_it);
 
   __asm volatile("sti");
 
@@ -112,13 +97,14 @@ void main(unsigned int *mboot_info)
 
   puts("Hello world\n\n");
 
-  sem_init(&sping, -1);
-  sem_init(&spong, 0);
+  init_keymapping();
+  init_queue();
+
+  sem_init(&keyboard_sem, 0);
 
-  ctx_t *ping_ctx = create_ctx(ping, NULL);
-  ctx_t *pong_ctx = create_ctx(pong, NULL);
+  ctx_t *counter_ctx = create_ctx(&counter_handler, NULL);
+  ctx_t *keyboard_ctx = create_ctx(&keyboard_handler, NULL);
 
-  //sem_down(&sping);
   yield();
 
   for (;;)