Nucleus

diff --git a/task/scheduler.c b/task/scheduler.c
new file mode 100644
index 0000000..9ad1405
--- /dev/null
+++ b/task/scheduler.c
@@ -0,0 +1,61 @@
+/*
+ * This file contains the scheduler.  It implements a basic task switching
+ * routine, as well as the schedule() function.  The scheduler can be called
+ * from anywhere, and will switch to the next task decided by the scheduler
+ * rules.  If it cannot find a task to schedule, it just idles until one becomes
+ * available.  This avoids the need for an idle task.
+ */
+
+#include <nucleus/panic.h>
+#include <nucleus/task.h>
+
+/* Read the EIP */
+static uintptr_t
+read_eip(void)
+{
+	uintptr_t eip;
+	asm volatile("movl 4(%%ebp), %0" : "=r" (eip));
+	return eip;
+}
+
+/* Switch to a task */
+static void
+switch_to_task(Task *task)
+{
+	uintptr_t esp, ebp, eip;
+	asm volatile("mov %%esp, %0" : "=r" (esp));
+	asm volatile("mov %%ebp, %0" : "=r" (ebp));
+	eip = read_eip();
+	if (eip == 0x10032004) /* Magic number */
+		return;
+
+	current->esp = esp;
+	current->ebp = ebp;
+	current->eip = eip;
+	current = task;
+	esp = current->esp;
+	ebp = current->ebp;
+	eip = current->eip;
+
+	asm volatile (
+		"cli;"
+		"movl %0, %%ecx;"
+		"movl %1, %%esp;"
+		"movl %2, %%ebp;"
+		"movl %3, %%cr3;"
+		"movl $0x10032004, %%eax;"
+		"sti;"
+		"jmp *%%ecx"
+		:: "g" (eip), "g" (esp), "g" (ebp), "g" (current->pageDir)
+	);
+}
+
+/* Schedule the next task */
+void
+schedule(void)
+{
+	Task *task = current;
+	if (!task->next)
+		return;
+	switch_to_task(task->next);
+}