/*
 * $Id: early_init.S,v 1.5 2009-07-29 07:15:36 vrsieh Exp $ 
 *
 * Copyright (C) 2009 FAUmachine Team <info@faumachine.org>.
 * This program is free software. You can redistribute it and/or modify it
 * under the terms of the GNU General Public License, either version 2 of
 * the License, or (at your option) any later version. See COPYING.
 */

/* FIXME */
#define HACK_FOR_KVM_INTEL	1

#include "build_config.h"
#include "libsys.h"
#include "compiler.h"

#define PCI_ADDR(bus, dev, func) \
        (((bus) << 16) | ((dev) << 11) | ((func) << 8))
#define PCICONF_ADDR    0x0cf8
#define PCICONF_DATA    0x0cfc


#ifdef INIT_EARLY
	.section .text.reset, "ax"
	/* .org 0xfffffff0 */
	.code16
_C_LABEL(bios_entry): .globl _C_LABEL(bios_entry)
	jmp early_init_entry

	.section .text
	.code16gcc
early_init_entry: .globl early_init_entry
	/*
	 * test warm reset
	 */
	movb $0x0f, %al /* read reset type */
	outb %al, $0x70
	inb $0x71, %al
	cmp $0x05, %al
	jnz start_init
	movb $0x0f, %al /* set reset type to default value */
	outb %al, $0x70
	movb $0x00, %al
	outb %al, $0x71

	ljmpw *(0x0467)

start_init:
	/*
	 * Switch to protected mode.
	 */
	movl $init_gdt, %eax
	andl $0xffff, %eax
	lgdtl %cs:(%eax)

	movl %cr0, %eax
	orl $1, %eax
	movl %eax, %cr0

	movw $16, %ax
	movw %ax, %ds
	movw %ax, %es

	/* Ugly -- FIXME */
	ljmpw $8, $(start_init2 - early_init_entry + 0xf000)

	// .code32
start_init2:
	/*
	 * Enable 1Mbyte BIOS.
	 */
	movl $PCI_ADDR(0, 7, 0) | 0x80000000 | 0x4c, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA + 2, %dx
	inw %dx
	orw $(1 << 9) | (1 << 7) | (1 << 6), %ax
	outw %ax, %dx

	/*
	 * Init Memory
	 *
	 * See 82443BX Host Bridge Datasheet.
	 */
	movw $0, %cx    /* Counter for 8 registers to set. */
	movl $0, %esi   /* Up to that limit memory is found. */
	movl $0, %edi   /* Up to that limit memory is tested. */

mem_init_loop:
	/* Set register of host bridge to support as much memory as */
	/* possible (0x80 * 8 MByte) to be able to access/test it... */
	/* reg(0x60 + %cx) <= 0x80 */
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x60, %eax
	orb %cl, %al
	andb $0xfc, %al
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw %cx, %dx
	andw $0x03, %dx
	orw $PCICONF_DATA, %dx
	movb $0x80, %al
	outb %al, %dx

mem_init_test:
	movb $0x00, (%edi)
	cmpb $0x00, (%edi)
	jne mem_end_found
	movb $0xff, (%edi)
	cmpb $0xff, (%edi)
	jne mem_end_found
	movb $0x55, (%edi)
	cmpb $0x55, (%edi)
	jne mem_end_found
	movb $0xaa, (%edi)
	cmpb $0xaa, (%edi)
	jne mem_end_found

	addl $8*1024*1024, %edi

	movb $0x00, (%esi)
	movb $0xff, (%edi)
	cmpb $0x00, (%esi)
	je mem_init_test

mem_end_found:
	movl %edi, %eax
	shrl $23, %eax

	/* PCICONF_ADDR already set. */
	/* %dx already set. */
	outb %al, %dx

	movl %edi, %esi
	addw $1, %cx
	cmpw $8, %cx
	jne mem_init_loop

	/*
	 * Copy BIOS INIT data to "vma_init_rm_data".
	 */
	movl $evma_init_rm_data, %ecx
	subl $vma_init_rm_data, %ecx
	movl $vma_init_rm_data, %edi
	movl $lma_init_rm_data, %esi
	rep movsb %ds:(%esi), %es:(%edi)

	/*
	 * Clear BIOS INIT bss at "vma_init_rm_bss".
	 */
	movl $evma_init_rm_bss, %ecx
	subl $vma_init_rm_bss, %ecx
	movl $vma_init_rm_bss, %edi
	movb $0, %al
	rep stosb %es:(%edi)

	/*
	 * Copy BIOS INIT part to 0x10000.
	 */
	movl $evma_init_rm, %ecx
	subl $vma_init_rm, %ecx
	movl $vma_init_rm, %edi
	movl $lma_init_rm, %esi
	rep movsb %ds:(%esi), %es:(%edi)

