Enhance experimental option rom "threading" - enable preemption.

[seabios.git] / src / stacks.c

diff --git a/src/stacks.c b/src/stacks.c
index d71381fa134a1735a8d947b6a09a168983d4d7f9..bd1c75f19da73cabaf8cb60b23606b7bb4b02513 100644 (file)
--- a/src/stacks.c
+++ b/src/stacks.c
@@ -6,6 +6,7 @@
 
 #include "biosvar.h" // get_ebda_seg
 #include "util.h" // dprintf
+#include "bregs.h" // CR0_PE
 
 
 /****************************************************************
@@ -51,15 +52,18 @@ struct thread_info {
     void *stackpos;
 };
 
-struct thread_info MainThread;
+struct thread_info VAR16VISIBLE MainThread;
+int VAR16VISIBLE CanPreempt;
 
 void
 thread_setup()
 {
     MainThread.next = &MainThread;
     MainThread.stackpos = NULL;
+    CanPreempt = 0;
 }
 
+// Return the 'struct thread_info' for the currently running thread.
 struct thread_info *
 getCurThread()
 {
@@ -69,6 +73,24 @@ getCurThread()
     return (void*)ALIGN_DOWN(esp, THREADSTACKSIZE);
 }
 
+// Switch to next thread stack.
+static void
+switch_next(struct thread_info *cur)
+{
+    struct thread_info *next = cur->next;
+    asm volatile(
+        "  pushl $1f\n"                 // store return pc
+        "  pushl %%ebp\n"               // backup %ebp
+        "  movl %%esp, 4(%%eax)\n"      // cur->stackpos = %esp
+        "  movl 4(%%ecx), %%esp\n"      // %esp = next->stackpos
+        "  popl %%ebp\n"                // restore %ebp
+        "  retl\n"                      // restore pc
+        "1:\n"
+        : "+a"(cur), "+c"(next)
+        :
+        : "ebx", "edx", "esi", "edi", "cc", "memory");
+}
+
 // Briefly permit irqs to occur.
 void
 yield()
@@ -84,18 +106,7 @@ yield()
         check_irqs();
 
     // Switch to the next thread
-    struct thread_info *next = cur->next;
-    asm volatile(
-        "  pushl $1f\n"                 // store return pc
-        "  pushl %%ebp\n"               // backup %ebp
-        "  movl %%esp, 4(%%eax)\n"      // cur->stackpos = %esp
-        "  movl 4(%%ecx), %%esp\n"      // %esp = next->stackpos
-        "  popl %%ebp\n"                // restore %ebp
-        "  retl\n"                      // restore pc
-        "1:\n"
-        : "+a"(cur), "+c"(next)
-        :
-        : "ebx", "edx", "esi", "edi", "cc", "memory");
+    switch_next(cur);
 }
 
 // Last thing called from a thread (called on "next" stack).
@@ -110,6 +121,7 @@ __end_thread(struct thread_info *old)
     dprintf(DEBUG_thread, "\\%08x/ End thread\n", (u32)old);
 }
 
+// Create a new thread and start executing 'func' in it.
 void
 run_thread(void (*func)(void*), void *data)
 {
@@ -152,6 +164,7 @@ fail:
     func(data);
 }
 
+// Wait for all threads (other than the main thread) to complete.
 void
 wait_threads()
 {
@@ -161,3 +174,114 @@ wait_threads()
     while (MainThread.next != &MainThread)
         yield();
 }
+
+
+/****************************************************************
+ * Thread preemption
+ ****************************************************************/
+
+static u32 PreemptCount;
+
+// Turn on RTC irqs and arrange for them to check the 32bit threads.
+void
+start_preempt()
+{
+    if (! CONFIG_THREADS || ! CONFIG_THREAD_OPTIONROMS)
+        return;
+    CanPreempt = 1;
+    PreemptCount = 0;
+    useRTC();
+}
+
+// Turn off RTC irqs / stop checking for thread execution.
+void
+finish_preempt()
+{
+    if (! CONFIG_THREADS || ! CONFIG_THREAD_OPTIONROMS)
+        return;
+    CanPreempt = 0;
+    releaseRTC();
+    dprintf(1, "Done preempt - %d checks\n", PreemptCount);
+}
+
+static inline u32 getcr0() {
+    u32 cr0;
+    asm("movl %%cr0, %0" : "=r"(cr0));
+    return cr0;
+}
+static inline void sgdt(struct descloc_s *desc) {
+    asm("sgdtl %0" : "=m"(*desc));
+}
+static inline void lgdt(struct descloc_s *desc) {
+    asm("lgdtl %0" : : "m"(*desc) : "memory");
+}
+
+#if !MODE16
+// Try to execute 32bit threads.
+void VISIBLE32
+yield_preempt()
+{
+    PreemptCount++;
+    switch_next(&MainThread);
+}
+#endif
+
+// 16bit code that checks if threads are pending and executes them if so.
+void
+check_preempt()
+{
+    ASSERT16();
+    if (! CONFIG_THREADS || ! CONFIG_THREAD_OPTIONROMS
+        || !GET_GLOBAL(CanPreempt)
+        || GET_GLOBAL(MainThread.next) == &MainThread)
+        return;
+    u32 cr0 = getcr0();
+    if (cr0 & CR0_PE)
+        // Called in 16bit protected mode?!
+        return;
+
+    // Backup cmos index register and disable nmi
+    u8 cmosindex = inb(PORT_CMOS_INDEX);
+    outb(cmosindex | NMI_DISABLE_BIT, PORT_CMOS_INDEX);
+    inb(PORT_CMOS_DATA);
+
+    // Backup fs/gs and gdt
+    u16 fs = GET_SEG(FS), gs = GET_SEG(GS);
+    struct descloc_s gdt;
+    sgdt(&gdt);
+
+    u32 bkup_ss, bkup_esp;
+    asm volatile(
+        // Backup ss/esp / set esp to flat stack location
+        "  movl %%ss, %0\n"
+        "  movl %%esp, %1\n"
+        "  shll $4, %0\n"
+        "  addl %0, %%esp\n"
+        "  movl %%ss, %0\n"
+
+        // Transition to 32bit mode, call yield_preempt, return to 16bit
+        "  pushl $(" __stringify(BUILD_BIOS_ADDR) " + 1f)\n"
+        "  jmp transition32\n"
+        "  .code32\n"
+        "1:calll (yield_preempt - " __stringify(BUILD_BIOS_ADDR) ")\n"
+        "  pushl $2f\n"
+        "  jmp transition16big\n"
+        "  .code16gcc\n"
+        "2:\n"
+
+        // Restore ds/ss/esp
+        "  movl %0, %%ds\n"
+        "  movl %0, %%ss\n"
+        "  movl %1, %%esp\n"
+        : "=&r" (bkup_ss), "=&r" (bkup_esp)
+        : : "eax", "ecx", "edx", "cc", "memory");
+
+    // Restore gdt and fs/gs
+    lgdt(&gdt);
+    SET_SEG(FS, fs);
+    SET_SEG(GS, gs);
+
+    // Restore cmos index register
+    outb(cmosindex, PORT_CMOS_INDEX);
+    inb(PORT_CMOS_DATA);
+}