cleaned up A20 test and set code

2 files changed, 101 insertions, 96 deletions

diff --git a/src/boot.asm b/src/boot.asm
index 3505e55..99b4eae 100644
--- a/src/boot.asm
+++ b/src/boot.asm
@@ -75,106 +75,23 @@ times 510-($-$$) db 0
 dw 0xaa55
 
 stage2:
+	nop
+	nop
+	nop
+	
 ; print a message that we are indeed in stage 2 now
 	mov si, MESSAGE_STAGE_2_LOADED
 	call print_string
 
 ; check A20 gate
-A20_CHECK:
-	call check_A20_enabled
-	cmp ax, 1
-	je A20_ENABLED
-	mov si, MESSAGE_STAGE_2_A20_DISABLED
-	call print_string
-	jmp HALT_OS_REAL
-
-A20_ENABLE_VIA_BIOS:
-	mov ax, 0x2401
-	int 0x15
-
-	call check_A20_enabled
-	cmp ax, 1
-	je A20_ENABLED
-	mov si, MESSAGE_STAGE_2_A20_DISABLED
-	call print_string
-
-A20_ENABLE_KBD_PORT:
-	mov al, 0xdd
-	out 0x64, al
-
-	call check_A20_enabled
+	call check_and_enable_A20
 	cmp ax, 1
-	je A20_ENABLED
+	je .A20_enabled
 	mov si, MESSAGE_STAGE_2_A20_DISABLED
 	call print_string
+	jmp .halt_os_realmode
 
-A20_ENABLE_KBD_OUT:
-
-	cli
-
-        call    .wait_input
-        mov     al,0xAD
-        out     0x64,al		; disable keyboard
-        call    .wait_input
-
-        mov     al,0xD0
-        out     0x64,al		; tell controller to read output port
-        call    .wait_output
-
-        in      al,0x60
-        push    eax		; get output port data and store it
-        call    .wait_input
-
-        mov     al,0xD1
-        out     0x64,al		; tell controller to write output port
-        call    .wait_input
-
-        pop     eax
-        or      al,2		; set bit 1 (enable a20)
-        out     0x60,al		; write out data back to the output port
-
-        call    .wait_input
-        mov     al,0xAE		; enable keyboard
-        out     0x64,al
-
-        call    .wait_input
-	popa
-        sti
-        jmp .retest
-
-; wait for input buffer to be clear
-.wait_input:
-        in      al,0x64
-        test    al,2
-        jnz     .wait_input
-        ret
-
-; wait for output buffer to be clear
-.wait_output:
-        in      al,0x64
-        test    al,1
-        jz      .wait_output
-        ret
-
-.retest:
-	call check_A20_enabled
-	cmp ax, 1
-	je A20_ENABLED
-	mov si, MESSAGE_STAGE_2_A20_DISABLED
-	call print_string
-
-A20_FAST_SPECIAL_PORT:
-	in al, 0x92
-	or al, 2
-	out 0x92, al
-
-	call check_A20_enabled
-	cmp ax, 1
-	je A20_ENABLED
-	mov si, MESSAGE_STAGE_2_A20_DISABLED
-	call print_string
-  	
-A20_ENABLED:
+.A20_enabled:
 	mov si, MESSAGE_STAGE_2_A20_ENABLED
 	call print_string
 	
@@ -190,7 +107,7 @@ A20_ENABLED:
 	jmp $
 
 ; stop loading in real mode
-HALT_OS_REAL:
+.halt_os_realmode:
 	mov si, MESSAGE_HALTED_REAL
 	call print_string
 .loop:	cli
@@ -225,7 +142,7 @@ BEGIN_PROTECTED_MODE:
 ; position
 	call check_magic
 	cmp ax, 1
-	jnz HALT_OS
+	jnz .halt_os_pm
 
 ; print a message before we call the C level kernel	
 	mov si, MESSAGE_CALL_C_ENTRY
@@ -239,7 +156,7 @@ BEGIN_PROTECTED_MODE:
 	call read_hardware_vga_cursor
 
 ; "kernel halted" message, when we terminate the C kernel
-HALT_OS:
+.halt_os_pm:
 	mov si, MESSAGE_HALTED
 	call pm_print_string
 	call pm_print_newline
diff --git a/src/stage2_a20.asm b/src/stage2_a20.asm
index 094407b..630c67a 100644
--- a/src/stage2_a20.asm
+++ b/src/stage2_a20.asm
@@ -1,5 +1,5 @@
 ; functions to handle A20 address line status and switching
-; returns 0 if not enabled, 1 if enabled in AX
+; returns 0 if notA20_ENABLED, 1 ifA20_ENABLED in AX
 check_A20_enabled:
 	pushf
 	push ds
@@ -25,7 +25,7 @@ check_A20_enabled:
 	mov byte [es:di], 0x00		; now the test: write 0x00 to 0000:0500
 	mov byte [ds:si], 0xFF		; write 0xff to ffff:0510
 	
-	cmp byte [es:di], 0xFF		; memory wrap? A20 not enabled
+	cmp byte [es:di], 0xFF		; memory wrap? A20 notA20_ENABLED
 	
 	pop ax				; restore original memory contents
 	mov byte [ds:si], al
@@ -45,3 +45,91 @@ check_A20_enabled:
 	popf
 	
 	ret
+
+check_and_enable_A20:
+	call check_A20_enabled
+	cmp ax, 1
+	je A20_ENABLED
+	ret
+
+A20_ENABLE_VIA_BIOS:
+	mov ax, 0x2401
+	int 0x15
+
+	call check_A20_enabled
+	cmp ax, 1
+	je A20_ENABLED
+
+A20_ENABLE_KBD_PORT:
+	mov al, 0xdd
+	out 0x64, al
+
+	call check_A20_enabled
+	cmp ax, 1
+	je A20_ENABLED
+
+A20_ENABLE_KBD_OUT:
+
+	cli
+
+        call    .wait_input
+        mov     al,0xAD
+        out     0x64,al		; disable keyboard
+        call    .wait_input
+
+        mov     al,0xD0
+        out     0x64,al		; tell controller to read output port
+        call    .wait_output
+
+        in      al,0x60
+        push    eax		; get output port data and store it
+        call    .wait_input
+
+        mov     al,0xD1
+        out     0x64,al		; tell controller to write output port
+        call    .wait_input
+
+        pop     eax
+        or      al,2		; set bit 1 (enable a20)
+        out     0x60,al		; write out data back to the output port
+
+        call    .wait_input
+        mov     al,0xAE		; enable keyboard
+        out     0x64,al
+
+        call    .wait_input
+	popa
+        sti
+        jmp .retest
+
+; wait for input buffer to be clear
+.wait_input:
+        in      al,0x64
+        test    al,2
+        jnz     .wait_input
+        ret
+
+; wait for output buffer to be clear
+.wait_output:
+        in      al,0x64
+        test    al,1
+        jz      .wait_output
+        ret
+
+.retest:
+	call check_A20_enabled
+	cmp ax, 1
+	je A20_ENABLED
+
+A20_FAST_SPECIAL_PORT:
+	in al, 0x92
+	or al, 2
+	out 0x92, al
+
+	call check_A20_enabled
+	cmp ax, 1
+	je A20_ENABLED
+	ret
+  	
+A20_ENABLED:
+	ret