#include <efi.h>
#include <efilib.h>
#include <efiapi.h>

#include <cpuid.h>
#include <x86/msr.h>
#include <x86/cr.h>
#include <x86/idt.h>
#include <x86/gdt.h>
#include <x86/vmx.h>

#include "virt.h"
#include "std.h"
#include "procvisor.h"

static struct VMXON *vmxon_region;
static struct VMCS  *vmcs_region;

static bool
vintel_has_bios_support(void)
{
	uint64_t value = readmsr64(IA32_FEATURE_CONTROL);
	return (value & 0x5) == 0x5;
}

static bool
vintel_has_cpu_support(void)
{
	unsigned info[4];
	__get_cpuid(0x1, &info[0], &info[1], &info[2], &info[3]);
	return (info[2] & (1 << 5)) != 0;
}

bool
vintel_has_support(char *err, size_t errmax)
{
	if (!vintel_has_cpu_support()) {
		strlcpy(err, "CPU does not support VT-x.", errmax);
		return false;
	}

	if (!vintel_has_bios_support()) {
		strlcpy(err, "VT-x support is not enabled in BIOS.", errmax);
		return false;
	}

	return true;
}

static void
vintel_fix_cr_bits(void)
{
	uint64_t cr0_fixed0 = readmsr64(IA32_VMX_CR0_FIXED0);
	uint64_t cr0_fixed1 = readmsr64(IA32_VMX_CR0_FIXED1);
	uint64_t cr4_fixed0 = readmsr64(IA32_VMX_CR4_FIXED0);
	uint64_t cr4_fixed1 = readmsr64(IA32_VMX_CR4_FIXED1);

	writecr0((readcr0() | cr0_fixed0) & cr0_fixed1);
	writecr4((readcr4() | cr4_fixed0) & cr4_fixed1);
}

static void
checked_vmwrite(uint64_t field, uint64_t value)
{
	uint64_t status = vmwrite(field, value);
	if (status & (1 << 0)) {
		AsciiPrint("vmwrite(%lx): Invalid VMCS Pointer\n", field);
	}
	if (status & (1 << 6)) {
		AsciiPrint("vmwrite(%lx): %a\n", vmcs_describe_error());
	}
}

static uint64_t
vmx_adjust_controls(uint64_t value, uint32_t msr)
{
    uint64_t msrValue = readmsr64(msr);
    value = ((msrValue & 0xffffffff) | value) & (msrValue >> 32);
    return value;
}

static void
vmx_init_execution_controls(void)
{
    uint64_t vmxBasic = readmsr64(IA32_VMX_BASIC);
    uint64_t pinBasedMsr = ((vmxBasic >> 55) & 1) ? IA32_VMX_TRUE_PINBASED_CTLS : IA32_VMX_PINBASED_CTLS;
    uint64_t procBasedMsr = ((vmxBasic >> 55) & 1) ? IA32_VMX_TRUE_PROCBASED_CTLS : IA32_VMX_PROCBASED_CTLS;
    uint64_t procBasedMsr2 = IA32_VMX_PROCBASED_CTLS2;

    uint64_t pinBasedControls  = vmx_adjust_controls(0, pinBasedMsr);
    uint64_t procBasedControls = vmx_adjust_controls(0, procBasedMsr);
    uint64_t procBasedControls2 = vmx_adjust_controls(0, procBasedMsr2);

    checked_vmwrite(PIN_BASED_VM_EXEC_CONTROL, pinBasedControls);
    checked_vmwrite(CPU_BASED_VM_EXEC_CONTROL, procBasedControls);
    checked_vmwrite(CPU_BASED_VM_EXEC_CONTROL2, procBasedControls2);

    AsciiPrint("PIN_BASED_VM_EXEC_CONTROL: %lx\n", pinBasedControls);
    AsciiPrint("CPU_BASED_VM_EXEC_CONTROL: %lx\n", procBasedControls);
    AsciiPrint("CPU_BASED_VM_EXEC_CONTROL2: %lx\n", procBasedControls2);
}

// HACK
char guest_stack[4096];

void
guest_main(void)
{
	AsciiPrint("Hello from guest!\n");

	for (;;) {}
}

static void
vintel_write_guest_segment(struct GDTR gdtr, int reg, int ss, uint32_t hibase)
{
	struct segdescr *descr = &gdtr.base[ss >> 3];

	checked_vmwrite(GUEST_ES_SELECTOR + 2 * reg, ss & 0xfff8);
	checked_vmwrite(GUEST_ES_BASE     + 2 * reg, getsegbase(descr) | ((uint64_t)hibase << 32));
	checked_vmwrite(GUEST_ES_LIMIT    + 2 * reg, getseglimit(descr));
	checked_vmwrite(GUEST_ES_ACCESS   + 2 * reg, getsegaccess(descr, false));
}

