kernel: ps2 initialisation

kernel-src/arch/x86-64/main.c

@@ -19,6 +19,7 @@
 #include <kernel/initrd.h>
 #include <kernel/devfs.h>
 #include <kernel/pseudodevices.h>
+#include <arch/ps2.h>
 
 static cpu_t bsp_cpu;
 
@@ -58,5 +59,7 @@ void kernel_entry() {
 
 	pseudodevices_init();
 
+	arch_ps2_init();
+
 	__assert(!"kernel entry end");
 }

kernel-src/arch/x86-64/ps2.c

@@ -0,0 +1,104 @@
+#include <arch/ps2.h>
+#include <printf.h>
+
+static bool reset_selftest(int port) {
+	uint8_t r = RESEND;
+	bool timeout = false;
+	int attempts = 0;
+
+	while (r == RESEND && attempts < 10) {
+		device_command(port, DEVICE_CMD_RESETSELFTEST);
+		r = read_data_timeout(5, &timeout);
+	}
+
+	if (attempts == 10)
+		timeout = true;
+
+	if (!timeout) {
+		if (read_data() != DEVICE_SELFTEST_OK)
+			printf("ps2: %s port device self test failed!\n", port == 1 ? "First" : "Second");
+		else
+			return true;
+	}
+
+	return false;
+}
+
+void arch_ps2_init() {
+	// XXX actually check if the controller is there
+	write_command(CTLR_CMD_DISABLEP1);
+	write_command(CTLR_CMD_DISABLEP2);
+
+	inb(PS2_PORT_DATA); // flush output buffer
+
+	write_command(CTLR_CMD_READCFG);
+	uint8_t control = read_data();
+
+	bool dualport = control & CONFIG_CLOCKP2;
+
+	control &= ~(CONFIG_IRQP1 | CONFIG_IRQP2 | CONFIG_TRANSLATION);
+
+	write_command(CTLR_CMD_WRITECFG);
+	write_data(control);
+
+	write_command(CTLR_CMD_SELFTEST);
+	if (read_data() != CTLR_SELFTEST_OK)
+		printf("ps2: controller self test failed.\n");
+
+	write_command(CTLR_CMD_WRITECFG);
+	write_data(control); // restore config if it was reset
+
+	// confirm existance of second port
+
+	write_command(CTLR_CMD_ENABLEP2);
+
+	write_command(CTLR_CMD_READCFG);
+	control = read_data();
+
+	if (dualport && (control & CONFIG_CLOCKP2)) // not enabled, not dual port.
+		dualport = false;
+	else
+		write_command(CTLR_CMD_DISABLEP2); // disable second port again
+
+	int workingflag = 0;
+
+	if (dualport) {
+		write_command(CTLR_CMD_P2SELFTEST);
+		if (read_data() == 0)
+			workingflag |= 2;
+		else
+			printf("ps2: econd port self test failed!\n");
+	}
+
+	if (workingflag == 0) {
+		printf("ps2: no working ports.\n");
+		return;
+	}
+
+	printf("ps2: controller with %d ports\n", dualport ? 2 : 1);
+
+	write_command(CTLR_CMD_ENABLEP1);
+
+	if (dualport)
+		write_command(CTLR_CMD_ENABLEP2);
+
+	// enable irqs and translation in config
+
+	write_command(CTLR_CMD_READCFG);
+	control = read_data();
+
+	control |= (dualport ? CONFIG_IRQP1 | CONFIG_IRQP2 : CONFIG_IRQP1) | CONFIG_TRANSLATION;
+
+	write_command(CTLR_CMD_WRITECFG);
+	write_data(control);
+
+	// reset and self test devices
+
+	int connectedflag = 0;
+
+	if (workingflag & 1 && reset_selftest(1))
+		connectedflag |= 1;
+
+	if (workingflag & 2 && reset_selftest(2))
+		connectedflag |= 2;
+}