#include <asm/fsl_serdes.h>
 #include <asm/fsl_ifc.h>
 #include <asm/fsl_pci.h>
-
-#ifndef CONFIG_SDCARD
 #include <hwconfig.h>
-#endif
+#include <i2c.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
 #define MUX_CPLD_TDM                   0x01
 #define MUX_CPLD_SPICS0_FLASH          0x00
 #define MUX_CPLD_SPICS0_SLIC           0x02
+#define PMUXCR1_IFC_MASK       0x00ffff00
+#define PMUXCR1_SDHC_MASK      0x00fff000
+#define PMUXCR1_SDHC_ENABLE    0x00555000
+
+enum {
+       MUX_TYPE_IFC,
+       MUX_TYPE_SDHC,
+};
+
+static uint sd_ifc_mux;
 
-#ifndef CONFIG_SDCARD
 struct cpld_data {
        u8 cpld_ver; /* cpld revision */
        u8 pcba_ver; /* pcb revision number */
        u8 por2; /* POR Options */
        u8 por3; /* POR Options */
 };
-#endif
 
 int board_early_init_f(void)
 {
        ccsr_gpio_t *pgpio = (void *)(CONFIG_SYS_MPC85xx_GPIO_ADDR);
-#ifndef CONFIG_SDCARD
        struct fsl_ifc *ifc = (void *)CONFIG_SYS_IFC_ADDR;
 
        /* Clock configuration to access CPLD using IFC(GPCM) */
        setbits_be32(&ifc->ifc_gcr, 1 << IFC_GCR_TBCTL_TRN_TIME_SHIFT);
-#endif
        /*
        * Reset PCIe slots via GPIO4
        */
 
 int board_early_init_r(void)
 {
-#ifndef CONFIG_SDCARD
        const unsigned int flashbase = CONFIG_SYS_FLASH_BASE;
        const u8 flash_esel = find_tlb_idx((void *)flashbase, 1);
 
                        CONFIG_SYS_FLASH_BASE_PHYS + 0x1000000,
                        MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
                        0, flash_esel+1, BOOKE_PAGESZ_16M, 1);
-#endif
        return 0;
 }
 
 }
 #endif /* ifdef CONFIG_PCI */
 
+int config_board_mux(int ctrl_type)
+{
+       ccsr_gur_t __iomem *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+       u8 tmp;
+
+       switch (ctrl_type) {
+       case MUX_TYPE_IFC:
+               i2c_set_bus_num(I2C_PCA9557_BUS_NUM);
+               tmp = 0xf0;
+               i2c_write(I2C_PCA9557_ADDR1, 3, 1, &tmp, 1);
+               tmp = 0x01;
+               i2c_write(I2C_PCA9557_ADDR1, 1, 1, &tmp, 1);
+               sd_ifc_mux = MUX_TYPE_IFC;
+               clrbits_be32(&gur->pmuxcr, PMUXCR1_IFC_MASK);
+               break;
+       case MUX_TYPE_SDHC:
+               i2c_set_bus_num(I2C_PCA9557_BUS_NUM);
+               tmp = 0xf0;
+               i2c_write(I2C_PCA9557_ADDR1, 3, 1, &tmp, 1);
+               tmp = 0x05;
+               i2c_write(I2C_PCA9557_ADDR1, 1, 1, &tmp, 1);
+               sd_ifc_mux = MUX_TYPE_SDHC;
+               clrsetbits_be32(&gur->pmuxcr, PMUXCR1_SDHC_MASK,
+                               PMUXCR1_SDHC_ENABLE);
+               break;
+       default:
+               break;
+       }
+
+       return 0;
+}
+
 int checkboard(void)
 {
        struct cpu_type *cpu;
        cpu = gd->arch.cpu;
        printf("Board: %sRDB\n", cpu->name);
 
+#ifdef CONFIG_SDCARD
+       /* switch to IFC to read info from CPLD */
+       config_board_mux(MUX_TYPE_IFC);
+#endif
+
        return 0;
 }
 
        }
 }
 
+void fdt_del_ifc(void *blob)
+{
+       int nodeoff = 0;
+
+       while ((nodeoff = fdt_node_offset_by_compatible(blob, 0,
+                               "fsl,ifc")) >= 0) {
+               fdt_del_node(blob, nodeoff);
+       }
+}
+
 void fdt_disable_uart1(void *blob)
 {
        int nodeoff;
                fdt_del_flexcan(blob);
                fdt_del_node_and_alias(blob, "ethernet2");
        }
-#ifndef CONFIG_SDCARD
-       /* disable sdhc due to sdhc bug */
-       fdt_del_sdhc(blob);
+
+       /* Delete IFC node as IFC pins are multiplexing with SDHC */
+       if (sd_ifc_mux != MUX_TYPE_IFC)
+               fdt_del_ifc(blob);
+       else
+               fdt_del_sdhc(blob);
+
        if (hwconfig_subarg_cmp("fsl_p1010mux", "tdm_can", "can")) {
                fdt_del_tdm(blob);
                fdt_del_spi_slic(blob);
                fdt_del_flexcan(blob);
                fdt_disable_uart1(blob);
        }
+}
 #endif
