diff --git a/assembly/bios/driver_hd44780.asm b/assembly/bios/driver_hd44780.asm
index 7f83c2d..7485c46 100644
--- a/assembly/bios/driver_hd44780.asm
+++ b/assembly/bios/driver_hd44780.asm
@@ -61,6 +61,13 @@ Lcd_print:
     ld (lcd_cur_x), a   ; else set y pos to 0
     jp Lcd_print
 
+; Writes a single character at current cursror position
+; @param A Value of character to print
+Lcd_printc:
+    out (LCD_DATA_REG),a
+    ret
+
+
 ; Set cursor position
 ; @param B X-axis position (0 to 19)
 ; @param C Y-axis position (0 to 3)
diff --git a/assembly/bios/driver_keyboard.asm b/assembly/bios/driver_keyboard.asm
index dddf841..f3c0340 100644
--- a/assembly/bios/driver_keyboard.asm
+++ b/assembly/bios/driver_keyboard.asm
@@ -27,4 +27,24 @@
 ; Reads the keyboard
 ; @return: a 0-terminated array of keycodes representing the pressed keys
 Keyb_read:
-    in (KEYB_A0_REG),a
+    in a, (KEYB_A0_REG)
+    cp 0
+    jp z, _keyb_read_a1
+    add a, %01000000
+    call Lcd_printc     ; A already contains char to print
+    _loop:
+    in a, (KEYB_A0_REG)
+    cp 0
+    jp nz, _loop
+    ret
+    _keyb_read_a1:
+    in a, (KEYB_A1_REG)
+    cp 0
+    ret z
+    add a, %01010000
+    call Lcd_printc     ; A already contains char to print
+    _loop2:
+    in a, (KEYB_A1_REG)
+    cp 0
+    jp nz, _loop2
+    ret
diff --git a/assembly/bios/main.asm b/assembly/bios/main.asm
index f361321..f94b601 100644
--- a/assembly/bios/main.asm
+++ b/assembly/bios/main.asm
@@ -74,5 +74,5 @@ Sysinit:
     ; poll keyboard
     _poll_keyb:
     call Keyb_read
-    jp poll_keyb
+    jp _poll_keyb