X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Kernel%2Farch%2Fx86_64%2Fproc.c;h=e3d7e90b651f9272e27890e3eab53abc27f99428;hb=d01b7e1a4924a41a5219eedfd2f6d32b310368cd;hp=31dd80e373e4b321ff44ccf2a6823455be2ec393;hpb=818dd00ebd8c60ea2d47a01a26381b4adc087bdb;p=tpg%2Facess2.git

diff --git a/Kernel/arch/x86_64/proc.c b/Kernel/arch/x86_64/proc.c
index 31dd80e3..e3d7e90b 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,13 @@ 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 Uint	Proc_CloneInt(Uint *RSP, Uint *CR3);
+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;
@@ -45,8 +48,9 @@ extern int	giTotalTickets;
 extern int	giNumActiveThreads;
 extern tThread	gThreadZero;
 extern void	Threads_Dump(void);
-extern void	Proc_ReturnToUser(void);
+extern void	Proc_ReturnToUser(tVAddr Handler, tVAddr KStackTop, int Argument);
 extern void	Time_UpdateTimestamp(void);
+extern void	SwitchTasks(Uint NewSP, Uint *OldSP, Uint NewIP, Uint *OldIO, Uint CR3);
 
 // === PROTOTYPES ===
 //void	ArchThreads_Init(void);
@@ -54,20 +58,24 @@ 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_Reschedule(void);
+void	Proc_Scheduler(int CPU, Uint RSP, Uint RIP);
 
 // === GLOBALS ===
+//!\brief Used by desctab.asm in SyscallStub
+const int ci_offsetof_tThread_KernelStack = offsetof(tThread, KernelStack);
 // --- Multiprocessing ---
 #if USE_MP
 volatile int	giNumInitingCPUs = 0;
@@ -302,6 +310,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
@@ -313,11 +322,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
@@ -348,6 +354,22 @@ 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
+		Threads_Yield();
+	}
+}
+
 /**
  * \fn void Proc_Start(void)
  * \brief Start process scheduler
@@ -365,15 +387,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
@@ -390,25 +407,13 @@ 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");
 	#endif
 	MM_FinishVirtualInit();
-	Log("Multithreading started");
+	Log_Log("Proc", "Multithreading started");
 }
 
 /**
@@ -425,55 +430,42 @@ tThread *Proc_GetCurThread(void)
 }
 
 /**
- * \fn void Proc_ChangeStack(void)
- * \brief Swaps the current stack for a new one (in the proper stack reigon)
+ * \brief Create a new kernel thread
  */
-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(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;
 }
 
 /**
@@ -482,74 +474,35 @@ 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));
-	
-	newThread = Threads_CloneTCB(NULL, Flags);
-	if(!newThread)	return -1;
-	
-	Log("Proc_Clone: newThread = %p", newThread);
-	
-	// 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;
-		}
+	tThread	*newThread, *cur = Proc_GetCurThread();
+	Uint	rip;
 
-		// 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;
-		}
+	// Sanity check	
+	if( !(Flags & CLONE_VM) ) {
+		Log_Error("Proc", "Proc_Clone: Don't leave CLONE_VM unset, use Proc_NewKThread instead");
+		return -1;
 	}
+
+	// Create new TCB
+	newThread = Threads_CloneTCB(Flags);
+	if(!newThread)	return -1;
 	
 	// 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;
-	}
-	
-	// Set EIP as parent
+	rip = Proc_CloneInt(&newThread->SavedState.RSP, &newThread->MemState.CR3);
+	if(rip == 0)	return 0;	// Child
+	newThread->KernelStack = cur->KernelStack;
 	newThread->SavedState.RIP = rip;
+
+	// DEBUG
+	#if 0
+	Log("New (Clone) %p, rsp = %p, cr3 = %p", rip, newThread->SavedState.RSP, newThread->MemState.CR3);
+	{
+		Uint cr3;
+		__asm__ __volatile__ ("mov %%cr3, %0" : "=r" (cr3));
+		Log("Current CR3 = 0x%x, PADDR(RSP) = 0x%x", cr3, MM_GetPhysAddr(newThread->SavedState.RSP));
+	}
+	#endif
+	// /DEBUG
 	
 	// Lock list and add to active
 	Threads_AddActive(newThread);
@@ -561,11 +514,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
@@ -577,28 +530,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 );
@@ -607,7 +554,6 @@ int Proc_SpawnWorker(void)
 }
 
 /**
- * \fn Uint Proc_MakeUserStack(void)
  * \brief Creates a new user stack
  */
 Uint Proc_MakeUserStack(void)
@@ -617,15 +563,22 @@ Uint Proc_MakeUserStack(void)
 	
 	// Check Prospective Space
 	for( i = USER_STACK_SZ >> 12; i--; )
