diff --git a/arch/arm/include/asm/arch-mx5/iomux-mx51.h b/arch/arm/include/asm/arch-mx5/iomux-mx51.h
index f944c13..0a99ae4 100644
--- a/arch/arm/include/asm/arch-mx5/iomux-mx51.h
+++ b/arch/arm/include/asm/arch-mx5/iomux-mx51.h
@@ -79,6 +79,20 @@
 
 /*													PAD    MUX   ALT INPSE PATH PADCTRL */
 enum {
+	MX51_PAD_EIM_D16__USBH2_DATA0		= IOMUX_PAD(0x3f0, 0x05c, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D17__USBH2_DATA1		= IOMUX_PAD(0x3f4, 0x060, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D18__USBH2_DATA2		= IOMUX_PAD(0x3f8, 0x064, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D19__USBH2_DATA3		= IOMUX_PAD(0x3fc, 0x068, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D20__USBH2_DATA4		= IOMUX_PAD(0x400, 0x06c, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D21__USBH2_DATA5		= IOMUX_PAD(0x404, 0x070, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D22__USBH2_DATA6		= IOMUX_PAD(0x408, 0x074, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D23__USBH2_DATA7		= IOMUX_PAD(0x40c, 0x078, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_D27__GPIO2_9			= IOMUX_PAD(0x41c, 0x088, 1, __NA_, 0, MX51_GPIO_PAD_CTRL),
+	MX51_PAD_EIM_A24__USBH2_CLK			= IOMUX_PAD(0x450, 0x0bc, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_A25__USBH2_DIR			= IOMUX_PAD(0x454, 0x0c0, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_A26__GPIO2_20			= IOMUX_PAD(0x458, 0x0c4, 1, __NA_, 0, MX51_GPIO_PAD_CTRL),
+	MX51_PAD_EIM_A26__USBH2_STP			= IOMUX_PAD(0x458, 0x0c4, 2, __NA_, 0, NO_PAD_CTRL),
+	MX51_PAD_EIM_A27__USBH2_NXT			= IOMUX_PAD(0x45c, 0x0c8, 2, __NA_, 0, NO_PAD_CTRL),	
 	MX51_PAD_EIM_CS0__GPIO2_25			= IOMUX_PAD(0x474, 0x0e0, 1, __NA_, 0, MX51_GPIO_PAD_CTRL),
 	MX51_PAD_EIM_CS2__SD1_CD			= IOMUX_PAD(0x47c, 0x0e8, 1, __NA_, 0, MX51_ESDHC_PAD_CTRL),
 	MX51_PAD_EIM_CS3__GPIO2_28			= IOMUX_PAD(0x480, 0x0ec, 1, __NA_, 0, MX51_GPIO_PAD_CTRL),
@@ -121,6 +135,19 @@ enum {
 	MX51_PAD_UART1_TXD__UART1_TXD		= IOMUX_PAD(0x61c, 0x22c, 0, __NA_, 0, MX51_UART_PAD_CTRL),
 	MX51_PAD_UART1_RTS__UART1_RTS		= IOMUX_PAD(0x620, 0x230, 0, 0x9e0, 0, MX51_UART_PAD_CTRL),
 	MX51_PAD_UART1_CTS__UART1_CTS		= IOMUX_PAD(0x624, 0x234, 0, __NA_, 0, MX51_UART_PAD_CTRL),
+	MX51_PAD_USBH1_CLK__USBH1_CLK		= IOMUX_PAD(0x678, 0x278, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DIR__USBH1_DIR		= IOMUX_PAD(0x67c, 0x27c, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_STP__USBH1_STP		= IOMUX_PAD(0x680, 0x280, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_STP__GPIO1_27		= IOMUX_PAD(0x680, 0x280, 2, __NA_, 0, MX51_GPIO_PAD_CTRL),
+	MX51_PAD_USBH1_NXT__USBH1_NXT		= IOMUX_PAD(0x684, 0x284, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA0__USBH1_DATA0	= IOMUX_PAD(0x688, 0x288, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA1__USBH1_DATA1	= IOMUX_PAD(0x68c, 0x28c, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA2__USBH1_DATA2	= IOMUX_PAD(0x690, 0x290, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA3__USBH1_DATA3	= IOMUX_PAD(0x694, 0x294, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA4__USBH1_DATA4	= IOMUX_PAD(0x698, 0x298, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA5__USBH1_DATA5	= IOMUX_PAD(0x69c, 0x29c, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA6__USBH1_DATA6	= IOMUX_PAD(0x6a0, 0x2a0, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
+	MX51_PAD_USBH1_DATA7__USBH1_DATA7	= IOMUX_PAD(0x6a4, 0x2a4, 0, __NA_, 0, MX51_USBH1_PAD_CTRL),
 	MX51_PAD_SD1_CMD__SD1_CMD			= IOMUX_PAD(0x79c, 0x394, 0x10, __NA_, 0, MX51_SDHCI_PAD_CTRL),
 	MX51_PAD_SD1_CLK__SD1_CLK			= IOMUX_PAD(0x7a0, 0x398, 0x10, __NA_, 0, MX51_SDHCI_PAD_CTRL | PAD_CTL_HYS),
 	MX51_PAD_SD1_DATA0__SD1_DATA0		= IOMUX_PAD(0x7a4, 0x39c, 0x10, __NA_, 0, MX51_SDHCI_PAD_CTRL),
@@ -136,6 +163,7 @@ enum {
 	MX51_PAD_SD2_DATA2__SD2_DATA2		= IOMUX_PAD(0x7cc, 0x3c4, 0x10, __NA_, 0, MX51_SDHCI_PAD_CTRL),
 	MX51_PAD_SD2_DATA3__SD2_DATA3		= IOMUX_PAD(0x7d0, 0x3c8, 0x10, __NA_, 0, MX51_SDHCI_PAD_CTRL),
 	MX51_PAD_GPIO1_3__GPIO1_3			= IOMUX_PAD(0x7d8, 0x3d0, 0, __NA_, 0, MX51_GPIO_PAD_CTRL),
+	MX51_PAD_GPIO1_5__GPIO1_5			= IOMUX_PAD(0x808, 0x3dc, 0, __NA_, 0, MX51_GPIO_PAD_CTRL),
 	MX51_PAD_GPIO1_6__GPIO1_6			= IOMUX_PAD(0x80c, 0x3e0, 0, __NA_, 0, MX51_GPIO_PAD_CTRL),
 	MX51_PAD_GPIO1_7__SD2_WP			= IOMUX_PAD(0x810, 0x3e4, 6, __NA_, 0, MX51_ESDHC_PAD_CTRL),
 	MX51_PAD_GPIO1_8__SD2_CD			= IOMUX_PAD(0x814, 0x3e8, 6, __NA_, 0, MX51_ESDHC_PAD_CTRL),
diff --git a/board/genesi/mx51_efikamx/efikamx-usb.c b/board/genesi/mx51_efikamx/efikamx-usb.c
index e9273d0..0935c38 100644
--- a/board/genesi/mx51_efikamx/efikamx-usb.c
+++ b/board/genesi/mx51_efikamx/efikamx-usb.c
@@ -1,7 +1,7 @@
 /*
+ * Copyright (C) 2009 Freescale Semiconductor, Inc.
  * Copyright (C) 2010 Marek Vasut <marek.vasut@gmail.com>
- *
- * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ * Copyright (C) 2009-2012 Genesi USA, Inc.
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -25,9 +25,7 @@
 #include <common.h>
 #include <usb.h>
 #include <asm/io.h>
-#include <asm/arch/imx-regs.h>
-#include <asm/arch/mx5x_pins.h>
-#include <asm/arch/iomux.h>
+#include <asm/arch/iomux-mx51.h>
 #include <asm/gpio.h>
 #include <usb/ehci-fsl.h>
 #include <usb/ulpi.h>
@@ -35,103 +33,93 @@
 
 #include "../../../drivers/usb/host/ehci.h"
 
-/* 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)
-
-/*
- * Configure the USB H1 and USB H2 IOMUX
- */
-void setup_iomux_usb(void)
+static iomux_v3_cfg_t efikamx_usbh1_pads[] = {
+	MX51_PAD_USBH1_CLK__USBH1_CLK,
+	MX51_PAD_USBH1_DIR__USBH1_DIR,
+	MX51_PAD_USBH1_STP__USBH1_STP,
+	MX51_PAD_USBH1_NXT__USBH1_NXT,
+	MX51_PAD_USBH1_DATA0__USBH1_DATA0,
+	MX51_PAD_USBH1_DATA1__USBH1_DATA1,
+	MX51_PAD_USBH1_DATA2__USBH1_DATA2,
+	MX51_PAD_USBH1_DATA3__USBH1_DATA3,
+	MX51_PAD_USBH1_DATA4__USBH1_DATA4,
+	MX51_PAD_USBH1_DATA5__USBH1_DATA5,
+	MX51_PAD_USBH1_DATA6__USBH1_DATA6,
+	MX51_PAD_USBH1_DATA7__USBH1_DATA7,
+};
+
+static iomux_v3_cfg_t efikasb_usbh2_pads[] = {
+	MX51_PAD_EIM_A24__USBH2_CLK,
+	MX51_PAD_EIM_A25__USBH2_DIR,
+	MX51_PAD_EIM_A26__USBH2_STP,
+	MX51_PAD_EIM_A27__USBH2_NXT,
+	MX51_PAD_EIM_D16__USBH2_DATA0,
+	MX51_PAD_EIM_D17__USBH2_DATA1,
+	MX51_PAD_EIM_D18__USBH2_DATA2,
+	MX51_PAD_EIM_D19__USBH2_DATA3,
+	MX51_PAD_EIM_D20__USBH2_DATA4,
+	MX51_PAD_EIM_D21__USBH2_DATA5,
+	MX51_PAD_EIM_D22__USBH2_DATA6,
+	MX51_PAD_EIM_D23__USBH2_DATA7,
+};
+
+static iomux_v3_cfg_t efikamx_usbctrl_pads[] = {
+	MX51_PAD_EIM_D27__GPIO2_9,
+	MX51_PAD_GPIO1_5__GPIO1_5,
+};
+
+#define EFIKAMX_USB_HUB_RESET	GPIO_NUMBER(1, 5)
+#define EFIKAMX_USB_PHY_RESET	GPIO_NUMBER(2, 9)
+
+void efikamx_usb_setup_pads(void)
 {
-	setup_iomux_usb_h1();
-
-	if (machine_is_efikasb())
-		setup_iomux_usb_h2();
-
-	/* USB PHY reset */
-	mxc_request_iomux(MX51_PIN_EIM_D27, IOMUX_CONFIG_ALT1);
-	mxc_iomux_set_pad(MX51_PIN_EIM_D27, PAD_CTL_PKE_ENABLE |
-			PAD_CTL_SRE_FAST | PAD_CTL_DRV_HIGH);
-
-	/* USB HUB reset */
-	mxc_request_iomux(MX51_PIN_GPIO1_5, IOMUX_CONFIG_ALT0);
-	mxc_iomux_set_pad(MX51_PIN_GPIO1_5, PAD_CTL_PKE_ENABLE |
-			PAD_CTL_SRE_FAST | PAD_CTL_DRV_HIGH);
-
-	/* WIFI EN (act low) */
-	mxc_request_iomux(MX51_PIN_EIM_A22, IOMUX_CONFIG_GPIO);
-	mxc_iomux_set_pad(MX51_PIN_EIM_A22, 0);
-	/* WIFI RESET */
-	mxc_request_iomux(MX51_PIN_EIM_A16, IOMUX_CONFIG_GPIO);
-	mxc_iomux_set_pad(MX51_PIN_EIM_A16, 0);
-	/* BT EN (act low) */
-	mxc_request_iomux(MX51_PIN_EIM_A17, IOMUX_CONFIG_GPIO);
-	mxc_iomux_set_pad(MX51_PIN_EIM_A17, 0);
-}
+	imx_iomux_v3_setup_multiple_pads(efikamx_usbh1_pads, ARRAY_SIZE(efikamx_usbh1_pads));
 
-/*
- * Enable devices connected to USB BUSes
- */
-static void efika_usb_enable_devices(void)
-{
-	/* Enable Bluetooth */
-	gpio_direction_output(IOMUX_TO_GPIO(MX51_PIN_EIM_A17), 0);
-	udelay(10000);
-	gpio_set_value(IOMUX_TO_GPIO(MX51_PIN_EIM_A17), 1);
-
-	/* Enable WiFi */
-	gpio_direction_output(IOMUX_TO_GPIO(MX51_PIN_EIM_A22), 1);
-	udelay(10000);
+	if (machine_is_efikasb()) {
+		imx_iomux_v3_setup_multiple_pads(efikasb_usbh2_pads, ARRAY_SIZE(efikasb_usbh2_pads));
+	}
 
-	/* Reset the WiFi chip */
-	gpio_direction_output(IOMUX_TO_GPIO(MX51_PIN_EIM_A16), 0);
-	udelay(10000);
-	gpio_set_value(IOMUX_TO_GPIO(MX51_PIN_EIM_A16), 1);
+	imx_iomux_v3_setup_multiple_pads(efikamx_usbctrl_pads, ARRAY_SIZE(efikamx_usbctrl_pads));
 }
 
 /*
  * Reset USB HUB (or HUBs on EfikaSB)
  */
-static void efika_usb_hub_reset(void)
+static void efikamx_usb_hub_reset(void)
 {
-	/* HUB reset */
-	gpio_direction_output(IOMUX_TO_GPIO(MX51_PIN_GPIO1_5), 1);
+	gpio_direction_output(EFIKAMX_USB_HUB_RESET, 1);
 	udelay(1000);
-	gpio_set_value(IOMUX_TO_GPIO(MX51_PIN_GPIO1_5), 0);
+	gpio_set_value(EFIKAMX_USB_HUB_RESET, 0);
 	udelay(1000);
-	gpio_set_value(IOMUX_TO_GPIO(MX51_PIN_GPIO1_5), 1);
+	gpio_set_value(EFIKAMX_USB_HUB_RESET, 1);
 }
 
 /*
  * Reset USB PHY (or PHYs on EfikaSB)
  */
-static void efika_usb_phy_reset(void)
+static void efikamx_usb_phy_reset(void)
 {
 	/* SMSC 3317 PHY reset */
-	gpio_direction_output(IOMUX_TO_GPIO(MX51_PIN_EIM_D27), 0);
+	gpio_direction_output(EFIKAMX_USB_PHY_RESET, 0);
 	udelay(1000);
-	gpio_set_value(IOMUX_TO_GPIO(MX51_PIN_EIM_D27), 1);
+	gpio_set_value(EFIKAMX_USB_PHY_RESET, 1);
 }
 
-static void efika_ehci_init(struct usb_ehci *ehci, uint32_t stp_gpio,
-				uint32_t alt0, uint32_t alt1)
+static void efikamx_usb_init(struct usb_ehci *ehci,
+							iomux_v3_cfg_t stp_pad,
+							iomux_v3_cfg_t gpio_pad,
+							u32 gpio_nr)
 {
 	int ret;
 	struct ulpi_regs *ulpi = (struct ulpi_regs *)0;
 	struct ulpi_viewport ulpi_vp;
 
-	mxc_request_iomux(stp_gpio, alt0);
-	mxc_iomux_set_pad(stp_gpio, PAD_CTL_DRV_HIGH |
-				PAD_CTL_PKE_ENABLE | PAD_CTL_SRE_FAST);
-	gpio_direction_output(IOMUX_TO_GPIO(stp_gpio), 0);
+	imx_iomux_v3_setup_pad(gpio_pad);
+	gpio_direction_output(gpio_nr, 0);
 	udelay(1000);
-	gpio_set_value(IOMUX_TO_GPIO(stp_gpio), 1);
+	gpio_set_value(gpio_nr, 1);
 	udelay(1000);
-
-	mxc_request_iomux(stp_gpio, alt1);
-	mxc_iomux_set_pad(stp_gpio, USB_PAD_CONFIG);
+	imx_iomux_v3_setup_pad(stp_pad);
 	udelay(10000);
 
 	ulpi_vp.viewport_addr = (u32)&ehci->ulpi_viewpoint;
@@ -159,18 +147,18 @@ static void efika_ehci_init(struct usb_ehci *ehci, uint32_t stp_gpio,
 	/*
 	 * Set VBusChrg
 	 *
-	 * NOTE: This violates USB specification, but otherwise, USB on Efika
+	 * NOTE: This violates USB specification, but otherwise, USB on Efika MX
 	 * doesn't work.
 	 */
 	ulpi_write(&ulpi_vp, &ulpi->otg_ctrl_set, ULPI_OTG_CHRGVBUS);
+	
+	mdelay(10);
 }
 
 int board_ehci_hcd_init(int port)
 {
-	/* Init iMX51 EHCI */
-	efika_usb_phy_reset();
-	efika_usb_hub_reset();
-	efika_usb_enable_devices();
+	efikamx_usb_phy_reset();
+	efikamx_usb_hub_reset();
 
 	return 0;
 }
@@ -204,13 +192,12 @@ void board_ehci_hcd_postinit(struct usb_ehci *ehci, int port)
 		tmp = (tmp & ~0x3) | 0x01;
 		writel(tmp, OTG_BASE_ADDR + 0x80c);
 	} else if (port == 1) {
-		efika_ehci_init(ehci, MX51_PIN_USBH1_STP,
-				IOMUX_CONFIG_ALT2, IOMUX_CONFIG_ALT0);
+		efikamx_usb_init(ehci, MX51_PAD_USBH1_STP__USBH1_STP,
+							   MX51_PAD_USBH1_STP__GPIO1_27,
+							   GPIO_NUMBER(1, 27));
 	} else if ((port == 2) && machine_is_efikasb()) {
-		efika_ehci_init(ehci, MX51_PIN_EIM_A26,
-				IOMUX_CONFIG_ALT1, IOMUX_CONFIG_ALT2);
+		efikamx_usb_init(ehci, MX51_PAD_EIM_A26__USBH2_STP,
+							   MX51_PAD_EIM_A26__GPIO2_20,
+							   GPIO_NUMBER(2, 20));
 	}
-
-	if (port)
-		mdelay(10);
 }
diff --git a/board/genesi/mx51_efikamx/efikamx.c b/board/genesi/mx51_efikamx/efikamx.c
index 16e877f..a22db1e 100644
--- a/board/genesi/mx51_efikamx/efikamx.c
+++ b/board/genesi/mx51_efikamx/efikamx.c
@@ -402,9 +402,9 @@ static iomux_v3_cfg_t efikamx_pata_pads[] = {
  * EHCI USB
  */
 #ifdef	CONFIG_CMD_USB
-extern void setup_iomux_usb(void);
+extern void efikamx_usb_setup_pads(void);
 #else
-static inline void setup_iomux_usb(void) { }
+static inline void efikamx_usb_setup_pads(void) { }
 #endif
 
 /*
@@ -484,7 +484,7 @@ int board_late_init(void)
 	power_init();
 
 	imx_iomux_v3_setup_multiple_pads(efikamx_pata_pads, ARRAY_SIZE(efikamx_pata_pads));
-	setup_iomux_usb();
+	efikamx_usb_setup_pads();
 
 	if (machine_is_efikasb())
 		setenv("preboot", "usb reset ; setenv stdin usbkbd\0");
