#include <arch/irq.h>
#include <arch/ports.h>
#include <mango/cpu.h>
#include <mango/libc/string.h>
#include <mango/machine/cpu.h>
#include <mango/machine/irq.h>
#include <mango/panic.h>
#include <mango/sched.h>
#include <stddef.h>

#define MAX_ISR_HANDLERS 16

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

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

static struct idt idt;
static int idt_initialised = 0;
static uintptr_t int_entry_points[NR_IDT_ENTRIES];

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) & 0x03;
	int index = (regs->err_no >> 3) & 0x1FFF;

	panic_irq(
		regs,
		"general protection fault (%016llx %02x %02x %04x %016llx)",
		regs->err_no,
		ext,
		table,
		index,
		regs->rip);
}

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

#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(void)
{
	// 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);

	for (int i = 0; i < NR_IDT_ENTRIES; i++) {
		set_idt_gate(&idt, i, int_entry_points[i], 0x08, 0x8E);
	}

	init_pic();

	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);
	} else {
		panic_irq(regs, "unhandled exception %u", regs->int_no);
	}
}

void irq_ack(unsigned int vec)
{
	if (vec >= 40) {
		outportb(0xA0, 0x20);
	}

	outportb(0x20, 0x20);
}

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();
	}

	if (need_resched()) {
		schedule(SCHED_IRQ);
	}

	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);
}

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 _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 _irq16();
extern void _irq17();
extern void _irq18();
extern void _irq19();
extern void _irq20();
extern void _irq21();
extern void _irq22();
extern void _irq23();
extern void _irq24();
extern void _irq25();
extern void _irq26();
extern void _irq27();
extern void _irq28();
extern void _irq29();
extern void _irq30();
extern void _irq31();
extern void _irq32();
extern void _irq33();
extern void _irq34();
extern void _irq35();
extern void _irq36();
extern void _irq37();
extern void _irq38();
extern void _irq39();
extern void _irq40();
extern void _irq41();
extern void _irq42();
extern void _irq43();
extern void _irq44();
extern void _irq45();
extern void _irq46();
extern void _irq47();
extern void _irq48();
extern void _irq49();
extern void _irq50();
extern void _irq51();
extern void _irq52();
extern void _irq53();
extern void _irq54();
extern void _irq55();
extern void _irq56();
extern void _irq57();
extern void _irq58();
extern void _irq59();
extern void _irq60();
extern void _irq61();
extern void _irq62();
extern void _irq63();
extern void _irq64();
extern void _irq65();
extern void _irq66();
extern void _irq67();
extern void _irq68();
extern void _irq69();
extern void _irq70();
extern void _irq71();
extern void _irq72();
extern void _irq73();
extern void _irq74();
extern void _irq75();
extern void _irq76();
extern void _irq77();
extern void _irq78();
extern void _irq79();
extern void _irq80();
extern void _irq81();
extern void _irq82();
extern void _irq83();
extern void _irq84();
extern void _irq85();
extern void _irq86();
extern void _irq87();
extern void _irq88();
extern void _irq89();
extern void _irq90();
extern void _irq91();
extern void _irq92();
extern void _irq93();
extern void _irq94();
extern void _irq95();
extern void _irq96();
extern void _irq97();
extern void _irq98();
extern void _irq99();
extern void _irq100();
extern void _irq101();
extern void _irq102();
extern void _irq103();
extern void _irq104();
extern void _irq105();
extern void _irq106();
extern void _irq107();
extern void _irq108();
extern void _irq109();
extern void _irq110();
extern void _irq111();
extern void _irq112();
extern void _irq113();
extern void _irq114();
extern void _irq115();
extern void _irq116();
extern void _irq117();
extern void _irq118();
extern void _irq119();
extern void _irq120();
extern void _irq121();
extern void _irq122();
extern void _irq123();
extern void _irq124();
extern void _irq125();
extern void _irq126();
extern void _irq127();
extern void _irq128();
extern void _irq129();
extern void _irq130();
extern void _irq131();
extern void _irq132();
extern void _irq133();
extern void _irq134();
extern void _irq135();
extern void _irq136();
extern void _irq137();
extern void _irq138();
extern void _irq139();
extern void _irq140();
extern void _irq141();
extern void _irq142();
extern void _irq143();
extern void _irq144();
extern void _irq145();
extern void _irq146();
extern void _irq147();
extern void _irq148();
extern void _irq149();
extern void _irq150();
extern void _irq151();
extern void _irq152();
extern void _irq153();
extern void _irq154();
extern void _irq155();
extern void _irq156();
extern void _irq157();
extern void _irq158();
extern void _irq159();
extern void _irq160();
extern void _irq161();
extern void _irq162();
extern void _irq163();
extern void _irq164();
extern void _irq165();
extern void _irq166();
extern void _irq167();
extern void _irq168();
extern void _irq169();
extern void _irq170();
extern void _irq171();
extern void _irq172();
extern void _irq173();
extern void _irq174();
extern void _irq175();
extern void _irq176();
extern void _irq177();
extern void _irq178();
extern void _irq179();
extern void _irq180();
extern void _irq181();
extern void _irq182();
extern void _irq183();
extern void _irq184();
extern void _irq185();
extern void _irq186();
extern void _irq187();
extern void _irq188();
extern void _irq189();
extern void _irq190();
extern void _irq191();
extern void _irq192();
extern void _irq193();
extern void _irq194();
extern void _irq195();
extern void _irq196();
extern void _irq197();
extern void _irq198();
extern void _irq199();
extern void _irq200();
extern void _irq201();
extern void _irq202();
extern void _irq203();
extern void _irq204();
extern void _irq205();
extern void _irq206();
extern void _irq207();
extern void _irq208();
extern void _irq209();
extern void _irq210();
extern void _irq211();
extern void _irq212();
extern void _irq213();
extern void _irq214();
extern void _irq215();
extern void _irq216();
extern void _irq217();
extern void _irq218();
extern void _irq219();
extern void _irq220();
extern void _irq221();
extern void _irq222();
extern void _irq223();

