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

#define GUEST_CR0 0x6800
#define GUEST_CR3 0x6802
#define GUEST_CR4 0x6804
#define GUEST_DR7 0x681A
#define GUEST_RSP 0x681C
#define GUEST_RIP 0x681E
#define GUEST_RFLAGS 0x6820
#define VMCS_LINK_POINTER 0x2800
#define GUEST_IA32_DEBUGCTL 0x2802
#define GUEST_IA32_PAT 0x2804
#define GUEST_IA32_EFER 0x2806
#define GUEST_FS_BASE 0x680E
#define GUEST_GS_BASE 0x6810
#define GUEST_SYSENTER_CS  0x482A
#define GUEST_SYSENTER_ESP 0x6824
#define GUEST_SYSENTER_EIP 0x6826

#define GUEST_ES_SELECTOR 0x0800
#define GUEST_CS_SELECTOR 0x0802
#define GUEST_SS_SELECTOR 0x0804
#define GUEST_DS_SELECTOR 0x0806
#define GUEST_FS_SELECTOR 0x0808
#define GUEST_GS_SELECTOR 0x080A

#define GUEST_GDTR_BASE 0x6816
#define GUEST_IDTR_BASE 0x6818

// HACK
char guest_stack[4096];

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

	for (;;) {}
}

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

	// Set segment selectors
	vmwrite(GUEST_ES_SELECTOR, reades() & 0xfff8);
	vmwrite(GUEST_CS_SELECTOR, readcs() & 0xfff8);
	vmwrite(GUEST_SS_SELECTOR, readss() & 0xfff8);
	vmwrite(GUEST_DS_SELECTOR, readds() & 0xfff8);
	vmwrite(GUEST_FS_SELECTOR, readfs() & 0xfff8);
	vmwrite(GUEST_GS_SELECTOR, readgs() & 0xfff8);

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

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

	// Set GDT and IDT
	struct GDTR gdtr = storegdt();
	struct IDTR idtr = storeidt();
	vmwrite(GUEST_GDTR_BASE, gdtr.base);
	vmwrite(GUEST_IDTR_BASE, idtr.base);
}

#define HOST_ES_SELECTOR 0xC00
#define HOST_CS_SELECTOR 0xC02
#define HOST_SS_SELECTOR 0xC04
#define HOST_DS_SELECTOR 0xC06
#define HOST_FS_SELECTOR 0xC08
#define HOST_GS_SELECTOR 0xC0A
#define HOST_TR_SELECTOR 0xC0C

#define HOST_CR0 0x6C00
#define HOST_CR3 0x6C02
#define HOST_CR4 0x6C04

#define HOST_RSP 0x6C14
#define HOST_RIP 0x6C16

#define HOST_IA32_PAT  0x2C00
#define HOST_IA32_EFER 0x2C02

#define HOST_FS_BASE 0x6C06
#define HOST_GS_BASE 0x6C08
#define HOST_TR_BASE 0x6C0A

#define HOST_SYSENTER_CS  0x4C00
#define HOST_SYSENTER_ESP 0x6C10
#define HOST_SYSENTER_EIP 0x6C12

#define HOST_GDTR_BASE 0x6C0C
#define HOST_IDTR_BASE 0x6C0E

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
	vmwrite(HOST_ES_SELECTOR, 0x10);
	vmwrite(HOST_CS_SELECTOR, 0x08);
	vmwrite(HOST_SS_SELECTOR, 0x10);
	vmwrite(HOST_DS_SELECTOR, 0x10);
	vmwrite(HOST_FS_SELECTOR, 0x00);
	vmwrite(HOST_GS_SELECTOR, 0x00);
	vmwrite(HOST_TR_SELECTOR, 0x18);

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

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

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

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

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

	vintel_init_guest();
	vintel_init_host();

	{
		AsciiPrint("* Initializing VMX Entry Controls\n");
		uint64_t vmxBasic = readmsr64(IA32_VMX_BASIC);
		uint64_t mask;
		if (vmxBasic & (1ul << 55)) {
			mask = readmsr64(IA32_VMX_TRUE_ENTRY_CTLS);
		} else {
			mask = readmsr64(IA32_VMX_ENTRY_CTLS);
		}
		AsciiPrint("Entry Controls Mask: %lx\n", mask);
		uint64_t entryCtls = (0x200 | (mask & 0xffffffff)) & (mask >> 32);
		AsciiPrint("Entry Controls: %lx\n", entryCtls);
		vmwrite(VM_ENTRY_CONTROLS, entryCtls);
	}

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