.text
.global _start

/*
 * Bootloader configuration
 */

.set STACK1, 0x7000
.set STACK2, 0xe000
.set BUFFER, 0x8000 # 0x80000 - 0x8000 = 480k
.set READBLOCKS, 1

/*
 * Kernel configuration (sync with Kernel.odc!)
 */

.set KernelSizeAdr, 0x7e00
.set BootDiskAdr, 0x7e04
.set BootDiskDxAdr, 0x7e06
.set BootDiskCxAdr, 0x7e08
.set MemoryMapAdr, 0x7e0A
.set ImageBase, 0x1000000

# =========================================================================== #
# Stage 1
# =========================================================================== #

.code16
_start:
stage1:
# 1. Setup canonical %cs:%eip, segment registers %ds, %es, %fs, %gs, %ss and stack %sp
	cli
	ljmp $0x0, $canonized_label; canonized_label:
	xor %ax, %ax
	mov %ax, %ds
	mov %ax, %es
#	mov %ax, %fs
#	mov %ax, %gs
	mov %ax, %ss
	mov $STACK1, %sp

	mov %dx, BootDiskAdr
	call mapmem

# 2. Enable A20 gate
	call enable_A20
	sti

# 3. Reset disk controller (%dl and %dh initialized by bios) and read Kernel into buffer
	movl $(kernel_code_end - kernel_code_start), KernelSizeAdr

	stage1__read:
		mov current_sector, %ax
		add $1, %ax
		call lba2chs

		mov current_sector, %ax
		shl $5, %ax
		add $(BUFFER / 16), %ax
		mov %ax, %es
		call readsectors

		mov current_sector, %ax
		add $READBLOCKS, %ax
		mov %ax, current_sector
		cmp $((kernel_code_end - kernel_code_start + 511) / 512), %ax
		jl stage1__read

# 4. Disable interrupts and load GDT
	mov $msg_protected_mode, %si
	call print
	cli
	lgdt gdt_info

# 5. Switch to protected mode
	mov %cr0, %eax
	or $1, %al
	mov %eax, %cr0
	jmp $8, $stage2

# =========================================================================== #
# Stage 2
# =========================================================================== #

.code32
stage2:
# 6. Setup data segments %ds, %es, %fs, %gs  and stack %sp
	mov $16, %ax
	mov %ax, %ds
	mov %ax, %ss
	mov %ax, %es
	mov %ax, %fs
	mov %ax, %gs
	mov $STACK2, %sp
# 7. Copy Kernel from buffer
	xor %eax, %eax
	stage2_copy_loop:
		mov BUFFER(%eax), %bh
		mov %bh, ImageBase(%eax)
		inc %eax
		cmp $(kernel_code_end - kernel_code_start), %eax
		jne stage2_copy_loop
# 8. Exec Kernel
	jmp ImageBase


# =========================================================================== #
# Print asciiz string
#   input: %si = String
# =========================================================================== #

.code16
print:
	pusha
	movb $0x0e, %ah
	print__loop:
		lodsb
		orb %al, %al
		jz print__ret
		int $0x10
		jmp print__loop
	print__ret:
		popa
		ret

# =========================================================================== #
# Make memory map for kernel
# =========================================================================== #

.code16
mapmem:
	pusha
	movl $0, MemoryMapAdr
	xor %ebx, %ebx
	mov $(MemoryMapAdr + 4), %di
	mapmem__loop:
		mov $0xe820, %eax
		mov $20, %ecx
		mov $0x0534D4150, %edx
		int $0x15
		jc mapmem__failed
		cmp %edx, %eax
		jne mapmem__failed
		cmp $0, %ebx
		je mapmem__done
		add $20, %di
		incl MemoryMapAdr
		jmp mapmem__loop
	mapmem__done:
		popa
		ret # <============
	mapmem__failed:
		mov $msg_mapmem_failed, %si
		call print
		cli
		hlt

# =========================================================================== #
# Convert LBA to CHS
#   input: %ax = LBA %dl = disk
#   output: %ch = cylider %cl = sector %dh = head %dl = disk
# =========================================================================== #

.code16
lba2chs:
#	push %es
#	push %di
	push %dx
	push %ax

	clc
	mov $0x0, %ax
	mov %ax, %es
	mov %ax, %di
	mov $0x8, %ah
	int $0x13
	jnc lba2chs__success
		mov $msg_chs_error, %si
		call print
		cli
		hlt
	lba2chs__success:
		mov %dx, BootDiskDxAdr
		mov %cx, BootDiskCxAdr

		# SPT = %cl & 63
		# HPC = %dh + 1
		# CYL = LBA / ( HPC * SPT )
		# TEMP = LBA % ( HPC * SPT )
		# HEAD = TEMP / SPT
		# SECT = TEMP % SPT + 1

		# %ax = FREE %bx = FREE %cx = GSPT %dx = HPC-1
		and $0x3F, %cl
		movzx %cl, %cx

		# %ax = FREE %bx = FREE %cx = SPT  %dx = HPC-1
		movzx %dh, %dx
		inc %dx

		# %ax = FREE %bx = FREE %cx = SPT  %dx = HPC
		mov %dx, %ax
		mul %cx; # overflow in %dx
		mov %ax, %dx

		# %ax = FREE %bx = FREE %cx = SPT  %dx = (HPC * SPT)
		pop %ax # LBA
		mov %dx, %bx
		xor %dx, %dx
		div %bx
		xchg %ax, %dx

		# %ax = TEMP %bx = FREE %cx = SPT  %dx = CYL
		mov %dx, %bx
		xor %dx, %dx
		div %cx
		inc %dx

		# %ax = HEAD %bx = CYL  %cx = FREE %dx = SECT
		mov %bl, %ch
		mov %bh, %cl
		shl $6, %cl
		add %dx, %cx

		# %ax = HEAD %bx = FREE %cx = CYL&SECT %dx = FREE
		pop %dx
		mov %al, %dh

		# %ax = FREE %bx = FREE %cx = CYL&SECT %dx = HEAD&DISK

