Index: vm86.c
===================================================================
--- vm86.c	(revision 486)
+++ vm86.c	(working copy)
@@ -5,6 +5,7 @@
  *  Copyright (C) 2001 University of California.  LA-CC Number 01-67.
  *  Copyright (C) 2005 Nick.Barker9@btinternet.com
  *  Copyright (C) 2007 coresystems GmbH
+ *  Copyright (C) 2007 Siqon Ltd. (written by Alex Beregszaszi)
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -98,7 +99,7 @@
 );
 
 /* The address arguments to this function are PHYSICAL ADDRESSES */ 
-static void real_mode_switch_call_vga(unsigned long devfn)
+static void real_mode_switch_call_vga(unsigned long devfn, unsigned long addr)
 {
 	__asm__ __volatile__ (
 		/* paranoia -- does ecx get saved? not sure. 
@@ -106,16 +107,12 @@
 		 */
 		"	pushal			\n"
 		/* save the stack */
-		"	mov 	%esp, __stack	\n"
+		"	mov 	%%esp, __stack	\n"
 		"	jmp 	1f		\n"
 		"__stack: .long 0		\n"
 		"1:\n"
-		/* get devfn into %ecx */
-		"	movl    %esp, %ebp	\n"
-		// FIXME: why is this 8? 
-		"	movl    8(%ebp), %ecx	\n"
 		/* load 'our' gdt */
-		"	lgdt	%cs:__mygdtaddr	\n"
+		"	lgdt	%%cs:__mygdtaddr	\n"
 
 		/*  This configures CS properly for real mode. */
 		"	ljmp	$0x28, $__rms_16bit\n"
@@ -128,17 +125,17 @@
 		 * configurations (limits, writability, etc.) once 
 		 * protected mode is turned off. 
 		 */
-		"	mov	$0x30, %ax	\n"
-		"	mov	%ax, %ds       	\n"
-		"	mov	%ax, %es       	\n"
-		"	mov	%ax, %fs       	\n"
-		"	mov	%ax, %gs       	\n"
-		"	mov	%ax, %ss       	\n"
+		"	mov	$0x30, %%ax	\n"
+		"	mov	%%ax, %%ds       	\n"
+		"	mov	%%ax, %%es       	\n"
+		"	mov	%%ax, %%fs       	\n"
+		"	mov	%%ax, %%gs       	\n"
+		"	mov	%%ax, %%ss       	\n"
 
 		/* Turn off protection (bit 0 in CR0) */
-		"	movl	%cr0, %eax	\n"
-		"	andl	$0xFFFFFFFE, %eax \n"
-		"	movl	%eax, %cr0	\n"
+		"	movl	%%cr0, %%eax	\n"
+		"	andl	$0xFFFFFFFE, %%eax \n"
+		"	movl	%%eax, %%cr0	\n"
 
 		/* Now really going into real mode */
 		"	ljmp	$0,  $__rms_real\n"
@@ -148,33 +145,49 @@
 		 * That way we can easily share it between real and
 		 * protected, since the 16-bit ESP at segment 0 will 
 		 * work for any case. */
-		"	mov	$0x0, %ax	\n"
-		"	mov	%ax, %ss	\n"
-		"	movl	$0x1000, %eax	\n"
-		"	movl	%eax, %esp	\n"
+		"	mov	$0x0, %%ax	\n"
+		"	mov	%%ax, %%ss	\n"
+		"	movl	$0x1000, %%eax	\n"
+		"	movl	%%eax, %%esp	\n"
 
 		/* Load our 16 it idt */
-		"	xor	%ax, %ax	\n"
-		"	mov	%ax, %ds	\n"
+		"	xor	%%ax, %%ax	\n"
+		"	mov	%%ax, %%ds	\n"
 		"	lidt	__myidt		\n"
 
 		/* Dump zeros in the other segment registers */
-		"	mov	%ax, %es       	\n"
-		"	mov	%ax, %fs       	\n"
-		"	mov	%ax, %gs       	\n"
-		"	mov	$0x40, %ax	\n"
-		"	mov	%ax, %ds	\n"
-		"	mov	%cx, %ax	\n"
+		"	mov	%%ax, %%es       	\n"
+		"	mov	%%ax, %%fs       	\n"
+		"	mov	%%ax, %%gs       	\n"
+		"	mov	$0x40, %%ax	\n"
+		"	mov	%%ax, %%ds	\n"
+		"	mov	%%cx, %%ax	\n"
 
 		/* run VGA BIOS at 0xc000:0003 */
-		"	lcall	$0xc000, $0x0003\n"
+/*		"	lcall	$0xc000, $0x0003   \n"*/
 
+		/* calculate addr */
+		"       mov     %%edx, %%ebx      \n"
+		"       add     $3, %%ebx        \n"
+		"       and     $0xffff, %%ebx   \n"
+		/* calculate segment */
+		"       and     $0xf0000, %%edx  \n"
+		"       shr     $4, %%edx        \n"
+
+		"       pushw   %%dx             \n"
+		"       pushw   %%bx             \n"
+		"       mov     %%esp, %%ebp      \n"
+		/* idea from bochs bios' rom_scan */
+		/* call far ss:[bp+0] */
+		"       .byte 0xff, 0x5e, 0x00   \n"
+		"       add     $4, %%esp         \n"
+
 		/* If we got here, just about done. 
 		 * Need to get back to protected mode 
 		 */
-		"	movl	%cr0, %eax	\n"
-		"	orl	$0x0000001, %eax\n" /* PE = 1 */
-		"	movl	%eax, %cr0	\n"
+		"	movl	%%cr0, %%eax	\n"
+		"	orl	$0x0000001, %%eax\n" /* PE = 1 */
+		"	movl	%%eax, %%cr0	\n"
 
 		/* Now that we are in protected mode 
 		 * jump to a 32 bit code segment. 
@@ -182,21 +195,23 @@
 		"	data32	ljmp	$0x10, $vgarestart\n"
 		"vgarestart:\n"
 		"	.code32\n"
-		"	movw	$0x18, %ax     	\n"
-		"	mov	%ax, %ds       	\n"
-		"	mov	%ax, %es	\n"
-		"	mov	%ax, %fs	\n"
-		"	mov	%ax, %gs	\n"
-		"	mov	%ax, %ss	\n"
+		"	movw	$0x18, %%ax     	\n"
+		"	mov	%%ax, %%ds       	\n"
+		"	mov	%%ax, %%es	\n"
+		"	mov	%%ax, %%fs	\n"
+		"	mov	%%ax, %%gs	\n"
+		"	mov	%%ax, %%ss	\n"
 
 		/* restore proper gdt and idt */
-		"	lgdt	%cs:gdtarg	\n"
+		"	lgdt	%%cs:gdtarg	\n"
 		"	lidt	idtarg		\n"
 
 		".globl vga_exit		\n"
 		"vga_exit:			\n"
-		"	mov	__stack, %esp	\n"
+		"	mov	__stack, %%esp	\n"
 		"	popal			\n"
+		:
+		: "c" (devfn), "d" (addr)
 		);
 }
 
