diff --git a/src/core/scheduler.c b/src/core/scheduler.c
index 848b7b7..0a8c5c7 100644
--- a/src/core/scheduler.c
+++ b/src/core/scheduler.c
@@ -1,12 +1,67 @@
+#include "scheduler.h"
 #include "libc/stdio.h"
 #include "gdt.h"
-#include "mem.h"
 #include "paging.h"
 
 char show_tics=0;
+char scheduler_on=0;
+PROC procs[MAX_PROC];
+u16 current_id;
+u16 nproc;
 
 void schedule(){
+  // Note that this function is called by clock
+  // clock is called by INT_CLOCK (core/int.S)
+  // which store all the process information on
+  // the stack. Thus, knowing the C calling conventions
+  // and that schedule() is call by two functions with no parameters,
+  // the first process register can be accessed by ebp+2
+  u32 *stack;
+  asm("mov %%ebp, %0":"=r" (stack));
+  
+  // No proc to schedule
+  if(nproc<2)
+    return;
+  
+  PROC *p=&procs[current_id];
+  p->regs.gs=stack[2]; // ebp+2=gs cf note above
+  p->regs.fs=stack[3];
+  p->regs.es=stack[4];
+  p->regs.ds=stack[5];
+  p->regs.edi=stack[6];
+  p->regs.esi=stack[7];
+  p->regs.ebp=stack[8];
+  // We do not take p->regs.esp=stack[9]
+  // since it corresponds to the kernel stack 
+  // (it was push during the interruption)
+  p->regs.edx=stack[10];
+  p->regs.ecx=stack[11];
+  p->regs.ebx=stack[12];
+  p->regs.eax=stack[13];
+  p->regs.eip=stack[14];
+  p->regs.cs=stack[15];
+  p->regs.eflags=stack[16];
+  p->regs.esp=stack[17];
+  p->regs.ss=stack[18];
 
+  current_id++;
+  if(current_id>=nproc)
+    current_id=0;
+
+  // Have a clean stack on next interrupt
+  TSS.esp0=(u32)stack+19;
+  asm("mov %%ss, %0": "=m" (TSS.ss0));
+
+  // Ensure interrupts are activated and NT flag is clear
+  p->regs.eflags|=0x200;
+  p->regs.eflags&=0xffffbfff;
+
+  // Perform task switch
+  asm(
+    "mov %0, %%esi    \n\t"
+    "jmp task_switch  \n\t"
+    :: "a" (p)
+  );
 }
 
 void clock(){
@@ -19,7 +74,8 @@ void clock(){
     if(show_tics)
       putchar('.');
   }
-  schedule();
+  if(scheduler_on==1)
+    schedule();
 }
 
 void run_task(int *page_dir, void *task, int task_size){
diff --git a/src/core/scheduler.h b/src/core/scheduler.h
index 8495e77..9055fda 100644
--- a/src/core/scheduler.h
+++ b/src/core/scheduler.h
@@ -1,7 +1,36 @@
 #ifndef SCHEDULER_H
 #define SCHEDULER_H
 
+#include "mem.h"
+
+
+#define MAX_PROC 10
+
+// If you change the following struct
+// please consider updating the code in scheduler_asm.S
+typedef struct REGISTERS {
+    u32 eax, ebx, ecx, edx;
+    u32 cs, eip;
+    u32 ss, esp, ebp;
+    u32 esi, edi;
+    u32 ds, es, fs, gs;
+    u32 eflags;
+} __attribute__((packed)) REGISTERS;
+
+// If you change the following struct
+// please consider updating the code in scheduler_asm.S
+typedef struct PROC {
+    u16 id;
+    u16 pid;
+    u32* page_dir;
+    REGISTERS regs;
+} __attribute__((packed)) PROC;
+
 extern char show_tics;
+extern char scheduler_on;
+extern PROC procs[MAX_PROC];
+extern u16 current_id;
+extern u16 nproc;
 
 void clock();
 void schedule();
diff --git a/src/core/scheduler_asm.S b/src/core/scheduler_asm.S
new file mode 100644
index 0000000..92e3f2e
--- /dev/null
+++ b/src/core/scheduler_asm.S
@@ -0,0 +1,46 @@
+.globl task_switch
+
+// This function takes a pointer
+// to a PROC structure and jump
+// to this process
+task_switch:
+    push 12(%esi)  # eax (cf PROC struct)
+    push 16(%esi)  # ebx
+    push 20(%esi)  # ecx
+    push 24(%esi)  # edx
+    push 44(%esi)  # ebp
+    push 48(%esi)  # esi
+    push 52(%esi)  # edi
+    push 56(%esi)  # ds
+    push 60(%esi)  # es
+    push 64(%esi)  # fs
+    push 68(%esi)  # gs
+
+    // Don't forget to clear the interrupt
+    movb $0x20, %al
+    outb %al, $0x20
+
+    // Setup process page directory
+    mov 8(%esi), %eax
+    mov %eax, %cr3
+
+    // Setup registers
+    pop %gs
+    pop %fs
+    pop %es
+    pop %ds
+    pop %edi
+    pop %esi
+    pop %ebp
+    pop %edx
+    pop %ecx
+    pop %ebx
+    pop %eax
+
+    // Perform the task switch
+    push 36(%esi)  # ss
+    push 40(%esi)  # esp
+    push 72(%esi)  # eflags
+    push 28(%esi)  # cs
+    push 32(%esi)  # eip
+    iret