#ifdef CONFIG_SMI_SUPPORT
	/*
	 * Copy BIOS SMM redirect part to 0x38000.
	 */
	movl $evma_smi_redirect, %ecx
	subl $vma_smi_redirect, %ecx
	movl $vma_smi_redirect, %edi
	movl $lma_smi_redirect, %esi
	rep movsb %ds:(%esi), %es:(%edi)

	/*
	 * Open SMM space to be able to copy SMM part of BIOS to 0xa8000.
	 */
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x70, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA + 2, %dx
	movb $(1 << 6) | (1 << 3), %al
	outb %al, %dx

	/*
	 * Copy BIOS SMM part to 0xa8000.
	 */
	movl $evma_smi, %ecx
	subl $vma_smi, %ecx
	movl $vma_smi, %edi
	movl $lma_smi, %esi
	rep movsb %ds:(%esi), %es:(%edi)

	/*
	 * Lock SMM space.
	 */
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x70, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA + 2, %dx
	movb $(1 << 4) | (1 << 3), %al
	outb %al, %dx
#endif /* CONFIG_SMI_SUPPORT */

	/*
	 * Set PAM registers to be able to copy RUNTIME part of
	 * BIOS to 0xe0000-0xfffff.
	 */
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x58, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA, %dx
	movl $0x00003000, %eax
	outl %eax, %dx

	/*
	 * Set PAM registers to be able to copy RUNTIME part of
	 * BIOS to 0xe0000-0xfffff.
	 */
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x58, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA, %dx
	movl $0x00003000, %eax
	outl %eax, %dx
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x5c, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA, %dx
	movl $0x33330000, %eax
	outl %eax, %dx

	/*
	 * Copy BIOS RUNTIME part to 0xe0000-0xfffff.
	 */
	movl $evma_rt_legacy, %ecx
	subl $vma_rt_legacy, %ecx
	movl $vma_rt_legacy, %edi
	movl $lma_rt_legacy, %esi
	rep movsb %ds:(%esi), %es:(%edi)

	/*
	 * Set PAM registers to make BIOS RUNTIME part read-only.
	 */
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x58, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA, %dx
	inl %dx, %eax
	andl $0xfffffbff, %eax  /* WE bit of range 0xf0000-0xfffff. */
	outl %eax, %dx
	movl $PCI_ADDR(0, 0, 0) | 0x80000000 | 0x5c, %eax
	movw $PCICONF_ADDR, %dx
	outl %eax, %dx
	movw $PCICONF_DATA, %dx
	inl %dx, %eax
	andl $0xbbbbffff, %eax  /* WE bits of range 0xe0000-0xeffff. */
	outl %eax, %dx

	/*
	 * Switch back to real-mode.
	 */
#ifdef HACK_FOR_KVM_INTEL
	movw $0xfffe, %dx
	movb $42, %al
	outb %al, %dx
#endif
	movl %cr0, %eax
	andl $~1, %eax
	movl %eax, %cr0

	/*
	 * Jump to INIT part of BIOS.
	 * This changes %cs segment.
	 */
	ex_ljmp init_rm_bsp

	.section .rodata
init_gdt:
	/* Null descriptor/GDT descriptor */
	.word 3 * 8 - 1
	.long init_gdt
	.word 0

	/* Code descriptor */
	.word 0xffff		/* Limit 15:0 */
	.word 0x0000		/* Base 15:0 */
	.word (1 << 15)		/* Present */ \
	    | (0 << 13)		/* DPL */ \
	    | (1 << 12)		/* Code/Data */ \
	    | (0xb << 8)	/* Code+Accessed */ \
	    | 0xff		/* Base 23:16 */
	.word (0xff << (24-16))	/* Base 31:24 */ \
	    | (1 << (23-16))	/* Granularity */ \
	    | (0 << (22-16))	/* 16bit Code */ \
	    | (0 << (21-16))	/* Unused */ \
	    | (0 << (20-16))	/* Available */ \
	    | (0xf << (16-16))	/* Limit 19:16 */

	/* Data descriptor */
	.word 0xffff		/* Limit 15:0 */
	.word 0x0000		/* Base 15:0 */
	.word (1 << 15)		/* Present */ \
	    | (0 << 13)		/* DPL */ \
	    | (1 << 12)		/* Code/Data */ \
	    | (0x3 << 8)	/* Data+Accessed */ \
	    | 0x00		/* Base 23:16 */
	.word (0x00 << (24-16))	/* Base 31:24 */ \
	    | (1 << (23-16))	/* Granularity */ \
	    | (0 << (22-16))	/* 16bit Data */ \
	    | (0 << (21-16))	/* Unused */ \
	    | (0 << (20-16))	/* Available */ \
	    | (0xf << (16-16))	/* Limit 19:16 */

#endif /* INIT_EARLY */