A20 patch

[Marco Gerards]

Hi,

The A20 code of GRUB Legacy does not work in all cases, as some of you
already know.  For me the most important thing is that it does not
work on my MacBook.  Here is a backport from GRUB2's A20 code.  The
only change it the calling convention (regparm -> stack).

Okuji, can I commit this patch?  I see this as a bugfix, do you?

Thanks,
Marco


2006-07-17  Marco Gerards  <address@hidden>

        * stage2/asm.S (gateA20): Code backported from GRUB2.  Written by
        Yoshinori K. Okuji  <address@hidden>.


Index: stage2/asm.S
===================================================================
RCS file: /sources/grub/grub/stage2/asm.S,v
retrieving revision 1.72
diff -u -p -r1.72 asm.S
--- stage2/asm.S        20 Jun 2004 13:48:46 -0000      1.72
+++ stage2/asm.S        17 Jul 2006 19:19:23 -0000
@@ -1622,79 +1622,139 @@ ENTRY(set_vbe_mode)
  */
 
 ENTRY(gateA20)
-       /* first, try a BIOS call */
-       pushl   %ebp
        movl    8(%esp), %edx
-       
-       call    EXT_C(prot_to_real)
-       
+
+gate_a20_test_current_state:   
+       /* first of all, test if already in a good state */
+       call    gate_a20_check_state
+       cmpb    %al, %dl
+       jnz     gate_a20_try_bios
+       ret
+
+gate_a20_try_bios:
+       /* second, try a BIOS call */
+       pushl   %ebp
+       call    prot_to_real
+
        .code16
        movw    $0x2400, %ax
-       testw   %dx, %dx
+       testb   %dl, %dl
        jz      1f
        incw    %ax
-1:     stc
-       int     $0x15
-       jnc     2f
-
-       /* set non-zero if failed */
-       movb    $1, %ah
-
-       /* save the status */
-2:     movb    %ah, %dl
+1:     int     $0x15
 
-       DATA32  call    EXT_C(real_to_prot)
+       DATA32  call    real_to_prot
        .code32
 
        popl    %ebp
-       testb   %dl, %dl
-       jnz     3f
+       call    gate_a20_check_state
+       cmpb    %al, %dl
+       jnz     gate_a20_try_keyboard_controller
        ret
+       
+gate_a20_flush_keyboard_buffer:
+       inb     $0x64
+       andb    $0x02, %al
+       jnz     gate_a20_flush_keyboard_buffer
+2:     
+       inb     $0x64
+       andb    $0x01, %al
+       jz      3f
+       inb     $0x60
+       jmp     2b
+3:
+       ret
+       
+gate_a20_try_keyboard_controller:      
+       /* third, try the keyboard controller */
+       call    gate_a20_flush_keyboard_buffer
+
+       movb    $0xd1, %al
+       outb    $0x64
+4:     
+       inb     $0x64
+       andb    $0x02, %al
+       jnz     4b
 
-3:     /* use keyboard controller */
-       pushl   %eax
-
-       call    gloop1
-
-       movb    $KC_CMD_WOUT, %al
-       outb    $K_CMD
-
-gloopint1:
-       inb     $K_STATUS
-       andb    $K_IBUF_FUL, %al
-       jnz     gloopint1
-
-       movb    $KB_OUTPUT_MASK, %al
-       cmpb    $0, 0x8(%esp)
-       jz      gdoit
-
-       orb     $KB_A20_ENABLE, %al
-gdoit:
-       outb    $K_RDWR
-
-       call    gloop1
+       movb    $0xdd, %al
+       testb   %dl, %dl
+       jz      5f
+       orb     $0x02, %al
+5:     outb    $0x60
+       call    gate_a20_flush_keyboard_buffer
 
        /* output a dummy command (USB keyboard hack) */
        movb    $0xff, %al
-       outb    $K_CMD
-       call    gloop1
-       
-       popl    %eax
-       ret
-
-gloop1:
-       inb     $K_STATUS
-       andb    $K_IBUF_FUL, %al
-       jnz     gloop1
+       outb    $0x64
+       call    gate_a20_flush_keyboard_buffer
 
-gloop2:
-       inb     $K_STATUS
-       andb    $K_OBUF_FUL, %al
-       jz      gloop2ret
-       inb     $K_RDWR
-       jmp     gloop2
+       call    gate_a20_check_state
+       cmpb    %al, %dl
+       jnz     gate_a20_try_system_control_port_a
+       ret
 
-gloop2ret:
+gate_a20_try_system_control_port_a:
+       /* fourth, try the system control port A */
+       inb     $0x92
+       andb    $(~0x03), %al
+       testb   %dl, %dl
+       jz      6f
+       orb     $0x02, %al
+6:     outb    $0x92
+
+       /* When turning off Gate A20, do not check the state strictly,
+          because a failure is not fatal usually, and Gate A20 is always
+          on some modern machines.  */
+       testb   %dl, %dl
+       jz      7f      
+       call    gate_a20_check_state
+       cmpb    %al, %dl
+       /* everything failed, so restart from the beginning */
+       jnz     gate_a20_try_bios
+7:     ret
+       
+gate_a20_check_state:
+       /* iterate the checking for a while */
+       movl    $100, %ecx
+1:     
+       call    3f
+       cmpb    %al, %dl
+       jz      2f
+       loop    1b
+2:
+       ret
+3:     
+       pushl   %ebx
+       pushl   %ecx
+       xorl    %eax, %eax
+       /* compare the byte at 0x8000 with that at 0x108000 */
+       movl    $STAGE1_BOOT_DRIVE, %ebx
+       pushl   %ebx
+       /* save the original byte in CL */
+       movb    (%ebx), %cl
+       /* store the value at 0x108000 in AL */
+       addl    $0x100000, %ebx
+       movb    (%ebx), %al
+       /* try to set one less value at 0x8000 */
+       popl    %ebx
+       movb    %al, %ch
+       decb    %ch
+       movb    %ch, (%ebx)
+       /* serialize */
+       outb    %al, $0x80
+       outb    %al, $0x80
+       /* obtain the value at 0x108000 in CH */
+       pushl   %ebx
+       addl    $0x100000, %ebx
+       movb    (%ebx), %ch
+       /* this result is 1 if A20 is on or 0 if it is off */
+       subb    %ch, %al
+       xorb    $1, %al
+       /* restore the original */
+       popl    %ebx
+       movb    %cl, (%ebx)
+       popl    %ecx
+       popl    %ebx
        ret