diff --git a/arch/x86/boot/boot.h b/arch/x86/boot/boot.h
index 5f9a2e72a731..d2b5adf46512 100644
--- a/arch/x86/boot/boot.h
+++ b/arch/x86/boot/boot.h
@@ -17,6 +17,8 @@
 #ifndef BOOT_BOOT_H
 #define BOOT_BOOT_H
 
+#define STACK_SIZE	512	/* Minimum number of bytes for stack */
+
 #ifndef __ASSEMBLY__
 
 #include <stdarg.h>
@@ -198,8 +200,6 @@ static inline int isdigit(int ch)
 }
 
 /* Heap -- available for dynamic lists. */
-#define STACK_SIZE	512	/* Minimum number of bytes for stack */
-
 extern char _end[];
 extern char *HEAP;
 extern char *heap_end;
@@ -216,9 +216,9 @@ static inline char *__get_heap(size_t s, size_t a, size_t n)
 #define GET_HEAP(type, n) \
 	((type *)__get_heap(sizeof(type),__alignof__(type),(n)))
 
-static inline int heap_free(void)
+static inline bool heap_free(size_t n)
 {
-	return heap_end-HEAP;
+	return (int)(heap_end-HEAP) >= (int)n;
 }
 
 /* copy.S */
diff --git a/arch/x86/boot/header.S b/arch/x86/boot/header.S
index 8353c81c41c0..6ef5a060fa11 100644
--- a/arch/x86/boot/header.S
+++ b/arch/x86/boot/header.S
@@ -173,7 +173,8 @@ ramdisk_size:	.long	0		# its size in bytes
 bootsect_kludge:
 		.long	0		# obsolete
 
-heap_end_ptr:	.word	_end+1024	# (Header version 0x0201 or later)
+heap_end_ptr:	.word	_end+STACK_SIZE-512
+					# (Header version 0x0201 or later)
 					# space from here (exclusive) down to
 					# end of setup code can be used by setup
 					# for local heap purposes.
@@ -230,28 +231,53 @@ start_of_setup:
 	int	$0x13
 #endif
 
-# We will have entered with %cs = %ds+0x20, normalize %cs so
-# it is on par with the other segments.
-	pushw	%ds
-	pushw	$setup2
-	lretw
-
-setup2:
 # Force %es = %ds
 	movw	%ds, %ax
 	movw	%ax, %es
 	cld
 
-# Stack paranoia: align the stack and make sure it is good
-# for both 16- and 32-bit references.  In particular, if we
-# were meant to have been using the full 16-bit segment, the
-# caller might have set %sp to zero, which breaks %esp-based
-# references.
-	andw	$~3, %sp	# dword align (might as well...)
-	jnz	1f
-	movw	$0xfffc, %sp	# Make sure we're not zero
-1:	movzwl	%sp, %esp	# Clear upper half of %esp
-	sti
+# Apparently some ancient versions of LILO invoked the kernel
+# with %ss != %ds, which happened to work by accident for the
+# old code.  If the CAN_USE_HEAP flag is set in loadflags, or
+# %ss != %ds, then adjust the stack pointer.
+
+	# Smallest possible stack we can tolerate
+	movw	$(_end+STACK_SIZE), %cx
+
+	movw	heap_end_ptr, %dx
+	addw	$512, %dx
+	jnc	1f
+	xorw	%dx, %dx	# Wraparound - whole segment available
+1:	testb	$CAN_USE_HEAP, loadflags
+	jnz	2f
+
+	# No CAN_USE_HEAP
+	movw	%ss, %dx
+	cmpw	%ax, %dx	# %ds == %ss?
+	movw	%sp, %dx
+	# If so, assume %sp is reasonably set, otherwise use
+	# the smallest possible stack.
+	jne	4f		# -> Smallest possible stack...
+
+	# Make sure the stack is at least minimum size.  Take a value
+	# of zero to mean "full segment."
+2:
+	andw	$~3, %dx	# dword align (might as well...)
+	jnz	3f
+	movw	$0xfffc, %dx	# Make sure we're not zero
+3:	cmpw	%cx, %dx
+	jnb	5f
+4:	movw	%cx, %dx	# Minimum value we can possibly use
+5:	movw	%ax, %ss
+	movzwl	%dx, %esp	# Clear upper half of %esp
+	sti			# Now we should have a working stack
+
+# We will have entered with %cs = %ds+0x20, normalize %cs so
+# it is on par with the other segments.
+	pushw	%ds
+	pushw	$6f
+	lretw
+6:
 
 # Check signature at end of setup
 	cmpl	$0x5a5aaa55, setup_sig
diff --git a/arch/x86/boot/video-bios.c b/arch/x86/boot/video-bios.c
index 68e65d95cdfd..ed0672a81870 100644
--- a/arch/x86/boot/video-bios.c
+++ b/arch/x86/boot/video-bios.c
@@ -79,7 +79,7 @@ static int bios_probe(void)
 	video_bios.modes = GET_HEAP(struct mode_info, 0);
 
 	for (mode = 0x14; mode <= 0x7f; mode++) {
-		if (heap_free() < sizeof(struct mode_info))
+		if (!heap_free(sizeof(struct mode_info)))
 			break;
 
 		if (mode_defined(VIDEO_FIRST_BIOS+mode))
diff --git a/arch/x86/boot/video-vesa.c b/arch/x86/boot/video-vesa.c
index 192190710710..4716b9a96357 100644
--- a/arch/x86/boot/video-vesa.c
+++ b/arch/x86/boot/video-vesa.c
@@ -57,7 +57,7 @@ static int vesa_probe(void)
 	while ((mode = rdfs16(mode_ptr)) != 0xffff) {
 		mode_ptr += 2;
 
-		if (heap_free() < sizeof(struct mode_info))
+		if (!heap_free(sizeof(struct mode_info)))
 			break;	/* Heap full, can't save mode info */
 
 		if (mode & ~0x1ff)
diff --git a/arch/x86/boot/video.c b/arch/x86/boot/video.c
index e4ba897bf9a3..ad9712f01739 100644
--- a/arch/x86/boot/video.c
+++ b/arch/x86/boot/video.c
@@ -371,7 +371,7 @@ static void save_screen(void)
 	saved.curx = boot_params.screen_info.orig_x;
 	saved.cury = boot_params.screen_info.orig_y;
 
-	if (heap_free() < saved.x*saved.y*sizeof(u16)+512)
+	if (!heap_free(saved.x*saved.y*sizeof(u16)+512))
 		return;		/* Not enough heap to save the screen */
 
 	saved.data = GET_HEAP(u16, saved.x*saved.y);