Merge git://git.denx.de/u-boot

85 files changed, 4718 insertions, 2727 deletions

diff --git a/drivers/block/ahci.c b/drivers/block/ahci.c
index 7b2ec505e1..8c785ae923 100644
--- a/drivers/block/ahci.c
+++ b/drivers/block/ahci.c
@@ -37,11 +37,28 @@
 #include <linux/ctype.h>
 #include <ahci.h>
 
+static int ata_io_flush(u8 port);
+
 struct ahci_probe_ent *probe_ent = NULL;
 hd_driveid_t *ataid[AHCI_MAX_PORTS];
 
 #define writel_with_flush(a,b)	do { writel(a,b); readl(b); } while (0)
 
+/*
+ * Some controllers limit number of blocks they can read/write at once.
+ * Contemporary SSD devices work much faster if the read/write size is aligned
+ * to a power of 2.  Let's set default to 128 and allowing to be overwritten if
+ * needed.
+ */
+#ifndef MAX_SATA_BLOCKS_READ_WRITE
+#define MAX_SATA_BLOCKS_READ_WRITE	0x80
+#endif
+
+/* Maximum timeouts for each event */
+#define WAIT_MS_SPINUP	10000
+#define WAIT_MS_DATAIO	5000
+#define WAIT_MS_FLUSH	5000
+#define WAIT_MS_LINKUP	4
 
 static inline u32 ahci_port_base(u32 base, u32 port)
 {
@@ -60,7 +77,39 @@ static void ahci_setup_port(struct ahci_ioports *port, unsigned long base,
 
 
 #define msleep(a) udelay(a * 1000)
-#define ssleep(a) msleep(a * 1000)
+
+static void ahci_dcache_flush_range(unsigned begin, unsigned len)
+{
+	const unsigned long start = begin;
+	const unsigned long end = start + len;
+
+	debug("%s: flush dcache: [%#lx, %#lx)\n", __func__, start, end);
+	flush_dcache_range(start, end);
+}
+
+/*
+ * SATA controller DMAs to physical RAM.  Ensure data from the
+ * controller is invalidated from dcache; next access comes from
+ * physical RAM.
+ */
+static void ahci_dcache_invalidate_range(unsigned begin, unsigned len)
+{
+	const unsigned long start = begin;
+	const unsigned long end = start + len;
+
+	debug("%s: invalidate dcache: [%#lx, %#lx)\n", __func__, start, end);
+	invalidate_dcache_range(start, end);
+}
+
+/*
+ * Ensure data for SATA controller is flushed out of dcache and
+ * written to physical memory.
+ */
+static void ahci_dcache_flush_sata_cmd(struct ahci_ioports *pp)
+{
+	ahci_dcache_flush_range((unsigned long)pp->cmd_slot,
+				AHCI_PORT_PRIV_DMA_SZ);
+}
 
 static int waiting_for_cmd_completed(volatile u8 *offset,
 				     int timeout_msec,
@@ -84,13 +133,15 @@ static int ahci_host_init(struct ahci_probe_ent *probe_ent)
 	unsigned short vendor;
 #endif
 	volatile u8 *mmio = (volatile u8 *)probe_ent->mmio_base;
-	u32 tmp, cap_save;
+	u32 tmp, cap_save, cmd;
 	int i, j;
 	volatile u8 *port_mmio;
 
+	debug("ahci_host_init: start\n");
+
 	cap_save = readl(mmio + HOST_CAP);
 	cap_save &= ((1 << 28) | (1 << 17));
-	cap_save |= (1 << 27);
+	cap_save |= (1 << 27);  /* Staggered Spin-up. Not needed. */
 
 	/* global controller reset */
 	tmp = readl(mmio + HOST_CTL);
@@ -100,13 +151,15 @@ static int ahci_host_init(struct ahci_probe_ent *probe_ent)
 	/* reset must complete within 1 second, or
 	 * the hardware should be considered fried.
 	 */
-	ssleep(1);
-
-	tmp = readl(mmio + HOST_CTL);
-	if (tmp & HOST_RESET) {
-		debug("controller reset failed (0x%x)\n", tmp);
-		return -1;
-	}
+	i = 1000;
+	do {
+		udelay(1000);
+		tmp = readl(mmio + HOST_CTL);
+		if (!i--) {
+			debug("controller reset failed (0x%x)\n", tmp);
+			return -1;
+		}
+	} while (tmp & HOST_RESET);
 
 	writel_with_flush(HOST_AHCI_EN, mmio + HOST_CTL);
 	writel(cap_save, mmio + HOST_CAP);
@@ -129,6 +182,9 @@ static int ahci_host_init(struct ahci_probe_ent *probe_ent)
 	debug("cap 0x%x  port_map 0x%x  n_ports %d\n",
 	      probe_ent->cap, probe_ent->port_map, probe_ent->n_ports);
 
+	if (probe_ent->n_ports > CONFIG_SYS_SCSI_MAX_SCSI_ID)
+		probe_ent->n_ports = CONFIG_SYS_SCSI_MAX_SCSI_ID;
+
 	for (i = 0; i < probe_ent->n_ports; i++) {
 		probe_ent->port[i].port_mmio = ahci_port_base((u32) mmio, i);
 		port_mmio = (u8 *) probe_ent->port[i].port_mmio;
@@ -138,6 +194,7 @@ static int ahci_host_init(struct ahci_probe_ent *probe_ent)
 		tmp = readl(port_mmio + PORT_CMD);
 		if (tmp & (PORT_CMD_LIST_ON | PORT_CMD_FIS_ON |
 			   PORT_CMD_FIS_RX | PORT_CMD_START)) {
+			debug("Port %d is active. Deactivating.\n", i);
 			tmp &= ~(PORT_CMD_LIST_ON | PORT_CMD_FIS_ON |
 				 PORT_CMD_FIS_RX | PORT_CMD_START);
 			writel_with_flush(tmp, port_mmio + PORT_CMD);
@@ -148,16 +205,53 @@ static int ahci_host_init(struct ahci_probe_ent *probe_ent)
 			msleep(500);
 		}
 
-		writel(PORT_CMD_SPIN_UP, port_mmio + PORT_CMD);
-
+		/* Add the spinup command to whatever mode bits may
+		 * already be on in the command register.
+		 */
+		cmd = readl(port_mmio + PORT_CMD);
+		cmd |= PORT_CMD_FIS_RX;
+		cmd |= PORT_CMD_SPIN_UP;
+		writel_with_flush(cmd, port_mmio + PORT_CMD);
+
+		/* Bring up SATA link.
+		 * SATA link bringup time is usually less than 1 ms; only very
+		 * rarely has it taken between 1-2 ms. Never seen it above 2 ms.
+		 */
 		j = 0;
-		while (j < 100) {
-			msleep(10);
+		while (j < WAIT_MS_LINKUP) {
 			tmp = readl(port_mmio + PORT_SCR_STAT);
 			if ((tmp & 0xf) == 0x3)
 				break;
+			udelay(1000);
 			j++;
 		}
+		if (j == WAIT_MS_LINKUP) {
+			printf("SATA link %d timeout.\n", i);
+			continue;
+		} else {
+			debug("SATA link ok.\n");
+		}
+
+		/* Clear error status */
+		tmp = readl(port_mmio + PORT_SCR_ERR);
+		if (tmp)
+			writel(tmp, port_mmio + PORT_SCR_ERR);
+
+		debug("Spinning up device on SATA port %d... ", i);
+
+		j = 0;
+		while (j < WAIT_MS_SPINUP) {
+			tmp = readl(port_mmio + PORT_TFDATA);
+			if (!(tmp & (ATA_STAT_BUSY | ATA_STAT_DRQ)))
+				break;
+			udelay(1000);
+			j++;
+		}
+		printf("Target spinup took %d ms.\n", j);
+		if (j == WAIT_MS_SPINUP)
+			debug("timeout.\n");
+		else
+			debug("ok.\n");
 
 		tmp = readl(port_mmio + PORT_SCR_ERR);
 		debug("PORT_SCR_ERR 0x%x\n", tmp);
@@ -174,9 +268,9 @@ static int ahci_host_init(struct ahci_probe_ent *probe_ent)
 		/* set irq mask (enables interrupts) */
 		writel(DEF_PORT_IRQ, port_mmio + PORT_IRQ_MASK);
 
-		/*register linkup ports */
+		/* register linkup ports */
 		tmp = readl(port_mmio + PORT_SCR_STAT);
-		debug("Port %d status: 0x%x\n", i, tmp);
+		debug("SATA port %d status: 0x%x\n", i, tmp);
 		if ((tmp & 0xf) == 0x03)
 			probe_ent->link_port_map |= (0x01 << i);
 	}
@@ -202,12 +296,13 @@ static void ahci_print_info(struct ahci_probe_ent *probe_ent)
 	u16 cc;
 #endif
 	volatile u8 *mmio = (volatile u8 *)probe_ent->mmio_base;
-	u32 vers, cap, impl, speed;
+	u32 vers, cap, cap2, impl, speed;
 	const char *speed_s;
 	const char *scc_s;
 
 	vers = readl(mmio + HOST_VERSION);
 	cap = probe_ent->cap;
+	cap2 = readl(mmio + HOST_CAP2);
 	impl = probe_ent->port_map;
 
 	speed = (cap >> 20) & 0xf;
@@ -215,6 +310,8 @@ static void ahci_print_info(struct ahci_probe_ent *probe_ent)
 		speed_s = "1.5";
 	else if (speed == 2)
 		speed_s = "3";
+	else if (speed == 3)
+		speed_s = "6";
 	else
 		speed_s = "?";
 
@@ -240,8 +337,9 @@ static void ahci_print_info(struct ahci_probe_ent *probe_ent)
 	       ((cap >> 8) & 0x1f) + 1, (cap & 0x1f) + 1, speed_s, impl, scc_s);
 
 	printf("flags: "
-	       "%s%s%s%s%s%s"
-	       "%s%s%s%s%s%s%s\n",
+	       "%s%s%s%s%s%s%s"
+	       "%s%s%s%s%s%s%s"
+	       "%s%s%s%s%s%s\n",
 	       cap & (1 << 31) ? "64bit " : "",
 	       cap & (1 << 30) ? "ncq " : "",
 	       cap & (1 << 28) ? "ilck " : "",
@@ -252,9 +350,16 @@ static void ahci_print_info(struct ahci_probe_ent *probe_ent)
 	       cap & (1 << 19) ? "nz " : "",
 	       cap & (1 << 18) ? "only " : "",
 	       cap & (1 << 17) ? "pmp " : "",
+	       cap & (1 << 16) ? "fbss " : "",
 	       cap & (1 << 15) ? "pio " : "",
 	       cap & (1 << 14) ? "slum " : "",
-	       cap & (1 << 13) ? "part " : "");
+	       cap & (1 << 13) ? "part " : "",
+	       cap & (1 << 7) ? "ccc " : "",
+	       cap & (1 << 6) ? "ems " : "",
+	       cap & (1 << 5) ? "sxs " : "",
+	       cap2 & (1 << 2) ? "apst " : "",
+	       cap2 & (1 << 1) ? "nvmp " : "",
+	       cap2 & (1 << 0) ? "boh " : "");
 }
 
 #ifndef CONFIG_SCSI_AHCI_PLAT
@@ -277,8 +382,8 @@ static int ahci_init_one(pci_dev_t pdev)
 	probe_ent->pio_mask = 0x1f;
 	probe_ent->udma_mask = 0x7f;	/*Fixme,assume to support UDMA6 */
 
-	probe_ent->mmio_base = (u32)pci_map_bar(pdev, AHCI_PCI_BAR,
-						PCI_REGION_MEM);
+	pci_read_config_dword(pdev, PCI_BASE_ADDRESS_5, &probe_ent->mmio_base);
+	debug("ahci mmio_base=0x%08x\n", probe_ent->mmio_base);
 
 	/* Take from kernel:
 	 * JMicron-specific fixup:
@@ -342,6 +447,7 @@ static void ahci_fill_cmd_slot(struct ahci_ioports *pp, u32 opts)
 }
 
 
+#ifdef CONFIG_AHCI_SETFEATURES_XFER
 static void ahci_set_feature(u8 port)
 {
 	struct ahci_ioports *pp = &(probe_ent->port[port]);
@@ -349,23 +455,26 @@ static void ahci_set_feature(u8 port)
 	u32 cmd_fis_len = 5;	/* five dwords */
 	u8 fis[20];
 
-	/*set feature */
-	memset(fis, 0, 20);
+	/* set feature */
+	memset(fis, 0, sizeof(fis));
 	fis[0] = 0x27;
 	fis[1] = 1 << 7;
 	fis[2] = ATA_CMD_SETF;
 	fis[3] = SETFEATURES_XFER;
 	fis[12] = __ilog2(probe_ent->udma_mask + 1) + 0x40 - 0x01;
 
-	memcpy((unsigned char *)pp->cmd_tbl, fis, 20);
+	memcpy((unsigned char *)pp->cmd_tbl, fis, sizeof(fis));
 	ahci_fill_cmd_slot(pp, cmd_fis_len);
+	ahci_dcache_flush_sata_cmd(pp);
 	writel(1, port_mmio + PORT_CMD_ISSUE);
 	readl(port_mmio + PORT_CMD_ISSUE);
 
-	if (waiting_for_cmd_completed(port_mmio + PORT_CMD_ISSUE, 150, 0x1)) {
-		printf("set feature error!\n");
+	if (waiting_for_cmd_completed(port_mmio + PORT_CMD_ISSUE,
+				WAIT_MS_DATAIO, 0x1)) {
+		printf("set feature error on port %d!\n", port);
 	}
 }
+#endif
 
 
 static int ahci_port_start(u8 port)
@@ -397,25 +506,27 @@ static int ahci_port_start(u8 port)
 	 * First item in chunk of DMA memory: 32-slot command table,
 	 * 32 bytes each in size
 	 */
-	pp->cmd_slot = (struct ahci_cmd_hdr *)mem;
-	debug("cmd_slot = %p\n", pp->cmd_slot);
+	pp->cmd_slot =
+		(struct ahci_cmd_hdr *)(uintptr_t)virt_to_phys((void *)mem);
+	debug("cmd_slot = 0x%x\n", (unsigned)pp->cmd_slot);
 	mem += (AHCI_CMD_SLOT_SZ + 224);
 
 	/*
 	 * Second item: Received-FIS area
 	 */
-	pp->rx_fis = mem;
+	pp->rx_fis = virt_to_phys((void *)mem);
 	mem += AHCI_RX_FIS_SZ;
 
 	/*
 	 * Third item: data area for storing a single command
 	 * and its scatter-gather table
 	 */
-	pp->cmd_tbl = mem;
+	pp->cmd_tbl = virt_to_phys((void *)mem);
 	debug("cmd_tbl_dma = 0x%x\n", pp->cmd_tbl);
 
 	mem += AHCI_CMD_TBL_HDR;
-	pp->cmd_tbl_sg = (struct ahci_sg *)mem;
+	pp->cmd_tbl_sg =
+			(struct ahci_sg *)(uintptr_t)virt_to_phys((void *)mem);
 
 	writel_with_flush((u32) pp->cmd_slot, port_mmio + PORT_LST_ADDR);
 
@@ -431,8 +542,8 @@ static int ahci_port_start(u8 port)
 }
 
 
-static int get_ahci_device_data(u8 port, u8 *fis, int fis_len, u8 *buf,
-				int buf_len)
+static int ahci_device_data_io(u8 port, u8 *fis, int fis_len, u8 *buf,
+				int buf_len, u8 is_write)
 {
 
 	struct ahci_ioports *pp = &(probe_ent->port[port]);
@@ -441,10 +552,10 @@ static int get_ahci_device_data(u8 port, u8 *fis, int fis_len, u8 *buf,
 	u32 port_status;
 	int sg_count;
 
-	debug("Enter get_ahci_device_data: for port %d\n", port);
+	debug("Enter %s: for port %d\n", __func__, port);
 
 	if (port > probe_ent->n_ports) {
-		printf("Invaild port number %d\n", port);
+		printf("Invalid port number %d\n", port);
 		return -1;
 	}
 
@@ -457,17 +568,22 @@ static int get_ahci_device_data(u8 port, u8 *fis, int fis_len, u8 *buf,
 	memcpy((unsigned char *)pp->cmd_tbl, fis, fis_len);
 
 	sg_count = ahci_fill_sg(port, buf, buf_len);
-	opts = (fis_len >> 2) | (sg_count << 16);
+	opts = (fis_len >> 2) | (sg_count << 16) | (is_write << 6);
 	ahci_fill_cmd_slot(pp, opts);
 
+	ahci_dcache_flush_sata_cmd(pp);
+	ahci_dcache_flush_range((unsigned)buf, (unsigned)buf_len);
+
 	writel_with_flush(1, port_mmio + PORT_CMD_ISSUE);
 
-	if (waiting_for_cmd_completed(port_mmio + PORT_CMD_ISSUE, 150, 0x1)) {
+	if (waiting_for_cmd_completed(port_mmio + PORT_CMD_ISSUE,
+				WAIT_MS_DATAIO, 0x1)) {
 		printf("timeout exit!\n");
 		return -1;
 	}
-	debug("get_ahci_device_data: %d byte transferred.\n",
-	      pp->cmd_slot->status);
+
+	ahci_dcache_invalidate_range((unsigned)buf, (unsigned)buf_len);
+	debug("%s: %d byte transferred.\n", __func__, pp->cmd_slot->status);
 
 	return 0;
 }
@@ -526,7 +642,7 @@ static int ata_scsiop_inquiry(ccb *pccb)
 	if (pccb->datalen <= 35)
 		return 0;
 
-	memset(fis, 0, 20);
+	memset(fis, 0, sizeof(fis));
 	/* Construct the FIS */
 	fis[0] = 0x27;		/* Host to device FIS. */
 	fis[1] = 1 << 7;	/* Command FIS. */
@@ -537,8 +653,8 @@ static int ata_scsiop_inquiry(ccb *pccb)
 	if (!(tmpid = malloc(sizeof(hd_driveid_t))))
 		return -ENOMEM;
 
-	if (get_ahci_device_data(port, (u8 *) & fis, 20,
-				 tmpid, sizeof(hd_driveid_t))) {
+	if (ahci_device_data_io(port, (u8 *) &fis, sizeof(fis), tmpid,
+				sizeof(hd_driveid_t), 0)) {
 		debug("scsi_ahci: SCSI inquiry command failure.\n");
 		return -EIO;
 	}
@@ -557,46 +673,91 @@ static int ata_scsiop_inquiry(ccb *pccb)
 
 
 /*
- * SCSI READ10 command operation.
+ * SCSI READ10/WRITE10 command operation.
  */
-static int ata_scsiop_read10(ccb * pccb)
+static int ata_scsiop_read_write(ccb *pccb, u8 is_write)
 {
-	u32 len = 0;
+	u32 lba = 0;
+	u16 blocks = 0;
 	u8 fis[20];
+	u8 *user_buffer = pccb->pdata;
+	u32 user_buffer_size = pccb->datalen;
 
-	len = (((u32) pccb->cmd[7]) << 8) | ((u32) pccb->cmd[8]);
+	/* Retrieve the base LBA number from the ccb structure. */
+	memcpy(&lba, pccb->cmd + 2, sizeof(lba));
+	lba = be32_to_cpu(lba);
 
-	/* For 10-byte and 16-byte SCSI R/W commands, transfer
+	/*
+	 * And the number of blocks.
+	 *
+	 * For 10-byte and 16-byte SCSI R/W commands, transfer
 	 * length 0 means transfer 0 block of data.
 	 * However, for ATA R/W commands, sector count 0 means
 	 * 256 or 65536 sectors, not 0 sectors as in SCSI.
 	 *
 	 * WARNING: one or two older ATA drives treat 0 as 0...
 	 */
-	if (!len)
-		return 0;
-	memset(fis, 0, 20);
+	blocks = (((u16)pccb->cmd[7]) << 8) | ((u16) pccb->cmd[8]);
 
-	/* Construct the FIS */
-	fis[0] = 0x27;		/* Host to device FIS. */
-	fis[1] = 1 << 7;	/* Command FIS. */
-	fis[2] = ATA_CMD_RD_DMA;	/* Command byte. */
-
-	/* LBA address, only support LBA28 in this driver */
-	fis[4] = pccb->cmd[5];
-	fis[5] = pccb->cmd[4];
-	fis[6] = pccb->cmd[3];
-	fis[7] = (pccb->cmd[2] & 0x0f) | 0xe0;
-
-	/* Sector Count */
-	fis[12] = pccb->cmd[8];
-	fis[13] = pccb->cmd[7];
-
-	/* Read from ahci */
-	if (get_ahci_device_data(pccb->target, (u8 *) & fis, 20,
-				 pccb->pdata, pccb->datalen)) {
-		debug("scsi_ahci: SCSI READ10 command failure.\n");
-		return -EIO;
+	debug("scsi_ahci: %s %d blocks starting from lba 0x%x\n",
+	      is_write ?  "write" : "read", (unsigned)lba, blocks);
+
+	/* Preset the FIS */
+	memset(fis, 0, sizeof(fis));
+	fis[0] = 0x27;		 /* Host to device FIS. */
+	fis[1] = 1 << 7;	 /* Command FIS. */
+	/* Command byte (read/write). */
+	fis[2] = is_write ? ATA_CMD_WRITE_EXT : ATA_CMD_READ_EXT;
+
+	while (blocks) {
+		u16 now_blocks; /* number of blocks per iteration */
+		u32 transfer_size; /* number of bytes per iteration */
+
+		now_blocks = min(MAX_SATA_BLOCKS_READ_WRITE, blocks);
+
+		transfer_size = ATA_BLOCKSIZE * now_blocks;
+		if (transfer_size > user_buffer_size) {
+			printf("scsi_ahci: Error: buffer too small.\n");
+			return -EIO;
+		}
+
+		/* LBA48 SATA command but only use 32bit address range within
+		 * that. The next smaller command range (28bit) is too small.
+		 */
+		fis[4] = (lba >> 0) & 0xff;
+		fis[5] = (lba >> 8) & 0xff;
+		fis[6] = (lba >> 16) & 0xff;
+		fis[7] = 1 << 6; /* device reg: set LBA mode */
+		fis[8] = ((lba >> 24) & 0xff);
+		fis[3] = 0xe0; /* features */
+
+		/* Block (sector) count */
+		fis[12] = (now_blocks >> 0) & 0xff;
+		fis[13] = (now_blocks >> 8) & 0xff;
+
+		/* Read/Write from ahci */
+		if (ahci_device_data_io(pccb->target, (u8 *) &fis, sizeof(fis),
+					user_buffer, user_buffer_size,
+					is_write)) {
+			debug("scsi_ahci: SCSI %s10 command failure.\n",
+			      is_write ? "WRITE" : "READ");
+			return -EIO;
+		}
+
+		/* If this transaction is a write, do a following flush.
+		 * Writes in u-boot are so rare, and the logic to know when is
+		 * the last write and do a flush only there is sufficiently
+		 * difficult. Just do a flush after every write. This incurs,
+		 * usually, one extra flush when the rare writes do happen.
+		 */
+		if (is_write) {
+			if (-EIO == ata_io_flush(pccb->target))
+				return -EIO;
+		}
+		user_buffer += transfer_size;
+		user_buffer_size -= transfer_size;
+		blocks -= now_blocks;
+		lba += now_blocks;
 	}
 
 	return 0;
@@ -609,6 +770,7 @@ static int ata_scsiop_read10(ccb * pccb)
 static int ata_scsiop_read_capacity10(ccb *pccb)
 {
 	u32 cap;
+	u32 block_size;
 
 	if (!ataid[pccb->target]) {
 		printf("scsi_ahci: SCSI READ CAPACITY10 command failure. "
@@ -618,11 +780,52 @@ static int ata_scsiop_read_capacity10(ccb *pccb)
 	}
 
 	cap = le32_to_cpu(ataid[pccb->target]->lba_capacity);
+	if (cap == 0xfffffff) {
+		unsigned short *cap48 = ataid[pccb->target]->lba48_capacity;
+		if (cap48[2] || cap48[3]) {
+			cap = 0xffffffff;
+		} else {
+			cap = (le16_to_cpu(cap48[1]) << 16) |
+			      (le16_to_cpu(cap48[0]));
+		}
+	}
+
+	cap = cpu_to_be32(cap);
+	memcpy(pccb->pdata, &cap, sizeof(cap));
+
+	block_size = cpu_to_be32((u32)512);
+	memcpy(&pccb->pdata[4], &block_size, 4);
+
+	return 0;
+}
+
+
+/*
+ * SCSI READ CAPACITY16 command operation.
+ */
+static int ata_scsiop_read_capacity16(ccb *pccb)
+{
+	u64 cap;
+	u64 block_size;
+
+	if (!ataid[pccb->target]) {
+		printf("scsi_ahci: SCSI READ CAPACITY16 command failure. "
+		       "\tNo ATA info!\n"
+		       "\tPlease run SCSI commmand INQUIRY firstly!\n");
+		return -EPERM;
+	}
+
+	cap = le32_to_cpu(ataid[pccb->target]->lba_capacity);
+	if (cap == 0xfffffff) {
+		memcpy(&cap, ataid[pccb->target]->lba48_capacity, sizeof(cap));
+		cap = le64_to_cpu(cap);
+	}
+
+	cap = cpu_to_be64(cap);
 	memcpy(pccb->pdata, &cap, sizeof(cap));
 
-	pccb->pdata[4] = pccb->pdata[5] = 0;
-	pccb->pdata[6] = 512 >> 8;
-	pccb->pdata[7] = 512 & 0xff;
+	block_size = cpu_to_be64((u64)512);
+	memcpy(&pccb->pdata[8], &block_size, 8);
 
 	return 0;
 }
@@ -643,11 +846,17 @@ int scsi_exec(ccb *pccb)
 
 	switch (pccb->cmd[0]) {
 	case SCSI_READ10:
-		ret = ata_scsiop_read10(pccb);
+		ret = ata_scsiop_read_write(pccb, 0);
 		break;
-	case SCSI_RD_CAPAC:
+	case SCSI_WRITE10:
+		ret = ata_scsiop_read_write(pccb, 1);
+		break;
+	case SCSI_RD_CAPAC10:
 		ret = ata_scsiop_read_capacity10(pccb);
 		break;
+	case SCSI_RD_CAPAC16:
+		ret = ata_scsiop_read_capacity16(pccb);
+		break;
 	case SCSI_TST_U_RDY:
 		ret = ata_scsiop_test_unit_ready(pccb);
 		break;
@@ -685,7 +894,9 @@ void scsi_low_level_init(int busdevfunc)
 				printf("Can not start port %d\n", i);
 				continue;
 			}
+#ifdef CONFIG_AHCI_SETFEATURES_XFER
 			ahci_set_feature((u8) i);
+#endif
 		}
 	}
 }
@@ -726,7 +937,9 @@ int ahci_init(u32 base)
 				printf("Can not start port %d\n", i);
 				continue;
 			}
+#ifdef CONFIG_AHCI_SETFEATURES_XFER
 			ahci_set_feature((u8) i);
+#endif
 		}
 	}
 err_out:
@@ -734,6 +947,42 @@ err_out:
 }
 #endif
 
