#include "kernel.h"
#include "paging.h"

extern void main();

asm(
	".globl entry\n"
	"entry:\n"
	"mov $entry_stack_top, %esp\n"
	"push %ebx\n"
	"push %eax\n"
	"call entry_main\n"
);

void entry_serial_out(char* str);
void entry_serial_out_hex(uint32_t x);
void entry_copy_multiboot(multiboot_info_t* mb_info);

void entry_main(uint32_t mb_magic, multiboot_info_t* mb_info) {
	entry_serial_out("entry_main()\n");
	if(mb_magic != 0x2badb002) {
		entry_serial_out("Multiboot magic word is wrong!\n");
		goto stop;
	}
	entry_serial_out("Multiboot ok\n");
	
	entry_copy_multiboot(mb_info);
	
	/*
	entry_serial_out("Flags: ");
	entry_serial_out_hex(mb_info->flags);
	
	if(mb_info->flags & (1 << 2)) {
		entry_serial_out(mb_info->cmdline);
		entry_serial_out("\n");
	} else {
		entry_serial_out("No cmdline.\n");
	}
	
	entry_serial_out("Module addr: ");
	entry_serial_out_hex(mb_info->mods_addr->mod_start);
	*/
	
	// Create initial page tables.
	for(unsigned int i = 0; i < 1024; i++) {
		// Clear page directory.
		entry_pagedir[i] = 0;
		
		// Identity mapping of the current code.
		entry_pagetable_low[i] = ((uint32_t)&addr_phys & 0xffc00000) | (i << 12) | P1_P | P1_W;
		
		// Map kernelspace.
		if((i << 12) < ((uint32_t)&addr_virt_end & 0x003fffff)) {
			entry_pagetable_high[i] = ((uint32_t)&addr_load_virt + (i << 12)) | P1_P | P1_W | P1_G;
		} else {
			entry_pagetable_high[i] = 0;
		}
	}
	
	entry_pagetable_high[((uint32_t)&multiboot_info & 0x003ff000) >> 12] = (uint32_t)&entry_multiboot | P1_P | P1_W | P1_G;
	
	entry_pagedir[(uint32_t)&addr_phys >> 22] = (uint32_t)&entry_pagetable_low | P2_P | P2_W;
	entry_pagedir[(uint32_t)&addr_virt >> 22] = (uint32_t)&entry_pagetable_high | P2_P | P2_W | P2_G;
	
	// Map map_p1.
	entry_pagedir[1022] = (uint32_t)&entry_pagedir | P2_P | P2_W | P2_G;
	
	// Mapping stack to top without creating another page directory.
	entry_pagetable_low[1023] = (uint32_t)&entry_stack | P1_P | P1_W | P1_G;
	entry_pagedir[1023] = (uint32_t)&entry_pagetable_low | P2_P | P2_W | P2_G;
	
	// Enable paging.
	asm volatile(
		"mov %0, %%cr3\n"
		"mov %%cr0, %0\n"
		"or $0x80000000, %0\n"
		"mov %0, %%cr0\n"
		:: "r" (&entry_pagedir) : "%0"
	);
	
	// TODO: Check that segments are loaded and mapped right.
	
	// Call main() in virtual memory.
	asm volatile("mov $stack_top, %esp");
	main();
	
	stop:
	entry_serial_out("Halting.\n");
	asm volatile(
		"cli\n"
		"hlt\n"
	);
	while(1);
}

#define memcpy(source, target, size) asm volatile( \
	"cld\n" \
	"rep movsd\n" \
	:: "S" (source), "D" (target), "c" (size / 4) \
	: "flags", "memory")

unsigned int strcpy(char* source, char* target) {
	unsigned int i = 0;
		while(*source) {
			*target++ = *source++;
			i++;
		}
		*target = 0;
		return i + 1;
}

void entry_copy_multiboot(multiboot_info_t* mb_info) {
	// Copy multiboot structure.
	memcpy(mb_info, &entry_multiboot, sizeof(multiboot_info_t));
	uint32_t low_p = (uint32_t)&entry_multiboot + sizeof(multiboot_info_t);
	uint32_t high_p = (uint32_t)&multiboot_info + sizeof(multiboot_info_t);
	
	// Copy module structures.
	memcpy(mb_info->mods_addr, low_p, mb_info->mods_count * sizeof(multiboot_module_t));
	entry_multiboot.mods_addr = (multiboot_module_t*)high_p;
	multiboot_module_t* mods_addr = (multiboot_module_t*)low_p;
	low_p += mb_info->mods_count * sizeof(multiboot_module_t);
	high_p += mb_info->mods_count * sizeof(multiboot_module_t);
	
	// Copy strings.
	unsigned int s;
	s = strcpy(entry_multiboot.cmdline, (char*)low_p);
	entry_multiboot.cmdline = (char*)high_p;
	low_p += s;
	high_p += s;
	
	for(unsigned int i = 0; i < entry_multiboot.mods_count; i++) {
		s = strcpy(mods_addr[i].string, (char*)low_p);
		mods_addr[i].string = (char*)high_p;
		low_p += s;
		high_p += s;
	}
}

#define outb(port, value) asm volatile("out %b0,%w1" : : "a" (value), "d" (port))

void entry_serial_out(char* str) {
	while(*str) {
		if(*str == '\n') {
			outb(0x3f8, '\r');
		}
		outb(0x3f8, *str++);
	}
}

void entry_serial_out_hex(uint32_t x) {
	char str[] = "00000000\n";
	for(int i = 0; i < 8; i++) {
		int z = (x >> (28 - i * 4)) & 0xf;
		str[i] = z < 10 ? '0' + z : 'a' - 10 + z;
	}
	entry_serial_out(str);
}