Add simple cooperative threading scheme to allow parallel hw init.

Enable system for running hardware initialization in parallel.
The yield() call can now round-robin between "threads".
Rework ata controller init to use a thread per controller.
Make sure internal drives are registered in a defined order.
Run keyboard initialization in a thread.
Rework usb init to use a thread per controller.

diff --git a/src/ata.c b/src/ata.c
index 2401557ff39a0658b0b8f830574e246242adbca1..e30f1860a5b19274750163b43180d6cf2bbe0e02 100644 (file)
--- a/src/ata.c
+++ b/src/ata.c
@@ -636,8 +636,10 @@ init_drive_ata(struct drive_s *dummy, u16 *buffer)
     return drive_g;
 }
 
+static u64 SpinupEnd;
+
 static int
-powerup_await_non_bsy(u16 base, u64 end)
+powerup_await_non_bsy(u16 base)
 {
     u8 orstatus = 0;
     u8 status;
@@ -650,7 +652,7 @@ powerup_await_non_bsy(u16 base, u64 end)
             dprintf(1, "powerup IDE floating\n");
             return orstatus;
         }
-        if (check_time(end)) {
+        if (check_time(SpinupEnd)) {
             dprintf(1, "powerup IDE time out\n");
             return -1;
         }
@@ -661,14 +663,15 @@ powerup_await_non_bsy(u16 base, u64 end)
 }
 
 static void
-ata_detect()
+ata_detect(void *data)
 {
+    struct ata_channel_s *atachannel = data;
+    int startid = (atachannel - ATA_channels) * 2;
     struct drive_s dummy;
     memset(&dummy, 0, sizeof(dummy));
     // Device detection
-    u64 end = calc_future_tsc(IDE_TIMEOUT);
     int ataid, last_reset_ataid=-1;
-    for (ataid=0; ataid<CONFIG_MAX_ATA_INTERFACES*2; ataid++) {
+    for (ataid=startid; ataid<startid+2; ataid++) {
         u8 channel = ataid / 2;
         u8 slave = ataid % 2;
 
@@ -677,13 +680,13 @@ ata_detect()
             break;
 
         // Wait for not-bsy.
-        int status = powerup_await_non_bsy(iobase1, end);
+        int status = powerup_await_non_bsy(iobase1);
         if (status < 0)
             continue;
         u8 newdh = slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0;
         outb(newdh, iobase1+ATA_CB_DH);
         ndelay(400);
-        status = powerup_await_non_bsy(iobase1, end);
+        status = powerup_await_non_bsy(iobase1);
         if (status < 0)
             continue;
 
@@ -732,10 +735,6 @@ ata_detect()
                 continue;
         }
 
-        // Report drive info to user.
-        describe_drive(drive_g);
-        printf("\n");
-
         u16 resetresult = buffer[93];
         dprintf(6, "ata_detect resetresult=%04x\n", resetresult);
         if (!slave && (resetresult & 0xdf61) == 0x4041)
@@ -744,8 +743,6 @@ ata_detect()
             // detection.
             ataid++;
     }
-
-    printf("\n");
 }
 
 static void
@@ -778,6 +775,7 @@ ata_init()
         SET_GLOBAL(ATA_channels[count].iobase2, port2);
         dprintf(1, "ATA controller %d at %x/%x (dev %x prog_if %x)\n"
                 , count, port1, port2, bdf, prog_if);
+        run_thread(ata_detect, &ATA_channels[count]);
         count++;
 
         if (prog_if & 4) {
@@ -791,6 +789,7 @@ ata_init()
                 , count, port1, port2, bdf, prog_if);
         SET_GLOBAL(ATA_channels[count].iobase1, port1);
         SET_GLOBAL(ATA_channels[count].iobase2, port2);
+        run_thread(ata_detect, &ATA_channels[count]);
         count++;
     }
 }