+/*
+ * In the general case of generic rotating media it makes sense to have a
+ * flush capability. It probably even makes sense in the case of SSDs because
+ * one cannot always know for sure what kind of internal cache/flush mechanism
+ * is embodied therein. At first it was planned to invoke this after the last
+ * write to disk and before rebooting. In practice, knowing, a priori, which
+ * is the last write is difficult. Because writing to the disk in u-boot is
+ * very rare, this flush command will be invoked after every block write.
+ */
+static int ata_io_flush(u8 port)
+{
+	u8 fis[20];
+	struct ahci_ioports *pp = &(probe_ent->port[port]);
+	volatile u8 *port_mmio = (volatile u8 *)pp->port_mmio;
+	u32 cmd_fis_len = 5;	/* five dwords */
+
+	/* Preset the FIS */
+	memset(fis, 0, 20);
+	fis[0] = 0x27;		 /* Host to device FIS. */
+	fis[1] = 1 << 7;	 /* Command FIS. */
+	fis[2] = ATA_CMD_FLUSH_EXT;
+
+	memcpy((unsigned char *)pp->cmd_tbl, fis, 20);
+	ahci_fill_cmd_slot(pp, cmd_fis_len);
+	writel_with_flush(1, port_mmio + PORT_CMD_ISSUE);
+
+	if (waiting_for_cmd_completed(port_mmio + PORT_CMD_ISSUE,
+			WAIT_MS_FLUSH, 0x1)) {
+		debug("scsi_ahci: flush command timeout on port %d.\n", port);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+
 void scsi_bus_reset(void)
 {
 	/*Not implement*/
diff --git a/drivers/block/fsl_sata.c b/drivers/block/fsl_sata.c
index fda3389e8b..1f9d7b097c 100644
--- a/drivers/block/fsl_sata.c
+++ b/drivers/block/fsl_sata.c
@@ -56,25 +56,6 @@ static inline void sdelay(unsigned long sec)
 		mdelay(1000);
 }
 
-void dprint_buffer(unsigned char *buf, int len)
-{
-	int i, j;
-
-	i = 0;
-	j = 0;
-	printf("\n\r");
-
-	for (i = 0; i < len; i++) {
-		printf("%02x ", *buf++);
-		j++;
-		if (j == 16) {
-			printf("\n\r");
-			j = 0;
-		}
-	}
-	printf("\n\r");
-}
-
 static void fsl_sata_dump_sfis(struct sata_fis_d2h *s)
 {
 	printf("Status FIS dump:\n\r");
@@ -94,7 +75,7 @@ static void fsl_sata_dump_sfis(struct sata_fis_d2h *s)
 	printf("sector_count_exp:	%02x\n\r", s->sector_count_exp);
 }
 
-static int ata_wait_register(volatile unsigned *addr, u32 mask,
+static int ata_wait_register(unsigned __iomem *addr, u32 mask,
 			 u32 val, u32 timeout_msec)
 {
 	int i;
@@ -112,7 +93,7 @@ int init_sata(int dev)
 	cmd_hdr_tbl_t *cmd_hdr;
 	u32 cda;
 	u32 val32;
-	fsl_sata_reg_t *reg;
+	fsl_sata_reg_t __iomem *reg;
 	u32 sig;
 	int i;
 	fsl_sata_t *sata;
@@ -287,42 +268,7 @@ int init_sata(int dev)
 	return 0;
 }
 
-/* Hardware reset, like Power-on and COMRESET */
-void fsl_sata_hardware_reset(u32 reg_base)
-{
-	fsl_sata_reg_t *reg = (fsl_sata_reg_t *)reg_base;
-	u32 scontrol;
-
-	/* Disable the SATA interface and put PHY offline */
-	scontrol = in_le32(&reg->scontrol);
-	scontrol = (scontrol & 0x0f0) | 0x304;
-	out_le32(&reg->scontrol, scontrol);
-
-	/* No speed strict */
-	scontrol = in_le32(&reg->scontrol);
-	scontrol = scontrol & ~0x0f0;
-	out_le32(&reg->scontrol, scontrol);
-
-	/* Issue PHY wake/reset, Hardware_reset_asserted */
-	scontrol = in_le32(&reg->scontrol);
-	scontrol = (scontrol & 0x0f0) | 0x301;
-	out_le32(&reg->scontrol, scontrol);
-
-	mdelay(100);
-
-	/* Resume PHY, COMRESET negated, the device initialize hardware
-	 * and execute diagnostics, send good status-signature to host,
-	 * which is D2H register FIS, and then the device enter idle state.
-	 */
-	scontrol = in_le32(&reg->scontrol);
-	scontrol = (scontrol & 0x0f0) | 0x300;
-	out_le32(&reg->scontrol, scontrol);
-
-	mdelay(100);
-	return;
-}
-
-static void fsl_sata_dump_regs(fsl_sata_reg_t *reg)
+static void fsl_sata_dump_regs(fsl_sata_reg_t __iomem *reg)
 {
 	printf("\n\rSATA:           %08x\n\r", (u32)reg);
 	printf("CQR:            %08x\n\r", in_le32(&reg->cqr));
@@ -363,7 +309,7 @@ static int fsl_ata_exec_ata_cmd(struct fsl_sata *sata, struct sata_fis_h2d *cfis
 	u32 prde_count;
 	u32 val32;
 	u32 ttl;
-	fsl_sata_reg_t *reg = sata->reg_base;
+	fsl_sata_reg_t __iomem *reg = sata->reg_base;
 	int i;
 
 	/* Check xfer length */
@@ -620,7 +566,7 @@ static u32 fsl_sata_rw_cmd(int dev, u32 start, u32 blkcnt, u8 *buffer, int is_wr
 	return blkcnt;
 }
 
-void fsl_sata_flush_cache(int dev)
+static void fsl_sata_flush_cache(int dev)
 {
 	fsl_sata_t *sata = (fsl_sata_t *)sata_dev_desc[dev].priv;
 	struct sata_fis_h2d h2d, *cfis = &h2d;
@@ -664,7 +610,8 @@ static u32 fsl_sata_rw_cmd_ext(int dev, u32 start, u32 blkcnt, u8 *buffer, int i
 	return blkcnt;
 }
 
-u32 fsl_sata_rw_ncq_cmd(int dev, u32 start, u32 blkcnt, u8 *buffer, int is_write)
+static u32 fsl_sata_rw_ncq_cmd(int dev, u32 start, u32 blkcnt, u8 *buffer,
+			       int is_write)
 {
 	fsl_sata_t *sata = (fsl_sata_t *)sata_dev_desc[dev].priv;
 	struct sata_fis_h2d h2d, *cfis = &h2d;
@@ -707,7 +654,7 @@ u32 fsl_sata_rw_ncq_cmd(int dev, u32 start, u32 blkcnt, u8 *buffer, int is_write
 	return blkcnt;
 }
 
-void fsl_sata_flush_cache_ext(int dev)
+static void fsl_sata_flush_cache_ext(int dev)
 {
 	fsl_sata_t *sata = (fsl_sata_t *)sata_dev_desc[dev].priv;
 	struct sata_fis_h2d h2d, *cfis = &h2d;
@@ -721,12 +668,6 @@ void fsl_sata_flush_cache_ext(int dev)
 	fsl_sata_exec_cmd(sata, cfis, CMD_ATA, 0, NULL, 0);
 }
 
-/* Software reset, set SRST of the Device Control register */
-void fsl_sata_software_reset(int dev)
-{
-	return;
-}
-
 static void fsl_sata_init_wcache(int dev, u16 *id)
 {
 	fsl_sata_t *sata = (fsl_sata_t *)sata_dev_desc[dev].priv;
@@ -757,7 +698,7 @@ static int fsl_sata_get_flush_ext(int dev)
 	return sata->flush_ext;
 }
 
-u32 ata_low_level_rw_lba48(int dev, u32 blknr, lbaint_t blkcnt,
+static u32 ata_low_level_rw_lba48(int dev, u32 blknr, lbaint_t blkcnt,
 		const void *buffer, int is_write)
 {
 	u32 start, blks;
@@ -792,8 +733,8 @@ u32 ata_low_level_rw_lba48(int dev, u32 blknr, lbaint_t blkcnt,
 	return blkcnt;
 }
 
-u32 ata_low_level_rw_lba28(int dev, u32 blknr, u32 blkcnt, const void *buffer,
-		int is_write)
+static u32 ata_low_level_rw_lba28(int dev, u32 blknr, u32 blkcnt,
+				  const void *buffer, int is_write)
 {
 	u32 start, blks;
 	u8 *addr;
diff --git a/drivers/block/fsl_sata.h b/drivers/block/fsl_sata.h
index cecff68da3..a9c27bc868 100644
--- a/drivers/block/fsl_sata.h
+++ b/drivers/block/fsl_sata.h
@@ -176,10 +176,11 @@ typedef struct fsl_sata_reg {
 * Command Header Entry
 */
 typedef struct cmd_hdr_entry {
-	u32 cda;		/* Command Descriptor Address, 4 bytes aligned */
-	u32 prde_fis_len;	/* Number of PRD entries and FIS length */
-	u32 ttl;		/* Total transfer length */
-	u32 attribute;		/* the attribute of command */
+	__le32 cda;		/* Command Descriptor Address,
+				   4 bytes aligned */
+	__le32 prde_fis_len;	/* Number of PRD entries and FIS length */
+	__le32 ttl;		/* Total transfer length */
+	__le32 attribute;	/* the attribute of command */
 } __attribute__ ((packed)) cmd_hdr_entry_t;
 
 #define SATA_HC_CMD_HDR_ENTRY_SIZE	sizeof(struct cmd_hdr_entry)
@@ -230,10 +231,10 @@ typedef struct cmd_hdr_tbl {
 * PRD entry - Physical Region Descriptor entry
 */
 typedef struct prd_entry {
-	u32 dba;	/* Data base address, 4 bytes aligned */
+	__le32 dba;	/* Data base address, 4 bytes aligned */
 	u32 res1;
 	u32 res2;
-	u32 ext_c_ddc;	/* Indirect PRD flags, snoop and data word count */
+	__le32 ext_c_ddc; /* Indirect PRD flags, snoop and data word count */
 } __attribute__ ((packed)) prd_entry_t;
 
 #define SATA_HC_CMD_DESC_PRD_SIZE	sizeof(struct prd_entry)
diff --git a/drivers/block/sata_sil.c b/drivers/block/sata_sil.c
index 245b872f99..b70f04de8b 100644
--- a/drivers/block/sata_sil.c
+++ b/drivers/block/sata_sil.c
@@ -27,6 +27,7 @@
 #include <fis.h>
 #include <sata.h>
 #include <libata.h>
+#include <sata.h>
 #include "sata_sil.h"
 
 /* Convert sectorsize to wordsize */
@@ -369,8 +370,8 @@ static ulong sil_sata_rw_cmd_ext(int dev, ulong start, ulong blkcnt,
 	return blkcnt;
 }
 
-ulong sil_sata_rw_lba28(int dev, ulong blknr, lbaint_t blkcnt,
-		const void *buffer, int is_write)
+static ulong sil_sata_rw_lba28(int dev, ulong blknr, lbaint_t blkcnt,
+			       const void *buffer, int is_write)
 {
 	ulong start, blks, max_blks;
 	u8 *addr;
@@ -397,8 +398,8 @@ ulong sil_sata_rw_lba28(int dev, ulong blknr, lbaint_t blkcnt,
 	return blkcnt;
 }
 
-ulong sil_sata_rw_lba48(int dev, ulong blknr, lbaint_t blkcnt,
-		const void *buffer, int is_write)
+static ulong sil_sata_rw_lba48(int dev, ulong blknr, lbaint_t blkcnt,
+			       const void *buffer, int is_write)
 {
 	ulong start, blks, max_blks;
 	u8 *addr;
@@ -427,7 +428,7 @@ ulong sil_sata_rw_lba48(int dev, ulong blknr, lbaint_t blkcnt,
 	return blkcnt;
 }
 
-void sil_sata_cmd_flush_cache(int dev)
+static void sil_sata_cmd_flush_cache(int dev)
 {
 	struct sil_cmd_block cmdb, *pcmd = &cmdb;
 
@@ -439,7 +440,7 @@ void sil_sata_cmd_flush_cache(int dev)
 	sil_exec_cmd(dev, pcmd, 0);
 }
 
-void sil_sata_cmd_flush_cache_ext(int dev)
+static void sil_sata_cmd_flush_cache_ext(int dev)
 {
 	struct sil_cmd_block cmdb, *pcmd = &cmdb;
 
diff --git a/drivers/fpga/cyclon2.c b/drivers/fpga/cyclon2.c
index 26494a3f89..0773e731eb 100644
--- a/drivers/fpga/cyclon2.c
+++ b/drivers/fpga/cyclon2.c
@@ -144,6 +144,8 @@ static int CYC2_ps_load(Altera_desc *desc, const void *buf, size_t bsize)
 		}
 
 		/* Establish the initial state */
+		(*fn->config) (FALSE, TRUE, cookie);	/* De-assert nCONFIG */
+		udelay(100);
 		(*fn->config) (TRUE, TRUE, cookie);	/* Assert nCONFIG */
 
 		udelay(2);		/* T_cfg > 2us	*/
diff --git a/drivers/gpio/mpc83xx_gpio.c b/drivers/gpio/mpc83xx_gpio.c
index a9afcb2dd6..e167852b83 100644
--- a/drivers/gpio/mpc83xx_gpio.c
+++ b/drivers/gpio/mpc83xx_gpio.c
@@ -163,7 +163,7 @@ int gpio_set_value(unsigned gpio, int value)
 }
 
 /* Configure GPIO registers early */
-void mpc83xx_gpio_init_f()
+void mpc83xx_gpio_init_f(void)
 {
 	immap_t *im = (immap_t *)CONFIG_SYS_IMMR;
 
@@ -187,7 +187,7 @@ void mpc83xx_gpio_init_f()
 }
 
 /* Initialize GPIO soft-copies */
-void mpc83xx_gpio_init_r()
+void mpc83xx_gpio_init_r(void)
 {
 #if MPC83XX_GPIO_CTRLRS >= 1
 	gpio_output_value[0] = CONFIG_MPC83XX_GPIO_0_INIT_VALUE;
diff --git a/drivers/i2c/fsl_i2c.c b/drivers/i2c/fsl_i2c.c
index 5b017a910c..3cb232fdd1 100644
--- a/drivers/i2c/fsl_i2c.c
+++ b/drivers/i2c/fsl_i2c.c
@@ -214,7 +214,7 @@ static unsigned int set_i2c_bus_speed(const struct fsl_i2c *dev,
 	return speed;
 }
 
-unsigned int get_i2c_clock(int bus)
+static unsigned int get_i2c_clock(int bus)
 {
 	if (bus)
 		return gd->i2c2_clk;	/* I2C2 clock */
diff --git a/drivers/i2c/sh_i2c.c b/drivers/i2c/sh_i2c.c
index 3147123bba..44ba90ef57 100644
--- a/drivers/i2c/sh_i2c.c
+++ b/drivers/i2c/sh_i2c.c
@@ -43,12 +43,18 @@ static struct sh_i2c *base;
 #define SH_I2C_ICCR_SCP		(1 << 0)
 
 /* ICSR / ICIC */
-#define SH_IC_BUSY	(1 << 3)
+#define SH_IC_BUSY	(1 << 4)
 #define SH_IC_TACK	(1 << 2)
 #define SH_IC_WAIT	(1 << 1)
 #define SH_IC_DTE	(1 << 0)
 
-static u8 iccl, icch;
+#ifdef CONFIG_SH_I2C_8BIT
+/* store 8th bit of iccl and icch in ICIC register */
+#define SH_I2C_ICIC_ICCLB8	(1 << 7)
+#define SH_I2C_ICIC_ICCHB8	(1 << 6)
+#endif
+
+static u16 iccl, icch;
 
 #define IRQ_WAIT 1000
 
@@ -63,6 +69,20 @@ static void irq_dte(struct sh_i2c *base)
 	}
 }
 
+static int irq_dte_with_tack(struct sh_i2c *base)
+{
+	int i;
+
+	for (i = 0 ; i < IRQ_WAIT ; i++) {
+		if (SH_IC_DTE & readb(&base->icsr))
+			break;
+		if (SH_IC_TACK & readb(&base->icsr))
+			return -1;
+		udelay(10);
+	}
+	return 0;
+}
+
 static void irq_busy(struct sh_i2c *base)
 {
 	int i;
@@ -74,71 +94,97 @@ static void irq_busy(struct sh_i2c *base)
 	}
 }
 
-static void i2c_set_addr(struct sh_i2c *base, u8 id, u8 reg, int stop)
+static int i2c_set_addr(struct sh_i2c *base, u8 id, u8 reg, int stop)
 {
-	writeb(readb(&base->iccr) & ~SH_I2C_ICCR_ICE, &base->iccr);
-	writeb(readb(&base->iccr) | SH_I2C_ICCR_ICE, &base->iccr);
-
-	writeb(iccl, &base->iccl);
-	writeb(icch, &base->icch);
-	writeb(0, &base->icic);
+	u8 icic = SH_IC_TACK;
+
+	clrbits_8(&base->iccr, SH_I2C_ICCR_ICE);
+	setbits_8(&base->iccr, SH_I2C_ICCR_ICE);
+
+	writeb(iccl & 0xff, &base->iccl);
+	writeb(icch & 0xff, &base->icch);
+#ifdef CONFIG_SH_I2C_8BIT
+	if (iccl > 0xff)
+		icic |= SH_I2C_ICIC_ICCLB8;
+	if (icch > 0xff)
+		icic |= SH_I2C_ICIC_ICCHB8;
+#endif
+	writeb(icic, &base->icic);
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RTS|SH_I2C_ICCR_BUSY), &base->iccr);
 	irq_dte(base);
 
+	clrbits_8(&base->icsr, SH_IC_TACK);
 	writeb(id << 1, &base->icdr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		return -1;
 
 	writeb(reg, &base->icdr);
 	if (stop)
 		writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RTS), &base->iccr);
 
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		return -1;
+	return 0;
 }
 
 static void i2c_finish(struct sh_i2c *base)
 {
 	writeb(0, &base->icsr);
-	writeb(readb(&base->iccr) & ~SH_I2C_ICCR_ICE, &base->iccr);
+	clrbits_8(&base->iccr, SH_I2C_ICCR_ICE);
 }
 
-static void i2c_raw_write(struct sh_i2c *base, u8 id, u8 reg, u8 val)
+static int i2c_raw_write(struct sh_i2c *base, u8 id, u8 reg, u8 val)
 {
-	i2c_set_addr(base, id, reg, 0);
+	int ret = -1;
+	if (i2c_set_addr(base, id, reg, 0) != 0)
+		goto exit0;
 	udelay(10);
 
 	writeb(val, &base->icdr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 
 	writeb((SH_I2C_ICCR_ICE | SH_I2C_ICCR_RTS), &base->iccr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 	irq_busy(base);
-
+	ret = 0;
+exit0:
 	i2c_finish(base);
+	return ret;
 }
 
-static u8 i2c_raw_read(struct sh_i2c *base, u8 id, u8 reg)
+static int i2c_raw_read(struct sh_i2c *base, u8 id, u8 reg)
 {
-	u8 ret;
-
-	i2c_set_addr(base, id, reg, 1);
+	int ret = -1;
+
+#if defined(CONFIG_SH73A0)
+	if (i2c_set_addr(base, id, reg, 0) != 0)
+		goto exit0;
+#else
+	if (i2c_set_addr(base, id, reg, 1) != 0)
+		goto exit0;
 	udelay(100);
+#endif
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RTS|SH_I2C_ICCR_BUSY), &base->iccr);
 	irq_dte(base);
 
 	writeb(id << 1 | 0x01, &base->icdr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_SCP), &base->iccr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 
-	ret = readb(&base->icdr);
+	ret = readb(&base->icdr) & 0xff;
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RACK), &base->iccr);
 	readb(&base->icdr); /* Dummy read */
 	irq_busy(base);
-
+exit0:
 	i2c_finish(base);
 
 	return ret;
@@ -166,6 +212,21 @@ int i2c_set_bus_num(unsigned int bus)
 	case 1:
 		base = (void *)CONFIG_SH_I2C_BASE1;
 		break;
+#ifdef CONFIG_SH_I2C_BASE2
+	case 2:
+		base = (void *)CONFIG_SH_I2C_BASE2;
+		break;
+#endif
+#ifdef CONFIG_SH_I2C_BASE3
+	case 3:
+		base = (void *)CONFIG_SH_I2C_BASE3;
+		break;
+#endif
+#ifdef CONFIG_SH_I2C_BASE4
+	case 4:
+		base = (void *)CONFIG_SH_I2C_BASE4;
+		break;
+#endif
 	default:
 		return -1;
 	}
@@ -206,18 +267,18 @@ void i2c_init(int speed, int slaveaddr)
 	denom = speed * (CONFIG_SH_I2C_DATA_HIGH + CONFIG_SH_I2C_DATA_LOW);
 	tmp = num * 10 / denom;
 	if (tmp % 10 >= 5)
-		iccl = (u8)((num/denom) + 1);
+		iccl = (u16)((num/denom) + 1);
 	else
-		iccl = (u8)(num/denom);
+		iccl = (u16)(num/denom);
 
 	/* Calculate the value for icch. From the data sheet:
 	   icch = (p clock / transfer rate) * (H / (L + H)) */
 	num = CONFIG_SH_I2C_CLOCK * CONFIG_SH_I2C_DATA_HIGH;
 	tmp = num * 10 / denom;
 	if (tmp % 10 >= 5)
-		icch = (u8)((num/denom) + 1);
+		icch = (u16)((num/denom) + 1);
 	else
-		icch = (u8)(num/denom);
+		icch = (u16)(num/denom);
 }
 
 /*
@@ -235,10 +296,14 @@ void i2c_init(int speed, int slaveaddr)
  */
 int i2c_read(u8 chip, u32 addr, int alen, u8 *buffer, int len)
 {
+	int ret;
 	int i = 0;
-	for (i = 0 ; i < len ; i++)
-		buffer[i] = i2c_raw_read(base, chip, addr + i);
-
+	for (i = 0 ; i < len ; i++) {
+		ret = i2c_raw_read(base, chip, addr + i);
+		if (ret < 0)
+			return -1;
+		buffer[i] = ret & 0xff;
+	}
 	return 0;
 }
 
@@ -259,8 +324,8 @@ int i2c_write(u8 chip, u32 addr, int alen, u8 *buffer, int len)
 {
 	int i = 0;
 	for (i = 0; i < len ; i++)
-		i2c_raw_write(base, chip, addr + i, buffer[i]);
-
+		if (i2c_raw_write(base, chip, addr + i, buffer[i]) != 0)
+			return -1;
 	return 0;
 }
 
@@ -272,5 +337,9 @@ int i2c_write(u8 chip, u32 addr, int alen, u8 *buffer, int len)
  */
 int i2c_probe(u8 chip)
 {
-	return 0;
+	int ret;
+
+	ret = i2c_set_addr(base, chip, 0, 1);
+	i2c_finish(base);
+	return ret;
 }
diff --git a/drivers/i2c/soft_i2c.c b/drivers/i2c/soft_i2c.c
index 36c6114e3c..1595c0714a 100644
--- a/drivers/i2c/soft_i2c.c
+++ b/drivers/i2c/soft_i2c.c
@@ -41,9 +41,6 @@
 #ifdef	CONFIG_IXP425			/* only valid for IXP425 */
 #include <asm/arch/ixp425.h>
 #endif
-#ifdef CONFIG_LPC2292
-#include <asm/arch/hardware.h>
-#endif
 #if defined(CONFIG_MPC852T) || defined(CONFIG_MPC866)
 #include <asm/io.h>
 #endif
diff --git a/drivers/input/i8042.c b/drivers/input/i8042.c
index c3bc5360ca..26958aaa3e 100644
--- a/drivers/input/i8042.c
+++ b/drivers/input/i8042.c
@@ -26,6 +26,7 @@
 /* includes */
 
 #include <common.h>
+#include <linux/compiler.h>
 
 #ifdef CONFIG_USE_CPCIDVI
 extern u8 gt_cpcidvi_in8(u32 offset);
@@ -320,6 +321,54 @@ static int kbd_controller_present(void)
 	return in8(I8042_STATUS_REG) != 0xff;
 }
 
+/*
+ * Implement a weak default function for boards that optionally
+ * need to skip the i8042 initialization.
+ */
+int __weak board_i8042_skip(void)
+{
+	/* As default, don't skip */
+	return 0;
+}
+
+void i8042_flush(void)
+{
+	int timeout;
+
+	/*
+	 * The delay is to give the keyboard controller some time to fill the
+	 * next byte.
+	 */
+	while (1) {
+		timeout = 100;  /* wait for no longer than 100us */
+		while (timeout > 0 && !(in8(I8042_STATUS_REG) & 0x01)) {
+			udelay(1);
+			timeout--;
+		}
+
+		/* Try to pull next byte if not timeout. */
+		if (in8(I8042_STATUS_REG) & 0x01)
+			in8(I8042_DATA_REG);
+		else
+			break;
+	}
+}
+
+int i8042_disable(void)
+{
+	if (kbd_input_empty() == 0)
+		return -1;
+
+	/* Disable keyboard */
+	out8(I8042_COMMAND_REG, 0xad);
+
+	if (kbd_input_empty() == 0)
+		return -1;
+
+	return 0;
+}
+
+
 /*******************************************************************************
  *
  * i8042_kbd_init - reset keyboard and init state flags
@@ -329,7 +378,7 @@ int i8042_kbd_init(void)
 	int keymap, try;
 	char *penv;
 
-	if (!kbd_controller_present())
+	if (!kbd_controller_present() || board_i8042_skip())
 		return -1;
 
 #ifdef CONFIG_USE_CPCIDVI
@@ -607,11 +656,22 @@ static void kbd_led_set(void)
 
 static int kbd_input_empty(void)
 {
-	int kbdTimeout = KBD_TIMEOUT;
+	int kbdTimeout = KBD_TIMEOUT * 1000;
+
+	while ((in8(I8042_STATUS_REG) & I8042_STATUS_IN_DATA) && kbdTimeout--)
+		udelay(1);
+
+	return kbdTimeout != -1;
+}
+
+/******************************************************************************/
+
+static int wait_until_kbd_output_full(void)
+{
+	int kbdTimeout = KBD_TIMEOUT * 1000;
 
-	/* wait for input buf empty */
-	while ((in8(I8042_STATUS_REG) & 0x02) && kbdTimeout--)
-		udelay(1000);
+	while (((in8(I8042_STATUS_REG) & 0x01) == 0) && kbdTimeout--)
+		udelay(1);
 
 	return kbdTimeout != -1;
 }
@@ -620,31 +680,39 @@ static int kbd_input_empty(void)
 
 static int kbd_reset(void)
 {
+	/* KB Reset */
 	if (kbd_input_empty() == 0)
 		return -1;
 
 	out8(I8042_DATA_REG, 0xff);
 
-	udelay(250000);
+	if (wait_until_kbd_output_full() == 0)
+		return -1;
+
+	if (in8(I8042_DATA_REG) != 0xfa) /* ACK */
+		return -1;
+
+	if (wait_until_kbd_output_full() == 0)
+		return -1;
+
+	if (in8(I8042_DATA_REG) != 0xaa) /* Test Pass*/
+		return -1;
 
 	if (kbd_input_empty() == 0)
 		return -1;
 
-#ifdef CONFIG_USE_CPCIDVI
+	/* Set KBC mode */
 	out8(I8042_COMMAND_REG, 0x60);
-#else
-	out8(I8042_DATA_REG, 0x60);
-#endif
 
 	if (kbd_input_empty() == 0)
 		return -1;
 
 	out8(I8042_DATA_REG, 0x45);
 
-
 	if (kbd_input_empty() == 0)
 		return -1;
 
+	/* Enable Keyboard */
 	out8(I8042_COMMAND_REG, 0xae);
 
 	if (kbd_input_empty() == 0)
diff --git a/drivers/input/input.c b/drivers/input/input.c
index 5b2b4b0714..04fa5f0bd2 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -93,8 +93,24 @@ static unsigned char kbd_ctrl_xlate[] = {
 	'\r', 0xff, 0xff
 };
 
+/*
+ * Scan key code to ANSI 3.64 escape sequence table.  This table is
+ * incomplete in that it does not include all possible extra keys.
+ */
+static struct {
+	int kbd_scan_code;
+	char *escape;
+} kbd_to_ansi364[] = {
+	{ KEY_UP, "\033[A"},
+	{ KEY_DOWN, "\033[B"},
+	{ KEY_RIGHT, "\033[C"},
+	{ KEY_LEFT, "\033[D"},
+};
+
+/* Maximum number of output characters that an ANSI sequence expands to */
+#define ANSI_CHAR_MAX	3
 
-int input_queue_ascii(struct input_config *config, int ch)
+static int input_queue_ascii(struct input_config *config, int ch)
 {
 	if (config->fifo_in + 1 == INPUT_BUFFER_LEN) {
 		if (!config->fifo_out)
@@ -289,24 +305,67 @@ static int input_check_keycodes(struct input_config *config,
 }
 
 /**
+ * Checks and converts a special key code into ANSI 3.64 escape sequence.
+ *
+ * @param config	Input state
+ * @param keycode	Key code to examine
+ * @param output_ch	Buffer to place output characters into. It should
+ *			be at least ANSI_CHAR_MAX bytes long, to allow for
+ *			an ANSI sequence.
+ * @param max_chars	Maximum number of characters to add to output_ch
+ * @return number of characters output, if the key was converted, otherwise 0.
+ *	This may be larger than max_chars, in which case the overflow
+ *	characters are not output.
+ */
+static int input_keycode_to_ansi364(struct input_config *config,
+		int keycode, char output_ch[], int max_chars)
+{
+	const char *escape;
+	int ch_count;
+	int i;
+
+	for (i = ch_count = 0; i < ARRAY_SIZE(kbd_to_ansi364); i++) {
+		if (keycode != kbd_to_ansi364[i].kbd_scan_code)
+			continue;
+		for (escape = kbd_to_ansi364[i].escape; *escape; escape++) {
+			if (ch_count < max_chars)
+				output_ch[ch_count] = *escape;
+			ch_count++;
+		}
+		return ch_count;
+	}
+
+	return 0;
+}
+
+/**
+ * Converts and queues a list of key codes in escaped ASCII string form
  * Convert a list of key codes into ASCII
  *
  * You must call input_check_keycodes() before this. It turns the keycode
- * list into a list of ASCII characters which are ready to send to the
- * input layer.
+ * list into a list of ASCII characters and sends them to the input layer.
  *
  * Characters which were seen last time do not generate fresh ASCII output.
+ * The output (calls to queue_ascii) may be longer than num_keycodes, if the
+ * keycode contains special keys that was encoded to longer escaped sequence.
  *
  * @param config	Input state
  * @param keycode	List of key codes to examine
  * @param num_keycodes	Number of key codes
+ * @param output_ch	Buffer to place output characters into. It should
+ *			be at last ANSI_CHAR_MAX * num_keycodes, to allow for
+ *			ANSI sequences.
+ * @param max_chars	Maximum number of characters to add to output_ch
  * @param same		Number of key codes which are the same
+ * @return number of characters written into output_ch, or -1 if we would
+ *	exceed max_chars chars.
  */
 static int input_keycodes_to_ascii(struct input_config *config,
-		int keycode[], int num_keycodes, char output_ch[], int same)
+		int keycode[], int num_keycodes, char output_ch[],
+		int max_chars, int same)
 {
 	struct input_key_xlate *table;
-	int ch_count;
+	int ch_count = 0;
 	int i;
 
 	table = &config->table[0];
@@ -321,19 +380,31 @@ static int input_keycodes_to_ascii(struct input_config *config,
 		}
 	}
 
-	/* now find normal keys */
-	for (i = ch_count = 0; i < num_keycodes; i++) {
+	/* Start conversion by looking for the first new keycode (by same). */
+	for (i = same; i < num_keycodes; i++) {
 		int key = keycode[i];
+		int ch = (key < table->num_entries) ? table->xlate[key] : 0xff;
 
-		if (key < table->num_entries && i >= same) {
-			int ch = table->xlate[key];
-
-			/* If a normal key with an ASCII value, add it! */
-			if (ch != 0xff)
-				output_ch[ch_count++] = (uchar)ch;
+		/*
+		 * For a normal key (with an ASCII value), add it; otherwise
+		 * translate special key to escape sequence if possible.
+		 */
+		if (ch != 0xff) {
+			if (ch_count < max_chars)
+				output_ch[ch_count] = (uchar)ch;
+			ch_count++;
+		} else {
+			ch_count += input_keycode_to_ansi364(config, key,
+						output_ch, max_chars);
 		}
 	}
 
+	if (ch_count > max_chars) {
+		debug("%s: Output char buffer overflow size=%d, need=%d\n",
+		      __func__, max_chars, ch_count);
+		return -1;
+	}
+
 	/* ok, so return keys */
 	return ch_count;
 }
@@ -341,7 +412,7 @@ static int input_keycodes_to_ascii(struct input_config *config,
 int input_send_keycodes(struct input_config *config,
 			int keycode[], int num_keycodes)
 {
-	char ch[num_keycodes];
+	char ch[num_keycodes * ANSI_CHAR_MAX];
 	int count, i, same = 0;
 	int is_repeat = 0;
 	unsigned delay_ms;
@@ -363,7 +434,7 @@ int input_send_keycodes(struct input_config *config,
 	}
 
 	count = input_keycodes_to_ascii(config, keycode, num_keycodes,
-					ch, is_repeat ? 0 : same);
+					ch, sizeof(ch), is_repeat ? 0 : same);
 	for (i = 0; i < count; i++)
 		input_queue_ascii(config, ch[i]);
 	delay_ms = is_repeat ?
@@ -371,7 +442,8 @@ int input_send_keycodes(struct input_config *config,
 			config->repeat_delay_ms;
 
 	config->next_repeat_ms = get_timer(0) + delay_ms;
-	return 0;
+
+	return count;
 }
 
 int input_add_table(struct input_config *config, int left_keycode,
diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
index 565ba6a383..a1dd7302bf 100644
--- a/drivers/mmc/Makefile
+++ b/drivers/mmc/Makefile
@@ -41,12 +41,12 @@ COBJS-$(CONFIG_MV_SDHCI) += mv_sdhci.o
 COBJS-$(CONFIG_MXC_MMC) += mxcmmc.o
 COBJS-$(CONFIG_MXS_MMC) += mxsmmc.o
 COBJS-$(CONFIG_OMAP_HSMMC) += omap_hsmmc.o
-COBJS-$(CONFIG_PXA_MMC) += pxa_mmc.o
 COBJS-$(CONFIG_PXA_MMC_GENERIC) += pxa_mmc_gen.o
 COBJS-$(CONFIG_SDHCI) += sdhci.o
 COBJS-$(CONFIG_S5P_SDHCI) += s5p_sdhci.o
 COBJS-$(CONFIG_SH_MMCIF) += sh_mmcif.o
 COBJS-$(CONFIG_TEGRA_MMC) += tegra_mmc.o
+COBJS-$(CONFIG_DWMMC) += dw_mmc.o
 
 COBJS	:= $(COBJS-y)
 SRCS	:= $(COBJS:.o=.c)
diff --git a/drivers/mmc/dw_mmc.c b/drivers/mmc/dw_mmc.c
new file mode 100644
index 0000000000..4070d4ea53
--- /dev/null
+++ b/drivers/mmc/dw_mmc.c
@@ -0,0 +1,385 @@
+/*
+ * (C) Copyright 2012 SAMSUNG Electronics
+ * Jaehoon Chung <jh80.chung@samsung.com>
+ * Rajeshawari Shinde <rajeshwari.s@samsung.com>
+ *
+ * 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
+ *
+ */
+
+#include <common.h>
+#include <malloc.h>
+#include <mmc.h>
+#include <dwmmc.h>
+#include <asm/arch/clk.h>
+#include <asm-generic/errno.h>
+
+#define PAGE_SIZE 4096
+
+static int dwmci_wait_reset(struct dwmci_host *host, u32 value)
+{
+	unsigned long timeout = 1000;
+	u32 ctrl;
+
+	dwmci_writel(host, DWMCI_CTRL, value);
+
+	while (timeout--) {
+		ctrl = dwmci_readl(host, DWMCI_CTRL);
+		if (!(ctrl & DWMCI_RESET_ALL))
+			return 1;
+	}
+	return 0;
+}
+
+static void dwmci_set_idma_desc(struct dwmci_idmac *idmac,
+		u32 desc0, u32 desc1, u32 desc2)
+{
+	struct dwmci_idmac *desc = idmac;
+
+	desc->flags = desc0;
+	desc->cnt = desc1;
+	desc->addr = desc2;
+	desc->next_addr = (unsigned int)desc + sizeof(struct dwmci_idmac);
+}
+
+static void dwmci_prepare_data(struct dwmci_host *host,
+		struct mmc_data *data)
+{
+	unsigned long ctrl;
+	unsigned int i = 0, flags, cnt, blk_cnt;
+	ulong data_start, data_end, start_addr;
+	ALLOC_CACHE_ALIGN_BUFFER(struct dwmci_idmac, cur_idmac, data->blocks);
+
+
+	blk_cnt = data->blocks;
+
+	dwmci_wait_reset(host, DWMCI_CTRL_FIFO_RESET);
+
+	data_start = (ulong)cur_idmac;
+	dwmci_writel(host, DWMCI_DBADDR, (unsigned int)cur_idmac);
+
+	if (data->flags == MMC_DATA_READ)
+		start_addr = (unsigned int)data->dest;
+	else
+		start_addr = (unsigned int)data->src;
+
+	do {
+		flags = DWMCI_IDMAC_OWN | DWMCI_IDMAC_CH ;
+		flags |= (i == 0) ? DWMCI_IDMAC_FS : 0;
+		if (blk_cnt <= 8) {
+			flags |= DWMCI_IDMAC_LD;
+			cnt = data->blocksize * blk_cnt;
+		} else
+			cnt = data->blocksize * 8;
+
+		dwmci_set_idma_desc(cur_idmac, flags, cnt,
+				start_addr + (i * PAGE_SIZE));
+
+		if(blk_cnt < 8)
+			break;
+		blk_cnt -= 8;
+		cur_idmac++;
+		i++;
+	} while(1);
+
+	data_end = (ulong)cur_idmac;
+	flush_dcache_range(data_start, data_end + ARCH_DMA_MINALIGN);
+
+	ctrl = dwmci_readl(host, DWMCI_CTRL);
+	ctrl |= DWMCI_IDMAC_EN | DWMCI_DMA_EN;
+	dwmci_writel(host, DWMCI_CTRL, ctrl);
+
+	ctrl = dwmci_readl(host, DWMCI_BMOD);
+	ctrl |= DWMCI_BMOD_IDMAC_FB | DWMCI_BMOD_IDMAC_EN;
+	dwmci_writel(host, DWMCI_BMOD, ctrl);
+
+	dwmci_writel(host, DWMCI_BLKSIZ, data->blocksize);
+	dwmci_writel(host, DWMCI_BYTCNT, data->blocksize * data->blocks);
+}
+
+static int dwmci_set_transfer_mode(struct dwmci_host *host,
+		struct mmc_data *data)
+{
+	unsigned long mode;
+
+	mode = DWMCI_CMD_DATA_EXP;
+	if (data->flags & MMC_DATA_WRITE)
+		mode |= DWMCI_CMD_RW;
+
+	return mode;
+}
+
+static int dwmci_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
+		struct mmc_data *data)
+{
+	struct dwmci_host *host = (struct dwmci_host *)mmc->priv;
+	int flags = 0, i;
+	unsigned int timeout = 100000;
+	u32 retry = 10000;
+	u32 mask, ctrl;
+
+	while (dwmci_readl(host, DWMCI_STATUS) & DWMCI_BUSY) {
+		if (timeout == 0) {
+			printf("Timeout on data busy\n");
+			return TIMEOUT;
+		}
+		timeout--;
+	}
+
+	dwmci_writel(host, DWMCI_RINTSTS, DWMCI_INTMSK_ALL);
+
+	if (data)
+		dwmci_prepare_data(host, data);
+
+
+	dwmci_writel(host, DWMCI_CMDARG, cmd->cmdarg);
+
+	if (data)
+		flags = dwmci_set_transfer_mode(host, data);
+
+	if ((cmd->resp_type & MMC_RSP_136) && (cmd->resp_type & MMC_RSP_BUSY))
+		return -1;
+
+	if (cmd->cmdidx == MMC_CMD_STOP_TRANSMISSION)
+		flags |= DWMCI_CMD_ABORT_STOP;
+	else
+		flags |= DWMCI_CMD_PRV_DAT_WAIT;
+
+	if (cmd->resp_type & MMC_RSP_PRESENT) {
+		flags |= DWMCI_CMD_RESP_EXP;
+		if (cmd->resp_type & MMC_RSP_136)
+			flags |= DWMCI_CMD_RESP_LENGTH;
+	}
+
+	if (cmd->resp_type & MMC_RSP_CRC)
+		flags |= DWMCI_CMD_CHECK_CRC;
+
+	flags |= (cmd->cmdidx | DWMCI_CMD_START | DWMCI_CMD_USE_HOLD_REG);
+
+	debug("Sending CMD%d\n",cmd->cmdidx);
+
+	dwmci_writel(host, DWMCI_CMD, flags);
+
+	for (i = 0; i < retry; i++) {
+		mask = dwmci_readl(host, DWMCI_RINTSTS);
+		if (mask & DWMCI_INTMSK_CDONE) {
+			if (!data)
+				dwmci_writel(host, DWMCI_RINTSTS, mask);
+			break;
+		}
+	}
+
+	if (i == retry)
+		return TIMEOUT;
+
+	if (mask & DWMCI_INTMSK_RTO) {
+		debug("Response Timeout..\n");
+		return TIMEOUT;
+	} else if (mask & DWMCI_INTMSK_RE) {
+		debug("Response Error..\n");
+		return -1;
+	}
+
+
+	if (cmd->resp_type & MMC_RSP_PRESENT) {
+		if (cmd->resp_type & MMC_RSP_136) {
+			cmd->response[0] = dwmci_readl(host, DWMCI_RESP3);
+			cmd->response[1] = dwmci_readl(host, DWMCI_RESP2);
+			cmd->response[2] = dwmci_readl(host, DWMCI_RESP1);
+			cmd->response[3] = dwmci_readl(host, DWMCI_RESP0);
+		} else {
+			cmd->response[0] = dwmci_readl(host, DWMCI_RESP0);
+		}
+	}
+
+	if (data) {
+		do {
+			mask = dwmci_readl(host, DWMCI_RINTSTS);
+			if (mask & (DWMCI_DATA_ERR | DWMCI_DATA_TOUT)) {
+				debug("DATA ERROR!\n");
+				return -1;
+			}
+		} while (!(mask & DWMCI_INTMSK_DTO));
+
+		dwmci_writel(host, DWMCI_RINTSTS, mask);
+
+		ctrl = dwmci_readl(host, DWMCI_CTRL);
+		ctrl &= ~(DWMCI_DMA_EN);
+		dwmci_writel(host, DWMCI_CTRL, ctrl);
+	}
+
+	udelay(100);
+
+	return 0;
+}
+
+static int dwmci_setup_bus(struct dwmci_host *host, u32 freq)
+{
+	u32 div, status;
+	int timeout = 10000;
+	unsigned long sclk;
+
+	if (freq == host->clock)
+		return 0;
+
+	/*
+	 * If host->mmc_clk didn't define,
+	 * then assume that host->bus_hz is source clock value.
+	 * host->bus_hz should be set from user.
+	 */
+	if (host->mmc_clk)
+		sclk = host->mmc_clk(host->dev_index);
+	else if (host->bus_hz)
+		sclk = host->bus_hz;
+	else {
+		printf("Didn't get source clock value..\n");
+		return -EINVAL;
+	}
+
+	div = DIV_ROUND_UP(sclk, 2 * freq);
+
+	dwmci_writel(host, DWMCI_CLKENA, 0);
+	dwmci_writel(host, DWMCI_CLKSRC, 0);
+
+	dwmci_writel(host, DWMCI_CLKDIV, div);
+	dwmci_writel(host, DWMCI_CMD, DWMCI_CMD_PRV_DAT_WAIT |
+			DWMCI_CMD_UPD_CLK | DWMCI_CMD_START);
+
+	do {
+		status = dwmci_readl(host, DWMCI_CMD);
+		if (timeout-- < 0) {
+			printf("TIMEOUT error!!\n");
+			return -ETIMEDOUT;
+		}
+	} while (status & DWMCI_CMD_START);
+
+	dwmci_writel(host, DWMCI_CLKENA, DWMCI_CLKEN_ENABLE |
+			DWMCI_CLKEN_LOW_PWR);
+
+	dwmci_writel(host, DWMCI_CMD, DWMCI_CMD_PRV_DAT_WAIT |
+			DWMCI_CMD_UPD_CLK | DWMCI_CMD_START);
+
+	timeout = 10000;
+	do {
+		status = dwmci_readl(host, DWMCI_CMD);
+		if (timeout-- < 0) {
+			printf("TIMEOUT error!!\n");
+			return -ETIMEDOUT;
+		}
+	} while (status & DWMCI_CMD_START);
+
+	host->clock = freq;
+
+	return 0;
+}
+
+static void dwmci_set_ios(struct mmc *mmc)
+{
+	struct dwmci_host *host = (struct dwmci_host *)mmc->priv;
+	u32 ctype;
+
+	debug("Buswidth = %d, clock: %d\n",mmc->bus_width, mmc->clock);
+
+	dwmci_setup_bus(host, mmc->clock);
+	switch (mmc->bus_width) {
+	case 8:
+		ctype = DWMCI_CTYPE_8BIT;
+		break;
+	case 4:
+		ctype = DWMCI_CTYPE_4BIT;
+		break;
+	default:
+		ctype = DWMCI_CTYPE_1BIT;
+		break;
+	}
+
+	dwmci_writel(host, DWMCI_CTYPE, ctype);
+
+	if (host->clksel)
+		host->clksel(host);
+}
+
+static int dwmci_init(struct mmc *mmc)
+{
+	struct dwmci_host *host = (struct dwmci_host *)mmc->priv;
+	u32 fifo_size, fifoth_val;
+
+	dwmci_writel(host, DWMCI_PWREN, 1);
+
+	if (!dwmci_wait_reset(host, DWMCI_RESET_ALL)) {
+		debug("%s[%d] Fail-reset!!\n",__func__,__LINE__);
+		return -1;
+	}
+
+	dwmci_writel(host, DWMCI_RINTSTS, 0xFFFFFFFF);
+	dwmci_writel(host, DWMCI_INTMASK, 0);
+
+	dwmci_writel(host, DWMCI_TMOUT, 0xFFFFFFFF);
+
+	dwmci_writel(host, DWMCI_IDINTEN, 0);
+	dwmci_writel(host, DWMCI_BMOD, 1);
+
+	fifo_size = dwmci_readl(host, DWMCI_FIFOTH);
+	if (host->fifoth_val)
+		fifoth_val = host->fifoth_val;
+	else
+		fifoth_val = MSIZE(0x2) | RX_WMARK(fifo_size/2 -1) |
+			TX_WMARK(fifo_size/2);
+	dwmci_writel(host, DWMCI_FIFOTH, fifoth_val);
+
+	dwmci_writel(host, DWMCI_CLKENA, 0);
+	dwmci_writel(host, DWMCI_CLKSRC, 0);
+
+	return 0;
+}
+
+int add_dwmci(struct dwmci_host *host, u32 max_clk, u32 min_clk)
+{
+	struct mmc *mmc;
+	int err = 0;
+
+	mmc = malloc(sizeof(struct mmc));
+	if (!mmc) {
+		printf("mmc malloc fail!\n");
+		return -1;
+	}
+
+	mmc->priv = host;
+	host->mmc = mmc;
+
+	sprintf(mmc->name, "%s", host->name);
+	mmc->send_cmd = dwmci_send_cmd;
+	mmc->set_ios = dwmci_set_ios;
+	mmc->init = dwmci_init;
+	mmc->f_min = min_clk;
+	mmc->f_max = max_clk;
+
+	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
+
+	mmc->host_caps = host->caps;
+
+	if (host->buswidth == 8) {
+		mmc->host_caps |= MMC_MODE_8BIT;
+		mmc->host_caps &= ~MMC_MODE_4BIT;
+	} else {
+		mmc->host_caps |= MMC_MODE_4BIT;
+		mmc->host_caps &= ~MMC_MODE_8BIT;
+	}
+	mmc->host_caps |= MMC_MODE_HS | MMC_MODE_HS_52MHz | MMC_MODE_HC;
+
+	err = mmc_register(mmc);
+
+	return err;
+}
diff --git a/drivers/mmc/fsl_esdhc.c b/drivers/mmc/fsl_esdhc.c
index aa6a9f13ea..e93e38ac43 100644
--- a/drivers/mmc/fsl_esdhc.c
+++ b/drivers/mmc/fsl_esdhc.c
@@ -68,7 +68,7 @@ struct fsl_esdhc {
 };
 
 /* Return the XFERTYP flags for a given command and data packet */
-uint esdhc_xfertyp(struct mmc_cmd *cmd, struct mmc_data *data)
+static uint esdhc_xfertyp(struct mmc_cmd *cmd, struct mmc_data *data)
 {
 	uint xfertyp = 0;
 
diff --git a/drivers/mmc/gen_atmel_mci.c b/drivers/mmc/gen_atmel_mci.c
index 4968c5e491..67b2dbe8d4 100644
--- a/drivers/mmc/gen_atmel_mci.c
+++ b/drivers/mmc/gen_atmel_mci.c
@@ -87,6 +87,11 @@ static void mci_set_mode(struct mmc *mmc, u32 hz, u32 blklen)
 		 | MMCI_BF(BLKLEN, blklen)
 		 | MMCI_BIT(RDPROOF)
 		 | MMCI_BIT(WRPROOF)), &mci->mr);
+	/*
+	 * On some new platforms BLKLEN in mci->mr is ignored.
+	 * Should use the BLKLEN in the block register.
+	 */
+	writel(MMCI_BF(BLKLEN, blklen), &mci->blkr);
 	initialized = 1;
 }
 
@@ -183,6 +188,12 @@ mci_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	/* Figure out the transfer arguments */
 	cmdr = mci_encode_cmd(cmd, data, &error_flags);
 
+	/* For multi blocks read/write, set the block register */
+	if ((cmd->cmdidx == MMC_CMD_READ_MULTIPLE_BLOCK)
+			|| (cmd->cmdidx == MMC_CMD_WRITE_MULTIPLE_BLOCK))
+		writel(data->blocks | MMCI_BF(BLKLEN, mmc->read_bl_len),
+			&mci->blkr);
+
 	/* Send the command */
 	writel(cmd->cmdarg, &mci->argr);
 	writel(cmdr, &mci->cmdr);
@@ -310,8 +321,8 @@ static int mci_init(struct mmc *mmc)
 	writel(MMCI_BIT(MCIEN), &mci->cr);	/* enable mci */
 	writel(MMCI_BF(SCDSEL, MCI_BUS), &mci->sdcr);	/* select port */
 
-	/* Initial Time-outs */
-	writel(0x5f, &mci->dtor);
+	/* This delay can be optimized, but stick with max value */
+	writel(0x7f, &mci->dtor);
 	/* Disable Interrupts */
 	writel(~0UL, &mci->idr);
 
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index a60cfe1cb0..5ffd8c59e6 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -47,104 +47,14 @@ int __board_mmc_getcd(struct mmc *mmc) {
 int board_mmc_getcd(struct mmc *mmc)__attribute__((weak,
 	alias("__board_mmc_getcd")));
 
-#ifdef CONFIG_MMC_BOUNCE_BUFFER
-static int mmc_bounce_need_bounce(struct mmc_data *orig)
-{
-	ulong addr, len;
-
-	if (orig->flags & MMC_DATA_READ)
-		addr = (ulong)orig->dest;
-	else
-		addr = (ulong)orig->src;
-
-	if (addr % ARCH_DMA_MINALIGN) {
-		debug("MMC: Unaligned data destination address %08lx!\n", addr);
-		return 1;
-	}
-
-	len = (ulong)(orig->blocksize * orig->blocks);
-	if (len % ARCH_DMA_MINALIGN) {
-		debug("MMC: Unaligned data destination length %08lx!\n", len);
-		return 1;
-	}
-
-	return 0;
-}
-
-static int mmc_bounce_buffer_start(struct mmc_data *backup,
-					struct mmc_data *orig)
-{
-	ulong origlen, len;
-	void *buffer;
-
-	if (!orig)
-		return 0;
-
-	if (!mmc_bounce_need_bounce(orig))
-		return 0;
-
-	memcpy(backup, orig, sizeof(struct mmc_data));
-
-	origlen = orig->blocksize * orig->blocks;
-	len = roundup(origlen, ARCH_DMA_MINALIGN);
-	buffer = memalign(ARCH_DMA_MINALIGN, len);
-	if (!buffer) {
-		puts("MMC: Error allocating MMC bounce buffer!\n");
-		return 1;
-	}
-
-	if (orig->flags & MMC_DATA_READ) {
-		orig->dest = buffer;
-	} else {
-		memcpy(buffer, orig->src, origlen);
-		orig->src = buffer;
-	}
-
-	return 0;
-}
-
-static void mmc_bounce_buffer_stop(struct mmc_data *backup,
-					struct mmc_data *orig)
-{
-	ulong len;
-
-	if (!orig)
-		return;
-
-	if (!mmc_bounce_need_bounce(backup))
-		return;
-
-	if (backup->flags & MMC_DATA_READ) {
-		len = backup->blocksize * backup->blocks;
-		memcpy(backup->dest, orig->dest, len);
-		free(orig->dest);
-		orig->dest = backup->dest;
-	} else {
-		free((void *)orig->src);
-		orig->src = backup->src;
-	}
-
-	return;
-
-}
-#else
-static inline int mmc_bounce_buffer_start(struct mmc_data *backup,
-					struct mmc_data *orig) { return 0; }
-static inline void mmc_bounce_buffer_stop(struct mmc_data *backup,
-					struct mmc_data *orig) { }
-#endif
-
-int mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
+static int mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
+			struct mmc_data *data)
 {
 	struct mmc_data backup;
 	int ret;
 
 	memset(&backup, 0, sizeof(backup));
 
-	ret = mmc_bounce_buffer_start(&backup, data);
-	if (ret)
-		return ret;
-
 #ifdef CONFIG_MMC_TRACE
 	int i;
 	u8 *ptr;
@@ -196,11 +106,10 @@ int mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 #else
 	ret = mmc->send_cmd(mmc, cmd, data);
 #endif
-	mmc_bounce_buffer_stop(&backup, data);
 	return ret;
 }
 
-int mmc_send_status(struct mmc *mmc, int timeout)
+static int mmc_send_status(struct mmc *mmc, int timeout)
 {
 	struct mmc_cmd cmd;
 	int err, retries = 5;
@@ -244,7 +153,7 @@ int mmc_send_status(struct mmc *mmc, int timeout)
 	return 0;
 }
 
-int mmc_set_blocklen(struct mmc *mmc, int len)
+static int mmc_set_blocklen(struct mmc *mmc, int len)
 {
 	struct mmc_cmd cmd;
 
@@ -437,7 +346,8 @@ mmc_bwrite(int dev_num, ulong start, lbaint_t blkcnt, const void*src)
 	return blkcnt;
 }
 
-int mmc_read_blocks(struct mmc *mmc, void *dst, ulong start, lbaint_t blkcnt)
+static int mmc_read_blocks(struct mmc *mmc, void *dst, ulong start,
+			   lbaint_t blkcnt)
 {
 	struct mmc_cmd cmd;
 	struct mmc_data data;
@@ -507,7 +417,7 @@ static ulong mmc_bread(int dev_num, ulong start, lbaint_t blkcnt, void *dst)
 	return blkcnt;
 }
 
-int mmc_go_idle(struct mmc* mmc)
+static int mmc_go_idle(struct mmc *mmc)
 {
 	struct mmc_cmd cmd;
 	int err;
@@ -528,8 +438,7 @@ int mmc_go_idle(struct mmc* mmc)
 	return 0;
 }
 
-int
-sd_send_op_cond(struct mmc *mmc)
+static int sd_send_op_cond(struct mmc *mmc)
 {
 	int timeout = 1000;
 	int err;
@@ -594,7 +503,7 @@ sd_send_op_cond(struct mmc *mmc)
 	return 0;
 }
 
-int mmc_send_op_cond(struct mmc *mmc)
+static int mmc_send_op_cond(struct mmc *mmc)
 {
 	int timeout = 10000;
 	struct mmc_cmd cmd;
@@ -658,7 +567,7 @@ int mmc_send_op_cond(struct mmc *mmc)
 }
 
 
-int mmc_send_ext_csd(struct mmc *mmc, u8 *ext_csd)
+static int mmc_send_ext_csd(struct mmc *mmc, u8 *ext_csd)
 {
 	struct mmc_cmd cmd;
 	struct mmc_data data;
@@ -680,7 +589,7 @@ int mmc_send_ext_csd(struct mmc *mmc, u8 *ext_csd)
 }
 
 
-int mmc_switch(struct mmc *mmc, u8 set, u8 index, u8 value)
+static int mmc_switch(struct mmc *mmc, u8 set, u8 index, u8 value)
 {
 	struct mmc_cmd cmd;
 	int timeout = 1000;
@@ -702,7 +611,7 @@ int mmc_switch(struct mmc *mmc, u8 set, u8 index, u8 value)
 
 }
 
-int mmc_change_freq(struct mmc *mmc)
+static int mmc_change_freq(struct mmc *mmc)
 {
 	ALLOC_CACHE_ALIGN_BUFFER(u8, ext_csd, 512);
 	char cardtype;
@@ -772,7 +681,7 @@ int mmc_getcd(struct mmc *mmc)
 	return cd;
 }
 
-int sd_switch(struct mmc *mmc, int mode, int group, u8 value, u8 *resp)
+static int sd_switch(struct mmc *mmc, int mode, int group, u8 value, u8 *resp)
 {
 	struct mmc_cmd cmd;
 	struct mmc_data data;
@@ -793,7 +702,7 @@ int sd_switch(struct mmc *mmc, int mode, int group, u8 value, u8 *resp)
 }
 
 
-int sd_change_freq(struct mmc *mmc)
+static int sd_change_freq(struct mmc *mmc)
 {
 	int err;
 	struct mmc_cmd cmd;
@@ -932,7 +841,7 @@ static const int multipliers[] = {
 	80,
 };
 
-void mmc_set_ios(struct mmc *mmc)
+static void mmc_set_ios(struct mmc *mmc)
 {
 	mmc->set_ios(mmc);
 }
@@ -950,14 +859,14 @@ void mmc_set_clock(struct mmc *mmc, uint clock)
 	mmc_set_ios(mmc);
 }
 
-void mmc_set_bus_width(struct mmc *mmc, uint width)
+static void mmc_set_bus_width(struct mmc *mmc, uint width)
 {
 	mmc->bus_width = width;
 
 	mmc_set_ios(mmc);
 }
 
-int mmc_startup(struct mmc *mmc)
+static int mmc_startup(struct mmc *mmc)
 {
 	int err, width;
 	uint mult, freq;
@@ -1105,7 +1014,7 @@ int mmc_startup(struct mmc *mmc)
 	if (!IS_SD(mmc) && (mmc->version >= MMC_VERSION_4)) {
 		/* check  ext_csd version and capacity */
 		err = mmc_send_ext_csd(mmc, ext_csd);
-		if (!err & (ext_csd[EXT_CSD_REV] >= 2)) {
+		if (!err && (ext_csd[EXT_CSD_REV] >= 2)) {
 			/*
 			 * According to the JEDEC Standard, the value of
 			 * ext_csd's capacity is valid if the value is more
@@ -1240,7 +1149,7 @@ int mmc_startup(struct mmc *mmc)
 	return 0;
 }
 
-int mmc_send_if_cond(struct mmc *mmc)
+static int mmc_send_if_cond(struct mmc *mmc)
 {
 	struct mmc_cmd cmd;
 	int err;
diff --git a/drivers/mmc/mmc_spi.c b/drivers/mmc/mmc_spi.c
index de43a85355..11ba532b0c 100644
--- a/drivers/mmc/mmc_spi.c
+++ b/drivers/mmc/mmc_spi.c
@@ -176,8 +176,8 @@ static int mmc_spi_request(struct mmc *mmc, struct mmc_cmd *cmd,
 	u8 r1;
 	int i;
 	int ret = 0;
-	debug("%s:cmd%d %x %x %x\n", __func__,
-	      cmd->cmdidx, cmd->resp_type, cmd->cmdarg, cmd->flags);
+	debug("%s:cmd%d %x %x\n", __func__,
+	      cmd->cmdidx, cmd->resp_type, cmd->cmdarg);
 	spi_claim_bus(spi);
 	spi_cs_activate(spi);
 	r1 = mmc_spi_sendcmd(mmc, cmd->cmdidx, cmd->cmdarg);
diff --git a/drivers/mmc/mxsmmc.c b/drivers/mmc/mxsmmc.c
index c80b41b192..109acbf623 100644
--- a/drivers/mmc/mxsmmc.c
+++ b/drivers/mmc/mxsmmc.c
@@ -42,6 +42,7 @@
 #include <asm/arch/imx-regs.h>
 #include <asm/arch/sys_proto.h>
 #include <asm/arch/dma.h>
+#include <bouncebuf.h>
 
 struct mxsmmc_priv {
 	int			id;
@@ -95,28 +96,33 @@ static int mxsmmc_send_cmd_pio(struct mxsmmc_priv *priv, struct mmc_data *data)
 static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 {
 	uint32_t data_count = data->blocksize * data->blocks;
-	uint32_t cache_data_count;
+	uint32_t cache_data_count = roundup(data_count, ARCH_DMA_MINALIGN);
 	int dmach;
 	struct mxs_dma_desc *desc = priv->desc;
+	void *addr, *backup;
+	uint8_t flags;
 
 	memset(desc, 0, sizeof(struct mxs_dma_desc));
 	desc->address = (dma_addr_t)desc;
 
-	if (data_count % ARCH_DMA_MINALIGN)
-		cache_data_count = roundup(data_count, ARCH_DMA_MINALIGN);
-	else
-		cache_data_count = data_count;
-
 	if (data->flags & MMC_DATA_READ) {
 		priv->desc->cmd.data = MXS_DMA_DESC_COMMAND_DMA_WRITE;
-		priv->desc->cmd.address = (dma_addr_t)data->dest;
+		addr = data->dest;
+		flags = GEN_BB_WRITE;
 	} else {
 		priv->desc->cmd.data = MXS_DMA_DESC_COMMAND_DMA_READ;
-		priv->desc->cmd.address = (dma_addr_t)data->src;
+		addr = (void *)data->src;
+		flags = GEN_BB_READ;
+	}
+
+	bounce_buffer_start(&addr, data_count, &backup, flags);
+
+	priv->desc->cmd.address = (dma_addr_t)addr;
 
+	if (data->flags & MMC_DATA_WRITE) {
 		/* Flush data to DRAM so DMA can pick them up */
-		flush_dcache_range((uint32_t)priv->desc->cmd.address,
-			(uint32_t)(priv->desc->cmd.address + cache_data_count));
+		flush_dcache_range((uint32_t)addr,
+			(uint32_t)(addr) + cache_data_count);
 	}
 
 	/* Invalidate the area, so no writeback into the RAM races with DMA */
@@ -128,15 +134,19 @@ static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 
 	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id;
 	mxs_dma_desc_append(dmach, priv->desc);
-	if (mxs_dma_go(dmach))
+	if (mxs_dma_go(dmach)) {
+		bounce_buffer_stop(&addr, data_count, &backup, flags);
 		return COMM_ERR;
+	}
 
 	/* The data arrived into DRAM, invalidate cache over them */
 	if (data->flags & MMC_DATA_READ) {
-		invalidate_dcache_range((uint32_t)priv->desc->cmd.address,
-			(uint32_t)(priv->desc->cmd.address + cache_data_count));
+		invalidate_dcache_range((uint32_t)addr,
+			(uint32_t)(addr) + cache_data_count);
 	}
 
+	bounce_buffer_stop(&addr, data_count, &backup, flags);
+
 	return 0;
 }
 
diff --git a/drivers/mmc/pxa_mmc.c b/drivers/mmc/pxa_mmc.c
deleted file mode 100644
index 80c4445034..0000000000
--- a/drivers/mmc/pxa_mmc.c
+++ /dev/null
@@ -1,643 +0,0 @@
-/*
- * (C) Copyright 2003
- * Kyle Harris, Nexus Technologies, Inc. kharris@nexus-tech.net
- *
- * 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
- */
-
-#include <config.h>
-#include <common.h>
-#include <mmc.h>
-#include <asm/errno.h>
-#include <asm/arch/hardware.h>
-#include <part.h>
-#include <asm/io.h>
-
-#include "pxa_mmc.h"
-
-extern int fat_register_device(block_dev_desc_t * dev_desc, int part_no);
-
-static block_dev_desc_t mmc_dev;
-
-block_dev_desc_t *mmc_get_dev(int dev)
-{
-	return ((block_dev_desc_t *) & mmc_dev);
-}
-
-/*
- * FIXME needs to read cid and csd info to determine block size
- * and other parameters
- */
-static uchar mmc_buf[MMC_BLOCK_SIZE];
-static uchar spec_ver;
-static int mmc_ready = 0;
-static int wide = 0;
-
-static uint32_t *
-/****************************************************/
-mmc_cmd(ushort cmd, ushort argh, ushort argl, ushort cmdat)
-/****************************************************/
-{
-	static uint32_t resp[4], a, b, c;
-	uint32_t status;
-	int i;
-
-	debug("mmc_cmd %u 0x%04x 0x%04x 0x%04x\n", cmd, argh, argl,
-	      cmdat | wide);
-	writel(MMC_STRPCL_STOP_CLK, MMC_STRPCL);
-	writel(~MMC_I_MASK_CLK_IS_OFF, MMC_I_MASK);
-	while (!(readl(MMC_I_REG) & MMC_I_REG_CLK_IS_OFF))
-		;
-	writel(cmd, MMC_CMD);
-	writel(argh, MMC_ARGH);
-	writel(argl, MMC_ARGL);
-	writel(cmdat | wide, MMC_CMDAT);
-	writel(~MMC_I_MASK_END_CMD_RES, MMC_I_MASK);
-	writel(MMC_STRPCL_START_CLK, MMC_STRPCL);
-	while (!(readl(MMC_I_REG) & MMC_I_REG_END_CMD_RES))
-		;
-
-	status = readl(MMC_STAT);
-	debug("MMC status 0x%08x\n", status);
-	if (status & MMC_STAT_TIME_OUT_RESPONSE) {
-		return 0;
-	}
-
-	/* Linux says:
-	 * Did I mention this is Sick.  We always need to
-	 * discard the upper 8 bits of the first 16-bit word.
-	 */
-	a = (readl(MMC_RES) & 0xffff);
-	for (i = 0; i < 4; i++) {
-		b = (readl(MMC_RES) & 0xffff);
-		c = (readl(MMC_RES) & 0xffff);
-		resp[i] = (a << 24) | (b << 8) | (c >> 8);
-		a = c;
-		debug("MMC resp[%d] = %#08x\n", i, resp[i]);
-	}
-
-	return resp;
-}
-
-int
-/****************************************************/
-mmc_block_read(uchar * dst, uint32_t src, int len)
-/****************************************************/
-{
-	ushort argh, argl;
-	ulong status;
-
-	if (len == 0) {
-		return 0;
-	}
-
-	debug("mmc_block_rd dst %p src %08x len %d\n", dst, src, len);
-
-	argh = len >> 16;
-	argl = len & 0xffff;
-
-	/* set block len */
-	mmc_cmd(MMC_CMD_SET_BLOCKLEN, argh, argl, MMC_CMDAT_R1);
-
-	/* send read command */
-	argh = src >> 16;
-	argl = src & 0xffff;
-	writel(MMC_STRPCL_STOP_CLK, MMC_STRPCL);
-	writel(0xffff, MMC_RDTO);
-	writel(1, MMC_NOB);
-	writel(len, MMC_BLKLEN);
-	mmc_cmd(MMC_CMD_READ_SINGLE_BLOCK, argh, argl,
-		MMC_CMDAT_R1 | MMC_CMDAT_READ | MMC_CMDAT_BLOCK |
-		MMC_CMDAT_DATA_EN);
-
-	writel(~MMC_I_MASK_RXFIFO_RD_REQ, MMC_I_MASK);
-	while (len) {
-		if (readl(MMC_I_REG) & MMC_I_REG_RXFIFO_RD_REQ) {
-#if defined(CONFIG_CPU_PXA27X) || defined(CONFIG_CPU_MONAHANS)
-			int i;
-			for (i = min(len, 32); i; i--) {
-				*dst++ = readb(MMC_RXFIFO);
-				len--;
-			}
-#else
-			*dst++ = readb(MMC_RXFIFO);
-			len--;
-#endif
-		}
-		status = readl(MMC_STAT);
-		if (status & MMC_STAT_ERRORS) {
-			printf("MMC_STAT error %lx\n", status);
-			return -1;
-		}
-	}
-	writel(~MMC_I_MASK_DATA_TRAN_DONE, MMC_I_MASK);
-	while (!(readl(MMC_I_REG) & MMC_I_REG_DATA_TRAN_DONE))
-		;
-	status = readl(MMC_STAT);
-	if (status & MMC_STAT_ERRORS) {
-		printf("MMC_STAT error %lx\n", status);
-		return -1;
-	}
-	return 0;
-}
-
-int
-/****************************************************/
-mmc_block_write(ulong dst, uchar * src, int len)
-/****************************************************/
-{
-	ushort argh, argl;
-	ulong status;
-
-	if (len == 0) {
-		return 0;
-	}
-
-	debug("mmc_block_wr dst %lx src %lx len %d\n", dst, (ulong) src, len);
-
-	argh = len >> 16;
-	argl = len & 0xffff;
-
-	/* set block len */
-	mmc_cmd(MMC_CMD_SET_BLOCKLEN, argh, argl, MMC_CMDAT_R1);
-
-	/* send write command */
-	argh = dst >> 16;
-	argl = dst & 0xffff;
-	writel(MMC_STRPCL_STOP_CLK, MMC_STRPCL);
-	writel(1, MMC_NOB);
-	writel(len, MMC_BLKLEN);
-	mmc_cmd(MMC_CMD_WRITE_SINGLE_BLOCK, argh, argl,
-		MMC_CMDAT_R1 | MMC_CMDAT_WRITE | MMC_CMDAT_BLOCK |
-		MMC_CMDAT_DATA_EN);
-
-	writel(~MMC_I_MASK_TXFIFO_WR_REQ, MMC_I_MASK);
-	while (len) {
-		if (readl(MMC_I_REG) & MMC_I_REG_TXFIFO_WR_REQ) {
-			int i, bytes = min(32, len);
-
-			for (i = 0; i < bytes; i++) {
-				writel(*src++, MMC_TXFIFO);
-			}
-			if (bytes < 32) {
-				writel(MMC_PRTBUF_BUF_PART_FULL, MMC_PRTBUF);
-			}
-			len -= bytes;
-		}
-		status = readl(MMC_STAT);
-		if (status & MMC_STAT_ERRORS) {
-			printf("MMC_STAT error %lx\n", status);
-			return -1;
-		}
-	}
-	writel(~MMC_I_MASK_DATA_TRAN_DONE, MMC_I_MASK);
-	while (!(readl(MMC_I_REG) & MMC_I_REG_DATA_TRAN_DONE))
-		;
-	writel(~MMC_I_MASK_PRG_DONE, MMC_I_MASK);
-	while (!(readl(MMC_I_REG) & MMC_I_REG_PRG_DONE))
-		;
-	status = readl(MMC_STAT);
-	if (status & MMC_STAT_ERRORS) {
-		printf("MMC_STAT error %lx\n", status);
-		return -1;
-	}
-	return 0;
-}
-
-int
-/****************************************************/
-pxa_mmc_read(long src, uchar * dst, int size)
-/****************************************************/
-{
-	ulong end, part_start, part_end, part_len, aligned_start, aligned_end;
-	ulong mmc_block_size, mmc_block_address;
-
-	if (size == 0) {
-		return 0;
-	}
-
-	if (!mmc_ready) {
-		printf("Please initial the MMC first\n");
-		return -1;
-	}
-
-	mmc_block_size = MMC_BLOCK_SIZE;
-	mmc_block_address = ~(mmc_block_size - 1);
-
-	src -= CONFIG_SYS_MMC_BASE;
-	end = src + size;
-	part_start = ~mmc_block_address & src;
-	part_end = ~mmc_block_address & end;
-	aligned_start = mmc_block_address & src;
-	aligned_end = mmc_block_address & end;
-
-	/* all block aligned accesses */
-	debug
-	    ("src %lx dst %lx end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-	     src, (ulong) dst, end, part_start, part_end, aligned_start,
-	     aligned_end);
-	if (part_start) {
-		part_len = mmc_block_size - part_start;
-		debug
-		    ("ps src %lx dst %lx end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-		     src, (ulong) dst, end, part_start, part_end, aligned_start,
-		     aligned_end);
-		if ((mmc_block_read(mmc_buf, aligned_start, mmc_block_size)) <
-		    0) {
-			return -1;
-		}
-		memcpy(dst, mmc_buf + part_start, part_len);
-		dst += part_len;
-		src += part_len;
-	}
-	debug
-	    ("src %lx dst %lx end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-	     src, (ulong) dst, end, part_start, part_end, aligned_start,
-	     aligned_end);
-	for (; src < aligned_end; src += mmc_block_size, dst += mmc_block_size) {
-		debug
-		    ("al src %lx dst %lx end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-		     src, (ulong) dst, end, part_start, part_end, aligned_start,
-		     aligned_end);
-		if ((mmc_block_read((uchar *) (dst), src, mmc_block_size)) < 0) {
-			return -1;
-		}
-	}
-	debug
-	    ("src %lx dst %lx end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-	     src, (ulong) dst, end, part_start, part_end, aligned_start,
-	     aligned_end);
-	if (part_end && src < end) {
-		debug
-		    ("pe src %lx dst %lx end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-		     src, (ulong) dst, end, part_start, part_end, aligned_start,
-		     aligned_end);
-		if ((mmc_block_read(mmc_buf, aligned_end, mmc_block_size)) < 0) {
-			return -1;
-		}
-		memcpy(dst, mmc_buf, part_end);
-	}
-	return 0;
-}
-
-int
-/****************************************************/
-pxa_mmc_write(uchar * src, uint32_t dst, int size)
-/****************************************************/
-{
-	ulong end, part_start, part_end, part_len, aligned_start, aligned_end;
-	ulong mmc_block_size, mmc_block_address;
-
-	if (size == 0) {
-		return 0;
-	}
-
-	if (!mmc_ready) {
-		printf("Please initial the MMC first\n");
-		return -1;
-	}
-
-	mmc_block_size = MMC_BLOCK_SIZE;
-	mmc_block_address = ~(mmc_block_size - 1);
-
-	dst -= CONFIG_SYS_MMC_BASE;
-	end = dst + size;
-	part_start = ~mmc_block_address & dst;
-	part_end = ~mmc_block_address & end;
-	aligned_start = mmc_block_address & dst;
-	aligned_end = mmc_block_address & end;
-
-	/* all block aligned accesses */
-	debug
-	    ("src %p dst %08x end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-	     src, dst, end, part_start, part_end, aligned_start,
-	     aligned_end);
-	if (part_start) {
-		part_len = mmc_block_size - part_start;
-		debug
-		    ("ps src %p dst %08x end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-		     src, dst, end, part_start, part_end, aligned_start,
-		     aligned_end);
-		if ((mmc_block_read(mmc_buf, aligned_start, mmc_block_size)) <
-		    0) {
-			return -1;
-		}
-		memcpy(mmc_buf + part_start, src, part_len);
-		if ((mmc_block_write(aligned_start, mmc_buf, mmc_block_size)) <
-		    0) {
-			return -1;
-		}
-		dst += part_len;
-		src += part_len;
-	}
-	debug
-	    ("src %p dst %08x end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-	     src, dst, end, part_start, part_end, aligned_start,
-	     aligned_end);
-	for (; dst < aligned_end; src += mmc_block_size, dst += mmc_block_size) {
-		debug
-		    ("al src %p dst %08x end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-		     src, dst, end, part_start, part_end, aligned_start,
-		     aligned_end);
-		if ((mmc_block_write(dst, (uchar *) src, mmc_block_size)) < 0) {
-			return -1;
-		}
-	}
-	debug
-	    ("src %p dst %08x end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-	     src, dst, end, part_start, part_end, aligned_start,
-	     aligned_end);
-	if (part_end && dst < end) {
-		debug
-		    ("pe src %p dst %08x end %lx pstart %lx pend %lx astart %lx aend %lx\n",
-		     src, dst, end, part_start, part_end, aligned_start,
-		     aligned_end);
-		if ((mmc_block_read(mmc_buf, aligned_end, mmc_block_size)) < 0) {
-			return -1;
-		}
-		memcpy(mmc_buf, src, part_end);
-		if ((mmc_block_write(aligned_end, mmc_buf, mmc_block_size)) < 0) {
-			return -1;
-		}
-	}
-	return 0;
-}
-
-static ulong
-/****************************************************/
-mmc_bread(int dev_num, ulong blknr, lbaint_t blkcnt, void *dst)
-/****************************************************/
-{
-	int mmc_block_size = MMC_BLOCK_SIZE;
-	ulong src = blknr * mmc_block_size + CONFIG_SYS_MMC_BASE;
-
-	pxa_mmc_read(src, (uchar *) dst, blkcnt * mmc_block_size);
-	return blkcnt;
-}
-
-#ifdef __GNUC__
-#define likely(x)       __builtin_expect(!!(x), 1)
-#define unlikely(x)     __builtin_expect(!!(x), 0)
-#else
-#define likely(x)	(x)
-#define unlikely(x)	(x)
-#endif
-
-#define UNSTUFF_BITS(resp,start,size)					\
-	({								\
-		const int __size = size;				\
-		const uint32_t __mask = (__size < 32 ? 1 << __size : 0) - 1;	\
-		const int32_t __off = 3 - ((start) / 32);			\
-		const int32_t __shft = (start) & 31;			\
-		uint32_t __res;						\
-									\
-		__res = resp[__off] >> __shft;				\
-		if (__size + __shft > 32)				\
-			__res |= resp[__off-1] << ((32 - __shft) % 32);	\
-		__res & __mask;						\
-	})
-
-/*
- * Given the decoded CSD structure, decode the raw CID to our CID structure.
- */
-static void mmc_decode_cid(uint32_t * resp)
-{
-	if (IF_TYPE_SD == mmc_dev.if_type) {
-		/*
-		 * SD doesn't currently have a version field so we will
-		 * have to assume we can parse this.
-		 */
-		sprintf((char *)mmc_dev.vendor,
-			"Man %02x OEM %c%c \"%c%c%c%c%c\" Date %02u/%04u",
-			UNSTUFF_BITS(resp, 120, 8), UNSTUFF_BITS(resp, 112, 8),
-			UNSTUFF_BITS(resp, 104, 8), UNSTUFF_BITS(resp, 96, 8),
-			UNSTUFF_BITS(resp, 88, 8), UNSTUFF_BITS(resp, 80, 8),
-			UNSTUFF_BITS(resp, 72, 8), UNSTUFF_BITS(resp, 64, 8),
-			UNSTUFF_BITS(resp, 8, 4), UNSTUFF_BITS(resp, 12,
-							       8) + 2000);
-		sprintf((char *)mmc_dev.revision, "%d.%d",
-			UNSTUFF_BITS(resp, 60, 4), UNSTUFF_BITS(resp, 56, 4));
-		sprintf((char *)mmc_dev.product, "%u",
-			UNSTUFF_BITS(resp, 24, 32));
-	} else {
-		/*
-		 * The selection of the format here is based upon published
-		 * specs from sandisk and from what people have reported.
-		 */
-		switch (spec_ver) {
-		case 0:	/* MMC v1.0 - v1.2 */
-		case 1:	/* MMC v1.4 */
-			sprintf((char *)mmc_dev.vendor,
-				"Man %02x%02x%02x \"%c%c%c%c%c%c%c\" Date %02u/%04u",
-				UNSTUFF_BITS(resp, 120, 8), UNSTUFF_BITS(resp,
-									 112,
-									 8),
-				UNSTUFF_BITS(resp, 104, 8), UNSTUFF_BITS(resp,
-									 96, 8),
-				UNSTUFF_BITS(resp, 88, 8), UNSTUFF_BITS(resp,
-									80, 8),
-				UNSTUFF_BITS(resp, 72, 8), UNSTUFF_BITS(resp,
-									64, 8),
-				UNSTUFF_BITS(resp, 56, 8), UNSTUFF_BITS(resp,
-									48, 8),
-				UNSTUFF_BITS(resp, 12, 4), UNSTUFF_BITS(resp, 8,
-									4) +
-				1997);
-			sprintf((char *)mmc_dev.revision, "%d.%d",
-				UNSTUFF_BITS(resp, 44, 4), UNSTUFF_BITS(resp,
-									40, 4));
-			sprintf((char *)mmc_dev.product, "%u",
-				UNSTUFF_BITS(resp, 16, 24));
-			break;
-
-		case 2:	/* MMC v2.0 - v2.2 */
-		case 3:	/* MMC v3.1 - v3.3 */
-		case 4:	/* MMC v4 */
-			sprintf((char *)mmc_dev.vendor,
-				"Man %02x OEM %04x \"%c%c%c%c%c%c\" Date %02u/%04u",
-				UNSTUFF_BITS(resp, 120, 8), UNSTUFF_BITS(resp,
-									 104,
-									 16),
-				UNSTUFF_BITS(resp, 96, 8), UNSTUFF_BITS(resp,
-									88, 8),
-				UNSTUFF_BITS(resp, 80, 8), UNSTUFF_BITS(resp,
-									72, 8),
-				UNSTUFF_BITS(resp, 64, 8), UNSTUFF_BITS(resp,
-									56, 8),
-				UNSTUFF_BITS(resp, 12, 4), UNSTUFF_BITS(resp, 8,
-									4) +
-				1997);
-			sprintf((char *)mmc_dev.product, "%u",
-				UNSTUFF_BITS(resp, 16, 32));
-			sprintf((char *)mmc_dev.revision, "N/A");
-			break;
-
-		default:
-			printf("MMC card has unknown MMCA version %d\n",
-			       spec_ver);
-			break;
-		}
-	}
-	printf("%s card.\nVendor: %s\nProduct: %s\nRevision: %s\n",
-	       (IF_TYPE_SD == mmc_dev.if_type) ? "SD" : "MMC", mmc_dev.vendor,
-	       mmc_dev.product, mmc_dev.revision);
-}
-
-/*
- * Given a 128-bit response, decode to our card CSD structure.
- */
-static void mmc_decode_csd(uint32_t * resp)
-{
-	unsigned int mult, csd_struct;
-
-	if (IF_TYPE_SD == mmc_dev.if_type) {
-		csd_struct = UNSTUFF_BITS(resp, 126, 2);
-		if (csd_struct != 0) {
-			printf("SD: unrecognised CSD structure version %d\n",
-			       csd_struct);
-			return;
-		}
-	} else {
-		/*
-		 * We only understand CSD structure v1.1 and v1.2.
-		 * v1.2 has extra information in bits 15, 11 and 10.
-		 */
-		csd_struct = UNSTUFF_BITS(resp, 126, 2);
-		if (csd_struct != 1 && csd_struct != 2) {
-			printf("MMC: unrecognised CSD structure version %d\n",
-			       csd_struct);
-			return;
-		}
-
-		spec_ver = UNSTUFF_BITS(resp, 122, 4);
-		mmc_dev.if_type = IF_TYPE_MMC;
-	}
-
-	mult = 1 << (UNSTUFF_BITS(resp, 47, 3) + 2);
-	mmc_dev.lba = (1 + UNSTUFF_BITS(resp, 62, 12)) * mult;
-	mmc_dev.blksz = 1 << UNSTUFF_BITS(resp, 80, 4);
-
-	/* FIXME: The following just makes assumes that's the partition type -- should really read it */
-	mmc_dev.part_type = PART_TYPE_DOS;
-	mmc_dev.dev = 0;
-	mmc_dev.lun = 0;
-	mmc_dev.type = DEV_TYPE_HARDDISK;
-	mmc_dev.removable = 0;
-	mmc_dev.block_read = mmc_bread;
-
-	printf("Detected: %lu blocks of %lu bytes (%luMB) ",
-		mmc_dev.lba,
-		mmc_dev.blksz,
-		mmc_dev.lba * mmc_dev.blksz / (1024 * 1024));
-}
-
-int
-/****************************************************/
-mmc_legacy_init(int verbose)
-/****************************************************/
-{
-	int retries, rc = -ENODEV;
-	uint32_t cid_resp[4];
-	uint32_t *resp;
-	uint16_t rca = 0;
-
-	/* Reset device interface type */
-	mmc_dev.if_type = IF_TYPE_UNKNOWN;
-
-#ifdef CONFIG_CPU_MONAHANS	/* pxa3xx */
-	writel(readl(CKENA) | CKENA_12_MMC0 | CKENA_13_MMC1, CKENA);
-#else	/* pxa2xx */
-	writel(readl(CKEN) | CKEN12_MMC, CKEN);	/* enable MMC unit clock */
-#endif
-	writel(MMC_CLKRT_0_3125MHZ, MMC_CLKRT);
-	writel(MMC_RES_TO_MAX, MMC_RESTO);
-	writel(MMC_SPI_DISABLE, MMC_SPI);
-
-	/* reset */
-	mmc_cmd(MMC_CMD_GO_IDLE_STATE, 0, 0, MMC_CMDAT_INIT | MMC_CMDAT_R0);
-	udelay(200000);
-	retries = 3;
-	while (retries--) {
-		resp = mmc_cmd(MMC_CMD_APP_CMD, 0, 0, MMC_CMDAT_R1);
-		if (!(resp[0] & 0x00000020)) {	/* Card does not support APP_CMD */
-			debug("Card does not support APP_CMD\n");
-			break;
-		}
-
-		/* Select 3.2-3.3V and 3.3-3.4V */
-		resp = mmc_cmd(SD_CMD_APP_SEND_OP_COND, 0x0030, 0x0000,
-				MMC_CMDAT_R3 | (retries < 2 ? 0
-					: MMC_CMDAT_INIT));
-		if (resp[0] & 0x80000000) {
-			mmc_dev.if_type = IF_TYPE_SD;
-			debug("Detected SD card\n");
-			break;
-		}
-		udelay(200000);
-	}
-
-	if (retries <= 0 || !(IF_TYPE_SD == mmc_dev.if_type)) {
-		debug("Failed to detect SD Card, trying MMC\n");
-		resp =
-		    mmc_cmd(MMC_CMD_SEND_OP_COND, 0x00ff, 0x8000, MMC_CMDAT_R3);
-
-		retries = 10;
-		while (retries-- && resp && !(resp[0] & 0x80000000)) {
-			udelay(200000);
-			resp =
-			    mmc_cmd(MMC_CMD_SEND_OP_COND, 0x00ff, 0x8000,
-				    MMC_CMDAT_R3);
-		}
-	}
-
-	/* try to get card id */
-	resp =
-	    mmc_cmd(MMC_CMD_ALL_SEND_CID, 0, 0, MMC_CMDAT_R2 | MMC_CMDAT_BUSY);
-	if (resp) {
-		memcpy(cid_resp, resp, sizeof(cid_resp));
-
-		/* MMC exists, get CSD too */
-		resp = mmc_cmd(MMC_CMD_SET_RELATIVE_ADDR, 0, 0, MMC_CMDAT_R1);
-		if (IF_TYPE_SD == mmc_dev.if_type)
-			rca = ((resp[0] & 0xffff0000) >> 16);
-		resp = mmc_cmd(MMC_CMD_SEND_CSD, rca, 0, MMC_CMDAT_R2);
-		if (resp) {
-			mmc_decode_csd(resp);
-			rc = 0;
-			mmc_ready = 1;
-		}
-
-		mmc_decode_cid(cid_resp);
-	}
-
-	writel(0, MMC_CLKRT);		/* 20 MHz */
-	resp = mmc_cmd(MMC_CMD_SELECT_CARD, rca, 0, MMC_CMDAT_R1);
-
-#if defined(CONFIG_CPU_PXA27X) || defined(CONFIG_CPU_MONAHANS)
-	if (IF_TYPE_SD == mmc_dev.if_type) {
-		resp = mmc_cmd(MMC_CMD_APP_CMD, rca, 0, MMC_CMDAT_R1);
-		resp = mmc_cmd(SD_CMD_APP_SET_BUS_WIDTH, 0, 2, MMC_CMDAT_R1);
-		wide = MMC_CMDAT_SD_4DAT;
-	}
-#endif
-
-	fat_register_device(&mmc_dev, 1);	/* partitions start counting with 1 */
-
-	return rc;
-}
diff --git a/drivers/mmc/s5p_sdhci.c b/drivers/mmc/s5p_sdhci.c
index b9782367e2..dc49d37f5c 100644
--- a/drivers/mmc/s5p_sdhci.c
+++ b/drivers/mmc/s5p_sdhci.c
@@ -83,7 +83,8 @@ int s5p_sdhci_init(u32 regbase, int index, int bus_width)
 	host->ioaddr = (void *)regbase;
 
 	host->quirks = SDHCI_QUIRK_NO_HISPD_BIT | SDHCI_QUIRK_BROKEN_VOLTAGE |
-		SDHCI_QUIRK_BROKEN_R1B | SDHCI_QUIRK_32BIT_DMA_ADDR;
+		SDHCI_QUIRK_BROKEN_R1B | SDHCI_QUIRK_32BIT_DMA_ADDR |
+		SDHCI_QUIRK_WAIT_SEND_CMD;
 	host->voltages = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
 	host->version = sdhci_readw(host, SDHCI_HOST_VERSION);
 
diff --git a/drivers/mmc/sdhci.c b/drivers/mmc/sdhci.c
index 2e3c408bc5..7845f873ac 100644
--- a/drivers/mmc/sdhci.c
+++ b/drivers/mmc/sdhci.c
@@ -82,8 +82,15 @@ static int sdhci_transfer_data(struct sdhci_host *host, struct mmc_data *data,
 				unsigned int start_addr)
 {
 	unsigned int stat, rdy, mask, timeout, block = 0;
+#ifdef CONFIG_MMC_SDMA
+	unsigned char ctrl;
+	ctrl = sdhci_readl(host, SDHCI_HOST_CONTROL);
+	ctrl &= ~SDHCI_CTRL_DMA_MASK;
+	ctrl |= SDHCI_CTRL_SDMA;
+	sdhci_writel(host, ctrl, SDHCI_HOST_CONTROL);
+#endif
 
-	timeout = 10000;
+	timeout = 1000000;
 	rdy = SDHCI_INT_SPACE_AVAIL | SDHCI_INT_DATA_AVAIL;
 	mask = SDHCI_DATA_AVAILABLE | SDHCI_SPACE_AVAILABLE;
 	do {
@@ -233,6 +240,9 @@ int sdhci_send_command(struct mmc *mmc, struct mmc_cmd *cmd,
 	if (!ret && data)
 		ret = sdhci_transfer_data(host, data, start_addr);
 
+	if (host->quirks & SDHCI_QUIRK_WAIT_SEND_CMD)
+		udelay(1000);
+
 	stat = sdhci_readl(host, SDHCI_INT_STATUS);
 	sdhci_writel(host, SDHCI_INT_ALL_MASK, SDHCI_INT_STATUS);
 	if (!ret) {
diff --git a/drivers/mtd/cfi_flash.c b/drivers/mtd/cfi_flash.c
index 43140f3647..b2dfc5369d 100644
--- a/drivers/mtd/cfi_flash.c
+++ b/drivers/mtd/cfi_flash.c
@@ -752,8 +752,8 @@ static void flash_add_byte (flash_info_t * info, cfiword_t * cword, uchar c)
  */
 static flash_sect_t find_sector (flash_info_t * info, ulong addr)
 {
-	static flash_sect_t saved_sector = 0; /* previously found sector */
-	static flash_info_t *saved_info = 0; /* previously used flash bank */
+	static flash_sect_t saved_sector; /* previously found sector */
+	static flash_info_t *saved_info; /* previously used flash bank */
 	flash_sect_t sector = saved_sector;
 
 	if ((info != saved_info) || (sector >= info->sector_count))
@@ -1147,8 +1147,9 @@ int flash_erase (flash_info_t * info, int s_first, int s_last)
 			}
 
 			if (use_flash_status_poll(info)) {
-				cfiword_t cword = (cfiword_t)0xffffffffffffffffULL;
+				cfiword_t cword;
 				void *dest;
+				cword.ll = 0xffffffffffffffffULL;
 				dest = flash_map(info, sect, 0);
 				st = flash_status_poll(info, &cword, dest,
 						       info->erase_blk_tout, "erase");
@@ -1430,8 +1431,8 @@ int write_buff (flash_info_t * info, uchar * src, ulong addr, ulong cnt)
 
 static int cfi_protect_bugfix(flash_info_t *info, long sector, int prot)
 {
-	if ((info->manufacturer_id == (uchar)INTEL_MANUFACT) &&
-		(info->device_id == NUMONYX_256MBIT)) {
+	if (info->manufacturer_id == ((INTEL_MANUFACT & FLASH_VENDMASK) >> 16)
+	    && info->device_id == NUMONYX_256MBIT) {
 		/*
 		 * see errata called
 		 * "Numonyx Axcell P33/P30 Specification Update" :)
@@ -1487,7 +1488,8 @@ int flash_real_protect (flash_info_t * info, long sector, int prot)
 		case CFI_CMDSET_AMD_EXTENDED:
 		case CFI_CMDSET_AMD_STANDARD:
 			/* U-Boot only checks the first byte */
-			if (info->manufacturer_id == (uchar)ATM_MANUFACT) {
+			if (info->manufacturer_id ==
+			    ((ATM_MANUFACT & FLASH_VENDMASK) >> 16)) {
 				if (prot) {
 					flash_unlock_seq (info, 0);
 					flash_write_cmd (info, 0,
@@ -1505,7 +1507,8 @@ int flash_real_protect (flash_info_t * info, long sector, int prot)
 							0, ATM_CMD_UNLOCK_SECT);
 				}
 			}
-			if (info->manufacturer_id == (uchar)AMD_MANUFACT) {
+			if (info->manufacturer_id ==
+			    ((AMD_MANUFACT & FLASH_VENDMASK) >> 16)) {
 				int flag = disable_interrupts();
 				int lock_flag;
 
@@ -1735,7 +1738,8 @@ static int cmdset_amd_init(flash_info_t *info, struct cfi_qry *qry)
 	flash_write_cmd(info, 0, info->cfi_offset, FLASH_CMD_CFI);
 
 #ifdef CONFIG_SYS_FLASH_PROTECTION
-	if (info->ext_addr && info->manufacturer_id == (uchar)AMD_MANUFACT) {
+	if (info->ext_addr && info->manufacturer_id ==
+	    ((AMD_MANUFACT & FLASH_VENDMASK) >> 16)) {
 		ushort spus;
 
 		/* read sector protect/unprotect scheme */
@@ -1854,7 +1858,7 @@ static void flash_read_cfi (flash_info_t *info, void *buf,
 		p[i] = flash_read_uchar(info, start + i);
 }
 
-void __flash_cmd_reset(flash_info_t *info)
+static void __flash_cmd_reset(flash_info_t *info)
 {
 	/*
 	 * We do not yet know what kind of commandset to use, so we issue
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 71f5027889..d3b71a50ad 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -2601,6 +2601,7 @@ static const struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 						  int *maf_id, int *dev_id,
 						  const struct nand_flash_dev *type)
 {
+	const char *name;
 	int i, maf_idx;
 	u8 id_data[8];
 	int ret;
@@ -2848,14 +2849,14 @@ ident_done:
 		chip->cmdfunc = nand_command_lp;
 
 	/* TODO onfi flash name */
-	MTDDEBUG (MTD_DEBUG_LEVEL0, "NAND device: Manufacturer ID:"
-		" 0x%02x, Chip ID: 0x%02x (%s %s)\n", *maf_id, *dev_id,
-		nand_manuf_ids[maf_idx].name,
+	name = type->name;
 #ifdef CONFIG_SYS_NAND_ONFI_DETECTION
-		chip->onfi_version ? chip->onfi_params.model : type->name);
-#else
-		type->name);
+	if (chip->onfi_version)
+		name = chip->onfi_params.model;
 #endif
+	MTDDEBUG(MTD_DEBUG_LEVEL0, "NAND device: Manufacturer ID:"
+		 " 0x%02x, Chip ID: 0x%02x (%s %s)\n", *maf_id, *dev_id,
+		 nand_manuf_ids[maf_idx].name, name);
 
 	return type;
 }
diff --git a/drivers/mtd/nand/nand_ecc.c b/drivers/mtd/nand/nand_ecc.c
index 81f0e0812b..097cf625b8 100644
--- a/drivers/mtd/nand/nand_ecc.c
+++ b/drivers/mtd/nand/nand_ecc.c
@@ -39,6 +39,7 @@
 
 #include <asm/errno.h>
 #include <linux/mtd/mtd.h>
+#include <linux/mtd/nand_ecc.h>
 
 /* The PPC4xx NDFC uses Smart Media (SMC) bytes order */
 #ifdef CONFIG_NAND_NDFC
diff --git a/drivers/mtd/nand/tegra_nand.c b/drivers/mtd/nand/tegra_nand.c
index 5408c51ffb..4d94cc6f56 100644
--- a/drivers/mtd/nand/tegra_nand.c
+++ b/drivers/mtd/nand/tegra_nand.c
@@ -219,6 +219,34 @@ static uint8_t read_byte(struct mtd_info *mtd)
 }
 
 /**
+ * Read len bytes from the chip into a buffer
+ *
+ * @param mtd	MTD device structure
+ * @param buf	buffer to store data to
+ * @param len	number of bytes to read
+ *
+ * Read function for 8bit bus-width
+ */
+static void read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
+{
+	int i, s;
+	unsigned int reg;
+	struct nand_chip *chip = mtd->priv;
+	struct nand_drv *info = (struct nand_drv *)chip->priv;
+
+	for (i = 0; i < len; i += 4) {
+		s = (len - i) > 4 ? 4 : len - i;
+		writel(CMD_PIO | CMD_RX | CMD_A_VALID | CMD_CE0 |
+			((s - 1) << CMD_TRANS_SIZE_SHIFT) | CMD_GO,
+			&info->reg->command);
+		if (!nand_waitfor_cmd_completion(info->reg))
+			puts("Command timeout during read_buf\n");
+		reg = readl(&info->reg->resp);
+		memcpy(buf + i, &reg, s);
+	}
+}
+
+/**
  * Check NAND status to see if it is ready or not
  *
  * @param mtd	MTD device structure
@@ -317,6 +345,7 @@ static void nand_command(struct mtd_info *mtd, unsigned int command,
 	switch (command) {
 	case NAND_CMD_READID:
 		writel(NAND_CMD_READID, &info->reg->cmd_reg1);
+		writel(column & 0xFF, &info->reg->addr_reg1);
 		writel(CMD_GO | CMD_CLE | CMD_ALE | CMD_PIO
 			| CMD_RX |
 			((4 - 1) << CMD_TRANS_SIZE_SHIFT)
@@ -324,6 +353,12 @@ static void nand_command(struct mtd_info *mtd, unsigned int command,
 			&info->reg->command);
 		info->pio_byte_index = 0;
 		break;
+	case NAND_CMD_PARAM:
+		writel(NAND_CMD_PARAM, &info->reg->cmd_reg1);
+		writel(column & 0xFF, &info->reg->addr_reg1);
+		writel(CMD_GO | CMD_CLE | CMD_ALE | CMD_CE0,
+			&info->reg->command);
+		break;
 	case NAND_CMD_READ0:
 		writel(NAND_CMD_READ0, &info->reg->cmd_reg1);
 		writel(NAND_CMD_READSTART, &info->reg->cmd_reg2);
@@ -976,6 +1011,7 @@ int tegra_nand_init(struct nand_chip *nand, int devnum)
 	nand->options = LP_OPTIONS;
 	nand->cmdfunc = nand_command;
 	nand->read_byte = read_byte;
+	nand->read_buf = read_buf;
 	nand->ecc.read_page = nand_read_page_hwecc;
 	nand->ecc.write_page = nand_write_page_hwecc;
 	nand->ecc.read_page_raw = nand_read_page_raw;
diff --git a/drivers/mtd/ubi/crc32.c b/drivers/mtd/ubi/crc32.c
index a7e26b0456..ab439b3e20 100644
--- a/drivers/mtd/ubi/crc32.c
+++ b/drivers/mtd/ubi/crc32.c
@@ -38,17 +38,9 @@
 #include "crc32defs.h"
 #define CRC_LE_BITS 8
 
-# define __force
-#ifndef __constant_cpu_to_le32
-#define __constant_cpu_to_le32(x) ((__force __le32)(__u32)(x))
-#endif
-#ifndef __constant_le32_to_cpu
-#define __constant_le32_to_cpu(x) ((__force __u32)(__le32)(x))
-#endif
-
 #if CRC_LE_BITS == 8
-#define tole(x) __constant_cpu_to_le32(x)
-#define tobe(x) __constant_cpu_to_be32(x)
+#define tole(x) cpu_to_le32(x)
+#define tobe(x) cpu_to_be32(x)
 #else
 #define tole(x) (x)
 #define tobe(x) (x)
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index e4abac7c8f..786a6567a5 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -59,7 +59,6 @@ COBJS-$(CONFIG_MVGBE) += mvgbe.o
 COBJS-$(CONFIG_NATSEMI) += natsemi.o
 COBJS-$(CONFIG_DRIVER_NE2000) += ne2000.o ne2000_base.o
 COBJS-$(CONFIG_DRIVER_AX88796L) += ax88796.o ne2000_base.o
-COBJS-$(CONFIG_DRIVER_NETARMETH) += netarm_eth.o
 COBJS-$(CONFIG_NETCONSOLE) += netconsole.o
 COBJS-$(CONFIG_NS8382X) += ns8382x.o
 COBJS-$(CONFIG_PCNET) += pcnet.o
diff --git a/drivers/net/fm/Makefile b/drivers/net/fm/Makefile
index cc57354755..7a1fcdd81f 100644
--- a/drivers/net/fm/Makefile
+++ b/drivers/net/fm/Makefile
@@ -32,6 +32,10 @@ COBJS-y += init.o
 COBJS-y += tgec.o
 COBJS-y += tgec_phy.o
 
+# Soc have FMAN v3 with mEMAC
+COBJS-$(CONFIG_SYS_FMAN_V3) += memac_phy.o
+COBJS-$(CONFIG_SYS_FMAN_V3) += memac.o
+
 # SoC specific SERDES support
 COBJS-$(CONFIG_P1017)	+= p1023.o
 COBJS-$(CONFIG_P1023)	+= p1023.o
@@ -40,6 +44,8 @@ COBJS-$(CONFIG_PPC_P2041) += p5020.o
 COBJS-$(CONFIG_PPC_P3041) += p5020.o
 COBJS-$(CONFIG_PPC_P4080) += p4080.o
 COBJS-$(CONFIG_PPC_P5020) += p5020.o
+COBJS-$(CONFIG_PPC_T4240) += t4240.o
+COBJS-$(CONFIG_PPC_B4860) += b4860.o
 endif
 
 COBJS	:= $(COBJS-y)
diff --git a/drivers/net/fm/eth.c b/drivers/net/fm/eth.c
index 2b616adb6e..54b142f47d 100644
--- a/drivers/net/fm/eth.c
+++ b/drivers/net/fm/eth.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2011 Freescale Semiconductor, Inc.
+ * Copyright 2009-2012 Freescale Semiconductor, Inc.
  *	Dave Liu <daveliu@freescale.com>
  *
  * This program is free software; you can redistribute it and/or
@@ -28,6 +28,7 @@
 #include <phy.h>
 #include <asm/fsl_dtsec.h>
 #include <asm/fsl_tgec.h>
+#include <asm/fsl_memac.h>
 
 #include "fm.h"
 
@@ -45,8 +46,30 @@ static int num_controllers;
 			TBICR_FULL_DUPLEX | TBICR_SPEED1_SET)
 
 /* Configure the TBI for SGMII operation */
-void dtsec_configure_serdes(struct fm_eth *priv)
+static void dtsec_configure_serdes(struct fm_eth *priv)
 {
+#ifdef CONFIG_SYS_FMAN_V3
+	u32 value;
+	struct mii_dev bus;
+	bus.priv = priv->mac->phyregs;
+
+	/* SGMII IF mode + AN enable */
+	value = PHY_SGMII_IF_MODE_AN | PHY_SGMII_IF_MODE_SGMII;
+	memac_mdio_write(&bus, 0, MDIO_DEVAD_NONE, 0x14, value);
+
+	/* Dev ability according to SGMII specification */
+	value = PHY_SGMII_DEV_ABILITY_SGMII;
+	memac_mdio_write(&bus, 0, MDIO_DEVAD_NONE, 0x4, value);
+
+	/* Adjust link timer for SGMII  -
+	1.6 ms in units of 8 ns = 2 * 10^5 = 0x30d40 */
+	memac_mdio_write(&bus, 0, MDIO_DEVAD_NONE, 0x13, 0x3);
+	memac_mdio_write(&bus, 0, MDIO_DEVAD_NONE, 0x12, 0xd40);
+
+	/* Restart AN */
+	value = PHY_SGMII_CR_DEF_VAL | PHY_SGMII_CR_RESET_AN;
+	memac_mdio_write(&bus, 0, MDIO_DEVAD_NONE, 0, value);
+#else
 	struct dtsec *regs = priv->mac->base;
 	struct tsec_mii_mng *phyregs = priv->mac->phyregs;
 
@@ -60,15 +83,18 @@ void dtsec_configure_serdes(struct fm_eth *priv)
 			TBIANA_SGMII_ACK);
 	tsec_local_mdio_write(phyregs, in_be32(&regs->tbipa), 0,
 			TBI_CR, TBICR_SETTINGS);
+#endif
 }
 
 static void dtsec_init_phy(struct eth_device *dev)
 {
 	struct fm_eth *fm_eth = dev->priv;
-	struct dtsec *regs = (struct dtsec *)fm_eth->mac->base;
+#ifndef CONFIG_SYS_FMAN_V3
+	struct dtsec *regs = (struct dtsec *)CONFIG_SYS_FSL_FM1_DTSEC1_ADDR;
 
 	/* Assign a Physical address to the TBI */
 	out_be32(&regs->tbipa, CONFIG_SYS_TBIPA_VALUE);
+#endif
 
 	if (fm_eth->enet_if == PHY_INTERFACE_MODE_SGMII)
 		dtsec_configure_serdes(fm_eth);
@@ -541,6 +567,10 @@ static int fm_eth_init_mac(struct fm_eth *fm_eth, struct ccsr_fman *reg)
 
 	num = fm_eth->num;
 
+#ifdef CONFIG_SYS_FMAN_V3
+	base = &reg->memac[num].fm_memac;
+	phyregs = &reg->memac[num].fm_memac_mdio;
+#else
 	/* Get the mac registers base address */
 	if (fm_eth->type == FM_ETH_1G_E) {
 		base = &reg->mac_1g[num].fm_dtesc;
@@ -549,6 +579,7 @@ static int fm_eth_init_mac(struct fm_eth *fm_eth, struct ccsr_fman *reg)
 		base = &reg->mac_10g[num].fm_10gec;
 		phyregs = &reg->mac_10g[num].fm_10gec_mdio;
 	}
+#endif
 
 	/* alloc mac controller */
 	mac = malloc(sizeof(struct fsl_enet_mac));
@@ -559,10 +590,14 @@ static int fm_eth_init_mac(struct fm_eth *fm_eth, struct ccsr_fman *reg)
 	/* save the mac to fm_eth struct */
 	fm_eth->mac = mac;
 
+#ifdef CONFIG_SYS_FMAN_V3
+	init_memac(mac, base, phyregs, MAX_RXBUF_LEN);
+#else
 	if (fm_eth->type == FM_ETH_1G_E)
 		init_dtsec(mac, base, phyregs, MAX_RXBUF_LEN);
 	else
 		init_tgec(mac, base, phyregs, MAX_RXBUF_LEN);
+#endif
 
 	return 1;
 }
diff --git a/drivers/net/fm/init.c b/drivers/net/fm/init.c
index 736b8b9582..ae389b8842 100644
--- a/drivers/net/fm/init.c
+++ b/drivers/net/fm/init.c
@@ -38,6 +38,15 @@ struct fm_eth_info fm_info[] = {
 #if (CONFIG_SYS_NUM_FM1_DTSEC >= 5)
 	FM_DTSEC_INFO_INITIALIZER(1, 5),
 #endif
+#if (CONFIG_SYS_NUM_FM1_DTSEC >= 6)
+	FM_DTSEC_INFO_INITIALIZER(1, 6),
+#endif
+#if (CONFIG_SYS_NUM_FM1_DTSEC >= 7)
+	FM_DTSEC_INFO_INITIALIZER(1, 9),
+#endif
+#if (CONFIG_SYS_NUM_FM1_DTSEC >= 8)
+	FM_DTSEC_INFO_INITIALIZER(1, 10),
+#endif
 #if (CONFIG_SYS_NUM_FM2_DTSEC >= 1)
 	FM_DTSEC_INFO_INITIALIZER(2, 1),
 #endif
@@ -53,6 +62,15 @@ struct fm_eth_info fm_info[] = {
 #if (CONFIG_SYS_NUM_FM2_DTSEC >= 5)
 	FM_DTSEC_INFO_INITIALIZER(2, 5),
 #endif
+#if (CONFIG_SYS_NUM_FM2_DTSEC >= 6)
+	FM_DTSEC_INFO_INITIALIZER(2, 6),
+#endif
+#if (CONFIG_SYS_NUM_FM2_DTSEC >= 7)
+	FM_DTSEC_INFO_INITIALIZER(2, 9),
+#endif
+#if (CONFIG_SYS_NUM_FM2_DTSEC >= 8)
+	FM_DTSEC_INFO_INITIALIZER(2, 10),
+#endif
 #if (CONFIG_SYS_NUM_FM1_10GEC >= 1)
 	FM_TGEC_INFO_INITIALIZER(1, 1),
 #endif
diff --git a/drivers/net/fm/memac.c b/drivers/net/fm/memac.c
new file mode 100644
index 0000000000..32c7054e35
--- /dev/null
+++ b/drivers/net/fm/memac.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *	Roy Zang <tie-fei.zang@freescale.com>
+ *
+ * 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
+ */
+
+/* MAXFRM - maximum frame length */
+#define MAXFRM_MASK	0x0000ffff
+
+#include <common.h>
+#include <phy.h>
+#include <asm/types.h>
+#include <asm/io.h>
+#include <asm/fsl_enet.h>
+#include <asm/fsl_memac.h>
+
+#include "fm.h"
+
+static void memac_init_mac(struct fsl_enet_mac *mac)
+{
+	struct memac *regs = mac->base;
+
+	/* mask all interrupt */
+	out_be32(&regs->imask, IMASK_MASK_ALL);
+
+	/* clear all events */
+	out_be32(&regs->ievent, IEVENT_CLEAR_ALL);
+
+	/* set the max receive length */
+	out_be32(&regs->maxfrm, mac->max_rx_len & MAXFRM_MASK);
+
+	/* multicast frame reception for the hash entry disable */
+	out_be32(&regs->hashtable_ctrl, 0);
+}
+
+static void memac_enable_mac(struct fsl_enet_mac *mac)
+{
+	struct memac *regs = mac->base;
+
+	setbits_be32(&regs->command_config, MEMAC_CMD_CFG_RXTX_EN);
+}
+
+static void memac_disable_mac(struct fsl_enet_mac *mac)
+{
+	struct memac *regs = mac->base;
+
+	clrbits_be32(&regs->command_config, MEMAC_CMD_CFG_RXTX_EN);
+}
+
+static void memac_set_mac_addr(struct fsl_enet_mac *mac, u8 *mac_addr)
+{
+	struct memac *regs = mac->base;
+	u32 mac_addr0, mac_addr1;
+
+	/*
+	 * if a station address of 0x12345678ABCD, perform a write to
+	 * MAC_ADDR0 of 0x78563412, MAC_ADDR1 of 0x0000CDAB
+	 */
+	mac_addr0 = (mac_addr[3] << 24) | (mac_addr[2] << 16) | \
+			(mac_addr[1] << 8)  | (mac_addr[0]);
+	out_be32(&regs->mac_addr_0, mac_addr0);
+
+	mac_addr1 = ((mac_addr[5] << 8) | mac_addr[4]) & 0x0000ffff;
+	out_be32(&regs->mac_addr_1, mac_addr1);
+}
+
+static void memac_set_interface_mode(struct fsl_enet_mac *mac,
+					phy_interface_t type, int speed)
+{
+	/* Roy need more work here */
+
+	struct memac *regs = mac->base;
+	u32 if_mode, if_status;
+
+	/* clear all bits relative with interface mode */
+	if_mode = in_be32(&regs->if_mode);
+	if_status = in_be32(&regs->if_status);
+
+	/* set interface mode */
+	switch (type) {
+	case PHY_INTERFACE_MODE_GMII:
+		if_mode &= ~IF_MODE_MASK;
+		if_mode |= IF_MODE_GMII;
+		break;
+	case PHY_INTERFACE_MODE_RGMII:
+		if_mode |= (IF_MODE_GMII | IF_MODE_RG);
+		break;
+	case PHY_INTERFACE_MODE_RMII:
+		if_mode |= (IF_MODE_GMII | IF_MODE_RM);
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		if_mode &= ~IF_MODE_MASK;
+		if_mode |= (IF_MODE_GMII);
+		break;
+	default:
+		break;
+	}
+	/* Enable automatic speed selection */
+	if_mode |= IF_MODE_EN_AUTO;
+
+	debug(" %s, if_mode = %x\n", __func__,  if_mode);
+	debug(" %s, if_status = %x\n", __func__,  if_status);
+	out_be32(&regs->if_mode, if_mode);
+	return;
+}
+
+void init_memac(struct fsl_enet_mac *mac, void *base,
+		void *phyregs, int max_rx_len)
+{
+	mac->base = base;
+	mac->phyregs = phyregs;
+	mac->max_rx_len = max_rx_len;
+	mac->init_mac = memac_init_mac;
+	mac->enable_mac = memac_enable_mac;
+	mac->disable_mac = memac_disable_mac;
+	mac->set_mac_addr = memac_set_mac_addr;
+	mac->set_if_mode = memac_set_interface_mode;
+}
diff --git a/drivers/net/fm/memac_phy.c b/drivers/net/fm/memac_phy.c
new file mode 100644
index 0000000000..ea6118b9b9
--- /dev/null
+++ b/drivers/net/fm/memac_phy.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *	Andy Fleming <afleming@freescale.com>
+ *	Roy Zang <tie-fei.zang@freescale.com>
+ *
+ * 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 part is taken from tsec.c
+ */
+#include <common.h>
+#include <miiphy.h>
+#include <phy.h>
+#include <asm/io.h>
+#include <asm/fsl_memac.h>
+#include <fm_eth.h>
+
+/*
+ * Write value to the PHY for this device to the register at regnum, waiting
+ * until the write is done before it returns.  All PHY configuration has to be
+ * done through the TSEC1 MIIM regs
+ */
+int memac_mdio_write(struct mii_dev *bus, int port_addr, int dev_addr,
+			int regnum, u16 value)
+{
+	u32 mdio_ctl;
+	struct memac_mdio_controller *regs = bus->priv;
+	u32 c45 = 1; /* Default to 10G interface */
+
+	if (dev_addr == MDIO_DEVAD_NONE) {
+		c45 = 0; /* clause 22 */
+		dev_addr = regnum & 0x1f;
+		clrbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+	} else {
+		setbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+		setbits_be32(&regs->mdio_stat, MDIO_STAT_HOLD_15_CLK);
+	}
+
+	/* Wait till the bus is free */
+	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+		;
+
+	/* Set the port and dev addr */
+	mdio_ctl = MDIO_CTL_PORT_ADDR(port_addr) | MDIO_CTL_DEV_ADDR(dev_addr);
+	out_be32(&regs->mdio_ctl, mdio_ctl);
+
+	/* Set the register address */
+	if (c45)
+		out_be32(&regs->mdio_addr, regnum & 0xffff);
+
+	/* Wait till the bus is free */
+	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+		;
+
+	/* Write the value to the register */
+	out_be32(&regs->mdio_data, MDIO_DATA(value));
+
+	/* Wait till the MDIO write is complete */
+	while ((in_be32(&regs->mdio_data)) & MDIO_DATA_BSY)
+		;
+
+	return 0;
+}
+
+/*
+ * Reads from register regnum in the PHY for device dev, returning the value.
+ * Clears miimcom first.  All PHY configuration has to be done through the
+ * TSEC1 MIIM regs
+ */
+int memac_mdio_read(struct mii_dev *bus, int port_addr, int dev_addr,
+			int regnum)
+{
+	u32 mdio_ctl;
+	struct memac_mdio_controller *regs = bus->priv;
+	u32 c45 = 1;
+
+	if (dev_addr == MDIO_DEVAD_NONE) {
+		c45 = 0; /* clause 22 */
+		dev_addr = regnum & 0x1f;
+		clrbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+	} else {
+		setbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+		setbits_be32(&regs->mdio_stat, MDIO_STAT_HOLD_15_CLK);
+	}
+
+	/* Wait till the bus is free */
+	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+		;
+
+	/* Set the Port and Device Addrs */
+	mdio_ctl = MDIO_CTL_PORT_ADDR(port_addr) | MDIO_CTL_DEV_ADDR(dev_addr);
+	out_be32(&regs->mdio_ctl, mdio_ctl);
+
+	/* Set the register address */
+	if (c45)
+		out_be32(&regs->mdio_addr, regnum & 0xffff);
+
+	/* Wait till the bus is free */
+	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+		;
+
+	/* Initiate the read */
+	mdio_ctl |= MDIO_CTL_READ;
+	out_be32(&regs->mdio_ctl, mdio_ctl);
+
+	/* Wait till the MDIO write is complete */
+	while ((in_be32(&regs->mdio_data)) & MDIO_DATA_BSY)
+		;
+
+	/* Return all Fs if nothing was there */
+	if (in_be32(&regs->mdio_stat) & MDIO_STAT_RD_ER)
+		return 0xffff;
+
+	return in_be32(&regs->mdio_data) & 0xffff;
+}
+
+int memac_mdio_reset(struct mii_dev *bus)
+{
+	return 0;
+}
+
+int fm_memac_mdio_init(bd_t *bis, struct memac_mdio_info *info)
+{
+	struct mii_dev *bus = mdio_alloc();
+
+	if (!bus) {
+		printf("Failed to allocate FM TGEC MDIO bus\n");
+		return -1;
+	}
+
+	bus->read = memac_mdio_read;
+	bus->write = memac_mdio_write;
+	bus->reset = memac_mdio_reset;
+	sprintf(bus->name, info->name);
+
+	bus->priv = info->regs;
+
+	return mdio_register(bus);
+}
diff --git a/drivers/net/fm/p1023.c b/drivers/net/fm/p1023.c
index 9765da5b23..387d2a382d 100644
--- a/drivers/net/fm/p1023.c
+++ b/drivers/net/fm/p1023.c
@@ -23,7 +23,7 @@
 #include <asm/immap_85xx.h>
 #include <asm/fsl_serdes.h>
 
-u32 port_to_devdisr[] = {
+static u32 port_to_devdisr[] = {
 	[FM1_DTSEC1] = MPC85xx_DEVDISR_TSEC1,
 	[FM1_DTSEC2] = MPC85xx_DEVDISR_TSEC2,
 };
diff --git a/drivers/net/fm/p4080.c b/drivers/net/fm/p4080.c
index 9dc6049c36..b04fd0f656 100644
--- a/drivers/net/fm/p4080.c
+++ b/drivers/net/fm/p4080.c
@@ -23,7 +23,7 @@
 #include <asm/immap_85xx.h>
 #include <asm/fsl_serdes.h>
 
-u32 port_to_devdisr[] = {
+static u32 port_to_devdisr[] = {
 	[FM1_DTSEC1] = FSL_CORENET_DEVDISR2_DTSEC1_1,
 	[FM1_DTSEC2] = FSL_CORENET_DEVDISR2_DTSEC1_2,
 	[FM1_DTSEC3] = FSL_CORENET_DEVDISR2_DTSEC1_3,
diff --git a/drivers/net/fm/p5020.c b/drivers/net/fm/p5020.c
index a7a6e43fe2..5391044833 100644
--- a/drivers/net/fm/p5020.c
+++ b/drivers/net/fm/p5020.c
@@ -23,7 +23,7 @@
 #include <asm/immap_85xx.h>
 #include <asm/fsl_serdes.h>
 
-u32 port_to_devdisr[] = {
+static u32 port_to_devdisr[] = {
 	[FM1_DTSEC1] = FSL_CORENET_DEVDISR2_DTSEC1_1,
 	[FM1_DTSEC2] = FSL_CORENET_DEVDISR2_DTSEC1_2,
 	[FM1_DTSEC3] = FSL_CORENET_DEVDISR2_DTSEC1_3,
diff --git a/drivers/net/fm/t4240.c b/drivers/net/fm/t4240.c
new file mode 100644
index 0000000000..48c530c915
--- /dev/null
+++ b/drivers/net/fm/t4240.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *	Roy Zang <tie-fei.zang@freescale.com>
+ *
+ * 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
+ */
+#include <common.h>
+#include <phy.h>
+#include <fm_eth.h>
+#include <asm/io.h>
+#include <asm/immap_85xx.h>
+#include <asm/fsl_serdes.h>
+
+u32 port_to_devdisr[] = {
+	[FM1_DTSEC1] = FSL_CORENET_DEVDISR2_DTSEC1_1,
+	[FM1_DTSEC2] = FSL_CORENET_DEVDISR2_DTSEC1_2,
+	[FM1_DTSEC3] = FSL_CORENET_DEVDISR2_DTSEC1_3,
+	[FM1_DTSEC4] = FSL_CORENET_DEVDISR2_DTSEC1_4,
+	[FM1_DTSEC5] = FSL_CORENET_DEVDISR2_DTSEC1_5,
+	[FM1_DTSEC6] = FSL_CORENET_DEVDISR2_DTSEC1_6,
+	[FM1_DTSEC9] = FSL_CORENET_DEVDISR2_DTSEC1_9,
+	[FM1_DTSEC10] = FSL_CORENET_DEVDISR2_DTSEC1_10,
+	[FM1_10GEC1] = FSL_CORENET_DEVDISR2_10GEC1_1,
+	[FM1_10GEC2] = FSL_CORENET_DEVDISR2_10GEC1_2,
+	[FM2_DTSEC1] = FSL_CORENET_DEVDISR2_DTSEC2_1,
+	[FM2_DTSEC2] = FSL_CORENET_DEVDISR2_DTSEC2_2,
+	[FM2_DTSEC3] = FSL_CORENET_DEVDISR2_DTSEC2_3,
+	[FM2_DTSEC4] = FSL_CORENET_DEVDISR2_DTSEC2_4,
+	[FM2_DTSEC5] = FSL_CORENET_DEVDISR2_DTSEC2_5,
+	[FM2_DTSEC6] = FSL_CORENET_DEVDISR2_DTSEC2_6,
+	[FM2_DTSEC9] = FSL_CORENET_DEVDISR2_DTSEC2_9,
+	[FM2_DTSEC10] = FSL_CORENET_DEVDISR2_DTSEC2_10,
+	[FM2_10GEC1] = FSL_CORENET_DEVDISR2_10GEC2_1,
+	[FM2_10GEC2] = FSL_CORENET_DEVDISR2_10GEC2_2,
+};
+
+static int is_device_disabled(enum fm_port port)
+{
+	ccsr_gur_t *gur = (void __iomem *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+	u32 devdisr2 = in_be32(&gur->devdisr2);
+
+	return port_to_devdisr[port] & devdisr2;
+}
+
+void fman_disable_port(enum fm_port port)
+{
+	ccsr_gur_t *gur = (void __iomem *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+
+	setbits_be32(&gur->devdisr2, port_to_devdisr[port]);
+}
+
+phy_interface_t fman_port_enet_if(enum fm_port port)
+{
+	ccsr_gur_t *gur = (void __iomem *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+	u32 rcwsr13 = in_be32(&gur->rcwsr[13]);
+
+	if (is_device_disabled(port))
+		return PHY_INTERFACE_MODE_NONE;
+
+	if ((port == FM1_10GEC1 || port == FM1_10GEC2)
+			&& (is_serdes_configured(XAUI_FM1)))
+		return PHY_INTERFACE_MODE_XGMII;
+
+	if ((port == FM2_10GEC1 || port == FM2_10GEC2)
+			&& (is_serdes_configured(XAUI_FM2)))
+		return PHY_INTERFACE_MODE_XGMII;
+
+#define FSL_CORENET_RCWSR13_EC1			0x60000000 /* bits 417..418 */
+#define FSL_CORENET_RCWSR13_EC1_FM2_DTSEC5_RGMII	0x00000000
+#define FSL_CORENET_RCWSR13_EC1_FM2_GPIO		0x40000000
+#define FSL_CORENET_RCWSR13_EC2			0x18000000 /* bits 419..420 */
+#define FSL_CORENET_RCWSR13_EC2_FM1_DTSEC5_RGMII	0x00000000
+#define FSL_CORENET_RCWSR13_EC2_FM2_DTSEC6_RGMII	0x08000000
+#define FSL_CORENET_RCWSR13_EC2_FM1_GPIO		0x10000000
+	/* handle RGMII first */
+	if ((port == FM2_DTSEC5) && ((rcwsr13 & FSL_CORENET_RCWSR13_EC1) ==
+		FSL_CORENET_RCWSR13_EC1_FM2_DTSEC5_RGMII))
+		return PHY_INTERFACE_MODE_RGMII;
+
+	if ((port == FM1_DTSEC5) && ((rcwsr13 & FSL_CORENET_RCWSR13_EC2) ==
+		FSL_CORENET_RCWSR13_EC2_FM1_DTSEC5_RGMII))
+		return PHY_INTERFACE_MODE_RGMII;
+
+	if ((port == FM2_DTSEC6) && ((rcwsr13 & FSL_CORENET_RCWSR13_EC2) ==
+		FSL_CORENET_RCWSR13_EC2_FM2_DTSEC6_RGMII))
+		return PHY_INTERFACE_MODE_RGMII;
+	switch (port) {
+	case FM1_DTSEC1:
+	case FM1_DTSEC2:
+	case FM1_DTSEC3:
+	case FM1_DTSEC4:
+	case FM1_DTSEC5:
+	case FM1_DTSEC6:
+	case FM1_DTSEC9:
+	case FM1_DTSEC10:
+		if (is_serdes_configured(SGMII_FM1_DTSEC1 + port - FM1_DTSEC1))
+			return PHY_INTERFACE_MODE_SGMII;
+		break;
+	case FM2_DTSEC1:
+	case FM2_DTSEC2:
+	case FM2_DTSEC3:
+	case FM2_DTSEC4:
+	case FM2_DTSEC5:
+	case FM2_DTSEC6:
+	case FM2_DTSEC9:
+	case FM2_DTSEC10:
+		if (is_serdes_configured(SGMII_FM2_DTSEC1 + port - FM2_DTSEC1))
+			return PHY_INTERFACE_MODE_SGMII;
+		break;
+	default:
+		return PHY_INTERFACE_MODE_NONE;
+	}
+
+	return PHY_INTERFACE_MODE_NONE;
+}
diff --git a/drivers/net/fm/tgec_phy.c b/drivers/net/fm/tgec_phy.c
index 2d349ad037..2be69d7162 100644
--- a/drivers/net/fm/tgec_phy.c
+++ b/drivers/net/fm/tgec_phy.c
@@ -30,8 +30,8 @@
  * until the write is done before it returns.  All PHY configuration has to be
  * done through the TSEC1 MIIM regs
  */
-int tgec_mdio_write(struct mii_dev *bus, int port_addr, int dev_addr,
-			int regnum, u16 value)
+static int tgec_mdio_write(struct mii_dev *bus, int port_addr, int dev_addr,
+			   int regnum, u16 value)
 {
 	u32 mdio_ctl;
 	u32 stat_val;
@@ -72,8 +72,8 @@ int tgec_mdio_write(struct mii_dev *bus, int port_addr, int dev_addr,
  * Clears miimcom first.  All PHY configuration has to be done through the
  * TSEC1 MIIM regs
  */
-int tgec_mdio_read(struct mii_dev *bus, int port_addr, int dev_addr,
-			int regnum)
+static int tgec_mdio_read(struct mii_dev *bus, int port_addr, int dev_addr,
+			  int regnum)
 {
 	u32 mdio_ctl;
 	u32 stat_val;
@@ -114,7 +114,7 @@ int tgec_mdio_read(struct mii_dev *bus, int port_addr, int dev_addr,
 	return in_be32(&regs->mdio_data) & 0xffff;
 }
 
-int tgec_mdio_reset(struct mii_dev *bus)
+static int tgec_mdio_reset(struct mii_dev *bus)
 {
 	return 0;
 }
diff --git a/drivers/net/macb.c b/drivers/net/macb.c
index 0e1ced71c5..8bacbda719 100644
--- a/drivers/net/macb.c
+++ b/drivers/net/macb.c
@@ -51,8 +51,6 @@
 
 #include "macb.h"
 
-#define barrier() asm volatile("" ::: "memory")
-
 #define CONFIG_SYS_MACB_RX_BUFFER_SIZE		4096
 #define CONFIG_SYS_MACB_RX_RING_SIZE		(CONFIG_SYS_MACB_RX_BUFFER_SIZE / 128)
 #define CONFIG_SYS_MACB_TX_RING_SIZE		16
diff --git a/drivers/net/mcfmii.c b/drivers/net/mcfmii.c
index 471c5efea1..5e64dbdd17 100644
--- a/drivers/net/mcfmii.c
+++ b/drivers/net/mcfmii.c
@@ -315,13 +315,11 @@ int mcffec_miiphy_read(const char *devname, unsigned char addr, unsigned char re
 int mcffec_miiphy_write(const char *devname, unsigned char addr, unsigned char reg,
 			unsigned short value)
 {
-	short rdreg;		/* register working value */
-
 #ifdef MII_DEBUG
 	printf("miiphy_write(0x%x) @ 0x%x = ", reg, addr);
 #endif
 
-	rdreg = mii_send(mk_mii_write(addr, reg, value));
+	mii_send(mk_mii_write(addr, reg, value));
 
 #ifdef MII_DEBUG
 	printf("0x%04x\n", value);
diff --git a/drivers/net/netarm_eth.c b/drivers/net/netarm_eth.c
deleted file mode 100644
index 325f16c3a8..0000000000
--- a/drivers/net/netarm_eth.c
+++ /dev/null
@@ -1,352 +0,0 @@
-/*
- * Copyright (C) 2004 IMMS gGmbH <www.imms.de>
- *
- * 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
- *
- * author(s): Thomas Elste, <info@elste.org>
- *            (some parts derived from uCLinux Netarm Ethernet Driver)
- */
-
-
-#include <common.h>
-#include <command.h>
-#include <net.h>
-#include "netarm_eth.h"
-#include <asm/arch/netarm_registers.h>
-
-static int na_mii_poll_busy (void);
-
-static void na_get_mac_addr (void)
-{
-	unsigned short p[3];
-	char *m_addr;
-	char ethaddr[20];
-
-	m_addr = (char *) p;
-
-	p[0] = (unsigned short) GET_EADDR (NETARM_ETH_SAL_STATION_ADDR_1);
-	p[1] = (unsigned short) GET_EADDR (NETARM_ETH_SAL_STATION_ADDR_2);
-	p[2] = (unsigned short) GET_EADDR (NETARM_ETH_SAL_STATION_ADDR_3);
-
-	sprintf (ethaddr, "%02X:%02X:%02X:%02X:%02X:%02X",
-		 m_addr[0], m_addr[1],
-		 m_addr[2], m_addr[3], m_addr[4], m_addr[5]);
-
-	printf ("HW-MAC Address:  %s\n", ethaddr);
-
-	/* set env, todo: check if already an adress is set */
-	setenv ("ethaddr", ethaddr);
-}
-
-static void na_mii_write (int reg, int value)
-{
-	int mii_addr;
-
-	/* Select register */
-	mii_addr = CONFIG_SYS_ETH_PHY_ADDR + reg;
-	SET_EADDR (NETARM_ETH_MII_ADDR, mii_addr);
-	/* Write value */
-	SET_EADDR (NETARM_ETH_MII_WRITE, value);
-	na_mii_poll_busy ();
-}
-
-static unsigned int na_mii_read (int reg)
-{
-	int mii_addr, val;
-
-	/* Select register */
-	mii_addr = CONFIG_SYS_ETH_PHY_ADDR + reg;
-	SET_EADDR (NETARM_ETH_MII_ADDR, mii_addr);
-	/* do one management cycle */
-	SET_EADDR (NETARM_ETH_MII_CMD,
-		   GET_EADDR (NETARM_ETH_MII_CMD) | NETARM_ETH_MIIC_RSTAT);
-	na_mii_poll_busy ();
-	/* Return read value */
-	val = GET_EADDR (NETARM_ETH_MII_READ);
-	return val;
-}
-
-static int na_mii_poll_busy (void)
-{
-	ulong start;
-	/* arm simple, non interrupt dependent timer */
-	start = get_timer(0));
-	while (get_timer(start) < NA_MII_POLL_BUSY_DELAY) {
-		if (!(GET_EADDR (NETARM_ETH_MII_IND) & NETARM_ETH_MIII_BUSY)) {
-			return 1;
-		}
-	}
-	printf ("na_mii_busy timeout\n");
-	return (0);
-}
-
-static int na_mii_identify_phy (void)
-{
-	int id_reg_a = 0;
-
-	/* get phy id register */
-	id_reg_a = na_mii_read (MII_PHY_ID);
-
-	if (id_reg_a == 0x0043) {
-		/* This must be an Enable or a Lucent LU3X31 PHY chip */
-		return 1;
-	} else if (id_reg_a == 0x0013) {
-		/* it is an Intel LXT971A */
-		return 1;
-	}
-	return (0);
-}
-
-static int na_mii_negotiate (void)
-{
-	int i = 0;
-
-	/* Enable auto-negotiation */
-	na_mii_write (MII_PHY_AUTONEGADV, 0x01e1);
-	/* FIXME: 0x01E1 is 100Mb half and full duplex, 0x0061 is 10Mb only */
-	/* Restart auto-negotiation */
-	na_mii_write (MII_PHY_CONTROL, 0x1200);
-
-	/* status register is 0xffff after setting the autoneg restart bit */
-	while (na_mii_read (MII_PHY_STATUS) == 0xffff) {
-		i++;
-	}
-
-	/* na_mii_read uses the timer already, so we can't use it again for
-	   timeout checking.
-	   Instead we just try some times.
-	 */
-	for (i = 0; i < 40000; i++) {
-		if ((na_mii_read (MII_PHY_STATUS) & 0x0024) == 0x0024) {
-			return 0;
-		}
-	}
-	/*
-	   printf("*Warning* autonegotiation timeout, status: 0x%x\n",na_mii_read(MII_PHY_STATUS));
-	 */
-	return (1);
-}
-
-static unsigned int na_mii_check_speed (void)
-{
-	unsigned int status;
-
-	/* Read Status register */
-	status = na_mii_read (MII_PHY_STATUS);
-	/* Check link status.  If 0, default to 100 Mbps. */
-	if ((status & 0x0004) == 0) {
-		printf ("*Warning* no link detected, set default speed to 100Mbs\n");
-		return 1;
-	} else {
-		if ((na_mii_read (17) & 0x4000) != 0) {
-			printf ("100Mbs link detected\n");
-			return 1;
-		} else {
-			printf ("10Mbs link detected\n");
-			return 0;
-		}
-	}
-	return 0;
-}
-
-static int reset_eth (void)
-{
-	int pt;
-	ulong start;
-
-	na_get_mac_addr ();
-	pt = na_mii_identify_phy ();
-
-	/* reset the phy */
-	na_mii_write (MII_PHY_CONTROL, 0x8000);
-	start = get_timer(0);
-	while (get_timer(start) < NA_MII_NEGOTIATE_DELAY) {
-		if ((na_mii_read (MII_PHY_STATUS) & 0x8000) == 0) {
-			break;
-		}
-	}
-	if (get_timer(start) >= NA_MII_NEGOTIATE_DELAY)
-		printf ("phy reset timeout\n");
-
-	/* set the PCS reg */
-	SET_EADDR (NETARM_ETH_PCS_CFG, NETARM_ETH_PCSC_CLKS_25M |
-		   NETARM_ETH_PCSC_ENJAB | NETARM_ETH_PCSC_NOCFR);
-
-	na_mii_negotiate ();
-	na_mii_check_speed ();
-
-	/* Delay 10 millisecond.  (Maybe this should be 1 second.) */
-	udelay (10000);
-
-	/* Turn receive on.
-	   Enable statistics register autozero on read.
-	   Do not insert MAC address on transmit.
-	   Do not enable special test modes.  */
-	SET_EADDR (NETARM_ETH_STL_CFG,
-		   (NETARM_ETH_STLC_AUTOZ | NETARM_ETH_STLC_RXEN));
-
-	/* Set the inter-packet gap delay to 0.96us for MII.
-	   The NET+ARM H/W Reference Guide indicates that the Back-to-back IPG
-	   Gap Timer Register should be set to 0x15 and the Non Back-to-back IPG
-	   Gap Timer Register should be set to 0x00000C12 for the MII PHY. */
-	SET_EADDR (NETARM_ETH_B2B_IPG_GAP_TMR, 0x15);
-	SET_EADDR (NETARM_ETH_NB2B_IPG_GAP_TMR, 0x00000C12);
-
-	/* Add CRC to end of packets.
-	   Pad packets to minimum length of 64 bytes.
-	   Allow unlimited length transmit packets.
-	   Receive all broadcast packets.
-	   NOTE:  Multicast addressing is NOT enabled here currently. */
-	SET_EADDR (NETARM_ETH_MAC_CFG,
-		   (NETARM_ETH_MACC_CRCEN |
-		    NETARM_ETH_MACC_PADEN | NETARM_ETH_MACC_HUGEN));
-	SET_EADDR (NETARM_ETH_SAL_FILTER, NETARM_ETH_SALF_BROAD);
-
-	/* enable fifos */
-	SET_EADDR (NETARM_ETH_GEN_CTRL,
-		   (NETARM_ETH_GCR_ERX | NETARM_ETH_GCR_ETX));
-
-	return (0);
-}
-
-
-extern int eth_init (bd_t * bd)
-{
-	reset_eth ();
-	return 0;
-}
-
-extern void eth_halt (void)
-{
-	SET_EADDR (NETARM_ETH_GEN_CTRL, 0);
-}
-
-/* Get a data block via Ethernet */
-extern int eth_rx (void)
-{
-	int i;
-	unsigned short rxlen;
-	unsigned int *addr;
-	unsigned int rxstatus, lastrxlen;
-	char *pa;
-
-	/* RXBR is 1, data block was received */
-	if ((GET_EADDR (NETARM_ETH_GEN_STAT) & NETARM_ETH_GST_RXBR) == 0)
-		return 0;
-
-	/* get status register and the length of received block */
-	rxstatus = GET_EADDR (NETARM_ETH_RX_STAT);
-	rxlen = (rxstatus & NETARM_ETH_RXSTAT_SIZE) >> 16;
-
-	if (rxlen == 0)
-		return 0;
-
-	/* clear RXBR to make fifo available */
-	SET_EADDR (NETARM_ETH_GEN_STAT,
-		   GET_EADDR (NETARM_ETH_GEN_STAT) & ~NETARM_ETH_GST_RXBR);
-
-	/* clear TXBC to make fifo available */
-	/* According to NETARM50 data manual you just have to clear
-	   RXBR but that has no effect. Only after clearing TXBC the
-	   Fifo becomes readable. */
-	SET_EADDR (NETARM_ETH_GEN_STAT,
-		   GET_EADDR (NETARM_ETH_GEN_STAT) & ~NETARM_ETH_GST_TXBC);
-
-	addr = (unsigned int *) NetRxPackets[0];
-	pa = (char *) NetRxPackets[0];
-
-	/* read the fifo */
-	for (i = 0; i < rxlen / 4; i++) {
-		*addr = GET_EADDR (NETARM_ETH_FIFO_DAT1);
-		addr++;
-	}
-
-	if (GET_EADDR (NETARM_ETH_GEN_STAT) & NETARM_ETH_GST_RXREGR) {
-		/* RXFDB indicates wether the last word is 1,2,3 or 4 bytes long */
-		lastrxlen =
-			(GET_EADDR (NETARM_ETH_GEN_STAT) &
-			 NETARM_ETH_GST_RXFDB) >> 28;
-		*addr = GET_EADDR (NETARM_ETH_FIFO_DAT1);
-		switch (lastrxlen) {
-		case 1:
-			*addr &= 0xff000000;
-			break;
-		case 2:
-			*addr &= 0xffff0000;
-			break;
-		case 3:
-			*addr &= 0xffffff00;
-			break;
-		}
-	}
-
-	/* Pass the packet up to the protocol layers. */
-	NetReceive (NetRxPackets[0], rxlen);
-
-	return rxlen;
-}
-
-/* Send a data block via Ethernet. */
-extern int eth_send(void *packet, int length)
-{
-	int i, length32;
-	char *pa;
-	unsigned int *pa32, lastp = 0, rest;
-
-	pa = (char *) packet;
-	pa32 = (unsigned int *) packet;
-	length32 = length / 4;
-	rest = length % 4;
-
-	/* make sure there's no garbage in the last word */
-	switch (rest) {
-	case 0:
-		lastp = pa32[length32];
-		length32--;
-		break;
-	case 1:
-		lastp = pa32[length32] & 0x000000ff;
-		break;
-	case 2:
-		lastp = pa32[length32] & 0x0000ffff;
-		break;
-	case 3:
-		lastp = pa32[length32] & 0x00ffffff;
-		break;
-	}
-
-	/* write to the fifo */
-	for (i = 0; i < length32; i++)
-		SET_EADDR (NETARM_ETH_FIFO_DAT1, pa32[i]);
-
-	/* the last word is written to an extra register, this
-	   starts the transmission */
-	SET_EADDR (NETARM_ETH_FIFO_DAT2, lastp);
-
-	/* NETARM_ETH_TXSTAT_TXOK should be checked, to know if the transmission
-	   went fine. But we can't use the timer for a timeout loop because
-	   of it is used already in upper layers. So we just try some times. */
-	i = 0;
-	while (i < 50000) {
-		if ((GET_EADDR (NETARM_ETH_TX_STAT) & NETARM_ETH_TXSTAT_TXOK)
-		    == NETARM_ETH_TXSTAT_TXOK)
-			return 0;
-		i++;
-	}
-
-	printf ("eth_send timeout\n");
-	return 1;
-}
diff --git a/drivers/net/netarm_eth.h b/drivers/net/netarm_eth.h
deleted file mode 100644
index 8edab82da3..0000000000
--- a/drivers/net/netarm_eth.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2003 IMMS gGmbH <www.imms.de>
- *
- * 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
- *
- * author(s): Thomas Elste, <info@elste.org>
- */
-
-#include <asm/types.h>
-#include <config.h>
-
-#ifdef CONFIG_DRIVER_NETARMETH
-
-#define SET_EADDR(ad,val) *(volatile unsigned int*)(ad + NETARM_ETH_MODULE_BASE) = val
-#define GET_EADDR(ad) (*(volatile unsigned int*)(ad + NETARM_ETH_MODULE_BASE))
-
-#define NA_MII_POLL_BUSY_DELAY 900
-
-/* MII negotiation timeout value
-   500 jiffies = 5 seconds */
-#define NA_MII_NEGOTIATE_DELAY 30
-
-/* Registers in the physical layer chip */
-#define MII_PHY_CONTROL		0
-#define MII_PHY_STATUS		1
-#define MII_PHY_ID              2
-#define MII_PHY_AUTONEGADV	4
-
-#endif /* CONFIG_DRIVER_NETARMETH */
diff --git a/drivers/net/phy/atheros.c b/drivers/net/phy/atheros.c
index 798473dd68..9b3808bfa9 100644
--- a/drivers/net/phy/atheros.c
+++ b/drivers/net/phy/atheros.c
@@ -30,7 +30,7 @@ static int ar8021_config(struct phy_device *phydev)
 	return 0;
 }
 
-struct phy_driver AR8021_driver =  {
+static struct phy_driver AR8021_driver =  {
 	.name = "AR8021",
 	.uid = 0x4dd040,
 	.mask = 0xfffff0,
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index baef60f827..1ffa791dce 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -43,7 +43,7 @@
  *   what is supported.  Returns < 0 on error, 0 if the PHY's advertisement
  *   hasn't changed, and > 0 if it has changed.
  */
-int genphy_config_advert(struct phy_device *phydev)
+static int genphy_config_advert(struct phy_device *phydev)
 {
 	u32 advertise;
 	int oldadv, adv;
@@ -118,7 +118,7 @@ int genphy_config_advert(struct phy_device *phydev)
  * Description: Configures MII_BMCR to force speed/duplex
  *   to the values in phydev. Assumes that the values are valid.
  */
-int genphy_setup_forced(struct phy_device *phydev)
+static int genphy_setup_forced(struct phy_device *phydev)
 {
 	int err;
 	int ctl = 0;
@@ -465,7 +465,7 @@ int phy_register(struct phy_driver *drv)
 	return 0;
 }
 
-int phy_probe(struct phy_device *phydev)
+static int phy_probe(struct phy_device *phydev)
 {
 	int err = 0;
 
@@ -488,7 +488,7 @@ static struct phy_driver *generic_for_interface(phy_interface_t interface)
 	return &genphy_driver;
 }
 
-struct phy_driver *get_phy_driver(struct phy_device *phydev,
+static struct phy_driver *get_phy_driver(struct phy_device *phydev,
 				phy_interface_t interface)
 {
 	struct list_head *entry;
@@ -505,8 +505,9 @@ struct phy_driver *get_phy_driver(struct phy_device *phydev,
 	return generic_for_interface(interface);
 }
 
-struct phy_device *phy_device_create(struct mii_dev *bus, int addr, int phy_id,
-					phy_interface_t interface)
+static struct phy_device *phy_device_create(struct mii_dev *bus, int addr,
+					    int phy_id,
+					    phy_interface_t interface)
 {
 	struct phy_device *dev;
 
@@ -549,7 +550,7 @@ struct phy_device *phy_device_create(struct mii_dev *bus, int addr, int phy_id,
  * Description: Reads the ID registers of the PHY at @addr on the
  *   @bus, stores it in @phy_id and returns zero on success.
  */
-int get_phy_id(struct mii_dev *bus, int addr, int devad, u32 *phy_id)
+static int get_phy_id(struct mii_dev *bus, int addr, int devad, u32 *phy_id)
 {
 	int phy_reg;
 
@@ -581,8 +582,8 @@ int get_phy_id(struct mii_dev *bus, int addr, int devad, u32 *phy_id)
  * Description: Reads the ID registers of the PHY at @addr on the
  *   @bus, then allocates and returns the phy_device to represent it.
  */
-struct phy_device *get_phy_device(struct mii_dev *bus, int addr,
-				phy_interface_t interface)
+static struct phy_device *get_phy_device(struct mii_dev *bus, int addr,
+					 phy_interface_t interface)
 {
 	u32 phy_id = 0x1fffffff;
 	int i;
diff --git a/drivers/net/phy/vitesse.c b/drivers/net/phy/vitesse.c
index 0a0f40dbde..6c5cb99774 100644
--- a/drivers/net/phy/vitesse.c
+++ b/drivers/net/phy/vitesse.c
@@ -123,7 +123,7 @@ static int cis8204_config(struct phy_device *phydev)
 }
 
 /* Vitesse VSC8601 */
-int vsc8601_config(struct phy_device *phydev)
+static int vsc8601_config(struct phy_device *phydev)
 {
 	/* Configure some basic stuff */
 #ifdef CONFIG_SYS_VSC8601_SKEWFIX
diff --git a/drivers/net/smc91111.c b/drivers/net/smc91111.c
index 6dc7ad52e4..1357edee6c 100644
--- a/drivers/net/smc91111.c
+++ b/drivers/net/smc91111.c
@@ -178,8 +178,6 @@ static void smc_phy_configure(struct eth_device *dev);
  * inx,outx functions fixed this problem.
  */
 
-#define barrier() __asm__ __volatile__("": : :"memory")
-
 static inline word SMC_inw(struct eth_device *dev, dword offset)
 {
 	word v;
diff --git a/drivers/net/vsc7385.c b/drivers/net/vsc7385.c
index ada42c41f8..a5110e516d 100644
--- a/drivers/net/vsc7385.c
+++ b/drivers/net/vsc7385.c
@@ -16,6 +16,7 @@
 #include <common.h>
 #include <asm/io.h>
 #include <asm/errno.h>
+#include "vsc7385.h"
 
 /*
  * Upload a Vitesse VSC7385 firmware image to the hardware
diff --git a/drivers/pci/fsl_pci_init.c b/drivers/pci/fsl_pci_init.c
index 0d46c96312..48ae16374d 100644
--- a/drivers/pci/fsl_pci_init.c
+++ b/drivers/pci/fsl_pci_init.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2007-2011 Freescale Semiconductor, Inc.
+ * Copyright 2007-2012 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 as published by the Free
@@ -211,7 +211,7 @@ static int fsl_pci_setup_inbound_windows(struct pci_controller *hose,
 	return 1;
 }
 
-#ifdef CONFIG_FSL_CORENET
+#ifdef CONFIG_SYS_FSL_SRIO_PCIE_BOOT_MASTER
 static void fsl_pcie_boot_master(pit_t *pi)
 {
 	/* configure inbound window for slave's u-boot image */
@@ -276,14 +276,18 @@ static void fsl_pcie_boot_master_release_slave(int port)
 		release_addr = CONFIG_SYS_PCIE1_MEM_VIRT
 			+ CONFIG_SRIO_PCIE_BOOT_BRR_OFFSET;
 		break;
+#ifdef CONFIG_SYS_PCIE2_MEM_VIRT
 	case 2:
 		release_addr = CONFIG_SYS_PCIE2_MEM_VIRT
 			+ CONFIG_SRIO_PCIE_BOOT_BRR_OFFSET;
 		break;
+#endif
+#ifdef CONFIG_SYS_PCIE3_MEM_VIRT
 	case 3:
 		release_addr = CONFIG_SYS_PCIE3_MEM_VIRT
 			+ CONFIG_SRIO_PCIE_BOOT_BRR_OFFSET;
 		break;
+#endif
 	default:
 		release_addr = 0;
 		break;
@@ -384,7 +388,7 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 	/* see if we are a PCIe or PCI controller */
 	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
 
-#ifdef CONFIG_FSL_CORENET
+#ifdef CONFIG_SYS_FSL_SRIO_PCIE_BOOT_MASTER
 	/* boot from PCIE --master */
 	char *s = getenv("bootmaster");
 	char pcie[6];
@@ -499,13 +503,7 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 	}
 
 #ifndef CONFIG_PCI_NOSCAN
-	pci_hose_read_config_byte(hose, dev, PCI_CLASS_PROG, &temp8);
-
-	/* Programming Interface (PCI_CLASS_PROG)
-	 * 0 == pci host or pcie root-complex,
-	 * 1 == pci agent or pcie end-point
-	 */
-	if (!temp8) {
+	if (!fsl_is_pci_agent(hose)) {
 		debug("           Scanning PCI bus %02x\n",
 			hose->current_busno);
 		hose->last_busno = pci_hose_scan_bus(hose, hose->current_busno);
@@ -543,12 +541,22 @@ void fsl_pci_init(struct pci_controller *hose, struct fsl_pci_info *pci_info)
 
 int fsl_is_pci_agent(struct pci_controller *hose)
 {
-	u8 prog_if;
+	u8 pcie_cap;
 	pci_dev_t dev = PCI_BDF(hose->first_busno, 0, 0);
 
-	pci_hose_read_config_byte(hose, dev, PCI_CLASS_PROG, &prog_if);
+	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
+	if (pcie_cap == PCI_CAP_ID_EXP) {
+		u8 header_type;
 
-	return (prog_if == FSL_PROG_IF_AGENT);
+		pci_hose_read_config_byte(hose, dev, PCI_HEADER_TYPE,
+					  &header_type);
+		return (header_type & 0x7f) == PCI_HEADER_TYPE_NORMAL;
+	} else {
+		u8 prog_if;
+
+		pci_hose_read_config_byte(hose, dev, PCI_CLASS_PROG, &prog_if);
+		return (prog_if == FSL_PROG_IF_AGENT);
+	}
 }
 
 int fsl_pci_init_port(struct fsl_pci_info *pci_info,
@@ -594,7 +602,7 @@ int fsl_pci_init_port(struct fsl_pci_info *pci_info,
 	if (fsl_is_pci_agent(hose)) {
 		fsl_pci_config_unlock(hose);
 		hose->last_busno = hose->first_busno;
-#ifdef CONFIG_FSL_CORENET
+#ifdef CONFIG_SYS_FSL_SRIO_PCIE_BOOT_MASTER
 	} else {
 		/* boot from PCIE --master releases slave's core 0 */
 		char *s = getenv("bootmaster");
@@ -618,12 +626,10 @@ int fsl_pci_init_port(struct fsl_pci_info *pci_info,
 void fsl_pci_config_unlock(struct pci_controller *hose)
 {
 	pci_dev_t dev = PCI_BDF(hose->first_busno,0,0);
-	u8 agent;
 	u8 pcie_cap;
 	u16 pbfr;
 
-	pci_hose_read_config_byte(hose, dev, PCI_CLASS_PROG, &agent);
-	if (!agent)
+	if (!fsl_is_pci_agent(hose))
 		return;
 
 	pci_hose_read_config_byte(hose, dev, FSL_PCIE_CAP_ID, &pcie_cap);
@@ -660,10 +666,17 @@ int fsl_configure_pcie(struct fsl_pci_info *info,
 }
 
 #if defined(CONFIG_FSL_CORENET)
+#ifdef CONFIG_SYS_FSL_QORIQ_CHASSIS2
+	#define _DEVDISR_PCIE1 FSL_CORENET_DEVDISR3_PCIE1
+	#define _DEVDISR_PCIE2 FSL_CORENET_DEVDISR3_PCIE2
+	#define _DEVDISR_PCIE3 FSL_CORENET_DEVDISR3_PCIE3
+	#define _DEVDISR_PCIE4 FSL_CORENET_DEVDISR3_PCIE4
+#else
 	#define _DEVDISR_PCIE1 FSL_CORENET_DEVDISR_PCIE1
 	#define _DEVDISR_PCIE2 FSL_CORENET_DEVDISR_PCIE2
 	#define _DEVDISR_PCIE3 FSL_CORENET_DEVDISR_PCIE3
 	#define _DEVDISR_PCIE4 FSL_CORENET_DEVDISR_PCIE4
+#endif
 	#define CONFIG_SYS_MPC8xxx_GUTS_ADDR CONFIG_SYS_MPC85xx_GUTS_ADDR
 #elif defined(CONFIG_MPC85xx)
 	#define _DEVDISR_PCIE1 MPC85xx_DEVDISR_PCIE
@@ -743,34 +756,42 @@ int fsl_pcie_init_board(int busno)
 {
 	struct fsl_pci_info pci_info;
 	ccsr_gur_t *gur = (void *)CONFIG_SYS_MPC8xxx_GUTS_ADDR;
-	u32 devdisr = in_be32(&gur->devdisr);
+	u32 devdisr;
+	u32 *addr;
+
+#ifdef CONFIG_SYS_FSL_QORIQ_CHASSIS2
+	addr = &gur->devdisr3;
+#else
+	addr = &gur->devdisr;
+#endif
+	devdisr = in_be32(addr);
 
 #ifdef CONFIG_PCIE1
 	SET_STD_PCIE_INFO(pci_info, 1);
 	busno = fsl_pcie_init_ctrl(busno, devdisr, PCIE1, &pci_info);
 #else
-	setbits_be32(&gur->devdisr, _DEVDISR_PCIE1); /* disable */
+	setbits_be32(addr, _DEVDISR_PCIE1); /* disable */
 #endif
 
 #ifdef CONFIG_PCIE2
 	SET_STD_PCIE_INFO(pci_info, 2);
 	busno = fsl_pcie_init_ctrl(busno, devdisr, PCIE2, &pci_info);
 #else
-	setbits_be32(&gur->devdisr, _DEVDISR_PCIE2); /* disable */
+	setbits_be32(addr, _DEVDISR_PCIE2); /* disable */
 #endif
 
 #ifdef CONFIG_PCIE3
 	SET_STD_PCIE_INFO(pci_info, 3);
 	busno = fsl_pcie_init_ctrl(busno, devdisr, PCIE3, &pci_info);
 #else
-	setbits_be32(&gur->devdisr, _DEVDISR_PCIE3); /* disable */
+	setbits_be32(addr, _DEVDISR_PCIE3); /* disable */
 #endif
 
 #ifdef CONFIG_PCIE4
 	SET_STD_PCIE_INFO(pci_info, 4);
 	busno = fsl_pcie_init_ctrl(busno, devdisr, PCIE4, &pci_info);
 #else
-	setbits_be32(&gur->devdisr, _DEVDISR_PCIE4); /* disable */
+	setbits_be32(addr, _DEVDISR_PCIE4); /* disable */
 #endif
 
  	return busno;
diff --git a/drivers/qe/qe.c b/drivers/qe/qe.c
index 9f711519ed..345587be63 100644
--- a/drivers/qe/qe.c
+++ b/drivers/qe/qe.c
@@ -113,18 +113,21 @@ static void qe_sdma_init(void)
  * we just need to know what the SNUMs are for the threads.
  */
 static u8 thread_snum[] = {
+/* Evthreads 16-29 are not supported in MPC8309 */
+#if !defined(CONFIG_MPC8309)
 	0x04, 0x05, 0x0c, 0x0d,
 	0x14, 0x15, 0x1c, 0x1d,
 	0x24, 0x25, 0x2c, 0x2d,
-	0x34, 0x35, 0x88, 0x89,
-	0x98, 0x99, 0xa8, 0xa9,
-	0xb8, 0xb9, 0xc8, 0xc9,
-	0xd8, 0xd9, 0xe8, 0xe9,
-	0x08, 0x09, 0x18, 0x19,
-	0x28, 0x29, 0x38, 0x39,
-	0x48, 0x49, 0x58, 0x59,
-	0x68, 0x69, 0x78, 0x79,
-	0x80, 0x81
+	0x34, 0x35,
+#endif
+	0x88, 0x89, 0x98, 0x99,
+	0xa8, 0xa9, 0xb8, 0xb9,
+	0xc8, 0xc9, 0xd8, 0xd9,
+	0xe8, 0xe9, 0x08, 0x09,
+	0x18, 0x19, 0x28, 0x29,
+	0x38, 0x39, 0x48, 0x49,
+	0x58, 0x59, 0x68, 0x69,
+	0x78, 0x79, 0x80, 0x81
 };
 
 static void qe_snums_init(void)
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index 3c32f97abb..5e8b64873d 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -36,19 +36,14 @@ COBJS-$(CONFIG_MCFUART) += mcfuart.o
 COBJS-$(CONFIG_NS9750_UART) += ns9750_serial.o
 COBJS-$(CONFIG_OPENCORES_YANU) += opencores_yanu.o
 COBJS-$(CONFIG_SYS_NS16550) += ns16550.o
-COBJS-$(CONFIG_DRIVER_S3C4510_UART) += s3c4510b_uart.o
 COBJS-$(CONFIG_S3C64XX) += s3c64xx.o
 COBJS-$(CONFIG_S5P) += serial_s5p.o
 COBJS-$(CONFIG_SYS_NS16550_SERIAL) += serial_ns16550.o
-COBJS-$(CONFIG_CLPS7111_SERIAL) += serial_clps7111.o
 COBJS-$(CONFIG_IMX_SERIAL) += serial_imx.o
 COBJS-$(CONFIG_IXP_SERIAL) += serial_ixp.o
 COBJS-$(CONFIG_KS8695_SERIAL) += serial_ks8695.o
-COBJS-$(CONFIG_LPC2292_SERIAL) += serial_lpc2292.o
-COBJS-$(CONFIG_LH7A40X_SERIAL) += serial_lh7a40x.o
 COBJS-$(CONFIG_MAX3100_SERIAL) += serial_max3100.o
 COBJS-$(CONFIG_MXC_UART) += serial_mxc.o
-COBJS-$(CONFIG_NETARM_SERIAL) += serial_netarm.o
 COBJS-$(CONFIG_PL010_SERIAL) += serial_pl01x.o
 COBJS-$(CONFIG_PL011_SERIAL) += serial_pl01x.o
 COBJS-$(CONFIG_PXA_SERIAL) += serial_pxa.o
diff --git a/drivers/serial/altera_jtag_uart.c b/drivers/serial/altera_jtag_uart.c
index 654b5019eb..28319baf18 100644
--- a/drivers/serial/altera_jtag_uart.c
+++ b/drivers/serial/altera_jtag_uart.c
@@ -59,12 +59,6 @@ static void altera_jtag_serial_putc(char c)
 	writel ((unsigned char)c, &jtag->data);
 }
 
-static void altera_jtag_serial_puts(const char *s)
-{
-	while (*s != 0)
-		serial_putc (*s++);
-}
-
 static int altera_jtag_serial_tstc(void)
 {
 	return ( readl (&jtag->control) & NIOS_JTAG_RRDY);
@@ -91,7 +85,7 @@ static struct serial_device altera_jtag_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= altera_jtag_serial_setbrg,
 	.putc	= altera_jtag_serial_putc,
-	.puts	= altera_jtag_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= altera_jtag_serial_getc,
 	.tstc	= altera_jtag_serial_tstc,
 };
diff --git a/drivers/serial/altera_uart.c b/drivers/serial/altera_uart.c
index 27550ed48d..118cd58ece 100644
--- a/drivers/serial/altera_uart.c
+++ b/drivers/serial/altera_uart.c
@@ -82,13 +82,6 @@ static void altera_serial_putc(char c)
 	writel ((unsigned char)c, &uart->txdata);
 }
 
-static void altera_serial_puts(const char *s)
-{
-	while (*s != 0) {
-		serial_putc (*s++);
-	}
-}
-
 static int altera_serial_tstc(void)
 {
 	return (readl (&uart->status) & NIOS_UART_RRDY);
@@ -107,7 +100,7 @@ static struct serial_device altera_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= altera_serial_setbrg,
 	.putc	= altera_serial_putc,
-	.puts	= altera_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= altera_serial_getc,
 	.tstc	= altera_serial_tstc,
 };
diff --git a/drivers/serial/atmel_usart.c b/drivers/serial/atmel_usart.c
index 130303129e..c4d7432efc 100644
--- a/drivers/serial/atmel_usart.c
+++ b/drivers/serial/atmel_usart.c
@@ -86,12 +86,6 @@ static void atmel_serial_putc(char c)
 	writel(c, &usart->thr);
 }
 
-static void atmel_serial_puts(const char *s)
-{
-	while (*s)
-		serial_putc(*s++);
-}
-
 static int atmel_serial_getc(void)
 {
 	atmel_usart3_t *usart = (atmel_usart3_t *)CONFIG_USART_BASE;
@@ -113,7 +107,7 @@ static struct serial_device atmel_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= atmel_serial_setbrg,
 	.putc	= atmel_serial_putc,
-	.puts	= atmel_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= atmel_serial_getc,
 	.tstc	= atmel_serial_tstc,
 };
diff --git a/drivers/serial/lpc32xx_hsuart.c b/drivers/serial/lpc32xx_hsuart.c
index 02429b5541..7559916578 100644
--- a/drivers/serial/lpc32xx_hsuart.c
+++ b/drivers/serial/lpc32xx_hsuart.c
@@ -77,19 +77,13 @@ static int lpc32xx_serial_init(void)
 	return 0;
 }
 
-static void lpc32xx_serial_puts(const char *s)
-{
-	while (*s)
-		serial_putc(*s++);
-}
-
 static struct serial_device lpc32xx_serial_drv = {
 	.name	= "lpc32xx_serial",
 	.start	= lpc32xx_serial_init,
 	.stop	= NULL,
 	.setbrg	= lpc32xx_serial_setbrg,
 	.putc	= lpc32xx_serial_putc,
-	.puts	= lpc32xx_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= lpc32xx_serial_getc,
 	.tstc	= lpc32xx_serial_tstc,
 };
diff --git a/drivers/serial/mcfuart.c b/drivers/serial/mcfuart.c
index 00a7114691..7e25797661 100644
--- a/drivers/serial/mcfuart.c
+++ b/drivers/serial/mcfuart.c
@@ -28,6 +28,8 @@
  */
 
 #include <common.h>
+#include <serial.h>
+#include <linux/compiler.h>
 
 #include <asm/immap.h>
 #include <asm/uart.h>
@@ -87,13 +89,6 @@ static void mcf_serial_putc(const char c)
 	uart->utb = c;
 }
 
-static void mcf_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc(*s++);
-	}
-}
-
 static int mcf_serial_getc(void)
 {
 	volatile uart_t *uart = (volatile uart_t *)(CONFIG_SYS_UART_BASE);
@@ -136,7 +131,7 @@ static struct serial_device mcf_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= mcf_serial_setbrg,
 	.putc	= mcf_serial_putc,
-	.puts	= mcf_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= mcf_serial_getc,
 	.tstc	= mcf_serial_tstc,
 };
diff --git a/drivers/serial/ns9750_serial.c b/drivers/serial/ns9750_serial.c
index cb545c4065..85fc68a076 100644
--- a/drivers/serial/ns9750_serial.c
+++ b/drivers/serial/ns9750_serial.c
@@ -100,19 +100,6 @@ static void ns9750_serial_putc(const char c)
 }
 
 /***********************************************************************
- * @Function: serial_puts
- * @Return: n/a
- * @Descr: writes non-zero string to the FIFO.
- ***********************************************************************/
-
-static void ns9750_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc( *s++ );
-	}
-}
-
-/***********************************************************************
  * @Function: serial_getc
  * @Return: the character read
  * @Descr: performs only 8bit accesses to the FIFO. No error handling
@@ -215,7 +202,7 @@ static struct serial_device ns9750_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= ns9750_serial_setbrg,
 	.putc	= ns9750_serial_putc,
-	.puts	= ns9750_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= ns9750_serial_getc,
 	.tstc	= ns9750_serial_tstc,
 };
diff --git a/drivers/serial/opencores_yanu.c b/drivers/serial/opencores_yanu.c
index 49bccf3a6c..4ca6ef0b25 100644
--- a/drivers/serial/opencores_yanu.c
+++ b/drivers/serial/opencores_yanu.c
@@ -161,14 +161,6 @@ static void oc_serial_putc(char c)
 	writel((unsigned char)c, &uart->data);
 }
 
-static void oc_serial_puts(const char *s)
-{
-	while (*s != 0) {
-		serial_putc (*s++);
-	}
-}
-
-
 static int oc_serial_tstc(void)
 {
 	unsigned status ;
@@ -195,7 +187,7 @@ static struct serial_device oc_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= oc_serial_setbrg,
 	.putc	= oc_serial_putc,
-	.puts	= oc_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= oc_serial_getc,
 	.tstc	= oc_serial_tstc,
 };
diff --git a/drivers/serial/s3c4510b_uart.c b/drivers/serial/s3c4510b_uart.c
deleted file mode 100644
index 423d26e678..0000000000
--- a/drivers/serial/s3c4510b_uart.c
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * Copyright (c) 2004	Cucy Systems (http://www.cucy.com)
- * Curt Brune <curt@cucy.com>
- *
- * (C) Copyright 2004
- * DAVE Srl
- * http://www.dave-tech.it
- * http://www.wawnet.biz
- * mailto:info@wawnet.biz
- *
- * (C) Copyright 2002-2004
- * Wolfgang Denk, DENX Software Engineering, <wd@denx.de>
- *
- * (C) Copyright 2002
- * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
- * Marius Groeger <mgroeger@sysgo.de>
- *
- * (C) Copyright 2002
- * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
- * Alex Zuepke <azu@sysgo.de>
- *
- * Copyright (C) 1999 2000 2001 Erik Mouw (J.A.K.Mouw@its.tudelft.nl)
- *
- * 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
- *
- * MODULE:        $Id:$
- * Description:   UART/Serial interface for Samsung S3C4510B SoC
- * Runtime Env:   ARM7TDMI
- * Change History:
- *     03-02-04    Create (Curt Brune) curt@cucy.com
- *
- */
-
-#include <common.h>
-
-#include <asm/hardware.h>
-#include "s3c4510b_uart.h"
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static UART    *uart;
-
-/* flush serial input queue. returns 0 on success or negative error
- * number otherwise
- */
-static int serial_flush_input(void)
-{
-	volatile u32 tmp;
-
-	/* keep on reading as long as the receiver is not empty */
-	while( uart->m_stat.bf.rxReady) {
-		tmp = uart->m_rx;
-	}
-
-	return 0;
-}
-
-
-/* flush output queue. returns 0 on success or negative error number
- * otherwise
- */
-static int serial_flush_output(void)
-{
-	/* wait until the transmitter is no longer busy */
-	while( !uart->m_stat.bf.txBufEmpty);
-
-	return 0;
-}
-
-
-static void s3c4510b_serial_setbrg(void)
-{
-	UART_LINE_CTRL ulctrl;
-	UART_CTRL      uctrl;
-	UART_BAUD_DIV  ubd;
-
-	serial_flush_output();
-	serial_flush_input();
-
-	/* control register */
-	uctrl.ui = 0x0;
-	uctrl.bf.rxMode = 0x1;
-	uctrl.bf.rxIrq = 0x0;
-	uctrl.bf.txMode = 0x1;
-	uctrl.bf.DSR = 0x0;
-	uctrl.bf.sendBreak = 0x0;
-	uctrl.bf.loopBack = 0x0;
-	uart->m_ctrl.ui = uctrl.ui;
-
-	/* line control register */
-	ulctrl.ui  = 0x0;
-	ulctrl.bf.wordLen   = 0x3; /* 8 bit data */
-	ulctrl.bf.nStop     = 0x0; /* 1 stop bit */
-	ulctrl.bf.parity    = 0x0; /* no parity */
-	ulctrl.bf.clk       = 0x0; /* internal clock */
-	ulctrl.bf.infra_red = 0x0; /* no infra_red */
-	uart->m_lineCtrl.ui = ulctrl.ui;
-
-	ubd.ui = 0x0;
-
-	/* see table on page 10-15 in SAMSUNG S3C4510B manual */
-	/* get correct divisor */
-	switch(gd->baudrate) {
-	case   1200:	ubd.bf.cnt0 = 1301;	break;
-	case   2400:	ubd.bf.cnt0 =  650;	break;
-	case   4800:	ubd.bf.cnt0 =  324;	break;
-	case   9600:	ubd.bf.cnt0 =  162;	break;
-	case  19200:	ubd.bf.cnt0 =   80;	break;
-	case  38400:	ubd.bf.cnt0 =   40;	break;
-	case  57600:	ubd.bf.cnt0 =   26;	break;
-	case 115200:	ubd.bf.cnt0 =   13;	break;
-	}
-
-	uart->m_baudDiv.ui = ubd.ui;
-	uart->m_baudCnt = 0x0;
-	uart->m_baudClk = 0x0;
-
-}
-
-
-/*
- * Initialise the serial port with the given baudrate. The settings
- * are always 8 data bits, no parity, 1 stop bit, no start bits.
- *
- */
-static int s3c4510b_serial_init(void)
-{
-
-#if   CONFIG_SERIAL1 == 1
-	uart = (UART *)UART0_BASE;
-#elif CONFIG_SERIAL1 == 2
-	uart = (UART *)UART1_BASE;
-#else
-#error CONFIG_SERIAL1 not equal to 1 or 2
-#endif
-
-	serial_setbrg ();
-
-	return (0);
-}
-
-
-/*
- * Output a single byte to the serial port.
- */
-static void s3c4510_serial_putc(const char c)
-{
-	/* wait for room in the transmit FIFO */
-	while( !uart->m_stat.bf.txBufEmpty);
-
-	uart->m_tx = c;
-
-	/*
-		to be polite with serial console add a line feed
-		to the carriage return character
-	*/
-	if (c=='\n')
-		serial_putc('\r');
-}
-
-/*
- * Test if an input byte is ready from the serial port. Returns non-zero on
- * success, 0 otherwise.
- */
-static int s3c4510b_serial_tstc(void)
-{
-	return uart->m_stat.bf.rxReady;
-}
-
-/*
- * Read a single byte from the serial port. Returns 1 on success, 0
- * otherwise. When the function is succesfull, the character read is
- * written into its argument c.
- */
-static int s3c4510b_serial_getc(void)
-{
-	int rv;
-
-	for(;;) {
-		rv = serial_tstc();
-
-		if (rv) {
-			return uart->m_rx & 0xFF;
-		}
-	}
-}
-
-static void s3c4510b_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-
-	/* busy wait for tx complete */
-	while ( !uart->m_stat.bf.txComplete);
-
-	/* clear break */
-	uart->m_ctrl.bf.sendBreak = 0;
-
-}
-
-static struct serial_device s3c4510b_serial_drv = {
-	.name	= "s3c4510b_serial",
-	.start	= s3c4510b_serial_init,
-	.stop	= NULL,
-	.setbrg	= s3c4510b_serial_setbrg,
-	.putc	= s3c4510b_serial_putc,
-	.puts	= s3c4510b_serial_puts,
-	.getc	= s3c4510b_serial_getc,
-	.tstc	= s3c4510b_serial_tstc,
-};
-
-void s3c4510b_serial_initialize(void)
-{
-	serial_register(&s3c4510b_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &s3c4510b_serial_drv;
-}
diff --git a/drivers/serial/s3c4510b_uart.h b/drivers/serial/s3c4510b_uart.h
deleted file mode 100644
index b06c76d84a..0000000000
--- a/drivers/serial/s3c4510b_uart.h
+++ /dev/null
@@ -1,109 +0,0 @@
-#ifndef __UART_H
-#define __UART_H
-
-/*
- * Copyright (c) 2004	Cucy Systems (http://www.cucy.com)
- * Curt Brune <curt@cucy.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
- *
- * Description:   S3C4510B UART register layout
- */
-
-/* UART LINE CONTROL register */
-typedef struct __BF_UART_LINE_CTRL {
-	u32    wordLen: 2;
-	u32      nStop: 1;
-	u32     parity: 3;
-	u32        clk: 1;
-	u32  infra_red: 1;
-	u32     unused:24;
-} BF_UART_LINE_CTRL;
-
-typedef union _UART_LINE_CTRL {
-	u32               ui;
-	BF_UART_LINE_CTRL bf;
-} UART_LINE_CTRL;
-
-/* UART CONTROL register */
-typedef struct __BF_UART_CTRL {
-	u32     rxMode: 2;
-	u32      rxIrq: 1;
-	u32     txMode: 2;
-	u32        DSR: 1;
-	u32  sendBreak: 1;
-	u32   loopBack: 1;
-	u32     unused:24;
-} BF_UART_CTRL;
-
-typedef union _UART_CTRL {
-	u32            ui;
-	BF_UART_CTRL   bf;
-} UART_CTRL;
-
-/* UART STATUS register */
-typedef struct __BF_UART_STAT {
-	u32      overrun: 1;
-	u32       parity: 1;
-	u32        frame: 1;
-	u32     breakIrq: 1;
-	u32          DTR: 1;
-	u32      rxReady: 1;
-	u32   txBufEmpty: 1;
-	u32   txComplete: 1;
-	u32       unused:24;
-} BF_UART_STAT;
-
-typedef union _UART_STAT {
-	u32            ui;
-	BF_UART_STAT   bf;
-} UART_STAT;
-
-/* UART BAUD_DIV register */
-typedef struct __BF_UART_BAUD_DIV {
-	u32      cnt1: 4;
-	u32      cnt0:12;
-	u32    unused:16;
-} BF_UART_BAUD_DIV;
-
-typedef union _UART_BAUD_DIV {
-	u32                ui;
-	BF_UART_BAUD_DIV   bf;
-} UART_BAUD_DIV;
-
-/* UART register block */
-typedef struct __UART {
-	volatile UART_LINE_CTRL  m_lineCtrl;
-	volatile UART_CTRL           m_ctrl;
-	volatile UART_STAT           m_stat;
-	volatile u32                   m_tx;
-	volatile u32                   m_rx;
-	volatile UART_BAUD_DIV    m_baudDiv;
-	volatile u32              m_baudCnt;
-	volatile u32              m_baudClk;
-} UART;
-
-#define NL          0x0A
-#define CR          0x0D
-#define BSP         0x08
-#define ESC         0x1B
-#define CTRLZ       0x1A
-#define RUBOUT      0x7F
-
-#endif
diff --git a/drivers/serial/s3c64xx.c b/drivers/serial/s3c64xx.c
index 9ab8a28d83..f53c2bf003 100644
--- a/drivers/serial/s3c64xx.c
+++ b/drivers/serial/s3c64xx.c
@@ -40,8 +40,6 @@ DECLARE_GLOBAL_DATA_PTR;
 #error "Bad: you didn't configure serial ..."
 #endif
 
-#define barrier() asm volatile("" ::: "memory")
-
 /*
  * The coefficient, used to calculate the baudrate on S3C6400 UARTs is
  * calculated as
@@ -166,19 +164,13 @@ static int s3c64xx_serial_tstc(void)
 	return uart->UTRSTAT & 0x1;
 }
 
-static void s3c64xx_serial_puts(const char *s)
-{
-	while (*s)
-		serial_putc(*s++);
-}
-
 static struct serial_device s3c64xx_serial_drv = {
 	.name	= "s3c64xx_serial",
 	.start	= s3c64xx_serial_init,
 	.stop	= NULL,
 	.setbrg	= s3c64xx_serial_setbrg,
 	.putc	= s3c64xx_serial_putc,
-	.puts	= s3c64xx_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= s3c64xx_serial_getc,
 	.tstc	= s3c64xx_serial_tstc,
 };
diff --git a/drivers/serial/serial.c b/drivers/serial/serial.c
index 5bbf3aeb44..f5f43a6ddc 100644
--- a/drivers/serial/serial.c
+++ b/drivers/serial/serial.c
@@ -26,16 +26,35 @@
 #include <stdio_dev.h>
 #include <post.h>
 #include <linux/compiler.h>
+#include <errno.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
 static struct serial_device *serial_devices;
 static struct serial_device *serial_current;
 
+/**
+ * serial_null() - Void registration routine of a serial driver
+ *
+ * This routine implements a void registration routine of a serial
+ * driver. The registration routine of a particular driver is aliased
+ * to this empty function in case the driver is not compiled into
+ * U-Boot.
+ */
 static void serial_null(void)
 {
 }
 
+/**
+ * serial_initfunc() - Forward declare of driver registration routine
+ * @name:	Name of the real driver registration routine.
+ *
+ * This macro expands onto forward declaration of a driver registration
+ * routine, which is then used below in serial_initialize() function.
+ * The declaration is made weak and aliases to serial_null() so in case
+ * the driver is not compiled in, the function is still declared and can
+ * be used, but aliases to serial_null() and thus is optimized away.
+ */
 #define serial_initfunc(name)					\
 	void name(void)						\
 		__attribute__((weak, alias("serial_null")));
@@ -77,7 +96,6 @@ serial_initfunc(lpc32xx_serial_initialize);
 serial_initfunc(mcf_serial_initialize);
 serial_initfunc(ns9750_serial_initialize);
 serial_initfunc(oc_serial_initialize);
-serial_initfunc(s3c4510b_serial_initialize);
 serial_initfunc(s3c64xx_serial_initialize);
 serial_initfunc(sandbox_serial_initialize);
 serial_initfunc(clps7111_serial_initialize);
@@ -85,15 +103,23 @@ serial_initfunc(imx_serial_initialize);
 serial_initfunc(ixp_serial_initialize);
 serial_initfunc(ks8695_serial_initialize);
 serial_initfunc(lh7a40x_serial_initialize);
-serial_initfunc(lpc2292_serial_initialize);
 serial_initfunc(max3100_serial_initialize);
 serial_initfunc(mxc_serial_initialize);
-serial_initfunc(netarm_serial_initialize);
 serial_initfunc(pl01x_serial_initialize);
 serial_initfunc(s3c44b0_serial_initialize);
 serial_initfunc(sa1100_serial_initialize);
 serial_initfunc(sh_serial_initialize);
 
+/**
+ * serial_register() - Register serial driver with serial driver core
+ * @dev:	Pointer to the serial driver structure
+ *
+ * This function registers the serial driver supplied via @dev with
+ * serial driver core, thus making U-Boot aware of it and making it
+ * available for U-Boot to use. On platforms that still require manual
+ * relocation of constant variables, relocation of the supplied structure
+ * is performed.
+ */
 void serial_register(struct serial_device *dev)
 {
 #ifdef CONFIG_NEEDS_MANUAL_RELOC
@@ -117,6 +143,15 @@ void serial_register(struct serial_device *dev)
 	serial_devices = dev;
 }
 
+/**
+ * serial_initialize() - Register all compiled-in serial port drivers
+ *
+ * This function registers all serial port drivers that are compiled
+ * into the U-Boot binary with the serial core, thus making them
+ * available to U-Boot to use. Lastly, this function assigns a default
+ * serial port to the serial core. That serial port is then used as a
+ * default output.
+ */
 void serial_initialize(void)
 {
 	mpc8xx_serial_initialize();
@@ -156,7 +191,6 @@ void serial_initialize(void)
 	mcf_serial_initialize();
 	ns9750_serial_initialize();
 	oc_serial_initialize();
-	s3c4510b_serial_initialize();
 	s3c64xx_serial_initialize();
 	sandbox_serial_initialize();
 	clps7111_serial_initialize();
@@ -164,10 +198,8 @@ void serial_initialize(void)
 	ixp_serial_initialize();
 	ks8695_serial_initialize();
 	lh7a40x_serial_initialize();
-	lpc2292_serial_initialize();
 	max3100_serial_initialize();
 	mxc_serial_initialize();
-	netarm_serial_initialize();
 	pl01x_serial_initialize();
 	s3c44b0_serial_initialize();
 	sa1100_serial_initialize();
@@ -176,6 +208,13 @@ void serial_initialize(void)
 	serial_assign(default_serial_console()->name);
 }
 
+/**
+ * serial_stdio_init() - Register serial ports with STDIO core
+ *
+ * This function generates a proxy driver for each serial port driver.
+ * These proxy drivers then register with the STDIO core, making the
+ * serial drivers available as STDIO devices.
+ */
 void serial_stdio_init(void)
 {
 	struct stdio_dev dev;
@@ -200,20 +239,38 @@ void serial_stdio_init(void)
 	}
 }
 
+/**
+ * serial_assign() - Select the serial output device by name
+ * @name:	Name of the serial driver to be used as default output
+ *
+ * This function configures the serial output multiplexing by
+ * selecting which serial device will be used as default. In case
+ * the STDIO "serial" device is selected as stdin/stdout/stderr,
+ * the serial device previously configured by this function will be
+ * used for the particular operation.
+ *
+ * Returns 0 on success, negative on error.
+ */
 int serial_assign(const char *name)
 {
 	struct serial_device *s;
 
 	for (s = serial_devices; s; s = s->next) {
-		if (strcmp(s->name, name) == 0) {
-			serial_current = s;
-			return 0;
-		}
+		if (strcmp(s->name, name))
+			continue;
+		serial_current = s;
+		return 0;
 	}
 
-	return 1;
+	return -EINVAL;
 }
 
+/**
+ * serial_reinit_all() - Reinitialize all compiled-in serial ports
+ *
+ * This function reinitializes all serial ports that are compiled
+ * into U-Boot by calling their serial_start() functions.
+ */
 void serial_reinit_all(void)
 {
 	struct serial_device *s;
@@ -222,60 +279,173 @@ void serial_reinit_all(void)
 		s->start();
 }
 
+/**
+ * get_current() - Return pointer to currently selected serial port
+ *
+ * This function returns a pointer to currently selected serial port.
+ * The currently selected serial port is altered by serial_assign()
+ * function.
+ *
+ * In case this function is called before relocation or before any serial
+ * port is configured, this function calls default_serial_console() to
+ * determine the serial port. Otherwise, the configured serial port is
+ * returned.
+ *
+ * Returns pointer to the currently selected serial port on success,
+ * NULL on error.
+ */
 static struct serial_device *get_current(void)
 {
 	struct serial_device *dev;
 
-	if (!(gd->flags & GD_FLG_RELOC) || !serial_current) {
+	if (!(gd->flags & GD_FLG_RELOC))
+		dev = default_serial_console();
+	else if (!serial_current)
 		dev = default_serial_console();
+	else
+		dev = serial_current;
 
-		/* We must have a console device */
-		if (!dev) {
+	/* We must have a console device */
+	if (!dev) {
 #ifdef CONFIG_SPL_BUILD
-			puts("Cannot find console\n");
-			hang();
+		puts("Cannot find console\n");
+		hang();
 #else
-			panic("Cannot find console\n");
+		panic("Cannot find console\n");
 #endif
-		}
-	} else
-		dev = serial_current;
+	}
+
 	return dev;
 }
 
+/**
+ * serial_init() - Initialize currently selected serial port
+ *
+ * This function initializes the currently selected serial port. This
+ * usually involves setting up the registers of that particular port,
+ * enabling clock and such. This function uses the get_current() call
+ * to determine which port is selected.
+ *
+ * Returns 0 on success, negative on error.
+ */
 int serial_init(void)
 {
 	return get_current()->start();
 }
 
+/**
+ * serial_setbrg() - Configure baud-rate of currently selected serial port
+ *
+ * This function configures the baud-rate of the currently selected
+ * serial port. The baud-rate is retrieved from global data within
+ * the serial port driver. This function uses the get_current() call
+ * to determine which port is selected.
+ *
+ * Returns 0 on success, negative on error.
+ */
 void serial_setbrg(void)
 {
 	get_current()->setbrg();
 }
 
+/**
+ * serial_getc() - Read character from currently selected serial port
+ *
+ * This function retrieves a character from currently selected serial
+ * port. In case there is no character waiting on the serial port,
+ * this function will block and wait for the character to appear. This
+ * function uses the get_current() call to determine which port is
+ * selected.
+ *
+ * Returns the character on success, negative on error.
+ */
 int serial_getc(void)
 {
 	return get_current()->getc();
 }
 
+/**
+ * serial_tstc() - Test if data is available on currently selected serial port
+ *
+ * This function tests if one or more characters are available on
+ * currently selected serial port. This function never blocks. This
+ * function uses the get_current() call to determine which port is
+ * selected.
+ *
+ * Returns positive if character is available, zero otherwise.
+ */
 int serial_tstc(void)
 {
 	return get_current()->tstc();
 }
 
+/**
+ * serial_putc() - Output character via currently selected serial port
+ * @c:	Single character to be output from the serial port.
+ *
+ * This function outputs a character via currently selected serial
+ * port. This character is passed to the serial port driver responsible
+ * for controlling the hardware. The hardware may still be in process
+ * of transmitting another character, therefore this function may block
+ * for a short amount of time. This function uses the get_current()
+ * call to determine which port is selected.
+ */
 void serial_putc(const char c)
 {
 	get_current()->putc(c);
 }
 
+/**
+ * serial_puts() - Output string via currently selected serial port
+ * @s:	Zero-terminated string to be output from the serial port.
+ *
+ * This function outputs a zero-terminated string via currently
+ * selected serial port. This function behaves as an accelerator
+ * in case the hardware can queue multiple characters for transfer.
+ * The whole string that is to be output is available to the function
+ * implementing the hardware manipulation. Transmitting the whole
+ * string may take some time, thus this function may block for some
+ * amount of time. This function uses the get_current() call to
+ * determine which port is selected.
+ */
 void serial_puts(const char *s)
 {
 	get_current()->puts(s);
 }
 
+/**
+ * default_serial_puts() - Output string by calling serial_putc() in loop
+ * @s:	Zero-terminated string to be output from the serial port.
+ *
+ * This function outputs a zero-terminated string by calling serial_putc()
+ * in a loop. Most drivers do not support queueing more than one byte for
+ * transfer, thus this function precisely implements their serial_puts().
+ *
+ * To optimize the number of get_current() calls, this function only
+ * calls get_current() once and then directly accesses the putc() call
+ * of the &struct serial_device .
+ */
+void default_serial_puts(const char *s)
+{
+	struct serial_device *dev = get_current();
+	while (*s)
+		dev->putc(*s++);
+}
+
 #if CONFIG_POST & CONFIG_SYS_POST_UART
 static const int bauds[] = CONFIG_SYS_BAUDRATE_TABLE;
 
+/**
+ * uart_post_test() - Test the currently selected serial port using POST
+ * @flags:	POST framework flags
+ *
+ * Do a loopback test of the currently selected serial port. This
+ * function is only useful in the context of the POST testing framwork.
+ * The serial port is firstly configured into loopback mode and then
+ * characters are sent through it.
+ *
+ * Returns 0 on success, value otherwise.
+ */
 /* Mark weak until post/cpu/.../uart.c migrate over */
 __weak
 int uart_post_test(int flags)
diff --git a/drivers/serial/serial_clps7111.c b/drivers/serial/serial_clps7111.c
deleted file mode 100644
index 65473e8608..0000000000
--- a/drivers/serial/serial_clps7111.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/*
- * (C) Copyright 2002-2004
- * Wolfgang Denk, DENX Software Engineering, <wd@denx.de>
- *
- * (C) Copyright 2002
- * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
- * Marius Groeger <mgroeger@sysgo.de>
- *
- * (C) Copyright 2002
- * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
- * Alex Zuepke <azu@sysgo.de>
- *
- * Copyright (C) 1999 2000 2001 Erik Mouw (J.A.K.Mouw@its.tudelft.nl)
- *
- * 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
- *
- */
-
-#include <common.h>
-#include <clps7111.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static void clps7111_serial_setbrg(void)
-{
-	unsigned int reg = 0;
-
-	switch (gd->baudrate) {
-	case   1200:	reg = 191;	break;
-	case   9600:	reg =  23;	break;
-	case  19200:	reg =  11;	break;
-	case  38400:	reg =   5;	break;
-	case  57600:	reg =   3;	break;
-	case 115200:	reg =   1;	break;
-	default:	hang ();	break;
-	}
-
-	/* init serial serial 1,2 */
-	IO_SYSCON1 = SYSCON1_UART1EN;
-	IO_SYSCON2 = SYSCON2_UART2EN;
-
-	reg |= UBRLCR_WRDLEN8;
-
-	IO_UBRLCR1 = reg;
-	IO_UBRLCR2 = reg;
-}
-
-
-/*
- * Initialise the serial port with the given baudrate. The settings
- * are always 8 data bits, no parity, 1 stop bit, no start bits.
- *
- */
-static int clps7111_serial_init(void)
-{
-	serial_setbrg ();
-
-	return (0);
-}
-
-
-/*
- * Output a single byte to the serial port.
- */
-static void clps7111_serial_putc(const char c)
-{
-	int tmo;
-
-	/* If \n, also do \r */
-	if (c == '\n')
-		serial_putc ('\r');
-
-	tmo = get_timer (0) + 1 * CONFIG_SYS_HZ;
-	while (IO_SYSFLG1 & SYSFLG1_UTXFF)
-		if (get_timer (0) > tmo)
-			break;
-
-	IO_UARTDR1 = c;
-}
-
-/*
- * Read a single byte from the serial port. Returns 1 on success, 0
- * otherwise. When the function is succesfull, the character read is
- * written into its argument c.
- */
-static int clps7111_serial_tstc(void)
-{
-	return !(IO_SYSFLG1 & SYSFLG1_URXFE);
-}
-
-/*
- * Read a single byte from the serial port. Returns 1 on success, 0
- * otherwise. When the function is succesfull, the character read is
- * written into its argument c.
- */
-static int clps7111_serial_getc(void)
-{
-	while (IO_SYSFLG1 & SYSFLG1_URXFE);
-
-	return IO_UARTDR1 & 0xff;
-}
-
-static void clps7111_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
-static struct serial_device clps7111_serial_drv = {
-	.name	= "clps7111_serial",
-	.start	= clps7111_serial_init,
-	.stop	= NULL,
-	.setbrg	= clps7111_serial_setbrg,
-	.putc	= clps7111_serial_putc,
-	.puts	= clps7111_serial_puts,
-	.getc	= clps7111_serial_getc,
-	.tstc	= clps7111_serial_tstc,
-};
-
-void clps7111_serial_initialize(void)
-{
-	serial_register(&clps7111_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &clps7111_serial_drv;
-}
diff --git a/drivers/serial/serial_imx.c b/drivers/serial/serial_imx.c
index 6c075b5b91..9b9be44c52 100644
--- a/drivers/serial/serial_imx.c
+++ b/drivers/serial/serial_imx.c
@@ -214,20 +214,13 @@ static int imx_serial_tstc(void)
 	return 1;
 }
 
-static void imx_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
 static struct serial_device imx_serial_drv = {
 	.name	= "imx_serial",
 	.start	= imx_serial_init,
 	.stop	= NULL,
 	.setbrg	= imx_serial_setbrg,
 	.putc	= imx_serial_putc,
-	.puts	= imx_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= imx_serial_getc,
 	.tstc	= imx_serial_tstc,
 };
diff --git a/drivers/serial/serial_ixp.c b/drivers/serial/serial_ixp.c
index c8b3658d47..09a3df4016 100644
--- a/drivers/serial/serial_ixp.c
+++ b/drivers/serial/serial_ixp.c
@@ -121,20 +121,13 @@ static int ixp_serial_getc(void)
 	return (char) RBR(CONFIG_SYS_IXP425_CONSOLE) & 0xff;
 }
 
-static void ixp_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
 static struct serial_device ixp_serial_drv = {
 	.name	= "ixp_serial",
 	.start	= ixp_serial_init,
 	.stop	= NULL,
 	.setbrg	= ixp_serial_setbrg,
 	.putc	= ixp_serial_putc,
-	.puts	= ixp_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= ixp_serial_getc,
 	.tstc	= ixp_serial_tstc,
 };
diff --git a/drivers/serial/serial_ks8695.c b/drivers/serial/serial_ks8695.c
index 60e8007201..8b1c974de9 100644
--- a/drivers/serial/serial_ks8695.c
+++ b/drivers/serial/serial_ks8695.c
@@ -102,13 +102,6 @@ static int ks8695_serial_tstc(void)
 	return 0;
 }
 
-static void ks8695_serial_puts(const char *s)
-{
-	char c;
-	while ((c = *s++) != 0)
-		serial_putc(c);
-}
-
 static int ks8695_serial_getc(void)
 {
 	volatile struct ks8695uart *uartp = KS8695_UART_ADDR;
@@ -124,7 +117,7 @@ static struct serial_device ks8695_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= ks8695_serial_setbrg,
 	.putc	= ks8695_serial_putc,
-	.puts	= ks8695_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= ks8695_serial_getc,
 	.tstc	= ks8695_serial_tstc,
 };
diff --git a/drivers/serial/serial_lh7a40x.c b/drivers/serial/serial_lh7a40x.c
deleted file mode 100644
index 6c9628581b..0000000000
--- a/drivers/serial/serial_lh7a40x.c
+++ /dev/null
@@ -1,204 +0,0 @@
-/*
- * (C) Copyright 2002
- * Gary Jennejohn, DENX Software Engineering, <garyj@denx.de>
- *
- * 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
- *
- */
-
-#include <common.h>
-#include <lh7a40x.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-#if defined(CONFIG_CONSOLE_UART1)
-# define UART_CONSOLE 1
-#elif defined(CONFIG_CONSOLE_UART2)
-# define UART_CONSOLE 2
-#elif defined(CONFIG_CONSOLE_UART3)
-# define UART_CONSOLE 3
-#else
-# error "No console configured ... "
-#endif
-
-static void lh7a40x_serial_setbrg(void)
-{
-	lh7a40x_uart_t* uart = LH7A40X_UART_PTR(UART_CONSOLE);
-	int i;
-	unsigned int reg = 0;
-
-	/*
-	 * userguide 15.1.2.4
-	 *
-	 * BAUDDIV is (UART_REF_FREQ/(16 X BAUD))-1
-	 *
-	 *   UART_REF_FREQ = external system clock input / 2 (Hz)
-	 *   BAUD is desired baudrate (bits/s)
-	 *
-	 *   NOTE: we add (divisor/2) to numerator to round for
-	 *         more precision
-	 */
-	reg = (((get_PLLCLK()/2) + ((16*gd->baudrate)/2)) / (16 * gd->baudrate)) - 1;
-	uart->brcon = reg;
-
-	for (i = 0; i < 100; i++);
-}
-
-/*
- * Initialise the serial port with the given baudrate. The settings
- * are always 8 data bits, no parity, 1 stop bit, no start bits.
- *
- */
-static int lh7a40x_serial_init(void)
-{
-	lh7a40x_uart_t* uart = LH7A40X_UART_PTR(UART_CONSOLE);
-
-	/* UART must be enabled before writing to any config registers */
-	uart->con |= (UART_EN);
-
-#ifdef CONFIG_CONSOLE_UART1
-	/* infrared disabled */
-	uart->con |= UART_SIRD;
-#endif
-	/* loopback disabled */
-	uart->con &= ~(UART_LBE);
-
-	/* modem lines and tx/rx polarities */
-	uart->con &= ~(UART_MXP | UART_TXP | UART_RXP);
-
-	/* FIFO enable, N81 */
-	uart->fcon = (UART_WLEN_8 | UART_FEN | UART_STP2_1);
-
-	/* set baudrate */
-	serial_setbrg ();
-
-	/* enable rx interrupt */
-	uart->inten |= UART_RI;
-
-	return (0);
-}
-
-/*
- * Read a single byte from the serial port. Returns 1 on success, 0
- * otherwise. When the function is succesfull, the character read is
- * written into its argument c.
- */
-static int lh7a40x_serial_getc(void)
-{
-	lh7a40x_uart_t* uart = LH7A40X_UART_PTR(UART_CONSOLE);
-
-	/* wait for character to arrive */
-	while (uart->status & UART_RXFE);
-
-	return(uart->data & 0xff);
-}
-
-#ifdef CONFIG_HWFLOW
-static int hwflow = 0; /* turned off by default */
-int hwflow_onoff(int on)
-{
-	switch(on) {
-	case 0:
-	default:
-		break; /* return current */
-	case 1:
-		hwflow = 1; /* turn on */
-		break;
-	case -1:
-		hwflow = 0; /* turn off */
-		break;
-	}
-	return hwflow;
-}
-#endif
-
-#ifdef CONFIG_MODEM_SUPPORT
-static int be_quiet = 0;
-void disable_putc(void)
-{
-	be_quiet = 1;
-}
-
-void enable_putc(void)
-{
-	be_quiet = 0;
-}
-#endif
-
-
-/*
- * Output a single byte to the serial port.
- */
-static void lh7a40x_serial_putc(const char c)
-{
-	lh7a40x_uart_t* uart = LH7A40X_UART_PTR(UART_CONSOLE);
-
-#ifdef CONFIG_MODEM_SUPPORT
-	if (be_quiet)
-		return;
-#endif
-
-	/* wait for room in the tx FIFO */
-	while (!(uart->status & UART_TXFE));
-
-#ifdef CONFIG_HWFLOW
-	/* Wait for CTS up */
-	while(hwflow && !(uart->status & UART_CTS));
-#endif
-
-	uart->data = c;
-
-	/* If \n, also do \r */
-	if (c == '\n')
-		serial_putc ('\r');
-}
-
-/*
- * Test whether a character is in the RX buffer
- */
-static int lh7a40x_serial_tstc(void)
-{
-	lh7a40x_uart_t* uart = LH7A40X_UART_PTR(UART_CONSOLE);
-
-	return(!(uart->status & UART_RXFE));
-}
-
-static void lh7a40x_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
-static struct serial_device lh7a40x_serial_drv = {
-	.name	= "lh7a40x_serial",
-	.start	= lh7a40x_serial_init,
-	.stop	= NULL,
-	.setbrg	= lh7a40x_serial_setbrg,
-	.putc	= lh7a40x_serial_putc,
-	.puts	= lh7a40x_serial_puts,
-	.getc	= lh7a40x_serial_getc,
-	.tstc	= lh7a40x_serial_tstc,
-};
-
-void lh7a40x_serial_initialize(void)
-{
-	serial_register(&lh7a40x_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &lh7a40x_serial_drv;
-}
diff --git a/drivers/serial/serial_lpc2292.c b/drivers/serial/serial_lpc2292.c
deleted file mode 100644
index fcab20280e..0000000000
--- a/drivers/serial/serial_lpc2292.c
+++ /dev/null
@@ -1,124 +0,0 @@
-/*
- * (C) Copyright 2002-2004
- * Wolfgang Denk, DENX Software Engineering, <wd@denx.de>
- *
- * (C) Copyright 2002
- * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
- * Marius Groeger <mgroeger@sysgo.de>
- *
- * (C) Copyright 2002
- * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
- * Alex Zuepke <azu@sysgo.de>
- *
- * Copyright (C) 1999 2000 2001 Erik Mouw (J.A.K.Mouw@its.tudelft.nl)
- *
- * 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
- *
- */
-
-#include <common.h>
-#include <asm/arch/hardware.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static void lpc2292_serial_setbrg(void)
-{
-	unsigned short divisor = 0;
-
-	switch (gd->baudrate) {
-	case   1200:	divisor = 3072;	break;
-	case   9600:	divisor =  384;	break;
-	case  19200:	divisor =  192;	break;
-	case  38400:	divisor =   96;	break;
-	case  57600:	divisor =   64;	break;
-	case 115200:	divisor =   32;	break;
-	default:	hang ();	break;
-	}
-
-	/* init serial UART0 */
-	PUT8(U0LCR, 0);
-	PUT8(U0IER, 0);
-	PUT8(U0LCR, 0x80);	/* DLAB=1 */
-	PUT8(U0DLL, (unsigned char)(divisor & 0x00FF));
-	PUT8(U0DLM, (unsigned char)(divisor >> 8));
-	PUT8(U0LCR, 0x03);	/* 8N1, DLAB=0  */
-	PUT8(U0FCR, 1);		/* Enable RX and TX FIFOs */
-}
-
-static int lpc2292_serial_init(void)
-{
-	unsigned long pinsel0;
-
-	serial_setbrg ();
-
-	pinsel0 = GET32(PINSEL0);
-	pinsel0 &= ~(0x00000003);
-	pinsel0 |= 5;
-	PUT32(PINSEL0, pinsel0);
-
-	return (0);
-}
-
-static void lpc2292_serial_putc(const char c)
-{
-	if (c == '\n')
-	{
-		while((GET8(U0LSR) & (1<<5)) == 0); /* Wait for empty U0THR */
-		PUT8(U0THR, '\r');
-	}
-
-	while((GET8(U0LSR) & (1<<5)) == 0); /* Wait for empty U0THR */
-	PUT8(U0THR, c);
-}
-
-static int lpc2292_serial_getc(void)
-{
-	while((GET8(U0LSR) & 1) == 0);
-	return GET8(U0RBR);
-}
-
-static void lpc2292_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
-/* Test if there is a byte to read */
-static int lpc2292_serial_tstc(void)
-{
-	return (GET8(U0LSR) & 1);
-}
-
-static struct serial_device lpc2292_serial_drv = {
-	.name	= "lpc2292_serial",
-	.start	= lpc2292_serial_init,
-	.stop	= NULL,
-	.setbrg	= lpc2292_serial_setbrg,
-	.putc	= lpc2292_serial_putc,
-	.puts	= lpc2292_serial_puts,
-	.getc	= lpc2292_serial_getc,
-	.tstc	= lpc2292_serial_tstc,
-};
-
-void lpc2292_serial_initialize(void)
-{
-	serial_register(&lpc2292_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &lpc2292_serial_drv;
-}
diff --git a/drivers/serial/serial_mxc.c b/drivers/serial/serial_mxc.c
index b0612f5aca..9227d64088 100644
--- a/drivers/serial/serial_mxc.c
+++ b/drivers/serial/serial_mxc.c
@@ -187,13 +187,6 @@ static int mxc_serial_tstc(void)
 	return 1;
 }
 
-static void mxc_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
 /*
  * Initialise the serial port with the given baudrate. The settings
  * are always 8 data bits, no parity, 1 stop bit, no start bits.
@@ -228,7 +221,7 @@ static struct serial_device mxc_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= mxc_serial_setbrg,
 	.putc	= mxc_serial_putc,
-	.puts	= mxc_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= mxc_serial_getc,
 	.tstc	= mxc_serial_tstc,
 };
diff --git a/drivers/serial/serial_netarm.c b/drivers/serial/serial_netarm.c
deleted file mode 100644
index d30adc3183..0000000000
--- a/drivers/serial/serial_netarm.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- * Serial Port stuff - taken from Linux
- *
- * (C) Copyright 2002
- * MAZeT GmbH <www.mazet.de>
- * Stephan Linz <linz@mazet.de>, <linz@li-pro.net>
- *
- * (c) 2004
- * IMMS gGmbH <www.imms.de>
- * Thomas Elste <info@elste.org>
- *
- * 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
- *
- */
-
-#include <common.h>
-#include <asm/hardware.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-#define PORTA	(*(volatile unsigned int *)(NETARM_GEN_MODULE_BASE + NETARM_GEN_PORTA))
-#if !defined(CONFIG_NETARM_NS7520)
-#define PORTB	(*(volatile unsigned int *)(NETARM_GEN_MODULE_BASE + NETARM_GEN_PORTB))
-#else
-#define PORTC	(*(volatile unsigned int *)(NETARM_GEN_MODULE_BASE + NETARM_GEN_PORTC))
-#endif
-
-/* wait until transmitter is ready for another character */
-#define TXWAITRDY(registers)							\
-{										\
-	ulong tmo = get_timer(0) + 1 * CONFIG_SYS_HZ;					\
-	while (((registers)->status_a & NETARM_SER_STATA_TX_RDY) == 0 )	{	\
-		if (get_timer(0) > tmo)						\
-			break;							\
-	}									\
-}
-
-
-volatile netarm_serial_channel_t *serial_reg_ch1 = get_serial_channel(0);
-volatile netarm_serial_channel_t *serial_reg_ch2 = get_serial_channel(1);
-
-extern void _netarm_led_FAIL1(void);
-
-/*
- * Setup both serial i/f with given baudrate
- */
-static void netarm_serial_setbrg(void)
-{
-	/* set 0 ... make sure pins are configured for serial */
-#if !defined(CONFIG_NETARM_NS7520)
-	PORTA = PORTB =
-		NETARM_GEN_PORT_MODE (0xef) | NETARM_GEN_PORT_DIR (0xe0);
-#else
-	PORTA = NETARM_GEN_PORT_MODE (0xef) | NETARM_GEN_PORT_DIR (0xe0);
-	PORTC = NETARM_GEN_PORT_CSF (0xef) | NETARM_GEN_PORT_MODE (0xef) | NETARM_GEN_PORT_DIR (0xe0);
-#endif
-
-	/* first turn em off */
-	serial_reg_ch1->ctrl_a = serial_reg_ch2->ctrl_a = 0;
-
-	/* clear match register, we don't need it */
-	serial_reg_ch1->rx_match = serial_reg_ch2->rx_match = 0;
-
-	/* setup bit rate generator and rx buffer gap timer (1 byte only) */
-	if ((gd->baudrate >= MIN_BAUD_RATE)
-	    && (gd->baudrate <= MAX_BAUD_RATE)) {
-		serial_reg_ch1->bitrate = serial_reg_ch2->bitrate =
-			NETARM_SER_BR_X16 (gd->baudrate);
-		serial_reg_ch1->rx_buf_timer = serial_reg_ch2->rx_buf_timer =
-			0;
-		serial_reg_ch1->rx_char_timer = serial_reg_ch2->rx_char_timer =
-			NETARM_SER_RXGAP (gd->baudrate);
-	} else {
-		hang ();
-	}
-
-	/* setup port mode */
-	serial_reg_ch1->ctrl_b = serial_reg_ch2->ctrl_b =
-		( NETARM_SER_CTLB_RCGT_EN |
-		  NETARM_SER_CTLB_UART_MODE);
-	serial_reg_ch1->ctrl_a = serial_reg_ch2->ctrl_a =
-		( NETARM_SER_CTLA_ENABLE |
-		  NETARM_SER_CTLA_P_NONE |
-		  /* see errata */
-		  NETARM_SER_CTLA_2STOP |
-		  NETARM_SER_CTLA_8BITS |
-		  NETARM_SER_CTLA_DTR_EN |
-		  NETARM_SER_CTLA_RTS_EN);
-}
-
-
-/*
- * Initialise the serial port with the given baudrate. The settings
- * are always 8 data bits, no parity, 1 stop bit, no start bits.
- */
-static int netarm_serial_init(void)
-{
-	serial_setbrg ();
-	return 0;
-}
-
-
-/*
- * Output a single byte to the serial port.
- */
-static void netarm_serial_putc(const char c)
-{
-	volatile unsigned char *fifo;
-
-	/* If \n, also do \r */
-	if (c == '\n')
-		serial_putc ('\r');
-
-	fifo = (volatile unsigned char *) &(serial_reg_ch1->fifo);
-	TXWAITRDY (serial_reg_ch1);
-	*fifo = c;
-}
-
-/*
- * Test of a single byte from the serial port. Returns 1 on success, 0
- * otherwise.
- */
-static int netarm_serial_tstc(void)
-{
-	return serial_reg_ch1->status_a & NETARM_SER_STATA_RX_RDY;
-}
-
-/*
- * Read a single byte from the serial port. Returns 1 on success, 0
- * otherwise.
- */
-static int netarm_serial_getc(void)
-{
-	unsigned int ch_uint;
-	volatile unsigned int *fifo;
-	volatile unsigned char *fifo_char = NULL;
-	int buf_count = 0;
-
-	while (!(serial_reg_ch1->status_a & NETARM_SER_STATA_RX_RDY))
-		/* NOP */ ;
-
-	fifo = (volatile unsigned int *) &(serial_reg_ch1->fifo);
-	fifo_char = (unsigned char *) &ch_uint;
-	ch_uint = *fifo;
-
-	buf_count = NETARM_SER_STATA_RXFDB (serial_reg_ch1->status_a);
-	switch (buf_count) {
-	case NETARM_SER_STATA_RXFDB_4BYTES:
-		buf_count = 4;
-		break;
-	case NETARM_SER_STATA_RXFDB_3BYTES:
-		buf_count = 3;
-		break;
-	case NETARM_SER_STATA_RXFDB_2BYTES:
-		buf_count = 2;
-		break;
-	case NETARM_SER_STATA_RXFDB_1BYTES:
-		buf_count = 1;
-		break;
-	default:
-		/* panic, be never here */
-		break;
-	}
-
-	serial_reg_ch1->status_a |= NETARM_SER_STATA_RX_CLOSED;
-
-	return ch_uint & 0xff;
-}
-
-static void netarm_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
-static struct serial_device netarm_serial_drv = {
-	.name	= "netarm_serial",
-	.start	= netarm_serial_init,
-	.stop	= NULL,
-	.setbrg	= netarm_serial_setbrg,
-	.putc	= netarm_serial_putc,
-	.puts	= netarm_serial_puts,
-	.getc	= netarm_serial_getc,
-	.tstc	= netarm_serial_tstc,
-};
-
-void netarm_serial_initialize(void)
-{
-	serial_register(&netarm_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &netarm_serial_drv;
-}
diff --git a/drivers/serial/serial_ns16550.c b/drivers/serial/serial_ns16550.c
index b5d12481ca..c1c0134bcb 100644
--- a/drivers/serial/serial_ns16550.c
+++ b/drivers/serial/serial_ns16550.c
@@ -34,7 +34,7 @@
 DECLARE_GLOBAL_DATA_PTR;
 
 #if !defined(CONFIG_CONS_INDEX)
-#elif (CONFIG_CONS_INDEX < 1) || (CONFIG_CONS_INDEX > 4)
+#elif (CONFIG_CONS_INDEX < 1) || (CONFIG_CONS_INDEX > 6)
 #error	"Invalid console index value."
 #endif
 
@@ -46,12 +46,16 @@ DECLARE_GLOBAL_DATA_PTR;
 #error	"Console port 3 defined but not configured."
 #elif CONFIG_CONS_INDEX == 4 && !defined(CONFIG_SYS_NS16550_COM4)
 #error	"Console port 4 defined but not configured."
+#elif CONFIG_CONS_INDEX == 5 && !defined(CONFIG_SYS_NS16550_COM5)
+#error	"Console port 5 defined but not configured."
+#elif CONFIG_CONS_INDEX == 6 && !defined(CONFIG_SYS_NS16550_COM6)
+#error	"Console port 6 defined but not configured."
 #endif
 
 /* Note: The port number specified in the functions is 1 based.
  *	 the array is 0 based.
  */
-static NS16550_t serial_ports[4] = {
+static NS16550_t serial_ports[6] = {
 #ifdef CONFIG_SYS_NS16550_COM1
 	(NS16550_t)CONFIG_SYS_NS16550_COM1,
 #else
@@ -68,7 +72,17 @@ static NS16550_t serial_ports[4] = {
 	NULL,
 #endif
 #ifdef CONFIG_SYS_NS16550_COM4
-	(NS16550_t)CONFIG_SYS_NS16550_COM4
+	(NS16550_t)CONFIG_SYS_NS16550_COM4,
+#else
+	NULL,
+#endif
+#ifdef CONFIG_SYS_NS16550_COM5
+	(NS16550_t)CONFIG_SYS_NS16550_COM5,
+#else
+	NULL,
+#endif
+#ifdef CONFIG_SYS_NS16550_COM6
+	(NS16550_t)CONFIG_SYS_NS16550_COM6
 #else
 	NULL
 #endif
@@ -78,21 +92,33 @@ static NS16550_t serial_ports[4] = {
 
 /* Multi serial device functions */
 #define DECLARE_ESERIAL_FUNCTIONS(port) \
-    int  eserial##port##_init (void) {\
-	int clock_divisor; \
-	clock_divisor = calc_divisor(serial_ports[port-1]); \
-	NS16550_init(serial_ports[port-1], clock_divisor); \
-	return(0);}\
-    void eserial##port##_setbrg (void) {\
-	serial_setbrg_dev(port);}\
-    int  eserial##port##_getc (void) {\
-	return serial_getc_dev(port);}\
-    int  eserial##port##_tstc (void) {\
-	return serial_tstc_dev(port);}\
-    void eserial##port##_putc (const char c) {\
-	serial_putc_dev(port, c);}\
-    void eserial##port##_puts (const char *s) {\
-	serial_puts_dev(port, s);}
+	static int  eserial##port##_init(void) \
+	{ \
+		int clock_divisor; \
+		clock_divisor = calc_divisor(serial_ports[port-1]); \
+		NS16550_init(serial_ports[port-1], clock_divisor); \
+		return 0 ; \
+	} \
+	static void eserial##port##_setbrg(void) \
+	{ \
+		serial_setbrg_dev(port); \
+	} \
+	static int  eserial##port##_getc(void) \
+	{ \
+		return serial_getc_dev(port); \
+	} \
+	static int  eserial##port##_tstc(void) \
+	{ \
+		return serial_tstc_dev(port); \
+	} \
+	static void eserial##port##_putc(const char c) \
+	{ \
+		serial_putc_dev(port, c); \
+	} \
+	static void eserial##port##_puts(const char *s) \
+	{ \
+		serial_puts_dev(port, s); \
+	}
 
 /* Serial device descriptor */
 #define INIT_ESERIAL_STRUCTURE(port, __name) {	\
@@ -231,6 +257,12 @@ struct serial_device eserial3_device =
 DECLARE_ESERIAL_FUNCTIONS(4);
 struct serial_device eserial4_device =
 	INIT_ESERIAL_STRUCTURE(4, "eserial3");
+DECLARE_ESERIAL_FUNCTIONS(5);
+struct serial_device eserial5_device =
+	INIT_ESERIAL_STRUCTURE(5, "eserial4");
+DECLARE_ESERIAL_FUNCTIONS(6);
+struct serial_device eserial6_device =
+	INIT_ESERIAL_STRUCTURE(6, "eserial5");
 
 __weak struct serial_device *default_serial_console(void)
 {
@@ -242,6 +274,10 @@ __weak struct serial_device *default_serial_console(void)
 	return &eserial3_device;
 #elif CONFIG_CONS_INDEX == 4
 	return &eserial4_device;
+#elif CONFIG_CONS_INDEX == 5
+	return &eserial5_device;
+#elif CONFIG_CONS_INDEX == 6
+	return &eserial6_device;
 #else
 #error "Bad CONFIG_CONS_INDEX."
 #endif
@@ -261,4 +297,10 @@ void ns16550_serial_initialize(void)
 #if defined(CONFIG_SYS_NS16550_COM4)
 	serial_register(&eserial4_device);
 #endif
+#if defined(CONFIG_SYS_NS16550_COM5)
+	serial_register(&eserial5_device);
+#endif
+#if defined(CONFIG_SYS_NS16550_COM6)
+	serial_register(&eserial6_device);
+#endif
 }
diff --git a/drivers/serial/serial_pl01x.c b/drivers/serial/serial_pl01x.c
index 7db7b65c3f..b331be794b 100644
--- a/drivers/serial/serial_pl01x.c
+++ b/drivers/serial/serial_pl01x.c
@@ -179,13 +179,6 @@ static void pl01x_serial_putc(const char c)
 	pl01x_putc (CONSOLE_PORT, c);
 }
 
-static void pl01x_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
 static int pl01x_serial_getc(void)
 {
 	return pl01x_getc (CONSOLE_PORT);
@@ -259,7 +252,7 @@ static struct serial_device pl01x_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= pl01x_serial_setbrg,
 	.putc	= pl01x_serial_putc,
-	.puts	= pl01x_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= pl01x_serial_getc,
 	.tstc	= pl01x_serial_tstc,
 };
diff --git a/drivers/serial/serial_s3c44b0.c b/drivers/serial/serial_s3c44b0.c
index a4428e0f47..9cae843088 100644
--- a/drivers/serial/serial_s3c44b0.c
+++ b/drivers/serial/serial_s3c44b0.c
@@ -209,20 +209,13 @@ static int s3c44b0_serial_getc(void)
 	}
 }
 
-static void s3c44b0_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
 static struct serial_device s3c44b0_serial_drv = {
 	.name	= "s3c44b0_serial",
 	.start	= s3c44b0_serial_init,
 	.stop	= NULL,
 	.setbrg	= s3c44b0_serial_setbrg,
 	.putc	= s3c44b0_serial_putc,
-	.puts	= s3c44b0_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= s3c44b0_serial_getc,
 	.tstc	= s3c44b0_serial_tstc,
 };
diff --git a/drivers/serial/serial_sa1100.c b/drivers/serial/serial_sa1100.c
index c6b34db7c6..3c0f4c59fb 100644
--- a/drivers/serial/serial_sa1100.c
+++ b/drivers/serial/serial_sa1100.c
@@ -153,20 +153,13 @@ static int sa1100_serial_getc(void)
 #endif
 }
 
-static void sa1100_serial_puts(const char *s)
-{
-	while (*s) {
-		serial_putc (*s++);
-	}
-}
-
 static struct serial_device sa1100_serial_drv = {
 	.name	= "sa1100_serial",
 	.start	= sa1100_serial_init,
 	.stop	= NULL,
 	.setbrg	= sa1100_serial_setbrg,
 	.putc	= sa1100_serial_putc,
-	.puts	= sa1100_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= sa1100_serial_getc,
 	.tstc	= sa1100_serial_tstc,
 };
diff --git a/drivers/serial/serial_sh.c b/drivers/serial/serial_sh.c
index 1ddfc7d87f..3c931d0212 100644
--- a/drivers/serial/serial_sh.c
+++ b/drivers/serial/serial_sh.c
@@ -136,13 +136,6 @@ static void sh_serial_putc(const char c)
 	serial_raw_putc(c);
 }
 
-static void sh_serial_puts(const char *s)
-{
-	char c;
-	while ((c = *s++) != 0)
-		serial_putc(c);
-}
-
 static int sh_serial_tstc(void)
 {
 	return serial_rx_fifo_level() ? 1 : 0;
@@ -196,7 +189,7 @@ static struct serial_device sh_serial_drv = {
 	.stop	= NULL,
 	.setbrg	= sh_serial_setbrg,
 	.putc	= sh_serial_putc,
-	.puts	= sh_serial_puts,
+	.puts	= default_serial_puts,
 	.getc	= sh_serial_getc,
 	.tstc	= sh_serial_tstc,
 };
diff --git a/drivers/spi/fsl_espi.c b/drivers/spi/fsl_espi.c
index a1ebd330d7..eb99e90bec 100644
--- a/drivers/spi/fsl_espi.c
+++ b/drivers/spi/fsl_espi.c
@@ -216,10 +216,8 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *data_out,
 			return 1;
 		}
 		memcpy(buffer, cmd_buf, cmd_len);
-		if (cmd_len != 1) {
-			if (data_in == NULL)
-				memcpy(buffer + cmd_len, data_out, data_len);
-		}
+		if (data_in == NULL)
+			memcpy(buffer + cmd_len, data_out, data_len);
 		break;
 	case SPI_XFER_BEGIN | SPI_XFER_END:
 		len = data_len;
diff --git a/drivers/spi/omap3_spi.c b/drivers/spi/omap3_spi.c
index e40a632caa..6791a7e0ec 100644
--- a/drivers/spi/omap3_spi.c
+++ b/drivers/spi/omap3_spi.c
@@ -173,14 +173,13 @@ int spi_claim_bus(struct spi_slave *slave)
 	/* standard 4-wire master mode:	SCK, MOSI/out, MISO/in, nCS
 	 * REVISIT: this controller could support SPI_3WIRE mode.
 	 */
-#ifdef CONFIG_AM33XX
+#ifdef CONFIG_OMAP3_SPI_D0_D1_SWAPPED
 	/*
-	 * The reference design on AM33xx has D0 and D1 wired up opposite
-	 * of how it has been done on previous platforms.  We assume that
-	 * custom hardware will also follow this convention.
+	 * Some boards have D0 wired as MOSI / D1 as MISO instead of
+	 * The normal D0 as MISO / D1 as MOSI.
 	 */
-	conf &= OMAP3_MCSPI_CHCONF_DPE0;
-	conf |= ~(OMAP3_MCSPI_CHCONF_IS|OMAP3_MCSPI_CHCONF_DPE1);
+	conf &= ~OMAP3_MCSPI_CHCONF_DPE0;
+	conf |= OMAP3_MCSPI_CHCONF_IS|OMAP3_MCSPI_CHCONF_DPE1;
 #else
 	conf &= ~(OMAP3_MCSPI_CHCONF_IS|OMAP3_MCSPI_CHCONF_DPE1);
 	conf |= OMAP3_MCSPI_CHCONF_DPE0;
diff --git a/drivers/usb/gadget/ether.c b/drivers/usb/gadget/ether.c
index 1e187e5b52..8b24e00e27 100644
--- a/drivers/usb/gadget/ether.c
+++ b/drivers/usb/gadget/ether.c
@@ -1978,8 +1978,8 @@ static int is_eth_addr_valid(char *str)
 			p = q;
 		}
 
-		if (i == 6) /* it looks ok */
-			return 1;
+		/* Now check the contents. */
+		return is_valid_ether_addr(ea);
 	}
 	return 0;
 }
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index 5d7b638b38..02cae0fc75 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -58,12 +58,6 @@
 #define	gadget_is_sa1100(g)	0
 #endif
 
-#ifdef CONFIG_USB_GADGET_LH7A40X
-#define	gadget_is_lh7a40x(g)	(!strcmp("lh7a40x_udc", (g)->name))
-#else
-#define	gadget_is_lh7a40x(g)	0
-#endif
-
 /* handhelds.org tree (?) */
 #ifdef CONFIG_USB_GADGET_MQ11XX
 #define	gadget_is_mq11xx(g)	(!strcmp("mq11xx_udc", (g)->name))
@@ -195,33 +189,31 @@ static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x07;
 	else if (gadget_is_omap(gadget))
 		return 0x08;
-	else if (gadget_is_lh7a40x(gadget))
-		return 0x09;
 	else if (gadget_is_n9604(gadget))
-		return 0x10;
+		return 0x09;
 	else if (gadget_is_pxa27x(gadget))
-		return 0x11;
+		return 0x10;
 	else if (gadget_is_s3c2410(gadget))
-		return 0x12;
+		return 0x11;
 	else if (gadget_is_at91(gadget))
-		return 0x13;
+		return 0x12;
 	else if (gadget_is_imx(gadget))
-		return 0x14;
+		return 0x13;
 	else if (gadget_is_musbhsfc(gadget))
-		return 0x15;
+		return 0x14;
 	else if (gadget_is_musbhdrc(gadget))
-		return 0x16;
+		return 0x15;
 	else if (gadget_is_mpc8272(gadget))
-		return 0x17;
+		return 0x16;
 	else if (gadget_is_atmel_usba(gadget))
-		return 0x18;
+		return 0x17;
 	else if (gadget_is_fsl_usb2(gadget))
-		return 0x19;
+		return 0x18;
 	else if (gadget_is_amd5536udc(gadget))
-		return 0x20;
+		return 0x19;
 	else if (gadget_is_m66592(gadget))
-		return 0x21;
+		return 0x20;
 	else if (gadget_is_mv(gadget))
-		return 0x22;
+		return 0x21;
 	return -ENOENT;
 }
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index bcb4662c47..6c94794929 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -33,6 +33,7 @@ COBJS-$(CONFIG_USB_ISP116X_HCD) += isp116x-hcd.o
 COBJS-$(CONFIG_USB_R8A66597_HCD) += r8a66597-hcd.o
 COBJS-$(CONFIG_USB_S3C64XX) += s3c64xx-hcd.o
 COBJS-$(CONFIG_USB_SL811HS) += sl811-hcd.o
+COBJS-$(CONFIG_USB_OHCI_S3C24XX) += ohci-s3c24xx.o
 
 # echi
 COBJS-$(CONFIG_USB_EHCI) += ehci-hcd.o
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 7b8f033b03..f54b408966 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -30,6 +30,18 @@
 
 #include "ehci.h"
 
+/* Check USB PHY clock valid */
+static int usb_phy_clk_valid(struct usb_ehci *ehci)
+{
+	if (!((in_be32(&ehci->control) & PHY_CLK_VALID) ||
+			in_be32(&ehci->prictrl))) {
+		printf("USB PHY clock invalid!\n");
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
 /*
  * Create the appropriate control structures to manage
  * a new EHCI host controller.
@@ -82,18 +94,16 @@ int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor)
 		udelay(1000); /* delay required for PHY Clk to appear */
 #endif
 		out_le32(&(*hcor)->or_portsc[0], PORT_PTS_UTMI);
+		setbits_be32(&ehci->control, USB_EN);
 	} else {
-#if defined(CONFIG_SYS_FSL_USB_INTERNAL_UTMI_PHY)
-		clrbits_be32(&ehci->control, UTMI_PHY_EN);
 		setbits_be32(&ehci->control, PHY_CLK_SEL_ULPI);
+		clrsetbits_be32(&ehci->control, UTMI_PHY_EN, USB_EN);
 		udelay(1000); /* delay required for PHY Clk to appear */
-#endif
+		if (!usb_phy_clk_valid(ehci))
+			return -EINVAL;
 		out_le32(&(*hcor)->or_portsc[0], PORT_PTS_ULPI);
 	}
 
-	/* Enable interface. */
-	setbits_be32(&ehci->control, USB_EN);
-
 	out_be32(&ehci->prictrl, 0x0000000c);
 	out_be32(&ehci->age_cnt_limit, 0x00000040);
 	out_be32(&ehci->sictrl, 0x00000001);
diff --git a/drivers/usb/host/ohci-s3c24xx.c b/drivers/usb/host/ohci-s3c24xx.c
new file mode 100644
index 0000000000..03cd4c3d2a
--- /dev/null
+++ b/drivers/usb/host/ohci-s3c24xx.c
@@ -0,0 +1,1801 @@
+/*
+ * URB OHCI HCD (Host Controller Driver) for USB on the S3C2400.
+ *
+ * (C) Copyright 2003
+ * Gary Jennejohn, DENX Software Engineering <garyj@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
+ *
+ * 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 - this driver is intended for use with USB Mass Storage Devices
+ *     (BBB) ONLY. There is NO support for Interrupt or Isochronous pipes!
+ */
+
+#include <common.h>
+/* #include <pci.h> no PCI on the S3C24X0 */
+
+#if defined(CONFIG_USB_OHCI) && defined(CONFIG_S3C24X0)
+
+#include <asm/arch/s3c24x0_cpu.h>
+#include <asm/io.h>
+#include <malloc.h>
+#include <usb.h>
+#include "ohci-s3c24xx.h"
+
+#define OHCI_USE_NPS		/* force NoPowerSwitching mode */
+#undef OHCI_VERBOSE_DEBUG	/* not always helpful */
+
+
+/* For initializing controller (mask in an HCFS mode too) */
+#define	OHCI_CONTROL_INIT \
+	(OHCI_CTRL_CBSR & 0x3) | OHCI_CTRL_IE | OHCI_CTRL_PLE
+
+#define min_t(type, x, y) \
+	({ type __x = (x); type __y = (y); __x < __y ? __x : __y; })
+
+#undef DEBUG
+#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
+
+#define m16_swap(x) swap_16(x)
+#define m32_swap(x) swap_32(x)
+
+/* global struct ohci */
+static struct ohci 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;
+/* urb_priv */
+struct urb_priv urb_priv;
+/* RHSC flag */
+int got_rhsc;
+/* device which was disconnected */
+struct usb_device *devgone;
+/* flag guarding URB transation */
+int urb_finished = 0;
+
+/*-------------------------------------------------------------------------*/
+
+/* 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, struct urb_priv *urb,
+			  int interval);
+
+/*-------------------------------------------------------------------------*
+ * URB support functions
+ *-------------------------------------------------------------------------*/
+
+/* free HCD-private data associated with this URB */
+
+static void urb_free_priv(struct urb_priv *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;
+			}
+		}
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+#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(struct usb_device *dev, unsigned long pipe, void *buffer,
+		      int transfer_len, struct devrequest *setup, char *str,
+		      int small)
+{
+	struct urb_priv *purb = &urb_priv;
+
+	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->actual_length, 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->actual_length, transfer_len);
+			len = usb_pipeout(pipe) ?
+			    transfer_len : purb->actual_length;
+			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(struct ohci *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--) {
+			struct ed *ed = (struct ed *) 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)
+{
+	struct ed *edp = (struct ed *) 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(struct ohci *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(struct ohci *controller, int verbose)
+{
+	__u32 temp, ndp, i;
+
+	temp = roothub_a(controller);
+	ndp = (temp & RH_A_NDP);
+
+	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(struct ohci *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(struct usb_device *dev, unsigned long pipe, void *buffer,
+		     int transfer_len, struct devrequest *setup, int interval)
+{
+	struct ohci *ohci;
+	struct ed *ed;
+	struct urb_priv *purb_priv;
+	int i, size = 0;
+
+	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;
+	}
+
+	/* if we have an unfinished URB from previous transaction let's
+	 * fail and scream as quickly as possible so as not to corrupt
+	 * further communication */
+	if (!urb_finished) {
+		err("sohci_submit_job: URB NOT FINISHED");
+		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 */
+	ed = ep_add_ed(dev, pipe);
+	if (!ed) {
+		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;
+	}
+
+	if (size >= (N_URB_TD - 1)) {
+		err("need %d TDs, only have %d", size, N_URB_TD);
+		return -1;
+	}
+	purb_priv = &urb_priv;
+	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;
+}
+
+/*-------------------------------------------------------------------------*/
+
+#ifdef DEBUG
+/* tell us the current USB frame number */
+
+static int sohci_get_current_frame_number(struct usb_device *usb_dev)
+{
+	struct ohci *ohci = &gohci;
+
+	return m16_swap(ohci->hcca->frame_no);
+}
+#endif
+
+/*-------------------------------------------------------------------------*
+ * ED handling functions
+ *-------------------------------------------------------------------------*/
+
+/* link an ed into one of the HC chains */
+
+static int ep_link(struct ohci *ohci, struct ed *edi)
+{
+	struct ed *ed = edi;
+
+	ed->state = ED_OPER;
+
+	switch (ed->type) {
+	case PIPE_CONTROL:
+		ed->hwNextED = 0;
+		if (ohci->ed_controltail == NULL) {
+			writel((u32)ed, &ohci->regs->ed_controlhead);
+		} else {
+			ohci->ed_controltail->hwNextED = (__u32) m32_swap(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((u32)ed, &ohci->regs->ed_bulkhead);
+		} else {
+			ohci->ed_bulktail->hwNextED = (__u32) m32_swap(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;
+	}
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* 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(struct ohci *ohci, struct ed *ed)
+{
+	struct ed *next;
+	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 {
+			next = (struct ed *)m32_swap(*((__u32 *)&ed->hwNextED));
+			next->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 {
+			next = (struct ed *)m32_swap(*((__u32 *)&ed->hwNextED));
+			next->ed_prev = ed->ed_prev;
+		}
+		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 struct ed *ep_add_ed(struct usb_device *usb_dev, unsigned long pipe)
+{
+	struct td *td;
+	struct ed *ed_ret;
+	struct ed *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 = (__u32) m32_swap(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);
+
+	return ed_ret;
+}
+
+/*-------------------------------------------------------------------------*
+ * TD handling functions
+ *-------------------------------------------------------------------------*/
+
+/* enqueue next TD for this URB (OHCI spec 5.2.8.2) */
+
+static void td_fill(struct ohci *ohci, unsigned int info, void *data, int len,
+		    struct usb_device *dev, int index,
+		    struct urb_priv *urb_priv)
+{
+	struct td *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] =
+	    (struct td *) (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_pipebulk(urb_priv->pipe) && 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 = (__u32) m32_swap(info);
+	td->hwCBP = (__u32) m32_swap(data);
+	if (data)
+		td->hwBE = (__u32) m32_swap(data + len - 1);
+	else
+		td->hwBE = 0;
+	td->hwNextTD = (__u32) m32_swap(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, struct urb_priv *urb,
+			  int interval)
+{
+	struct ohci *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)
+			/* start bulk list */
+			writel(OHCI_BLF, &ohci->regs->cmdstatus);
+		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)
+			/* start Control list */
+			writel(OHCI_CLF, &ohci->regs->cmdstatus);
+		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(struct td *td)
+{
+	__u32 tdBE, tdCBP;
+	struct urb_priv *lurb_priv = &urb_priv;
+
+	tdBE = m32_swap(td->hwBE);
+	tdCBP = m32_swap(td->hwCBP);
+
+	if (!(usb_pipecontrol(lurb_priv->pipe) &&
+	      ((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 struct td *dl_reverse_done_list(struct ohci *ohci)
+{
+	__u32 td_list_hc;
+	__u32 tmp;
+	struct td *td_rev = NULL;
+	struct td *td_list = NULL;
+	struct urb_priv *lurb_priv = NULL;
+
+	td_list_hc = m32_swap(ohci->hcca->done_head) & 0xfffffff0;
+	ohci->hcca->done_head = 0;
+
+	while (td_list_hc) {
+		td_list = (struct td *) td_list_hc;
+
+		if (TD_CC_GET(m32_swap(td_list->hwINFO))) {
+			lurb_priv = &urb_priv;
+			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)) {
+					tmp = lurb_priv->length - 1;
+					td_list->ed->hwHeadP =
+						(lurb_priv->td[tmp]->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);
+			}
+		}
+
+		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(struct ohci *ohci, struct td *td_list)
+{
+	struct td *td_list_next = NULL;
+	struct ed *ed;
+	int cc = 0;
+	int stat = 0;
+	/* urb_t *urb; */
+	struct urb_priv *lurb_priv;
+	__u32 tdINFO, edHeadP, edTailP;
+
+	while (td_list) {
+		td_list_next = td_list->next_dl_td;
+
+		lurb_priv = &urb_priv;
+		tdINFO = m32_swap(td_list->hwINFO);
+
+		ed = td_list->ed;
+
+		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 ((ed->state & (ED_OPER | ED_UNLINK)))
+				urb_finished = 1;
+			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) {
+			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(struct ohci *controller)
+{
+	__u32 temp, ndp, i;
+	int res;
+
+	res = -1;
+	temp = roothub_a(controller);
+	ndp = (temp & RH_A_NDP);
+	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;
+	union {
+		__u32 word[4];
+		__u16 hword[8];
+		__u8 byte[16];
+	} datab;
+	__u8 *data_buf = datab.byte;
+	__u16 bmRType_bReq;
+	__u16 wValue;
+	__u16 wIndex;
+	__u16 wLength;
+
+#ifdef DEBUG
+	urb_priv.actual_length = 0;
+	pkt_print(dev, pipe, buffer, transfer_len, cmd, "SUB(rh)",
+		  usb_pipein(pipe));
+#else
+	mdelay(1);
+#endif
+	if (usb_pipeint(pipe)) {
+		info("Root-Hub submit IRQ: NOT implemented");
+		return 0;
+	}
+
+	bmRType_bReq = cmd->requesttype | (cmd->request << 8);
+	wValue = m16_swap(cmd->value);
+	wIndex = m16_swap(cmd->index);
+	wLength = m16_swap(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:
+		datab.hword[0] = m16_swap(1);
+		OK(2);
+	case RH_GET_STATUS | RH_INTERFACE:
+		datab.hword[0] = m16_swap(0);
+		OK(2);
+	case RH_GET_STATUS | RH_ENDPOINT:
+		datab.hword[0] = m16_swap(0);
+		OK(2);
+	case RH_GET_STATUS | RH_CLASS:
+		datab.word[0] =
+		    m32_swap(RD_RH_STAT & ~(RH_HS_CRWE | RH_HS_DRWE));
+		OK(4);
+	case RH_GET_STATUS | RH_OTHER | RH_CLASS:
+		datab.word[0] = m32_swap(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);
+			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;
+			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.word[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
+	mdelay(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
+	if (transfer_len)
+		urb_priv.actual_length = transfer_len;
+	pkt_print(dev, pipe, buffer, transfer_len, cmd, "RET(rh)",
+		  0 /*usb_pipein(pipe) */);
+#else
+	mdelay(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;
+
+	/* device pulled? Shortcut the action. */
+	if (devgone == dev) {
+		dev->status = USB_ST_CRC_ERR;
+		return 0;
+	}
+#ifdef DEBUG
+	urb_priv.actual_length = 0;
+	pkt_print(dev, pipe, buffer, transfer_len, setup, "SUB",
+		  usb_pipein(pipe));
+#else
+	mdelay(1);
+#endif
+	if (!maxsize) {
+		err("submit_common_message: pipesize for pipe %lx is zero",
+		    pipe);
+		return -1;
+	}
+
+	if (sohci_submit_job(dev, pipe, buffer, transfer_len, setup, interval) <
+	    0) {
+		err("sohci_submit_job failed");
+		return -1;
+	}
+
+	mdelay(10);
+	/* ohci_dump_status(&gohci); */
+
+	/* allow more time for a BULK device to react - some are slow */
+#define BULK_TO	 5000		/* timeout in milliseconds */
+	if (usb_pipebulk(pipe))
+		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) {
+			mdelay(1);
+			if (!urb_finished)
+				dbg("\%");
+
+		} else {
+			err("CTL:TIMEOUT ");
+			dbg("submit_common_msg: TO status %x\n", stat);
+			stat = USB_ST_CRC_ERR;
+			urb_finished = 1;
+			break;
+		}
+	}
+
+#if 0
+	/* we got an Root Hub Status Change interrupt */
+	if (got_rhsc) {
+#ifdef DEBUG
+		ohci_dump_roothub(&gohci, 1);
+#endif
+		got_rhsc = 0;
+		/* abuse timeout */
+		timeout = rh_check_port_status(&gohci);
+		if (timeout >= 0) {
+#if 0			/* this does nothing useful, but leave it here
+			   in case that changes */
+			/* the called routine adds 1 to the passed value */
+			usb_hub_port_connect_change(gohci.rh.dev, timeout - 1);
+#endif
+			/*
+			 * XXX
+			 * This is potentially dangerous because it assumes
+			 * that only one device is ever plugged in!
+			 */
+			devgone = dev;
+		}
+	}
+#endif
+
+	dev->status = stat;
+	dev->act_len = transfer_len;
+
+#ifdef DEBUG
+	pkt_print(dev, pipe, buffer, transfer_len, setup, "RET(ctlr)",
+		  usb_pipein(pipe));
+#else
+	mdelay(1);
+#endif
+
+	/* free TDs in urb_priv */
+	urb_free_priv(&urb_priv);
+	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
+	urb_priv.actual_length = 0;
+	pkt_print(dev, pipe, buffer, transfer_len, setup, "SUB",
+		  usb_pipein(pipe));
+#else
+	mdelay(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 -1;
+}
+
+/*-------------------------------------------------------------------------*
+ * HC functions
+ *-------------------------------------------------------------------------*/
+
+/* reset the HC and BUS */
+
+static int hc_reset(struct ohci *ohci)
+{
+	int timeout = 30;
+	int smm_timeout = 50;	/* 0,5 sec */
+
+	if (readl(&ohci->regs->control) & OHCI_CTRL_IR) {
+		/* SMM owns the HC - request ownership */
+		writel(OHCI_OCR, &ohci->regs->cmdstatus);
+		info("USB HC TakeOver from SMM");
+		while (readl(&ohci->regs->control) & OHCI_CTRL_IR) {
+			mdelay(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 ;",
+	    ohci->slot_name, readl(&ohci->regs->control));
+
+	/* Reset USB (needed by some controllers) */
+	writel(0, &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(struct ohci *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);
+
+	/* a reset clears this */
+	writel((__u32) ohci->hcca, &ohci->regs->hcca);
+
+	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 */
+
+	/* 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;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* an interrupt happens */
+
+static int hc_interrupt(void)
+{
+	struct ohci *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 {
+		ints = readl(&regs->intrstatus);
+		if (ints == ~(u32) 0) {
+			ohci->disabled++;
+			err("%s device removed!", ohci->slot_name);
+			return -1;
+		}
+		ints &= readl(&regs->intrenable);
+		if (ints == 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
+		mdelay(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) {
+		mdelay(1);
+
+		writel(OHCI_INTR_WDH, &regs->intrdisable);
+		stat = dl_done_list(&gohci, dl_reverse_done_list(&gohci));
+		writel(OHCI_INTR_WDH, &regs->intrenable);
+	}
+
+	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;
+		mdelay(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(struct ohci *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(int index, void **controller)
+{
+	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
+	struct s3c24x0_gpio *gpio = s3c24x0_get_base_gpio();
+
+	/*
+	 * Set the 48 MHz UPLL clocking. Values are taken from
+	 * "PLL value selection guide", 6-23, s3c2400_UM.pdf.
+	 */
+	clk_power->upllcon = ((40 << 12) + (1 << 4) + 2);
+	gpio->misccr |= 0x8;	/* 1 = use pads related USB for USB host */
+
+	/*
+	 * Enable USB host clock.
+	 */
+	clk_power->clkcon |= (1 << 4);
+
+	memset(&gohci, 0, sizeof(struct ohci));
+	memset(&urb_priv, 0, sizeof(struct urb_priv));
+
+	/* 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(struct td) * (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;
+	gohci.regs = (struct ohci_regs *)S3C24X0_USB_HOST_BASE;
+
+	gohci.flags = 0;
+	gohci.slot_name = "s3c2400";
+
+	if (hc_reset(&gohci) < 0) {
+		hc_release_ohci(&gohci);
+		/* Initialization failed */
+		clk_power->clkcon &= ~(1 << 4);
+		return -1;
+	}
+
+	/* FIXME this is a second HC reset; why?? */
+	gohci.hc_control = OHCI_USB_RESET;
+	writel(gohci.hc_control, &gohci.regs->control);
+	mdelay(10);
+
+	if (hc_start(&gohci) < 0) {
+		err("can't start usb-%s", gohci.slot_name);
+		hc_release_ohci(&gohci);
+		/* Initialization failed */
+		clk_power->clkcon &= ~(1 << 4);
+		return -1;
+	}
+#ifdef	DEBUG
+	ohci_dump(&gohci, 1);
+#else
+	mdelay(1);
+#endif
+	ohci_inited = 1;
+	urb_finished = 1;
+
+	return 0;
+}
+
+int usb_lowlevel_stop(int index)
+{
+	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
+
+	/* 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);
+	/* may not want to do this */
+	clk_power->clkcon &= ~(1 << 4);
+	return 0;
+}
+
+#endif /* defined(CONFIG_USB_OHCI) && defined(CONFIG_S3C24X0) */
+
+#if defined(CONFIG_USB_OHCI_NEW) && \
+    defined(CONFIG_SYS_USB_OHCI_CPU_INIT) && \
+    defined(CONFIG_S3C24X0)
+
+int usb_cpu_init(void)
+{
+	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
+	struct s3c24x0_gpio *gpio = s3c24x0_get_base_gpio();
+
+	/*
+	 * Set the 48 MHz UPLL clocking. Values are taken from
+	 * "PLL value selection guide", 6-23, s3c2400_UM.pdf.
+	 */
+	writel((40 << 12) + (1 << 4) + 2, &clk_power->upllcon);
+	/* 1 = use pads related USB for USB host */
+	writel(readl(&gpio->misccr) | 0x8, &gpio->misccr);
+
+	/*
+	 * Enable USB host clock.
+	 */
+	writel(readl(&clk_power->clkcon) | (1 << 4), &clk_power->clkcon);
+
+	return 0;
+}
+
+int usb_cpu_stop(void)
+{
+	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
+	/* may not want to do this */
+	writel(readl(&clk_power->clkcon) & ~(1 << 4), &clk_power->clkcon);
+	return 0;
+}
+
+int usb_cpu_init_fail(void)
+{
+	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
+	writel(readl(&clk_power->clkcon) & ~(1 << 4), &clk_power->clkcon);
+	return 0;
+}
+
+#endif /* defined(CONFIG_USB_OHCI_NEW) && \
+	   defined(CONFIG_SYS_USB_OHCI_CPU_INIT) && \
+	   defined(CONFIG_S3C24X0) */
diff --git a/drivers/usb/host/ohci-s3c24xx.h b/drivers/usb/host/ohci-s3c24xx.h
new file mode 100644
index 0000000000..f272d78859
--- /dev/null
+++ b/drivers/usb/host/ohci-s3c24xx.h
@@ -0,0 +1,409 @@
+/*
+ * 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
+ */
+
+
+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;
+	__u32 unused[3];
+} __attribute__ ((aligned(16)));
+
+/* 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 */
+
+	__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)));
+
+#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 */
+	__u16 frame_no;		/* current frame number */
+	__u16 pad1;		/* set to 0 on each frame_no change */
+	__u32 done_head;	/* info returned for an interrupt */
+	u8 reserved_for_hc[116];
+} __attribute__ ((aligned(256)));
+
+/*
+ * Maximum number of root hub ports.
+ */
+#define MAX_ROOT_PORTS	15	/* maximum OHCI root hub ports */
+
+/*
+ * 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[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
+struct urb_priv {
+	struct ed *ed;
+	__u16 length;		/* number of tds associated with this request */
+	__u16 td_cnt;		/* number of tds already serviced */
+	int state;
+	unsigned long pipe;
+	int actual_length;
+	struct td *td[N_URB_TD];	/* list pointer to all corresponding TDs
+					   associated with this request */
+};
+#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)
+ */
+
+
+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 */
+
+	struct ed *ed_rm_list[2];  /* lists of all endpoints to be removed */
+	struct ed *ed_bulktail;    /* last endpoint of bulk list */
+	struct ed *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;
+};
+
+#define NUM_EDS 8		/* num of preallocated endpoint descriptors */
+
+struct ohci_device {
+	struct ed ed[NUM_EDS];
+	int ed_cnt;
+};
+
+/* hcd */
+/* endpoint */
+static int ep_link(struct ohci *ohci, struct ed *ed);
+static int ep_unlink(struct ohci *ohci, struct ed *ed);
+static struct ed *ep_add_ed(struct usb_device *usb_dev, unsigned long pipe);
+
+/*-------------------------------------------------------------------------*/
+
+/* we need more TDs than EDs */
+#define NUM_TD 64
+
+/* +1 so we can align the storage */
+struct td gtd[NUM_TD + 1];
+
+/* pointers to aligned storage */
+struct td *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/usb/musb/musb_udc.c b/drivers/usb/musb/musb_udc.c
index 09cdec31a9..e0b4217dc3 100644
--- a/drivers/usb/musb/musb_udc.c
+++ b/drivers/usb/musb/musb_udc.c
@@ -640,8 +640,17 @@ static void musb_peri_ep0(void)
 
 static void musb_peri_rx_ep(unsigned int ep)
 {
-	u16 peri_rxcount = readw(&musbr->ep[ep].epN.rxcount);
+	u16 peri_rxcount;
+	u8 peri_rxcsr = readw(&musbr->ep[ep].epN.rxcsr);
 
+	if (!(peri_rxcsr & MUSB_RXCSR_RXPKTRDY)) {
+		if (debug_level > 0)
+			serial_printf("ERROR : %s %d without MUSB_RXCSR_RXPKTRDY set\n",
+				      __PRETTY_FUNCTION__, ep);
+		return;
+	}
+
+	peri_rxcount = readw(&musbr->ep[ep].epN.rxcount);
 	if (peri_rxcount) {
 		struct usb_endpoint_instance *endpoint;
 		u32 length;
diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index 9f7794fe53..9c67b63bf4 100644
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -385,6 +385,13 @@ static u32 eorx, fgx, bgx;	/* color pats */
 
 static int cfb_do_flush_cache;
 
+#ifdef CONFIG_CFB_CONSOLE_ANSI
+static char ansi_buf[10];
+static int ansi_buf_size;
+static int ansi_colors_need_revert;
+static int ansi_cursor_hidden;
+#endif
+
 static const int video_font_draw_table8[] = {
 	0x00000000, 0x000000ff, 0x0000ff00, 0x0000ffff,
 	0x00ff0000, 0x00ff00ff, 0x00ffff00, 0x00ffffff,
@@ -768,9 +775,97 @@ static void console_back(void)
 	}
 }
 
-static void console_newline(void)
+#ifdef CONFIG_CFB_CONSOLE_ANSI
+
+static void console_clear(void)
+{
+#ifdef VIDEO_HW_RECTFILL
+	video_hw_rectfill(VIDEO_PIXEL_SIZE,	/* bytes per pixel */
+			  0,			/* dest pos x */
+			  video_logo_height,	/* dest pos y */
+			  VIDEO_VISIBLE_COLS,	/* frame width */
+			  VIDEO_VISIBLE_ROWS,	/* frame height */
+			  bgx			/* fill color */
+	);
+#else
+	memsetl(CONSOLE_ROW_FIRST, CONSOLE_SIZE, bgx);
+#endif
+}
+
+static void console_cursor_fix(void)
+{
+	if (console_row < 0)
+		console_row = 0;
+	if (console_row >= CONSOLE_ROWS)
+		console_row = CONSOLE_ROWS - 1;
+	if (console_col < 0)
+		console_col = 0;
+	if (console_col >= CONSOLE_COLS)
+		console_col = CONSOLE_COLS - 1;
+}
+
+static void console_cursor_up(int n)
+{
+	console_row -= n;
+	console_cursor_fix();
+}
+
+static void console_cursor_down(int n)
+{
+	console_row += n;
+	console_cursor_fix();
+}
+
+static void console_cursor_left(int n)
+{
+	console_col -= n;
+	console_cursor_fix();
+}
+
+static void console_cursor_right(int n)
+{
+	console_col += n;
+	console_cursor_fix();
+}
+
+static void console_cursor_set_position(int row, int col)
+{
+	if (console_row != -1)
+		console_row = row;
+	if (console_col != -1)
+		console_col = col;
+	console_cursor_fix();
+}
+
+static void console_previousline(int n)
 {
-	console_row++;
+	/* FIXME: also scroll terminal ? */
+	console_row -= n;
+	console_cursor_fix();
+}
+
+static void console_swap_colors(void)
+{
+	eorx = fgx;
+	fgx = bgx;
+	bgx = eorx;
+	eorx = fgx ^ bgx;
+}
+
+static inline int console_cursor_is_visible(void)
+{
+	return !ansi_cursor_hidden;
+}
+#else
+static inline int console_cursor_is_visible(void)
+{
+	return 1;
+}
+#endif
+
+static void console_newline(int n)
+{
+	console_row += n;
 	console_col = 0;
 
 	/* Check if we need to scroll the terminal */
@@ -779,7 +874,7 @@ static void console_newline(void)
 		console_scrollup();
 
 		/* Decrement row number */
-		console_row--;
+		console_row = CONSOLE_ROWS - 1;
 	}
 }
 
@@ -788,11 +883,12 @@ static void console_cr(void)
 	console_col = 0;
 }
 
-void video_putc(const char c)
+static void parse_putc(const char c)
 {
 	static int nl = 1;
 
-	CURSOR_OFF;
+	if (console_cursor_is_visible())
+		CURSOR_OFF;
 
 	switch (c) {
 	case 13:		/* back to first column */
@@ -801,7 +897,7 @@ void video_putc(const char c)
 
 	case '\n':		/* next line */
 		if (console_col || (!console_col && nl))
-			console_newline();
+			console_newline(1);
 		nl = 1;
 		break;
 
@@ -810,7 +906,7 @@ void video_putc(const char c)
 		console_col &= ~0x0007;
 
 		if (console_col >= CONSOLE_COLS)
-			console_newline();
+			console_newline(1);
 		break;
 
 	case 8:		/* backspace */
@@ -827,11 +923,225 @@ void video_putc(const char c)
 
 		/* check for newline */
 		if (console_col >= CONSOLE_COLS) {
-			console_newline();
+			console_newline(1);
 			nl = 0;
 		}
 	}
-	CURSOR_SET;
+
+	if (console_cursor_is_visible())
+		CURSOR_SET;
+}
+
+void video_putc(const char c)
+{
+#ifdef CONFIG_CFB_CONSOLE_ANSI
+	int i;
+
+	if (c == 27) {
+		for (i = 0; i < ansi_buf_size; ++i)
+			parse_putc(ansi_buf[i]);
+		ansi_buf[0] = 27;
+		ansi_buf_size = 1;
+		return;
+	}
+
+	if (ansi_buf_size > 0) {
+		/*
+		 * 0 - ESC
+		 * 1 - [
+		 * 2 - num1
+		 * 3 - ..
+		 * 4 - ;
+		 * 5 - num2
+		 * 6 - ..
+		 * - cchar
+		 */
+		int next = 0;
+
+		int flush = 0;
+		int fail = 0;
+
+		int num1 = 0;
+		int num2 = 0;
+		int cchar = 0;
+
+		ansi_buf[ansi_buf_size++] = c;
+
+		if (ansi_buf_size >= sizeof(ansi_buf))
+			fail = 1;
+
+		for (i = 0; i < ansi_buf_size; ++i) {
+			if (fail)
+				break;
+
+			switch (next) {
+			case 0:
+				if (ansi_buf[i] == 27)
+					next = 1;
+				else
+					fail = 1;
+				break;
+
+			case 1:
+				if (ansi_buf[i] == '[')
+					next = 2;
+				else
+					fail = 1;
+				break;
+
+			case 2:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num1 = ansi_buf[i]-'0';
+					next = 3;
+				} else if (ansi_buf[i] != '?') {
+					--i;
+					num1 = 1;
+					next = 4;
+				}
+				break;
+
+			case 3:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num1 *= 10;
+					num1 += ansi_buf[i]-'0';
+				} else {
+					--i;
+					next = 4;
+				}
+				break;
+
+			case 4:
+				if (ansi_buf[i] != ';') {
+					--i;
+					next = 7;
+				} else
+					next = 5;
+				break;
+
+			case 5:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num2 = ansi_buf[i]-'0';
+					next = 6;
+				} else
+					fail = 1;
+				break;
+
+			case 6:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num2 *= 10;
+					num2 += ansi_buf[i]-'0';
+				} else {
+					--i;
+					next = 7;
+				}
+				break;
+
+			case 7:
+				if ((ansi_buf[i] >= 'A' && ansi_buf[i] <= 'H')
+					|| ansi_buf[i] == 'J'
+					|| ansi_buf[i] == 'K'
+					|| ansi_buf[i] == 'h'
+					|| ansi_buf[i] == 'l'
+					|| ansi_buf[i] == 'm') {
+					cchar = ansi_buf[i];
+					flush = 1;
+				} else
+					fail = 1;
+				break;
+			}
+		}
+
+		if (fail) {
+			for (i = 0; i < ansi_buf_size; ++i)
+				parse_putc(ansi_buf[i]);
+			ansi_buf_size = 0;
+			return;
+		}
+
+		if (flush) {
+			if (!ansi_cursor_hidden)
+				CURSOR_OFF;
+			ansi_buf_size = 0;
+			switch (cchar) {
+			case 'A':
+				/* move cursor num1 rows up */
+				console_cursor_up(num1);
+				break;
+			case 'B':
+				/* move cursor num1 rows down */
+				console_cursor_down(num1);
+				break;
+			case 'C':
+				/* move cursor num1 columns forward */
+				console_cursor_right(num1);
+				break;
+			case 'D':
+				/* move cursor num1 columns back */
+				console_cursor_left(num1);
+				break;
+			case 'E':
+				/* move cursor num1 rows up at begin of row */
+				console_previousline(num1);
+				break;
+			case 'F':
+				/* move cursor num1 rows down at begin of row */
+				console_newline(num1);
+				break;
+			case 'G':
+				/* move cursor to column num1 */
+				console_cursor_set_position(-1, num1-1);
+				break;
+			case 'H':
+				/* move cursor to row num1, column num2 */
+				console_cursor_set_position(num1-1, num2-1);
+				break;
+			case 'J':
+				/* clear console and move cursor to 0, 0 */
+				console_clear();
+				console_cursor_set_position(0, 0);
+				break;
+			case 'K':
+				/* clear line */
+				if (num1 == 0)
+					console_clear_line(console_row,
+							console_col,
+							CONSOLE_COLS-1);
+				else if (num1 == 1)
+					console_clear_line(console_row,
+							0, console_col);
+				else
+					console_clear_line(console_row,
+							0, CONSOLE_COLS-1);
+				break;
+			case 'h':
+				ansi_cursor_hidden = 0;
+				break;
+			case 'l':
+				ansi_cursor_hidden = 1;
+				break;
+			case 'm':
+				if (num1 == 0) { /* reset swapped colors */
+					if (ansi_colors_need_revert) {
+						console_swap_colors();
+						ansi_colors_need_revert = 0;
+					}
+				} else if (num1 == 7) { /* once swap colors */
+					if (!ansi_colors_need_revert) {
+						console_swap_colors();
+						ansi_colors_need_revert = 1;
+					}
+				}
+				break;
+			}
+			if (!ansi_cursor_hidden)
+				CURSOR_SET;
+		}
+	} else {
+		parse_putc(c);
+	}
+#else
+	parse_putc(c);
+#endif
 }
 
 void video_puts(const char *s)
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 5579bf2f14..923acb9f3a 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -27,6 +27,7 @@ LIB	:= $(obj)libwatchdog.o
 
 COBJS-$(CONFIG_AT91SAM9_WATCHDOG) += at91sam9_wdt.o
 COBJS-$(CONFIG_FTWDT010_WATCHDOG) += ftwdt010_wdt.o
+COBJS-$(CONFIG_TNETV107X_WATCHDOG) += tnetv107x_wdt.o
 
 COBJS	:= $(COBJS-y)
 SRCS	:= $(COBJS:.o=.c)
diff --git a/drivers/watchdog/tnetv107x_wdt.c b/drivers/watchdog/tnetv107x_wdt.c
new file mode 100644
index 0000000000..18aadb0c65
--- /dev/null
+++ b/drivers/watchdog/tnetv107x_wdt.c
@@ -0,0 +1,180 @@
+/*
+ * TNETV107X: Watchdog timer implementation (for reset)
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/clock.h>
+
+#define MAX_DIV		0xFFFE0001
+
+struct wdt_regs {
+	u32 kick_lock;
+#define KICK_LOCK_1	0x5555
+#define KICK_LOCK_2	0xaaaa
+	u32 kick;
+
+	u32 change_lock;
+#define CHANGE_LOCK_1	0x6666
+#define CHANGE_LOCK_2	0xbbbb
+	u32 change;
+
+	u32 disable_lock;
+#define DISABLE_LOCK_1	0x7777
+#define DISABLE_LOCK_2	0xcccc
+#define DISABLE_LOCK_3	0xdddd
+	u32 disable;
+
+	u32 prescale_lock;
+#define PRESCALE_LOCK_1	0x5a5a
+#define PRESCALE_LOCK_2	0xa5a5
+	u32 prescale;
+};
+
+static struct wdt_regs* regs = (struct wdt_regs *)TNETV107X_WDT0_ARM_BASE;
+
+#define wdt_reg_read(reg)	__raw_readl(&regs->reg)
+#define wdt_reg_write(reg, val)	__raw_writel((val), &regs->reg)
+
+static int write_prescale_reg(unsigned long prescale_value)
+{
+	wdt_reg_write(prescale_lock, PRESCALE_LOCK_1);
+	if ((wdt_reg_read(prescale_lock) & 0x3) != 0x1)
+		return -1;
+
+	wdt_reg_write(prescale_lock, PRESCALE_LOCK_2);
+	if ((wdt_reg_read(prescale_lock) & 0x3) != 0x3)
+		return -1;
+
+	wdt_reg_write(prescale, prescale_value);
+
+	return 0;
+}
+
+static int write_change_reg(unsigned long initial_timer_value)
+{
+	wdt_reg_write(change_lock, CHANGE_LOCK_1);
+	if ((wdt_reg_read(change_lock) & 0x3) != 0x1)
+		return -1;
+
+	wdt_reg_write(change_lock, CHANGE_LOCK_2);
+	if ((wdt_reg_read(change_lock) & 0x3) != 0x3)
+		return -1;
+
+	wdt_reg_write(change, initial_timer_value);
+
+	return 0;
+}
+
+static int wdt_control(unsigned long disable_value)
+{
+	wdt_reg_write(disable_lock, DISABLE_LOCK_1);
+	if ((wdt_reg_read(disable_lock) & 0x3) != 0x1)
+		return -1;
+
+	wdt_reg_write(disable_lock, DISABLE_LOCK_2);
+	if ((wdt_reg_read(disable_lock) & 0x3) != 0x2)
+		return -1;
+
+	wdt_reg_write(disable_lock, DISABLE_LOCK_3);
+	if ((wdt_reg_read(disable_lock) & 0x3) != 0x3)
+		return -1;
+
+	wdt_reg_write(disable, disable_value);
+	return 0;
+}
+
+static int wdt_set_period(unsigned long msec)
+{
+	unsigned long change_value, count_value;
+	unsigned long prescale_value = 1;
+	unsigned long refclk_khz, maxdiv;
+	int ret;
+
+	refclk_khz = clk_get_rate(TNETV107X_LPSC_WDT_ARM);
+	maxdiv = (MAX_DIV / refclk_khz);
+
+	if ((!msec) || (msec > maxdiv))
+		return -1;
+
+	count_value = refclk_khz * msec;
+	if (count_value > 0xffff) {
+		change_value = count_value / 0xffff + 1;
+		prescale_value = count_value / change_value;
+	} else {
+		change_value = count_value;
+	}
+
+	ret = write_prescale_reg(prescale_value - 1);
+	if (ret)
+		return ret;
+
+	ret = write_change_reg(change_value);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+unsigned long last_wdt = -1;
+
+int wdt_start(unsigned long msecs)
+{
+	int ret;
+	ret = wdt_control(0);
+	if (ret)
+		return ret;
+	ret = wdt_set_period(msecs);
+	if (ret)
+		return ret;
+	ret = wdt_control(1);
+	if (ret)
+		return ret;
+	ret = wdt_kick();
+	last_wdt = msecs;
+	return ret;
+}
+
+int wdt_stop(void)
+{
+	last_wdt = -1;
+	return wdt_control(0);
+}
+
+int wdt_kick(void)
+{
+	wdt_reg_write(kick_lock, KICK_LOCK_1);
+	if ((wdt_reg_read(kick_lock) & 0x3) != 0x1)
+		return -1;
+
+	wdt_reg_write(kick_lock, KICK_LOCK_2);
+	if ((wdt_reg_read(kick_lock) & 0x3) != 0x3)
+		return -1;
+
+	wdt_reg_write(kick, 1);
+	return 0;
+}
+
+void reset_cpu(ulong addr)
+{
+	clk_enable(TNETV107X_LPSC_WDT_ARM);
+	wdt_start(1);
+	wdt_kick();
+}