#	pop %di
#	pop %es
	ret

# =========================================================================== #
# Read sectors to buffer
#   input: %cx = Cylider+Sector; %es = 64k buf
# =========================================================================== #

.code16
readsectors:
        xor %ah, %ah
        int $0x13

        xor %bx, %bx
        mov $0x2, %ah
        mov $READBLOCKS, %al
	clc
        int $0x13

        jnc readsectors__success

	/* zero if sectors not readed */
	or %ah, %ah
	jnz readsectors__success

	mov $msg_kernel_read_failed, %si
	call print
	cli
	hlt

readsectors__success:
	ret

# =========================================================================== #
# Check that A20 Gate is enabled
#   output: set zero flag if disabled
#   restore: %ds %es
# =========================================================================== #

.code16
check_A20:
	push %ds
	push %es
#	push %ax
#	push %di
#	push %si

	xor %ax, %ax
	mov %ax, %es

	not %ax
	mov %ax, %ds

	mov $0x0500, %di
	mov $0x0510, %si

	mov %es:0(%di), %al
	push %ax

	mov %ds:0(%si), %al
	push %ax

	movb $0x00, %es:0(%di)
	movb $0xff, %ds:0(%si)

	cmpb $0xff, %es:0(%di)

	pop %ax
	mov %es:0(%di), %al

	pop %ax
	mov %es:0(%di), %al

	mov $0, %ax

	je check_A20__exit

	mov $1, %ax

check_A20__exit:
#	pop %si
#	pop %di
#	pop %ax
	pop %es
	pop %ds
	or %al, %al
	ret

# =========================================================================== #
# Enable A20 Gate
# =========================================================================== #

enable_A20:
	/* First check */
	call check_A20
	jnz enable_A20__success

	/* BIOS method */
	mov $0x2401, %ax
	int $0x15

	call check_A20
	jnz enable_A20__success

	/* Keyboard method 1 */
	mov $0xd0, %al
	out %al, $0x64

	call check_A20
	jnz enable_A20__success

	/* Keyboard method 2 */
/*
#	cli
	
	call enable_A20__wait1
	mov $0xad, %al
	out %al, $0x64

	call enable_A20__wait1
	mov $0xd0, %al
	out %al, $0x64

	enable_A20__wait2:
	in $0x64, %al
	test $1, %al
	jz enable_A20__wait2

	in $0x60, %al
	push %eax

	call enable_A20__wait1
	mov $0xd1, %al
	out %al, $0x64

	call enable_A20__wait1
	pop %eax
	or $2, %al
	out %al, $0x64

	call enable_A20__wait1
	mov $0xae, %al
	out %al, $0x64

	call enable_A20__wait1
#	sti

#	call check_A20
#	jnz enable_A20__success
*/

	/* Control port A method */
	mov $1, %al
	out %al, $0x92

	call check_A20
	jnz enable_A20__success

	/* Fast method */
	in $0xee, %al

	call check_A20
	jnz enable_A20__success

	mov $msg_a20_failed, %si
	call print
#	cli
#	hlt

enable_A20__success:
	ret

/*
enable_A20__wait1:
	in $0x64, %al
	test $2, %al
	jnz enable_A20__wait1
	ret
*/

# =========================================================================== #
# =========================================================================== #
# =========================================================================== #

msg_mapmem_failed: .asciz "1"
msg_a20_failed: .asciz "2"
msg_chs_error: .asciz "3"
msg_kernel_read_failed: .asciz "4"

msg_protected_mode: .asciz ">5"

current_sector:
.word 0
gdt_begin:
.byte 0x00, 0x00, 0, 0, 0, 0b00000000, 0b00000000, 0 # null dsc
.byte 0xff, 0xff, 0, 0, 0, 0b10011010, 0b11001111, 0 # code dsc
.byte 0xff, 0xff, 0, 0, 0, 0b10010010, 0b11001111, 0 # data dsc
gdt_info:
.word (gdt_info - gdt_begin - 1)
.quad (gdt_begin)

.fill _start + 510 - .
.word 0xaa55

kernel_code_start:
.incbin "../BlackBox.bin"
kernel_code_end:

.fill _start + (1440 * 1024) - .