@@ -802,8 +801,9 @@ ata_setup()
         return;
 
     dprintf(3, "init hard drives\n");
+
+    SpinupEnd = calc_future_tsc(IDE_TIMEOUT);
     ata_init();
-    ata_detect();
 
     SET_BDA(disk_control_byte, 0xc0);
 

diff --git a/src/boot.c b/src/boot.c
index 668ddb02735f1c762a525477720c3366e9f81768..d80eae491c9c5ea16d9644d6d11d8d2f6ca1c052 100644 (file)
--- a/src/boot.c
+++ b/src/boot.c
@@ -115,6 +115,23 @@ add_bcv_internal(struct drive_s *drive_g)
         return;
 
     struct ipl_entry_s *ie = &IPL.bcv[IPL.bcvcount++];
+    if (CONFIG_THREADS) {
+        // Add to bcv list with assured drive order.
+        struct ipl_entry_s *end = ie;
+        for (;;) {
+            struct ipl_entry_s *prev = ie - 1;
+            if (prev < IPL.bcv || prev->type != BCV_TYPE_INTERNAL)
+                break;
+            struct drive_s *prevdrive = (void*)prev->vector;
+            if (prevdrive->type < drive_g->type
+                || (prevdrive->type == drive_g->type
+                    && prevdrive->cntl_id < drive_g->cntl_id))
+                break;
+            ie--;
+        }
+        if (ie != end)
+            memmove(ie+1, ie, (void*)end-(void*)ie);
+    }
     ie->type = BCV_TYPE_INTERNAL;
     ie->vector = (u32)drive_g;
     ie->description = "";
@@ -297,6 +314,8 @@ boot_prep()
     if (! CONFIG_BOOT)
         return;
 
+    // XXX - show available drives?
+
     // Allow user to modify BCV/IPL order.
     interactive_bootmenu();
 

diff --git a/src/config.h b/src/config.h
index 7b472ad5e610cbb6a2754c706f1f0262e7667e63..ab243deb3cb1ac630714845986651f1c74853b1c 100644 (file)
--- a/src/config.h
+++ b/src/config.h
@@ -22,6 +22,8 @@
 // Screen writes are also sent to debug ports.
 #define CONFIG_SCREEN_AND_DEBUG 1
 
+// Support running hardware initialization in parallel
+#define CONFIG_THREADS 1
 // Support int13 disk/floppy drive functions
 #define CONFIG_DRIVES 1
 // Support floppy drive access

diff --git a/src/kbd.c b/src/kbd.c
index 29eb29a60e4efe8ca1827166024b68ec766edb6a..461d1b77dacb40d354f90ce66aefa08ee47cb06e 100644 (file)
--- a/src/kbd.c
+++ b/src/kbd.c
@@ -118,7 +118,7 @@ kbd_setup()
     if (! CONFIG_KEYBOARD)
         return;
 
-    keyboard_init();
+    run_thread(keyboard_init, NULL);
 
     enable_hwirq(1, entry_09);
 }

diff --git a/src/post.c b/src/post.c
index a566b34bfdb847e5b0d000a7595b899eb841818c..3024eb8a994186df66aff400f9da730570001ec6 100644 (file)
--- a/src/post.c
+++ b/src/post.c
@@ -198,6 +198,7 @@ post()
     ata_setup();
     ramdisk_setup();
 
+    wait_threads();
     optionrom_setup();
 
     // Run BCVs

diff --git a/src/usb-hid.c b/src/usb-hid.c
index c849aa151c23288d1b3be3818b4132ea80732ea3..8cb501ad6e5a1408a1ecdbe727fd10bde12aaf83 100644 (file)
--- a/src/usb-hid.c
+++ b/src/usb-hid.c
@@ -47,6 +47,7 @@ usb_keyboard_init(u32 endp, struct usb_interface_descriptor *iface, int imax)
     if (! CONFIG_USB_KEYBOARD)
         return -1;
     if (keyboard_pipe)