static void
vintel_init_guest(void)
{
	struct GDTR gdtr = storegdt();
	struct IDTR idtr = storeidt();

	uint64_t guestSP = (uintptr_t)guest_stack + 4096;
	uint64_t guestIP = (uintptr_t)(void *)guest_main;

	// Set segment selectors
	vintel_write_guest_segment(gdtr, 0, reades(), 0);
	vintel_write_guest_segment(gdtr, 1, readcs(), 0);
	vintel_write_guest_segment(gdtr, 2, readss(), 0);
	vintel_write_guest_segment(gdtr, 3, readds(), 0);
	vintel_write_guest_segment(gdtr, 4, readfs(), 0);
	vintel_write_guest_segment(gdtr, 5, readgs(), 0);
	
	/* VMX requires a usable task register.
	 * UEFI requires us to use their GDT, which does not include a task register.
	 * So we try something stupid: load temporary but valid values into the TR shadow registers.
	 * Then later reload the GDT to make the shadow registers consistent again.
	 * Sigh.
	 */
	checked_vmwrite(GUEST_ES_SELECTOR + 2 * 7, readtr() & 0xfff8);
	checked_vmwrite(GUEST_ES_BASE + 2 * 7, (uintptr_t)myprocvisor()->host_tss); // just abuse the host's TSS for this
	checked_vmwrite(GUEST_ES_LIMIT + 2 * 7, sizeof (struct TSS) - 1);
	checked_vmwrite(GUEST_ES_ACCESS + 2 * 7, 0x00000089);

	// Set control registers
	checked_vmwrite(GUEST_CR0, readcr0());
	checked_vmwrite(GUEST_CR3, readcr3());
	checked_vmwrite(GUEST_CR4, readcr4());

	checked_vmwrite(GUEST_DR7,           readdr7());
	checked_vmwrite(GUEST_RFLAGS,        readflags());
	checked_vmwrite(GUEST_RSP,           guestSP);
	checked_vmwrite(GUEST_RIP,           guestIP);
	checked_vmwrite(VMCS_LINK_POINTER,   -1LL);
	checked_vmwrite(GUEST_IA32_DEBUGCTL, readmsr64(IA32_DEBUGCTL));
	checked_vmwrite(GUEST_IA32_PAT,      readmsr64(IA32_PAT));
	checked_vmwrite(GUEST_IA32_EFER,     readmsr64(IA32_EFER));
	checked_vmwrite(GUEST_FS_BASE,       readmsr64(IA32_FS_BASE));
	checked_vmwrite(GUEST_GS_BASE,       readmsr64(IA32_GS_BASE));
	checked_vmwrite(GUEST_SYSENTER_CS,   readmsr64(0x174));
	checked_vmwrite(GUEST_SYSENTER_ESP,  readmsr64(0x175));
	checked_vmwrite(GUEST_SYSENTER_EIP,  readmsr64(0x176));

	// Set GDT and IDT
	checked_vmwrite(GUEST_GDTR_BASE, (uintptr_t)gdtr.base);
	checked_vmwrite(GUEST_IDTR_BASE, (uintptr_t)idtr.base);
}

static void
vintel_init_host(void)
{
	struct procvisor *pv = myprocvisor();

	uint64_t hostSP = (uintptr_t)pv->host_stack;
	uint64_t hostIP = 0x0;

	// Set segment selectors
	checked_vmwrite(HOST_ES_SELECTOR, 0x10);
	checked_vmwrite(HOST_CS_SELECTOR, 0x08);
	checked_vmwrite(HOST_SS_SELECTOR, 0x10);
	checked_vmwrite(HOST_DS_SELECTOR, 0x10);
	checked_vmwrite(HOST_FS_SELECTOR, 0x00);
	checked_vmwrite(HOST_GS_SELECTOR, 0x00);
	checked_vmwrite(HOST_TR_SELECTOR, 0x18);

	// Set control registers
	checked_vmwrite(HOST_CR0, readcr0());
	checked_vmwrite(HOST_CR3, readcr3());
	checked_vmwrite(HOST_CR4, readcr4());

	// Set RSP and RIP
	checked_vmwrite(HOST_RSP, hostSP);
	checked_vmwrite(HOST_RIP, hostIP);

	// Set MSRs
	checked_vmwrite(HOST_IA32_PAT,  readmsr64(IA32_PAT));
	checked_vmwrite(HOST_IA32_EFER, readmsr64(IA32_EFER));

	// Set GDT and IDT
	checked_vmwrite(HOST_GDTR_BASE, (uintptr_t)pv->host_gdtr.base);
	checked_vmwrite(HOST_IDTR_BASE, (uintptr_t)pv->host_idtr.base);

	// Dummy sysenter & sysexit values
	checked_vmwrite(HOST_SYSENTER_ESP, 0x4000);
	checked_vmwrite(HOST_SYSENTER_EIP, 0x4000);
}

