From d1d3bfa26bd78e298ad2431c4f86e2ea94515aab Mon Sep 17 00:00:00 2001
From: John Hodge <tpg@mutabah.net>
Date: Fri, 23 Sep 2011 13:49:04 +0800
Subject: [PATCH] Kernel/x86_64 - Fucking about removing inline asm

- Doesn't boot
- Removed almost all state saves, replaced them with new stacks
- _Clone is broken atm
---
 Kernel/arch/x86_64/Makefile          |   2 +-
 Kernel/arch/x86_64/desctab.asm       |  46 +----
 Kernel/arch/x86_64/errors.c          |   2 +
 Kernel/arch/x86_64/include/mm_virt.h |   2 +-
 Kernel/arch/x86_64/include/proc.h    |   2 +-
 Kernel/arch/x86_64/mm_phys.c         |  19 +-
 Kernel/arch/x86_64/mm_virt.c         |  93 ++++++---
 Kernel/arch/x86_64/proc.c            | 276 ++++++++++-----------------
 Kernel/arch/x86_64/start32.asm       |  30 ++-
 Kernel/arch/x86_64/start64.asm       |  12 +-
 10 files changed, 225 insertions(+), 259 deletions(-)

diff --git a/Kernel/arch/x86_64/Makefile b/Kernel/arch/x86_64/Makefile
index 2c577b07..a72216a3 100644
--- a/Kernel/arch/x86_64/Makefile
+++ b/Kernel/arch/x86_64/Makefile
@@ -23,7 +23,7 @@ else
 endif
 	
 
-A_OBJ := start32.ao start64.ao desctab.ao
+A_OBJ := start32.ao start64.ao desctab.ao proc.ao
 A_OBJ += main.o lib.o proc.o mm_virt.o mm_phys.o
 A_OBJ += kernelpanic.o errors.o time.o pci.o
 A_OBJ += vm8086.o
diff --git a/Kernel/arch/x86_64/desctab.asm b/Kernel/arch/x86_64/desctab.asm
index 411cea3e..50fc126c 100644
--- a/Kernel/arch/x86_64/desctab.asm
+++ b/Kernel/arch/x86_64/desctab.asm
@@ -1,6 +1,7 @@
 ;
 ;
 ;
+%include "arch/x86_64/include/common.inc.asm"
 [BITS 64]
 
 [extern Log]
@@ -11,45 +12,6 @@
 
 MM_LOCALAPIC	equ	0xFFFFFD0000000000
 
-%macro PUSH_GPR	0
-	mov [rsp-0x60], rsp
-	mov [rsp-0x08], r15
-	mov [rsp-0x10], r14
-	mov [rsp-0x18], r13
-	mov [rsp-0x20], r12
-	mov [rsp-0x28], r11
-	mov [rsp-0x30], r10
-	mov [rsp-0x38], r9
-	mov [rsp-0x40], r8
-	mov [rsp-0x48], rdi
-	mov [rsp-0x50], rsi
-	mov [rsp-0x58], rbp
-	mov [rsp-0x68], rbx
-	mov [rsp-0x70], rdx
-	mov [rsp-0x78], rcx
-	mov [rsp-0x80], rax
-	sub rsp, 0x80
-%endmacro
-%macro POP_GPR	0
-	add rsp, 0x80
-	mov r15, [rsp-0x08]
-	mov r14, [rsp-0x10]
-	mov r13, [rsp-0x18]
-	mov r12, [rsp-0x20]
-	mov r11, [rsp-0x28]
-	mov r10, [rsp-0x30]
-	mov r9,  [rsp-0x38]
-	mov r8,  [rsp-0x40]
-	mov rdi, [rsp-0x48]
-	mov rsi, [rsp-0x50]
-	mov rbp, [rsp-0x58]
-	;mov rsp, [rsp-0x60]
-	mov rbx, [rsp-0x68]
-	mov rdx, [rsp-0x70]
-	mov rcx, [rsp-0x78]
-	mov rax, [rsp-0x80]
-%endmacro
-
 [section .text]
 [global Desctab_Init]
 Desctab_Init:	
@@ -375,10 +337,12 @@ SchedulerIRQ:
 	mov rax, dr0
 	push rax
 	
-	; Get the CPU Number
-	mov rdi, dr1
+	mov rdi, dr1	; Get the CPU Number
+	mov rsi, rsp	; Save stack pointer
+	mov rdx, SchedulerIRQ.restoreState
 	; Call the Scheduler
 	call Proc_Scheduler
+.restoreState:
 	
 	; Restore Thread Pointer
 	pop rax
diff --git a/Kernel/arch/x86_64/errors.c b/Kernel/arch/x86_64/errors.c
index 0d5ede6f..9ddc950b 100644
--- a/Kernel/arch/x86_64/errors.c
+++ b/Kernel/arch/x86_64/errors.c
@@ -39,6 +39,8 @@ void Error_Handler(tRegs *Regs)
 	}
 	
 	Debug_KernelPanic();
+
+	Error_Backtrace(Regs->RIP, Regs->RBP);
 	
 	Log("CPU Error %x, Code: 0x%x", Regs->IntNum, Regs->ErrorCode);
 //	Log(" - %s", csaERROR_NAMES[Regs->IntNum]);
