Enhance experimental option rom "threading" - enable preemption.

When experimental support for parallelizing option roms and hardware
   init (default disabled) is selected, add support for checking on
   hardware init progress from the RTC irq handler.
Enable ability for RTC to be turned on for additional users.
Allow regular option roms (not just vga option roms) to run in
   parallel with hardware init.
Don't use stack in transition32 / transition16 until new mode is
   entered.
Also, cleanup leaking of data handlers in usb code.
Also, decrease frequency of iomemcpy checks (every 2K instead of 1K).

1 files changed, 137 insertions, 13 deletions

diff --git a/src/stacks.c b/src/stacks.c
index d71381f..bd1c75f 100644
--- 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);
+}