imx6: Engicam i.CoreM6/Is.IoT eMMC boot support

index e5dbd93..036ebb2 100644
@@ -115,3 +115,15 @@ void boot_mode_apply(unsigned cfg_val)
 	writel(reg, &psrc->gpr10);
 }
 #endif
+
+#if defined(CONFIG_MX6)
+u32 imx6_src_get_boot_mode(void)
+{
+	struct src *psrc = (struct src *)SRC_BASE_ADDR;
+
+	if (imx6_is_bmode_from_gpr9())
+		return readl(&psrc->gpr9);
+	else
+		return readl(&psrc->sbmr1);
+}
+#endif

index 60c4adf..a7f9705 100644
@@ -10,6 +10,7 @@
 #include <common.h>
 #include <asm/io.h>
 #include <asm/arch/imx-regs.h>
+#include <asm/arch/sys_proto.h>
 #include <asm/spl.h>
 #include <spl.h>
 #include <asm/imx-common/hab.h>

@@ -19,16 +20,15 @@
 u32 spl_boot_device(void)
 {
 	struct src *psrc = (struct src *)SRC_BASE_ADDR;
-	unsigned int gpr10_boot = readl(&psrc->gpr10) & (1 << 28);
-	unsigned reg = gpr10_boot ? readl(&psrc->gpr9) : readl(&psrc->sbmr1);
 	unsigned int bmode = readl(&psrc->sbmr2);
+	u32 reg = imx6_src_get_boot_mode();
 
 	/*
 	 * Check for BMODE if serial downloader is enabled
 	 * BOOT_MODE - see IMX6DQRM Table 8-1
 	 */
 	if ((((bmode >> 24) & 0x03)  == 0x01) || /* Serial Downloader */
-		(gpr10_boot && (reg == 1)))
+		(imx6_is_bmode_from_gpr9() && (reg == 1)))
 		return BOOT_DEVICE_UART;
 	/* BOOT_CFG1[7:4] - see IMX6DQRM Table 8-8 */
 	switch ((reg & 0x000000FF) >> 4) {

index 539d34b..99e3869 100644
@@ -8,6 +8,7 @@
 #ifndef _SYS_PROTO_H_
 #define _SYS_PROTO_H_
 
+#include <asm/io.h>
 #include <asm/imx-common/regs-common.h>
 #include <common.h>
 #include "../arch-imx/cpu.h"

@@ -38,6 +39,19 @@
 #define is_mx6ull() (is_cpu_type(MXC_CPU_MX6ULL))
 #define is_mx6sll() (is_cpu_type(MXC_CPU_MX6SLL))
 
+#ifdef CONFIG_MX6
+#define IMX6_SRC_GPR10_BMODE		BIT(28)
+
+static inline u8 imx6_is_bmode_from_gpr9(void)
+{
+	struct src *psrc = (struct src *)SRC_BASE_ADDR;
+
+	return readl(&psrc->gpr10) & IMX6_SRC_GPR10_BMODE;
+}
+
+u32 imx6_src_get_boot_mode(void);
+#endif /* CONFIG_MX6 */
+
 u32 get_nr_cpus(void);
 u32 get_cpu_rev(void);
 u32 get_cpu_speed_grade_hz(void);

index a7f9705..fc3704b 100644
@@ -61,8 +61,8 @@ u32 spl_boot_device(void)
 	case 0x6:
 	case 0x7:
 		return BOOT_DEVICE_MMC1;
-	/* NAND Flash: 8.5.2 */
-	case 0x8 ... 0xf:
+	/* NAND Flash: 8.5.2, Table 8-10 */
+	case 0x8:
 		return BOOT_DEVICE_NAND;
 	}
 	return BOOT_DEVICE_NONE;

index fc3704b..38789b2 100644
@@ -30,39 +30,48 @@ u32 spl_boot_device(void)
 	if ((((bmode >> 24) & 0x03)  == 0x01) || /* Serial Downloader */
 		(imx6_is_bmode_from_gpr9() && (reg == 1)))
 		return BOOT_DEVICE_UART;
+
 	/* BOOT_CFG1[7:4] - see IMX6DQRM Table 8-8 */