+        // XXX - this enables the first found keyboard (could be random)
         return -1;
     dprintf(2, "usb_keyboard_setup %x\n", endp);
 

diff --git a/src/usb-ohci.c b/src/usb-ohci.c
index 9e07c8953cceb48aae9263724105a4ed3998c33e..15841dc208c4cd2a6b84684f8e9ada8df38d007a 100644 (file)
--- a/src/usb-ohci.c
+++ b/src/usb-ohci.c
@@ -121,11 +121,12 @@ shutdown:
     return 0;
 }
 
-int
-ohci_init(struct usb_s *cntl)
+void
+ohci_init(void *data)
 {
     if (! CONFIG_USB_OHCI)
-        return 0;
+        return;
+    struct usb_s *cntl = data;
 
     cntl->type = USB_TYPE_OHCI;
     u32 baseaddr = pci_config_readl(cntl->bdf, PCI_BASE_ADDRESS_0);
@@ -150,7 +151,7 @@ ohci_init(struct usb_s *cntl)
     struct ohci_ed *control_ed = malloc_high(sizeof(*control_ed));
     if (!hcca || !control_ed) {
         dprintf(1, "No ram for ohci init\n");
-        return 0;
+        return;
     }
     memset(hcca, 0, sizeof(*hcca));
     memset(control_ed, 0, sizeof(*control_ed));
@@ -164,13 +165,12 @@ ohci_init(struct usb_s *cntl)
     int count = check_ohci_ports(cntl);
     if (! count)
         goto err;
-    return count;
+    return;
 
 err:
     stop_ohci(cntl);
     free(hcca);
     free(control_ed);
-    return 0;
 }
 
 static int

diff --git a/src/usb-ohci.h b/src/usb-ohci.h
index e5d21270ca3b5f213e2bc4d2477490888d28b646..5a4f735a3a646e46994452181e5143ddfb27cd21 100644 (file)
--- a/src/usb-ohci.h
+++ b/src/usb-ohci.h
@@ -3,7 +3,7 @@
 
 // usb-ohci.c
 struct usb_s;
-int ohci_init(struct usb_s *cntl);
+void ohci_init(void *data);
 int ohci_control(u32 endp, int dir, const void *cmd, int cmdsize
                  , void *data, int datasize);
 struct usb_pipe *ohci_alloc_intr_pipe(u32 endp, int period);

diff --git a/src/usb-uhci.c b/src/usb-uhci.c
index 8045375aee48689ba35ce3a8b70872a89351bb89..f9fa7260cfcaef88d463d66029799a36f8b27ca8 100644 (file)
--- a/src/usb-uhci.c
+++ b/src/usb-uhci.c
@@ -123,11 +123,12 @@ check_ports(struct usb_s *cntl)
     return totalcount;
 }
 
