ENGR00299848 ARM:imx6 Change static environment SD/MMC storage to dynamic

imx6 boards (sabresd, sabreauto, arm2 and slevk) have multiple SD/MMC ports to boot.
But current uboot hard code the SD/MMC port for environment variables storage. So
if customer changes a port without modifying the configuration "CONFIG_SYS_MMC_ENV_DEV",
error will issue at saving and loading environment.

Implement a mechanism to detect SD/MMC port from SRC SMBR register, and override the
default "mmc_get_env_devno". The "board_late_mmc_env_init" is used to set "mmcdev"
when booting from SD/MMC port. Finally after booting from SD/MMC, the environment storage
device and "mmcdev" are both set to current SD/MMC port. Customers don't need to re-build
the image if booting from different SD/MMC port.

This patch also adds SD1 and SD3 support to imx6slevk BSP, and adds support for sabreauto
SD1 slot on base board.

Signed-off-by: Ye.Li <B37916@freescale.com>
(cherry picked from commit 7f789db44b3d5bcdf0aa4952f724701aa90e5fc1)

8 files changed, 351 insertions, 25 deletions

diff --git a/arch/arm/include/asm/arch-mx6/mx6sl_pins.h b/arch/arm/include/asm/arch-mx6/mx6sl_pins.h
index 22749c0..907b65b 100644
--- a/arch/arm/include/asm/arch-mx6/mx6sl_pins.h
+++ b/arch/arm/include/asm/arch-mx6/mx6sl_pins.h
@@ -13,12 +13,34 @@
 #include <asm/imx-common/iomux-v3.h>
 
 enum {
+	MX6_PAD_SD1_CLK__USDHC1_CLK             = IOMUX_PAD(0x0534, 0x022C, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_CMD__USDHC1_CMD             = IOMUX_PAD(0x0538, 0x0230, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT0__USDHC1_DAT0           = IOMUX_PAD(0x053C, 0x0234, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT1__USDHC1_DAT1           = IOMUX_PAD(0x0540, 0x0238, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT2__USDHC1_DAT2           = IOMUX_PAD(0x0544, 0x023C, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT3__USDHC1_DAT3           = IOMUX_PAD(0x0548, 0x0240, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT4__USDHC1_DAT4           = IOMUX_PAD(0x054C, 0x0244, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT5__USDHC1_DAT5           = IOMUX_PAD(0x0550, 0x0248, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT6__USDHC1_DAT6           = IOMUX_PAD(0x0554, 0x024C, 0, 0x0000, 0, 0),
+	MX6_PAD_SD1_DAT7__USDHC1_DAT7           = IOMUX_PAD(0x0558, 0x0250, 0, 0x0000, 0, 0),
+	MX6_PAD_KEY_ROW7__GPIO_4_7              = IOMUX_PAD(0x04B0, 0x01A8, 5, 0x0000, 0, 0),
+
 	MX6_PAD_SD2_CLK__USDHC2_CLK				= IOMUX_PAD(0x055C, 0x0254, 0, 0x0000, 0, 0),
 	MX6_PAD_SD2_CMD__USDHC2_CMD				= IOMUX_PAD(0x0560, 0x0258, 0, 0x0000, 0, 0),
 	MX6_PAD_SD2_DAT0__USDHC2_DAT0				= IOMUX_PAD(0x0564, 0x025C, 0, 0x0000, 0, 0),
 	MX6_PAD_SD2_DAT1__USDHC2_DAT1				= IOMUX_PAD(0x0568, 0x0260, 0, 0x0000, 0, 0),
 	MX6_PAD_SD2_DAT2__USDHC2_DAT2				= IOMUX_PAD(0x056C, 0x0264, 0, 0x0000, 0, 0),
 	MX6_PAD_SD2_DAT3__USDHC2_DAT3				= IOMUX_PAD(0x0570, 0x0268, 0, 0x0000, 0, 0),
+	MX6_PAD_SD2_DAT7__GPIO_5_0              = IOMUX_PAD(0x0580, 0x0278, 5, 0x0000, 0, 0),
+
+	MX6_PAD_SD3_CLK__USDHC3_CLK              = IOMUX_PAD(0x0588, 0x0280, 0, 0x0000, 0, 0),
+	MX6_PAD_SD3_CMD__USDHC3_CMD              = IOMUX_PAD(0x058C, 0x0284, 0, 0x0000, 0, 0),
+	MX6_PAD_SD3_DAT0__USDHC3_DAT0            = IOMUX_PAD(0x0590, 0x0288, 0, 0x0000, 0, 0),
+	MX6_PAD_SD3_DAT1__USDHC3_DAT1            = IOMUX_PAD(0x0594, 0x028C, 0, 0x0000, 0, 0),
+	MX6_PAD_SD3_DAT2__USDHC3_DAT2            = IOMUX_PAD(0x0598, 0x0290, 0, 0x0000, 0, 0),
+	MX6_PAD_SD3_DAT3__USDHC3_DAT3            = IOMUX_PAD(0x059C, 0x0294, 0, 0x0000, 0, 0),
+	MX6_PAD_REF_CLK_32K__GPIO_3_22           = IOMUX_PAD(0x0530, 0x0228, 5, 0x0000, 0, 0),
+
 	MX6_PAD_UART1_RXD__UART1_RXD				= IOMUX_PAD(0x05A0, 0x0298, 0, 0x07FC, 0, 0),
 	MX6_PAD_UART1_TXD__UART1_TXD				= IOMUX_PAD(0x05A4, 0x029C, 0, 0x0000, 0, 0),
 
diff --git a/board/freescale/mx6qarm2/mx6qarm2.c b/board/freescale/mx6qarm2/mx6qarm2.c
index 4782dc9..b2bc86c 100644
--- a/board/freescale/mx6qarm2/mx6qarm2.c
+++ b/board/freescale/mx6qarm2/mx6qarm2.c
@@ -122,6 +122,23 @@ struct fsl_esdhc_cfg usdhc_cfg[2] = {
 	{USDHC4_BASE_ADDR},
 };
 
+int mmc_get_env_devno(void)
+{
+	u32 soc_sbmr = readl(SRC_BASE_ADDR + 0x4);
+	u32 dev_no;
+
+	/* BOOT_CFG2[3] and BOOT_CFG2[4] */
+	dev_no = (soc_sbmr & 0x00001800) >> 11;
+
+	/* need ubstract 1 to map to the mmc device id
+	 * see the comments in board_mmc_init function
+	 */
+
+	dev_no -= 2;
+
+	return dev_no;
+}
+
 int board_mmc_getcd(struct mmc *mmc)
 {
 	struct fsl_esdhc_cfg *cfg = (struct fsl_esdhc_cfg *)mmc->priv;
@@ -166,6 +183,18 @@ int board_mmc_init(bd_t *bis)
 
 	return status;
 }
+
+void board_late_mmc_env_init(void)
+{
+	char cmd[32];
+	u32 dev_no = mmc_get_env_devno();
+
+	setenv_ulong("mmcdev", dev_no);
+
+	sprintf(cmd, "mmc dev %d", dev_no);
+	run_command(cmd, 0);
+}
+
 #endif
 
 #define MII_MMD_ACCESS_CTRL_REG		0xd
@@ -243,6 +272,15 @@ int board_init(void)
 	return 0;
 }
 
+int board_late_init(void)
+{
+#ifdef CONFIG_ENV_IS_IN_MMC
+	board_late_mmc_env_init();
+#endif
+
+	return 0;
+}
+
 int checkboard(void)
 {
 	puts("Board: MX6Q-Armadillo2\n");
diff --git a/board/freescale/mx6qsabreauto/mx6qsabreauto.c b/board/freescale/mx6qsabreauto/mx6qsabreauto.c
index de59829..b8c26a1 100644
--- a/board/freescale/mx6qsabreauto/mx6qsabreauto.c
+++ b/board/freescale/mx6qsabreauto/mx6qsabreauto.c
@@ -65,6 +65,11 @@ DECLARE_GLOBAL_DATA_PTR;
 	PAD_CTL_PUS_47K_UP  | PAD_CTL_SPEED_LOW |               \
 	PAD_CTL_DSE_80ohm   | PAD_CTL_SRE_FAST  | PAD_CTL_HYS)
 
+/*Need more drive strength for SD1 slot on base board*/
+#define USDHC1_PAD_CTRL (PAD_CTL_PKE | PAD_CTL_PUE |            \
+	PAD_CTL_PUS_47K_UP  | PAD_CTL_SPEED_LOW |               \
+	PAD_CTL_DSE_40ohm   | PAD_CTL_SRE_FAST  | PAD_CTL_HYS)
+
 #define ENET_PAD_CTRL  (PAD_CTL_PKE | PAD_CTL_PUE |		\
 	PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_MED   |		\
 	PAD_CTL_DSE_40ohm   | PAD_CTL_HYS)
@@ -163,6 +168,19 @@ static void setup_iomux_enet(void)
 	imx_iomux_v3_setup_multiple_pads(enet_pads, ARRAY_SIZE(enet_pads));
 }
 
+iomux_v3_cfg_t const usdhc1_pads[] = {
+	/*To avoid pin conflict with NAND, set usdhc1 to 4 pins*/
+	MX6_PAD_SD1_CLK__USDHC1_CLK	| MUX_PAD_CTRL(USDHC1_PAD_CTRL),
+	MX6_PAD_SD1_CMD__USDHC1_CMD	| MUX_PAD_CTRL(USDHC1_PAD_CTRL),
+	MX6_PAD_SD1_DAT0__USDHC1_DAT0	| MUX_PAD_CTRL(USDHC1_PAD_CTRL),
+	MX6_PAD_SD1_DAT1__USDHC1_DAT1	| MUX_PAD_CTRL(USDHC1_PAD_CTRL),
+	MX6_PAD_SD1_DAT2__USDHC1_DAT2	| MUX_PAD_CTRL(USDHC1_PAD_CTRL),
+	MX6_PAD_SD1_DAT3__USDHC1_DAT3	| MUX_PAD_CTRL(USDHC1_PAD_CTRL),
+
+	/*CD pin*/
+	MX6_PAD_GPIO_1__GPIO_1_1 | MUX_PAD_CTRL(NO_PAD_CTRL),
+};
+
 iomux_v3_cfg_t const usdhc3_pads[] = {
 	MX6_PAD_SD3_CLK__USDHC3_CLK	| MUX_PAD_CTRL(USDHC_PAD_CTRL),
 	MX6_PAD_SD3_CMD__USDHC3_CMD	| MUX_PAD_CTRL(USDHC_PAD_CTRL),
@@ -285,23 +303,97 @@ static void setup_iomux_uart(void)
 }
 
 #ifdef CONFIG_FSL_ESDHC
-struct fsl_esdhc_cfg usdhc_cfg[1] = {
+
+#define USDHC1_CD_GPIO	IMX_GPIO_NR(1, 1)
+#define USDHC3_CD_GPIO	IMX_GPIO_NR(6, 15)
+
+struct fsl_esdhc_cfg usdhc_cfg[2] = {
+	{USDHC1_BASE_ADDR, 0, 4},
 	{USDHC3_BASE_ADDR},
 };
 
+int mmc_get_env_devno(void)
+{
+	u32 soc_sbmr = readl(SRC_BASE_ADDR + 0x4);
+	u32 dev_no;
+
+	/* BOOT_CFG2[3] and BOOT_CFG2[4] */
+	dev_no = (soc_sbmr & 0x00001800) >> 11;
+
+	/* need ubstract 1 to map to the mmc3 device id
+	 * see the comments in board_mmc_init function
+	 */
+	if (2 == dev_no)
+		dev_no--;
+
+	return dev_no;
+}
+
 int board_mmc_getcd(struct mmc *mmc)
 {
-	gpio_direction_input(IMX_GPIO_NR(6, 15));
-	return !gpio_get_value(IMX_GPIO_NR(6, 15));
+	struct fsl_esdhc_cfg *cfg = (struct fsl_esdhc_cfg *)mmc->priv;
+	int ret = 0;
+
+	switch (cfg->esdhc_base) {
+	case USDHC1_BASE_ADDR:
+		ret = !gpio_get_value(USDHC1_CD_GPIO);
+		break;
+	case USDHC3_BASE_ADDR:
+		ret = !gpio_get_value(USDHC3_CD_GPIO);
+		break;
+	}
+
+	return ret;
 }
 
 int board_mmc_init(bd_t *bis)
 {
-	imx_iomux_v3_setup_multiple_pads(usdhc3_pads, ARRAY_SIZE(usdhc3_pads));
+	int i;
+
+	/*
+	* According to the board_mmc_init() the following map is done:
+	* (U-boot device node)    (Physical Port)
+	* mmc0				USDHC1
+	* mmc1				USDHC3
+	*/
+	for (i = 0; i < CONFIG_SYS_FSL_USDHC_NUM; i++) {
+		switch (i) {
+		case 0:
+			imx_iomux_v3_setup_multiple_pads(
+				usdhc1_pads, ARRAY_SIZE(usdhc1_pads));
+			gpio_direction_input(USDHC1_CD_GPIO);
+			usdhc_cfg[0].sdhc_clk = mxc_get_clock(MXC_ESDHC_CLK);
+			break;
+		case 1:
+			imx_iomux_v3_setup_multiple_pads(
+				usdhc3_pads, ARRAY_SIZE(usdhc3_pads));
+			gpio_direction_input(USDHC3_CD_GPIO);
+			usdhc_cfg[1].sdhc_clk = mxc_get_clock(MXC_ESDHC3_CLK);
+			break;
+		default:
+			printf("Warning: you configured more USDHC controllers"
+				"(%d) than supported by the board\n", i + 1);
+			return 0;
+			}
+
+		if (fsl_esdhc_initialize(bis, &usdhc_cfg[i]))
+			printf("Warning: failed to initialize mmc dev %d\n", i);
+	}
+
+	return 0;
+}
+
+void board_late_mmc_env_init(void)
+{
+	char cmd[32];
+	u32 dev_no = mmc_get_env_devno();
+
+	setenv_ulong("mmcdev", dev_no);
 
-	usdhc_cfg[0].sdhc_clk = mxc_get_clock(MXC_ESDHC3_CLK);
-	return fsl_esdhc_initialize(bis, &usdhc_cfg[0]);
+	sprintf(cmd, "mmc dev %d", dev_no);
+	run_command(cmd, 0);
 }
+
 #endif
 
 #ifdef CONFIG_SYS_USE_SPINOR
@@ -847,6 +939,10 @@ int board_late_init(void)
 	setup_max7310();
 #endif
 
+#ifdef CONFIG_ENV_IS_IN_MMC
+	board_late_mmc_env_init();
+#endif
+
 	return 0;
 }
 
@@ -864,13 +960,20 @@ void board_fastboot_setup(void)
 		break;
 #endif /*CONFIG_FASTBOOT_STORAGE_SATA*/
 #if defined(CONFIG_FASTBOOT_STORAGE_MMC)
-	case SD3_BOOT:
-	case MMC3_BOOT:
+	case SD1_BOOT:
+	case MMC1_BOOT:
 		if (!getenv("fastboot_dev"))
 			setenv("fastboot_dev", "mmc0");
 		if (!getenv("bootcmd"))
 			setenv("bootcmd", "booti mmc0");
 		break;
+	case SD3_BOOT:
+	case MMC3_BOOT:
+		if (!getenv("fastboot_dev"))
+			setenv("fastboot_dev", "mmc1");
+		if (!getenv("bootcmd"))
+			setenv("bootcmd", "booti mmc1");
+		break;
 #endif /*CONFIG_FASTBOOT_STORAGE_MMC*/
 #if defined(CONFIG_FASTBOOT_STORAGE_NAND)
 	case NAND_BOOT:
@@ -931,10 +1034,15 @@ void board_recovery_setup(void)
 		break;
 #endif /*CONFIG_FASTBOOT_STORAGE_SATA*/
 #if defined(CONFIG_FASTBOOT_STORAGE_MMC)
+	case SD1_BOOT:
+	case MMC1_BOOT:
+		if (!getenv("bootcmd_android_recovery"))
+			setenv("bootcmd_android_recovery", "booti mmc0 recovery");
+		break;
 	case SD3_BOOT:
 	case MMC3_BOOT:
 		if (!getenv("bootcmd_android_recovery"))
-			setenv("bootcmd_android_recovery", "booti mmc0 recovery");
+			setenv("bootcmd_android_recovery", "booti mmc1 recovery");
 		break;
 #endif /*CONFIG_FASTBOOT_STORAGE_MMC*/
 #if defined(CONFIG_FASTBOOT_STORAGE_NAND)
diff --git a/board/freescale/mx6qsabresd/mx6qsabresd.c b/board/freescale/mx6qsabresd/mx6qsabresd.c
index b825675..c729e9a 100644
--- a/board/freescale/mx6qsabresd/mx6qsabresd.c
+++ b/board/freescale/mx6qsabresd/mx6qsabresd.c
@@ -430,8 +430,6 @@ int mmc_get_env_devno(void)
 
 	dev_no--;
 
-	setenv_ulong("mmcdev", dev_no);
-
 	return dev_no;
 }
 
@@ -501,6 +499,17 @@ int board_mmc_init(bd_t *bis)
 
 	return 0;
 }
+
+void board_late_mmc_env_init(void)
+{
+	char cmd[32];
+	u32 dev_no = mmc_get_env_devno();
+
+	setenv_ulong("mmcdev", dev_no);
+
+	sprintf(cmd, "mmc dev %d", dev_no);
+	run_command(cmd, 0);
+}
 #endif
 
 #if defined(CONFIG_MX6DL) && defined(CONFIG_MXC_EPDC)
@@ -1074,6 +1083,10 @@ int board_late_init(void)
 		return -1;
 #endif
 
+#ifdef CONFIG_ENV_IS_IN_MMC
+	board_late_mmc_env_init();
+#endif
+
 	return 0;
 }
 
diff --git a/board/freescale/mx6slevk/mx6slevk.c b/board/freescale/mx6slevk/mx6slevk.c
index 050125c..33c82d9 100644
--- a/board/freescale/mx6slevk/mx6slevk.c
+++ b/board/freescale/mx6slevk/mx6slevk.c
@@ -93,6 +93,23 @@ static iomux_v3_cfg_t const uart1_pads[] = {
 	MX6_PAD_UART1_RXD__UART1_RXD | MUX_PAD_CTRL(UART_PAD_CTRL),
 };
 
+static iomux_v3_cfg_t const usdhc1_pads[] = {
+	/* 8 bit SD */
+	MX6_PAD_SD1_CLK__USDHC1_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_CMD__USDHC1_CMD | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT0__USDHC1_DAT0 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT1__USDHC1_DAT1 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT2__USDHC1_DAT2 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT3__USDHC1_DAT3 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT4__USDHC1_DAT4 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT5__USDHC1_DAT5 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT6__USDHC1_DAT6 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD1_DAT7__USDHC1_DAT7 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+
+	/*CD pin*/
+	MX6_PAD_KEY_ROW7__GPIO_4_7 | MUX_PAD_CTRL(NO_PAD_CTRL),
+};
+
 static iomux_v3_cfg_t const usdhc2_pads[] = {
 	MX6_PAD_SD2_CLK__USDHC2_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL),
 	MX6_PAD_SD2_CMD__USDHC2_CMD | MUX_PAD_CTRL(USDHC_PAD_CTRL),
@@ -100,6 +117,21 @@ static iomux_v3_cfg_t const usdhc2_pads[] = {
 	MX6_PAD_SD2_DAT1__USDHC2_DAT1 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
 	MX6_PAD_SD2_DAT2__USDHC2_DAT2 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
 	MX6_PAD_SD2_DAT3__USDHC2_DAT3 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+
+	/*CD pin*/
+	MX6_PAD_SD2_DAT7__GPIO_5_0 | MUX_PAD_CTRL(NO_PAD_CTRL),
+};
+
+static iomux_v3_cfg_t const usdhc3_pads[] = {
+	MX6_PAD_SD3_CLK__USDHC3_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD3_CMD__USDHC3_CMD | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD3_DAT0__USDHC3_DAT0 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD3_DAT1__USDHC3_DAT1 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD3_DAT2__USDHC3_DAT2 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_SD3_DAT3__USDHC3_DAT3 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+
+	/*CD pin*/
+	MX6_PAD_REF_CLK_32K__GPIO_3_22 | MUX_PAD_CTRL(NO_PAD_CTRL),
 };
 
 static iomux_v3_cfg_t const fec_pads[] = {
@@ -193,22 +225,104 @@ void setup_spinor(void)
 }
 #endif
 
-static struct fsl_esdhc_cfg usdhc_cfg[1] = {
-	{USDHC2_BASE_ADDR},
+#ifdef CONFIG_FSL_ESDHC
+
+#define USDHC1_CD_GPIO	IMX_GPIO_NR(4, 7)
+#define USDHC2_CD_GPIO	IMX_GPIO_NR(5, 0)
+#define USDHC3_CD_GPIO	IMX_GPIO_NR(3, 22)
+
+static struct fsl_esdhc_cfg usdhc_cfg[3] = {
+	{USDHC1_BASE_ADDR},
+	{USDHC2_BASE_ADDR, 0, 4},
+	{USDHC3_BASE_ADDR, 0, 4},
 };
 
+int mmc_get_env_devno(void)
+{
+	u32 soc_sbmr = readl(SRC_BASE_ADDR + 0x4);
+	u32 dev_no;
+
+	/* BOOT_CFG2[3] and BOOT_CFG2[4] */
+	dev_no = (soc_sbmr & 0x00001800) >> 11;
+
+	return dev_no;
+}
+
 int board_mmc_getcd(struct mmc *mmc)
 {
-	return 1;	/* Assume boot SD always present */
+	struct fsl_esdhc_cfg *cfg = (struct fsl_esdhc_cfg *)mmc->priv;
+	int ret = 0;
+
+	switch (cfg->esdhc_base) {
+	case USDHC1_BASE_ADDR:
+		ret = !gpio_get_value(USDHC1_CD_GPIO);
+		break;
+	case USDHC2_BASE_ADDR:
+		ret = !gpio_get_value(USDHC2_CD_GPIO);
+		break;
+	case USDHC3_BASE_ADDR:
+		ret = !gpio_get_value(USDHC3_CD_GPIO);
+		break;
+	}
+
+	return ret;
 }
 
 int board_mmc_init(bd_t *bis)
 {
-	imx_iomux_v3_setup_multiple_pads(usdhc2_pads, ARRAY_SIZE(usdhc2_pads));
+	int i;
+
+	/*
+	 * According to the board_mmc_init() the following map is done:
+	 * (U-boot device node)    (Physical Port)
+	 * mmc0                    USDHC1
+	 * mmc1                    USDHC2
+	 * mmc2                    USDHC3
+	 */
+	for (i = 0; i < CONFIG_SYS_FSL_USDHC_NUM; i++) {
+		switch (i) {
+		case 0:
+			imx_iomux_v3_setup_multiple_pads(
+				usdhc1_pads, ARRAY_SIZE(usdhc1_pads));
+			gpio_direction_input(USDHC1_CD_GPIO);
+			usdhc_cfg[0].sdhc_clk = mxc_get_clock(MXC_ESDHC_CLK);
+			break;
+		case 1:
+			imx_iomux_v3_setup_multiple_pads(
+				usdhc2_pads, ARRAY_SIZE(usdhc2_pads));
+			gpio_direction_input(USDHC2_CD_GPIO);
+			usdhc_cfg[1].sdhc_clk = mxc_get_clock(MXC_ESDHC2_CLK);
+			break;
+		case 2:
+			imx_iomux_v3_setup_multiple_pads(
+				usdhc3_pads, ARRAY_SIZE(usdhc3_pads));
+			gpio_direction_input(USDHC3_CD_GPIO);
+			usdhc_cfg[2].sdhc_clk = mxc_get_clock(MXC_ESDHC3_CLK);
+			break;
+		default:
+			printf("Warning: you configured more USDHC controllers"
+				"(%d) than supported by the board\n", i + 1);
+			return 0;
+			}
+
+			if (fsl_esdhc_initialize(bis, &usdhc_cfg[i]))
+				printf("Warning: failed to initialize mmc dev %d\n", i);
+	}
+
+	return 0;
+}
+
+void board_late_mmc_env_init(void)
+{
+	char cmd[32];
+	u32 dev_no = mmc_get_env_devno();
+
+	setenv_ulong("mmcdev", dev_no);
 
-	usdhc_cfg[0].sdhc_clk = mxc_get_clock(MXC_ESDHC2_CLK);
-	return fsl_esdhc_initialize(bis, &usdhc_cfg[0]);
+	sprintf(cmd, "mmc dev %d", dev_no);
+	run_command(cmd, 0);
 }
+#endif
 
 #ifdef CONFIG_MXC_EPDC
 #ifdef CONFIG_SPLASH_SCREEN
@@ -649,6 +763,10 @@ int board_late_init(void)
 		return -1;
 #endif
 
+#ifdef CONFIG_ENV_IS_IN_MMC
+	board_late_mmc_env_init();
+#endif
+
 	return 0;
 }
 
@@ -661,16 +779,29 @@ u32 get_board_rev(void)
 
 void board_fastboot_setup(void)
 {
-	/*current uboot BSP only supports USDHC2*/
 	switch (get_boot_device()) {
 #if defined(CONFIG_FASTBOOT_STORAGE_MMC)
-	case SD2_BOOT:
-	case MMC2_BOOT:
+	case SD1_BOOT:
+	case MMC1_BOOT:
 		if (!getenv("fastboot_dev"))
 			setenv("fastboot_dev", "mmc0");
 		if (!getenv("bootcmd"))
 			setenv("bootcmd", "booti mmc0");
 		break;
+	case SD2_BOOT:
+	case MMC2_BOOT:
+		if (!getenv("fastboot_dev"))
+			setenv("fastboot_dev", "mmc1");
+		if (!getenv("bootcmd"))
+			setenv("bootcmd", "booti mmc1");
+		break;
+	case SD3_BOOT:
+	case MMC3_BOOT:
+		if (!getenv("fastboot_dev"))
+			setenv("fastboot_dev", "mmc2");
+		if (!getenv("bootcmd"))
+			setenv("bootcmd", "booti mmc2");
+		break;
 #endif /*CONFIG_FASTBOOT_STORAGE_MMC*/
 	default:
 		printf("unsupported boot devices\n");
@@ -692,11 +823,23 @@ void board_recovery_setup(void)
 	/*current uboot BSP only supports USDHC2*/
 	switch (bootdev) {
 #if defined(CONFIG_FASTBOOT_STORAGE_MMC)
+	case SD1_BOOT:
+	case MMC1_BOOT:
+		if (!getenv("bootcmd_android_recovery"))
+			setenv("bootcmd_android_recovery",
+					"booti mmc0 recovery");
+		break;
 	case SD2_BOOT:
 	case MMC2_BOOT:
 		if (!getenv("bootcmd_android_recovery"))
 			setenv("bootcmd_android_recovery",
-					"booti mmc0 recovery");
+					"booti mmc1 recovery");
+		break;
+	case SD3_BOOT:
+	case MMC3_BOOT:
+		if (!getenv("bootcmd_android_recovery"))
+			setenv("bootcmd_android_recovery",
+					"booti mmc2 recovery");
 		break;
 #endif /*CONFIG_FASTBOOT_STORAGE_MMC*/
 	default:
diff --git a/include/configs/mx6qarm2.h b/include/configs/mx6qarm2.h
index 22f39e8..11f6861 100644
--- a/include/configs/mx6qarm2.h
+++ b/include/configs/mx6qarm2.h
@@ -51,6 +51,7 @@
 #define CONFIG_SYS_MALLOC_LEN		(CONFIG_ENV_SIZE + 2 * 1024 * 1024)
 
 #define CONFIG_BOARD_EARLY_INIT_F
+#define CONFIG_BOARD_LATE_INIT
 #define CONFIG_MXC_GPIO
 
 #define CONFIG_MXC_UART
@@ -182,7 +183,7 @@
 /* FLASH and environment organization */
 #define CONFIG_SYS_NO_FLASH
 
-#define CONFIG_ENV_OFFSET		(6 * 64 * 1024)
+#define CONFIG_ENV_OFFSET		(8 * 64 * 1024)
 #define CONFIG_ENV_SIZE			(8 * 1024)
 #define CONFIG_ENV_IS_IN_MMC
 #define CONFIG_SYS_MMC_ENV_DEV		1
diff --git a/include/configs/mx6qsabreauto.h b/include/configs/mx6qsabreauto.h
index 3a078af..43be4da 100644
--- a/include/configs/mx6qsabreauto.h
+++ b/include/configs/mx6qsabreauto.h
@@ -46,7 +46,7 @@
 #endif
 
 #define CONFIG_SYS_FSL_USDHC_NUM	2
-#define CONFIG_SYS_MMC_ENV_DEV		0
+#define CONFIG_SYS_MMC_ENV_DEV		1
 #define CONFIG_SYS_MMC_ENV_PART		0	/* user partition */
 
 #ifdef CONFIG_SYS_USE_SPINOR
diff --git a/include/configs/mx6slevk.h b/include/configs/mx6slevk.h
index 060e8f1..aad4615 100644
--- a/include/configs/mx6slevk.h
+++ b/include/configs/mx6slevk.h
@@ -61,6 +61,7 @@
 #define CONFIG_FSL_ESDHC
 #define CONFIG_FSL_USDHC
 #define CONFIG_SYS_FSL_ESDHC_ADDR	0
+#define CONFIG_SYS_FSL_USDHC_NUM 3
 
 #define CONFIG_MMC
 #define CONFIG_CMD_MMC
@@ -127,7 +128,7 @@
 	"fdt_addr=0x83000000\0" \
 	"boot_fdt=try\0" \
 	"ip_dyn=yes\0" \
-	"mmcdev=0\0" \
+	"mmcdev=1\0" \
 	"mmcpart=1\0" \
 	"mmcroot=/dev/mmcblk0p2 rootwait rw\0" \
 	"mmcargs=setenv bootargs console=${console},${baudrate} " \
@@ -230,7 +231,7 @@
 #define CONFIG_SYS_NO_FLASH
 
 #define CONFIG_ENV_SIZE			SZ_8K
-#define CONFIG_SYS_MMC_ENV_DEV		0
+#define CONFIG_SYS_MMC_ENV_DEV		1
 
 #if defined CONFIG_SYS_BOOT_SPINOR
 #define CONFIG_SYS_USE_SPINOR