diff --git a/Kernel/arch/x86_64/include/mm_virt.h b/Kernel/arch/x86_64/include/mm_virt.h
index 1af7bd52..3c7babc6 100644
--- a/Kernel/arch/x86_64/include/mm_virt.h
+++ b/Kernel/arch/x86_64/include/mm_virt.h
@@ -85,6 +85,6 @@
 void	MM_FinishVirtualInit(void);
 tVAddr	MM_NewKStack(void);
 tVAddr	MM_Clone(void);
-tVAddr	MM_NewWorkerStack(void);
+tVAddr	MM_NewWorkerStack(void *StackData, size_t StackSize);
 
 #endif
diff --git a/Kernel/arch/x86_64/include/proc.h b/Kernel/arch/x86_64/include/proc.h
index 2913f738..a23aa37a 100644
--- a/Kernel/arch/x86_64/include/proc.h
+++ b/Kernel/arch/x86_64/include/proc.h
@@ -38,7 +38,7 @@ typedef struct sMemoryState
  */
 typedef struct sTaskState
 {
-	Uint	RIP, RSP, RBP;
+	Uint	RIP, RSP;
 	Uint64	UserRIP, UserCS;
 }	tTaskState;
 
diff --git a/Kernel/arch/x86_64/mm_phys.c b/Kernel/arch/x86_64/mm_phys.c
index eb495b33..893fb5b1 100644
--- a/Kernel/arch/x86_64/mm_phys.c
+++ b/Kernel/arch/x86_64/mm_phys.c
@@ -461,7 +461,7 @@ tPAddr MM_AllocPhys(void)
 		if( gaiStaticAllocPages[i] ) {
 			tPAddr	ret = gaiStaticAllocPages[i];
 			gaiStaticAllocPages[i] = 0;
-			Log("MM_AllocPhys: Return %x, static alloc %i", ret, i);
+			Log("MM_AllocPhys: Return %P, static alloc %i", ret, i);
 			return ret;
 		}
 	}
@@ -536,6 +536,23 @@ void MM_DerefPhys(tPAddr PAddr)
 	}
 }
 
