almost done multitasking

16 files changed, 240 insertions, 90 deletions

diff --git a/kernel/Makefile b/kernel/Makefile
index 5e6cb9d..d1c8d32 100644
--- a/kernel/Makefile
+++ b/kernel/Makefile
@@ -20,6 +20,7 @@ OBJS = \
 	src/cpu/kcpuid.o \
 	src/cpu/msr.o \
 	src/cpu/pic.o \
+	src/cpu/regs.o \
 	src/cpu/tss.o \
 	src/devices/disc.o \
 	src/devices/keyboard.o \
diff --git a/kernel/include/idt.h b/kernel/include/idt.h
index 2d28be0..54b8ccb 100644
--- a/kernel/include/idt.h
+++ b/kernel/include/idt.h
@@ -30,7 +30,8 @@ void disable_interrupts(void);
 void init_idt(void);
 void load_idt(idtp *pointer);
 void init_idt_table(void);
-void add_to_idt(uint16_t num, uint64_t offset, uint16_t selector, uint8_t type);
+void add_to_idt(uint16_t num, uint64_t offset, uint16_t selector, uint8_t type,
+		uint8_t ist);
 
 extern idtp idt_pointer;
 
diff --git a/kernel/include/process.h b/kernel/include/process.h
index ff82924..14107c2 100644
--- a/kernel/include/process.h
+++ b/kernel/include/process.h
@@ -4,7 +4,7 @@
 #include <types.h>
 #include <libk/list.h>
 
-void save_context_to_rsp(uint64_t irq_rsp, uint64_t task_rsp);
+uint64_t save_context_from_rsp(uint64_t irq_rsp);
 __attribute__((noreturn)) void restore_context_from_rsp(uint64_t task_rsp);
 
 struct process_t {
@@ -16,6 +16,12 @@ typedef struct process_t process_t;
 extern process_t process_queue;
 extern process_t *curr_process;
 
+process_t *init_process(uint64_t priv_lvl, uint64_t rip, uint64_t argc,
+			uint64_t *argv);
+void process_init_wrapper(uint64_t rip, uint64_t argc, uint64_t *argv);
+__attribute__((noreturn)) void idle_thread(void);
+__attribute__((noreturn)) void idle_thread2(void);
+__attribute__((noreturn)) void remove_current_process(void);
 __attribute__((noreturn)) void context_switch(uint64_t irq_rsp);
 
 #endif
diff --git a/kernel/include/regs.h b/kernel/include/regs.h
index ec2ed89..67795ae 100644
--- a/kernel/include/regs.h
+++ b/kernel/include/regs.h
@@ -52,4 +52,6 @@ struct regs_t {
 } __attribute__((packed));
 typedef struct regs_t regs_t;
 
+void print_regs_from_rsp(uint64_t rsp);
+
 #endif
diff --git a/kernel/include/scheduler.h b/kernel/include/scheduler.h
index 73a9d2b..79f462c 100644
--- a/kernel/include/scheduler.h
+++ b/kernel/include/scheduler.h
@@ -2,7 +2,12 @@
 #define SCHEDULER_H
 
 #include <process.h>
+#include <atomic.h>
 
+__attribute__((noreturn)) void init_scheduler(void);
 process_t *scheduler(void);
 
+extern mutex_t scheduler_lock;
+extern uint32_t sched_init;
+
 #endif
diff --git a/kernel/include/x86_64_regs.S b/kernel/include/x86_64_regs.S
index 644f4f8..a1d014d 100644
--- a/kernel/include/x86_64_regs.S
+++ b/kernel/include/x86_64_regs.S
@@ -1,3 +1,6 @@
+#ifndef X86_64_REGS_H
+#define X86_64_REGS_H
+
 /* isr stack */
 /* ss:rsp (original rsp) -> rflags -> cs -> rip */
 
@@ -99,3 +102,5 @@
 	pop %rcx
 	pop %rax
 .endm
+
+#endif
diff --git a/kernel/src/check/panic.c b/kernel/src/check/panic.c
index b5e02a2..277ee37 100644
--- a/kernel/src/check/panic.c
+++ b/kernel/src/check/panic.c
@@ -15,33 +15,7 @@ __attribute__((noreturn)) void panic(uint64_t rsp, const char *s, ...)
 	va_end(list);
 
 	if (rsp) {
-		regs_t *regs = (regs_t *)rsp;
-
-		printf("info regs:\n");
-		printf("rax: 0x%x; ", regs->rax);
-		printf("rbx: 0x%x; ", regs->rbx);
-		printf("rcx: 0x%x; ", regs->rcx);
-		printf("rdx: 0x%x; ", regs->rdx);
-		printf("rdi: 0x%x; ", regs->rdi);
-		printf("rsi: 0x%x; ", regs->rsi);
-		printf("rsp: 0x%x; ", regs->rsp);
-		printf("rbp: 0x%x; ", regs->rbp);
-		printf("r8: 0x%x; ", regs->r8);
-		printf("r9: 0x%x; ", regs->r9);
-		printf("r10: 0x%x; ", regs->r10);
-		printf("r11: 0x%x; ", regs->r11);
-		printf("r12: 0x%x; ", regs->r12);
-		printf("r13: 0x%x; ", regs->r13);
-		printf("r14: 0x%x; ", regs->r14);
-		printf("r15: 0x%x; ", regs->r15);
-		printf("rip: 0x%x; ", regs->rip);
-		printf("cs: 0x%x; ", regs->cs);
-		printf("ds: 0x%x; ", regs->seg >> 48);
-		printf("ss: 0x%x; ", regs->ss);
-		printf("es: 0x%x; ", (regs->seg >> 32) & 0xFFFF);
-		printf("fs: 0x%x; ", (regs->seg >> 16) & 0xFFFF);
-		printf("gs: 0x%x; ", regs->seg & 0xFFFF);
-		printf("rflags: 0x%x; ", regs->rflags);
+		print_regs_from_rsp(rsp);
 	}
 
 	disable_interrupts();
diff --git a/kernel/src/cpu/idt.c b/kernel/src/cpu/idt.c
index 5ac6386..b60b8b9 100644
--- a/kernel/src/cpu/idt.c
+++ b/kernel/src/cpu/idt.c
@@ -20,60 +20,61 @@ void load_idt(idtp *pointer)
 	__asm__ __volatile__("lidt (%0);" : : "r"(pointer) :);
 }
 