+	{
 		if( MM_GetPhysAddr( base + (i<<12) ) != 0 )
 			break;
+	}
 	
 	if(i != -1)	return 0;
 	
 	// Allocate Stack - Allocate incrementally to clean up MM_Dump output
-	for( i = 0; i < USER_STACK_SZ/0x1000; i++ )
+	for( i = 0; i < (USER_STACK_SZ-USER_STACK_PREALLOC)/0x1000; i++ )
 	{
-		if( !MM_Allocate( base + (i<<12) ) )
+		MM_AllocateZero( base + (i<<12) );
+	}
+	for( ; i < USER_STACK_SZ/0x1000; i++ )
+	{
+		tPAddr	alloc = MM_Allocate( base + (i<<12) );
+		if( !alloc )
 		{
 			// Error
 			Log_Error("Proc", "Unable to allocate user stack (%i pages requested)", USER_STACK_SZ/0x1000);
@@ -639,68 +592,76 @@ Uint Proc_MakeUserStack(void)
 }
 
 
-/**
- * \fn void Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize)
- * \brief Starts a user task
- */
-void Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize)
+void Proc_StartUser(Uint Entrypoint, Uint Base, int ArgC, char **ArgV, int DataSize)
 {
-	Uint	*stack = (void*)Proc_MakeUserStack();
+	Uint	*stack;
+	char	**envp;
 	 int	i;
 	Uint	delta;
 	Uint16	ss, cs;
 	
-	LOG("stack = 0x%x", stack);
 	
 	// Copy Arguments
-	stack = (void*)( (Uint)stack - DataSize );
+	stack = (void*)Proc_MakeUserStack();
+	if(!stack) {
+		Log_Error("Proc", "Unable to create user stack!");
+		Threads_Exit(0, -1);
+	}
+	stack -= (DataSize+7)/8;
+	LOG("stack = 0x%x", stack);
+	Log("stack = %p, DataSize = %i", stack, DataSize);
 	memcpy( stack, ArgV, DataSize );
+	free(ArgV);
 	
 	// Adjust Arguments and environment
 	delta = (Uint)stack - (Uint)ArgV;
 	ArgV = (char**)stack;
 	for( i = 0; ArgV[i]; i++ )	ArgV[i] += delta;
-	i ++;
-	EnvP = &ArgV[i];
-	for( i = 0; EnvP[i]; i++ )	EnvP[i] += delta;
+	envp = &ArgV[i+1];
+	for( i = 0; envp[i]; i++ )	envp[i] += delta;
 	
 	// User Mode Segments
-	ss = 0x23;	cs = 0x1B;
+	// 0x2B = 64-bit
+	ss = 0x23;	cs = 0x2B;
 	
 	// Arguments
-	*--stack = (Uint)EnvP;
+	*--stack = (Uint)envp;
 	*--stack = (Uint)ArgV;
 	*--stack = (Uint)ArgC;
-	while(*Bases)
-		*--stack = *Bases++;
-	*--stack = 0;	// Return Address
+	*--stack = Base;
 	
 	Proc_StartProcess(ss, (Uint)stack, 0x202, cs, Entrypoint);
 }
 
 void Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP)
 {
-	Uint	*stack = (void*)Stack;
-	*--stack = SS;		//Stack Segment
-	*--stack = Stack;	//Stack Pointer
-	*--stack = Flags;	//EFLAGS (Resvd (0x2) and IF (0x20))
-	*--stack = CS;		//Code Segment
-	*--stack = IP;	//EIP
-	//PUSHAD
-	*--stack = 0xAAAAAAAA;	// eax
-	*--stack = 0xCCCCCCCC;	// ecx
-	*--stack = 0xDDDDDDDD;	// edx
-	*--stack = 0xBBBBBBBB;	// ebx
-	*--stack = 0xD1D1D1D1;	// edi
-	*--stack = 0x54545454;	// rsp - NOT POPED
-	*--stack = 0x51515151;	// esi
-	*--stack = 0xB4B4B4B4;	// rbp
-	//Individual PUSHs
-	*--stack = SS;	// ds
-	
-	__asm__ __volatile__ (
-	"mov %%rax,%%rsp;\n\t"	// Set stack pointer
-	"iret;\n\t" : : "a" (stack));
+	if( !(CS == 0x1B || CS == 0x2B) || SS != 0x23 ) {
+		Log_Error("Proc", "Proc_StartProcess: CS / SS are not valid (%x, %x)",
+			CS, SS);
+		Threads_Exit(0, -1);
+	}
+//	Log("Proc_StartProcess: (SS=%x, Stack=%p, Flags=%x, CS=%x, IP=%p)", SS, Stack, Flags, CS, IP);
+//	MM_DumpTables(0, USER_MAX);
+	if(CS == 0x1B)
+	{
+		// 32-bit return
+		__asm__ __volatile__ (
+			"mov %0, %%rsp;\n\t"	// Set stack pointer
+			"mov %2, %%r11;\n\t"	// Set RFLAGS
+			"sysret;\n\t"
+			: : "r" (Stack), "c" (IP), "r" (Flags)
+			);
+	}
+	else
+	{
+		// 64-bit return
+		__asm__ __volatile__ (
+			"mov %0, %%rsp;\n\t"	// Set stack pointer
+			"mov %2, %%r11;\n\t"	// Set RFLAGS
+			"sysretq;\n\t"
+			: : "r" (Stack), "c" (IP), "r" (Flags)
+			);
+	}
 	for(;;);
 }
 
