diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 399520e..0e7c9db 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -35,7 +35,11 @@
 
 # echi
 COBJS-$(CONFIG_USB_EHCI) += ehci-hcd.o
+ifdef CONFIG_MPC512X
+COBJS-$(CONFIG_USB_EHCI_FSL) += ehci-mpc512x.o
+else
 COBJS-$(CONFIG_USB_EHCI_FSL) += ehci-fsl.o
+endif
 COBJS-$(CONFIG_USB_EHCI_MXC) += ehci-mxc.o
 COBJS-$(CONFIG_USB_EHCI_PPC4XX) += ehci-ppc4xx.o
 COBJS-$(CONFIG_USB_EHCI_IXP4XX) += ehci-ixp.o
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index c674929..6e0043a 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -40,7 +40,7 @@
 {
 	struct usb_ehci *ehci;
 
-	ehci = (struct usb_ehci *)CONFIG_SYS_MPC8xxx_USB_ADDR;
+	ehci = (struct usb_ehci *)CONFIG_SYS_FSL_USB_ADDR;
 	hccr = (struct ehci_hccr *)((uint32_t)&ehci->caplength);
 	hcor = (struct ehci_hcor *)((uint32_t) hccr +
 			HC_LENGTH(ehci_readl(&hccr->cr_capbase)));
diff --git a/drivers/usb/host/ehci-mpc512x.c b/drivers/usb/host/ehci-mpc512x.c
new file mode 100644
index 0000000..d360108
--- /dev/null
+++ b/drivers/usb/host/ehci-mpc512x.c
@@ -0,0 +1,159 @@
+/*
+ * (C) Copyright 2010, Damien Dusha, <d.dusha@gmail.com>
+ *
+ * (C) Copyright 2009, Value Team S.p.A.
+ * Francesco Rendine, <francesco.rendine@valueteam.com>
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * (C) Copyright 2008, Excito Elektronik i Sk=E5ne AB
+ *
+ * Author: Tor Krill tor@excito.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 <pci.h>
+#include <usb.h>
+#include <asm/io.h>
+#include <usb/ehci-fsl.h>
+
+#include "ehci.h"
+#include "ehci-core.h"
+
+static void fsl_setup_phy(volatile struct ehci_hcor *);
+static void fsl_platform_set_host_mode(volatile struct usb_ehci *ehci);
+static int reset_usb_controller(volatile struct usb_ehci *ehci);
+static void usb_platform_dr_init(volatile struct usb_ehci *ehci);
+
+/*
+ * Initialize SOC FSL EHCI Controller
+ *
+ * This code is derived from EHCI FSL USB Linux driver for MPC5121
+ *
+ */
+int ehci_hcd_init(void)
+{
+	volatile struct usb_ehci *ehci;
+
+	/* Hook the memory mapped registers for EHCI-Controller */
+	ehci = (struct usb_ehci *)CONFIG_SYS_FSL_USB_ADDR;
+	hccr = (struct ehci_hccr *)((uint32_t)&(ehci->caplength));
+	hcor = (struct ehci_hcor *)((uint32_t) hccr +
+				HC_LENGTH(ehci_readl(&hccr->cr_capbase)));
+
+	/* configure interface for UTMI_WIDE */
+	usb_platform_dr_init(ehci);
+
+	/* Init Phy USB0 to UTMI+ */
+	fsl_setup_phy(hcor);
+
+	/* Set to host mode */
+	fsl_platform_set_host_mode(ehci);
+
+	/*
+	 * Setting the burst size seems to be required to prevent the
+	 * USB from hanging when communicating with certain USB Mass
+	 * storage devices. This was determined by analysing the
+	 * EHCI registers under Linux vs U-Boot and burstsize was the
+	 * major non-interrupt related difference between the two
+	 * implementations.
+	 *
+	 * Some USB sticks behave better than others. In particular,
+	 * the following USB stick is especially problematic:
+	 * 0930:6545 Toshiba Corp
+	 *
+	 * The burstsize is set here to match the Linux implementation.
+	 */
+	out_be32(&ehci->burstsize, FSL_EHCI_TXPBURST(8) |
+				   FSL_EHCI_RXPBURST(8));
+
+	return 0;
+}
+
+/*
+ * Destroy the appropriate control structures corresponding
+ * the the EHCI host controller.
+ */
+int ehci_hcd_stop(void)
+{
+	volatile struct usb_ehci *ehci;
+	int exit_status = 0;
+
+	if (hcor) {
+		/* Unhook struct */
+		hccr = NULL;
+		hcor = NULL;
+
+		/* Reset the USB controller */
+		ehci = (struct usb_ehci *)CONFIG_SYS_FSL_USB_ADDR;
+		exit_status = reset_usb_controller(ehci);
+	}
+
+	return exit_status;
+}
+
+static int reset_usb_controller(volatile struct usb_ehci *ehci)
+{
+	unsigned int i;
+
+	/* Command a reset of the USB Controller */
+	out_be32(&(ehci->usbcmd), EHCI_FSL_USBCMD_RST);
+
+	/* Wait for the reset process to finish */
+	for (i = 65535 ; i > 0 ; i--) {
+		/*
+		 * The host will set this bit to zero once the
+		 * reset process is complete
+		 */
+		if ((in_be32(&(ehci->usbcmd)) & EHCI_FSL_USBCMD_RST) == 0)
+			return 0;
+	}
+
+	/* Hub did not reset in time */
+	return -1;
+}
+
+static void fsl_setup_phy(volatile struct ehci_hcor *hcor)
+{
+	uint32_t portsc;
+
+	portsc  = ehci_readl(&hcor->or_portsc[0]);
+	portsc &= ~(PORT_PTS_MSK | PORT_PTS_PTW);
+
+	/* Enable the phy mode to UTMI Wide */
+	portsc |= PORT_PTS_PTW;
+	portsc |= PORT_PTS_UTMI;
+
+	ehci_writel(&hcor->or_portsc[0], portsc);
+}
+
+static void fsl_platform_set_host_mode(volatile struct usb_ehci *ehci)
+{
+	uint32_t temp;
+
+	temp  = in_le32(&ehci->usbmode);
+	temp |= CM_HOST | ES_BE;
+	out_le32(&ehci->usbmode, temp);
+}
+
+static void usb_platform_dr_init(volatile struct usb_ehci *ehci)
+{
+	/* Configure interface for UTMI_WIDE */
+	out_be32(&ehci->isiphyctrl, PHYCTRL_PHYE | PHYCTRL_PXE);
+	out_be32(&ehci->usbgenctrl, GC_PPP | GC_PFP );
+}
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index b3c1d5d..6fae8ba 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -71,6 +71,11 @@
 #define	STD_ASS		(1 << 15)
 #define STS_HALT	(1 << 12)
 	uint32_t or_usbintr;
+#define INTR_UE         (1 << 0)                /* USB interrupt enable */
+#define INTR_UEE        (1 << 1)                /* USB error interrupt enable */
+#define INTR_PCE        (1 << 2)                /* Port change detect enable */
+#define INTR_SEE        (1 << 4)                /* system error enable */
+#define INTR_AAE        (1 << 5)                /* Interrupt on async adavance enable */
 	uint32_t or_frindex;
 	uint32_t or_ctrldssegment;
 	uint32_t or_periodiclistbase;