setup.S: A20 enable sequence (once again)

diff -ur linux-2.4.0-test10/arch/i386/boot/setup.S linux/arch/i386/boot/setup.S
@@ -631,6 +631,26 @@
 						# appropriate
 
 # that was painless, now we enable a20
+#	Try the "fast A20" version first:
+#	You must preserve the other bits here. Otherwise embarrasing things
+#	like laptops powering off on boot happen. Corrected version by Kira
+#	Brown from Linux 2.2
+#
+	inb	$0x92, %al			# 
+	orb	$02, %al			# "fast A20" version
+	andb	$0xfe, %al			# be sure not to reset CPU
+	outb	%al, $0x92			# some chips have only this
+
+#	See if that already did the job, skip keyboard controller
+#	accesses if so: some boards do not have a keyboard controller.
+#	These boards (especially if running on a slowish 386 EX)
+#	can spend several *minutes* in silence, polling for the
+#	non-existent keyboard controller.
+
+	movw	$0x100,%cx			# small retry count
+	call	a20_check
+	jne		a20_enabled
+
 	call	empty_8042
 
 	movb	$0xD1, %al			# command write

@@ -641,20 +661,20 @@
 	outb	%al, $0x60
 	call	empty_8042
 
-#
-#	You must preserve the other bits here. Otherwise embarrasing things
-#	like laptops powering off on boot happen. Corrected version by Kira
-#	Brown from Linux 2.2
-#
-	inb	$0x92, %al			# 
-	orb	$02, %al			# "fast A20" version
-	outb	%al, $0x92			# some chips have only this
-
 # wait until a20 really *is* enabled; it can take a fair amount of
 # time on certain systems; Toshiba Tecras are known to have this
 # problem.  The memory location used here (0x200) is the int 0x80
 # vector, which should be safe to use.
 
+a20_recheck:
+	movw	$0xffff,%cx		# large retry count
+	call	a20_check
+	jne		a20_enabled
+	jmp	a20_recheck			# loop until no longer aliased
+	
+#
+# cx has retry count
+a20_check:
 	xorw	%ax, %ax			# segment 0x0000
 	movw	%ax, %fs
 	decw	%ax				# segment 0xffff (HMA)

@@ -663,9 +683,11 @@
 	incw	%ax				# unused memory location <0xfff0
 	movw	%ax, %fs:(0x200)		# we use the "int 0x80" vector
 	cmpw	%gs:(0x210), %ax		# and its corresponding HMA addr
-	je	a20_wait			# loop until no longer aliased
+	loopz	a20_wait
+	ret
 
 # make sure any possible coprocessor is properly reset..
+a20_enabled:
 	xorw	%ax, %ax
 	outb	%al, $0xf0
 	call	delay

@@ -631,39 +631,45 @@
 						# appropriate
 
 # that was painless, now we enable a20
+
+#
+# First, try the "fast A20 gate".
+#
+	inb	$0x92,%al
+	orb	$0x02,%al			# Fast A20 on
+	andb	$0xfe,%al			# Don't reset CPU!
+	outb	%al,$0x92
+
+#
+# Now comes the tricky part: some machines don't have a KBC and thus
+# would end up looping almost indefinitely here.  HOWEVER, once we
+# have done the first command write, we must not stop the sequence.
+# Therefore, the first empty_8042 should check to see if the fast A20
+# did the trick and stop its probing at that stage; but subsequent ones
+# must not do so.
+#
+	movb	$0x01,%dl			# A20-sensitive
 	call	empty_8042
+	jnz	a20_wait			# A20 already on?
 
 	movb	$0xD1, %al			# command write
 	outb	%al, $0x64
+	xorb	%dl,%dl				# Not A20-sensitive
 	call	empty_8042
 
 	movb	$0xDF, %al			# A20 on
 	outb	%al, $0x60
+	xorb	%dl,%dl				# Not A20-sensitive
 	call	empty_8042
 
-#
-#	You must preserve the other bits here. Otherwise embarrasing things
-#	like laptops powering off on boot happen. Corrected version by Kira
-#	Brown from Linux 2.2
-#
-	inb	$0x92, %al			# 
-	orb	$02, %al			# "fast A20" version
-	outb	%al, $0x92			# some chips have only this
-
 # wait until a20 really *is* enabled; it can take a fair amount of
 # time on certain systems; Toshiba Tecras are known to have this
 # problem.  The memory location used here (0x200) is the int 0x80
 # vector, which should be safe to use.
 
-	xorw	%ax, %ax			# segment 0x0000
-	movw	%ax, %fs
-	decw	%ax				# segment 0xffff (HMA)
-	movw	%ax, %gs
 a20_wait:
-	incw	%ax				# unused memory location <0xfff0
-	movw	%ax, %fs:(0x200)		# we use the "int 0x80" vector
-	cmpw	%gs:(0x210), %ax		# and its corresponding HMA addr
-	je	a20_wait			# loop until no longer aliased
+	call	a20_test
+	jz	a20_wait
 
 # make sure any possible coprocessor is properly reset..
 	xorw	%ax, %ax

@@ -825,14 +831,24 @@
 #
 # Some machines have delusions that the keyboard buffer is always full
 # with no keyboard attached...
+#
+# If %dl is nonzero on entry, terminate with ZF=0 if A20 becomes alive,
+# otherwise terminate with ZF=1.
 
 empty_8042:
 	pushl	%ecx
-	movl	$0x00FFFFFF, %ecx
+	movl	$0x000FFFFF, %ecx
 
 empty_8042_loop:
 	decl	%ecx
-	jz	empty_8042_end_loop
+	jz	empty_8042_end_loop		# ZF=1
+
+	# Always call the test routine to keep delays constant
+	call	a20_test
+	jz	ignore_a20
+	and	%dl,%dl
+	jnz	empty_8042_end_loop		# ZF=0
+ignore_a20:
 
 	call	delay
 

@@ -847,10 +863,38 @@
 no_output:
 	testb	$2, %al				# is input buffer full?
 	jnz	empty_8042_loop			# yes - loop
+	# ZF=1
+
 empty_8042_end_loop:
 	popl	%ecx
 	ret
 
+a20_test:
+	pushw	%ax
+	pushw	%cx
+	pushw	%fs
+	pushw	%gs
+	xorw	%ax, %ax			# segment 0x0000
+	movw	%ax, %fs
+	decw	%ax				# segment 0xffff (HMA)
+	movw	%ax, %gs
+	movw	0x100,%cx
+	movw	%fs:(0x200),%ax			# So we keep cycling...
+	pushw	%ax				# Be extra paranoid...
+a20_loop:
+	incw	%ax				# unused memory location <0xfff0
+	movw	%ax, %fs:(0x200)		# we use the "int 0x80" vector
+	cmpw	%gs:(0x210), %ax		# and its corresponding HMA addr
+	jnz	a20_ret				# if ZF not set A20 is functional
+	loop	a20_loop
+a20_ret:
+	popw	%fs:(0x200)
+	popw	%gs
+	popw	%fs
+	popw	%cx
+	popw	%ax
+	ret					# if ZF set A20 is not operational
+
 # Read the cmos clock. Return the seconds in al
 gettime:
 	pushw	%cx

@@ -867,7 +911,8 @@
 
 # Delay is needed after doing I/O
 delay:
-	jmp	.+2				# jmp $+2
+	outb	%al,$0x80			# What the main kernel uses
+	outb	%al,$0x80
 	ret
 
 # Descriptor tables