Merge commit 'upstream/master'

80 files changed, 23932 insertions, 464 deletions

diff --git a/drivers/Makefile b/drivers/Makefile
index c7fcc3f23e5..fc980407623 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -27,11 +27,11 @@ include $(TOPDIR)/config.mk
 
 LIB	= $(obj)libdrivers.a
 
-COBJS	= 3c589.o 5701rls.o ali512x.o atmel_usart.o \
+COBJS	= 3c589.o 5701rls.o ali512x.o ata_piix.o atmel_usart.o \
 	  bcm570x.o bcm570x_autoneg.o cfb_console.o cfi_flash.o \
 	  cs8900.o ct69000.o dataflash.o dc2114x.o dm9000x.o \
 	  e1000.o eepro100.o enc28j60.o \
-	  i8042.o inca-ip_sw.o keyboard.o \
+	  i8042.o inca-ip_sw.o isp116x-hcd.o keyboard.o \
 	  lan91c96.o macb.o \
 	  natsemi.o ne2000.o netarm_eth.o netconsole.o \
 	  ns16550.o ns8382x.o ns87308.o ns7520_eth.o omap1510_i2c.o \
@@ -47,12 +47,14 @@ COBJS	= 3c589.o 5701rls.o ali512x.o atmel_usart.o \
 	  status_led.o sym53c8xx.o systemace.o ahci.o \
 	  ti_pci1410a.o tigon3.o tsec.o \
 	  tsi108_eth.o tsi108_i2c.o tsi108_pci.o \
-	  usbdcore.o usbdcore_ep0.o usbdcore_omap1510.o usbtty.o \
+	  usb_ohci.o \
+	  usbdcore.o usbdcore_ep0.o usbdcore_mpc8xx.o usbdcore_omap1510.o \
+	  usbtty.o \
 	  videomodes.o w83c553f.o \
 	  ks8695eth.o \
 	  pxa_pcmcia.o mpc8xx_pcmcia.o tqm8xx_pcmcia.o	\
 	  rpx_pcmcia.o \
-	  fsl_i2c.o
+	  fsl_i2c.o fsl_pci_init.o ati_radeon_fb.o
 
 SRCS	:= $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(COBJS))
diff --git a/drivers/ahci.c b/drivers/ahci.c
index 8ceff00925f..ccd4d71e8ae 100644
--- a/drivers/ahci.c
+++ b/drivers/ahci.c
@@ -253,7 +253,8 @@ static void ahci_print_info(struct ahci_probe_ent *probe_ent)
 
 static int ahci_init_one(pci_dev_t pdev)
 {
-	u32 iobase, vendor;
+	u32 iobase;
+	u16 vendor;
 	int rc;
 
 	memset((void *)ataid, 0, sizeof(hd_driveid_t *) * AHCI_MAX_PORTS);
diff --git a/drivers/ata_piix.c b/drivers/ata_piix.c
new file mode 100644
index 00000000000..42456d7be33
--- /dev/null
+++ b/drivers/ata_piix.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) Procsys. All rights reserved.
+ * Author: Mushtaq Khan <mushtaq_k@procsys.com>
+ *			<mushtaqk_921@yahoo.co.in>
+ *
+ * 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 the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ * with the reference to ata_piix driver in kernel 2.4.32
+ */
+
+/*
+ * This file contains SATA controller and SATA drive initialization functions
+ */
+
+#include <common.h>
+#include <pci.h>
+#include <command.h>
+#include <config.h>
+#include <asm/byteorder.h>
+#include <ide.h>
+#include <ata.h>
+
+#ifdef CFG_ATA_PIIX		/*ata_piix driver */
+
+#define DEBUG_SATA 0		/*For debug prints set DEBUG_SATA to 1 */
+
+#define DRV_DECL		/*For file specific declarations */
+#include <sata.h>
+#undef DRV_DECL
+
+/*Macros realted to PCI*/
+#define PCI_SATA_BUS	0x00
+#define PCI_SATA_DEV	0x1f
+#define PCI_SATA_FUNC	0x02
+
+#define PCI_SATA_BASE1 0x10
+#define PCI_SATA_BASE2 0x14
+#define PCI_SATA_BASE3 0x18
+#define PCI_SATA_BASE4 0x1c
+#define PCI_SATA_BASE5 0x20
+#define PCI_PMR         0x90
+#define PCI_PI          0x09
+#define PCI_PCS         0x92
+#define PCI_DMA_CTL     0x48
+
+#define PORT_PRESENT (1<<0)
+#define PORT_ENABLED (1<<4)
+
+u32 bdf;
+u32 iobase1 = 0;		/*Primary cmd block */
+u32 iobase2 = 0;		/*Primary ctl block */
+u32 iobase3 = 0;		/*Sec cmd block */
+u32 iobase4 = 0;		/*sec ctl block */
+u32 iobase5 = 0;		/*BMDMA*/
+int
+pci_sata_init (void)
+{
+	u32 bus = PCI_SATA_BUS;
+	u32 dev = PCI_SATA_DEV;
+	u32 fun = PCI_SATA_FUNC;
+	u16 cmd = 0;
+	u8 lat = 0, pcibios_max_latency = 0xff;
+	u8 pmr;			/*Port mapping reg */
+	u8 pi;			/*Prgming Interface reg */
+
+	bdf = PCI_BDF (bus, dev, fun);
+	pci_read_config_dword (bdf, PCI_SATA_BASE1, &iobase1);
+	pci_read_config_dword (bdf, PCI_SATA_BASE2, &iobase2);
+	pci_read_config_dword (bdf, PCI_SATA_BASE3, &iobase3);
+	pci_read_config_dword (bdf, PCI_SATA_BASE4, &iobase4);
+	pci_read_config_dword (bdf, PCI_SATA_BASE5, &iobase5);
+
+	if ((iobase1 == 0xFFFFFFFF) || (iobase2 == 0xFFFFFFFF) ||
+	    (iobase3 == 0xFFFFFFFF) || (iobase4 == 0xFFFFFFFF) ||
+	    (iobase5 == 0xFFFFFFFF)) {
+		printf ("error no base addr for SATA controller\n");
+		return 1;
+	 /*ERROR*/}
+
+	iobase1 &= 0xFFFFFFFE;
+	iobase2 &= 0xFFFFFFFE;
+	iobase3 &= 0xFFFFFFFE;
+	iobase4 &= 0xFFFFFFFE;
+	iobase5 &= 0xFFFFFFFE;
+
+	/*check for mode */
+	pci_read_config_byte (bdf, PCI_PMR, &pmr);
+	if (pmr > 1) {
+		printf ("combined mode not supported\n");
+		return 1;
+	}
+
+	pci_read_config_byte (bdf, PCI_PI, &pi);
+	if ((pi & 0x05) != 0x05) {
+		printf ("Sata is in Legacy mode\n");
+		return 1;
+	} else {
+		printf ("sata is in Native mode\n");
+	}
+
+	/*MASTER CFG AND IO CFG */
+	pci_read_config_word (bdf, PCI_COMMAND, &cmd);
+	cmd |= PCI_COMMAND_MASTER | PCI_COMMAND_IO;
+	pci_write_config_word (bdf, PCI_COMMAND, cmd);
+	pci_read_config_byte (dev, PCI_LATENCY_TIMER, &lat);
+
+	if (lat < 16)
+		lat = (64 <= pcibios_max_latency) ? 64 : pcibios_max_latency;
+	else if (lat > pcibios_max_latency)
+		lat = pcibios_max_latency;
+	pci_write_config_byte (dev, PCI_LATENCY_TIMER, lat);
+
+	return 0;
+}
+
+int
+sata_bus_probe (int port_no)
+{
+	int orig_mask, mask;
+	u16 pcs;
+
+	mask = (PORT_PRESENT << port_no);
+	pci_read_config_word (bdf, PCI_PCS, &pcs);
+	orig_mask = (int) pcs & 0xff;
+	if ((orig_mask & mask) != mask)
+		return 0;
+	else
+		return 1;
+}
+
+int
+init_sata (void)
+{
+	u8 i, rv = 0;
+
+	for (i = 0; i < CFG_SATA_MAXDEVICES; i++) {
+		sata_dev_desc[i].type = DEV_TYPE_UNKNOWN;
+		sata_dev_desc[i].if_type = IF_TYPE_IDE;
+		sata_dev_desc[i].dev = i;
+		sata_dev_desc[i].part_type = PART_TYPE_UNKNOWN;
+		sata_dev_desc[i].blksz = 0;
+		sata_dev_desc[i].lba = 0;
+		sata_dev_desc[i].block_read = sata_read;
+	}
+
+	rv = pci_sata_init ();
+	if (rv == 1) {
+		printf ("pci initialization failed\n");
+		return 1;
+	}
+
+	port[0].port_no = 0;
+	port[0].ioaddr.cmd_addr = iobase1;
+	port[0].ioaddr.altstatus_addr = port[0].ioaddr.ctl_addr =
+	    iobase2 | ATA_PCI_CTL_OFS;
+	port[0].ioaddr.bmdma_addr = iobase5;
+
+	port[1].port_no = 1;
+	port[1].ioaddr.cmd_addr = iobase3;
+	port[1].ioaddr.altstatus_addr = port[1].ioaddr.ctl_addr =
+	    iobase4 | ATA_PCI_CTL_OFS;
+	port[1].ioaddr.bmdma_addr = iobase5 + 0x8;
+
+	for (i = 0; i < CFG_SATA_MAXBUS; i++)
+		sata_port (&port[i].ioaddr);
+
+	for (i = 0; i < CFG_SATA_MAXBUS; i++) {
+		if (!(sata_bus_probe (i))) {
+			port[i].port_state = 0;
+			printf ("SATA#%d port is not present \n", i);
+		} else {
+			printf ("SATA#%d port is present\n", i);
+			if (sata_bus_softreset (i)) {
+				port[i].port_state = 0;
+			} else {
+				port[i].port_state = 1;
+			}
+		}
+	}
+
+	for (i = 0; i < CFG_SATA_MAXBUS; i++) {
+		u8 j, devno;
+
+		if (port[i].port_state == 0)
+			continue;
+		for (j = 0; j < CFG_SATA_DEVS_PER_BUS; j++) {
+			sata_identify (i, j);
+			set_Feature_cmd (i, j);
+			devno = i * CFG_SATA_DEVS_PER_BUS + j;
+			if ((sata_dev_desc[devno].lba > 0) &&
+			    (sata_dev_desc[devno].blksz > 0)) {
+				dev_print (&sata_dev_desc[devno]);
+				/* initialize partition type */
+				init_part (&sata_dev_desc[devno]);
+				if (curr_dev < 0)
+					curr_dev =
+					    i * CFG_SATA_DEVS_PER_BUS + j;
+			}
+		}
+	}
+	return 0;
+}
+#endif
diff --git a/drivers/ati_ids.h b/drivers/ati_ids.h
new file mode 100644
index 00000000000..3e72a7dd4c0
--- /dev/null
+++ b/drivers/ati_ids.h
@@ -0,0 +1,211 @@
+/*
+ * ATI PCI IDs from XFree86, kept here to make sync'ing with
+ * XFree much simpler. Currently, this list is only used by
+ * radeonfb
+ */
+
+#define PCI_CHIP_RV380_3150             0x3150
+#define PCI_CHIP_RV380_3151             0x3151
+#define PCI_CHIP_RV380_3152             0x3152
+#define PCI_CHIP_RV380_3153             0x3153
+#define PCI_CHIP_RV380_3154             0x3154
+#define PCI_CHIP_RV380_3156             0x3156
+#define PCI_CHIP_RV380_3E50             0x3E50
+#define PCI_CHIP_RV380_3E51             0x3E51
+#define PCI_CHIP_RV380_3E52             0x3E52
+#define PCI_CHIP_RV380_3E53             0x3E53
+#define PCI_CHIP_RV380_3E54             0x3E54
+#define PCI_CHIP_RV380_3E56             0x3E56
+#define PCI_CHIP_RS100_4136		0x4136
+#define PCI_CHIP_RS200_4137		0x4137
+#define PCI_CHIP_R300_AD		0x4144
+#define PCI_CHIP_R300_AE		0x4145
+#define PCI_CHIP_R300_AF		0x4146
+#define PCI_CHIP_R300_AG		0x4147
+#define PCI_CHIP_R350_AH                0x4148
+#define PCI_CHIP_R350_AI                0x4149
+#define PCI_CHIP_R350_AJ                0x414A
+#define PCI_CHIP_R350_AK                0x414B
+#define PCI_CHIP_RV350_AP               0x4150
+#define PCI_CHIP_RV350_AQ               0x4151
+#define PCI_CHIP_RV360_AR               0x4152
+#define PCI_CHIP_RV350_AS               0x4153
+#define PCI_CHIP_RV350_AT               0x4154
+#define PCI_CHIP_RV350_AV               0x4156
+#define PCI_CHIP_MACH32		0x4158
+#define PCI_CHIP_RS250_4237		0x4237
+#define PCI_CHIP_R200_BB		0x4242
+#define PCI_CHIP_R200_BC		0x4243
+#define PCI_CHIP_RS100_4336		0x4336
+#define PCI_CHIP_RS200_4337		0x4337
+#define PCI_CHIP_MACH64CT		0x4354
+#define PCI_CHIP_MACH64CX		0x4358
+#define PCI_CHIP_RS250_4437		0x4437
+#define PCI_CHIP_MACH64ET		0x4554
+#define PCI_CHIP_MACH64GB		0x4742
+#define PCI_CHIP_MACH64GD		0x4744
+#define PCI_CHIP_MACH64GI		0x4749
+#define PCI_CHIP_MACH64GL		0x474C
+#define PCI_CHIP_MACH64GM		0x474D
+#define PCI_CHIP_MACH64GN		0x474E
+#define PCI_CHIP_MACH64GO		0x474F
+#define PCI_CHIP_MACH64GP		0x4750
+#define PCI_CHIP_MACH64GQ		0x4751
+#define PCI_CHIP_MACH64GR		0x4752
+#define PCI_CHIP_MACH64GS		0x4753
+#define PCI_CHIP_MACH64GT		0x4754
+#define PCI_CHIP_MACH64GU		0x4755
+#define PCI_CHIP_MACH64GV		0x4756
+#define PCI_CHIP_MACH64GW		0x4757
+#define PCI_CHIP_MACH64GX		0x4758
+#define PCI_CHIP_MACH64GY		0x4759
+#define PCI_CHIP_MACH64GZ		0x475A
+#define PCI_CHIP_RV250_Id		0x4964
+#define PCI_CHIP_RV250_Ie		0x4965
+#define PCI_CHIP_RV250_If		0x4966
+#define PCI_CHIP_RV250_Ig		0x4967
+#define PCI_CHIP_R420_JH                0x4A48
+#define PCI_CHIP_R420_JI                0x4A49
+#define PCI_CHIP_R420_JJ                0x4A4A
+#define PCI_CHIP_R420_JK                0x4A4B
+#define PCI_CHIP_R420_JL                0x4A4C
+#define PCI_CHIP_R420_JM                0x4A4D
+#define PCI_CHIP_R420_JN                0x4A4E
+#define PCI_CHIP_R420_JP                0x4A50
+#define PCI_CHIP_MACH64LB		0x4C42
+#define PCI_CHIP_MACH64LD		0x4C44
+#define PCI_CHIP_RAGE128LE		0x4C45
+#define PCI_CHIP_RAGE128LF		0x4C46
+#define PCI_CHIP_MACH64LG		0x4C47
+#define PCI_CHIP_MACH64LI		0x4C49
+#define PCI_CHIP_MACH64LM		0x4C4D
+#define PCI_CHIP_MACH64LN		0x4C4E
+#define PCI_CHIP_MACH64LP		0x4C50
+#define PCI_CHIP_MACH64LQ		0x4C51
+#define PCI_CHIP_MACH64LR		0x4C52
+#define PCI_CHIP_MACH64LS		0x4C53
+#define PCI_CHIP_MACH64LT		0x4C54
+#define PCI_CHIP_RADEON_LW		0x4C57
+#define PCI_CHIP_RADEON_LX		0x4C58
+#define PCI_CHIP_RADEON_LY		0x4C59
+#define PCI_CHIP_RADEON_LZ		0x4C5A
+#define PCI_CHIP_RV250_Ld		0x4C64
+#define PCI_CHIP_RV250_Le		0x4C65
+#define PCI_CHIP_RV250_Lf		0x4C66
+#define PCI_CHIP_RV250_Lg		0x4C67
+#define PCI_CHIP_RV250_Ln		0x4C6E
+#define PCI_CHIP_RAGE128MF		0x4D46
+#define PCI_CHIP_RAGE128ML		0x4D4C
+#define PCI_CHIP_R300_ND		0x4E44
+#define PCI_CHIP_R300_NE		0x4E45
+#define PCI_CHIP_R300_NF		0x4E46
+#define PCI_CHIP_R300_NG		0x4E47
+#define PCI_CHIP_R350_NH                0x4E48
+#define PCI_CHIP_R350_NI                0x4E49
+#define PCI_CHIP_R360_NJ                0x4E4A
+#define PCI_CHIP_R350_NK                0x4E4B
+#define PCI_CHIP_RV350_NP               0x4E50
+#define PCI_CHIP_RV350_NQ               0x4E51
+#define PCI_CHIP_RV350_NR               0x4E52
+#define PCI_CHIP_RV350_NS               0x4E53
+#define PCI_CHIP_RV350_NT               0x4E54
+#define PCI_CHIP_RV350_NV               0x4E56
+#define PCI_CHIP_RAGE128PA		0x5041
+#define PCI_CHIP_RAGE128PB		0x5042
+#define PCI_CHIP_RAGE128PC		0x5043
+#define PCI_CHIP_RAGE128PD		0x5044
+#define PCI_CHIP_RAGE128PE		0x5045
+#define PCI_CHIP_RAGE128PF		0x5046
+#define PCI_CHIP_RAGE128PG		0x5047
+#define PCI_CHIP_RAGE128PH		0x5048
+#define PCI_CHIP_RAGE128PI		0x5049
+#define PCI_CHIP_RAGE128PJ		0x504A
+#define PCI_CHIP_RAGE128PK		0x504B
+#define PCI_CHIP_RAGE128PL		0x504C
+#define PCI_CHIP_RAGE128PM		0x504D
+#define PCI_CHIP_RAGE128PN		0x504E
+#define PCI_CHIP_RAGE128PO		0x504F
+#define PCI_CHIP_RAGE128PP		0x5050
+#define PCI_CHIP_RAGE128PQ		0x5051
+#define PCI_CHIP_RAGE128PR		0x5052
+#define PCI_CHIP_RAGE128PS		0x5053
+#define PCI_CHIP_RAGE128PT		0x5054
+#define PCI_CHIP_RAGE128PU		0x5055
+#define PCI_CHIP_RAGE128PV		0x5056
+#define PCI_CHIP_RAGE128PW		0x5057
+#define PCI_CHIP_RAGE128PX		0x5058
+#define PCI_CHIP_RADEON_QD		0x5144
+#define PCI_CHIP_RADEON_QE		0x5145
+#define PCI_CHIP_RADEON_QF		0x5146
+#define PCI_CHIP_RADEON_QG		0x5147
+#define PCI_CHIP_R200_QH		0x5148
+#define PCI_CHIP_R200_QI		0x5149
+#define PCI_CHIP_R200_QJ		0x514A
+#define PCI_CHIP_R200_QK		0x514B
+#define PCI_CHIP_R200_QL		0x514C
+#define PCI_CHIP_R200_QM		0x514D
+#define PCI_CHIP_R200_QN		0x514E
+#define PCI_CHIP_R200_QO		0x514F
+#define PCI_CHIP_RV200_QW		0x5157
+#define PCI_CHIP_RV200_QX		0x5158
+#define PCI_CHIP_RV100_QY		0x5159
+#define PCI_CHIP_RV100_QZ		0x515A
+#define PCI_CHIP_RN50			0x515E
+#define PCI_CHIP_RAGE128RE		0x5245
+#define PCI_CHIP_RAGE128RF		0x5246
+#define PCI_CHIP_RAGE128RG		0x5247
+#define PCI_CHIP_RAGE128RK		0x524B
+#define PCI_CHIP_RAGE128RL		0x524C
+#define PCI_CHIP_RAGE128SE		0x5345
+#define PCI_CHIP_RAGE128SF		0x5346
+#define PCI_CHIP_RAGE128SG		0x5347
+#define PCI_CHIP_RAGE128SH		0x5348
+#define PCI_CHIP_RAGE128SK		0x534B
+#define PCI_CHIP_RAGE128SL		0x534C
+#define PCI_CHIP_RAGE128SM		0x534D
+#define PCI_CHIP_RAGE128SN		0x534E
+#define PCI_CHIP_RAGE128TF		0x5446
+#define PCI_CHIP_RAGE128TL		0x544C
+#define PCI_CHIP_RAGE128TR		0x5452
+#define PCI_CHIP_RAGE128TS		0x5453
+#define PCI_CHIP_RAGE128TT		0x5454
+#define PCI_CHIP_RAGE128TU		0x5455
+#define PCI_CHIP_RV370_5460             0x5460
+#define PCI_CHIP_RV370_5461             0x5461
+#define PCI_CHIP_RV370_5462             0x5462
+#define PCI_CHIP_RV370_5463             0x5463
+#define PCI_CHIP_RV370_5464             0x5464
+#define PCI_CHIP_RV370_5465             0x5465
+#define PCI_CHIP_RV370_5466             0x5466
+#define PCI_CHIP_RV370_5467             0x5467
+#define PCI_CHIP_R423_UH                0x5548
+#define PCI_CHIP_R423_UI                0x5549
+#define PCI_CHIP_R423_UJ                0x554A
+#define PCI_CHIP_R423_UK                0x554B
+#define PCI_CHIP_R423_UQ                0x5551
+#define PCI_CHIP_R423_UR                0x5552
+#define PCI_CHIP_R423_UT                0x5554
+#define PCI_CHIP_MACH64VT		0x5654
+#define PCI_CHIP_MACH64VU		0x5655
+#define PCI_CHIP_MACH64VV		0x5656
+#define PCI_CHIP_RS300_5834		0x5834
+#define PCI_CHIP_RS300_5835		0x5835
+#define PCI_CHIP_RS300_5836		0x5836
+#define PCI_CHIP_RS300_5837		0x5837
+#define PCI_CHIP_RV370_5B60             0x5B60
+#define PCI_CHIP_RV370_5B61             0x5B61
+#define PCI_CHIP_RV370_5B62             0x5B62
+#define PCI_CHIP_RV370_5B63             0x5B63
+#define PCI_CHIP_RV370_5B64             0x5B64
+#define PCI_CHIP_RV370_5B65             0x5B65
+#define PCI_CHIP_RV370_5B66             0x5B66
+#define PCI_CHIP_RV370_5B67             0x5B67
+#define PCI_CHIP_RV280_5960		0x5960
+#define PCI_CHIP_RV280_5961		0x5961
+#define PCI_CHIP_RV280_5962		0x5962
+#define PCI_CHIP_RV280_5964		0x5964
+#define PCI_CHIP_RV280_5C61		0x5C61
+#define PCI_CHIP_RV280_5C63		0x5C63
+#define PCI_CHIP_R423_5D57              0x5D57
+#define PCI_CHIP_RS350_7834             0x7834
+#define PCI_CHIP_RS350_7835             0x7835
diff --git a/drivers/ati_radeon_fb.c b/drivers/ati_radeon_fb.c
new file mode 100644
index 00000000000..c174f37b3fa
--- /dev/null
+++ b/drivers/ati_radeon_fb.c
@@ -0,0 +1,486 @@
+/*
+ * ATI Radeon Video card Framebuffer driver.
+ *
+ * Copyright 2007 Freescale Semiconductor, Inc.
+ * Zhang Wei <wei.zhang@freescale.com>
+ * Jason Jin <jason.jin@freescale.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * 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 the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ * Some codes of this file is partly ported from Linux kernel
+ * ATI video framebuffer driver.
+ *
+ * Now the driver is tested on below ATI chips:
+ *   9200
+ *   X300
+ *   X700
+ *
+ */
+
+#include <common.h>
+
+#ifdef CONFIG_ATI_RADEON_FB
+
+#include <command.h>
+#include <pci.h>
+#include <asm/processor.h>
+#include <asm/errno.h>
+#include <asm/io.h>
+#include <malloc.h>
+#include <video_fb.h>
+
+#include <radeon.h>
+#include "ati_ids.h"
+#include "ati_radeon_fb.h"
+
+#undef DEBUG
+
+#ifdef DEBUG
+#define DPRINT(x...) printf(x)
+#else
+#define DPRINT(x...) do{}while(0)
+#endif
+
+#ifndef min_t
+#define min_t(type,x,y) \
+	({ type __x = (x); type __y = (y); __x < __y ? __x: __y; })
+#endif
+
+#define MAX_MAPPED_VRAM	(2048*2048*4)
+#define MIN_MAPPED_VRAM	(1024*768*1)
+
+/*#define PCI_VENDOR_ID_ATI*/
+#define PCI_CHIP_RV280_5960		0x5960
+#define PCI_CHIP_RV280_5961		0x5961
+#define PCI_CHIP_RV280_5962		0x5962
+#define PCI_CHIP_RV280_5964		0x5964
+#define PCI_CHIP_RV370_5B60		0x5B60
+#define PCI_CHIP_RV380_5657		0x5657
+#define PCI_CHIP_R420_554d		0x554d
+
+static struct pci_device_id ati_radeon_pci_ids[] = {
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_RV280_5960},
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_RV280_5961},
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_RV280_5962},
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_RV280_5964},
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_RV370_5B60},
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_RV380_5657},
+	{PCI_VENDOR_ID_ATI, PCI_CHIP_R420_554d},
+	{0, 0}
+};
+
+static u16 ati_radeon_id_family_table[][2] = {
+	{PCI_CHIP_RV280_5960, CHIP_FAMILY_RV280},
+	{PCI_CHIP_RV280_5961, CHIP_FAMILY_RV280},
+	{PCI_CHIP_RV280_5962, CHIP_FAMILY_RV280},
+	{PCI_CHIP_RV280_5964, CHIP_FAMILY_RV280},
+	{PCI_CHIP_RV370_5B60, CHIP_FAMILY_RV380},
+	{PCI_CHIP_RV380_5657, CHIP_FAMILY_RV380},
+	{PCI_CHIP_R420_554d,  CHIP_FAMILY_R420},
+	{0, 0}
+};
+
+u16 get_radeon_id_family(u16 device)
+{
+	int i;
+	for (i=0; ati_radeon_id_family_table[0][i]; i+=2)
+		if (ati_radeon_id_family_table[0][i] == device)
+			return ati_radeon_id_family_table[0][i + 1];
+	return 0;
+}
+
+struct radeonfb_info *rinfo;
+
+static void radeon_identify_vram(struct radeonfb_info *rinfo)
+{
+	u32 tmp;
+
+	/* framebuffer size */
+	if ((rinfo->family == CHIP_FAMILY_RS100) ||
+		(rinfo->family == CHIP_FAMILY_RS200) ||
+		(rinfo->family == CHIP_FAMILY_RS300)) {
+		u32 tom = INREG(NB_TOM);
+		tmp = ((((tom >> 16) - (tom & 0xffff) + 1) << 6) * 1024);
+
+		radeon_fifo_wait(6);
+		OUTREG(MC_FB_LOCATION, tom);
+		OUTREG(DISPLAY_BASE_ADDR, (tom & 0xffff) << 16);
+		OUTREG(CRTC2_DISPLAY_BASE_ADDR, (tom & 0xffff) << 16);
+		OUTREG(OV0_BASE_ADDR, (tom & 0xffff) << 16);
+
+		/* This is supposed to fix the crtc2 noise problem. */
+		OUTREG(GRPH2_BUFFER_CNTL, INREG(GRPH2_BUFFER_CNTL) & ~0x7f0000);
+
+		if ((rinfo->family == CHIP_FAMILY_RS100) ||
+			(rinfo->family == CHIP_FAMILY_RS200)) {
+		/* This is to workaround the asic bug for RMX, some versions
+		   of BIOS dosen't have this register initialized correctly.
+		*/
+			OUTREGP(CRTC_MORE_CNTL, CRTC_H_CUTOFF_ACTIVE_EN,
+				~CRTC_H_CUTOFF_ACTIVE_EN);
+		}
+	} else {
+		tmp = INREG(CONFIG_MEMSIZE);
+	}
+
+	/* mem size is bits [28:0], mask off the rest */
+	rinfo->video_ram = tmp & CONFIG_MEMSIZE_MASK;
+
+	/*
+	 * Hack to get around some busted production M6's
+	 * reporting no ram
+	 */
+	if (rinfo->video_ram == 0) {
+		switch (rinfo->pdev.device) {
+		case PCI_CHIP_RADEON_LY:
+		case PCI_CHIP_RADEON_LZ:
+			rinfo->video_ram = 8192 * 1024;
+			break;
+		default:
+			break;
+		}
+	}
+
+	/*
+	 * Now try to identify VRAM type
+	 */
+	if ((rinfo->family >= CHIP_FAMILY_R300) ||
+	    (INREG(MEM_SDRAM_MODE_REG) & (1<<30)))
+		rinfo->vram_ddr = 1;
+	else
+		rinfo->vram_ddr = 0;
+
+	tmp = INREG(MEM_CNTL);
+	if (IS_R300_VARIANT(rinfo)) {
+		tmp &=  R300_MEM_NUM_CHANNELS_MASK;
+		switch (tmp) {
+		case 0:  rinfo->vram_width = 64; break;
+		case 1:  rinfo->vram_width = 128; break;
+		case 2:  rinfo->vram_width = 256; break;
+		default: rinfo->vram_width = 128; break;
+		}
+	} else if ((rinfo->family == CHIP_FAMILY_RV100) ||
+		   (rinfo->family == CHIP_FAMILY_RS100) ||
+		   (rinfo->family == CHIP_FAMILY_RS200)){
+		if (tmp & RV100_MEM_HALF_MODE)
+			rinfo->vram_width = 32;
+		else
+			rinfo->vram_width = 64;
+	} else {
+		if (tmp & MEM_NUM_CHANNELS_MASK)
+			rinfo->vram_width = 128;
+		else
+			rinfo->vram_width = 64;
+	}
+
+	/* This may not be correct, as some cards can have half of channel disabled
+	 * ToDo: identify these cases
+	 */
+
+	DPRINT("radeonfb: Found %ldk of %s %d bits wide videoram\n",
+	       rinfo->video_ram / 1024,
+	       rinfo->vram_ddr ? "DDR" : "SDRAM",
+	       rinfo->vram_width);
+
+}
+
+static void radeon_write_pll_regs(struct radeonfb_info *rinfo, struct radeon_regs *mode)
+{
+	int i;
+
+	radeon_fifo_wait(20);
+
+#if 0
+	/* Workaround from XFree */
+	if (rinfo->is_mobility) {
+		/* A temporal workaround for the occational blanking on certain laptop
+		 * panels. This appears to related to the PLL divider registers
+		 * (fail to lock?). It occurs even when all dividers are the same
+		 * with their old settings. In this case we really don't need to
+		 * fiddle with PLL registers. By doing this we can avoid the blanking
+		 * problem with some panels.
+		 */
+		if ((mode->ppll_ref_div == (INPLL(PPLL_REF_DIV) & PPLL_REF_DIV_MASK)) &&
+		    (mode->ppll_div_3 == (INPLL(PPLL_DIV_3) &
+					  (PPLL_POST3_DIV_MASK | PPLL_FB3_DIV_MASK)))) {
+			/* We still have to force a switch to selected PPLL div thanks to
+			 * an XFree86 driver bug which will switch it away in some cases
+			 * even when using UseFDev */
+			OUTREGP(CLOCK_CNTL_INDEX,
+				mode->clk_cntl_index & PPLL_DIV_SEL_MASK,
+				~PPLL_DIV_SEL_MASK);
+			radeon_pll_errata_after_index(rinfo);
+			radeon_pll_errata_after_data(rinfo);
+			return;
+		}
+	}
+#endif
+	if(rinfo->pdev.device == PCI_CHIP_RV370_5B60) return;
+
+	/* Swich VCKL clock input to CPUCLK so it stays fed while PPLL updates*/
+	OUTPLLP(VCLK_ECP_CNTL, VCLK_SRC_SEL_CPUCLK, ~VCLK_SRC_SEL_MASK);
+
+	/* Reset PPLL & enable atomic update */
+	OUTPLLP(PPLL_CNTL,
+		PPLL_RESET | PPLL_ATOMIC_UPDATE_EN | PPLL_VGA_ATOMIC_UPDATE_EN,
+		~(PPLL_RESET | PPLL_ATOMIC_UPDATE_EN | PPLL_VGA_ATOMIC_UPDATE_EN));
+
+	/* Switch to selected PPLL divider */
+	OUTREGP(CLOCK_CNTL_INDEX,
+		mode->clk_cntl_index & PPLL_DIV_SEL_MASK,
+		~PPLL_DIV_SEL_MASK);
+
+	/* Set PPLL ref. div */
+	if (rinfo->family == CHIP_FAMILY_R300 ||
+	    rinfo->family == CHIP_FAMILY_RS300 ||
+	    rinfo->family == CHIP_FAMILY_R350 ||
+	    rinfo->family == CHIP_FAMILY_RV350) {
+		if (mode->ppll_ref_div & R300_PPLL_REF_DIV_ACC_MASK) {
+			/* When restoring console mode, use saved PPLL_REF_DIV
+			 * setting.
+			 */
+			OUTPLLP(PPLL_REF_DIV, mode->ppll_ref_div, 0);
+		} else {
+			/* R300 uses ref_div_acc field as real ref divider */
+			OUTPLLP(PPLL_REF_DIV,
+				(mode->ppll_ref_div << R300_PPLL_REF_DIV_ACC_SHIFT),
+				~R300_PPLL_REF_DIV_ACC_MASK);
+		}
+	} else
+		OUTPLLP(PPLL_REF_DIV, mode->ppll_ref_div, ~PPLL_REF_DIV_MASK);
+
+	/* Set PPLL divider 3 & post divider*/
+	OUTPLLP(PPLL_DIV_3, mode->ppll_div_3, ~PPLL_FB3_DIV_MASK);
+	OUTPLLP(PPLL_DIV_3, mode->ppll_div_3, ~PPLL_POST3_DIV_MASK);
+
+	/* Write update */
+	while (INPLL(PPLL_REF_DIV) & PPLL_ATOMIC_UPDATE_R)
+		;
+	OUTPLLP(PPLL_REF_DIV, PPLL_ATOMIC_UPDATE_W, ~PPLL_ATOMIC_UPDATE_W);
+
+	/* Wait read update complete */
+	/* FIXME: Certain revisions of R300 can't recover here.  Not sure of
+	   the cause yet, but this workaround will mask the problem for now.
+	   Other chips usually will pass at the very first test, so the
+	   workaround shouldn't have any effect on them. */
+	for (i = 0; (i < 10000 && INPLL(PPLL_REF_DIV) & PPLL_ATOMIC_UPDATE_R); i++)
+		;
+
+	OUTPLL(HTOTAL_CNTL, 0);
+
+	/* Clear reset & atomic update */
+	OUTPLLP(PPLL_CNTL, 0,
+		~(PPLL_RESET | PPLL_SLEEP | PPLL_ATOMIC_UPDATE_EN | PPLL_VGA_ATOMIC_UPDATE_EN));
+
+	/* We may want some locking ... oh well */
+	udelay(5000);
+
+	/* Switch back VCLK source to PPLL */
+	OUTPLLP(VCLK_ECP_CNTL, VCLK_SRC_SEL_PPLLCLK, ~VCLK_SRC_SEL_MASK);
+}
+
+typedef struct {
+	u16 reg;
+	u32 val;
+} reg_val;
+
+
+/* these common regs are cleared before mode setting so they do not
+ * interfere with anything
+ */
+static reg_val common_regs[] = {
+	{ OVR_CLR, 0 },
+	{ OVR_WID_LEFT_RIGHT, 0 },
+	{ OVR_WID_TOP_BOTTOM, 0 },
+	{ OV0_SCALE_CNTL, 0 },
+	{ SUBPIC_CNTL, 0 },
+	{ VIPH_CONTROL, 0 },
+	{ I2C_CNTL_1, 0 },
+	{ GEN_INT_CNTL, 0 },
+	{ CAP0_TRIG_CNTL, 0 },
+	{ CAP1_TRIG_CNTL, 0 },
+};
+
+
+void radeon_setmode(void)
+{
+	int i;
+	struct radeon_regs *mode = malloc(sizeof(struct radeon_regs));
+
+	mode->crtc_gen_cntl = 0x03000200;
+	mode->crtc_ext_cntl = 0x00008048;
+	mode->dac_cntl = 0xff002100;
+	mode->crtc_h_total_disp = 0x4f0063;
+	mode->crtc_h_sync_strt_wid = 0x8c02a2;
+	mode->crtc_v_total_disp = 0x01df020c;
+	mode->crtc_v_sync_strt_wid = 0x8201ea;
+	mode->crtc_pitch = 0x00500050;
+
+	OUTREG(CRTC_GEN_CNTL, mode->crtc_gen_cntl);
+	OUTREGP(CRTC_EXT_CNTL, mode->crtc_ext_cntl,
+		~(CRTC_HSYNC_DIS | CRTC_VSYNC_DIS | CRTC_DISPLAY_DIS));
+	OUTREGP(DAC_CNTL, mode->dac_cntl, DAC_RANGE_CNTL | DAC_BLANKING);
+	OUTREG(CRTC_H_TOTAL_DISP, mode->crtc_h_total_disp);
+	OUTREG(CRTC_H_SYNC_STRT_WID, mode->crtc_h_sync_strt_wid);
+	OUTREG(CRTC_V_TOTAL_DISP, mode->crtc_v_total_disp);
+	OUTREG(CRTC_V_SYNC_STRT_WID, mode->crtc_v_sync_strt_wid);
+	OUTREG(CRTC_OFFSET, 0);
+	OUTREG(CRTC_OFFSET_CNTL, 0);
+	OUTREG(CRTC_PITCH, mode->crtc_pitch);
+
+	mode->clk_cntl_index = 0x300;
+	mode->ppll_ref_div = 0xc;
+	mode->ppll_div_3 = 0x00030059;
+
+	radeon_write_pll_regs(rinfo, mode);
+}
+
+int radeon_probe(struct radeonfb_info *rinfo)
+{
+	pci_dev_t pdev;
+	u16 did;
+
+	pdev = pci_find_devices(ati_radeon_pci_ids, 0);
+
+	if (pdev != -1) {
+		pci_read_config_word(pdev, PCI_DEVICE_ID, &did);
+		printf("ATI Radeon video card (%04x, %04x) found @(%d:%d:%d)\n",
+				PCI_VENDOR_ID_ATI, did, (pdev >> 16) & 0xff,
+				(pdev >> 11) & 0x1f, (pdev >> 8) & 0x7);
+
+		strcpy(rinfo->name, "ATI Radeon");
+		rinfo->pdev.vendor = PCI_VENDOR_ID_ATI;
+		rinfo->pdev.device = did;
+		rinfo->family = get_radeon_id_family(rinfo->pdev.device);
+		pci_read_config_dword(pdev, PCI_BASE_ADDRESS_0,
+				&rinfo->fb_base_phys);
+		pci_read_config_dword(pdev, PCI_BASE_ADDRESS_2,
+				&rinfo->mmio_base_phys);
+		rinfo->fb_base_phys &= 0xfffff000;
+		rinfo->mmio_base_phys &= ~0x04;
+
+		rinfo->mmio_base = (void *)rinfo->mmio_base_phys;
+		DPRINT("rinfo->mmio_base = 0x%x\n",rinfo->mmio_base);
+		rinfo->fb_local_base = INREG(MC_FB_LOCATION) << 16;
+		DPRINT("rinfo->fb_local_base = 0x%x\n",rinfo->fb_local_base);
+		/* PostBIOS with x86 emulater */
+		BootVideoCardBIOS(pdev, NULL, 0);
+
+		/*
+		 * Check for errata
+		 * (These will be added in the future for the chipfamily
+		 * R300, RV200, RS200, RV100, RS100.)
+		 */
+
+		/* Get VRAM size and type */
+		radeon_identify_vram(rinfo);
+
+		rinfo->mapped_vram = min_t(unsigned long, MAX_MAPPED_VRAM,
+				rinfo->video_ram);
+		rinfo->fb_base = (void *)rinfo->fb_base_phys;
+
+		DPRINT("Radeon: framebuffer base phy address 0x%08x," \
+		      "MMIO base phy address 0x%08x," \
+		      "framebuffer local base 0x%08x.\n ",
+		      rinfo->fb_base_phys, rinfo->mmio_base_phys,
+		      rinfo->fb_local_base);
+
+		return 0;
+	}
+	return -1;
+}
+
+/*
+ * The Graphic Device
+ */
+GraphicDevice ctfb;
+
+#define CURSOR_SIZE	0x1000	/* in KByte for HW Cursor */
+#define PATTERN_ADR	(pGD->dprBase + CURSOR_SIZE)	/* pattern Memory after Cursor Memory */
+#define PATTERN_SIZE	8*8*4	/* 4 Bytes per Pixel 8 x 8 Pixel */
+#define ACCELMEMORY	(CURSOR_SIZE + PATTERN_SIZE)	/* reserved Memory for BITBlt and hw cursor */
+
+void *video_hw_init(void)
+{
+	GraphicDevice *pGD = (GraphicDevice *) & ctfb;
+	int i;
+	u32 *vm;
+
+	rinfo = malloc(sizeof(struct radeonfb_info));
+
+	if(radeon_probe(rinfo)) {
+		printf("No radeon video card found!\n");
+		return NULL;
+	}
+
+	/* fill in Graphic device struct */
+	sprintf (pGD->modeIdent, "%dx%dx%d %ldkHz %ldHz", 640,
+		 480, 16, (1000 / 1000),
+		 (2000 / 1000));
+	printf ("%s\n", pGD->modeIdent);
+
+	pGD->winSizeX = 640;
+	pGD->winSizeY = 480;
+	pGD->plnSizeX = 640;
+	pGD->plnSizeY = 480;
+
+	pGD->gdfBytesPP = 1;
+	pGD->gdfIndex = GDF__8BIT_INDEX;
+
+	pGD->isaBase = CFG_ISA_IO_BASE_ADDRESS;
+	pGD->pciBase = rinfo->fb_base_phys;
+	pGD->frameAdrs = rinfo->fb_base_phys;
+	pGD->memSize = 64 * 1024 * 1024;
+
+	/* Cursor Start Address */
+	pGD->dprBase =
+	    (pGD->winSizeX * pGD->winSizeY * pGD->gdfBytesPP) + rinfo->fb_base_phys;
+	if ((pGD->dprBase & 0x0fff) != 0) {
+		/* allign it */
+		pGD->dprBase &= 0xfffff000;
+		pGD->dprBase += 0x00001000;
+	}
+	DPRINT ("Cursor Start %x Pattern Start %x\n", pGD->dprBase,
+		PATTERN_ADR);
+	pGD->vprBase = rinfo->fb_base_phys;	/* Dummy */
+	pGD->cprBase = rinfo->fb_base_phys;	/* Dummy */
+	/* set up Hardware */
+
+	/* Clear video memory */
+	i = pGD->memSize / 4;
+	vm = (unsigned int *) pGD->pciBase;
+	while (i--)
+		*vm++ = 0;
+	/*SetDrawingEngine (bits_per_pixel);*/
+
+	radeon_setmode();
+
+	return ((void *) pGD);
+}
+
+void video_set_lut (unsigned int index,	/* color number */
+	       unsigned char r,	/* red */
+	       unsigned char g,	/* green */
+	       unsigned char b	/* blue */
+	       )
+{
+	OUTREG(PALETTE_INDEX, index);
+	OUTREG(PALETTE_DATA, (r << 16) | (g << 8) | b);
+}
+#endif
diff --git a/drivers/ati_radeon_fb.h b/drivers/ati_radeon_fb.h
new file mode 100644
index 00000000000..b5c4b8b5779
--- /dev/null
+++ b/drivers/ati_radeon_fb.h
@@ -0,0 +1,282 @@
+#ifndef __ATI_RADEON_FB_H
+#define __ATI_RADEON_FB_H
+
+/***************************************************************
+ * Most of the definitions here are adapted right from XFree86 *
+ ***************************************************************/
+
+/*
+ * Chip families. Must fit in the low 16 bits of a long word
+ */
+enum radeon_family {
+	CHIP_FAMILY_UNKNOW,
+	CHIP_FAMILY_LEGACY,
+	CHIP_FAMILY_RADEON,
+	CHIP_FAMILY_RV100,
+	CHIP_FAMILY_RS100,    /* U1 (IGP320M) or A3 (IGP320)*/
+	CHIP_FAMILY_RV200,
+	CHIP_FAMILY_RS200,    /* U2 (IGP330M/340M/350M) or A4 (IGP330/340/345/350),
+				 RS250 (IGP 7000) */
+	CHIP_FAMILY_R200,
+	CHIP_FAMILY_RV250,
+	CHIP_FAMILY_RS300,    /* Radeon 9000 IGP */
+	CHIP_FAMILY_RV280,
+	CHIP_FAMILY_R300,
+	CHIP_FAMILY_R350,
+	CHIP_FAMILY_RV350,
+	CHIP_FAMILY_RV380,    /* RV370/RV380/M22/M24 */
+	CHIP_FAMILY_R420,     /* R420/R423/M18 */
+	CHIP_FAMILY_LAST,
+};
+
+#define IS_RV100_VARIANT(rinfo) (((rinfo)->family == CHIP_FAMILY_RV100)  || \
+				 ((rinfo)->family == CHIP_FAMILY_RV200)  || \
+				 ((rinfo)->family == CHIP_FAMILY_RS100)  || \
+				 ((rinfo)->family == CHIP_FAMILY_RS200)  || \
+				 ((rinfo)->family == CHIP_FAMILY_RV250)  || \
+				 ((rinfo)->family == CHIP_FAMILY_RV280)  || \
+				 ((rinfo)->family == CHIP_FAMILY_RS300))
+
+#define IS_R300_VARIANT(rinfo) (((rinfo)->family == CHIP_FAMILY_R300)  || \
+				((rinfo)->family == CHIP_FAMILY_RV350) || \
+				((rinfo)->family == CHIP_FAMILY_R350)  || \
+				((rinfo)->family == CHIP_FAMILY_RV380) || \
+				((rinfo)->family == CHIP_FAMILY_R420))
+
+struct radeonfb_info {
+	char name[20];
+
+	struct pci_device_id	pdev;
+	u16 			family;
+
+	u32 			fb_base_phys;
+	u32 			mmio_base_phys;
+
+	void 			*mmio_base;
+	void 			*fb_base;
+
+	u32			video_ram;
+	u32			mapped_vram;
+	int			vram_width;
+	int			vram_ddr;
+
+	u32 			fb_local_base;
+};
+
+#define INREG8(addr)		readb((rinfo->mmio_base)+addr)
+#define OUTREG8(addr,val)	writeb(val, (rinfo->mmio_base)+addr)
+#define INREG16(addr)		readw((rinfo->mmio_base)+addr)
+#define OUTREG16(addr,val)	writew(val, (rinfo->mmio_base)+addr)
+#define INREG(addr)		readl((rinfo->mmio_base)+addr)
+#define OUTREG(addr,val)	writel(val, (rinfo->mmio_base)+addr)
+
+static inline void _OUTREGP(struct radeonfb_info *rinfo, u32 addr,
+		       u32 val, u32 mask)
+{
+	unsigned int tmp;
+
+	tmp = INREG(addr);
+	tmp &= (mask);
+	tmp |= (val);
+	OUTREG(addr, tmp);
+}
+
+#define OUTREGP(addr,val,mask)	_OUTREGP(rinfo, addr, val,mask)
+
+/*
+ * 2D Engine helper routines
+ */
+static inline void radeon_engine_flush (struct radeonfb_info *rinfo)
+{
+	int i;
+
+	/* initiate flush */
+	OUTREGP(RB2D_DSTCACHE_CTLSTAT, RB2D_DC_FLUSH_ALL,
+	        ~RB2D_DC_FLUSH_ALL);
+
+	for (i=0; i < 2000000; i++) {
+		if (!(INREG(RB2D_DSTCACHE_CTLSTAT) & RB2D_DC_BUSY))
+			return;
+		udelay(1);
+	}
+	printf("radeonfb: Flush Timeout !\n");
+}
+
+static inline void _radeon_fifo_wait(struct radeonfb_info *rinfo, int entries)
+{
+	int i;
+
+	for (i=0; i<2000000; i++) {
+		if ((INREG(RBBM_STATUS) & 0x7f) >= entries)
+			return;
+		udelay(1);
+	}
+	printf("radeonfb: FIFO Timeout !\n");
+}
+
+static inline void _radeon_engine_idle(struct radeonfb_info *rinfo)
+{
+	int i;
+
+	/* ensure FIFO is empty before waiting for idle */
+	_radeon_fifo_wait (rinfo, 64);
+
+	for (i=0; i<2000000; i++) {
+		if (((INREG(RBBM_STATUS) & GUI_ACTIVE)) == 0) {
+			radeon_engine_flush (rinfo);
+			return;
+		}
+		udelay(1);
+	}
+	printf("radeonfb: Idle Timeout !\n");
+}
+
+#define radeon_engine_idle()		_radeon_engine_idle(rinfo)
+#define radeon_fifo_wait(entries)	_radeon_fifo_wait(rinfo,entries)
+#define radeon_msleep(ms)		_radeon_msleep(rinfo,ms)
+
+/*
+ * This structure contains the various registers manipulated by this
+ * driver for setting or restoring a mode. It's mostly copied from
+ * XFree's RADEONSaveRec structure. A few chip settings might still be
+ * tweaked without beeing reflected or saved in these registers though
+ */
+struct radeon_regs {
+	/* Common registers */
+	u32		ovr_clr;
+	u32		ovr_wid_left_right;
+	u32		ovr_wid_top_bottom;
+	u32		ov0_scale_cntl;
+	u32		mpp_tb_config;
+	u32		mpp_gp_config;
+	u32		subpic_cntl;
+	u32		viph_control;
+	u32		i2c_cntl_1;
+	u32		gen_int_cntl;
+	u32		cap0_trig_cntl;
+	u32		cap1_trig_cntl;
+	u32		bus_cntl;
+	u32		surface_cntl;
+	u32		bios_5_scratch;
+
+	/* Other registers to save for VT switches or driver load/unload */
+	u32		dp_datatype;
+	u32		rbbm_soft_reset;
+	u32		clock_cntl_index;
+	u32		amcgpio_en_reg;
+	u32		amcgpio_mask;
+
+	/* Surface/tiling registers */
+	u32		surf_lower_bound[8];
+	u32		surf_upper_bound[8];
+	u32		surf_info[8];
+
+	/* CRTC registers */
+	u32		crtc_gen_cntl;
+	u32		crtc_ext_cntl;
+	u32		dac_cntl;
+	u32		crtc_h_total_disp;
+	u32		crtc_h_sync_strt_wid;
+	u32		crtc_v_total_disp;
+	u32		crtc_v_sync_strt_wid;
+	u32		crtc_offset;
+	u32		crtc_offset_cntl;
+	u32		crtc_pitch;
+	u32		disp_merge_cntl;
+	u32		grph_buffer_cntl;
+	u32		crtc_more_cntl;
+
+	/* CRTC2 registers */
+	u32		crtc2_gen_cntl;
+	u32		dac2_cntl;
+	u32		disp_output_cntl;
+	u32		disp_hw_debug;
+	u32		disp2_merge_cntl;
+	u32		grph2_buffer_cntl;
+	u32		crtc2_h_total_disp;
+	u32		crtc2_h_sync_strt_wid;
+	u32		crtc2_v_total_disp;
+	u32		crtc2_v_sync_strt_wid;
+	u32		crtc2_offset;
+	u32		crtc2_offset_cntl;
+	u32		crtc2_pitch;
+
+	/* Flat panel regs */
+	u32 		fp_crtc_h_total_disp;
+	u32		fp_crtc_v_total_disp;
+	u32		fp_gen_cntl;
+	u32		fp2_gen_cntl;
+	u32		fp_h_sync_strt_wid;
+	u32		fp2_h_sync_strt_wid;
+	u32		fp_horz_stretch;
+	u32		fp_panel_cntl;
+	u32		fp_v_sync_strt_wid;
+	u32		fp2_v_sync_strt_wid;
+	u32		fp_vert_stretch;
+	u32		lvds_gen_cntl;
+	u32		lvds_pll_cntl;
+	u32		tmds_crc;
+	u32		tmds_transmitter_cntl;
+
+	/* Computed values for PLL */
+	u32		dot_clock_freq;
+	int		feedback_div;
+	int		post_div;
+
+	/* PLL registers */
+	u32		ppll_div_3;
+	u32		ppll_ref_div;
+	u32		vclk_ecp_cntl;
+	u32		clk_cntl_index;
+
+	/* Computed values for PLL2 */
+	u32		dot_clock_freq_2;
+	int		feedback_div_2;
+	int		post_div_2;
+
+	/* PLL2 registers */
+	u32		p2pll_ref_div;
+	u32		p2pll_div_0;
+	u32		htotal_cntl2;
+
+	/* Palette */
+	int		palette_valid;
+};
+
+static inline u32 __INPLL(struct radeonfb_info *rinfo, u32 addr)
+{
+	u32 data;
+
+	OUTREG8(CLOCK_CNTL_INDEX, addr & 0x0000003f);
+	/* radeon_pll_errata_after_index(rinfo); */
+	data = INREG(CLOCK_CNTL_DATA);
+	/* radeon_pll_errata_after_data(rinfo); */
+	return data;
+}
+
+static inline void __OUTPLL(struct radeonfb_info *rinfo, unsigned int index,
+			    u32 val)
+{
+
+	OUTREG8(CLOCK_CNTL_INDEX, (index & 0x0000003f) | 0x00000080);
+	/* radeon_pll_errata_after_index(rinfo); */
+	OUTREG(CLOCK_CNTL_DATA, val);
+	/* radeon_pll_errata_after_data(rinfo); */
+}
+
+static inline void __OUTPLLP(struct radeonfb_info *rinfo, unsigned int index,
+			     u32 val, u32 mask)
+{
+	unsigned int tmp;
+
+	tmp  = __INPLL(rinfo, index);
+	tmp &= (mask);
+	tmp |= (val);
+	__OUTPLL(rinfo, index, tmp);
+}
+
+#define INPLL(addr)			__INPLL(rinfo, addr)
+#define OUTPLL(index, val)		__OUTPLL(rinfo, index, val)
+#define OUTPLLP(index, val, mask)	__OUTPLLP(rinfo, index, val, mask)
+
+#endif
diff --git a/drivers/bcm570x.c b/drivers/bcm570x.c
index 5f632a64692..703cda48653 100644
--- a/drivers/bcm570x.c
+++ b/drivers/bcm570x.c
@@ -6,8 +6,8 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && (!defined(CONFIG_NET_MULTI)) && \
-	defined(CONFIG_BCM570x)
+#if defined(CONFIG_CMD_NET) \
+	&& (!defined(CONFIG_NET_MULTI)) && defined(CONFIG_BCM570x)
 
 #ifdef CONFIG_BMW
 #include <mpc824x.h>
@@ -1688,4 +1688,4 @@ QQ_GetTail(
     return pQueue->Array[Idx];
 }
 
-#endif	/* CFG_CMD_NET, !CONFIG_NET_MULTI, CONFIG_BCM570x */
+#endif
diff --git a/drivers/bios_emulator/Makefile b/drivers/bios_emulator/Makefile
new file mode 100644
index 00000000000..586e83be8f2
--- /dev/null
+++ b/drivers/bios_emulator/Makefile
@@ -0,0 +1,30 @@
+include $(TOPDIR)/config.mk
+
+LIB := $(obj)libatibiosemu.a
+
+X86DIR  = ./x86emu
+
+OBJS	= atibios.o biosemu.o besys.o bios.o  \
+	$(X86DIR)/decode.o \
+	$(X86DIR)/ops2.o \
+	$(X86DIR)/ops.o \
+	$(X86DIR)/prim_ops.o \
+	$(X86DIR)/sys.o \
+	$(X86DIR)/debug.o
+
+CFLAGS += -I. -I./include  -I$(X86DIR) -I$(TOPDIR)/include \
+	-D__PPC__  -D__BIG_ENDIAN__
+
+all:	$(LIB)
+
+$(LIB): $(OBJS)
+	$(AR) crv $@ $(OBJS)
+
+#########################################################################
+
+.depend:	Makefile $(OBJS:.o=.c)
+		$(CC) -M $(CFLAGS) $(OBJS:.o=.c) > $@
+
+sinclude .depend
+
+#########################################################################
diff --git a/drivers/bios_emulator/atibios.c b/drivers/bios_emulator/atibios.c
new file mode 100644
index 00000000000..5779f99f1f6
--- /dev/null
+++ b/drivers/bios_emulator/atibios.c
@@ -0,0 +1,340 @@
+/****************************************************************************
+*
+*		     Video BOOT Graphics Card POST Module
+*
+*  ========================================================================
+*   Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*   Jason Jin <Jason.jin@freescale.com>
+*
+*   Copyright (C) 1991-2004 SciTech Software, Inc. All rights reserved.
+*
+*   This file may be distributed and/or modified under the terms of the
+*   GNU General Public License version 2.0 as published by the Free
+*   Software Foundation and appearing in the file LICENSE.GPL included
+*   in the packaging of this file.
+*
+*   Licensees holding a valid Commercial License for this product from
+*   SciTech Software, Inc. may use this file in accordance with the
+*   Commercial License Agreement provided with the Software.
+*
+*   This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING
+*   THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+*   PURPOSE.
+*
+*   See http://www.scitechsoft.com/license/ for information about
+*   the licensing options available and how to purchase a Commercial
+*   License Agreement.
+*
+*   Contact license@scitechsoft.com if any conditions of this licensing
+*   are not clear to you, or you have questions about licensing options.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Linux Kernel
+* Developer:	Kendall Bennett
+*
+* Description:	Module to implement booting PCI/AGP controllers on the
+*		bus. We use the x86 real mode emulator to run the BIOS on
+*		graphics controllers to bring the cards up.
+*
+*		Note that at present this module does *not* support
+*		multiple controllers.
+*
+*		The orignal name of this file is warmboot.c.
+*		Jason ported this file to u-boot to run the ATI video card
+*		BIOS in u-boot.
+****************************************************************************/
+#include <common.h>
+
+#ifdef CONFIG_BIOSEMU
+
+#include "biosemui.h"
+#include <malloc.h>
+
+/* Length of the BIOS image */
+#define MAX_BIOSLEN	    (128 * 1024L)
+
+/* Define some useful types and macros */
+#define true		    1
+#define false		    0
+
+/* Place to save PCI BAR's that we change and later restore */
+static u32 saveROMBaseAddress;
+static u32 saveBaseAddress10;
+static u32 saveBaseAddress14;
+static u32 saveBaseAddress18;
+static u32 saveBaseAddress20;
+
+/****************************************************************************
+PARAMETERS:
+pcidev	- PCI device info for the video card on the bus to boot
+VGAInfo - BIOS emulator VGA info structure
+
+REMARKS:
+This function executes the BIOS POST code on the controller. We assume that
+at this stage the controller has its I/O and memory space enabled and
+that all other controllers are in a disabled state.
+****************************************************************************/
+static void PCI_doBIOSPOST(pci_dev_t pcidev, BE_VGAInfo * VGAInfo)
+{
+	RMREGS regs;
+	RMSREGS sregs;
+
+	/* Determine the value to store in AX for BIOS POST. Per the PCI specs,
+	 AH must contain the bus and AL must contain the devfn, encoded as
+	 (dev << 3) | fn
+	 */
+	memset(&regs, 0, sizeof(regs));
+	memset(&sregs, 0, sizeof(sregs));
+	regs.x.ax = ((int)PCI_BUS(pcidev) << 8) |
+	    ((int)PCI_DEV(pcidev) << 3) | (int)PCI_FUNC(pcidev);
+
+	/*Setup the X86 emulator for the VGA BIOS*/
+	BE_setVGA(VGAInfo);
+
+	/*Execute the BIOS POST code*/
+	BE_callRealMode(0xC000, 0x0003, &regs, &sregs);
+
+	/*Cleanup and exit*/
+	BE_getVGA(VGAInfo);
+}
+
+/****************************************************************************
+PARAMETERS:
+pcidev	- PCI device info for the video card on the bus
+bar	- Place to return the base address register offset to use
+
+RETURNS:
+The address to use to map the secondary BIOS (AGP devices)
+
+REMARKS:
+Searches all the PCI base address registers for the device looking for a
+memory mapping that is large enough to hold our ROM BIOS. We usually end up
+finding the framebuffer mapping (usually BAR 0x10), and we use this mapping
+to map the BIOS for the device into. We use a mapping that is already
+assigned to the device to ensure the memory range will be passed through
+by any PCI->PCI or AGP->PCI bridge that may be present.
+
+NOTE: Usually this function is only used for AGP devices, but it may be
+      used for PCI devices that have already been POST'ed and the BIOS
+      ROM base address has been zero'ed out.
+
+NOTE: This function leaves the original memory aperture disabled by leaving
+      it programmed to all 1's. It must be restored to the correct value
+      later.
+****************************************************************************/
+static u32 PCI_findBIOSAddr(pci_dev_t pcidev, int *bar)
+{
+	u32 base, size;
+
+	for (*bar = 0x10; *bar <= 0x14; (*bar) += 4) {
+		pci_read_config_dword(pcidev, *bar, &base);
+		if (!(base & 0x1)) {
+			pci_write_config_dword(pcidev, *bar, 0xFFFFFFFF);
+			pci_read_config_dword(pcidev, *bar, &size);
+			size = ~(size & ~0xFF) + 1;
+			if (size >= MAX_BIOSLEN)
+				return base & ~0xFF;
+		}
+	}
+	return 0;
+}
+
+/****************************************************************************
+REMARKS:
+Some non-x86 Linux kernels map PCI relocateable I/O to values that
+are above 64K, which will not work with the BIOS image that requires
+the offset for the I/O ports to be a maximum of 16-bits. Ideally
+someone should fix the kernel to map the I/O ports for VGA compatible
+devices to a different location (or just all I/O ports since it is
+unlikely you can have enough devices in the machine to use up all
+64K of the I/O space - a total of more than 256 cards would be
+necessary).
+
+Anyway to fix this we change all I/O mapped base registers and
+chop off the top bits.
+****************************************************************************/
+static void PCI_fixupIObase(pci_dev_t pcidev, int reg, u32 * base)
+{
+	if ((*base & 0x1) && (*base > 0xFFFE)) {
+		*base &= 0xFFFF;
+		pci_write_config_dword(pcidev, reg, *base);
+
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+pcidev	- PCI device info for the video card on the bus
+
+RETURNS:
+Pointers to the mapped BIOS image
+
+REMARKS:
+Maps a pointer to the BIOS image on the graphics card on the PCI bus.
+****************************************************************************/
+void *PCI_mapBIOSImage(pci_dev_t pcidev)
+{
+	u32 BIOSImagePhys;
+	int BIOSImageBAR;
+	u8 *BIOSImage;
+
+	/*Save PCI BAR registers that might get changed*/
+	pci_read_config_dword(pcidev, PCI_ROM_ADDRESS, &saveROMBaseAddress);
+	pci_read_config_dword(pcidev, PCI_BASE_ADDRESS_0, &saveBaseAddress10);
+	pci_read_config_dword(pcidev, PCI_BASE_ADDRESS_1, &saveBaseAddress14);
+	pci_read_config_dword(pcidev, PCI_BASE_ADDRESS_2, &saveBaseAddress18);
+	pci_read_config_dword(pcidev, PCI_BASE_ADDRESS_4, &saveBaseAddress20);
+
+	/*Fix up I/O base registers to less than 64K */
+	if(saveBaseAddress14 != 0)
+		PCI_fixupIObase(pcidev, PCI_BASE_ADDRESS_1, &saveBaseAddress14);
+	else
+		PCI_fixupIObase(pcidev, PCI_BASE_ADDRESS_4, &saveBaseAddress20);
+
+	/* Some cards have problems that stop us from being able to read the
+	 BIOS image from the ROM BAR. To fix this we have to do some chipset
+	 specific programming for different cards to solve this problem.
+	*/
+
+	if ((BIOSImagePhys = PCI_findBIOSAddr(pcidev, &BIOSImageBAR)) == 0) {
+		printf("Find bios addr error\n");
+		return NULL;
+	}
+
+	BIOSImage = (u8 *) BIOSImagePhys;
+
+	/*Change the PCI BAR registers to map it onto the bus.*/
+	pci_write_config_dword(pcidev, BIOSImageBAR, 0);
+	pci_write_config_dword(pcidev, PCI_ROM_ADDRESS, BIOSImagePhys | 0x1);
+
+	udelay(1);
+
+	/*Check that the BIOS image is valid. If not fail, or return the
+	 compiled in BIOS image if that option was enabled
+	 */
+	if (BIOSImage[0] != 0x55 || BIOSImage[1] != 0xAA || BIOSImage[2] == 0) {
+		return NULL;
+	}
+
+	return BIOSImage;
+}
+
+/****************************************************************************
+PARAMETERS:
+pcidev	- PCI device info for the video card on the bus
+
+REMARKS:
+Unmaps the BIOS image for the device and restores framebuffer mappings
+****************************************************************************/
+void PCI_unmapBIOSImage(pci_dev_t pcidev, void *BIOSImage)
+{
+	pci_write_config_dword(pcidev, PCI_ROM_ADDRESS, saveROMBaseAddress);
+	pci_write_config_dword(pcidev, PCI_BASE_ADDRESS_0, saveBaseAddress10);
+	pci_write_config_dword(pcidev, PCI_BASE_ADDRESS_1, saveBaseAddress14);
+	pci_write_config_dword(pcidev, PCI_BASE_ADDRESS_2, saveBaseAddress18);
+	pci_write_config_dword(pcidev, PCI_BASE_ADDRESS_4, saveBaseAddress20);
+}
+
+/****************************************************************************
+PARAMETERS:
+pcidev	- PCI device info for the video card on the bus to boot
+VGAInfo - BIOS emulator VGA info structure
+
+RETURNS:
+True if successfully initialised, false if not.
+
+REMARKS:
+Loads and POST's the display controllers BIOS, directly from the BIOS
+image we can extract over the PCI bus.
+****************************************************************************/
+static int PCI_postController(pci_dev_t pcidev, BE_VGAInfo * VGAInfo)
+{
+	u32 BIOSImageLen;
+	uchar *mappedBIOS;
+	uchar *copyOfBIOS;
+
+	/*Allocate memory to store copy of BIOS from display controller*/
+	if ((mappedBIOS = PCI_mapBIOSImage(pcidev)) == NULL) {
+		printf("videoboot: Video ROM failed to map!\n");
+		return false;
+	}
+
+	BIOSImageLen = mappedBIOS[2] * 512;
+
+	if ((copyOfBIOS = malloc(BIOSImageLen)) == NULL) {
+		printf("videoboot: Out of memory!\n");
+		return false;
+	}
+	memcpy(copyOfBIOS, mappedBIOS, BIOSImageLen);
+
+	PCI_unmapBIOSImage(pcidev, mappedBIOS);
+
+	/*Save information in VGAInfo structure*/
+	VGAInfo->function = PCI_FUNC(pcidev);
+	VGAInfo->device = PCI_DEV(pcidev);
+	VGAInfo->bus = PCI_BUS(pcidev);
+	VGAInfo->pcidev = pcidev;
+	VGAInfo->BIOSImage = copyOfBIOS;
+	VGAInfo->BIOSImageLen = BIOSImageLen;
+
+	/*Now execute the BIOS POST for the device*/
+	if (copyOfBIOS[0] != 0x55 || copyOfBIOS[1] != 0xAA) {
+		printf("videoboot: Video ROM image is invalid!\n");
+		return false;
+	}
+
+	PCI_doBIOSPOST(pcidev, VGAInfo);
+
+	/*Reset the size of the BIOS image to the final size*/
+	VGAInfo->BIOSImageLen = copyOfBIOS[2] * 512;
+	return true;
+}
+
+/****************************************************************************
+PARAMETERS:
+pcidev	    - PCI device info for the video card on the bus to boot
+pVGAInfo    - Place to return VGA info structure is requested
+cleanUp	    - True to clean up on exit, false to leave emulator active
+
+REMARKS:
+Boots the PCI/AGP video card on the bus using the Video ROM BIOS image
+and the X86 BIOS emulator module.
+****************************************************************************/
+int BootVideoCardBIOS(pci_dev_t pcidev, BE_VGAInfo ** pVGAInfo, int cleanUp)
+{
+	BE_VGAInfo *VGAInfo;
+
+	printf("videoboot: Booting PCI video card bus %d, function %d, device %d\n",
+	     PCI_BUS(pcidev), PCI_FUNC(pcidev), PCI_DEV(pcidev));
+
+	/*Initialise the x86 BIOS emulator*/
+	if ((VGAInfo = malloc(sizeof(*VGAInfo))) == NULL) {
+		printf("videoboot: Out of memory!\n");
+		return false;
+	}
+	memset(VGAInfo, 0, sizeof(*VGAInfo));
+	BE_init(0, 65536, VGAInfo, 0);
+
+	/*Post all the display controller BIOS'es*/
+	PCI_postController(pcidev, VGAInfo);
+
+	/*Cleanup and exit the emulator if requested. If the BIOS emulator
+	is needed after booting the card, we will not call BE_exit and
+	leave it enabled for further use (ie: VESA driver etc).
+	*/
+	if (cleanUp) {
+		BE_exit();
+		if (VGAInfo->BIOSImage)
+			free(VGAInfo->BIOSImage);
+		free(VGAInfo);
+		VGAInfo = NULL;
+	}
+	/*Return VGA info pointer if the caller requested it*/
+	if (pVGAInfo)
+		*pVGAInfo = VGAInfo;
+	return true;
+}
+
+#endif
diff --git a/drivers/bios_emulator/besys.c b/drivers/bios_emulator/besys.c
new file mode 100644
index 00000000000..4c4bc8d7ba1
--- /dev/null
+++ b/drivers/bios_emulator/besys.c
@@ -0,0 +1,721 @@
+/****************************************************************************
+*
+*                        BIOS emulator and interface
+*                      to Realmode X86 Emulator Library
+*
+*  ========================================================================
+*
+*   Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*   Jason Jin<Jason.jin@freescale.com>
+*
+*   Copyright (C) 1991-2004 SciTech Software, Inc. All rights reserved.
+*
+*   This file may be distributed and/or modified under the terms of the
+*   GNU General Public License version 2.0 as published by the Free
+*   Software Foundation and appearing in the file LICENSE.GPL included
+*   in the packaging of this file.
+*
+*   Licensees holding a valid Commercial License for this product from
+*   SciTech Software, Inc. may use this file in accordance with the
+*   Commercial License Agreement provided with the Software.
+*
+*   This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING
+*   THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+*   PURPOSE.
+*
+*   See http://www.scitechsoft.com/license/ for information about
+*   the licensing options available and how to purchase a Commercial
+*   License Agreement.
+*
+*   Contact license@scitechsoft.com if any conditions of this licensing
+*   are not clear to you, or you have questions about licensing options.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  This file includes BIOS emulator I/O and memory access
+*               functions.
+*
+*		Jason ported this file to u-boot to run the ATI video card
+*		BIOS in u-boot. Removed some emulate functions such as the
+*		timer port access. Made all the VGA port except reading 0x3c3
+*		be emulated. Seems like reading 0x3c3 should return the high
+*		16 bit of the io port.
+*
+****************************************************************************/
+
+#include "biosemui.h"
+
+#if defined(CONFIG_BIOSEMU)
+/*------------------------- Global Variables ------------------------------*/
+
+#ifndef __i386__
+static char *BE_biosDate = "08/14/99";
+static u8 BE_model = 0xFC;
+static u8 BE_submodel = 0x00;
+#endif
+
+/*----------------------------- Implementation ----------------------------*/
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to convert
+
+RETURNS:
+Actual memory address to read or write the data
+
+REMARKS:
+This function converts an emulator memory address in a 32-bit range to
+a real memory address that we wish to access. It handles splitting up the
+memory address space appropriately to access the emulator BIOS image, video
+memory and system BIOS etc.
+****************************************************************************/
+static u8 *BE_memaddr(u32 addr)
+{
+	if (addr >= 0xC0000 && addr <= _BE_env.biosmem_limit) {
+		return (u8*)(_BE_env.biosmem_base + addr - 0xC0000);
+	} else if (addr > _BE_env.biosmem_limit && addr < 0xD0000) {
+		DB(printf("BE_memaddr: address %#lx may be invalid!\n", addr);)
+		return M.mem_base;
+	} else if (addr >= 0xA0000 && addr <= 0xBFFFF) {
+		return (u8*)(_BE_env.busmem_base + addr - 0xA0000);
+	}
+#ifdef __i386__
+	else if (addr >= 0xD0000 && addr <= 0xFFFFF) {
+		/* We map the real System BIOS directly on real PC's */
+		DB(printf("BE_memaddr: System BIOS address %#lx\n", addr);)
+		    return _BE_env.busmem_base + addr - 0xA0000;
+	}
+#else
+	else if (addr >= 0xFFFF5 && addr < 0xFFFFE) {
+		/* Return a faked BIOS date string for non-x86 machines */
+		DB(printf("BE_memaddr - Returning BIOS date\n");)
+		return BE_biosDate + addr - 0xFFFF5;
+	} else if (addr == 0xFFFFE) {
+		/* Return system model identifier for non-x86 machines */
+		DB(printf("BE_memaddr - Returning model\n");)
+		return &BE_model;
+	} else if (addr == 0xFFFFF) {
+		/* Return system submodel identifier for non-x86 machines */
+		DB(printf("BE_memaddr - Returning submodel\n");)
+		return &BE_submodel;
+	}
+#endif
+	else if (addr > M.mem_size - 1) {
+		HALT_SYS();
+		return M.mem_base;
+	}
+
+	return M.mem_base + addr;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+
+RETURNS:
+Byte value read from emulator memory.
+
+REMARKS:
+Reads a byte value from the emulator memory. We have three distinct memory
+regions that are handled differently, which this function handles.
+****************************************************************************/
+u8 X86API BE_rdb(u32 addr)
+{
+	if (_BE_env.emulateVGA && addr >= 0xA0000 && addr <= 0xBFFFF)
+		return 0;
+	else {
+		u8 val = readb_le(BE_memaddr(addr));
+		return val;
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+
+RETURNS:
+Word value read from emulator memory.
+
+REMARKS:
+Reads a word value from the emulator memory. We have three distinct memory
+regions that are handled differently, which this function handles.
+****************************************************************************/
+u16 X86API BE_rdw(u32 addr)
+{
+	if (_BE_env.emulateVGA && addr >= 0xA0000 && addr <= 0xBFFFF)
+		return 0;
+	else {
+		u8 *base = BE_memaddr(addr);
+		u16 val = readw_le(base);
+		return val;
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+
+RETURNS:
+Long value read from emulator memory.
+
+REMARKS:
+Reads a 32-bit value from the emulator memory. We have three distinct memory
+regions that are handled differently, which this function handles.
+****************************************************************************/
+u32 X86API BE_rdl(u32 addr)
+{
+	if (_BE_env.emulateVGA && addr >= 0xA0000 && addr <= 0xBFFFF)
+		return 0;
+	else {
+		u8 *base = BE_memaddr(addr);
+		u32 val = readl_le(base);
+		return val;
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+val     - Value to store
+
+REMARKS:
+Writes a byte value to emulator memory. We have three distinct memory
+regions that are handled differently, which this function handles.
+****************************************************************************/
+void X86API BE_wrb(u32 addr, u8 val)
+{
+	if (!(_BE_env.emulateVGA && addr >= 0xA0000 && addr <= 0xBFFFF)) {
+		writeb_le(BE_memaddr(addr), val);
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+val     - Value to store
+
+REMARKS:
+Writes a word value to emulator memory. We have three distinct memory
+regions that are handled differently, which this function handles.
+****************************************************************************/
+void X86API BE_wrw(u32 addr, u16 val)
+{
+	if (!(_BE_env.emulateVGA && addr >= 0xA0000 && addr <= 0xBFFFF)) {
+		u8 *base = BE_memaddr(addr);
+		writew_le(base, val);
+
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+val     - Value to store
+
+REMARKS:
+Writes a 32-bit value to emulator memory. We have three distinct memory
+regions that are handled differently, which this function handles.
+****************************************************************************/
+void X86API BE_wrl(u32 addr, u32 val)
+{
+	if (!(_BE_env.emulateVGA && addr >= 0xA0000 && addr <= 0xBFFFF)) {
+		u8 *base = BE_memaddr(addr);
+		writel_le(base, val);
+	}
+}
+
+#if defined(DEBUG) || !defined(__i386__)
+
+/* For Non-Intel machines we may need to emulate some I/O port accesses that
+ * the BIOS may try to access, such as the PCI config registers.
+ */
+
+#define IS_TIMER_PORT(port) (0x40 <= port && port <= 0x43)
+#define IS_CMOS_PORT(port)  (0x70 <= port && port <= 0x71)
+/*#define IS_VGA_PORT(port)   (_BE_env.emulateVGA && 0x3C0 <= port && port <= 0x3DA)*/
+#define IS_VGA_PORT(port)   (0x3C0 <= port && port <= 0x3DA)
+#define IS_PCI_PORT(port)   (0xCF8 <= port && port <= 0xCFF)
+#define IS_SPKR_PORT(port)  (port == 0x61)
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to read from
+type    - Type of access to perform
+
+REMARKS:
+Performs an emulated read from the Standard VGA I/O ports. If the target
+hardware does not support mapping the VGA I/O and memory (such as some
+PowerPC systems), we emulate the VGA so that the BIOS will still be able to
+set NonVGA display modes such as on ATI hardware.
+****************************************************************************/
+static u8 VGA_inpb (const int port)
+{
+	u8 val = 0xff;
+
+	switch (port) {
+	case 0x3C0:
+		/* 3C0 has funky characteristics because it can act as either
+		   a data register or index register depending on the state
+		   of an internal flip flop in the hardware. Hence we have
+		   to emulate that functionality in here. */
+		if (_BE_env.flipFlop3C0 == 0) {
+			/* Access 3C0 as index register */
+			val = _BE_env.emu3C0;
+		} else {
+			/* Access 3C0 as data register */
+			if (_BE_env.emu3C0 < ATT_C)
+				val = _BE_env.emu3C1[_BE_env.emu3C0];
+		}
+		_BE_env.flipFlop3C0 ^= 1;
+		break;
+	case 0x3C1:
+		if (_BE_env.emu3C0 < ATT_C)
+			return _BE_env.emu3C1[_BE_env.emu3C0];
+		break;
+	case 0x3CC:
+		return _BE_env.emu3C2;
+	case 0x3C4:
+		return _BE_env.emu3C4;
+	case 0x3C5:
+		if (_BE_env.emu3C4 < ATT_C)
+			return _BE_env.emu3C5[_BE_env.emu3C4];
+		break;
+	case 0x3C6:
+		return _BE_env.emu3C6;
+	case 0x3C7:
+		return _BE_env.emu3C7;
+	case 0x3C8:
+		return _BE_env.emu3C8;
+	case 0x3C9:
+		if (_BE_env.emu3C7 < PAL_C)
+			return _BE_env.emu3C9[_BE_env.emu3C7++];
+		break;
+	case 0x3CE:
+		return _BE_env.emu3CE;
+	case 0x3CF:
+		if (_BE_env.emu3CE < GRA_C)
+			return _BE_env.emu3CF[_BE_env.emu3CE];
+		break;
+	case 0x3D4:
+		if (_BE_env.emu3C2 & 0x1)
+			return _BE_env.emu3D4;
+		break;
+	case 0x3D5:
+		if ((_BE_env.emu3C2 & 0x1) && (_BE_env.emu3D4 < CRT_C))
+			return _BE_env.emu3D5[_BE_env.emu3D4];
+		break;
+	case 0x3DA:
+		_BE_env.flipFlop3C0 = 0;
+		val = _BE_env.emu3DA;
+		_BE_env.emu3DA ^= 0x9;
+		break;
+	}
+	return val;
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+type    - Type of access to perform
+
+REMARKS:
+Performs an emulated write to one of the 8253 timer registers. For now
+we only emulate timer 0 which is the only timer that the BIOS code appears
+to use.
+****************************************************************************/
+static void VGA_outpb (int port, u8 val)
+{
+	switch (port) {
+	case 0x3C0:
+		/* 3C0 has funky characteristics because it can act as either
+		   a data register or index register depending on the state
+		   of an internal flip flop in the hardware. Hence we have
+		   to emulate that functionality in here. */
+		if (_BE_env.flipFlop3C0 == 0) {
+			/* Access 3C0 as index register */
+			_BE_env.emu3C0 = val;
+		} else {
+			/* Access 3C0 as data register */
+			if (_BE_env.emu3C0 < ATT_C)
+				_BE_env.emu3C1[_BE_env.emu3C0] = val;
+		}
+		_BE_env.flipFlop3C0 ^= 1;
+		break;
+	case 0x3C2:
+		_BE_env.emu3C2 = val;
+		break;
+	case 0x3C4:
+		_BE_env.emu3C4 = val;
+		break;
+	case 0x3C5:
+		if (_BE_env.emu3C4 < ATT_C)
+			_BE_env.emu3C5[_BE_env.emu3C4] = val;
+		break;
+	case 0x3C6:
+		_BE_env.emu3C6 = val;
+		break;
+	case 0x3C7:
+		_BE_env.emu3C7 = (int) val *3;
+
+		break;
+	case 0x3C8:
+		_BE_env.emu3C8 = (int) val *3;
+
+		break;
+	case 0x3C9:
+		if (_BE_env.emu3C8 < PAL_C)
+			_BE_env.emu3C9[_BE_env.emu3C8++] = val;
+		break;
+	case 0x3CE:
+		_BE_env.emu3CE = val;
+		break;
+	case 0x3CF:
+		if (_BE_env.emu3CE < GRA_C)
+			_BE_env.emu3CF[_BE_env.emu3CE] = val;
+		break;
+	case 0x3D4:
+		if (_BE_env.emu3C2 & 0x1)
+			_BE_env.emu3D4 = val;
+		break;
+	case 0x3D5:
+		if ((_BE_env.emu3C2 & 0x1) && (_BE_env.emu3D4 < CRT_C))
+			_BE_env.emu3D5[_BE_env.emu3D4] = val;
+		break;
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+regOffset   - Offset into register space for non-DWORD accesses
+value       - Value to write to register for PCI_WRITE_* operations
+func        - Function to perform (PCIAccessRegFlags)
+
+RETURNS:
+Value read from configuration register for PCI_READ_* operations
+
+REMARKS:
+Accesses a PCI configuration space register by decoding the value currently
+stored in the _BE_env.configAddress variable and passing it through to the
+portable PCI_accessReg function.
+****************************************************************************/
+static u32 BE_accessReg(int regOffset, u32 value, int func)
+{
+#ifdef __KERNEL__
+	int function, device, bus;
+	u8 val8;
+	u16 val16;
+	u32 val32;
+
+
+	/* Decode the configuration register values for the register we wish to
+	 * access
+	 */
+	regOffset += (_BE_env.configAddress & 0xFF);
+	function = (_BE_env.configAddress >> 8) & 0x7;
+	device = (_BE_env.configAddress >> 11) & 0x1F;
+	bus = (_BE_env.configAddress >> 16) & 0xFF;
+
+	/* Ignore accesses to all devices other than the one we're POSTing */
+	if ((function == _BE_env.vgaInfo.function) &&
+	    (device == _BE_env.vgaInfo.device) &&
+	    (bus == _BE_env.vgaInfo.bus)) {
+		switch (func) {
+		case REG_READ_BYTE:
+			pci_read_config_byte(_BE_env.vgaInfo.pcidev, regOffset,
+					     &val8);
+			return val8;
+		case REG_READ_WORD:
+			pci_read_config_word(_BE_env.vgaInfo.pcidev, regOffset,
+					     &val16);
+			return val16;
+		case REG_READ_DWORD:
+			pci_read_config_dword(_BE_env.vgaInfo.pcidev, regOffset,
+					      &val32);
+			return val32;
+		case REG_WRITE_BYTE:
+			pci_write_config_byte(_BE_env.vgaInfo.pcidev, regOffset,
+					      value);
+
+			return 0;
+		case REG_WRITE_WORD:
+			pci_write_config_word(_BE_env.vgaInfo.pcidev, regOffset,
+					      value);
+
+			return 0;
+		case REG_WRITE_DWORD:
+			pci_write_config_dword(_BE_env.vgaInfo.pcidev,
+					       regOffset, value);
+
+			return 0;
+		}
+	}
+	return 0;
+#else
+	PCIDeviceInfo pciInfo;
+
+	pciInfo.mech1 = 1;
+	pciInfo.slot.i = 0;
+	pciInfo.slot.p.Function = (_BE_env.configAddress >> 8) & 0x7;
+	pciInfo.slot.p.Device = (_BE_env.configAddress >> 11) & 0x1F;
+	pciInfo.slot.p.Bus = (_BE_env.configAddress >> 16) & 0xFF;
+	pciInfo.slot.p.Enable = 1;
+
+	/* Ignore accesses to all devices other than the one we're POSTing */
+	if ((pciInfo.slot.p.Function ==
+	     _BE_env.vgaInfo.pciInfo->slot.p.Function)
+	    && (pciInfo.slot.p.Device == _BE_env.vgaInfo.pciInfo->slot.p.Device)
+	    && (pciInfo.slot.p.Bus == _BE_env.vgaInfo.pciInfo->slot.p.Bus))
+		return PCI_accessReg((_BE_env.configAddress & 0xFF) + regOffset,
+				     value, func, &pciInfo);
+	return 0;
+#endif
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to read from
+type    - Type of access to perform
+
+REMARKS:
+Performs an emulated read from one of the PCI configuration space registers.
+We emulate this using our PCI_accessReg function which will access the PCI
+configuration space registers in a portable fashion.
+****************************************************************************/
+static u32 PCI_inp(int port, int type)
+{
+	switch (type) {
+	case REG_READ_BYTE:
+		if ((_BE_env.configAddress & 0x80000000) && 0xCFC <= port
+		    && port <= 0xCFF)
+			return BE_accessReg(port - 0xCFC, 0, REG_READ_BYTE);
+		break;
+	case REG_READ_WORD:
+		if ((_BE_env.configAddress & 0x80000000) && 0xCFC <= port
+		    && port <= 0xCFF)
+			return BE_accessReg(port - 0xCFC, 0, REG_READ_WORD);
+		break;
+	case REG_READ_DWORD:
+		if (port == 0xCF8)
+			return _BE_env.configAddress;
+		else if ((_BE_env.configAddress & 0x80000000) && port == 0xCFC)
+			return BE_accessReg(0, 0, REG_READ_DWORD);
+		break;
+	}
+	return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+type    - Type of access to perform
+
+REMARKS:
+Performs an emulated write to one of the PCI control registers.
+****************************************************************************/
+static void PCI_outp(int port, u32 val, int type)
+{
+	switch (type) {
+	case REG_WRITE_BYTE:
+		if ((_BE_env.configAddress & 0x80000000) && 0xCFC <= port
+		    && port <= 0xCFF)
+			BE_accessReg(port - 0xCFC, val, REG_WRITE_BYTE);
+		break;
+	case REG_WRITE_WORD:
+		if ((_BE_env.configAddress & 0x80000000) && 0xCFC <= port
+		    && port <= 0xCFF)
+			BE_accessReg(port - 0xCFC, val, REG_WRITE_WORD);
+		break;
+	case REG_WRITE_DWORD:
+		if (port == 0xCF8)
+		{
+			_BE_env.configAddress = val & 0x80FFFFFC;
+		}
+		else if ((_BE_env.configAddress & 0x80000000) && port == 0xCFC)
+			BE_accessReg(0, val, REG_WRITE_DWORD);
+		break;
+	}
+}
+
+#endif
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+
+RETURNS:
+Value read from the I/O port
+
+REMARKS:
+Performs an emulated 8-bit read from an I/O port. We handle special cases
+that we need to emulate in here, and fall through to reflecting the write
+through to the real hardware if we don't need to special case it.
+****************************************************************************/
+u8 X86API BE_inb(X86EMU_pioAddr port)
+{
+	u8 val = 0;
+
+#if defined(DEBUG) || !defined(__i386__)
+	if (IS_VGA_PORT(port)){
+		/*seems reading port 0x3c3 return the high 16 bit of io port*/
+		if(port == 0x3c3)
+			val = LOG_inpb(port);
+		else
+			val = VGA_inpb(port);
+	}
+	else if (IS_TIMER_PORT(port))
+		DB(printf("Can not interept TIMER port now!\n");)
+	else if (IS_SPKR_PORT(port))
+		DB(printf("Can not interept SPEAKER port now!\n");)
+	else if (IS_CMOS_PORT(port))
+		DB(printf("Can not interept CMOS port now!\n");)
+	else if (IS_PCI_PORT(port))
+		val = PCI_inp(port, REG_READ_BYTE);
+	else if (port < 0x100) {
+		DB(printf("WARN: INVALID inb.%04X -> %02X\n", (u16) port, val);)
+		val = LOG_inpb(port);
+	} else
+#endif
+		val = LOG_inpb(port);
+	return val;
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+
+RETURNS:
+Value read from the I/O port
+
+REMARKS:
+Performs an emulated 16-bit read from an I/O port. We handle special cases
+that we need to emulate in here, and fall through to reflecting the write
+through to the real hardware if we don't need to special case it.
+****************************************************************************/
+u16 X86API BE_inw(X86EMU_pioAddr port)
+{
+	u16 val = 0;
+
+#if defined(DEBUG) || !defined(__i386__)
+	if (IS_PCI_PORT(port))
+		val = PCI_inp(port, REG_READ_WORD);
+	else if (port < 0x100) {
+		DB(printf("WARN: Maybe INVALID inw.%04X -> %04X\n", (u16) port, val);)
+		val = LOG_inpw(port);
+	} else
+#endif
+		val = LOG_inpw(port);
+	return val;
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+
+RETURNS:
+Value read from the I/O port
+
+REMARKS:
+Performs an emulated 32-bit read from an I/O port. We handle special cases
+that we need to emulate in here, and fall through to reflecting the write
+through to the real hardware if we don't need to special case it.
+****************************************************************************/
+u32 X86API BE_inl(X86EMU_pioAddr port)
+{
+	u32 val = 0;
+
+#if defined(DEBUG) || !defined(__i386__)
+	if (IS_PCI_PORT(port))
+		val = PCI_inp(port, REG_READ_DWORD);
+	else if (port < 0x100) {
+		val = LOG_inpd(port);
+	} else
+#endif
+		val = LOG_inpd(port);
+	return val;
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+val     - Value to write to port
+
+REMARKS:
+Performs an emulated 8-bit write to an I/O port. We handle special cases
+that we need to emulate in here, and fall through to reflecting the write
+through to the real hardware if we don't need to special case it.
+****************************************************************************/
+void X86API BE_outb(X86EMU_pioAddr port, u8 val)
+{
+#if defined(DEBUG) || !defined(__i386__)
+	if (IS_VGA_PORT(port))
+		VGA_outpb(port, val);
+	else if (IS_TIMER_PORT(port))
+		DB(printf("Can not interept TIMER port now!\n");)
+	else if (IS_SPKR_PORT(port))
+		DB(printf("Can not interept SPEAKER port now!\n");)
+	else if (IS_CMOS_PORT(port))
+		DB(printf("Can not interept CMOS port now!\n");)
+	else if (IS_PCI_PORT(port))
+		PCI_outp(port, val, REG_WRITE_BYTE);
+	else if (port < 0x100) {
+		DB(printf("WARN:Maybe INVALID outb.%04X <- %02X\n", (u16) port, val);)
+		LOG_outpb(port, val);
+	} else
+#endif
+		LOG_outpb(port, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+val     - Value to write to port
+
+REMARKS:
+Performs an emulated 16-bit write to an I/O port. We handle special cases
+that we need to emulate in here, and fall through to reflecting the write
+through to the real hardware if we don't need to special case it.
+****************************************************************************/
+void X86API BE_outw(X86EMU_pioAddr port, u16 val)
+{
+#if defined(DEBUG) || !defined(__i386__)
+		if (IS_VGA_PORT(port)) {
+			VGA_outpb(port, val);
+			VGA_outpb(port + 1, val >> 8);
+		} else if (IS_PCI_PORT(port))
+			PCI_outp(port, val, REG_WRITE_WORD);
+		else if (port < 0x100) {
+			DB(printf("WARN: MAybe INVALID outw.%04X <- %04X\n", (u16) port,
+			       val);)
+			LOG_outpw(port, val);
+		} else
+#endif
+			LOG_outpw(port, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+port    - Port to write to
+val     - Value to write to port
+
+REMARKS:
+Performs an emulated 32-bit write to an I/O port. We handle special cases
+that we need to emulate in here, and fall through to reflecting the write
+through to the real hardware if we don't need to special case it.
+****************************************************************************/
+void X86API BE_outl(X86EMU_pioAddr port, u32 val)
+{
+#if defined(DEBUG) || !defined(__i386__)
+	if (IS_PCI_PORT(port))
+		PCI_outp(port, val, REG_WRITE_DWORD);
+	else if (port < 0x100) {
+		DB(printf("WARN: INVALID outl.%04X <- %08X\n", (u16) port,val);)
+		LOG_outpd(port, val);
+	} else
+#endif
+		LOG_outpd(port, val);
+}
+#endif
diff --git a/drivers/bios_emulator/bios.c b/drivers/bios_emulator/bios.c
new file mode 100644
index 00000000000..7aa1bfb2eb6
--- /dev/null
+++ b/drivers/bios_emulator/bios.c
@@ -0,0 +1,323 @@
+/****************************************************************************
+*
+*                        BIOS emulator and interface
+*                      to Realmode X86 Emulator Library
+*
+*  Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*  Jason Jin <Jason.jin@freescale.com>
+*
+*               Copyright (C) 1996-1999 SciTech Software, Inc.
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Module implementing the BIOS specific functions.
+*
+* 		Jason ported this file to u-boot to run the ATI video card
+* 		video BIOS.
+*
+****************************************************************************/
+
+#include "biosemui.h"
+
+#if defined(CONFIG_BIOSEMU)
+/*----------------------------- Implementation ----------------------------*/
+
+/****************************************************************************
+PARAMETERS:
+intno   - Interrupt number being serviced
+
+REMARKS:
+Handler for undefined interrupts.
+****************************************************************************/
+static void X86API undefined_intr(int intno)
+{
+	if (BE_rdw(intno * 4 + 2) == BIOS_SEG) {
+		DB(printf("biosEmu: undefined interrupt %xh called!\n", intno);)
+	} else
+		X86EMU_prepareForInt(intno);
+}
+
+/****************************************************************************
+PARAMETERS:
+intno   - Interrupt number being serviced
+
+REMARKS:
+This function handles the default system BIOS Int 10h (the default is stored
+in the Int 42h vector by the system BIOS at bootup). We only need to handle
+a small number of special functions used by the BIOS during POST time.
+****************************************************************************/
+static void X86API int42(int intno)
+{
+	if (M.x86.R_AH == 0x12 && M.x86.R_BL == 0x32) {
+		if (M.x86.R_AL == 0) {
+			/* Enable CPU accesses to video memory */
+			PM_outpb(0x3c2, PM_inpb(0x3cc) | (u8) 0x02);
+			return;
+		} else if (M.x86.R_AL == 1) {
+			/* Disable CPU accesses to video memory */
+			PM_outpb(0x3c2, PM_inpb(0x3cc) & (u8) ~ 0x02);
+			return;
+		}
+#ifdef  DEBUG
+		else {
+			printf("int42: unknown function AH=0x12, BL=0x32, AL=%#02x\n",
+			     M.x86.R_AL);
+		}
+#endif
+	}
+#ifdef  DEBUG
+	else {
+		printf("int42: unknown function AH=%#02x, AL=%#02x, BL=%#02x\n",
+		     M.x86.R_AH, M.x86.R_AL, M.x86.R_BL);
+	}
+#endif
+}
+
+/****************************************************************************
+PARAMETERS:
+intno   - Interrupt number being serviced
+
+REMARKS:
+This function handles the default system BIOS Int 10h. If the POST code
+has not yet re-vectored the Int 10h BIOS interrupt vector, we handle this
+by simply calling the int42 interrupt handler above. Very early in the
+BIOS POST process, the vector gets replaced and we simply let the real
+mode interrupt handler process the interrupt.
+****************************************************************************/
+static void X86API int10(int intno)
+{
+	if (BE_rdw(intno * 4 + 2) == BIOS_SEG)
+		int42(intno);
+	else
+		X86EMU_prepareForInt(intno);
+}
+
+/* Result codes returned by the PCI BIOS */
+
+#define SUCCESSFUL          0x00
+#define FUNC_NOT_SUPPORT    0x81
+#define BAD_VENDOR_ID       0x83
+#define DEVICE_NOT_FOUND    0x86
+#define BAD_REGISTER_NUMBER 0x87
+#define SET_FAILED          0x88
+#define BUFFER_TOO_SMALL    0x89
+
+/****************************************************************************
+PARAMETERS:
+intno   - Interrupt number being serviced
+
+REMARKS:
+This function handles the default Int 1Ah interrupt handler for the real
+mode code, which provides support for the PCI BIOS functions. Since we only
+want to allow the real mode BIOS code *only* see the PCI config space for
+its own device, we only return information for the specific PCI config
+space that we have passed in to the init function. This solves problems
+when using the BIOS to warm boot a secondary adapter when there is an
+identical adapter before it on the bus (some BIOS'es get confused in this
+case).
+****************************************************************************/
+static void X86API int1A(int unused)
+{
+	u16 pciSlot;
+
+#ifdef __KERNEL__
+	u8 interface, subclass, baseclass;
+
+	/* Initialise the PCI slot number */
+	pciSlot = ((int)_BE_env.vgaInfo.bus << 8) |
+	    ((int)_BE_env.vgaInfo.device << 3) | (int)_BE_env.vgaInfo.function;
+#else
+/* Fail if no PCI device information has been registered */
+	if (!_BE_env.vgaInfo.pciInfo)
+		return;
+
+	pciSlot = (u16) (_BE_env.vgaInfo.pciInfo->slot.i >> 8);
+#endif
+	switch (M.x86.R_AX) {
+	case 0xB101:		/* PCI bios present? */
+		M.x86.R_AL = 0x00;	/* no config space/special cycle generation support */
+		M.x86.R_EDX = 0x20494350;	/* " ICP" */
+		M.x86.R_BX = 0x0210;	/* Version 2.10 */
+		M.x86.R_CL = 0;	/* Max bus number in system */
+		CLEAR_FLAG(F_CF);
+		break;
+	case 0xB102:		/* Find PCI device */
+		M.x86.R_AH = DEVICE_NOT_FOUND;
+#ifdef __KERNEL__
+		if (M.x86.R_DX == _BE_env.vgaInfo.VendorID &&
+		    M.x86.R_CX == _BE_env.vgaInfo.DeviceID && M.x86.R_SI == 0) {
+#else
+		if (M.x86.R_DX == _BE_env.vgaInfo.pciInfo->VendorID &&
+		    M.x86.R_CX == _BE_env.vgaInfo.pciInfo->DeviceID &&
+		    M.x86.R_SI == 0) {
+#endif
+			M.x86.R_AH = SUCCESSFUL;
+			M.x86.R_BX = pciSlot;
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB103:		/* Find PCI class code */
+		M.x86.R_AH = DEVICE_NOT_FOUND;
+#ifdef __KERNEL__
+		pci_read_config_byte(_BE_env.vgaInfo.pcidev, PCI_CLASS_PROG,
+				     &interface);
+		pci_read_config_byte(_BE_env.vgaInfo.pcidev, PCI_CLASS_DEVICE,
+				     &subclass);
+		pci_read_config_byte(_BE_env.vgaInfo.pcidev,
+				     PCI_CLASS_DEVICE + 1, &baseclass);
+		if (M.x86.R_CL == interface && M.x86.R_CH == subclass
+		    && (u8) (M.x86.R_ECX >> 16) == baseclass) {
+#else
+		if (M.x86.R_CL == _BE_env.vgaInfo.pciInfo->Interface &&
+		    M.x86.R_CH == _BE_env.vgaInfo.pciInfo->SubClass &&
+		    (u8) (M.x86.R_ECX >> 16) ==
+		    _BE_env.vgaInfo.pciInfo->BaseClass) {
+#endif
+			M.x86.R_AH = SUCCESSFUL;
+			M.x86.R_BX = pciSlot;
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB108:		/* Read configuration byte */
+		M.x86.R_AH = BAD_REGISTER_NUMBER;
+		if (M.x86.R_BX == pciSlot) {
+			M.x86.R_AH = SUCCESSFUL;
+#ifdef __KERNEL__
+			pci_read_config_byte(_BE_env.vgaInfo.pcidev, M.x86.R_DI,
+					     &M.x86.R_CL);
+#else
+			M.x86.R_CL =
+			    (u8) PCI_accessReg(M.x86.R_DI, 0, PCI_READ_BYTE,
+					       _BE_env.vgaInfo.pciInfo);
+#endif
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB109:		/* Read configuration word */
+		M.x86.R_AH = BAD_REGISTER_NUMBER;
+		if (M.x86.R_BX == pciSlot) {
+			M.x86.R_AH = SUCCESSFUL;
+#ifdef __KERNEL__
+			pci_read_config_word(_BE_env.vgaInfo.pcidev, M.x86.R_DI,
+					     &M.x86.R_CX);
+#else
+			M.x86.R_CX =
+			    (u16) PCI_accessReg(M.x86.R_DI, 0, PCI_READ_WORD,
+						_BE_env.vgaInfo.pciInfo);
+#endif
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB10A:		/* Read configuration dword */
+		M.x86.R_AH = BAD_REGISTER_NUMBER;
+		if (M.x86.R_BX == pciSlot) {
+			M.x86.R_AH = SUCCESSFUL;
+#ifdef __KERNEL__
+			pci_read_config_dword(_BE_env.vgaInfo.pcidev,
+					      M.x86.R_DI, &M.x86.R_ECX);
+#else
+			M.x86.R_ECX =
+			    (u32) PCI_accessReg(M.x86.R_DI, 0, PCI_READ_DWORD,
+						_BE_env.vgaInfo.pciInfo);
+#endif
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB10B:		/* Write configuration byte */
+		M.x86.R_AH = BAD_REGISTER_NUMBER;
+		if (M.x86.R_BX == pciSlot) {
+			M.x86.R_AH = SUCCESSFUL;
+#ifdef __KERNEL__
+			pci_write_config_byte(_BE_env.vgaInfo.pcidev,
+					      M.x86.R_DI, M.x86.R_CL);
+#else
+			PCI_accessReg(M.x86.R_DI, M.x86.R_CL, PCI_WRITE_BYTE,
+				      _BE_env.vgaInfo.pciInfo);
+#endif
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB10C:		/* Write configuration word */
+		M.x86.R_AH = BAD_REGISTER_NUMBER;
+		if (M.x86.R_BX == pciSlot) {
+			M.x86.R_AH = SUCCESSFUL;
+#ifdef __KERNEL__
+			pci_write_config_word(_BE_env.vgaInfo.pcidev,
+					      M.x86.R_DI, M.x86.R_CX);
+#else
+			PCI_accessReg(M.x86.R_DI, M.x86.R_CX, PCI_WRITE_WORD,
+				      _BE_env.vgaInfo.pciInfo);
+#endif
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	case 0xB10D:		/* Write configuration dword */
+		M.x86.R_AH = BAD_REGISTER_NUMBER;
+		if (M.x86.R_BX == pciSlot) {
+			M.x86.R_AH = SUCCESSFUL;
+#ifdef __KERNEL__
+			pci_write_config_dword(_BE_env.vgaInfo.pcidev,
+					       M.x86.R_DI, M.x86.R_ECX);
+#else
+			PCI_accessReg(M.x86.R_DI, M.x86.R_ECX, PCI_WRITE_DWORD,
+				      _BE_env.vgaInfo.pciInfo);
+#endif
+		}
+		CONDITIONAL_SET_FLAG((M.x86.R_AH != SUCCESSFUL), F_CF);
+		break;
+	default:
+		printf("biosEmu/bios.int1a: unknown function AX=%#04x\n",
+		       M.x86.R_AX);
+	}
+}
+
+/****************************************************************************
+REMARKS:
+This function initialises the BIOS emulation functions for the specific
+PCI display device. We insulate the real mode BIOS from any other devices
+on the bus, so that it will work correctly thinking that it is the only
+device present on the bus (ie: avoiding any adapters present in from of
+the device we are trying to control).
+****************************************************************************/
+#define BE_constLE_32(v)    ((((((v)&0xff00)>>8)|(((v)&0xff)<<8))<<16)|(((((v)&0xff000000)>>8)|(((v)&0x00ff0000)<<8))>>16))
+
+void _BE_bios_init(u32 * intrTab)
+{
+	int i;
+	X86EMU_intrFuncs bios_intr_tab[256];
+
+	for (i = 0; i < 256; ++i) {
+		intrTab[i] = BE_constLE_32(BIOS_SEG << 16);
+		bios_intr_tab[i] = undefined_intr;
+	}
+	bios_intr_tab[0x10] = int10;
+	bios_intr_tab[0x1A] = int1A;
+	bios_intr_tab[0x42] = int42;
+	bios_intr_tab[0x6D] = int10;
+	X86EMU_setupIntrFuncs(bios_intr_tab);
+}
+#endif
diff --git a/drivers/bios_emulator/biosemu.c b/drivers/bios_emulator/biosemu.c
new file mode 100644
index 00000000000..4c3aedf4130
--- /dev/null
+++ b/drivers/bios_emulator/biosemu.c
@@ -0,0 +1,373 @@
+/****************************************************************************
+*
+*			 BIOS emulator and interface
+*		       to Realmode X86 Emulator Library
+*
+*  Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*  Jason Jin <Jason.jin@freescale.com>
+*
+*		Copyright (C) 1996-1999 SciTech Software, Inc.
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	Module implementing the system specific functions. This
+*		module is always compiled and linked in the OS depedent
+*		libraries, and never in a binary portable driver.
+*
+*		Jason ported this file to u-boot to run the ATI video card BIOS
+*		in u-boot. Made all the video memory be emulated during the
+*		BIOS runing process which may affect the VGA function but the
+*		frambuffer function can work after run the BIOS.
+*
+****************************************************************************/
+
+#include "biosemui.h"
+#include <malloc.h>
+
+#if defined(CONFIG_BIOSEMU)
+
+BE_sysEnv _BE_env = {{0}};
+static X86EMU_memFuncs _BE_mem __attribute__((section(".got2"))) = {
+	BE_rdb,
+	BE_rdw,
+	BE_rdl,
+	BE_wrb,
+	BE_wrw,
+	BE_wrl,
+	};
+
+static X86EMU_pioFuncs _BE_pio __attribute__((section(".got2"))) = {
+	BE_inb,
+	BE_inw,
+	BE_inl,
+	BE_outb,
+	BE_outw,
+	BE_outl,
+	};
+
+#define OFF(addr)	(u16)(((addr) >> 0) & 0xffff)
+#define SEG(addr)	(u16)(((addr) >> 4) & 0xf000)
+
+/****************************************************************************
+PARAMETERS:
+debugFlags  - Flags to enable debugging options (debug builds only)
+memSize	    - Amount of memory to allocate for real mode machine
+info	    - Pointer to default VGA device information
+
+REMARKS:
+This functions initialises the BElib, and uses the passed in
+BIOS image as the BIOS that is used and emulated at 0xC0000.
+****************************************************************************/
+int X86API BE_init(u32 debugFlags, int memSize, BE_VGAInfo * info, int shared)
+{
+#if !defined(__DRIVER__)  && !defined(__KERNEL__)
+
+	PM_init();
+#endif
+	memset(&M, 0, sizeof(M));
+	if (memSize < 20480){
+		printf("Emulator requires at least 20Kb of memory!\n");
+		return 0;
+	}
+
+	M.mem_base = (unsigned long)malloc(memSize);
+
+	if (M.mem_base == NULL){
+		printf("Biosemu:Out of memory!");
+		return 0;
+	}
+	M.mem_size = memSize;
+
+	_BE_env.emulateVGA = 0;
+	_BE_env.busmem_base = (unsigned long)malloc(128 * 1024);
+	if (_BE_env.busmem_base == NULL){
+		printf("Biosemu:Out of memory!");
+		return 0;
+	}
+	M.x86.debug = debugFlags;
+	_BE_bios_init((u32*)info->LowMem);
+	X86EMU_setupMemFuncs(&_BE_mem);
+	X86EMU_setupPioFuncs(&_BE_pio);
+	BE_setVGA(info);
+	return 1;
+}
+
+/****************************************************************************
+PARAMETERS:
+info	    - Pointer to VGA device information to make current
+
+REMARKS:
+This function sets the VGA BIOS functions in the emulator to point to the
+specific VGA BIOS in use. This includes swapping the BIOS interrupt
+vectors, BIOS image and BIOS data area to the new BIOS. This allows the
+real mode BIOS to be swapped without resetting the entire emulator.
+****************************************************************************/
+void X86API BE_setVGA(BE_VGAInfo * info)
+{
+
+#ifdef __KERNEL__
+	_BE_env.vgaInfo.function = info->function;
+	_BE_env.vgaInfo.device = info->device;
+	_BE_env.vgaInfo.bus = info->bus;
+	_BE_env.vgaInfo.pcidev = info->pcidev;
+#else
+	_BE_env.vgaInfo.pciInfo = info->pciInfo;
+#endif
+	_BE_env.vgaInfo.BIOSImage = info->BIOSImage;
+	if (info->BIOSImage) {
+		_BE_env.biosmem_base = (ulong) info->BIOSImage;
+		_BE_env.biosmem_limit = 0xC0000 + info->BIOSImageLen - 1;
+	} else {
+		_BE_env.biosmem_base = _BE_env.busmem_base + 0x20000;
+		_BE_env.biosmem_limit = 0xC7FFF;
+	}
+	if (*((u32 *) info->LowMem) == 0)
+		_BE_bios_init((u32 *) info->LowMem);
+	memcpy((u8 *) M.mem_base, info->LowMem, sizeof(info->LowMem));
+}
+
+/****************************************************************************
+PARAMETERS:
+info	    - Pointer to VGA device information to retrieve current
+
+REMARKS:
+This function returns the VGA BIOS functions currently active in the
+emulator, so they can be restored at a later date.
+****************************************************************************/
+void X86API BE_getVGA(BE_VGAInfo * info)
+{
+#ifdef __KERNEL__
+	info->function = _BE_env.vgaInfo.function;
+	info->device = _BE_env.vgaInfo.device;
+	info->bus = _BE_env.vgaInfo.bus;
+	info->pcidev = _BE_env.vgaInfo.pcidev;
+#else
+	info->pciInfo = _BE_env.vgaInfo.pciInfo;
+#endif
+	info->BIOSImage = _BE_env.vgaInfo.BIOSImage;
+	memcpy(info->LowMem, (u8 *) M.mem_base, sizeof(info->LowMem));
+}
+
+/****************************************************************************
+PARAMETERS:
+r_seg	- Segment for pointer to convert
+r_off	- Offset for pointer to convert
+
+REMARKS:
+This function maps a real mode pointer in the emulator memory to a protected
+mode pointer that can be used to directly access the memory.
+
+NOTE:	The memory is *always* in little endian format, son on non-x86
+	systems you will need to do endian translations to access this
+	memory.
+****************************************************************************/
+void *X86API BE_mapRealPointer(uint r_seg, uint r_off)
+{
+	u32 addr = ((u32) r_seg << 4) + r_off;
+
+	if (addr >= 0xC0000 && addr <= _BE_env.biosmem_limit) {
+		return (void *)(_BE_env.biosmem_base + addr - 0xC0000);
+	} else if (addr >= 0xA0000 && addr <= 0xFFFFF) {
+		return (void *)(_BE_env.busmem_base + addr - 0xA0000);
+	}
+	return (void *)(M.mem_base + addr);
+}
+
+/****************************************************************************
+PARAMETERS:
+len	- Return the length of the VESA buffer
+rseg	- Place to store VESA buffer segment
+roff	- Place to store VESA buffer offset
+
+REMARKS:
+This function returns the address of the VESA transfer buffer in real
+_BE_piomode emulator memory. The VESA transfer buffer is always 1024 bytes long,
+and located at 15Kb into the start of the real mode memory (16Kb is where
+we put the real mode code we execute for issuing interrupts).
+
+NOTE:	The memory is *always* in little endian format, son on non-x86
+	systems you will need to do endian translations to access this
+	memory.
+****************************************************************************/
+void *X86API BE_getVESABuf(uint * len, uint * rseg, uint * roff)
+{
+	*len = 1024;
+	*rseg = SEG(0x03C00);
+	*roff = OFF(0x03C00);
+	return (void *)(M.mem_base + ((u32) * rseg << 4) + *roff);
+}
+
+/****************************************************************************
+REMARKS:
+Cleans up and exits the emulator.
+****************************************************************************/
+void X86API BE_exit(void)
+{
+	free(M.mem_base);
+	free(_BE_env.busmem_base);
+}
+
+/****************************************************************************
+PARAMETERS:
+seg	- Segment of code to call
+off	- Offset of code to call
+regs	- Real mode registers to load
+sregs	- Real mode segment registers to load
+
+REMARKS:
+This functions calls a real mode far function at the specified address,
+and loads all the x86 registers from the passed in registers structure.
+On exit the registers returned from the call are returned in the same
+structures.
+****************************************************************************/
+void X86API BE_callRealMode(uint seg, uint off, RMREGS * regs, RMSREGS * sregs)
+{
+	M.x86.R_EAX = regs->e.eax;
+	M.x86.R_EBX = regs->e.ebx;
+	M.x86.R_ECX = regs->e.ecx;
+	M.x86.R_EDX = regs->e.edx;
+	M.x86.R_ESI = regs->e.esi;
+	M.x86.R_EDI = regs->e.edi;
+	M.x86.R_DS = sregs->ds;
+	M.x86.R_ES = sregs->es;
+	M.x86.R_FS = sregs->fs;
+	M.x86.R_GS = sregs->gs;
+
+	((u8 *) M.mem_base)[0x4000] = 0x9A;
+	((u8 *) M.mem_base)[0x4001] = (u8) off;
+	((u8 *) M.mem_base)[0x4002] = (u8) (off >> 8);
+	((u8 *) M.mem_base)[0x4003] = (u8) seg;
+	((u8 *) M.mem_base)[0x4004] = (u8) (seg >> 8);
+	((u8 *) M.mem_base)[0x4005] = 0xF1;	/* Illegal op-code */
+	M.x86.R_CS = SEG(0x04000);
+	M.x86.R_IP = OFF(0x04000);
+
+	M.x86.R_SS = SEG(M.mem_size - 2);
+	M.x86.R_SP = OFF(M.mem_size - 2) + 2;
+
+	X86EMU_exec();
+
+	regs->e.cflag = M.x86.R_EFLG & F_CF;
+	regs->e.eax = M.x86.R_EAX;
+	regs->e.ebx = M.x86.R_EBX;
+	regs->e.ecx = M.x86.R_ECX;
+	regs->e.edx = M.x86.R_EDX;
+	regs->e.esi = M.x86.R_ESI;
+	regs->e.edi = M.x86.R_EDI;
+	sregs->ds = M.x86.R_DS;
+	sregs->es = M.x86.R_ES;
+	sregs->fs = M.x86.R_FS;
+	sregs->gs = M.x86.R_GS;
+}
+
+/****************************************************************************
+PARAMETERS:
+intno	- Interrupt number to execute
+in	- Real mode registers to load
+out	- Place to store resulting real mode registers
+
+REMARKS:
+This functions calls a real mode interrupt function at the specified address,
+and loads all the x86 registers from the passed in registers structure.
+On exit the registers returned from the call are returned in out stucture.
+****************************************************************************/
+int X86API BE_int86(int intno, RMREGS * in, RMREGS * out)
+{
+	M.x86.R_EAX = in->e.eax;
+	M.x86.R_EBX = in->e.ebx;
+	M.x86.R_ECX = in->e.ecx;
+	M.x86.R_EDX = in->e.edx;
+	M.x86.R_ESI = in->e.esi;
+	M.x86.R_EDI = in->e.edi;
+	((u8 *) M.mem_base)[0x4000] = 0xCD;
+	((u8 *) M.mem_base)[0x4001] = (u8) intno;
+	((u8 *) M.mem_base)[0x4002] = 0xF1;
+	M.x86.R_CS = SEG(0x04000);
+	M.x86.R_IP = OFF(0x04000);
+
+	M.x86.R_SS = SEG(M.mem_size - 1);
+	M.x86.R_SP = OFF(M.mem_size - 1) - 1;
+
+	X86EMU_exec();
+	out->e.cflag = M.x86.R_EFLG & F_CF;
+	out->e.eax = M.x86.R_EAX;
+	out->e.ebx = M.x86.R_EBX;
+	out->e.ecx = M.x86.R_ECX;
+	out->e.edx = M.x86.R_EDX;
+	out->e.esi = M.x86.R_ESI;
+	out->e.edi = M.x86.R_EDI;
+	return out->x.ax;
+}
+
+/****************************************************************************
+PARAMETERS:
+intno	- Interrupt number to execute
+in	- Real mode registers to load
+out	- Place to store resulting real mode registers
+sregs	- Real mode segment registers to load
+
+REMARKS:
+This functions calls a real mode interrupt function at the specified address,
+and loads all the x86 registers from the passed in registers structure.
+On exit the registers returned from the call are returned in out stucture.
+****************************************************************************/
+int X86API BE_int86x(int intno, RMREGS * in, RMREGS * out, RMSREGS * sregs)
+{
+	M.x86.R_EAX = in->e.eax;
+	M.x86.R_EBX = in->e.ebx;
+	M.x86.R_ECX = in->e.ecx;
+	M.x86.R_EDX = in->e.edx;
+	M.x86.R_ESI = in->e.esi;
+	M.x86.R_EDI = in->e.edi;
+	M.x86.R_DS = sregs->ds;
+	M.x86.R_ES = sregs->es;
+	M.x86.R_FS = sregs->fs;
+	M.x86.R_GS = sregs->gs;
+	((u8 *) M.mem_base)[0x4000] = 0xCD;
+	((u8 *) M.mem_base)[0x4001] = (u8) intno;
+	((u8 *) M.mem_base)[0x4002] = 0xF1;
+	M.x86.R_CS = SEG(0x04000);
+	M.x86.R_IP = OFF(0x04000);
+
+	M.x86.R_SS = SEG(M.mem_size - 1);
+	M.x86.R_SP = OFF(M.mem_size - 1) - 1;
+
+	X86EMU_exec();
+	out->e.cflag = M.x86.R_EFLG & F_CF;
+	out->e.eax = M.x86.R_EAX;
+	out->e.ebx = M.x86.R_EBX;
+	out->e.ecx = M.x86.R_ECX;
+	out->e.edx = M.x86.R_EDX;
+	out->e.esi = M.x86.R_ESI;
+	out->e.edi = M.x86.R_EDI;
+	sregs->ds = M.x86.R_DS;
+	sregs->es = M.x86.R_ES;
+	sregs->fs = M.x86.R_FS;
+	sregs->gs = M.x86.R_GS;
+	return out->x.ax;
+}
+#endif
diff --git a/drivers/bios_emulator/biosemui.h b/drivers/bios_emulator/biosemui.h
new file mode 100644
index 00000000000..e85e656cb45
--- /dev/null
+++ b/drivers/bios_emulator/biosemui.h
@@ -0,0 +1,169 @@
+/****************************************************************************
+*
+*			 BIOS emulator and interface
+*		       to Realmode X86 Emulator Library
+*
+*  Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*  Jason Jin <Jason.jin@freescale.com>
+*
+*		Copyright (C) 1996-1999 SciTech Software, Inc.
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	Internal header file for the BIOS emulator library.
+*
+*		Jason ported this file to u-boot, Added some architecture
+*		related Macro.
+*
+****************************************************************************/
+
+#ifndef __BIOSEMUI_H
+#define __BIOSEMUI_H
+
+#include "biosemu.h"
+#include <asm/io.h>
+/*---------------------- Macros and type definitions ----------------------*/
+
+#ifdef DEBUG
+#define DB(x)	x
+#else
+#define DB(x)	do{}while(0);
+#endif
+
+#define BIOS_SEG	0xfff0
+extern X86EMU_sysEnv _X86EMU_env;
+#define M		_X86EMU_env
+
+/* Macros to read and write values to x86 emulator memory. Memory is always
+ * considered to be little endian, so we use macros to do endian swapping
+ * where necessary.
+ */
+
+#ifdef __BIG_ENDIAN__
+#define readb_le(base)	    *((u8*)(base))
+#define readw_le(base)	    ((u16)readb_le(base) | ((u16)readb_le((base) + 1) << 8))
+#define readl_le(base)	    ((u32)readb_le((base) + 0) | ((u32)readb_le((base) + 1) << 8) | \
+			    ((u32)readb_le((base) + 2) << 16) | ((u32)readb_le((base) + 3) << 24))
+#define writeb_le(base, v)  *((u8*)(base)) = (v)
+#define writew_le(base, v)  writeb_le(base + 0, (v >> 0) & 0xff),	\
+			    writeb_le(base + 1, (v >> 8) & 0xff)
+#define writel_le(base, v)  writeb_le(base + 0, (v >> 0) & 0xff),	\
+			    writeb_le(base + 1, (v >> 8) & 0xff),	\
+			    writeb_le(base + 2, (v >> 16) & 0xff),	\
+			    writeb_le(base + 3, (v >> 24) & 0xff)
+#else
+#define readb_le(base)	    *((u8*)(base))
+#define readw_le(base)	    *((u16*)(base))
+#define readl_le(base)	    *((u32*)(base))
+#define writeb_le(base, v)  *((u8*)(base)) = (v)
+#define writew_le(base, v)  *((u16*)(base)) = (v)
+#define writel_le(base, v)  *((u32*)(base)) = (v)
+#endif
+
+/****************************************************************************
+REMARKS:
+Function codes passed to the emulated I/O port functions to determine the
+type of operation to perform.
+****************************************************************************/
+typedef enum {
+	REG_READ_BYTE = 0,
+	REG_READ_WORD = 1,
+	REG_READ_DWORD = 2,
+	REG_WRITE_BYTE = 3,
+	REG_WRITE_WORD = 4,
+	REG_WRITE_DWORD = 5
+} RegisterFlags;
+
+/****************************************************************************
+REMARKS:
+Function codes passed to the emulated I/O port functions to determine the
+type of operation to perform.
+****************************************************************************/
+typedef enum {
+	PORT_BYTE = 1,
+	PORT_WORD = 2,
+	PORT_DWORD = 3,
+} PortInfoFlags;
+
+/****************************************************************************
+REMARKS:
+Data structure used to describe the details for the BIOS emulator system
+environment as used by the X86 emulator library.
+
+HEADER:
+biosemu.h
+
+MEMBERS:
+type	    - Type of port access (1 = byte, 2 = word, 3 = dword)
+defVal	    - Default power on value
+finalVal    - Final value
+****************************************************************************/
+typedef struct {
+	u8 type;
+	u32 defVal;
+	u32 finalVal;
+} BE_portInfo;
+
+#define PM_inpb(port)	inb(port+VIDEO_IO_OFFSET)
+#define PM_inpw(port)	inw(port+VIDEO_IO_OFFSET)
+#define PM_inpd(port)	inl(port+VIDEO_IO_OFFSET)
+#define PM_outpb(port,val)	outb(val,port+VIDEO_IO_OFFSET)
+#define PM_outpw(port,val)	outw(val,port+VIDEO_IO_OFFSET)
+#define PM_outpd(port,val)	outl(val,port+VIDEO_IO_OFFSET)
+
+#define LOG_inpb(port)	PM_inpb(port)
+#define LOG_inpw(port)	PM_inpw(port)
+#define LOG_inpd(port)	PM_inpd(port)
+#define LOG_outpb(port,val)	PM_outpb(port,val)
+#define LOG_outpw(port,val)	PM_outpw(port,val)
+#define LOG_outpd(port,val)	PM_outpd(port,val)
+
+/*-------------------------- Function Prototypes --------------------------*/
+
+/* bios.c */
+
+void _BE_bios_init(u32 * intrTab);
+void _BE_setup_funcs(void);
+
+/* besys.c */
+#define DEBUG_IO()	(M.x86.debug & DEBUG_IO_TRACE_F)
+
+u8 X86API BE_rdb(u32 addr);
+u16 X86API BE_rdw(u32 addr);
+u32 X86API BE_rdl(u32 addr);
+void X86API BE_wrb(u32 addr, u8 val);
+void X86API BE_wrw(u32 addr, u16 val);
+void X86API BE_wrl(u32 addr, u32 val);
+
+u8 X86API BE_inb(X86EMU_pioAddr port);
+u16 X86API BE_inw(X86EMU_pioAddr port);
+u32 X86API BE_inl(X86EMU_pioAddr port);
+void X86API BE_outb(X86EMU_pioAddr port, u8 val);
+void X86API BE_outw(X86EMU_pioAddr port, u16 val);
+void X86API BE_outl(X86EMU_pioAddr port, u32 val);
+#endif
+/* __BIOSEMUI_H */
diff --git a/drivers/bios_emulator/include/biosemu.h b/drivers/bios_emulator/include/biosemu.h
new file mode 100644
index 00000000000..13cb3170c18
--- /dev/null
+++ b/drivers/bios_emulator/include/biosemu.h
@@ -0,0 +1,392 @@
+/****************************************************************************
+*
+*                        BIOS emulator and interface
+*                      to Realmode X86 Emulator Library
+*
+*               Copyright (C) 1996-1999 SciTech Software, Inc.
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Header file for the real mode x86 BIOS emulator, which is
+*               used to warmboot any number of VGA compatible PCI/AGP
+*               controllers under any OS, on any processor family that
+*               supports PCI. We also allow the user application to call
+*               real mode BIOS functions and Int 10h functions (including
+*               the VESA BIOS).
+*
+****************************************************************************/
+
+#ifndef __BIOSEMU_H
+#define __BIOSEMU_H
+
+#ifdef __KERNEL__
+#include "x86emu.h"
+#else
+#include "x86emu.h"
+#include "pmapi.h"
+#include "pcilib.h"
+#endif
+
+/*---------------------- Macros and type definitions ----------------------*/
+
+#pragma pack(1)
+
+#ifndef __KERNEL__
+/****************************************************************************
+REMARKS:
+Data structure used to describe the details specific to a particular VGA
+controller. This information is used to allow the VGA controller to be
+swapped on the fly within the BIOS emulator.
+
+HEADER:
+biosemu.h
+
+MEMBERS:
+pciInfo         - PCI device information block for the controller
+BIOSImage       - Pointer to a read/write copy of the BIOS image
+BIOSImageLen    - Length of the BIOS image
+LowMem          - Copy of key low memory areas
+****************************************************************************/
+typedef struct {
+	PCIDeviceInfo *pciInfo;
+	void *BIOSImage;
+	ulong BIOSImageLen;
+	uchar LowMem[1536];
+} BE_VGAInfo;
+#else
+/****************************************************************************
+REMARKS:
+Data structure used to describe the details for the BIOS emulator system
+environment as used by the X86 emulator library.
+
+HEADER:
+biosemu.h
+
+MEMBERS:
+vgaInfo         - VGA BIOS information structure
+biosmem_base    - Base of the BIOS image
+biosmem_limit   - Limit of the BIOS image
+busmem_base     - Base of the VGA bus memory
+****************************************************************************/
+typedef struct {
+	int function;
+	int device;
+	int bus;
+	u32 VendorID;
+	u32 DeviceID;
+	pci_dev_t pcidev;
+	void *BIOSImage;
+	u32 BIOSImageLen;
+	u8 LowMem[1536];
+} BE_VGAInfo;
+
+#endif				/* __KERNEL__ */
+
+#define CRT_C   24		/* 24  CRT Controller Registers             */
+#define ATT_C   21		/* 21  Attribute Controller Registers       */
+#define GRA_C   9		/* 9   Graphics Controller Registers        */
+#define SEQ_C   5		/* 5   Sequencer Registers                  */
+#define PAL_C   768		/* 768 Palette Registers                    */
+
+/****************************************************************************
+REMARKS:
+Data structure used to describe the details for the BIOS emulator system
+environment as used by the X86 emulator library.
+
+HEADER:
+biosemu.h
+
+MEMBERS:
+vgaInfo         - VGA BIOS information structure
+biosmem_base    - Base of the BIOS image
+biosmem_limit   - Limit of the BIOS image
+busmem_base     - Base of the VGA bus memory
+timer           - Timer used to emulate PC timer ports
+timer0          - Latched value for timer 0
+timer0Latched   - True if timer 0 value was just latched
+timer2          - Current value for timer 2
+emulateVGA      - True to emulate VGA I/O and memory accesses
+****************************************************************************/
+
+typedef struct {
+	BE_VGAInfo vgaInfo;
+	ulong biosmem_base;
+	ulong biosmem_limit;
+	ulong busmem_base;
+
+	u32 timer0;
+	int timer0Latched;
+	u32 timer1;
+	int timer1Latched;
+	u32 timer2;
+	int timer2Latched;
+
+	int emulateVGA;
+	u8 emu61;
+	u8 emu70;
+	int flipFlop3C0;
+	u32 configAddress;
+	u8 emu3C0;
+	u8 emu3C1[ATT_C];
+	u8 emu3C2;
+	u8 emu3C4;
+	u8 emu3C5[SEQ_C];
+	u8 emu3C6;
+	uint emu3C7;
+	uint emu3C8;
+	u8 emu3C9[PAL_C];
+	u8 emu3CE;
+	u8 emu3CF[GRA_C];
+	u8 emu3D4;
+	u8 emu3D5[CRT_C];
+	u8 emu3DA;
+
+} BE_sysEnv;
+
+#ifdef __KERNEL__
+
+/* Define some types when compiling for the Linux kernel that normally
+ * come from the SciTech PM library.
+ */
+
+/****************************************************************************
+REMARKS:
+Structure describing the 32-bit extended x86 CPU registers
+
+HEADER:
+pmapi.h
+
+MEMBERS:
+eax     - Value of the EAX register
+ebx     - Value of the EBX register
+ecx     - Value of the ECX register
+edx     - Value of the EDX register
+esi     - Value of the ESI register
+edi     - Value of the EDI register
+cflag   - Value of the carry flag
+****************************************************************************/
+typedef struct {
+	u32 eax;
+	u32 ebx;
+	u32 ecx;
+	u32 edx;
+	u32 esi;
+	u32 edi;
+	u32 cflag;
+} RMDWORDREGS;
+
+/****************************************************************************
+REMARKS:
+Structure describing the 16-bit x86 CPU registers
+
+HEADER:
+pmapi.h
+
+MEMBERS:
+ax      - Value of the AX register
+bx      - Value of the BX register
+cx      - Value of the CX register
+dx      - Value of the DX register
+si      - Value of the SI register
+di      - Value of the DI register
+cflag   - Value of the carry flag
+****************************************************************************/
+#ifdef __BIG_ENDIAN__
+typedef struct {
+	u16 ax_hi, ax;
+	u16 bx_hi, bx;
+	u16 cx_hi, cx;
+	u16 dx_hi, dx;
+	u16 si_hi, si;
+	u16 di_hi, di;
+	u16 cflag_hi, cflag;
+} RMWORDREGS;
+#else
+typedef struct {
+	u16 ax, ax_hi;
+	u16 bx, bx_hi;
+	u16 cx, cx_hi;
+	u16 dx, dx_hi;
+	u16 si, si_hi;
+	u16 di, di_hi;
+	u16 cflag, cflag_hi;
+} RMWORDREGS;
+#endif
+
+/****************************************************************************
+REMARKS:
+Structure describing the 8-bit x86 CPU registers
+
+HEADER:
+pmapi.h
+
+MEMBERS:
+al      - Value of the AL register
+ah      - Value of the AH register
+bl      - Value of the BL register
+bh      - Value of the BH register
+cl      - Value of the CL register
+ch      - Value of the CH register
+dl      - Value of the DL register
+dh      - Value of the DH register
+****************************************************************************/
+#ifdef __BIG_ENDIAN__
+typedef struct {
+	u16 ax_hi;
+	u8 ah, al;
+	u16 bx_hi;
+	u8 bh, bl;
+	u16 cx_hi;
+	u8 ch, cl;
+	u16 dx_hi;
+	u8 dh, dl;
+} RMBYTEREGS;
+#else
+typedef struct {
+	u8 al;
+	u8 ah;
+	u16 ax_hi;
+	u8 bl;
+	u8 bh;
+	u16 bx_hi;
+	u8 cl;
+	u8 ch;
+	u16 cx_hi;
+	u8 dl;
+	u8 dh;
+	u16 dx_hi;
+} RMBYTEREGS;
+#endif
+
+/****************************************************************************
+REMARKS:
+Structure describing all the x86 CPU registers
+
+HEADER:
+pmapi.h
+
+MEMBERS:
+e   - Member to access registers as 32-bit values
+x   - Member to access registers as 16-bit values
+h   - Member to access registers as 8-bit values
+****************************************************************************/
+typedef union {
+	RMDWORDREGS e;
+	RMWORDREGS x;
+	RMBYTEREGS h;
+} RMREGS;
+
+/****************************************************************************
+REMARKS:
+Structure describing all the x86 segment registers
+
+HEADER:
+pmapi.h
+
+MEMBERS:
+es  - ES segment register
+cs  - CS segment register
+ss  - SS segment register
+ds  - DS segment register
+fs  - FS segment register
+gs  - GS segment register
+****************************************************************************/
+typedef struct {
+	u16 es;
+	u16 cs;
+	u16 ss;
+	u16 ds;
+	u16 fs;
+	u16 gs;
+} RMSREGS;
+
+#endif				/* __KERNEL__ */
+
+#ifndef __KERNEL__
+
+/****************************************************************************
+REMARKS:
+Structure defining all the BIOS Emulator API functions as exported from
+the Binary Portable DLL.
+{secret}
+****************************************************************************/
+typedef struct {
+	ulong dwSize;
+	 ibool(PMAPIP BE_init) (u32 debugFlags, int memSize, BE_VGAInfo * info);
+	void (PMAPIP BE_setVGA) (BE_VGAInfo * info);
+	void (PMAPIP BE_getVGA) (BE_VGAInfo * info);
+	void *(PMAPIP BE_mapRealPointer) (uint r_seg, uint r_off);
+	void *(PMAPIP BE_getVESABuf) (uint * len, uint * rseg, uint * roff);
+	void (PMAPIP BE_callRealMode) (uint seg, uint off, RMREGS * regs,
+				       RMSREGS * sregs);
+	int (PMAPIP BE_int86) (int intno, RMREGS * in, RMREGS * out);
+	int (PMAPIP BE_int86x) (int intno, RMREGS * in, RMREGS * out,
+				RMSREGS * sregs);
+	void *reserved1;
+	void (PMAPIP BE_exit) (void);
+} BE_exports;
+
+/****************************************************************************
+REMARKS:
+Function pointer type for the Binary Portable DLL initialisation entry point.
+{secret}
+****************************************************************************/
+typedef BE_exports *(PMAPIP BE_initLibrary_t) (PM_imports * PMImp);
+#endif
+
+#pragma pack()
+
+/*---------------------------- Global variables ---------------------------*/
+
+#ifdef  __cplusplus
+extern "C" {			/* Use "C" linkage when in C++ mode */
+#endif
+
+/* {secret} Global BIOS emulator system environment */
+	extern BE_sysEnv _BE_env;
+
+/*-------------------------- Function Prototypes --------------------------*/
+
+/* BIOS emulator library entry points */
+	int X86API BE_init(u32 debugFlags, int memSize, BE_VGAInfo * info,
+			   int shared);
+	void X86API BE_setVGA(BE_VGAInfo * info);
+	void X86API BE_getVGA(BE_VGAInfo * info);
+	void X86API BE_setDebugFlags(u32 debugFlags);
+	void *X86API BE_mapRealPointer(uint r_seg, uint r_off);
+	void *X86API BE_getVESABuf(uint * len, uint * rseg, uint * roff);
+	void X86API BE_callRealMode(uint seg, uint off, RMREGS * regs,
+				    RMSREGS * sregs);
+	int X86API BE_int86(int intno, RMREGS * in, RMREGS * out);
+	int X86API BE_int86x(int intno, RMREGS * in, RMREGS * out,
+			     RMSREGS * sregs);
+	void X86API BE_exit(void);
+
+#ifdef  __cplusplus
+}				/* End of "C" linkage for C++       */
+#endif
+#endif				/* __BIOSEMU_H */
diff --git a/drivers/bios_emulator/include/x86emu.h b/drivers/bios_emulator/include/x86emu.h
new file mode 100644
index 00000000000..6004beb05de
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu.h
@@ -0,0 +1,191 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1996-1999 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Header file for public specific functions.
+*               Any application linking against us should only
+*               include this header
+*
+****************************************************************************/
+
+#ifndef __X86EMU_X86EMU_H
+#define __X86EMU_X86EMU_H
+
+#include <asm/types.h>
+#include <common.h>
+#include <pci.h>
+#include <asm/io.h>
+#define X86API
+#define X86APIP *
+typedef u16 X86EMU_pioAddr;
+
+#include "x86emu/regs.h"
+
+/*---------------------- Macros and type definitions ----------------------*/
+
+#pragma pack(1)
+
+/****************************************************************************
+REMARKS:
+Data structure containing ponters to programmed I/O functions used by the
+emulator. This is used so that the user program can hook all programmed
+I/O for the emulator to handled as necessary by the user program. By
+default the emulator contains simple functions that do not do access the
+hardware in any way. To allow the emualtor access the hardware, you will
+need to override the programmed I/O functions using the X86EMU_setupPioFuncs
+function.
+
+HEADER:
+x86emu.h
+
+MEMBERS:
+inb     - Function to read a byte from an I/O port
+inw     - Function to read a word from an I/O port
+inl     - Function to read a dword from an I/O port
+outb    - Function to write a byte to an I/O port
+outw    - Function to write a word to an I/O port
+outl    - Function to write a dword to an I/O port
+****************************************************************************/
+typedef struct {
+	u8(X86APIP inb) (X86EMU_pioAddr addr);
+	u16(X86APIP inw) (X86EMU_pioAddr addr);
+	u32(X86APIP inl) (X86EMU_pioAddr addr);
+	void (X86APIP outb) (X86EMU_pioAddr addr, u8 val);
+	void (X86APIP outw) (X86EMU_pioAddr addr, u16 val);
+	void (X86APIP outl) (X86EMU_pioAddr addr, u32 val);
+} X86EMU_pioFuncs;
+
+/****************************************************************************
+REMARKS:
+Data structure containing ponters to memory access functions used by the
+emulator. This is used so that the user program can hook all memory
+access functions as necessary for the emulator. By default the emulator
+contains simple functions that only access the internal memory of the
+emulator. If you need specialised functions to handle access to different
+types of memory (ie: hardware framebuffer accesses and BIOS memory access
+etc), you will need to override this using the X86EMU_setupMemFuncs
+function.
+
+HEADER:
+x86emu.h
+
+MEMBERS:
+rdb     - Function to read a byte from an address
+rdw     - Function to read a word from an address
+rdl     - Function to read a dword from an address
+wrb     - Function to write a byte to an address
+wrw     - Function to write a word to an address
+wrl     - Function to write a dword to an address
+****************************************************************************/
+typedef struct {
+	u8(X86APIP rdb) (u32 addr);
+	u16(X86APIP rdw) (u32 addr);
+	u32(X86APIP rdl) (u32 addr);
+	void (X86APIP wrb) (u32 addr, u8 val);
+	void (X86APIP wrw) (u32 addr, u16 val);
+	void (X86APIP wrl) (u32 addr, u32 val);
+} X86EMU_memFuncs;
+
+/****************************************************************************
+  Here are the default memory read and write
+  function in case they are needed as fallbacks.
+***************************************************************************/
+extern u8 X86API rdb(u32 addr);
+extern u16 X86API rdw(u32 addr);
+extern u32 X86API rdl(u32 addr);
+extern void X86API wrb(u32 addr, u8 val);
+extern void X86API wrw(u32 addr, u16 val);
+extern void X86API wrl(u32 addr, u32 val);
+
+#pragma pack()
+
+/*--------------------- type definitions -----------------------------------*/
+
+typedef void (X86APIP X86EMU_intrFuncs) (int num);
+extern X86EMU_intrFuncs _X86EMU_intrTab[256];
+
+/*-------------------------- Function Prototypes --------------------------*/
+
+#ifdef  __cplusplus
+extern "C" {			/* Use "C" linkage when in C++ mode */
+#endif
+
+	void X86EMU_setupMemFuncs(X86EMU_memFuncs * funcs);
+	void X86EMU_setupPioFuncs(X86EMU_pioFuncs * funcs);
+	void X86EMU_setupIntrFuncs(X86EMU_intrFuncs funcs[]);
+	void X86EMU_prepareForInt(int num);
+
+/* decode.c */
+
+	void X86EMU_exec(void);
+	void X86EMU_halt_sys(void);
+
+#ifdef  DEBUG
+#define HALT_SYS()  \
+    printf("halt_sys: file %s, line %d\n", __FILE__, __LINE__), \
+    X86EMU_halt_sys()
+#else
+#define HALT_SYS()  X86EMU_halt_sys()
+#endif
+
+/* Debug options */
+
+#define DEBUG_DECODE_F          0x0001	/* print decoded instruction  */
+#define DEBUG_TRACE_F           0x0002	/* dump regs before/after execution */
+#define DEBUG_STEP_F            0x0004
+#define DEBUG_DISASSEMBLE_F     0x0008
+#define DEBUG_BREAK_F           0x0010
+#define DEBUG_SVC_F             0x0020
+#define DEBUG_SAVE_CS_IP        0x0040
+#define DEBUG_FS_F              0x0080
+#define DEBUG_PROC_F            0x0100
+#define DEBUG_SYSINT_F          0x0200	/* bios system interrupts. */
+#define DEBUG_TRACECALL_F       0x0400
+#define DEBUG_INSTRUMENT_F      0x0800
+#define DEBUG_MEM_TRACE_F       0x1000
+#define DEBUG_IO_TRACE_F        0x2000
+#define DEBUG_TRACECALL_REGS_F  0x4000
+#define DEBUG_DECODE_NOPRINT_F  0x8000
+#define DEBUG_EXIT              0x10000
+#define DEBUG_SYS_F             (DEBUG_SVC_F|DEBUG_FS_F|DEBUG_PROC_F)
+
+	void X86EMU_trace_regs(void);
+	void X86EMU_trace_xregs(void);
+	void X86EMU_dump_memory(u16 seg, u16 off, u32 amt);
+	int X86EMU_trace_on(void);
+	int X86EMU_trace_off(void);
+
+#ifdef  __cplusplus
+}				/* End of "C" linkage for C++       */
+#endif
+#endif				/* __X86EMU_X86EMU_H */
diff --git a/drivers/bios_emulator/include/x86emu/debug.h b/drivers/bios_emulator/include/x86emu/debug.h
new file mode 100644
index 00000000000..268c9d391e2
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/debug.h
@@ -0,0 +1,209 @@
+/****************************************************************************
+*
+*			Realmode X86 Emulator Library
+*
+*		Copyright (C) 1991-2004 SciTech Software, Inc.
+*		     Copyright (C) David Mosberger-Tang
+*		       Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	Header file for debug definitions.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_DEBUG_H
+#define __X86EMU_DEBUG_H
+
+/*---------------------- Macros and type definitions ----------------------*/
+
+/* checks to be enabled for "runtime" */
+
+#define CHECK_IP_FETCH_F		0x1
+#define CHECK_SP_ACCESS_F		0x2
+#define CHECK_MEM_ACCESS_F		0x4	/*using regular linear pointer */
+#define CHECK_DATA_ACCESS_F		0x8	/*using segment:offset */
+
+#ifdef DEBUG
+# define CHECK_IP_FETCH()		(M.x86.check & CHECK_IP_FETCH_F)
+# define CHECK_SP_ACCESS()		(M.x86.check & CHECK_SP_ACCESS_F)
+# define CHECK_MEM_ACCESS()		(M.x86.check & CHECK_MEM_ACCESS_F)
+# define CHECK_DATA_ACCESS()		(M.x86.check & CHECK_DATA_ACCESS_F)
+#else
+# define CHECK_IP_FETCH()
+# define CHECK_SP_ACCESS()
+# define CHECK_MEM_ACCESS()
+# define CHECK_DATA_ACCESS()
+#endif
+
+#ifdef DEBUG
+# define DEBUG_INSTRUMENT()	(M.x86.debug & DEBUG_INSTRUMENT_F)
+# define DEBUG_DECODE()		(M.x86.debug & DEBUG_DECODE_F)
+# define DEBUG_TRACE()		(M.x86.debug & DEBUG_TRACE_F)
+# define DEBUG_STEP()		(M.x86.debug & DEBUG_STEP_F)
+# define DEBUG_DISASSEMBLE()	(M.x86.debug & DEBUG_DISASSEMBLE_F)
+# define DEBUG_BREAK()		(M.x86.debug & DEBUG_BREAK_F)
+# define DEBUG_SVC()		(M.x86.debug & DEBUG_SVC_F)
+# define DEBUG_SAVE_IP_CS()	(M.x86.debug & DEBUG_SAVE_CS_IP)
+
+# define DEBUG_FS()		(M.x86.debug & DEBUG_FS_F)
+# define DEBUG_PROC()		(M.x86.debug & DEBUG_PROC_F)
+# define DEBUG_SYSINT()		(M.x86.debug & DEBUG_SYSINT_F)
+# define DEBUG_TRACECALL()	(M.x86.debug & DEBUG_TRACECALL_F)
+# define DEBUG_TRACECALLREGS()	(M.x86.debug & DEBUG_TRACECALL_REGS_F)
+# define DEBUG_SYS()		(M.x86.debug & DEBUG_SYS_F)
+# define DEBUG_MEM_TRACE()	(M.x86.debug & DEBUG_MEM_TRACE_F)
+# define DEBUG_IO_TRACE()	(M.x86.debug & DEBUG_IO_TRACE_F)
+# define DEBUG_DECODE_NOPRINT() (M.x86.debug & DEBUG_DECODE_NOPRINT_F)
+#else
+# define DEBUG_INSTRUMENT()	0
+# define DEBUG_DECODE()		0
+# define DEBUG_TRACE()		0
+# define DEBUG_STEP()		0
+# define DEBUG_DISASSEMBLE()	0
+# define DEBUG_BREAK()		0
+# define DEBUG_SVC()		0
+# define DEBUG_SAVE_IP_CS()	0
+# define DEBUG_FS()		0
+# define DEBUG_PROC()		0
+# define DEBUG_SYSINT()		0
+# define DEBUG_TRACECALL()	0
+# define DEBUG_TRACECALLREGS()	0
+# define DEBUG_SYS()		0
+# define DEBUG_MEM_TRACE()	0
+# define DEBUG_IO_TRACE()	0
+# define DEBUG_DECODE_NOPRINT() 0
+#endif
+
+#ifdef DEBUG
+
+# define DECODE_PRINTF(x)	if (DEBUG_DECODE()) \
+				    x86emu_decode_printf(x)
+# define DECODE_PRINTF2(x,y)	if (DEBUG_DECODE()) \
+				    x86emu_decode_printf2(x,y)
+
+/*
+ * The following allow us to look at the bytes of an instruction.  The
+ * first INCR_INSTRN_LEN, is called everytime bytes are consumed in
+ * the decoding process.  The SAVE_IP_CS is called initially when the
+ * major opcode of the instruction is accessed.
+ */
+#define INC_DECODED_INST_LEN(x)			    \
+    if (DEBUG_DECODE())				    \
+	x86emu_inc_decoded_inst_len(x)
+
+#define SAVE_IP_CS(x,y)						\
+    if (DEBUG_DECODE() | DEBUG_TRACECALL() | DEBUG_BREAK() \
+	      | DEBUG_IO_TRACE() | DEBUG_SAVE_IP_CS()) { \
+	M.x86.saved_cs = x;					\
+	M.x86.saved_ip = y;					\
+    }
+#else
+# define INC_DECODED_INST_LEN(x)
+# define DECODE_PRINTF(x)
+# define DECODE_PRINTF2(x,y)
+# define SAVE_IP_CS(x,y)
+#endif
+
+#ifdef DEBUG
+#define TRACE_REGS()					    \
+    if (DEBUG_DISASSEMBLE()) {				    \
+	x86emu_just_disassemble();			    \
+	goto EndOfTheInstructionProcedure;		    \
+    }							    \
+    if (DEBUG_TRACE() || DEBUG_DECODE()) X86EMU_trace_regs()
+#else
+# define TRACE_REGS()
+#endif
+
+#ifdef DEBUG
+# define SINGLE_STEP()	    if (DEBUG_STEP()) x86emu_single_step()
+#else
+# define SINGLE_STEP()
+#endif
+
+#define TRACE_AND_STEP()    \
+    TRACE_REGS();	    \
+    SINGLE_STEP()
+
+#ifdef DEBUG
+# define START_OF_INSTR()
+# define END_OF_INSTR()	    EndOfTheInstructionProcedure: x86emu_end_instr();
+# define END_OF_INSTR_NO_TRACE()    x86emu_end_instr();
+#else
+# define START_OF_INSTR()
+# define END_OF_INSTR()
+# define END_OF_INSTR_NO_TRACE()
+#endif
+
+#ifdef DEBUG
+# define  CALL_TRACE(u,v,w,x,s)					\
+    if (DEBUG_TRACECALLREGS())					\
+	x86emu_dump_regs();					\
+    if (DEBUG_TRACECALL())					\
+	printk("%04x:%04x: CALL %s%04x:%04x\n", u , v, s, w, x);
+# define RETURN_TRACE(n,u,v)					\
+    if (DEBUG_TRACECALLREGS())					\
+	x86emu_dump_regs();					\
+    if (DEBUG_TRACECALL())					\
+	printk("%04x:%04x: %s\n",u,v,n);
+#else
+# define CALL_TRACE(u,v,w,x,s)
+# define RETURN_TRACE(n,u,v)
+#endif
+
+#ifdef DEBUG
+#define DB(x)	x
+#else
+#define DB(x)
+#endif
+
+/*-------------------------- Function Prototypes --------------------------*/
+
+#ifdef	__cplusplus
+extern "C" {			/* Use "C" linkage when in C++ mode */
+#endif
+
+	extern void x86emu_inc_decoded_inst_len(int x);
+	extern void x86emu_decode_printf(char *x);
+	extern void x86emu_decode_printf2(char *x, int y);
+	extern void x86emu_just_disassemble(void);
+	extern void x86emu_single_step(void);
+	extern void x86emu_end_instr(void);
+	extern void x86emu_dump_regs(void);
+	extern void x86emu_dump_xregs(void);
+	extern void x86emu_print_int_vect(u16 iv);
+	extern void x86emu_instrument_instruction(void);
+	extern void x86emu_check_ip_access(void);
+	extern void x86emu_check_sp_access(void);
+	extern void x86emu_check_mem_access(u32 p);
+	extern void x86emu_check_data_access(uint s, uint o);
+
+#ifdef	__cplusplus
+}				/* End of "C" linkage for C++	    */
+#endif
+#endif				/* __X86EMU_DEBUG_H */
diff --git a/drivers/bios_emulator/include/x86emu/decode.h b/drivers/bios_emulator/include/x86emu/decode.h
new file mode 100644
index 00000000000..77769f0094c
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/decode.h
@@ -0,0 +1,88 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Header file for instruction decoding logic.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_DECODE_H
+#define __X86EMU_DECODE_H
+
+/*---------------------- Macros and type definitions ----------------------*/
+
+/* Instruction Decoding Stuff */
+
+#define FETCH_DECODE_MODRM(mod,rh,rl)   fetch_decode_modrm(&mod,&rh,&rl)
+#define DECODE_RM_BYTE_REGISTER(r)      decode_rm_byte_register(r)
+#define DECODE_RM_WORD_REGISTER(r)      decode_rm_word_register(r)
+#define DECODE_RM_LONG_REGISTER(r)      decode_rm_long_register(r)
+#define DECODE_CLEAR_SEGOVR()           M.x86.mode &= ~SYSMODE_CLRMASK
+
+/*-------------------------- Function Prototypes --------------------------*/
+
+#ifdef  __cplusplus
+extern "C" {                        /* Use "C" linkage when in C++ mode */
+#endif
+
+void    x86emu_intr_raise (u8 type);
+void    fetch_decode_modrm (int *mod,int *regh,int *regl);
+u8      fetch_byte_imm (void);
+u16     fetch_word_imm (void);
+u32     fetch_long_imm (void);
+u8      fetch_data_byte (uint offset);
+u8      fetch_data_byte_abs (uint segment, uint offset);
+u16     fetch_data_word (uint offset);
+u16     fetch_data_word_abs (uint segment, uint offset);
+u32     fetch_data_long (uint offset);
+u32     fetch_data_long_abs (uint segment, uint offset);
+void    store_data_byte (uint offset, u8 val);
+void    store_data_byte_abs (uint segment, uint offset, u8 val);
+void    store_data_word (uint offset, u16 val);
+void    store_data_word_abs (uint segment, uint offset, u16 val);
+void    store_data_long (uint offset, u32 val);
+void    store_data_long_abs (uint segment, uint offset, u32 val);
+u8*     decode_rm_byte_register(int reg);
+u16*    decode_rm_word_register(int reg);
+u32*    decode_rm_long_register(int reg);
+u16*    decode_rm_seg_register(int reg);
+unsigned decode_rm00_address(int rm);
+unsigned decode_rm01_address(int rm);
+unsigned decode_rm10_address(int rm);
+unsigned decode_rmXX_address(int mod, int rm);
+
+#ifdef  __cplusplus
+}                                   /* End of "C" linkage for C++       */
+#endif
+
+#endif /* __X86EMU_DECODE_H */
diff --git a/drivers/bios_emulator/include/x86emu/ops.h b/drivers/bios_emulator/include/x86emu/ops.h
new file mode 100644
index 00000000000..a4f2316ba0d
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/ops.h
@@ -0,0 +1,45 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Header file for operand decoding functions.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_OPS_H
+#define __X86EMU_OPS_H
+
+extern void (*x86emu_optab[0x100])(u8 op1);
+extern void (*x86emu_optab2[0x100])(u8 op2);
+
+#endif /* __X86EMU_OPS_H */
diff --git a/drivers/bios_emulator/include/x86emu/prim_asm.h b/drivers/bios_emulator/include/x86emu/prim_asm.h
new file mode 100644
index 00000000000..4cb4cab5d55
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/prim_asm.h
@@ -0,0 +1,970 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     Watcom C++ 10.6 or later
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Inline assembler versions of the primitive operand
+*               functions for faster performance. At the moment this is
+*               x86 inline assembler, but these functions could be replaced
+*               with native inline assembler for each supported processor
+*               platform.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_PRIM_ASM_H
+#define __X86EMU_PRIM_ASM_H
+
+#ifdef  __WATCOMC__
+
+#ifndef VALIDATE
+#define __HAVE_INLINE_ASSEMBLER__
+#endif
+
+u32 get_flags_asm(void);
+#pragma aux get_flags_asm =         \
+    "pushf"                         \
+    "pop    eax"                    \
+    value [eax]                     \
+    modify exact [eax];
+
+u16 aaa_word_asm(u32 * flags, u16 d);
+#pragma aux aaa_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "aaa"                           \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u16 aas_word_asm(u32 * flags, u16 d);
+#pragma aux aas_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "aas"                           \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u16 aad_word_asm(u32 * flags, u16 d);
+#pragma aux aad_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "aad"                           \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u16 aam_word_asm(u32 * flags, u8 d);
+#pragma aux aam_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "aam"                           \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u8 adc_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux adc_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "adc    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 adc_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux adc_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "adc    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 adc_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux adc_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "adc    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+u8 add_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux add_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "add    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 add_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux add_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "add    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 add_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux add_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "add    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+u8 and_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux and_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "and    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 and_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux and_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "and    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 and_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux and_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "and    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+u8 cmp_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux cmp_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "cmp    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 cmp_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux cmp_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "cmp    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 cmp_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux cmp_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "cmp    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+u8 daa_byte_asm(u32 * flags, u8 d);
+#pragma aux daa_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "daa"                           \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [al]                      \
+    modify exact [al];
+
+u8 das_byte_asm(u32 * flags, u8 d);
+#pragma aux das_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "das"                           \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [al]                      \
+    modify exact [al];
+
+u8 dec_byte_asm(u32 * flags, u8 d);
+#pragma aux dec_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "dec    al"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [al]                      \
+    modify exact [al];
+
+u16 dec_word_asm(u32 * flags, u16 d);
+#pragma aux dec_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "dec    ax"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u32 dec_long_asm(u32 * flags, u32 d);
+#pragma aux dec_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "dec    eax"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax]                \
+    value [eax]                     \
+    modify exact [eax];
+
+u8 inc_byte_asm(u32 * flags, u8 d);
+#pragma aux inc_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "inc    al"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [al]                      \
+    modify exact [al];
+
+u16 inc_word_asm(u32 * flags, u16 d);
+#pragma aux inc_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "inc    ax"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u32 inc_long_asm(u32 * flags, u32 d);
+#pragma aux inc_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "inc    eax"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax]                \
+    value [eax]                     \
+    modify exact [eax];
+
+u8 or_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux or_byte_asm =           \
+    "push   [edi]"                  \
+    "popf"                          \
+    "or al,bl"                      \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 or_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux or_word_asm =           \
+    "push   [edi]"                  \
+    "popf"                          \
+    "or ax,bx"                      \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 or_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux or_long_asm =           \
+    "push   [edi]"                  \
+    "popf"                          \
+    "or eax,ebx"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+u8 neg_byte_asm(u32 * flags, u8 d);
+#pragma aux neg_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "neg    al"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [al]                      \
+    modify exact [al];
+
+u16 neg_word_asm(u32 * flags, u16 d);
+#pragma aux neg_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "neg    ax"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u32 neg_long_asm(u32 * flags, u32 d);
+#pragma aux neg_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "neg    eax"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax]                \
+    value [eax]                     \
+    modify exact [eax];
+
+u8 not_byte_asm(u32 * flags, u8 d);
+#pragma aux not_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "not    al"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al]                 \
+    value [al]                      \
+    modify exact [al];
+
+u16 not_word_asm(u32 * flags, u16 d);
+#pragma aux not_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "not    ax"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax]                 \
+    value [ax]                      \
+    modify exact [ax];
+
+u32 not_long_asm(u32 * flags, u32 d);
+#pragma aux not_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "not    eax"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax]                \
+    value [eax]                     \
+    modify exact [eax];
+
+u8 rcl_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux rcl_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rcl    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 rcl_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux rcl_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rcl    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 rcl_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux rcl_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rcl    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u8 rcr_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux rcr_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rcr    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 rcr_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux rcr_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rcr    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 rcr_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux rcr_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rcr    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u8 rol_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux rol_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rol    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 rol_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux rol_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rol    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 rol_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux rol_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "rol    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u8 ror_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux ror_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "ror    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 ror_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux ror_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "ror    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 ror_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux ror_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "ror    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u8 shl_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux shl_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shl    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 shl_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux shl_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shl    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 shl_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux shl_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shl    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u8 shr_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux shr_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shr    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 shr_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux shr_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shr    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 shr_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux shr_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shr    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u8 sar_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux sar_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sar    al,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [cl]            \
+    value [al]                      \
+    modify exact [al cl];
+
+u16 sar_word_asm(u32 * flags, u16 d, u8 s);
+#pragma aux sar_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sar    ax,cl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [cl]            \
+    value [ax]                      \
+    modify exact [ax cl];
+
+u32 sar_long_asm(u32 * flags, u32 d, u8 s);
+#pragma aux sar_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sar    eax,cl"                 \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [cl]           \
+    value [eax]                     \
+    modify exact [eax cl];
+
+u16 shld_word_asm(u32 * flags, u16 d, u16 fill, u8 s);
+#pragma aux shld_word_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shld   ax,dx,cl"               \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [dx] [cl]       \
+    value [ax]                      \
+    modify exact [ax dx cl];
+
+u32 shld_long_asm(u32 * flags, u32 d, u32 fill, u8 s);
+#pragma aux shld_long_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shld   eax,edx,cl"             \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [edx] [cl]     \
+    value [eax]                     \
+    modify exact [eax edx cl];
+
+u16 shrd_word_asm(u32 * flags, u16 d, u16 fill, u8 s);
+#pragma aux shrd_word_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shrd   ax,dx,cl"               \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [dx] [cl]       \
+    value [ax]                      \
+    modify exact [ax dx cl];
+
+u32 shrd_long_asm(u32 * flags, u32 d, u32 fill, u8 s);
+#pragma aux shrd_long_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "shrd   eax,edx,cl"             \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [edx] [cl]     \
+    value [eax]                     \
+    modify exact [eax edx cl];
+
+u8 sbb_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux sbb_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sbb    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 sbb_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux sbb_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sbb    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 sbb_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux sbb_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sbb    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+u8 sub_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux sub_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sub    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 sub_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux sub_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sub    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 sub_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux sub_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "sub    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+void test_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux test_byte_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "test   al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    modify exact [al bl];
+
+void test_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux test_word_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "test   ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    modify exact [ax bx];
+
+void test_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux test_long_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "test   eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    modify exact [eax ebx];
+
+u8 xor_byte_asm(u32 * flags, u8 d, u8 s);
+#pragma aux xor_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "xor    al,bl"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [al] [bl]            \
+    value [al]                      \
+    modify exact [al bl];
+
+u16 xor_word_asm(u32 * flags, u16 d, u16 s);
+#pragma aux xor_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "xor    ax,bx"                  \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [ax] [bx]            \
+    value [ax]                      \
+    modify exact [ax bx];
+
+u32 xor_long_asm(u32 * flags, u32 d, u32 s);
+#pragma aux xor_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "xor    eax,ebx"                \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    parm [edi] [eax] [ebx]          \
+    value [eax]                     \
+    modify exact [eax ebx];
+
+void imul_byte_asm(u32 * flags, u16 * ax, u8 d, u8 s);
+#pragma aux imul_byte_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "imul   bl"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],ax"               \
+    parm [edi] [esi] [al] [bl]      \
+    modify exact [esi ax bl];
+
+void imul_word_asm(u32 * flags, u16 * ax, u16 * dx, u16 d, u16 s);
+#pragma aux imul_word_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "imul   bx"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],ax"               \
+    "mov    [ecx],dx"               \
+    parm [edi] [esi] [ecx] [ax] [bx]\
+    modify exact [esi edi ax bx dx];
+
+void imul_long_asm(u32 * flags, u32 * eax, u32 * edx, u32 d, u32 s);
+#pragma aux imul_long_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "imul   ebx"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],eax"              \
+    "mov    [ecx],edx"              \
+    parm [edi] [esi] [ecx] [eax] [ebx] \
+    modify exact [esi edi eax ebx edx];
+
+void mul_byte_asm(u32 * flags, u16 * ax, u8 d, u8 s);
+#pragma aux mul_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "mul    bl"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],ax"               \
+    parm [edi] [esi] [al] [bl]      \
+    modify exact [esi ax bl];
+
+void mul_word_asm(u32 * flags, u16 * ax, u16 * dx, u16 d, u16 s);
+#pragma aux mul_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "mul    bx"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],ax"               \
+    "mov    [ecx],dx"               \
+    parm [edi] [esi] [ecx] [ax] [bx]\
+    modify exact [esi edi ax bx dx];
+
+void mul_long_asm(u32 * flags, u32 * eax, u32 * edx, u32 d, u32 s);
+#pragma aux mul_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "mul    ebx"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],eax"              \
+    "mov    [ecx],edx"              \
+    parm [edi] [esi] [ecx] [eax] [ebx] \
+    modify exact [esi edi eax ebx edx];
+
+void idiv_byte_asm(u32 * flags, u8 * al, u8 * ah, u16 d, u8 s);
+#pragma aux idiv_byte_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "idiv   bl"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],al"               \
+    "mov    [ecx],ah"               \
+    parm [edi] [esi] [ecx] [ax] [bl]\
+    modify exact [esi edi ax bl];
+
+void idiv_word_asm(u32 * flags, u16 * ax, u16 * dx, u16 dlo, u16 dhi, u16 s);
+#pragma aux idiv_word_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "idiv   bx"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],ax"               \
+    "mov    [ecx],dx"               \
+    parm [edi] [esi] [ecx] [ax] [dx] [bx]\
+    modify exact [esi edi ax dx bx];
+
+void idiv_long_asm(u32 * flags, u32 * eax, u32 * edx, u32 dlo, u32 dhi, u32 s);
+#pragma aux idiv_long_asm =         \
+    "push   [edi]"                  \
+    "popf"                          \
+    "idiv   ebx"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],eax"              \
+    "mov    [ecx],edx"              \
+    parm [edi] [esi] [ecx] [eax] [edx] [ebx]\
+    modify exact [esi edi eax edx ebx];
+
+void div_byte_asm(u32 * flags, u8 * al, u8 * ah, u16 d, u8 s);
+#pragma aux div_byte_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "div    bl"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],al"               \
+    "mov    [ecx],ah"               \
+    parm [edi] [esi] [ecx] [ax] [bl]\
+    modify exact [esi edi ax bl];
+
+void div_word_asm(u32 * flags, u16 * ax, u16 * dx, u16 dlo, u16 dhi, u16 s);
+#pragma aux div_word_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "div    bx"                     \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],ax"               \
+    "mov    [ecx],dx"               \
+    parm [edi] [esi] [ecx] [ax] [dx] [bx]\
+    modify exact [esi edi ax dx bx];
+
+void div_long_asm(u32 * flags, u32 * eax, u32 * edx, u32 dlo, u32 dhi, u32 s);
+#pragma aux div_long_asm =          \
+    "push   [edi]"                  \
+    "popf"                          \
+    "div    ebx"                    \
+    "pushf"                         \
+    "pop    [edi]"                  \
+    "mov    [esi],eax"              \
+    "mov    [ecx],edx"              \
+    parm [edi] [esi] [ecx] [eax] [edx] [ebx]\
+    modify exact [esi edi eax edx ebx];
+
+#endif
+
+#endif				/* __X86EMU_PRIM_ASM_H */
diff --git a/drivers/bios_emulator/include/x86emu/prim_ops.h b/drivers/bios_emulator/include/x86emu/prim_ops.h
new file mode 100644
index 00000000000..2291e8488a5
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/prim_ops.h
@@ -0,0 +1,141 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Header file for primitive operation functions.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_PRIM_OPS_H
+#define __X86EMU_PRIM_OPS_H
+
+#ifdef  __cplusplus
+extern "C" {                        /* Use "C" linkage when in C++ mode */
+#endif
+
+u16     aaa_word (u16 d);
+u16     aas_word (u16 d);
+u16     aad_word (u16 d);
+u16     aam_word (u8 d);
+u8      adc_byte (u8 d, u8 s);
+u16     adc_word (u16 d, u16 s);
+u32     adc_long (u32 d, u32 s);
+u8      add_byte (u8 d, u8 s);
+u16     add_word (u16 d, u16 s);
+u32     add_long (u32 d, u32 s);
+u8      and_byte (u8 d, u8 s);
+u16     and_word (u16 d, u16 s);
+u32     and_long (u32 d, u32 s);
+u8      cmp_byte (u8 d, u8 s);
+u16     cmp_word (u16 d, u16 s);
+u32     cmp_long (u32 d, u32 s);
+u8      daa_byte (u8 d);
+u8      das_byte (u8 d);
+u8      dec_byte (u8 d);
+u16     dec_word (u16 d);
+u32     dec_long (u32 d);
+u8      inc_byte (u8 d);
+u16     inc_word (u16 d);
+u32     inc_long (u32 d);
+u8      or_byte (u8 d, u8 s);
+u16     or_word (u16 d, u16 s);
+u32     or_long (u32 d, u32 s);
+u8      neg_byte (u8 s);
+u16     neg_word (u16 s);
+u32     neg_long (u32 s);
+u8      not_byte (u8 s);
+u16     not_word (u16 s);
+u32     not_long (u32 s);
+u8      rcl_byte (u8 d, u8 s);
+u16     rcl_word (u16 d, u8 s);
+u32     rcl_long (u32 d, u8 s);
+u8      rcr_byte (u8 d, u8 s);
+u16     rcr_word (u16 d, u8 s);
+u32     rcr_long (u32 d, u8 s);
+u8      rol_byte (u8 d, u8 s);
+u16     rol_word (u16 d, u8 s);
+u32     rol_long (u32 d, u8 s);
+u8      ror_byte (u8 d, u8 s);
+u16     ror_word (u16 d, u8 s);
+u32     ror_long (u32 d, u8 s);
+u8      shl_byte (u8 d, u8 s);
+u16     shl_word (u16 d, u8 s);
+u32     shl_long (u32 d, u8 s);
+u8      shr_byte (u8 d, u8 s);
+u16     shr_word (u16 d, u8 s);
+u32     shr_long (u32 d, u8 s);
+u8      sar_byte (u8 d, u8 s);
+u16     sar_word (u16 d, u8 s);
+u32     sar_long (u32 d, u8 s);
+u16     shld_word (u16 d, u16 fill, u8 s);
+u32     shld_long (u32 d, u32 fill, u8 s);
+u16     shrd_word (u16 d, u16 fill, u8 s);
+u32     shrd_long (u32 d, u32 fill, u8 s);
+u8      sbb_byte (u8 d, u8 s);
+u16     sbb_word (u16 d, u16 s);
+u32     sbb_long (u32 d, u32 s);
+u8      sub_byte (u8 d, u8 s);
+u16     sub_word (u16 d, u16 s);
+u32     sub_long (u32 d, u32 s);
+void    test_byte (u8 d, u8 s);
+void    test_word (u16 d, u16 s);
+void    test_long (u32 d, u32 s);
+u8      xor_byte (u8 d, u8 s);
+u16     xor_word (u16 d, u16 s);
+u32     xor_long (u32 d, u32 s);
+void    imul_byte (u8 s);
+void    imul_word (u16 s);
+void    imul_long (u32 s);
+void    imul_long_direct(u32 *res_lo, u32* res_hi,u32 d, u32 s);
+void    mul_byte (u8 s);
+void    mul_word (u16 s);
+void    mul_long (u32 s);
+void    idiv_byte (u8 s);
+void    idiv_word (u16 s);
+void    idiv_long (u32 s);
+void    div_byte (u8 s);
+void    div_word (u16 s);
+void    div_long (u32 s);
+void    ins (int size);
+void    outs (int size);
+u16     mem_access_word (int addr);
+void    push_word (u16 w);
+void    push_long (u32 w);
+u16     pop_word (void);
+u32     pop_long (void);
+
+#ifdef  __cplusplus
+}                                   /* End of "C" linkage for C++       */
+#endif
+
+#endif /* __X86EMU_PRIM_OPS_H */
diff --git a/drivers/bios_emulator/include/x86emu/regs.h b/drivers/bios_emulator/include/x86emu/regs.h
new file mode 100644
index 00000000000..a7fedd2f6ca
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/regs.h
@@ -0,0 +1,340 @@
+/****************************************************************************
+*
+*			Realmode X86 Emulator Library
+*
+*		Copyright (C) 1991-2004 SciTech Software, Inc.
+*		     Copyright (C) David Mosberger-Tang
+*		       Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	Header file for x86 register definitions.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_REGS_H
+#define __X86EMU_REGS_H
+
+/*---------------------- Macros and type definitions ----------------------*/
+
+#pragma pack(1)
+
+/*
+ * General EAX, EBX, ECX, EDX type registers.  Note that for
+ * portability, and speed, the issue of byte swapping is not addressed
+ * in the registers.  All registers are stored in the default format
+ * available on the host machine.  The only critical issue is that the
+ * registers should line up EXACTLY in the same manner as they do in
+ * the 386.  That is:
+ *
+ * EAX & 0xff  === AL
+ * EAX & 0xffff == AX
+ *
+ * etc.	 The result is that alot of the calculations can then be
+ * done using the native instruction set fully.
+ */
+
+#ifdef	__BIG_ENDIAN__
+
+typedef struct {
+	u32 e_reg;
+} I32_reg_t;
+
+typedef struct {
+	u16 filler0, x_reg;
+} I16_reg_t;
+
+typedef struct {
+	u8 filler0, filler1, h_reg, l_reg;
+} I8_reg_t;
+
+#else				/* !__BIG_ENDIAN__ */
+
+typedef struct {
+	u32 e_reg;
+} I32_reg_t;
+
+typedef struct {
+	u16 x_reg;
+} I16_reg_t;
+
+typedef struct {
+	u8 l_reg, h_reg;
+} I8_reg_t;
+
+#endif				/* BIG_ENDIAN */
+
+typedef union {
+	I32_reg_t I32_reg;
+	I16_reg_t I16_reg;
+	I8_reg_t I8_reg;
+} i386_general_register;
+
+struct i386_general_regs {
+	i386_general_register A, B, C, D;
+};
+
+typedef struct i386_general_regs Gen_reg_t;
+
+struct i386_special_regs {
+	i386_general_register SP, BP, SI, DI, IP;
+	u32 FLAGS;
+};
+
+/*
+ * Segment registers here represent the 16 bit quantities
+ * CS, DS, ES, SS.
+ */
+
+#undef CS
+#undef DS
+#undef SS
+#undef ES
+#undef FS
+#undef GS
+
+struct i386_segment_regs {
+	u16 CS, DS, SS, ES, FS, GS;
+};
+
+/* 8 bit registers */
+#define R_AH  gen.A.I8_reg.h_reg
+#define R_AL  gen.A.I8_reg.l_reg
+#define R_BH  gen.B.I8_reg.h_reg
+#define R_BL  gen.B.I8_reg.l_reg
+#define R_CH  gen.C.I8_reg.h_reg
+#define R_CL  gen.C.I8_reg.l_reg
+#define R_DH  gen.D.I8_reg.h_reg
+#define R_DL  gen.D.I8_reg.l_reg
+
+/* 16 bit registers */
+#define R_AX  gen.A.I16_reg.x_reg
+#define R_BX  gen.B.I16_reg.x_reg
+#define R_CX  gen.C.I16_reg.x_reg
+#define R_DX  gen.D.I16_reg.x_reg
+
+/* 32 bit extended registers */
+#define R_EAX  gen.A.I32_reg.e_reg
+#define R_EBX  gen.B.I32_reg.e_reg
+#define R_ECX  gen.C.I32_reg.e_reg
+#define R_EDX  gen.D.I32_reg.e_reg
+
+/* special registers */
+#define R_SP  spc.SP.I16_reg.x_reg
+#define R_BP  spc.BP.I16_reg.x_reg
+#define R_SI  spc.SI.I16_reg.x_reg
+#define R_DI  spc.DI.I16_reg.x_reg
+#define R_IP  spc.IP.I16_reg.x_reg
+#define R_FLG spc.FLAGS
+
+/* special registers */
+#define R_SP  spc.SP.I16_reg.x_reg
+#define R_BP  spc.BP.I16_reg.x_reg
+#define R_SI  spc.SI.I16_reg.x_reg
+#define R_DI  spc.DI.I16_reg.x_reg
+#define R_IP  spc.IP.I16_reg.x_reg
+#define R_FLG spc.FLAGS
+
+/* special registers */
+#define R_ESP  spc.SP.I32_reg.e_reg
+#define R_EBP  spc.BP.I32_reg.e_reg
+#define R_ESI  spc.SI.I32_reg.e_reg
+#define R_EDI  spc.DI.I32_reg.e_reg
+#define R_EIP  spc.IP.I32_reg.e_reg
+#define R_EFLG spc.FLAGS
+
+/* segment registers */
+#define R_CS  seg.CS
+#define R_DS  seg.DS
+#define R_SS  seg.SS
+#define R_ES  seg.ES
+#define R_FS  seg.FS
+#define R_GS  seg.GS
+
+/* flag conditions   */
+#define FB_CF 0x0001		/* CARRY flag  */
+#define FB_PF 0x0004		/* PARITY flag */
+#define FB_AF 0x0010		/* AUX	flag   */
+#define FB_ZF 0x0040		/* ZERO flag   */
+#define FB_SF 0x0080		/* SIGN flag   */
+#define FB_TF 0x0100		/* TRAP flag   */
+#define FB_IF 0x0200		/* INTERRUPT ENABLE flag */
+#define FB_DF 0x0400		/* DIR flag    */
+#define FB_OF 0x0800		/* OVERFLOW flag */
+
+/* 80286 and above always have bit#1 set */
+#define F_ALWAYS_ON  (0x0002)	/* flag bits always on */
+
+/*
+ * Define a mask for only those flag bits we will ever pass back
+ * (via PUSHF)
+ */
+#define F_MSK (FB_CF|FB_PF|FB_AF|FB_ZF|FB_SF|FB_TF|FB_IF|FB_DF|FB_OF)
+
+/* following bits masked in to a 16bit quantity */
+
+#define F_CF 0x0001		/* CARRY flag  */
+#define F_PF 0x0004		/* PARITY flag */
+#define F_AF 0x0010		/* AUX	flag   */
+#define F_ZF 0x0040		/* ZERO flag   */
+#define F_SF 0x0080		/* SIGN flag   */
+#define F_TF 0x0100		/* TRAP flag   */
+#define F_IF 0x0200		/* INTERRUPT ENABLE flag */
+#define F_DF 0x0400		/* DIR flag    */
+#define F_OF 0x0800		/* OVERFLOW flag */
+
+#define TOGGLE_FLAG(flag)	(M.x86.R_FLG ^= (flag))
+#define SET_FLAG(flag)		(M.x86.R_FLG |= (flag))
+#define CLEAR_FLAG(flag)	(M.x86.R_FLG &= ~(flag))
+#define ACCESS_FLAG(flag)	(M.x86.R_FLG & (flag))
+#define CLEARALL_FLAG(m)	(M.x86.R_FLG = 0)
+
+#define CONDITIONAL_SET_FLAG(COND,FLAG) \
+  if (COND) SET_FLAG(FLAG); else CLEAR_FLAG(FLAG)
+
+#define F_PF_CALC 0x010000	/* PARITY flag has been calced	  */
+#define F_ZF_CALC 0x020000	/* ZERO flag has been calced	  */
+#define F_SF_CALC 0x040000	/* SIGN flag has been calced	  */
+
+#define F_ALL_CALC	0xff0000	/* All have been calced	  */
+
+/*
+ * Emulator machine state.
+ * Segment usage control.
+ */
+#define SYSMODE_SEG_DS_SS	0x00000001
+#define SYSMODE_SEGOVR_CS	0x00000002
+#define SYSMODE_SEGOVR_DS	0x00000004
+#define SYSMODE_SEGOVR_ES	0x00000008
+#define SYSMODE_SEGOVR_FS	0x00000010
+#define SYSMODE_SEGOVR_GS	0x00000020
+#define SYSMODE_SEGOVR_SS	0x00000040
+#define SYSMODE_PREFIX_REPE	0x00000080
+#define SYSMODE_PREFIX_REPNE	0x00000100
+#define SYSMODE_PREFIX_DATA	0x00000200
+#define SYSMODE_PREFIX_ADDR	0x00000400
+#define SYSMODE_INTR_PENDING	0x10000000
+#define SYSMODE_EXTRN_INTR	0x20000000
+#define SYSMODE_HALTED		0x40000000
+
+#define SYSMODE_SEGMASK (SYSMODE_SEG_DS_SS	| \
+			 SYSMODE_SEGOVR_CS	| \
+			 SYSMODE_SEGOVR_DS	| \
+			 SYSMODE_SEGOVR_ES	| \
+			 SYSMODE_SEGOVR_FS	| \
+			 SYSMODE_SEGOVR_GS	| \
+			 SYSMODE_SEGOVR_SS)
+#define SYSMODE_CLRMASK (SYSMODE_SEG_DS_SS	| \
+			 SYSMODE_SEGOVR_CS	| \
+			 SYSMODE_SEGOVR_DS	| \
+			 SYSMODE_SEGOVR_ES	| \
+			 SYSMODE_SEGOVR_FS	| \
+			 SYSMODE_SEGOVR_GS	| \
+			 SYSMODE_SEGOVR_SS	| \
+			 SYSMODE_PREFIX_DATA	| \
+			 SYSMODE_PREFIX_ADDR)
+
+#define	 INTR_SYNCH	      0x1
+#define	 INTR_ASYNCH	      0x2
+#define	 INTR_HALTED	      0x4
+
+typedef struct {
+	struct i386_general_regs gen;
+	struct i386_special_regs spc;
+	struct i386_segment_regs seg;
+	/*
+	 * MODE contains information on:
+	 *  REPE prefix		    2 bits  repe,repne
+	 *  SEGMENT overrides	    5 bits  normal,DS,SS,CS,ES
+	 *  Delayed flag set	    3 bits  (zero, signed, parity)
+	 *  reserved		    6 bits
+	 *  interrupt #		    8 bits  instruction raised interrupt
+	 *  BIOS video segregs	    4 bits
+	 *  Interrupt Pending	    1 bits
+	 *  Extern interrupt	    1 bits
+	 *  Halted		    1 bits
+	 */
+	long mode;
+	u8 intno;
+	volatile int intr;	/* mask of pending interrupts */
+	int debug;
+#ifdef DEBUG
+	int check;
+	u16 saved_ip;
+	u16 saved_cs;
+	int enc_pos;
+	int enc_str_pos;
+	char decode_buf[32];	/* encoded byte stream	*/
+	char decoded_buf[256];	/* disassembled strings */
+#endif
+} X86EMU_regs;
+
+/****************************************************************************
+REMARKS:
+Structure maintaining the emulator machine state.
+
+MEMBERS:
+x86		- X86 registers
+mem_base	- Base real mode memory for the emulator
+mem_size	- Size of the real mode memory block for the emulator
+****************************************************************************/
+#undef x86
+typedef struct {
+	X86EMU_regs x86;
+	u8 *mem_base;
+	u32 mem_size;
+	void *private;
+} X86EMU_sysEnv;
+
+#pragma pack()
+
+/*----------------------------- Global Variables --------------------------*/
+
+#ifdef	__cplusplus
+extern "C" {			/* Use "C" linkage when in C++ mode */
+#endif
+
+/* Global emulator machine state.
+ *
+ * We keep it global to avoid pointer dereferences in the code for speed.
+ */
+
+	extern X86EMU_sysEnv _X86EMU_env;
+#define	  M		_X86EMU_env
+
+/*-------------------------- Function Prototypes --------------------------*/
+
+/* Function to log information at runtime */
+
+#ifndef __KERNEL__
+	void printk(const char *fmt, ...);
+#endif
+
+#ifdef	__cplusplus
+}				/* End of "C" linkage for C++	    */
+#endif
+#endif				/* __X86EMU_REGS_H */
diff --git a/drivers/bios_emulator/include/x86emu/x86emui.h b/drivers/bios_emulator/include/x86emu/x86emui.h
new file mode 100644
index 00000000000..a74957d992a
--- /dev/null
+++ b/drivers/bios_emulator/include/x86emu/x86emui.h
@@ -0,0 +1,101 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  Header file for system specific functions. These functions
+*               are always compiled and linked in the OS depedent libraries,
+*               and never in a binary portable driver.
+*
+****************************************************************************/
+
+#ifndef __X86EMU_X86EMUI_H
+#define __X86EMU_X86EMUI_H
+
+/* If we are compiling in C++ mode, we can compile some functions as
+ * inline to increase performance (however the code size increases quite
+ * dramatically in this case).
+ */
+
+#if defined(__cplusplus) && !defined(_NO_INLINE)
+#define _INLINE inline
+#else
+#define _INLINE static
+#endif
+
+/* Get rid of unused parameters in C++ compilation mode */
+
+#ifdef __cplusplus
+#define X86EMU_UNUSED(v)
+#else
+#define X86EMU_UNUSED(v)    v
+#endif
+
+#include "x86emu.h"
+#include "x86emu/regs.h"
+#include "x86emu/debug.h"
+#include "x86emu/decode.h"
+#include "x86emu/ops.h"
+#include "x86emu/prim_ops.h"
+#ifndef __KERNEL__
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#endif
+
+#define printk printf
+
+
+/*--------------------------- Inline Functions ----------------------------*/
+
+#ifdef  __cplusplus
+extern "C" {			/* Use "C" linkage when in C++ mode */
+#endif
+
+	extern u8(X86APIP sys_rdb) (u32 addr);
+	extern u16(X86APIP sys_rdw) (u32 addr);
+	extern u32(X86APIP sys_rdl) (u32 addr);
+	extern void (X86APIP sys_wrb) (u32 addr, u8 val);
+	extern void (X86APIP sys_wrw) (u32 addr, u16 val);
+	extern void (X86APIP sys_wrl) (u32 addr, u32 val);
+
+	extern u8(X86APIP sys_inb) (X86EMU_pioAddr addr);
+	extern u16(X86APIP sys_inw) (X86EMU_pioAddr addr);
+	extern u32(X86APIP sys_inl) (X86EMU_pioAddr addr);
+	extern void (X86APIP sys_outb) (X86EMU_pioAddr addr, u8 val);
+	extern void (X86APIP sys_outw) (X86EMU_pioAddr addr, u16 val);
+	extern void (X86APIP sys_outl) (X86EMU_pioAddr addr, u32 val);
+
+#ifdef  __cplusplus
+}				/* End of "C" linkage for C++       */
+#endif
+#endif				/* __X86EMU_X86EMUI_H */
diff --git a/drivers/bios_emulator/x86emu/debug.c b/drivers/bios_emulator/x86emu/debug.c
new file mode 100644
index 00000000000..915739c5b03
--- /dev/null
+++ b/drivers/bios_emulator/x86emu/debug.c
@@ -0,0 +1,465 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  This file contains the code to handle debugging of the
+*               emulator.
+*
+****************************************************************************/
+
+#include "x86emu/x86emui.h"
+#include <stdarg.h>
+
+#if defined(CONFIG_BIOSEMU)
+
+/*----------------------------- Implementation ----------------------------*/
+
+#ifdef DEBUG
+
+static void print_encoded_bytes(u16 s, u16 o);
+static void print_decoded_instruction(void);
+static int parse_line(char *s, int *ps, int *n);
+
+/* should look something like debug's output. */
+void X86EMU_trace_regs(void)
+{
+	if (DEBUG_TRACE()) {
+		x86emu_dump_regs();
+	}
+	if (DEBUG_DECODE() && !DEBUG_DECODE_NOPRINT()) {
+		printk("%04x:%04x ", M.x86.saved_cs, M.x86.saved_ip);
+		print_encoded_bytes(M.x86.saved_cs, M.x86.saved_ip);
+		print_decoded_instruction();
+	}
+}
+
+void X86EMU_trace_xregs(void)
+{
+	if (DEBUG_TRACE()) {
+		x86emu_dump_xregs();
+	}
+}
+
+void x86emu_just_disassemble(void)
+{
+	/*
+	 * This routine called if the flag DEBUG_DISASSEMBLE is set kind
+	 * of a hack!
+	 */
+	printk("%04x:%04x ", M.x86.saved_cs, M.x86.saved_ip);
+	print_encoded_bytes(M.x86.saved_cs, M.x86.saved_ip);
+	print_decoded_instruction();
+}
+
+static void disassemble_forward(u16 seg, u16 off, int n)
+{
+	X86EMU_sysEnv tregs;
+	int i;
+	u8 op1;
+	/*
+	 * hack, hack, hack.  What we do is use the exact machinery set up
+	 * for execution, except that now there is an additional state
+	 * flag associated with the "execution", and we are using a copy
+	 * of the register struct.  All the major opcodes, once fully
+	 * decoded, have the following two steps: TRACE_REGS(r,m);
+	 * SINGLE_STEP(r,m); which disappear if DEBUG is not defined to
+	 * the preprocessor.  The TRACE_REGS macro expands to:
+	 *
+	 * if (debug&DEBUG_DISASSEMBLE)
+	 *     {just_disassemble(); goto EndOfInstruction;}
+	 *     if (debug&DEBUG_TRACE) trace_regs(r,m);
+	 *
+	 * ......  and at the last line of the routine.
+	 *
+	 * EndOfInstruction: end_instr();
+	 *
+	 * Up to the point where TRACE_REG is expanded, NO modifications
+	 * are done to any register EXCEPT the IP register, for fetch and
+	 * decoding purposes.
+	 *
+	 * This was done for an entirely different reason, but makes a
+	 * nice way to get the system to help debug codes.
+	 */
+	tregs = M;
+	tregs.x86.R_IP = off;
+	tregs.x86.R_CS = seg;
+
+	/* reset the decoding buffers */
+	tregs.x86.enc_str_pos = 0;
+	tregs.x86.enc_pos = 0;
+
+	/* turn on the "disassemble only, no execute" flag */
+	tregs.x86.debug |= DEBUG_DISASSEMBLE_F;
+
+	/* DUMP NEXT n instructions to screen in straight_line fashion */
+	/*
+	 * This looks like the regular instruction fetch stream, except
+	 * that when this occurs, each fetched opcode, upon seeing the
+	 * DEBUG_DISASSEMBLE flag set, exits immediately after decoding
+	 * the instruction.  XXX --- CHECK THAT MEM IS NOT AFFECTED!!!
+	 * Note the use of a copy of the register structure...
+	 */
+	for (i = 0; i < n; i++) {
+		op1 = (*sys_rdb) (((u32) M.x86.R_CS << 4) + (M.x86.R_IP++));
+		(x86emu_optab[op1]) (op1);
+	}
+	/* end major hack mode. */
+}
+
+void x86emu_check_ip_access(void)
+{
+	/* NULL as of now */
+}
+
+void x86emu_check_sp_access(void)
+{
+}
+
+void x86emu_check_mem_access(u32 dummy)
+{
+	/*  check bounds, etc */
+}
+
+void x86emu_check_data_access(uint dummy1, uint dummy2)
+{
+	/*  check bounds, etc */
+}
+
+void x86emu_inc_decoded_inst_len(int x)
+{
+	M.x86.enc_pos += x;
+}
+
+void x86emu_decode_printf(char *x)
+{
+	sprintf(M.x86.decoded_buf + M.x86.enc_str_pos, "%s", x);
+	M.x86.enc_str_pos += strlen(x);
+}
+
+void x86emu_decode_printf2(char *x, int y)
+{
+	char temp[100];
+	sprintf(temp, x, y);
+	sprintf(M.x86.decoded_buf + M.x86.enc_str_pos, "%s", temp);
+	M.x86.enc_str_pos += strlen(temp);
+}
+
+void x86emu_end_instr(void)
+{
+	M.x86.enc_str_pos = 0;
+	M.x86.enc_pos = 0;
+}
+
+static void print_encoded_bytes(u16 s, u16 o)
+{
+	int i;
+	char buf1[64];
+	for (i = 0; i < M.x86.enc_pos; i++) {
+		sprintf(buf1 + 2 * i, "%02x", fetch_data_byte_abs(s, o + i));
+	}
+	printk("%-20s", buf1);
+}
+
+static void print_decoded_instruction(void)
+{
+	printk("%s", M.x86.decoded_buf);
+}
+
+void x86emu_print_int_vect(u16 iv)
+{
+	u16 seg, off;
+
+	if (iv > 256)
+		return;
+	seg = fetch_data_word_abs(0, iv * 4);
+	off = fetch_data_word_abs(0, iv * 4 + 2);
+	printk("%04x:%04x ", seg, off);
+}
+
+void X86EMU_dump_memory(u16 seg, u16 off, u32 amt)
+{
+	u32 start = off & 0xfffffff0;
+	u32 end = (off + 16) & 0xfffffff0;
+	u32 i;
+	u32 current;
+
+	current = start;
+	while (end <= off + amt) {
+		printk("%04x:%04x ", seg, start);
+		for (i = start; i < off; i++)
+			printk("   ");
+		for (; i < end; i++)
+			printk("%02x ", fetch_data_byte_abs(seg, i));
+		printk("\n");
+		start = end;
+		end = start + 16;
+	}
+}
+
+void x86emu_single_step(void)
+{
+	char s[1024];
+	int ps[10];
+	int ntok;
+	int cmd;
+	int done;
+	int segment;
+	int offset;
+	static int breakpoint;
+	static int noDecode = 1;
+
+	char *p;
+
+	if (DEBUG_BREAK()) {
+		if (M.x86.saved_ip != breakpoint) {
+			return;
+		} else {
+			M.x86.debug &= ~DEBUG_DECODE_NOPRINT_F;
+			M.x86.debug |= DEBUG_TRACE_F;
+			M.x86.debug &= ~DEBUG_BREAK_F;
+			print_decoded_instruction();
+			X86EMU_trace_regs();
+		}
+	}
+	done = 0;
+	offset = M.x86.saved_ip;
+	while (!done) {
+		printk("-");
+		cmd = parse_line(s, ps, &ntok);
+		switch (cmd) {
+		case 'u':
+			disassemble_forward(M.x86.saved_cs, (u16) offset, 10);
+			break;
+		case 'd':
+			if (ntok == 2) {
+				segment = M.x86.saved_cs;
+				offset = ps[1];
+				X86EMU_dump_memory(segment, (u16) offset, 16);
+				offset += 16;
+			} else if (ntok == 3) {
+				segment = ps[1];
+				offset = ps[2];
+				X86EMU_dump_memory(segment, (u16) offset, 16);
+				offset += 16;
+			} else {
+				segment = M.x86.saved_cs;
+				X86EMU_dump_memory(segment, (u16) offset, 16);
+				offset += 16;
+			}
+			break;
+		case 'c':
+			M.x86.debug ^= DEBUG_TRACECALL_F;
+			break;
+		case 's':
+			M.x86.debug ^=
+			    DEBUG_SVC_F | DEBUG_SYS_F | DEBUG_SYSINT_F;
+			break;
+		case 'r':
+			X86EMU_trace_regs();
+			break;
+		case 'x':
+			X86EMU_trace_xregs();
+			break;
+		case 'g':
+			if (ntok == 2) {
+				breakpoint = ps[1];
+				if (noDecode) {
+					M.x86.debug |= DEBUG_DECODE_NOPRINT_F;
+				} else {
+					M.x86.debug &= ~DEBUG_DECODE_NOPRINT_F;
+				}
+				M.x86.debug &= ~DEBUG_TRACE_F;
+				M.x86.debug |= DEBUG_BREAK_F;
+				done = 1;
+			}
+			break;
+		case 'q':
+			M.x86.debug |= DEBUG_EXIT;
+			return;
+		case 'P':
+			noDecode = (noDecode) ? 0 : 1;
+			printk("Toggled decoding to %s\n",
+			       (noDecode) ? "FALSE" : "TRUE");
+			break;
+		case 't':
+		case 0:
+			done = 1;
+			break;
+		}
+	}
+}
+
+int X86EMU_trace_on(void)
+{
+	return M.x86.debug |= DEBUG_STEP_F | DEBUG_DECODE_F | DEBUG_TRACE_F;
+}
+
+int X86EMU_trace_off(void)
+{
+	return M.x86.debug &= ~(DEBUG_STEP_F | DEBUG_DECODE_F | DEBUG_TRACE_F);
+}
+
+static int parse_line(char *s, int *ps, int *n)
+{
+	int cmd;
+
+	*n = 0;
+	while (*s == ' ' || *s == '\t')
+		s++;
+	ps[*n] = *s;
+	switch (*s) {
+	case '\n':
+		*n += 1;
+		return 0;
+	default:
+		cmd = *s;
+		*n += 1;
+	}
+
+	while (1) {
+		while (*s != ' ' && *s != '\t' && *s != '\n')
+			s++;
+
+		if (*s == '\n')
+			return cmd;
+
+		while (*s == ' ' || *s == '\t')
+			s++;
+
+		*n += 1;
+	}
+}
+
+#endif				/* DEBUG */
+
+void x86emu_dump_regs(void)
+{
+	printk("\tAX=%04x  ", M.x86.R_AX);
+	printk("BX=%04x  ", M.x86.R_BX);
+	printk("CX=%04x  ", M.x86.R_CX);
+	printk("DX=%04x  ", M.x86.R_DX);
+	printk("SP=%04x  ", M.x86.R_SP);
+	printk("BP=%04x  ", M.x86.R_BP);
+	printk("SI=%04x  ", M.x86.R_SI);
+	printk("DI=%04x\n", M.x86.R_DI);
+	printk("\tDS=%04x  ", M.x86.R_DS);
+	printk("ES=%04x  ", M.x86.R_ES);
+	printk("SS=%04x  ", M.x86.R_SS);
+	printk("CS=%04x  ", M.x86.R_CS);
+	printk("IP=%04x   ", M.x86.R_IP);
+	if (ACCESS_FLAG(F_OF))
+		printk("OV ");	/* CHECKED... */
+	else
+		printk("NV ");
+	if (ACCESS_FLAG(F_DF))
+		printk("DN ");
+	else
+		printk("UP ");
+	if (ACCESS_FLAG(F_IF))
+		printk("EI ");
+	else
+		printk("DI ");
+	if (ACCESS_FLAG(F_SF))
+		printk("NG ");
+	else
+		printk("PL ");
+	if (ACCESS_FLAG(F_ZF))
+		printk("ZR ");
+	else
+		printk("NZ ");
+	if (ACCESS_FLAG(F_AF))
+		printk("AC ");
+	else
+		printk("NA ");
+	if (ACCESS_FLAG(F_PF))
+		printk("PE ");
+	else
+		printk("PO ");
+	if (ACCESS_FLAG(F_CF))
+		printk("CY ");
+	else
+		printk("NC ");
+	printk("\n");
+}
+
+void x86emu_dump_xregs(void)
+{
+	printk("\tEAX=%08x  ", M.x86.R_EAX);
+	printk("EBX=%08x  ", M.x86.R_EBX);
+	printk("ECX=%08x  ", M.x86.R_ECX);
+	printk("EDX=%08x  \n", M.x86.R_EDX);
+	printk("\tESP=%08x  ", M.x86.R_ESP);
+	printk("EBP=%08x  ", M.x86.R_EBP);
+	printk("ESI=%08x  ", M.x86.R_ESI);
+	printk("EDI=%08x\n", M.x86.R_EDI);
+	printk("\tDS=%04x  ", M.x86.R_DS);
+	printk("ES=%04x  ", M.x86.R_ES);
+	printk("SS=%04x  ", M.x86.R_SS);
+	printk("CS=%04x  ", M.x86.R_CS);
+	printk("EIP=%08x\n\t", M.x86.R_EIP);
+	if (ACCESS_FLAG(F_OF))
+		printk("OV ");	/* CHECKED... */
+	else
+		printk("NV ");
+	if (ACCESS_FLAG(F_DF))
+		printk("DN ");
+	else
+		printk("UP ");
+	if (ACCESS_FLAG(F_IF))
+		printk("EI ");
+	else
+		printk("DI ");
+	if (ACCESS_FLAG(F_SF))
+		printk("NG ");
+	else
+		printk("PL ");
+	if (ACCESS_FLAG(F_ZF))
+		printk("ZR ");
+	else
+		printk("NZ ");
+	if (ACCESS_FLAG(F_AF))
+		printk("AC ");
+	else
+		printk("NA ");
+	if (ACCESS_FLAG(F_PF))
+		printk("PE ");
+	else
+		printk("PO ");
+	if (ACCESS_FLAG(F_CF))
+		printk("CY ");
+	else
+		printk("NC ");
+	printk("\n");
+}
+
+#endif
diff --git a/drivers/bios_emulator/x86emu/decode.c b/drivers/bios_emulator/x86emu/decode.c
new file mode 100644
index 00000000000..879f0a06d17
--- /dev/null
+++ b/drivers/bios_emulator/x86emu/decode.c
@@ -0,0 +1,1148 @@
+/****************************************************************************
+*
+*			Realmode X86 Emulator Library
+*
+*		Copyright (C) 1991-2004 SciTech Software, Inc.
+*		     Copyright (C) David Mosberger-Tang
+*		       Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	This file includes subroutines which are related to
+*		instruction decoding and accessess of immediate data via IP.  etc.
+*
+****************************************************************************/
+
+#include "x86emu/x86emui.h"
+
+#if defined(CONFIG_BIOSEMU)
+
+/*----------------------------- Implementation ----------------------------*/
+
+/****************************************************************************
+REMARKS:
+Handles any pending asychronous interrupts.
+****************************************************************************/
+static void x86emu_intr_handle(void)
+{
+    u8	intno;
+
+    if (M.x86.intr & INTR_SYNCH) {
+	intno = M.x86.intno;
+	if (_X86EMU_intrTab[intno]) {
+	    (*_X86EMU_intrTab[intno])(intno);
+	} else {
+	    push_word((u16)M.x86.R_FLG);
+	    CLEAR_FLAG(F_IF);
+	    CLEAR_FLAG(F_TF);
+	    push_word(M.x86.R_CS);
+	    M.x86.R_CS = mem_access_word(intno * 4 + 2);
+	    push_word(M.x86.R_IP);
+	    M.x86.R_IP = mem_access_word(intno * 4);
+	    M.x86.intr = 0;
+	}
+    }
+}
+
+/****************************************************************************
+PARAMETERS:
+intrnum - Interrupt number to raise
+
+REMARKS:
+Raise the specified interrupt to be handled before the execution of the
+next instruction.
+****************************************************************************/
+void x86emu_intr_raise(
+    u8 intrnum)
+{
+    M.x86.intno = intrnum;
+    M.x86.intr |= INTR_SYNCH;
+}
+
+/****************************************************************************
+REMARKS:
+Main execution loop for the emulator. We return from here when the system
+halts, which is normally caused by a stack fault when we return from the
+original real mode call.
+****************************************************************************/
+void X86EMU_exec(void)
+{
+    u8 op1;
+
+    M.x86.intr = 0;
+    DB(x86emu_end_instr();)
+
+    for (;;) {
+DB(	if (CHECK_IP_FETCH())
+	    x86emu_check_ip_access();)
+	/* If debugging, save the IP and CS values. */
+	SAVE_IP_CS(M.x86.R_CS, M.x86.R_IP);
+	INC_DECODED_INST_LEN(1);
+	if (M.x86.intr) {
+	    if (M.x86.intr & INTR_HALTED) {
+DB(		if (M.x86.R_SP != 0) {
+		    printk("halted\n");
+		    X86EMU_trace_regs();
+		    }
+		else {
+		    if (M.x86.debug)
+			printk("Service completed successfully\n");
+		    })
+		return;
+	    }
+	    if (((M.x86.intr & INTR_SYNCH) && (M.x86.intno == 0 || M.x86.intno == 2)) ||
+		!ACCESS_FLAG(F_IF)) {
+		x86emu_intr_handle();
+	    }
+	}
+	op1 = (*sys_rdb)(((u32)M.x86.R_CS << 4) + (M.x86.R_IP++));
+	(*x86emu_optab[op1])(op1);
+	if (M.x86.debug & DEBUG_EXIT) {
+	    M.x86.debug &= ~DEBUG_EXIT;
+	    return;
+	}
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Halts the system by setting the halted system flag.
+****************************************************************************/
+void X86EMU_halt_sys(void)
+{
+    M.x86.intr |= INTR_HALTED;
+}
+
+/****************************************************************************
+PARAMETERS:
+mod	- Mod value from decoded byte
+regh	- Reg h value from decoded byte
+regl	- Reg l value from decoded byte
+
+REMARKS:
+Raise the specified interrupt to be handled before the execution of the
+next instruction.
+
+NOTE: Do not inline this function, as (*sys_rdb) is already inline!
+****************************************************************************/
+void fetch_decode_modrm(
+    int *mod,
+    int *regh,
+    int *regl)
+{
+    int fetched;
+
+DB( if (CHECK_IP_FETCH())
+	x86emu_check_ip_access();)
+    fetched = (*sys_rdb)(((u32)M.x86.R_CS << 4) + (M.x86.R_IP++));
+    INC_DECODED_INST_LEN(1);
+    *mod  = (fetched >> 6) & 0x03;
+    *regh = (fetched >> 3) & 0x07;
+    *regl = (fetched >> 0) & 0x07;
+}
+
+/****************************************************************************
+RETURNS:
+Immediate byte value read from instruction queue
+
+REMARKS:
+This function returns the immediate byte from the instruction queue, and
+moves the instruction pointer to the next value.
+
+NOTE: Do not inline this function, as (*sys_rdb) is already inline!
+****************************************************************************/
+u8 fetch_byte_imm(void)
+{
+    u8 fetched;
+
+DB( if (CHECK_IP_FETCH())
+	x86emu_check_ip_access();)
+    fetched = (*sys_rdb)(((u32)M.x86.R_CS << 4) + (M.x86.R_IP++));
+    INC_DECODED_INST_LEN(1);
+    return fetched;
+}
+
+/****************************************************************************
+RETURNS:
+Immediate word value read from instruction queue
+
+REMARKS:
+This function returns the immediate byte from the instruction queue, and
+moves the instruction pointer to the next value.
+
+NOTE: Do not inline this function, as (*sys_rdw) is already inline!
+****************************************************************************/
+u16 fetch_word_imm(void)
+{
+    u16 fetched;
+
+DB( if (CHECK_IP_FETCH())
+	x86emu_check_ip_access();)
+    fetched = (*sys_rdw)(((u32)M.x86.R_CS << 4) + (M.x86.R_IP));
+    M.x86.R_IP += 2;
+    INC_DECODED_INST_LEN(2);
+    return fetched;
+}
+
+/****************************************************************************
+RETURNS:
+Immediate lone value read from instruction queue
+
+REMARKS:
+This function returns the immediate byte from the instruction queue, and
+moves the instruction pointer to the next value.
+
+NOTE: Do not inline this function, as (*sys_rdw) is already inline!
+****************************************************************************/
+u32 fetch_long_imm(void)
+{
+    u32 fetched;
+
+DB( if (CHECK_IP_FETCH())
+	x86emu_check_ip_access();)
+    fetched = (*sys_rdl)(((u32)M.x86.R_CS << 4) + (M.x86.R_IP));
+    M.x86.R_IP += 4;
+    INC_DECODED_INST_LEN(4);
+    return fetched;
+}
+
+/****************************************************************************
+RETURNS:
+Value of the default data segment
+
+REMARKS:
+Inline function that returns the default data segment for the current
+instruction.
+
+On the x86 processor, the default segment is not always DS if there is
+no segment override. Address modes such as -3[BP] or 10[BP+SI] all refer to
+addresses relative to SS (ie: on the stack). So, at the minimum, all
+decodings of addressing modes would have to set/clear a bit describing
+whether the access is relative to DS or SS.  That is the function of the
+cpu-state-varible M.x86.mode. There are several potential states:
+
+    repe prefix seen  (handled elsewhere)
+    repne prefix seen  (ditto)
+
+    cs segment override
+    ds segment override
+    es segment override
+    fs segment override
+    gs segment override
+    ss segment override
+
+    ds/ss select (in absense of override)
+
+Each of the above 7 items are handled with a bit in the mode field.
+****************************************************************************/
+_INLINE u32 get_data_segment(void)
+{
+#define GET_SEGMENT(segment)
+    switch (M.x86.mode & SYSMODE_SEGMASK) {
+      case 0:			/* default case: use ds register */
+      case SYSMODE_SEGOVR_DS:
+      case SYSMODE_SEGOVR_DS | SYSMODE_SEG_DS_SS:
+	return	M.x86.R_DS;
+      case SYSMODE_SEG_DS_SS:	/* non-overridden, use ss register */
+	return	M.x86.R_SS;
+      case SYSMODE_SEGOVR_CS:
+      case SYSMODE_SEGOVR_CS | SYSMODE_SEG_DS_SS:
+	return	M.x86.R_CS;
+      case SYSMODE_SEGOVR_ES:
+      case SYSMODE_SEGOVR_ES | SYSMODE_SEG_DS_SS:
+	return	M.x86.R_ES;
+      case SYSMODE_SEGOVR_FS:
+      case SYSMODE_SEGOVR_FS | SYSMODE_SEG_DS_SS:
+	return	M.x86.R_FS;
+      case SYSMODE_SEGOVR_GS:
+      case SYSMODE_SEGOVR_GS | SYSMODE_SEG_DS_SS:
+	return	M.x86.R_GS;
+      case SYSMODE_SEGOVR_SS:
+      case SYSMODE_SEGOVR_SS | SYSMODE_SEG_DS_SS:
+	return	M.x86.R_SS;
+      default:
+#ifdef	DEBUG
+	printk("error: should not happen:  multiple overrides.\n");
+#endif
+	HALT_SYS();
+	return 0;
+    }
+}
+
+/****************************************************************************
+PARAMETERS:
+offset	- Offset to load data from
+
+RETURNS:
+Byte value read from the absolute memory location.
+
+NOTE: Do not inline this function as (*sys_rdX) is already inline!
+****************************************************************************/
+u8 fetch_data_byte(
+    uint offset)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access((u16)get_data_segment(), offset);
+#endif
+    return (*sys_rdb)((get_data_segment() << 4) + offset);
+}
+
+/****************************************************************************
+PARAMETERS:
+offset	- Offset to load data from
+
+RETURNS:
+Word value read from the absolute memory location.
+
+NOTE: Do not inline this function as (*sys_rdX) is already inline!
+****************************************************************************/
+u16 fetch_data_word(
+    uint offset)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access((u16)get_data_segment(), offset);
+#endif
+    return (*sys_rdw)((get_data_segment() << 4) + offset);
+}
+
+/****************************************************************************
+PARAMETERS:
+offset	- Offset to load data from
+
+RETURNS:
+Long value read from the absolute memory location.
+
+NOTE: Do not inline this function as (*sys_rdX) is already inline!
+****************************************************************************/
+u32 fetch_data_long(
+    uint offset)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access((u16)get_data_segment(), offset);
+#endif
+    return (*sys_rdl)((get_data_segment() << 4) + offset);
+}
+
+/****************************************************************************
+PARAMETERS:
+segment - Segment to load data from
+offset	- Offset to load data from
+
+RETURNS:
+Byte value read from the absolute memory location.
+
+NOTE: Do not inline this function as (*sys_rdX) is already inline!
+****************************************************************************/
+u8 fetch_data_byte_abs(
+    uint segment,
+    uint offset)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access(segment, offset);
+#endif
+    return (*sys_rdb)(((u32)segment << 4) + offset);
+}
+
+/****************************************************************************
+PARAMETERS:
+segment - Segment to load data from
+offset	- Offset to load data from
+
+RETURNS:
+Word value read from the absolute memory location.
+
+NOTE: Do not inline this function as (*sys_rdX) is already inline!
+****************************************************************************/
+u16 fetch_data_word_abs(
+    uint segment,
+    uint offset)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access(segment, offset);
+#endif
+    return (*sys_rdw)(((u32)segment << 4) + offset);
+}
+
+/****************************************************************************
+PARAMETERS:
+segment - Segment to load data from
+offset	- Offset to load data from
+
+RETURNS:
+Long value read from the absolute memory location.
+
+NOTE: Do not inline this function as (*sys_rdX) is already inline!
+****************************************************************************/
+u32 fetch_data_long_abs(
+    uint segment,
+    uint offset)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access(segment, offset);
+#endif
+    return (*sys_rdl)(((u32)segment << 4) + offset);
+}
+
+/****************************************************************************
+PARAMETERS:
+offset	- Offset to store data at
+val	- Value to store
+
+REMARKS:
+Writes a word value to an segmented memory location. The segment used is
+the current 'default' segment, which may have been overridden.
+
+NOTE: Do not inline this function as (*sys_wrX) is already inline!
+****************************************************************************/
+void store_data_byte(
+    uint offset,
+    u8 val)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access((u16)get_data_segment(), offset);
+#endif
+    (*sys_wrb)((get_data_segment() << 4) + offset, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+offset	- Offset to store data at
+val	- Value to store
+
+REMARKS:
+Writes a word value to an segmented memory location. The segment used is
+the current 'default' segment, which may have been overridden.
+
+NOTE: Do not inline this function as (*sys_wrX) is already inline!
+****************************************************************************/
+void store_data_word(
+    uint offset,
+    u16 val)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access((u16)get_data_segment(), offset);
+#endif
+    (*sys_wrw)((get_data_segment() << 4) + offset, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+offset	- Offset to store data at
+val	- Value to store
+
+REMARKS:
+Writes a long value to an segmented memory location. The segment used is
+the current 'default' segment, which may have been overridden.
+
+NOTE: Do not inline this function as (*sys_wrX) is already inline!
+****************************************************************************/
+void store_data_long(
+    uint offset,
+    u32 val)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access((u16)get_data_segment(), offset);
+#endif
+    (*sys_wrl)((get_data_segment() << 4) + offset, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+segment - Segment to store data at
+offset	- Offset to store data at
+val	- Value to store
+
+REMARKS:
+Writes a byte value to an absolute memory location.
+
+NOTE: Do not inline this function as (*sys_wrX) is already inline!
+****************************************************************************/
+void store_data_byte_abs(
+    uint segment,
+    uint offset,
+    u8 val)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access(segment, offset);
+#endif
+    (*sys_wrb)(((u32)segment << 4) + offset, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+segment - Segment to store data at
+offset	- Offset to store data at
+val	- Value to store
+
+REMARKS:
+Writes a word value to an absolute memory location.
+
+NOTE: Do not inline this function as (*sys_wrX) is already inline!
+****************************************************************************/
+void store_data_word_abs(
+    uint segment,
+    uint offset,
+    u16 val)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access(segment, offset);
+#endif
+    (*sys_wrw)(((u32)segment << 4) + offset, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+segment - Segment to store data at
+offset	- Offset to store data at
+val	- Value to store
+
+REMARKS:
+Writes a long value to an absolute memory location.
+
+NOTE: Do not inline this function as (*sys_wrX) is already inline!
+****************************************************************************/
+void store_data_long_abs(
+    uint segment,
+    uint offset,
+    u32 val)
+{
+#ifdef DEBUG
+    if (CHECK_DATA_ACCESS())
+	x86emu_check_data_access(segment, offset);
+#endif
+    (*sys_wrl)(((u32)segment << 4) + offset, val);
+}
+
+/****************************************************************************
+PARAMETERS:
+reg - Register to decode
+
+RETURNS:
+Pointer to the appropriate register
+
+REMARKS:
+Return a pointer to the register given by the R/RM field of the
+modrm byte, for byte operands. Also enables the decoding of instructions.
+****************************************************************************/
+u8* decode_rm_byte_register(
+    int reg)
+{
+    switch (reg) {
+      case 0:
+	DECODE_PRINTF("AL");
+	return &M.x86.R_AL;
+      case 1:
+	DECODE_PRINTF("CL");
+	return &M.x86.R_CL;
+      case 2:
+	DECODE_PRINTF("DL");
+	return &M.x86.R_DL;
+      case 3:
+	DECODE_PRINTF("BL");
+	return &M.x86.R_BL;
+      case 4:
+	DECODE_PRINTF("AH");
+	return &M.x86.R_AH;
+      case 5:
+	DECODE_PRINTF("CH");
+	return &M.x86.R_CH;
+      case 6:
+	DECODE_PRINTF("DH");
+	return &M.x86.R_DH;
+      case 7:
+	DECODE_PRINTF("BH");
+	return &M.x86.R_BH;
+    }
+    HALT_SYS();
+    return NULL;		/* NOT REACHED OR REACHED ON ERROR */
+}
+
+/****************************************************************************
+PARAMETERS:
+reg - Register to decode
+
+RETURNS:
+Pointer to the appropriate register
+
+REMARKS:
+Return a pointer to the register given by the R/RM field of the
+modrm byte, for word operands.	Also enables the decoding of instructions.
+****************************************************************************/
+u16* decode_rm_word_register(
+    int reg)
+{
+    switch (reg) {
+      case 0:
+	DECODE_PRINTF("AX");
+	return &M.x86.R_AX;
+      case 1:
+	DECODE_PRINTF("CX");
+	return &M.x86.R_CX;
+      case 2:
+	DECODE_PRINTF("DX");
+	return &M.x86.R_DX;
+      case 3:
+	DECODE_PRINTF("BX");
+	return &M.x86.R_BX;
+      case 4:
+	DECODE_PRINTF("SP");
+	return &M.x86.R_SP;
+      case 5:
+	DECODE_PRINTF("BP");
+	return &M.x86.R_BP;
+      case 6:
+	DECODE_PRINTF("SI");
+	return &M.x86.R_SI;
+      case 7:
+	DECODE_PRINTF("DI");
+	return &M.x86.R_DI;
+    }
+    HALT_SYS();
+    return NULL;		/* NOTREACHED OR REACHED ON ERROR */
+}
+
+/****************************************************************************
+PARAMETERS:
+reg - Register to decode
+
+RETURNS:
+Pointer to the appropriate register
+
+REMARKS:
+Return a pointer to the register given by the R/RM field of the
+modrm byte, for dword operands.	 Also enables the decoding of instructions.
+****************************************************************************/
+u32* decode_rm_long_register(
+    int reg)
+{
+    switch (reg) {
+      case 0:
+	DECODE_PRINTF("EAX");
+	return &M.x86.R_EAX;
+      case 1:
+	DECODE_PRINTF("ECX");
+	return &M.x86.R_ECX;
+      case 2:
+	DECODE_PRINTF("EDX");
+	return &M.x86.R_EDX;
+      case 3:
+	DECODE_PRINTF("EBX");
+	return &M.x86.R_EBX;
+      case 4:
+	DECODE_PRINTF("ESP");
+	return &M.x86.R_ESP;
+      case 5:
+	DECODE_PRINTF("EBP");
+	return &M.x86.R_EBP;
+      case 6:
+	DECODE_PRINTF("ESI");
+	return &M.x86.R_ESI;
+      case 7:
+	DECODE_PRINTF("EDI");
+	return &M.x86.R_EDI;
+    }
+    HALT_SYS();
+    return NULL;		/* NOTREACHED OR REACHED ON ERROR */
+}
+
+/****************************************************************************
+PARAMETERS:
+reg - Register to decode
+
+RETURNS:
+Pointer to the appropriate register
+
+REMARKS:
+Return a pointer to the register given by the R/RM field of the
+modrm byte, for word operands, modified from above for the weirdo
+special case of segreg operands.  Also enables the decoding of instructions.
+****************************************************************************/
+u16* decode_rm_seg_register(
+    int reg)
+{
+    switch (reg) {
+      case 0:
+	DECODE_PRINTF("ES");
+	return &M.x86.R_ES;
+      case 1:
+	DECODE_PRINTF("CS");
+	return &M.x86.R_CS;
+      case 2:
+	DECODE_PRINTF("SS");
+	return &M.x86.R_SS;
+      case 3:
+	DECODE_PRINTF("DS");
+	return &M.x86.R_DS;
+      case 4:
+	DECODE_PRINTF("FS");
+	return &M.x86.R_FS;
+      case 5:
+	DECODE_PRINTF("GS");
+	return &M.x86.R_GS;
+      case 6:
+      case 7:
+	DECODE_PRINTF("ILLEGAL SEGREG");
+	break;
+    }
+    HALT_SYS();
+    return NULL;		/* NOT REACHED OR REACHED ON ERROR */
+}
+
+/****************************************************************************
+PARAMETERS:
+scale - scale value of SIB byte
+index - index value of SIB byte
+
+RETURNS:
+Value of scale * index
+
+REMARKS:
+Decodes scale/index of SIB byte and returns relevant offset part of
+effective address.
+****************************************************************************/
+unsigned decode_sib_si(
+    int scale,
+    int index)
+{
+    scale = 1 << scale;
+    if (scale > 1) {
+	DECODE_PRINTF2("[%d*", scale);
+    } else {
+	DECODE_PRINTF("[");
+    }
+    switch (index) {
+      case 0:
+	DECODE_PRINTF("EAX]");
+	return M.x86.R_EAX * index;
+      case 1:
+	DECODE_PRINTF("ECX]");
+	return M.x86.R_ECX * index;
+      case 2:
+	DECODE_PRINTF("EDX]");
+	return M.x86.R_EDX * index;
+      case 3:
+	DECODE_PRINTF("EBX]");
+	return M.x86.R_EBX * index;
+      case 4:
+	DECODE_PRINTF("0]");
+	return 0;
+      case 5:
+	DECODE_PRINTF("EBP]");
+	return M.x86.R_EBP * index;
+      case 6:
+	DECODE_PRINTF("ESI]");
+	return M.x86.R_ESI * index;
+      case 7:
+	DECODE_PRINTF("EDI]");
+	return M.x86.R_EDI * index;
+    }
+    HALT_SYS();
+    return 0;			/* NOT REACHED OR REACHED ON ERROR */
+}
+
+/****************************************************************************
+PARAMETERS:
+mod - MOD value of preceding ModR/M byte
+
+RETURNS:
+Offset in memory for the address decoding
+
+REMARKS:
+Decodes SIB addressing byte and returns calculated effective address.
+****************************************************************************/
+unsigned decode_sib_address(
+    int mod)
+{
+    int sib   = fetch_byte_imm();
+    int ss    = (sib >> 6) & 0x03;
+    int index = (sib >> 3) & 0x07;
+    int base  = sib & 0x07;
+    int offset = 0;
+    int displacement;
+
+    switch (base) {
+      case 0:
+	DECODE_PRINTF("[EAX]");
+	offset = M.x86.R_EAX;
+	break;
+      case 1:
+	DECODE_PRINTF("[ECX]");
+	offset = M.x86.R_ECX;
+	break;
+      case 2:
+	DECODE_PRINTF("[EDX]");
+	offset = M.x86.R_EDX;
+	break;
+      case 3:
+	DECODE_PRINTF("[EBX]");
+	offset = M.x86.R_EBX;
+	break;
+      case 4:
+	DECODE_PRINTF("[ESP]");
+	offset = M.x86.R_ESP;
+	break;
+      case 5:
+	switch (mod) {
+	  case 0:
+	    displacement = (s32)fetch_long_imm();
+	    DECODE_PRINTF2("[%d]", displacement);
+	    offset = displacement;
+	    break;
+	  case 1:
+	    displacement = (s8)fetch_byte_imm();
+	    DECODE_PRINTF2("[%d][EBP]", displacement);
+	    offset = M.x86.R_EBP + displacement;
+	    break;
+	  case 2:
+	    displacement = (s32)fetch_long_imm();
+	    DECODE_PRINTF2("[%d][EBP]", displacement);
+	    offset = M.x86.R_EBP + displacement;
+	    break;
+	  default:
+	    HALT_SYS();
+	}
+	DECODE_PRINTF("[EAX]");
+	offset = M.x86.R_EAX;
+	break;
+      case 6:
+	DECODE_PRINTF("[ESI]");
+	offset = M.x86.R_ESI;
+	break;
+      case 7:
+	DECODE_PRINTF("[EDI]");
+	offset = M.x86.R_EDI;
+	break;
+      default:
+	HALT_SYS();
+    }
+    offset += decode_sib_si(ss, index);
+    return offset;
+
+}
+
+/****************************************************************************
+PARAMETERS:
+rm  - RM value to decode
+
+RETURNS:
+Offset in memory for the address decoding
+
+REMARKS:
+Return the offset given by mod=00 addressing.  Also enables the
+decoding of instructions.
+
+NOTE:	The code which specifies the corresponding segment (ds vs ss)
+	below in the case of [BP+..].  The assumption here is that at the
+	point that this subroutine is called, the bit corresponding to
+	SYSMODE_SEG_DS_SS will be zero.	 After every instruction
+	except the segment override instructions, this bit (as well
+	as any bits indicating segment overrides) will be clear.  So
+	if a SS access is needed, set this bit.	 Otherwise, DS access
+	occurs (unless any of the segment override bits are set).
+****************************************************************************/
+unsigned decode_rm00_address(
+    int rm)
+{
+    unsigned offset;
+
+    if (M.x86.mode & SYSMODE_PREFIX_ADDR) {
+	/* 32-bit addressing */
+	switch (rm) {
+	  case 0:
+	    DECODE_PRINTF("[EAX]");
+	    return M.x86.R_EAX;
+	  case 1:
+	    DECODE_PRINTF("[ECX]");
+	    return M.x86.R_ECX;
+	  case 2:
+	    DECODE_PRINTF("[EDX]");
+	    return M.x86.R_EDX;
+	  case 3:
+	    DECODE_PRINTF("[EBX]");
+	    return M.x86.R_EBX;
+	  case 4:
+	    return decode_sib_address(0);
+	  case 5:
+	    offset = fetch_long_imm();
+	    DECODE_PRINTF2("[%08x]", offset);
+	    return offset;
+	  case 6:
+	    DECODE_PRINTF("[ESI]");
+	    return M.x86.R_ESI;
+	  case 7:
+	    DECODE_PRINTF("[EDI]");
+	    return M.x86.R_EDI;
+	}
+    } else {
+	/* 16-bit addressing */
+	switch (rm) {
+	  case 0:
+	    DECODE_PRINTF("[BX+SI]");
+	    return (M.x86.R_BX + M.x86.R_SI) & 0xffff;
+	  case 1:
+	    DECODE_PRINTF("[BX+DI]");
+	    return (M.x86.R_BX + M.x86.R_DI) & 0xffff;
+	  case 2:
+	    DECODE_PRINTF("[BP+SI]");
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + M.x86.R_SI) & 0xffff;
+	  case 3:
+	    DECODE_PRINTF("[BP+DI]");
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + M.x86.R_DI) & 0xffff;
+	  case 4:
+	    DECODE_PRINTF("[SI]");
+	    return M.x86.R_SI;
+	  case 5:
+	    DECODE_PRINTF("[DI]");
+	    return M.x86.R_DI;
+	  case 6:
+	    offset = fetch_word_imm();
+	    DECODE_PRINTF2("[%04x]", offset);
+	    return offset;
+	  case 7:
+	    DECODE_PRINTF("[BX]");
+	    return M.x86.R_BX;
+	}
+    }
+    HALT_SYS();
+    return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+rm  - RM value to decode
+
+RETURNS:
+Offset in memory for the address decoding
+
+REMARKS:
+Return the offset given by mod=01 addressing.  Also enables the
+decoding of instructions.
+****************************************************************************/
+unsigned decode_rm01_address(
+    int rm)
+{
+    int displacement;
+
+    if (M.x86.mode & SYSMODE_PREFIX_ADDR) {
+	/* 32-bit addressing */
+	if (rm != 4)
+	    displacement = (s8)fetch_byte_imm();
+	else
+	    displacement = 0;
+
+	switch (rm) {
+	  case 0:
+	    DECODE_PRINTF2("%d[EAX]", displacement);
+	    return M.x86.R_EAX + displacement;
+	  case 1:
+	    DECODE_PRINTF2("%d[ECX]", displacement);
+	    return M.x86.R_ECX + displacement;
+	  case 2:
+	    DECODE_PRINTF2("%d[EDX]", displacement);
+	    return M.x86.R_EDX + displacement;
+	  case 3:
+	    DECODE_PRINTF2("%d[EBX]", displacement);
+	    return M.x86.R_EBX + displacement;
+	  case 4: {
+	    int offset = decode_sib_address(1);
+	    displacement = (s8)fetch_byte_imm();
+	    DECODE_PRINTF2("[%d]", displacement);
+	    return offset + displacement;
+	  }
+	  case 5:
+	    DECODE_PRINTF2("%d[EBP]", displacement);
+	    return M.x86.R_EBP + displacement;
+	  case 6:
+	    DECODE_PRINTF2("%d[ESI]", displacement);
+	    return M.x86.R_ESI + displacement;
+	  case 7:
+	    DECODE_PRINTF2("%d[EDI]", displacement);
+	    return M.x86.R_EDI + displacement;
+	}
+    } else {
+	/* 16-bit addressing */
+	displacement = (s8)fetch_byte_imm();
+	switch (rm) {
+	  case 0:
+	    DECODE_PRINTF2("%d[BX+SI]", displacement);
+	    return (M.x86.R_BX + M.x86.R_SI + displacement) & 0xffff;
+	  case 1:
+	    DECODE_PRINTF2("%d[BX+DI]", displacement);
+	    return (M.x86.R_BX + M.x86.R_DI + displacement) & 0xffff;
+	  case 2:
+	    DECODE_PRINTF2("%d[BP+SI]", displacement);
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + M.x86.R_SI + displacement) & 0xffff;
+	  case 3:
+	    DECODE_PRINTF2("%d[BP+DI]", displacement);
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + M.x86.R_DI + displacement) & 0xffff;
+	  case 4:
+	    DECODE_PRINTF2("%d[SI]", displacement);
+	    return (M.x86.R_SI + displacement) & 0xffff;
+	  case 5:
+	    DECODE_PRINTF2("%d[DI]", displacement);
+	    return (M.x86.R_DI + displacement) & 0xffff;
+	  case 6:
+	    DECODE_PRINTF2("%d[BP]", displacement);
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + displacement) & 0xffff;
+	  case 7:
+	    DECODE_PRINTF2("%d[BX]", displacement);
+	    return (M.x86.R_BX + displacement) & 0xffff;
+	}
+    }
+    HALT_SYS();
+    return 0;			/* SHOULD NOT HAPPEN */
+}
+
+/****************************************************************************
+PARAMETERS:
+rm  - RM value to decode
+
+RETURNS:
+Offset in memory for the address decoding
+
+REMARKS:
+Return the offset given by mod=10 addressing.  Also enables the
+decoding of instructions.
+****************************************************************************/
+unsigned decode_rm10_address(
+    int rm)
+{
+    if (M.x86.mode & SYSMODE_PREFIX_ADDR) {
+	int displacement;
+
+	/* 32-bit addressing */
+	if (rm != 4)
+	    displacement = (s32)fetch_long_imm();
+	else
+	    displacement = 0;
+
+	switch (rm) {
+	  case 0:
+	    DECODE_PRINTF2("%d[EAX]", displacement);
+	    return M.x86.R_EAX + displacement;
+	  case 1:
+	    DECODE_PRINTF2("%d[ECX]", displacement);
+	    return M.x86.R_ECX + displacement;
+	  case 2:
+	    DECODE_PRINTF2("%d[EDX]", displacement);
+	    return M.x86.R_EDX + displacement;
+	  case 3:
+	    DECODE_PRINTF2("%d[EBX]", displacement);
+	    return M.x86.R_EBX + displacement;
+	  case 4: {
+	    int offset = decode_sib_address(2);
+	    displacement = (s32)fetch_long_imm();
+	    DECODE_PRINTF2("[%d]", displacement);
+	    return offset + displacement;
+	  }
+	  case 5:
+	    DECODE_PRINTF2("%d[EBP]", displacement);
+	    return M.x86.R_EBP + displacement;
+	  case 6:
+	    DECODE_PRINTF2("%d[ESI]", displacement);
+	    return M.x86.R_ESI + displacement;
+	  case 7:
+	    DECODE_PRINTF2("%d[EDI]", displacement);
+	    return M.x86.R_EDI + displacement;
+	}
+    } else {
+	int displacement = (s16)fetch_word_imm();
+
+	/* 16-bit addressing */
+	switch (rm) {
+	  case 0:
+	    DECODE_PRINTF2("%d[BX+SI]", displacement);
+	    return (M.x86.R_BX + M.x86.R_SI + displacement) & 0xffff;
+	  case 1:
+	    DECODE_PRINTF2("%d[BX+DI]", displacement);
+	    return (M.x86.R_BX + M.x86.R_DI + displacement) & 0xffff;
+	  case 2:
+	    DECODE_PRINTF2("%d[BP+SI]", displacement);
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + M.x86.R_SI + displacement) & 0xffff;
+	  case 3:
+	    DECODE_PRINTF2("%d[BP+DI]", displacement);
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + M.x86.R_DI + displacement) & 0xffff;
+	  case 4:
+	    DECODE_PRINTF2("%d[SI]", displacement);
+	    return (M.x86.R_SI + displacement) & 0xffff;
+	  case 5:
+	    DECODE_PRINTF2("%d[DI]", displacement);
+	    return (M.x86.R_DI + displacement) & 0xffff;
+	  case 6:
+	    DECODE_PRINTF2("%d[BP]", displacement);
+	    M.x86.mode |= SYSMODE_SEG_DS_SS;
+	    return (M.x86.R_BP + displacement) & 0xffff;
+	  case 7:
+	    DECODE_PRINTF2("%d[BX]", displacement);
+	    return (M.x86.R_BX + displacement) & 0xffff;
+	}
+    }
+    HALT_SYS();
+    return 0;			/* SHOULD NOT HAPPEN */
+}
+
+/****************************************************************************
+PARAMETERS:
+mod - modifier
+rm  - RM value to decode
+
+RETURNS:
+Offset in memory for the address decoding, multiplexing calls to
+the decode_rmXX_address functions
+
+REMARKS:
+Return the offset given by "mod" addressing.
+****************************************************************************/
+
+unsigned decode_rmXX_address(int mod, int rm)
+{
+  if(mod == 0)
+    return decode_rm00_address(rm);
+  if(mod == 1)
+    return decode_rm01_address(rm);
+  return decode_rm10_address(rm);
+}
+
+#endif
diff --git a/drivers/bios_emulator/x86emu/ops.c b/drivers/bios_emulator/x86emu/ops.c
new file mode 100644
index 00000000000..d334fb5b1cf
--- /dev/null
+++ b/drivers/bios_emulator/x86emu/ops.c
@@ -0,0 +1,5436 @@
+/****************************************************************************
+*			Realmode X86 Emulator Library
+*
+*  Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*  Jason Jin <Jason.jin@freescale.com>
+*
+*		Copyright (C) 1991-2004 SciTech Software, Inc.
+*				     Copyright (C) David Mosberger-Tang
+*					   Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:		ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	This file includes subroutines to implement the decoding
+*		and emulation of all the x86 processor instructions.
+*
+* There are approximately 250 subroutines in here, which correspond
+* to the 256 byte-"opcodes" found on the 8086.	The table which
+* dispatches this is found in the files optab.[ch].
+*
+* Each opcode proc has a comment preceeding it which gives it's table
+* address.  Several opcodes are missing (undefined) in the table.
+*
+* Each proc includes information for decoding (DECODE_PRINTF and
+* DECODE_PRINTF2), debugging (TRACE_REGS, SINGLE_STEP), and misc
+* functions (START_OF_INSTR, END_OF_INSTR).
+*
+* Many of the procedures are *VERY* similar in coding.	This has
+* allowed for a very large amount of code to be generated in a fairly
+* short amount of time (i.e. cut, paste, and modify).  The result is
+* that much of the code below could have been folded into subroutines
+* for a large reduction in size of this file.  The downside would be
+* that there would be a penalty in execution speed.  The file could
+* also have been *MUCH* larger by inlining certain functions which
+* were called.	This could have resulted even faster execution.	 The
+* prime directive I used to decide whether to inline the code or to
+* modularize it, was basically: 1) no unnecessary subroutine calls,
+* 2) no routines more than about 200 lines in size, and 3) modularize
+* any code that I might not get right the first time.  The fetch_*
+* subroutines fall into the latter category.  The The decode_* fall
+* into the second category.  The coding of the "switch(mod){ .... }"
+* in many of the subroutines below falls into the first category.
+* Especially, the coding of {add,and,or,sub,...}_{byte,word}
+* subroutines are an especially glaring case of the third guideline.
+* Since so much of the code is cloned from other modules (compare
+* opcode #00 to opcode #01), making the basic operations subroutine
+* calls is especially important; otherwise mistakes in coding an
+* "add" would represent a nightmare in maintenance.
+*
+* Jason ported this file to u-boot. place all the function pointer in
+* the got2 sector. Removed some opcode.
+*
+****************************************************************************/
+
+#include "x86emu/x86emui.h"
+
+#if defined(CONFIG_BIOSEMU)
+
+/*----------------------------- Implementation ----------------------------*/
+
+/* constant arrays to do several instructions in just one function */
+
+#ifdef DEBUG
+static char *x86emu_GenOpName[8] = {
+    "ADD", "OR", "ADC", "SBB", "AND", "SUB", "XOR", "CMP"};
+#endif
+
+/* used by several opcodes  */
+static u8 (*genop_byte_operation[])(u8 d, u8 s) __attribute__ ((section(".got2"))) =
+{
+    add_byte,		/* 00 */
+    or_byte,		/* 01 */
+    adc_byte,		/* 02 */
+    sbb_byte,		/* 03 */
+    and_byte,		/* 04 */
+    sub_byte,		/* 05 */
+    xor_byte,		/* 06 */
+    cmp_byte,		/* 07 */
+};
+
+static u16 (*genop_word_operation[])(u16 d, u16 s) __attribute__ ((section(".got2"))) =
+{
+    add_word,		/*00 */
+    or_word,		/*01 */
+    adc_word,		/*02 */
+    sbb_word,		/*03 */
+    and_word,		/*04 */
+    sub_word,		/*05 */
+    xor_word,		/*06 */
+    cmp_word,		/*07 */
+};
+
+static u32 (*genop_long_operation[])(u32 d, u32 s) __attribute__ ((section(".got2"))) =
+{
+    add_long,		/*00 */
+    or_long,		/*01 */
+    adc_long,		/*02 */
+    sbb_long,		/*03 */
+    and_long,		/*04 */
+    sub_long,		/*05 */
+    xor_long,		/*06 */
+    cmp_long,		/*07 */
+};
+
+/* used by opcodes 80, c0, d0, and d2. */
+static u8(*opcD0_byte_operation[])(u8 d, u8 s) __attribute__ ((section(".got2"))) =
+{
+    rol_byte,
+    ror_byte,
+    rcl_byte,
+    rcr_byte,
+    shl_byte,
+    shr_byte,
+    shl_byte,		/* sal_byte === shl_byte  by definition */
+    sar_byte,
+};
+
+/* used by opcodes c1, d1, and d3. */
+static u16(*opcD1_word_operation[])(u16 s, u8 d) __attribute__ ((section(".got2"))) =
+{
+    rol_word,
+    ror_word,
+    rcl_word,
+    rcr_word,
+    shl_word,
+    shr_word,
+    shl_word,		/* sal_byte === shl_byte  by definition */
+    sar_word,
+};
+
+/* used by opcodes c1, d1, and d3. */
+static u32 (*opcD1_long_operation[])(u32 s, u8 d) __attribute__ ((section(".got2"))) =
+{
+    rol_long,
+    ror_long,
+    rcl_long,
+    rcr_long,
+    shl_long,
+    shr_long,
+    shl_long,		/* sal_byte === shl_byte  by definition */
+    sar_long,
+};
+
+#ifdef DEBUG
+
+static char *opF6_names[8] =
+  { "TEST\t", "", "NOT\t", "NEG\t", "MUL\t", "IMUL\t", "DIV\t", "IDIV\t" };
+
+#endif
+
+/****************************************************************************
+PARAMETERS:
+op1 - Instruction op code
+
+REMARKS:
+Handles illegal opcodes.
+****************************************************************************/
+void x86emuOp_illegal_op(
+    u8 op1)
+{
+    START_OF_INSTR();
+    if (M.x86.R_SP != 0) {
+	DECODE_PRINTF("ILLEGAL X86 OPCODE\n");
+	TRACE_REGS();
+	DB( printk("%04x:%04x: %02X ILLEGAL X86 OPCODE!\n",
+	    M.x86.R_CS, M.x86.R_IP-1,op1));
+	HALT_SYS();
+	}
+    else {
+	/* If we get here, it means the stack pointer is back to zero
+	 * so we are just returning from an emulator service call
+	 * so therte is no need to display an error message. We trap
+	 * the emulator with an 0xF1 opcode to finish the service
+	 * call.
+	 */
+	X86EMU_halt_sys();
+	}
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcodes 0x00, 0x08, 0x10, 0x18, 0x20, 0x28, 0x30, 0x38
+****************************************************************************/
+void x86emuOp_genop_byte_RM_R(u8 op1)
+{
+    int mod, rl, rh;
+    uint destoffset;
+    u8 *destreg, *srcreg;
+    u8 destval;
+
+    op1 = (op1 >> 3) & 0x7;
+
+    START_OF_INSTR();
+    DECODE_PRINTF(x86emu_GenOpName[op1]);
+    DECODE_PRINTF("\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if(mod<3)
+	{ destoffset = decode_rmXX_address(mod,rl);
+	DECODE_PRINTF(",");
+	destval = fetch_data_byte(destoffset);
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	destval = genop_byte_operation[op1](destval, *srcreg);
+	store_data_byte(destoffset, destval);
+	}
+    else
+	{			/* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = genop_byte_operation[op1](*destreg, *srcreg);
+	}
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcodes 0x01, 0x09, 0x11, 0x19, 0x21, 0x29, 0x31, 0x39
+****************************************************************************/
+void x86emuOp_genop_word_RM_R(u8 op1)
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    op1 = (op1 >> 3) & 0x7;
+
+    START_OF_INSTR();
+    DECODE_PRINTF(x86emu_GenOpName[op1]);
+    DECODE_PRINTF("\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+
+    if(mod<3) {
+	destoffset = decode_rmXX_address(mod,rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+	    u32 *srcreg;
+
+	    DECODE_PRINTF(",");
+	    destval = fetch_data_long(destoffset);
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    destval = genop_long_operation[op1](destval, *srcreg);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+	    u16 *srcreg;
+
+	    DECODE_PRINTF(",");
+	    destval = fetch_data_word(destoffset);
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    destval = genop_word_operation[op1](destval, *srcreg);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			/* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = genop_long_operation[op1](*destreg, *srcreg);
+	} else {
+	    u16 *destreg,*srcreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = genop_word_operation[op1](*destreg, *srcreg);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcodes 0x02, 0x0a, 0x12, 0x1a, 0x22, 0x2a, 0x32, 0x3a
+****************************************************************************/
+void x86emuOp_genop_byte_R_RM(u8 op1)
+{
+    int mod, rl, rh;
+    u8 *destreg, *srcreg;
+    uint srcoffset;
+    u8 srcval;
+
+    op1 = (op1 >> 3) & 0x7;
+
+    START_OF_INSTR();
+    DECODE_PRINTF(x86emu_GenOpName[op1]);
+    DECODE_PRINTF("\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod,rl);
+	srcval = fetch_data_byte(srcoffset);
+    } else {	 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rl);
+	srcval = *srcreg;
+    }
+    DECODE_PRINTF("\n");
+    TRACE_AND_STEP();
+    *destreg = genop_byte_operation[op1](*destreg, srcval);
+
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcodes 0x03, 0x0b, 0x13, 0x1b, 0x23, 0x2b, 0x33, 0x3b
+****************************************************************************/
+void x86emuOp_genop_word_R_RM(u8 op1)
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    u32 *destreg32, srcval;
+    u16 *destreg;
+
+    op1 = (op1 >> 3) & 0x7;
+
+    START_OF_INSTR();
+    DECODE_PRINTF(x86emu_GenOpName[op1]);
+    DECODE_PRINTF("\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod,rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    destreg32 = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcval = fetch_data_long(srcoffset);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg32 = genop_long_operation[op1](*destreg32, srcval);
+	} else {
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcval = fetch_data_word(srcoffset);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = genop_word_operation[op1](*destreg, srcval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg;
+	    destreg32 = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg32 = genop_long_operation[op1](*destreg32, *srcreg);
+	} else {
+	    u16 *srcreg;
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = genop_word_operation[op1](*destreg, *srcreg);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcodes 0x04, 0x0c, 0x14, 0x1c, 0x24, 0x2c, 0x34, 0x3c
+****************************************************************************/
+void x86emuOp_genop_byte_AL_IMM(u8 op1)
+{
+    u8 srcval;
+
+    op1 = (op1 >> 3) & 0x7;
+
+    START_OF_INSTR();
+    DECODE_PRINTF(x86emu_GenOpName[op1]);
+    DECODE_PRINTF("\tAL,");
+    srcval = fetch_byte_imm();
+    DECODE_PRINTF2("%x\n", srcval);
+    TRACE_AND_STEP();
+    M.x86.R_AL = genop_byte_operation[op1](M.x86.R_AL, srcval);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcodes 0x05, 0x0d, 0x15, 0x1d, 0x25, 0x2d, 0x35, 0x3d
+****************************************************************************/
+void x86emuOp_genop_word_AX_IMM(u8 op1)
+{
+    u32 srcval;
+
+    op1 = (op1 >> 3) & 0x7;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF(x86emu_GenOpName[op1]);
+	DECODE_PRINTF("\tEAX,");
+	srcval = fetch_long_imm();
+    } else {
+	DECODE_PRINTF(x86emu_GenOpName[op1]);
+	DECODE_PRINTF("\tAX,");
+	srcval = fetch_word_imm();
+    }
+    DECODE_PRINTF2("%x\n", srcval);
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	M.x86.R_EAX = genop_long_operation[op1](M.x86.R_EAX, srcval);
+    } else {
+	M.x86.R_AX = genop_word_operation[op1](M.x86.R_AX, (u16)srcval);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x06
+****************************************************************************/
+void x86emuOp_push_ES(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("PUSH\tES\n");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_ES);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x07
+****************************************************************************/
+void x86emuOp_pop_ES(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("POP\tES\n");
+    TRACE_AND_STEP();
+    M.x86.R_ES = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0e
+****************************************************************************/
+void x86emuOp_push_CS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("PUSH\tCS\n");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_CS);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f. Escape for two-byte opcode (286 or better)
+****************************************************************************/
+void x86emuOp_two_byte(u8 X86EMU_UNUSED(op1))
+{
+    u8 op2 = (*sys_rdb)(((u32)M.x86.R_CS << 4) + (M.x86.R_IP++));
+    INC_DECODED_INST_LEN(1);
+    (*x86emu_optab2[op2])(op2);
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x16
+****************************************************************************/
+void x86emuOp_push_SS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("PUSH\tSS\n");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_SS);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x17
+****************************************************************************/
+void x86emuOp_pop_SS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("POP\tSS\n");
+    TRACE_AND_STEP();
+    M.x86.R_SS = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x1e
+****************************************************************************/
+void x86emuOp_push_DS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("PUSH\tDS\n");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_DS);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x1f
+****************************************************************************/
+void x86emuOp_pop_DS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("POP\tDS\n");
+    TRACE_AND_STEP();
+    M.x86.R_DS = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x26
+****************************************************************************/
+void x86emuOp_segovr_ES(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("ES:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_SEGOVR_ES;
+    /*
+     * note the lack of DECODE_CLEAR_SEGOVR(r) since, here is one of 4
+     * opcode subroutines we do not want to do this.
+     */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x27
+****************************************************************************/
+void x86emuOp_daa(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("DAA\n");
+    TRACE_AND_STEP();
+    M.x86.R_AL = daa_byte(M.x86.R_AL);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x2e
+****************************************************************************/
+void x86emuOp_segovr_CS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("CS:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_SEGOVR_CS;
+    /* note no DECODE_CLEAR_SEGOVR here. */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x2f
+****************************************************************************/
+void x86emuOp_das(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("DAS\n");
+    TRACE_AND_STEP();
+    M.x86.R_AL = das_byte(M.x86.R_AL);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x36
+****************************************************************************/
+void x86emuOp_segovr_SS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("SS:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_SEGOVR_SS;
+    /* no DECODE_CLEAR_SEGOVR ! */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x37
+****************************************************************************/
+void x86emuOp_aaa(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("AAA\n");
+    TRACE_AND_STEP();
+    M.x86.R_AX = aaa_word(M.x86.R_AX);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x3e
+****************************************************************************/
+void x86emuOp_segovr_DS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("DS:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_SEGOVR_DS;
+    /* NO DECODE_CLEAR_SEGOVR! */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x3f
+****************************************************************************/
+void x86emuOp_aas(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("AAS\n");
+    TRACE_AND_STEP();
+    M.x86.R_AX = aas_word(M.x86.R_AX);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x40 - 0x47
+****************************************************************************/
+void x86emuOp_inc_register(u8 op1)
+{
+    START_OF_INSTR();
+    op1 &= 0x7;
+    DECODE_PRINTF("INC\t");
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 *reg;
+	reg = DECODE_RM_LONG_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*reg = inc_long(*reg);
+    } else {
+	u16 *reg;
+	reg = DECODE_RM_WORD_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*reg = inc_word(*reg);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x48 - 0x4F
+****************************************************************************/
+void x86emuOp_dec_register(u8 op1)
+{
+    START_OF_INSTR();
+    op1 &= 0x7;
+    DECODE_PRINTF("DEC\t");
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 *reg;
+	reg = DECODE_RM_LONG_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*reg = dec_long(*reg);
+    } else {
+	u16 *reg;
+	reg = DECODE_RM_WORD_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*reg = dec_word(*reg);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x50 - 0x57
+****************************************************************************/
+void x86emuOp_push_register(u8 op1)
+{
+    START_OF_INSTR();
+    op1 &= 0x7;
+    DECODE_PRINTF("PUSH\t");
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 *reg;
+	reg = DECODE_RM_LONG_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	push_long(*reg);
+    } else {
+	u16 *reg;
+	reg = DECODE_RM_WORD_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	push_word(*reg);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x58 - 0x5F
+****************************************************************************/
+void x86emuOp_pop_register(u8 op1)
+{
+    START_OF_INSTR();
+    op1 &= 0x7;
+    DECODE_PRINTF("POP\t");
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 *reg;
+	reg = DECODE_RM_LONG_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*reg = pop_long();
+    } else {
+	u16 *reg;
+	reg = DECODE_RM_WORD_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*reg = pop_word();
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x60
+****************************************************************************/
+void x86emuOp_push_all(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("PUSHAD\n");
+    } else {
+	DECODE_PRINTF("PUSHA\n");
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 old_sp = M.x86.R_ESP;
+
+	push_long(M.x86.R_EAX);
+	push_long(M.x86.R_ECX);
+	push_long(M.x86.R_EDX);
+	push_long(M.x86.R_EBX);
+	push_long(old_sp);
+	push_long(M.x86.R_EBP);
+	push_long(M.x86.R_ESI);
+	push_long(M.x86.R_EDI);
+    } else {
+	u16 old_sp = M.x86.R_SP;
+
+	push_word(M.x86.R_AX);
+	push_word(M.x86.R_CX);
+	push_word(M.x86.R_DX);
+	push_word(M.x86.R_BX);
+	push_word(old_sp);
+	push_word(M.x86.R_BP);
+	push_word(M.x86.R_SI);
+	push_word(M.x86.R_DI);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x61
+****************************************************************************/
+void x86emuOp_pop_all(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("POPAD\n");
+    } else {
+	DECODE_PRINTF("POPA\n");
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	M.x86.R_EDI = pop_long();
+	M.x86.R_ESI = pop_long();
+	M.x86.R_EBP = pop_long();
+	M.x86.R_ESP += 4;	       /* skip ESP */
+	M.x86.R_EBX = pop_long();
+	M.x86.R_EDX = pop_long();
+	M.x86.R_ECX = pop_long();
+	M.x86.R_EAX = pop_long();
+    } else {
+	M.x86.R_DI = pop_word();
+	M.x86.R_SI = pop_word();
+	M.x86.R_BP = pop_word();
+	M.x86.R_SP += 2;	       /* skip SP */
+	M.x86.R_BX = pop_word();
+	M.x86.R_DX = pop_word();
+	M.x86.R_CX = pop_word();
+	M.x86.R_AX = pop_word();
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/*opcode 0x62	ILLEGAL OP, calls x86emuOp_illegal_op() */
+/*opcode 0x63	ILLEGAL OP, calls x86emuOp_illegal_op() */
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x64
+****************************************************************************/
+void x86emuOp_segovr_FS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("FS:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_SEGOVR_FS;
+    /*
+     * note the lack of DECODE_CLEAR_SEGOVR(r) since, here is one of 4
+     * opcode subroutines we do not want to do this.
+     */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x65
+****************************************************************************/
+void x86emuOp_segovr_GS(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("GS:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_SEGOVR_GS;
+    /*
+     * note the lack of DECODE_CLEAR_SEGOVR(r) since, here is one of 4
+     * opcode subroutines we do not want to do this.
+     */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x66 - prefix for 32-bit register
+****************************************************************************/
+void x86emuOp_prefix_data(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("DATA:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_PREFIX_DATA;
+    /* note no DECODE_CLEAR_SEGOVR here. */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x67 - prefix for 32-bit address
+****************************************************************************/
+void x86emuOp_prefix_addr(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("ADDR:\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_PREFIX_ADDR;
+    /* note no DECODE_CLEAR_SEGOVR here. */
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x68
+****************************************************************************/
+void x86emuOp_push_word_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u32 imm;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	imm = fetch_long_imm();
+    } else {
+	imm = fetch_word_imm();
+    }
+    DECODE_PRINTF2("PUSH\t%x\n", imm);
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	push_long(imm);
+    } else {
+	push_word((u16)imm);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x69
+****************************************************************************/
+void x86emuOp_imul_word_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("IMUL\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+	    u32 res_lo,res_hi;
+	    s32 imm;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcval = fetch_data_long(srcoffset);
+	    imm = fetch_long_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    imul_long_direct(&res_lo,&res_hi,(s32)srcval,(s32)imm);
+	    if ((((res_lo & 0x80000000) == 0) && (res_hi == 0x00000000)) ||
+		(((res_lo & 0x80000000) != 0) && (res_hi == 0xFFFFFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u32)res_lo;
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+	    u32 res;
+	    s16 imm;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcval = fetch_data_word(srcoffset);
+	    imm = fetch_word_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    res = (s16)srcval * (s16)imm;
+	    if ((((res & 0x8000) == 0) && ((res >> 16) == 0x0000)) ||
+		(((res & 0x8000) != 0) && ((res >> 16) == 0xFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u16)res;
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+	    u32 res_lo,res_hi;
+	    s32 imm;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    imm = fetch_long_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    imul_long_direct(&res_lo,&res_hi,(s32)*srcreg,(s32)imm);
+	    if ((((res_lo & 0x80000000) == 0) && (res_hi == 0x00000000)) ||
+		(((res_lo & 0x80000000) != 0) && (res_hi == 0xFFFFFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u32)res_lo;
+	} else {
+	    u16 *destreg,*srcreg;
+	    u32 res;
+	    s16 imm;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    imm = fetch_word_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    res = (s16)*srcreg * (s16)imm;
+	    if ((((res & 0x8000) == 0) && ((res >> 16) == 0x0000)) ||
+		(((res & 0x8000) != 0) && ((res >> 16) == 0xFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u16)res;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x6a
+****************************************************************************/
+void x86emuOp_push_byte_IMM(u8 X86EMU_UNUSED(op1))
+{
+    s16 imm;
+
+    START_OF_INSTR();
+    imm = (s8)fetch_byte_imm();
+    DECODE_PRINTF2("PUSH\t%d\n", imm);
+    TRACE_AND_STEP();
+    push_word(imm);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x6b
+****************************************************************************/
+void x86emuOp_imul_byte_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    s8	imm;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("IMUL\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+	    u32 res_lo,res_hi;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcval = fetch_data_long(srcoffset);
+	    imm = fetch_byte_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    imul_long_direct(&res_lo,&res_hi,(s32)srcval,(s32)imm);
+	    if ((((res_lo & 0x80000000) == 0) && (res_hi == 0x00000000)) ||
+		(((res_lo & 0x80000000) != 0) && (res_hi == 0xFFFFFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u32)res_lo;
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+	    u32 res;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcval = fetch_data_word(srcoffset);
+	    imm = fetch_byte_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    res = (s16)srcval * (s16)imm;
+	    if ((((res & 0x8000) == 0) && ((res >> 16) == 0x0000)) ||
+		(((res & 0x8000) != 0) && ((res >> 16) == 0xFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u16)res;
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+	    u32 res_lo,res_hi;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    imm = fetch_byte_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    imul_long_direct(&res_lo,&res_hi,(s32)*srcreg,(s32)imm);
+	    if ((((res_lo & 0x80000000) == 0) && (res_hi == 0x00000000)) ||
+		(((res_lo & 0x80000000) != 0) && (res_hi == 0xFFFFFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u32)res_lo;
+	} else {
+	    u16 *destreg,*srcreg;
+	    u32 res;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    imm = fetch_byte_imm();
+	    DECODE_PRINTF2(",%d\n", (s32)imm);
+	    TRACE_AND_STEP();
+	    res = (s16)*srcreg * (s16)imm;
+	    if ((((res & 0x8000) == 0) && ((res >> 16) == 0x0000)) ||
+		(((res & 0x8000) != 0) && ((res >> 16) == 0xFFFF))) {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    } else {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    }
+	    *destreg = (u16)res;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x6c
+****************************************************************************/
+void x86emuOp_ins_byte(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("INSB\n");
+    ins(1);
+    TRACE_AND_STEP();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x6d
+****************************************************************************/
+void x86emuOp_ins_word(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("INSD\n");
+	ins(4);
+    } else {
+	DECODE_PRINTF("INSW\n");
+	ins(2);
+    }
+    TRACE_AND_STEP();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x6e
+****************************************************************************/
+void x86emuOp_outs_byte(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("OUTSB\n");
+    outs(1);
+    TRACE_AND_STEP();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x6f
+****************************************************************************/
+void x86emuOp_outs_word(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("OUTSD\n");
+	outs(4);
+    } else {
+	DECODE_PRINTF("OUTSW\n");
+	outs(2);
+    }
+    TRACE_AND_STEP();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x70 - 0x7F
+****************************************************************************/
+int x86emu_check_jump_condition(u8 op);
+
+void x86emuOp_jump_near_cond(u8 op1)
+{
+    s8 offset;
+    u16 target;
+    int cond;
+
+    /* jump to byte offset if overflow flag is set */
+    START_OF_INSTR();
+    cond = x86emu_check_jump_condition(op1 & 0xF);
+    offset = (s8)fetch_byte_imm();
+    target = (u16)(M.x86.R_IP + (s16)offset);
+    DECODE_PRINTF2("%x\n", target);
+    TRACE_AND_STEP();
+    if (cond)
+	M.x86.R_IP = target;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x80
+****************************************************************************/
+void x86emuOp_opc80_byte_RM_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 imm;
+    u8 destval;
+
+    /*
+     * Weirdo special case instruction format.	Part of the opcode
+     * held below in "RH".  Doubly nested case would result, except
+     * that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ADD\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("OR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("ADC\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("SBB\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("AND\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SUB\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("XOR\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("CMP\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	destval = fetch_data_byte(destoffset);
+	imm = fetch_byte_imm();
+	DECODE_PRINTF2("%x\n", imm);
+	TRACE_AND_STEP();
+	destval = (*genop_byte_operation[rh]) (destval, imm);
+	if (rh != 7)
+	    store_data_byte(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",");
+	imm = fetch_byte_imm();
+	DECODE_PRINTF2("%x\n", imm);
+	TRACE_AND_STEP();
+	destval = (*genop_byte_operation[rh]) (*destreg, imm);
+	if (rh != 7)
+	    *destreg = destval;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x81
+****************************************************************************/
+void x86emuOp_opc81_word_RM_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    /*
+     * Weirdo special case instruction format.	Part of the opcode
+     * held below in "RH".  Doubly nested case would result, except
+     * that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ADD\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("OR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("ADC\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("SBB\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("AND\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SUB\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("XOR\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("CMP\t");
+	    break;
+	}
+    }
+#endif
+    /*
+     * Know operation, decode the mod byte to find the addressing
+     * mode.
+     */
+    if (mod < 3) {
+	DECODE_PRINTF("DWORD PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval,imm;
+
+	    DECODE_PRINTF(",");
+	    destval = fetch_data_long(destoffset);
+	    imm = fetch_long_imm();
+	    DECODE_PRINTF2("%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_long_operation[rh]) (destval, imm);
+	    if (rh != 7)
+		store_data_long(destoffset, destval);
+	} else {
+	    u16 destval,imm;
+
+	    DECODE_PRINTF(",");
+	    destval = fetch_data_word(destoffset);
+	    imm = fetch_word_imm();
+	    DECODE_PRINTF2("%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_word_operation[rh]) (destval, imm);
+	    if (rh != 7)
+		store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 destval,imm;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    imm = fetch_long_imm();
+	    DECODE_PRINTF2("%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_long_operation[rh]) (*destreg, imm);
+	    if (rh != 7)
+		*destreg = destval;
+	} else {
+	    u16 *destreg;
+	    u16 destval,imm;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    imm = fetch_word_imm();
+	    DECODE_PRINTF2("%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_word_operation[rh]) (*destreg, imm);
+	    if (rh != 7)
+		*destreg = destval;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x82
+****************************************************************************/
+void x86emuOp_opc82_byte_RM_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 imm;
+    u8 destval;
+
+    /*
+     * Weirdo special case instruction format.	Part of the opcode
+     * held below in "RH".  Doubly nested case would result, except
+     * that the decoded instruction Similar to opcode 81, except that
+     * the immediate byte is sign extended to a word length.
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ADD\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("OR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("ADC\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("SBB\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("AND\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SUB\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("XOR\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("CMP\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	destval = fetch_data_byte(destoffset);
+	imm = fetch_byte_imm();
+	DECODE_PRINTF2(",%x\n", imm);
+	TRACE_AND_STEP();
+	destval = (*genop_byte_operation[rh]) (destval, imm);
+	if (rh != 7)
+	    store_data_byte(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	imm = fetch_byte_imm();
+	DECODE_PRINTF2(",%x\n", imm);
+	TRACE_AND_STEP();
+	destval = (*genop_byte_operation[rh]) (*destreg, imm);
+	if (rh != 7)
+	    *destreg = destval;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x83
+****************************************************************************/
+void x86emuOp_opc83_word_RM_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    /*
+     * Weirdo special case instruction format.	Part of the opcode
+     * held below in "RH".  Doubly nested case would result, except
+     * that the decoded instruction Similar to opcode 81, except that
+     * the immediate byte is sign extended to a word length.
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+       switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ADD\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("OR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("ADC\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("SBB\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("AND\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SUB\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("XOR\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("CMP\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	DECODE_PRINTF("DWORD PTR ");
+	destoffset = decode_rmXX_address(mod,rl);
+
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval,imm;
+
+	    destval = fetch_data_long(destoffset);
+	    imm = (s8) fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_long_operation[rh]) (destval, imm);
+	    if (rh != 7)
+		store_data_long(destoffset, destval);
+	} else {
+	    u16 destval,imm;
+
+	    destval = fetch_data_word(destoffset);
+	    imm = (s8) fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_word_operation[rh]) (destval, imm);
+	    if (rh != 7)
+		store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 destval,imm;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    imm = (s8) fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_long_operation[rh]) (*destreg, imm);
+	    if (rh != 7)
+		*destreg = destval;
+	} else {
+	    u16 *destreg;
+	    u16 destval,imm;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    imm = (s8) fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    destval = (*genop_word_operation[rh]) (*destreg, imm);
+	    if (rh != 7)
+		*destreg = destval;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x84
+****************************************************************************/
+void x86emuOp_test_byte_RM_R(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg, *srcreg;
+    uint destoffset;
+    u8 destval;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("TEST\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	destval = fetch_data_byte(destoffset);
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	test_byte(destval, *srcreg);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	test_byte(*destreg, *srcreg);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x85
+****************************************************************************/
+void x86emuOp_test_word_RM_R(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("TEST\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+	    u32 *srcreg;
+
+	    DECODE_PRINTF(",");
+	    destval = fetch_data_long(destoffset);
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    test_long(destval, *srcreg);
+	} else {
+	    u16 destval;
+	    u16 *srcreg;
+
+	    DECODE_PRINTF(",");
+	    destval = fetch_data_word(destoffset);
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    test_word(destval, *srcreg);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    test_long(*destreg, *srcreg);
+	} else {
+	    u16 *destreg,*srcreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    test_word(*destreg, *srcreg);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x86
+****************************************************************************/
+void x86emuOp_xchg_byte_RM_R(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg, *srcreg;
+    uint destoffset;
+    u8 destval;
+    u8 tmp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("XCHG\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	destval = fetch_data_byte(destoffset);
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	tmp = *srcreg;
+	*srcreg = destval;
+	destval = tmp;
+	store_data_byte(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	tmp = *srcreg;
+	*srcreg = *destreg;
+	*destreg = tmp;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x87
+****************************************************************************/
+void x86emuOp_xchg_word_RM_R(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("XCHG\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg;
+	    u32 destval,tmp;
+
+	    destval = fetch_data_long(destoffset);
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    tmp = *srcreg;
+	    *srcreg = destval;
+	    destval = tmp;
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 *srcreg;
+	    u16 destval,tmp;
+
+	    destval = fetch_data_word(destoffset);
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    tmp = *srcreg;
+	    *srcreg = destval;
+	    destval = tmp;
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+	    u32 tmp;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    tmp = *srcreg;
+	    *srcreg = *destreg;
+	    *destreg = tmp;
+	} else {
+	    u16 *destreg,*srcreg;
+	    u16 tmp;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    tmp = *srcreg;
+	    *srcreg = *destreg;
+	    *destreg = tmp;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x88
+****************************************************************************/
+void x86emuOp_mov_byte_RM_R(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg, *srcreg;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	store_data_byte(destoffset, *srcreg);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = *srcreg;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x89
+****************************************************************************/
+void x86emuOp_mov_word_RM_R(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg;
+
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    store_data_long(destoffset, *srcreg);
+	} else {
+	    u16 *srcreg;
+
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    store_data_word(destoffset, *srcreg);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = *srcreg;
+	} else {
+	    u16 *destreg,*srcreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = *srcreg;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x8a
+****************************************************************************/
+void x86emuOp_mov_byte_R_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg, *srcreg;
+    uint srcoffset;
+    u8 srcval;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	srcval = fetch_data_byte(srcoffset);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = srcval;
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = *srcreg;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x8b
+****************************************************************************/
+void x86emuOp_mov_word_R_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = fetch_data_long(srcoffset);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = srcval;
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = fetch_data_word(srcoffset);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = srcval;
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg, *srcreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = *srcreg;
+	} else {
+	    u16 *destreg, *srcreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = *srcreg;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x8c
+****************************************************************************/
+void x86emuOp_mov_word_RM_SR(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u16 *destreg, *srcreg;
+    uint destoffset;
+    u16 destval;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	srcreg = decode_rm_seg_register(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	destval = *srcreg;
+	store_data_word(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_WORD_REGISTER(rl);
+	DECODE_PRINTF(",");
+	srcreg = decode_rm_seg_register(rh);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = *srcreg;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x8d
+****************************************************************************/
+void x86emuOp_lea_word_R_M(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u16 *srcreg;
+    uint destoffset;
+
+/*
+ * TODO: Need to handle address size prefix!
+ *
+ * lea	eax,[eax+ebx*2] ??
+ */
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LEA\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcreg = DECODE_RM_WORD_REGISTER(rh);
+	DECODE_PRINTF(",");
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*srcreg = (u16)destoffset;
+	}
+    /* } else { undefined.  Do nothing. } */
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x8e
+****************************************************************************/
+void x86emuOp_mov_word_SR_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u16 *destreg, *srcreg;
+    uint srcoffset;
+    u16 srcval;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destreg = decode_rm_seg_register(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	srcval = fetch_data_word(srcoffset);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = srcval;
+    } else {			 /* register to register */
+	destreg = decode_rm_seg_register(rh);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_WORD_REGISTER(rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = *srcreg;
+    }
+    /*
+     * Clean up, and reset all the R_xSP pointers to the correct
+     * locations.  This is about 3x too much overhead (doing all the
+     * segreg ptrs when only one is needed, but this instruction
+     * *cannot* be that common, and this isn't too much work anyway.
+     */
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x8f
+****************************************************************************/
+void x86emuOp_pop_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("POP\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (rh != 0) {
+	DECODE_PRINTF("ILLEGAL DECODE OF OPCODE 8F\n");
+	HALT_SYS();
+    }
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    destval = pop_long();
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    destval = pop_word();
+	    store_data_word(destoffset, destval);
+	}
+    } else {			/* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = pop_long();
+	} else {
+	    u16 *destreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = pop_word();
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x90
+****************************************************************************/
+void x86emuOp_nop(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("NOP\n");
+    TRACE_AND_STEP();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x91-0x97
+****************************************************************************/
+void x86emuOp_xchg_word_AX_register(u8 X86EMU_UNUSED(op1))
+{
+    u32 tmp;
+
+    op1 &= 0x7;
+
+    START_OF_INSTR();
+
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 *reg32;
+	DECODE_PRINTF("XCHG\tEAX,");
+	reg32 = DECODE_RM_LONG_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	tmp = M.x86.R_EAX;
+	M.x86.R_EAX = *reg32;
+	*reg32 = tmp;
+    } else {
+	u16 *reg16;
+	DECODE_PRINTF("XCHG\tAX,");
+	reg16 = DECODE_RM_WORD_REGISTER(op1);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	tmp = M.x86.R_AX;
+	M.x86.R_EAX = *reg16;
+	*reg16 = (u16)tmp;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x98
+****************************************************************************/
+void x86emuOp_cbw(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("CWDE\n");
+    } else {
+	DECODE_PRINTF("CBW\n");
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	if (M.x86.R_AX & 0x8000) {
+	    M.x86.R_EAX |= 0xffff0000;
+	} else {
+	    M.x86.R_EAX &= 0x0000ffff;
+	}
+    } else {
+	if (M.x86.R_AL & 0x80) {
+	    M.x86.R_AH = 0xff;
+	} else {
+	    M.x86.R_AH = 0x0;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x99
+****************************************************************************/
+void x86emuOp_cwd(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("CDQ\n");
+    } else {
+	DECODE_PRINTF("CWD\n");
+    }
+    DECODE_PRINTF("CWD\n");
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	if (M.x86.R_EAX & 0x80000000) {
+	    M.x86.R_EDX = 0xffffffff;
+	} else {
+	    M.x86.R_EDX = 0x0;
+	}
+    } else {
+	if (M.x86.R_AX & 0x8000) {
+	    M.x86.R_DX = 0xffff;
+	} else {
+	    M.x86.R_DX = 0x0;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x9a
+****************************************************************************/
+void x86emuOp_call_far_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 farseg, faroff;
+
+    START_OF_INSTR();
+	DECODE_PRINTF("CALL\t");
+	faroff = fetch_word_imm();
+	farseg = fetch_word_imm();
+	DECODE_PRINTF2("%04x:", farseg);
+	DECODE_PRINTF2("%04x\n", faroff);
+	CALL_TRACE(M.x86.saved_cs, M.x86.saved_ip, farseg, faroff, "FAR ");
+
+    /* XXX
+     *
+     * Hooked interrupt vectors calling into our "BIOS" will cause
+     * problems unless all intersegment stuff is checked for BIOS
+     * access.	Check needed here.  For moment, let it alone.
+     */
+    TRACE_AND_STEP();
+    push_word(M.x86.R_CS);
+    M.x86.R_CS = farseg;
+    push_word(M.x86.R_IP);
+    M.x86.R_IP = faroff;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x9b
+****************************************************************************/
+void x86emuOp_wait(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("WAIT");
+    TRACE_AND_STEP();
+    /* NADA.  */
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x9c
+****************************************************************************/
+void x86emuOp_pushf_word(u8 X86EMU_UNUSED(op1))
+{
+    u32 flags;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("PUSHFD\n");
+    } else {
+	DECODE_PRINTF("PUSHF\n");
+    }
+    TRACE_AND_STEP();
+
+    /* clear out *all* bits not representing flags, and turn on real bits */
+    flags = (M.x86.R_EFLG & F_MSK) | F_ALWAYS_ON;
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	push_long(flags);
+    } else {
+	push_word((u16)flags);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x9d
+****************************************************************************/
+void x86emuOp_popf_word(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("POPFD\n");
+    } else {
+	DECODE_PRINTF("POPF\n");
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	M.x86.R_EFLG = pop_long();
+    } else {
+	M.x86.R_FLG = pop_word();
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x9e
+****************************************************************************/
+void x86emuOp_sahf(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("SAHF\n");
+    TRACE_AND_STEP();
+    /* clear the lower bits of the flag register */
+    M.x86.R_FLG &= 0xffffff00;
+    /* or in the AH register into the flags register */
+    M.x86.R_FLG |= M.x86.R_AH;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x9f
+****************************************************************************/
+void x86emuOp_lahf(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("LAHF\n");
+    TRACE_AND_STEP();
+	M.x86.R_AH = (u8)(M.x86.R_FLG & 0xff);
+    /*undocumented TC++ behavior??? Nope.  It's documented, but
+       you have too look real hard to notice it. */
+    M.x86.R_AH |= 0x2;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa0
+****************************************************************************/
+void x86emuOp_mov_AL_M_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 offset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\tAL,");
+    offset = fetch_word_imm();
+    DECODE_PRINTF2("[%04x]\n", offset);
+    TRACE_AND_STEP();
+    M.x86.R_AL = fetch_data_byte(offset);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa1
+****************************************************************************/
+void x86emuOp_mov_AX_M_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 offset;
+
+    START_OF_INSTR();
+    offset = fetch_word_imm();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF2("MOV\tEAX,[%04x]\n", offset);
+    } else {
+	DECODE_PRINTF2("MOV\tAX,[%04x]\n", offset);
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	M.x86.R_EAX = fetch_data_long(offset);
+    } else {
+	M.x86.R_AX = fetch_data_word(offset);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa2
+****************************************************************************/
+void x86emuOp_mov_M_AL_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 offset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    offset = fetch_word_imm();
+    DECODE_PRINTF2("[%04x],AL\n", offset);
+    TRACE_AND_STEP();
+    store_data_byte(offset, M.x86.R_AL);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa3
+****************************************************************************/
+void x86emuOp_mov_M_AX_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 offset;
+
+    START_OF_INSTR();
+    offset = fetch_word_imm();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF2("MOV\t[%04x],EAX\n", offset);
+    } else {
+	DECODE_PRINTF2("MOV\t[%04x],AX\n", offset);
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	store_data_long(offset, M.x86.R_EAX);
+    } else {
+	store_data_word(offset, M.x86.R_AX);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa4
+****************************************************************************/
+void x86emuOp_movs_byte(u8 X86EMU_UNUSED(op1))
+{
+    u8	val;
+    u32 count;
+    int inc;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOVS\tBYTE\n");
+    if (ACCESS_FLAG(F_DF))   /* down */
+	inc = -1;
+    else
+	inc = 1;
+    TRACE_AND_STEP();
+    count = 1;
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* move them until CX is ZERO. */
+	count = M.x86.R_CX;
+	M.x86.R_CX = 0;
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    }
+    while (count--) {
+	val = fetch_data_byte(M.x86.R_SI);
+	store_data_byte_abs(M.x86.R_ES, M.x86.R_DI, val);
+	M.x86.R_SI += inc;
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa5
+****************************************************************************/
+void x86emuOp_movs_word(u8 X86EMU_UNUSED(op1))
+{
+    u32 val;
+    int inc;
+    u32 count;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("MOVS\tDWORD\n");
+	if (ACCESS_FLAG(F_DF))	    /* down */
+	    inc = -4;
+	else
+	    inc = 4;
+    } else {
+	DECODE_PRINTF("MOVS\tWORD\n");
+	if (ACCESS_FLAG(F_DF))	    /* down */
+	    inc = -2;
+	else
+	    inc = 2;
+    }
+    TRACE_AND_STEP();
+    count = 1;
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* move them until CX is ZERO. */
+	count = M.x86.R_CX;
+	M.x86.R_CX = 0;
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    }
+    while (count--) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    val = fetch_data_long(M.x86.R_SI);
+	    store_data_long_abs(M.x86.R_ES, M.x86.R_DI, val);
+	} else {
+	    val = fetch_data_word(M.x86.R_SI);
+	    store_data_word_abs(M.x86.R_ES, M.x86.R_DI, (u16)val);
+	}
+	M.x86.R_SI += inc;
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa6
+****************************************************************************/
+void x86emuOp_cmps_byte(u8 X86EMU_UNUSED(op1))
+{
+    s8 val1, val2;
+    int inc;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("CMPS\tBYTE\n");
+    TRACE_AND_STEP();
+    if (ACCESS_FLAG(F_DF))   /* down */
+	inc = -1;
+    else
+	inc = 1;
+
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* REPE	 */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    val1 = fetch_data_byte(M.x86.R_SI);
+	    val2 = fetch_data_byte_abs(M.x86.R_ES, M.x86.R_DI);
+		     cmp_byte(val1, val2);
+	    M.x86.R_CX -= 1;
+	    M.x86.R_SI += inc;
+	    M.x86.R_DI += inc;
+	    if ( (M.x86.mode & SYSMODE_PREFIX_REPE) && (ACCESS_FLAG(F_ZF) == 0) ) break;
+	    if ( (M.x86.mode & SYSMODE_PREFIX_REPNE) && ACCESS_FLAG(F_ZF) ) break;
+	}
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    } else {
+	val1 = fetch_data_byte(M.x86.R_SI);
+	val2 = fetch_data_byte_abs(M.x86.R_ES, M.x86.R_DI);
+	cmp_byte(val1, val2);
+	M.x86.R_SI += inc;
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa7
+****************************************************************************/
+void x86emuOp_cmps_word(u8 X86EMU_UNUSED(op1))
+{
+    u32 val1,val2;
+    int inc;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("CMPS\tDWORD\n");
+	inc = 4;
+    } else {
+	DECODE_PRINTF("CMPS\tWORD\n");
+	inc = 2;
+    }
+    if (ACCESS_FLAG(F_DF))   /* down */
+	inc = -inc;
+
+    TRACE_AND_STEP();
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* REPE	 */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		val1 = fetch_data_long(M.x86.R_SI);
+		val2 = fetch_data_long_abs(M.x86.R_ES, M.x86.R_DI);
+		cmp_long(val1, val2);
+	    } else {
+		val1 = fetch_data_word(M.x86.R_SI);
+		val2 = fetch_data_word_abs(M.x86.R_ES, M.x86.R_DI);
+		cmp_word((u16)val1, (u16)val2);
+	    }
+	    M.x86.R_CX -= 1;
+	    M.x86.R_SI += inc;
+	    M.x86.R_DI += inc;
+	    if ( (M.x86.mode & SYSMODE_PREFIX_REPE) && ACCESS_FLAG(F_ZF) == 0 ) break;
+	    if ( (M.x86.mode & SYSMODE_PREFIX_REPNE) && ACCESS_FLAG(F_ZF) ) break;
+	}
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    } else {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    val1 = fetch_data_long(M.x86.R_SI);
+	    val2 = fetch_data_long_abs(M.x86.R_ES, M.x86.R_DI);
+	    cmp_long(val1, val2);
+	} else {
+	    val1 = fetch_data_word(M.x86.R_SI);
+	    val2 = fetch_data_word_abs(M.x86.R_ES, M.x86.R_DI);
+	    cmp_word((u16)val1, (u16)val2);
+	}
+	M.x86.R_SI += inc;
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa8
+****************************************************************************/
+void x86emuOp_test_AL_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int imm;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("TEST\tAL,");
+    imm = fetch_byte_imm();
+    DECODE_PRINTF2("%04x\n", imm);
+    TRACE_AND_STEP();
+	test_byte(M.x86.R_AL, (u8)imm);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xa9
+****************************************************************************/
+void x86emuOp_test_AX_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u32 srcval;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("TEST\tEAX,");
+	srcval = fetch_long_imm();
+    } else {
+	DECODE_PRINTF("TEST\tAX,");
+	srcval = fetch_word_imm();
+    }
+    DECODE_PRINTF2("%x\n", srcval);
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	test_long(M.x86.R_EAX, srcval);
+    } else {
+	test_word(M.x86.R_AX, (u16)srcval);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xaa
+****************************************************************************/
+void x86emuOp_stos_byte(u8 X86EMU_UNUSED(op1))
+{
+    int inc;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("STOS\tBYTE\n");
+    if (ACCESS_FLAG(F_DF))   /* down */
+	inc = -1;
+    else
+	inc = 1;
+    TRACE_AND_STEP();
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    store_data_byte_abs(M.x86.R_ES, M.x86.R_DI, M.x86.R_AL);
+	    M.x86.R_CX -= 1;
+	    M.x86.R_DI += inc;
+	}
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    } else {
+	store_data_byte_abs(M.x86.R_ES, M.x86.R_DI, M.x86.R_AL);
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xab
+****************************************************************************/
+void x86emuOp_stos_word(u8 X86EMU_UNUSED(op1))
+{
+    int inc;
+    u32 count;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("STOS\tDWORD\n");
+	if (ACCESS_FLAG(F_DF))	 /* down */
+	    inc = -4;
+	else
+	    inc = 4;
+    } else {
+	DECODE_PRINTF("STOS\tWORD\n");
+	if (ACCESS_FLAG(F_DF))	 /* down */
+	    inc = -2;
+	else
+	    inc = 2;
+    }
+    TRACE_AND_STEP();
+    count = 1;
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* move them until CX is ZERO. */
+	count = M.x86.R_CX;
+	M.x86.R_CX = 0;
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    }
+    while (count--) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    store_data_long_abs(M.x86.R_ES, M.x86.R_DI, M.x86.R_EAX);
+	} else {
+	    store_data_word_abs(M.x86.R_ES, M.x86.R_DI, M.x86.R_AX);
+	}
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xac
+****************************************************************************/
+void x86emuOp_lods_byte(u8 X86EMU_UNUSED(op1))
+{
+    int inc;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LODS\tBYTE\n");
+    TRACE_AND_STEP();
+    if (ACCESS_FLAG(F_DF))   /* down */
+	inc = -1;
+    else
+	inc = 1;
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    M.x86.R_AL = fetch_data_byte(M.x86.R_SI);
+	    M.x86.R_CX -= 1;
+	    M.x86.R_SI += inc;
+	}
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    } else {
+	M.x86.R_AL = fetch_data_byte(M.x86.R_SI);
+	M.x86.R_SI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xad
+****************************************************************************/
+void x86emuOp_lods_word(u8 X86EMU_UNUSED(op1))
+{
+    int inc;
+    u32 count;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("LODS\tDWORD\n");
+	if (ACCESS_FLAG(F_DF))	 /* down */
+	    inc = -4;
+	else
+	    inc = 4;
+    } else {
+	DECODE_PRINTF("LODS\tWORD\n");
+	if (ACCESS_FLAG(F_DF))	 /* down */
+	    inc = -2;
+	else
+	    inc = 2;
+    }
+    TRACE_AND_STEP();
+    count = 1;
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* move them until CX is ZERO. */
+	count = M.x86.R_CX;
+	M.x86.R_CX = 0;
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    }
+    while (count--) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    M.x86.R_EAX = fetch_data_long(M.x86.R_SI);
+	} else {
+	    M.x86.R_AX = fetch_data_word(M.x86.R_SI);
+	}
+	M.x86.R_SI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xae
+****************************************************************************/
+void x86emuOp_scas_byte(u8 X86EMU_UNUSED(op1))
+{
+    s8 val2;
+    int inc;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("SCAS\tBYTE\n");
+    TRACE_AND_STEP();
+    if (ACCESS_FLAG(F_DF))   /* down */
+	inc = -1;
+    else
+	inc = 1;
+    if (M.x86.mode & SYSMODE_PREFIX_REPE) {
+	/* REPE	 */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    val2 = fetch_data_byte_abs(M.x86.R_ES, M.x86.R_DI);
+	    cmp_byte(M.x86.R_AL, val2);
+	    M.x86.R_CX -= 1;
+	    M.x86.R_DI += inc;
+	    if (ACCESS_FLAG(F_ZF) == 0)
+		break;
+	}
+	M.x86.mode &= ~SYSMODE_PREFIX_REPE;
+    } else if (M.x86.mode & SYSMODE_PREFIX_REPNE) {
+	/* REPNE  */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    val2 = fetch_data_byte_abs(M.x86.R_ES, M.x86.R_DI);
+	    cmp_byte(M.x86.R_AL, val2);
+	    M.x86.R_CX -= 1;
+	    M.x86.R_DI += inc;
+	    if (ACCESS_FLAG(F_ZF))
+		break;		/* zero flag set means equal */
+	}
+	M.x86.mode &= ~SYSMODE_PREFIX_REPNE;
+    } else {
+	val2 = fetch_data_byte_abs(M.x86.R_ES, M.x86.R_DI);
+	cmp_byte(M.x86.R_AL, val2);
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xaf
+****************************************************************************/
+void x86emuOp_scas_word(u8 X86EMU_UNUSED(op1))
+{
+    int inc;
+    u32 val;
+
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("SCAS\tDWORD\n");
+	if (ACCESS_FLAG(F_DF))	 /* down */
+	    inc = -4;
+	else
+	    inc = 4;
+    } else {
+	DECODE_PRINTF("SCAS\tWORD\n");
+	if (ACCESS_FLAG(F_DF))	 /* down */
+	    inc = -2;
+	else
+	    inc = 2;
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_REPE) {
+	/* REPE	 */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		val = fetch_data_long_abs(M.x86.R_ES, M.x86.R_DI);
+		cmp_long(M.x86.R_EAX, val);
+	    } else {
+		val = fetch_data_word_abs(M.x86.R_ES, M.x86.R_DI);
+		cmp_word(M.x86.R_AX, (u16)val);
+	    }
+	    M.x86.R_CX -= 1;
+	    M.x86.R_DI += inc;
+	    if (ACCESS_FLAG(F_ZF) == 0)
+		break;
+	}
+	M.x86.mode &= ~SYSMODE_PREFIX_REPE;
+    } else if (M.x86.mode & SYSMODE_PREFIX_REPNE) {
+	/* REPNE  */
+	/* move them until CX is ZERO. */
+	while (M.x86.R_CX != 0) {
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		val = fetch_data_long_abs(M.x86.R_ES, M.x86.R_DI);
+		cmp_long(M.x86.R_EAX, val);
+	    } else {
+		val = fetch_data_word_abs(M.x86.R_ES, M.x86.R_DI);
+		cmp_word(M.x86.R_AX, (u16)val);
+	    }
+	    M.x86.R_CX -= 1;
+	    M.x86.R_DI += inc;
+	    if (ACCESS_FLAG(F_ZF))
+		break;		/* zero flag set means equal */
+	}
+	M.x86.mode &= ~SYSMODE_PREFIX_REPNE;
+    } else {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    val = fetch_data_long_abs(M.x86.R_ES, M.x86.R_DI);
+	    cmp_long(M.x86.R_EAX, val);
+	} else {
+	    val = fetch_data_word_abs(M.x86.R_ES, M.x86.R_DI);
+	    cmp_word(M.x86.R_AX, (u16)val);
+	}
+	M.x86.R_DI += inc;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xb0 - 0xb7
+****************************************************************************/
+void x86emuOp_mov_byte_register_IMM(u8 op1)
+{
+    u8 imm, *ptr;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    ptr = DECODE_RM_BYTE_REGISTER(op1 & 0x7);
+    DECODE_PRINTF(",");
+    imm = fetch_byte_imm();
+    DECODE_PRINTF2("%x\n", imm);
+    TRACE_AND_STEP();
+    *ptr = imm;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xb8 - 0xbf
+****************************************************************************/
+void x86emuOp_mov_word_register_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u32 srcval;
+
+    op1 &= 0x7;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	u32 *reg32;
+	reg32 = DECODE_RM_LONG_REGISTER(op1);
+	srcval = fetch_long_imm();
+	DECODE_PRINTF2(",%x\n", srcval);
+	TRACE_AND_STEP();
+	*reg32 = srcval;
+    } else {
+	u16 *reg16;
+	reg16 = DECODE_RM_WORD_REGISTER(op1);
+	srcval = fetch_word_imm();
+	DECODE_PRINTF2(",%x\n", srcval);
+	TRACE_AND_STEP();
+	*reg16 = (u16)srcval;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc0
+****************************************************************************/
+void x86emuOp_opcC0_byte_RM_MEM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 destval;
+    u8 amt;
+
+    /*
+     * Yet another weirdo special case instruction format.  Part of
+     * the opcode held below in "RH".  Doubly nested case would
+     * result, except that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ROL\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("ROR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("RCL\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("RCR\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("SHL\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SHR\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("SAL\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("SAR\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	amt = fetch_byte_imm();
+	DECODE_PRINTF2(",%x\n", amt);
+	destval = fetch_data_byte(destoffset);
+	TRACE_AND_STEP();
+	destval = (*opcD0_byte_operation[rh]) (destval, amt);
+	store_data_byte(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	amt = fetch_byte_imm();
+	DECODE_PRINTF2(",%x\n", amt);
+	TRACE_AND_STEP();
+	destval = (*opcD0_byte_operation[rh]) (*destreg, amt);
+	*destreg = destval;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc1
+****************************************************************************/
+void x86emuOp_opcC1_word_RM_MEM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+    u8 amt;
+
+    /*
+     * Yet another weirdo special case instruction format.  Part of
+     * the opcode held below in "RH".  Doubly nested case would
+     * result, except that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ROL\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("ROR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("RCL\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("RCR\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("SHL\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SHR\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("SAL\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("SAR\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+
+	    DECODE_PRINTF("DWORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    amt = fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", amt);
+	    destval = fetch_data_long(destoffset);
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_long_operation[rh]) (destval, amt);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+
+	    DECODE_PRINTF("WORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    amt = fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", amt);
+	    destval = fetch_data_word(destoffset);
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_word_operation[rh]) (destval, amt);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    amt = fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", amt);
+	    TRACE_AND_STEP();
+	    *destreg = (*opcD1_long_operation[rh]) (*destreg, amt);
+	} else {
+	    u16 *destreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    amt = fetch_byte_imm();
+	    DECODE_PRINTF2(",%x\n", amt);
+	    TRACE_AND_STEP();
+	    *destreg = (*opcD1_word_operation[rh]) (*destreg, amt);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc2
+****************************************************************************/
+void x86emuOp_ret_near_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 imm;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("RET\t");
+    imm = fetch_word_imm();
+    DECODE_PRINTF2("%x\n", imm);
+	RETURN_TRACE("RET",M.x86.saved_cs,M.x86.saved_ip);
+	TRACE_AND_STEP();
+    M.x86.R_IP = pop_word();
+    M.x86.R_SP += imm;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc3
+****************************************************************************/
+void x86emuOp_ret_near(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("RET\n");
+	RETURN_TRACE("RET",M.x86.saved_cs,M.x86.saved_ip);
+	TRACE_AND_STEP();
+    M.x86.R_IP = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc4
+****************************************************************************/
+void x86emuOp_les_R_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rh, rl;
+    u16 *dstreg;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LES\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	dstreg = DECODE_RM_WORD_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*dstreg = fetch_data_word(srcoffset);
+	M.x86.R_ES = fetch_data_word(srcoffset + 2);
+    }
+    /* else UNDEFINED!			 register to register */
+
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc5
+****************************************************************************/
+void x86emuOp_lds_R_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rh, rl;
+    u16 *dstreg;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LDS\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	dstreg = DECODE_RM_WORD_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*dstreg = fetch_data_word(srcoffset);
+	M.x86.R_DS = fetch_data_word(srcoffset + 2);
+    }
+    /* else UNDEFINED! */
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc6
+****************************************************************************/
+void x86emuOp_mov_byte_RM_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 imm;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (rh != 0) {
+	DECODE_PRINTF("ILLEGAL DECODE OF OPCODE c6\n");
+	HALT_SYS();
+    }
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	imm = fetch_byte_imm();
+	DECODE_PRINTF2(",%2x\n", imm);
+	TRACE_AND_STEP();
+	store_data_byte(destoffset, imm);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	imm = fetch_byte_imm();
+	DECODE_PRINTF2(",%2x\n", imm);
+	TRACE_AND_STEP();
+	*destreg = imm;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc7
+****************************************************************************/
+void x86emuOp_mov_word_RM_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOV\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (rh != 0) {
+	DECODE_PRINTF("ILLEGAL DECODE OF OPCODE 8F\n");
+	HALT_SYS();
+    }
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 imm;
+
+	    DECODE_PRINTF("DWORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    imm = fetch_long_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    store_data_long(destoffset, imm);
+	} else {
+	    u16 imm;
+
+	    DECODE_PRINTF("WORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    imm = fetch_word_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    store_data_word(destoffset, imm);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+			u32 *destreg;
+			u32 imm;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    imm = fetch_long_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    *destreg = imm;
+	} else {
+			u16 *destreg;
+			u16 imm;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    imm = fetch_word_imm();
+	    DECODE_PRINTF2(",%x\n", imm);
+	    TRACE_AND_STEP();
+	    *destreg = imm;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc8
+****************************************************************************/
+void x86emuOp_enter(u8 X86EMU_UNUSED(op1))
+{
+    u16 local,frame_pointer;
+    u8	nesting;
+    int i;
+
+    START_OF_INSTR();
+    local = fetch_word_imm();
+    nesting = fetch_byte_imm();
+    DECODE_PRINTF2("ENTER %x\n", local);
+    DECODE_PRINTF2(",%x\n", nesting);
+    TRACE_AND_STEP();
+    push_word(M.x86.R_BP);
+    frame_pointer = M.x86.R_SP;
+    if (nesting > 0) {
+	for (i = 1; i < nesting; i++) {
+	    M.x86.R_BP -= 2;
+	    push_word(fetch_data_word_abs(M.x86.R_SS, M.x86.R_BP));
+	    }
+	push_word(frame_pointer);
+	}
+    M.x86.R_BP = frame_pointer;
+    M.x86.R_SP = (u16)(M.x86.R_SP - local);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xc9
+****************************************************************************/
+void x86emuOp_leave(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("LEAVE\n");
+    TRACE_AND_STEP();
+    M.x86.R_SP = M.x86.R_BP;
+    M.x86.R_BP = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xca
+****************************************************************************/
+void x86emuOp_ret_far_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 imm;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("RETF\t");
+    imm = fetch_word_imm();
+    DECODE_PRINTF2("%x\n", imm);
+	RETURN_TRACE("RETF",M.x86.saved_cs,M.x86.saved_ip);
+	TRACE_AND_STEP();
+    M.x86.R_IP = pop_word();
+    M.x86.R_CS = pop_word();
+    M.x86.R_SP += imm;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xcb
+****************************************************************************/
+void x86emuOp_ret_far(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("RETF\n");
+	RETURN_TRACE("RETF",M.x86.saved_cs,M.x86.saved_ip);
+	TRACE_AND_STEP();
+    M.x86.R_IP = pop_word();
+    M.x86.R_CS = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xcc
+****************************************************************************/
+void x86emuOp_int3(u8 X86EMU_UNUSED(op1))
+{
+    u16 tmp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("INT 3\n");
+    tmp = (u16) mem_access_word(3 * 4 + 2);
+    /* access the segment register */
+    TRACE_AND_STEP();
+	if (_X86EMU_intrTab[3]) {
+		(*_X86EMU_intrTab[3])(3);
+    } else {
+	push_word((u16)M.x86.R_FLG);
+	CLEAR_FLAG(F_IF);
+	CLEAR_FLAG(F_TF);
+	push_word(M.x86.R_CS);
+	M.x86.R_CS = mem_access_word(3 * 4 + 2);
+	push_word(M.x86.R_IP);
+	M.x86.R_IP = mem_access_word(3 * 4);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xcd
+****************************************************************************/
+void x86emuOp_int_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 tmp;
+    u8 intnum;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("INT\t");
+    intnum = fetch_byte_imm();
+    DECODE_PRINTF2("%x\n", intnum);
+    tmp = mem_access_word(intnum * 4 + 2);
+    TRACE_AND_STEP();
+	if (_X86EMU_intrTab[intnum]) {
+		(*_X86EMU_intrTab[intnum])(intnum);
+    } else {
+	push_word((u16)M.x86.R_FLG);
+	CLEAR_FLAG(F_IF);
+	CLEAR_FLAG(F_TF);
+	push_word(M.x86.R_CS);
+	M.x86.R_CS = mem_access_word(intnum * 4 + 2);
+	push_word(M.x86.R_IP);
+	M.x86.R_IP = mem_access_word(intnum * 4);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xce
+****************************************************************************/
+void x86emuOp_into(u8 X86EMU_UNUSED(op1))
+{
+    u16 tmp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("INTO\n");
+    TRACE_AND_STEP();
+    if (ACCESS_FLAG(F_OF)) {
+	tmp = mem_access_word(4 * 4 + 2);
+		if (_X86EMU_intrTab[4]) {
+			(*_X86EMU_intrTab[4])(4);
+	} else {
+	    push_word((u16)M.x86.R_FLG);
+	    CLEAR_FLAG(F_IF);
+	    CLEAR_FLAG(F_TF);
+	    push_word(M.x86.R_CS);
+	    M.x86.R_CS = mem_access_word(4 * 4 + 2);
+	    push_word(M.x86.R_IP);
+	    M.x86.R_IP = mem_access_word(4 * 4);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xcf
+****************************************************************************/
+void x86emuOp_iret(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("IRET\n");
+
+    TRACE_AND_STEP();
+
+    M.x86.R_IP = pop_word();
+    M.x86.R_CS = pop_word();
+    M.x86.R_FLG = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd0
+****************************************************************************/
+void x86emuOp_opcD0_byte_RM_1(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 destval;
+
+    /*
+     * Yet another weirdo special case instruction format.  Part of
+     * the opcode held below in "RH".  Doubly nested case would
+     * result, except that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ROL\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("ROR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("RCL\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("RCR\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("SHL\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SHR\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("SAL\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("SAR\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",1\n");
+	destval = fetch_data_byte(destoffset);
+	TRACE_AND_STEP();
+	destval = (*opcD0_byte_operation[rh]) (destval, 1);
+	store_data_byte(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",1\n");
+	TRACE_AND_STEP();
+	destval = (*opcD0_byte_operation[rh]) (*destreg, 1);
+	*destreg = destval;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd1
+****************************************************************************/
+void x86emuOp_opcD1_word_RM_1(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    /*
+     * Yet another weirdo special case instruction format.  Part of
+     * the opcode held below in "RH".  Doubly nested case would
+     * result, except that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ROL\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("ROR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("RCL\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("RCR\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("SHL\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SHR\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("SAL\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("SAR\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+
+	    DECODE_PRINTF("DWORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    DECODE_PRINTF(",1\n");
+	    destval = fetch_data_long(destoffset);
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_long_operation[rh]) (destval, 1);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+
+	    DECODE_PRINTF("WORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    DECODE_PRINTF(",1\n");
+	    destval = fetch_data_word(destoffset);
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_word_operation[rh]) (destval, 1);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+			u32 destval;
+			u32 *destreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",1\n");
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_long_operation[rh]) (*destreg, 1);
+	    *destreg = destval;
+	} else {
+			u16 destval;
+			u16 *destreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",1\n");
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_word_operation[rh]) (*destreg, 1);
+	    *destreg = destval;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd2
+****************************************************************************/
+void x86emuOp_opcD2_byte_RM_CL(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 destval;
+    u8 amt;
+
+    /*
+     * Yet another weirdo special case instruction format.  Part of
+     * the opcode held below in "RH".  Doubly nested case would
+     * result, except that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ROL\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("ROR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("RCL\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("RCR\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("SHL\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SHR\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("SAL\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("SAR\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    amt = M.x86.R_CL;
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",CL\n");
+	destval = fetch_data_byte(destoffset);
+	TRACE_AND_STEP();
+	destval = (*opcD0_byte_operation[rh]) (destval, amt);
+	store_data_byte(destoffset, destval);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF(",CL\n");
+	TRACE_AND_STEP();
+	destval = (*opcD0_byte_operation[rh]) (*destreg, amt);
+	*destreg = destval;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd3
+****************************************************************************/
+void x86emuOp_opcD3_word_RM_CL(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+    u8 amt;
+
+    /*
+     * Yet another weirdo special case instruction format.  Part of
+     * the opcode held below in "RH".  Doubly nested case would
+     * result, except that the decoded instruction
+     */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("ROL\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("ROR\t");
+	    break;
+	case 2:
+	    DECODE_PRINTF("RCL\t");
+	    break;
+	case 3:
+	    DECODE_PRINTF("RCR\t");
+	    break;
+	case 4:
+	    DECODE_PRINTF("SHL\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("SHR\t");
+	    break;
+	case 6:
+	    DECODE_PRINTF("SAL\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("SAR\t");
+	    break;
+	}
+    }
+#endif
+    /* know operation, decode the mod byte to find the addressing
+       mode. */
+    amt = M.x86.R_CL;
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+
+	    DECODE_PRINTF("DWORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    DECODE_PRINTF(",CL\n");
+	    destval = fetch_data_long(destoffset);
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_long_operation[rh]) (destval, amt);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+
+	    DECODE_PRINTF("WORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    DECODE_PRINTF(",CL\n");
+	    destval = fetch_data_word(destoffset);
+	    TRACE_AND_STEP();
+	    destval = (*opcD1_word_operation[rh]) (destval, amt);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    *destreg = (*opcD1_long_operation[rh]) (*destreg, amt);
+	} else {
+	    u16 *destreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    *destreg = (*opcD1_word_operation[rh]) (*destreg, amt);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd4
+****************************************************************************/
+void x86emuOp_aam(u8 X86EMU_UNUSED(op1))
+{
+    u8 a;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("AAM\n");
+    a = fetch_byte_imm();      /* this is a stupid encoding. */
+    if (a != 10) {
+	DECODE_PRINTF("ERROR DECODING AAM\n");
+	TRACE_REGS();
+	HALT_SYS();
+    }
+    TRACE_AND_STEP();
+    /* note the type change here --- returning AL and AH in AX. */
+    M.x86.R_AX = aam_word(M.x86.R_AL);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd5
+****************************************************************************/
+void x86emuOp_aad(u8 X86EMU_UNUSED(op1))
+{
+    u8 a;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("AAD\n");
+    a = fetch_byte_imm();
+    TRACE_AND_STEP();
+    M.x86.R_AX = aad_word(M.x86.R_AX);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/* opcode 0xd6 ILLEGAL OPCODE */
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xd7
+****************************************************************************/
+void x86emuOp_xlat(u8 X86EMU_UNUSED(op1))
+{
+    u16 addr;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("XLAT\n");
+    TRACE_AND_STEP();
+	addr = (u16)(M.x86.R_BX + (u8)M.x86.R_AL);
+    M.x86.R_AL = fetch_data_byte(addr);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/* instuctions	D8 .. DF are in i87_ops.c */
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe0
+****************************************************************************/
+void x86emuOp_loopne(u8 X86EMU_UNUSED(op1))
+{
+    s16 ip;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LOOPNE\t");
+    ip = (s8) fetch_byte_imm();
+    ip += (s16) M.x86.R_IP;
+    DECODE_PRINTF2("%04x\n", ip);
+    TRACE_AND_STEP();
+    M.x86.R_CX -= 1;
+    if (M.x86.R_CX != 0 && !ACCESS_FLAG(F_ZF))	    /* CX != 0 and !ZF */
+	M.x86.R_IP = ip;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe1
+****************************************************************************/
+void x86emuOp_loope(u8 X86EMU_UNUSED(op1))
+{
+    s16 ip;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LOOPE\t");
+    ip = (s8) fetch_byte_imm();
+    ip += (s16) M.x86.R_IP;
+    DECODE_PRINTF2("%04x\n", ip);
+    TRACE_AND_STEP();
+    M.x86.R_CX -= 1;
+    if (M.x86.R_CX != 0 && ACCESS_FLAG(F_ZF))	    /* CX != 0 and ZF */
+	M.x86.R_IP = ip;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe2
+****************************************************************************/
+void x86emuOp_loop(u8 X86EMU_UNUSED(op1))
+{
+    s16 ip;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LOOP\t");
+    ip = (s8) fetch_byte_imm();
+    ip += (s16) M.x86.R_IP;
+    DECODE_PRINTF2("%04x\n", ip);
+    TRACE_AND_STEP();
+    M.x86.R_CX -= 1;
+    if (M.x86.R_CX != 0)
+	M.x86.R_IP = ip;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe3
+****************************************************************************/
+void x86emuOp_jcxz(u8 X86EMU_UNUSED(op1))
+{
+    u16 target;
+    s8	offset;
+
+    /* jump to byte offset if overflow flag is set */
+    START_OF_INSTR();
+    DECODE_PRINTF("JCXZ\t");
+    offset = (s8)fetch_byte_imm();
+    target = (u16)(M.x86.R_IP + offset);
+    DECODE_PRINTF2("%x\n", target);
+    TRACE_AND_STEP();
+    if (M.x86.R_CX == 0)
+	M.x86.R_IP = target;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe4
+****************************************************************************/
+void x86emuOp_in_byte_AL_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u8 port;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("IN\t");
+	port = (u8) fetch_byte_imm();
+    DECODE_PRINTF2("%x,AL\n", port);
+    TRACE_AND_STEP();
+    M.x86.R_AL = (*sys_inb)(port);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe5
+****************************************************************************/
+void x86emuOp_in_word_AX_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u8 port;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("IN\t");
+	port = (u8) fetch_byte_imm();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF2("EAX,%x\n", port);
+    } else {
+	DECODE_PRINTF2("AX,%x\n", port);
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	M.x86.R_EAX = (*sys_inl)(port);
+    } else {
+	M.x86.R_AX = (*sys_inw)(port);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe6
+****************************************************************************/
+void x86emuOp_out_byte_IMM_AL(u8 X86EMU_UNUSED(op1))
+{
+    u8 port;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("OUT\t");
+	port = (u8) fetch_byte_imm();
+    DECODE_PRINTF2("%x,AL\n", port);
+    TRACE_AND_STEP();
+    (*sys_outb)(port, M.x86.R_AL);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe7
+****************************************************************************/
+void x86emuOp_out_word_IMM_AX(u8 X86EMU_UNUSED(op1))
+{
+    u8 port;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("OUT\t");
+	port = (u8) fetch_byte_imm();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF2("%x,EAX\n", port);
+    } else {
+	DECODE_PRINTF2("%x,AX\n", port);
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	(*sys_outl)(port, M.x86.R_EAX);
+    } else {
+	(*sys_outw)(port, M.x86.R_AX);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe8
+****************************************************************************/
+void x86emuOp_call_near_IMM(u8 X86EMU_UNUSED(op1))
+{
+    s16 ip;
+
+    START_OF_INSTR();
+	DECODE_PRINTF("CALL\t");
+	ip = (s16) fetch_word_imm();
+	ip += (s16) M.x86.R_IP;	   /* CHECK SIGN */
+	DECODE_PRINTF2("%04x\n", ip);
+	CALL_TRACE(M.x86.saved_cs, M.x86.saved_ip, M.x86.R_CS, ip, "");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_IP);
+    M.x86.R_IP = ip;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xe9
+****************************************************************************/
+void x86emuOp_jump_near_IMM(u8 X86EMU_UNUSED(op1))
+{
+    int ip;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("JMP\t");
+    ip = (s16)fetch_word_imm();
+    ip += (s16)M.x86.R_IP;
+    DECODE_PRINTF2("%04x\n", ip);
+    TRACE_AND_STEP();
+    M.x86.R_IP = (u16)ip;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xea
+****************************************************************************/
+void x86emuOp_jump_far_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 cs, ip;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("JMP\tFAR ");
+    ip = fetch_word_imm();
+    cs = fetch_word_imm();
+    DECODE_PRINTF2("%04x:", cs);
+    DECODE_PRINTF2("%04x\n", ip);
+    TRACE_AND_STEP();
+    M.x86.R_IP = ip;
+    M.x86.R_CS = cs;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xeb
+****************************************************************************/
+void x86emuOp_jump_byte_IMM(u8 X86EMU_UNUSED(op1))
+{
+    u16 target;
+    s8 offset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("JMP\t");
+    offset = (s8)fetch_byte_imm();
+    target = (u16)(M.x86.R_IP + offset);
+    DECODE_PRINTF2("%x\n", target);
+    TRACE_AND_STEP();
+    M.x86.R_IP = target;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xec
+****************************************************************************/
+void x86emuOp_in_byte_AL_DX(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("IN\tAL,DX\n");
+    TRACE_AND_STEP();
+    M.x86.R_AL = (*sys_inb)(M.x86.R_DX);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xed
+****************************************************************************/
+void x86emuOp_in_word_AX_DX(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("IN\tEAX,DX\n");
+    } else {
+	DECODE_PRINTF("IN\tAX,DX\n");
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	M.x86.R_EAX = (*sys_inl)(M.x86.R_DX);
+    } else {
+	M.x86.R_AX = (*sys_inw)(M.x86.R_DX);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xee
+****************************************************************************/
+void x86emuOp_out_byte_DX_AL(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("OUT\tDX,AL\n");
+    TRACE_AND_STEP();
+    (*sys_outb)(M.x86.R_DX, M.x86.R_AL);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xef
+****************************************************************************/
+void x86emuOp_out_word_DX_AX(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	DECODE_PRINTF("OUT\tDX,EAX\n");
+    } else {
+	DECODE_PRINTF("OUT\tDX,AX\n");
+    }
+    TRACE_AND_STEP();
+    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	(*sys_outl)(M.x86.R_DX, M.x86.R_EAX);
+    } else {
+	(*sys_outw)(M.x86.R_DX, M.x86.R_AX);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf0
+****************************************************************************/
+void x86emuOp_lock(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("LOCK:\n");
+    TRACE_AND_STEP();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/*opcode 0xf1 ILLEGAL OPERATION */
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf2
+****************************************************************************/
+void x86emuOp_repne(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("REPNE\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_PREFIX_REPNE;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf3
+****************************************************************************/
+void x86emuOp_repe(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("REPE\n");
+    TRACE_AND_STEP();
+    M.x86.mode |= SYSMODE_PREFIX_REPE;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf4
+****************************************************************************/
+void x86emuOp_halt(u8 X86EMU_UNUSED(op1))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("HALT\n");
+    TRACE_AND_STEP();
+    HALT_SYS();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf5
+****************************************************************************/
+void x86emuOp_cmc(u8 X86EMU_UNUSED(op1))
+{
+    /* complement the carry flag. */
+    START_OF_INSTR();
+    DECODE_PRINTF("CMC\n");
+    TRACE_AND_STEP();
+    TOGGLE_FLAG(F_CF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf6
+****************************************************************************/
+void x86emuOp_opcF6_byte_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    u8 *destreg;
+    uint destoffset;
+    u8 destval, srcval;
+
+    /* long, drawn out code follows.  Double switch for a total
+       of 32 cases.  */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    DECODE_PRINTF(opF6_names[rh]);
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	destval = fetch_data_byte(destoffset);
+
+	switch (rh) {
+	case 0:		/* test byte imm */
+	    DECODE_PRINTF(",");
+	    srcval = fetch_byte_imm();
+	    DECODE_PRINTF2("%02x\n", srcval);
+	    TRACE_AND_STEP();
+	    test_byte(destval, srcval);
+	    break;
+	case 1:
+	    DECODE_PRINTF("ILLEGAL OP MOD=00 RH=01 OP=F6\n");
+	    HALT_SYS();
+	    break;
+	case 2:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    destval = not_byte(destval);
+	    store_data_byte(destoffset, destval);
+	    break;
+	case 3:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    destval = neg_byte(destval);
+	    store_data_byte(destoffset, destval);
+	    break;
+	case 4:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    mul_byte(destval);
+	    break;
+	case 5:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    imul_byte(destval);
+	    break;
+	case 6:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    div_byte(destval);
+	    break;
+	default:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    idiv_byte(destval);
+	    break;
+	}
+    } else {			 /* mod=11 */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	switch (rh) {
+	case 0:		/* test byte imm */
+	    DECODE_PRINTF(",");
+	    srcval = fetch_byte_imm();
+	    DECODE_PRINTF2("%02x\n", srcval);
+	    TRACE_AND_STEP();
+	    test_byte(*destreg, srcval);
+	    break;
+	case 1:
+	    DECODE_PRINTF("ILLEGAL OP MOD=00 RH=01 OP=F6\n");
+	    HALT_SYS();
+	    break;
+	case 2:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = not_byte(*destreg);
+	    break;
+	case 3:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = neg_byte(*destreg);
+	    break;
+	case 4:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    mul_byte(*destreg);	     /*!!!  */
+	    break;
+	case 5:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    imul_byte(*destreg);
+	    break;
+	case 6:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    div_byte(*destreg);
+	    break;
+	default:
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    idiv_byte(*destreg);
+	    break;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf7
+****************************************************************************/
+void x86emuOp_opcF7_word_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    DECODE_PRINTF(opF6_names[rh]);
+    if (mod < 3) {
+
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval, srcval;
+
+	    DECODE_PRINTF("DWORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    destval = fetch_data_long(destoffset);
+
+	    switch (rh) {
+	    case 0:
+		DECODE_PRINTF(",");
+		srcval = fetch_long_imm();
+		DECODE_PRINTF2("%x\n", srcval);
+		TRACE_AND_STEP();
+		test_long(destval, srcval);
+		break;
+	    case 1:
+		DECODE_PRINTF("ILLEGAL OP MOD=00 RH=01 OP=F7\n");
+		HALT_SYS();
+		break;
+	    case 2:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		destval = not_long(destval);
+		store_data_long(destoffset, destval);
+		break;
+	    case 3:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		destval = neg_long(destval);
+		store_data_long(destoffset, destval);
+		break;
+	    case 4:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		mul_long(destval);
+		break;
+	    case 5:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		imul_long(destval);
+		break;
+	    case 6:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		div_long(destval);
+		break;
+	    case 7:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		idiv_long(destval);
+		break;
+	    }
+	} else {
+	    u16 destval, srcval;
+
+	    DECODE_PRINTF("WORD PTR ");
+	    destoffset = decode_rmXX_address(mod, rl);
+	    destval = fetch_data_word(destoffset);
+
+	    switch (rh) {
+	    case 0:	    /* test word imm */
+		DECODE_PRINTF(",");
+		srcval = fetch_word_imm();
+		DECODE_PRINTF2("%x\n", srcval);
+		TRACE_AND_STEP();
+		test_word(destval, srcval);
+		break;
+	    case 1:
+		DECODE_PRINTF("ILLEGAL OP MOD=00 RH=01 OP=F7\n");
+		HALT_SYS();
+		break;
+	    case 2:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		destval = not_word(destval);
+		store_data_word(destoffset, destval);
+		break;
+	    case 3:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		destval = neg_word(destval);
+		store_data_word(destoffset, destval);
+		break;
+	    case 4:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		mul_word(destval);
+		break;
+	    case 5:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		imul_word(destval);
+		break;
+	    case 6:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		div_word(destval);
+		break;
+	    case 7:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		idiv_word(destval);
+		break;
+	    }
+	}
+
+    } else {			 /* mod=11 */
+
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+
+	    switch (rh) {
+	    case 0:	    /* test word imm */
+		DECODE_PRINTF(",");
+		srcval = fetch_long_imm();
+		DECODE_PRINTF2("%x\n", srcval);
+		TRACE_AND_STEP();
+		test_long(*destreg, srcval);
+		break;
+	    case 1:
+		DECODE_PRINTF("ILLEGAL OP MOD=00 RH=01 OP=F6\n");
+		HALT_SYS();
+		break;
+	    case 2:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = not_long(*destreg);
+		break;
+	    case 3:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = neg_long(*destreg);
+		break;
+	    case 4:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		mul_long(*destreg);	 /*!!!	*/
+		break;
+	    case 5:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		imul_long(*destreg);
+		break;
+	    case 6:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		div_long(*destreg);
+		break;
+	    case 7:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		idiv_long(*destreg);
+		break;
+	    }
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+
+	    switch (rh) {
+	    case 0:	    /* test word imm */
+		DECODE_PRINTF(",");
+		srcval = fetch_word_imm();
+		DECODE_PRINTF2("%x\n", srcval);
+		TRACE_AND_STEP();
+		test_word(*destreg, srcval);
+		break;
+	    case 1:
+		DECODE_PRINTF("ILLEGAL OP MOD=00 RH=01 OP=F6\n");
+		HALT_SYS();
+		break;
+	    case 2:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = not_word(*destreg);
+		break;
+	    case 3:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = neg_word(*destreg);
+		break;
+	    case 4:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		mul_word(*destreg);	 /*!!!	*/
+		break;
+	    case 5:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		imul_word(*destreg);
+		break;
+	    case 6:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		div_word(*destreg);
+		break;
+	    case 7:
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		idiv_word(*destreg);
+		break;
+	    }
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf8
+****************************************************************************/
+void x86emuOp_clc(u8 X86EMU_UNUSED(op1))
+{
+    /* clear the carry flag. */
+    START_OF_INSTR();
+    DECODE_PRINTF("CLC\n");
+    TRACE_AND_STEP();
+    CLEAR_FLAG(F_CF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xf9
+****************************************************************************/
+void x86emuOp_stc(u8 X86EMU_UNUSED(op1))
+{
+    /* set the carry flag. */
+    START_OF_INSTR();
+    DECODE_PRINTF("STC\n");
+    TRACE_AND_STEP();
+    SET_FLAG(F_CF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xfa
+****************************************************************************/
+void x86emuOp_cli(u8 X86EMU_UNUSED(op1))
+{
+    /* clear interrupts. */
+    START_OF_INSTR();
+    DECODE_PRINTF("CLI\n");
+    TRACE_AND_STEP();
+    CLEAR_FLAG(F_IF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xfb
+****************************************************************************/
+void x86emuOp_sti(u8 X86EMU_UNUSED(op1))
+{
+    /* enable  interrupts. */
+    START_OF_INSTR();
+    DECODE_PRINTF("STI\n");
+    TRACE_AND_STEP();
+    SET_FLAG(F_IF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xfc
+****************************************************************************/
+void x86emuOp_cld(u8 X86EMU_UNUSED(op1))
+{
+    /* clear interrupts. */
+    START_OF_INSTR();
+    DECODE_PRINTF("CLD\n");
+    TRACE_AND_STEP();
+    CLEAR_FLAG(F_DF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xfd
+****************************************************************************/
+void x86emuOp_std(u8 X86EMU_UNUSED(op1))
+{
+    /* clear interrupts. */
+    START_OF_INSTR();
+    DECODE_PRINTF("STD\n");
+    TRACE_AND_STEP();
+    SET_FLAG(F_DF);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xfe
+****************************************************************************/
+void x86emuOp_opcFE_byte_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rh, rl;
+    u8 destval;
+    uint destoffset;
+    u8 *destreg;
+
+    /* Yet another special case instruction. */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+
+	switch (rh) {
+	case 0:
+	    DECODE_PRINTF("INC\t");
+	    break;
+	case 1:
+	    DECODE_PRINTF("DEC\t");
+	    break;
+	case 2:
+	case 3:
+	case 4:
+	case 5:
+	case 6:
+	case 7:
+	    DECODE_PRINTF2("ILLEGAL OP MAJOR OP 0xFE MINOR OP %x \n", mod);
+	    HALT_SYS();
+	    break;
+	}
+    }
+#endif
+    if (mod < 3) {
+	DECODE_PRINTF("BYTE PTR ");
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	destval = fetch_data_byte(destoffset);
+	TRACE_AND_STEP();
+	if (rh == 0)
+	  destval = inc_byte(destval);
+	else
+	  destval = dec_byte(destval);
+	store_data_byte(destoffset, destval);
+    } else {
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	if (rh == 0)
+	  *destreg = inc_byte(*destreg);
+	else
+	  *destreg = dec_byte(*destreg);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0xff
+****************************************************************************/
+void x86emuOp_opcFF_word_RM(u8 X86EMU_UNUSED(op1))
+{
+    int mod, rh, rl;
+    uint destoffset = 0;
+	u16 *destreg;
+	u16 destval,destval2;
+
+    /* Yet another special case instruction. */
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+#ifdef DEBUG
+    if (DEBUG_DECODE()) {
+	/* XXX DECODE_PRINTF may be changed to something more
+	   general, so that it is important to leave the strings
+	   in the same format, even though the result is that the
+	   above test is done twice. */
+
+	switch (rh) {
+	case 0:
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		DECODE_PRINTF("INC\tDWORD PTR ");
+	    } else {
+		DECODE_PRINTF("INC\tWORD PTR ");
+	    }
+	    break;
+	case 1:
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		DECODE_PRINTF("DEC\tDWORD PTR ");
+	    } else {
+		DECODE_PRINTF("DEC\tWORD PTR ");
+	    }
+	    break;
+	case 2:
+	    DECODE_PRINTF("CALL\t ");
+	    break;
+	case 3:
+	    DECODE_PRINTF("CALL\tFAR ");
+	    break;
+	case 4:
+	    DECODE_PRINTF("JMP\t");
+	    break;
+	case 5:
+	    DECODE_PRINTF("JMP\tFAR ");
+	    break;
+	case 6:
+	    DECODE_PRINTF("PUSH\t");
+	    break;
+	case 7:
+	    DECODE_PRINTF("ILLEGAL DECODING OF OPCODE FF\t");
+	    HALT_SYS();
+	    break;
+	}
+    }
+#endif
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	switch (rh) {
+	case 0:		/* inc word ptr ... */
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		u32 destval;
+
+		destval = fetch_data_long(destoffset);
+		TRACE_AND_STEP();
+		destval = inc_long(destval);
+		store_data_long(destoffset, destval);
+	    } else {
+		u16 destval;
+
+		destval = fetch_data_word(destoffset);
+		TRACE_AND_STEP();
+		destval = inc_word(destval);
+		store_data_word(destoffset, destval);
+	    }
+	    break;
+	case 1:		/* dec word ptr ... */
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		u32 destval;
+
+		destval = fetch_data_long(destoffset);
+		TRACE_AND_STEP();
+		destval = dec_long(destval);
+		store_data_long(destoffset, destval);
+	    } else {
+		u16 destval;
+
+		destval = fetch_data_word(destoffset);
+		TRACE_AND_STEP();
+		destval = dec_word(destval);
+		store_data_word(destoffset, destval);
+	    }
+	    break;
+	case 2:		/* call word ptr ... */
+	    destval = fetch_data_word(destoffset);
+	    TRACE_AND_STEP();
+	    push_word(M.x86.R_IP);
+	    M.x86.R_IP = destval;
+	    break;
+	case 3:		/* call far ptr ... */
+	    destval = fetch_data_word(destoffset);
+	    destval2 = fetch_data_word(destoffset + 2);
+	    TRACE_AND_STEP();
+	    push_word(M.x86.R_CS);
+	    M.x86.R_CS = destval2;
+	    push_word(M.x86.R_IP);
+	    M.x86.R_IP = destval;
+	    break;
+	case 4:		/* jmp word ptr ... */
+	    destval = fetch_data_word(destoffset);
+	    TRACE_AND_STEP();
+	    M.x86.R_IP = destval;
+	    break;
+	case 5:		/* jmp far ptr ... */
+	    destval = fetch_data_word(destoffset);
+	    destval2 = fetch_data_word(destoffset + 2);
+	    TRACE_AND_STEP();
+	    M.x86.R_IP = destval;
+	    M.x86.R_CS = destval2;
+	    break;
+	case 6:		/*  push word ptr ... */
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		u32 destval;
+
+		destval = fetch_data_long(destoffset);
+		TRACE_AND_STEP();
+		push_long(destval);
+	    } else {
+		u16 destval;
+
+		destval = fetch_data_word(destoffset);
+		TRACE_AND_STEP();
+		push_word(destval);
+	    }
+	    break;
+	}
+    } else {
+	switch (rh) {
+	case 0:
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		u32 *destreg;
+
+		destreg = DECODE_RM_LONG_REGISTER(rl);
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = inc_long(*destreg);
+	    } else {
+		u16 *destreg;
+
+		destreg = DECODE_RM_WORD_REGISTER(rl);
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = inc_word(*destreg);
+	    }
+	    break;
+	case 1:
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		u32 *destreg;
+
+		destreg = DECODE_RM_LONG_REGISTER(rl);
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = dec_long(*destreg);
+	    } else {
+		u16 *destreg;
+
+		destreg = DECODE_RM_WORD_REGISTER(rl);
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		*destreg = dec_word(*destreg);
+	    }
+	    break;
+	case 2:		/* call word ptr ... */
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    push_word(M.x86.R_IP);
+	    M.x86.R_IP = *destreg;
+	    break;
+	case 3:		/* jmp far ptr ... */
+	    DECODE_PRINTF("OPERATION UNDEFINED 0XFF \n");
+	    TRACE_AND_STEP();
+	    HALT_SYS();
+	    break;
+
+	case 4:		/* jmp	... */
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    M.x86.R_IP = (u16) (*destreg);
+	    break;
+	case 5:		/* jmp far ptr ... */
+	    DECODE_PRINTF("OPERATION UNDEFINED 0XFF \n");
+	    TRACE_AND_STEP();
+	    HALT_SYS();
+	    break;
+	case 6:
+	    if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+		u32 *destreg;
+
+		destreg = DECODE_RM_LONG_REGISTER(rl);
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		push_long(*destreg);
+	    } else {
+		u16 *destreg;
+
+		destreg = DECODE_RM_WORD_REGISTER(rl);
+		DECODE_PRINTF("\n");
+		TRACE_AND_STEP();
+		push_word(*destreg);
+	    }
+	    break;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/***************************************************************************
+ * Single byte operation code table:
+ **************************************************************************/
+void (*x86emu_optab[256])(u8) __attribute__ ((section(".got2"))) =
+{
+/*  0x00 */ x86emuOp_genop_byte_RM_R,
+/*  0x01 */ x86emuOp_genop_word_RM_R,
+/*  0x02 */ x86emuOp_genop_byte_R_RM,
+/*  0x03 */ x86emuOp_genop_word_R_RM,
+/*  0x04 */ x86emuOp_genop_byte_AL_IMM,
+/*  0x05 */ x86emuOp_genop_word_AX_IMM,
+/*  0x06 */ x86emuOp_push_ES,
+/*  0x07 */ x86emuOp_pop_ES,
+
+/*  0x08 */ x86emuOp_genop_byte_RM_R,
+/*  0x09 */ x86emuOp_genop_word_RM_R,
+/*  0x0a */ x86emuOp_genop_byte_R_RM,
+/*  0x0b */ x86emuOp_genop_word_R_RM,
+/*  0x0c */ x86emuOp_genop_byte_AL_IMM,
+/*  0x0d */ x86emuOp_genop_word_AX_IMM,
+/*  0x0e */ x86emuOp_push_CS,
+/*  0x0f */ x86emuOp_two_byte,
+
+/*  0x10 */ x86emuOp_genop_byte_RM_R,
+/*  0x11 */ x86emuOp_genop_word_RM_R,
+/*  0x12 */ x86emuOp_genop_byte_R_RM,
+/*  0x13 */ x86emuOp_genop_word_R_RM,
+/*  0x14 */ x86emuOp_genop_byte_AL_IMM,
+/*  0x15 */ x86emuOp_genop_word_AX_IMM,
+/*  0x16 */ x86emuOp_push_SS,
+/*  0x17 */ x86emuOp_pop_SS,
+
+/*  0x18 */ x86emuOp_genop_byte_RM_R,
+/*  0x19 */ x86emuOp_genop_word_RM_R,
+/*  0x1a */ x86emuOp_genop_byte_R_RM,
+/*  0x1b */ x86emuOp_genop_word_R_RM,
+/*  0x1c */ x86emuOp_genop_byte_AL_IMM,
+/*  0x1d */ x86emuOp_genop_word_AX_IMM,
+/*  0x1e */ x86emuOp_push_DS,
+/*  0x1f */ x86emuOp_pop_DS,
+
+/*  0x20 */ x86emuOp_genop_byte_RM_R,
+/*  0x21 */ x86emuOp_genop_word_RM_R,
+/*  0x22 */ x86emuOp_genop_byte_R_RM,
+/*  0x23 */ x86emuOp_genop_word_R_RM,
+/*  0x24 */ x86emuOp_genop_byte_AL_IMM,
+/*  0x25 */ x86emuOp_genop_word_AX_IMM,
+/*  0x26 */ x86emuOp_segovr_ES,
+/*  0x27 */ x86emuOp_daa,
+
+/*  0x28 */ x86emuOp_genop_byte_RM_R,
+/*  0x29 */ x86emuOp_genop_word_RM_R,
+/*  0x2a */ x86emuOp_genop_byte_R_RM,
+/*  0x2b */ x86emuOp_genop_word_R_RM,
+/*  0x2c */ x86emuOp_genop_byte_AL_IMM,
+/*  0x2d */ x86emuOp_genop_word_AX_IMM,
+/*  0x2e */ x86emuOp_segovr_CS,
+/*  0x2f */ x86emuOp_das,
+
+/*  0x30 */ x86emuOp_genop_byte_RM_R,
+/*  0x31 */ x86emuOp_genop_word_RM_R,
+/*  0x32 */ x86emuOp_genop_byte_R_RM,
+/*  0x33 */ x86emuOp_genop_word_R_RM,
+/*  0x34 */ x86emuOp_genop_byte_AL_IMM,
+/*  0x35 */ x86emuOp_genop_word_AX_IMM,
+/*  0x36 */ x86emuOp_segovr_SS,
+/*  0x37 */ x86emuOp_aaa,
+
+/*  0x38 */ x86emuOp_genop_byte_RM_R,
+/*  0x39 */ x86emuOp_genop_word_RM_R,
+/*  0x3a */ x86emuOp_genop_byte_R_RM,
+/*  0x3b */ x86emuOp_genop_word_R_RM,
+/*  0x3c */ x86emuOp_genop_byte_AL_IMM,
+/*  0x3d */ x86emuOp_genop_word_AX_IMM,
+/*  0x3e */ x86emuOp_segovr_DS,
+/*  0x3f */ x86emuOp_aas,
+
+/*  0x40 */ x86emuOp_inc_register,
+/*  0x41 */ x86emuOp_inc_register,
+/*  0x42 */ x86emuOp_inc_register,
+/*  0x43 */ x86emuOp_inc_register,
+/*  0x44 */ x86emuOp_inc_register,
+/*  0x45 */ x86emuOp_inc_register,
+/*  0x46 */ x86emuOp_inc_register,
+/*  0x47 */ x86emuOp_inc_register,
+
+/*  0x48 */ x86emuOp_dec_register,
+/*  0x49 */ x86emuOp_dec_register,
+/*  0x4a */ x86emuOp_dec_register,
+/*  0x4b */ x86emuOp_dec_register,
+/*  0x4c */ x86emuOp_dec_register,
+/*  0x4d */ x86emuOp_dec_register,
+/*  0x4e */ x86emuOp_dec_register,
+/*  0x4f */ x86emuOp_dec_register,
+
+/*  0x50 */ x86emuOp_push_register,
+/*  0x51 */ x86emuOp_push_register,
+/*  0x52 */ x86emuOp_push_register,
+/*  0x53 */ x86emuOp_push_register,
+/*  0x54 */ x86emuOp_push_register,
+/*  0x55 */ x86emuOp_push_register,
+/*  0x56 */ x86emuOp_push_register,
+/*  0x57 */ x86emuOp_push_register,
+
+/*  0x58 */ x86emuOp_pop_register,
+/*  0x59 */ x86emuOp_pop_register,
+/*  0x5a */ x86emuOp_pop_register,
+/*  0x5b */ x86emuOp_pop_register,
+/*  0x5c */ x86emuOp_pop_register,
+/*  0x5d */ x86emuOp_pop_register,
+/*  0x5e */ x86emuOp_pop_register,
+/*  0x5f */ x86emuOp_pop_register,
+
+/*  0x60 */ x86emuOp_push_all,
+/*  0x61 */ x86emuOp_pop_all,
+/*  0x62 */ x86emuOp_illegal_op,   /* bound */
+/*  0x63 */ x86emuOp_illegal_op,   /* arpl */
+/*  0x64 */ x86emuOp_segovr_FS,
+/*  0x65 */ x86emuOp_segovr_GS,
+/*  0x66 */ x86emuOp_prefix_data,
+/*  0x67 */ x86emuOp_prefix_addr,
+
+/*  0x68 */ x86emuOp_push_word_IMM,
+/*  0x69 */ x86emuOp_imul_word_IMM,
+/*  0x6a */ x86emuOp_push_byte_IMM,
+/*  0x6b */ x86emuOp_imul_byte_IMM,
+/*  0x6c */ x86emuOp_ins_byte,
+/*  0x6d */ x86emuOp_ins_word,
+/*  0x6e */ x86emuOp_outs_byte,
+/*  0x6f */ x86emuOp_outs_word,
+
+/*  0x70 */ x86emuOp_jump_near_cond,
+/*  0x71 */ x86emuOp_jump_near_cond,
+/*  0x72 */ x86emuOp_jump_near_cond,
+/*  0x73 */ x86emuOp_jump_near_cond,
+/*  0x74 */ x86emuOp_jump_near_cond,
+/*  0x75 */ x86emuOp_jump_near_cond,
+/*  0x76 */ x86emuOp_jump_near_cond,
+/*  0x77 */ x86emuOp_jump_near_cond,
+
+/*  0x78 */ x86emuOp_jump_near_cond,
+/*  0x79 */ x86emuOp_jump_near_cond,
+/*  0x7a */ x86emuOp_jump_near_cond,
+/*  0x7b */ x86emuOp_jump_near_cond,
+/*  0x7c */ x86emuOp_jump_near_cond,
+/*  0x7d */ x86emuOp_jump_near_cond,
+/*  0x7e */ x86emuOp_jump_near_cond,
+/*  0x7f */ x86emuOp_jump_near_cond,
+
+/*  0x80 */ x86emuOp_opc80_byte_RM_IMM,
+/*  0x81 */ x86emuOp_opc81_word_RM_IMM,
+/*  0x82 */ x86emuOp_opc82_byte_RM_IMM,
+/*  0x83 */ x86emuOp_opc83_word_RM_IMM,
+/*  0x84 */ x86emuOp_test_byte_RM_R,
+/*  0x85 */ x86emuOp_test_word_RM_R,
+/*  0x86 */ x86emuOp_xchg_byte_RM_R,
+/*  0x87 */ x86emuOp_xchg_word_RM_R,
+
+/*  0x88 */ x86emuOp_mov_byte_RM_R,
+/*  0x89 */ x86emuOp_mov_word_RM_R,
+/*  0x8a */ x86emuOp_mov_byte_R_RM,
+/*  0x8b */ x86emuOp_mov_word_R_RM,
+/*  0x8c */ x86emuOp_mov_word_RM_SR,
+/*  0x8d */ x86emuOp_lea_word_R_M,
+/*  0x8e */ x86emuOp_mov_word_SR_RM,
+/*  0x8f */ x86emuOp_pop_RM,
+
+/*  0x90 */ x86emuOp_nop,
+/*  0x91 */ x86emuOp_xchg_word_AX_register,
+/*  0x92 */ x86emuOp_xchg_word_AX_register,
+/*  0x93 */ x86emuOp_xchg_word_AX_register,
+/*  0x94 */ x86emuOp_xchg_word_AX_register,
+/*  0x95 */ x86emuOp_xchg_word_AX_register,
+/*  0x96 */ x86emuOp_xchg_word_AX_register,
+/*  0x97 */ x86emuOp_xchg_word_AX_register,
+
+/*  0x98 */ x86emuOp_cbw,
+/*  0x99 */ x86emuOp_cwd,
+/*  0x9a */ x86emuOp_call_far_IMM,
+/*  0x9b */ x86emuOp_wait,
+/*  0x9c */ x86emuOp_pushf_word,
+/*  0x9d */ x86emuOp_popf_word,
+/*  0x9e */ x86emuOp_sahf,
+/*  0x9f */ x86emuOp_lahf,
+
+/*  0xa0 */ x86emuOp_mov_AL_M_IMM,
+/*  0xa1 */ x86emuOp_mov_AX_M_IMM,
+/*  0xa2 */ x86emuOp_mov_M_AL_IMM,
+/*  0xa3 */ x86emuOp_mov_M_AX_IMM,
+/*  0xa4 */ x86emuOp_movs_byte,
+/*  0xa5 */ x86emuOp_movs_word,
+/*  0xa6 */ x86emuOp_cmps_byte,
+/*  0xa7 */ x86emuOp_cmps_word,
+/*  0xa8 */ x86emuOp_test_AL_IMM,
+/*  0xa9 */ x86emuOp_test_AX_IMM,
+/*  0xaa */ x86emuOp_stos_byte,
+/*  0xab */ x86emuOp_stos_word,
+/*  0xac */ x86emuOp_lods_byte,
+/*  0xad */ x86emuOp_lods_word,
+/*  0xac */ x86emuOp_scas_byte,
+/*  0xad */ x86emuOp_scas_word,
+
+/*  0xb0 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb1 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb2 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb3 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb4 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb5 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb6 */ x86emuOp_mov_byte_register_IMM,
+/*  0xb7 */ x86emuOp_mov_byte_register_IMM,
+
+/*  0xb8 */ x86emuOp_mov_word_register_IMM,
+/*  0xb9 */ x86emuOp_mov_word_register_IMM,
+/*  0xba */ x86emuOp_mov_word_register_IMM,
+/*  0xbb */ x86emuOp_mov_word_register_IMM,
+/*  0xbc */ x86emuOp_mov_word_register_IMM,
+/*  0xbd */ x86emuOp_mov_word_register_IMM,
+/*  0xbe */ x86emuOp_mov_word_register_IMM,
+/*  0xbf */ x86emuOp_mov_word_register_IMM,
+
+/*  0xc0 */ x86emuOp_opcC0_byte_RM_MEM,
+/*  0xc1 */ x86emuOp_opcC1_word_RM_MEM,
+/*  0xc2 */ x86emuOp_ret_near_IMM,
+/*  0xc3 */ x86emuOp_ret_near,
+/*  0xc4 */ x86emuOp_les_R_IMM,
+/*  0xc5 */ x86emuOp_lds_R_IMM,
+/*  0xc6 */ x86emuOp_mov_byte_RM_IMM,
+/*  0xc7 */ x86emuOp_mov_word_RM_IMM,
+/*  0xc8 */ x86emuOp_enter,
+/*  0xc9 */ x86emuOp_leave,
+/*  0xca */ x86emuOp_ret_far_IMM,
+/*  0xcb */ x86emuOp_ret_far,
+/*  0xcc */ x86emuOp_int3,
+/*  0xcd */ x86emuOp_int_IMM,
+/*  0xce */ x86emuOp_into,
+/*  0xcf */ x86emuOp_iret,
+
+/*  0xd0 */ x86emuOp_opcD0_byte_RM_1,
+/*  0xd1 */ x86emuOp_opcD1_word_RM_1,
+/*  0xd2 */ x86emuOp_opcD2_byte_RM_CL,
+/*  0xd3 */ x86emuOp_opcD3_word_RM_CL,
+/*  0xd4 */ x86emuOp_aam,
+/*  0xd5 */ x86emuOp_aad,
+/*  0xd6 */ x86emuOp_illegal_op,   /* Undocumented SETALC instruction */
+/*  0xd7 */ x86emuOp_xlat,
+/*  0xd8 */ NULL, /*x86emuOp_esc_coprocess_d8,*/
+/*  0xd9 */ NULL, /*x86emuOp_esc_coprocess_d9,*/
+/*  0xda */ NULL, /*x86emuOp_esc_coprocess_da,*/
+/*  0xdb */ NULL, /*x86emuOp_esc_coprocess_db,*/
+/*  0xdc */ NULL, /*x86emuOp_esc_coprocess_dc,*/
+/*  0xdd */ NULL, /*x86emuOp_esc_coprocess_dd,*/
+/*  0xde */ NULL, /*x86emuOp_esc_coprocess_de,*/
+/*  0xdf */ NULL, /*x86emuOp_esc_coprocess_df,*/
+
+/*  0xe0 */ x86emuOp_loopne,
+/*  0xe1 */ x86emuOp_loope,
+/*  0xe2 */ x86emuOp_loop,
+/*  0xe3 */ x86emuOp_jcxz,
+/*  0xe4 */ x86emuOp_in_byte_AL_IMM,
+/*  0xe5 */ x86emuOp_in_word_AX_IMM,
+/*  0xe6 */ x86emuOp_out_byte_IMM_AL,
+/*  0xe7 */ x86emuOp_out_word_IMM_AX,
+
+/*  0xe8 */ x86emuOp_call_near_IMM,
+/*  0xe9 */ x86emuOp_jump_near_IMM,
+/*  0xea */ x86emuOp_jump_far_IMM,
+/*  0xeb */ x86emuOp_jump_byte_IMM,
+/*  0xec */ x86emuOp_in_byte_AL_DX,
+/*  0xed */ x86emuOp_in_word_AX_DX,
+/*  0xee */ x86emuOp_out_byte_DX_AL,
+/*  0xef */ x86emuOp_out_word_DX_AX,
+
+/*  0xf0 */ x86emuOp_lock,
+/*  0xf1 */ x86emuOp_illegal_op,
+/*  0xf2 */ x86emuOp_repne,
+/*  0xf3 */ x86emuOp_repe,
+/*  0xf4 */ x86emuOp_halt,
+/*  0xf5 */ x86emuOp_cmc,
+/*  0xf6 */ x86emuOp_opcF6_byte_RM,
+/*  0xf7 */ x86emuOp_opcF7_word_RM,
+
+/*  0xf8 */ x86emuOp_clc,
+/*  0xf9 */ x86emuOp_stc,
+/*  0xfa */ x86emuOp_cli,
+/*  0xfb */ x86emuOp_sti,
+/*  0xfc */ x86emuOp_cld,
+/*  0xfd */ x86emuOp_std,
+/*  0xfe */ x86emuOp_opcFE_byte_RM,
+/*  0xff */ x86emuOp_opcFF_word_RM,
+};
+
+#endif
diff --git a/drivers/bios_emulator/x86emu/ops2.c b/drivers/bios_emulator/x86emu/ops2.c
new file mode 100644
index 00000000000..81c0d49a333
--- /dev/null
+++ b/drivers/bios_emulator/x86emu/ops2.c
@@ -0,0 +1,1774 @@
+/****************************************************************************
+*
+*			Realmode X86 Emulator Library
+*
+*  Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+*  Jason Jin <Jason.jin@freescale.com>
+*
+*		Copyright (C) 1991-2004 SciTech Software, Inc.
+*		     Copyright (C) David Mosberger-Tang
+*		       Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	This file includes subroutines to implement the decoding
+*		and emulation of all the x86 extended two-byte processor
+*		instructions.
+*
+*		Jason port this file to u-boot. Put the function pointer into
+*		got2 sector.
+*
+****************************************************************************/
+
+#include "x86emu/x86emui.h"
+
+#if defined(CONFIG_BIOSEMU)
+
+/*----------------------------- Implementation ----------------------------*/
+
+/****************************************************************************
+PARAMETERS:
+op1 - Instruction op code
+
+REMARKS:
+Handles illegal opcodes.
+****************************************************************************/
+void x86emuOp2_illegal_op(
+    u8 op2)
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("ILLEGAL EXTENDED X86 OPCODE\n");
+    TRACE_REGS();
+    printk("%04x:%04x: %02X ILLEGAL EXTENDED X86 OPCODE!\n",
+	M.x86.R_CS, M.x86.R_IP-2,op2);
+    HALT_SYS();
+    END_OF_INSTR();
+}
+
+#define xorl(a,b)   ((a) && !(b)) || (!(a) && (b))
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0x80-0x8F
+****************************************************************************/
+int x86emu_check_jump_condition(u8 op)
+{
+    switch (op) {
+      case 0x0:
+	DECODE_PRINTF("JO\t");
+	return ACCESS_FLAG(F_OF);
+      case 0x1:
+	DECODE_PRINTF("JNO\t");
+	return !ACCESS_FLAG(F_OF);
+	break;
+      case 0x2:
+	DECODE_PRINTF("JB\t");
+	return ACCESS_FLAG(F_CF);
+	break;
+      case 0x3:
+	DECODE_PRINTF("JNB\t");
+	return !ACCESS_FLAG(F_CF);
+	break;
+      case 0x4:
+	DECODE_PRINTF("JZ\t");
+	return ACCESS_FLAG(F_ZF);
+	break;
+      case 0x5:
+	DECODE_PRINTF("JNZ\t");
+	return !ACCESS_FLAG(F_ZF);
+	break;
+      case 0x6:
+	DECODE_PRINTF("JBE\t");
+	return ACCESS_FLAG(F_CF) || ACCESS_FLAG(F_ZF);
+	break;
+      case 0x7:
+	DECODE_PRINTF("JNBE\t");
+	return !(ACCESS_FLAG(F_CF) || ACCESS_FLAG(F_ZF));
+	break;
+      case 0x8:
+	DECODE_PRINTF("JS\t");
+	return ACCESS_FLAG(F_SF);
+	break;
+      case 0x9:
+	DECODE_PRINTF("JNS\t");
+	return !ACCESS_FLAG(F_SF);
+	break;
+      case 0xa:
+	DECODE_PRINTF("JP\t");
+	return ACCESS_FLAG(F_PF);
+	break;
+      case 0xb:
+	DECODE_PRINTF("JNP\t");
+	return !ACCESS_FLAG(F_PF);
+	break;
+      case 0xc:
+	DECODE_PRINTF("JL\t");
+	return xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF));
+	break;
+      case 0xd:
+	DECODE_PRINTF("JNL\t");
+	return !xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF));
+	break;
+      case 0xe:
+	DECODE_PRINTF("JLE\t");
+	return (xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF)) ||
+		ACCESS_FLAG(F_ZF));
+	break;
+      default:
+	DECODE_PRINTF("JNLE\t");
+	return !(xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF)) ||
+		 ACCESS_FLAG(F_ZF));
+    }
+}
+
+void x86emuOp2_long_jump(u8 op2)
+{
+    s32 target;
+    int cond;
+
+    /* conditional jump to word offset. */
+    START_OF_INSTR();
+    cond = x86emu_check_jump_condition(op2 & 0xF);
+    target = (s16) fetch_word_imm();
+    target += (s16) M.x86.R_IP;
+    DECODE_PRINTF2("%04x\n", target);
+    TRACE_AND_STEP();
+    if (cond)
+	M.x86.R_IP = (u16)target;
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0x90-0x9F
+****************************************************************************/
+void x86emuOp2_set_byte(u8 op2)
+{
+    int mod, rl, rh;
+    uint destoffset;
+    u8	*destreg;
+    char *name = 0;
+    int cond = 0;
+
+    START_OF_INSTR();
+    switch (op2) {
+      case 0x90:
+	name = "SETO\t";
+	cond =	ACCESS_FLAG(F_OF);
+	break;
+      case 0x91:
+	name = "SETNO\t";
+	cond = !ACCESS_FLAG(F_OF);
+	break;
+      case 0x92:
+	name = "SETB\t";
+	cond = ACCESS_FLAG(F_CF);
+	break;
+      case 0x93:
+	name = "SETNB\t";
+	cond = !ACCESS_FLAG(F_CF);
+	break;
+      case 0x94:
+	name = "SETZ\t";
+	cond = ACCESS_FLAG(F_ZF);
+	break;
+      case 0x95:
+	name = "SETNZ\t";
+	cond = !ACCESS_FLAG(F_ZF);
+	break;
+      case 0x96:
+	name = "SETBE\t";
+	cond = ACCESS_FLAG(F_CF) || ACCESS_FLAG(F_ZF);
+	break;
+      case 0x97:
+	name = "SETNBE\t";
+	cond = !(ACCESS_FLAG(F_CF) || ACCESS_FLAG(F_ZF));
+	break;
+      case 0x98:
+	name = "SETS\t";
+	cond = ACCESS_FLAG(F_SF);
+	break;
+      case 0x99:
+	name = "SETNS\t";
+	cond = !ACCESS_FLAG(F_SF);
+	break;
+      case 0x9a:
+	name = "SETP\t";
+	cond = ACCESS_FLAG(F_PF);
+	break;
+      case 0x9b:
+	name = "SETNP\t";
+	cond = !ACCESS_FLAG(F_PF);
+	break;
+      case 0x9c:
+	name = "SETL\t";
+	cond = xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF));
+	break;
+      case 0x9d:
+	name = "SETNL\t";
+	cond = !xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF));
+	break;
+      case 0x9e:
+	name = "SETLE\t";
+	cond = (xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF)) ||
+		ACCESS_FLAG(F_ZF));
+	break;
+      case 0x9f:
+	name = "SETNLE\t";
+	cond = !(xorl(ACCESS_FLAG(F_SF), ACCESS_FLAG(F_OF)) ||
+		 ACCESS_FLAG(F_ZF));
+	break;
+    }
+    DECODE_PRINTF(name);
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	TRACE_AND_STEP();
+	store_data_byte(destoffset, cond ? 0x01 : 0x00);
+    } else {			 /* register to register */
+	destreg = DECODE_RM_BYTE_REGISTER(rl);
+	TRACE_AND_STEP();
+	*destreg = cond ? 0x01 : 0x00;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa0
+****************************************************************************/
+void x86emuOp2_push_FS(u8 X86EMU_UNUSED(op2))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("PUSH\tFS\n");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_FS);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa1
+****************************************************************************/
+void x86emuOp2_pop_FS(u8 X86EMU_UNUSED(op2))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("POP\tFS\n");
+    TRACE_AND_STEP();
+    M.x86.R_FS = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa3
+****************************************************************************/
+void x86emuOp2_bt_R(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    int bit,disp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("BT\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval;
+	    u32 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    disp = (s16)*shiftreg >> 5;
+	    srcval = fetch_data_long(srcoffset+disp);
+	    CONDITIONAL_SET_FLAG(srcval & (0x1 << bit),F_CF);
+	} else {
+	    u16 srcval;
+	    u16 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    disp = (s16)*shiftreg >> 4;
+	    srcval = fetch_data_word(srcoffset+disp);
+	    CONDITIONAL_SET_FLAG(srcval & (0x1 << bit),F_CF);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg,*shiftreg;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    CONDITIONAL_SET_FLAG(*srcreg & (0x1 << bit),F_CF);
+	} else {
+	    u16 *srcreg,*shiftreg;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    CONDITIONAL_SET_FLAG(*srcreg & (0x1 << bit),F_CF);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa4
+****************************************************************************/
+void x86emuOp2_shld_IMM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint destoffset;
+    u8 shift;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("SHLD\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+	    u32 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    destval = fetch_data_long(destoffset);
+	    destval = shld_long(destval,*shiftreg,shift);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+	    u16 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    destval = fetch_data_word(destoffset);
+	    destval = shld_word(destval,*shiftreg,shift);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    *destreg = shld_long(*destreg,*shiftreg,shift);
+	} else {
+	    u16 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    *destreg = shld_word(*destreg,*shiftreg,shift);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa5
+****************************************************************************/
+void x86emuOp2_shld_CL(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("SHLD\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+	    u32 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    destval = fetch_data_long(destoffset);
+	    destval = shld_long(destval,*shiftreg,M.x86.R_CL);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+	    u16 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    destval = fetch_data_word(destoffset);
+	    destval = shld_word(destval,*shiftreg,M.x86.R_CL);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    *destreg = shld_long(*destreg,*shiftreg,M.x86.R_CL);
+	} else {
+	    u16 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    *destreg = shld_word(*destreg,*shiftreg,M.x86.R_CL);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa8
+****************************************************************************/
+void x86emuOp2_push_GS(u8 X86EMU_UNUSED(op2))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("PUSH\tGS\n");
+    TRACE_AND_STEP();
+    push_word(M.x86.R_GS);
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xa9
+****************************************************************************/
+void x86emuOp2_pop_GS(u8 X86EMU_UNUSED(op2))
+{
+    START_OF_INSTR();
+    DECODE_PRINTF("POP\tGS\n");
+    TRACE_AND_STEP();
+    M.x86.R_GS = pop_word();
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xaa
+****************************************************************************/
+void x86emuOp2_bts_R(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    int bit,disp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("BTS\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval,mask;
+	    u32 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    disp = (s16)*shiftreg >> 5;
+	    srcval = fetch_data_long(srcoffset+disp);
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    store_data_long(srcoffset+disp, srcval | mask);
+	} else {
+	    u16 srcval,mask;
+	    u16 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    disp = (s16)*shiftreg >> 4;
+	    srcval = fetch_data_word(srcoffset+disp);
+	    mask = (u16)(0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    store_data_word(srcoffset+disp, srcval | mask);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg,*shiftreg;
+	    u32 mask;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    *srcreg |= mask;
+	} else {
+	    u16 *srcreg,*shiftreg;
+	    u16 mask;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    mask = (u16)(0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    *srcreg |= mask;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xac
+****************************************************************************/
+void x86emuOp2_shrd_IMM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint destoffset;
+    u8 shift;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("SHLD\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+	    u32 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    destval = fetch_data_long(destoffset);
+	    destval = shrd_long(destval,*shiftreg,shift);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+	    u16 *shiftreg;
+
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    destval = fetch_data_word(destoffset);
+	    destval = shrd_word(destval,*shiftreg,shift);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    *destreg = shrd_long(*destreg,*shiftreg,shift);
+	} else {
+	    u16 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2("%d\n", shift);
+	    TRACE_AND_STEP();
+	    *destreg = shrd_word(*destreg,*shiftreg,shift);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xad
+****************************************************************************/
+void x86emuOp2_shrd_CL(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint destoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("SHLD\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 destval;
+	    u32 *shiftreg;
+
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    destval = fetch_data_long(destoffset);
+	    destval = shrd_long(destval,*shiftreg,M.x86.R_CL);
+	    store_data_long(destoffset, destval);
+	} else {
+	    u16 destval;
+	    u16 *shiftreg;
+
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    destval = fetch_data_word(destoffset);
+	    destval = shrd_word(destval,*shiftreg,M.x86.R_CL);
+	    store_data_word(destoffset, destval);
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    *destreg = shrd_long(*destreg,*shiftreg,M.x86.R_CL);
+	} else {
+	    u16 *destreg,*shiftreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",CL\n");
+	    TRACE_AND_STEP();
+	    *destreg = shrd_word(*destreg,*shiftreg,M.x86.R_CL);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xaf
+****************************************************************************/
+void x86emuOp2_imul_R_RM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("IMUL\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+	    u32 res_lo,res_hi;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = fetch_data_long(srcoffset);
+	    TRACE_AND_STEP();
+	    imul_long_direct(&res_lo,&res_hi,(s32)*destreg,(s32)srcval);
+	    if (res_hi != 0) {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    } else {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    }
+	    *destreg = (u32)res_lo;
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+	    u32 res;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = fetch_data_word(srcoffset);
+	    TRACE_AND_STEP();
+	    res = (s16)*destreg * (s16)srcval;
+	    if (res > 0xFFFF) {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    } else {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    }
+	    *destreg = (u16)res;
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg,*srcreg;
+	    u32 res_lo,res_hi;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    TRACE_AND_STEP();
+	    imul_long_direct(&res_lo,&res_hi,(s32)*destreg,(s32)*srcreg);
+	    if (res_hi != 0) {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    } else {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    }
+	    *destreg = (u32)res_lo;
+	} else {
+	    u16 *destreg,*srcreg;
+	    u32 res;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    res = (s16)*destreg * (s16)*srcreg;
+	    if (res > 0xFFFF) {
+		SET_FLAG(F_CF);
+		SET_FLAG(F_OF);
+	    } else {
+		CLEAR_FLAG(F_CF);
+		CLEAR_FLAG(F_OF);
+	    }
+	    *destreg = (u16)res;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xb2
+****************************************************************************/
+void x86emuOp2_lss_R_IMM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rh, rl;
+    u16 *dstreg;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LSS\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	dstreg = DECODE_RM_WORD_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*dstreg = fetch_data_word(srcoffset);
+	M.x86.R_SS = fetch_data_word(srcoffset + 2);
+    } else {			 /* register to register */
+	/* UNDEFINED! */
+	TRACE_AND_STEP();
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xb3
+****************************************************************************/
+void x86emuOp2_btr_R(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    int bit,disp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("BTR\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval,mask;
+	    u32 *shiftreg;
+
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    disp = (s16)*shiftreg >> 5;
+	    srcval = fetch_data_long(srcoffset+disp);
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    store_data_long(srcoffset+disp, srcval & ~mask);
+	} else {
+	    u16 srcval,mask;
+	    u16 *shiftreg;
+
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    disp = (s16)*shiftreg >> 4;
+	    srcval = fetch_data_word(srcoffset+disp);
+	    mask = (u16)(0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    store_data_word(srcoffset+disp, (u16)(srcval & ~mask));
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg,*shiftreg;
+	    u32 mask;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    *srcreg &= ~mask;
+	} else {
+	    u16 *srcreg,*shiftreg;
+	    u16 mask;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    mask = (u16)(0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    *srcreg &= ~mask;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xb4
+****************************************************************************/
+void x86emuOp2_lfs_R_IMM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rh, rl;
+    u16 *dstreg;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LFS\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	dstreg = DECODE_RM_WORD_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*dstreg = fetch_data_word(srcoffset);
+	M.x86.R_FS = fetch_data_word(srcoffset + 2);
+    } else {			 /* register to register */
+	/* UNDEFINED! */
+	TRACE_AND_STEP();
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xb5
+****************************************************************************/
+void x86emuOp2_lgs_R_IMM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rh, rl;
+    u16 *dstreg;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("LGS\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	dstreg = DECODE_RM_WORD_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*dstreg = fetch_data_word(srcoffset);
+	M.x86.R_GS = fetch_data_word(srcoffset + 2);
+    } else {			 /* register to register */
+	/* UNDEFINED! */
+	TRACE_AND_STEP();
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xb6
+****************************************************************************/
+void x86emuOp2_movzx_byte_R_RM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOVZX\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = fetch_data_byte(srcoffset);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = srcval;
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = fetch_data_byte(srcoffset);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = srcval;
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u8	*srcreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_BYTE_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = *srcreg;
+	} else {
+	    u16 *destreg;
+	    u8	*srcreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_BYTE_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = *srcreg;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xb7
+****************************************************************************/
+void x86emuOp2_movzx_word_R_RM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    u32 *destreg;
+    u32 srcval;
+    u16 *srcreg;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOVZX\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destreg = DECODE_RM_LONG_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	srcval = fetch_data_word(srcoffset);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = srcval;
+    } else {			 /* register to register */
+	destreg = DECODE_RM_LONG_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_WORD_REGISTER(rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = *srcreg;
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xba
+****************************************************************************/
+void x86emuOp2_btX_I(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    u8 shift;
+    int bit;
+
+    START_OF_INSTR();
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    switch (rh) {
+    case 4:
+	DECODE_PRINTF("BT\t");
+	break;
+    case 5:
+	DECODE_PRINTF("BTS\t");
+	break;
+    case 6:
+	DECODE_PRINTF("BTR\t");
+	break;
+    case 7:
+	DECODE_PRINTF("BTC\t");
+	break;
+    default:
+	DECODE_PRINTF("ILLEGAL EXTENDED X86 OPCODE\n");
+	TRACE_REGS();
+	printk("%04x:%04x: %02X%02X ILLEGAL EXTENDED X86 OPCODE EXTENSION!\n",
+		M.x86.R_CS, M.x86.R_IP-3,op2, (mod<<6)|(rh<<3)|rl);
+	HALT_SYS();
+    }
+    if (mod < 3) {
+
+	srcoffset = decode_rmXX_address(mod, rl);
+	shift = fetch_byte_imm();
+	DECODE_PRINTF2(",%d\n", shift);
+	TRACE_AND_STEP();
+
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval, mask;
+
+	    bit = shift & 0x1F;
+	    srcval = fetch_data_long(srcoffset);
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    switch (rh) {
+	    case 5:
+		store_data_long(srcoffset, srcval | mask);
+		break;
+	    case 6:
+		store_data_long(srcoffset, srcval & ~mask);
+		break;
+	    case 7:
+		store_data_long(srcoffset, srcval ^ mask);
+		break;
+	    default:
+		break;
+	    }
+	} else {
+	    u16 srcval, mask;
+
+	    bit = shift & 0xF;
+	    srcval = fetch_data_word(srcoffset);
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    switch (rh) {
+	    case 5:
+		store_data_word(srcoffset, srcval | mask);
+		break;
+	    case 6:
+		store_data_word(srcoffset, srcval & ~mask);
+		break;
+	    case 7:
+		store_data_word(srcoffset, srcval ^ mask);
+		break;
+	    default:
+		break;
+	    }
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg;
+	    u32 mask;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2(",%d\n", shift);
+	    TRACE_AND_STEP();
+	    bit = shift & 0x1F;
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    switch (rh) {
+	    case 5:
+		*srcreg |= mask;
+		break;
+	    case 6:
+		*srcreg &= ~mask;
+		break;
+	    case 7:
+		*srcreg ^= mask;
+		break;
+	    default:
+		break;
+	    }
+	} else {
+	    u16 *srcreg;
+	    u16 mask;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    shift = fetch_byte_imm();
+	    DECODE_PRINTF2(",%d\n", shift);
+	    TRACE_AND_STEP();
+	    bit = shift & 0xF;
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    switch (rh) {
+	    case 5:
+		*srcreg |= mask;
+		break;
+	    case 6:
+		*srcreg &= ~mask;
+		break;
+	    case 7:
+		*srcreg ^= mask;
+		break;
+	    default:
+		break;
+	    }
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xbb
+****************************************************************************/
+void x86emuOp2_btc_R(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    int bit,disp;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("BTC\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval,mask;
+	    u32 *shiftreg;
+
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    disp = (s16)*shiftreg >> 5;
+	    srcval = fetch_data_long(srcoffset+disp);
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    store_data_long(srcoffset+disp, srcval ^ mask);
+	} else {
+	    u16 srcval,mask;
+	    u16 *shiftreg;
+
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    disp = (s16)*shiftreg >> 4;
+	    srcval = fetch_data_word(srcoffset+disp);
+	    mask = (u16)(0x1 << bit);
+	    CONDITIONAL_SET_FLAG(srcval & mask,F_CF);
+	    store_data_word(srcoffset+disp, (u16)(srcval ^ mask));
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg,*shiftreg;
+	    u32 mask;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0x1F;
+	    mask = (0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    *srcreg ^= mask;
+	} else {
+	    u16 *srcreg,*shiftreg;
+	    u16 mask;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    shiftreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    bit = *shiftreg & 0xF;
+	    mask = (u16)(0x1 << bit);
+	    CONDITIONAL_SET_FLAG(*srcreg & mask,F_CF);
+	    *srcreg ^= mask;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xbc
+****************************************************************************/
+void x86emuOp2_bsf(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("BSF\n");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval, *dstreg;
+
+	    dstreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    srcval = fetch_data_long(srcoffset);
+	    CONDITIONAL_SET_FLAG(srcval == 0, F_ZF);
+	    for(*dstreg = 0; *dstreg < 32; (*dstreg)++)
+		if ((srcval >> *dstreg) & 1) break;
+	} else {
+	    u16 srcval, *dstreg;
+
+	    dstreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    srcval = fetch_data_word(srcoffset);
+	    CONDITIONAL_SET_FLAG(srcval == 0, F_ZF);
+	    for(*dstreg = 0; *dstreg < 16; (*dstreg)++)
+		if ((srcval >> *dstreg) & 1) break;
+	}
+    } else {		 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg, *dstreg;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    dstreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    CONDITIONAL_SET_FLAG(*srcreg == 0, F_ZF);
+	    for(*dstreg = 0; *dstreg < 32; (*dstreg)++)
+		if ((*srcreg >> *dstreg) & 1) break;
+	} else {
+	    u16 *srcreg, *dstreg;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    dstreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    CONDITIONAL_SET_FLAG(*srcreg == 0, F_ZF);
+	    for(*dstreg = 0; *dstreg < 16; (*dstreg)++)
+		if ((*srcreg >> *dstreg) & 1) break;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xbd
+****************************************************************************/
+void x86emuOp2_bsr(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("BSF\n");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	srcoffset = decode_rmXX_address(mod, rl);
+	DECODE_PRINTF(",");
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 srcval, *dstreg;
+
+	    dstreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    srcval = fetch_data_long(srcoffset);
+	    CONDITIONAL_SET_FLAG(srcval == 0, F_ZF);
+	    for(*dstreg = 31; *dstreg > 0; (*dstreg)--)
+		if ((srcval >> *dstreg) & 1) break;
+	} else {
+	    u16 srcval, *dstreg;
+
+	    dstreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    srcval = fetch_data_word(srcoffset);
+	    CONDITIONAL_SET_FLAG(srcval == 0, F_ZF);
+	    for(*dstreg = 15; *dstreg > 0; (*dstreg)--)
+		if ((srcval >> *dstreg) & 1) break;
+	}
+    } else {		 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *srcreg, *dstreg;
+
+	    srcreg = DECODE_RM_LONG_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    dstreg = DECODE_RM_LONG_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    CONDITIONAL_SET_FLAG(*srcreg == 0, F_ZF);
+	    for(*dstreg = 31; *dstreg > 0; (*dstreg)--)
+		if ((*srcreg >> *dstreg) & 1) break;
+	} else {
+	    u16 *srcreg, *dstreg;
+
+	    srcreg = DECODE_RM_WORD_REGISTER(rl);
+	    DECODE_PRINTF(",");
+	    dstreg = DECODE_RM_WORD_REGISTER(rh);
+	    TRACE_AND_STEP();
+	    CONDITIONAL_SET_FLAG(*srcreg == 0, F_ZF);
+	    for(*dstreg = 15; *dstreg > 0; (*dstreg)--)
+		if ((*srcreg >> *dstreg) & 1) break;
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xbe
+****************************************************************************/
+void x86emuOp2_movsx_byte_R_RM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOVSX\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u32 srcval;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = (s32)((s8)fetch_data_byte(srcoffset));
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = srcval;
+	} else {
+	    u16 *destreg;
+	    u16 srcval;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcoffset = decode_rmXX_address(mod, rl);
+	    srcval = (s16)((s8)fetch_data_byte(srcoffset));
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = srcval;
+	}
+    } else {			 /* register to register */
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    u32 *destreg;
+	    u8	*srcreg;
+
+	    destreg = DECODE_RM_LONG_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_BYTE_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = (s32)((s8)*srcreg);
+	} else {
+	    u16 *destreg;
+	    u8	*srcreg;
+
+	    destreg = DECODE_RM_WORD_REGISTER(rh);
+	    DECODE_PRINTF(",");
+	    srcreg = DECODE_RM_BYTE_REGISTER(rl);
+	    DECODE_PRINTF("\n");
+	    TRACE_AND_STEP();
+	    *destreg = (s16)((s8)*srcreg);
+	}
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/****************************************************************************
+REMARKS:
+Handles opcode 0x0f,0xbf
+****************************************************************************/
+void x86emuOp2_movsx_word_R_RM(u8 X86EMU_UNUSED(op2))
+{
+    int mod, rl, rh;
+    uint srcoffset;
+    u32 *destreg;
+    u32 srcval;
+    u16 *srcreg;
+
+    START_OF_INSTR();
+    DECODE_PRINTF("MOVSX\t");
+    FETCH_DECODE_MODRM(mod, rh, rl);
+    if (mod < 3) {
+	destreg = DECODE_RM_LONG_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcoffset = decode_rmXX_address(mod, rl);
+	srcval = (s32)((s16)fetch_data_word(srcoffset));
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = srcval;
+    } else {			 /* register to register */
+	destreg = DECODE_RM_LONG_REGISTER(rh);
+	DECODE_PRINTF(",");
+	srcreg = DECODE_RM_WORD_REGISTER(rl);
+	DECODE_PRINTF("\n");
+	TRACE_AND_STEP();
+	*destreg = (s32)((s16)*srcreg);
+    }
+    DECODE_CLEAR_SEGOVR();
+    END_OF_INSTR();
+}
+
+/***************************************************************************
+ * Double byte operation code table:
+ **************************************************************************/
+void (*x86emu_optab2[256])(u8) __attribute__((section(".got2"))) =
+{
+/*  0x00 */ x86emuOp2_illegal_op,  /* Group F (ring 0 PM)      */
+/*  0x01 */ x86emuOp2_illegal_op,  /* Group G (ring 0 PM)      */
+/*  0x02 */ x86emuOp2_illegal_op,  /* lar (ring 0 PM)	       */
+/*  0x03 */ x86emuOp2_illegal_op,  /* lsl (ring 0 PM)	       */
+/*  0x04 */ x86emuOp2_illegal_op,
+/*  0x05 */ x86emuOp2_illegal_op,  /* loadall (undocumented)   */
+/*  0x06 */ x86emuOp2_illegal_op,  /* clts (ring 0 PM)	       */
+/*  0x07 */ x86emuOp2_illegal_op,  /* loadall (undocumented)   */
+/*  0x08 */ x86emuOp2_illegal_op,  /* invd (ring 0 PM)	       */
+/*  0x09 */ x86emuOp2_illegal_op,  /* wbinvd (ring 0 PM)       */
+/*  0x0a */ x86emuOp2_illegal_op,
+/*  0x0b */ x86emuOp2_illegal_op,
+/*  0x0c */ x86emuOp2_illegal_op,
+/*  0x0d */ x86emuOp2_illegal_op,
+/*  0x0e */ x86emuOp2_illegal_op,
+/*  0x0f */ x86emuOp2_illegal_op,
+
+/*  0x10 */ x86emuOp2_illegal_op,
+/*  0x11 */ x86emuOp2_illegal_op,
+/*  0x12 */ x86emuOp2_illegal_op,
+/*  0x13 */ x86emuOp2_illegal_op,
+/*  0x14 */ x86emuOp2_illegal_op,
+/*  0x15 */ x86emuOp2_illegal_op,
+/*  0x16 */ x86emuOp2_illegal_op,
+/*  0x17 */ x86emuOp2_illegal_op,
+/*  0x18 */ x86emuOp2_illegal_op,
+/*  0x19 */ x86emuOp2_illegal_op,
+/*  0x1a */ x86emuOp2_illegal_op,
+/*  0x1b */ x86emuOp2_illegal_op,
+/*  0x1c */ x86emuOp2_illegal_op,
+/*  0x1d */ x86emuOp2_illegal_op,
+/*  0x1e */ x86emuOp2_illegal_op,
+/*  0x1f */ x86emuOp2_illegal_op,
+
+/*  0x20 */ x86emuOp2_illegal_op,  /* mov reg32,creg (ring 0 PM) */
+/*  0x21 */ x86emuOp2_illegal_op,  /* mov reg32,dreg (ring 0 PM) */
+/*  0x22 */ x86emuOp2_illegal_op,  /* mov creg,reg32 (ring 0 PM) */
+/*  0x23 */ x86emuOp2_illegal_op,  /* mov dreg,reg32 (ring 0 PM) */
+/*  0x24 */ x86emuOp2_illegal_op,  /* mov reg32,treg (ring 0 PM) */
+/*  0x25 */ x86emuOp2_illegal_op,
+/*  0x26 */ x86emuOp2_illegal_op,  /* mov treg,reg32 (ring 0 PM) */
+/*  0x27 */ x86emuOp2_illegal_op,
+/*  0x28 */ x86emuOp2_illegal_op,
+/*  0x29 */ x86emuOp2_illegal_op,
+/*  0x2a */ x86emuOp2_illegal_op,
+/*  0x2b */ x86emuOp2_illegal_op,
+/*  0x2c */ x86emuOp2_illegal_op,
+/*  0x2d */ x86emuOp2_illegal_op,
+/*  0x2e */ x86emuOp2_illegal_op,
+/*  0x2f */ x86emuOp2_illegal_op,
+
+/*  0x30 */ x86emuOp2_illegal_op,
+/*  0x31 */ x86emuOp2_illegal_op,
+/*  0x32 */ x86emuOp2_illegal_op,
+/*  0x33 */ x86emuOp2_illegal_op,
+/*  0x34 */ x86emuOp2_illegal_op,
+/*  0x35 */ x86emuOp2_illegal_op,
+/*  0x36 */ x86emuOp2_illegal_op,
+/*  0x37 */ x86emuOp2_illegal_op,
+/*  0x38 */ x86emuOp2_illegal_op,
+/*  0x39 */ x86emuOp2_illegal_op,
+/*  0x3a */ x86emuOp2_illegal_op,
+/*  0x3b */ x86emuOp2_illegal_op,
+/*  0x3c */ x86emuOp2_illegal_op,
+/*  0x3d */ x86emuOp2_illegal_op,
+/*  0x3e */ x86emuOp2_illegal_op,
+/*  0x3f */ x86emuOp2_illegal_op,
+
+/*  0x40 */ x86emuOp2_illegal_op,
+/*  0x41 */ x86emuOp2_illegal_op,
+/*  0x42 */ x86emuOp2_illegal_op,
+/*  0x43 */ x86emuOp2_illegal_op,
+/*  0x44 */ x86emuOp2_illegal_op,
+/*  0x45 */ x86emuOp2_illegal_op,
+/*  0x46 */ x86emuOp2_illegal_op,
+/*  0x47 */ x86emuOp2_illegal_op,
+/*  0x48 */ x86emuOp2_illegal_op,
+/*  0x49 */ x86emuOp2_illegal_op,
+/*  0x4a */ x86emuOp2_illegal_op,
+/*  0x4b */ x86emuOp2_illegal_op,
+/*  0x4c */ x86emuOp2_illegal_op,
+/*  0x4d */ x86emuOp2_illegal_op,
+/*  0x4e */ x86emuOp2_illegal_op,
+/*  0x4f */ x86emuOp2_illegal_op,
+
+/*  0x50 */ x86emuOp2_illegal_op,
+/*  0x51 */ x86emuOp2_illegal_op,
+/*  0x52 */ x86emuOp2_illegal_op,
+/*  0x53 */ x86emuOp2_illegal_op,
+/*  0x54 */ x86emuOp2_illegal_op,
+/*  0x55 */ x86emuOp2_illegal_op,
+/*  0x56 */ x86emuOp2_illegal_op,
+/*  0x57 */ x86emuOp2_illegal_op,
+/*  0x58 */ x86emuOp2_illegal_op,
+/*  0x59 */ x86emuOp2_illegal_op,
+/*  0x5a */ x86emuOp2_illegal_op,
+/*  0x5b */ x86emuOp2_illegal_op,
+/*  0x5c */ x86emuOp2_illegal_op,
+/*  0x5d */ x86emuOp2_illegal_op,
+/*  0x5e */ x86emuOp2_illegal_op,
+/*  0x5f */ x86emuOp2_illegal_op,
+
+/*  0x60 */ x86emuOp2_illegal_op,
+/*  0x61 */ x86emuOp2_illegal_op,
+/*  0x62 */ x86emuOp2_illegal_op,
+/*  0x63 */ x86emuOp2_illegal_op,
+/*  0x64 */ x86emuOp2_illegal_op,
+/*  0x65 */ x86emuOp2_illegal_op,
+/*  0x66 */ x86emuOp2_illegal_op,
+/*  0x67 */ x86emuOp2_illegal_op,
+/*  0x68 */ x86emuOp2_illegal_op,
+/*  0x69 */ x86emuOp2_illegal_op,
+/*  0x6a */ x86emuOp2_illegal_op,
+/*  0x6b */ x86emuOp2_illegal_op,
+/*  0x6c */ x86emuOp2_illegal_op,
+/*  0x6d */ x86emuOp2_illegal_op,
+/*  0x6e */ x86emuOp2_illegal_op,
+/*  0x6f */ x86emuOp2_illegal_op,
+
+/*  0x70 */ x86emuOp2_illegal_op,
+/*  0x71 */ x86emuOp2_illegal_op,
+/*  0x72 */ x86emuOp2_illegal_op,
+/*  0x73 */ x86emuOp2_illegal_op,
+/*  0x74 */ x86emuOp2_illegal_op,
+/*  0x75 */ x86emuOp2_illegal_op,
+/*  0x76 */ x86emuOp2_illegal_op,
+/*  0x77 */ x86emuOp2_illegal_op,
+/*  0x78 */ x86emuOp2_illegal_op,
+/*  0x79 */ x86emuOp2_illegal_op,
+/*  0x7a */ x86emuOp2_illegal_op,
+/*  0x7b */ x86emuOp2_illegal_op,
+/*  0x7c */ x86emuOp2_illegal_op,
+/*  0x7d */ x86emuOp2_illegal_op,
+/*  0x7e */ x86emuOp2_illegal_op,
+/*  0x7f */ x86emuOp2_illegal_op,
+
+/*  0x80 */ x86emuOp2_long_jump,
+/*  0x81 */ x86emuOp2_long_jump,
+/*  0x82 */ x86emuOp2_long_jump,
+/*  0x83 */ x86emuOp2_long_jump,
+/*  0x84 */ x86emuOp2_long_jump,
+/*  0x85 */ x86emuOp2_long_jump,
+/*  0x86 */ x86emuOp2_long_jump,
+/*  0x87 */ x86emuOp2_long_jump,
+/*  0x88 */ x86emuOp2_long_jump,
+/*  0x89 */ x86emuOp2_long_jump,
+/*  0x8a */ x86emuOp2_long_jump,
+/*  0x8b */ x86emuOp2_long_jump,
+/*  0x8c */ x86emuOp2_long_jump,
+/*  0x8d */ x86emuOp2_long_jump,
+/*  0x8e */ x86emuOp2_long_jump,
+/*  0x8f */ x86emuOp2_long_jump,
+
+/*  0x90 */ x86emuOp2_set_byte,
+/*  0x91 */ x86emuOp2_set_byte,
+/*  0x92 */ x86emuOp2_set_byte,
+/*  0x93 */ x86emuOp2_set_byte,
+/*  0x94 */ x86emuOp2_set_byte,
+/*  0x95 */ x86emuOp2_set_byte,
+/*  0x96 */ x86emuOp2_set_byte,
+/*  0x97 */ x86emuOp2_set_byte,
+/*  0x98 */ x86emuOp2_set_byte,
+/*  0x99 */ x86emuOp2_set_byte,
+/*  0x9a */ x86emuOp2_set_byte,
+/*  0x9b */ x86emuOp2_set_byte,
+/*  0x9c */ x86emuOp2_set_byte,
+/*  0x9d */ x86emuOp2_set_byte,
+/*  0x9e */ x86emuOp2_set_byte,
+/*  0x9f */ x86emuOp2_set_byte,
+
+/*  0xa0 */ x86emuOp2_push_FS,
+/*  0xa1 */ x86emuOp2_pop_FS,
+/*  0xa2 */ x86emuOp2_illegal_op,
+/*  0xa3 */ x86emuOp2_bt_R,
+/*  0xa4 */ x86emuOp2_shld_IMM,
+/*  0xa5 */ x86emuOp2_shld_CL,
+/*  0xa6 */ x86emuOp2_illegal_op,
+/*  0xa7 */ x86emuOp2_illegal_op,
+/*  0xa8 */ x86emuOp2_push_GS,
+/*  0xa9 */ x86emuOp2_pop_GS,
+/*  0xaa */ x86emuOp2_illegal_op,
+/*  0xab */ x86emuOp2_bt_R,
+/*  0xac */ x86emuOp2_shrd_IMM,
+/*  0xad */ x86emuOp2_shrd_CL,
+/*  0xae */ x86emuOp2_illegal_op,
+/*  0xaf */ x86emuOp2_imul_R_RM,
+
+/*  0xb0 */ x86emuOp2_illegal_op,  /* TODO: cmpxchg */
+/*  0xb1 */ x86emuOp2_illegal_op,  /* TODO: cmpxchg */
+/*  0xb2 */ x86emuOp2_lss_R_IMM,
+/*  0xb3 */ x86emuOp2_btr_R,
+/*  0xb4 */ x86emuOp2_lfs_R_IMM,
+/*  0xb5 */ x86emuOp2_lgs_R_IMM,
+/*  0xb6 */ x86emuOp2_movzx_byte_R_RM,
+/*  0xb7 */ x86emuOp2_movzx_word_R_RM,
+/*  0xb8 */ x86emuOp2_illegal_op,
+/*  0xb9 */ x86emuOp2_illegal_op,
+/*  0xba */ x86emuOp2_btX_I,
+/*  0xbb */ x86emuOp2_btc_R,
+/*  0xbc */ x86emuOp2_bsf,
+/*  0xbd */ x86emuOp2_bsr,
+/*  0xbe */ x86emuOp2_movsx_byte_R_RM,
+/*  0xbf */ x86emuOp2_movsx_word_R_RM,
+
+/*  0xc0 */ x86emuOp2_illegal_op,  /* TODO: xadd */
+/*  0xc1 */ x86emuOp2_illegal_op,  /* TODO: xadd */
+/*  0xc2 */ x86emuOp2_illegal_op,
+/*  0xc3 */ x86emuOp2_illegal_op,
+/*  0xc4 */ x86emuOp2_illegal_op,
+/*  0xc5 */ x86emuOp2_illegal_op,
+/*  0xc6 */ x86emuOp2_illegal_op,
+/*  0xc7 */ x86emuOp2_illegal_op,
+/*  0xc8 */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xc9 */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xca */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xcb */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xcc */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xcd */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xce */ x86emuOp2_illegal_op,  /* TODO: bswap */
+/*  0xcf */ x86emuOp2_illegal_op,  /* TODO: bswap */
+
+/*  0xd0 */ x86emuOp2_illegal_op,
+/*  0xd1 */ x86emuOp2_illegal_op,
+/*  0xd2 */ x86emuOp2_illegal_op,
+/*  0xd3 */ x86emuOp2_illegal_op,
+/*  0xd4 */ x86emuOp2_illegal_op,
+/*  0xd5 */ x86emuOp2_illegal_op,
+/*  0xd6 */ x86emuOp2_illegal_op,
+/*  0xd7 */ x86emuOp2_illegal_op,
+/*  0xd8 */ x86emuOp2_illegal_op,
+/*  0xd9 */ x86emuOp2_illegal_op,
+/*  0xda */ x86emuOp2_illegal_op,
+/*  0xdb */ x86emuOp2_illegal_op,
+/*  0xdc */ x86emuOp2_illegal_op,
+/*  0xdd */ x86emuOp2_illegal_op,
+/*  0xde */ x86emuOp2_illegal_op,
+/*  0xdf */ x86emuOp2_illegal_op,
+
+/*  0xe0 */ x86emuOp2_illegal_op,
+/*  0xe1 */ x86emuOp2_illegal_op,
+/*  0xe2 */ x86emuOp2_illegal_op,
+/*  0xe3 */ x86emuOp2_illegal_op,
+/*  0xe4 */ x86emuOp2_illegal_op,
+/*  0xe5 */ x86emuOp2_illegal_op,
+/*  0xe6 */ x86emuOp2_illegal_op,
+/*  0xe7 */ x86emuOp2_illegal_op,
+/*  0xe8 */ x86emuOp2_illegal_op,
+/*  0xe9 */ x86emuOp2_illegal_op,
+/*  0xea */ x86emuOp2_illegal_op,
+/*  0xeb */ x86emuOp2_illegal_op,
+/*  0xec */ x86emuOp2_illegal_op,
+/*  0xed */ x86emuOp2_illegal_op,
+/*  0xee */ x86emuOp2_illegal_op,
+/*  0xef */ x86emuOp2_illegal_op,
+
+/*  0xf0 */ x86emuOp2_illegal_op,
+/*  0xf1 */ x86emuOp2_illegal_op,
+/*  0xf2 */ x86emuOp2_illegal_op,
+/*  0xf3 */ x86emuOp2_illegal_op,
+/*  0xf4 */ x86emuOp2_illegal_op,
+/*  0xf5 */ x86emuOp2_illegal_op,
+/*  0xf6 */ x86emuOp2_illegal_op,
+/*  0xf7 */ x86emuOp2_illegal_op,
+/*  0xf8 */ x86emuOp2_illegal_op,
+/*  0xf9 */ x86emuOp2_illegal_op,
+/*  0xfa */ x86emuOp2_illegal_op,
+/*  0xfb */ x86emuOp2_illegal_op,
+/*  0xfc */ x86emuOp2_illegal_op,
+/*  0xfd */ x86emuOp2_illegal_op,
+/*  0xfe */ x86emuOp2_illegal_op,
+/*  0xff */ x86emuOp2_illegal_op,
+};
+
+#endif
diff --git a/drivers/bios_emulator/x86emu/prim_ops.c b/drivers/bios_emulator/x86emu/prim_ops.c
new file mode 100644
index 00000000000..c1152eae341
--- /dev/null
+++ b/drivers/bios_emulator/x86emu/prim_ops.c
@@ -0,0 +1,2449 @@
+/****************************************************************************
+*
+*			Realmode X86 Emulator Library
+*
+*		Copyright (C) 1991-2004 SciTech Software, Inc.
+*		     Copyright (C) David Mosberger-Tang
+*		       Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.	The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:	ANSI C
+* Environment:	Any
+* Developer:	Kendall Bennett
+*
+* Description:	This file contains the code to implement the primitive
+*		machine operations used by the emulation code in ops.c
+*
+* Carry Chain Calculation
+*
+* This represents a somewhat expensive calculation which is
+* apparently required to emulate the setting of the OF343364 and AF flag.
+* The latter is not so important, but the former is.  The overflow
+* flag is the XOR of the top two bits of the carry chain for an
+* addition (similar for subtraction).  Since we do not want to
+* simulate the addition in a bitwise manner, we try to calculate the
+* carry chain given the two operands and the result.
+*
+* So, given the following table, which represents the addition of two
+* bits, we can derive a formula for the carry chain.
+*
+* a   b	  cin	r     cout
+* 0   0	  0	0     0
+* 0   0	  1	1     0
+* 0   1	  0	1     0
+* 0   1	  1	0     1
+* 1   0	  0	1     0
+* 1   0	  1	0     1
+* 1   1	  0	0     1
+* 1   1	  1	1     1
+*
+* Construction of table for cout:
+*
+* ab
+* r  \	00   01	  11  10
+* |------------------
+* 0  |	 0    1	   1   1
+* 1  |	 0    0	   1   0
+*
+* By inspection, one gets:  cc = ab +  r'(a + b)
+*
+* That represents alot of operations, but NO CHOICE....
+*
+* Borrow Chain Calculation.
+*
+* The following table represents the subtraction of two bits, from
+* which we can derive a formula for the borrow chain.
+*
+* a   b	  bin	r     bout
+* 0   0	  0	0     0
+* 0   0	  1	1     1
+* 0   1	  0	1     1
+* 0   1	  1	0     1
+* 1   0	  0	1     0
+* 1   0	  1	0     0
+* 1   1	  0	0     0
+* 1   1	  1	1     1
+*
+* Construction of table for cout:
+*
+* ab
+* r  \	00   01	  11  10
+* |------------------
+* 0  |	 0    1	   0   0
+* 1  |	 1    1	   1   0
+*
+* By inspection, one gets:  bc = a'b +	r(a' + b)
+*
+****************************************************************************/
+
+#define PRIM_OPS_NO_REDEFINE_ASM
+#include "x86emu/x86emui.h"
+
+#if defined(CONFIG_BIOSEMU)
+
+/*------------------------- Global Variables ------------------------------*/
+
+static u32 x86emu_parity_tab[8] =
+{
+    0x96696996,
+    0x69969669,
+    0x69969669,
+    0x96696996,
+    0x69969669,
+    0x96696996,
+    0x96696996,
+    0x69969669,
+};
+
+#define PARITY(x)   (((x86emu_parity_tab[(x) / 32] >> ((x) % 32)) & 1) == 0)
+#define XOR2(x)	    (((x) ^ ((x)>>1)) & 0x1)
+/*----------------------------- Implementation ----------------------------*/
+int abs(int v)
+{
+	return (v>0)?v:-v;
+}
+
+/*----------------------------- Implementation ----------------------------*/
+
+
+/*--------- Side effects helper functions -------*/
+
+/****************************************************************************
+REMARKS:
+implements side efects for byte operations that don't overflow
+****************************************************************************/
+
+static void set_parity_flag(u32 res)
+{
+    CONDITIONAL_SET_FLAG(PARITY(res & 0xFF), F_PF);
+}
+
+static void set_szp_flags_8(u8 res)
+{
+    CONDITIONAL_SET_FLAG(res & 0x80, F_SF);
+    CONDITIONAL_SET_FLAG(res == 0, F_ZF);
+    set_parity_flag(res);
+}
+
+static void set_szp_flags_16(u16 res)
+{
+    CONDITIONAL_SET_FLAG(res & 0x8000, F_SF);
+    CONDITIONAL_SET_FLAG(res == 0, F_ZF);
+    set_parity_flag(res);
+}
+
+static void set_szp_flags_32(u32 res)
+{
+    CONDITIONAL_SET_FLAG(res & 0x80000000, F_SF);
+    CONDITIONAL_SET_FLAG(res == 0, F_ZF);
+    set_parity_flag(res);
+}
+
+static void no_carry_byte_side_eff(u8 res)
+{
+    CLEAR_FLAG(F_OF);
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_AF);
+    set_szp_flags_8(res);
+}
+
+static void no_carry_word_side_eff(u16 res)
+{
+    CLEAR_FLAG(F_OF);
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_AF);
+    set_szp_flags_16(res);
+}
+
+static void no_carry_long_side_eff(u32 res)
+{
+    CLEAR_FLAG(F_OF);
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_AF);
+    set_szp_flags_32(res);
+}
+
+static void calc_carry_chain(int bits, u32 d, u32 s, u32 res, int set_carry)
+{
+    u32 cc;
+
+    cc = (s & d) | ((~res) & (s | d));
+    CONDITIONAL_SET_FLAG(XOR2(cc >> (bits - 2)), F_OF);
+    CONDITIONAL_SET_FLAG(cc & 0x8, F_AF);
+    if (set_carry) {
+	CONDITIONAL_SET_FLAG(res & (1 << bits), F_CF);
+    }
+}
+
+static void calc_borrow_chain(int bits, u32 d, u32 s, u32 res, int set_carry)
+{
+    u32 bc;
+
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> (bits - 2)), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    if (set_carry) {
+	CONDITIONAL_SET_FLAG(bc & (1 << (bits - 1)), F_CF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AAA instruction and side effects.
+****************************************************************************/
+u16 aaa_word(u16 d)
+{
+    u16 res;
+    if ((d & 0xf) > 0x9 || ACCESS_FLAG(F_AF)) {
+	d += 0x6;
+	d += 0x100;
+	SET_FLAG(F_AF);
+	SET_FLAG(F_CF);
+    } else {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_AF);
+    }
+    res = (u16)(d & 0xFF0F);
+    set_szp_flags_16(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AAA instruction and side effects.
+****************************************************************************/
+u16 aas_word(u16 d)
+{
+    u16 res;
+    if ((d & 0xf) > 0x9 || ACCESS_FLAG(F_AF)) {
+	d -= 0x6;
+	d -= 0x100;
+	SET_FLAG(F_AF);
+	SET_FLAG(F_CF);
+    } else {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_AF);
+    }
+    res = (u16)(d & 0xFF0F);
+    set_szp_flags_16(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AAD instruction and side effects.
+****************************************************************************/
+u16 aad_word(u16 d)
+{
+    u16 l;
+    u8 hb, lb;
+
+    hb = (u8)((d >> 8) & 0xff);
+    lb = (u8)((d & 0xff));
+    l = (u16)((lb + 10 * hb) & 0xFF);
+
+    no_carry_byte_side_eff(l & 0xFF);
+    return l;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AAM instruction and side effects.
+****************************************************************************/
+u16 aam_word(u8 d)
+{
+    u16 h, l;
+
+    h = (u16)(d / 10);
+    l = (u16)(d % 10);
+    l |= (u16)(h << 8);
+
+    no_carry_byte_side_eff(l & 0xFF);
+    return l;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ADC instruction and side effects.
+****************************************************************************/
+u8 adc_byte(u8 d, u8 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + s;
+    if (ACCESS_FLAG(F_CF)) res++;
+
+    set_szp_flags_8(res);
+    calc_carry_chain(8,s,d,res,1);
+
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ADC instruction and side effects.
+****************************************************************************/
+u16 adc_word(u16 d, u16 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + s;
+    if (ACCESS_FLAG(F_CF))
+	res++;
+
+    set_szp_flags_16((u16)res);
+    calc_carry_chain(16,s,d,res,1);
+
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ADC instruction and side effects.
+****************************************************************************/
+u32 adc_long(u32 d, u32 s)
+{
+    u32 lo;    /* all operands in native machine order */
+    u32 hi;
+    u32 res;
+
+    lo = (d & 0xFFFF) + (s & 0xFFFF);
+    res = d + s;
+
+    if (ACCESS_FLAG(F_CF)) {
+	lo++;
+	res++;
+    }
+
+    hi = (lo >> 16) + (d >> 16) + (s >> 16);
+
+    set_szp_flags_32(res);
+    calc_carry_chain(32,s,d,res,0);
+
+    CONDITIONAL_SET_FLAG(hi & 0x10000, F_CF);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ADD instruction and side effects.
+****************************************************************************/
+u8 add_byte(u8 d, u8 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + s;
+    set_szp_flags_8((u8)res);
+    calc_carry_chain(8,s,d,res,1);
+
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ADD instruction and side effects.
+****************************************************************************/
+u16 add_word(u16 d, u16 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + s;
+    set_szp_flags_16((u16)res);
+    calc_carry_chain(16,s,d,res,1);
+
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ADD instruction and side effects.
+****************************************************************************/
+u32 add_long(u32 d, u32 s)
+{
+    u32 res;
+
+    res = d + s;
+    set_szp_flags_32(res);
+    calc_carry_chain(32,s,d,res,0);
+
+    CONDITIONAL_SET_FLAG(res < d || res < s, F_CF);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AND instruction and side effects.
+****************************************************************************/
+u8 and_byte(u8 d, u8 s)
+{
+    u8 res;    /* all operands in native machine order */
+
+    res = d & s;
+
+    no_carry_byte_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AND instruction and side effects.
+****************************************************************************/
+u16 and_word(u16 d, u16 s)
+{
+    u16 res;   /* all operands in native machine order */
+
+    res = d & s;
+
+    no_carry_word_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the AND instruction and side effects.
+****************************************************************************/
+u32 and_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d & s;
+    no_carry_long_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the CMP instruction and side effects.
+****************************************************************************/
+u8 cmp_byte(u8 d, u8 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d - s;
+    set_szp_flags_8((u8)res);
+    calc_borrow_chain(8, d, s, res, 1);
+
+    return d;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the CMP instruction and side effects.
+****************************************************************************/
+u16 cmp_word(u16 d, u16 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d - s;
+    set_szp_flags_16((u16)res);
+    calc_borrow_chain(16, d, s, res, 1);
+
+    return d;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the CMP instruction and side effects.
+****************************************************************************/
+u32 cmp_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d - s;
+    set_szp_flags_32(res);
+    calc_borrow_chain(32, d, s, res, 1);
+
+    return d;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DAA instruction and side effects.
+****************************************************************************/
+u8 daa_byte(u8 d)
+{
+    u32 res = d;
+    if ((d & 0xf) > 9 || ACCESS_FLAG(F_AF)) {
+	res += 6;
+	SET_FLAG(F_AF);
+    }
+    if (res > 0x9F || ACCESS_FLAG(F_CF)) {
+	res += 0x60;
+	SET_FLAG(F_CF);
+    }
+    set_szp_flags_8((u8)res);
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DAS instruction and side effects.
+****************************************************************************/
+u8 das_byte(u8 d)
+{
+    if ((d & 0xf) > 9 || ACCESS_FLAG(F_AF)) {
+	d -= 6;
+	SET_FLAG(F_AF);
+    }
+    if (d > 0x9F || ACCESS_FLAG(F_CF)) {
+	d -= 0x60;
+	SET_FLAG(F_CF);
+    }
+    set_szp_flags_8(d);
+    return d;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DEC instruction and side effects.
+****************************************************************************/
+u8 dec_byte(u8 d)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d - 1;
+    set_szp_flags_8((u8)res);
+    calc_borrow_chain(8, d, 1, res, 0);
+
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DEC instruction and side effects.
+****************************************************************************/
+u16 dec_word(u16 d)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d - 1;
+    set_szp_flags_16((u16)res);
+    calc_borrow_chain(16, d, 1, res, 0);
+
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DEC instruction and side effects.
+****************************************************************************/
+u32 dec_long(u32 d)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d - 1;
+
+    set_szp_flags_32(res);
+    calc_borrow_chain(32, d, 1, res, 0);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the INC instruction and side effects.
+****************************************************************************/
+u8 inc_byte(u8 d)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + 1;
+    set_szp_flags_8((u8)res);
+    calc_carry_chain(8, d, 1, res, 0);
+
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the INC instruction and side effects.
+****************************************************************************/
+u16 inc_word(u16 d)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + 1;
+    set_szp_flags_16((u16)res);
+    calc_carry_chain(16, d, 1, res, 0);
+
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the INC instruction and side effects.
+****************************************************************************/
+u32 inc_long(u32 d)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d + 1;
+    set_szp_flags_32(res);
+    calc_carry_chain(32, d, 1, res, 0);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OR instruction and side effects.
+****************************************************************************/
+u8 or_byte(u8 d, u8 s)
+{
+    u8 res;    /* all operands in native machine order */
+
+    res = d | s;
+    no_carry_byte_side_eff(res);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OR instruction and side effects.
+****************************************************************************/
+u16 or_word(u16 d, u16 s)
+{
+    u16 res;   /* all operands in native machine order */
+
+    res = d | s;
+    no_carry_word_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OR instruction and side effects.
+****************************************************************************/
+u32 or_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d | s;
+    no_carry_long_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OR instruction and side effects.
+****************************************************************************/
+u8 neg_byte(u8 s)
+{
+    u8 res;
+
+    CONDITIONAL_SET_FLAG(s != 0, F_CF);
+    res = (u8)-s;
+    set_szp_flags_8(res);
+    calc_borrow_chain(8, 0, s, res, 0);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OR instruction and side effects.
+****************************************************************************/
+u16 neg_word(u16 s)
+{
+    u16 res;
+
+    CONDITIONAL_SET_FLAG(s != 0, F_CF);
+    res = (u16)-s;
+    set_szp_flags_16((u16)res);
+    calc_borrow_chain(16, 0, s, res, 0);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OR instruction and side effects.
+****************************************************************************/
+u32 neg_long(u32 s)
+{
+    u32 res;
+
+    CONDITIONAL_SET_FLAG(s != 0, F_CF);
+    res = (u32)-s;
+    set_szp_flags_32(res);
+    calc_borrow_chain(32, 0, s, res, 0);
+
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the NOT instruction and side effects.
+****************************************************************************/
+u8 not_byte(u8 s)
+{
+    return ~s;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the NOT instruction and side effects.
+****************************************************************************/
+u16 not_word(u16 s)
+{
+    return ~s;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the NOT instruction and side effects.
+****************************************************************************/
+u32 not_long(u32 s)
+{
+    return ~s;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the RCL instruction and side effects.
+****************************************************************************/
+u8 rcl_byte(u8 d, u8 s)
+{
+    unsigned int res, cnt, mask, cf;
+
+    /* s is the rotate distance.  It varies from 0 - 8. */
+    /* have
+
+       CF  B_7 B_6 B_5 B_4 B_3 B_2 B_1 B_0
+
+       want to rotate through the carry by "s" bits.  We could
+       loop, but that's inefficient.  So the width is 9,
+       and we split into three parts:
+
+       The new carry flag   (was B_n)
+       the stuff in B_n-1 .. B_0
+       the stuff in B_7 .. B_n+1
+
+       The new rotate is done mod 9, and given this,
+       for a rotation of n bits (mod 9) the new carry flag is
+       then located n bits from the MSB.  The low part is
+       then shifted up cnt bits, and the high part is or'd
+       in.  Using CAPS for new values, and lowercase for the
+       original values, this can be expressed as:
+
+       IF n > 0
+       1) CF <-	 b_(8-n)
+       2) B_(7) .. B_(n)  <-  b_(8-(n+1)) .. b_0
+       3) B_(n-1) <- cf
+       4) B_(n-2) .. B_0 <-  b_7 .. b_(8-(n-1))
+     */
+    res = d;
+    if ((cnt = s % 9) != 0) {
+	/* extract the new CARRY FLAG. */
+	/* CF <-  b_(8-n)	      */
+	cf = (d >> (8 - cnt)) & 0x1;
+
+	/* get the low stuff which rotated
+	   into the range B_7 .. B_cnt */
+	/* B_(7) .. B_(n)  <-  b_(8-(n+1)) .. b_0  */
+	/* note that the right hand side done by the mask */
+	res = (d << cnt) & 0xff;
+
+	/* now the high stuff which rotated around
+	   into the positions B_cnt-2 .. B_0 */
+	/* B_(n-2) .. B_0 <-  b_7 .. b_(8-(n-1)) */
+	/* shift it downward, 7-(n-2) = 9-n positions.
+	   and mask off the result before or'ing in.
+	 */
+	mask = (1 << (cnt - 1)) - 1;
+	res |= (d >> (9 - cnt)) & mask;
+
+	/* if the carry flag was set, or it in.	 */
+	if (ACCESS_FLAG(F_CF)) {     /* carry flag is set */
+	    /*	B_(n-1) <- cf */
+	    res |= 1 << (cnt - 1);
+	}
+	/* set the new carry flag, based on the variable "cf" */
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	/* OVERFLOW is set *IFF* cnt==1, then it is the
+	   xor of CF and the most significant bit.  Blecck. */
+	/* parenthesized this expression since it appears to
+	   be causing OF to be misset */
+	CONDITIONAL_SET_FLAG(cnt == 1 && XOR2(cf + ((res >> 6) & 0x2)),
+			     F_OF);
+
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the RCL instruction and side effects.
+****************************************************************************/
+u16 rcl_word(u16 d, u8 s)
+{
+    unsigned int res, cnt, mask, cf;
+
+    res = d;
+    if ((cnt = s % 17) != 0) {
+	cf = (d >> (16 - cnt)) & 0x1;
+	res = (d << cnt) & 0xffff;
+	mask = (1 << (cnt - 1)) - 1;
+	res |= (d >> (17 - cnt)) & mask;
+	if (ACCESS_FLAG(F_CF)) {
+	    res |= 1 << (cnt - 1);
+	}
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	CONDITIONAL_SET_FLAG(cnt == 1 && XOR2(cf + ((res >> 14) & 0x2)),
+			     F_OF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the RCL instruction and side effects.
+****************************************************************************/
+u32 rcl_long(u32 d, u8 s)
+{
+    u32 res, cnt, mask, cf;
+
+    res = d;
+    if ((cnt = s % 33) != 0) {
+	cf = (d >> (32 - cnt)) & 0x1;
+	res = (d << cnt) & 0xffffffff;
+	mask = (1 << (cnt - 1)) - 1;
+	res |= (d >> (33 - cnt)) & mask;
+	if (ACCESS_FLAG(F_CF)) {     /* carry flag is set */
+	    res |= 1 << (cnt - 1);
+	}
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	CONDITIONAL_SET_FLAG(cnt == 1 && XOR2(cf + ((res >> 30) & 0x2)),
+			     F_OF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the RCR instruction and side effects.
+****************************************************************************/
+u8 rcr_byte(u8 d, u8 s)
+{
+    u32 res, cnt;
+    u32 mask, cf, ocf = 0;
+
+    /* rotate right through carry */
+    /*
+       s is the rotate distance.  It varies from 0 - 8.
+       d is the byte object rotated.
+
+       have
+
+       CF  B_7 B_6 B_5 B_4 B_3 B_2 B_1 B_0
+
+       The new rotate is done mod 9, and given this,
+       for a rotation of n bits (mod 9) the new carry flag is
+       then located n bits from the LSB.  The low part is
+       then shifted up cnt bits, and the high part is or'd
+       in.  Using CAPS for new values, and lowercase for the
+       original values, this can be expressed as:
+
+       IF n > 0
+       1) CF <-	 b_(n-1)
+       2) B_(8-(n+1)) .. B_(0)	<-  b_(7) .. b_(n)
+       3) B_(8-n) <- cf
+       4) B_(7) .. B_(8-(n-1)) <-  b_(n-2) .. b_(0)
+     */
+    res = d;
+    if ((cnt = s % 9) != 0) {
+	/* extract the new CARRY FLAG. */
+	/* CF <-  b_(n-1)	       */
+	if (cnt == 1) {
+	    cf = d & 0x1;
+	    /* note hackery here.  Access_flag(..) evaluates to either
+	       0 if flag not set
+	       non-zero if flag is set.
+	       doing access_flag(..) != 0 casts that into either
+	       0..1 in any representation of the flags register
+	       (i.e. packed bit array or unpacked.)
+	     */
+	    ocf = ACCESS_FLAG(F_CF) != 0;
+	} else
+	    cf = (d >> (cnt - 1)) & 0x1;
+
+	/* B_(8-(n+1)) .. B_(0)	 <-  b_(7) .. b_n  */
+	/* note that the right hand side done by the mask
+	   This is effectively done by shifting the
+	   object to the right.	 The result must be masked,
+	   in case the object came in and was treated
+	   as a negative number.  Needed??? */
+
+	mask = (1 << (8 - cnt)) - 1;
+	res = (d >> cnt) & mask;
+
+	/* now the high stuff which rotated around
+	   into the positions B_cnt-2 .. B_0 */
+	/* B_(7) .. B_(8-(n-1)) <-  b_(n-2) .. b_(0) */
+	/* shift it downward, 7-(n-2) = 9-n positions.
+	   and mask off the result before or'ing in.
+	 */
+	res |= (d << (9 - cnt));
+
+	/* if the carry flag was set, or it in.	 */
+	if (ACCESS_FLAG(F_CF)) {     /* carry flag is set */
+	    /*	B_(8-n) <- cf */
+	    res |= 1 << (8 - cnt);
+	}
+	/* set the new carry flag, based on the variable "cf" */
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	/* OVERFLOW is set *IFF* cnt==1, then it is the
+	   xor of CF and the most significant bit.  Blecck. */
+	/* parenthesized... */
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(ocf + ((d >> 6) & 0x2)),
+				 F_OF);
+	}
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the RCR instruction and side effects.
+****************************************************************************/
+u16 rcr_word(u16 d, u8 s)
+{
+    u32 res, cnt;
+    u32 mask, cf, ocf = 0;
+
+    /* rotate right through carry */
+    res = d;
+    if ((cnt = s % 17) != 0) {
+	if (cnt == 1) {
+	    cf = d & 0x1;
+	    ocf = ACCESS_FLAG(F_CF) != 0;
+	} else
+	    cf = (d >> (cnt - 1)) & 0x1;
+	mask = (1 << (16 - cnt)) - 1;
+	res = (d >> cnt) & mask;
+	res |= (d << (17 - cnt));
+	if (ACCESS_FLAG(F_CF)) {
+	    res |= 1 << (16 - cnt);
+	}
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(ocf + ((d >> 14) & 0x2)),
+				 F_OF);
+	}
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the RCR instruction and side effects.
+****************************************************************************/
+u32 rcr_long(u32 d, u8 s)
+{
+    u32 res, cnt;
+    u32 mask, cf, ocf = 0;
+
+    /* rotate right through carry */
+    res = d;
+    if ((cnt = s % 33) != 0) {
+	if (cnt == 1) {
+	    cf = d & 0x1;
+	    ocf = ACCESS_FLAG(F_CF) != 0;
+	} else
+	    cf = (d >> (cnt - 1)) & 0x1;
+	mask = (1 << (32 - cnt)) - 1;
+	res = (d >> cnt) & mask;
+	if (cnt != 1)
+	    res |= (d << (33 - cnt));
+	if (ACCESS_FLAG(F_CF)) {     /* carry flag is set */
+	    res |= 1 << (32 - cnt);
+	}
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(ocf + ((d >> 30) & 0x2)),
+				 F_OF);
+	}
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ROL instruction and side effects.
+****************************************************************************/
+u8 rol_byte(u8 d, u8 s)
+{
+    unsigned int res, cnt, mask;
+
+    /* rotate left */
+    /*
+       s is the rotate distance.  It varies from 0 - 8.
+       d is the byte object rotated.
+
+       have
+
+       CF  B_7 ... B_0
+
+       The new rotate is done mod 8.
+       Much simpler than the "rcl" or "rcr" operations.
+
+       IF n > 0
+       1) B_(7) .. B_(n)  <-  b_(8-(n+1)) .. b_(0)
+       2) B_(n-1) .. B_(0) <-  b_(7) .. b_(8-n)
+     */
+    res = d;
+    if ((cnt = s % 8) != 0) {
+	/* B_(7) .. B_(n)  <-  b_(8-(n+1)) .. b_(0) */
+	res = (d << cnt);
+
+	/* B_(n-1) .. B_(0) <-	b_(7) .. b_(8-n) */
+	mask = (1 << cnt) - 1;
+	res |= (d >> (8 - cnt)) & mask;
+
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x1, F_CF);
+	/* OVERFLOW is set *IFF* s==1, then it is the
+	   xor of CF and the most significant bit.  Blecck. */
+	CONDITIONAL_SET_FLAG(s == 1 &&
+			     XOR2((res & 0x1) + ((res >> 6) & 0x2)),
+			     F_OF);
+    } if (s != 0) {
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x1, F_CF);
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ROL instruction and side effects.
+****************************************************************************/
+u16 rol_word(u16 d, u8 s)
+{
+    unsigned int res, cnt, mask;
+
+    res = d;
+    if ((cnt = s % 16) != 0) {
+	res = (d << cnt);
+	mask = (1 << cnt) - 1;
+	res |= (d >> (16 - cnt)) & mask;
+	CONDITIONAL_SET_FLAG(res & 0x1, F_CF);
+	CONDITIONAL_SET_FLAG(s == 1 &&
+			     XOR2((res & 0x1) + ((res >> 14) & 0x2)),
+			     F_OF);
+    } if (s != 0) {
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x1, F_CF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ROL instruction and side effects.
+****************************************************************************/
+u32 rol_long(u32 d, u8 s)
+{
+    u32 res, cnt, mask;
+
+    res = d;
+    if ((cnt = s % 32) != 0) {
+	res = (d << cnt);
+	mask = (1 << cnt) - 1;
+	res |= (d >> (32 - cnt)) & mask;
+	CONDITIONAL_SET_FLAG(res & 0x1, F_CF);
+	CONDITIONAL_SET_FLAG(s == 1 &&
+			     XOR2((res & 0x1) + ((res >> 30) & 0x2)),
+			     F_OF);
+    } if (s != 0) {
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x1, F_CF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ROR instruction and side effects.
+****************************************************************************/
+u8 ror_byte(u8 d, u8 s)
+{
+    unsigned int res, cnt, mask;
+
+    /* rotate right */
+    /*
+       s is the rotate distance.  It varies from 0 - 8.
+       d is the byte object rotated.
+
+       have
+
+       B_7 ... B_0
+
+       The rotate is done mod 8.
+
+       IF n > 0
+       1) B_(8-(n+1)) .. B_(0)	<-  b_(7) .. b_(n)
+       2) B_(7) .. B_(8-n) <-  b_(n-1) .. b_(0)
+     */
+    res = d;
+    if ((cnt = s % 8) != 0) {		/* not a typo, do nada if cnt==0 */
+	/* B_(7) .. B_(8-n) <-	b_(n-1) .. b_(0) */
+	res = (d << (8 - cnt));
+
+	/* B_(8-(n+1)) .. B_(0)	 <-  b_(7) .. b_(n) */
+	mask = (1 << (8 - cnt)) - 1;
+	res |= (d >> (cnt)) & mask;
+
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x80, F_CF);
+	/* OVERFLOW is set *IFF* s==1, then it is the
+	   xor of the two most significant bits.  Blecck. */
+	CONDITIONAL_SET_FLAG(s == 1 && XOR2(res >> 6), F_OF);
+    } else if (s != 0) {
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x80, F_CF);
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ROR instruction and side effects.
+****************************************************************************/
+u16 ror_word(u16 d, u8 s)
+{
+    unsigned int res, cnt, mask;
+
+    res = d;
+    if ((cnt = s % 16) != 0) {
+	res = (d << (16 - cnt));
+	mask = (1 << (16 - cnt)) - 1;
+	res |= (d >> (cnt)) & mask;
+	CONDITIONAL_SET_FLAG(res & 0x8000, F_CF);
+	CONDITIONAL_SET_FLAG(s == 1 && XOR2(res >> 14), F_OF);
+    } else if (s != 0) {
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x8000, F_CF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the ROR instruction and side effects.
+****************************************************************************/
+u32 ror_long(u32 d, u8 s)
+{
+    u32 res, cnt, mask;
+
+    res = d;
+    if ((cnt = s % 32) != 0) {
+	res = (d << (32 - cnt));
+	mask = (1 << (32 - cnt)) - 1;
+	res |= (d >> (cnt)) & mask;
+	CONDITIONAL_SET_FLAG(res & 0x80000000, F_CF);
+	CONDITIONAL_SET_FLAG(s == 1 && XOR2(res >> 30), F_OF);
+    } else if (s != 0) {
+	/* set the new carry flag, Note that it is the low order
+	   bit of the result!!!				      */
+	CONDITIONAL_SET_FLAG(res & 0x80000000, F_CF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHL instruction and side effects.
+****************************************************************************/
+u8 shl_byte(u8 d, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 8) {
+	cnt = s % 8;
+
+	/* last bit shifted out goes into carry flag */
+	if (cnt > 0) {
+	    res = d << cnt;
+	    cf = d & (1 << (8 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_8((u8)res);
+	} else {
+	    res = (u8) d;
+	}
+
+	if (cnt == 1) {
+	    /* Needs simplification. */
+	    CONDITIONAL_SET_FLAG(
+				    (((res & 0x80) == 0x80) ^
+				     (ACCESS_FLAG(F_CF) != 0)),
+	    /* was (M.x86.R_FLG&F_CF)==F_CF)), */
+				    F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CONDITIONAL_SET_FLAG((d << (s-1)) & 0x80, F_CF);
+	CLEAR_FLAG(F_OF);
+	CLEAR_FLAG(F_SF);
+	SET_FLAG(F_PF);
+	SET_FLAG(F_ZF);
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHL instruction and side effects.
+****************************************************************************/
+u16 shl_word(u16 d, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 16) {
+	cnt = s % 16;
+	if (cnt > 0) {
+	    res = d << cnt;
+	    cf = d & (1 << (16 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_16((u16)res);
+	} else {
+	    res = (u16) d;
+	}
+
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(
+				    (((res & 0x8000) == 0x8000) ^
+				     (ACCESS_FLAG(F_CF) != 0)),
+				    F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CONDITIONAL_SET_FLAG((d << (s-1)) & 0x8000, F_CF);
+	CLEAR_FLAG(F_OF);
+	CLEAR_FLAG(F_SF);
+	SET_FLAG(F_PF);
+	SET_FLAG(F_ZF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHL instruction and side effects.
+****************************************************************************/
+u32 shl_long(u32 d, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 32) {
+	cnt = s % 32;
+	if (cnt > 0) {
+	    res = d << cnt;
+	    cf = d & (1 << (32 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_32((u32)res);
+	} else {
+	    res = d;
+	}
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG((((res & 0x80000000) == 0x80000000) ^
+				  (ACCESS_FLAG(F_CF) != 0)), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CONDITIONAL_SET_FLAG((d << (s-1)) & 0x80000000, F_CF);
+	CLEAR_FLAG(F_OF);
+	CLEAR_FLAG(F_SF);
+	SET_FLAG(F_PF);
+	SET_FLAG(F_ZF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHR instruction and side effects.
+****************************************************************************/
+u8 shr_byte(u8 d, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 8) {
+	cnt = s % 8;
+	if (cnt > 0) {
+	    cf = d & (1 << (cnt - 1));
+	    res = d >> cnt;
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_8((u8)res);
+	} else {
+	    res = (u8) d;
+	}
+
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(res >> 6), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CONDITIONAL_SET_FLAG((d >> (s-1)) & 0x1, F_CF);
+	CLEAR_FLAG(F_OF);
+	CLEAR_FLAG(F_SF);
+	SET_FLAG(F_PF);
+	SET_FLAG(F_ZF);
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHR instruction and side effects.
+****************************************************************************/
+u16 shr_word(u16 d, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 16) {
+	cnt = s % 16;
+	if (cnt > 0) {
+	    cf = d & (1 << (cnt - 1));
+	    res = d >> cnt;
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_16((u16)res);
+	} else {
+	    res = d;
+	}
+
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(res >> 14), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+	SET_FLAG(F_ZF);
+	CLEAR_FLAG(F_SF);
+	CLEAR_FLAG(F_PF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHR instruction and side effects.
+****************************************************************************/
+u32 shr_long(u32 d, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 32) {
+	cnt = s % 32;
+	if (cnt > 0) {
+	    cf = d & (1 << (cnt - 1));
+	    res = d >> cnt;
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_32((u32)res);
+	} else {
+	    res = d;
+	}
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(res >> 30), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+	SET_FLAG(F_ZF);
+	CLEAR_FLAG(F_SF);
+	CLEAR_FLAG(F_PF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SAR instruction and side effects.
+****************************************************************************/
+u8 sar_byte(u8 d, u8 s)
+{
+    unsigned int cnt, res, cf, mask, sf;
+
+    res = d;
+    sf = d & 0x80;
+    cnt = s % 8;
+    if (cnt > 0 && cnt < 8) {
+	mask = (1 << (8 - cnt)) - 1;
+	cf = d & (1 << (cnt - 1));
+	res = (d >> cnt) & mask;
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	if (sf) {
+	    res |= ~mask;
+	}
+	set_szp_flags_8((u8)res);
+    } else if (cnt >= 8) {
+	if (sf) {
+	    res = 0xff;
+	    SET_FLAG(F_CF);
+	    CLEAR_FLAG(F_ZF);
+	    SET_FLAG(F_SF);
+	    SET_FLAG(F_PF);
+	} else {
+	    res = 0;
+	    CLEAR_FLAG(F_CF);
+	    SET_FLAG(F_ZF);
+	    CLEAR_FLAG(F_SF);
+	    CLEAR_FLAG(F_PF);
+	}
+    }
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SAR instruction and side effects.
+****************************************************************************/
+u16 sar_word(u16 d, u8 s)
+{
+    unsigned int cnt, res, cf, mask, sf;
+
+    sf = d & 0x8000;
+    cnt = s % 16;
+    res = d;
+    if (cnt > 0 && cnt < 16) {
+	mask = (1 << (16 - cnt)) - 1;
+	cf = d & (1 << (cnt - 1));
+	res = (d >> cnt) & mask;
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	if (sf) {
+	    res |= ~mask;
+	}
+	set_szp_flags_16((u16)res);
+    } else if (cnt >= 16) {
+	if (sf) {
+	    res = 0xffff;
+	    SET_FLAG(F_CF);
+	    CLEAR_FLAG(F_ZF);
+	    SET_FLAG(F_SF);
+	    SET_FLAG(F_PF);
+	} else {
+	    res = 0;
+	    CLEAR_FLAG(F_CF);
+	    SET_FLAG(F_ZF);
+	    CLEAR_FLAG(F_SF);
+	    CLEAR_FLAG(F_PF);
+	}
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SAR instruction and side effects.
+****************************************************************************/
+u32 sar_long(u32 d, u8 s)
+{
+    u32 cnt, res, cf, mask, sf;
+
+    sf = d & 0x80000000;
+    cnt = s % 32;
+    res = d;
+    if (cnt > 0 && cnt < 32) {
+	mask = (1 << (32 - cnt)) - 1;
+	cf = d & (1 << (cnt - 1));
+	res = (d >> cnt) & mask;
+	CONDITIONAL_SET_FLAG(cf, F_CF);
+	if (sf) {
+	    res |= ~mask;
+	}
+	set_szp_flags_32(res);
+    } else if (cnt >= 32) {
+	if (sf) {
+	    res = 0xffffffff;
+	    SET_FLAG(F_CF);
+	    CLEAR_FLAG(F_ZF);
+	    SET_FLAG(F_SF);
+	    SET_FLAG(F_PF);
+	} else {
+	    res = 0;
+	    CLEAR_FLAG(F_CF);
+	    SET_FLAG(F_ZF);
+	    CLEAR_FLAG(F_SF);
+	    CLEAR_FLAG(F_PF);
+	}
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHLD instruction and side effects.
+****************************************************************************/
+u16 shld_word (u16 d, u16 fill, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 16) {
+	cnt = s % 16;
+	if (cnt > 0) {
+	    res = (d << cnt) | (fill >> (16-cnt));
+	    cf = d & (1 << (16 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_16((u16)res);
+	} else {
+	    res = d;
+	}
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG((((res & 0x8000) == 0x8000) ^
+				  (ACCESS_FLAG(F_CF) != 0)), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CONDITIONAL_SET_FLAG((d << (s-1)) & 0x8000, F_CF);
+	CLEAR_FLAG(F_OF);
+	CLEAR_FLAG(F_SF);
+	SET_FLAG(F_PF);
+	SET_FLAG(F_ZF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHLD instruction and side effects.
+****************************************************************************/
+u32 shld_long (u32 d, u32 fill, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 32) {
+	cnt = s % 32;
+	if (cnt > 0) {
+	    res = (d << cnt) | (fill >> (32-cnt));
+	    cf = d & (1 << (32 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_32((u32)res);
+	} else {
+	    res = d;
+	}
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG((((res & 0x80000000) == 0x80000000) ^
+				  (ACCESS_FLAG(F_CF) != 0)), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CONDITIONAL_SET_FLAG((d << (s-1)) & 0x80000000, F_CF);
+	CLEAR_FLAG(F_OF);
+	CLEAR_FLAG(F_SF);
+	SET_FLAG(F_PF);
+	SET_FLAG(F_ZF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHRD instruction and side effects.
+****************************************************************************/
+u16 shrd_word (u16 d, u16 fill, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 16) {
+	cnt = s % 16;
+	if (cnt > 0) {
+	    cf = d & (1 << (cnt - 1));
+	    res = (d >> cnt) | (fill << (16 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_16((u16)res);
+	} else {
+	    res = d;
+	}
+
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(res >> 14), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+	SET_FLAG(F_ZF);
+	CLEAR_FLAG(F_SF);
+	CLEAR_FLAG(F_PF);
+    }
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SHRD instruction and side effects.
+****************************************************************************/
+u32 shrd_long (u32 d, u32 fill, u8 s)
+{
+    unsigned int cnt, res, cf;
+
+    if (s < 32) {
+	cnt = s % 32;
+	if (cnt > 0) {
+	    cf = d & (1 << (cnt - 1));
+	    res = (d >> cnt) | (fill << (32 - cnt));
+	    CONDITIONAL_SET_FLAG(cf, F_CF);
+	    set_szp_flags_32((u32)res);
+	} else {
+	    res = d;
+	}
+	if (cnt == 1) {
+	    CONDITIONAL_SET_FLAG(XOR2(res >> 30), F_OF);
+	} else {
+	    CLEAR_FLAG(F_OF);
+	}
+    } else {
+	res = 0;
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+	SET_FLAG(F_ZF);
+	CLEAR_FLAG(F_SF);
+	CLEAR_FLAG(F_PF);
+    }
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SBB instruction and side effects.
+****************************************************************************/
+u8 sbb_byte(u8 d, u8 s)
+{
+    u32 res;   /* all operands in native machine order */
+    u32 bc;
+
+    if (ACCESS_FLAG(F_CF))
+	res = d - s - 1;
+    else
+	res = d - s;
+    set_szp_flags_8((u8)res);
+
+    /* calculate the borrow chain.  See note at top */
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(bc & 0x80, F_CF);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> 6), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SBB instruction and side effects.
+****************************************************************************/
+u16 sbb_word(u16 d, u16 s)
+{
+    u32 res;   /* all operands in native machine order */
+    u32 bc;
+
+    if (ACCESS_FLAG(F_CF))
+	res = d - s - 1;
+    else
+	res = d - s;
+    set_szp_flags_16((u16)res);
+
+    /* calculate the borrow chain.  See note at top */
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(bc & 0x8000, F_CF);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> 14), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SBB instruction and side effects.
+****************************************************************************/
+u32 sbb_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+    u32 bc;
+
+    if (ACCESS_FLAG(F_CF))
+	res = d - s - 1;
+    else
+	res = d - s;
+
+    set_szp_flags_32(res);
+
+    /* calculate the borrow chain.  See note at top */
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(bc & 0x80000000, F_CF);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> 30), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SUB instruction and side effects.
+****************************************************************************/
+u8 sub_byte(u8 d, u8 s)
+{
+    u32 res;   /* all operands in native machine order */
+    u32 bc;
+
+    res = d - s;
+    set_szp_flags_8((u8)res);
+
+    /* calculate the borrow chain.  See note at top */
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(bc & 0x80, F_CF);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> 6), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    return (u8)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SUB instruction and side effects.
+****************************************************************************/
+u16 sub_word(u16 d, u16 s)
+{
+    u32 res;   /* all operands in native machine order */
+    u32 bc;
+
+    res = d - s;
+    set_szp_flags_16((u16)res);
+
+    /* calculate the borrow chain.  See note at top */
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(bc & 0x8000, F_CF);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> 14), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    return (u16)res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the SUB instruction and side effects.
+****************************************************************************/
+u32 sub_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+    u32 bc;
+
+    res = d - s;
+    set_szp_flags_32(res);
+
+    /* calculate the borrow chain.  See note at top */
+    bc = (res & (~d | s)) | (~d & s);
+    CONDITIONAL_SET_FLAG(bc & 0x80000000, F_CF);
+    CONDITIONAL_SET_FLAG(XOR2(bc >> 30), F_OF);
+    CONDITIONAL_SET_FLAG(bc & 0x8, F_AF);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the TEST instruction and side effects.
+****************************************************************************/
+void test_byte(u8 d, u8 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d & s;
+
+    CLEAR_FLAG(F_OF);
+    set_szp_flags_8((u8)res);
+    /* AF == dont care */
+    CLEAR_FLAG(F_CF);
+}
+
+/****************************************************************************
+REMARKS:
+Implements the TEST instruction and side effects.
+****************************************************************************/
+void test_word(u16 d, u16 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d & s;
+
+    CLEAR_FLAG(F_OF);
+    set_szp_flags_16((u16)res);
+    /* AF == dont care */
+    CLEAR_FLAG(F_CF);
+}
+
+/****************************************************************************
+REMARKS:
+Implements the TEST instruction and side effects.
+****************************************************************************/
+void test_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d & s;
+
+    CLEAR_FLAG(F_OF);
+    set_szp_flags_32(res);
+    /* AF == dont care */
+    CLEAR_FLAG(F_CF);
+}
+
+/****************************************************************************
+REMARKS:
+Implements the XOR instruction and side effects.
+****************************************************************************/
+u8 xor_byte(u8 d, u8 s)
+{
+    u8 res;    /* all operands in native machine order */
+
+    res = d ^ s;
+    no_carry_byte_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the XOR instruction and side effects.
+****************************************************************************/
+u16 xor_word(u16 d, u16 s)
+{
+    u16 res;   /* all operands in native machine order */
+
+    res = d ^ s;
+    no_carry_word_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the XOR instruction and side effects.
+****************************************************************************/
+u32 xor_long(u32 d, u32 s)
+{
+    u32 res;   /* all operands in native machine order */
+
+    res = d ^ s;
+    no_carry_long_side_eff(res);
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IMUL instruction and side effects.
+****************************************************************************/
+void imul_byte(u8 s)
+{
+    s16 res = (s16)((s8)M.x86.R_AL * (s8)s);
+
+    M.x86.R_AX = res;
+    if (((M.x86.R_AL & 0x80) == 0 && M.x86.R_AH == 0x00) ||
+	((M.x86.R_AL & 0x80) != 0 && M.x86.R_AH == 0xFF)) {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+    } else {
+	SET_FLAG(F_CF);
+	SET_FLAG(F_OF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IMUL instruction and side effects.
+****************************************************************************/
+void imul_word(u16 s)
+{
+    s32 res = (s16)M.x86.R_AX * (s16)s;
+
+    M.x86.R_AX = (u16)res;
+    M.x86.R_DX = (u16)(res >> 16);
+    if (((M.x86.R_AX & 0x8000) == 0 && M.x86.R_DX == 0x0000) ||
+	((M.x86.R_AX & 0x8000) != 0 && M.x86.R_DX == 0xFFFF)) {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+    } else {
+	SET_FLAG(F_CF);
+	SET_FLAG(F_OF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IMUL instruction and side effects.
+****************************************************************************/
+void imul_long_direct(u32 *res_lo, u32* res_hi,u32 d, u32 s)
+{
+#ifdef	__HAS_LONG_LONG__
+    s64 res = (s32)d * (s32)s;
+
+    *res_lo = (u32)res;
+    *res_hi = (u32)(res >> 32);
+#else
+    u32 d_lo,d_hi,d_sign;
+    u32 s_lo,s_hi,s_sign;
+    u32 rlo_lo,rlo_hi,rhi_lo;
+
+    if ((d_sign = d & 0x80000000) != 0)
+	d = -d;
+    d_lo = d & 0xFFFF;
+    d_hi = d >> 16;
+    if ((s_sign = s & 0x80000000) != 0)
+	s = -s;
+    s_lo = s & 0xFFFF;
+    s_hi = s >> 16;
+    rlo_lo = d_lo * s_lo;
+    rlo_hi = (d_hi * s_lo + d_lo * s_hi) + (rlo_lo >> 16);
+    rhi_lo = d_hi * s_hi + (rlo_hi >> 16);
+    *res_lo = (rlo_hi << 16) | (rlo_lo & 0xFFFF);
+    *res_hi = rhi_lo;
+    if (d_sign != s_sign) {
+	d = ~*res_lo;
+	s = (((d & 0xFFFF) + 1) >> 16) + (d >> 16);
+	*res_lo = ~*res_lo+1;
+	*res_hi = ~*res_hi+(s >> 16);
+	}
+#endif
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IMUL instruction and side effects.
+****************************************************************************/
+void imul_long(u32 s)
+{
+    imul_long_direct(&M.x86.R_EAX,&M.x86.R_EDX,M.x86.R_EAX,s);
+    if (((M.x86.R_EAX & 0x80000000) == 0 && M.x86.R_EDX == 0x00000000) ||
+	((M.x86.R_EAX & 0x80000000) != 0 && M.x86.R_EDX == 0xFFFFFFFF)) {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+    } else {
+	SET_FLAG(F_CF);
+	SET_FLAG(F_OF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the MUL instruction and side effects.
+****************************************************************************/
+void mul_byte(u8 s)
+{
+    u16 res = (u16)(M.x86.R_AL * s);
+
+    M.x86.R_AX = res;
+    if (M.x86.R_AH == 0) {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+    } else {
+	SET_FLAG(F_CF);
+	SET_FLAG(F_OF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the MUL instruction and side effects.
+****************************************************************************/
+void mul_word(u16 s)
+{
+    u32 res = M.x86.R_AX * s;
+
+    M.x86.R_AX = (u16)res;
+    M.x86.R_DX = (u16)(res >> 16);
+    if (M.x86.R_DX == 0) {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+    } else {
+	SET_FLAG(F_CF);
+	SET_FLAG(F_OF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the MUL instruction and side effects.
+****************************************************************************/
+void mul_long(u32 s)
+{
+#ifdef	__HAS_LONG_LONG__
+    u64 res = (u32)M.x86.R_EAX * (u32)s;
+
+    M.x86.R_EAX = (u32)res;
+    M.x86.R_EDX = (u32)(res >> 32);
+#else
+    u32 a,a_lo,a_hi;
+    u32 s_lo,s_hi;
+    u32 rlo_lo,rlo_hi,rhi_lo;
+
+    a = M.x86.R_EAX;
+    a_lo = a & 0xFFFF;
+    a_hi = a >> 16;
+    s_lo = s & 0xFFFF;
+    s_hi = s >> 16;
+    rlo_lo = a_lo * s_lo;
+    rlo_hi = (a_hi * s_lo + a_lo * s_hi) + (rlo_lo >> 16);
+    rhi_lo = a_hi * s_hi + (rlo_hi >> 16);
+    M.x86.R_EAX = (rlo_hi << 16) | (rlo_lo & 0xFFFF);
+    M.x86.R_EDX = rhi_lo;
+#endif
+    if (M.x86.R_EDX == 0) {
+	CLEAR_FLAG(F_CF);
+	CLEAR_FLAG(F_OF);
+    } else {
+	SET_FLAG(F_CF);
+	SET_FLAG(F_OF);
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IDIV instruction and side effects.
+****************************************************************************/
+void idiv_byte(u8 s)
+{
+    s32 dvd, div, mod;
+
+    dvd = (s16)M.x86.R_AX;
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    div = dvd / (s8)s;
+    mod = dvd % (s8)s;
+    if (abs(div) > 0x7f) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    M.x86.R_AL = (s8) div;
+    M.x86.R_AH = (s8) mod;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IDIV instruction and side effects.
+****************************************************************************/
+void idiv_word(u16 s)
+{
+    s32 dvd, div, mod;
+
+    dvd = (((s32)M.x86.R_DX) << 16) | M.x86.R_AX;
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    div = dvd / (s16)s;
+    mod = dvd % (s16)s;
+    if (abs(div) > 0x7fff) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_SF);
+    CONDITIONAL_SET_FLAG(div == 0, F_ZF);
+    set_parity_flag(mod);
+
+    M.x86.R_AX = (u16)div;
+    M.x86.R_DX = (u16)mod;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IDIV instruction and side effects.
+****************************************************************************/
+void idiv_long(u32 s)
+{
+#ifdef	__HAS_LONG_LONG__
+    s64 dvd, div, mod;
+
+    dvd = (((s64)M.x86.R_EDX) << 32) | M.x86.R_EAX;
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    div = dvd / (s32)s;
+    mod = dvd % (s32)s;
+    if (abs(div) > 0x7fffffff) {
+	x86emu_intr_raise(0);
+	return;
+    }
+#else
+    s32 div = 0, mod;
+    s32 h_dvd = M.x86.R_EDX;
+    u32 l_dvd = M.x86.R_EAX;
+    u32 abs_s = s & 0x7FFFFFFF;
+    u32 abs_h_dvd = h_dvd & 0x7FFFFFFF;
+    u32 h_s = abs_s >> 1;
+    u32 l_s = abs_s << 31;
+    int counter = 31;
+    int carry;
+
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    do {
+	div <<= 1;
+	carry = (l_dvd >= l_s) ? 0 : 1;
+
+	if (abs_h_dvd < (h_s + carry)) {
+	    h_s >>= 1;
+	    l_s = abs_s << (--counter);
+	    continue;
+	} else {
+	    abs_h_dvd -= (h_s + carry);
+	    l_dvd = carry ? ((0xFFFFFFFF - l_s) + l_dvd + 1)
+		: (l_dvd - l_s);
+	    h_s >>= 1;
+	    l_s = abs_s << (--counter);
+	    div |= 1;
+	    continue;
+	}
+
+    } while (counter > -1);
+    /* overflow */
+    if (abs_h_dvd || (l_dvd > abs_s)) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    /* sign */
+    div |= ((h_dvd & 0x10000000) ^ (s & 0x10000000));
+    mod = l_dvd;
+
+#endif
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_AF);
+    CLEAR_FLAG(F_SF);
+    SET_FLAG(F_ZF);
+    set_parity_flag(mod);
+
+    M.x86.R_EAX = (u32)div;
+    M.x86.R_EDX = (u32)mod;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DIV instruction and side effects.
+****************************************************************************/
+void div_byte(u8 s)
+{
+    u32 dvd, div, mod;
+
+    dvd = M.x86.R_AX;
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    div = dvd / (u8)s;
+    mod = dvd % (u8)s;
+    if (abs(div) > 0xff) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    M.x86.R_AL = (u8)div;
+    M.x86.R_AH = (u8)mod;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DIV instruction and side effects.
+****************************************************************************/
+void div_word(u16 s)
+{
+    u32 dvd, div, mod;
+
+    dvd = (((u32)M.x86.R_DX) << 16) | M.x86.R_AX;
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    div = dvd / (u16)s;
+    mod = dvd % (u16)s;
+    if (abs(div) > 0xffff) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_SF);
+    CONDITIONAL_SET_FLAG(div == 0, F_ZF);
+    set_parity_flag(mod);
+
+    M.x86.R_AX = (u16)div;
+    M.x86.R_DX = (u16)mod;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the DIV instruction and side effects.
+****************************************************************************/
+void div_long(u32 s)
+{
+#ifdef	__HAS_LONG_LONG__
+    u64 dvd, div, mod;
+
+    dvd = (((u64)M.x86.R_EDX) << 32) | M.x86.R_EAX;
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    div = dvd / (u32)s;
+    mod = dvd % (u32)s;
+    if (abs(div) > 0xffffffff) {
+	x86emu_intr_raise(0);
+	return;
+    }
+#else
+    s32 div = 0, mod;
+    s32 h_dvd = M.x86.R_EDX;
+    u32 l_dvd = M.x86.R_EAX;
+
+    u32 h_s = s;
+    u32 l_s = 0;
+    int counter = 32;
+    int carry;
+
+    if (s == 0) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    do {
+	div <<= 1;
+	carry = (l_dvd >= l_s) ? 0 : 1;
+
+	if (h_dvd < (h_s + carry)) {
+	    h_s >>= 1;
+	    l_s = s << (--counter);
+	    continue;
+	} else {
+	    h_dvd -= (h_s + carry);
+	    l_dvd = carry ? ((0xFFFFFFFF - l_s) + l_dvd + 1)
+		: (l_dvd - l_s);
+	    h_s >>= 1;
+	    l_s = s << (--counter);
+	    div |= 1;
+	    continue;
+	}
+
+    } while (counter > -1);
+    /* overflow */
+    if (h_dvd || (l_dvd > s)) {
+	x86emu_intr_raise(0);
+	return;
+    }
+    mod = l_dvd;
+#endif
+    CLEAR_FLAG(F_CF);
+    CLEAR_FLAG(F_AF);
+    CLEAR_FLAG(F_SF);
+    SET_FLAG(F_ZF);
+    set_parity_flag(mod);
+
+    M.x86.R_EAX = (u32)div;
+    M.x86.R_EDX = (u32)mod;
+}
+
+/****************************************************************************
+REMARKS:
+Implements the IN string instruction and side effects.
+****************************************************************************/
+
+static void single_in(int size)
+{
+    if(size == 1)
+	store_data_byte_abs(M.x86.R_ES, M.x86.R_DI,(*sys_inb)(M.x86.R_DX));
+    else if (size == 2)
+	store_data_word_abs(M.x86.R_ES, M.x86.R_DI,(*sys_inw)(M.x86.R_DX));
+    else
+	store_data_long_abs(M.x86.R_ES, M.x86.R_DI,(*sys_inl)(M.x86.R_DX));
+}
+
+void ins(int size)
+{
+    int inc = size;
+
+    if (ACCESS_FLAG(F_DF)) {
+	inc = -size;
+    }
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* in until CX is ZERO. */
+	u32 count = ((M.x86.mode & SYSMODE_PREFIX_DATA) ?
+		     M.x86.R_ECX : M.x86.R_CX);
+
+	while (count--) {
+	  single_in(size);
+	  M.x86.R_DI += inc;
+	  }
+	M.x86.R_CX = 0;
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    M.x86.R_ECX = 0;
+	}
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    } else {
+	single_in(size);
+	M.x86.R_DI += inc;
+    }
+}
+
+/****************************************************************************
+REMARKS:
+Implements the OUT string instruction and side effects.
+****************************************************************************/
+
+static void single_out(int size)
+{
+     if(size == 1)
+       (*sys_outb)(M.x86.R_DX,fetch_data_byte_abs(M.x86.R_ES, M.x86.R_SI));
+     else if (size == 2)
+       (*sys_outw)(M.x86.R_DX,fetch_data_word_abs(M.x86.R_ES, M.x86.R_SI));
+     else
+       (*sys_outl)(M.x86.R_DX,fetch_data_long_abs(M.x86.R_ES, M.x86.R_SI));
+}
+
+void outs(int size)
+{
+    int inc = size;
+
+    if (ACCESS_FLAG(F_DF)) {
+	inc = -size;
+    }
+    if (M.x86.mode & (SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE)) {
+	/* dont care whether REPE or REPNE */
+	/* out until CX is ZERO. */
+	u32 count = ((M.x86.mode & SYSMODE_PREFIX_DATA) ?
+		     M.x86.R_ECX : M.x86.R_CX);
+	while (count--) {
+	  single_out(size);
+	  M.x86.R_SI += inc;
+	  }
+	M.x86.R_CX = 0;
+	if (M.x86.mode & SYSMODE_PREFIX_DATA) {
+	    M.x86.R_ECX = 0;
+	}
+	M.x86.mode &= ~(SYSMODE_PREFIX_REPE | SYSMODE_PREFIX_REPNE);
+    } else {
+	single_out(size);
+	M.x86.R_SI += inc;
+    }
+}
+
+/****************************************************************************
+PARAMETERS:
+addr	- Address to fetch word from
+
+REMARKS:
+Fetches a word from emulator memory using an absolute address.
+****************************************************************************/
+u16 mem_access_word(int addr)
+{
+DB( if (CHECK_MEM_ACCESS())
+      x86emu_check_mem_access(addr);)
+    return (*sys_rdw)(addr);
+}
+
+/****************************************************************************
+REMARKS:
+Pushes a word onto the stack.
+
+NOTE: Do not inline this, as (*sys_wrX) is already inline!
+****************************************************************************/
+void push_word(u16 w)
+{
+DB( if (CHECK_SP_ACCESS())
+      x86emu_check_sp_access();)
+    M.x86.R_SP -= 2;
+    (*sys_wrw)(((u32)M.x86.R_SS << 4)  + M.x86.R_SP, w);
+}
+
+/****************************************************************************
+REMARKS:
+Pushes a long onto the stack.
+
+NOTE: Do not inline this, as (*sys_wrX) is already inline!
+****************************************************************************/
+void push_long(u32 w)
+{
+DB( if (CHECK_SP_ACCESS())
+      x86emu_check_sp_access();)
+    M.x86.R_SP -= 4;
+    (*sys_wrl)(((u32)M.x86.R_SS << 4)  + M.x86.R_SP, w);
+}
+
+/****************************************************************************
+REMARKS:
+Pops a word from the stack.
+
+NOTE: Do not inline this, as (*sys_rdX) is already inline!
+****************************************************************************/
+u16 pop_word(void)
+{
+    u16 res;
+
+DB( if (CHECK_SP_ACCESS())
+      x86emu_check_sp_access();)
+    res = (*sys_rdw)(((u32)M.x86.R_SS << 4)  + M.x86.R_SP);
+    M.x86.R_SP += 2;
+    return res;
+}
+
+/****************************************************************************
+REMARKS:
+Pops a long from the stack.
+
+NOTE: Do not inline this, as (*sys_rdX) is already inline!
+****************************************************************************/
+u32 pop_long(void)
+{
+    u32 res;
+
+DB( if (CHECK_SP_ACCESS())
+      x86emu_check_sp_access();)
+    res = (*sys_rdl)(((u32)M.x86.R_SS << 4)  + M.x86.R_SP);
+    M.x86.R_SP += 4;
+    return res;
+}
+
+#endif
diff --git a/drivers/bios_emulator/x86emu/sys.c b/drivers/bios_emulator/x86emu/sys.c
new file mode 100644
index 00000000000..566389f5862
--- /dev/null
+++ b/drivers/bios_emulator/x86emu/sys.c
@@ -0,0 +1,326 @@
+/****************************************************************************
+*
+*                       Realmode X86 Emulator Library
+*
+*               Copyright (C) 1991-2004 SciTech Software, Inc.
+*                    Copyright (C) David Mosberger-Tang
+*                      Copyright (C) 1999 Egbert Eich
+*
+*  ========================================================================
+*
+*  Permission to use, copy, modify, distribute, and sell this software and
+*  its documentation for any purpose is hereby granted without fee,
+*  provided that the above copyright notice appear in all copies and that
+*  both that copyright notice and this permission notice appear in
+*  supporting documentation, and that the name of the authors not be used
+*  in advertising or publicity pertaining to distribution of the software
+*  without specific, written prior permission.  The authors makes no
+*  representations about the suitability of this software for any purpose.
+*  It is provided "as is" without express or implied warranty.
+*
+*  THE AUTHORS DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+*  INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+*  EVENT SHALL THE AUTHORS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+*  CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF
+*  USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
+*  OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+*  PERFORMANCE OF THIS SOFTWARE.
+*
+*  ========================================================================
+*
+* Language:     ANSI C
+* Environment:  Any
+* Developer:    Kendall Bennett
+*
+* Description:  This file includes subroutines which are related to
+*               programmed I/O and memory access. Included in this module
+*               are default functions that do nothing. For real uses these
+*               functions will have to be overriden by the user library.
+*
+****************************************************************************/
+
+#include "x86emu/x86emui.h"
+
+#if defined(CONFIG_BIOSEMU)
+
+/*------------------------- Global Variables ------------------------------*/
+
+X86EMU_sysEnv _X86EMU_env;	/* Global emulator machine state */
+X86EMU_intrFuncs _X86EMU_intrTab[256];
+
+int debug_intr;
+
+/*----------------------------- Implementation ----------------------------*/
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+
+RETURNS:
+Byte value read from emulator memory.
+
+REMARKS:
+Reads a byte value from the emulator memory.
+****************************************************************************/
+u8 X86API rdb(u32 addr)
+{
+	return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+
+RETURNS:
+Word value read from emulator memory.
+
+REMARKS:
+Reads a word value from the emulator memory.
+****************************************************************************/
+u16 X86API rdw(u32 addr)
+{
+	return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+
+RETURNS:
+Long value read from emulator memory.
+REMARKS:
+Reads a long value from the emulator memory.
+****************************************************************************/
+u32 X86API rdl(u32 addr)
+{
+	return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+val     - Value to store
+
+REMARKS:
+Writes a byte value to emulator memory.
+****************************************************************************/
+void X86API wrb(u32 addr, u8 val)
+{
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+val     - Value to store
+
+REMARKS:
+Writes a word value to emulator memory.
+****************************************************************************/
+void X86API wrw(u32 addr, u16 val)
+{
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - Emulator memory address to read
+val     - Value to store
+
+REMARKS:
+Writes a long value to emulator memory.
+****************************************************************************/
+void X86API wrl(u32 addr, u32 val)
+{
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - PIO address to read
+RETURN:
+0
+REMARKS:
+Default PIO byte read function. Doesn't perform real inb.
+****************************************************************************/
+static u8 X86API p_inb(X86EMU_pioAddr addr)
+{
+	DB(if (DEBUG_IO_TRACE())
+	   printk("inb %#04x \n", addr);)
+		return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - PIO address to read
+RETURN:
+0
+REMARKS:
+Default PIO word read function. Doesn't perform real inw.
+****************************************************************************/
+static u16 X86API p_inw(X86EMU_pioAddr addr)
+{
+	DB(if (DEBUG_IO_TRACE())
+	   printk("inw %#04x \n", addr);)
+		return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - PIO address to read
+RETURN:
+0
+REMARKS:
+Default PIO long read function. Doesn't perform real inl.
+****************************************************************************/
+static u32 X86API p_inl(X86EMU_pioAddr addr)
+{
+	DB(if (DEBUG_IO_TRACE())
+	   printk("inl %#04x \n", addr);)
+		return 0;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - PIO address to write
+val     - Value to store
+REMARKS:
+Default PIO byte write function. Doesn't perform real outb.
+****************************************************************************/
+static void X86API p_outb(X86EMU_pioAddr addr, u8 val)
+{
+	DB(if (DEBUG_IO_TRACE())
+	   printk("outb %#02x -> %#04x \n", val, addr);)
+		return;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - PIO address to write
+val     - Value to store
+REMARKS:
+Default PIO word write function. Doesn't perform real outw.
+****************************************************************************/
+static void X86API p_outw(X86EMU_pioAddr addr, u16 val)
+{
+	DB(if (DEBUG_IO_TRACE())
+	   printk("outw %#04x -> %#04x \n", val, addr);)
+		return;
+}
+
+/****************************************************************************
+PARAMETERS:
+addr    - PIO address to write
+val     - Value to store
+REMARKS:
+Default PIO ;ong write function. Doesn't perform real outl.
+****************************************************************************/
+static void X86API p_outl(X86EMU_pioAddr addr, u32 val)
+{
+	DB(if (DEBUG_IO_TRACE())
+	   printk("outl %#08x -> %#04x \n", val, addr);)
+		return;
+}
+
+/*------------------------- Global Variables ------------------------------*/
+
+u8(X86APIP sys_rdb) (u32 addr) = rdb;
+u16(X86APIP sys_rdw) (u32 addr) = rdw;
+u32(X86APIP sys_rdl) (u32 addr) = rdl;
+void (X86APIP sys_wrb) (u32 addr, u8 val) = wrb;
+void (X86APIP sys_wrw) (u32 addr, u16 val) = wrw;
+void (X86APIP sys_wrl) (u32 addr, u32 val) = wrl;
+u8(X86APIP sys_inb) (X86EMU_pioAddr addr) = p_inb;
+u16(X86APIP sys_inw) (X86EMU_pioAddr addr) = p_inw;
+u32(X86APIP sys_inl) (X86EMU_pioAddr addr) = p_inl;
+void (X86APIP sys_outb) (X86EMU_pioAddr addr, u8 val) = p_outb;
+void (X86APIP sys_outw) (X86EMU_pioAddr addr, u16 val) = p_outw;
+void (X86APIP sys_outl) (X86EMU_pioAddr addr, u32 val) = p_outl;
+
+/*----------------------------- Setup -------------------------------------*/
+
+/****************************************************************************
+PARAMETERS:
+funcs   - New memory function pointers to make active
+
+REMARKS:
+This function is used to set the pointers to functions which access
+memory space, allowing the user application to override these functions
+and hook them out as necessary for their application.
+****************************************************************************/
+void X86EMU_setupMemFuncs(X86EMU_memFuncs * funcs)
+{
+	sys_rdb = funcs->rdb;
+	sys_rdw = funcs->rdw;
+	sys_rdl = funcs->rdl;
+	sys_wrb = funcs->wrb;
+	sys_wrw = funcs->wrw;
+	sys_wrl = funcs->wrl;
+}
+
+/****************************************************************************
+PARAMETERS:
+funcs   - New programmed I/O function pointers to make active
+
+REMARKS:
+This function is used to set the pointers to functions which access
+I/O space, allowing the user application to override these functions
+and hook them out as necessary for their application.
+****************************************************************************/
+void X86EMU_setupPioFuncs(X86EMU_pioFuncs * funcs)
+{
+	sys_inb = funcs->inb;
+	sys_inw = funcs->inw;
+	sys_inl = funcs->inl;
+	sys_outb = funcs->outb;
+	sys_outw = funcs->outw;
+	sys_outl = funcs->outl;
+}
+
+/****************************************************************************
+PARAMETERS:
+funcs   - New interrupt vector table to make active
+
+REMARKS:
+This function is used to set the pointers to functions which handle
+interrupt processing in the emulator, allowing the user application to
+hook interrupts as necessary for their application. Any interrupts that
+are not hooked by the user application, and reflected and handled internally
+in the emulator via the interrupt vector table. This allows the application
+to get control when the code being emulated executes specific software
+interrupts.
+****************************************************************************/
+void X86EMU_setupIntrFuncs(X86EMU_intrFuncs funcs[])
+{
+	int i;
+
+	for (i = 0; i < 256; i++)
+		_X86EMU_intrTab[i] = NULL;
+	if (funcs) {
+		for (i = 0; i < 256; i++)
+			_X86EMU_intrTab[i] = funcs[i];
+	}
+}
+
+/****************************************************************************
+PARAMETERS:
+int - New software interrupt to prepare for
+
+REMARKS:
+This function is used to set up the emulator state to exceute a software
+interrupt. This can be used by the user application code to allow an
+interrupt to be hooked, examined and then reflected back to the emulator
+so that the code in the emulator will continue processing the software
+interrupt as per normal. This essentially allows system code to actively
+hook and handle certain software interrupts as necessary.
+****************************************************************************/
+void X86EMU_prepareForInt(int num)
+{
+	push_word((u16) M.x86.R_FLG);
+	CLEAR_FLAG(F_IF);
+	CLEAR_FLAG(F_TF);
+	push_word(M.x86.R_CS);
+	M.x86.R_CS = mem_access_word(num * 4 + 2);
+	push_word(M.x86.R_IP);
+	M.x86.R_IP = mem_access_word(num * 4);
+	M.x86.intr = 0;
+}
+
+#endif
diff --git a/drivers/cfb_console.c b/drivers/cfb_console.c
index 9727aebbcb2..bcf877194e0 100644
--- a/drivers/cfb_console.c
+++ b/drivers/cfb_console.c
@@ -63,7 +63,7 @@
 			       loop in VIDEO_TSTC_FCT (i8042)
  CFG_CONSOLE_BLINK_COUNT     - value for delay loop - blink rate
  CONFIG_CONSOLE_TIME	     - display time/date in upper right corner,
-			       needs CFG_CMD_DATE and CONFIG_CONSOLE_CURSOR
+			       needs CONFIG_CMD_DATE and CONFIG_CONSOLE_CURSOR
  CONFIG_VIDEO_LOGO	     - display Linux Logo in upper left corner
  CONFIG_VIDEO_BMP_LOGO	     - use bmp_logo instead of linux_logo
  CONFIG_CONSOLE_EXTRA_INFO   - display additional board information strings
@@ -175,15 +175,15 @@ CONFIG_VIDEO_HW_CURSOR:	     - Uses the hardware cursor capability of the
 #include <linux/types.h>
 #include <devices.h>
 #include <video_font.h>
-#ifdef CFG_CMD_DATE
-#include <rtc.h>
 
+#if defined(CONFIG_CMD_DATE)
+#include <rtc.h>
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_BMP) || defined(CONFIG_SPLASH_SCREEN)
+#if defined(CONFIG_CMD_BMP) || defined(CONFIG_SPLASH_SCREEN)
 #include <watchdog.h>
 #include <bmp_layout.h>
-#endif /* (CONFIG_COMMANDS & CFG_CMD_BMP) || CONFIG_SPLASH_SCREEN */
+#endif
 
 /*****************************************************************************/
 /* Cursor definition:							     */
@@ -709,7 +709,7 @@ void video_puts (const char *s)
 
 /*****************************************************************************/
 
-#if (CONFIG_COMMANDS & CFG_CMD_BMP) || defined(CONFIG_SPLASH_SCREEN)
+#if defined(CONFIG_CMD_BMP) || defined(CONFIG_SPLASH_SCREEN)
 
 #define FILL_8BIT_332RGB(r,g,b)	{			\
 	*fb = ((r>>5)<<5) | ((g>>5)<<2) | (b>>6);	\
@@ -1004,7 +1004,7 @@ int video_display_bitmap (ulong bmp_image, int x, int y)
 
 	return (0);
 }
-#endif /* (CONFIG_COMMANDS & CFG_CMD_BMP) || CONFIG_SPLASH_SCREEN */
+#endif
 
 /*****************************************************************************/
 
diff --git a/drivers/cs8900.c b/drivers/cs8900.c
index 082434ca289..80c4ba21a68 100644
--- a/drivers/cs8900.c
+++ b/drivers/cs8900.c
@@ -43,7 +43,7 @@
 
 #ifdef CONFIG_DRIVER_CS8900
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 #undef DEBUG
 
diff --git a/drivers/dc2114x.c b/drivers/dc2114x.c
index c43cd5ec2fe..d5275dceb0f 100644
--- a/drivers/dc2114x.c
+++ b/drivers/dc2114x.c
@@ -20,8 +20,8 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) \
-	&& defined(CONFIG_TULIP)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_TULIP)
 
 #include <malloc.h>
 #include <net.h>
@@ -768,4 +768,4 @@ static void update_srom(struct eth_device *dev, bd_t *bis)
 }
 #endif	/* UPDATE_SROM */
 
-#endif	/* CFG_CMD_NET && CONFIG_NET_MULTI && CONFIG_TULIP */
+#endif
diff --git a/drivers/e1000.c b/drivers/e1000.c
index 927acbb2673..f0741da82b2 100644
--- a/drivers/e1000.c
+++ b/drivers/e1000.c
@@ -44,8 +44,8 @@ tested on both gig copper and gig fiber boards
 
 #include "e1000.h"
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
-	defined(CONFIG_E1000)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_E1000)
 
 #define TOUT_LOOP   100000
 
diff --git a/drivers/eepro100.c b/drivers/eepro100.c
index 04c17f69f6e..738146e6618 100644
--- a/drivers/eepro100.c
+++ b/drivers/eepro100.c
@@ -30,8 +30,8 @@
 
 #undef DEBUG
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
-	defined(CONFIG_EEPRO100)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_EEPRO100)
 
 	/* Ethernet chip registers.
 	 */
@@ -272,7 +272,7 @@ static inline void OUTL (struct eth_device *dev, int command, u_long addr)
 	*(volatile u32 *) ((addr + dev->iobase)) = cpu_to_le32 (command);
 }
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII)
 static inline int INL (struct eth_device *dev, u_long addr)
 {
 	return le32_to_cpu (*(volatile u32 *) (addr + dev->iobase));
@@ -386,7 +386,7 @@ static int eepro100_miiphy_write (char *devname, unsigned char addr,
 	return 0;
 }
 
-#endif /* defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII) */
+#endif
 
 /* Wait for the chip get the command.
 */
@@ -462,7 +462,7 @@ int eepro100_initialize (bd_t * bis)
 
 		eth_register (dev);
 
-#if defined (CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined (CONFIG_MII) || defined(CONFIG_CMD_MII)
 		/* register mii command access routines */
 		miiphy_register(dev->name,
 				eepro100_miiphy_read, eepro100_miiphy_write);
diff --git a/drivers/enc28j60.c b/drivers/enc28j60.c
index 98303aceee9..98303aceee9 100755..100644
--- a/drivers/enc28j60.c
+++ b/drivers/enc28j60.c
diff --git a/drivers/fsl_i2c.c b/drivers/fsl_i2c.c
index ebae5af154e..22485ea916f 100644
--- a/drivers/fsl_i2c.c
+++ b/drivers/fsl_i2c.c
@@ -69,9 +69,10 @@ i2c_init(int speed, int slaveadd)
 	dev = (struct fsl_i2c *) (CFG_IMMR + CFG_I2C2_OFFSET);
 
 	writeb(0, &dev->cr);			/* stop I2C controller */
+	udelay(5);				/* let it shutdown in peace */
 	writeb(0x3F, &dev->fdr);		/* set bus speed */
 	writeb(0x3F, &dev->dfsrr);		/* set default filter */
-	writeb(slaveadd, &dev->adr);		/* write slave address */
+	writeb(slaveadd << 1, &dev->adr);	/* write slave address */
 	writeb(0x0, &dev->sr);			/* clear status register */
 	writeb(I2C_CR_MEN, &dev->cr);		/* start I2C controller */
 #endif	/* CFG_I2C2_OFFSET */
diff --git a/drivers/fsl_pci_init.c b/drivers/fsl_pci_init.c
new file mode 100644
index 00000000000..1d1f6df997a
--- /dev/null
+++ b/drivers/fsl_pci_init.c
@@ -0,0 +1,180 @@
+/*
+ * Copyright 2007 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * Version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+#define DEBUG
+#include <common.h>
+
+#ifdef CONFIG_FSL_PCI_INIT
+
+/*
+ * PCI/PCIE Controller initialization for mpc85xx/mpc86xx soc's
+ *
+ * Initialize controller and call the common driver/pci pci_hose_scan to
+ * scan for bridges and devices.
+ *
+ * Hose fields which need to be pre-initialized by board specific code:
+ *   regions[]
+ *   first_busno
+ *
+ * Fields updated:
+ *   last_busno
+ */
+
+#include <pci.h>
+#include <asm/immap_fsl_pci.h>
+
+void pciauto_prescan_setup_bridge(struct pci_controller *hose,
+				pci_dev_t dev, int sub_bus);
+void pciauto_postscan_setup_bridge(struct pci_controller *hose,
+				pci_dev_t dev, int sub_bus);
+
+void pciauto_config_init(struct pci_controller *hose);
+void
+fsl_pci_init(struct pci_controller *hose)
+{
+	u16 temp16;
+	u32 temp32;
+	int busno = hose->first_busno;
+	int enabled;
+	u16 ltssm;
+	u8 temp8;
+	int r;
+	int bridge;
+	volatile ccsr_fsl_pci_t *pci = (ccsr_fsl_pci_t *) hose->cfg_addr;
+	pci_dev_t dev = PCI_BDF(busno,0,0);
+
+	/* Initialize ATMU registers based on hose regions and flags */
+	volatile pot_t *po=&pci->pot[1];	/* skip 0 */
+	volatile pit_t *pi=&pci->pit[0];	/* ranges from: 3 to 1 */
+
+#ifdef DEBUG
+	int neg_link_w;
+#endif
+
+	for (r=0; r<hose->region_count; r++) {
+		if (hose->regions[r].flags & PCI_REGION_MEMORY) { /* inbound */
+			pi->pitar = (hose->regions[r].bus_start >> 12) & 0x000fffff;
+			pi->piwbar = (hose->regions[r].phys_start >> 12) & 0x000fffff;
+			pi->piwbear = 0;
+			pi->piwar = PIWAR_EN | PIWAR_PF | PIWAR_LOCAL |
+				PIWAR_READ_SNOOP | PIWAR_WRITE_SNOOP |
+				(__ilog2(hose->regions[r].size) - 1);
+			pi++;
+		} else { /* Outbound */
+			po->powbar = (hose->regions[r].phys_start >> 12) & 0x000fffff;
+			po->potar = (hose->regions[r].bus_start >> 12) & 0x000fffff;
+			po->potear = 0;
+			if (hose->regions[r].flags & PCI_REGION_IO)
+				po->powar = POWAR_EN | POWAR_IO_READ | POWAR_IO_WRITE |
+					(__ilog2(hose->regions[r].size) - 1);
+			else
+				po->powar = POWAR_EN | POWAR_MEM_READ | POWAR_MEM_WRITE |
+					(__ilog2(hose->regions[r].size) - 1);
+			po++;
+		}
+	}
+
+	pci_register_hose(hose);
+	pciauto_config_init(hose);	/* grab pci_{mem,prefetch,io} */
+	hose->current_busno = hose->first_busno;
+
+	pci->pedr = 0xffffffff;		/* Clear any errors */
+	pci->peer = 0xffffffff;		/* Enable Error Interupts */
+	pci_hose_read_config_dword (hose, dev, PCI_DCR, &temp32);
+	temp32 |= 0xf000e;		/* set URR, FER, NFER (but not CER) */
+	pci_hose_write_config_dword(hose, dev, PCI_DCR, temp32);
+
+	pci_hose_read_config_byte (hose, dev, PCI_HEADER_TYPE, &temp8);
+	bridge = temp8 & PCI_HEADER_TYPE_BRIDGE; /* Bridge, such as pcie */
+
+	if ( bridge ) {
+
+		pci_hose_read_config_word(hose, dev, PCI_LTSSM, &ltssm);
+		enabled = ltssm >= PCI_LTSSM_L0;
+
+		if (!enabled) {
+			debug("....PCIE link error.  Skipping scan."
+			      "LTSSM=0x%02x\n", temp16);
+			hose->last_busno = hose->first_busno;
+			return;
+		}
+
+		pci->pme_msg_det = 0xffffffff;
+		pci->pme_msg_int_en = 0xffffffff;
+#ifdef DEBUG
+		pci_hose_read_config_word(hose, dev, PCI_LSR, &temp16);
+		neg_link_w = (temp16 & 0x3f0 ) >> 4;
+		debug("...PCIE LTSSM=0x%x, Negotiated link width=%d\n",
+		      ltssm, neg_link_w);
+#endif
+		hose->current_busno++; /* Start scan with secondary */
+		pciauto_prescan_setup_bridge(hose, dev, hose->current_busno);
+
+	} else {
+#if 0
+/* done in pci_hose_config_device() */
+		pci_hose_read_config_word(hose, dev, PCI_COMMAND, &temp16);
+		temp16 |= PCI_COMMAND_SERR | PCI_COMMAND_MASTER |
+			PCI_COMMAND_MEMORY | PCI_COMMAND_IO;
+		pci_hose_write_config_word(hose, dev, PCI_COMMAND, temp16);
+		pci_hose_write_config_word(hose, dev, PCI_STATUS, 0xffff);
+		pci_hose_write_config_byte(hose, dev, PCI_LATENCY_TIMER, 0x80);
+#endif
+	}
+
+	/* Call setup to allocate PCSRBAR window */
+	pciauto_setup_device(hose, dev, 1, hose->pci_mem,
+			     hose->pci_prefetch, hose->pci_io);
+
+	printf ("               Scanning PCI bus %02x\n", hose->current_busno);
+	hose->last_busno = pci_hose_scan_bus(hose,hose->current_busno);
+
+	if ( bridge ) { /* update limit regs and subordinate busno */
+		pciauto_postscan_setup_bridge(hose, dev, hose->last_busno);
+	}
+
+	/* Clear all error indications */
+
+	if (pci->pme_msg_det && pci->pme_msg_det != 0xffffffff) {
+		debug("pci_fsl_init: pme_msg_det@%x=%x.  Clearing\n",
+			&pci->pme_msg_det, pci->pme_msg_det);
+		pci->pme_msg_det = 0xffffffff;
+	}
+
+	if (pci->pedr) {
+		debug("pci_fsl_init: pedr@%x=%x.  Clearing\n",
+			&pci->pedr, pci->pedr);
+		pci->pedr = 0xffffffff;
+	}
+
+	pci_hose_read_config_word (hose, dev, PCI_DSR, &temp16);
+	if (temp16) {
+		debug("pci_fsl_init: PCI_DSR@%x=%x.  Clearing\n",
+			PCI_DSR, temp16);
+		pci_hose_write_config_word(hose, dev,
+					   PCI_DSR, 0xffff);
+	}
+
+	pci_hose_read_config_word (hose, dev, PCI_SEC_STATUS, &temp16);
+	if (temp16) {
+		debug("pci_fsl_init: PCI_SEC_STATUS@%x=%x.  Clearing\n",
+			PCI_SEC_STATUS, temp16);
+		pci_hose_write_config_word(hose, dev, PCI_SEC_STATUS, 0xffff);
+	}
+}
+
+#endif /* CONFIG_FSL_PCI */
diff --git a/drivers/inca-ip_sw.c b/drivers/inca-ip_sw.c
index ab22b4d5385..e4aaed6afbc 100644
--- a/drivers/inca-ip_sw.c
+++ b/drivers/inca-ip_sw.c
@@ -26,8 +26,8 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) \
-	&& defined(CONFIG_INCA_IP_SWITCH)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_INCA_IP_SWITCH)
 
 #include <malloc.h>
 #include <net.h>
diff --git a/drivers/isp116x-hcd.c b/drivers/isp116x-hcd.c
new file mode 100644
index 00000000000..8e2bc7adcc9
--- /dev/null
+++ b/drivers/isp116x-hcd.c
@@ -0,0 +1,1413 @@
+/*
+ * ISP116x HCD (Host Controller Driver) for u-boot.
+ *
+ * Copyright (C) 2006-2007 Rodolfo Giometti <giometti@linux.it>
+ * Copyright (C) 2006-2007 Eurotech S.p.A. <info@eurotech.it>
+ *
+ * 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 the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ *
+ * Derived in part from the SL811 HCD driver "u-boot/drivers/sl811_usb.c"
+ * (original copyright message follows):
+ *
+ *    (C) Copyright 2004
+ *    Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ *    This code is based on linux driver for sl811hs chip, source at
+ *    drivers/usb/host/sl811.c:
+ *
+ *    SL811 Host Controller Interface driver for USB.
+ *
+ *    Copyright (c) 2003/06, Courage Co., Ltd.
+ *
+ *    Based on:
+ *         1.uhci.c by Linus Torvalds, Johannes Erdfelt, Randy Dunlap,
+ *           Georg Acher, Deti Fliegl, Thomas Sailer, Roman Weissgaerber,
+ *           Adam Richter, Gregory P. Smith;
+ *         2.Original SL811 driver (hc_sl811.o) by Pei Liu <pbl@cypress.com>
+ *         3.Rewrited as sl811.o by Yin Aihua <yinah:couragetech.com.cn>
+ *
+ *    [[GNU/GPL disclaimer]]
+ *
+ * and in part from AU1x00 OHCI HCD driver "u-boot/cpu/mips/au1x00_usb_ohci.c"
+ * (original copyright message follows):
+ *
+ *    URB OHCI HCD (Host Controller Driver) for USB on the AU1x00.
+ *
+ *    (C) Copyright 2003
+ *    Gary Jennejohn, DENX Software Engineering <gj@denx.de>
+ *
+ *    [[GNU/GPL disclaimer]]
+ *
+ *    Note: Part of this code has been derived from linux
+ */
+
+#include <common.h>
+
+#ifdef CONFIG_USB_ISP116X_HCD
+#include <asm/io.h>
+#include <usb.h>
+#include <malloc.h>
+#include <linux/list.h>
+
+/*
+ * ISP116x chips require certain delays between accesses to its
+ * registers. The following timing options exist.
+ *
+ * 1. Configure your memory controller (the best)
+ * 2. Use ndelay (easiest, poorest). For that, enable the following macro.
+ *
+ * Value is in microseconds.
+ */
+#ifdef ISP116X_HCD_USE_UDELAY
+#define UDELAY		1
+#endif
+
+/*
+ * On some (slowly?) machines an extra delay after data packing into
+ * controller's FIFOs is required, * otherwise you may get the following
+ * error:
+ *
+ *   uboot> usb start
+ *   (Re)start USB...
+ *   USB:   scanning bus for devices... isp116x: isp116x_submit_job: CTL:TIMEOUT
+ *   isp116x: isp116x_submit_job: ****** FIFO not ready! ******
+ *
+ *         USB device not responding, giving up (status=4)
+ *         isp116x: isp116x_submit_job: ****** FIFO not empty! ******
+ *         isp116x: isp116x_submit_job: ****** FIFO not empty! ******
+ *         isp116x: isp116x_submit_job: ****** FIFO not empty! ******
+ *         3 USB Device(s) found
+ *                scanning bus for storage devices... 0 Storage Device(s) found
+ *
+ * Value is in milliseconds.
+ */
+#ifdef ISP116X_HCD_USE_EXTRA_DELAY
+#define EXTRA_DELAY	2
+#endif
+
+/*
+ * Enable the following defines if you wish enable debugging messages.
+ */
+#undef DEBUG			/* enable debugging messages */
+#undef TRACE			/* enable tracing code */
+#undef VERBOSE			/* verbose debugging messages */
+
+#include "isp116x.h"
+
+#define DRIVER_VERSION	"08 Jan 2007"
+static const char hcd_name[] = "isp116x-hcd";
+
+struct isp116x isp116x_dev;
+struct isp116x_platform_data isp116x_board;
+int got_rhsc = 0;		/* root hub status change */
+struct usb_device *devgone;	/* device which was disconnected */
+int rh_devnum = 0;		/* address of Root Hub endpoint */
+
+/* ------------------------------------------------------------------------- */
+
+#define ALIGN(x,a)	(((x)+(a)-1UL)&~((a)-1UL))
+#define min_t(type,x,y)	\
+	({ type __x = (x); type __y = (y); __x < __y ? __x : __y; })
+
+/* ------------------------------------------------------------------------- */
+
+static int isp116x_reset(struct isp116x *isp116x);
+
+/* --- Debugging functions ------------------------------------------------- */
+
+#define isp116x_show_reg(d, r) {				\
+	if ((r) < 0x20) {					\
+		DBG("%-12s[%02x]: %08x", #r,			\
+			r, isp116x_read_reg32(d, r));		\
+	} else {						\
+		DBG("%-12s[%02x]:     %04x", #r,		\
+			r, isp116x_read_reg16(d, r));  		\
+	}							\
+}
+
+#define isp116x_show_regs(d) {					\
+	isp116x_show_reg(d, HCREVISION);			\
+	isp116x_show_reg(d, HCCONTROL);				\
+	isp116x_show_reg(d, HCCMDSTAT);				\
+	isp116x_show_reg(d, HCINTSTAT);				\
+	isp116x_show_reg(d, HCINTENB);				\
+	isp116x_show_reg(d, HCFMINTVL);				\
+	isp116x_show_reg(d, HCFMREM);				\
+	isp116x_show_reg(d, HCFMNUM);				\
+	isp116x_show_reg(d, HCLSTHRESH);			\
+	isp116x_show_reg(d, HCRHDESCA);				\
+	isp116x_show_reg(d, HCRHDESCB);				\
+	isp116x_show_reg(d, HCRHSTATUS);			\
+	isp116x_show_reg(d, HCRHPORT1);				\
+	isp116x_show_reg(d, HCRHPORT2);				\
+	isp116x_show_reg(d, HCHWCFG);				\
+	isp116x_show_reg(d, HCDMACFG);				\
+	isp116x_show_reg(d, HCXFERCTR);				\
+	isp116x_show_reg(d, HCuPINT);				\
+	isp116x_show_reg(d, HCuPINTENB);			\
+	isp116x_show_reg(d, HCCHIPID);				\
+	isp116x_show_reg(d, HCSCRATCH);				\
+	isp116x_show_reg(d, HCITLBUFLEN);			\
+	isp116x_show_reg(d, HCATLBUFLEN);			\
+	isp116x_show_reg(d, HCBUFSTAT);				\
+	isp116x_show_reg(d, HCRDITL0LEN);			\
+	isp116x_show_reg(d, HCRDITL1LEN);			\
+}
+
+#if defined(TRACE)
+
+static int isp116x_get_current_frame_number(struct usb_device *usb_dev)
+{
+	struct isp116x *isp116x = &isp116x_dev;
+
+	return isp116x_read_reg32(isp116x, HCFMNUM);
+}
+
+static void dump_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		     int len, char *str)
+{
+#if defined(VERBOSE)
+	int i;
+#endif
+
+	DBG("%s URB:[%4x] dev:%2d,ep:%2d-%c,type:%s,len:%d stat:%#lx",
+	    str,
+	    isp116x_get_current_frame_number(dev),
+	    usb_pipedevice(pipe),
+	    usb_pipeendpoint(pipe),
+	    usb_pipeout(pipe) ? 'O' : 'I',
+	    usb_pipetype(pipe) < 2 ?
+	    (usb_pipeint(pipe) ?
+	     "INTR" : "ISOC") :
+	    (usb_pipecontrol(pipe) ? "CTRL" : "BULK"), len, dev->status);
+#if defined(VERBOSE)
+	if (len > 0 && buffer) {
+		printf(__FILE__ ": data(%d):", len);
+		for (i = 0; i < 16 && i < len; i++)
+			printf(" %02x", ((__u8 *) buffer)[i]);
+		printf("%s\n", i < len ? "..." : "");
+	}
+#endif
+}
+
+#define PTD_DIR_STR(ptd)  ({char __c;		\
+	switch(PTD_GET_DIR(ptd)){		\
+	case 0:  __c = 's'; break;		\
+	case 1:  __c = 'o'; break;		\
+	default: __c = 'i'; break;		\
+	}; __c;})
+
+/*
+  Dump PTD info. The code documents the format
+  perfectly, right :)
+*/
+static inline void dump_ptd(struct ptd *ptd)
+{
+#if defined(VERBOSE)
+	int k;
+#endif
+
+	DBG("PTD(ext) : cc:%x %d%c%d %d,%d,%d t:%x %x%x%x",
+	    PTD_GET_CC(ptd),
+	    PTD_GET_FA(ptd), PTD_DIR_STR(ptd), PTD_GET_EP(ptd),
+	    PTD_GET_COUNT(ptd), PTD_GET_LEN(ptd), PTD_GET_MPS(ptd),
+	    PTD_GET_TOGGLE(ptd),
+	    PTD_GET_ACTIVE(ptd), PTD_GET_SPD(ptd), PTD_GET_LAST(ptd));
+#if defined(VERBOSE)
+	printf("isp116x: %s: PTD(byte): ", __FUNCTION__);
+	for (k = 0; k < sizeof(struct ptd); ++k)
+		printf("%02x ", ((u8 *) ptd)[k]);
+	printf("\n");
+#endif
+}
+
+static inline void dump_ptd_data(struct ptd *ptd, u8 * buf, int type)
+{
+#if defined(VERBOSE)
+	int k;
+
+	if (type == 0 /* 0ut data */ ) {
+		printf("isp116x: %s: out data: ", __FUNCTION__);
+		for (k = 0; k < PTD_GET_LEN(ptd); ++k)
+			printf("%02x ", ((u8 *) buf)[k]);
+		printf("\n");
+	}
+	if (type == 1 /* 1n data */ ) {
+		printf("isp116x: %s: in data: ", __FUNCTION__);
+		for (k = 0; k < PTD_GET_COUNT(ptd); ++k)
+			printf("%02x ", ((u8 *) buf)[k]);
+		printf("\n");
+	}
+
+	if (PTD_GET_LAST(ptd))
+		DBG("--- last PTD ---");
+#endif
+}
+
+#else
+
+#define dump_msg(dev, pipe, buffer, len, str)			do { } while (0)
+#define dump_pkt(dev, pipe, buffer, len, setup, str, small)	do {} while (0)
+
+#define dump_ptd(ptd)			do {} while (0)
+#define dump_ptd_data(ptd, buf, type)	do {} while (0)
+
+#endif
+
+/* --- Virtual Root Hub ---------------------------------------------------- */
+
+/* Device descriptor */
+static __u8 root_hub_dev_des[] = {
+	0x12,			/*  __u8  bLength; */
+	0x01,			/*  __u8  bDescriptorType; Device */
+	0x10,			/*  __u16 bcdUSB; v1.1 */
+	0x01,
+	0x09,			/*  __u8  bDeviceClass; HUB_CLASSCODE */
+	0x00,			/*  __u8  bDeviceSubClass; */
+	0x00,			/*  __u8  bDeviceProtocol; */
+	0x08,			/*  __u8  bMaxPacketSize0; 8 Bytes */
+	0x00,			/*  __u16 idVendor; */
+	0x00,
+	0x00,			/*  __u16 idProduct; */
+	0x00,
+	0x00,			/*  __u16 bcdDevice; */
+	0x00,
+	0x00,			/*  __u8  iManufacturer; */
+	0x01,			/*  __u8  iProduct; */
+	0x00,			/*  __u8  iSerialNumber; */
+	0x01			/*  __u8  bNumConfigurations; */
+};
+
+/* Configuration descriptor */
+static __u8 root_hub_config_des[] = {
+	0x09,			/*  __u8  bLength; */
+	0x02,			/*  __u8  bDescriptorType; Configuration */
+	0x19,			/*  __u16 wTotalLength; */
+	0x00,
+	0x01,			/*  __u8  bNumInterfaces; */
+	0x01,			/*  __u8  bConfigurationValue; */
+	0x00,			/*  __u8  iConfiguration; */
+	0x40,			/*  __u8  bmAttributes;
+				   Bit 7: Bus-powered, 6: Self-powered, 5 Remote-wakwup, 4..0: resvd */
+	0x00,			/*  __u8  MaxPower; */
+
+	/* interface */
+	0x09,			/*  __u8  if_bLength; */
+	0x04,			/*  __u8  if_bDescriptorType; Interface */
+	0x00,			/*  __u8  if_bInterfaceNumber; */
+	0x00,			/*  __u8  if_bAlternateSetting; */
+	0x01,			/*  __u8  if_bNumEndpoints; */
+	0x09,			/*  __u8  if_bInterfaceClass; HUB_CLASSCODE */
+	0x00,			/*  __u8  if_bInterfaceSubClass; */
+	0x00,			/*  __u8  if_bInterfaceProtocol; */
+	0x00,			/*  __u8  if_iInterface; */
+
+	/* endpoint */
+	0x07,			/*  __u8  ep_bLength; */
+	0x05,			/*  __u8  ep_bDescriptorType; Endpoint */
+	0x81,			/*  __u8  ep_bEndpointAddress; IN Endpoint 1 */
+	0x03,			/*  __u8  ep_bmAttributes; Interrupt */
+	0x00,			/*  __u16 ep_wMaxPacketSize; ((MAX_ROOT_PORTS + 1) / 8 */
+	0x02,
+	0xff			/*  __u8  ep_bInterval; 255 ms */
+};
+
+static unsigned char root_hub_str_index0[] = {
+	0x04,			/*  __u8  bLength; */
+	0x03,			/*  __u8  bDescriptorType; String-descriptor */
+	0x09,			/*  __u8  lang ID */
+	0x04,			/*  __u8  lang ID */
+};
+
+static unsigned char root_hub_str_index1[] = {
+	0x22,			/*  __u8  bLength; */
+	0x03,			/*  __u8  bDescriptorType; String-descriptor */
+	'I',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'S',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'P',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'1',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'1',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'6',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'x',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	' ',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'R',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'o',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'o',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	't',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	' ',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'H',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'u',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+	'b',			/*  __u8  Unicode */
+	0,			/*  __u8  Unicode */
+};
+
+/*
+ * Hub class-specific descriptor is constructed dynamically
+ */
+
+/* --- Virtual root hub management functions ------------------------------- */
+
+static int rh_check_port_status(struct isp116x *isp116x)
+{
+	u32 temp, ndp, i;
+	int res;
+
+	res = -1;
+	temp = isp116x_read_reg32(isp116x, HCRHSTATUS);
+	ndp = (temp & RH_A_NDP);
+	for (i = 0; i < ndp; i++) {
+		temp = isp116x_read_reg32(isp116x, HCRHPORT1 + i);
+		/* check for a device disconnect */
+		if (((temp & (RH_PS_PESC | RH_PS_CSC)) ==
+		     (RH_PS_PESC | RH_PS_CSC)) && ((temp & RH_PS_CCS) == 0)) {
+			res = i;
+			break;
+		}
+	}
+	return res;
+}
+
+/* --- HC management functions --------------------------------------------- */
+
+/* Write len bytes to fifo, pad till 32-bit boundary
+ */
+static void write_ptddata_to_fifo(struct isp116x *isp116x, void *buf, int len)
+{
+	u8 *dp = (u8 *) buf;
+	u16 *dp2 = (u16 *) buf;
+	u16 w;
+	int quot = len % 4;
+
+	if ((unsigned long)dp2 & 1) {
+		/* not aligned */
+		for (; len > 1; len -= 2) {
+			w = *dp++;
+			w |= *dp++ << 8;
+			isp116x_raw_write_data16(isp116x, w);
+		}
+		if (len)
+			isp116x_write_data16(isp116x, (u16) * dp);
+	} else {
+		/* aligned */
+		for (; len > 1; len -= 2)
+			isp116x_raw_write_data16(isp116x, *dp2++);
+		if (len)
+			isp116x_write_data16(isp116x, 0xff & *((u8 *) dp2));
+	}
+	if (quot == 1 || quot == 2)
+		isp116x_raw_write_data16(isp116x, 0);
+}
+
+/* Read len bytes from fifo and then read till 32-bit boundary
+ */
+static void read_ptddata_from_fifo(struct isp116x *isp116x, void *buf, int len)
+{
+	u8 *dp = (u8 *) buf;
+	u16 *dp2 = (u16 *) buf;
+	u16 w;
+	int quot = len % 4;
+
+	if ((unsigned long)dp2 & 1) {
+		/* not aligned */
+		for (; len > 1; len -= 2) {
+			w = isp116x_raw_read_data16(isp116x);
+			*dp++ = w & 0xff;
+			*dp++ = (w >> 8) & 0xff;
+		}
+		if (len)
+			*dp = 0xff & isp116x_read_data16(isp116x);
+	} else {
+		/* aligned */
+		for (; len > 1; len -= 2)
+			*dp2++ = isp116x_raw_read_data16(isp116x);
+		if (len)
+			*(u8 *) dp2 = 0xff & isp116x_read_data16(isp116x);
+	}
+	if (quot == 1 || quot == 2)
+		isp116x_raw_read_data16(isp116x);
+}
+
+/* Write PTD's and data for scheduled transfers into the fifo ram.
+ * Fifo must be empty and ready */
+static void pack_fifo(struct isp116x *isp116x, struct usb_device *dev,
+		      unsigned long pipe, struct ptd *ptd, int n, void *data,
+		      int len)
+{
+	int buflen = n * sizeof(struct ptd) + len;
+	int i, done;
+
+	DBG("--- pack buffer %p - %d bytes (fifo %d) ---", data, len, buflen);
+
+	isp116x_write_reg16(isp116x, HCuPINT, HCuPINT_AIIEOT);
+	isp116x_write_reg16(isp116x, HCXFERCTR, buflen);
+	isp116x_write_addr(isp116x, HCATLPORT | ISP116x_WRITE_OFFSET);
+
+	done = 0;
+	for (i = 0; i < n; i++) {
+		DBG("i=%d - done=%d - len=%d", i, done, PTD_GET_LEN(&ptd[i]));
+
+		dump_ptd(&ptd[i]);
+		isp116x_write_data16(isp116x, ptd[i].count);
+		isp116x_write_data16(isp116x, ptd[i].mps);
+		isp116x_write_data16(isp116x, ptd[i].len);
+		isp116x_write_data16(isp116x, ptd[i].faddr);
+
+		dump_ptd_data(&ptd[i], (__u8 *) data + done, 0);
+		write_ptddata_to_fifo(isp116x,
+				      (__u8 *) data + done,
+				      PTD_GET_LEN(&ptd[i]));
+
+		done += PTD_GET_LEN(&ptd[i]);
+	}
+}
+
+/* Read the processed PTD's and data from fifo ram back to URBs' buffers.
+ * Fifo must be full and done */
+static int unpack_fifo(struct isp116x *isp116x, struct usb_device *dev,
+		       unsigned long pipe, struct ptd *ptd, int n, void *data,
+		       int len)
+{
+	int buflen = n * sizeof(struct ptd) + len;
+	int i, done, cc, ret;
+
+	isp116x_write_reg16(isp116x, HCuPINT, HCuPINT_AIIEOT);
+	isp116x_write_reg16(isp116x, HCXFERCTR, buflen);
+	isp116x_write_addr(isp116x, HCATLPORT);
+
+	ret = TD_CC_NOERROR;
+	done = 0;
+	for (i = 0; i < n; i++) {
+		DBG("i=%d - done=%d - len=%d", i, done, PTD_GET_LEN(&ptd[i]));
+
+		ptd[i].count = isp116x_read_data16(isp116x);
+		ptd[i].mps = isp116x_read_data16(isp116x);
+		ptd[i].len = isp116x_read_data16(isp116x);
+		ptd[i].faddr = isp116x_read_data16(isp116x);
+		dump_ptd(&ptd[i]);
+
+		read_ptddata_from_fifo(isp116x,
+				       (__u8 *) data + done,
+				       PTD_GET_LEN(&ptd[i]));
+		dump_ptd_data(&ptd[i], (__u8 *) data + done, 1);
+
+		done += PTD_GET_LEN(&ptd[i]);
+
+		cc = PTD_GET_CC(&ptd[i]);
+		if (cc == TD_DATAUNDERRUN) {	/* underrun is no error... */
+			DBG("allowed data underrun");
+			cc = TD_CC_NOERROR;
+		}
+		if (cc != TD_CC_NOERROR && ret == TD_CC_NOERROR)
+			ret = cc;
+	}
+
+	DBG("--- unpack buffer %p - %d bytes (fifo %d) ---", data, len, buflen);
+
+	return ret;
+}
+
+/* Interrupt handling
+ */
+static int isp116x_interrupt(struct isp116x *isp116x)
+{
+	u16 irqstat;
+	u32 intstat;
+	int ret = 0;
+
+	isp116x_write_reg16(isp116x, HCuPINTENB, 0);
+	irqstat = isp116x_read_reg16(isp116x, HCuPINT);
+	isp116x_write_reg16(isp116x, HCuPINT, irqstat);
+	DBG(">>>>>> irqstat %x <<<<<<", irqstat);
+
+	if (irqstat & HCuPINT_ATL) {
+		DBG(">>>>>> HCuPINT_ATL <<<<<<");
+		udelay(500);
+		ret = 1;
+	}
+
+	if (irqstat & HCuPINT_OPR) {
+		intstat = isp116x_read_reg32(isp116x, HCINTSTAT);
+		isp116x_write_reg32(isp116x, HCINTSTAT, intstat);
+		DBG(">>>>>> HCuPINT_OPR %x <<<<<<", intstat);
+
+		if (intstat & HCINT_UE) {
+			ERR("unrecoverable error, controller disabled");
+
+			/* FIXME: be optimistic, hope that bug won't repeat
+			 * often. Make some non-interrupt context restart the
+			 * controller. Count and limit the retries though;
+			 * either hardware or software errors can go forever...
+			 */
+			isp116x_reset(isp116x);
+			ret = -1;
+			return -1;
+		}
+
+		if (intstat & HCINT_RHSC) {
+			got_rhsc = 1;
+			ret = 1;
+			/* When root hub or any of its ports is going
+			   to come out of suspend, it may take more
+			   than 10ms for status bits to stabilize. */
+			wait_ms(20);
+		}
+
+		if (intstat & HCINT_SO) {
+			ERR("schedule overrun");
+			ret = -1;
+		}
+
+		irqstat &= ~HCuPINT_OPR;
+	}
+
+	return ret;
+}
+
+#define PTD_NUM			64	/* it should be enougth... */
+struct ptd ptd[PTD_NUM];
+static inline int max_transfer_len(struct usb_device *dev, unsigned long pipe)
+{
+	return min(PTD_NUM * usb_maxpacket(dev, pipe), PTD_NUM * 16);
+}
+
+/* Do an USB transfer
+ */
+static int isp116x_submit_job(struct usb_device *dev, unsigned long pipe,
+			      int dir, void *buffer, int len)
+{
+	struct isp116x *isp116x = &isp116x_dev;
+	int type = usb_pipetype(pipe);
+	int epnum = usb_pipeendpoint(pipe);
+	int max = usb_maxpacket(dev, pipe);
+	int dir_out = usb_pipeout(pipe);
+	int speed_low = usb_pipeslow(pipe);
+	int i, done, stat, timeout, cc;
+	int retries = 10;
+
+	DBG("------------------------------------------------");
+	dump_msg(dev, pipe, buffer, len, "SUBMIT");
+	DBG("------------------------------------------------");
+
+	if (isp116x->disabled) {
+		ERR("EPIPE");
+		dev->status = USB_ST_CRC_ERR;
+		return -1;
+	}
+
+	/* device pulled? Shortcut the action. */
+	if (devgone == dev) {
+		ERR("ENODEV");
+		dev->status = USB_ST_CRC_ERR;
+		return USB_ST_CRC_ERR;
+	}
+
+	if (!max) {
+		ERR("pipesize for pipe %lx is zero", pipe);
+		dev->status = USB_ST_CRC_ERR;
+		return -1;
+	}
+
+	if (type == PIPE_ISOCHRONOUS) {
+		ERR("isochronous transfers not supported");
+		dev->status = USB_ST_CRC_ERR;
+		return -1;
+	}
+
+	/* FIFO not empty? */
+	if (isp116x_read_reg16(isp116x, HCBUFSTAT) & HCBUFSTAT_ATL_FULL) {
+		ERR("****** FIFO not empty! ******");
+		dev->status = USB_ST_BUF_ERR;
+		return -1;
+	}
+
+      retry:
+	isp116x_write_reg32(isp116x, HCINTSTAT, 0xff);
+
+	/* Prepare the PTD data */
+	done = 0;
+	i = 0;
+	do {
+		ptd[i].count = PTD_CC_MSK | PTD_ACTIVE_MSK |
+		    PTD_TOGGLE(usb_gettoggle(dev, epnum, dir_out));
+		ptd[i].mps = PTD_MPS(max) | PTD_SPD(speed_low) | PTD_EP(epnum);
+		ptd[i].len = PTD_LEN(max > len - done ? len - done : max) |
+		    PTD_DIR(dir);
+		ptd[i].faddr = PTD_FA(usb_pipedevice(pipe));
+
+		usb_dotoggle(dev, epnum, dir_out);
+		done += PTD_GET_LEN(&ptd[i]);
+		i++;
+		if (i >= PTD_NUM) {
+			ERR("****** Cannot pack buffer! ******");
+			dev->status = USB_ST_BUF_ERR;
+			return -1;
+		}
+	} while (done < len);
+	ptd[i - 1].mps |= PTD_LAST_MSK;
+
+	/* Pack data into FIFO ram */
+	pack_fifo(isp116x, dev, pipe, ptd, i, buffer, len);
+#ifdef EXTRA_DELAY
+	wait_ms(EXTRA_DELAY);
+#endif
+
+	/* Start the data transfer */
+
+	/* Allow more time for a BULK device to react - some are slow */
+	if (usb_pipetype(pipe) == PIPE_BULK)
+		timeout = 5000;
+	else
+		timeout = 100;
+
+	/* Wait for it to complete */
+	for (;;) {
+		/* Check whether the controller is done */
+		stat = isp116x_interrupt(isp116x);
+
+		if (stat < 0) {
+			dev->status = USB_ST_CRC_ERR;
+			break;
+		}
+		if (stat > 0)
+			break;
+
+		/* Check the timeout */
+		if (--timeout)
+			udelay(1);
+		else {
+			ERR("CTL:TIMEOUT ");
+			stat = USB_ST_CRC_ERR;
+			break;
+		}
+	}
+
+	/* We got an Root Hub Status Change interrupt */
+	if (got_rhsc) {
+		isp116x_show_regs(isp116x);
+
+		got_rhsc = 0;
+
+		/* Abuse timeout */
+		timeout = rh_check_port_status(isp116x);
+		if (timeout >= 0) {
+			/*
+			 * FIXME! NOTE! AAAARGH!
+			 * This is potentially dangerous because it assumes
+			 * that only one device is ever plugged in!
+			 */
+			devgone = dev;
+		}
+	}
+
+	/* Ok, now we can read transfer status */
+
+	/* FIFO not ready? */
+	if (!(isp116x_read_reg16(isp116x, HCBUFSTAT) & HCBUFSTAT_ATL_DONE)) {
+		ERR("****** FIFO not ready! ******");
+		dev->status = USB_ST_BUF_ERR;
+		return -1;
+	}
+
+	/* Unpack data from FIFO ram */
+	cc = unpack_fifo(isp116x, dev, pipe, ptd, i, buffer, len);
+
+	/* Mmm... sometime we get 0x0f as cc which is a non sense!
+	 * Just retry the transfer...
+	 */
+	if (cc == 0x0f && retries-- > 0) {
+		usb_dotoggle(dev, epnum, dir_out);
+		goto retry;
+	}
+
+	if (cc != TD_CC_NOERROR) {
+		DBG("****** completition code error %x ******", cc);
+		switch (cc) {
+		case TD_CC_BITSTUFFING:
+			dev->status = USB_ST_BIT_ERR;
+			break;
+		case TD_CC_STALL:
+			dev->status = USB_ST_STALLED;
+			break;
+		case TD_BUFFEROVERRUN:
+		case TD_BUFFERUNDERRUN:
+			dev->status = USB_ST_BUF_ERR;
+			break;
+		default:
+			dev->status = USB_ST_CRC_ERR;
+		}
+		return -cc;
+	}
+
+	dump_msg(dev, pipe, buffer, len, "SUBMIT(ret)");
+
+	dev->status = 0;
+	return done;
+}
+
+/* Adapted from au1x00_usb_ohci.c
+ */
+static int isp116x_submit_rh_msg(struct usb_device *dev, unsigned long pipe,
+				 void *buffer, int transfer_len,
+				 struct devrequest *cmd)
+{
+	struct isp116x *isp116x = &isp116x_dev;
+	u32 tmp = 0;
+
+	int leni = transfer_len;
+	int len = 0;
+	int stat = 0;
+	u32 datab[4];
+	u8 *data_buf = (u8 *) datab;
+	u16 bmRType_bReq;
+	u16 wValue;
+	u16 wIndex;
+	u16 wLength;
+
+	if ((pipe & PIPE_INTERRUPT) == PIPE_INTERRUPT) {
+		INFO("Root-Hub submit IRQ: NOT implemented");
+		return 0;
+	}
+
+	bmRType_bReq = cmd->requesttype | (cmd->request << 8);
+	wValue = swap_16(cmd->value);
+	wIndex = swap_16(cmd->index);
+	wLength = swap_16(cmd->length);
+
+	DBG("--- HUB ----------------------------------------");
+	DBG("submit rh urb, req=%x val=%#x index=%#x len=%d",
+	    bmRType_bReq, wValue, wIndex, wLength);
+	dump_msg(dev, pipe, buffer, transfer_len, "RH");
+	DBG("------------------------------------------------");
+
+	switch (bmRType_bReq) {
+	case RH_GET_STATUS:
+		DBG("RH_GET_STATUS");
+
+		*(__u16 *) data_buf = swap_16(1);
+		len = 2;
+		break;
+
+	case RH_GET_STATUS | RH_INTERFACE:
+		DBG("RH_GET_STATUS | RH_INTERFACE");
+
+		*(__u16 *) data_buf = swap_16(0);
+		len = 2;
+		break;
+
+	case RH_GET_STATUS | RH_ENDPOINT:
+		DBG("RH_GET_STATUS | RH_ENDPOINT");
+
+		*(__u16 *) data_buf = swap_16(0);
+		len = 2;
+		break;
+
+	case RH_GET_STATUS | RH_CLASS:
+		DBG("RH_GET_STATUS | RH_CLASS");
+
+		tmp = isp116x_read_reg32(isp116x, HCRHSTATUS);
+
+		*(__u32 *) data_buf = swap_32(tmp & ~(RH_HS_CRWE | RH_HS_DRWE));
+		len = 4;
+		break;
+
+	case RH_GET_STATUS | RH_OTHER | RH_CLASS:
+		DBG("RH_GET_STATUS | RH_OTHER | RH_CLASS");
+
+		tmp = isp116x_read_reg32(isp116x, HCRHPORT1 + wIndex - 1);
+		*(__u32 *) data_buf = swap_32(tmp);
+		isp116x_show_regs(isp116x);
+		len = 4;
+		break;
+
+	case RH_CLEAR_FEATURE | RH_ENDPOINT:
+		DBG("RH_CLEAR_FEATURE | RH_ENDPOINT");
+
+		switch (wValue) {
+		case RH_ENDPOINT_STALL:
+			DBG("C_HUB_ENDPOINT_STALL");
+			len = 0;
+			break;
+		}
+		break;
+
+	case RH_CLEAR_FEATURE | RH_CLASS:
+		DBG("RH_CLEAR_FEATURE | RH_CLASS");
+
+		switch (wValue) {
+		case RH_C_HUB_LOCAL_POWER:
+			DBG("C_HUB_LOCAL_POWER");
+			len = 0;
+			break;
+
+		case RH_C_HUB_OVER_CURRENT:
+			DBG("C_HUB_OVER_CURRENT");
+			isp116x_write_reg32(isp116x, HCRHSTATUS, RH_HS_OCIC);
+			len = 0;
+			break;
+		}
+		break;
+
+	case RH_CLEAR_FEATURE | RH_OTHER | RH_CLASS:
+		DBG("RH_CLEAR_FEATURE | RH_OTHER | RH_CLASS");
+
+		switch (wValue) {
+		case RH_PORT_ENABLE:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_CCS);
+			len = 0;
+			break;
+
+		case RH_PORT_SUSPEND:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_POCI);
+			len = 0;
+			break;
+
+		case RH_PORT_POWER:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_LSDA);
+			len = 0;
+			break;
+
+		case RH_C_PORT_CONNECTION:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_CSC);
+			len = 0;
+			break;
+
+		case RH_C_PORT_ENABLE:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PESC);
+			len = 0;
+			break;
+
+		case RH_C_PORT_SUSPEND:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PSSC);
+			len = 0;
+			break;
+
+		case RH_C_PORT_OVER_CURRENT:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_POCI);
+			len = 0;
+			break;
+
+		case RH_C_PORT_RESET:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PRSC);
+			len = 0;
+			break;
+
+		default:
+			ERR("invalid wValue");
+			stat = USB_ST_STALLED;
+		}
+
+		isp116x_show_regs(isp116x);
+
+		break;
+
+	case RH_SET_FEATURE | RH_OTHER | RH_CLASS:
+		DBG("RH_SET_FEATURE | RH_OTHER | RH_CLASS");
+
+		switch (wValue) {
+		case RH_PORT_SUSPEND:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PSS);
+			len = 0;
+			break;
+
+		case RH_PORT_RESET:
+			/* Spin until any current reset finishes */
+			while (1) {
+				tmp =
+				    isp116x_read_reg32(isp116x,
+						       HCRHPORT1 + wIndex - 1);
+				if (!(tmp & RH_PS_PRS))
+					break;
+				wait_ms(1);
+			}
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PRS);
+			wait_ms(10);
+
+			len = 0;
+			break;
+
+		case RH_PORT_POWER:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PPS);
+			len = 0;
+			break;
+
+		case RH_PORT_ENABLE:
+			isp116x_write_reg32(isp116x, HCRHPORT1 + wIndex - 1,
+					    RH_PS_PES);
+			len = 0;
+			break;
+
+		default:
+			ERR("invalid wValue");
+			stat = USB_ST_STALLED;
+		}
+
+		isp116x_show_regs(isp116x);
+
+		break;
+
+	case RH_SET_ADDRESS:
+		DBG("RH_SET_ADDRESS");
+
+		rh_devnum = wValue;
+		len = 0;
+		break;
+
+	case RH_GET_DESCRIPTOR:
+		DBG("RH_GET_DESCRIPTOR: %x, %d", wValue, wLength);
+
+		switch (wValue) {
+		case (USB_DT_DEVICE << 8):	/* device descriptor */
+			len = min_t(unsigned int,
+				    leni, min_t(unsigned int,
+						sizeof(root_hub_dev_des),
+						wLength));
+			data_buf = root_hub_dev_des;
+			break;
+
+		case (USB_DT_CONFIG << 8):	/* configuration descriptor */
+			len = min_t(unsigned int,
+				    leni, min_t(unsigned int,
+						sizeof(root_hub_config_des),
+						wLength));
+			data_buf = root_hub_config_des;
+			break;
+
+		case ((USB_DT_STRING << 8) | 0x00):	/* string 0 descriptors */
+			len = min_t(unsigned int,
+				    leni, min_t(unsigned int,
+						sizeof(root_hub_str_index0),
+						wLength));
+			data_buf = root_hub_str_index0;
+			break;
+
+		case ((USB_DT_STRING << 8) | 0x01):	/* string 1 descriptors */
+			len = min_t(unsigned int,
+				    leni, min_t(unsigned int,
+						sizeof(root_hub_str_index1),
+						wLength));
+			data_buf = root_hub_str_index1;
+			break;
+
+		default:
+			ERR("invalid wValue");
+			stat = USB_ST_STALLED;
+		}
+
+		break;
+
+	case RH_GET_DESCRIPTOR | RH_CLASS:
+		DBG("RH_GET_DESCRIPTOR | RH_CLASS");
+
+		tmp = isp116x_read_reg32(isp116x, HCRHDESCA);
+
+		data_buf[0] = 0x09;	/* min length; */
+		data_buf[1] = 0x29;
+		data_buf[2] = tmp & RH_A_NDP;
+		data_buf[3] = 0;
+		if (tmp & RH_A_PSM)	/* per-port power switching? */
+			data_buf[3] |= 0x01;
+		if (tmp & RH_A_NOCP)	/* no overcurrent reporting? */
+			data_buf[3] |= 0x10;
+		else if (tmp & RH_A_OCPM)	/* per-port overcurrent rep? */
+			data_buf[3] |= 0x08;
+
+		/* Corresponds to data_buf[4-7] */
+		datab[1] = 0;
+		data_buf[5] = (tmp & RH_A_POTPGT) >> 24;
+
+		tmp = isp116x_read_reg32(isp116x, HCRHDESCB);
+
+		data_buf[7] = tmp & RH_B_DR;
+		if (data_buf[2] < 7)
+			data_buf[8] = 0xff;
+		else {
+			data_buf[0] += 2;
+			data_buf[8] = (tmp & RH_B_DR) >> 8;
+			data_buf[10] = data_buf[9] = 0xff;
+		}
+
+		len = min_t(unsigned int, leni,
+			    min_t(unsigned int, data_buf[0], wLength));
+		break;
+
+	case RH_GET_CONFIGURATION:
+		DBG("RH_GET_CONFIGURATION");
+
+		*(__u8 *) data_buf = 0x01;
+		len = 1;
+		break;
+
+	case RH_SET_CONFIGURATION:
+		DBG("RH_SET_CONFIGURATION");
+
+		isp116x_write_reg32(isp116x, HCRHSTATUS, RH_HS_LPSC);
+		len = 0;
+		break;
+
+	default:
+		ERR("*** *** *** unsupported root hub command *** *** ***");
+		stat = USB_ST_STALLED;
+	}
+
+	len = min_t(int, len, leni);
+	if (buffer != data_buf)
+		memcpy(buffer, data_buf, len);
+
+	dev->act_len = len;
+	dev->status = stat;
+	DBG("dev act_len %d, status %d", dev->act_len, dev->status);
+
+	dump_msg(dev, pipe, buffer, transfer_len, "RH(ret)");
+
+	return stat;
+}
+
+/* --- Transfer functions -------------------------------------------------- */
+
+int submit_int_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		   int len, int interval)
+{
+	DBG("dev=%p pipe=%#lx buf=%p size=%d int=%d",
+	    dev, pipe, buffer, len, interval);
+
+	return -1;
+}
+
+int submit_control_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		       int len, struct devrequest *setup)
+{
+	int devnum = usb_pipedevice(pipe);
+	int epnum = usb_pipeendpoint(pipe);
+	int max = max_transfer_len(dev, pipe);
+	int dir_in = usb_pipein(pipe);
+	int done, ret;
+
+	/* Control message is for the HUB? */
+	if (devnum == rh_devnum)
+		return isp116x_submit_rh_msg(dev, pipe, buffer, len, setup);
+
+	/* Ok, no HUB message so send the message to the device */
+
+	/* Setup phase */
+	DBG("--- SETUP PHASE --------------------------------");
+	usb_settoggle(dev, epnum, 1, 0);
+	ret = isp116x_submit_job(dev, pipe,
+				 PTD_DIR_SETUP,
+				 setup, sizeof(struct devrequest));
+	if (ret < 0) {
+		DBG("control setup phase error (ret = %d", ret);
+		return -1;
+	}
+
+	/* Data phase */
+	DBG("--- DATA PHASE ---------------------------------");
+	done = 0;
+	usb_settoggle(dev, epnum, !dir_in, 1);
+	while (done < len) {
+		ret = isp116x_submit_job(dev, pipe,
+					 dir_in ? PTD_DIR_IN : PTD_DIR_OUT,
+					 (__u8 *) buffer + done,
+					 max > len - done ? len - done : max);
+		if (ret < 0) {
+			DBG("control data phase error (ret = %d)", ret);
+			return -1;
+		}
+		done += ret;
+
+		if (dir_in && ret < max)	/* short packet */
+			break;
+	}
+
+	/* Status phase */
+	DBG("--- STATUS PHASE -------------------------------");
+	usb_settoggle(dev, epnum, !dir_in, 1);
+	ret = isp116x_submit_job(dev, pipe,
+				 !dir_in ? PTD_DIR_IN : PTD_DIR_OUT, NULL, 0);
+	if (ret < 0) {
+		DBG("control status phase error (ret = %d", ret);
+		return -1;
+	}
+
+	dev->act_len = done;
+
+	dump_msg(dev, pipe, buffer, len, "DEV(ret)");
+
+	return done;
+}
+
+int submit_bulk_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		    int len)
+{
+	int dir_out = usb_pipeout(pipe);
+	int max = max_transfer_len(dev, pipe);
+	int done, ret;
+
+	DBG("--- BULK ---------------------------------------");
+	DBG("dev=%ld pipe=%ld buf=%p size=%d dir_out=%d",
+	    usb_pipedevice(pipe), usb_pipeendpoint(pipe), buffer, len, dir_out);
+
+	done = 0;
+	while (done < len) {
+		ret = isp116x_submit_job(dev, pipe,
+					 !dir_out ? PTD_DIR_IN : PTD_DIR_OUT,
+					 (__u8 *) buffer + done,
+					 max > len - done ? len - done : max);
+		if (ret < 0) {
+			DBG("error on bulk message (ret = %d)", ret);
+			return -1;
+		}
+
+		done += ret;
+
+		if (!dir_out && ret < max)	/* short packet */
+			break;
+	}
+
+	dev->act_len = done;
+
+	return 0;
+}
+
+/* --- Basic functions ----------------------------------------------------- */
+
+static int isp116x_sw_reset(struct isp116x *isp116x)
+{
+	int retries = 15;
+	int ret = 0;
+
+	DBG("");
+
+	isp116x->disabled = 1;
+
+	isp116x_write_reg16(isp116x, HCSWRES, HCSWRES_MAGIC);
+	isp116x_write_reg32(isp116x, HCCMDSTAT, HCCMDSTAT_HCR);
+	while (--retries) {
+		/* It usually resets within 1 ms */
+		wait_ms(1);
+		if (!(isp116x_read_reg32(isp116x, HCCMDSTAT) & HCCMDSTAT_HCR))
+			break;
+	}
+	if (!retries) {
+		ERR("software reset timeout");
+		ret = -1;
+	}
+	return ret;
+}
+
+static int isp116x_reset(struct isp116x *isp116x)
+{
+	unsigned long t;
+	u16 clkrdy = 0;
+	int ret, timeout = 15 /* ms */ ;
+
+	DBG("");
+
+	ret = isp116x_sw_reset(isp116x);
+	if (ret)
+		return ret;
+
+	for (t = 0; t < timeout; t++) {
+		clkrdy = isp116x_read_reg16(isp116x, HCuPINT) & HCuPINT_CLKRDY;
+		if (clkrdy)
+			break;
+		wait_ms(1);
+	}
+	if (!clkrdy) {
+		ERR("clock not ready after %dms", timeout);
+		/* After sw_reset the clock won't report to be ready, if
+		   H_WAKEUP pin is high. */
+		ERR("please make sure that the H_WAKEUP pin is pulled low!");
+		ret = -1;
+	}
+	return ret;
+}
+
+static void isp116x_stop(struct isp116x *isp116x)
+{
+	u32 val;
+
+	DBG("");
+
+	isp116x_write_reg16(isp116x, HCuPINTENB, 0);
+
+	/* Switch off ports' power, some devices don't come up
+	   after next 'start' without this */
+	val = isp116x_read_reg32(isp116x, HCRHDESCA);
+	val &= ~(RH_A_NPS | RH_A_PSM);
+	isp116x_write_reg32(isp116x, HCRHDESCA, val);
+	isp116x_write_reg32(isp116x, HCRHSTATUS, RH_HS_LPS);
+
+	isp116x_sw_reset(isp116x);
+}
+
+/*
+ *  Configure the chip. The chip must be successfully reset by now.
+ */
+static int isp116x_start(struct isp116x *isp116x)
+{
+	struct isp116x_platform_data *board = isp116x->board;
+	u32 val;
+
+	DBG("");
+
+	/* Clear interrupt status and disable all interrupt sources */
+	isp116x_write_reg16(isp116x, HCuPINT, 0xff);
+	isp116x_write_reg16(isp116x, HCuPINTENB, 0);
+
+	isp116x_write_reg16(isp116x, HCITLBUFLEN, ISP116x_ITL_BUFSIZE);
+	isp116x_write_reg16(isp116x, HCATLBUFLEN, ISP116x_ATL_BUFSIZE);
+
+	/* Hardware configuration */
+	val = HCHWCFG_DBWIDTH(1);
+	if (board->sel15Kres)
+		val |= HCHWCFG_15KRSEL;
+	/* Remote wakeup won't work without working clock */
+	if (board->remote_wakeup_enable)
+		val |= HCHWCFG_CLKNOTSTOP;
+	if (board->oc_enable)
+		val |= HCHWCFG_ANALOG_OC;
+	isp116x_write_reg16(isp116x, HCHWCFG, val);
+
+	/* --- Root hub configuration */
+	val = (25 << 24) & RH_A_POTPGT;
+	/* AN10003_1.pdf recommends RH_A_NPS (no power switching) to
+	   be always set. Yet, instead, we request individual port
+	   power switching. */
+	val |= RH_A_PSM;
+	/* Report overcurrent per port */
+	val |= RH_A_OCPM;
+	isp116x_write_reg32(isp116x, HCRHDESCA, val);
+	isp116x->rhdesca = isp116x_read_reg32(isp116x, HCRHDESCA);
+
+	val = RH_B_PPCM;
+	isp116x_write_reg32(isp116x, HCRHDESCB, val);
+	isp116x->rhdescb = isp116x_read_reg32(isp116x, HCRHDESCB);
+
+	val = 0;
+	if (board->remote_wakeup_enable)
+		val |= RH_HS_DRWE;
+	isp116x_write_reg32(isp116x, HCRHSTATUS, val);
+	isp116x->rhstatus = isp116x_read_reg32(isp116x, HCRHSTATUS);
+
+	isp116x_write_reg32(isp116x, HCFMINTVL, 0x27782edf);
+
+	/* Go operational */
+	val = HCCONTROL_USB_OPER;
+	if (board->remote_wakeup_enable)
+		val |= HCCONTROL_RWE;
+	isp116x_write_reg32(isp116x, HCCONTROL, val);
+
+	/* Disable ports to avoid race in device enumeration */
+	isp116x_write_reg32(isp116x, HCRHPORT1, RH_PS_CCS);
+	isp116x_write_reg32(isp116x, HCRHPORT2, RH_PS_CCS);
+
+	isp116x_show_regs(isp116x);
+
+	isp116x->disabled = 0;
+
+	return 0;
+}
+
+/* --- Init functions ------------------------------------------------------ */
+
+int isp116x_check_id(struct isp116x *isp116x)
+{
+	int val;
+
+	val = isp116x_read_reg16(isp116x, HCCHIPID);
+	if ((val & HCCHIPID_MASK) != HCCHIPID_MAGIC) {
+		ERR("invalid chip ID %04x", val);
+		return -1;
+	}
+
+	return 0;
+}
+
+int usb_lowlevel_init(void)
+{
+	struct isp116x *isp116x = &isp116x_dev;
+
+	DBG("");
+
+	/* Init device registers addr */
+	isp116x->addr_reg = (u16 *) ISP116X_HCD_ADDR;
+	isp116x->data_reg = (u16 *) ISP116X_HCD_DATA;
+
+	/* Setup specific board settings */
+#ifdef ISP116X_HCD_SEL15kRES
+	isp116x_board.sel15Kres = 1;
+#endif
+#ifdef ISP116X_HCD_OC_ENABLE
+	isp116x_board.oc_enable = 1;
+#endif
+#ifdef ISP116X_HCD_REMOTE_WAKEUP_ENABLE
+	isp116x_board.remote_wakeup_enable = 1;
+#endif
+	isp116x->board = &isp116x_board;
+
+	/* Try to get ISP116x silicon chip ID */
+	if (isp116x_check_id(isp116x) < 0)
+		return -1;
+
+	isp116x->disabled = 1;
+	isp116x->sleeping = 0;
+
+	isp116x_reset(isp116x);
+	isp116x_start(isp116x);
+
+	return 0;
+}
+
+int usb_lowlevel_stop(void)
+{
+	struct isp116x *isp116x = &isp116x_dev;
+
+	DBG("");
+
+	if (!isp116x->disabled)
+		isp116x_stop(isp116x);
+
+	return 0;
+}
+
+#endif				/* CONFIG_USB_ISP116X_HCD */
diff --git a/drivers/isp116x.h b/drivers/isp116x.h
new file mode 100644
index 00000000000..a3ce3b582c7
--- /dev/null
+++ b/drivers/isp116x.h
@@ -0,0 +1,489 @@
+/*
+ * ISP116x register declarations and HCD data structures
+ *
+ * Copyright (C) 2007 Rodolfo Giometti <giometti@linux.it>
+ * Copyright (C) 2007 Eurotech S.p.A. <info@eurotech.it>
+ * Copyright (C) 2005 Olav Kongas <ok@artecdesign.ee>
+ * Portions:
+ * Copyright (C) 2004 Lothar Wassmann
+ * Copyright (C) 2004 Psion Teklogix
+ * Copyright (C) 2004 David Brownell
+ *
+ * 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 the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#ifdef DEBUG
+#define DBG(fmt, args...)	\
+		printf("isp116x: %s: " fmt "\n" , __FUNCTION__ , ## args)
+#else
+#define DBG(fmt, args...)	do {} while (0)
+#endif
+
+#ifdef VERBOSE
+#    define VDBG		DBG
+#else
+#    define VDBG(fmt, args...)	do {} while (0)
+#endif
+
+#define ERR(fmt, args...)	\
+		printf("isp116x: %s: " fmt "\n" , __FUNCTION__ , ## args)
+#define WARN(fmt, args...)	\
+		printf("isp116x: %s: " fmt "\n" , __FUNCTION__ , ## args)
+#define INFO(fmt, args...)	\
+		printf("isp116x: " fmt "\n" , ## args)
+
+/* ------------------------------------------------------------------------- */
+
+/* us of 1ms frame */
+#define  MAX_LOAD_LIMIT		850
+
+/* Full speed: max # of bytes to transfer for a single urb
+   at a time must be < 1024 && must be multiple of 64.
+   832 allows transfering 4kiB within 5 frames. */
+#define MAX_TRANSFER_SIZE_FULLSPEED	832
+
+/* Low speed: there is no reason to schedule in very big
+   chunks; often the requested long transfers are for
+   string descriptors containing short strings. */
+#define MAX_TRANSFER_SIZE_LOWSPEED	64
+
+/* Bytetime (us), a rough indication of how much time it
+   would take to transfer a byte of useful data over USB */
+#define BYTE_TIME_FULLSPEED	1
+#define BYTE_TIME_LOWSPEED	20
+
+/* Buffer sizes */
+#define ISP116x_BUF_SIZE	4096
+#define ISP116x_ITL_BUFSIZE	0
+#define ISP116x_ATL_BUFSIZE	((ISP116x_BUF_SIZE) - 2*(ISP116x_ITL_BUFSIZE))
+
+#define ISP116x_WRITE_OFFSET	0x80
+
+/* --- ISP116x registers/bits ---------------------------------------------- */
+
+#define	HCREVISION	0x00
+#define	HCCONTROL	0x01
+#define		HCCONTROL_HCFS	(3 << 6)	/* host controller
+						   functional state */
+#define		HCCONTROL_USB_RESET	(0 << 6)
+#define		HCCONTROL_USB_RESUME	(1 << 6)
+#define		HCCONTROL_USB_OPER	(2 << 6)
+#define		HCCONTROL_USB_SUSPEND	(3 << 6)
+#define		HCCONTROL_RWC	(1 << 9)	/* remote wakeup connected */
+#define		HCCONTROL_RWE	(1 << 10)	/* remote wakeup enable */
+#define	HCCMDSTAT	0x02
+#define		HCCMDSTAT_HCR	(1 << 0)	/* host controller reset */
+#define		HCCMDSTAT_SOC	(3 << 16)	/* scheduling overrun count */
+#define	HCINTSTAT	0x03
+#define		HCINT_SO	(1 << 0)	/* scheduling overrun */
+#define		HCINT_WDH	(1 << 1)	/* writeback of done_head */
+#define		HCINT_SF	(1 << 2)	/* start frame */
+#define		HCINT_RD	(1 << 3)	/* resume detect */
+#define		HCINT_UE	(1 << 4)	/* unrecoverable error */
+#define		HCINT_FNO	(1 << 5)	/* frame number overflow */
+#define		HCINT_RHSC	(1 << 6)	/* root hub status change */
+#define		HCINT_OC	(1 << 30)	/* ownership change */
+#define		HCINT_MIE	(1 << 31)	/* master interrupt enable */
+#define	HCINTENB	0x04
+#define	HCINTDIS	0x05
+#define	HCFMINTVL	0x0d
+#define	HCFMREM		0x0e
+#define	HCFMNUM		0x0f
+#define	HCLSTHRESH	0x11
+#define	HCRHDESCA	0x12
+#define		RH_A_NDP	(0x3 << 0)	/* # downstream ports */
+#define		RH_A_PSM	(1 << 8)	/* power switching mode */
+#define		RH_A_NPS	(1 << 9)	/* no power switching */
+#define		RH_A_DT		(1 << 10)	/* device type (mbz) */
+#define		RH_A_OCPM	(1 << 11)	/* overcurrent protection
+						   mode */
+#define		RH_A_NOCP	(1 << 12)	/* no overcurrent protection */
+#define		RH_A_POTPGT	(0xff << 24)	/* power on -> power good
+						   time */
+#define	HCRHDESCB	0x13
+#define		RH_B_DR		(0xffff << 0)	/* device removable flags */
+#define		RH_B_PPCM	(0xffff << 16)	/* port power control mask */
+#define	HCRHSTATUS	0x14
+#define		RH_HS_LPS	(1 << 0)	/* local power status */
+#define		RH_HS_OCI	(1 << 1)	/* over current indicator */
+#define		RH_HS_DRWE	(1 << 15)	/* device remote wakeup
+						   enable */
+#define		RH_HS_LPSC	(1 << 16)	/* local power status change */
+#define		RH_HS_OCIC	(1 << 17)	/* over current indicator
+						   change */
+#define		RH_HS_CRWE	(1 << 31)	/* clear remote wakeup
+						   enable */
+#define	HCRHPORT1	0x15
+#define		RH_PS_CCS	(1 << 0)	/* current connect status */
+#define		RH_PS_PES	(1 << 1)	/* port enable status */
+#define		RH_PS_PSS	(1 << 2)	/* port suspend status */
+#define		RH_PS_POCI	(1 << 3)	/* port over current
+						   indicator */
+#define		RH_PS_PRS	(1 << 4)	/* port reset status */
+#define		RH_PS_PPS	(1 << 8)	/* port power status */
+#define		RH_PS_LSDA	(1 << 9)	/* low speed device attached */
+#define		RH_PS_CSC	(1 << 16)	/* connect status change */
+#define		RH_PS_PESC	(1 << 17)	/* port enable status change */
+#define		RH_PS_PSSC	(1 << 18)	/* port suspend status
+						   change */
+#define		RH_PS_OCIC	(1 << 19)	/* over current indicator
+						   change */
+#define		RH_PS_PRSC	(1 << 20)	/* port reset status change */
+#define		HCRHPORT_CLRMASK	(0x1f << 16)
+#define	HCRHPORT2	0x16
+#define	HCHWCFG		0x20
+#define		HCHWCFG_15KRSEL		(1 << 12)
+#define		HCHWCFG_CLKNOTSTOP	(1 << 11)
+#define		HCHWCFG_ANALOG_OC	(1 << 10)
+#define		HCHWCFG_DACK_MODE	(1 << 8)
+#define		HCHWCFG_EOT_POL		(1 << 7)
+#define		HCHWCFG_DACK_POL	(1 << 6)
+#define		HCHWCFG_DREQ_POL	(1 << 5)
+#define		HCHWCFG_DBWIDTH_MASK	(0x03 << 3)
+#define		HCHWCFG_DBWIDTH(n)	(((n) << 3) & HCHWCFG_DBWIDTH_MASK)
+#define		HCHWCFG_INT_POL		(1 << 2)
+#define		HCHWCFG_INT_TRIGGER	(1 << 1)
+#define		HCHWCFG_INT_ENABLE	(1 << 0)
+#define	HCDMACFG	0x21
+#define		HCDMACFG_BURST_LEN_MASK	(0x03 << 5)
+#define		HCDMACFG_BURST_LEN(n)	(((n) << 5) & HCDMACFG_BURST_LEN_MASK)
+#define		HCDMACFG_BURST_LEN_1	HCDMACFG_BURST_LEN(0)
+#define		HCDMACFG_BURST_LEN_4	HCDMACFG_BURST_LEN(1)
+#define		HCDMACFG_BURST_LEN_8	HCDMACFG_BURST_LEN(2)
+#define		HCDMACFG_DMA_ENABLE	(1 << 4)
+#define		HCDMACFG_BUF_TYPE_MASK	(0x07 << 1)
+#define		HCDMACFG_CTR_SEL	(1 << 2)
+#define		HCDMACFG_ITLATL_SEL	(1 << 1)
+#define		HCDMACFG_DMA_RW_SELECT	(1 << 0)
+#define	HCXFERCTR	0x22
+#define	HCuPINT		0x24
+#define		HCuPINT_SOF		(1 << 0)
+#define		HCuPINT_ATL		(1 << 1)
+#define		HCuPINT_AIIEOT		(1 << 2)
+#define		HCuPINT_OPR		(1 << 4)
+#define		HCuPINT_SUSP		(1 << 5)
+#define		HCuPINT_CLKRDY		(1 << 6)
+#define	HCuPINTENB	0x25
+#define	HCCHIPID	0x27
+#define		HCCHIPID_MASK		0xff00
+#define		HCCHIPID_MAGIC		0x6100
+#define	HCSCRATCH	0x28
+#define	HCSWRES		0x29
+#define		HCSWRES_MAGIC		0x00f6
+#define	HCITLBUFLEN	0x2a
+#define	HCATLBUFLEN	0x2b
+#define	HCBUFSTAT	0x2c
+#define		HCBUFSTAT_ITL0_FULL	(1 << 0)
+#define		HCBUFSTAT_ITL1_FULL	(1 << 1)
+#define		HCBUFSTAT_ATL_FULL	(1 << 2)
+#define		HCBUFSTAT_ITL0_DONE	(1 << 3)
+#define		HCBUFSTAT_ITL1_DONE	(1 << 4)
+#define		HCBUFSTAT_ATL_DONE	(1 << 5)
+#define	HCRDITL0LEN	0x2d
+#define	HCRDITL1LEN	0x2e
+#define	HCITLPORT	0x40
+#define	HCATLPORT	0x41
+
+/* PTD accessor macros. */
+#define PTD_GET_COUNT(p)	(((p)->count & PTD_COUNT_MSK) >> 0)
+#define PTD_COUNT(v)		(((v) << 0) & PTD_COUNT_MSK)
+#define PTD_GET_TOGGLE(p)	(((p)->count & PTD_TOGGLE_MSK) >> 10)
+#define PTD_TOGGLE(v)		(((v) << 10) & PTD_TOGGLE_MSK)
+#define PTD_GET_ACTIVE(p)	(((p)->count & PTD_ACTIVE_MSK) >> 11)
+#define PTD_ACTIVE(v)		(((v) << 11) & PTD_ACTIVE_MSK)
+#define PTD_GET_CC(p)		(((p)->count & PTD_CC_MSK) >> 12)
+#define PTD_CC(v)		(((v) << 12) & PTD_CC_MSK)
+#define PTD_GET_MPS(p)		(((p)->mps & PTD_MPS_MSK) >> 0)
+#define PTD_MPS(v)		(((v) << 0) & PTD_MPS_MSK)
+#define PTD_GET_SPD(p)		(((p)->mps & PTD_SPD_MSK) >> 10)
+#define PTD_SPD(v)		(((v) << 10) & PTD_SPD_MSK)
+#define PTD_GET_LAST(p)		(((p)->mps & PTD_LAST_MSK) >> 11)
+#define PTD_LAST(v)		(((v) << 11) & PTD_LAST_MSK)
+#define PTD_GET_EP(p)		(((p)->mps & PTD_EP_MSK) >> 12)
+#define PTD_EP(v)		(((v) << 12) & PTD_EP_MSK)
+#define PTD_GET_LEN(p)		(((p)->len & PTD_LEN_MSK) >> 0)
+#define PTD_LEN(v)		(((v) << 0) & PTD_LEN_MSK)
+#define PTD_GET_DIR(p)		(((p)->len & PTD_DIR_MSK) >> 10)
+#define PTD_DIR(v)		(((v) << 10) & PTD_DIR_MSK)
+#define PTD_GET_B5_5(p)		(((p)->len & PTD_B5_5_MSK) >> 13)
+#define PTD_B5_5(v)		(((v) << 13) & PTD_B5_5_MSK)
+#define PTD_GET_FA(p)		(((p)->faddr & PTD_FA_MSK) >> 0)
+#define PTD_FA(v)		(((v) << 0) & PTD_FA_MSK)
+#define PTD_GET_FMT(p)		(((p)->faddr & PTD_FMT_MSK) >> 7)
+#define PTD_FMT(v)		(((v) << 7) & PTD_FMT_MSK)
+
+/*  Hardware transfer status codes -- CC from ptd->count */
+#define TD_CC_NOERROR      0x00
+#define TD_CC_CRC          0x01
+#define TD_CC_BITSTUFFING  0x02
+#define TD_CC_DATATOGGLEM  0x03
+#define TD_CC_STALL        0x04
+#define TD_DEVNOTRESP      0x05
+#define TD_PIDCHECKFAIL    0x06
+#define TD_UNEXPECTEDPID   0x07
+#define TD_DATAOVERRUN     0x08
+#define TD_DATAUNDERRUN    0x09
+    /* 0x0A, 0x0B reserved for hardware */
+#define TD_BUFFEROVERRUN   0x0C
+#define TD_BUFFERUNDERRUN  0x0D
+    /* 0x0E, 0x0F reserved for HCD */
+#define TD_NOTACCESSED     0x0F
+
+/* ------------------------------------------------------------------------- */
+
+#define	LOG2_PERIODIC_SIZE	5	/* arbitrary; this matches OHCI */
+#define	PERIODIC_SIZE		(1 << LOG2_PERIODIC_SIZE)
+
+/* Philips transfer descriptor */
+struct ptd {
+	u16 count;
+#define	PTD_COUNT_MSK	(0x3ff << 0)
+#define	PTD_TOGGLE_MSK	(1 << 10)
+#define	PTD_ACTIVE_MSK	(1 << 11)
+#define	PTD_CC_MSK	(0xf << 12)
+	u16 mps;
+#define	PTD_MPS_MSK	(0x3ff << 0)
+#define	PTD_SPD_MSK	(1 << 10)
+#define	PTD_LAST_MSK	(1 << 11)
+#define	PTD_EP_MSK	(0xf << 12)
+	u16 len;
+#define	PTD_LEN_MSK	(0x3ff << 0)
+#define	PTD_DIR_MSK	(3 << 10)
+#define	PTD_DIR_SETUP	(0)
+#define	PTD_DIR_OUT	(1)
+#define	PTD_DIR_IN	(2)
+#define	PTD_B5_5_MSK	(1 << 13)
+	u16 faddr;
+#define	PTD_FA_MSK	(0x7f << 0)
+#define	PTD_FMT_MSK	(1 << 7)
+} __attribute__ ((packed, aligned(2)));
+
+struct isp116x_ep {
+	struct usb_device *udev;
+	struct ptd ptd;
+
+	u8 maxpacket;
+	u8 epnum;
+	u8 nextpid;
+
+	u16 length;		/* of current packet */
+	unsigned char *data;	/* to databuf */
+
+	u16 error_count;
+};
+
+/* URB struct */
+#define N_URB_TD		48
+#define URB_DEL			1
+typedef struct {
+	struct isp116x_ep *ed;
+	void *transfer_buffer;	/* (in) associated data buffer */
+	int actual_length;	/* (return) actual transfer length */
+	unsigned long pipe;	/* (in) pipe information */
+#if 0
+	int state;
+#endif
+} urb_priv_t;
+
+struct isp116x_platform_data {
+	/* Enable internal resistors on downstream ports */
+	unsigned sel15Kres:1;
+	/* On-chip overcurrent detection */
+	unsigned oc_enable:1;
+	/* Enable wakeup by devices on usb bus (e.g. wakeup
+	   by attachment/detachment or by device activity
+	   such as moving a mouse). When chosen, this option
+	   prevents stopping internal clock, increasing
+	   thereby power consumption in suspended state. */
+	unsigned remote_wakeup_enable:1;
+};
+
+struct isp116x {
+	u16 *addr_reg;
+	u16 *data_reg;
+
+	struct isp116x_platform_data *board;
+
+	struct dentry *dentry;
+	unsigned long stat1, stat2, stat4, stat8, stat16;
+
+	/* Status flags */
+	unsigned disabled:1;
+	unsigned sleeping:1;
+
+	/* Root hub registers */
+	u32 rhdesca;
+	u32 rhdescb;
+	u32 rhstatus;
+	u32 rhport[2];
+
+	/* Schedule for the current frame */
+	struct isp116x_ep *atl_active;
+	int atl_buflen;
+	int atl_bufshrt;
+	int atl_last_dir;
+	int atl_finishing;
+};
+
+/* ------------------------------------------------- */
+
+/* Inter-io delay (ns). The chip is picky about access timings; it
+ * expects at least:
+ * 150ns delay between consecutive accesses to DATA_REG,
+ * 300ns delay between access to ADDR_REG and DATA_REG
+ * OE, WE MUST NOT be changed during these intervals
+ */
+#if defined(UDELAY)
+#define	isp116x_delay(h,d)	udelay(d)
+#else
+#define	isp116x_delay(h,d)	do {} while (0)
+#endif
+
+static inline void isp116x_write_addr(struct isp116x *isp116x, unsigned reg)
+{
+	writew(reg & 0xff, isp116x->addr_reg);
+	isp116x_delay(isp116x, UDELAY);
+}
+
+static inline void isp116x_write_data16(struct isp116x *isp116x, u16 val)
+{
+	writew(val, isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+}
+
+static inline void isp116x_raw_write_data16(struct isp116x *isp116x, u16 val)
+{
+	__raw_writew(val, isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+}
+
+static inline u16 isp116x_read_data16(struct isp116x *isp116x)
+{
+	u16 val;
+
+	val = readw(isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+	return val;
+}
+
+static inline u16 isp116x_raw_read_data16(struct isp116x *isp116x)
+{
+	u16 val;
+
+	val = __raw_readw(isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+	return val;
+}
+
+static inline void isp116x_write_data32(struct isp116x *isp116x, u32 val)
+{
+	writew(val & 0xffff, isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+	writew(val >> 16, isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+}
+
+static inline u32 isp116x_read_data32(struct isp116x *isp116x)
+{
+	u32 val;
+
+	val = (u32) readw(isp116x->data_reg);
+	isp116x_delay(isp116x, UDELAY);
+	val |= ((u32) readw(isp116x->data_reg)) << 16;
+	isp116x_delay(isp116x, UDELAY);
+	return val;
+}
+
+/* Let's keep register access functions out of line. Hint:
+   we wait at least 150 ns at every access.
+*/
+static u16 isp116x_read_reg16(struct isp116x *isp116x, unsigned reg)
+{
+	isp116x_write_addr(isp116x, reg);
+	return isp116x_read_data16(isp116x);
+}
+
+static u32 isp116x_read_reg32(struct isp116x *isp116x, unsigned reg)
+{
+	isp116x_write_addr(isp116x, reg);
+	return isp116x_read_data32(isp116x);
+}
+
+static void isp116x_write_reg16(struct isp116x *isp116x, unsigned reg,
+				unsigned val)
+{
+	isp116x_write_addr(isp116x, reg | ISP116x_WRITE_OFFSET);
+	isp116x_write_data16(isp116x, (u16) (val & 0xffff));
+}
+
+static void isp116x_write_reg32(struct isp116x *isp116x, unsigned reg,
+				unsigned val)
+{
+	isp116x_write_addr(isp116x, reg | ISP116x_WRITE_OFFSET);
+	isp116x_write_data32(isp116x, (u32) val);
+}
+
+/* --- USB HUB constants (not OHCI-specific; see hub.h) -------------------- */
+
+/* destination of request */
+#define RH_INTERFACE               0x01
+#define RH_ENDPOINT                0x02
+#define RH_OTHER                   0x03
+
+#define RH_CLASS                   0x20
+#define RH_VENDOR                  0x40
+
+/* Requests: bRequest << 8 | bmRequestType */
+#define RH_GET_STATUS           0x0080
+#define RH_CLEAR_FEATURE        0x0100
+#define RH_SET_FEATURE          0x0300
+#define RH_SET_ADDRESS          0x0500
+#define RH_GET_DESCRIPTOR       0x0680
+#define RH_SET_DESCRIPTOR       0x0700
+#define RH_GET_CONFIGURATION    0x0880
+#define RH_SET_CONFIGURATION    0x0900
+#define RH_GET_STATE            0x0280
+#define RH_GET_INTERFACE        0x0A80
+#define RH_SET_INTERFACE        0x0B00
+#define RH_SYNC_FRAME           0x0C80
+/* Our Vendor Specific Request */
+#define RH_SET_EP               0x2000
+
+/* Hub port features */
+#define RH_PORT_CONNECTION         0x00
+#define RH_PORT_ENABLE             0x01
+#define RH_PORT_SUSPEND            0x02
+#define RH_PORT_OVER_CURRENT       0x03
+#define RH_PORT_RESET              0x04
+#define RH_PORT_POWER              0x08
+#define RH_PORT_LOW_SPEED          0x09
+
+#define RH_C_PORT_CONNECTION       0x10
+#define RH_C_PORT_ENABLE           0x11
+#define RH_C_PORT_SUSPEND          0x12
+#define RH_C_PORT_OVER_CURRENT     0x13
+#define RH_C_PORT_RESET            0x14
+
+/* Hub features */
+#define RH_C_HUB_LOCAL_POWER       0x00
+#define RH_C_HUB_OVER_CURRENT      0x01
+
+#define RH_DEVICE_REMOTE_WAKEUP    0x00
+#define RH_ENDPOINT_STALL          0x01
+
+#define RH_ACK                     0x01
+#define RH_REQ_ERR                 -1
+#define RH_NACK                    0x00
diff --git a/drivers/lan91c96.c b/drivers/lan91c96.c
index a50c5f0abec..ecdcbd9b32b 100644
--- a/drivers/lan91c96.c
+++ b/drivers/lan91c96.c
@@ -65,7 +65,7 @@
 
 #ifdef CONFIG_DRIVER_LAN91C96
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 /*------------------------------------------------------------------------
  *
diff --git a/drivers/macb.c b/drivers/macb.c
index abe57bac410..95cdc496cba 100644
--- a/drivers/macb.c
+++ b/drivers/macb.c
@@ -17,7 +17,8 @@
  */
 #include <common.h>
 
-#if defined(CONFIG_MACB) && (CONFIG_COMMANDS & (CFG_CMD_NET | CFG_CMD_MII))
+#if defined(CONFIG_MACB) \
+	&& (defined(CONFIG_CMD_NET) || defined(CONFIG_CMD_MII))
 
 /*
  * The u-boot networking stack is a little weird.  It seems like the
@@ -165,7 +166,7 @@ static u16 macb_mdio_read(struct macb_device *macb, u8 reg)
 	return MACB_BFEXT(DATA, frame);
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 static int macb_send(struct eth_device *netdev, volatile void *packet,
 		     int length)
@@ -503,9 +504,9 @@ int macb_eth_initialize(int id, void *regs, unsigned int phy_addr)
 	return 0;
 }
 
-#endif /* (CONFIG_COMMANDS & CFG_CMD_NET) */
+#endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_MII)
+#if defined(CONFIG_CMD_MII)
 
 int miiphy_read(unsigned char addr, unsigned char reg, unsigned short *value)
 {
@@ -581,6 +582,6 @@ int miiphy_write(unsigned char addr, unsigned char reg, unsigned short value)
 	return 0;
 }
 
-#endif /* (CONFIG_COMMANDS & CFG_CMD_MII) */
+#endif
 
 #endif /* CONFIG_MACB */
diff --git a/drivers/mpc8xx_pcmcia.c b/drivers/mpc8xx_pcmcia.c
index 399a719e565..8a34cd30547 100644
--- a/drivers/mpc8xx_pcmcia.c
+++ b/drivers/mpc8xx_pcmcia.c
@@ -6,11 +6,11 @@
 
 #undef	CONFIG_PCMCIA
 
-#if	(CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 #define	CONFIG_PCMCIA
 #endif
 
-#if	(CONFIG_COMMANDS & CFG_CMD_IDE) && defined(CONFIG_IDE_8xx_PCCARD)
+#if defined(CONFIG_CMD_IDE) && defined(CONFIG_IDE_8xx_PCCARD)
 #define	CONFIG_PCMCIA
 #endif
 
@@ -23,7 +23,7 @@ extern int check_ide_device (int slot);
 extern int pcmcia_hardware_enable (int slot);
 extern int pcmcia_voltage_set(int slot, int vcc, int vpp);
 
-#if	(CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 extern int pcmcia_hardware_disable(int slot);
 #endif
 
@@ -189,7 +189,7 @@ int pcmcia_on (void)
 	return rc;
 }
 
-#if	(CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 int pcmcia_off (void)
 {
 	int i;
@@ -221,7 +221,7 @@ int pcmcia_off (void)
 	pcmcia_hardware_disable(_slot_);
 	return 0;
 }
-#endif	/* CFG_CMD_PCMCIA */
+#endif
 
 
 static u_int m8xx_get_graycode(u_int size)
diff --git a/drivers/nand/nand.c b/drivers/nand/nand.c
index 9fef71d6293..27b5792bcc9 100644
--- a/drivers/nand/nand.c
+++ b/drivers/nand/nand.c
@@ -23,7 +23,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
 
 #include <nand.h>
 
diff --git a/drivers/nand/nand_base.c b/drivers/nand/nand_base.c
index c6fee18222b..151f535c58b 100644
--- a/drivers/nand/nand_base.c
+++ b/drivers/nand/nand_base.c
@@ -72,7 +72,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
 
 #include <malloc.h>
 #include <watchdog.h>
diff --git a/drivers/nand/nand_bbt.c b/drivers/nand/nand_bbt.c
index aaa9400e54d..19a9bc2a5b6 100644
--- a/drivers/nand/nand_bbt.c
+++ b/drivers/nand/nand_bbt.c
@@ -54,7 +54,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
 
 #include <malloc.h>
 #include <linux/mtd/compat.h>
diff --git a/drivers/nand/nand_ecc.c b/drivers/nand/nand_ecc.c
index 90274e6d6dd..4c532b0794e 100644
--- a/drivers/nand/nand_ecc.c
+++ b/drivers/nand/nand_ecc.c
@@ -37,7 +37,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
 
 #include<linux/mtd/mtd.h>
 
@@ -197,4 +197,4 @@ int nand_correct_data(struct mtd_info *mtd, u_char *dat,
 	return -1;
 }
 
-#endif	/* CONFIG_COMMANDS & CFG_CMD_NAND */
+#endif
diff --git a/drivers/nand/nand_ids.c b/drivers/nand/nand_ids.c
index 8b58736bc10..075cae66b06 100644
--- a/drivers/nand/nand_ids.c
+++ b/drivers/nand/nand_ids.c
@@ -13,7 +13,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
 
 #include <linux/mtd/nand.h>
 
diff --git a/drivers/nand/nand_util.c b/drivers/nand/nand_util.c
index 10bf03659ed..aee87270390 100644
--- a/drivers/nand/nand_util.c
+++ b/drivers/nand/nand_util.c
@@ -32,11 +32,12 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && !defined(CFG_NAND_LEGACY)
 
 #include <command.h>
 #include <watchdog.h>
 #include <malloc.h>
+#include <div64.h>
 
 #include <nand.h>
 #include <jffs2/jffs2.h>
@@ -208,10 +209,10 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 		}
 
 		if (!opts->quiet) {
-			int percent = (int)
-				((unsigned long long)
+                        unsigned long long n =(unsigned long long)
 				 (erase.addr+meminfo->erasesize-opts->offset)
-				 * 100 / erase_length);
+				 * 100;
+			int percent = (int)do_div(n, erase_length);
 
 			/* output progress message only at whole percent
 			 * steps to reduce the number of messages printed
@@ -475,10 +476,9 @@ int nand_write_opts(nand_info_t *meminfo, const nand_write_options_t *opts)
 		imglen -= readlen;
 
 		if (!opts->quiet) {
-			int percent = (int)
-				((unsigned long long)
-				 (opts->length-imglen) * 100
-				 / opts->length);
+                        unsigned long long n = (unsigned long long)
+			         (opts->length-imglen) * 100;
+			int percent = (int)do_div(n, opts->length);
 			/* output progress message only at whole percent
 			 * steps to reduce the number of messages printed
 			 * on (slow) serial consoles
@@ -651,10 +651,9 @@ int nand_read_opts(nand_info_t *meminfo, const nand_read_options_t *opts)
 		}
 
 		if (!opts->quiet) {
-			int percent = (int)
-				((unsigned long long)
-				 (opts->length-imglen) * 100
-				 / opts->length);
+                        unsigned long long n = (unsigned long long)
+			         (opts->length-imglen) * 100;
+			int percent = (int)do_div(n ,opts->length);
 			/* output progress message only at whole percent
 			 * steps to reduce the number of messages printed
 			 * on (slow) serial consoles
@@ -859,4 +858,4 @@ int nand_unlock(nand_info_t *meminfo, ulong start, ulong length)
 	return ret;
 }
 
-#endif /* (CONFIG_COMMANDS & CFG_CMD_NAND) && !defined(CFG_NAND_LEGACY) */
+#endif
diff --git a/drivers/nand_legacy/nand_legacy.c b/drivers/nand_legacy/nand_legacy.c
index 458046d41c3..49d2ebb67d3 100644
--- a/drivers/nand_legacy/nand_legacy.c
+++ b/drivers/nand_legacy/nand_legacy.c
@@ -15,14 +15,7 @@
 #include <asm/io.h>
 #include <watchdog.h>
 
-#ifdef CONFIG_SHOW_BOOT_PROGRESS
-# include <status_led.h>
-# define SHOW_BOOT_PROGRESS(arg)	show_boot_progress(arg)
-#else
-# define SHOW_BOOT_PROGRESS(arg)
-#endif
-
-#if (CONFIG_COMMANDS & CFG_CMD_NAND) && defined(CFG_NAND_LEGACY)
+#if defined(CONFIG_CMD_NAND) && defined(CFG_NAND_LEGACY)
 
 #include <linux/mtd/nand_legacy.h>
 #include <linux/mtd/nand_ids.h>
@@ -1616,4 +1609,4 @@ int read_jffs2_nand(size_t start, size_t len,
 }
 #endif /* CONFIG_JFFS2_NAND */
 
-#endif /* (CONFIG_COMMANDS & CFG_CMD_NAND) && defined(CFG_NAND_LEGACY) */
+#endif
diff --git a/drivers/natsemi.c b/drivers/natsemi.c
index b009db63ebe..075d6c52c45 100644
--- a/drivers/natsemi.c
+++ b/drivers/natsemi.c
@@ -56,8 +56,8 @@
 #include <asm/io.h>
 #include <pci.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
-	defined(CONFIG_NATSEMI)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_NATSEMI)
 
 /* defines */
 #define EEPROM_SIZE 0xb /*12 16-bit chunks, or 24 bytes*/
diff --git a/drivers/netarm_eth.c b/drivers/netarm_eth.c
index 89b3a8394ed..a99ee5da2e3 100644
--- a/drivers/netarm_eth.c
+++ b/drivers/netarm_eth.c
@@ -30,7 +30,7 @@
 #include <asm/arch/netarm_registers.h>
 
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 static int na_mii_poll_busy (void);
 
diff --git a/drivers/ns8382x.c b/drivers/ns8382x.c
index 976f86aaff5..f8b143a01a6 100644
--- a/drivers/ns8382x.c
+++ b/drivers/ns8382x.c
@@ -56,8 +56,8 @@
 #include <asm/io.h>
 #include <pci.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
-	defined(CONFIG_NS8382X)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_NS8382X)
 
 /* defines */
 #define DSIZE     0x00000FFF
diff --git a/drivers/pci.c b/drivers/pci.c
index 050582f7826..41589192c1c 100644
--- a/drivers/pci.c
+++ b/drivers/pci.c
@@ -490,10 +490,16 @@ int pci_hose_scan_bus(struct pci_controller *hose, int bus)
 
 int pci_hose_scan(struct pci_controller *hose)
 {
+	/* Start scan at current_busno.
+	 * PCIe will start scan at first_busno+1.
+	 */
+	/* For legacy support, ensure current>=first */
+	if (hose->first_busno > hose->current_busno)
+		hose->current_busno = hose->first_busno;
 #ifdef CONFIG_PCI_PNP
 	pciauto_config_init(hose);
 #endif
-	return pci_hose_scan_bus(hose, hose->first_busno);
+	return pci_hose_scan_bus(hose, hose->current_busno);
 }
 
 void pci_init(void)
diff --git a/drivers/pci_auto.c b/drivers/pci_auto.c
index f170c2db89e..a3c609ba49c 100644
--- a/drivers/pci_auto.c
+++ b/drivers/pci_auto.c
@@ -65,7 +65,7 @@ int pciauto_region_allocate(struct pci_region* res, unsigned int size, unsigned
 
 	res->bus_lower = addr + size;
 
-	DEBUGF("address=0x%lx", addr);
+	DEBUGF("address=0x%lx bus_lower=%x", addr, res->bus_lower);
 
 	*bar = addr;
 	return 0;
@@ -154,7 +154,7 @@ void pciauto_setup_device(struct pci_controller *hose,
 	pci_hose_write_config_byte(hose, dev, PCI_LATENCY_TIMER, 0x80);
 }
 
-static void pciauto_prescan_setup_bridge(struct pci_controller *hose,
+void pciauto_prescan_setup_bridge(struct pci_controller *hose,
 					 pci_dev_t dev, int sub_bus)
 {
 	struct pci_region *pci_mem = hose->pci_mem;
@@ -165,8 +165,10 @@ static void pciauto_prescan_setup_bridge(struct pci_controller *hose,
 	pci_hose_read_config_dword(hose, dev, PCI_COMMAND, &cmdstat);
 
 	/* Configure bus number registers */
-	pci_hose_write_config_byte(hose, dev, PCI_PRIMARY_BUS, PCI_BUS(dev));
-	pci_hose_write_config_byte(hose, dev, PCI_SECONDARY_BUS, sub_bus);
+	pci_hose_write_config_byte(hose, dev, PCI_PRIMARY_BUS,
+				   PCI_BUS(dev) - hose->first_busno);
+	pci_hose_write_config_byte(hose, dev, PCI_SECONDARY_BUS,
+				   sub_bus - hose->first_busno);
 	pci_hose_write_config_byte(hose, dev, PCI_SUBORDINATE_BUS, 0xff);
 
 	if (pci_mem) {
@@ -211,7 +213,7 @@ static void pciauto_prescan_setup_bridge(struct pci_controller *hose,
 	pci_hose_write_config_dword(hose, dev, PCI_COMMAND, cmdstat | PCI_COMMAND_MASTER);
 }
 
-static void pciauto_postscan_setup_bridge(struct pci_controller *hose,
+void pciauto_postscan_setup_bridge(struct pci_controller *hose,
 					  pci_dev_t dev, int sub_bus)
 {
 	struct pci_region *pci_mem = hose->pci_mem;
@@ -219,7 +221,8 @@ static void pciauto_postscan_setup_bridge(struct pci_controller *hose,
 	struct pci_region *pci_io = hose->pci_io;
 
 	/* Configure bus number registers */
-	pci_hose_write_config_byte(hose, dev, PCI_SUBORDINATE_BUS, sub_bus);
+	pci_hose_write_config_byte(hose, dev, PCI_SUBORDINATE_BUS,
+				   sub_bus - hose->first_busno);
 
 	if (pci_mem) {
 		/* Round memory allocator to 1MB boundary */
@@ -282,25 +285,36 @@ void pciauto_config_init(struct pci_controller *hose)
 	if (hose->pci_mem) {
 		pciauto_region_init(hose->pci_mem);
 
-		DEBUGF("PCI Autoconfig: Memory region: [%lx-%lx]\n",
+		DEBUGF("PCI Autoconfig: Bus Memory region: [%lx-%lx],\n"
+		       "\t\tPhysical Memory [%x-%x]\n",
 		    hose->pci_mem->bus_start,
-		    hose->pci_mem->bus_start + hose->pci_mem->size - 1);
+		    hose->pci_mem->bus_start + hose->pci_mem->size - 1,
+		    hose->pci_mem->phys_start,
+		    hose->pci_mem->phys_start + hose->pci_mem->size - 1);
 	}
 
 	if (hose->pci_prefetch) {
 		pciauto_region_init(hose->pci_prefetch);
 
-		DEBUGF("PCI Autoconfig: Prefetchable Memory region: [%lx-%lx]\n",
+		DEBUGF("PCI Autoconfig: Bus Prefetchable Mem: [%lx-%lx],\n"
+		       "\t\tPhysical Memory [%x-%x]\n",
 		    hose->pci_prefetch->bus_start,
-		    hose->pci_prefetch->bus_start + hose->pci_prefetch->size - 1);
+		    hose->pci_prefetch->bus_start + hose->pci_prefetch->size - 1,
+		    hose->pci_prefetch->phys_start,
+		    hose->pci_prefetch->phys_start +
+				hose->pci_prefetch->size - 1);
 	}
 
 	if (hose->pci_io) {
 		pciauto_region_init(hose->pci_io);
 
-		DEBUGF("PCI Autoconfig: I/O region: [%lx-%lx]\n",
+		DEBUGF("PCI Autoconfig: Bus I/O region: [%lx-%lx],\n"
+		       "\t\tPhysical Memory: [%x-%x]\n",
 		    hose->pci_io->bus_start,
-		    hose->pci_io->bus_start + hose->pci_io->size - 1);
+		    hose->pci_io->bus_start + hose->pci_io->size - 1,
+		    hose->pci_io->phys_start,
+		    hose->pci_io->phys_start + hose->pci_io->size - 1);
+
 	}
 }
 
@@ -317,6 +331,12 @@ int pciauto_config_device(struct pci_controller *hose, pci_dev_t dev)
 	pci_hose_read_config_word(hose, dev, PCI_CLASS_DEVICE, &class);
 
 	switch(class) {
+	case PCI_CLASS_PROCESSOR_POWERPC: /* an agent or end-point */
+		DEBUGF("PCI AutoConfig: Found PowerPC device\n");
+		pciauto_setup_device(hose, dev, 6, hose->pci_mem,
+				     hose->pci_prefetch, hose->pci_io);
+		break;
+
 	case PCI_CLASS_BRIDGE_PCI:
 		hose->current_busno++;
 		pciauto_setup_device(hose, dev, 2, hose->pci_mem, hose->pci_prefetch, hose->pci_io);
diff --git a/drivers/pci_indirect.c b/drivers/pci_indirect.c
index d7be0810f57..a8220fb4117 100644
--- a/drivers/pci_indirect.c
+++ b/drivers/pci_indirect.c
@@ -45,7 +45,7 @@ indirect_##rw##_config_##size(struct pci_controller *hose, 		 \
 	cfg_##rw(val, hose->cfg_data + (offset & mask), type, op);	 \
 	return 0;    					 		 \
 }
-#elif defined(CONFIG_E500)
+#elif defined(CONFIG_E500) || defined(CONFIG_MPC86xx)
 #define INDIRECT_PCI_OP(rw, size, type, op, mask)                        \
 static int                                                               \
 indirect_##rw##_config_##size(struct pci_controller *hose,               \
@@ -55,7 +55,7 @@ indirect_##rw##_config_##size(struct pci_controller *hose,               \
 	b = PCI_BUS(dev); d = PCI_DEV(dev); f = PCI_FUNC(dev);		 \
 	b = b - hose->first_busno;					 \
 	dev = PCI_BDF(b, d, f);						 \
-	*(hose->cfg_addr) = dev | (offset & 0xfc) | 0x80000000;          \
+	*(hose->cfg_addr) = dev | (offset & 0xfc) | ((offset & 0xf00) << 16) | 0x80000000; \
 	sync();                                                          \
 	cfg_##rw(val, hose->cfg_data + (offset & mask), type, op);       \
 	return 0;                                                        \
diff --git a/drivers/pcnet.c b/drivers/pcnet.c
index da9ac7f99af..2af0e8f244f 100644
--- a/drivers/pcnet.c
+++ b/drivers/pcnet.c
@@ -45,8 +45,8 @@
 #define PCNET_DEBUG2(fmt,args...)
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) \
-    && defined(CONFIG_PCNET)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_PCNET)
 
 #if !defined(CONF_PCNET_79C973) && defined(CONF_PCNET_79C975)
 #error "Macro for PCnet chip version is not defined!"
diff --git a/drivers/plb2800_eth.c b/drivers/plb2800_eth.c
index 4c683d7a5c4..0ae5d808a6f 100644
--- a/drivers/plb2800_eth.c
+++ b/drivers/plb2800_eth.c
@@ -25,8 +25,8 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) \
-	&& defined(CONFIG_PLB2800_ETHER)
+#if defined(CONFIG_CMD_NET) \
+	&& defined(CONFIG_NET_MULTI) && defined(CONFIG_PLB2800_ETHER)
 
 #include <malloc.h>
 #include <net.h>
diff --git a/drivers/pxa_pcmcia.c b/drivers/pxa_pcmcia.c
index d9d38bbfcd8..6020e46283a 100644
--- a/drivers/pxa_pcmcia.c
+++ b/drivers/pxa_pcmcia.c
@@ -85,7 +85,7 @@ int pcmcia_on (void)
 	return rc;
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 int pcmcia_off (void)
 {
 	return 0;
diff --git a/drivers/qe/qe.c b/drivers/qe/qe.c
index 5f209629f40..0f5232a72af 100644
--- a/drivers/qe/qe.c
+++ b/drivers/qe/qe.c
@@ -98,7 +98,7 @@ static void qe_sdma_init(void)
 	out_be32(&p->sdaqmr, 0);
 
 	/* Allocate 2KB temporary buffer for sdma */
-	sdma_buffer_base = qe_muram_alloc(2048, 64);
+	sdma_buffer_base = qe_muram_alloc(2048, 4096);
 	out_be32(&p->sdwbcr, sdma_buffer_base & QE_SDEBCR_BA_MASK);
 
 	/* Clear sdma status */
diff --git a/drivers/qe/qe.h b/drivers/qe/qe.h
index 0bcd0a9573e..400b1a6f603 100644
--- a/drivers/qe/qe.h
+++ b/drivers/qe/qe.h
@@ -29,7 +29,7 @@
 #define QE_NUM_OF_BRGS	16
 #define UCC_MAX_NUM	8
 
-#define QE_DATAONLY_BASE	(uint)(128)
+#define QE_DATAONLY_BASE	0
 #define QE_DATAONLY_SIZE	(QE_MURAM_SIZE - QE_DATAONLY_BASE)
 
 /* QE threads SNUM
diff --git a/drivers/rpx_pcmcia.c b/drivers/rpx_pcmcia.c
index 2a0a9e05a27..c7c425b93cf 100644
--- a/drivers/rpx_pcmcia.c
+++ b/drivers/rpx_pcmcia.c
@@ -9,11 +9,11 @@
 
 #undef	CONFIG_PCMCIA
 
-#if	CONFIG_COMMANDS & CFG_CMD_PCMCIA
+#if defined(CONFIG_CMD_PCMCIA)
 #define	CONFIG_PCMCIA
 #endif
 
-#if	(CONFIG_COMMANDS & CFG_CMD_IDE) && defined(CONFIG_IDE_8xx_PCCARD)
+#if defined(CONFIG_CMD_IDE) && defined(CONFIG_IDE_8xx_PCCARD)
 #define	CONFIG_PCMCIA
 #endif
 
@@ -62,12 +62,12 @@ int pcmcia_hardware_enable (int slot)
 	return 0;	/* No hardware to enable */
 }
 
-#if (CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 static int pcmcia_hardware_disable(int slot)
 {
 	return 0;	/* No hardware to disable */
 }
-#endif	/* CONFIG_COMMANDS & CFG_CMD_PCMCIA */
+#endif
 
 
 #endif	/* CONFIG_PCMCIA && (CONFIG_RPXCLASSIC || CONFIG_RPXLITE) */
diff --git a/drivers/rtl8019.c b/drivers/rtl8019.c
index 62b9245517b..409a69f0214 100644
--- a/drivers/rtl8019.c
+++ b/drivers/rtl8019.c
@@ -34,7 +34,7 @@
 
 #ifdef CONFIG_DRIVER_RTL8019
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET)
+#if defined(CONFIG_CMD_NET)
 
 
 /* packet page register access functions */
diff --git a/drivers/rtl8139.c b/drivers/rtl8139.c
index afe1a4fdaf6..9045523a312 100644
--- a/drivers/rtl8139.c
+++ b/drivers/rtl8139.c
@@ -77,7 +77,7 @@
 #include <asm/io.h>
 #include <pci.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
 	defined(CONFIG_RTL8139)
 
 #define TICKS_PER_SEC	CFG_HZ
@@ -535,4 +535,4 @@ static void rtl_disable(struct eth_device *dev)
 		udelay (100); /* wait 100us */
 	}
 }
-#endif	/* CFG_CMD_NET && CONFIG_NET_MULTI && CONFIG_RTL8139 */
+#endif
diff --git a/drivers/rtl8169.c b/drivers/rtl8169.c
index 3393ba890ae..63ea2cca9b1 100644
--- a/drivers/rtl8169.c
+++ b/drivers/rtl8169.c
@@ -55,7 +55,7 @@
 #include <asm/io.h>
 #include <pci.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
 	defined(CONFIG_RTL8169)
 
 #undef DEBUG_RTL8169
diff --git a/drivers/sk98lin/uboot_drv.c b/drivers/sk98lin/uboot_drv.c
index 263dac81069..d02cd1be242 100644
--- a/drivers/sk98lin/uboot_drv.c
+++ b/drivers/sk98lin/uboot_drv.c
@@ -25,7 +25,7 @@
 
 #include <common.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) && \
 	defined(CONFIG_SK98)
 
 #include "h/skdrv1st.h"
diff --git a/drivers/sym53c8xx.c b/drivers/sym53c8xx.c
index ae10f80ecfb..29eeccd9a8f 100644
--- a/drivers/sym53c8xx.c
+++ b/drivers/sym53c8xx.c
@@ -51,7 +51,7 @@
 #define PRINTF(fmt,args...)
 #endif
 
-#if (CONFIG_COMMANDS & CFG_CMD_SCSI) && defined(CONFIG_SCSI_SYM53C8XX)
+#if defined(CONFIG_CMD_SCSI) && defined(CONFIG_SCSI_SYM53C8XX)
 
 #undef SCSI_SINGLE_STEP
 /*
@@ -787,7 +787,7 @@ void scsi_chip_init(void)
 	scsi_write_byte(DMODE,0x00);
 #endif
 }
-#endif /* (CONFIG_COMMANDS & CFG_CMD_SCSI) */
+#endif
 
 
 #endif /* CONFIG_SCSI_SYM53C8XX */
diff --git a/drivers/ti_pci1410a.c b/drivers/ti_pci1410a.c
index d5297b57213..208ca50a743 100644
--- a/drivers/ti_pci1410a.c
+++ b/drivers/ti_pci1410a.c
@@ -64,7 +64,7 @@
 
 #include <pcmcia.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_PCMCIA) && defined(CONFIG_IDE_TI_CARDBUS)
+#if defined(CONFIG_CMD_PCMCIA) && defined(CONFIG_IDE_TI_CARDBUS)
 
 int pcmcia_on(int ide_base_bus);
 
diff --git a/drivers/tigon3.c b/drivers/tigon3.c
index ec2cd2ac36f..860a8894fc7 100644
--- a/drivers/tigon3.c
+++ b/drivers/tigon3.c
@@ -12,7 +12,7 @@
 /******************************************************************************/
 #include <common.h>
 #include <asm/types.h>
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && !defined(CONFIG_NET_MULTI) && \
+#if defined(CONFIG_CMD_NET) && !defined(CONFIG_NET_MULTI) && \
 	defined(CONFIG_TIGON3)
 #ifdef CONFIG_BMW
 #include <mpc824x.h>
@@ -6197,4 +6197,4 @@ LM_DmaTest(PLM_DEVICE_BLOCK pDevice, PLM_UINT8 pBufferVirt,
     }
     return LM_STATUS_SUCCESS;
 }
-#endif /* CFG_CMD_NET, !CONFIG_NET_MULTI, CONFIG_TIGON3 */
+#endif
diff --git a/drivers/tqm8xx_pcmcia.c b/drivers/tqm8xx_pcmcia.c
index a0f53cd6841..132c7a5169b 100644
--- a/drivers/tqm8xx_pcmcia.c
+++ b/drivers/tqm8xx_pcmcia.c
@@ -10,11 +10,11 @@
 
 #undef	CONFIG_PCMCIA
 
-#if	(CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 #define	CONFIG_PCMCIA
 #endif
 
-#if	(CONFIG_COMMANDS & CFG_CMD_IDE) && defined(CONFIG_IDE_8xx_PCCARD)
+#if defined(CONFIG_CMD_IDE) && defined(CONFIG_IDE_8xx_PCCARD)
 #define	CONFIG_PCMCIA
 #endif
 
@@ -241,7 +241,7 @@ int pcmcia_hardware_enable(int slot)
 }
 
 
-#if (CONFIG_COMMANDS & CFG_CMD_PCMCIA)
+#if defined(CONFIG_CMD_PCMCIA)
 int pcmcia_hardware_disable(int slot)
 {
 	u_long reg;
@@ -263,7 +263,7 @@ int pcmcia_hardware_disable(int slot)
 
 	return (0);
 }
-#endif	/* CFG_CMD_PCMCIA */
+#endif
 
 int pcmcia_voltage_set(int slot, int vcc, int vpp)
 {
diff --git a/drivers/tsec.c b/drivers/tsec.c
index b4187739cb0..c0111234940 100644
--- a/drivers/tsec.c
+++ b/drivers/tsec.c
@@ -65,33 +65,33 @@ struct tsec_info_struct {
  *   FEC_PHYIDX
  */
 static struct tsec_info_struct tsec_info[] = {
-#if defined(CONFIG_MPC85XX_TSEC1) || defined(CONFIG_MPC83XX_TSEC1)
-#if defined(CONFIG_MPC8544DS)
+#if defined(CONFIG_TSEC1)
+#if defined(CONFIG_MPC8544DS) || defined(CONFIG_MPC8641HPCN)
 	{TSEC1_PHY_ADDR, TSEC_GIGABIT | TSEC_REDUCED, TSEC1_PHYIDX},
 #else
 	{TSEC1_PHY_ADDR, TSEC_GIGABIT, TSEC1_PHYIDX},
 #endif
-#elif defined(CONFIG_MPC86XX_TSEC1)
-	{TSEC1_PHY_ADDR, TSEC_GIGABIT | TSEC_REDUCED, TSEC1_PHYIDX},
 #else
 	{0, 0, 0},
 #endif
-#if defined(CONFIG_MPC85XX_TSEC2) || defined(CONFIG_MPC83XX_TSEC2)
-	{TSEC2_PHY_ADDR, TSEC_GIGABIT, TSEC2_PHYIDX},
-#elif defined(CONFIG_MPC86XX_TSEC2)
+#if defined(CONFIG_TSEC2)
+#if defined(CONFIG_MPC8641HPCN)
 	{TSEC2_PHY_ADDR, TSEC_GIGABIT | TSEC_REDUCED, TSEC2_PHYIDX},
 #else
+	{TSEC2_PHY_ADDR, TSEC_GIGABIT, TSEC2_PHYIDX},
+#endif
+#else
 	{0, 0, 0},
 #endif
 #ifdef CONFIG_MPC85XX_FEC
 	{FEC_PHY_ADDR, 0, FEC_PHYIDX},
 #else
-#if defined(CONFIG_MPC85XX_TSEC3) || defined(CONFIG_MPC83XX_TSEC3) || defined(CONFIG_MPC86XX_TSEC3)
+#if defined(CONFIG_TSEC3)
 	{TSEC3_PHY_ADDR, TSEC_GIGABIT | TSEC_REDUCED, TSEC3_PHYIDX},
 #else
 	{0, 0, 0},
 #endif
-#if defined(CONFIG_MPC85XX_TSEC4) || defined(CONFIG_MPC83XX_TSEC4) || defined(CONFIG_MPC86XX_TSEC4)
+#if defined(CONFIG_TSEC4)
 	{TSEC4_PHY_ADDR, TSEC_GIGABIT | TSEC_REDUCED, TSEC4_PHYIDX},
 #else
 	{0, 0, 0},
@@ -178,7 +178,7 @@ int tsec_initialize(bd_t * bis, int index, char *devname)
 	priv->regs->maccfg1 |= MACCFG1_SOFT_RESET;
 	priv->regs->maccfg1 &= ~(MACCFG1_SOFT_RESET);
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII) \
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII) \
 	&& !defined(BITBANGMII)
 	miiphy_register(dev->name, tsec_miiphy_read, tsec_miiphy_write);
 #endif
@@ -298,9 +298,9 @@ static int init_phy(struct eth_device *dev)
 	volatile tsec_t *regs = (volatile tsec_t *)(TSEC_BASE_ADDR);
 
 	/* Assign a Physical address to the TBI */
-	regs->tbipa = TBIPA_VALUE;
+	regs->tbipa = CFG_TBIPA_VALUE;
 	regs = (volatile tsec_t *)(TSEC_BASE_ADDR + TSEC_SIZE);
-	regs->tbipa = TBIPA_VALUE;
+	regs->tbipa = CFG_TBIPA_VALUE;
 	asm("sync");
 
 	/* Reset MII (due to new addresses) */
@@ -900,6 +900,39 @@ static void tsec_halt(struct eth_device *dev)
 		phy_run_commands(priv, priv->phyinfo->shutdown);
 }
 
+struct phy_info phy_info_M88E1149S = {
+	0x1410ca,
+	"Marvell 88E1149S",
+	4,
+	(struct phy_cmd[]){     /* config */
+		/* Reset and configure the PHY */
+		{MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
+		{0x1d, 0x1f, NULL},
+		{0x1e, 0x200c, NULL},
+		{0x1d, 0x5, NULL},
+		{0x1e, 0x0, NULL},
+		{0x1e, 0x100, NULL},
+		{MIIM_GBIT_CONTROL, MIIM_GBIT_CONTROL_INIT, NULL},
+		{MIIM_ANAR, MIIM_ANAR_INIT, NULL},
+		{MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
+		{MIIM_CONTROL, MIIM_CONTROL_INIT, &mii_cr_init},
+		{miim_end,}
+	},
+	(struct phy_cmd[]){     /* startup */
+		/* Status is read once to clear old link state */
+		{MIIM_STATUS, miim_read, NULL},
+		/* Auto-negotiate */
+		{MIIM_STATUS, miim_read, &mii_parse_sr},
+		/* Read the status */
+		{MIIM_88E1011_PHY_STATUS, miim_read,
+		 &mii_parse_88E1011_psr},
+		{miim_end,}
+	},
+	(struct phy_cmd[]){     /* shutdown */
+		{miim_end,}
+	},
+};
+
 /* The 5411 id is 0x206070, the 5421 is 0x2060e0 */
 struct phy_info phy_info_BCM5461S = {
 	0x02060c1,	/* 5461 ID */
@@ -928,6 +961,33 @@ struct phy_info phy_info_BCM5461S = {
 	},
 };
 
+struct phy_info phy_info_BCM5464S = {
+	0x02060b1,	/* 5464 ID */
+	"Broadcom BCM5464S",
+	0, /* not clear to me what minor revisions we can shift away */
+	(struct phy_cmd[]) { /* config */
+		/* Reset and configure the PHY */
+		{MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
+		{MIIM_GBIT_CONTROL, MIIM_GBIT_CONTROL_INIT, NULL},
+		{MIIM_ANAR, MIIM_ANAR_INIT, NULL},
+		{MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
+		{MIIM_CONTROL, MIIM_CONTROL_INIT, &mii_cr_init},
+		{miim_end,}
+	},
+	(struct phy_cmd[]) { /* startup */
+		/* Status is read once to clear old link state */
+		{MIIM_STATUS, miim_read, NULL},
+		/* Auto-negotiate */
+		{MIIM_STATUS, miim_read, &mii_parse_sr},
+		/* Read the status */
+		{MIIM_BCM54xx_AUXSTATUS, miim_read, &mii_parse_BCM54xx_sr},
+		{miim_end,}
+	},
+	(struct phy_cmd[]) { /* shutdown */
+		{miim_end,}
+	},
+};
+
 struct phy_info phy_info_M88E1011S = {
 	0x01410c6,
 	"Marvell 88E1011S",
@@ -968,11 +1028,6 @@ struct phy_info phy_info_M88E1111S = {
 	(struct phy_cmd[]){	/* config */
 			   /* Reset and configure the PHY */
 			   {MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
-			   {0x1d, 0x1f, NULL},
-			   {0x1e, 0x200c, NULL},
-			   {0x1d, 0x5, NULL},
-			   {0x1e, 0x0, NULL},
-			   {0x1e, 0x100, NULL},
 			   {0x14, 0x0cd2, NULL}, /* Delay RGMII TX and RX */
 			   {MIIM_GBIT_CONTROL, MIIM_GBIT_CONTROL_INIT, NULL},
 			   {MIIM_ANAR, MIIM_ANAR_INIT, NULL},
@@ -1012,14 +1067,16 @@ static struct phy_info phy_info_M88E1145 = {
 	"Marvell 88E1145",
 	4,
 	(struct phy_cmd[]){	/* config */
+			   /* Reset the PHY */
+			   {MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
+
 			   /* Errata E0, E1 */
 			   {29, 0x001b, NULL},
 			   {30, 0x418f, NULL},
 			   {29, 0x0016, NULL},
 			   {30, 0xa2da, NULL},
 
-			   /* Reset and configure the PHY */
-			   {MIIM_CONTROL, MIIM_CONTROL_RESET, NULL},
+			   /* Configure the PHY */
 			   {MIIM_GBIT_CONTROL, MIIM_GBIT_CONTROL_INIT, NULL},
 			   {MIIM_ANAR, MIIM_ANAR_INIT, NULL},
 			   {MIIM_88E1011_PHY_SCR, MIIM_88E1011_PHY_MDI_X_AUTO,
@@ -1292,9 +1349,11 @@ struct phy_info *phy_info[] = {
 	&phy_info_cis8204,
 	&phy_info_cis8201,
 	&phy_info_BCM5461S,
+	&phy_info_BCM5464S,
 	&phy_info_M88E1011S,
 	&phy_info_M88E1111S,
 	&phy_info_M88E1145,
+	&phy_info_M88E1149S,
 	&phy_info_dm9161,
 	&phy_info_lxt971,
 	&phy_info_VSC8244,
@@ -1325,8 +1384,10 @@ struct phy_info *get_phy_info(struct eth_device *dev)
 	/* loop through all the known PHY types, and find one that */
 	/* matches the ID we read from the PHY. */
 	for (i = 0; phy_info[i]; i++) {
-		if (phy_info[i]->id == (phy_ID >> phy_info[i]->shift))
+		if (phy_info[i]->id == (phy_ID >> phy_info[i]->shift)) {
 			theInfo = phy_info[i];
+			break;
+		}
 	}
 
 	if (theInfo == NULL) {
@@ -1417,7 +1478,7 @@ static void relocate_cmds(void)
 	relocated = 1;
 }
 
-#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII) \
+#if defined(CONFIG_MII) || defined(CONFIG_CMD_MII) \
 	&& !defined(BITBANGMII)
 
 struct tsec_private *get_priv_for_phy(unsigned char phyaddr)
@@ -1476,7 +1537,6 @@ static int tsec_miiphy_write(char *devname, unsigned char addr,
 	return 0;
 }
 
-#endif /* defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
-		&& !defined(BITBANGMII) */
+#endif
 
 #endif /* CONFIG_TSEC_ENET */
diff --git a/drivers/tsec.h b/drivers/tsec.h
index 7bf3dee2b68..2f0092ad598 100644
--- a/drivers/tsec.h
+++ b/drivers/tsec.h
@@ -70,7 +70,9 @@
 #define miim_end -2
 #define miim_read -1
 
-#define TBIPA_VALUE		0x1f
+#ifndef CFG_TBIPA_VALUE
+    #define CFG_TBIPA_VALUE	0x1f
+#endif
 #define MIIMCFG_INIT_VALUE	0x00000003
 #define MIIMCFG_RESET		0x80000000
 
diff --git a/drivers/tsi108_eth.c b/drivers/tsi108_eth.c
index 47341bee7f1..524e9daa4cb 100644
--- a/drivers/tsi108_eth.c
+++ b/drivers/tsi108_eth.c
@@ -27,7 +27,7 @@
 
 #include <config.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_NET) && defined(CONFIG_NET_MULTI) \
+#if defined(CONFIG_CMD_NET) && defined(CONFIG_NET_MULTI) \
 	&& defined(CONFIG_TSI108_ETH)
 
 #if !defined(CONFIG_TSI108_ETH_NUM_PORTS) || (CONFIG_TSI108_ETH_NUM_PORTS > 2)
diff --git a/drivers/tsi108_i2c.c b/drivers/tsi108_i2c.c
index eb52cb66c96..3a3b75c39bf 100644
--- a/drivers/tsi108_i2c.c
+++ b/drivers/tsi108_i2c.c
@@ -28,7 +28,7 @@
 #ifdef CONFIG_TSI108_I2C
 #include <tsi108.h>
 
-#if (CONFIG_COMMANDS & CFG_CMD_I2C)
+#if defined(CONFIG_CMD_I2C)
 
 #define I2C_DELAY	100000
 #undef  DEBUG_I2C
@@ -279,5 +279,5 @@ int i2c_probe (uchar chip)
 	return i2c_read (chip, 0, 1, (char *)&tmp, 1);
 }
 
-#endif	/* (CONFIG_COMMANDS & CFG_CMD_I2C) */
+#endif
 #endif /* CONFIG_TSI108_I2C */
diff --git a/drivers/usb_ohci.c b/drivers/usb_ohci.c
new file mode 100644
index 00000000000..d6b745fadd6
--- /dev/null
+++ b/drivers/usb_ohci.c
@@ -0,0 +1,1929 @@
+/*
+ * URB OHCI HCD (Host Controller Driver) for USB on the AT91RM9200 and PCI bus.
+ *
+ * Interrupt support is added. Now, it has been tested
+ * on ULI1575 chip and works well with USB keyboard.
+ *
+ * (C) Copyright 2007
+ * Zhang Wei, Freescale Semiconductor, Inc. <wei.zhang@freescale.com>
+ *
+ * (C) Copyright 2003
+ * Gary Jennejohn, DENX Software Engineering <gj@denx.de>
+ *
+ * Note: Much of this code has been derived from Linux 2.4
+ * (C) Copyright 1999 Roman Weissgaerber <weissg@vienna.at>
+ * (C) Copyright 2000-2002 David Brownell
+ *
+ * Modified for the MP2USB by (C) Copyright 2005 Eric Benard
+ * ebenard@eukrea.com - based on s3c24x0's driver
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * 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 the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ */
+/*
+ * IMPORTANT NOTES
+ * 1 - Read doc/README.generic_usb_ohci
+ * 2 - this driver is intended for use with USB Mass Storage Devices
+ *     (BBB) and USB keyboard. There is NO support for Isochronous pipes!
+ * 2 - when running on a PQFP208 AT91RM9200, define CONFIG_AT91C_PQFP_UHPBUG
+ *     to activate workaround for bug #41 or this driver will NOT work!
+ */
+
+#include <common.h>
+
+#ifdef CONFIG_USB_OHCI_NEW
+
+#include <asm/byteorder.h>
+
+#if defined(CONFIG_PCI_OHCI)
+# include <pci.h>
+#endif
+
+#include <malloc.h>
+#include <usb.h>
+#include "usb_ohci.h"
+
+#if defined(CONFIG_ARM920T) || \
+    defined(CONFIG_S3C2400) || \
+    defined(CONFIG_S3C2410) || \
+    defined(CONFIG_440EP) || \
+    defined(CONFIG_PCI_OHCI) || \
+    defined(CONFIG_MPC5200)
+# define OHCI_USE_NPS		/* force NoPowerSwitching mode */
+#endif
+
+#undef OHCI_VERBOSE_DEBUG	/* not always helpful */
+#undef DEBUG
+#undef SHOW_INFO
+#undef OHCI_FILL_TRACE
+
+/* For initializing controller (mask in an HCFS mode too) */
+#define OHCI_CONTROL_INIT \
+	(OHCI_CTRL_CBSR & 0x3) | OHCI_CTRL_IE | OHCI_CTRL_PLE
+
+/*
+ * e.g. PCI controllers need this
+ */
+#ifdef CFG_OHCI_SWAP_REG_ACCESS
+# define readl(a) __swap_32(*((vu_long *)(a)))
+# define writel(a, b) (*((vu_long *)(b)) = __swap_32((vu_long)a))
+#else
+# define readl(a) (*((vu_long *)(a)))
+# define writel(a, b) (*((vu_long *)(b)) = ((vu_long)a))
+#endif /* CFG_OHCI_SWAP_REG_ACCESS */
+
+#define min_t(type,x,y) ({ type __x = (x); type __y = (y); __x < __y ? __x: __y; })
+
+#ifdef CONFIG_PCI_OHCI
+static struct pci_device_id ohci_pci_ids[] = {
+	{0x10b9, 0x5237},	/* ULI1575 PCI OHCI module ids */
+	/* Please add supported PCI OHCI controller ids here */
+	{0, 0}
+};
+#endif
+
+#ifdef DEBUG
+#define dbg(format, arg...) printf("DEBUG: " format "\n", ## arg)
+#else
+#define dbg(format, arg...) do {} while(0)
+#endif /* DEBUG */
+#define err(format, arg...) printf("ERROR: " format "\n", ## arg)
+#undef SHOW_INFO
+#ifdef SHOW_INFO
+#define info(format, arg...) printf("INFO: " format "\n", ## arg)
+#else
+#define info(format, arg...) do {} while(0)
+#endif
+
+#ifdef CFG_OHCI_BE_CONTROLLER
+# define m16_swap(x) cpu_to_be16(x)
+# define m32_swap(x) cpu_to_be32(x)
+#else
+# define m16_swap(x) cpu_to_le16(x)
+# define m32_swap(x) cpu_to_le32(x)
+#endif /* CFG_OHCI_BE_CONTROLLER */
+
+/* global ohci_t */
+static ohci_t gohci;
+/* this must be aligned to a 256 byte boundary */
+struct ohci_hcca ghcca[1];
+/* a pointer to the aligned storage */
+struct ohci_hcca *phcca;
+/* this allocates EDs for all possible endpoints */
+struct ohci_device ohci_dev;
+/* RHSC flag */
+int got_rhsc;
+/* device which was disconnected */
+struct usb_device *devgone;
+
+
+
+/*-------------------------------------------------------------------------*/
+
+/* AMD-756 (D2 rev) reports corrupt register contents in some cases.
+ * The erratum (#4) description is incorrect.  AMD's workaround waits
+ * till some bits (mostly reserved) are clear; ok for all revs.
+ */
+#define OHCI_QUIRK_AMD756 0xabcd
+#define read_roothub(hc, register, mask) ({ \
+	u32 temp = readl (&hc->regs->roothub.register); \
+	if (hc->flags & OHCI_QUIRK_AMD756) \
+		while (temp & mask) \
+			temp = readl (&hc->regs->roothub.register); \
+	temp; })
+
+static u32 roothub_a (struct ohci *hc)
+	{ return read_roothub (hc, a, 0xfc0fe000); }
+static inline u32 roothub_b (struct ohci *hc)
+	{ return readl (&hc->regs->roothub.b); }
+static inline u32 roothub_status (struct ohci *hc)
+	{ return readl (&hc->regs->roothub.status); }
+static u32 roothub_portstatus (struct ohci *hc, int i)
+	{ return read_roothub (hc, portstatus [i], 0xffe0fce0); }
+
+
+/* forward declaration */
+static int hc_interrupt (void);
+static void
+td_submit_job (struct usb_device * dev, unsigned long pipe, void * buffer,
+	int transfer_len, struct devrequest * setup, urb_priv_t * urb, int interval);
+
+/*-------------------------------------------------------------------------*
+ * URB support functions
+ *-------------------------------------------------------------------------*/
+
+/* free HCD-private data associated with this URB */
+
+static void urb_free_priv (urb_priv_t * urb)
+{
+	int		i;
+	int		last;
+	struct td	* td;
+
+	last = urb->length - 1;
+	if (last >= 0) {
+		for (i = 0; i <= last; i++) {
+			td = urb->td[i];
+			if (td) {
+				td->usb_dev = NULL;
+				urb->td[i] = NULL;
+			}
+		}
+	}
+	free(urb);
+}
+
+/*-------------------------------------------------------------------------*/
+
+#ifdef DEBUG
+static int sohci_get_current_frame_number (struct usb_device * dev);
+
+/* debug| print the main components of an URB
+ * small: 0) header + data packets 1) just header */
+
+static void pkt_print (urb_priv_t *purb, struct usb_device * dev,
+	unsigned long pipe, void * buffer,
+	int transfer_len, struct devrequest * setup, char * str, int small)
+{
+	dbg("%s URB:[%4x] dev:%2d,ep:%2d-%c,type:%s,len:%d/%d stat:%#lx",
+			str,
+			sohci_get_current_frame_number (dev),
+			usb_pipedevice (pipe),
+			usb_pipeendpoint (pipe),
+			usb_pipeout (pipe)? 'O': 'I',
+			usb_pipetype (pipe) < 2? (usb_pipeint (pipe)? "INTR": "ISOC"):
+				(usb_pipecontrol (pipe)? "CTRL": "BULK"),
+			(purb ? purb->actual_length : 0),
+			transfer_len, dev->status);
+#ifdef	OHCI_VERBOSE_DEBUG
+	if (!small) {
+		int i, len;
+
+		if (usb_pipecontrol (pipe)) {
+			printf (__FILE__ ": cmd(8):");
+			for (i = 0; i < 8 ; i++)
+				printf (" %02x", ((__u8 *) setup) [i]);
+			printf ("\n");
+		}
+		if (transfer_len > 0 && buffer) {
+			printf (__FILE__ ": data(%d/%d):",
+				(purb ? purb->actual_length : 0),
+				transfer_len);
+			len = usb_pipeout (pipe)?
+					transfer_len:
+					(purb ? purb->actual_length : 0);
+			for (i = 0; i < 16 && i < len; i++)
+				printf (" %02x", ((__u8 *) buffer) [i]);
+			printf ("%s\n", i < len? "...": "");
+		}
+	}
+#endif
+}
+
+/* just for debugging; prints non-empty branches of the int ed tree inclusive iso eds*/
+void ep_print_int_eds (ohci_t *ohci, char * str) {
+	int i, j;
+	 __u32 * ed_p;
+	for (i= 0; i < 32; i++) {
+		j = 5;
+		ed_p = &(ohci->hcca->int_table [i]);
+		if (*ed_p == 0)
+		    continue;
+		printf (__FILE__ ": %s branch int %2d(%2x):", str, i, i);
+		while (*ed_p != 0 && j--) {
+			ed_t *ed = (ed_t *)m32_swap(ed_p);
+			printf (" ed: %4x;", ed->hwINFO);
+			ed_p = &ed->hwNextED;
+		}
+		printf ("\n");
+	}
+}
+
+static void ohci_dump_intr_mask (char *label, __u32 mask)
+{
+	dbg ("%s: 0x%08x%s%s%s%s%s%s%s%s%s",
+		label,
+		mask,
+		(mask & OHCI_INTR_MIE) ? " MIE" : "",
+		(mask & OHCI_INTR_OC) ? " OC" : "",
+		(mask & OHCI_INTR_RHSC) ? " RHSC" : "",
+		(mask & OHCI_INTR_FNO) ? " FNO" : "",
+		(mask & OHCI_INTR_UE) ? " UE" : "",
+		(mask & OHCI_INTR_RD) ? " RD" : "",
+		(mask & OHCI_INTR_SF) ? " SF" : "",
+		(mask & OHCI_INTR_WDH) ? " WDH" : "",
+		(mask & OHCI_INTR_SO) ? " SO" : ""
+		);
+}
+
+static void maybe_print_eds (char *label, __u32 value)
+{
+	ed_t *edp = (ed_t *)value;
+
+	if (value) {
+		dbg ("%s %08x", label, value);
+		dbg ("%08x", edp->hwINFO);
+		dbg ("%08x", edp->hwTailP);
+		dbg ("%08x", edp->hwHeadP);
+		dbg ("%08x", edp->hwNextED);
+	}
+}
+
+static char * hcfs2string (int state)
+{
+	switch (state) {
+		case OHCI_USB_RESET:	return "reset";
+		case OHCI_USB_RESUME:	return "resume";
+		case OHCI_USB_OPER:	return "operational";
+		case OHCI_USB_SUSPEND:	return "suspend";
+	}
+	return "?";
+}
+
+/* dump control and status registers */
+static void ohci_dump_status (ohci_t *controller)
+{
+	struct ohci_regs	*regs = controller->regs;
+	__u32			temp;
+
+	temp = readl (&regs->revision) & 0xff;
+	if (temp != 0x10)
+		dbg ("spec %d.%d", (temp >> 4), (temp & 0x0f));
+
+	temp = readl (&regs->control);
+	dbg ("control: 0x%08x%s%s%s HCFS=%s%s%s%s%s CBSR=%d", temp,
+		(temp & OHCI_CTRL_RWE) ? " RWE" : "",
+		(temp & OHCI_CTRL_RWC) ? " RWC" : "",
+		(temp & OHCI_CTRL_IR) ? " IR" : "",
+		hcfs2string (temp & OHCI_CTRL_HCFS),
+		(temp & OHCI_CTRL_BLE) ? " BLE" : "",
+		(temp & OHCI_CTRL_CLE) ? " CLE" : "",
+		(temp & OHCI_CTRL_IE) ? " IE" : "",
+		(temp & OHCI_CTRL_PLE) ? " PLE" : "",
+		temp & OHCI_CTRL_CBSR
+		);
+
+	temp = readl (&regs->cmdstatus);
+	dbg ("cmdstatus: 0x%08x SOC=%d%s%s%s%s", temp,
+		(temp & OHCI_SOC) >> 16,
+		(temp & OHCI_OCR) ? " OCR" : "",
+		(temp & OHCI_BLF) ? " BLF" : "",
+		(temp & OHCI_CLF) ? " CLF" : "",
+		(temp & OHCI_HCR) ? " HCR" : ""
+		);
+
+	ohci_dump_intr_mask ("intrstatus", readl (&regs->intrstatus));
+	ohci_dump_intr_mask ("intrenable", readl (&regs->intrenable));
+
+	maybe_print_eds ("ed_periodcurrent", readl (&regs->ed_periodcurrent));
+
+	maybe_print_eds ("ed_controlhead", readl (&regs->ed_controlhead));
+	maybe_print_eds ("ed_controlcurrent", readl (&regs->ed_controlcurrent));
+
+	maybe_print_eds ("ed_bulkhead", readl (&regs->ed_bulkhead));
+	maybe_print_eds ("ed_bulkcurrent", readl (&regs->ed_bulkcurrent));
+
+	maybe_print_eds ("donehead", readl (&regs->donehead));
+}
+
+static void ohci_dump_roothub (ohci_t *controller, int verbose)
+{
+	__u32			temp, ndp, i;
+
+	temp = roothub_a (controller);
+	ndp = (temp & RH_A_NDP);
+#ifdef CONFIG_AT91C_PQFP_UHPBUG
+	ndp = (ndp == 2) ? 1:0;
+#endif
+	if (verbose) {
+		dbg ("roothub.a: %08x POTPGT=%d%s%s%s%s%s NDP=%d", temp,
+			((temp & RH_A_POTPGT) >> 24) & 0xff,
+			(temp & RH_A_NOCP) ? " NOCP" : "",
+			(temp & RH_A_OCPM) ? " OCPM" : "",
+			(temp & RH_A_DT) ? " DT" : "",
+			(temp & RH_A_NPS) ? " NPS" : "",
+			(temp & RH_A_PSM) ? " PSM" : "",
+			ndp
+			);
+		temp = roothub_b (controller);
+		dbg ("roothub.b: %08x PPCM=%04x DR=%04x",
+			temp,
+			(temp & RH_B_PPCM) >> 16,
+			(temp & RH_B_DR)
+			);
+		temp = roothub_status (controller);
+		dbg ("roothub.status: %08x%s%s%s%s%s%s",
+			temp,
+			(temp & RH_HS_CRWE) ? " CRWE" : "",
+			(temp & RH_HS_OCIC) ? " OCIC" : "",
+			(temp & RH_HS_LPSC) ? " LPSC" : "",
+			(temp & RH_HS_DRWE) ? " DRWE" : "",
+			(temp & RH_HS_OCI) ? " OCI" : "",
+			(temp & RH_HS_LPS) ? " LPS" : ""
+			);
+	}
+
+	for (i = 0; i < ndp; i++) {
+		temp = roothub_portstatus (controller, i);
+		dbg ("roothub.portstatus [%d] = 0x%08x%s%s%s%s%s%s%s%s%s%s%s%s",
+			i,
+			temp,
+			(temp & RH_PS_PRSC) ? " PRSC" : "",
+			(temp & RH_PS_OCIC) ? " OCIC" : "",
+			(temp & RH_PS_PSSC) ? " PSSC" : "",
+			(temp & RH_PS_PESC) ? " PESC" : "",
+			(temp & RH_PS_CSC) ? " CSC" : "",
+
+			(temp & RH_PS_LSDA) ? " LSDA" : "",
+			(temp & RH_PS_PPS) ? " PPS" : "",
+			(temp & RH_PS_PRS) ? " PRS" : "",
+			(temp & RH_PS_POCI) ? " POCI" : "",
+			(temp & RH_PS_PSS) ? " PSS" : "",
+
+			(temp & RH_PS_PES) ? " PES" : "",
+			(temp & RH_PS_CCS) ? " CCS" : ""
+			);
+	}
+}
+
+static void ohci_dump (ohci_t *controller, int verbose)
+{
+	dbg ("OHCI controller usb-%s state", controller->slot_name);
+
+	/* dumps some of the state we know about */
+	ohci_dump_status (controller);
+	if (verbose)
+		ep_print_int_eds (controller, "hcca");
+	dbg ("hcca frame #%04x", controller->hcca->frame_no);
+	ohci_dump_roothub (controller, 1);
+}
+
+
+#endif /* DEBUG */
+
+/*-------------------------------------------------------------------------*
+ * Interface functions (URB)
+ *-------------------------------------------------------------------------*/
+
+/* get a transfer request */
+
+int sohci_submit_job(urb_priv_t *urb, struct devrequest *setup)
+{
+	ohci_t *ohci;
+	ed_t * ed;
+	urb_priv_t *purb_priv = urb;
+	int i, size = 0;
+	struct usb_device *dev = urb->dev;
+	unsigned long pipe = urb->pipe;
+	void *buffer = urb->transfer_buffer;
+	int transfer_len = urb->transfer_buffer_length;
+	int interval = urb->interval;
+
+	ohci = &gohci;
+
+	/* when controller's hung, permit only roothub cleanup attempts
+	 * such as powering down ports */
+	if (ohci->disabled) {
+		err("sohci_submit_job: EPIPE");
+		return -1;
+	}
+
+	/* we're about to begin a new transaction here so mark the URB unfinished */
+	urb->finished = 0;
+
+	/* every endpoint has a ed, locate and fill it */
+	if (!(ed = ep_add_ed (dev, pipe, interval, 1))) {
+		err("sohci_submit_job: ENOMEM");
+		return -1;
+	}
+
+	/* for the private part of the URB we need the number of TDs (size) */
+	switch (usb_pipetype (pipe)) {
+		case PIPE_BULK: /* one TD for every 4096 Byte */
+			size = (transfer_len - 1) / 4096 + 1;
+			break;
+		case PIPE_CONTROL: /* 1 TD for setup, 1 for ACK and 1 for every 4096 B */
+			size = (transfer_len == 0)? 2:
+						(transfer_len - 1) / 4096 + 3;
+			break;
+		case PIPE_INTERRUPT: /* 1 TD */
+			size = 1;
+			break;
+	}
+
+	ed->purb = urb;
+
+	if (size >= (N_URB_TD - 1)) {
+		err("need %d TDs, only have %d", size, N_URB_TD);
+		return -1;
+	}
+	purb_priv->pipe = pipe;
+
+	/* fill the private part of the URB */
+	purb_priv->length = size;
+	purb_priv->ed = ed;
+	purb_priv->actual_length = 0;
+
+	/* allocate the TDs */
+	/* note that td[0] was allocated in ep_add_ed */
+	for (i = 0; i < size; i++) {
+		purb_priv->td[i] = td_alloc (dev);
+		if (!purb_priv->td[i]) {
+			purb_priv->length = i;
+			urb_free_priv (purb_priv);
+			err("sohci_submit_job: ENOMEM");
+			return -1;
+		}
+	}
+
+	if (ed->state == ED_NEW || (ed->state & ED_DEL)) {
+		urb_free_priv (purb_priv);
+		err("sohci_submit_job: EINVAL");
+		return -1;
+	}
+
+	/* link the ed into a chain if is not already */
+	if (ed->state != ED_OPER)
+		ep_link (ohci, ed);
+
+	/* fill the TDs and link it to the ed */
+	td_submit_job(dev, pipe, buffer, transfer_len, setup, purb_priv, interval);
+
+	return 0;
+}
+
+static inline int sohci_return_job(struct ohci *hc, urb_priv_t *urb)
+{
+	struct ohci_regs *regs = hc->regs;
+
+	switch (usb_pipetype (urb->pipe)) {
+	case PIPE_INTERRUPT:
+		/* implicitly requeued */
+		if (urb->dev->irq_handle &&
+				(urb->dev->irq_act_len = urb->actual_length)) {
+			writel (OHCI_INTR_WDH, &regs->intrenable);
+			readl (&regs->intrenable); /* PCI posting flush */
+			urb->dev->irq_handle(urb->dev);
+			writel (OHCI_INTR_WDH, &regs->intrdisable);
+			readl (&regs->intrdisable); /* PCI posting flush */
+		}
+		urb->actual_length = 0;
+		td_submit_job (
+				urb->dev,
+				urb->pipe,
+				urb->transfer_buffer,
+				urb->transfer_buffer_length,
+				NULL,
+				urb,
+				urb->interval);
+		break;
+	case PIPE_CONTROL:
+	case PIPE_BULK:
+		break;
+	default:
+		return 0;
+	}
+	return 1;
+}
+
+/*-------------------------------------------------------------------------*/
+
+#ifdef DEBUG
+/* tell us the current USB frame number */
+
+static int sohci_get_current_frame_number (struct usb_device *usb_dev)
+{
+	ohci_t *ohci = &gohci;
+
+	return m16_swap (ohci->hcca->frame_no);
+}
+#endif
+
+/*-------------------------------------------------------------------------*
+ * ED handling functions
+ *-------------------------------------------------------------------------*/
+
+/* search for the right branch to insert an interrupt ed into the int tree
+ * do some load ballancing;
+ * returns the branch and
+ * sets the interval to interval = 2^integer (ld (interval)) */
+
+static int ep_int_ballance (ohci_t * ohci, int interval, int load)
+{
+	int i, branch = 0;
+
+	/* search for the least loaded interrupt endpoint
+	 * branch of all 32 branches
+	 */
+	for (i = 0; i < 32; i++)
+		if (ohci->ohci_int_load [branch] > ohci->ohci_int_load [i])
+			branch = i;
+
+	branch = branch % interval;
+	for (i = branch; i < 32; i += interval)
+		ohci->ohci_int_load [i] += load;
+
+	return branch;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*  2^int( ld (inter)) */
+
+static int ep_2_n_interval (int inter)
+{
+	int i;
+	for (i = 0; ((inter >> i) > 1 ) && (i < 5); i++);
+	return 1 << i;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* the int tree is a binary tree
+ * in order to process it sequentially the indexes of the branches have to be mapped
+ * the mapping reverses the bits of a word of num_bits length */
+
+static int ep_rev (int num_bits, int word)
+{
+	int i, wout = 0;
+
+	for (i = 0; i < num_bits; i++)
+		wout |= (((word >> i) & 1) << (num_bits - i - 1));
+	return wout;
+}
+
+/*-------------------------------------------------------------------------*
+ * ED handling functions
+ *-------------------------------------------------------------------------*/
+
+/* link an ed into one of the HC chains */
+
+static int ep_link (ohci_t *ohci, ed_t *edi)
+{
+	volatile ed_t *ed = edi;
+	int int_branch;
+	int i;
+	int inter;
+	int interval;
+	int load;
+	__u32 * ed_p;
+
+	ed->state = ED_OPER;
+	ed->int_interval = 0;
+
+	switch (ed->type) {
+	case PIPE_CONTROL:
+		ed->hwNextED = 0;
+		if (ohci->ed_controltail == NULL) {
+			writel (ed, &ohci->regs->ed_controlhead);
+		} else {
+			ohci->ed_controltail->hwNextED = m32_swap ((unsigned long)ed);
+		}
+		ed->ed_prev = ohci->ed_controltail;
+		if (!ohci->ed_controltail && !ohci->ed_rm_list[0] &&
+			!ohci->ed_rm_list[1] && !ohci->sleeping) {
+			ohci->hc_control |= OHCI_CTRL_CLE;
+			writel (ohci->hc_control, &ohci->regs->control);
+		}
+		ohci->ed_controltail = edi;
+		break;
+
+	case PIPE_BULK:
+		ed->hwNextED = 0;
+		if (ohci->ed_bulktail == NULL) {
+			writel (ed, &ohci->regs->ed_bulkhead);
+		} else {
+			ohci->ed_bulktail->hwNextED = m32_swap ((unsigned long)ed);
+		}
+		ed->ed_prev = ohci->ed_bulktail;
+		if (!ohci->ed_bulktail && !ohci->ed_rm_list[0] &&
+			!ohci->ed_rm_list[1] && !ohci->sleeping) {
+			ohci->hc_control |= OHCI_CTRL_BLE;
+			writel (ohci->hc_control, &ohci->regs->control);
+		}
+		ohci->ed_bulktail = edi;
+		break;
+
+	case PIPE_INTERRUPT:
+		load = ed->int_load;
+		interval = ep_2_n_interval (ed->int_period);
+		ed->int_interval = interval;
+		int_branch = ep_int_ballance (ohci, interval, load);
+		ed->int_branch = int_branch;
+
+		for (i = 0; i < ep_rev (6, interval); i += inter) {
+			inter = 1;
+			for (ed_p = &(ohci->hcca->int_table[ep_rev (5, i) + int_branch]);
+				(*ed_p != 0) && (((ed_t *)ed_p)->int_interval >= interval);
+				ed_p = &(((ed_t *)ed_p)->hwNextED))
+					inter = ep_rev (6, ((ed_t *)ed_p)->int_interval);
+			ed->hwNextED = *ed_p;
+			*ed_p = m32_swap(ed);
+		}
+		break;
+	}
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* scan the periodic table to find and unlink this ED */
+static void periodic_unlink ( struct ohci *ohci, volatile struct ed *ed,
+		unsigned index, unsigned period)
+{
+	for (; index < NUM_INTS; index += period) {
+		__u32	*ed_p = &ohci->hcca->int_table [index];
+
+		/* ED might have been unlinked through another path */
+		while (*ed_p != 0) {
+			if (((struct ed *)m32_swap (ed_p)) == ed) {
+				*ed_p = ed->hwNextED;
+				break;
+			}
+			ed_p = & (((struct ed *)m32_swap (ed_p))->hwNextED);
+		}
+	}
+}
+
+
+/* unlink an ed from one of the HC chains.
+ * just the link to the ed is unlinked.
+ * the link from the ed still points to another operational ed or 0
+ * so the HC can eventually finish the processing of the unlinked ed */
+
+static int ep_unlink (ohci_t *ohci, ed_t *edi)
+{
+	volatile ed_t *ed = edi;
+	int i;
+
+	ed->hwINFO |= m32_swap (OHCI_ED_SKIP);
+
+	switch (ed->type) {
+	case PIPE_CONTROL:
+		if (ed->ed_prev == NULL) {
+			if (!ed->hwNextED) {
+				ohci->hc_control &= ~OHCI_CTRL_CLE;
+				writel (ohci->hc_control, &ohci->regs->control);
+			}
+			writel (m32_swap (*((__u32 *)&ed->hwNextED)), &ohci->regs->ed_controlhead);
+		} else {
+			ed->ed_prev->hwNextED = ed->hwNextED;
+		}
+		if (ohci->ed_controltail == ed) {
+			ohci->ed_controltail = ed->ed_prev;
+		} else {
+			((ed_t *)m32_swap (*((__u32 *)&ed->hwNextED)))->ed_prev = ed->ed_prev;
+		}
+		break;
+
+	case PIPE_BULK:
+		if (ed->ed_prev == NULL) {
+			if (!ed->hwNextED) {
+				ohci->hc_control &= ~OHCI_CTRL_BLE;
+				writel (ohci->hc_control, &ohci->regs->control);
+			}
+			writel (m32_swap (*((__u32 *)&ed->hwNextED)), &ohci->regs->ed_bulkhead);
+		} else {
+			ed->ed_prev->hwNextED = ed->hwNextED;
+		}
+		if (ohci->ed_bulktail == ed) {
+			ohci->ed_bulktail = ed->ed_prev;
+		} else {
+			((ed_t *)m32_swap (*((__u32 *)&ed->hwNextED)))->ed_prev = ed->ed_prev;
+		}
+		break;
+
+	case PIPE_INTERRUPT:
+		periodic_unlink (ohci, ed, 0, 1);
+		for (i = ed->int_branch; i < 32; i += ed->int_interval)
+		    ohci->ohci_int_load[i] -= ed->int_load;
+		break;
+	}
+	ed->state = ED_UNLINK;
+	return 0;
+}
+
+
+/*-------------------------------------------------------------------------*/
+
+/* add/reinit an endpoint; this should be done once at the
+ * usb_set_configuration command, but the USB stack is a little bit
+ * stateless so we do it at every transaction if the state of the ed
+ * is ED_NEW then a dummy td is added and the state is changed to
+ * ED_UNLINK in all other cases the state is left unchanged the ed
+ * info fields are setted anyway even though most of them should not
+ * change
+ */
+static ed_t * ep_add_ed (struct usb_device *usb_dev, unsigned long pipe,
+		int interval, int load)
+{
+	td_t *td;
+	ed_t *ed_ret;
+	volatile ed_t *ed;
+
+	ed = ed_ret = &ohci_dev.ed[(usb_pipeendpoint (pipe) << 1) |
+			(usb_pipecontrol (pipe)? 0: usb_pipeout (pipe))];
+
+	if ((ed->state & ED_DEL) || (ed->state & ED_URB_DEL)) {
+		err("ep_add_ed: pending delete");
+		/* pending delete request */
+		return NULL;
+	}
+
+	if (ed->state == ED_NEW) {
+		ed->hwINFO = m32_swap (OHCI_ED_SKIP); /* skip ed */
+		/* dummy td; end of td list for ed */
+		td = td_alloc (usb_dev);
+		ed->hwTailP = m32_swap ((unsigned long)td);
+		ed->hwHeadP = ed->hwTailP;
+		ed->state = ED_UNLINK;
+		ed->type = usb_pipetype (pipe);
+		ohci_dev.ed_cnt++;
+	}
+
+	ed->hwINFO = m32_swap (usb_pipedevice (pipe)
+			| usb_pipeendpoint (pipe) << 7
+			| (usb_pipeisoc (pipe)? 0x8000: 0)
+			| (usb_pipecontrol (pipe)? 0: (usb_pipeout (pipe)? 0x800: 0x1000))
+			| usb_pipeslow (pipe) << 13
+			| usb_maxpacket (usb_dev, pipe) << 16);
+
+	if (ed->type == PIPE_INTERRUPT && ed->state == ED_UNLINK) {
+		ed->int_period = interval;
+		ed->int_load = load;
+	}
+
+	return ed_ret;
+}
+
+/*-------------------------------------------------------------------------*
+ * TD handling functions
+ *-------------------------------------------------------------------------*/
+
+/* enqueue next TD for this URB (OHCI spec 5.2.8.2) */
+
+static void td_fill (ohci_t *ohci, unsigned int info,
+	void *data, int len,
+	struct usb_device *dev, int index, urb_priv_t *urb_priv)
+{
+	volatile td_t  *td, *td_pt;
+#ifdef OHCI_FILL_TRACE
+	int i;
+#endif
+
+	if (index > urb_priv->length) {
+		err("index > length");
+		return;
+	}
+	/* use this td as the next dummy */
+	td_pt = urb_priv->td [index];
+	td_pt->hwNextTD = 0;
+
+	/* fill the old dummy TD */
+	td = urb_priv->td [index] = (td_t *)(m32_swap (urb_priv->ed->hwTailP) & ~0xf);
+
+	td->ed = urb_priv->ed;
+	td->next_dl_td = NULL;
+	td->index = index;
+	td->data = (__u32)data;
+#ifdef OHCI_FILL_TRACE
+	if ((usb_pipetype(urb_priv->pipe) == PIPE_BULK) && usb_pipeout(urb_priv->pipe)) {
+		for (i = 0; i < len; i++)
+		printf("td->data[%d] %#2x ",i, ((unsigned char *)td->data)[i]);
+		printf("\n");
+	}
+#endif
+	if (!len)
+		data = 0;
+
+	td->hwINFO = m32_swap (info);
+	td->hwCBP = m32_swap ((unsigned long)data);
+	if (data)
+		td->hwBE = m32_swap ((unsigned long)(data + len - 1));
+	else
+		td->hwBE = 0;
+	td->hwNextTD = m32_swap ((unsigned long)td_pt);
+
+	/* append to queue */
+	td->ed->hwTailP = td->hwNextTD;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* prepare all TDs of a transfer */
+
+static void td_submit_job (struct usb_device *dev, unsigned long pipe, void *buffer,
+	int transfer_len, struct devrequest *setup, urb_priv_t *urb, int interval)
+{
+	ohci_t *ohci = &gohci;
+	int data_len = transfer_len;
+	void *data;
+	int cnt = 0;
+	__u32 info = 0;
+	unsigned int toggle = 0;
+
+	/* OHCI handles the DATA-toggles itself, we just use the USB-toggle bits for reseting */
+	if(usb_gettoggle(dev, usb_pipeendpoint(pipe), usb_pipeout(pipe))) {
+		toggle = TD_T_TOGGLE;
+	} else {
+		toggle = TD_T_DATA0;
+		usb_settoggle(dev, usb_pipeendpoint(pipe), usb_pipeout(pipe), 1);
+	}
+	urb->td_cnt = 0;
+	if (data_len)
+		data = buffer;
+	else
+		data = 0;
+
+	switch (usb_pipetype (pipe)) {
+	case PIPE_BULK:
+		info = usb_pipeout (pipe)?
+			TD_CC | TD_DP_OUT : TD_CC | TD_DP_IN ;
+		while(data_len > 4096) {
+			td_fill (ohci, info | (cnt? TD_T_TOGGLE:toggle), data, 4096, dev, cnt, urb);
+			data += 4096; data_len -= 4096; cnt++;
+		}
+		info = usb_pipeout (pipe)?
+			TD_CC | TD_DP_OUT : TD_CC | TD_R | TD_DP_IN ;
+		td_fill (ohci, info | (cnt? TD_T_TOGGLE:toggle), data, data_len, dev, cnt, urb);
+		cnt++;
+
+		if (!ohci->sleeping)
+			writel (OHCI_BLF, &ohci->regs->cmdstatus); /* start bulk list */
+		break;
+
+	case PIPE_CONTROL:
+		info = TD_CC | TD_DP_SETUP | TD_T_DATA0;
+		td_fill (ohci, info, setup, 8, dev, cnt++, urb);
+		if (data_len > 0) {
+			info = usb_pipeout (pipe)?
+				TD_CC | TD_R | TD_DP_OUT | TD_T_DATA1 : TD_CC | TD_R | TD_DP_IN | TD_T_DATA1;
+			/* NOTE:  mishandles transfers >8K, some >4K */
+			td_fill (ohci, info, data, data_len, dev, cnt++, urb);
+		}
+		info = usb_pipeout (pipe)?
+			TD_CC | TD_DP_IN | TD_T_DATA1: TD_CC | TD_DP_OUT | TD_T_DATA1;
+		td_fill (ohci, info, data, 0, dev, cnt++, urb);
+		if (!ohci->sleeping)
+			writel (OHCI_CLF, &ohci->regs->cmdstatus); /* start Control list */
+		break;
+
+	case PIPE_INTERRUPT:
+		info = usb_pipeout (urb->pipe)?
+			TD_CC | TD_DP_OUT | toggle:
+			TD_CC | TD_R | TD_DP_IN | toggle;
+		td_fill (ohci, info, data, data_len, dev, cnt++, urb);
+		break;
+	}
+	if (urb->length != cnt)
+		dbg("TD LENGTH %d != CNT %d", urb->length, cnt);
+}
+
+/*-------------------------------------------------------------------------*
+ * Done List handling functions
+ *-------------------------------------------------------------------------*/
+
+
+/* calculate the transfer length and update the urb */
+
+static void dl_transfer_length(td_t * td)
+{
+	__u32 tdINFO, tdBE, tdCBP;
+	urb_priv_t *lurb_priv = td->ed->purb;
+
+	tdINFO = m32_swap (td->hwINFO);
+	tdBE   = m32_swap (td->hwBE);
+	tdCBP  = m32_swap (td->hwCBP);
+
+
+	if (!(usb_pipetype (lurb_priv->pipe) == PIPE_CONTROL &&
+	    ((td->index == 0) || (td->index == lurb_priv->length - 1)))) {
+		if (tdBE != 0) {
+			if (td->hwCBP == 0)
+				lurb_priv->actual_length += tdBE - td->data + 1;
+			else
+				lurb_priv->actual_length += tdCBP - td->data;
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* replies to the request have to be on a FIFO basis so
+ * we reverse the reversed done-list */
+
+static td_t * dl_reverse_done_list (ohci_t *ohci)
+{
+	__u32 td_list_hc;
+	td_t *td_rev = NULL;
+	td_t *td_list = NULL;
+	urb_priv_t *lurb_priv = NULL;
+
+	td_list_hc = m32_swap (ohci->hcca->done_head) & 0xfffffff0;
+	ohci->hcca->done_head = 0;
+
+	while (td_list_hc) {
+		td_list = (td_t *)td_list_hc;
+
+		if (TD_CC_GET (m32_swap (td_list->hwINFO))) {
+			lurb_priv = td_list->ed->purb;
+			dbg(" USB-error/status: %x : %p",
+					TD_CC_GET (m32_swap (td_list->hwINFO)), td_list);
+			if (td_list->ed->hwHeadP & m32_swap (0x1)) {
+				if (lurb_priv && ((td_list->index + 1) < lurb_priv->length)) {
+					td_list->ed->hwHeadP =
+						(lurb_priv->td[lurb_priv->length - 1]->hwNextTD & m32_swap (0xfffffff0)) |
+									(td_list->ed->hwHeadP & m32_swap (0x2));
+					lurb_priv->td_cnt += lurb_priv->length - td_list->index - 1;
+				} else
+					td_list->ed->hwHeadP &= m32_swap (0xfffffff2);
+			}
+#ifdef CONFIG_MPC5200
+			td_list->hwNextTD = 0;
+#endif
+		}
+
+		td_list->next_dl_td = td_rev;
+		td_rev = td_list;
+		td_list_hc = m32_swap (td_list->hwNextTD) & 0xfffffff0;
+	}
+	return td_list;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* td done list */
+static int dl_done_list (ohci_t *ohci, td_t *td_list)
+{
+	td_t *td_list_next = NULL;
+	ed_t *ed;
+	int cc = 0;
+	int stat = 0;
+	/* urb_t *urb; */
+	urb_priv_t *lurb_priv;
+	__u32 tdINFO, edHeadP, edTailP;
+
+	while (td_list) {
+		td_list_next = td_list->next_dl_td;
+
+		tdINFO = m32_swap (td_list->hwINFO);
+
+		ed = td_list->ed;
+		lurb_priv = ed->purb;
+
+		dl_transfer_length(td_list);
+
+		/* error code of transfer */
+		cc = TD_CC_GET (tdINFO);
+		if (cc != 0) {
+			dbg("ConditionCode %#x", cc);
+			stat = cc_to_error[cc];
+		}
+
+		/* see if this done list makes for all TD's of current URB,
+		 * and mark the URB finished if so */
+		if (++(lurb_priv->td_cnt) == lurb_priv->length) {
+#if 1
+			if ((ed->state & (ED_OPER | ED_UNLINK)) &&
+			    (lurb_priv->state != URB_DEL))
+#else
+			if ((ed->state & (ED_OPER | ED_UNLINK)))
+#endif
+				lurb_priv->finished = sohci_return_job(ohci,
+						lurb_priv);
+			else
+				dbg("dl_done_list: strange.., ED state %x, ed->state\n");
+		} else
+			dbg("dl_done_list: processing TD %x, len %x\n", lurb_priv->td_cnt,
+				lurb_priv->length);
+		if (ed->state != ED_NEW &&
+			  (usb_pipetype (lurb_priv->pipe) != PIPE_INTERRUPT)) {
+			edHeadP = m32_swap (ed->hwHeadP) & 0xfffffff0;
+			edTailP = m32_swap (ed->hwTailP);
+
+			/* unlink eds if they are not busy */
+			if ((edHeadP == edTailP) && (ed->state == ED_OPER))
+				ep_unlink (ohci, ed);
+		}
+
+		td_list = td_list_next;
+	}
+	return stat;
+}
+
+/*-------------------------------------------------------------------------*
+ * Virtual Root Hub
+ *-------------------------------------------------------------------------*/
+
+/* Device descriptor */
+static __u8 root_hub_dev_des[] =
+{
+	0x12,	    /*	__u8  bLength; */
+	0x01,	    /*	__u8  bDescriptorType; Device */
+	0x10,	    /*	__u16 bcdUSB; v1.1 */
+	0x01,
+	0x09,	    /*	__u8  bDeviceClass; HUB_CLASSCODE */
+	0x00,	    /*	__u8  bDeviceSubClass; */
+	0x00,	    /*	__u8  bDeviceProtocol; */
+	0x08,	    /*	__u8  bMaxPacketSize0; 8 Bytes */
+	0x00,	    /*	__u16 idVendor; */
+	0x00,
+	0x00,	    /*	__u16 idProduct; */
+	0x00,
+	0x00,	    /*	__u16 bcdDevice; */
+	0x00,
+	0x00,	    /*	__u8  iManufacturer; */
+	0x01,	    /*	__u8  iProduct; */
+	0x00,	    /*	__u8  iSerialNumber; */
+	0x01	    /*	__u8  bNumConfigurations; */
+};
+
+
+/* Configuration descriptor */
+static __u8 root_hub_config_des[] =
+{
+	0x09,	    /*	__u8  bLength; */
+	0x02,	    /*	__u8  bDescriptorType; Configuration */
+	0x19,	    /*	__u16 wTotalLength; */
+	0x00,
+	0x01,	    /*	__u8  bNumInterfaces; */
+	0x01,	    /*	__u8  bConfigurationValue; */
+	0x00,	    /*	__u8  iConfiguration; */
+	0x40,	    /*	__u8  bmAttributes;
+		 Bit 7: Bus-powered, 6: Self-powered, 5 Remote-wakwup, 4..0: resvd */
+	0x00,	    /*	__u8  MaxPower; */
+
+	/* interface */
+	0x09,	    /*	__u8  if_bLength; */
+	0x04,	    /*	__u8  if_bDescriptorType; Interface */
+	0x00,	    /*	__u8  if_bInterfaceNumber; */
+	0x00,	    /*	__u8  if_bAlternateSetting; */
+	0x01,	    /*	__u8  if_bNumEndpoints; */
+	0x09,	    /*	__u8  if_bInterfaceClass; HUB_CLASSCODE */
+	0x00,	    /*	__u8  if_bInterfaceSubClass; */
+	0x00,	    /*	__u8  if_bInterfaceProtocol; */
+	0x00,	    /*	__u8  if_iInterface; */
+
+	/* endpoint */
+	0x07,	    /*	__u8  ep_bLength; */
+	0x05,	    /*	__u8  ep_bDescriptorType; Endpoint */
+	0x81,	    /*	__u8  ep_bEndpointAddress; IN Endpoint 1 */
+	0x03,	    /*	__u8  ep_bmAttributes; Interrupt */
+	0x02,	    /*	__u16 ep_wMaxPacketSize; ((MAX_ROOT_PORTS + 1) / 8 */
+	0x00,
+	0xff	    /*	__u8  ep_bInterval; 255 ms */
+};
+
+static unsigned char root_hub_str_index0[] =
+{
+	0x04,			/*  __u8  bLength; */
+	0x03,			/*  __u8  bDescriptorType; String-descriptor */
+	0x09,			/*  __u8  lang ID */
+	0x04,			/*  __u8  lang ID */
+};
+
+static unsigned char root_hub_str_index1[] =
+{
+	28,			/*  __u8  bLength; */
+	0x03,			/*  __u8  bDescriptorType; String-descriptor */
+	'O',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'H',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'C',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'I',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	' ',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'R',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'o',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'o',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	't',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	' ',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'H',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'u',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+	'b',			/*  __u8  Unicode */
+	0,				/*  __u8  Unicode */
+};
+
+/* Hub class-specific descriptor is constructed dynamically */
+
+
+/*-------------------------------------------------------------------------*/
+
+#define OK(x)			len = (x); break
+#ifdef DEBUG
+#define WR_RH_STAT(x)		{info("WR:status %#8x", (x));writel((x), &gohci.regs->roothub.status);}
+#define WR_RH_PORTSTAT(x)	{info("WR:portstatus[%d] %#8x", wIndex-1, (x));writel((x), &gohci.regs->roothub.portstatus[wIndex-1]);}
+#else
+#define WR_RH_STAT(x)		writel((x), &gohci.regs->roothub.status)
+#define WR_RH_PORTSTAT(x)	writel((x), &gohci.regs->roothub.portstatus[wIndex-1])
+#endif
+#define RD_RH_STAT		roothub_status(&gohci)
+#define RD_RH_PORTSTAT		roothub_portstatus(&gohci,wIndex-1)
+
+/* request to virtual root hub */
+
+int rh_check_port_status(ohci_t *controller)
+{
+	__u32 temp, ndp, i;
+	int res;
+
+	res = -1;
+	temp = roothub_a (controller);
+	ndp = (temp & RH_A_NDP);
+#ifdef CONFIG_AT91C_PQFP_UHPBUG
+	ndp = (ndp == 2) ? 1:0;
+#endif
+	for (i = 0; i < ndp; i++) {
+		temp = roothub_portstatus (controller, i);
+		/* check for a device disconnect */
+		if (((temp & (RH_PS_PESC | RH_PS_CSC)) ==
+			(RH_PS_PESC | RH_PS_CSC)) &&
+			((temp & RH_PS_CCS) == 0)) {
+			res = i;
+			break;
+		}
+	}
+	return res;
+}
+
+static int ohci_submit_rh_msg(struct usb_device *dev, unsigned long pipe,
+		void *buffer, int transfer_len, struct devrequest *cmd)
+{
+	void * data = buffer;
+	int leni = transfer_len;
+	int len = 0;
+	int stat = 0;
+	__u32 datab[4];
+	__u8 *data_buf = (__u8 *)datab;
+	__u16 bmRType_bReq;
+	__u16 wValue;
+	__u16 wIndex;
+	__u16 wLength;
+
+#ifdef DEBUG
+pkt_print(NULL, dev, pipe, buffer, transfer_len, cmd, "SUB(rh)", usb_pipein(pipe));
+#else
+	wait_ms(1);
+#endif
+	if ((pipe & PIPE_INTERRUPT) == PIPE_INTERRUPT) {
+		info("Root-Hub submit IRQ: NOT implemented");
+		return 0;
+	}
+
+	bmRType_bReq  = cmd->requesttype | (cmd->request << 8);
+	wValue	      = cpu_to_le16 (cmd->value);
+	wIndex	      = cpu_to_le16 (cmd->index);
+	wLength	      = cpu_to_le16 (cmd->length);
+
+	info("Root-Hub: adr: %2x cmd(%1x): %08x %04x %04x %04x",
+		dev->devnum, 8, bmRType_bReq, wValue, wIndex, wLength);
+
+	switch (bmRType_bReq) {
+	/* Request Destination:
+	   without flags: Device,
+	   RH_INTERFACE: interface,
+	   RH_ENDPOINT: endpoint,
+	   RH_CLASS means HUB here,
+	   RH_OTHER | RH_CLASS	almost ever means HUB_PORT here
+	*/
+
+	case RH_GET_STATUS:
+			*(__u16 *) data_buf = cpu_to_le16 (1); OK (2);
+	case RH_GET_STATUS | RH_INTERFACE:
+			*(__u16 *) data_buf = cpu_to_le16 (0); OK (2);
+	case RH_GET_STATUS | RH_ENDPOINT:
+			*(__u16 *) data_buf = cpu_to_le16 (0); OK (2);
+	case RH_GET_STATUS | RH_CLASS:
+			*(__u32 *) data_buf = cpu_to_le32 (
+				RD_RH_STAT & ~(RH_HS_CRWE | RH_HS_DRWE));
+			OK (4);
+	case RH_GET_STATUS | RH_OTHER | RH_CLASS:
+			*(__u32 *) data_buf = cpu_to_le32 (RD_RH_PORTSTAT); OK (4);
+
+	case RH_CLEAR_FEATURE | RH_ENDPOINT:
+		switch (wValue) {
+			case (RH_ENDPOINT_STALL): OK (0);
+		}
+		break;
+
+	case RH_CLEAR_FEATURE | RH_CLASS:
+		switch (wValue) {
+			case RH_C_HUB_LOCAL_POWER:
+				OK(0);
+			case (RH_C_HUB_OVER_CURRENT):
+					WR_RH_STAT(RH_HS_OCIC); OK (0);
+		}
+		break;
+
+	case RH_CLEAR_FEATURE | RH_OTHER | RH_CLASS:
+		switch (wValue) {
+			case (RH_PORT_ENABLE):
+					WR_RH_PORTSTAT (RH_PS_CCS ); OK (0);
+			case (RH_PORT_SUSPEND):
+					WR_RH_PORTSTAT (RH_PS_POCI); OK (0);
+			case (RH_PORT_POWER):
+					WR_RH_PORTSTAT (RH_PS_LSDA); OK (0);
+			case (RH_C_PORT_CONNECTION):
+					WR_RH_PORTSTAT (RH_PS_CSC ); OK (0);
+			case (RH_C_PORT_ENABLE):
+					WR_RH_PORTSTAT (RH_PS_PESC); OK (0);
+			case (RH_C_PORT_SUSPEND):
+					WR_RH_PORTSTAT (RH_PS_PSSC); OK (0);
+			case (RH_C_PORT_OVER_CURRENT):
+					WR_RH_PORTSTAT (RH_PS_OCIC); OK (0);
+			case (RH_C_PORT_RESET):
+					WR_RH_PORTSTAT (RH_PS_PRSC); OK (0);
+		}
+		break;
+
+	case RH_SET_FEATURE | RH_OTHER | RH_CLASS:
+		switch (wValue) {
+			case (RH_PORT_SUSPEND):
+					WR_RH_PORTSTAT (RH_PS_PSS ); OK (0);
+			case (RH_PORT_RESET): /* BUG IN HUP CODE *********/
+					if (RD_RH_PORTSTAT & RH_PS_CCS)
+					    WR_RH_PORTSTAT (RH_PS_PRS);
+					OK (0);
+			case (RH_PORT_POWER):
+					WR_RH_PORTSTAT (RH_PS_PPS );
+					wait_ms(100);
+					OK (0);
+			case (RH_PORT_ENABLE): /* BUG IN HUP CODE *********/
+					if (RD_RH_PORTSTAT & RH_PS_CCS)
+					    WR_RH_PORTSTAT (RH_PS_PES );
+					OK (0);
+		}
+		break;
+
+	case RH_SET_ADDRESS: gohci.rh.devnum = wValue; OK(0);
+
+	case RH_GET_DESCRIPTOR:
+		switch ((wValue & 0xff00) >> 8) {
+			case (0x01): /* device descriptor */
+				len = min_t(unsigned int,
+					  leni,
+					  min_t(unsigned int,
+					      sizeof (root_hub_dev_des),
+					      wLength));
+				data_buf = root_hub_dev_des; OK(len);
+			case (0x02): /* configuration descriptor */
+				len = min_t(unsigned int,
+					  leni,
+					  min_t(unsigned int,
+					      sizeof (root_hub_config_des),
+					      wLength));
+				data_buf = root_hub_config_des; OK(len);
+			case (0x03): /* string descriptors */
+				if(wValue==0x0300) {
+					len = min_t(unsigned int,
+						  leni,
+						  min_t(unsigned int,
+						      sizeof (root_hub_str_index0),
+						      wLength));
+					data_buf = root_hub_str_index0;
+					OK(len);
+				}
+				if(wValue==0x0301) {
+					len = min_t(unsigned int,
+						  leni,
+						  min_t(unsigned int,
+						      sizeof (root_hub_str_index1),
+						      wLength));
+					data_buf = root_hub_str_index1;
+					OK(len);
+			}
+			default:
+				stat = USB_ST_STALLED;
+		}
+		break;
+
+	case RH_GET_DESCRIPTOR | RH_CLASS:
+	{
+		__u32 temp = roothub_a (&gohci);
+
+		data_buf [0] = 9;		/* min length; */
+		data_buf [1] = 0x29;
+		data_buf [2] = temp & RH_A_NDP;
+#ifdef CONFIG_AT91C_PQFP_UHPBUG
+		data_buf [2] = (data_buf [2] == 2) ? 1:0;
+#endif
+		data_buf [3] = 0;
+		if (temp & RH_A_PSM)	/* per-port power switching? */
+			data_buf [3] |= 0x1;
+		if (temp & RH_A_NOCP)	/* no overcurrent reporting? */
+			data_buf [3] |= 0x10;
+		else if (temp & RH_A_OCPM)	/* per-port overcurrent reporting? */
+			data_buf [3] |= 0x8;
+
+		/* corresponds to data_buf[4-7] */
+		datab [1] = 0;
+		data_buf [5] = (temp & RH_A_POTPGT) >> 24;
+		temp = roothub_b (&gohci);
+		data_buf [7] = temp & RH_B_DR;
+		if (data_buf [2] < 7) {
+			data_buf [8] = 0xff;
+		} else {
+			data_buf [0] += 2;
+			data_buf [8] = (temp & RH_B_DR) >> 8;
+			data_buf [10] = data_buf [9] = 0xff;
+		}
+
+		len = min_t(unsigned int, leni,
+			    min_t(unsigned int, data_buf [0], wLength));
+		OK (len);
+	}
+
+	case RH_GET_CONFIGURATION:	*(__u8 *) data_buf = 0x01; OK (1);
+
+	case RH_SET_CONFIGURATION:	WR_RH_STAT (0x10000); OK (0);
+
+	default:
+		dbg ("unsupported root hub command");
+		stat = USB_ST_STALLED;
+	}
+
+#ifdef	DEBUG
+	ohci_dump_roothub (&gohci, 1);
+#else
+	wait_ms(1);
+#endif
+
+	len = min_t(int, len, leni);
+	if (data != data_buf)
+	    memcpy (data, data_buf, len);
+	dev->act_len = len;
+	dev->status = stat;
+
+#ifdef DEBUG
+	pkt_print(NULL, dev, pipe, buffer, transfer_len, cmd, "RET(rh)", 0/*usb_pipein(pipe)*/);
+#else
+	wait_ms(1);
+#endif
+
+	return stat;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* common code for handling submit messages - used for all but root hub */
+/* accesses. */
+int submit_common_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		int transfer_len, struct devrequest *setup, int interval)
+{
+	int stat = 0;
+	int maxsize = usb_maxpacket(dev, pipe);
+	int timeout;
+	urb_priv_t *urb;
+
+	urb = malloc(sizeof(urb_priv_t));
+	memset(urb, 0, sizeof(urb_priv_t));
+
+	urb->dev = dev;
+	urb->pipe = pipe;
+	urb->transfer_buffer = buffer;
+	urb->transfer_buffer_length = transfer_len;
+	urb->interval = interval;
+
+	/* device pulled? Shortcut the action. */
+	if (devgone == dev) {
+		dev->status = USB_ST_CRC_ERR;
+		return 0;
+	}
+
+#ifdef DEBUG
+	urb->actual_length = 0;
+	pkt_print(urb, dev, pipe, buffer, transfer_len, setup, "SUB", usb_pipein(pipe));
+#else
+	wait_ms(1);
+#endif
+	if (!maxsize) {
+		err("submit_common_message: pipesize for pipe %lx is zero",
+			pipe);
+		return -1;
+	}
+
+	if (sohci_submit_job(urb, setup) < 0) {
+		err("sohci_submit_job failed");
+		return -1;
+	}
+
+#if 0
+	wait_ms(10);
+	/* ohci_dump_status(&gohci); */
+#endif
+
+	/* allow more time for a BULK device to react - some are slow */
+#define BULK_TO	 5000	/* timeout in milliseconds */
+	if (usb_pipetype (pipe) == PIPE_BULK)
+		timeout = BULK_TO;
+	else
+		timeout = 100;
+
+	/* wait for it to complete */
+	for (;;) {
+		/* check whether the controller is done */
+		stat = hc_interrupt();
+		if (stat < 0) {
+			stat = USB_ST_CRC_ERR;
+			break;
+		}
+
+		/* NOTE: since we are not interrupt driven in U-Boot and always
+		 * handle only one URB at a time, we cannot assume the
+		 * transaction finished on the first successful return from
+		 * hc_interrupt().. unless the flag for current URB is set,
+		 * meaning that all TD's to/from device got actually
+		 * transferred and processed. If the current URB is not
+		 * finished we need to re-iterate this loop so as
+		 * hc_interrupt() gets called again as there needs to be some
+		 * more TD's to process still */
+		if ((stat >= 0) && (stat != 0xff) && (urb->finished)) {
+			/* 0xff is returned for an SF-interrupt */
+			break;
+		}
+
+		if (--timeout) {
+			wait_ms(1);
+			if (!urb->finished)
+				dbg("\%");
+
+		} else {
+			err("CTL:TIMEOUT ");
+			dbg("submit_common_msg: TO status %x\n", stat);
+			urb->finished = 1;
+			stat = USB_ST_CRC_ERR;
+			break;
+		}
+	}
+
+	dev->status = stat;
+	dev->act_len = transfer_len;
+
+#ifdef DEBUG
+	pkt_print(urb, dev, pipe, buffer, transfer_len, setup, "RET(ctlr)", usb_pipein(pipe));
+#else
+	wait_ms(1);
+#endif
+
+	/* free TDs in urb_priv */
+	if (usb_pipetype (pipe) != PIPE_INTERRUPT)
+		urb_free_priv (urb);
+	return 0;
+}
+
+/* submit routines called from usb.c */
+int submit_bulk_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		int transfer_len)
+{
+	info("submit_bulk_msg");
+	return submit_common_msg(dev, pipe, buffer, transfer_len, NULL, 0);
+}
+
+int submit_control_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		int transfer_len, struct devrequest *setup)
+{
+	int maxsize = usb_maxpacket(dev, pipe);
+
+	info("submit_control_msg");
+#ifdef DEBUG
+	pkt_print(NULL, dev, pipe, buffer, transfer_len, setup, "SUB", usb_pipein(pipe));
+#else
+	wait_ms(1);
+#endif
+	if (!maxsize) {
+		err("submit_control_message: pipesize for pipe %lx is zero",
+			pipe);
+		return -1;
+	}
+	if (((pipe >> 8) & 0x7f) == gohci.rh.devnum) {
+		gohci.rh.dev = dev;
+		/* root hub - redirect */
+		return ohci_submit_rh_msg(dev, pipe, buffer, transfer_len,
+			setup);
+	}
+
+	return submit_common_msg(dev, pipe, buffer, transfer_len, setup, 0);
+}
+
+int submit_int_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		int transfer_len, int interval)
+{
+	info("submit_int_msg");
+	return submit_common_msg(dev, pipe, buffer, transfer_len, NULL,
+			interval);
+}
+
+/*-------------------------------------------------------------------------*
+ * HC functions
+ *-------------------------------------------------------------------------*/
+
+/* reset the HC and BUS */
+
+static int hc_reset (ohci_t *ohci)
+{
+	int timeout = 30;
+	int smm_timeout = 50; /* 0,5 sec */
+
+	dbg("%s\n", __FUNCTION__);
+
+	if (readl (&ohci->regs->control) & OHCI_CTRL_IR) { /* SMM owns the HC */
+		writel (OHCI_OCR, &ohci->regs->cmdstatus); /* request ownership */
+		info("USB HC TakeOver from SMM");
+		while (readl (&ohci->regs->control) & OHCI_CTRL_IR) {
+			wait_ms (10);
+			if (--smm_timeout == 0) {
+				err("USB HC TakeOver failed!");
+				return -1;
+			}
+		}
+	}
+
+	/* Disable HC interrupts */
+	writel (OHCI_INTR_MIE, &ohci->regs->intrdisable);
+
+	dbg("USB HC reset_hc usb-%s: ctrl = 0x%X ;\n",
+		ohci->slot_name,
+		readl(&ohci->regs->control));
+
+	/* Reset USB (needed by some controllers) */
+	ohci->hc_control = 0;
+	writel (ohci->hc_control, &ohci->regs->control);
+
+	/* HC Reset requires max 10 us delay */
+	writel (OHCI_HCR,  &ohci->regs->cmdstatus);
+	while ((readl (&ohci->regs->cmdstatus) & OHCI_HCR) != 0) {
+		if (--timeout == 0) {
+			err("USB HC reset timed out!");
+			return -1;
+		}
+		udelay (1);
+	}
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* Start an OHCI controller, set the BUS operational
+ * enable interrupts
+ * connect the virtual root hub */
+
+static int hc_start (ohci_t * ohci)
+{
+	__u32 mask;
+	unsigned int fminterval;
+
+	ohci->disabled = 1;
+
+	/* Tell the controller where the control and bulk lists are
+	 * The lists are empty now. */
+
+	writel (0, &ohci->regs->ed_controlhead);
+	writel (0, &ohci->regs->ed_bulkhead);
+
+	writel ((__u32)ohci->hcca, &ohci->regs->hcca); /* a reset clears this */
+
+	fminterval = 0x2edf;
+	writel ((fminterval * 9) / 10, &ohci->regs->periodicstart);
+	fminterval |= ((((fminterval - 210) * 6) / 7) << 16);
+	writel (fminterval, &ohci->regs->fminterval);
+	writel (0x628, &ohci->regs->lsthresh);
+
+	/* start controller operations */
+	ohci->hc_control = OHCI_CONTROL_INIT | OHCI_USB_OPER;
+	ohci->disabled = 0;
+	writel (ohci->hc_control, &ohci->regs->control);
+
+	/* disable all interrupts */
+	mask = (OHCI_INTR_SO | OHCI_INTR_WDH | OHCI_INTR_SF | OHCI_INTR_RD |
+			OHCI_INTR_UE | OHCI_INTR_FNO | OHCI_INTR_RHSC |
+			OHCI_INTR_OC | OHCI_INTR_MIE);
+	writel (mask, &ohci->regs->intrdisable);
+	/* clear all interrupts */
+	mask &= ~OHCI_INTR_MIE;
+	writel (mask, &ohci->regs->intrstatus);
+	/* Choose the interrupts we care about now  - but w/o MIE */
+	mask = OHCI_INTR_RHSC | OHCI_INTR_UE | OHCI_INTR_WDH | OHCI_INTR_SO;
+	writel (mask, &ohci->regs->intrenable);
+
+#ifdef	OHCI_USE_NPS
+	/* required for AMD-756 and some Mac platforms */
+	writel ((roothub_a (ohci) | RH_A_NPS) & ~RH_A_PSM,
+		&ohci->regs->roothub.a);
+	writel (RH_HS_LPSC, &ohci->regs->roothub.status);
+#endif	/* OHCI_USE_NPS */
+
+#define mdelay(n) ({unsigned long msec=(n); while (msec--) udelay(1000);})
+	/* POTPGT delay is bits 24-31, in 2 ms units. */
+	mdelay ((roothub_a (ohci) >> 23) & 0x1fe);
+
+	/* connect the virtual root hub */
+	ohci->rh.devnum = 0;
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* Poll USB interrupt. */
+void usb_event_poll(void)
+{
+	hc_interrupt();
+}
+
+/* an interrupt happens */
+
+static int hc_interrupt (void)
+{
+	ohci_t *ohci = &gohci;
+	struct ohci_regs *regs = ohci->regs;
+	int ints;
+	int stat = -1;
+
+	if ((ohci->hcca->done_head != 0) &&
+	    !(m32_swap (ohci->hcca->done_head) & 0x01)) {
+		ints =  OHCI_INTR_WDH;
+	} else if ((ints = readl (&regs->intrstatus)) == ~(u32)0) {
+		ohci->disabled++;
+		err ("%s device removed!", ohci->slot_name);
+		return -1;
+	} else if ((ints &= readl (&regs->intrenable)) == 0) {
+		dbg("hc_interrupt: returning..\n");
+		return 0xff;
+	}
+
+	/* dbg("Interrupt: %x frame: %x", ints, le16_to_cpu (ohci->hcca->frame_no)); */
+
+	if (ints & OHCI_INTR_RHSC) {
+		got_rhsc = 1;
+		stat = 0xff;
+	}
+
+	if (ints & OHCI_INTR_UE) {
+		ohci->disabled++;
+		err ("OHCI Unrecoverable Error, controller usb-%s disabled",
+			ohci->slot_name);
+		/* e.g. due to PCI Master/Target Abort */
+
+#ifdef	DEBUG
+		ohci_dump (ohci, 1);
+#else
+	wait_ms(1);
+#endif
+		/* FIXME: be optimistic, hope that bug won't repeat often. */
+		/* Make some non-interrupt context restart the controller. */
+		/* Count and limit the retries though; either hardware or */
+		/* software errors can go forever... */
+		hc_reset (ohci);
+		return -1;
+	}
+
+	if (ints & OHCI_INTR_WDH) {
+		wait_ms(1);
+		writel (OHCI_INTR_WDH, &regs->intrdisable);
+		(void)readl (&regs->intrdisable); /* flush */
+		stat = dl_done_list (&gohci, dl_reverse_done_list (&gohci));
+		writel (OHCI_INTR_WDH, &regs->intrenable);
+		(void)readl (&regs->intrdisable); /* flush */
+	}
+
+	if (ints & OHCI_INTR_SO) {
+		dbg("USB Schedule overrun\n");
+		writel (OHCI_INTR_SO, &regs->intrenable);
+		stat = -1;
+	}
+
+	/* FIXME:  this assumes SOF (1/ms) interrupts don't get lost... */
+	if (ints & OHCI_INTR_SF) {
+		unsigned int frame = m16_swap (ohci->hcca->frame_no) & 1;
+		wait_ms(1);
+		writel (OHCI_INTR_SF, &regs->intrdisable);
+		if (ohci->ed_rm_list[frame] != NULL)
+			writel (OHCI_INTR_SF, &regs->intrenable);
+		stat = 0xff;
+	}
+
+	writel (ints, &regs->intrstatus);
+	return stat;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*-------------------------------------------------------------------------*/
+
+/* De-allocate all resources.. */
+
+static void hc_release_ohci (ohci_t *ohci)
+{
+	dbg ("USB HC release ohci usb-%s", ohci->slot_name);
+
+	if (!ohci->disabled)
+		hc_reset (ohci);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * low level initalisation routine, called from usb.c
+ */
+static char ohci_inited = 0;
+
+int usb_lowlevel_init(void)
+{
+#ifdef CONFIG_PCI_OHCI
+	pci_dev_t pdev;
+#endif
+
+#ifdef CFG_USB_OHCI_CPU_INIT
+	/* cpu dependant init */
+	if(usb_cpu_init())
+		return -1;
+#endif
+
+#ifdef CFG_USB_OHCI_BOARD_INIT
+	/*  board dependant init */
+	if(usb_board_init())
+		return -1;
+#endif
+	memset (&gohci, 0, sizeof (ohci_t));
+
+	/* align the storage */
+	if ((__u32)&ghcca[0] & 0xff) {
+		err("HCCA not aligned!!");
+		return -1;
+	}
+	phcca = &ghcca[0];
+	info("aligned ghcca %p", phcca);
+	memset(&ohci_dev, 0, sizeof(struct ohci_device));
+	if ((__u32)&ohci_dev.ed[0] & 0x7) {
+		err("EDs not aligned!!");
+		return -1;
+	}
+	memset(gtd, 0, sizeof(td_t) * (NUM_TD + 1));
+	if ((__u32)gtd & 0x7) {
+		err("TDs not aligned!!");
+		return -1;
+	}
+	ptd = gtd;
+	gohci.hcca = phcca;
+	memset (phcca, 0, sizeof (struct ohci_hcca));
+
+	gohci.disabled = 1;
+	gohci.sleeping = 0;
+	gohci.irq = -1;
+#ifdef CONFIG_PCI_OHCI
+	pdev = pci_find_devices(ohci_pci_ids, 0);
+
+	if (pdev != -1) {
+		u16 vid, did;
+		u32 base;
+		pci_read_config_word(pdev, PCI_VENDOR_ID, &vid);
+		pci_read_config_word(pdev, PCI_DEVICE_ID, &did);
+		printf("OHCI pci controller (%04x, %04x) found @(%d:%d:%d)\n",
+				vid, did, (pdev >> 16) & 0xff,
+				(pdev >> 11) & 0x1f, (pdev >> 8) & 0x7);
+		pci_read_config_dword(pdev, PCI_BASE_ADDRESS_0, &base);
+		printf("OHCI regs address 0x%08x\n", base);
+		gohci.regs = (struct ohci_regs *)base;
+	} else
+		return -1;
+#else
+	gohci.regs = (struct ohci_regs *)CFG_USB_OHCI_REGS_BASE;
+#endif
+
+	gohci.flags = 0;
+	gohci.slot_name = CFG_USB_OHCI_SLOT_NAME;
+
+	if (hc_reset (&gohci) < 0) {
+		hc_release_ohci (&gohci);
+		err ("can't reset usb-%s", gohci.slot_name);
+#ifdef CFG_USB_OHCI_BOARD_INIT
+		/* board dependant cleanup */
+		usb_board_init_fail();
+#endif
+
+#ifdef CFG_USB_OHCI_CPU_INIT
+		/* cpu dependant cleanup */
+		usb_cpu_init_fail();
+#endif
+		return -1;
+	}
+
+	/* FIXME this is a second HC reset; why?? */
+	/* writel(gohci.hc_control = OHCI_USB_RESET, &gohci.regs->control);
+	   wait_ms(10); */
+	if (hc_start (&gohci) < 0) {
+		err ("can't start usb-%s", gohci.slot_name);
+		hc_release_ohci (&gohci);
+		/* Initialization failed */
+#ifdef CFG_USB_OHCI_BOARD_INIT
+		/* board dependant cleanup */
+		usb_board_stop();
+#endif
+
+#ifdef CFG_USB_OHCI_CPU_INIT
+		/* cpu dependant cleanup */
+		usb_cpu_stop();
+#endif
+		return -1;
+	}
+
+#ifdef	DEBUG
+	ohci_dump (&gohci, 1);
+#else
+	wait_ms(1);
+#endif
+	ohci_inited = 1;
+	return 0;
+}
+
+int usb_lowlevel_stop(void)
+{
+	/* this gets called really early - before the controller has */
+	/* even been initialized! */
+	if (!ohci_inited)
+		return 0;
+	/* TODO release any interrupts, etc. */
+	/* call hc_release_ohci() here ? */
+	hc_reset (&gohci);
+
+#ifdef CFG_USB_OHCI_BOARD_INIT
+	/* board dependant cleanup */
+	if(usb_board_stop())
+		return -1;
+#endif
+
+#ifdef CFG_USB_OHCI_CPU_INIT
+	/* cpu dependant cleanup */
+	if(usb_cpu_stop())
+		return -1;
+#endif
+
+	return 0;
+}
+#endif /* CONFIG_USB_OHCI_NEW */
diff --git a/drivers/usb_ohci.h b/drivers/usb_ohci.h
new file mode 100644
index 00000000000..380cb4c9273
--- /dev/null
+++ b/drivers/usb_ohci.h
@@ -0,0 +1,445 @@
+/*
+ * URB OHCI HCD (Host Controller Driver) for USB.
+ *
+ * (C) Copyright 1999 Roman Weissgaerber <weissg@vienna.at>
+ * (C) Copyright 2000-2001 David Brownell <dbrownell@users.sourceforge.net>
+ *
+ * usb-ohci.h
+ */
+
+/* functions for doing board or CPU specific setup/cleanup */
+extern int usb_board_init(void);
+extern int usb_board_stop(void);
+extern int usb_board_init_fail(void);
+
+extern int usb_cpu_init(void);
+extern int usb_cpu_stop(void);
+extern int usb_cpu_init_fail(void);
+
+
+static int cc_to_error[16] = {
+
+/* mapping of the OHCI CC status to error codes */
+	/* No  Error  */	       0,
+	/* CRC Error  */	       USB_ST_CRC_ERR,
+	/* Bit Stuff  */	       USB_ST_BIT_ERR,
+	/* Data Togg  */	       USB_ST_CRC_ERR,
+	/* Stall      */	       USB_ST_STALLED,
+	/* DevNotResp */	       -1,
+	/* PIDCheck   */	       USB_ST_BIT_ERR,
+	/* UnExpPID   */	       USB_ST_BIT_ERR,
+	/* DataOver   */	       USB_ST_BUF_ERR,
+	/* DataUnder  */	       USB_ST_BUF_ERR,
+	/* reservd    */	       -1,
+	/* reservd    */	       -1,
+	/* BufferOver */	       USB_ST_BUF_ERR,
+	/* BuffUnder  */	       USB_ST_BUF_ERR,
+	/* Not Access */	       -1,
+	/* Not Access */	       -1
+};
+
+/* ED States */
+
+#define ED_NEW		0x00
+#define ED_UNLINK	0x01
+#define ED_OPER		0x02
+#define ED_DEL		0x04
+#define ED_URB_DEL	0x08
+
+/* usb_ohci_ed */
+struct ed {
+	__u32 hwINFO;
+	__u32 hwTailP;
+	__u32 hwHeadP;
+	__u32 hwNextED;
+
+	struct ed *ed_prev;
+	__u8 int_period;
+	__u8 int_branch;
+	__u8 int_load;
+	__u8 int_interval;
+	__u8 state;
+	__u8 type;
+	__u16 last_iso;
+	struct ed *ed_rm_list;
+
+	struct usb_device *usb_dev;
+	void *purb;
+	__u32 unused[2];
+} __attribute((aligned(16)));
+typedef struct ed ed_t;
+
+
+/* TD info field */
+#define TD_CC	    0xf0000000
+#define TD_CC_GET(td_p) ((td_p >>28) & 0x0f)
+#define TD_CC_SET(td_p, cc) (td_p) = ((td_p) & 0x0fffffff) | (((cc) & 0x0f) << 28)
+#define TD_EC	    0x0C000000
+#define TD_T	    0x03000000
+#define TD_T_DATA0  0x02000000
+#define TD_T_DATA1  0x03000000
+#define TD_T_TOGGLE 0x00000000
+#define TD_R	    0x00040000
+#define TD_DI	    0x00E00000
+#define TD_DI_SET(X) (((X) & 0x07)<< 21)
+#define TD_DP	    0x00180000
+#define TD_DP_SETUP 0x00000000
+#define TD_DP_IN    0x00100000
+#define TD_DP_OUT   0x00080000
+
+#define TD_ISO	    0x00010000
+#define TD_DEL	    0x00020000
+
+/* CC Codes */
+#define TD_CC_NOERROR	   0x00
+#define TD_CC_CRC	   0x01
+#define TD_CC_BITSTUFFING  0x02
+#define TD_CC_DATATOGGLEM  0x03
+#define TD_CC_STALL	   0x04
+#define TD_DEVNOTRESP	   0x05
+#define TD_PIDCHECKFAIL	   0x06
+#define TD_UNEXPECTEDPID   0x07
+#define TD_DATAOVERRUN	   0x08
+#define TD_DATAUNDERRUN	   0x09
+#define TD_BUFFEROVERRUN   0x0C
+#define TD_BUFFERUNDERRUN  0x0D
+#define TD_NOTACCESSED	   0x0F
+
+
+#define MAXPSW 1
+
+struct td {
+	__u32 hwINFO;
+	__u32 hwCBP;		/* Current Buffer Pointer */
+	__u32 hwNextTD;		/* Next TD Pointer */
+	__u32 hwBE;		/* Memory Buffer End Pointer */
+
+/* #ifndef CONFIG_MPC5200 /\* this seems wrong *\/ */
+	__u16 hwPSW[MAXPSW];
+/* #endif */
+	__u8 unused;
+	__u8 index;
+	struct ed *ed;
+	struct td *next_dl_td;
+	struct usb_device *usb_dev;
+	int transfer_len;
+	__u32 data;
+
+	__u32 unused2[2];
+} __attribute((aligned(32)));
+typedef struct td td_t;
+
+#define OHCI_ED_SKIP	(1 << 14)
+
+/*
+ * The HCCA (Host Controller Communications Area) is a 256 byte
+ * structure defined in the OHCI spec. that the host controller is
+ * told the base address of.  It must be 256-byte aligned.
+ */
+
+#define NUM_INTS 32	/* part of the OHCI standard */
+struct ohci_hcca {
+	__u32	int_table[NUM_INTS];	/* Interrupt ED table */
+#if defined(CONFIG_MPC5200)
+	__u16	pad1;			/* set to 0 on each frame_no change */
+	__u16	frame_no;		/* current frame number */
+#else
+	__u16	frame_no;		/* current frame number */
+	__u16	pad1;			/* set to 0 on each frame_no change */
+#endif
+	__u32	done_head;		/* info returned for an interrupt */
+	u8		reserved_for_hc[116];
+} __attribute((aligned(256)));
+
+
+/*
+ * Maximum number of root hub ports.
+ */
+#ifndef CFG_USB_OHCI_MAX_ROOT_PORTS
+# error "CFG_USB_OHCI_MAX_ROOT_PORTS undefined!"
+#endif
+
+/*
+ * This is the structure of the OHCI controller's memory mapped I/O
+ * region.  This is Memory Mapped I/O.	You must use the readl() and
+ * writel() macros defined in asm/io.h to access these!!
+ */
+struct ohci_regs {
+	/* control and status registers */
+	__u32	revision;
+	__u32	control;
+	__u32	cmdstatus;
+	__u32	intrstatus;
+	__u32	intrenable;
+	__u32	intrdisable;
+	/* memory pointers */
+	__u32	hcca;
+	__u32	ed_periodcurrent;
+	__u32	ed_controlhead;
+	__u32	ed_controlcurrent;
+	__u32	ed_bulkhead;
+	__u32	ed_bulkcurrent;
+	__u32	donehead;
+	/* frame counters */
+	__u32	fminterval;
+	__u32	fmremaining;
+	__u32	fmnumber;
+	__u32	periodicstart;
+	__u32	lsthresh;
+	/* Root hub ports */
+	struct	ohci_roothub_regs {
+		__u32	a;
+		__u32	b;
+		__u32	status;
+		__u32	portstatus[CFG_USB_OHCI_MAX_ROOT_PORTS];
+	} roothub;
+} __attribute((aligned(32)));
+
+
+/* OHCI CONTROL AND STATUS REGISTER MASKS */
+
+/*
+ * HcControl (control) register masks
+ */
+#define OHCI_CTRL_CBSR	(3 << 0)	/* control/bulk service ratio */
+#define OHCI_CTRL_PLE	(1 << 2)	/* periodic list enable */
+#define OHCI_CTRL_IE	(1 << 3)	/* isochronous enable */
+#define OHCI_CTRL_CLE	(1 << 4)	/* control list enable */
+#define OHCI_CTRL_BLE	(1 << 5)	/* bulk list enable */
+#define OHCI_CTRL_HCFS	(3 << 6)	/* host controller functional state */
+#define OHCI_CTRL_IR	(1 << 8)	/* interrupt routing */
+#define OHCI_CTRL_RWC	(1 << 9)	/* remote wakeup connected */
+#define OHCI_CTRL_RWE	(1 << 10)	/* remote wakeup enable */
+
+/* pre-shifted values for HCFS */
+#	define OHCI_USB_RESET	(0 << 6)
+#	define OHCI_USB_RESUME	(1 << 6)
+#	define OHCI_USB_OPER	(2 << 6)
+#	define OHCI_USB_SUSPEND (3 << 6)
+
+/*
+ * HcCommandStatus (cmdstatus) register masks
+ */
+#define OHCI_HCR	(1 << 0)	/* host controller reset */
+#define OHCI_CLF	(1 << 1)	/* control list filled */
+#define OHCI_BLF	(1 << 2)	/* bulk list filled */
+#define OHCI_OCR	(1 << 3)	/* ownership change request */
+#define OHCI_SOC	(3 << 16)	/* scheduling overrun count */
+
+/*
+ * masks used with interrupt registers:
+ * HcInterruptStatus (intrstatus)
+ * HcInterruptEnable (intrenable)
+ * HcInterruptDisable (intrdisable)
+ */
+#define OHCI_INTR_SO	(1 << 0)	/* scheduling overrun */
+#define OHCI_INTR_WDH	(1 << 1)	/* writeback of done_head */
+#define OHCI_INTR_SF	(1 << 2)	/* start frame */
+#define OHCI_INTR_RD	(1 << 3)	/* resume detect */
+#define OHCI_INTR_UE	(1 << 4)	/* unrecoverable error */
+#define OHCI_INTR_FNO	(1 << 5)	/* frame number overflow */
+#define OHCI_INTR_RHSC	(1 << 6)	/* root hub status change */
+#define OHCI_INTR_OC	(1 << 30)	/* ownership change */
+#define OHCI_INTR_MIE	(1 << 31)	/* master interrupt enable */
+
+
+/* Virtual Root HUB */
+struct virt_root_hub {
+	int devnum; /* Address of Root Hub endpoint */
+	void *dev;  /* was urb */
+	void *int_addr;
+	int send;
+	int interval;
+};
+
+/* USB HUB CONSTANTS (not OHCI-specific; see hub.h) */
+
+/* destination of request */
+#define RH_INTERFACE		   0x01
+#define RH_ENDPOINT		   0x02
+#define RH_OTHER		   0x03
+
+#define RH_CLASS		   0x20
+#define RH_VENDOR		   0x40
+
+/* Requests: bRequest << 8 | bmRequestType */
+#define RH_GET_STATUS		0x0080
+#define RH_CLEAR_FEATURE	0x0100
+#define RH_SET_FEATURE		0x0300
+#define RH_SET_ADDRESS		0x0500
+#define RH_GET_DESCRIPTOR	0x0680
+#define RH_SET_DESCRIPTOR	0x0700
+#define RH_GET_CONFIGURATION	0x0880
+#define RH_SET_CONFIGURATION	0x0900
+#define RH_GET_STATE		0x0280
+#define RH_GET_INTERFACE	0x0A80
+#define RH_SET_INTERFACE	0x0B00
+#define RH_SYNC_FRAME		0x0C80
+/* Our Vendor Specific Request */
+#define RH_SET_EP		0x2000
+
+
+/* Hub port features */
+#define RH_PORT_CONNECTION	   0x00
+#define RH_PORT_ENABLE		   0x01
+#define RH_PORT_SUSPEND		   0x02
+#define RH_PORT_OVER_CURRENT	   0x03
+#define RH_PORT_RESET		   0x04
+#define RH_PORT_POWER		   0x08
+#define RH_PORT_LOW_SPEED	   0x09
+
+#define RH_C_PORT_CONNECTION	   0x10
+#define RH_C_PORT_ENABLE	   0x11
+#define RH_C_PORT_SUSPEND	   0x12
+#define RH_C_PORT_OVER_CURRENT	   0x13
+#define RH_C_PORT_RESET		   0x14
+
+/* Hub features */
+#define RH_C_HUB_LOCAL_POWER	   0x00
+#define RH_C_HUB_OVER_CURRENT	   0x01
+
+#define RH_DEVICE_REMOTE_WAKEUP	   0x00
+#define RH_ENDPOINT_STALL	   0x01
+
+#define RH_ACK			   0x01
+#define RH_REQ_ERR		   -1
+#define RH_NACK			   0x00
+
+
+/* OHCI ROOT HUB REGISTER MASKS */
+
+/* roothub.portstatus [i] bits */
+#define RH_PS_CCS	     0x00000001		/* current connect status */
+#define RH_PS_PES	     0x00000002		/* port enable status*/
+#define RH_PS_PSS	     0x00000004		/* port suspend status */
+#define RH_PS_POCI	     0x00000008		/* port over current indicator */
+#define RH_PS_PRS	     0x00000010		/* port reset status */
+#define RH_PS_PPS	     0x00000100		/* port power status */
+#define RH_PS_LSDA	     0x00000200		/* low speed device attached */
+#define RH_PS_CSC	     0x00010000		/* connect status change */
+#define RH_PS_PESC	     0x00020000		/* port enable status change */
+#define RH_PS_PSSC	     0x00040000		/* port suspend status change */
+#define RH_PS_OCIC	     0x00080000		/* over current indicator change */
+#define RH_PS_PRSC	     0x00100000		/* port reset status change */
+
+/* roothub.status bits */
+#define RH_HS_LPS	     0x00000001		/* local power status */
+#define RH_HS_OCI	     0x00000002		/* over current indicator */
+#define RH_HS_DRWE	     0x00008000		/* device remote wakeup enable */
+#define RH_HS_LPSC	     0x00010000		/* local power status change */
+#define RH_HS_OCIC	     0x00020000		/* over current indicator change */
+#define RH_HS_CRWE	     0x80000000		/* clear remote wakeup enable */
+
+/* roothub.b masks */
+#define RH_B_DR		0x0000ffff		/* device removable flags */
+#define RH_B_PPCM	0xffff0000		/* port power control mask */
+
+/* roothub.a masks */
+#define RH_A_NDP	(0xff << 0)		/* number of downstream ports */
+#define RH_A_PSM	(1 << 8)		/* power switching mode */
+#define RH_A_NPS	(1 << 9)		/* no power switching */
+#define RH_A_DT		(1 << 10)		/* device type (mbz) */
+#define RH_A_OCPM	(1 << 11)		/* over current protection mode */
+#define RH_A_NOCP	(1 << 12)		/* no over current protection */
+#define RH_A_POTPGT	(0xff << 24)		/* power on to power good time */
+
+/* urb */
+#define N_URB_TD 48
+typedef struct
+{
+	ed_t *ed;
+	__u16 length;	/* number of tds associated with this request */
+	__u16 td_cnt;	/* number of tds already serviced */
+	struct usb_device *dev;
+	int   state;
+	unsigned long pipe;
+	void *transfer_buffer;
+	int transfer_buffer_length;
+	int interval;
+	int actual_length;
+	int finished;
+	td_t *td[N_URB_TD];	/* list pointer to all corresponding TDs associated with this request */
+} urb_priv_t;
+#define URB_DEL 1
+
+/*
+ * This is the full ohci controller description
+ *
+ * Note how the "proper" USB information is just
+ * a subset of what the full implementation needs. (Linus)
+ */
+
+
+typedef struct ohci {
+	struct ohci_hcca *hcca;		/* hcca */
+	/*dma_addr_t hcca_dma;*/
+
+	int irq;
+	int disabled;			/* e.g. got a UE, we're hung */
+	int sleeping;
+	unsigned long flags;		/* for HC bugs */
+
+	struct ohci_regs *regs; /* OHCI controller's memory */
+
+	int ohci_int_load[32];	 /* load of the 32 Interrupt Chains (for load balancing)*/
+	ed_t *ed_rm_list[2];	 /* lists of all endpoints to be removed */
+	ed_t *ed_bulktail;	 /* last endpoint of bulk list */
+	ed_t *ed_controltail;	 /* last endpoint of control list */
+	int intrstatus;
+	__u32 hc_control;		/* copy of the hc control reg */
+	struct usb_device *dev[32];
+	struct virt_root_hub rh;
+
+	const char	*slot_name;
+} ohci_t;
+
+#define NUM_EDS 8		/* num of preallocated endpoint descriptors */
+
+struct ohci_device {
+	ed_t	ed[NUM_EDS];
+	int ed_cnt;
+};
+
+/* hcd */
+/* endpoint */
+static int ep_link(ohci_t * ohci, ed_t * ed);
+static int ep_unlink(ohci_t * ohci, ed_t * ed);
+static ed_t * ep_add_ed(struct usb_device * usb_dev, unsigned long pipe,
+		int interval, int load);
+
+/*-------------------------------------------------------------------------*/
+
+/* we need more TDs than EDs */
+#define NUM_TD 64
+
+/* +1 so we can align the storage */
+td_t gtd[NUM_TD+1];
+/* pointers to aligned storage */
+td_t *ptd;
+
+/* TDs ... */
+static inline struct td *
+td_alloc (struct usb_device *usb_dev)
+{
+	int i;
+	struct td	*td;
+
+	td = NULL;
+	for (i = 0; i < NUM_TD; i++)
+	{
+		if (ptd[i].usb_dev == NULL)
+		{
+			td = &ptd[i];
+			td->usb_dev = usb_dev;
+			break;
+		}
+	}
+
+	return td;
+}
+
+static inline void
+ed_free (struct ed *ed)
+{
+	ed->usb_dev = NULL;
+}
diff --git a/drivers/usbdcore_ep0.c b/drivers/usbdcore_ep0.c
index 260befe9786..1e44f322a79 100644
--- a/drivers/usbdcore_ep0.c
+++ b/drivers/usbdcore_ep0.c
@@ -2,6 +2,9 @@
  * (C) Copyright 2003
  * Gerry Hamel, geh@ti.com, Texas Instruments
  *
+ * (C) Copyright 2006
+ * Bryan O'Donoghue, deckard@CodeHermit.ie
+ *
  * Based on
  * linux/drivers/usbd/ep0.c
  *
@@ -39,11 +42,17 @@
  * function driver. This may need to change.
  *
  * XXX
+ *
+ * As alluded to above, a simple callback cdc_recv_setup has been implemented
+ * in the usb_device data structure to facilicate passing
+ * Common Device Class packets to a function driver.
+ *
+ * XXX
  */
 
 #include <common.h>
 
-#if defined(CONFIG_OMAP1510) && defined(CONFIG_USB_DEVICE)
+#if defined(CONFIG_USB_DEVICE)
 #include "usbdcore.h"
 
 #if 0
@@ -69,7 +78,7 @@ static int ep0_get_status (struct usb_device_instance *device,
 	char *cp;
 
 	urb->actual_length = 2;
-	cp = urb->buffer;
+	cp = (char*)urb->buffer;
 	cp[0] = cp[1] = 0;
 
 	switch (requesttype) {
@@ -115,7 +124,7 @@ static int ep0_get_one (struct usb_device_instance *device, struct urb *urb,
  *
  * Copy configuration data to urb transfer buffer if there is room for it.
  */
-static void copy_config (struct urb *urb, void *data, int max_length,
+void copy_config (struct urb *urb, void *data, int max_length,
 			 int max_buf)
 {
 	int available;
@@ -128,10 +137,7 @@ static void copy_config (struct urb *urb, void *data, int max_length,
 		dbg_ep0 (1, "data is NULL");
 		return;
 	}
-	if (!(length = *(unsigned char *) data)) {
-		dbg_ep0 (1, "length is zero");
-		return;
-	}
+	length = max_length;
 
 	if (length > max_length) {
 		dbg_ep0 (1, "length: %d >= max_length: %d", length,
@@ -192,7 +198,7 @@ static int ep0_get_descriptor (struct usb_device_instance *device,
 
 	/* setup tx urb */
 	urb->actual_length = 0;
-	cp = urb->buffer;
+	cp = (char*)urb->buffer;
 
 	dbg_ep0 (2, "%s", USBD_DEVICE_DESCRIPTORS (descriptor_type));
 
@@ -200,7 +206,6 @@ static int ep0_get_descriptor (struct usb_device_instance *device,
 	case USB_DESCRIPTOR_TYPE_DEVICE:
 		{
 			struct usb_device_descriptor *device_descriptor;
-
 			if (!
 			    (device_descriptor =
 			     usbd_device_device_descriptor (device, port))) {
@@ -214,20 +219,16 @@ static int ep0_get_descriptor (struct usb_device_instance *device,
 			/* correct the correct control endpoint 0 max packet size into the descriptor */
 			device_descriptor =
 				(struct usb_device_descriptor *) urb->buffer;
-			device_descriptor->bMaxPacketSize0 =
-				urb->device->bus->maxpacketsize;
 
 		}
-		/*dbg_ep0(3, "copied device configuration, actual_length: %x", urb->actual_length); */
+		dbg_ep0(3, "copied device configuration, actual_length: 0x%x", urb->actual_length);
 		break;
 
 	case USB_DESCRIPTOR_TYPE_CONFIGURATION:
 		{
-			int bNumInterface;
 			struct usb_configuration_descriptor
 				*configuration_descriptor;
 			struct usb_device_descriptor *device_descriptor;
-
 			if (!
 			    (device_descriptor =
 			     usbd_device_device_descriptor (device, port))) {
@@ -251,130 +252,35 @@ static int ep0_get_descriptor (struct usb_device_instance *device,
 					 index);
 				return -1;
 			}
+			dbg_ep0(0, "attempt to copy %d bytes to urb\n",cpu_to_le16(configuration_descriptor->wTotalLength));
 			copy_config (urb, configuration_descriptor,
-				     sizeof (struct
-					     usb_configuration_descriptor),
-				     max);
-
-
-			/* iterate across interfaces for specified configuration */
-			dbg_ep0 (0, "bNumInterfaces: %d",
-				 configuration_descriptor->bNumInterfaces);
-			for (bNumInterface = 0;
-			     bNumInterface <
-			     configuration_descriptor->bNumInterfaces;
-			     bNumInterface++) {
 
-				int bAlternateSetting;
-				struct usb_interface_instance
-					*interface_instance;
-
-				dbg_ep0 (3, "[%d] bNumInterfaces: %d",
-					 bNumInterface,
-					 configuration_descriptor->bNumInterfaces);
-
-				if (! (interface_instance = usbd_device_interface_instance (device,
-								     port, index, bNumInterface)))
-				{
-					dbg_ep0 (3, "[%d] interface_instance NULL",
-						 bNumInterface);
-					return -1;
-				}
-				/* iterate across interface alternates */
-				for (bAlternateSetting = 0;
-				     bAlternateSetting < interface_instance->alternates;
-				     bAlternateSetting++) {
-					/*int class; */
-					int bNumEndpoint;
-					struct usb_interface_descriptor *interface_descriptor;
-
-					struct usb_alternate_instance *alternate_instance;
-
-					dbg_ep0 (3, "[%d:%d] alternates: %d",
-						 bNumInterface,
-						 bAlternateSetting,
-						 interface_instance->alternates);
-
-					if (! (alternate_instance = usbd_device_alternate_instance (device, port, index, bNumInterface, bAlternateSetting))) {
-						dbg_ep0 (3, "[%d] alternate_instance NULL",
-							 bNumInterface);
-						return -1;
-					}
-					/* copy descriptor for this interface */
-					copy_config (urb, alternate_instance->interface_descriptor,
-						     sizeof (struct usb_interface_descriptor),
-						     max);
-
-					/*dbg_ep0(3, "[%d:%d] classes: %d endpoints: %d", bNumInterface, bAlternateSetting, */
-					/*        alternate_instance->classes, alternate_instance->endpoints); */
-
-					/* iterate across classes for this alternate interface */
-#if 0
-					for (class = 0;
-					     class < alternate_instance->classes;
-					     class++) {
-						struct usb_class_descriptor *class_descriptor;
-						/*dbg_ep0(3, "[%d:%d:%d] classes: %d", bNumInterface, bAlternateSetting, */
-						/*        class, alternate_instance->classes); */
-						if (!(class_descriptor = usbd_device_class_descriptor_index (device, port, index, bNumInterface, bAlternateSetting, class))) {
-							dbg_ep0 (3, "[%d] class NULL",
-								 class);
-							return -1;
-						}
-						/* copy descriptor for this class */
-						copy_config (urb, class_descriptor,
-							sizeof (struct usb_class_descriptor),
-							max);
-					}
-#endif
-
-					/* iterate across endpoints for this alternate interface */
-					interface_descriptor = alternate_instance->interface_descriptor;
-					for (bNumEndpoint = 0;
-					     bNumEndpoint < alternate_instance->endpoints;
-					     bNumEndpoint++) {
-						struct usb_endpoint_descriptor *endpoint_descriptor;
-						dbg_ep0 (3, "[%d:%d:%d] endpoint: %d",
-							 bNumInterface,
-							 bAlternateSetting,
-							 bNumEndpoint,
-							 interface_descriptor->
-							 bNumEndpoints);
-						if (!(endpoint_descriptor = usbd_device_endpoint_descriptor_index (device, port, index, bNumInterface, bAlternateSetting, bNumEndpoint))) {
-							dbg_ep0 (3, "[%d] endpoint NULL",
-								 bNumEndpoint);
-							return -1;
-						}
-						/* copy descriptor for this endpoint */
-						copy_config (urb, endpoint_descriptor,
-							     sizeof (struct usb_endpoint_descriptor),
-							     max);
-					}
-				}
-			}
-			dbg_ep0 (3, "lengths: %d %d",
-				 le16_to_cpu (configuration_descriptor->wTotalLength),
-				 urb->actual_length);
+					cpu_to_le16(configuration_descriptor->wTotalLength),
+				     max);
 		}
+
 		break;
 
 	case USB_DESCRIPTOR_TYPE_STRING:
 		{
 			struct usb_string_descriptor *string_descriptor;
-
 			if (!(string_descriptor = usbd_get_string (index))) {
+				serial_printf("Invalid string index %d\n", index);
 				return -1;
 			}
-			/*dbg_ep0(3, "string_descriptor: %p", string_descriptor); */
+			dbg_ep0(3, "string_descriptor: %p length %d", string_descriptor, string_descriptor->bLength);
 			copy_config (urb, string_descriptor, string_descriptor->bLength, max);
 		}
 		break;
 	case USB_DESCRIPTOR_TYPE_INTERFACE:
+	serial_printf("USB_DESCRIPTOR_TYPE_INTERFACE - error not implemented\n");
 		return -1;
 	case USB_DESCRIPTOR_TYPE_ENDPOINT:
+		serial_printf("USB_DESCRIPTOR_TYPE_ENDPOINT - error not implemented\n");
 		return -1;
 	case USB_DESCRIPTOR_TYPE_HID:
 		{
+			serial_printf("USB_DESCRIPTOR_TYPE_HID - error not implemented\n");
 			return -1;	/* unsupported at this time */
 #if 0
 			int bNumInterface =
@@ -403,6 +309,7 @@ static int ep0_get_descriptor (struct usb_device_instance *device,
 		break;
 	case USB_DESCRIPTOR_TYPE_REPORT:
 		{
+			serial_printf("USB_DESCRIPTOR_TYPE_REPORT - error not implemented\n");
 			return -1;	/* unsupported at this time */
 #if 0
 			int bNumInterface =
@@ -434,12 +341,19 @@ static int ep0_get_descriptor (struct usb_device_instance *device,
 #endif
 		}
 		break;
+	case USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER:
+		{
+			/* If a USB device supports both a full speed and low speed operation
+			 * we must send a Device_Qualifier descriptor here
+			 */
+			return -1;
+		}
 	default:
 		return -1;
 	}
 
 
-	dbg_ep0 (1, "urb: buffer: %p buffer_length: %2d actual_length: %2d packet size: %2d",
+	dbg_ep0 (1, "urb: buffer: %p buffer_length: %2d actual_length: %2d tx_packetSize: %2d",
 		 urb->buffer, urb->buffer_length, urb->actual_length,
 		 device->bus->endpoint_array[0].tx_packetSize);
 /*
@@ -495,6 +409,12 @@ int ep0_recv_setup (struct urb *urb)
 
 	/* handle USB Standard Request (c.f. USB Spec table 9-2) */
 	if ((request->bmRequestType & USB_REQ_TYPE_MASK) != 0) {
+		if(device->device_state <= STATE_CONFIGURED){
+			/*	Attempt to handle a CDC specific request if we are
+			 *	in the configured state.
+			 */
+			return device->cdc_recv_setup(request,urb);
+		}
 		dbg_ep0 (1, "non standard request: %x",
 			 request->bmRequestType & USB_REQ_TYPE_MASK);
 		return -1;	/* Stall here */
@@ -567,6 +487,7 @@ int ep0_recv_setup (struct urb *urb)
 						   le16_to_cpu (request->wValue) & 0xff);
 
 		case USB_REQ_GET_CONFIGURATION:
+			serial_printf("get config %d\n", device->configuration);
 			return ep0_get_one (device, urb,
 					    device->configuration);
 
@@ -642,7 +563,6 @@ int ep0_recv_setup (struct urb *urb)
 			/*dbg_ep0(2, "address: %d %d %d", */
 			/*        request->wValue, le16_to_cpu(request->wValue), device->address); */
 
-			serial_printf ("DEVICE_ADDRESS_ASSIGNED.. event?\n");
 			return 0;
 
 		case USB_REQ_SET_DESCRIPTOR:	/* XXX should we support this? */
@@ -653,9 +573,10 @@ int ep0_recv_setup (struct urb *urb)
 			/* c.f. 9.4.7 - the top half of wValue is reserved */
 			/* */
 			if ((device->configuration =
-			     le16_to_cpu (request->wValue) & 0x7f) != 0) {
+				le16_to_cpu (request->wValue) & 0xFF80) != 0) {
 				/* c.f. 9.4.7 - zero is the default or addressed state, in our case this */
 				/* is the same is configuration zero */
+				serial_printf("error setting dev->config to zero!\n");
 				device->configuration = 0;	/* TBR - ?????? */
 			}
 			/* reset interface and alternate settings */
diff --git a/drivers/usbdcore_mpc8xx.c b/drivers/usbdcore_mpc8xx.c
new file mode 100644
index 00000000000..e87284b1787
--- /dev/null
+++ b/drivers/usbdcore_mpc8xx.c
@@ -0,0 +1,1400 @@
+/*
+ * Copyright (C) 2006 by Bryan O'Donoghue, CodeHermit
+ * bodonoghue@CodeHermit.ie
+ *
+ * References
+ * DasUBoot/drivers/usbdcore_omap1510.c, for design and implementation ideas.
+ *
+ * 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ */
+
+/*
+ * Notes :
+ * 1.	#define __SIMULATE_ERROR__ to inject a CRC error into every 2nd TX
+ *		packet to force the USB re-transmit protocol.
+ *
+ * 2.	#define __DEBUG_UDC__ to switch on debug tracing to serial console
+ *	be careful that tracing doesn't create Hiesen-bugs with respect to
+ *	response timeouts to control requests.
+ *
+ * 3.	This driver should be able to support any higher level driver that
+ *	that wants to do either of the two standard UDC implementations
+ *	Control-Bulk-Interrupt or  Bulk-IN/Bulk-Out standards. Hence
+ *	gserial and cdc_acm should work with this code.
+ *
+ * 4.	NAK events never actually get raised at all, the documentation
+ *	is just wrong !
+ *
+ * 5.	For some reason, cbd_datlen is *always* +2 the value it should be.
+ *	this means that having an RX cbd of 16 bytes is not possible, since
+ *	the same size is reported for 14 bytes received as 16 bytes received
+ *	until we can find out why this happens, RX cbds must be limited to 8
+ *	bytes. TODO: check errata for this behaviour.
+ *
+ * 6.	Right now this code doesn't support properly powering up with the USB
+ *	cable attached to the USB host my development board the Adder87x doesn't
+ *	have a pull-up fitted to allow this, so it is necessary to power the
+ *	board and *then* attached the USB cable to the host. However somebody
+ *	with a different design in their board may be able to keep the cable
+ *	constantly connected and simply enable/disable a pull-up  re
+ *	figure 31.1 in MPC885RM.pdf instead of having to power up the board and
+ *	then attach the cable !
+ *
+ */
+#include <common.h>
+#include <config.h>
+
+#if defined(CONFIG_MPC885_FAMILY) && defined(CONFIG_USB_DEVICE)
+#include <commproc.h>
+#include "usbdcore.h"
+#include "usbdcore_mpc8xx.h"
+#include "usbdcore_ep0.h"
+
+#define ERR(fmt, args...)\
+	serial_printf("ERROR : [%s] %s:%d: "fmt,\
+				__FILE__,__FUNCTION__,__LINE__, ##args)
+#ifdef __DEBUG_UDC__
+#define DBG(fmt,args...)\
+		serial_printf("[%s] %s:%d: "fmt,\
+				__FILE__,__FUNCTION__,__LINE__, ##args)
+#else
+#define DBG(fmt,args...)
+#endif
+
+/* Static Data */
+#ifdef __SIMULATE_ERROR__
+static char err_poison_test = 0;
+#endif
+static struct mpc8xx_ep ep_ref[MAX_ENDPOINTS];
+static u32 address_base = STATE_NOT_READY;
+static mpc8xx_udc_state_t udc_state = 0;
+static struct usb_device_instance *udc_device = 0;
+static volatile usb_epb_t *endpoints[MAX_ENDPOINTS];
+static volatile cbd_t *tx_cbd[TX_RING_SIZE];
+static volatile cbd_t *rx_cbd[RX_RING_SIZE];
+static volatile immap_t *immr = 0;
+static volatile cpm8xx_t *cp = 0;
+static volatile usb_pram_t *usb_paramp = 0;
+static volatile usb_t *usbp = 0;
+static int rx_ct = 0;
+static int tx_ct = 0;
+
+/* Static Function Declarations */
+static void mpc8xx_udc_state_transition_up (usb_device_state_t initial,
+					    usb_device_state_t final);
+static void mpc8xx_udc_state_transition_down (usb_device_state_t initial,
+					      usb_device_state_t final);
+static void mpc8xx_udc_stall (unsigned int ep);
+static void mpc8xx_udc_flush_tx_fifo (int epid);
+static void mpc8xx_udc_flush_rx_fifo (void);
+static void mpc8xx_udc_clear_rxbd (volatile cbd_t * rx_cbdp);
+static void mpc8xx_udc_init_tx (struct usb_endpoint_instance *epi,
+				struct urb *tx_urb);
+static void mpc8xx_udc_dump_request (struct usb_device_request *request);
+static void mpc8xx_udc_clock_init (volatile immap_t * immr,
+				   volatile cpm8xx_t * cp);
+static int mpc8xx_udc_ep_tx (struct usb_endpoint_instance *epi);
+static int mpc8xx_udc_epn_rx (unsigned int epid, volatile cbd_t * rx_cbdp);
+static void mpc8xx_udc_ep0_rx (volatile cbd_t * rx_cbdp);
+static void mpc8xx_udc_cbd_init (void);
+static void mpc8xx_udc_endpoint_init (void);
+static void mpc8xx_udc_cbd_attach (int ep, uchar tx_size, uchar rx_size);
+static u32 mpc8xx_udc_alloc (u32 data_size, u32 alignment);
+static int mpc8xx_udc_ep0_rx_setup (volatile cbd_t * rx_cbdp);
+static void mpc8xx_udc_set_nak (unsigned int ep);
+static short mpc8xx_udc_handle_txerr (void);
+static void mpc8xx_udc_advance_rx (volatile cbd_t ** rx_cbdp, int epid);
+
+/******************************************************************************
+			       Global Linkage
+ *****************************************************************************/
+
+/* udc_init
+ *
+ * Do initial bus gluing
+ */
+int udc_init (void)
+{
+	/* Init various pointers */
+	immr = (immap_t *) CFG_IMMR;
+	cp = (cpm8xx_t *) & (immr->im_cpm);
+	usb_paramp = (usb_pram_t *) & (cp->cp_dparam[PROFF_USB]);
+	usbp = (usb_t *) & (cp->cp_scc[0]);
+
+	memset (ep_ref, 0x00, (sizeof (struct mpc8xx_ep) * MAX_ENDPOINTS));
+
+	udc_device = 0;
+	udc_state = STATE_NOT_READY;
+
+	usbp->usmod = 0x00;
+	usbp->uscom = 0;
+
+	/* Set USB Frame #0, Respond at Address & Get a clock source  */
+	usbp->usaddr = 0x00;
+	mpc8xx_udc_clock_init (immr, cp);
+
+	/* PA15, PA14 as perhiperal USBRXD and USBOE */
+	immr->im_ioport.iop_padir &= ~0x0003;
+	immr->im_ioport.iop_papar |= 0x0003;
+
+	/* PC11/PC10 as peripheral USBRXP USBRXN */
+	immr->im_ioport.iop_pcso |= 0x0030;
+
+	/* PC7/PC6 as perhiperal USBTXP and USBTXN */
+	immr->im_ioport.iop_pcdir |= 0x0300;
+	immr->im_ioport.iop_pcpar |= 0x0300;
+
+	/* Set the base address */
+	address_base = (u32) (cp->cp_dpmem + CPM_USB_BASE);
+
+	/* Initialise endpoints and circular buffers */
+	mpc8xx_udc_endpoint_init ();
+	mpc8xx_udc_cbd_init ();
+
+	/* Assign allocated Dual Port Endpoint descriptors */
+	usb_paramp->ep0ptr = (u32) endpoints[0];
+	usb_paramp->ep1ptr = (u32) endpoints[1];
+	usb_paramp->ep2ptr = (u32) endpoints[2];
+	usb_paramp->ep3ptr = (u32) endpoints[3];
+	usb_paramp->frame_n = 0;
+
+	DBG ("ep0ptr=0x%08x ep1ptr=0x%08x ep2ptr=0x%08x ep3ptr=0x%08x\n",
+	     usb_paramp->ep0ptr, usb_paramp->ep1ptr, usb_paramp->ep2ptr,
+	     usb_paramp->ep3ptr);
+
+	return 0;
+}
+
+/* udc_irq
+ *
+ * Poll for whatever events may have occured
+ */
+void udc_irq (void)
+{
+	int epid = 0;
+	volatile cbd_t *rx_cbdp = 0;
+	volatile cbd_t *rx_cbdp_base = 0;
+
+	if (udc_state != STATE_READY) {
+		return;
+	}
+
+	if (usbp->usber & USB_E_BSY) {
+		/* This shouldn't happen. If it does then it's a bug ! */
+		usbp->usber |= USB_E_BSY;
+		mpc8xx_udc_flush_rx_fifo ();
+	}
+
+	/* Scan all RX/Bidirectional Endpoints for RX data. */
+	for (epid = 0; epid < MAX_ENDPOINTS; epid++) {
+		if (!ep_ref[epid].prx) {
+			continue;
+		}
+		rx_cbdp = rx_cbdp_base = ep_ref[epid].prx;
+
+		do {
+			if (!(rx_cbdp->cbd_sc & RX_BD_E)) {
+
+				if (rx_cbdp->cbd_sc & 0x1F) {
+					/* Corrupt data discard it.
+					 * Controller has NAK'd this packet.
+					 */
+					mpc8xx_udc_clear_rxbd (rx_cbdp);
+
+				} else {
+					if (!epid) {
+						mpc8xx_udc_ep0_rx (rx_cbdp);
+
+					} else {
+						/* Process data */
+						mpc8xx_udc_set_nak (epid);
+						mpc8xx_udc_epn_rx (epid, rx_cbdp);
+						mpc8xx_udc_clear_rxbd (rx_cbdp);
+					}
+				}
+
+				/* Advance RX CBD pointer */
+				mpc8xx_udc_advance_rx (&rx_cbdp, epid);
+				ep_ref[epid].prx = rx_cbdp;
+			} else {
+				/* Advance RX CBD pointer */
+				mpc8xx_udc_advance_rx (&rx_cbdp, epid);
+			}
+
+		} while (rx_cbdp != rx_cbdp_base);
+	}
+
+	/* Handle TX events as appropiate, the correct place to do this is
+	 * in a tx routine. Perhaps TX on epn was pre-empted by ep0
+	 */
+
+	if (usbp->usber & USB_E_TXB) {
+		usbp->usber |= USB_E_TXB;
+	}
+
+	if (usbp->usber & (USB_TX_ERRMASK)) {
+		mpc8xx_udc_handle_txerr ();
+	}
+
+	/* Switch to the default state, respond at the default address */
+	if (usbp->usber & USB_E_RESET) {
+		usbp->usber |= USB_E_RESET;
+		usbp->usaddr = 0x00;
+		udc_device->device_state = STATE_DEFAULT;
+	}
+
+	/* if(usbp->usber&USB_E_IDLE){
+	   We could suspend here !
+	   usbp->usber|=USB_E_IDLE;
+	   DBG("idle state change\n");
+	   }
+	   if(usbp->usbs){
+	   We could resume here when IDLE is deasserted !
+	   Not worth doing, so long as we are self powered though.
+	   }
+	*/
+
+	return;
+}
+
+/* udc_endpoint_write
+ *
+ * Write some data to an endpoint
+ */
+int udc_endpoint_write (struct usb_endpoint_instance *epi)
+{
+	int ep = 0;
+	short epid = 1, unnak = 0, ret = 0;
+
+	if (udc_state != STATE_READY) {
+		ERR ("invalid udc_state != STATE_READY!\n");
+		return -1;
+	}
+
+	if (!udc_device || !epi) {
+		return -1;
+	}
+
+	if (udc_device->device_state != STATE_CONFIGURED) {
+		return -1;
+	}
+
+	ep = epi->endpoint_address & 0x03;
+	if (ep >= MAX_ENDPOINTS) {
+		return -1;
+	}
+
+	/* Set NAK for all RX endpoints during TX */
+	for (epid = 1; epid < MAX_ENDPOINTS; epid++) {
+
+		/* Don't set NAK on DATA IN/CONTROL endpoints */
+		if (ep_ref[epid].sc & USB_DIR_IN) {
+			continue;
+		}
+
+		if (!(usbp->usep[epid] & (USEP_THS_NAK | USEP_RHS_NAK))) {
+			unnak |= 1 << epid;
+		}
+
+		mpc8xx_udc_set_nak (epid);
+	}
+
+	mpc8xx_udc_init_tx (&udc_device->bus->endpoint_array[ep],
+			    epi->tx_urb);
+	ret = mpc8xx_udc_ep_tx (&udc_device->bus->endpoint_array[ep]);
+
+	/* Remove temporary NAK */
+	for (epid = 1; epid < MAX_ENDPOINTS; epid++) {
+		if (unnak & (1 << epid)) {
+			udc_unset_nak (epid);
+		}
+	}
+
+	return ret;
+}
+
+/* mpc8xx_udc_assign_urb
+ *
+ * Associate a given urb to an endpoint TX or RX transmit/receive buffers
+ */
+static int mpc8xx_udc_assign_urb (int ep, char direction)
+{
+	struct usb_endpoint_instance *epi = 0;
+
+	if (ep >= MAX_ENDPOINTS) {
+		goto err;
+	}
+	epi = &udc_device->bus->endpoint_array[ep];
+	if (!epi) {
+		goto err;
+	}
+
+	if (!ep_ref[ep].urb) {
+		ep_ref[ep].urb = usbd_alloc_urb (udc_device, udc_device->bus->endpoint_array);
+		if (!ep_ref[ep].urb) {
+			goto err;
+		}
+	} else {
+		ep_ref[ep].urb->actual_length = 0;
+	}
+
+	switch (direction) {
+	case USB_DIR_IN:
+		epi->tx_urb = ep_ref[ep].urb;
+		break;
+	case USB_DIR_OUT:
+		epi->rcv_urb = ep_ref[ep].urb;
+		break;
+	default:
+		goto err;
+	}
+	return 0;
+
+      err:
+	udc_state = STATE_ERROR;
+	return -1;
+}
+
+/* udc_setup_ep
+ *
+ * Associate U-Boot software endpoints to mpc8xx endpoint parameter ram
+ * Isochronous endpoints aren't yet supported!
+ */
+void udc_setup_ep (struct usb_device_instance *device, unsigned int ep,
+		   struct usb_endpoint_instance *epi)
+{
+	uchar direction = 0;
+	int ep_attrib = 0;
+
+	if (epi && (ep < MAX_ENDPOINTS)) {
+
+		if (ep == 0) {
+			if (epi->rcv_attributes != USB_ENDPOINT_XFER_CONTROL
+			    || epi->tx_attributes !=
+			    USB_ENDPOINT_XFER_CONTROL) {
+
+				/* ep0 must be a control endpoint */
+				udc_state = STATE_ERROR;
+				return;
+
+			}
+			if (!(ep_ref[ep].sc & EP_ATTACHED)) {
+				mpc8xx_udc_cbd_attach (ep, epi->tx_packetSize,
+						       epi->rcv_packetSize);
+			}
+			usbp->usep[ep] = 0x0000;
+			return;
+		}
+
+		if ((epi->endpoint_address & USB_ENDPOINT_DIR_MASK)
+		    == USB_DIR_IN) {
+
+			direction = 1;
+			ep_attrib = epi->tx_attributes;
+			epi->rcv_packetSize = 0;
+			ep_ref[ep].sc |= USB_DIR_IN;
+		} else {
+
+			direction = 0;
+			ep_attrib = epi->rcv_attributes;
+			epi->tx_packetSize = 0;
+			ep_ref[ep].sc &= ~USB_DIR_IN;
+		}
+
+		if (mpc8xx_udc_assign_urb (ep, epi->endpoint_address
+					   & USB_ENDPOINT_DIR_MASK)) {
+			return;
+		}
+
+		switch (ep_attrib) {
+		case USB_ENDPOINT_XFER_CONTROL:
+			if (!(ep_ref[ep].sc & EP_ATTACHED)) {
+				mpc8xx_udc_cbd_attach (ep,
+						       epi->tx_packetSize,
+						       epi->rcv_packetSize);
+			}
+			usbp->usep[ep] = ep << 12;
+			epi->rcv_urb = epi->tx_urb = ep_ref[ep].urb;
+
+			break;
+		case USB_ENDPOINT_XFER_BULK:
+		case USB_ENDPOINT_XFER_INT:
+			if (!(ep_ref[ep].sc & EP_ATTACHED)) {
+				if (direction) {
+					mpc8xx_udc_cbd_attach (ep,
+							       epi->tx_packetSize,
+							       0);
+				} else {
+					mpc8xx_udc_cbd_attach (ep,
+							       0,
+							       epi->rcv_packetSize);
+				}
+			}
+			usbp->usep[ep] = (ep << 12) | ((ep_attrib) << 8);
+
+			break;
+		case USB_ENDPOINT_XFER_ISOC:
+		default:
+			serial_printf ("Error endpoint attrib %d>3\n", ep_attrib);
+			udc_state = STATE_ERROR;
+			break;
+		}
+	}
+
+}
+
+/* udc_connect
+ *
+ * Move state, switch on the USB
+ */
+void udc_connect (void)
+{
+	/* Enable pull-up resistor on D+
+	 * TODO: fit a pull-up resistor to drive SE0 for > 2.5us
+	 */
+
+	if (udc_state != STATE_ERROR) {
+		udc_state = STATE_READY;
+		usbp->usmod |= USMOD_EN;
+	}
+}
+
+/* udc_disconnect
+ *
+ * Disconnect is not used but, is included for completeness
+ */
+void udc_disconnect (void)
+{
+	/* Disable pull-up resistor on D-
+	 * TODO: fix a pullup resistor to control this
+	 */
+
+	if (udc_state != STATE_ERROR) {
+		udc_state = STATE_NOT_READY;
+	}
+	usbp->usmod &= ~USMOD_EN;
+}
+
+/* udc_enable
+ *
+ * Grab an EP0 URB, register interest in a subset of USB events
+ */
+void udc_enable (struct usb_device_instance *device)
+{
+	if (udc_state == STATE_ERROR) {
+		return;
+	}
+
+	udc_device = device;
+
+	if (!ep_ref[0].urb) {
+		ep_ref[0].urb = usbd_alloc_urb (device, device->bus->endpoint_array);
+	}
+
+	/* Register interest in all events except SOF, enable transceiver */
+	usbp->usber = 0x03FF;
+	usbp->usbmr = 0x02F7;
+
+	return;
+}
+
+/* udc_disable
+ *
+ * disable the currently hooked device
+ */
+void udc_disable (void)
+{
+	int i = 0;
+
+	if (udc_state == STATE_ERROR) {
+		DBG ("Won't disable UDC. udc_state==STATE_ERROR !\n");
+		return;
+	}
+
+	udc_device = 0;
+
+	for (; i < MAX_ENDPOINTS; i++) {
+		if (ep_ref[i].urb) {
+			usbd_dealloc_urb (ep_ref[i].urb);
+			ep_ref[i].urb = 0;
+		}
+	}
+
+	usbp->usbmr = 0x00;
+	usbp->usmod = ~USMOD_EN;
+	udc_state = STATE_NOT_READY;
+}
+
+/* udc_startup_events
+ *
+ * Enable the specified device
+ */
+void udc_startup_events (struct usb_device_instance *device)
+{
+	udc_enable (device);
+	if (udc_state == STATE_READY) {
+		usbd_device_event_irq (device, DEVICE_CREATE, 0);
+	}
+}
+
+/* udc_set_nak
+ *
+ * Allow upper layers to signal lower layers should not accept more RX data
+ *
+ */
+void udc_set_nak (int epid)
+{
+	if (epid) {
+		mpc8xx_udc_set_nak (epid);
+	}
+}
+
+/* udc_unset_nak
+ *
+ * Suspend sending of NAK tokens for DATA OUT tokens on a given endpoint.
+ * Switch off NAKing on this endpoint to accept more data output from host.
+ *
+ */
+void udc_unset_nak (int epid)
+{
+	if (epid > MAX_ENDPOINTS) {
+		return;
+	}
+
+	if (usbp->usep[epid] & (USEP_THS_NAK | USEP_RHS_NAK)) {
+		usbp->usep[epid] &= ~(USEP_THS_NAK | USEP_RHS_NAK);
+		__asm__ ("eieio");
+	}
+}
+
+/******************************************************************************
+			      Static Linkage
+******************************************************************************/
+
+/* udc_state_transition_up
+ * udc_state_transition_down
+ *
+ * Helper functions to implement device state changes.	The device states and
+ * the events that transition between them are:
+ *
+ *				STATE_ATTACHED
+ *				||	/\
+ *				\/	||
+ *	DEVICE_HUB_CONFIGURED			DEVICE_HUB_RESET
+ *				||	/\
+ *				\/	||
+ *				STATE_POWERED
+ *				||	/\
+ *				\/	||
+ *	DEVICE_RESET				DEVICE_POWER_INTERRUPTION
+ *				||	/\
+ *				\/	||
+ *				STATE_DEFAULT
+ *				||	/\
+ *				\/	||
+ *	DEVICE_ADDRESS_ASSIGNED			DEVICE_RESET
+ *				||	/\
+ *				\/	||
+ *				STATE_ADDRESSED
+ *				||	/\
+ *				\/	||
+ *	DEVICE_CONFIGURED			DEVICE_DE_CONFIGURED
+ *				||	/\
+ *				\/	||
+ *				STATE_CONFIGURED
+ *
+ * udc_state_transition_up transitions up (in the direction from STATE_ATTACHED
+ * to STATE_CONFIGURED) from the specified initial state to the specified final
+ * state, passing through each intermediate state on the way.  If the initial
+ * state is at or above (i.e. nearer to STATE_CONFIGURED) the final state, then
+ * no state transitions will take place.
+ *
+ * udc_state_transition_down transitions down (in the direction from
+ * STATE_CONFIGURED to STATE_ATTACHED) from the specified initial state to the
+ * specified final state, passing through each intermediate state on the way.
+ * If the initial state is at or below (i.e. nearer to STATE_ATTACHED) the final
+ * state, then no state transitions will take place.
+ *
+ */
+
+static void mpc8xx_udc_state_transition_up (usb_device_state_t initial,
+					    usb_device_state_t final)
+{
+	if (initial < final) {
+		switch (initial) {
+		case STATE_ATTACHED:
+			usbd_device_event_irq (udc_device,
+					       DEVICE_HUB_CONFIGURED, 0);
+			if (final == STATE_POWERED)
+				break;
+		case STATE_POWERED:
+			usbd_device_event_irq (udc_device, DEVICE_RESET, 0);
+			if (final == STATE_DEFAULT)
+				break;
+		case STATE_DEFAULT:
+			usbd_device_event_irq (udc_device,
+					       DEVICE_ADDRESS_ASSIGNED, 0);
+			if (final == STATE_ADDRESSED)
+				break;
+		case STATE_ADDRESSED:
+			usbd_device_event_irq (udc_device, DEVICE_CONFIGURED,
+					       0);
+		case STATE_CONFIGURED:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static void mpc8xx_udc_state_transition_down (usb_device_state_t initial,
+					      usb_device_state_t final)
+{
+	if (initial > final) {
+		switch (initial) {
+		case STATE_CONFIGURED:
+			usbd_device_event_irq (udc_device,
+					       DEVICE_DE_CONFIGURED, 0);
+			if (final == STATE_ADDRESSED)
+				break;
+		case STATE_ADDRESSED:
+			usbd_device_event_irq (udc_device, DEVICE_RESET, 0);
+			if (final == STATE_DEFAULT)
+				break;
+		case STATE_DEFAULT:
+			usbd_device_event_irq (udc_device,
+					       DEVICE_POWER_INTERRUPTION, 0);
+			if (final == STATE_POWERED)
+				break;
+		case STATE_POWERED:
+			usbd_device_event_irq (udc_device, DEVICE_HUB_RESET,
+					       0);
+		case STATE_ATTACHED:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+/* mpc8xx_udc_stall
+ *
+ * Force returning of STALL tokens on the given endpoint. Protocol or function
+ * STALL conditions are permissable here
+ */
+static void mpc8xx_udc_stall (unsigned int ep)
+{
+	usbp->usep[ep] |= STALL_BITMASK;
+}
+
+/* mpc8xx_udc_set_nak
+ *
+ * Force returning of NAK responses for the given endpoint as a kind of very
+ * simple flow control
+ */
+static void mpc8xx_udc_set_nak (unsigned int ep)
+{
+	usbp->usep[ep] |= NAK_BITMASK;
+	__asm__ ("eieio");
+}
+
+/* mpc8xx_udc_handle_txerr
+ *
+ * Handle errors relevant to TX. Return a status code to allow calling
+ * indicative of what if anything happened
+ */
+static short mpc8xx_udc_handle_txerr ()
+{
+	short ep = 0, ret = 0;
+
+	for (; ep < TX_RING_SIZE; ep++) {
+		if (usbp->usber & (0x10 << ep)) {
+
+			/* Timeout or underrun */
+			if (tx_cbd[ep]->cbd_sc & 0x06) {
+				ret = 1;
+				mpc8xx_udc_flush_tx_fifo (ep);
+
+			} else {
+				if (usbp->usep[ep] & STALL_BITMASK) {
+					if (!ep) {
+						usbp->usep[ep] &= ~STALL_BITMASK;
+					}
+				}	/* else NAK */
+			}
+			usbp->usber |= (0x10 << ep);
+		}
+	}
+	return ret;
+}
+
+/* mpc8xx_udc_advance_rx
+ *
+ * Advance cbd rx
+ */
+static void mpc8xx_udc_advance_rx (volatile cbd_t ** rx_cbdp, int epid)
+{
+	if ((*rx_cbdp)->cbd_sc & RX_BD_W) {
+		*rx_cbdp = (volatile cbd_t *) (endpoints[epid]->rbase + CFG_IMMR);
+
+	} else {
+		(*rx_cbdp)++;
+	}
+}
+
+
+/* mpc8xx_udc_flush_tx_fifo
+ *
+ * Flush a given TX fifo. Assumes one tx cbd per endpoint
+ */
+static void mpc8xx_udc_flush_tx_fifo (int epid)
+{
+	volatile cbd_t *tx_cbdp = 0;
+
+	if (epid > MAX_ENDPOINTS) {
+		return;
+	}
+
+	/* TX stop */
+	immr->im_cpm.cp_cpcr = ((epid << 2) | 0x1D01);
+	__asm__ ("eieio");
+	while (immr->im_cpm.cp_cpcr & 0x01);
+
+	usbp->uscom = 0x40 | 0;
+
+	/* reset ring */
+	tx_cbdp = (cbd_t *) (endpoints[epid]->tbptr + CFG_IMMR);
+	tx_cbdp->cbd_sc = (TX_BD_I | TX_BD_W);
+
+
+	endpoints[epid]->tptr = endpoints[epid]->tbase;
+	endpoints[epid]->tstate = 0x00;
+	endpoints[epid]->tbcnt = 0x00;
+
+	/* TX start */
+	immr->im_cpm.cp_cpcr = ((epid << 2) | 0x2D01);
+	__asm__ ("eieio");
+	while (immr->im_cpm.cp_cpcr & 0x01);
+
+	return;
+}
+
+/* mpc8xx_udc_flush_rx_fifo
+ *
+ * For the sake of completeness of the namespace, it seems like
+ * a good-design-decision (tm) to include mpc8xx_udc_flush_rx_fifo();
+ * If RX_BD_E is true => a driver bug either here or in an upper layer
+ * not polling frequently enough. If RX_BD_E is true we have told the host
+ * we have accepted data but, the CPM found it had no-where to put that data
+ * which needless to say would be a bad thing.
+ */
+static void mpc8xx_udc_flush_rx_fifo ()
+{
+	int i = 0;
+
+	for (i = 0; i < RX_RING_SIZE; i++) {
+		if (!(rx_cbd[i]->cbd_sc & RX_BD_E)) {
+			ERR ("buf %p used rx data len = 0x%x sc=0x%x!\n",
+			     rx_cbd[i], rx_cbd[i]->cbd_datlen,
+			     rx_cbd[i]->cbd_sc);
+
+		}
+	}
+	ERR ("BUG : Input over-run\n");
+}
+
+/* mpc8xx_udc_clear_rxbd
+ *
+ * Release control of RX CBD to CP.
+ */
+static void mpc8xx_udc_clear_rxbd (volatile cbd_t * rx_cbdp)
+{
+	rx_cbdp->cbd_datlen = 0x0000;
+	rx_cbdp->cbd_sc = ((rx_cbdp->cbd_sc & RX_BD_W) | (RX_BD_E | RX_BD_I));
+	__asm__ ("eieio");
+}
+
+/* mpc8xx_udc_tx_irq
+ *
+ * Parse for tx timeout, control RX or USB reset/busy conditions
+ * Return -1 on timeout, -2 on fatal error, else return zero
+ */
+static int mpc8xx_udc_tx_irq (int ep)
+{
+	int i = 0;
+
+	if (usbp->usber & (USB_TX_ERRMASK)) {
+		if (mpc8xx_udc_handle_txerr ()) {
+			/* Timeout, controlling function must retry send */
+			return -1;
+		}
+	}
+
+	if (usbp->usber & (USB_E_RESET | USB_E_BSY)) {
+		/* Fatal, abandon TX transaction */
+		return -2;
+	}
+
+	if (usbp->usber & USB_E_RXB) {
+		for (i = 0; i < RX_RING_SIZE; i++) {
+			if (!(rx_cbd[i]->cbd_sc & RX_BD_E)) {
+				if ((rx_cbd[i] == ep_ref[0].prx) || ep) {
+					return -2;
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+/* mpc8xx_udc_ep_tx
+ *
+ * Transmit in a re-entrant fashion outbound USB packets.
+ * Implement retry/timeout mechanism described in USB specification
+ * Toggle DATA0/DATA1 pids as necessary
+ * Introduces non-standard tx_retry. The USB standard has no scope for slave
+ * devices to give up TX, however tx_retry stops us getting stuck in an endless
+ * TX loop.
+ */
+static int mpc8xx_udc_ep_tx (struct usb_endpoint_instance *epi)
+{
+	struct urb *urb = epi->tx_urb;
+	volatile cbd_t *tx_cbdp = 0;
+	unsigned int ep = 0, pkt_len = 0, x = 0, tx_retry = 0;
+	int ret = 0;
+
+	if (!epi || (epi->endpoint_address & 0x03) >= MAX_ENDPOINTS || !urb) {
+		return -1;
+	}
+
+	ep = epi->endpoint_address & 0x03;
+	tx_cbdp = (cbd_t *) (endpoints[ep]->tbptr + CFG_IMMR);
+
+	if (tx_cbdp->cbd_sc & TX_BD_R || usbp->usber & USB_E_TXB) {
+		mpc8xx_udc_flush_tx_fifo (ep);
+		usbp->usber |= USB_E_TXB;
+	};
+
+	while (tx_retry++ < 100) {
+		ret = mpc8xx_udc_tx_irq (ep);
+		if (ret == -1) {
+			/* ignore timeout here */
+		} else if (ret == -2) {
+			/* Abandon TX */
+			mpc8xx_udc_flush_tx_fifo (ep);
+			return -1;
+		}
+
+		tx_cbdp = (cbd_t *) (endpoints[ep]->tbptr + CFG_IMMR);
+		while (tx_cbdp->cbd_sc & TX_BD_R) {
+		};
+		tx_cbdp->cbd_sc = (tx_cbdp->cbd_sc & TX_BD_W);
+
+		pkt_len = urb->actual_length - epi->sent;
+
+		if (pkt_len > epi->tx_packetSize || pkt_len > EP_MAX_PKT) {
+			pkt_len = MIN (epi->tx_packetSize, EP_MAX_PKT);
+		}
+
+		for (x = 0; x < pkt_len; x++) {
+			*((unsigned char *) (tx_cbdp->cbd_bufaddr + x)) =
+				urb->buffer[epi->sent + x];
+		}
+		tx_cbdp->cbd_datlen = pkt_len;
+		tx_cbdp->cbd_sc |= (CBD_TX_BITMASK | ep_ref[ep].pid);
+		__asm__ ("eieio");
+
+#ifdef __SIMULATE_ERROR__
+		if (++err_poison_test == 2) {
+			err_poison_test = 0;
+			tx_cbdp->cbd_sc &= ~TX_BD_TC;
+		}
+#endif
+
+		usbp->uscom = (USCOM_STR | ep);
+
+		while (!(usbp->usber & USB_E_TXB)) {
+			ret = mpc8xx_udc_tx_irq (ep);
+			if (ret == -1) {
+				/* TX timeout */
+				break;
+			} else if (ret == -2) {
+				if (usbp->usber & USB_E_TXB) {
+					usbp->usber |= USB_E_TXB;
+				}
+				mpc8xx_udc_flush_tx_fifo (ep);
+				return -1;
+			}
+		};
+
+		if (usbp->usber & USB_E_TXB) {
+			usbp->usber |= USB_E_TXB;
+		}
+
+		/* ACK must be present <= 18bit times from TX */
+		if (ret == -1) {
+			continue;
+		}
+
+		/* TX ACK : USB 2.0 8.7.2, Toggle PID, Advance TX */
+		epi->sent += pkt_len;
+		epi->last = MIN (urb->actual_length - epi->sent, epi->tx_packetSize);
+		TOGGLE_TX_PID (ep_ref[ep].pid);
+
+		if (epi->sent >= epi->tx_urb->actual_length) {
+
+			epi->tx_urb->actual_length = 0;
+			epi->sent = 0;
+
+			if (ep_ref[ep].sc & EP_SEND_ZLP) {
+				ep_ref[ep].sc &= ~EP_SEND_ZLP;
+			} else {
+				return 0;
+			}
+		}
+	}
+
+	ERR ("TX fail, endpoint 0x%x tx bytes 0x%x/0x%x\n", ep, epi->sent,
+	     epi->tx_urb->actual_length);
+
+	return -1;
+}
+
+/* mpc8xx_udc_dump_request
+ *
+ * Dump a control request to console
+ */
+static void mpc8xx_udc_dump_request (struct usb_device_request *request)
+{
+	DBG ("bmRequestType:%02x bRequest:%02x wValue:%04x "
+	     "wIndex:%04x wLength:%04x ?\n",
+	     request->bmRequestType,
+	     request->bRequest,
+	     request->wValue, request->wIndex, request->wLength);
+
+	return;
+}
+
+/* mpc8xx_udc_ep0_rx_setup
+ *
+ * Decode received ep0 SETUP packet. return non-zero on error
+ */
+static int mpc8xx_udc_ep0_rx_setup (volatile cbd_t * rx_cbdp)
+{
+	unsigned int x = 0;
+	struct urb *purb = ep_ref[0].urb;
+	struct usb_endpoint_instance *epi =
+		&udc_device->bus->endpoint_array[0];
+
+	for (; x < rx_cbdp->cbd_datlen; x++) {
+		*(((unsigned char *) &ep_ref[0].urb->device_request) + x) =
+			*((unsigned char *) (rx_cbdp->cbd_bufaddr + x));
+	}
+
+	mpc8xx_udc_clear_rxbd (rx_cbdp);
+
+	if (ep0_recv_setup (purb)) {
+		mpc8xx_udc_dump_request (&purb->device_request);
+		return -1;
+	}
+
+	if ((purb->device_request.bmRequestType & USB_REQ_DIRECTION_MASK)
+	    == USB_REQ_HOST2DEVICE) {
+
+		switch (purb->device_request.bRequest) {
+		case USB_REQ_SET_ADDRESS:
+			/* Send the Status OUT ZLP */
+			ep_ref[0].pid = TX_BD_PID_DATA1;
+			purb->actual_length = 0;
+			mpc8xx_udc_init_tx (epi, purb);
+			mpc8xx_udc_ep_tx (epi);
+
+			/* Move to the addressed state */
+			usbp->usaddr = udc_device->address;
+			mpc8xx_udc_state_transition_up (udc_device->device_state,
+							STATE_ADDRESSED);
+			return 0;
+
+		case USB_REQ_SET_CONFIGURATION:
+			if (!purb->device_request.wValue) {
+				/* Respond at default address */
+				usbp->usaddr = 0x00;
+				mpc8xx_udc_state_transition_down (udc_device->device_state,
+								  STATE_ADDRESSED);
+			} else {
+				/* TODO: Support multiple configurations */
+				mpc8xx_udc_state_transition_up (udc_device->device_state,
+								STATE_CONFIGURED);
+				for (x = 1; x < MAX_ENDPOINTS; x++) {
+					if ((udc_device->bus->endpoint_array[x].endpoint_address & USB_ENDPOINT_DIR_MASK)
+					    == USB_DIR_IN) {
+						ep_ref[x].pid = TX_BD_PID_DATA0;
+					} else {
+						ep_ref[x].pid = RX_BD_PID_DATA0;
+					}
+					/* Set configuration must unstall endpoints */
+					usbp->usep[x] &= ~STALL_BITMASK;
+				}
+			}
+			break;
+		default:
+			/* CDC/Vendor specific */
+			break;
+		}
+
+		/* Send ZLP as ACK in Status OUT phase */
+		ep_ref[0].pid = TX_BD_PID_DATA1;
+		purb->actual_length = 0;
+		mpc8xx_udc_init_tx (epi, purb);
+		mpc8xx_udc_ep_tx (epi);
+
+	} else {
+
+		if (purb->actual_length) {
+			ep_ref[0].pid = TX_BD_PID_DATA1;
+			mpc8xx_udc_init_tx (epi, purb);
+
+			if (!(purb->actual_length % EP0_MAX_PACKET_SIZE)) {
+				ep_ref[0].sc |= EP_SEND_ZLP;
+			}
+
+			if (purb->device_request.wValue ==
+			    USB_DESCRIPTOR_TYPE_DEVICE) {
+				if (le16_to_cpu (purb->device_request.wLength)
+				    > purb->actual_length) {
+					/* Send EP0_MAX_PACKET_SIZE bytes
+					 * unless correct size requested.
+					 */
+					if (purb->actual_length > epi->tx_packetSize) {
+						purb->actual_length = epi->tx_packetSize;
+					}
+				}
+			}
+			mpc8xx_udc_ep_tx (epi);
+
+		} else {
+			/* Corrupt SETUP packet? */
+			ERR ("Zero length data or SETUP with DATA-IN phase ?\n");
+			return 1;
+		}
+	}
+	return 0;
+}
+
+/* mpc8xx_udc_init_tx
+ *
+ * Setup some basic parameters for a TX transaction
+ */
+static void mpc8xx_udc_init_tx (struct usb_endpoint_instance *epi,
+				struct urb *tx_urb)
+{
+	epi->sent = 0;
+	epi->last = 0;
+	epi->tx_urb = tx_urb;
+}
+
+/* mpc8xx_udc_ep0_rx
+ *
+ * Receive ep0/control USB data. Parse and possibly send a response.
+ */
+static void mpc8xx_udc_ep0_rx (volatile cbd_t * rx_cbdp)
+{
+	if (rx_cbdp->cbd_sc & RX_BD_PID_SETUP) {
+
+		/* Unconditionally accept SETUP packets */
+		if (mpc8xx_udc_ep0_rx_setup (rx_cbdp)) {
+			mpc8xx_udc_stall (0);
+		}
+
+	} else {
+
+		mpc8xx_udc_clear_rxbd (rx_cbdp);
+
+		if ((rx_cbdp->cbd_datlen - 2)) {
+			/* SETUP with a DATA phase
+			 * outside of SETUP packet.
+			 * Reply with STALL.
+			 */
+			mpc8xx_udc_stall (0);
+		}
+	}
+}
+
+/* mpc8xx_udc_epn_rx
+ *
+ * Receive some data from cbd into USB system urb data abstraction
+ * Upper layers should NAK if there is insufficient RX data space
+ */
+static int mpc8xx_udc_epn_rx (unsigned int epid, volatile cbd_t * rx_cbdp)
+{
+	struct usb_endpoint_instance *epi = 0;
+	struct urb *urb = 0;
+	unsigned int x = 0;
+
+	if (epid >= MAX_ENDPOINTS || !rx_cbdp->cbd_datlen) {
+		return 0;
+	}
+
+	/* USB 2.0 PDF section 8.6.4
+	 * Discard data with invalid PID it is a resend.
+	 */
+	if (ep_ref[epid].pid != (rx_cbdp->cbd_sc & 0xC0)) {
+		return 1;
+	}
+	TOGGLE_RX_PID (ep_ref[epid].pid);
+
+	epi = &udc_device->bus->endpoint_array[epid];
+	urb = epi->rcv_urb;
+
+	for (; x < (rx_cbdp->cbd_datlen - 2); x++) {
+		*((unsigned char *) (urb->buffer + urb->actual_length + x)) =
+			*((unsigned char *) (rx_cbdp->cbd_bufaddr + x));
+	}
+
+	if (x) {
+		usbd_rcv_complete (epi, x, 0);
+		if (ep_ref[epid].urb->status == RECV_ERROR) {
+			DBG ("RX error unset NAK\n");
+			udc_unset_nak (epid);
+		}
+	}
+	return x;
+}
+
+/* mpc8xx_udc_clock_init
+ *
+ * Obtain a clock reference for Full Speed Signaling
+ */
+static void mpc8xx_udc_clock_init (volatile immap_t * immr,
+				   volatile cpm8xx_t * cp)
+{
+
+#if defined(CFG_USB_EXTC_CLK)
+
+	/* This has been tested with a 48MHz crystal on CLK6 */
+	switch (CFG_USB_EXTC_CLK) {
+	case 1:
+		immr->im_ioport.iop_papar |= 0x0100;
+		immr->im_ioport.iop_padir &= ~0x0100;
+		cp->cp_sicr |= 0x24;
+		break;
+	case 2:
+		immr->im_ioport.iop_papar |= 0x0200;
+		immr->im_ioport.iop_padir &= ~0x0200;
+		cp->cp_sicr |= 0x2D;
+		break;
+	case 3:
+		immr->im_ioport.iop_papar |= 0x0400;
+		immr->im_ioport.iop_padir &= ~0x0400;
+		cp->cp_sicr |= 0x36;
+		break;
+	case 4:
+		immr->im_ioport.iop_papar |= 0x0800;
+		immr->im_ioport.iop_padir &= ~0x0800;
+		cp->cp_sicr |= 0x3F;
+		break;
+	default:
+		udc_state = STATE_ERROR;
+		break;
+	}
+
+#elif defined(CFG_USB_BRGCLK)
+
+	/* This has been tested with brgclk == 50MHz */
+	DECLARE_GLOBAL_DATA_PTR;
+	int divisor = 0;
+
+	if (gd->cpu_clk < 48000000L) {
+		ERR ("brgclk is too slow for full-speed USB!\n");
+		udc_state = STATE_ERROR;
+		return;
+	}
+
+	/* Assume the brgclk is 'good enough', we want !(gd->cpu_clk%48Mhz)
+	 * but, can /probably/ live with close-ish alternative rates.
+	 */
+	divisor = (gd->cpu_clk / 48000000L) - 1;
+	cp->cp_sicr &= ~0x0000003F;
+
+	switch (CFG_USB_BRGCLK) {
+	case 1:
+		cp->cp_brgc1 |= (divisor | CPM_BRG_EN);
+		cp->cp_sicr &= ~0x2F;
+		break;
+	case 2:
+		cp->cp_brgc2 |= (divisor | CPM_BRG_EN);
+		cp->cp_sicr |= 0x00000009;
+		break;
+	case 3:
+		cp->cp_brgc3 |= (divisor | CPM_BRG_EN);
+		cp->cp_sicr |= 0x00000012;
+		break;
+	case 4:
+		cp->cp_brgc4 = (divisor | CPM_BRG_EN);
+		cp->cp_sicr |= 0x0000001B;
+		break;
+	default:
+		udc_state = STATE_ERROR;
+		break;
+	}
+
+#else
+#error "CFG_USB_EXTC_CLK or CFG_USB_BRGCLK must be defined"
+#endif
+
+}
+
+/* mpc8xx_udc_cbd_attach
+ *
+ * attach a cbd to and endpoint
+ */
+static void mpc8xx_udc_cbd_attach (int ep, uchar tx_size, uchar rx_size)
+{
+
+	if (!tx_cbd[ep] || !rx_cbd[ep] || ep >= MAX_ENDPOINTS) {
+		udc_state = STATE_ERROR;
+		return;
+	}
+
+	if (tx_size > USB_MAX_PKT || rx_size > USB_MAX_PKT ||
+	    (!tx_size && !rx_size)) {
+		udc_state = STATE_ERROR;
+		return;
+	}
+
+	/* Attach CBD to appropiate Parameter RAM Endpoint data structure */
+	if (rx_size) {
+		endpoints[ep]->rbase = (u32) rx_cbd[rx_ct];
+		endpoints[ep]->rbptr = (u32) rx_cbd[rx_ct];
+		rx_ct++;
+
+		if (!ep) {
+
+			endpoints[ep]->rbptr = (u32) rx_cbd[rx_ct];
+			rx_cbd[rx_ct]->cbd_sc |= RX_BD_W;
+			rx_ct++;
+
+		} else {
+			rx_ct += 2;
+			endpoints[ep]->rbptr = (u32) rx_cbd[rx_ct];
+			rx_cbd[rx_ct]->cbd_sc |= RX_BD_W;
+			rx_ct++;
+		}
+
+		/* Where we expect to RX data on this endpoint */
+		ep_ref[ep].prx = rx_cbd[rx_ct - 1];
+	} else {
+
+		ep_ref[ep].prx = 0;
+		endpoints[ep]->rbase = 0;
+		endpoints[ep]->rbptr = 0;
+	}
+
+	if (tx_size) {
+		endpoints[ep]->tbase = (u32) tx_cbd[tx_ct];
+		endpoints[ep]->tbptr = (u32) tx_cbd[tx_ct];
+		tx_ct++;
+	} else {
+		endpoints[ep]->tbase = 0;
+		endpoints[ep]->tbptr = 0;
+	}
+
+	endpoints[ep]->tstate = 0;
+	endpoints[ep]->tbcnt = 0;
+	endpoints[ep]->mrblr = EP_MAX_PKT;
+	endpoints[ep]->rfcr = 0x18;
+	endpoints[ep]->tfcr = 0x18;
+	ep_ref[ep].sc |= EP_ATTACHED;
+
+	DBG ("ep %d rbase 0x%08x rbptr 0x%08x tbase 0x%08x tbptr 0x%08x prx = %p\n",
+		ep, endpoints[ep]->rbase, endpoints[ep]->rbptr,
+		endpoints[ep]->tbase, endpoints[ep]->tbptr,
+		ep_ref[ep].prx);
+
+	return;
+}
+
+/* mpc8xx_udc_cbd_init
+ *
+ * Allocate space for a cbd and allocate TX/RX data space
+ */
+static void mpc8xx_udc_cbd_init (void)
+{
+	int i = 0;
+
+	for (; i < TX_RING_SIZE; i++) {
+		tx_cbd[i] = (cbd_t *)
+			mpc8xx_udc_alloc (sizeof (cbd_t), sizeof (int));
+	}
+
+	for (i = 0; i < RX_RING_SIZE; i++) {
+		rx_cbd[i] = (cbd_t *)
+			mpc8xx_udc_alloc (sizeof (cbd_t), sizeof (int));
+	}
+
+	for (i = 0; i < TX_RING_SIZE; i++) {
+		tx_cbd[i]->cbd_bufaddr =
+			mpc8xx_udc_alloc (EP_MAX_PKT, sizeof (int));
+
+		tx_cbd[i]->cbd_sc = (TX_BD_I | TX_BD_W);
+		tx_cbd[i]->cbd_datlen = 0x0000;
+	}
+
+
+	for (i = 0; i < RX_RING_SIZE; i++) {
+		rx_cbd[i]->cbd_bufaddr =
+			mpc8xx_udc_alloc (EP_MAX_PKT, sizeof (int));
+		rx_cbd[i]->cbd_sc = (RX_BD_I | RX_BD_E);
+		rx_cbd[i]->cbd_datlen = 0x0000;
+
+	}
+
+	return;
+}
+
+/* mpc8xx_udc_endpoint_init
+ *
+ * Attach an endpoint to some dpram
+ */
+static void mpc8xx_udc_endpoint_init (void)
+{
+	int i = 0;
+
+	for (; i < MAX_ENDPOINTS; i++) {
+		endpoints[i] = (usb_epb_t *)
+			mpc8xx_udc_alloc (sizeof (usb_epb_t), 32);
+	}
+}
+
+/* mpc8xx_udc_alloc
+ *
+ * Grab the address of some dpram
+ */
+static u32 mpc8xx_udc_alloc (u32 data_size, u32 alignment)
+{
+	u32 retaddr = address_base;
+
+	while (retaddr % alignment) {
+		retaddr++;
+	}
+	address_base += data_size;
+
+	return retaddr;
+}
+
+#endif /* CONFIG_MPC885_FAMILY && CONFIG_USB_DEVICE) */
diff --git a/drivers/usbdcore_omap1510.c b/drivers/usbdcore_omap1510.c
index 1d54a635755..84bb936d869 100644
--- a/drivers/usbdcore_omap1510.c
+++ b/drivers/usbdcore_omap1510.c
@@ -1517,4 +1517,31 @@ void udc_startup_events (struct usb_device_instance *device)
 	udc_enable (device);
 }
 
+/**
+ * udc_irq - do pseudo interrupts
+ */
+void udc_irq(void)
+{
+	/* Loop while we have interrupts.
+	 * If we don't do this, the input chain
+	 * polling delay is likely to miss
+	 * host requests.
+	 */
+	while (inw (UDC_IRQ_SRC) & ~UDC_SOF_Flg) {
+		/* Handle any new IRQs */
+		omap1510_udc_irq ();
+		omap1510_udc_noniso_irq ();
+	}
+}
+
+/* Flow control */
+void udc_set_nak(int epid)
+{
+	/* TODO: implement this functionality in omap1510 */
+}
+
+void udc_unset_nak (int epid)
+{
+	/* TODO: implement this functionality in omap1510 */
+}
 #endif
diff --git a/drivers/usbtty.c b/drivers/usbtty.c
index ce4a12e16e0..a3b50131dfc 100644
--- a/drivers/usbtty.c
+++ b/drivers/usbtty.c
@@ -2,6 +2,9 @@
  * (C) Copyright 2003
  * Gerry Hamel, geh@ti.com, Texas Instruments
  *
+ * (C) Copyright 2006
+ * Bryan O'Donoghue, bodonoghue@codehermit.ie
+ *
  * 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
  * the Free Software Foundation; either version 2 of the License, or
@@ -25,20 +28,42 @@
 #include <circbuf.h>
 #include <devices.h>
 #include "usbtty.h"
+#include "usb_cdc_acm.h"
+#include "usbdescriptors.h"
+#include <config.h>		/* If defined, override Linux identifiers with
+			   	 * vendor specific ones */
 
 #if 0
-#define TTYDBG(fmt,args...) serial_printf("[%s] %s %d: "fmt, __FILE__,__FUNCTION__,__LINE__,##args)
+#define TTYDBG(fmt,args...)\
+	serial_printf("[%s] %s %d: "fmt, __FILE__,__FUNCTION__,__LINE__,##args)
 #else
 #define TTYDBG(fmt,args...) do{}while(0)
 #endif
 
-#if 0
-#define TTYERR(fmt,args...) serial_printf("ERROR![%s] %s %d: "fmt, __FILE__,__FUNCTION__,__LINE__,##args)
+#if 1
+#define TTYERR(fmt,args...)\
+	serial_printf("ERROR![%s] %s %d: "fmt, __FILE__,__FUNCTION__,\
+	__LINE__,##args)
 #else
 #define TTYERR(fmt,args...) do{}while(0)
 #endif
 
 /*
+ * Defines
+ */
+#define NUM_CONFIGS    1
+#define MAX_INTERFACES 2
+#define NUM_ENDPOINTS  3
+#define ACM_TX_ENDPOINT 3
+#define ACM_RX_ENDPOINT 2
+#define GSERIAL_TX_ENDPOINT 2
+#define GSERIAL_RX_ENDPOINT 1
+#define NUM_ACM_INTERFACES 2
+#define NUM_GSERIAL_INTERFACES 1
+#define CONFIG_USBD_DATA_INTERFACE_STR "Bulk Data Interface"
+#define CONFIG_USBD_CTRL_INTERFACE_STR "Control Interface"
+
+/*
  * Buffers to hold input and output data
  */
 #define USBTTY_BUFFER_SIZE 256
@@ -50,157 +75,336 @@ static circbuf_t usbtty_output;
  * Instance variables
  */
 static device_t usbttydev;
-static struct usb_device_instance	 device_instance[1];
-static struct usb_bus_instance		 bus_instance[1];
+static struct usb_device_instance device_instance[1];
+static struct usb_bus_instance bus_instance[1];
 static struct usb_configuration_instance config_instance[NUM_CONFIGS];
-static struct usb_interface_instance	 interface_instance[NUM_INTERFACES];
-static struct usb_alternate_instance	 alternate_instance[NUM_INTERFACES];
-static struct usb_endpoint_instance	 endpoint_instance[NUM_ENDPOINTS+1]; /* one extra for control endpoint */
-
-/*
- * Static allocation of urbs
- */
-#define RECV_ENDPOINT 1
-#define TX_ENDPOINT 2
+static struct usb_interface_instance interface_instance[MAX_INTERFACES];
+static struct usb_alternate_instance alternate_instance[MAX_INTERFACES];
+/* one extra for control endpoint */
+static struct usb_endpoint_instance endpoint_instance[NUM_ENDPOINTS+1];
 
 /*
  * Global flag
  */
 int usbtty_configured_flag = 0;
 
-
 /*
  * Serial number
  */
 static char serial_number[16];
 
+
 /*
- * Descriptors
+ * Descriptors, Strings, Local variables.
  */
+
+/* defined and used by usbdcore_ep0.c */
+extern struct usb_string_descriptor **usb_strings;
+
+/* Indicies, References */
+static unsigned short rx_endpoint = 0;
+static unsigned short tx_endpoint = 0;
+static unsigned short interface_count = 0;
+static struct usb_string_descriptor *usbtty_string_table[STR_COUNT];
+
+/* USB Descriptor Strings */
 static u8 wstrLang[4] = {4,USB_DT_STRING,0x9,0x4};
 static u8 wstrManufacturer[2 + 2*(sizeof(CONFIG_USBD_MANUFACTURER)-1)];
 static u8 wstrProduct[2 + 2*(sizeof(CONFIG_USBD_PRODUCT_NAME)-1)];
 static u8 wstrSerial[2 + 2*(sizeof(serial_number) - 1)];
 static u8 wstrConfiguration[2 + 2*(sizeof(CONFIG_USBD_CONFIGURATION_STR)-1)];
-static u8 wstrInterface[2 + 2*(sizeof(CONFIG_USBD_INTERFACE_STR)-1)];
-
-static struct usb_string_descriptor *usbtty_string_table[] = {
-  (struct usb_string_descriptor*)wstrLang,
-  (struct usb_string_descriptor*)wstrManufacturer,
-  (struct usb_string_descriptor*)wstrProduct,
-  (struct usb_string_descriptor*)wstrSerial,
-  (struct usb_string_descriptor*)wstrConfiguration,
-  (struct usb_string_descriptor*)wstrInterface
-};
-extern struct usb_string_descriptor **usb_strings; /* defined and used by omap1510_ep0.c */
+static u8 wstrDataInterface[2 + 2*(sizeof(CONFIG_USBD_DATA_INTERFACE_STR)-1)];
+static u8 wstrCtrlInterface[2 + 2*(sizeof(CONFIG_USBD_DATA_INTERFACE_STR)-1)];
 
+/* Standard USB Data Structures */
+static struct usb_interface_descriptor interface_descriptors[MAX_INTERFACES];
+static struct usb_endpoint_descriptor *ep_descriptor_ptrs[NUM_ENDPOINTS];
+static struct usb_configuration_descriptor	*configuration_descriptor = 0;
 static struct usb_device_descriptor device_descriptor = {
-  bLength:	      sizeof(struct usb_device_descriptor),
-  bDescriptorType:    USB_DT_DEVICE,
-  bcdUSB:	      USB_BCD_VERSION,
-  bDeviceClass:	      USBTTY_DEVICE_CLASS,
-  bDeviceSubClass:    USBTTY_DEVICE_SUBCLASS,
-  bDeviceProtocol:    USBTTY_DEVICE_PROTOCOL,
-  bMaxPacketSize0:    EP0_MAX_PACKET_SIZE,
-  idVendor:	      CONFIG_USBD_VENDORID,
-  idProduct:	      CONFIG_USBD_PRODUCTID,
-  bcdDevice:	      USBTTY_BCD_DEVICE,
-  iManufacturer:      STR_MANUFACTURER,
-  iProduct:	      STR_PRODUCT,
-  iSerialNumber:      STR_SERIAL,
-  bNumConfigurations: NUM_CONFIGS
-  };
-static struct usb_configuration_descriptor config_descriptors[NUM_CONFIGS] = {
-  {
-    bLength:		 sizeof(struct usb_configuration_descriptor),
-    bDescriptorType:	 USB_DT_CONFIG,
-    wTotalLength:	 (sizeof(struct usb_configuration_descriptor)*NUM_CONFIGS) +
-			 (sizeof(struct usb_interface_descriptor)*NUM_INTERFACES) +
-			 (sizeof(struct usb_endpoint_descriptor)*NUM_ENDPOINTS),
-    bNumInterfaces:	 NUM_INTERFACES,
-    bConfigurationValue: 1,
-    iConfiguration:	 STR_CONFIG,
-    bmAttributes:	 BMATTRIBUTE_SELF_POWERED | BMATTRIBUTE_RESERVED,
-    bMaxPower:		 USBTTY_MAXPOWER
-  },
-};
-static struct usb_interface_descriptor interface_descriptors[NUM_INTERFACES] = {
-  {
-    bLength:		 sizeof(struct usb_interface_descriptor),
-    bDescriptorType:	 USB_DT_INTERFACE,
-    bInterfaceNumber:	 0,
-    bAlternateSetting:	 0,
-    bNumEndpoints:	 NUM_ENDPOINTS,
-    bInterfaceClass:	 USBTTY_INTERFACE_CLASS,
-    bInterfaceSubClass:	 USBTTY_INTERFACE_SUBCLASS,
-    bInterfaceProtocol:	 USBTTY_INTERFACE_PROTOCOL,
-    iInterface:		 STR_INTERFACE
-  },
+	.bLength = sizeof(struct usb_device_descriptor),
+	.bDescriptorType =	USB_DT_DEVICE,
+	.bcdUSB = 		cpu_to_le16(USB_BCD_VERSION),
+	.bDeviceSubClass =	0x00,
+	.bDeviceProtocol =	0x00,
+	.bMaxPacketSize0 =	EP0_MAX_PACKET_SIZE,
+	.idVendor =		cpu_to_le16(CONFIG_USBD_VENDORID),
+	.bcdDevice =		cpu_to_le16(USBTTY_BCD_DEVICE),
+	.iManufacturer =	STR_MANUFACTURER,
+	.iProduct =		STR_PRODUCT,
+	.iSerialNumber =	STR_SERIAL,
+	.bNumConfigurations =	NUM_CONFIGS
 };
-static struct usb_endpoint_descriptor ep_descriptors[NUM_ENDPOINTS] = {
-  {
-    bLength:		 sizeof(struct usb_endpoint_descriptor),
-    bDescriptorType:	 USB_DT_ENDPOINT,
-    bEndpointAddress:	 CONFIG_USBD_SERIAL_OUT_ENDPOINT | USB_DIR_OUT,
-    bmAttributes:	 USB_ENDPOINT_XFER_BULK,
-    wMaxPacketSize:	 CONFIG_USBD_SERIAL_OUT_PKTSIZE,
-    bInterval:		 0
-  },
-  {
-    bLength:		 sizeof(struct usb_endpoint_descriptor),
-    bDescriptorType:	 USB_DT_ENDPOINT,
-    bEndpointAddress:	 CONFIG_USBD_SERIAL_IN_ENDPOINT | USB_DIR_IN,
-    bmAttributes:	 USB_ENDPOINT_XFER_BULK,
-    wMaxPacketSize:	 CONFIG_USBD_SERIAL_IN_PKTSIZE,
-    bInterval:		 0
-  },
-  {
-    bLength:		 sizeof(struct usb_endpoint_descriptor),
-    bDescriptorType:	 USB_DT_ENDPOINT,
-    bEndpointAddress:	 CONFIG_USBD_SERIAL_INT_ENDPOINT | USB_DIR_IN,
-    bmAttributes:	 USB_ENDPOINT_XFER_INT,
-    wMaxPacketSize:	 CONFIG_USBD_SERIAL_INT_PKTSIZE,
-    bInterval:		 0
-  },
+
+
+/*
+ * Static CDC ACM specific descriptors
+ */
+
+struct acm_config_desc {
+	struct usb_configuration_descriptor configuration_desc;
+
+	/* Master Interface */
+	struct usb_interface_descriptor interface_desc;
+
+	struct usb_class_header_function_descriptor usb_class_header;
+	struct usb_class_call_management_descriptor usb_class_call_mgt;
+	struct usb_class_abstract_control_descriptor usb_class_acm;
+	struct usb_class_union_function_descriptor usb_class_union;
+	struct usb_endpoint_descriptor notification_endpoint;
+
+	/* Slave Interface */
+	struct usb_interface_descriptor data_class_interface;
+	struct usb_endpoint_descriptor
+		data_endpoints[NUM_ENDPOINTS-1] __attribute__((packed));
+} __attribute__((packed));
+
+static struct acm_config_desc acm_configuration_descriptors[NUM_CONFIGS] = {
+	{
+		.configuration_desc ={
+			.bLength =
+				sizeof(struct usb_configuration_descriptor),
+    			.bDescriptorType = USB_DT_CONFIG,
+			.wTotalLength =
+				cpu_to_le16(sizeof(struct acm_config_desc)),
+	    		.bNumInterfaces = NUM_ACM_INTERFACES,
+    			.bConfigurationValue = 1,
+			.iConfiguration = STR_CONFIG,
+			.bmAttributes =
+				BMATTRIBUTE_SELF_POWERED|BMATTRIBUTE_RESERVED,
+			.bMaxPower = USBTTY_MAXPOWER
+		},
+		/* Interface 1 */
+		.interface_desc = {
+			.bLength  = sizeof(struct usb_interface_descriptor),
+			.bDescriptorType = USB_DT_INTERFACE,
+			.bInterfaceNumber = 0,
+			.bAlternateSetting = 0,
+			.bNumEndpoints = 0x01,
+			.bInterfaceClass =
+				COMMUNICATIONS_INTERFACE_CLASS_CONTROL,
+			.bInterfaceSubClass = COMMUNICATIONS_ACM_SUBCLASS,
+			.bInterfaceProtocol = COMMUNICATIONS_V25TER_PROTOCOL,
+			.iInterface = STR_CTRL_INTERFACE,
+		},
+		.usb_class_header = {
+			.bFunctionLength	=
+				sizeof(struct usb_class_header_function_descriptor),
+			.bDescriptorType	= CS_INTERFACE,
+			.bDescriptorSubtype	= USB_ST_HEADER,
+			.bcdCDC	= cpu_to_le16(110),
+		},
+		.usb_class_call_mgt = {
+			.bFunctionLength	=
+				sizeof(struct usb_class_call_management_descriptor),
+			.bDescriptorType	= CS_INTERFACE,
+			.bDescriptorSubtype	= USB_ST_CMF,
+			.bmCapabilities		= 0x00,
+			.bDataInterface		= 0x01,
+		},
+		.usb_class_acm = {
+			.bFunctionLength	=
+				sizeof(struct usb_class_abstract_control_descriptor),
+			.bDescriptorType	= CS_INTERFACE,
+			.bDescriptorSubtype	= USB_ST_ACMF,
+			.bmCapabilities		= 0x00,
+		},
+		.usb_class_union = {
+			.bFunctionLength	=
+				sizeof(struct usb_class_union_function_descriptor),
+			.bDescriptorType	= CS_INTERFACE,
+			.bDescriptorSubtype	= USB_ST_UF,
+			.bMasterInterface	= 0x00,
+			.bSlaveInterface0	= 0x01,
+		},
+		.notification_endpoint = {
+			.bLength =
+				sizeof(struct usb_endpoint_descriptor),
+			.bDescriptorType	= USB_DT_ENDPOINT,
+			.bEndpointAddress	= 0x01 | USB_DIR_IN,
+			.bmAttributes		= USB_ENDPOINT_XFER_INT,
+			.wMaxPacketSize
+				= cpu_to_le16(CONFIG_USBD_SERIAL_INT_PKTSIZE),
+			.bInterval		= 0xFF,
+		},
+
+		/* Interface 2 */
+		.data_class_interface = {
+			.bLength		=
+				sizeof(struct usb_interface_descriptor),
+			.bDescriptorType	= USB_DT_INTERFACE,
+			.bInterfaceNumber	= 0x01,
+			.bAlternateSetting	= 0x00,
+			.bNumEndpoints		= 0x02,
+			.bInterfaceClass	=
+				COMMUNICATIONS_INTERFACE_CLASS_DATA,
+			.bInterfaceSubClass	= DATA_INTERFACE_SUBCLASS_NONE,
+			.bInterfaceProtocol	= DATA_INTERFACE_PROTOCOL_NONE,
+			.iInterface		= STR_DATA_INTERFACE,
+		},
+		.data_endpoints = {
+			{
+				.bLength		=
+					sizeof(struct usb_endpoint_descriptor),
+				.bDescriptorType	= USB_DT_ENDPOINT,
+				.bEndpointAddress	= 0x02 | USB_DIR_OUT,
+				.bmAttributes		=
+					USB_ENDPOINT_XFER_BULK,
+				.wMaxPacketSize		=
+					cpu_to_le16(CONFIG_USBD_SERIAL_BULK_PKTSIZE),
+				.bInterval		= 0xFF,
+			},
+			{
+				.bLength		=
+					sizeof(struct usb_endpoint_descriptor),
+				.bDescriptorType	= USB_DT_ENDPOINT,
+				.bEndpointAddress	= 0x03 | USB_DIR_IN,
+				.bmAttributes		=
+					USB_ENDPOINT_XFER_BULK,
+				.wMaxPacketSize		=
+					cpu_to_le16(CONFIG_USBD_SERIAL_BULK_PKTSIZE),
+				.bInterval		= 0xFF,
+			},
+		},
+	},
 };
-static struct usb_endpoint_descriptor *ep_descriptor_ptrs[NUM_ENDPOINTS] = {
-  &(ep_descriptors[0]),
-  &(ep_descriptors[1]),
-  &(ep_descriptors[2]),
+
+static struct rs232_emu rs232_desc={
+		.dter 		=  	115200,
+	   	.stop_bits	=	0x00,
+	   	.parity		=	0x00,
+		.data_bits	=	0x08
 };
 
-/* utility function for converting char* to wide string used by USB */
-static void str2wide (char *str, u16 * wide)
-{
-	int i;
 
-	for (i = 0; i < strlen (str) && str[i]; i++)
-		wide[i] = (u16) str[i];
-}
+/*
+ * Static Generic Serial specific data
+ */
+
+
+struct gserial_config_desc {
+
+	struct usb_configuration_descriptor configuration_desc;
+	struct usb_interface_descriptor
+		interface_desc[NUM_GSERIAL_INTERFACES] __attribute__((packed));
+	struct usb_endpoint_descriptor
+		data_endpoints[NUM_ENDPOINTS] __attribute__((packed));
+
+} __attribute__((packed));
+
+static struct gserial_config_desc
+gserial_configuration_descriptors[NUM_CONFIGS] ={
+	{
+		.configuration_desc ={
+			.bLength = sizeof(struct usb_configuration_descriptor),
+			.bDescriptorType = USB_DT_CONFIG,
+			.wTotalLength =
+				cpu_to_le16(sizeof(struct gserial_config_desc)),
+			.bNumInterfaces = NUM_GSERIAL_INTERFACES,
+			.bConfigurationValue = 1,
+			.iConfiguration = STR_CONFIG,
+			.bmAttributes =
+				BMATTRIBUTE_SELF_POWERED|BMATTRIBUTE_RESERVED,
+			.bMaxPower = USBTTY_MAXPOWER
+		},
+		.interface_desc = {
+			{
+				.bLength  =
+					sizeof(struct usb_interface_descriptor),
+				.bDescriptorType = USB_DT_INTERFACE,
+				.bInterfaceNumber = 0,
+				.bAlternateSetting = 0,
+				.bNumEndpoints = NUM_ENDPOINTS,
+				.bInterfaceClass =
+					COMMUNICATIONS_INTERFACE_CLASS_VENDOR,
+				.bInterfaceSubClass =
+					COMMUNICATIONS_NO_SUBCLASS,
+				.bInterfaceProtocol =
+					COMMUNICATIONS_NO_PROTOCOL,
+				.iInterface = STR_DATA_INTERFACE
+			},
+  		},
+		.data_endpoints  = {
+			{
+				.bLength =
+					sizeof(struct usb_endpoint_descriptor),
+				.bDescriptorType =	USB_DT_ENDPOINT,
+				.bEndpointAddress =	0x01 | USB_DIR_OUT,
+				.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+				.wMaxPacketSize =
+					cpu_to_le16(CONFIG_USBD_SERIAL_OUT_PKTSIZE),
+				.bInterval=		0xFF,
+			},
+			{
+				.bLength =
+					sizeof(struct usb_endpoint_descriptor),
+				.bDescriptorType =	USB_DT_ENDPOINT,
+				.bEndpointAddress =	0x02 | USB_DIR_IN,
+				.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+				.wMaxPacketSize =
+					cpu_to_le16(CONFIG_USBD_SERIAL_IN_PKTSIZE),
+				.bInterval = 		0xFF,
+			},
+			{
+				.bLength =
+					sizeof(struct usb_endpoint_descriptor),
+				.bDescriptorType =	USB_DT_ENDPOINT,
+				.bEndpointAddress =	0x03 | USB_DIR_IN,
+				.bmAttributes =		USB_ENDPOINT_XFER_INT,
+    				.wMaxPacketSize =
+					cpu_to_le16(CONFIG_USBD_SERIAL_INT_PKTSIZE),
+				.bInterval =		0xFF,
+			},
+		},
+	},
+};
 
 /*
- * Prototypes
+ * Static Function Prototypes
  */
+
 static void usbtty_init_strings (void);
 static void usbtty_init_instances (void);
 static void usbtty_init_endpoints (void);
-
+static void usbtty_init_terminal_type(short type);
 static void usbtty_event_handler (struct usb_device_instance *device,
-				  usb_device_event_t event, int data);
+				usb_device_event_t event, int data);
+static int usbtty_cdc_setup(struct usb_device_request *request,
+				struct urb *urb);
 static int usbtty_configured (void);
-
 static int write_buffer (circbuf_t * buf);
 static int fill_buffer (circbuf_t * buf);
 
 void usbtty_poll (void);
-static void pretend_interrupts (void);
 
+/* utility function for converting char* to wide string used by USB */
+static void str2wide (char *str, u16 * wide)
+{
+	int i;
+	for (i = 0; i < strlen (str) && str[i]; i++){
+		#if defined(__LITTLE_ENDIAN)
+			wide[i] = (u16) str[i];
+		#elif defined(__BIG_ENDIAN)
+			wide[i] = ((u16)(str[i])<<8);
+		#else
+			#error "__LITTLE_ENDIAN or __BIG_ENDIAN undefined"
+		#endif
+	}
+}
 
 /*
  * Test whether a character is in the RX buffer
  */
+
 int usbtty_tstc (void)
 {
+	struct usb_endpoint_instance *endpoint =
+		&endpoint_instance[rx_endpoint];
+
+	/* If no input data exists, allow more RX to be accepted */
+	if(usbtty_input.size <= 0){
+		udc_unset_nak(endpoint->endpoint_address&0x03);
+	}
+
 	usbtty_poll ();
 	return (usbtty_input.size > 0);
 }
@@ -210,15 +414,21 @@ int usbtty_tstc (void)
  * otherwise. When the function is succesfull, the character read is
  * written into its argument c.
  */
+
 int usbtty_getc (void)
 {
 	char c;
+	struct usb_endpoint_instance *endpoint =
+		&endpoint_instance[rx_endpoint];
 
 	while (usbtty_input.size <= 0) {
+		udc_unset_nak(endpoint->endpoint_address&0x03);
 		usbtty_poll ();
 	}
 
 	buf_pop (&usbtty_input, &c, 1);
+	udc_set_nak(endpoint->endpoint_address&0x03);
+
 	return c;
 }
 
@@ -238,7 +448,6 @@ void usbtty_putc (const char c)
 	}
 }
 
-
 /* usbtty_puts() helper function for finding the next '\n' in a string */
 static int next_nl_pos (const char *s)
 {
@@ -252,8 +461,9 @@ static int next_nl_pos (const char *s)
 }
 
 /*
- * Output a string to the usb client port.
+ * Output a string to the usb client port - implementing flow control
  */
+
 static void __usbtty_puts (const char *str, int len)
 {
 	int maxlen = usbtty_output.totalsize;
@@ -261,22 +471,19 @@ static void __usbtty_puts (const char *str, int len)
 
 	/* break str into chunks < buffer size, if needed */
 	while (len > 0) {
-		space = maxlen - usbtty_output.size;
+		usbtty_poll ();
 
+		space = maxlen - usbtty_output.size;
 		/* Empty buffer here, if needed, to ensure space... */
-		if (space <= 0) {
+		if (space) {
 			write_buffer (&usbtty_output);
-			space = maxlen - usbtty_output.size;
-			if (space <= 0) {
-				space = len;	/* allow old data to be overwritten. */
-			}
-		}
 
-		n = MIN (space, MIN (len, maxlen));
-		buf_push (&usbtty_output, str, n);
+			n = MIN (space, MIN (len, maxlen));
+			buf_push (&usbtty_output, str, n);
 
-		str += n;
-		len -= n;
+			str += n;
+			len -= n;
+		}
 	}
 }
 
@@ -313,8 +520,10 @@ int drv_usbtty_init (void)
 {
 	int rc;
 	char * sn;
+	char * tt;
 	int snlen;
 
+	/* Ger seiral number */
 	if (!(sn = getenv("serial#"))) {
 		sn = "000000000000";
 	}
@@ -327,6 +536,14 @@ int drv_usbtty_init (void)
 	memcpy (serial_number, sn, snlen);
 	serial_number[snlen] = '\0';
 
+	/* Decide on which type of UDC device to be.
+	 */
+
+	if(!(tt = getenv("usbtty"))) {
+		tt = "generic";
+	}
+	usbtty_init_terminal_type(strcmp(tt,"cdc_acm"));
+
 	/* prepare buffers... */
 	buf_init (&usbtty_input, USBTTY_BUFFER_SIZE);
 	buf_init (&usbtty_output, USBTTY_BUFFER_SIZE);
@@ -337,7 +554,7 @@ int drv_usbtty_init (void)
 	usbtty_init_strings ();
 	usbtty_init_instances ();
 
-	udc_startup_events (device_instance);	/* Enable our device, initialize udc pointers */
+	udc_startup_events (device_instance);/* Enable dev, init udc pointers */
 	udc_connect ();		/* Enable pullup for host detection */
 
 	usbtty_init_endpoints ();
@@ -362,30 +579,48 @@ static void usbtty_init_strings (void)
 {
 	struct usb_string_descriptor *string;
 
+	usbtty_string_table[STR_LANG] =
+		(struct usb_string_descriptor*)wstrLang;
+
 	string = (struct usb_string_descriptor *) wstrManufacturer;
-	string->bLength = sizeof (wstrManufacturer);
+	string->bLength = sizeof(wstrManufacturer);
 	string->bDescriptorType = USB_DT_STRING;
 	str2wide (CONFIG_USBD_MANUFACTURER, string->wData);
+	usbtty_string_table[STR_MANUFACTURER]=string;
+
 
 	string = (struct usb_string_descriptor *) wstrProduct;
-	string->bLength = sizeof (wstrProduct);
+	string->bLength = sizeof(wstrProduct);
 	string->bDescriptorType = USB_DT_STRING;
 	str2wide (CONFIG_USBD_PRODUCT_NAME, string->wData);
+	usbtty_string_table[STR_PRODUCT]=string;
+
 
 	string = (struct usb_string_descriptor *) wstrSerial;
-	string->bLength = 2 + 2*strlen(serial_number);
+	string->bLength = sizeof(serial_number);
 	string->bDescriptorType = USB_DT_STRING;
 	str2wide (serial_number, string->wData);
+	usbtty_string_table[STR_SERIAL]=string;
+
 
 	string = (struct usb_string_descriptor *) wstrConfiguration;
-	string->bLength = sizeof (wstrConfiguration);
+	string->bLength = sizeof(wstrConfiguration);
 	string->bDescriptorType = USB_DT_STRING;
 	str2wide (CONFIG_USBD_CONFIGURATION_STR, string->wData);
+	usbtty_string_table[STR_CONFIG]=string;
 
-	string = (struct usb_string_descriptor *) wstrInterface;
-	string->bLength = sizeof (wstrInterface);
+
+	string = (struct usb_string_descriptor *) wstrDataInterface;
+	string->bLength = sizeof(wstrDataInterface);
 	string->bDescriptorType = USB_DT_STRING;
-	str2wide (CONFIG_USBD_INTERFACE_STR, string->wData);
+	str2wide (CONFIG_USBD_DATA_INTERFACE_STR, string->wData);
+	usbtty_string_table[STR_DATA_INTERFACE]=string;
+
+	string = (struct usb_string_descriptor *) wstrCtrlInterface;
+	string->bLength = sizeof(wstrCtrlInterface);
+	string->bDescriptorType = USB_DT_STRING;
+	str2wide (CONFIG_USBD_CTRL_INTERFACE_STR, string->wData);
+	usbtty_string_table[STR_CTRL_INTERFACE]=string;
 
 	/* Now, initialize the string table for ep0 handling */
 	usb_strings = usbtty_string_table;
@@ -400,6 +635,7 @@ static void usbtty_init_instances (void)
 	device_instance->device_state = STATE_INIT;
 	device_instance->device_descriptor = &device_descriptor;
 	device_instance->event = usbtty_event_handler;
+	device_instance->cdc_recv_setup = usbtty_cdc_setup;
 	device_instance->bus = bus_instance;
 	device_instance->configurations = NUM_CONFIGS;
 	device_instance->configuration_instance_array = config_instance;
@@ -415,8 +651,8 @@ static void usbtty_init_instances (void)
 	/* configuration instance */
 	memset (config_instance, 0,
 		sizeof (struct usb_configuration_instance));
-	config_instance->interfaces = NUM_INTERFACES;
-	config_instance->configuration_descriptor = config_descriptors;
+	config_instance->interfaces = interface_count;
+	config_instance->configuration_descriptor = configuration_descriptor;
 	config_instance->interface_instance_array = interface_instance;
 
 	/* interface instance */
@@ -447,17 +683,22 @@ static void usbtty_init_instances (void)
 			sizeof (struct usb_endpoint_instance));
 
 		endpoint_instance[i].endpoint_address =
-			ep_descriptors[i - 1].bEndpointAddress;
+			ep_descriptor_ptrs[i - 1]->bEndpointAddress;
 
-		endpoint_instance[i].rcv_packetSize =
-			ep_descriptors[i - 1].wMaxPacketSize;
 		endpoint_instance[i].rcv_attributes =
-			ep_descriptors[i - 1].bmAttributes;
+			ep_descriptor_ptrs[i - 1]->bmAttributes;
+
+		endpoint_instance[i].rcv_packetSize =
+			le16_to_cpu(ep_descriptor_ptrs[i - 1]->wMaxPacketSize);
+
+		endpoint_instance[i].tx_attributes =
+			ep_descriptor_ptrs[i - 1]->bmAttributes;
 
 		endpoint_instance[i].tx_packetSize =
-			ep_descriptors[i - 1].wMaxPacketSize;
+			le16_to_cpu(ep_descriptor_ptrs[i - 1]->wMaxPacketSize);
+
 		endpoint_instance[i].tx_attributes =
-			ep_descriptors[i - 1].bmAttributes;
+			ep_descriptor_ptrs[i - 1]->bmAttributes;
 
 		urb_link_init (&endpoint_instance[i].rcv);
 		urb_link_init (&endpoint_instance[i].rdy);
@@ -480,13 +721,79 @@ static void usbtty_init_endpoints (void)
 	int i;
 
 	bus_instance->max_endpoints = NUM_ENDPOINTS + 1;
-	for (i = 0; i <= NUM_ENDPOINTS; i++) {
+	for (i = 1; i <= NUM_ENDPOINTS; i++) {
 		udc_setup_ep (device_instance, i, &endpoint_instance[i]);
 	}
 }
 
+/* usbtty_init_terminal_type
+ *
+ * Do some late binding for our device type.
+ */
+static void usbtty_init_terminal_type(short type)
+{
+	switch(type){
+		/* CDC ACM */
+		case 0:
+			/* Assign endpoint descriptors */
+			ep_descriptor_ptrs[0] =
+				&acm_configuration_descriptors[0].notification_endpoint;
+			ep_descriptor_ptrs[1] =
+				&acm_configuration_descriptors[0].data_endpoints[0];
+			ep_descriptor_ptrs[2] =
+				&acm_configuration_descriptors[0].data_endpoints[1];
+
+			/* Enumerate Device Descriptor */
+			device_descriptor.bDeviceClass =
+				COMMUNICATIONS_DEVICE_CLASS;
+			device_descriptor.idProduct =
+				cpu_to_le16(CONFIG_USBD_PRODUCTID_CDCACM);
+
+			/* Assign endpoint indices */
+			tx_endpoint = ACM_TX_ENDPOINT;
+			rx_endpoint = ACM_RX_ENDPOINT;
+
+			/* Configuration Descriptor */
+			configuration_descriptor =
+				(struct usb_configuration_descriptor*)
+				&acm_configuration_descriptors;
+
+			/* Interface count */
+			interface_count = NUM_ACM_INTERFACES;
+		break;
+
+		/* BULK IN/OUT & Default */
+		case 1:
+		default:
+			/* Assign endpoint descriptors */
+			ep_descriptor_ptrs[0] =
+				&gserial_configuration_descriptors[0].data_endpoints[0];
+			ep_descriptor_ptrs[1] =
+				&gserial_configuration_descriptors[0].data_endpoints[1];
+			ep_descriptor_ptrs[2] =
+				&gserial_configuration_descriptors[0].data_endpoints[2];
+
+			/* Enumerate Device Descriptor */
+			device_descriptor.bDeviceClass = 0xFF;
+			device_descriptor.idProduct =
+				cpu_to_le16(CONFIG_USBD_PRODUCTID_GSERIAL);
+
+			/* Assign endpoint indices */
+			tx_endpoint = GSERIAL_TX_ENDPOINT;
+			rx_endpoint = GSERIAL_RX_ENDPOINT;
+
+			/* Configuration Descriptor */
+			configuration_descriptor =
+				(struct usb_configuration_descriptor*)
+				&gserial_configuration_descriptors;
+
+			/* Interface count */
+			interface_count = NUM_GSERIAL_INTERFACES;
+		break;
+	}
+}
 
-/*********************************************************************************/
+/******************************************************************************/
 
 static struct urb *next_urb (struct usb_device_instance *device,
 			     struct usb_endpoint_instance *endpoint)
@@ -526,27 +833,39 @@ static int write_buffer (circbuf_t * buf)
 		return 0;
 	}
 
-	if (buf->size) {
+	struct usb_endpoint_instance *endpoint =
+			&endpoint_instance[tx_endpoint];
+	struct urb *current_urb = NULL;
 
-		struct usb_endpoint_instance *endpoint =
-			&endpoint_instance[TX_ENDPOINT];
-		struct urb *current_urb = NULL;
+	current_urb = next_urb (device_instance, endpoint);
+	/* TX data still exists - send it now
+	 */
+	if(endpoint->sent < current_urb->actual_length){
+		if(udc_endpoint_write (endpoint)){
+			/* Write pre-empted by RX */
+			return -1;
+		}
+	}
+
+	if (buf->size) {
 		char *dest;
 
 		int space_avail;
 		int popnum, popped;
 		int total = 0;
 
-		/* Break buffer into urb sized pieces, and link each to the endpoint */
+		/* Break buffer into urb sized pieces,
+		 * and link each to the endpoint
+		 */
 		while (buf->size > 0) {
-			current_urb = next_urb (device_instance, endpoint);
+
 			if (!current_urb) {
 				TTYERR ("current_urb is NULL, buf->size %d\n",
 					buf->size);
 				return total;
 			}
 
-			dest = current_urb->buffer +
+			dest = (char*)current_urb->buffer +
 				current_urb->actual_length;
 
 			space_avail =
@@ -562,14 +881,19 @@ static int write_buffer (circbuf_t * buf)
 			current_urb->actual_length += popped;
 			total += popped;
 
-			/* If endpoint->last == 0, then transfers have not started on this endpoint */
+			/* If endpoint->last == 0, then transfers have
+			 * not started on this endpoint
+			 */
 			if (endpoint->last == 0) {
-				udc_endpoint_write (endpoint);
+				if(udc_endpoint_write (endpoint)){
+					/* Write pre-empted by RX */
+					return -1;
+				}
 			}
 
-		}		/* end while */
+		}/* end while */
 		return total;
-	}			/* end if tx_urb */
+	}
 
 	return 0;
 }
@@ -577,18 +901,22 @@ static int write_buffer (circbuf_t * buf)
 static int fill_buffer (circbuf_t * buf)
 {
 	struct usb_endpoint_instance *endpoint =
-		&endpoint_instance[RECV_ENDPOINT];
+		&endpoint_instance[rx_endpoint];
 
 	if (endpoint->rcv_urb && endpoint->rcv_urb->actual_length) {
-		unsigned int nb = endpoint->rcv_urb->actual_length;
+		unsigned int nb = 0;
 		char *src = (char *) endpoint->rcv_urb->buffer;
+		unsigned int rx_avail = buf->totalsize - buf->size;
+
+		if(rx_avail >= endpoint->rcv_urb->actual_length){
 
-		buf_push (buf, src, nb);
-		endpoint->rcv_urb->actual_length = 0;
+			nb = endpoint->rcv_urb->actual_length;
+			buf_push (buf, src, nb);
+			endpoint->rcv_urb->actual_length = 0;
 
+		}
 		return nb;
 	}
-
 	return 0;
 }
 
@@ -597,7 +925,7 @@ static int usbtty_configured (void)
 	return usbtty_configured_flag;
 }
 
-/*********************************************************************************/
+/******************************************************************************/
 
 static void usbtty_event_handler (struct usb_device_instance *device,
 				  usb_device_event_t event, int data)
@@ -619,8 +947,34 @@ static void usbtty_event_handler (struct usb_device_instance *device,
 	}
 }
 
-/*********************************************************************************/
+/******************************************************************************/
+
+int usbtty_cdc_setup(struct usb_device_request *request, struct urb *urb)
+{
+	switch (request->bRequest){
+
+	   	case ACM_SET_CONTROL_LINE_STATE:	/* Implies DTE ready */
+			break;
+	   	case ACM_SEND_ENCAPSULATED_COMMAND :	/* Required */
+			break;
+		case ACM_SET_LINE_ENCODING :		/* DTE stop/parity bits
+							 * per character */
+			break;
+	   	case ACM_GET_ENCAPSULATED_RESPONSE :	/* request response */
+			break;
+		case ACM_GET_LINE_ENCODING :		/* request DTE rate,
+							 * stop/parity bits */
+			memcpy (urb->buffer , &rs232_desc, sizeof(rs232_desc));
+			urb->actual_length = sizeof(rs232_desc);
+
+			break;
+	 	default:
+			return 1;
+	}
+	return 0;
+}
 
+/******************************************************************************/
 
 /*
  * Since interrupt handling has not yet been implemented, we use this function
@@ -630,36 +984,29 @@ static void usbtty_event_handler (struct usb_device_instance *device,
 void usbtty_poll (void)
 {
 	/* New interrupts? */
-	pretend_interrupts ();
+	udc_irq();
 
-	/* Write any output data to host buffer (do this before checking interrupts to avoid missing one) */
+	/* Write any output data to host buffer
+	 * (do this before checking interrupts to avoid missing one)
+	 */
 	if (usbtty_configured ()) {
 		write_buffer (&usbtty_output);
 	}
 
 	/* New interrupts? */
-	pretend_interrupts ();
+	udc_irq();
 
-	/* Check for new data from host.. (do this after checking interrupts to get latest data) */
+	/* Check for new data from host..
+	 * (do this after checking interrupts to get latest data)
+	 */
 	if (usbtty_configured ()) {
 		fill_buffer (&usbtty_input);
 	}
 
 	/* New interrupts? */
-	pretend_interrupts ();
-}
+	udc_irq();
 
-static void pretend_interrupts (void)
-{
-	/* Loop while we have interrupts.
-	 * If we don't do this, the input chain
-	 * polling delay is likely to miss
-	 * host requests.
-	 */
-	while (inw (UDC_IRQ_SRC) & ~UDC_SOF_Flg) {
-		/* Handle any new IRQs */
-		omap1510_udc_irq ();
-		omap1510_udc_noniso_irq ();
-	}
 }
+
+
 #endif
diff --git a/drivers/usbtty.h b/drivers/usbtty.h
index 79c2fe57d7a..8154e3072ef 100644
--- a/drivers/usbtty.h
+++ b/drivers/usbtty.h
@@ -2,6 +2,9 @@
  * (C) Copyright 2003
  * Gerry Hamel, geh@ti.com, Texas Instruments
  *
+ * (C) Copyright 2006
+ * Bryan O'Donoghue, bodonoghue@codehermit.ie, CodeHermit
+ *
  * 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
  * the Free Software Foundation; either version 2 of the License, or
@@ -21,44 +24,47 @@
 #ifndef __USB_TTY_H__
 #define __USB_TTY_H__
 
-
 #include "usbdcore.h"
+#if defined(CONFIG_PPC)
+#include "usbdcore_mpc8xx.h"
+#elif defined(CONFIG_ARM)
 #include "usbdcore_omap1510.h"
+#endif
 
+#include <version_autogenerated.h>
 
-#define NUM_CONFIGS    1
-#define NUM_INTERFACES 1
-#define NUM_ENDPOINTS  3
+/* If no VendorID/ProductID is defined in config.h, pretend to be Linux
+ * DO NOT Reuse this Vendor/Product setup with protocol incompatible devices */
 
-#define EP0_MAX_PACKET_SIZE 64
+#define CONFIG_USBD_VENDORID 0x0525 	/* Linux/NetChip */
+#define CONFIG_USBD_PRODUCTID_GSERIAL 0xa4a6	/* gserial */
+#define CONFIG_USBD_PRODUCTID_CDCACM  0xa4a7	/* CDC ACM */
+#define CONFIG_USBD_MANUFACTURER "Das U-Boot"
+#define CONFIG_USBD_PRODUCT_NAME U_BOOT_VERSION
 
-#define CONFIG_USBD_CONFIGURATION_STR "TTY via USB"
-#define CONFIG_USBD_INTERFACE_STR     "Simple Serial Data Interface - Bulk Mode"
 
+#define CONFIG_USBD_CONFIGURATION_STR "TTY via USB"
 
-#define CONFIG_USBD_SERIAL_OUT_ENDPOINT 2
-#define CONFIG_USBD_SERIAL_OUT_PKTSIZE	64
-#define CONFIG_USBD_SERIAL_IN_ENDPOINT	1
-#define CONFIG_USBD_SERIAL_IN_PKTSIZE	64
-#define CONFIG_USBD_SERIAL_INT_ENDPOINT 5
-#define CONFIG_USBD_SERIAL_INT_PKTSIZE	16
-
+#define CONFIG_USBD_SERIAL_OUT_ENDPOINT UDC_OUT_ENDPOINT
+#define CONFIG_USBD_SERIAL_OUT_PKTSIZE	UDC_OUT_PACKET_SIZE
+#define CONFIG_USBD_SERIAL_IN_ENDPOINT	UDC_IN_ENDPOINT
+#define CONFIG_USBD_SERIAL_IN_PKTSIZE	UDC_IN_PACKET_SIZE
+#define CONFIG_USBD_SERIAL_INT_ENDPOINT UDC_INT_ENDPOINT
+#define CONFIG_USBD_SERIAL_INT_PKTSIZE	UDC_INT_PACKET_SIZE
+#define CONFIG_USBD_SERIAL_BULK_PKTSIZE	UDC_BULK_PACKET_SIZE
 
 #define USBTTY_DEVICE_CLASS	COMMUNICATIONS_DEVICE_CLASS
-#define USBTTY_DEVICE_SUBCLASS	COMMUNICATIONS_NO_SUBCLASS
-#define USBTTY_DEVICE_PROTOCOL	COMMUNICATIONS_NO_PROTOCOL
-
-#define USBTTY_INTERFACE_CLASS	   0xFF /* Vendor Specific */
-#define USBTTY_INTERFACE_SUBCLASS  0x02
-#define USBTTY_INTERFACE_PROTOCOL  0x01
 
-#define USBTTY_BCD_DEVICE 0x0
-#define USBTTY_MAXPOWER	  0x0
+#define USBTTY_BCD_DEVICE 	0x00
+#define USBTTY_MAXPOWER	  	0x00
 
-#define STR_MANUFACTURER 1
-#define STR_PRODUCT	 2
-#define STR_SERIAL	 3
-#define STR_CONFIG	 4
-#define STR_INTERFACE	 5
+#define STR_LANG		0x00
+#define STR_MANUFACTURER	0x01
+#define STR_PRODUCT		0x02
+#define STR_SERIAL		0x03
+#define STR_CONFIG		0x04
+#define STR_DATA_INTERFACE	0x05
+#define STR_CTRL_INTERFACE	0x06
+#define STR_COUNT		0x07
 
 #endif