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

#include <memory-add.h>
uint32 InitialStack;

void TaskingSetInitialStack(uint32 addr)
{
	InitialStack = addr;
}

void TaskingMoveStack (uint32 newstart, uint32 size)
{

	uint32 i;
	for (i = newstart; i >= newstart-size; i-= 0x1000)
		MemPhAllocFrame(PagingGetPage(i, 1, CurrentDirectory), 0, 1);

	// Flush TLB by rewriting cr3
	uint32 t;
	asm volatile ("mov %%cr3, %0" : "=r" (t));
	asm volatile ("mov %0, %%cr3" : : "r" (t));

	// Read old esp, ebp regs
	uint32 oldStackPointer, oldBasePointer;
	asm volatile ("mov %%esp, %0" : "=r"(oldStackPointer));
	asm volatile ("mov %%ebp, %0" : "=r"(oldBasePointer));

	uint32 offset = newstart - InitialStack;
	uint32 newStackPointer = oldStackPointer + offset;
	uint32 newBasePointer = oldBasePointer + offset;

	// Copy stack content
	memcpy((void*)newStackPointer, (void*)oldStackPointer, InitialStack-oldStackPointer);

	// (Attempt to) change EBP addresses in new stack
	for (i = newstart; i > newstart - size; i-=4)
	{
		uint32 *temp = (uint32*)i;
		if (oldStackPointer < *temp && *temp < InitialStack)
			*temp += offset;
	}

	// Set stack pointers to new stack
	asm volatile ("mov %0, %%esp" : : "r" (newStackPointer));
	asm volatile ("mov %0, %%ebp" : : "r" (newBasePointer));
}