#include <arch/irq.h>
#include <arch/ports.h>
#include <arch/acpi.h>
#include <socks/printk.h>
#include <socks/sched.h>
#include <socks/libc/string.h>
#include <socks/machine/irq.h>
#include <socks/machine/cpu.h>
#include <stddef.h>

#define MAX_ISR_HANDLERS 16

extern void _isr0();
extern void _isr1();
extern void _isr2();
extern void _isr3();
extern void _isr4();
extern void _isr5();
extern void _isr6();
extern void _isr7();
extern void _isr8();
extern void _isr9();
extern void _isr10();
extern void _isr11();
extern void _isr12();
extern void _isr13();
extern void _isr14();
extern void _isr15();
extern void _isr16();
extern void _isr17();
extern void _isr18();
extern void _isr19();
extern void _isr20();
extern void _isr21();
extern void _isr22();
extern void _isr23();
extern void _isr24();
extern void _isr25();
extern void _isr26();
extern void _isr27();
extern void _isr28();
extern void _isr29();
extern void _isr30();
extern void _isr31();
extern void _isr128();

extern void _irq0();
extern void _irq1();
extern void _irq2();
extern void _irq3();
extern void _irq4();
extern void _irq5();
extern void _irq6();
extern void _irq7();
extern void _irq8();
extern void _irq9();
extern void _irq10();
extern void _irq11();
extern void _irq12();
extern void _irq13();
extern void _irq14();
extern void _irq15();

extern void syscall_gate();
extern uintptr_t pf_faultptr(void);

static int_hook isr_handlers[NR_IDT_ENTRIES];
static struct queue irq_hooks[32];

static struct idt idt;
static int idt_initialised = 0;

static void set_idt_gate(struct idt *idt, uint8_t index, uintptr_t base, uint16_t sel, uint8_t flags)
{
	idt->i_entries[index].base_low    = base & 0xFFFF;
	idt->i_entries[index].base_middle = (base >> 16) & 0xFFFF;
	idt->i_entries[index].base_high   = (base >> 32) & 0xFFFFFFFF;
	idt->i_entries[index].selector    = sel;
	idt->i_entries[index].always0     = 0;
	idt->i_entries[index].present     = 1;
	idt->i_entries[index].dpl         = 3;
	idt->i_entries[index].zero        = 0;
	idt->i_entries[index].type        = 0xE;
	idt->i_entries[index].reserved    = 0;
}

static void gpf_handler(struct cpu_context *regs)
{
	int ext   = regs->err_no & 1;
	int table = (regs->err_no >> 1) & 2;
	int index = (regs->err_no >> 3) & 13;

	printk("general protection fault (%08x %08x %08x %016llx)", ext, table, index, regs->rip);
	ml_halt_cpu();
}

static void pf_handler(struct cpu_context *regs)
{
	printk("page fault (%016llx %016llx)", pf_faultptr(), regs->rip);
	ml_halt_cpu();
}

#if 0
static void set_syscall_gate(uintptr_t rip)
{
	/* sysret adds 0x10 to this to get cs, and 0x8 to get ss
	 * note that the CPU should force the RPL to 3 when loading
	 * the selector by using user_cs | 3. However, this doesn't happen
	 * in certain scenarios (specifically, QEMU + KVM on a Ryzen 5 1600X). */
	uint64_t user_cs = 0x13;
	uint64_t kernel_cs = 0x8;

	uintptr_t star_reg = 0xC0000081;
	uintptr_t lstar_reg = 0xC0000082;
	uintptr_t sfmask_reg = 0xC0000084;

	uint64_t selectors = 0;
	selectors |= (user_cs) << 48;
	selectors |= (kernel_cs) << 32;

	/* disable interrupts */
	uint64_t flag_mask = 0x200;

	write_msr(star_reg, selectors);
	write_msr(lstar_reg, rip);
	write_msr(sfmask_reg, flag_mask);
}
#endif

static void init_pic()
{
	// Remap the PIC
	outportb(0x20, 0x11);
	outportb(0xA0, 0x11);
	outportb(0x21, 0x20);
	outportb(0xA1, 0x28);
	outportb(0x21, 0x04);
	outportb(0xA1, 0x02);
	outportb(0x21, 0x01);
	outportb(0xA1, 0x01);
	outportb(0x21, 0x0);
	outportb(0xA1, 0x0);

	isr_handlers[13] = gpf_handler;
	isr_handlers[14] = pf_handler;
}