-int
-uhci_init(struct usb_s *cntl)
+void
+uhci_init(void *data)
 {
     if (! CONFIG_USB_UHCI)
-        return 0;
+        return;
+    struct usb_s *cntl = data;
 
     cntl->type = USB_TYPE_UHCI;
     cntl->uhci.iobase = (pci_config_readl(cntl->bdf, PCI_BASE_ADDRESS_4)
@@ -146,10 +147,7 @@ uhci_init(struct usb_s *cntl)
     int count = check_ports(cntl);
     if (! count) {
         // XXX - no devices; free data structures.
-        return 0;
     }
-
-    return count;
 }
 
 static int

diff --git a/src/usb-uhci.h b/src/usb-uhci.h
index b284fcc98738b480d413e602c50329dcd821d901..5de7da0d262586a0d22527cdf88d0502364a3e0a 100644 (file)
--- a/src/usb-uhci.h
+++ b/src/usb-uhci.h
@@ -5,7 +5,7 @@
 
 // usb-uhci.c
 struct usb_s;
-int uhci_init(struct usb_s *cntl);
+void uhci_init(void *data);
 int uhci_control(u32 endp, int dir, const void *cmd, int cmdsize
                  , void *data, int datasize);
 struct usb_pipe *uhci_alloc_intr_pipe(u32 endp, int period);

diff --git a/src/usb.c b/src/usb.c
index cb75e785fd2bc4d05ccab65003bd384b55375aef..fcc404bebde77a7336869dd99027e5f55fbca48a 100644 (file)
--- a/src/usb.c
+++ b/src/usb.c
@@ -216,17 +216,15 @@ usb_setup()
         struct usb_s *cntl = &USBControllers[count];
         cntl->bdf = bdf;
 
-        int devcount = 0;
         if (code == PCI_CLASS_SERIAL_USB_UHCI)
-            devcount = uhci_init(cntl);
+            run_thread(uhci_init, cntl);
         else if (code == PCI_CLASS_SERIAL_USB_OHCI)
-            devcount = ohci_init(cntl);
-
-        if (devcount > 0) {
-            // Success
-            count++;
-            if (count >= ARRAY_SIZE(USBControllers))
-                break;
-        }
+            run_thread(ohci_init, cntl);
+        else
+            continue;
+
+        count++;
+        if (count >= ARRAY_SIZE(USBControllers))
+            break;
     }
 }

diff --git a/src/util.c b/src/util.c
index 408694844b852e2ae04cbb6c947541388394f669..21c7a62c577f2299c5a9536c7e3df808031a596f 100644 (file)
--- a/src/util.c
+++ b/src/util.c
@@ -9,6 +9,12 @@
 #include "farptr.h" // GET_FLATPTR
 #include "biosvar.h" // get_ebda_seg
 