@@ -307,55 +322,16 @@
 		);
 }
 
-void do_vgabios(void)
+void run_bios(struct device *dev, unsigned long addr)
 {
-	struct device *dev;
-	unsigned int busdevfn;
-	unsigned long rom = 0;
-	unsigned char *buf;
-	unsigned int size = 64*1024;
 	int i;
 	
 	/* clear vga bios data area */
 	for (i = 0x400; i < 0x500; i++) {
 		*(unsigned char *) i = 0;
 	}
-
-	dev = dev_find_class(PCI_CLASS_DISPLAY_VGA<<8 , 0);
-
-	if (!dev) {
-		printk(BIOS_DEBUG, "NO VGA FOUND\n");
-		return;
-	}
-	printk(BIOS_DEBUG,"found VGA: vid=%x, did=%x\n", dev->vendor, dev->device);
-
-	/* declare rom address here - keep any config data out of the way
-	 * of core LXB stuff */
-#warning fix rom address
-	rom = 0xc0000;
-	pci_write_config32(dev, PCI_ROM_ADDRESS, rom|1);
-	printk(BIOS_DEBUG, "rom base, size: %p\n", (void *)rom);
-
-	buf = (unsigned char *) rom;
-	if ((buf[0] == 0x55) && (buf[1] == 0xaa)) {
-		memcpy((void *) 0xc0000, buf, size);
-
-#warning Implement write_protect_vgabios()
-		//write_protect_vgabios();  // in northbridge
-
-	  	// check signature again
-		buf = (unsigned char *) 0xc0000;
-		if (buf[0]==0x55 && buf[1]==0xAA) {
-			busdevfn = (dev->bus->secondary << 8) | dev->path.u.pci.devfn;
-			printk(BIOS_DEBUG, "bus/devfn = %#x\n", busdevfn);
-
-		    	real_mode_switch_call_vga(busdevfn);
-		} else
-			printk(BIOS_DEBUG, "Failed to copy VGA BIOS to 0xc0000\n");
-	} else 
-		printk(BIOS_DEBUG, "BAD SIGNATURE 0x%x 0x%x\n", buf[0], buf[1]);
-
-	pci_write_config32(dev, PCI_ROM_ADDRESS, 0);
+	
+	real_mode_switch_call_vga((dev->bus->secondary << 8) | dev->path.u.pci.devfn, addr);
 }
 
 
