/* -----------------------------------------------------------------------
 *
 *   Copyright 2008 rPath, Inc. - All Rights Reserved
 *
 *   Permission is hereby granted, free of charge, to any person
 *   obtaining a copy of this software and associated documentation
 *   files (the "Software"), to deal in the Software without
 *   restriction, including without limitation the rights to use,
 *   copy, modify, merge, publish, distribute, sublicense, and/or
 *   sell copies of the Software, and to permit persons to whom
 *   the Software is furnished to do so, subject to the following
 *   conditions:
 *
 *   The above copyright notice and this permission notice shall
 *   be included in all copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 *   EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 *   OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 *   NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 *   HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 *   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 *   OTHER DEALINGS IN THE SOFTWARE.
 *
 * ----------------------------------------------------------------------- */

/*
 * Code to enable A20.  Based on code in SYSLINUX.
 */
	.code16
	.section ".text16","ax"

	.type	enable_a20, @function
	.globl	enable_a20
enable_a20:
	pushal

	/* Are we enabled already? */
	call	a20_test
	jnz	1f

	/* Try the BIOS */
3:
	movw	$0x2401, %ax
	pushfl
	int	$0x15
	popfl
	call	a20_test
	jnz	1f

	/* Try the keyboard controller */
	movb	$1, %cl
	call	empty_8042
	jnz	1f		/* A20 live, no need for KBC */

	xorw	%cx, %cx
	movb	$0xd1, %al	/* Command write */
	outb	%al, $0x64
	call	empty_8042

	movb	$0xdf, %al	/* Enable A20 */
	outb	%al, $0x60
	call	empty_8042

	movb	$0xff, %al	/* Null command, required by UHCI spec */
	outb	%al, $0x64
	call	empty_8042

	/* Loop until A20 is enabled, or for 2^16 loops */
	/* %cx == 0 */
2:	call	a20_test
	jnz	1f
	loop	2b

	/* Urk.  Try the "fast A20 gate" (config port A) */
	inb	$0x92, %al
	orb	$0x02, %al	/* Enable A20 */
	andb	$0xfe, %al	/* Do not reset */
	outb	%al, $0x92

	/* Loop until A20 is enabled, or for 2^16 loops */
	/* %cx == 0 */
2:	call	a20_test
	jnz	1f
	loop	2b

	/* If we get here, we're in trouble.  Try again;
	   loop forever if need be. */
	jmp	3b

1:
	popal
	ret
	.size	enable_a20, .-enable_a20

/*
 * Test to see if A20 already is enabled.
 */
	.type	a20_test, @function
a20_test:
	pushw	%cx
	pushw	%ax
	pushw	%fs
	pushw	%gs

	xorw	%cx, %cx
	movw	%cx, %fs
	decw	%cx
	movw	%cx, %gs

	movw	$32, %cx	/* Loop count */
	movw	%fs:0,%ax	/* Divide by zero vector, hope it's safe */
	pushw	%ax
1:
	incw	%ax
	movw	%ax, %fs:0
	call	io_delay
	cmpw	%gs:0x10, %ax
	loope	1b
	popw	%fs:0		/* Restore previous value */
	popw	%gs
	popw	%fs
	popw	%ax
	popw	%cx
	ret
	.size	a20_test, .-a20_test


/*
 * Routine to empty the 8042 KBC controller.  If %cx != 0 then we will
 * test A20 in the loop and exit if A20 becomes enabled.
 */
	.type	empty_8042, @function
empty_8042:
4:
	call	a20_test
	jcxz	1f		/* If %cx == 0 continue regardless */
	jnz	2f
1:
	call	io_delay
	inb	$0x64, %al	/* Status port */
	testb	$0x01, %al
	jz	3f		/* No input */
	call	io_delay
	inb	$0x60, %al	/* Read input */
	jmp	4b
3:
	testb	$0x02, %al
	jnz	4b
	call	io_delay
2:
	ret
	.size	empty_8042, .-empty_8042

/*
 * I/O delay function
 */
#define DELAY_PORT	0x80

	.type	io_delay, @function
io_delay:
	pushw	%ax
	xorw	%ax, %ax
	outb	%al, $DELAY_PORT
	outb	%al, $DELAY_PORT
	popw	%ax
	ret
	.size	io_delay, .-io_delay