static void init_global_idt(void)
{
	memset((void *)&idt.i_entries, 0, sizeof idt.i_entries);

	set_idt_gate(&idt, 0, (uintptr_t)_isr0, 0x08, 0x8E);
	set_idt_gate(&idt, 1, (uintptr_t)_isr1, 0x08, 0x8E);
	set_idt_gate(&idt, 2, (uintptr_t)_isr2, 0x08, 0x8E);
	set_idt_gate(&idt, 3, (uintptr_t)_isr3, 0x08, 0x8E);
	set_idt_gate(&idt, 4, (uintptr_t)_isr4, 0x08, 0x8E);
	set_idt_gate(&idt, 5, (uintptr_t)_isr5, 0x08, 0x8E);
	set_idt_gate(&idt, 6, (uintptr_t)_isr6, 0x08, 0x8E);
	set_idt_gate(&idt, 7, (uintptr_t)_isr7, 0x08, 0x8E);
	set_idt_gate(&idt, 8, (uintptr_t)_isr8, 0x08, 0x8E);
	set_idt_gate(&idt, 9, (uintptr_t)_isr9, 0x08, 0x8E);
	set_idt_gate(&idt, 10, (uintptr_t)_isr10, 0x08, 0x8E);
	set_idt_gate(&idt, 11, (uintptr_t)_isr11, 0x08, 0x8E);
	set_idt_gate(&idt, 12, (uintptr_t)_isr12, 0x08, 0x8E);
	set_idt_gate(&idt, 13, (uintptr_t)_isr13, 0x08, 0x8E);
	set_idt_gate(&idt, 14, (uintptr_t)_isr14, 0x08, 0x8E);
	set_idt_gate(&idt, 15, (uintptr_t)_isr15, 0x08, 0x8E);
	set_idt_gate(&idt, 16, (uintptr_t)_isr16, 0x08, 0x8E);
	set_idt_gate(&idt, 17, (uintptr_t)_isr17, 0x08, 0x8E);
	set_idt_gate(&idt, 18, (uintptr_t)_isr18, 0x08, 0x8E);
	set_idt_gate(&idt, 19, (uintptr_t)_isr19, 0x08, 0x8E);
	set_idt_gate(&idt, 20, (uintptr_t)_isr20, 0x08, 0x8E);
	set_idt_gate(&idt, 21, (uintptr_t)_isr21, 0x08, 0x8E);
	set_idt_gate(&idt, 22, (uintptr_t)_isr22, 0x08, 0x8E);
	set_idt_gate(&idt, 23, (uintptr_t)_isr23, 0x08, 0x8E);
	set_idt_gate(&idt, 24, (uintptr_t)_isr24, 0x08, 0x8E);
	set_idt_gate(&idt, 25, (uintptr_t)_isr25, 0x08, 0x8E);
	set_idt_gate(&idt, 26, (uintptr_t)_isr26, 0x08, 0x8E);
	set_idt_gate(&idt, 27, (uintptr_t)_isr27, 0x08, 0x8E);
	set_idt_gate(&idt, 28, (uintptr_t)_isr28, 0x08, 0x8E);
	set_idt_gate(&idt, 29, (uintptr_t)_isr29, 0x08, 0x8E);
	set_idt_gate(&idt, 30, (uintptr_t)_isr30, 0x08, 0x8E);
	set_idt_gate(&idt, 31, (uintptr_t)_isr31, 0x08, 0x8E);

	init_pic();

	// Install the IRQs
	set_idt_gate(&idt, 32, (uintptr_t)_irq0, 0x08, 0x8E);
	set_idt_gate(&idt, 33, (uintptr_t)_irq1, 0x08, 0x8E);
	set_idt_gate(&idt, 34, (uintptr_t)_irq2, 0x08, 0x8E);
	set_idt_gate(&idt, 35, (uintptr_t)_irq3, 0x08, 0x8E);
	set_idt_gate(&idt, 36, (uintptr_t)_irq4, 0x08, 0x8E);
	set_idt_gate(&idt, 37, (uintptr_t)_irq5, 0x08, 0x8E);
	set_idt_gate(&idt, 38, (uintptr_t)_irq6, 0x08, 0x8E);
	set_idt_gate(&idt, 39, (uintptr_t)_irq7, 0x08, 0x8E);
	set_idt_gate(&idt, 40, (uintptr_t)_irq8, 0x08, 0x8E);
	set_idt_gate(&idt, 41, (uintptr_t)_irq9, 0x08, 0x8E);
	set_idt_gate(&idt, 42, (uintptr_t)_irq10, 0x08, 0x8E);
	set_idt_gate(&idt, 43, (uintptr_t)_irq11, 0x08, 0x8E);
	set_idt_gate(&idt, 44, (uintptr_t)_irq12, 0x08, 0x8E);
	set_idt_gate(&idt, 45, (uintptr_t)_irq13, 0x08, 0x8E);
	set_idt_gate(&idt, 46, (uintptr_t)_irq14, 0x08, 0x8E);
	set_idt_gate(&idt, 47, (uintptr_t)_irq15, 0x08, 0x8E);

	idt_initialised = 1;
}

int idt_init(struct idt_ptr *ptr)
{
	if (idt_initialised == 0) {
		init_global_idt();
	}

	ptr->i_limit = sizeof(idt) - 1;
	ptr->i_base  = (uintptr_t)&idt;

	return 0;
}

int idt_load(struct idt_ptr *ptr)
{
	__asm__ __volatile__("lidt (%0)" ::"r" (ptr));
	return 0;
}

void isr_dispatch(struct cpu_context *regs)
{
	int_hook h = isr_handlers[regs->int_no];
	if (h) {
		h(regs);
	}
}

void irq_dispatch(struct cpu_context *regs)
{
	end_charge_period();

	irq_ack(regs->int_no);
	struct queue *hooks = &irq_hooks[regs->int_no - IRQ0];
	queue_foreach(struct irq_hook, hook, hooks, irq_entry) {
		hook->irq_callback();
	}

	start_charge_period();
}

void syscall_dispatch(struct cpu_context *regs)
{

}

void hook_irq(enum irq_vector vec, struct irq_hook *hook)
{
	struct queue *hook_queue = &irq_hooks[vec - IRQ0];
	queue_push_back(hook_queue, &hook->irq_entry);
}

void unhook_irq(enum irq_vector vec, struct irq_hook *hook)
{
	struct queue *hook_queue = &irq_hooks[vec - IRQ0];
	queue_delete(hook_queue, &hook->irq_entry);
}