@@ -739,9 +700,8 @@ int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
  */
 void Proc_CallFaultHandler(tThread *Thread)
 {
-	// Rewinds the stack and calls the user function
 	// Never returns
-	__asm__ __volatile__ ("mov %0, %%rbp;\n\tcall Proc_ReturnToUser" :: "r"(Thread->FaultHandler));
+	Proc_ReturnToUser(Thread->FaultHandler, Thread->KernelStack, Thread->CurFaultNum);
 	for(;;);
 }
 
@@ -750,18 +710,57 @@ void Proc_DumpThreadCPUState(tThread *Thread)
 	Log("  At %04x:%016llx", Thread->SavedState.UserCS, Thread->SavedState.UserRIP);
 }
 
+void Proc_Reschedule(void)
+{
+	tThread	*nextthread, *curthread;
+	 int	cpu = GetCPUNum();
+
+	// TODO: Wait for it?
+	if(IS_LOCKED(&glThreadListLock))        return;
+	
+	curthread = gaCPUs[cpu].Current;
+
+	nextthread = Threads_GetNextToRun(cpu, curthread);
+
+	if(nextthread == curthread)	return ;
+	if(!nextthread)
+		nextthread = gaCPUs[cpu].IdleThread;
+	if(!nextthread)
+		return ;
+
+	#if DEBUG_TRACE_SWITCH
+	LogF("\nSwitching to task CR3 = 0x%x, RIP = %p, RSP = %p - %i (%s)\n",
+		nextthread->MemState.CR3,
+		nextthread->SavedState.RIP,
+		nextthread->SavedState.RSP,
+		nextthread->TID,
+		nextthread->ThreadName
+		);
+	#endif
+
+	// Update CPU state
+	gaCPUs[cpu].Current = nextthread;
+	gTSSs[cpu].RSP0 = nextthread->KernelStack-4;
+	__asm__ __volatile__ ("mov %0, %%db0" : : "r" (nextthread));
+
+	SwitchTasks(
+		nextthread->SavedState.RSP, &curthread->SavedState.RSP,
+		nextthread->SavedState.RIP, &curthread->SavedState.RIP,
+		nextthread->MemState.CR3
+		);
+	return ;
+}
+
 /**
  * \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;
+#if 0
+	{
 	tThread	*thread;
 
-	if( CPU == 0 )
-		Time_UpdateTimestamp();
-	
 	// If the spinlock is set, let it complete
 	if(IS_LOCKED(&glThreadListLock))	return;
 	
@@ -775,77 +774,20 @@ void Proc_Scheduler(int CPU)
 		if(thread->Remaining--)	return;
 		// 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;
 		
 		// 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
-	{
-	tThread	*oldthread = thread;
-	#endif
+	// ACK Timer here?
 
-	// Get next thread
-	thread = Threads_GetNextToRun(CPU, thread);
-	
-	// Error Check
-	if(thread == NULL) {
-		thread = gaCPUs[CPU].IdleThread;
-		//Warning("Hmm... Threads_GetNextToRun returned NULL, I don't think this should happen.\n");
-//		LogF("Zzzzz.\n");
-		//return;
-	}
-	if(thread == NULL ) {
-		return ;
-	}
-	#if BREAK_ON_SWITCH
-	if( thread != oldthread ) {
-		MAGIC_BREAK();
+	Proc_Reschedule();
 	}
-	}
-	#endif
-	
-	#if DEBUG_TRACE_SWITCH
-	LogF("Switching to task %i, CR3 = 0x%x, RIP = %p",
-		thread->TID,
-		thread->MemState.CR3,
-		thread->SavedState.RIP
-		);
-	#endif
-	
-	
-	if(CPU > MAX_CPUS)
-		LogF("CPU = %i", CPU);
-	// Set current thread
-	gaCPUs[CPU].Current = thread;
-	
-	// Update Kernel Stack pointer
-	gTSSs[CPU].RSP0 = thread->KernelStack-4;
-	
-	// 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)
-		);
-	for(;;);	// Shouldn't reach here
+#endif
 }
 
 // === EXPORTS ===