From 5b4e9dccd61f98f27382571f7a273e9ba9de8f8a Mon Sep 17 00:00:00 2001
From: Kat Richey <thedarkqueenaeris@zoho.com>
Date: Sat, 29 Oct 2022 10:06:37 -0500
Subject: [PATCH] Add PS/2 attempt

---
 arch/i386/init.c | 76 +++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 75 insertions(+), 1 deletion(-)

diff --git a/arch/i386/init.c b/arch/i386/init.c
index 7645b57..5e88ff3 100755
--- a/arch/i386/init.c
+++ b/arch/i386/init.c
@@ -3,6 +3,7 @@
 #include <string.h>
 #include <sys/io.h>
 #include "gdt.h"
+#include "ps2_kbd.h"
 
 int tss[16][2];
 
@@ -42,11 +43,84 @@ void setup_gdt(void) {
 
 extern void enter_pmode(void);
 
+int initialize_ps2_kbd() {
+  /* Should really make sure PS/2 controller exists here: */
+
+  /* Disable devices */
+  outb(0xAD, 0x64);
+  outb(0xA7, 0xAD);
+
+  /* Flush output buffer */
+  get_ps2_inbyte();
+
+  /* Set config byte */
+  outb(0x20, 0x64);
+  unsigned char config_byte = get_ps2_inbyte();
+  int is_dual_ch = (config_byte & 020) != 0 ? 1 : 0;
+  config_byte &= 0274;
+  outb(0x60, 0x64);
+  outb(config_byte, 0x60);
+
+  /* Quick self-test */
+  outb(0xAA, 0x64);
+  unsigned char test_response = get_ps2_inbyte();
+  if(test_response != 0x55) {
+    printf("error: PS/2 self-test failed\n");
+    return 1;
+  }
+  outb(0x60, 0x64);
+  outb(config_byte, 0x60);
+
+  /* Double check dual channel stuff */
+  if(is_dual_ch) {
+    outb(0xA8, 0x64);
+    outb(0x20, 0x64);
+    unsigned char new_config_byte = get_ps2_inbyte();
+    is_dual_ch = (new_config_byte & 020) == 0 ? 1 : 0;
+    if(is_dual_ch) {
+      outb(0xA7, 0x64);
+    }
+  }
+
+  /* Interface tests */
+  outb(0xAB, 0x64);
+  test_response = get_ps2_inbyte();
+  int working_ports = test_response == 0x00 ? 01 : 00;
+  if(test_response != 0x00 && !is_dual_ch) {
+    printf("error: could not initialize PS/2 device\n");
+    return 2;
+  }
+  if(is_dual_ch) {
+    outb(0xA9, 0x64);
+    test_response = get_ps2_inbyte();
+    working_ports |= test_response == 0x00 ? 02 : 00;
+    if(test_response != 0x00 && working_ports != 01) {
+      printf("error: could not initialize PS/2 device\n");
+      return 2;
+    }
+  }
+
+  /* Enable devices */
+  if(working_ports & 01) {
+    outb(0xAE, 0x64);
+    config_byte |= 01;
+  }
+  if(working_ports & 02) {
+    outb(0xA8, 0x64);
+    config_byte |= 02;
+  }
+  outb(0x60, 0x64);
+  outb(config_byte, 0x60);
+
+  return 0;
+}
+
 int init(void) {
   printf("Setting up GDT...\n");
   setup_gdt();
   printf("Enabling interrupts...\n");
   idt_init();
-  printf("Fenix Dev Pre-release v0.0.3\n");
+  printf("Initializing PS/2 controller...\n");
+  initialize_ps2_kbd();
   return 0;
 }