+int MM_GetRefCount( tPAddr PAddr )
+{
+	PAddr >>= 12;
+	
+	if( PAddr >> 12 > giMaxPhysPage )	return 0;
+
+	if( gaMultiBitmap[ PAddr >> 6 ] & (1LL << (PAddr&63)) ) {
+		return gaiPageReferences[PAddr];
+	}
+
+	if( gaMainBitmap[ PAddr >> 6 ] & (1LL << (PAddr&63)) )
+	{
+		return 1;
+	}
+	return 0;
+}
+
 /**
  * \brief Takes a physical address and returns the ID of its range
  * \param Addr	Physical address of page
diff --git a/Kernel/arch/x86_64/mm_virt.c b/Kernel/arch/x86_64/mm_virt.c
index 5252ea34..a34c6ad2 100644
--- a/Kernel/arch/x86_64/mm_virt.c
+++ b/Kernel/arch/x86_64/mm_virt.c
@@ -27,7 +27,8 @@
 #define	PF_PRESENT	0x001
 #define	PF_WRITE	0x002
 #define	PF_USER		0x004
-#define	PF_LARGE	0x000
+#define	PF_LARGE	0x080
+#define	PF_GLOBAL	0x100
 #define	PF_COW		0x200
 #define	PF_PAGED	0x400
 #define	PF_NX		0x80000000##00000000
@@ -88,24 +89,35 @@ void MM_FinishVirtualInit(void)
 void MM_PageFault(tVAddr Addr, Uint ErrorCode, tRegs *Regs)
 {
 	// TODO: Implement Copy-on-Write
-	#if 0
-	if( gaPageDir  [Addr>>22] & PF_PRESENT
-	 && gaPageTable[Addr>>12] & PF_PRESENT
-	 && gaPageTable[Addr>>12] & PF_COW )
+	#if 1
+	if( PAGEMAPLVL4(Addr>39) & PF_PRESENT
+	 && PAGEDIRPTR(Addr>>30) & PF_PRESENT
+	 && PAGEDIR(Addr>>21) & PF_PRESENT
+	 && PAGETABLE(Addr>>12) & PF_PRESENT
+	 && PAGETABLE(Addr>>12) & PF_COW )
 	{
 		tPAddr	paddr;
-		if(MM_GetRefCount( gaPageTable[Addr>>12] & PADDR_MASK ) == 1)
+		if(MM_GetRefCount( PAGETABLE(Addr>>12) & PADDR_MASK ) == 1)
 		{
-			gaPageTable[Addr>>12] &= ~PF_COW;
-			gaPageTable[Addr>>12] |= PF_PRESENT|PF_WRITE;
+			PAGETABLE(Addr>>12) &= ~PF_COW;
+			PAGETABLE(Addr>>12) |= PF_PRESENT|PF_WRITE;
 		}
 		else
 		{
 			//Log("MM_PageFault: COW - MM_DuplicatePage(0x%x)", Addr);
-			paddr = MM_DuplicatePage( Addr );
-			MM_DerefPhys( gaPageTable[Addr>>12] & PADDR_MASK );
-			gaPageTable[Addr>>12] &= PF_USER;
-			gaPageTable[Addr>>12] |= paddr|PF_PRESENT|PF_WRITE;
+			paddr = MM_AllocPhys();
+			if( !paddr ) {
+				Threads_SegFault(Addr);
+				return ;
+			}
+			{
+				void	*tmp = (void*)MM_MapTemp(paddr);
+				memcpy( tmp, (void*)(Addr & ~0xFFF), 0x1000 );
+				MM_FreeTemp( (tVAddr)tmp );
+			}
+			MM_DerefPhys( PAGETABLE(Addr>>12) & PADDR_MASK );
+			PAGETABLE(Addr>>12) &= PF_USER;
+			PAGETABLE(Addr>>12) |= paddr|PF_PRESENT|PF_WRITE;
 		}
 		
 		INVLPG( Addr & ~0xFFF );
@@ -192,7 +204,7 @@ void MM_DumpTables(tVAddr Start, tVAddr End)
 		{			
 			if(expected != CHANGEABLE_BITS)
 			{
-				Log("%016llx => %013llx : 0x%6llx (%c%c%c%c)",
+				Log("%016llx => %13llx : 0x%6llx (%c%c%c%c)",
 					CANOICAL(rangeStart),
 					PAGETABLE(rangeStart>>12) & PADDR_MASK,
 					curPos - rangeStart,
@@ -231,7 +243,7 @@ void MM_DumpTables(tVAddr Start, tVAddr End)
 	}
 	
 	if(expected != CHANGEABLE_BITS) {
-		Log("%016llx => %013llx : 0x%6llx (%c%c%c%c)",
+		Log("%016llx => %13llx : 0x%6llx (%c%c%c%c)",
 			CANOICAL(rangeStart),
 			PAGETABLE(rangeStart>>12) & PADDR_MASK,
 			curPos - rangeStart,
@@ -679,12 +691,6 @@ tPAddr MM_Clone(void)
 	 int	i;
 	tVAddr	kstackbase;
 
-	// tThread->KernelStack is the top
-	// There is 1 guard page below the stack
-	kstackbase = Proc_GetCurThread()->KernelStack - KERNEL_STACK_SIZE + 0x1000;
-
-	Log("MM_Clone: kstackbase = %p", kstackbase);
-	
 	// #1 Create a copy of the PML4
 	ret = MM_AllocPhys();
 	if(!ret)	return 0;
@@ -727,16 +733,30 @@ tPAddr MM_Clone(void)
 	TMPMAPLVL4(508) = ret | 3;
 	TMPMAPLVL4(509) = 0;	// Temp
 	
-	// #6 Create kernel stack (-1 to account for the guard)
-	TMPMAPLVL4(320) = 0;
-	for( i = 0; i < KERNEL_STACK_SIZE/0x1000-1; i ++ )
+	// #6 Create kernel stack
+	//  tThread->KernelStack is the top
+	//  There is 1 guard page below the stack
+	kstackbase = Proc_GetCurThread()->KernelStack - KERNEL_STACK_SIZE;
+
+//	Log("MM_Clone: kstackbase = %p", kstackbase);
+	
+	TMPMAPLVL4(MM_KSTACK_BASE >> PML4_SHIFT) = 0;
+	for( i = 1; i < KERNEL_STACK_SIZE/0x1000; i ++ )
 	{
 		tPAddr	phys = MM_AllocPhys();
 		tVAddr	tmpmapping;
 		MM_MapEx(kstackbase+i*0x1000, phys, 1, 0);
 		
+		Log_Debug("MM", "MM_Clone: Cloning stack page %p from %P to %P",
+			kstackbase+i*0x1000, MM_GetPhysAddr( kstackbase+i*0x1000 ), phys
+			);
 		tmpmapping = MM_MapTemp(phys);
-		memcpy((void*)tmpmapping, (void*)(kstackbase+i*0x1000), 0x1000);
+		if( MM_GetPhysAddr( kstackbase+i*0x1000 ) )
+			memcpy((void*)tmpmapping, (void*)(kstackbase+i*0x1000), 0x1000);
+		else
+			memset((void*)tmpmapping, 0, 0x1000);
+//		if( i == 0xF )
+//			Debug_HexDump("MM_Clone: *tmpmapping = ", (void*)tmpmapping, 0x1000);
 		MM_FreeTemp(tmpmapping);
 	}
 	
@@ -746,7 +766,7 @@ tPAddr MM_Clone(void)
 	TMPCR3() = 0;
 	INVLPG_ALL();
 	Mutex_Release(&glMM_TempFractalLock);
-	Log("MM_Clone: RETURN %P\n", ret);
+//	Log("MM_Clone: RETURN %P", ret);
 	return ret;
 }
 
@@ -829,7 +849,7 @@ void MM_ClearUser(void)
 	}
 }
 
-tVAddr MM_NewWorkerStack(void)
+tVAddr MM_NewWorkerStack(void *StackData, size_t StackSize)
 {
 	tVAddr	ret;
 	 int	i;
@@ -841,7 +861,9 @@ tVAddr MM_NewWorkerStack(void)
 	// #2 Scan for a free stack addresss < 2^47
 	for(ret = 0x100000; ret < (1ULL << 47); ret += KERNEL_STACK_SIZE)
 	{
-		if( MM_GetPhysAddr(ret) == 0 )	break;
+		tPAddr	*ptr;
+		if( MM_GetPageEntryPtr(ret, 1, 0, 0, &ptr) == 0 )	break;
+		if( !(*ptr & 1) )	break;
 	}
 	if( ret >= (1ULL << 47) ) {
 		Mutex_Release(&glMM_TempFractalLock);
@@ -860,6 +882,19 @@ tVAddr MM_NewWorkerStack(void)
 		}
 		MM_MapEx(ret + i*0x1000, phys, 1, 0);
 	}
+
+	if( StackSize > 0x1000 ) {
+		Log_Error("MM", "MM_NewWorkerStack: StackSize(0x%x) > 0x1000, cbf handling", StackSize);
+	}
+	else {
+		tPAddr	*ptr, paddr;
+		tVAddr	tmp_addr;
+		MM_GetPageEntryPtr(ret + i*0x1000, 1, 0, 0, &ptr);
+		paddr = *ptr & ~0xFFF;
+		tmp_addr = MM_MapTemp(paddr);
+		memcpy( (void*)(tmp_addr + (0x1000 - StackSize)), StackData, StackSize );
+		MM_FreeTemp(tmp_addr);
+	}
 	
 	Mutex_Release(&glMM_TempFractalLock);
 	
@@ -875,11 +910,11 @@ tVAddr MM_NewKStack(void)
 	Uint	i;
 	for( ; base < MM_KSTACK_TOP; base += KERNEL_STACK_SIZE )
 	{
-		if(MM_GetPhysAddr(base) != 0)
+		if(MM_GetPhysAddr(base+KERNEL_STACK_SIZE-0x1000) != 0)
 			continue;
 		
 		//Log("MM_NewKStack: Found one at %p", base + KERNEL_STACK_SIZE);
-		for( i = 0; i < KERNEL_STACK_SIZE; i += 0x1000)
+		for( i = 0x1000; i < KERNEL_STACK_SIZE; i += 0x1000)
 		{
 			if( !MM_Allocate(base+i) )
 			{
diff --git a/Kernel/arch/x86_64/proc.c b/Kernel/arch/x86_64/proc.c
index 8439b91b..e83c20a9 100644
--- a/Kernel/arch/x86_64/proc.c
+++ b/Kernel/arch/x86_64/proc.c
@@ -17,10 +17,9 @@
 
 // === FLAGS ===
 #define DEBUG_TRACE_SWITCH	0
-//#define BREAK_ON_SWITCH	1	// Break into bochs debugger on a task switch
+#define BREAK_ON_SWITCH 	0	// Break into bochs debugger on a task switch
 
 // === CONSTANTS ===
-#define	SWITCH_MAGIC	0x55ECAFFF##FFFACE55	// There is no code in this area
 
 // === TYPES ===
 typedef struct sCPU
@@ -35,9 +34,12 @@ typedef struct sCPU
 // === IMPORTS ===
 extern tGDT	gGDT[];
 extern void	APStartup(void);	// 16-bit AP startup code
+
 extern Uint	GetRIP(void);	// start.asm
+extern Uint	SaveState(Uint *RSP, Uint *Regs);
+extern void	NewTaskHeader(void);	// Actually takes cdecl args
+
 extern Uint64	gInitialPML4[512];	// start.asm
-extern char	gInitialKernelStack[];
 extern tShortSpinlock	glThreadListLock;
 extern int	giNumCPUs;
 extern int	giNextTID;
@@ -54,18 +56,19 @@ extern void	Time_UpdateTimestamp(void);
 void	MP_StartAP(int CPU);
 void	MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode);
 #endif
+void	Proc_IdleTask(void *unused);
 //void	Proc_Start(void);
 //tThread	*Proc_GetCurThread(void);
-void	Proc_ChangeStack(void);
+ int	Proc_NewKThread(void (*Fcn)(void*), void *Data);
 // int	Proc_Clone(Uint *Err, Uint Flags);
 // int	Proc_SpawnWorker(void);
 Uint	Proc_MakeUserStack(void);
 //void	Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize);
-void	Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP);
+void	Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP) NORETURN;
  int	Proc_Demote(Uint *Err, int Dest, tRegs *Regs);
 //void	Proc_CallFaultHandler(tThread *Thread);
 //void	Proc_DumpThreadCPUState(tThread *Thread);
-void	Proc_Scheduler(int CPU);
+void	Proc_Scheduler(int CPU, Uint RSP, Uint RIP);
 
 // === GLOBALS ===
 //!\brief Used by desctab.asm in SyscallStub
@@ -304,6 +307,7 @@ void ArchThreads_Init(void)
 	
 	gThreadZero.MemState.CR3 = (Uint)gInitialPML4 - KERNEL_BASE;
 	gThreadZero.CurCPU = 0;
+	gThreadZero.KernelStack = 0xFFFFA00000000000 + KERNEL_STACK_SIZE;
 	
 	// Set timer frequency
 	outb(0x43, 0x34);	// Set Channel 0, Low/High, Rate Generator
@@ -315,11 +319,8 @@ void ArchThreads_Init(void)
 	{
 		Warning("Oh, hell, Unable to allocate PPD for Thread#0");
 	}
-	
-	// Change Stacks
-	Proc_ChangeStack();
-	
-	Log("Multithreading initialised");
+
+	Log_Log("Proc", "Multithreading initialised");
 }
 
 #if USE_MP
@@ -350,6 +351,19 @@ void MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode)
 }
 #endif
 
+/**
+ * \brief Idle task
+ */
+void Proc_IdleTask(void *ptr)
+{
+	tCPU	*cpu = ptr;
+	cpu->IdleThread = Proc_GetCurThread();
+	cpu->IdleThread->ThreadName = (char*)"Idle Thread";
+	Threads_SetPriority( cpu->IdleThread, -1 );	// Never called randomly
+	cpu->IdleThread->Quantum = 1;	// 1 slice quantum
+	for(;;)	HALT();	// Just yeilds
+}
+
 /**
  * \fn void Proc_Start(void)
  * \brief Start process scheduler
@@ -367,15 +381,10 @@ void Proc_Start(void)
 		 int	tid;
 		if(i)	gaCPUs[i].Current = NULL;
 		
+		Proc_NewKThread(Proc_IdleTask, &gaCPUs[i]);		
+
 		// Create Idle Task
-		if( (tid = Proc_Clone(0, 0)) == 0)
-		{
-			for(;;)	HALT();	// Just yeilds
-		}
 		gaCPUs[i].IdleThread = Threads_GetThread(tid);
-		gaCPUs[i].IdleThread->ThreadName = "Idle Thread";
-		Threads_SetTickets( gaCPUs[i].IdleThread, 0 );	// Never called randomly
-		gaCPUs[i].IdleThread->Quantum = 1;	// 1 slice quantum
 		
 		
 		// Start the AP
@@ -392,19 +401,7 @@ void Proc_Start(void)
 	__asm__ __volatile__ ("sti");
 	while( giNumInitingCPUs )	__asm__ __volatile__ ("hlt");
 	#else
-	// Create Idle Task
-	if(Proc_Clone(0) == 0)
-	{
-		gaCPUs[0].IdleThread = Proc_GetCurThread();
-		gaCPUs[0].IdleThread->ThreadName = (char*)"Idle Thread";
-		Threads_SetPriority( gaCPUs[0].IdleThread, -1 );	// Never called randomly
-		gaCPUs[0].IdleThread->Quantum = 1;	// 1 slice quantum
-		for(;;)	HALT();	// Just yeilds
-	}
-	
-	// Set current task
-	gaCPUs[0].Current = &gThreadZero;
-	gaCPUs[0].Current->CurCPU = 0;
+	Proc_NewKThread(Proc_IdleTask, &gaCPUs[0]);
 	
 	// Start Interrupts (and hence scheduler)
 	__asm__ __volatile__("sti");
@@ -426,56 +423,40 @@ tThread *Proc_GetCurThread(void)
 	#endif
 }
 
-/**
- * \fn void Proc_ChangeStack(void)
- * \brief Swaps the current stack for a new one (in the proper stack reigon)
- */
-void Proc_ChangeStack(void)
+int Proc_NewKThread(void (*Fcn)(void*), void *Data)
 {
-	Uint	rsp, rbp;
-	Uint	tmp_rbp, old_rsp;
-	Uint	curBase, newBase;
-
-	__asm__ __volatile__ ("mov %%rsp, %0":"=r"(rsp));
-	__asm__ __volatile__ ("mov %%rbp, %0":"=r"(rbp));
-
-	old_rsp = rsp;
+	Uint	rsp;
+	tThread	*newThread, *cur;
+	
+	cur = Proc_GetCurThread();
+	newThread = Threads_CloneTCB(NULL, 0);
+	if(!newThread)	return -1;
+	
+	// Set CR3
+	newThread->MemState.CR3 = cur->MemState.CR3;
 
 	// Create new KStack
-	newBase = MM_NewKStack();
+	newThread->KernelStack = MM_NewKStack();
 	// Check for errors
-	if(newBase == 0) {
-		Panic("What the?? Unable to allocate space for initial kernel stack");
-		return;
+	if(newThread->KernelStack == 0) {
+		free(newThread);
+		return -1;
 	}
 
-	curBase = (Uint)&gInitialKernelStack;
+	rsp = newThread->KernelStack;
+	*(Uint*)(rsp-=8) = (Uint)Data;	// Data (shadowed)
+	*(Uint*)(rsp-=8) = 1;	// Number of params
+	*(Uint*)(rsp-=8) = (Uint)Fcn;	// Function to call
+	*(Uint*)(rsp-=8) = (Uint)newThread;	// Thread ID
 	
-	Log("curBase = 0x%x, newBase = 0x%x", curBase, newBase);
-
-	// Get ESP as a used size
-	rsp = curBase - rsp;
-	Log("memcpy( %p, %p, 0x%x )", (void*)(newBase - rsp), (void*)(curBase - rsp), rsp );
-	// Copy used stack
-	memcpy( (void*)(newBase - rsp), (void*)(curBase - rsp), rsp );
-	// Get ESP as an offset in the new stack
-	rsp = newBase - rsp;
-	// Adjust EBP
-	rbp = newBase - (curBase - rbp);
-
-	Log("Update stack");
-	// Repair EBPs & Stack Addresses
-	// Catches arguments also, but may trash stack-address-like values
-	for(tmp_rbp = rsp; tmp_rbp < newBase; tmp_rbp += sizeof(Uint))
-	{
-		if(old_rsp < *(Uint*)tmp_rbp && *(Uint*)tmp_rbp < curBase)
-			*(Uint*)tmp_rbp += newBase - curBase;
-	}
+	newThread->SavedState.RSP = rsp;
+	newThread->SavedState.RIP = (Uint)&NewTaskHeader;
+	Log("New (KThread) %p, rsp = %p\n", newThread->SavedState.RIP, newThread->SavedState.RSP);
 	
-	Log("Applying Changes");
-	Proc_GetCurThread()->KernelStack = newBase;
-	__asm__ __volatile__ ("mov %0, %%rsp"::"r"(rsp));
-	__asm__ __volatile__ ("mov %0, %%rbp"::"r"(rbp));
+//	MAGIC_BREAK();	
+	Threads_AddActive(newThread);
+
+	return newThread->TID;
 }
 
 /**
@@ -484,72 +465,33 @@ void Proc_ChangeStack(void)
  */
 int Proc_Clone(Uint Flags)
 {
-	tThread	*newThread;
-	tThread	*cur = Proc_GetCurThread();
-	Uint	rip, rsp, rbp;
-	
-	__asm__ __volatile__ ("mov %%rsp, %0": "=r"(rsp));
-	__asm__ __volatile__ ("mov %%rbp, %0": "=r"(rbp));
+	tThread	*newThread, *cur = Proc_GetCurThread();
+	Uint	rip;
+	Uint	_savedregs[16];
 	
 	newThread = Threads_CloneTCB(NULL, Flags);
 	if(!newThread)	return -1;
 	
-//	Log("Proc_Clone: newThread = %p", newThread);
+	// Save core machine state
+	rip = SaveState(&newThread->SavedState.RSP, &_savedregs[0]);
+	if(rip == 0) {
+		outb(0x20, 0x20);	// ACK Timer and return as child
+		__asm__ __volatile__ ("sti");
+		return 0;
+	}
 	
 	// Initialise Memory Space (New Addr space or kernel stack)
 	if(Flags & CLONE_VM) {
-//		Log("Proc_Clone: Cloning VM");
 		newThread->MemState.CR3 = MM_Clone();
 		newThread->KernelStack = cur->KernelStack;
-//		MAGIC_BREAK();
-	} else {
-		Uint	tmp_rbp, old_rsp = rsp;
-
-		// Set CR3
-		newThread->MemState.CR3 = cur->MemState.CR3;
-
-		// Create new KStack
-		newThread->KernelStack = MM_NewKStack();
-//		Log("Proc_Clone: newKStack = %p", newThread->KernelStack);
-		// Check for errors
-		if(newThread->KernelStack == 0) {
-			free(newThread);
-			return -1;
-		}
-
-		// Get ESP as a used size
-		rsp = cur->KernelStack - rsp;
-		// Copy used stack
-		memcpy(
-			(void*)(newThread->KernelStack - rsp),
-			(void*)(cur->KernelStack - rsp),
-			rsp
-			);
-		// Get ESP as an offset in the new stack
-		rsp = newThread->KernelStack - rsp;
-		// Adjust EBP
-		rbp = newThread->KernelStack - (cur->KernelStack - rbp);
-
-		// Repair EBPs & Stack Addresses
-		// Catches arguments also, but may trash stack-address-like values
-		for(tmp_rbp = rsp; tmp_rbp < newThread->KernelStack; tmp_rbp += sizeof(Uint))
-		{
-			if(old_rsp < *(Uint*)tmp_rbp && *(Uint*)tmp_rbp < cur->KernelStack)
-				*(Uint*)tmp_rbp += newThread->KernelStack - cur->KernelStack;
-		}
 	}
-	
-	// Save core machine state
-	newThread->SavedState.RSP = rsp;
-	newThread->SavedState.RBP = rbp;
-	rip = GetRIP();
-	if(rip == SWITCH_MAGIC) {
-		outb(0x20, 0x20);	// ACK Timer and return as child
-		__asm__ __volatile__ ("sti");
-//		MAGIC_BREAK();
-		return 0;
+	else {
+		Log_Error("Proc", "Proc_Clone: Don't leave CLONE_VM unset, use Proc_NewKThread instead");
+		return -1;
 	}
 	
+	Log("New (Clone) %p, rsp = %p\n", rip, newThread->SavedState.RSP);
+	
 	// Set EIP as parent
 	newThread->SavedState.RIP = rip;
 	
@@ -563,11 +505,11 @@ int Proc_Clone(Uint Flags)
  * \fn int Proc_SpawnWorker(void)
  * \brief Spawns a new worker thread
  */
-int Proc_SpawnWorker(void)
+int Proc_SpawnWorker(void (*Fcn)(void*), void *Data)
 {
 	tThread	*new, *cur;
-	Uint	rip, rsp, rbp;
-	
+	Uint	stack_contents[4];
+
 	cur = Proc_GetCurThread();
 	
 	// Create new thread
@@ -579,28 +521,22 @@ int Proc_SpawnWorker(void)
 	memcpy(new, &gThreadZero, sizeof(tThread));
 	// Set Thread ID
 	new->TID = giNextTID++;
+
+	// Create the stack contents
+	stack_contents[3] = (Uint)Data;
+	stack_contents[2] = 1;
+	stack_contents[1] = (Uint)Fcn;
+	stack_contents[0] = (Uint)new;
+	
 	// Create a new worker stack (in PID0's address space)
 	// The stack is relocated by this code
-	new->KernelStack = MM_NewWorkerStack();
+	new->KernelStack = MM_NewWorkerStack(stack_contents, sizeof(stack_contents));
 
-	// Get ESP and EBP based in the new stack
-	__asm__ __volatile__ ("mov %%rsp, %0": "=r"(rsp));
-	__asm__ __volatile__ ("mov %%rbp, %0": "=r"(rbp));
-	rsp = new->KernelStack - (cur->KernelStack - rsp);
-	rbp = new->KernelStack - (cur->KernelStack - rbp);	
+	new->SavedState.RSP = new->KernelStack - sizeof(stack_contents);
+	new->SavedState.RIP = (Uint)&NewTaskHeader;
 	
-	// Save core machine state
-	new->SavedState.RSP = rsp;
-	new->SavedState.RBP = rbp;
-	rip = GetRIP();
-	if(rip == SWITCH_MAGIC) {
-		outb(0x20, 0x20);	// ACK Timer and return as child
-		__asm__ __volatile__ ("sti");
-		return 0;
-	}
+	Log("New (Worker) %p, rsp = %p\n", new->SavedState.RIP, new->SavedState.RSP);
 	
-	// Set EIP as parent
-	new->SavedState.RIP = rip;
 	// Mark as active
 	new->Status = THREAD_STAT_PREINIT;
 	Threads_AddActive( new );
@@ -685,7 +621,7 @@ void Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP)
 	if( CS != 0x1B || SS != 0x23 ) {
 		Log_Error("Proc", "Proc_StartProcess: CS / SS are not valid (%x, %x)",
 			CS, SS);
-		return ;
+		Threads_Exit(0, -1);
 	}
 //	MAGIC_BREAK();	
 	__asm__ __volatile__ (
@@ -748,9 +684,8 @@ void Proc_DumpThreadCPUState(tThread *Thread)
  * \fn void Proc_Scheduler(int CPU)
  * \brief Swap current thread and clears dead threads
  */
-void Proc_Scheduler(int CPU)
+void Proc_Scheduler(int CPU, Uint RSP, Uint RIP)
 {
-	Uint	rsp, rbp, rip;
 	tThread	*thread;
 
 	if( CPU == 0 )
@@ -770,21 +705,18 @@ void Proc_Scheduler(int CPU)
 		// Reset quantum for next call
 		thread->Remaining = thread->Quantum;
 	
-		// Get machine state
-		__asm__ __volatile__ ("mov %%rsp, %0":"=r"(rsp));
-		__asm__ __volatile__ ("mov %%rbp, %0":"=r"(rbp));
-		rip = GetRIP();
-		if(rip == SWITCH_MAGIC)	return;	// Check if a switch happened
-		
 		// Save machine state
-		thread->SavedState.RSP = rsp;
-		thread->SavedState.RBP = rbp;
-		thread->SavedState.RIP = rip;
+		thread->SavedState.RSP = RSP;
+		thread->SavedState.RIP = RIP;
+		
+//		LogF("\nSaved %i, RIP = %p, rsp = %p\n", thread->TID, thread->SavedState.RIP, thread->SavedState.RSP);
 		
 		// TODO: Make this more stable somehow
-		regs = (tRegs*)(rbp+(2+1)*8);	// RBP,Ret + CurThread
-		thread->SavedState.UserCS = regs->CS;
-		thread->SavedState.UserRIP = regs->RIP;
+		{
+			regs = (tRegs*)(RSP+(1)*8);	// CurThread
+			thread->SavedState.UserCS = regs->CS;
+			thread->SavedState.UserRIP = regs->RIP;
+		}
 	}
 
 	#if BREAK_ON_SWITCH
@@ -813,10 +745,11 @@ void Proc_Scheduler(int CPU)
 	#endif
 	
 	#if DEBUG_TRACE_SWITCH
-	LogF("Switching to task %i, CR3 = 0x%x, RIP = %p",
+	LogF("\nSwitching to task %i, CR3 = 0x%x, RIP = %p, RSP = %p\n",
 		thread->TID,
 		thread->MemState.CR3,
-		thread->SavedState.RIP
+		thread->SavedState.RIP,
+		thread->SavedState.RSP
 		);
 	#endif
 	
@@ -831,13 +764,14 @@ void Proc_Scheduler(int CPU)
 	
 	// Switch threads
 	__asm__ __volatile__ (
-		"mov %4, %%cr3\n\t"
-		"mov %1, %%rsp\n\t"	// Restore RSP
-		"mov %2, %%rbp\n\t"	// and RBP
-		"jmp *%3" : :	// And return to where we saved state (Proc_Clone or Proc_Scheduler)
-		"a"(SWITCH_MAGIC), "r"(thread->SavedState.RSP),
-		"r"(thread->SavedState.RBP), "r"(thread->SavedState.RIP),
-		"r"(thread->MemState.CR3)
+		"mov %2, %%cr3\n\t"
+		"mov %0, %%rsp\n\t"	// Restore RSP
+		"invlpg (%%rsp)\n\t"
+		"invlpg 0x1000(%%rsp)\n\t"
+		"invlpg -0x1000(%%rsp)\n\t"
+		"jmp *%1" : :	// And return to where we saved state (Proc_Clone or Proc_Scheduler)
+		"r"(thread->SavedState.RSP), "r"(thread->SavedState.RIP),
+		"r"(thread->MemState.CR3), "a" (0)
 		);
 	for(;;);	// Shouldn't reach here
 }
diff --git a/Kernel/arch/x86_64/start32.asm b/Kernel/arch/x86_64/start32.asm
index 6ee9262b..e1db2c5c 100644
--- a/Kernel/arch/x86_64/start32.asm
+++ b/Kernel/arch/x86_64/start32.asm
@@ -96,7 +96,7 @@ gGDT:
 	dd	0x00000000, 0x00009200	; 0x10: 64-bit Data
 	dd	0x00000000, 0x0020FA00	; 0x18: 64-bit User Code
 	dd	0x00000000, 0x0000F200	; 0x20: 64-bit User Data
-	dd	0x00000000, 0x0040FA00	; 0x38: 32-bit User Code
+	dd	0x00000000, 0x0040FA00	; 0x28: 32-bit User Code
 	dd	0x00000000, 0x0040F200	; 0x30: 32-bit User Data
 	times MAX_CPUS	dd	0, 0x00008900, 0, 0	; 0x38+16*n: TSS 0
 gGDTPtr:
@@ -114,7 +114,9 @@ gMultibootPtr:
 [global gInitialPML4]
 gInitialPML4:	; Covers 256 TiB (Full 48-bit Virtual Address Space)
 	dd	gInitialPDP - KERNEL_BASE + 3, 0	; Identity Map Low 4Mb
-	times 512-4-1 dq	0
+	times 0xA0*2-1	dq	0
+	dd	gStackPDP - KERNEL_BASE + 3, 0
+	times 512-4-($-gInitialPML4)/8	dq	0
 	dd	gInitialPML4 - KERNEL_BASE + 3, 0	; Fractal Mapping
 	dq	0
 	dq	0
@@ -124,6 +126,10 @@ gInitialPDP:	; Covers 512 GiB
 	dd	gInitialPD - KERNEL_BASE + 3, 0
 	times 511	dq	0
 
+gStackPDP:
+	dd	gStackPD - KERNEL_BASE + 3, 0
+	times 511	dq	0
+
 gHighPDP:	; Covers 512 GiB
 	times 510	dq	0
 	;dq	0 + 0x143	; 1 GiB Page from zero
@@ -131,11 +137,25 @@ gHighPDP:	; Covers 512 GiB
 	dq	0
 
 gInitialPD:	; Covers 1 GiB
+;	dq	0 + 0x143	; 1 GiB Page from zero
 	dd	gInitialPT1 - KERNEL_BASE + 3, 0
 	dd	gInitialPT2 - KERNEL_BASE + 3, 0
 	times 510	dq	0
 
-gInitialPT1:	; Covers 2 MiB
+gStackPD:
+	dd	gKStackPT - KERNEL_BASE + 3, 0
+	times 511	dq	0
+
+gKStackPT:	; Covers 2 MiB
+	; Initial stack - 64KiB
+	dq	0
+	%assign i 0
+	%rep 16-1
+	dd	gInitialKernelStack - KERNEL_BASE + i*0x1000 + 0x103, 0
+	%assign i i+1
+	%endrep
+	times 512-16	dq 0
+gInitialPT1:	; 2 MiB
 	%assign i 0
 	%rep 512
 	dq	i*4096+0x103
@@ -148,6 +168,10 @@ gInitialPT2:	; 2 MiB
 	%assign i i+1
 	%endrep
 
+[section .padata]
+[global gInitialKernelStack]
+gInitialKernelStack:
+	times 0x1000*(16-1)	db 0	; 16 Pages
 
 [section .rodata]
 csNot64BitCapable:
diff --git a/Kernel/arch/x86_64/start64.asm b/Kernel/arch/x86_64/start64.asm
index b0a85868..5027a1e8 100644
--- a/Kernel/arch/x86_64/start64.asm
+++ b/Kernel/arch/x86_64/start64.asm
@@ -35,7 +35,7 @@ start64:
 	rep stosq
 	
 	; Set kernel stack
-	mov rsp, gInitialKernelStack
+	mov rsp, 0xFFFFA00000000000 + 0x10000
 	
 	; Call main
 	mov edi, [gMultibootMagic - KERNEL_BASE]
@@ -47,11 +47,6 @@ start64:
 	hlt
 	jmp .hlt
 
-[global GetRIP]
-GetRIP:
-	mov rax, [rsp]
-	ret
-
 [global GetCPUNum]
 GetCPUNum:
 	xor rax, rax
@@ -166,8 +161,3 @@ User_Syscall_Exit:
 	; RDI: Return Value
 	int 0xAC
 
-[section .bss]
-[global gInitialKernelStack]
-	resd	1024*4	; 4 Pages
-gInitialKernelStack:
-
-- 
2.20.1