Interruptible nanosleep()

include/sys/amd64/hw/timer.h

@@ -4,4 +4,5 @@
 struct thread;
 
 void timer_add_sleep(struct thread *thr);
+void timer_remove_sleep(struct thread *thr);
 void amd64_timer_init(void);

include/sys/thread.h

@@ -51,6 +51,8 @@ void thread_ioctx_fork(struct thread *dst, struct thread *src);
 int thread_init(struct thread *thr, uintptr_t entry, void *arg, int user);
 
 struct thread *thread_find(pid_t pid);
-
-void thread_sleep(struct thread *thr, uint64_t deadline);
+void thread_signal(struct thread *thr, int signum);
+int thread_check_signal(struct thread *thr, int ret);
 void thread_sigenter(int signum);
+
+int thread_sleep(struct thread *thr, uint64_t deadline, uint64_t *int_time);

sys/amd64/hw/timer.c

@@ -29,6 +29,31 @@ void timer_add_sleep(struct thread *thr) {
     sleep_head = thr;
 }
 
+void timer_remove_sleep(struct thread *target) {
+    struct thread *thr = sleep_head, *prev = NULL;
+    while (thr) {
+        struct thread *next = thr->wait_next;
+        if (thr == target) {
+            thr->wait_next = NULL;
+            if (prev) {
+                prev->wait_next = next;
+            } else {
+                sleep_head = next;
+            }
+
+            sched_queue(thr);
+
+            thr = next;
+            return;
+        }
+
+        prev = thr;
+        thr = next;
+    }
+
+    panic("No such thread\n");
+}
+
 static uint32_t timer_tick(void *arg) {
     static uint64_t last_debug_cycle = 0;
 

sys/sched.c

@@ -155,6 +155,20 @@ void sched_debug_cycle(uint64_t delta_ms) {
 }
 
 void yield(void) {
+    static uint64_t last_time = 0;
+    uint64_t delta = system_time - last_time;
+
+    if (delta >= 3000000000ULL) {
+        if (system_time) {
+            struct thread *t = thread_find(1);
+            if (t->state != THREAD_STOPPED) {
+                _assert(t);
+                thread_signal(t, 2);
+            }
+        }
+        last_time = system_time;
+    }
+
     struct thread *from = get_cpu()->thread;
     struct thread *to;
 
@@ -177,24 +191,7 @@ void yield(void) {
 
     // Check if instead of switching to a proper thread context we
     // have to use signal handling
-    if (to->sigq) {
-        // Pick one signal to handle at a time
-        int signum = 0;
-        for (int i = 0; i < 64; ++i) {
-            if (to->sigq & (1ULL << i)) {
-                to->sigq &= ~(1ULL << i);
-                signum = i + 1;
-                break;
-            }
-        }
-        _assert(signum);
-        thread_sigenter(signum);
-
-        // Theoretically, a rogue thread could steal all the CPU time by sending itself signals
-        // in normal context, as after returning from thread_sigenter() this code will return
-        // to a normal execution
-        // XXX: Maybe makes sense to just yield() here
-    }
+    thread_check_signal(from, 0);
 }
 
 void sched_init(void) {

sys/sys_sys.c

@@ -8,6 +8,7 @@
 #include "sys/assert.h"
 #include "sys/thread.h"
 #include "sys/fs/vfs.h"
+#include "sys/debug.h"
 
 int sys_mount(const char *dev_name, const char *dir_name, const char *type, unsigned long flags, void *data) {
     struct thread *thr = get_cpu()->thread;
@@ -50,11 +51,21 @@ int sys_umount(const char *dir_name) {
 int sys_nanosleep(const struct timespec *req, struct timespec *rem) {
     struct thread *thr = get_cpu()->thread;
     _assert(thr);
-    uint64_t delay = req->tv_sec * 1000000000ULL + req->tv_nsec;
-
-    thread_sleep(thr, system_time + delay);
-
-    return 0;
+    uint64_t deadline = req->tv_sec * 1000000000ULL + req->tv_nsec + system_time;
+    uint64_t int_time;
+    int ret = thread_sleep(thr, deadline, &int_time);
+    if (rem) {
+        if (ret) {
+            _assert(deadline > int_time);
+            uint64_t rem_time = deadline - int_time;
+            rem->tv_sec = rem_time / 1000000000ULL;
+            rem->tv_nsec = rem_time % 1000000000ULL;
+        } else {
+            rem->tv_sec = 0;
+            rem->tv_nsec = 0;
+        }
+    }
+    return ret;
 }
 
 int sys_gettimeofday(struct timeval *tv, struct timezone *tz) {

sys/thread.c

@@ -205,6 +205,7 @@ int thread_init(struct thread *thr, uintptr_t entry, void *arg, int user) {
 
     thr->data.rsp0 = (uintptr_t) stack;
     thr->sigq = 0;
+    thr->pid = -1;
 
     thread_add(thr);
 
@@ -383,16 +384,60 @@ __attribute__((noreturn)) void sys_exit(int status) {
     panic("This code shouldn't run\n");
 }
 
-void thread_sleep(struct thread *thr, uint64_t deadline) {
+int thread_sleep(struct thread *thr, uint64_t deadline, uint64_t *int_time) {
     thr->sleep_deadline = deadline;
     timer_add_sleep(thr);
     sched_unqueue(thr, THREAD_WAITING);
+    // Store time when interrupt occured
+    if (int_time) {
+        *int_time = system_time;
+    }
+    return thread_check_signal(thr, 0);
 }
 
 void sys_sigreturn(void) {
     context_sigreturn();
 }
 
+void thread_signal(struct thread *thr, int signum) {
+    if (thr == thread_self) {
+        thread_sigenter(signum);
+    } else {
+        thr->sigq |= 1ULL << (signum - 1);
+
+        if (thr->state == THREAD_WAITING) {
+            timer_remove_sleep(thr);
+        }
+
+        sched_queue(thr);
+    }
+}
+
+int thread_check_signal(struct thread *thr, int ret) {
+    if (thr->sigq) {
+        // Pick one signal to handle at a time
+        int signum = 0;
+        for (int i = 0; i < 64; ++i) {
+            if (thr->sigq & (1ULL << i)) {
+                thr->sigq &= ~(1ULL << i);
+                signum = i + 1;
+                break;
+            }
+        }
+        kdebug("%d: Handle signal %d\n", thr->pid, signum);
+        _assert(signum);
+        thread_sigenter(signum);
+
+        // Theoretically, a rogue thread could steal all the CPU time by sending itself signals
+        // in normal context, as after returning from thread_sigenter() this code will return
+        // to a normal execution
+        // XXX: Maybe makes sense to just yield() here
+        return -EINTR;
+    }
+
+    return ret;
+}
+
 int sys_kill(pid_t pid, int signum) {
     struct thread *thr;
 
@@ -418,11 +463,7 @@ int sys_kill(pid_t pid, int signum) {
         return -ESRCH;
     }
 
-    if (thr == thread_self) {
-        thread_sigenter(signum);
-    } else {
-        thr->sigq |= 1ULL << (1 - signum);
-    }
+    thread_signal(thr, signum);
 
     return 0;
 }