[U-Boot] [RESPIN PATCH v2 02/11] x86: bios: Synchronize stack between real and protected mode

[Bin Meng]

From: Jian Luo <jian.luo4 at boschrexroth.de>

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 at boschrexroth.de>
Signed-off-by: Bin Meng <bmeng.cn at gmail.com>

---
Changes in v2 respin:
- Use *protected* mode instead of 'protect mode'
- Correct typo of 'read mode'

Changes in v2:
- Add comments for the changes in the assembly codes

 arch/x86/lib/bios_asm.S | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/arch/x86/lib/bios_asm.S b/arch/x86/lib/bios_asm.S
index 4faa70e..9dbf969 100644
--- 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
-- 
1.8.2.1