x86: bios: Synchronize stack between real and protected mode

PCI option rom may use different SS during its execution, so it is not
safe to assume esp pointed to the same location in the protected mode.

Signed-off-by: Jian Luo <jian.luo4@boschrexroth.de>
Signed-off-by: Bin Meng <bmeng.cn@gmail.com>
Acked-by: Simon Glass <sjg@chromium.org>

diff --git a/arch/x86/lib/bios_asm.S b/arch/x86/lib/bios_asm.S
index 4faa70e314dc540c69a5754beb00e4ed7e813d74..9dbf969373611da1a94084a8665b0deb28eea8d3 100644 (file)
--- a/arch/x86/lib/bios_asm.S
+++ b/arch/x86/lib/bios_asm.S
@@ -246,6 +246,9 @@ __interrupt_handler_16bit = PTR_TO_REAL_MODE(.)
        push    %fs
        push    %gs
 
+       /* Save real mode SS */
+       movw    %ss, %cs:__realmode_ss
+
        /* Clear DF to not break ABI assumptions */
        cld
 
@@ -258,12 +261,29 @@ __interrupt_handler_16bit = PTR_TO_REAL_MODE(.)
 
        enter_protected_mode
 
+       /*
+        * Now we are in protected mode. We need compute the right ESP based
+        * on saved real mode SS otherwise interrupt_handler() won't get
+        * correct parameters from the stack.
+        */
+       movzwl  %cs:__realmode_ss, %ecx
+       shll    $4, %ecx
+       addl    %ecx, %esp
+
        /* Call the C interrupt handler */
        movl    $interrupt_handler, %eax
        call    *%eax
 
+       /* Restore real mode ESP based on saved SS */
+       movzwl  %cs:__realmode_ss, %ecx
+       shll    $4, %ecx
+       subl    %ecx, %esp
+
        enter_real_mode
 
+       /* Restore real mode SS */
+       movw    %cs:__realmode_ss, %ss
+
        /*
         * Restore all registers, including those manipulated by the C
         * handler
@@ -276,6 +296,9 @@ __interrupt_handler_16bit = PTR_TO_REAL_MODE(.)
        popal
        iret
 
+__realmode_ss = PTR_TO_REAL_MODE(.)
+       .word   0
+
        .globl asm_realmode_code_size
 asm_realmode_code_size:
        .long  . - asm_realmode_code