/*
 * tasking-multi.c
 *
 *  Created on: Sep 7, 2011
 *      Author: Tiberiu
 */

#include <tasking.h>
#include <debugio.h>
#include <stdio.h>

volatile Task *CurrentTask;
volatile Task *TaskQueue;
volatile uint32 NextPid = 1;

void TaskingInitialize()
{
	// Stop interrupts
	asm volatile ("cli");

	// Relocate stack
	TaskingMoveStack(0xE0000000, 0x4000);

	// Set up first task (kernel)
	CurrentTask = TaskQueue = (Task*) kmalloc(sizeof(Task));
	CurrentTask->Pid = NextPid ++;
	CurrentTask->Esp = CurrentTask->Ebp = CurrentTask->Eip = 0;
	CurrentTask->Pd = CurrentDirectory;
	CurrentTask->Next = NULL;

	// Reenable interrupts
	asm volatile ("sti");

	Log("Tasking", "Initialized, currenttask: pid=%u esp=%x ebp=%x eip=%x Pd=%x Next=%x\n", CurrentTask->Pid,
			CurrentTask->Esp, CurrentTask->Ebp, CurrentTask->Eip, CurrentTask->Pd, CurrentTask->Next);
}

int32 Fork()
{
	asm volatile ("cli");

	Task* Parent = (Task*) CurrentTask;
	PageDirectory* Dir = PagingCloneDirectory(CurrentDirectory);

	// Create a new process
	Task* n = (Task*)kmalloc(sizeof(Task));
	n->Pid = NextPid++;
	n->Esp = n->Ebp = 0;
	n->Eip = 0;
	n->Pd = Dir;
	n->Next = 0;

	// Add it at the end of the queue
	Task* temp = (Task*) TaskQueue;
	while (temp->Next) temp = temp->Next;	// Find end of queue
	temp->Next = n;							// Add it

	// Read eip
	uint32 eip = TaskingReadEip();

	// We could be the parent
	if (CurrentTask == Parent)
	{
		uint32 esp, ebp;
		asm volatile ("mov %%esp, %0" : "=r"(esp));
		asm volatile ("mov %%ebp, %0" : "=r"(ebp));
		n->Esp = esp;
		n->Ebp = ebp;
		n->Eip = eip;

		asm volatile ("sti");
		return n->Pid;
	}

	// Or the child
	else return 0;
}

void TaskingScheduler()
{
	// For now, just switch tasks
	TaskingSwitch();
}

void Func()
{

}

void TaskingSwitch()
{
	// Make sure tasking is initialized, or there are at least 2 processes running
	if (!CurrentTask) return;

	// Save esp & epb
	uint32 esp, ebp, eip;
	asm volatile("mov %%esp, %0" : "=r"(esp));
	asm volatile("mov %%ebp, %0" : "=r"(ebp));
	eip = TaskingReadEip();

	// Have we just switched tasks?
	if (eip == 0xABCDE) return;

	// No? Switch
	// Save stack address and current instruction pointer
	CurrentTask->Eip = eip;
	CurrentTask->Esp = esp;
	CurrentTask->Ebp = ebp;

	//Log("Tasking", "Saved eip=%x esp=%x ebp=%x\n", eip, esp, ebp);

	CurrentTask = CurrentTask ->Next;				// Switch task
	if (!CurrentTask) CurrentTask = TaskQueue;		// End of queue? Start over

	// Read registers
	eip = CurrentTask->Eip;
	esp = CurrentTask->Esp;
	ebp = CurrentTask->Ebp;

	//Log("Tasking", "Loaded eip=%x esp=%x ebp=%x", eip, esp, ebp);

	CurrentDirectory = CurrentTask->Pd;
	MagicBreakpoint();

	// Put data in registers
	asm volatile ("  "
			"cli;"
			"mov %0, %%ecx;"
			"mov %1, %%esp;"
			"mov %2, %%ebp;"
			"mov %3, %%cr3;"
			"mov $0xABCDE, %%eax;"
			"sti;"
			"jmp *%%ecx"
			: : "r"(eip), "r"(esp), "r"(ebp), "r"(CurrentDirectory->PhysicalAddr));
}

int32 GetPid()
{
	return CurrentTask->Pid;
}