X-Git-Url: https://git.sur5r.net/?a=blobdiff_plain;f=arch%2Fx86%2Flib%2Fbios_asm.S;h=9dbf969373611da1a94084a8665b0deb28eea8d3;hb=f4abfed1de34e3fa18be4a8a156416cd6ed21f4a;hp=4faa70e314dc540c69a5754beb00e4ed7e813d74;hpb=fc9b0b80435cda721fbdbe507c9e4f388b0ea62b;p=u-boot

diff --git a/arch/x86/lib/bios_asm.S b/arch/x86/lib/bios_asm.S
index 4faa70e314..9dbf969373 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