3 files changed, 125 insertions, 17 deletions

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: