mx6slevk: Fix MMC breakage for the SPL target

index 8afd5da..e9a9bbf 100644
@@ -66,7 +66,6 @@ static iomux_v3_cfg_t const uart1_pads[] = {
 	MX6_PAD_UART1_RXD__UART1_RXD | MUX_PAD_CTRL(UART_PAD_CTRL),
 };
 
-#ifdef CONFIG_SPL_BUILD
 static iomux_v3_cfg_t const usdhc1_pads[] = {
 	/* 8 bit SD */
 	MX6_PAD_SD1_CLK__USDHC1_CLK | MUX_PAD_CTRL(USDHC_PAD_CTRL),

@@ -107,7 +106,6 @@ static iomux_v3_cfg_t const usdhc3_pads[] = {
 	/*CD pin*/
 	MX6_PAD_REF_CLK_32K__GPIO_3_22 | MUX_PAD_CTRL(NO_PAD_CTRL),
 };
-#endif
 
 static iomux_v3_cfg_t const fec_pads[] = {
 	MX6_PAD_FEC_MDC__FEC_MDC | MUX_PAD_CTRL(ENET_PAD_CTRL),

@@ -157,11 +155,6 @@ static void setup_iomux_fec(void)
 	udelay(15000);
 }
 
-int board_mmc_get_env_dev(int devno)
-{
-	return devno;
-}
-
 #ifdef CONFIG_DM_PMIC_PFUZE100
 int power_init_board(void)
 {

@@ -294,17 +287,6 @@ int board_init(void)
 	return 0;
 }
 
-int checkboard(void)
-{
-	puts("Board: MX6SLEVK\n");
-
-	return 0;
-}
-
-#ifdef CONFIG_SPL_BUILD
-#include <spl.h>
-#include <libfdt.h>
-
 #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)

@@ -315,6 +297,11 @@ static struct fsl_esdhc_cfg usdhc_cfg[3] = {
 	{USDHC3_BASE_ADDR, 0, 4},
 };
 
+int board_mmc_get_env_dev(int devno)
+{
+	return devno;
+}
+
 int board_mmc_getcd(struct mmc *mmc)
 {
 	struct fsl_esdhc_cfg *cfg = (struct fsl_esdhc_cfg *)mmc->priv;

@@ -337,6 +324,52 @@ int board_mmc_getcd(struct mmc *mmc)
 
 int board_mmc_init(bd_t *bis)
 {
+#ifndef CONFIG_SPL_BUILD
+	int i, ret;
+
+	/*
+	 * 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 -EINVAL;
+		}
+
+		ret = fsl_esdhc_initialize(bis, &usdhc_cfg[i]);
+		if (ret) {
+			printf("Warning: failed to initialize "
+				"mmc dev %d\n", i);
+			return ret;
+		}
+	}
+
+	return 0;
+#else
 	struct src *src_regs = (struct src *)SRC_BASE_ADDR;
 	u32 val;
 	u32 port;

@@ -373,8 +406,20 @@ int board_mmc_init(bd_t *bis)
 
 	gd->arch.sdhc_clk = usdhc_cfg[0].sdhc_clk;
 	return fsl_esdhc_initialize(bis, &usdhc_cfg[0]);
+#endif
 }
 
+int checkboard(void)
+{
+	puts("Board: MX6SLEVK\n");
+
+	return 0;
+}
+
+#ifdef CONFIG_SPL_BUILD
+#include <spl.h>
+#include <libfdt.h>
+
 const struct mx6sl_iomux_ddr_regs mx6_ddr_ioregs = {
 	.dram_sdqs0 = 0x00003030,
 	.dram_sdqs1 = 0x00003030,