static uintptr_t int_entry_points[NR_IDT_ENTRIES] = {
	[0] = (uintptr_t)_isr0,     [1] = (uintptr_t)_isr1,
	[2] = (uintptr_t)_isr2,     [3] = (uintptr_t)_isr3,
	[4] = (uintptr_t)_isr4,     [5] = (uintptr_t)_isr5,
	[6] = (uintptr_t)_isr6,     [7] = (uintptr_t)_isr7,
	[8] = (uintptr_t)_isr8,     [9] = (uintptr_t)_isr9,
	[10] = (uintptr_t)_isr10,   [11] = (uintptr_t)_isr11,
	[12] = (uintptr_t)_isr12,   [13] = (uintptr_t)_isr13,
	[14] = (uintptr_t)_isr14,   [15] = (uintptr_t)_isr15,
	[16] = (uintptr_t)_isr16,   [17] = (uintptr_t)_isr17,
	[18] = (uintptr_t)_isr18,   [19] = (uintptr_t)_isr19,
	[20] = (uintptr_t)_isr20,   [21] = (uintptr_t)_isr21,
	[22] = (uintptr_t)_isr22,   [23] = (uintptr_t)_isr23,
	[24] = (uintptr_t)_isr24,   [25] = (uintptr_t)_isr25,
	[26] = (uintptr_t)_isr26,   [27] = (uintptr_t)_isr27,
	[28] = (uintptr_t)_isr28,   [29] = (uintptr_t)_isr29,
	[30] = (uintptr_t)_isr30,   [31] = (uintptr_t)_isr31,
	[32] = (uintptr_t)_irq0,    [33] = (uintptr_t)_irq1,
	[34] = (uintptr_t)_irq2,    [35] = (uintptr_t)_irq3,
	[36] = (uintptr_t)_irq4,    [37] = (uintptr_t)_irq5,
	[38] = (uintptr_t)_irq6,    [39] = (uintptr_t)_irq7,
	[40] = (uintptr_t)_irq8,    [41] = (uintptr_t)_irq9,
	[42] = (uintptr_t)_irq10,   [43] = (uintptr_t)_irq11,
	[44] = (uintptr_t)_irq12,   [45] = (uintptr_t)_irq13,
	[46] = (uintptr_t)_irq14,   [47] = (uintptr_t)_irq15,
	[48] = (uintptr_t)_irq16,   [49] = (uintptr_t)_irq17,
	[50] = (uintptr_t)_irq18,   [51] = (uintptr_t)_irq19,
	[52] = (uintptr_t)_irq20,   [53] = (uintptr_t)_irq21,
	[54] = (uintptr_t)_irq22,   [55] = (uintptr_t)_irq23,
	[56] = (uintptr_t)_irq24,   [57] = (uintptr_t)_irq25,
	[58] = (uintptr_t)_irq26,   [59] = (uintptr_t)_irq27,
	[60] = (uintptr_t)_irq28,   [61] = (uintptr_t)_irq29,
	[62] = (uintptr_t)_irq30,   [63] = (uintptr_t)_irq31,
	[64] = (uintptr_t)_irq32,   [65] = (uintptr_t)_irq33,
	[66] = (uintptr_t)_irq34,   [67] = (uintptr_t)_irq35,
	[68] = (uintptr_t)_irq36,   [69] = (uintptr_t)_irq37,
	[70] = (uintptr_t)_irq38,   [71] = (uintptr_t)_irq39,
	[72] = (uintptr_t)_irq40,   [73] = (uintptr_t)_irq41,
	[74] = (uintptr_t)_irq42,   [75] = (uintptr_t)_irq43,
	[76] = (uintptr_t)_irq44,   [77] = (uintptr_t)_irq45,
	[78] = (uintptr_t)_irq46,   [79] = (uintptr_t)_irq47,
	[80] = (uintptr_t)_irq48,   [81] = (uintptr_t)_irq49,
	[82] = (uintptr_t)_irq50,   [83] = (uintptr_t)_irq51,
	[84] = (uintptr_t)_irq52,   [85] = (uintptr_t)_irq53,
	[86] = (uintptr_t)_irq54,   [87] = (uintptr_t)_irq55,
	[88] = (uintptr_t)_irq56,   [89] = (uintptr_t)_irq57,
	[90] = (uintptr_t)_irq58,   [91] = (uintptr_t)_irq59,
	[92] = (uintptr_t)_irq60,   [93] = (uintptr_t)_irq61,
	[94] = (uintptr_t)_irq62,   [95] = (uintptr_t)_irq63,
	[96] = (uintptr_t)_irq64,   [97] = (uintptr_t)_irq65,
	[98] = (uintptr_t)_irq66,   [99] = (uintptr_t)_irq67,
	[100] = (uintptr_t)_irq68,  [101] = (uintptr_t)_irq69,
	[102] = (uintptr_t)_irq70,  [103] = (uintptr_t)_irq71,
	[104] = (uintptr_t)_irq72,  [105] = (uintptr_t)_irq73,
	[106] = (uintptr_t)_irq74,  [107] = (uintptr_t)_irq75,
	[108] = (uintptr_t)_irq76,  [109] = (uintptr_t)_irq77,
	[110] = (uintptr_t)_irq78,  [111] = (uintptr_t)_irq79,
	[112] = (uintptr_t)_irq80,  [113] = (uintptr_t)_irq81,
	[114] = (uintptr_t)_irq82,  [115] = (uintptr_t)_irq83,
	[116] = (uintptr_t)_irq84,  [117] = (uintptr_t)_irq85,
	[118] = (uintptr_t)_irq86,  [119] = (uintptr_t)_irq87,
	[120] = (uintptr_t)_irq88,  [121] = (uintptr_t)_irq89,
	[122] = (uintptr_t)_irq90,  [123] = (uintptr_t)_irq91,
	[124] = (uintptr_t)_irq92,  [125] = (uintptr_t)_irq93,
	[126] = (uintptr_t)_irq94,  [127] = (uintptr_t)_irq95,
	[128] = (uintptr_t)_irq96,  [129] = (uintptr_t)_irq97,
	[130] = (uintptr_t)_irq98,  [131] = (uintptr_t)_irq99,
	[132] = (uintptr_t)_irq100, [133] = (uintptr_t)_irq101,
	[134] = (uintptr_t)_irq102, [135] = (uintptr_t)_irq103,
	[136] = (uintptr_t)_irq104, [137] = (uintptr_t)_irq105,
	[138] = (uintptr_t)_irq106, [139] = (uintptr_t)_irq107,
	[140] = (uintptr_t)_irq108, [141] = (uintptr_t)_irq109,
	[142] = (uintptr_t)_irq110, [143] = (uintptr_t)_irq111,
	[144] = (uintptr_t)_irq112, [145] = (uintptr_t)_irq113,
	[146] = (uintptr_t)_irq114, [147] = (uintptr_t)_irq115,
	[148] = (uintptr_t)_irq116, [149] = (uintptr_t)_irq117,
	[150] = (uintptr_t)_irq118, [151] = (uintptr_t)_irq119,
	[152] = (uintptr_t)_irq120, [153] = (uintptr_t)_irq121,
	[154] = (uintptr_t)_irq122, [155] = (uintptr_t)_irq123,
	[156] = (uintptr_t)_irq124, [157] = (uintptr_t)_irq125,
	[158] = (uintptr_t)_irq126, [159] = (uintptr_t)_irq127,
	[160] = (uintptr_t)_irq128, [161] = (uintptr_t)_irq129,
	[162] = (uintptr_t)_irq130, [163] = (uintptr_t)_irq131,
	[164] = (uintptr_t)_irq132, [165] = (uintptr_t)_irq133,
	[166] = (uintptr_t)_irq134, [167] = (uintptr_t)_irq135,
	[168] = (uintptr_t)_irq136, [169] = (uintptr_t)_irq137,
	[170] = (uintptr_t)_irq138, [171] = (uintptr_t)_irq139,
	[172] = (uintptr_t)_irq140, [173] = (uintptr_t)_irq141,
	[174] = (uintptr_t)_irq142, [175] = (uintptr_t)_irq143,
	[176] = (uintptr_t)_irq144, [177] = (uintptr_t)_irq145,
	[178] = (uintptr_t)_irq146, [179] = (uintptr_t)_irq147,
	[180] = (uintptr_t)_irq148, [181] = (uintptr_t)_irq149,
	[182] = (uintptr_t)_irq150, [183] = (uintptr_t)_irq151,
	[184] = (uintptr_t)_irq152, [185] = (uintptr_t)_irq153,
	[186] = (uintptr_t)_irq154, [187] = (uintptr_t)_irq155,
	[188] = (uintptr_t)_irq156, [189] = (uintptr_t)_irq157,
	[190] = (uintptr_t)_irq158, [191] = (uintptr_t)_irq159,
	[192] = (uintptr_t)_irq160, [193] = (uintptr_t)_irq161,
	[194] = (uintptr_t)_irq162, [195] = (uintptr_t)_irq163,
	[196] = (uintptr_t)_irq164, [197] = (uintptr_t)_irq165,
	[198] = (uintptr_t)_irq166, [199] = (uintptr_t)_irq167,
	[200] = (uintptr_t)_irq168, [201] = (uintptr_t)_irq169,
	[202] = (uintptr_t)_irq170, [203] = (uintptr_t)_irq171,
	[204] = (uintptr_t)_irq172, [205] = (uintptr_t)_irq173,
	[206] = (uintptr_t)_irq174, [207] = (uintptr_t)_irq175,
	[208] = (uintptr_t)_irq176, [209] = (uintptr_t)_irq177,
	[210] = (uintptr_t)_irq178, [211] = (uintptr_t)_irq179,
	[212] = (uintptr_t)_irq180, [213] = (uintptr_t)_irq181,
	[214] = (uintptr_t)_irq182, [215] = (uintptr_t)_irq183,
	[216] = (uintptr_t)_irq184, [217] = (uintptr_t)_irq185,
	[218] = (uintptr_t)_irq186, [219] = (uintptr_t)_irq187,
	[220] = (uintptr_t)_irq188, [221] = (uintptr_t)_irq189,
	[222] = (uintptr_t)_irq190, [223] = (uintptr_t)_irq191,
	[224] = (uintptr_t)_irq192, [225] = (uintptr_t)_irq193,
	[226] = (uintptr_t)_irq194, [227] = (uintptr_t)_irq195,
	[228] = (uintptr_t)_irq196, [229] = (uintptr_t)_irq197,
	[230] = (uintptr_t)_irq198, [231] = (uintptr_t)_irq199,
	[232] = (uintptr_t)_irq200, [233] = (uintptr_t)_irq201,
	[234] = (uintptr_t)_irq202, [235] = (uintptr_t)_irq203,
	[236] = (uintptr_t)_irq204, [237] = (uintptr_t)_irq205,
	[238] = (uintptr_t)_irq206, [239] = (uintptr_t)_irq207,
	[240] = (uintptr_t)_irq208, [241] = (uintptr_t)_irq209,
	[242] = (uintptr_t)_irq210, [243] = (uintptr_t)_irq211,
	[244] = (uintptr_t)_irq212, [245] = (uintptr_t)_irq213,
	[246] = (uintptr_t)_irq214, [247] = (uintptr_t)_irq215,
	[248] = (uintptr_t)_irq216, [249] = (uintptr_t)_irq217,
	[250] = (uintptr_t)_irq218, [251] = (uintptr_t)_irq219,
	[252] = (uintptr_t)_irq220, [253] = (uintptr_t)_irq221,
	[254] = (uintptr_t)_irq222, [255] = (uintptr_t)_irq223,
};