1 files changed, 23 insertions, 12 deletions

diff --git a/src/core/scheduler.c b/src/core/scheduler.c
index 59f3b9d..0c1d255 100644
--- a/src/core/scheduler.c
+++ b/src/core/scheduler.c
@@ -37,8 +37,19 @@ void schedule(u32 *stack){
   p->regs.eip=stack[12];
   p->regs.cs=stack[13];
   p->regs.eflags=stack[14];
-  p->regs.esp=stack[15];
-  p->regs.ss=stack[16];
+
+  // If clock occurs during a syscall (another interrupt)
+  if(p->regs.cs==0x08){
+    // Since we where already in kernel mode
+    // the CPU did not push ss and esp
+    // thus esp point to 
+    p->regs.esp=&stack[15]; // Since we came from another interrupt this value will be restore by the last interrupt
+    p->regs.ss=TSS.ss0;
+  }
+  else {
+    p->regs.esp=stack[15];
+    p->regs.ss=stack[16];
+  }
 
   // Get the next task to run
   current_id++;
@@ -46,9 +57,9 @@ void schedule(u32 *stack){
     current_id=0;
   p=&procs[current_id];
  
-  // Have a clean stack on next interrupt
-  TSS.esp0=(u32)stack+17;
-  asm("mov %%ss, %0": "=m" (TSS.ss0));
+  // Use kernel stack of the next task
+  TSS.ss0=p->regs.ss0;
+  TSS.esp0=p->regs.esp0;
 
   // Ensure interrupts are activated and NT flag is clear
   p->regs.eflags|=0x200;
@@ -90,6 +101,8 @@ void task_create(void *task, int task_size){
     void *entry_point=PAGING_ENTRY_POINT_VIRT;
     // User stack start at the end of last allocated page (since addresses are going down)
     void *ustack=(void*)(PAGING_ENTRY_POINT_VIRT+allocated*4096);
+    // Kernel stack start at the end of last allocated page +1 (see paging_allocate() implementation)
+    void *kstack=(void*)(PAGING_ENTRY_POINT_VIRT+(allocated+1)*4096);
 
     // Load the task into memory
     memcpy(task,PAGING_ENTRY_POINT_PHY(page_dir), task_size);
@@ -105,6 +118,8 @@ void task_create(void *task, int task_size){
     p->regs.ebx=0;
     p->regs.ecx=0;
     p->regs.edx=0;
+    p->regs.ss0=0x18;
+    p->regs.esp0=kstack;
 
     // Manage eflags
     u32 eflags;
@@ -135,19 +150,15 @@ void scheduler_start(){
     // Enable scheduling
     scheduler_on=1;
 
-    // Save kernel stack state
-    u32 *stack;
-    asm("mov %%ebp, %0":"=r" (stack));
-    // Remove ebp from the (c call convention) and return address (call)
-    TSS.esp0=(u32)stack+2;
-    asm("mov %%ss, %0": "=m" (TSS.ss0));
-
     // Get first stack
     current_id=0;
     PROC *p=&procs[current_id];
     // Ensure interrupts are activated and NT flag is clear
     p->regs.eflags|=0x200;
     p->regs.eflags&=0xffffbfff;
+    // Use task kernel stack
+    TSS.ss0=p->regs.ss0;
+    TSS.esp0=p->regs.esp0;
     // Switch to user task
     asm(
       "push %0    \n\t"