commit 1f5100ec76c4468eab264a594bf8cd452b64a740
Author: Kevin O'Connor <kevin@koconnor.net>
Date:   Tue Feb 5 21:21:26 2013 -0500

    Modify CSM code for non-returning reloc_preinit system.
    
    Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

diff --git a/src/csm.c b/src/csm.c
index c453ebd..473d322 100644
--- a/src/csm.c
+++ b/src/csm.c
@@ -34,8 +34,26 @@ EFI_COMPATIBILITY16_TABLE csm_compat_table VAR32FLATVISIBLE __aligned(16) = {
 EFI_TO_COMPATIBILITY16_INIT_TABLE *csm_init_table;
 EFI_TO_COMPATIBILITY16_BOOT_TABLE *csm_boot_table;
 
+extern void csm_return(struct bregs *regs) __noreturn;
+
+static void
+csm_maininit(struct bregs *regs)
+{
+    interface_init();
+    timer_setup();
+    pci_probe_devices();
+
+    csm_compat_table.PnPInstallationCheckSegment = SEG_BIOS;
+    csm_compat_table.PnPInstallationCheckOffset = get_pnp_offset();
+
+    regs->ax = 0;
+
+    // Return directly to UEFI instead of unwinding stack.
+    csm_return(regs);
+}
+
 /* Legacy16InitializeYourself */
-void
+static void
 handle_csm_0000(struct bregs *regs)
 {
     dprintf(3, "Legacy16InitializeYourself table %04x:%04x\n", regs->es,
@@ -59,19 +77,11 @@ handle_csm_0000(struct bregs *regs)
                        csm_init_table->LowPmmMemorySizeInBytes,
                        csm_init_table->HiPmmMemory,
                        csm_init_table->HiPmmMemorySizeInBytes);
-    reloc_preinit();
-    interface_init();
-    timer_setup();
-    pci_probe_devices();
-
-    csm_compat_table.PnPInstallationCheckSegment = SEG_BIOS;
-    csm_compat_table.PnPInstallationCheckOffset = get_pnp_offset();
-
-    regs->ax = 0;
+    reloc_preinit(csm_maininit, regs);
 }
 
 /* Legacy16UpdateBbs */
-void VISIBLE32INIT
+static void
 handle_csm_0001(struct bregs *regs)
 {
     dprintf(3, "Legacy16UpdateBbs table %04x:%04x\n", regs->es, regs->bx);
@@ -97,7 +107,7 @@ handle_csm_0001(struct bregs *regs)
 }
 
 /* PrepareToBoot */
-void VISIBLE32INIT
+static void
 handle_csm_0002(struct bregs *regs)
 {
     dprintf(3, "PrepareToBoot table %04x:%04x\n", regs->es, regs->bx);
@@ -151,7 +161,7 @@ handle_csm_0002(struct bregs *regs)
 }
 
 /* Boot */
-void VISIBLE32INIT
+static void
 handle_csm_0003(struct bregs *regs)
 {
     dprintf(3, "Boot\n");
@@ -162,7 +172,7 @@ handle_csm_0003(struct bregs *regs)
 }
 
 /* Legacy16DispatchOprom */
-void VISIBLE32INIT
+static void
 handle_csm_0005(struct bregs *regs)
 {
     EFI_DISPATCH_OPROM_TABLE *table = MAKE_FLATPTR(regs->es, regs->bx);
@@ -191,7 +201,7 @@ handle_csm_0005(struct bregs *regs)
 }
 
 /* Legacy16GetTableAddress */
-void VISIBLE32INIT
+static void
 handle_csm_0006(struct bregs *regs)
 {
     u16 size = regs->cx;
@@ -221,7 +231,7 @@ handle_csm_0006(struct bregs *regs)
     }
 }
 
-void VISIBLE32FLAT
+void VISIBLE32INIT
 handle_csm(struct bregs *regs)
 {
     ASSERT32FLAT();
diff --git a/src/romlayout.S b/src/romlayout.S
index aebe2df..07d1645 100644
--- a/src/romlayout.S
+++ b/src/romlayout.S
@@ -385,38 +385,53 @@ entry_elf:
         .code16gcc
 
         EXPORTFUNC entry_csm
+        .global csm_return
 entry_csm:
+        // Backup register state
+        pushfw
+        cli
+        cld
+        pushl %eax                      // dummy
         PUSHBREGS
 
-        // Reset stack and store EFI's old SS:SP on it
-        movl %esp, %edx
-        movw %ss, %cx
+        // Backup stack location and convert to a "flat pointer"
+        movl %ss, %ebx
+        movw %bx, BREGS_code+2(%esp)    // Store %ss in bregs->code.seg
+        shll $4, %ebx
+        addl %esp, %ebx
 
+        // Change to BUILD_STACK_ADDR stack
         xorl %eax, %eax
         movw %ax, %ss
-        movw %ax, %ds
         movl $BUILD_STACK_ADDR, %esp
 
-        pushl %ecx // EFI's SS
-        pushl %edx // EFI's SP
-
-        // Turn %edx into a flat pointer (including segment base)
-        shll $4, %ecx
-        addl %ecx, %edx
+        // Jump to 32bit mode and call handle_csm32(bregs)
+        movl $(1f + BUILD_BIOS_ADDR), %edx
+        jmp transition32
+        .code32
+1:      movl %ebx, %eax
+        calll _cfunc32flat_handle_csm
+        movl $2f, %edx
+        jmp transition16big
 
-        // call32(handle_csm32, bregs, -1)
-        movl $_cfunc32flat_handle_csm, %eax
-        movl $-1, %ecx
-        calll call32
+csm_return:
+        movl %eax, %ebx
+        movl $2f, %edx
+        jmp transition16big
+        .code16gcc
 
-        // Switch back to EFI's stack and return
-        popl %edx
-        popl %ecx
-        movw %cx, %ss
-        movw %cx, %ds
-        movl %edx, %esp
+        // Switch back to original stack
+2:      movzwl BREGS_code+2(%ebx), %eax
+        movl %eax, %ecx
+        shll $4, %ecx
+        subl %ecx, %ebx
+        movl %eax, %ss
+        movl %ebx, %esp
 
+        // Restore register state and return.
         POPBREGS
+        addw $4, %sp                    // pop dummy
+        popfw
         lretw