+
+#ifdef CONFIG_SDCARD
+int board_mmc_init(bd_t *bis)
+{
+       config_board_mux(MUX_TYPE_SDHC);
+               return -1;
+}
+#else
+void board_reset(void)
+{
+       /* mux to IFC to enable CPLD for reset */
+       if (sd_ifc_mux != MUX_TYPE_IFC)
+               config_board_mux(MUX_TYPE_IFC);
 }
 #endif
 
-#ifndef CONFIG_SDCARD
+
 int misc_init_r(void)
 {
        struct cpld_data *cpld_data = (void *)(CONFIG_SYS_CPLD_BASE);
                out_8(&cpld_data->spi_cs0_sel, MUX_CPLD_SPICS0_FLASH);
        }
 
+       if (hwconfig("esdhc"))
+               config_board_mux(MUX_TYPE_SDHC);
+       else if (hwconfig("ifc"))
+               config_board_mux(MUX_TYPE_IFC);
+
        return 0;
 }
-#endif
+
+static int pin_mux_cmd(cmd_tbl_t *cmdtp, int flag, int argc,
+                               char * const argv[])
+{
+       if (argc < 2)
+               return CMD_RET_USAGE;
+       if (strcmp(argv[1], "ifc") == 0)
+               config_board_mux(MUX_TYPE_IFC);
+       else if (strcmp(argv[1], "sdhc") == 0)
+               config_board_mux(MUX_TYPE_SDHC);
+       else
+               return CMD_RET_USAGE;
+       return 0;
+}
+
+U_BOOT_CMD(
+       mux, 2, 0, pin_mux_cmd,
+       "configure multiplexing pin for IFC/SDHC bus in runtime",
+       "bus_type (e.g. mux sdhc)"
+);
 
                        0, 1, BOOKE_PAGESZ_1M, 1),
 
 #ifndef CONFIG_SPL_BUILD
-#ifndef CONFIG_SDCARD
        SET_TLB_ENTRY(1, CONFIG_SYS_FLASH_BASE, CONFIG_SYS_FLASH_BASE_PHYS,
                        MAS3_SX|MAS3_SR, MAS2_W|MAS2_G,
                        0, 2, BOOKE_PAGESZ_16M, 1),
                        CONFIG_SYS_FLASH_BASE_PHYS + 0x1000000,
                        MAS3_SX|MAS3_SR, MAS2_W|MAS2_G,
                        0, 3, BOOKE_PAGESZ_16M, 1),
-#endif
 
 #ifdef CONFIG_PCI
        /* *I*G* - PCI */
 #endif
 #endif
 
-#ifndef CONFIG_SDCARD
        /* *I*G - Board CPLD  */
        SET_TLB_ENTRY(1, CONFIG_SYS_CPLD_BASE, CONFIG_SYS_CPLD_BASE_PHYS,
                        MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
        SET_TLB_ENTRY(1, CONFIG_SYS_NAND_BASE, CONFIG_SYS_NAND_BASE_PHYS,
                        MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
                        0, 7, BOOKE_PAGESZ_1M, 1),
-#endif
 
 #if defined(CONFIG_SYS_RAMBOOT) || defined(CONFIG_SPL)
        SET_TLB_ENTRY(1, CONFIG_SYS_DDR_SDRAM_BASE, CONFIG_SYS_DDR_SDRAM_BASE,
 
 #define CONFIG_DDR_CLK_FREQ    66666666 /* DDRCLK on P1010 RDB */
 #define CONFIG_SYS_CLK_FREQ    66666666 /* SYSCLK for P1010 RDB */
 
-#ifndef CONFIG_SDCARD
 #define CONFIG_MISC_INIT_R
-#endif
-
 #define CONFIG_HWCONFIG
 /*
  * These can be toggled for performance analysis, otherwise use default.
  * 0xffe0_0000 0xffef_ffff     CCSR                    1M non-cacheable
  */
 
-/* In case of SD card boot, IFC interface is not available because of muxing */
-#ifdef CONFIG_SDCARD
-#define CONFIG_SYS_NO_FLASH
-#else
 /*
  * IFC Definitions
  */
                                        FTIM2_GPCM_TCH(0x0) | \
                                        FTIM2_GPCM_TWP(0x1f))
 #define CONFIG_SYS_CS3_FTIM3           0x0
-#endif /* CONFIG_SDCARD */
 
 #if defined(CONFIG_RAMBOOT_SDCARD) || defined(CONFIG_RAMBOOT_SPIFLASH)
 #define CONFIG_SYS_RAMBOOT
 #define CONFIG_SYS_FSL_I2C2_SPEED      400000
 #define CONFIG_SYS_FSL_I2C2_SLAVE      0x7F
 #define CONFIG_SYS_FSL_I2C2_OFFSET     0x3100
+#define I2C_PCA9557_ADDR1              0x18
+#define I2C_PCA9557_BUS_NUM            0
 
 /* I2C EEPROM */
 #undef CONFIG_ID_EEPROM
 #define CONFIG_LBA48
 #endif /* #ifdef CONFIG_FSL_SATA  */
 
-/*  SD interface will only be available in case of SD boot */
-#ifdef CONFIG_SDCARD
 #define CONFIG_MMC
-#define CONFIG_DEF_HWCONFIG            esdhc
-#endif
-
 #ifdef CONFIG_MMC
 #define CONFIG_CMD_MMC
 #define CONFIG_DOS_PARTITION