mango/kexts/ps2kbd/main.c

#include <socks/printk.h>
#include <socks/device.h>
#include <socks/kext.h>
#include <socks/libc/stdio.h>
#include <arch/ports.h>
#include <arch/irq.h>

#define PS2_CMD_P1ON 0xae
#define PS2_CMD_P1OFF 0xad
#define PS2_CMD_P2ON 0xa8
#define PS2_CMD_P2OFF 0xa7

#define PS2_CFG_P1_IRQ (1 << 0)
#define PS2_CFG_P2_IRQ (1 << 1)
#define PS2_CFG_SYSFLAG (1 << 2)
#define PS2_CFG_ZERO1 (1 << 3)
#define PS2_CFG_P1_CLOCK (1 << 4)
#define PS2_CFG_P2_CLOCK (1 << 5)
#define PS2_CFG_P1_TRL (1 << 6)
#define PS2_CFG_ZERO2 (1 << 7)

#define PS2_SC2_PSBRK 0xe1
#define PS2_SC2_RELEASED 0xf0

#define NEXT_MAP 0xffffffff

#define PS2_ACK 0xfa
#define PS2_RESEND 0xfe

#define PS2_IO_DATA 0x60
#define PS2_IO_STATUS 0x64
#define PS2_IO_CMD 0x64

static struct input_device *keyboard = NULL, *mouse = NULL;

static void send_cmd(uint8_t cmd)
{
	/* prevent the compiler from optimising this away */
	volatile int _i = 0;
	while (inportb(PS2_IO_STATUS) & 0x2) {
		_i++;
	}

	outportb(PS2_IO_CMD, cmd);
}

static void send_data(uint8_t data)
{
	/* prevent the compiler from optimising this away */
	volatile int _i = 0;
	while (inportb(PS2_IO_STATUS) & 0x2) {
		_i++;
	}

	outportb(PS2_IO_DATA, data);
}

static uint8_t recv_data()
{
	/* prevent the compiler from optimising this away */
	volatile int _i = 0;
	while (!(inportb(PS2_IO_STATUS) & 0x1)) {
		_i++;
	}

	return inportb(PS2_IO_DATA);
}

static int data_avail()
{
	return inportb(PS2_IO_STATUS) & 0x1;
}

static void flush_buffer()
{
	while (data_avail()) {
		recv_data();
	}
}

static uint8_t read_config()
{
	send_cmd(0x20);
	return recv_data();
}

static void set_config(uint8_t cfg)
{
	send_cmd(0x60);
	send_data(cfg);
}

static int int_handler(void)
{
	static int scancode_level = 0;
	static bool released = false;

	if (scancode_level == PS2_SC2_PSBRK) {
		/* TODO pause break keycode */
		scancode_level = 0;
		return 0;
	}

	while (data_avail()) {
		unsigned char scancode = recv_data();

		/* the only scancode set that begins with 0xe1 is the Pause key.
		 * Rather than try and interpret a sequence of 8 codes, we just
		 * assume the pause key was pressed */
		if (scancode == PS2_SC2_PSBRK) {
			flush_buffer();
			/* TODO pause break keycode */
			//mdk_input_event ev = { .type = MDK_INPUT_DEVICE_KEYBOARD };
			scancode_level = PS2_SC2_PSBRK;
			break;
		}

		if (scancode == PS2_SC2_RELEASED) {
			released = true;
			continue;
		}

		printk("ps2: got scancode 0x%02x (%s)", scancode, released ? "up" : "down");
		scancode_level = 0;
	}

	return 0;
}

static int init_controller(void)
{
	/* disable ps2 devices */
	send_cmd(PS2_CMD_P1OFF);
	send_cmd(PS2_CMD_P2OFF);

	flush_buffer();

	/* configure the controller */
	uint8_t config = read_config();
	config &= ~(PS2_CFG_P1_IRQ | PS2_CFG_P2_IRQ | PS2_CFG_P1_TRL);
	set_config(config);

	send_cmd(0xaa);
	uint8_t response = recv_data();
	if (response != 0x55) {
		return -1;
	}

	set_config(config);

	/* Switch to scancode set 2 */
	send_data(0xf0);
	send_data(0x02);
	if (recv_data() != PS2_ACK) {
		return -1;
	}

	/* enable 1st port */
	send_cmd(PS2_CMD_P1ON);

	/* enable interrupts */
	config = read_config();
	config |= PS2_CFG_P1_IRQ;
	set_config(config);

	/* perform keyboard test */
	send_data(0xff);
	uint8_t response1 = recv_data();
	uint8_t response2 = recv_data();
	if ((response1 != 0xaa && response2 != 0xfa) &&
            (response1 != 0xfa && response2 != 0xaa)) {
		return -1;
	}

	return 0;
}

static kern_status_t create_devices(void)
{
	struct input_device *kbd = input_device_create();
	struct device *kbd_base = input_device_base(kbd);
	snprintf(kbd_base->dev_name, sizeof kbd_base->dev_name, "ps2kbd");

	struct input_device *ms = input_device_create();
	struct device *ms_base = input_device_base(ms);
	snprintf(ms_base->dev_name, sizeof ms_base->dev_name, "ps2ms");

	kern_status_t status = device_register(kbd_base, misc_device());
	if (status != KERN_OK) {
		/* TODO destroy devices */
		return status;
	}

	status = device_register(ms_base, misc_device());
	if (status != KERN_OK) {
		return status;
	}

	keyboard = kbd;
	mouse = ms;

	return KERN_OK;
}

static struct irq_hook ps2_hook = {
	.irq_callback = int_handler,
};

static kern_status_t online(struct kext *self)
{
	int result = init_controller();
	if (result != 0) {
		return KERN_UNSUPPORTED;
	}

	kern_status_t status = create_devices();
	if (status != KERN_OK) {
		return status;
	}

	hook_irq(IRQ1, &ps2_hook);
	printk("ps2: controller initialised");
	return KERN_OK;
}

DEFINE_KEXT("net.doorstuck.socks.ps2kbd",
	    online, NULL,
	    KEXT_NO_DEPENDENCIES);