#include <panic.h>
#include <arch/i686/drivers/ps2ctrl.h>
#include <arch/i686/pic.h>
#include <arch/i686/asm.h>

#define STATUS_OUT_BUF ((uint8_t)0x01)
#define STATUS_IN_BUF  ((uint8_t)0x02)

#define CONFIG_INT_0   ((uint8_t)0x01)
#define CONFIG_INT_1   ((uint8_t)0x02)
#define CONFIG_SYS     ((uint8_t)0x04)
#define CONFIG_CLOCK_0 ((uint8_t)0x10)
#define CONFIG_CLOCK_1 ((uint8_t)0x20)
#define CONFIG_TRANS   ((uint8_t)0x40)

static bool is_init = false;

uint8_t ps2ctrl_in(void) {
    while((ps2ctrl_in_status() & STATUS_OUT_BUF) == 0) {
		io_wait();
	}
	return inb(0x60);
}

uint8_t ps2ctrl_in_status(void) {
	return inb(0x64);
}

void ps2ctrl_out_cmd(uint8_t cmd) {
    while((ps2ctrl_in_status() & STATUS_IN_BUF) != 0) {
        io_wait();
    }
    outb(0x64, cmd);
}

void ps2ctrl_out_data(uint8_t data) {
    while((ps2ctrl_in_status() & STATUS_IN_BUF) != 0) {
		io_wait();
	}
	outb(0x60, data);
}

void ps2ctrl_set_port2(void) {
    outb(0x64, 0xD4);
}

void ps2ctrl_init(void) {

    is_init = false;

    pic_mask(1); // keyboard
    pic_mask(12); // mouse
   
    inb(0x60);
    
    // self-test
	ps2ctrl_out_cmd(0xAA);
	uint8_t response = ps2ctrl_in();
	if(response != 0x55) {
		panic("PS/2 controller failed to initialize");
	}

	// set config
	ps2ctrl_out_cmd(0x20);
	uint8_t config = ps2ctrl_in();
	config = (config | CONFIG_INT_0 | CONFIG_INT_1) & ~CONFIG_TRANS;
    // config = 0xFF;
	ps2ctrl_out_cmd(0x60);
	ps2ctrl_out_data(config);

	// enable port 0
	ps2ctrl_out_cmd(0xAE);

    // enable port 2
	ps2ctrl_out_cmd(0xA9);
    response = ps2ctrl_in();
    if (response == 0x01) {
        panic("PS/2 port 2 not supported");
    }

	ps2ctrl_out_cmd(0xA8);

    is_init = true;

    pic_unmask(1);
    pic_unmask(12);
}

bool ps2ctrl_is_init(void) {
	return is_init;
}