+static inline u32 getesp() {
+    u32 esp;
+    asm("movl %%esp, %0" : "=rm"(esp));
+    return esp;
+}
+
 
 /****************************************************************
  * 16bit calls
@@ -19,6 +25,8 @@
 inline void
 call16(struct bregs *callregs)
 {
+    if (!MODE16 && getesp() > BUILD_STACK_ADDR)
+        panic("call16 with invalid stack\n");
     asm volatile(
 #if MODE16 == 1
         "calll __call16\n"
@@ -36,6 +44,8 @@ inline void
 call16big(struct bregs *callregs)
 {
     ASSERT32();
+    if (getesp() > BUILD_STACK_ADDR)
+        panic("call16 with invalid stack\n");
     asm volatile(
         "calll __call16big_from32"
         : "+a" (callregs), "+m" (*callregs)
@@ -83,32 +93,153 @@ stack_hop(u32 eax, u32 edx, u32 ecx, void *func)
 
 // 16bit trampoline for enabling irqs from 32bit mode.
 ASM16(
-    "  .global trampoline_yield\n"
-    "trampoline_yield:\n"
+    "  .global trampoline_checkirqs\n"
+    "trampoline_checkirqs:\n"
     "  rep ; nop\n"
     "  lretw"
     );
 
+static void
+check_irqs32()
+{
+    extern void trampoline_checkirqs();
+    struct bregs br;
+    br.flags = F_IF;
+    br.code.seg = SEG_BIOS;
+    br.code.offset = (u32)&trampoline_checkirqs;
+    call16big(&br);
+}
+
+static void
+check_irqs16()
+{
+    asm volatile(
+        "sti\n"
+        "nop\n"
+        "rep ; nop\n"
+        "cli\n"
+        "cld\n"
+        : : :"memory");
+}
+
+
+/****************************************************************
+ * Threads
+ ****************************************************************/
+
+#define THREADSTACKSIZE 4096
+
+struct thread_info {
+    struct thread_info *next;
+    void *stackpos;
+};
+
+static struct thread_info MainThread = {&MainThread, NULL};
+
+static struct thread_info *
+getCurThread()
+{
+    u32 esp = getesp();
+    if (esp <= BUILD_STACK_ADDR)
+        return &MainThread;
+    return (void*)ALIGN_DOWN(esp, THREADSTACKSIZE);
+}
+
 // Briefly permit irqs to occur.
 void
 yield()
 {
     if (MODE16) {
-        asm volatile(
-            "sti\n"
-            "nop\n"
-            "rep ; nop\n"
-            "cli\n"
-            "cld\n"
-            : : :"memory");
+        // In 16bit mode, just directly check irqs.
+        check_irqs16();
         return;
     }
-    extern void trampoline_yield();
-    struct bregs br;
-    br.flags = F_IF;
-    br.code.seg = SEG_BIOS;
-    br.code.offset = (u32)&trampoline_yield;
-    call16big(&br);
+    if (! CONFIG_THREADS) {
+        check_irqs32();
+        return;
+    }
+    struct thread_info *cur = getCurThread();
+    if (cur == &MainThread)
+        // Permit irqs to fire
+        check_irqs32();
+
+    // 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");
+}
+
+// Last thing called from a thread (called on "next" stack).
+static void
+__end_thread(struct thread_info *old)
+{
+    struct thread_info *pos = &MainThread;
+    while (pos->next != old)
+        pos = pos->next;
+    pos->next = old->next;
+    free(old);
+    dprintf(2, "=========== end thread %p\n", old);
+}
+
+void
+run_thread(void (*func)(void*), void *data)
+{
+    ASSERT32();
+    if (! CONFIG_THREADS)
+        goto fail;
+    struct thread_info *thread;
+    thread = memalign_tmphigh(THREADSTACKSIZE, THREADSTACKSIZE);
+    if (!thread)
+        goto fail;
+
+    thread->stackpos = (void*)thread + THREADSTACKSIZE;
+    struct thread_info *cur = getCurThread();
+    thread->next = cur->next;
+    cur->next = thread;
+
+    dprintf(2, "=========== start thread %p\n", thread);
+    asm volatile(
+        // Start thread
+        "  pushl $1f\n"                 // store return pc
+        "  pushl %%ebp\n"               // backup %ebp
+        "  movl %%esp, 4(%%edx)\n"      // cur->stackpos = %esp
+        "  movl 4(%%ebx), %%esp\n"      // %esp = thread->stackpos
+        "  calll *%%ecx\n"              // Call func
+
+        // End thread
+        "  movl (%%ebx), %%ecx\n"       // %ecx = thread->next
+        "  movl 4(%%ecx), %%esp\n"      // %esp = next->stackpos
+        "  movl %%ebx, %%eax\n"
+        "  calll %4\n"                  // call __end_thread(thread)
+        "  popl %%ebp\n"                // restore %ebp
+        "  retl\n"                      // restore pc
+        "1:\n"
+        : "+a"(data), "+c"(func), "+b"(thread), "+d"(cur)
+        : "m"(*(u8*)__end_thread)
+        : "esi", "edi", "cc", "memory");
+    return;
+
+fail:
+    func(data);
+}
+
+void
+wait_threads()
+{
+    ASSERT32();
+    if (! CONFIG_THREADS)
+        return;
+    while (MainThread.next != &MainThread)
+        yield();
 }
 
 

diff --git a/src/util.h b/src/util.h
index 01f46c0e69244c579a65059f7c84b5c05e867750..c2214c9f4a659843b2119593c351a265efe59c7d 100644 (file)
--- a/src/util.h
+++ b/src/util.h
@@ -139,6 +139,8 @@ static inline u8 readb(const void *addr) {
 
 // util.c
 inline u32 stack_hop(u32 eax, u32 edx, u32 ecx, void *func);
+void run_thread(void (*func)(void*), void *data);
+void wait_threads();
 u8 checksum_far(u16 buf_seg, void *buf_far, u32 len);
 u8 checksum(void *buf, u32 len);
 int memcmp(const void *s1, const void *s2, size_t n);