-	switch ((reg & 0x000000FF) >> 4) {
+	switch ((reg & IMX6_BMODE_MASK) >> IMX6_BMODE_SHIFT) {
 	 /* EIM: See 8.5.1, Table 8-9 */
-	case 0x0:
+	case IMX6_BMODE_EMI:
 		/* BOOT_CFG1[3]: NOR/OneNAND Selection */
-		if ((reg & 0x00000008) >> 3)
+		switch ((reg & IMX6_BMODE_EMI_MASK) >> IMX6_BMODE_EMI_SHIFT) {
+		case IMX6_BMODE_ONENAND:
 			return BOOT_DEVICE_ONENAND;
-		else
+		case IMX6_BMODE_NOR:
 			return BOOT_DEVICE_NOR;
-		break;
+		}
 	/* SATA: See 8.5.4, Table 8-20 */
-	case 0x2:
+	case IMX6_BMODE_SATA:
 		return BOOT_DEVICE_SATA;
 	/* Serial ROM: See 8.5.5.1, Table 8-22 */
-	case 0x3:
+	case IMX6_BMODE_SERIAL:
 		/* BOOT_CFG4[2:0] */
-		switch ((reg & 0x07000000) >> 24) {
-		case 0x0 ... 0x4:
+		switch ((reg & IMX6_BMODE_SERIAL_MASK) >>
+			IMX6_BMODE_SERIAL_SHIFT) {
+		case IMX6_BMODE_ECSPI1:
+		case IMX6_BMODE_ECSPI2:
+		case IMX6_BMODE_ECSPI3:
+		case IMX6_BMODE_ECSPI4:
+		case IMX6_BMODE_ECSPI5:
 			return BOOT_DEVICE_SPI;
-		case 0x5 ... 0x7:
+		case IMX6_BMODE_I2C1:
+		case IMX6_BMODE_I2C2:
+		case IMX6_BMODE_I2C3:
 			return BOOT_DEVICE_I2C;
 		}
 		break;
 	/* SD/eSD: 8.5.3, Table 8-15  */
-	case 0x4:
-	case 0x5:
+	case IMX6_BMODE_SD:
+	case IMX6_BMODE_ESD:
 		return BOOT_DEVICE_MMC1;
 	/* MMC/eMMC: 8.5.3 */
-	case 0x6:
-	case 0x7:
+	case IMX6_BMODE_MMC:
+	case IMX6_BMODE_EMMC:
 		return BOOT_DEVICE_MMC1;
 	/* NAND Flash: 8.5.2, Table 8-10 */
-	case 0x8:
+	case IMX6_BMODE_NAND:
 		return BOOT_DEVICE_NAND;
 	}
 	return BOOT_DEVICE_NONE;

index 99e3869..d0cf3f1 100644
@@ -42,6 +42,40 @@
 #ifdef CONFIG_MX6
 #define IMX6_SRC_GPR10_BMODE		BIT(28)
 
+#define IMX6_BMODE_MASK			GENMASK(7, 0)
+#define	IMX6_BMODE_SHIFT		BIT(2)
+#define IMX6_BMODE_EMI_MASK		BIT(3)
+#define IMX6_BMODE_EMI_SHIFT		GENMASK(1, 0)
+#define IMX6_BMODE_SERIAL_MASK		GENMASK(26, 24)
+#define IMX6_BMODE_SERIAL_SHIFT		GENMASK(4, 3)
+
+enum imx6_bmode_serial {
+	IMX6_BMODE_ECSPI1,
+	IMX6_BMODE_ECSPI2,
+	IMX6_BMODE_ECSPI3,
+	IMX6_BMODE_ECSPI4,
+	IMX6_BMODE_ECSPI5,
+	IMX6_BMODE_I2C1,
+	IMX6_BMODE_I2C2,
+	IMX6_BMODE_I2C3,
+};
+
+enum imx6_bmode_emi {
+	IMX6_BMODE_ONENAND,
+	IMX6_BMODE_NOR,
+};
+
+enum imx6_bmode {
+	IMX6_BMODE_EMI,
+	IMX6_BMODE_SATA		= 0x2,
+	IMX6_BMODE_SERIAL,
+	IMX6_BMODE_SD,
+	IMX6_BMODE_ESD,
+	IMX6_BMODE_MMC,
+	IMX6_BMODE_EMMC,
+	IMX6_BMODE_NAND,
+};
+
 static inline u8 imx6_is_bmode_from_gpr9(void)
 {
 	struct src *psrc = (struct src *)SRC_BASE_ADDR;

index 036ebb2..5b4f828 100644
@@ -119,11 +119,9 @@ void boot_mode_apply(unsigned cfg_val)
 #if defined(CONFIG_MX6)
 u32 imx6_src_get_boot_mode(void)
 {
-	struct src *psrc = (struct src *)SRC_BASE_ADDR;
-
 	if (imx6_is_bmode_from_gpr9())
-		return readl(&psrc->gpr9);
+		return readl(&src_base->gpr9);
 	else
-		return readl(&psrc->sbmr1);
+		return readl(&src_base->sbmr1);
 }
 #endif

index 38789b2..4d73a67 100644
@@ -19,8 +19,7 @@
 /* determine boot device from SRC_SBMR1 (BOOT_CFG[4:1]) or SRC_GPR9 register */
 u32 spl_boot_device(void)
 {
-	struct src *psrc = (struct src *)SRC_BASE_ADDR;
-	unsigned int bmode = readl(&psrc->sbmr2);
+	unsigned int bmode = readl(&src_base->sbmr2);
 	u32 reg = imx6_src_get_boot_mode();
 
 	/*

index 6727c56..646013d 100644
@@ -473,6 +473,8 @@ struct src {
 	u32     gpr10;
 };
 
+#define src_base ((struct src *)SRC_BASE_ADDR)
+
 #define SRC_SCR_M4_ENABLE_OFFSET                22
 #define SRC_SCR_M4_ENABLE_MASK                  (1 << 22)
 #define SRC_SCR_M4C_NON_SCLR_RST_OFFSET         4

index d0cf3f1..d149c9f 100644
@@ -78,9 +78,7 @@ enum imx6_bmode {
 
 static inline u8 imx6_is_bmode_from_gpr9(void)
 {
-	struct src *psrc = (struct src *)SRC_BASE_ADDR;
-
-	return readl(&psrc->gpr10) & IMX6_SRC_GPR10_BMODE;
+	return readl(&src_base->gpr10) & IMX6_SRC_GPR10_BMODE;
 }
 
 u32 imx6_src_get_boot_mode(void);

index ae3465f..6b84f02 100644
@@ -205,6 +205,32 @@ int board_mmc_init(bd_t *bis)
 
 	return 0;
 }
+
+#ifdef CONFIG_ENV_IS_IN_MMC
+void board_boot_order(u32 *spl_boot_list)
+{
+	u32 bmode = imx6_src_get_boot_mode();
+	u8 boot_dev = BOOT_DEVICE_MMC1;
+
+	switch ((bmode & IMX6_BMODE_MASK) >> IMX6_BMODE_SHIFT) {
+	case IMX6_BMODE_SD:
+	case IMX6_BMODE_ESD:
+		/* SD/eSD - BOOT_DEVICE_MMC1 */
+		break;
+	case IMX6_BMODE_MMC:
+	case IMX6_BMODE_EMMC:
+		/* MMC/eMMC */
+		boot_dev = BOOT_DEVICE_MMC2;
+		break;
+	default:
+		/* Default - BOOT_DEVICE_MMC1 */
+		printf("Wrong board boot order\n");
+		break;
+	}
+
+	spl_boot_list[0] = boot_dev;
+}
+#endif
 #endif /* CONFIG_FSL_ESDHC */
 
 static struct mx6ul_iomux_grp_regs mx6_grp_ioregs = {

index 6b84f02..17c19e0 100644
@@ -153,10 +153,24 @@ static iomux_v3_cfg_t const usdhc1_pads[] = {
 	MX6_PAD_GPIO1_IO09__GPIO1_IO09 | MUX_PAD_CTRL(NO_PAD_CTRL),
 };
 
+static iomux_v3_cfg_t const usdhc2_pads[] = {
+	MX6_PAD_NAND_ALE__USDHC2_RESET_B | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_RE_B__USDHC2_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_WE_B__USDHC2_CMD | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA00__USDHC2_DATA0| MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA01__USDHC2_DATA1 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA02__USDHC2_DATA2 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA03__USDHC2_DATA3 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA04__USDHC2_DATA4| MUX_PAD_CTRL(USDHC_PAD_CTRL),
+	MX6_PAD_NAND_DATA05__USDHC2_DATA5 | MUX_PAD_CTRL(USDHC_PAD_CTRL),
+};
+
 #define USDHC1_CD_GPIO	IMX_GPIO_NR(1, 19)
+#define USDHC2_CD_GPIO	IMX_GPIO_NR(4, 5)
 
-struct fsl_esdhc_cfg usdhc_cfg[1] = {
+struct fsl_esdhc_cfg usdhc_cfg[2] = {
 	{USDHC1_BASE_ADDR, 0, 4},
+	{USDHC2_BASE_ADDR, 0, 8},
 };
 
 int board_mmc_getcd(struct mmc *mmc)

@@ -168,6 +182,9 @@ int board_mmc_getcd(struct mmc *mmc)
 	case USDHC1_BASE_ADDR:
 		ret = !gpio_get_value(USDHC1_CD_GPIO);
 		break;
+	case USDHC2_BASE_ADDR:
+		ret = !gpio_get_value(USDHC2_CD_GPIO);
+		break;
 	}
 
 	return ret;

@@ -181,6 +198,7 @@ int board_mmc_init(bd_t *bis)
 	* According to the board_mmc_init() the following map is done:
 	* (U-boot device node)    (Physical Port)
 	* mmc0				USDHC1
+	* mmc1				USDHC2
 	*/
 	for (i = 0; i < CONFIG_SYS_FSL_USDHC_NUM; i++) {
 		switch (i) {

@@ -190,6 +208,12 @@ int board_mmc_init(bd_t *bis)
 			gpio_direction_input(USDHC1_CD_GPIO);
 			usdhc_cfg[i].sdhc_clk = mxc_get_clock(MXC_ESDHC_CLK);
 			break;
+		case 1:
+			imx_iomux_v3_setup_multiple_pads(
+				usdhc1_pads, ARRAY_SIZE(usdhc2_pads));
+			gpio_direction_input(USDHC2_CD_GPIO);
+			usdhc_cfg[i].sdhc_clk = mxc_get_clock(MXC_ESDHC2_CLK);
+			break;
 		default:
 			printf("Warning - USDHC%d controller not supporting\n",
 			       i + 1);

index 10311d0..1b0e436 100644
@@ -145,7 +145,7 @@
 /* MMC */
 #ifdef CONFIG_FSL_USDHC
 # define CONFIG_SYS_MMC_ENV_DEV		0
-# define CONFIG_SYS_FSL_USDHC_NUM	1
+# define CONFIG_SYS_FSL_USDHC_NUM	2
 # define CONFIG_SYS_FSL_ESDHC_ADDR	0
 #endif
 

index 17c19e0..3fc5bca 100644
@@ -103,6 +103,27 @@ static void setup_gpmi_nand(void)
 }
 #endif /* CONFIG_NAND_MXS */
 
+int board_late_init(void)
+{
+	switch ((imx6_src_get_boot_mode() & IMX6_BMODE_MASK) >>
+			IMX6_BMODE_SHIFT) {
+	case IMX6_BMODE_SD:
+	case IMX6_BMODE_ESD:
+	case IMX6_BMODE_MMC:
+	case IMX6_BMODE_EMMC:
+		setenv("modeboot", "mmcboot");
+		break;
+	case IMX6_BMODE_NAND:
+		setenv("modeboot", "nandboot");
+		break;
+	default:
+		setenv("modeboot", "");
+		break;
+	}
+
+	return 0;
+}
+
 int board_init(void)
 {
 	/* Address of boot parameters */

index 1b0e436..7258fed 100644
@@ -64,8 +64,7 @@
 	"fitboot=echo Booting FIT image from mmc ...; " \
 		"run mmcargs; " \
 		"bootm ${loadaddr}\0" \
-	"mmcboot=echo Booting from mmc ...; " \
-		"run mmcargs; " \
+	"_mmcboot=run mmcargs; " \
 		"if test ${boot_fdt} = yes || test ${boot_fdt} = try; then " \
 			"if run loadfdt; then " \
 				"bootm ${loadaddr} - ${fdt_addr}; " \

@@ -79,6 +78,20 @@
 		"else " \
 			"bootm; " \
 		"fi\0" \
+	"mmcboot=echo Booting from mmc ...; " \
+		"if mmc rescan; then " \
+			"if run loadbootscript; then " \
+				"run bootscript; " \
+			"else " \
+				"if run loadfit; then " \
+					"run fitboot; " \
+				"else " \
+					"if run loadimage; then " \
+						"run _mmcboot; " \
+					"fi; " \
+				"fi; " \
+			"fi; " \
+		"fi\0" \
 	"nandboot=echo Booting from nand ...; " \
 		"if mtdparts; then " \
 			"echo Starting nand boot ...; " \

@@ -90,24 +103,7 @@
 		"nand read ${fdt_addr} dtb 0x100000; " \
 		"bootm ${loadaddr} - ${fdt_addr}\0"
 
-#ifdef CONFIG_NAND_MXS
-# define CONFIG_BOOTCOMMAND		"run nandboot"
-#else
-# define CONFIG_BOOTCOMMAND \
-	"if mmc rescan; then " \
-		"if run loadbootscript; then " \
-			"run bootscript; " \
-		"else " \
-			"if run loadfit; then " \
-				"run fitboot; " \
-			"else " \
-				"if run loadimage; then " \
-					"run mmcboot; " \
-				"fi; " \
-			"fi; " \
-		"fi; " \
-	"fi"
-#endif
+#define CONFIG_BOOTCOMMAND		"run $modeboot"
 
 /* Miscellaneous configurable options */
 #define CONFIG_SYS_MEMTEST_START	0x80000000

index 3fc5bca..920d317 100644
@@ -7,6 +7,7 @@
  */
 
 #include <common.h>
+#include <mmc.h>
 
 #include <asm/io.h>
 #include <asm/gpio.h>

@@ -103,6 +104,24 @@ static void setup_gpmi_nand(void)
 }
 #endif /* CONFIG_NAND_MXS */
 
+#ifdef CONFIG_ENV_IS_IN_MMC
+static void mmc_late_init(void)
+{
+	char cmd[32];
+	char mmcblk[32];
+	u32 dev_no = mmc_get_env_dev();
+
+	setenv_ulong("mmcdev", dev_no);
+
+	/* Set mmcblk env */
+	sprintf(mmcblk, "/dev/mmcblk%dp2 rootwait rw", dev_no);
+	setenv("mmcroot", mmcblk);
+
+	sprintf(cmd, "mmc dev %d", dev_no);
+	run_command(cmd, 0);
+}
+#endif
+
 int board_late_init(void)
 {
 	switch ((imx6_src_get_boot_mode() & IMX6_BMODE_MASK) >>

@@ -111,6 +130,9 @@ int board_late_init(void)
 	case IMX6_BMODE_ESD:
 	case IMX6_BMODE_MMC:
 	case IMX6_BMODE_EMMC:
+#ifdef CONFIG_ENV_IS_IN_MMC
+		mmc_late_init();
+#endif
 		setenv("modeboot", "mmcboot");
 		break;
 	case IMX6_BMODE_NAND:

index 7258fed..4009648 100644
@@ -45,9 +45,7 @@
 	"fdt_file=" CONFIG_DEFAULT_FDT_FILE "\0" \
 	"fdt_addr=0x87800000\0" \
 	"boot_fdt=try\0" \
-	"mmcdev=0\0" \
 	"mmcpart=1\0" \
-	"mmcroot=/dev/mmcblk0p2 rootwait rw\0" \
 	"nandroot=ubi0:rootfs rootfstype=ubifs\0" \
 	"mmcautodetect=yes\0" \
 	"mmcargs=setenv bootargs console=${console},${baudrate} " \

index 920d317..df432a1 100644
@@ -105,6 +105,12 @@ static void setup_gpmi_nand(void)
 #endif /* CONFIG_NAND_MXS */
 
 #ifdef CONFIG_ENV_IS_IN_MMC
+int board_mmc_get_env_dev(int devno)
+{
+	/* dev 0 for SD/eSD, dev 1 for MMC/eMMC */
+	return (devno == 0) ? 0 : 1;
+}
+
 static void mmc_late_init(void)
 {
 	char cmd[32];

index 2769177..e3c520f 100644
@@ -125,6 +125,32 @@ int board_mmc_init(bd_t *bis)
 
 	return 0;
 }
+
+#ifdef CONFIG_ENV_IS_IN_MMC
+void board_boot_order(u32 *spl_boot_list)
+{
+	u32 bmode = imx6_src_get_boot_mode();
+	u8 boot_dev = BOOT_DEVICE_MMC1;
+
+	switch ((bmode & IMX6_BMODE_MASK) >> IMX6_BMODE_SHIFT) {
+	case IMX6_BMODE_SD:
+	case IMX6_BMODE_ESD:
+		/* SD/eSD - BOOT_DEVICE_MMC1 */
+		break;
+	case IMX6_BMODE_MMC:
+	case IMX6_BMODE_EMMC:
+		/* MMC/eMMC */
+		boot_dev = BOOT_DEVICE_MMC2;
+		break;
+	default:
+		/* Default - BOOT_DEVICE_MMC1 */
+		printf("Wrong board boot order\n");
+		break;
+	}
+
+	spl_boot_list[0] = boot_dev;
+}
+#endif
 #endif
 
 /*

index e3c520f..66cf487 100644
@@ -77,8 +77,22 @@ static iomux_v3_cfg_t const usdhc3_pads[] = {
 	IOMUX_PADS(PAD_SD3_DAT3__SD3_DATA3 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
 };
 
-struct fsl_esdhc_cfg usdhc_cfg[1] = {
+static iomux_v3_cfg_t const usdhc4_pads[] = {
+	IOMUX_PADS(PAD_SD4_CLK__SD4_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_CMD__SD4_CMD | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT0__SD4_DATA0 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT1__SD4_DATA1 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT2__SD4_DATA2 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT3__SD4_DATA3 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT4__SD4_DATA4 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT5__SD4_DATA5 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT6__SD4_DATA6 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+        IOMUX_PADS(PAD_SD4_DAT7__SD4_DATA7 | MUX_PAD_CTRL(USDHC_PAD_CTRL)),
+};
+
+struct fsl_esdhc_cfg usdhc_cfg[2] = {
 	{USDHC3_BASE_ADDR, 1, 4},
+	{USDHC4_BASE_ADDR, 1, 8},
 };
 
 int board_mmc_getcd(struct mmc *mmc)

@@ -88,6 +102,7 @@ int board_mmc_getcd(struct mmc *mmc)
 
 	switch (cfg->esdhc_base) {
 	case USDHC3_BASE_ADDR:
+	case USDHC4_BASE_ADDR:
 		ret = 1;
 		break;
 	}

@@ -102,7 +117,8 @@ int board_mmc_init(bd_t *bis)
 	/*
 	* According to the board_mmc_init() the following map is done:
 	* (U-boot device node)    (Physical Port)
-	* mmc0				USDHC3
+	* mmc0			USDHC3
+	* mmc1			USDHC4
 	*/
 	for (i = 0; i < CONFIG_SYS_FSL_USDHC_NUM; i++) {
 		switch (i) {

@@ -110,6 +126,10 @@ int board_mmc_init(bd_t *bis)
 			SETUP_IOMUX_PADS(usdhc3_pads);
 			usdhc_cfg[i].sdhc_clk = mxc_get_clock(MXC_ESDHC3_CLK);
 			break;
+		case 1:
+			SETUP_IOMUX_PADS(usdhc4_pads);
+			usdhc_cfg[i].sdhc_clk = mxc_get_clock(MXC_ESDHC4_CLK);
+			break;
 		default:
 			printf("Warning - USDHC%d controller not supporting\n",
 			       i + 1);

index 6f7195d..cd94c5f 100644
@@ -124,7 +124,7 @@
 /* MMC */
 #ifdef CONFIG_FSL_USDHC
 # define CONFIG_SYS_MMC_ENV_DEV		0
-# define CONFIG_SYS_FSL_USDHC_NUM	1
+# define CONFIG_SYS_FSL_USDHC_NUM	2
 # define CONFIG_SYS_FSL_ESDHC_ADDR	0
 #endif
 

index 66cf487..4bb43d1 100644
@@ -45,6 +45,24 @@ int board_init(void)
 	return 0;
 }
 
+int board_late_init(void)
+{
+	switch ((imx6_src_get_boot_mode() & IMX6_BMODE_MASK) >>
+			IMX6_BMODE_SHIFT) {
+	case IMX6_BMODE_SD:
+	case IMX6_BMODE_ESD:
+	case IMX6_BMODE_MMC:
+	case IMX6_BMODE_EMMC:
+		setenv("modeboot", "mmcboot");
+		break;
+	default:
+		setenv("modeboot", "");
+		break;
+	}
+
+	return 0;
+}
+
 int dram_init(void)
 {
 	gd->ram_size = imx_ddr_size();

index cd94c5f..c62c1d4 100644
@@ -56,8 +56,7 @@
 	"fitboot=echo Booting FIT image from mmc ...; " \
 		"run mmcargs; " \
 		"bootm ${loadaddr}\0" \
-	"mmcboot=echo Booting from mmc ...; " \
-		"run mmcargs; " \
+	"_mmcboot=run mmcargs; " \
 		"if test ${boot_fdt} = yes || test ${boot_fdt} = try; then " \
 			"if run loadfdt; then " \
 				"bootm ${loadaddr} - ${fdt_addr}; " \

@@ -70,23 +69,24 @@
 			"fi; " \
 		"else " \
 			"bootm; " \
-		"fi\0"
-
-#define CONFIG_BOOTCOMMAND \
-	"mmc dev ${mmcdev};" \
-	"if mmc rescan; then " \
-		"if run loadbootscript; then " \
-			"run bootscript; " \
-		"else " \
-			"if run loadfit; then " \
-				"run fitboot; " \
+		"fi\0" \
+	"mmcboot=echo Booting from mmc ...; " \
+		"mmc dev ${mmcdev};" \
+		"if mmc rescan; then " \
+			"if run loadbootscript; then " \
+				"run bootscript; " \
 			"else " \
-				"if run loadimage; then " \
-					"run mmcboot; " \
+				"if run loadfit; then " \
+					"run fitboot; " \
+				"else " \
+					"if run loadimage; then " \
+						"run _mmcboot; " \
+					"fi; " \
 				"fi; " \
 			"fi; " \
-		"fi; " \
-	"fi"
+		"fi\0"
+
+#define CONFIG_BOOTCOMMAND		"run $modeboot"
 
 /* Miscellaneous configurable options */
 #define CONFIG_SYS_MEMTEST_START	0x80000000

index 4bb43d1..f289e91 100644
@@ -7,6 +7,7 @@
  */
 
 #include <common.h>
+#include <mmc.h>
 
 #include <asm/io.h>
 #include <asm/gpio.h>

@@ -45,6 +46,24 @@ int board_init(void)
 	return 0;
 }
 
+#ifdef CONFIG_ENV_IS_IN_MMC
+static void mmc_late_init(void)
+{
+	char cmd[32];
+	char mmcblk[32];
+	u32 dev_no = mmc_get_env_dev();
+
+	setenv_ulong("mmcdev", dev_no);
+
+	/* Set mmcblk env */
+	sprintf(mmcblk, "/dev/mmcblk%dp2 rootwait rw", dev_no);
+	setenv("mmcroot", mmcblk);
+
+	sprintf(cmd, "mmc dev %d", dev_no);
+	run_command(cmd, 0);
+}
+#endif
+
 int board_late_init(void)
 {
 	switch ((imx6_src_get_boot_mode() & IMX6_BMODE_MASK) >>

@@ -53,6 +72,9 @@ int board_late_init(void)
 	case IMX6_BMODE_ESD:
 	case IMX6_BMODE_MMC:
 	case IMX6_BMODE_EMMC:
+#ifdef CONFIG_ENV_IS_IN_MMC
+		mmc_late_init();
+#endif
 		setenv("modeboot", "mmcboot");
 		break;
 	default:

index c62c1d4..3358320 100644
@@ -40,9 +40,7 @@
 	"fdt_file=" CONFIG_DEFAULT_FDT_FILE "\0" \
 	"fdt_addr=0x18000000\0" \
 	"boot_fdt=try\0" \
-	"mmcdev=0\0" \
 	"mmcpart=1\0" \
-	"mmcroot=/dev/mmcblk0p2 rootwait rw\0" \
 	"mmcautodetect=yes\0" \
 	"mmcargs=setenv bootargs console=${console},${baudrate} " \
 		"root=${mmcroot}\0" \

index f289e91..d2f9309 100644
@@ -47,6 +47,12 @@ int board_init(void)
 }
 
 #ifdef CONFIG_ENV_IS_IN_MMC
+int board_mmc_get_env_dev(int devno)
+{
+	/* dev 0 for SD/eSD, dev 1 for MMC/eMMC */
+	return (devno == 3) ? 1 : 0;
+}
+
 static void mmc_late_init(void)
 {
 	char cmd[32];