-void add_to_idt(uint16_t num, uint64_t offset, uint16_t selector, uint8_t type)
+void add_to_idt(uint16_t num, uint64_t offset, uint16_t selector, uint8_t type,
+		uint8_t ist)
 {
 	idt_table[num].offset_1 = offset & 0xFFFF;
 	idt_table[num].offset_2 = (offset >> 16) & 0xFFFF;
 	idt_table[num].offset_3 = (uint32_t)(offset >> 32);
 	idt_table[num].selector = selector;
 	idt_table[num].type_attributes = type;
-	idt_table[num].ist = 0;
+	idt_table[num].ist = ist;
 	idt_table[num].zero = 0;
 }
 
 void init_idt_table(void)
 {
 	/* exceptions */
-	add_to_idt(0, (uint64_t)isr0, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(1, (uint64_t)isr1, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(2, (uint64_t)isr2, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(3, (uint64_t)isr3, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(4, (uint64_t)isr4, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(5, (uint64_t)isr5, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(6, (uint64_t)isr6, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(7, (uint64_t)isr7, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(8, (uint64_t)isr8, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(9, (uint64_t)isr9, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(10, (uint64_t)isr10, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(11, (uint64_t)isr11, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(12, (uint64_t)isr12, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(13, (uint64_t)isr13, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(14, (uint64_t)isr14, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(15, (uint64_t)isr15, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(16, (uint64_t)isr16, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(17, (uint64_t)isr17, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(18, (uint64_t)isr18, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(19, (uint64_t)isr19, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(20, (uint64_t)isr20, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(21, (uint64_t)isr21, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(22, (uint64_t)isr22, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(23, (uint64_t)isr23, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(24, (uint64_t)isr24, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(25, (uint64_t)isr25, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(26, (uint64_t)isr26, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(27, (uint64_t)isr27, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(28, (uint64_t)isr28, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(29, (uint64_t)isr29, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(30, (uint64_t)isr30, GDT_CODE_SEG, TRAP_GATE);
-	add_to_idt(31, (uint64_t)isr31, GDT_CODE_SEG, TRAP_GATE);
+	add_to_idt(0, (uint64_t)isr0, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(1, (uint64_t)isr1, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(2, (uint64_t)isr2, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(3, (uint64_t)isr3, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(4, (uint64_t)isr4, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(5, (uint64_t)isr5, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(6, (uint64_t)isr6, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(7, (uint64_t)isr7, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(8, (uint64_t)isr8, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(9, (uint64_t)isr9, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(10, (uint64_t)isr10, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(11, (uint64_t)isr11, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(12, (uint64_t)isr12, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(13, (uint64_t)isr13, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(14, (uint64_t)isr14, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(15, (uint64_t)isr15, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(16, (uint64_t)isr16, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(17, (uint64_t)isr17, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(18, (uint64_t)isr18, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(19, (uint64_t)isr19, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(20, (uint64_t)isr20, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(21, (uint64_t)isr21, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(22, (uint64_t)isr22, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(23, (uint64_t)isr23, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(24, (uint64_t)isr24, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(25, (uint64_t)isr25, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(26, (uint64_t)isr26, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(27, (uint64_t)isr27, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(28, (uint64_t)isr28, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(29, (uint64_t)isr29, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(30, (uint64_t)isr30, GDT_CODE_SEG, TRAP_GATE, 0);
+	add_to_idt(31, (uint64_t)isr31, GDT_CODE_SEG, TRAP_GATE, 0);
 
 	/* interrupts */
-	add_to_idt(32, (uint64_t)irq0, GDT_CODE_SEG, INTERRUPT_GATE);
-	add_to_idt(33, (uint64_t)irq1, GDT_CODE_SEG, INTERRUPT_GATE);
+	add_to_idt(32, (uint64_t)irq0, GDT_CODE_SEG, INTERRUPT_GATE, 1);
+	add_to_idt(33, (uint64_t)irq1, GDT_CODE_SEG, INTERRUPT_GATE, 0);
 	size_t i;
 	for (i = 34; i < 256; i++) {
 		add_to_idt((uint16_t)i, (uint64_t)irq2, GDT_CODE_SEG,
-			   INTERRUPT_GATE);
+			   INTERRUPT_GATE, 0);
 	}
 }
 
diff --git a/kernel/src/cpu/irq.c b/kernel/src/cpu/irq.c
index 7480eef..becb7cb 100644
--- a/kernel/src/cpu/irq.c
+++ b/kernel/src/cpu/irq.c
@@ -77,12 +77,12 @@ void interrupt(uint64_t number, uint64_t rsp)
 {
 	switch (number) {
 	case 0x20:
-		timer_handler(rsp);
 		eoi(number);
+		timer_handler(rsp);
 		break;
 	case 0x21:
-		keyboard_handler();
 		eoi(number);
+		keyboard_handler();
 		break;
 	default:
 		printf("spurious interrupt\n");
diff --git a/kernel/src/cpu/regs.c b/kernel/src/cpu/regs.c
new file mode 100644
index 0000000..710448e
--- /dev/null
+++ b/kernel/src/cpu/regs.c
@@ -0,0 +1,33 @@
+#include <regs.h>
+#include <libk/stdio.h>
+
+void print_regs_from_rsp(uint64_t rsp)
+{
+	regs_t *regs = (regs_t *)rsp;
+
+	printf("info regs:\n");
+	printf("rax: 0x%x; ", regs->rax);
+	printf("rbx: 0x%x; ", regs->rbx);
+	printf("rcx: 0x%x; ", regs->rcx);
+	printf("rdx: 0x%x; ", regs->rdx);
+	printf("rdi: 0x%x; ", regs->rdi);
+	printf("rsi: 0x%x; ", regs->rsi);
+	printf("rsp: 0x%x; ", regs->rsp);
+	printf("rbp: 0x%x; ", regs->rbp);
+	printf("r8: 0x%x; ", regs->r8);
+	printf("r9: 0x%x; ", regs->r9);
+	printf("r10: 0x%x; ", regs->r10);
+	printf("r11: 0x%x; ", regs->r11);
+	printf("r12: 0x%x; ", regs->r12);
+	printf("r13: 0x%x; ", regs->r13);
+	printf("r14: 0x%x; ", regs->r14);
+	printf("r15: 0x%x; ", regs->r15);
+	printf("rip: 0x%x; ", regs->rip);
+	printf("cs: 0x%x; ", regs->cs);
+	printf("ds: 0x%x; ", regs->seg >> 48);
+	printf("ss: 0x%x; ", regs->ss);
+	printf("es: 0x%x; ", (regs->seg >> 32) & 0xFFFF);
+	printf("fs: 0x%x; ", (regs->seg >> 16) & 0xFFFF);
+	printf("gs: 0x%x; ", regs->seg & 0xFFFF);
+	printf("rflags: 0x%x; ", regs->rflags);
+}
diff --git a/kernel/src/cpu/tss.c b/kernel/src/cpu/tss.c
index cdfe2cc..f80f8db 100644
--- a/kernel/src/cpu/tss.c
+++ b/kernel/src/cpu/tss.c
@@ -14,10 +14,16 @@ void init_tss()
 {
 	memset(&tss, 0, sizeof(tss_type));
 	tss.iopb = sizeof(tss_type);
-	uint32_t stack_size = 4096 * 4;
+	uint32_t stack_size = 1024 * 16;
 	uint64_t stack = (uint64_t)kalloc(stack_size) + stack_size - 8;
 	tss.rsp0_low = (uint32_t)stack;
 	tss.rsp0_high = (uint32_t)(stack >> 32);
 
+	uint32_t ist_stack_size = 1024 * 32;
+	uint64_t ist_stack =
+		(uint64_t)kalloc(ist_stack_size) + ist_stack_size - 8;
+	tss.ist1_low = (uint32_t)ist_stack;
+	tss.ist1_high = (uint32_t)(ist_stack >> 32);
+
 	load_tss();
 }
diff --git a/kernel/src/devices/timer.c b/kernel/src/devices/timer.c
index 7971b20..41bcced 100644
--- a/kernel/src/devices/timer.c
+++ b/kernel/src/devices/timer.c
@@ -5,6 +5,7 @@
 #include <io.h>
 #include <idt.h>
 #include <process.h>
+#include <scheduler.h>
 
 uint32_t scheduler_ticks = 0;
 uint32_t seconds_tick = 0;
@@ -33,7 +34,9 @@ void timer_handler(uint64_t rsp)
 	scheduler_ticks++;
 	if (scheduler_ticks >= TICKS_PER_SECOND / CONTEXT_SWITCHES_PER_SECOND) {
 		scheduler_ticks = 0;
-/*		context_switch(rsp); */
+		if (sched_init) {
+			context_switch(rsp);
+		}
 	}
 }
 
diff --git a/kernel/src/main.c b/kernel/src/main.c
index 126c69e..d5e2492 100644
--- a/kernel/src/main.c
+++ b/kernel/src/main.c
@@ -23,6 +23,8 @@
 #include <ioapic.h>
 #include <atomic.h>
 #include <pmm.h>
+#include <scheduler.h>
+#include <process.h>
 
 int kernel_main(mb2_tag_header *multiboot_bootinfo, uint32_t multiboot_magic);
 int kernel_main(mb2_tag_header *multiboot_bootinfo, uint32_t multiboot_magic)
@@ -49,11 +51,14 @@ int kernel_main(mb2_tag_header *multiboot_bootinfo, uint32_t multiboot_magic)
 	parse_madt();
 	apic_remap_interrupts();
 	enable_interrupts();
-	init_ap_cpus();
-	jump_userspace();
+
+	init_scheduler();
+
+	/* init_ap_cpus(); */
+	/* jump_userspace(); */
 
 	for (;;) {
-		__asm__ __volatile__("hlt;");
+		__asm__ __volatile__("pause; hlt;");
 	}
 	return 0;
 }
diff --git a/kernel/src/scheduler/process.c b/kernel/src/scheduler/process.c
index e85cc11..e7ef7e5 100644
--- a/kernel/src/scheduler/process.c
+++ b/kernel/src/scheduler/process.c
@@ -3,24 +3,27 @@
 #include <heap.h>
 #include <scheduler.h>
 #include <libk/list.h>
+#include <libk/stdio.h>
 #include <process.h>
+#include <timer.h>
+#include <panic.h>
 
 process_t process_queue;
 process_t *curr_process;
 
-void init_process(uint64_t priv_lvl, uint64_t rip, uint64_t rdi, uint64_t rsi,
-		  uint64_t rdx)
+process_t *init_process(uint64_t priv_lvl, uint64_t rip, uint64_t argc,
+			uint64_t *argv)
 {
 	uint32_t stack_size = 32 * 1024;
-	uint64_t rsp = (uint64_t)kalloc(stack_size) + stack_size;
-	regs_t *regs = (regs_t *)(rsp + sizeof(regs_t));
+	uint64_t rsp = (uint64_t)kalloc(stack_size) + stack_size - 8;
+	regs_t *regs = (regs_t *)(rsp - sizeof(regs_t));
 
 	regs->rax = 0;
 	regs->rbx = 0;
 	regs->rcx = 0;
-	regs->rdx = rdx;
-	regs->rdi = rdi;
-	regs->rsi = rsi;
+	regs->rdx = (uint64_t)argv;
+	regs->rdi = rip;
+	regs->rsi = argc;
 	regs->rsp = rsp;
 	regs->rbp = 0;
 	regs->r8 = 0;
@@ -31,7 +34,7 @@ void init_process(uint64_t priv_lvl, uint64_t rip, uint64_t rdi, uint64_t rsi,
 	regs->r13 = 0;
 	regs->r14 = 0;
 	regs->r15 = 0;
-	regs->rip = rip;
+	regs->rip = (uint64_t)process_init_wrapper;
 	regs->rflags = 0;
 	regs->error = 0;
 
@@ -46,15 +49,84 @@ void init_process(uint64_t priv_lvl, uint64_t rip, uint64_t rdi, uint64_t rsi,
 	}
 
 	process_t *new_process = (process_t *)kalloc(sizeof(process_t));
+	new_process->rsp = rsp - sizeof(regs_t);
 	add_to_list(&new_process->list, &process_queue.list,
 		    process_queue.list.next);
+	return new_process;
 }
 
-__attribute__((noreturn)) void context_switch(uint64_t irq_rsp)
+void process_init_wrapper(uint64_t rip, uint64_t argc, uint64_t *argv)
+{
+	void (*f)(uint64_t rdi, ...) = (void (*)(uint64_t rdi, ...))rip;
+	switch (argc) {
+	case 0:
+		(*f)(argv[0]);
+		break;
+	case 1:
+		(*f)(argv[0]);
+		break;
+	case 2:
+		(*f)(argv[0], argv[1]);
+		break;
+	case 3:
+		(*f)(argv[0], argv[1], argv[2]);
+		break;
+	case 4:
+		(*f)(argv[0], argv[1], argv[2], argv[3]);
+		break;
+	case 5:
+		(*f)(argv[0], argv[1], argv[2], argv[3], argv[4]);
+		break;
+	default:
+		(*f)(argv[0], argv[1], argv[2], argv[3], argv[4], argv[5]);
+		break;
+	}
+	kfree(argv);
+	remove_current_process();
+}
+
+__attribute__((noreturn)) void idle_thread()
+{
+	uint64_t x = 0;
+	__asm__ __volatile__("mov %%rsp, %0;" : "=r"(x) : :);
+	printf("current rsp: 0x%x\n", x);
+	printf("idle_thread()\n");
+	for (;;) {
+		printf("1");
+		wait(2000);
+		__asm__ __volatile__("pause; hlt;");
+	}
+}
+
+__attribute__((noreturn)) void idle_thread2()
 {
-	save_context_to_rsp(irq_rsp, curr_process->rsp);
+	printf("idle_thread2()\n");
+	for (;;) {
+		printf("2");
+		wait(1000);
+		__asm__ __volatile__("pause; hlt;");
+	}
+}
 
-	process_t *next_process = scheduler();
+__attribute__((noreturn)) void remove_current_process()
+{
+	free_node(&curr_process->list);
+	kfree(curr_process);
+	curr_process = scheduler();
+	if (curr_process == NULL) {
+		printf("no processes left\n");
+		for (;;) {
+			__asm__ __volatile__("pause; hlt;");
+		}
+	}
+	restore_context_from_rsp(curr_process->rsp);
+}
 
-	restore_context_from_rsp(next_process->rsp);
+__attribute__((noreturn)) void context_switch(uint64_t irq_rsp)
+{
+	printf("irq_rsp: 0x%x\n", irq_rsp);
+	print_regs_from_rsp(irq_rsp);
+	curr_process->rsp = save_context_from_rsp(irq_rsp);
+	curr_process = scheduler();
+	restore_context_from_rsp(curr_process->rsp);
 }
diff --git a/kernel/src/scheduler/scheduler.c b/kernel/src/scheduler/scheduler.c
index 263c11a..3cbc461 100644
--- a/kernel/src/scheduler/scheduler.c
+++ b/kernel/src/scheduler/scheduler.c
@@ -2,11 +2,38 @@
 #include <scheduler.h>
 #include <libk/list.h>
 #include <process.h>
+#include <heap.h>
+#include <libk/string.h>
+#include <libk/stdio.h>
+#include <panic.h>
 
-process_t *scheduler(void)
+mutex_t scheduler_lock;
+uint32_t sched_init = 0;
+
+__attribute__((noreturn)) void init_scheduler()
+{
+	INIT_LIST(process_queue.list);
+	init_mutex(&scheduler_lock);
+	uint64_t argc = 6;
+	uint64_t *argv = (uint64_t *)kalloc(sizeof(uint64_t) * 6);
+	memset(argv, 0, sizeof(uint64_t) * 6);
+	init_process(0, (uint64_t)idle_thread2, argc, argv);
+	curr_process = init_process(0, (uint64_t)idle_thread, argc, argv);
+	sched_init = 1;
+	restore_context_from_rsp(curr_process->rsp);
+}
+
+process_t *scheduler()
 {
+	lock(scheduler_lock);
 	process_t *pos = curr_process;
+	if (list_is_empty((&process_queue.list)))
+		return NULL;
 	process_t *next_process = list_next_entry(pos, list);
+	while (next_process->rsp == 0) {
+		next_process = list_next_entry(next_process, list);
+	}
+	unlock(scheduler_lock);
 
 	return next_process;
 }
diff --git a/kernel/src/scheduler/switch.S b/kernel/src/scheduler/switch.S
index 4d5dfb6..12593ec 100644
--- a/kernel/src/scheduler/switch.S
+++ b/kernel/src/scheduler/switch.S
@@ -1,10 +1,13 @@
 #include "x86_64_regs.S"
 
 /* %rdi - irq's rsp */
-/* %rsi - task's rsp */
-.global save_context_to_rsp
-save_context_to_rsp:
-	mov %rsi, %rsp
+.global save_context_from_rsp
+save_context_from_rsp:
+	push %rbp
+	mov %rsp, %rbp
+
+	mov RSP_OFF(%rdi), %rsp
+
 	push SS_OFF(%rdi)
 	push RSP_OFF(%rdi)
 	push RFLAGS_OFF(%rdi)
@@ -28,6 +31,12 @@ save_context_to_rsp:
 	push R14_OFF(%rdi)
 	push R15_OFF(%rdi)
 
+	mov %rsp, %rax
+
+	mov %rbp, %rsp
+	pop %rbp
+	ret
+
 /* %rdi - task's rsp */
 .global restore_context_from_rsp
 restore_context_from_rsp: