[PATCH] Some improvements to optionrom preemption support.

[Kevin O'Connor]

Enable preemption during VGA mode switch call - this call can take
several milliseconds on real hardware.

Call yield() in finish_preempt() - when preemption is configured it
allows threads in wait_preempt() to run; when not configured it gives
an opportunity for threads to execute after the implicit delay from
optionrom execution.

Don't penalize priority in run_thread().  The run_thread() code would
implicitly yield because it created the new thread on the list after
the current thread and then jumped to it.  When in a preemption event,
a yield effectively waits approximately one millisecond (to the next
rtc irq).  The implicit yielding in run_thread thus limited the number
of threads one could launch during preemption to 1 per millisecond.
So, change the code so that the new thread is created prior to the
current thread - thus eliminating the effective yield from
run_thread().
---
 src/optionroms.c |    2 ++
 src/stacks.c     |   21 ++++++++++++++-------
 2 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/src/optionroms.c b/src/optionroms.c
index 47f5808..ad88e0c 100644
--- a/src/optionroms.c
+++ b/src/optionroms.c
@@ -470,7 +470,9 @@ vga_setup(void)
     memset(&br, 0, sizeof(br));
     br.flags = F_IF;
     br.ax = 0x0003;
+    start_preempt();
     call16_int(0x10, &br);
+    finish_preempt();

// Write to screen.
     printf("Starting SeaBIOS (version %s)\n\n", VERSION);
diff --git a/src/stacks.c b/src/stacks.c
index 570948a..92d91a0 100644
--- a/src/stacks.c
+++ b/src/stacks.c
@@ -116,9 +116,12 @@ stack_hop(u32 eax, u32 edx, void *func)

#define THREADSTACKSIZE 4096

+// Thread info - stored at bottom of each thread stack - don't change
+// without also updating the inline assembler below.
 struct thread_info {
     struct thread_info *next;
     void *stackpos;
+    struct thread_info **pprev;
 };

struct thread_info VAR16VISIBLE MainThread;
@@ -128,6 +131,7 @@ void
 thread_setup(void)
 {
     MainThread.next = &MainThread;
+    MainThread.pprev = &MainThread.next;
     MainThread.stackpos = NULL;
     CanPreempt = 0;
 }
@@ -185,10 +189,8 @@ yield(void)
 static void
 __end_thread(struct thread_info *old)
 {
-    struct thread_info *pos = &MainThread;
-    while (pos->next != old)
-        pos = pos->next;
-    pos->next = old->next;
+    old->next->pprev = old->pprev;
+    *old->pprev = old->next;
     free(old);
     dprintf(DEBUG_thread, "\%08x/ End thread\n", (u32)old);
 }
@@ -207,8 +209,10 @@ run_thread(void (*func)(void*), void *data)

thread->stackpos = (void*)thread + THREADSTACKSIZE;
     struct thread_info *cur = getCurThread();
-    thread->next = cur->next;
-    cur->next = thread;
+    thread->next = cur;
+    thread->pprev = cur->pprev;
+    cur->pprev = &thread->next;
+    *thread->pprev = thread;

dprintf(DEBUG_thread, "/%08x\ Start thread\n", (u32)thread);
     asm volatile(
@@ -289,11 +293,14 @@ start_preempt(void)
 void
 finish_preempt(void)
 {
-    if (! CONFIG_THREADS || ! CONFIG_THREAD_OPTIONROMS)
+    if (! CONFIG_THREADS || ! CONFIG_THREAD_OPTIONROMS) {
+        yield();
         return;
+    }
     CanPreempt = 0;
     releaseRTC();
     dprintf(1, "Done preempt - %d checks\n", PreemptCount);
+    yield();
 }

// Check if preemption is on, and wait for it to complete if so.