diff --git a/drivers/usb/host/ehci-mx5.c b/drivers/usb/host/ehci-mx5.c
index 4c961d3..7b88137 100644
--- a/drivers/usb/host/ehci-mx5.c
+++ b/drivers/usb/host/ehci-mx5.c
@@ -58,6 +58,77 @@
 /* USB_CTRL_1 */
 #define MXC_USB_CTRL_UH1_EXT_CLK_EN		(1 << 25)
 
+/* USB pin configuration */
+#define USB_PAD_CONFIG	(PAD_CTL_PKE_ENABLE | PAD_CTL_SRE_FAST | \
+			PAD_CTL_DRV_HIGH | PAD_CTL_100K_PU | \
+			PAD_CTL_HYS_ENABLE | PAD_CTL_PUE_PULL)
+
+#ifdef CONFIG_MX51
+/*
+ * Configure the MX51 USB H1 IOMUX
+ */
+void setup_iomux_usb_h1(void)
+{
+	mxc_request_iomux(MX51_PIN_USBH1_STP, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_STP, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_CLK, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_CLK, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DIR, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DIR, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_NXT, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_NXT, USB_PAD_CONFIG);
+
+	mxc_request_iomux(MX51_PIN_USBH1_DATA0, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA0, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA1, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA1, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA2, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA2, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA3, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA3, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA4, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA4, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA5, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA5, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA6, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA6, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_USBH1_DATA7, IOMUX_CONFIG_ALT0);
+	mxc_iomux_set_pad(MX51_PIN_USBH1_DATA7, USB_PAD_CONFIG);
+}
+
+/*
+ * Configure the MX51 USB H2 IOMUX
+ */
+void setup_iomux_usb_h2(void)
+{
+	mxc_request_iomux(MX51_PIN_EIM_A24, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_A24, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_A25, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_A25, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_A26, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_A26, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_A27, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_A27, USB_PAD_CONFIG);
+
+	mxc_request_iomux(MX51_PIN_EIM_D16, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D16, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D17, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D17, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D18, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D18, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D19, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D19, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D20, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D20, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D21, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D21, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D22, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D22, USB_PAD_CONFIG);
+	mxc_request_iomux(MX51_PIN_EIM_D23, IOMUX_CONFIG_ALT2);
+	mxc_iomux_set_pad(MX51_PIN_EIM_D23, USB_PAD_CONFIG);
+}
+#endif
+
 int mxc_set_usbcontrol(int port, unsigned int flags)
 {
 	unsigned int v;