3 files changed, 142 insertions, 5 deletions

diff --git a/src/sys/arch/amd64/cpu/cpu_mp.c b/src/sys/arch/amd64/cpu/cpu_mp.c
index d95ba70..ae7b367 100644
--- a/src/sys/arch/amd64/cpu/cpu_mp.c
+++ b/src/sys/arch/amd64/cpu/cpu_mp.c
@@ -77,6 +77,9 @@ ap_entry(struct limine_smp_info *)
     corelist[ncores_up - 1] = pcore;
     atomic_inc_64(&ncores_up);
     spinlock_release(&lock);
+
+    md_proc_yield();
+    __builtin_unreachable();
     for (;;);
 }
 
diff --git a/src/sys/arch/amd64/isa/i8042.c b/src/sys/arch/amd64/isa/i8042.c
index 9bbd85d..9417206 100644
--- a/src/sys/arch/amd64/isa/i8042.c
+++ b/src/sys/arch/amd64/isa/i8042.c
@@ -30,6 +30,7 @@
 #include <sys/syslog.h>
 #include <sys/proc.h>
 #include <sys/errno.h>
+#include <sys/ascii.h>
 #include <os/module.h>
 #include <os/clkdev.h>
 #include <os/iotap.h>
@@ -58,6 +59,11 @@ struct keybuf {
 static struct iotap_ops tap_port0_ops;
 static struct iotap_desc tap_port0;
 
+/* Key states */
+static bool shift_key = false;
+static bool capslock = false;
+static bool capslock_released = true;
+
 static struct clkdev *clk;
 static struct spinlock lock;
 static struct keybuf buf = {0};
@@ -73,6 +79,26 @@ static char keytab[] = {
     'b', 'n', 'm', ',', '.',  '/', '\0', '\0', '\0', ' '
 };
 
+static char keytab_shift[] = {
+    '\0', '\0', '!', '@', '#',  '$', '%', '^',  '&', '*', '(', ')',
+    '_', '+', '\b', '\t', 'Q',  'W', 'E', 'R',  'T', 'Y', 'U', 'I',
+    'O', 'P', '{', '}', '\n',  '\0', 'A', 'S',  'D', 'F', 'G', 'H',
+    'J', 'K', 'L', ':', '\"', '~', '\0', '|', 'Z', 'X', 'C', 'V',
+    'B', 'N', 'M', '<', '>',  '?', '\0', '\0', '\0', ' '
+};
+
+static char keytab_caps[] = {
+    '\0', '\0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0',
+    '-','=', '\b', '\t', 'Q', 'W', 'E', 'R', 'T', 'Y', 'U', 'I',
+    'O', 'P', '[', ']', '\n', '\0', 'A', 'S', 'D', 'F', 'G', 'H',
+    'J', 'K', 'L', ';', '\'', '`', '\0', '\\', 'Z', 'X', 'C', 'V',
+    'B', 'N', 'M', ',', '.', '/', '\0', '\0', '\0', ' '
+};
+
+static uint8_t i8042_read(void);
+static void i8042_write(bool is_cmd, uint8_t v);
+static int i8042_devsend(bool aux, uint8_t data);
+
 /*
  * Compute the length of a kbp ring
  */
@@ -215,16 +241,111 @@ i8042_read(void)
 }
 
 /*
+ * Send data to an i8042 device
+ *
+ * @aux: If true, send data to the mouse
+ * @data: Byte to send to the bus
+ *
+ * Returns zero on success
+ */
+static int
+i8042_devsend(bool aux, uint8_t data)
+{
+    if (aux) {
+       i8042_write(true, I8042_PORT1_SEND);
+    }
+
+    i8042_write(false, data);
+    return i8042_read();
+}
+
+/*
+ * Send a keyboard LED mask to the controller
+ *
+ * @mask: Keyboard LED mask (I8042_LED_*)
+ */
+static void
+i8042_kbdled(uint8_t mask)
+{
+    i8042_devsend(false, 0xED);
+    i8042_devsend(false, mask);
+}
+
+/*
+ * Convert scancode to character
+ *
+ * @sc: Scancode
+ * @chr: Character output
+ *
+ * Returns 0 when a char is given back.
+ */
+static int
+i8042_getc(uint8_t sc, char *chr)
+{
+    bool release = ISSET(sc, BIT(7));
+    uint8_t led_mask;
+
+    switch (sc) {
+    case 0x76:
+        *chr = ASCII_ESC;
+        return 0;
+    /* Caps lock [press] */
+    case 0x3A:
+        if (!capslock_released) {
+            return -EAGAIN;
+        }
+        capslock_released = false;
+        capslock = !capslock;
+        i8042_kbdled(capslock << CAPS_LED_SHIFT);
+        return -EAGAIN;
+    /* Caps lock [release] */
+    case 0xBA:
+        capslock_released = true;
+        return -EAGAIN;
+    /* Shift */
+    case 0x36:
+    case 0xAA:
+    case 0x2A:
+    case 0xB6:
+        if (!release) {
+            shift_key = true;
+        } else {
+            shift_key = false;
+        }
+        return -EAGAIN;
+    }
+
+    if (release) {
+        return -EAGAIN;
+    }
+
+    if (capslock) {
+        *chr = keytab_caps[sc];
+        return 0;
+    }
+
+    if (shift_key) {
+        *chr = keytab_shift[sc];
+        return 0;
+    }
+
+    *chr = keytab[sc];
+    return 0;
+}
+
+/*
  * IRQ 1 handler
  */
 static int
 i8042_irq(struct intr_hand *hp)
 {
+    char c;
     uint8_t scancode;
+    int error;
 
     scancode = i8042_read();
-    if (!ISSET(scancode, 0x80)) {
-        keybuf_enter(&buf, scancode);
+    if (i8042_getc(scancode, &c) == 0) {
+        keybuf_enter(&buf, c);
     }
     return 1;
 }
@@ -292,7 +413,7 @@ static struct iotap_ops tap_port0_ops = {
 };
 
 static struct iotap_desc tap_port0 = {
-    .name = "i8042.port.0",
+    .name = "input.igkbd",
     .ops = &tap_port0_ops
 };
 
diff --git a/src/sys/arch/amd64/os/os_proc.c b/src/sys/arch/amd64/os/os_proc.c
index d944f41..b8e7c99 100644
--- a/src/sys/arch/amd64/os/os_proc.c
+++ b/src/sys/arch/amd64/os/os_proc.c
@@ -137,11 +137,24 @@ md_proc_init(struct proc *procp, int flags)
 __dead void
 md_proc_yield(void)
 {
-    /* Clear pending interrupts and oneshot */
+    struct proc *proc;
+    struct pcore *core = this_core();
+    int error;
+
     lapic_eoi();
-    lapic_timer_oneshot_us(9000);
 
+    /*
+     * Set the oneshot timer and check if there is
+     * any procs in our runqueues. If not, wait for
+     * it to fire again.
+     */
     for (;;) {
+        lapic_timer_oneshot_us(9000);
+        error = sched_deq(&core->scq, &proc);
+        if (error == 0) {
+            core->curproc = proc;
+            md_proc_kick(proc);
+        }
         __ASMV("sti; hlt");
     }
 }