#include <socks/printk.h>
#include <socks/vm.h>
#include <socks/cpu.h>
#include <arch/acpi.h>
#include <arch/msr.h>
#include <arch/ports.h>
#include <arch/pit.h>

#define PIC1_DATA 0x21
#define PIC2_DATA 0xA1

#define IA32_APIC_BASE_MSR 0x1B
#define IA32_APIC_BASE_MSR_BSP 0x100
#define IA32_APIC_BASE_MSR_ENABLE 0x800

static uint32_t *lapic_base;

extern int check_apic(void);

static uintptr_t apic_get_base(void)
{
	return rdmsr(IA32_APIC_BASE_MSR);
}

static void apic_set_base(uintptr_t addr)
{
	wrmsr(IA32_APIC_BASE_MSR, addr);
}

static void disable_8259(void)
{
	/* mask all interrupts on PICs 1 and 2 */
	outportb(PIC1_DATA, 0xFF);
	outportb(PIC2_DATA, 0xFF);
}

static uint32_t local_apic_read(uint32_t reg)
{
	return READ_ONCE(lapic_base[reg >> 4]);
}

static void local_apic_write(uint32_t reg, uint32_t val)
{

}

static void *find_lapic(struct acpi_madt *madt)
{
	phys_addr_t local_apic = madt->m_lapic_ptr;

	unsigned char *p = (unsigned char *)madt + sizeof *madt;
	unsigned char *madt_end = (unsigned char *)madt + madt->m_base.s_length;

	while (p < madt_end) {
		struct acpi_madt_record *rec = (struct acpi_madt_record *)p;
		struct lapic_override_record *lapic;

		switch (rec->r_type) {
		case ACPI_MADT_LAPIC_OVERRIDE:
			lapic = (struct lapic_override_record *)(rec + 1);
			return vm_phys_to_virt(lapic->l_lapic_ptr);
		default:
			break;
		}

		p += rec->r_length;
	}

	return vm_phys_to_virt(local_apic);
}


kern_status_t local_apic_enable(void)
{
	struct acpi_madt *madt = (struct acpi_madt *)acpi_find_sdt(ACPI_SIG_MADT);
	if (!madt) {
		return KERN_UNSUPPORTED;
	}

	apic_set_base(apic_get_base());
	lapic_base = find_lapic(madt);

	local_apic_write(0xF0, local_apic_read(0xF0) | 0x100);

	printk("acpi: enabled local APIC on core %u", this_cpu());
	return KERN_OK;
}

static void configure_legacy_pic(void)
{
	printk("acpi: APIC unavailable, using 8259 PIC");
	pit_init(1000);
}

kern_status_t apic_init(void)
{
	if (check_apic() == 0) {
		configure_legacy_pic();
		return KERN_UNSUPPORTED;
	}

	if (local_apic_enable() != KERN_OK) {
		configure_legacy_pic();
		return KERN_UNSUPPORTED;
	}

	disable_8259();
	return KERN_OK;
}