void
vintel_enable(void)
{
	Print(L"Set CR4 Bit\n");
	writecr4(readcr4() | CR4_VMXE);

#if 0
	Print(L"Enable in Feature Control\n");
	uint64_t fctl = rdmsr64(IA32_FEATURE_CONTROL);
	fctl |= (1 << 0);
	wrmsr64(IA32_FEATURE_CONTROL, fctl);
#endif

	vintel_fix_cr_bits();

	AsciiPrint("VMX_BASIC = %lx\n", readmsr64(IA32_VMX_BASIC));

	Print(L"Allocating VMXON Page\n");
	uefi_call_wrapper(BS->AllocatePages, 4, AllocateAnyPages, EfiRuntimeServicesData, 1, &vmxon_region);

	Print(L"VMXON: %p\n", vmxon_region);
	vmxon_region->revisionID = readmsr64(IA32_VMX_BASIC) & 0xffffffff;

	uint64_t status = vmxon(vmxon_region);
	Print(L"VMXON Status: %lx\n", status);
	if (status & (1 << 0)) {
		Print(L"Invalid VMXON Pointer\n");
	}
	if (status & (1 << 6)) {
		Print(L"Extended VMX Error\n");
	}

	// FIXME Allocate the correct amount of space!
	Print(L"Allocating VMCS Page\n");
	uefi_call_wrapper(BS->AllocatePages, 4, AllocateAnyPages, EfiRuntimeServicesData, 1, &vmcs_region);

	Print(L"VMCS: %p\n", vmcs_region);
	vmcs_region->revisionID = readmsr64(IA32_VMX_BASIC) & 0xffffffff;

	status = vmclear(vmcs_region);
	Print(L"VMCLEAR Status: %lx\n", status);

	status = vmptrld(vmcs_region);
	Print(L"VMPTRLD Status: %lx\n", status);

	vmx_init_execution_controls();
	vintel_init_guest();
	vintel_init_host();

	{
		AsciiPrint("* Initializing VMX Entry Controls\n");
		uint64_t vmxBasic = readmsr64(IA32_VMX_BASIC);
		uint32_t msr = ((vmxBasic >> 55) & 1) ? IA32_VMX_TRUE_ENTRY_CTLS : IA32_VMX_ENTRY_CTLS;
		uint64_t entryControls = vmx_adjust_controls(0x200, msr); // 0x200 enables IA-32e mode
		checked_vmwrite(VM_ENTRY_CONTROLS, entryControls);
		AsciiPrint("VM_ENTRY_CONTROLS: %lx\n", entryControls);

		uint32_t exitMsr = ((vmxBasic >> 55) & 1) ? IA32_VMX_TRUE_EXIT_CTLS : IA32_VMX_EXIT_CTLS;
		uint64_t exitControls = vmx_adjust_controls(0x0, exitMsr);
		checked_vmwrite(VM_EXIT_CONTROLS, exitControls);
		AsciiPrint("VM_EXIT_CONTROLS: %lx\n", exitControls);

		checked_vmwrite(VM_ENTRY_MSR_LOAD_COUNT, 0);
		checked_vmwrite(VM_ENTRY_INTR_INFO_FIELD, 0);
		checked_vmwrite(VM_EXIT_MSR_STORE_COUNT, 0);
		checked_vmwrite(CR3_TARGET_COUNT, 0);

		checked_vmwrite(EXCEPTION_BITMAP, 0);
	}

	status = vmlaunch();
	Print(L"VMLAUNCH Status: %p\n", (void *)status);
	if (status & (1 << 0)) {
		Print(L"Invalid VMCS Pointer\n");
	}
	if (status & (1 << 6)) {
		AsciiPrint("%a\n", vmcs_describe_error());
	}
}

struct virt_vtable virt_vtable_intel = {
	.has_support = vintel_has_support,
	.enable      = vintel_enable,
};