Merge branch 'for-next/imx'

22 files changed, 588 insertions, 131 deletions

diff --git a/arch/arm/boards/guf-vincell/board.c b/arch/arm/boards/guf-vincell/board.c
index 43c195254e..007b6dd347 100644
--- a/arch/arm/boards/guf-vincell/board.c
+++ b/arch/arm/boards/guf-vincell/board.c
@@ -29,15 +29,6 @@
 #include <mach/bbu.h>
 #include <mach/imx5.h>
 
-#define LOCO_FEC_PHY_RST		IMX_GPIO_NR(7, 6)
-
-static void vincell_fec_reset(void)
-{
-	gpio_direction_output(LOCO_FEC_PHY_RST, 0);
-	mdelay(1);
-	gpio_set_value(LOCO_FEC_PHY_RST, 1);
-}
-
 static int vincell_devices_init(void)
 {
 	if (!of_machine_is_compatible("guf,imx53-vincell") &&
@@ -49,10 +40,8 @@ static int vincell_devices_init(void)
 	clk_set_rate(clk_lookup("emi_slow_podf"), 133333334);
 	clk_set_rate(clk_lookup("nfc_podf"), 33333334);
 
-	vincell_fec_reset();
-
 	imx53_bbu_internal_nand_register_handler("nand",
-		BBU_HANDLER_FLAG_DEFAULT, SZ_512K);
+		BBU_HANDLER_FLAG_DEFAULT, 0xe0000);
 
 	return 0;
 }
diff --git a/arch/arm/boards/guf-vincell/lowlevel.c b/arch/arm/boards/guf-vincell/lowlevel.c
index af7c65d9be..de706b68b2 100644
--- a/arch/arm/boards/guf-vincell/lowlevel.c
+++ b/arch/arm/boards/guf-vincell/lowlevel.c
@@ -11,6 +11,8 @@
 #include <mach/generic.h>
 #include <asm/barebox-arm.h>
 #include <asm/barebox-arm-head.h>
+#include <asm/cache.h>
+#include <mach/xload.h>
 
 #define IOMUX_PADCTL_DDRI_DDR (1 << 9)
 
@@ -123,12 +125,18 @@ void disable_watchdog(void)
 	writew(0x0, MX53_WDOG2_BASE_ADDR + 8);
 }
 
-static noinline void imx53_guf_vincell_init(void *fdt)
+extern char __dtb_imx53_guf_vincell_lt_start[];
+extern char __dtb_imx53_guf_vincell_start[];
+
+static noinline void imx53_guf_vincell_init(int is_lt)
 {
 	void __iomem *ccm = (void *)MX53_CCM_BASE_ADDR;
+	void __iomem *uart = IOMEM(MX53_UART2_BASE_ADDR);
+	void *fdt;
 	u32 r;
+	enum bootsource src;
+	int instance;
 
-	imx5_cpu_lowlevel_init();
 	arm_setup_stack(MX53_IRAM_BASE_ADDR + MX53_IRAM_SIZE - 8);
 
 	writel(0x0088494c, ccm + MX5_CCM_CBCDR);
@@ -137,12 +145,11 @@ static noinline void imx53_guf_vincell_init(void *fdt)
 
 	imx53_init_lowlevel_early(800);
 
-	if (IS_ENABLED(CONFIG_DEBUG_LL)) {
-		writel(0x3, MX53_IOMUXC_BASE_ADDR + 0x27c);
-		writel(0x3, MX53_IOMUXC_BASE_ADDR + 0x278);
-		imx53_uart_setup_ll();
-		putc_ll('>');
-	}
+	writel(0x3, MX53_IOMUXC_BASE_ADDR + 0x27c);
+	writel(0x3, MX53_IOMUXC_BASE_ADDR + 0x278);
+	imx53_uart_setup(uart);
+	pbl_set_putc(imx_uart_putc, uart);
+	pr_debug("GuF Vincell\n");
 
 	/* Skip SDRAM initialization if we run from RAM */
 	r = get_pc();
@@ -150,29 +157,39 @@ static noinline void imx53_guf_vincell_init(void *fdt)
 		disable_watchdog();
 		configure_dram_iomux();
 		imx_esdctlv4_init();
+
+		imx53_get_boot_source(&src, &instance);
+
+		if (src == BOOTSOURCE_NAND &&
+		    IS_ENABLED(CONFIG_MACH_GUF_VINCELL_XLOAD))
+			imx53_nand_start_image();
 	}
 
+	if (is_lt)
+		fdt = __dtb_imx53_guf_vincell_lt_start;
+	else
+		fdt = __dtb_imx53_guf_vincell_start;
+
 	imx53_barebox_entry(fdt);
 }
 
-extern char __dtb_imx53_guf_vincell_lt_start[];
-
-ENTRY_FUNCTION(start_imx53_guf_vincell_lt, r0, r1, r2)
+static void __imx53_guf_vincell_init(int is_lt)
 {
-	void *fdt;
-
-	fdt = __dtb_imx53_guf_vincell_lt_start - get_runtime_offset();
+	arm_early_mmu_cache_invalidate();
+	imx5_cpu_lowlevel_init();
+	relocate_to_current_adr();
+	setup_c();
+	barrier();
 
-	imx53_guf_vincell_init(fdt);
+	imx53_guf_vincell_init(is_lt);
 }
 
-extern char __dtb_imx53_guf_vincell_start[];
+ENTRY_FUNCTION(start_imx53_guf_vincell_lt, r0, r1, r2)
+{
+	__imx53_guf_vincell_init(1);
+}
 
 ENTRY_FUNCTION(start_imx53_guf_vincell, r0, r1, r2)
 {
-	void *fdt;
-
-	fdt = __dtb_imx53_guf_vincell_start - get_runtime_offset();
-
-	imx53_guf_vincell_init(fdt);
+	__imx53_guf_vincell_init(0);
 }
diff --git a/arch/arm/configs/imx_v7_defconfig b/arch/arm/configs/imx_v7_defconfig
index e3a8f478d2..51cbf60afd 100644
--- a/arch/arm/configs/imx_v7_defconfig
+++ b/arch/arm/configs/imx_v7_defconfig
@@ -3,8 +3,10 @@ CONFIG_IMX_MULTI_BOARDS=y
 CONFIG_MACH_EFIKA_MX_SMARTBOOK=y
 CONFIG_MACH_EMBEDSKY_E9=y
 CONFIG_MACH_FREESCALE_MX51_PDK=y
-CONFIG_MACH_FREESCALE_MX53_LOCO=y
 CONFIG_MACH_CCMX53=y
+CONFIG_MACH_FREESCALE_MX53_LOCO=y
+CONFIG_MACH_GUF_VINCELL=y
+CONFIG_MACH_GUF_VINCELL_XLOAD=y
 CONFIG_MACH_TQMA53=y
 CONFIG_MACH_FREESCALE_MX53_VMX53=y
 CONFIG_MACH_PHYTEC_SOM_IMX6=y
diff --git a/arch/arm/configs/vincell_defconfig b/arch/arm/configs/vincell_defconfig
index 83862dea0d..81bfbede3b 100644
--- a/arch/arm/configs/vincell_defconfig
+++ b/arch/arm/configs/vincell_defconfig
@@ -104,7 +104,6 @@ CONFIG_USB_EHCI=y
 CONFIG_USB_ULPI=y
 CONFIG_USB_STORAGE=y
 CONFIG_MCI=y
-CONFIG_MCI_MMC_BOOT_PARTITIONS=y
 CONFIG_MCI_IMX_ESDHC=y
 CONFIG_STATE_DRV=y
 CONFIG_EEPROM_AT25=y
@@ -112,5 +111,4 @@ CONFIG_EEPROM_AT24=y
 CONFIG_WATCHDOG=y
 CONFIG_WATCHDOG_IMX=y
 CONFIG_FS_TFTP=y
-CONFIG_FS_UBIFS=y
-CONFIG_FS_UBIFS_COMPRESSION_LZO=y
+CONFIG_LZO_DECOMPRESS=y
diff --git a/arch/arm/dts/imx53-guf-vincell-lt.dts b/arch/arm/dts/imx53-guf-vincell-lt.dts
index bcc378d07f..67a41332f5 100644
--- a/arch/arm/dts/imx53-guf-vincell-lt.dts
+++ b/arch/arm/dts/imx53-guf-vincell-lt.dts
@@ -136,6 +136,7 @@
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_fec>;
 	phy-mode = "rmii";
+	phy-reset-gpios = <&gpio7 6 GPIO_ACTIVE_HIGH>;
 	status = "okay";
 };
 
diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index ae35aaa35e..f23af99c11 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -221,6 +221,15 @@ config MACH_GUF_VINCELL
 	bool "Garz-Fricke Vincell"
 	select ARCH_IMX53
 
+config MACH_GUF_VINCELL_XLOAD
+	depends on MACH_GUF_VINCELL
+	bool "Garz-Fricke Vincell NAND xload support"
+	help
+	  The Vincell initializes SDRAM from board code. This normally limits
+	  the image size to the size of the SoC internal SRAM. Enable this
+	  option to be able to use bigger images when booting from NAND. Images
+	  built with this option are no longer bootable from USB though.
+
 config MACH_TQMA53
 	bool "TQ i.MX53 TQMa53"
 	select ARCH_IMX53
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 0763944895..a216c9bb07 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -25,4 +25,4 @@ obj-pbl-y += esdctl.o boot.o
 obj-$(CONFIG_BAREBOX_UPDATE) += imx-bbu-internal.o
 obj-$(CONFIG_BAREBOX_UPDATE_IMX_EXTERNAL_NAND) += imx-bbu-external-nand.o
 lwl-y += cpu_init.o
-pbl-y += xload-spi.o xload-esdhc.o xload-common.o
+pbl-y += xload-spi.o xload-esdhc.o xload-common.o xload-imx-nand.o
diff --git a/arch/arm/mach-imx/boot.c b/arch/arm/mach-imx/boot.c
index 376e370cf9..b66c29d8d9 100644
--- a/arch/arm/mach-imx/boot.c
+++ b/arch/arm/mach-imx/boot.c
@@ -20,7 +20,10 @@
 #include <io.h>
 #include <mach/generic.h>
 #include <mach/imx25-regs.h>
+#include <mach/imx27-regs.h>
 #include <mach/imx35-regs.h>
+#include <mach/imx51-regs.h>
+#include <mach/imx53-regs.h>
 #include <mach/imx6-regs.h>
 
 /* [CTRL][TYPE] */
@@ -75,8 +78,9 @@ static void imx25_35_boot_save_loc(unsigned int ctrl, unsigned int type)
 	bootsource_set(src);
 }
 
-void imx25_boot_save_loc(void __iomem *ccm_base)
+void imx25_get_boot_source(enum bootsource *src, int *instance)
 {
+	void __iomem *ccm_base = IOMEM(MX25_CCM_BASE_ADDR);
 	uint32_t val;
 
 	val = readl(ccm_base + MX25_CCM_RCSR);
@@ -84,8 +88,20 @@ void imx25_boot_save_loc(void __iomem *ccm_base)
 			       (val >> MX25_CCM_RCSR_MEM_TYPE_SHIFT) & 0x3);
 }
 
-void imx35_boot_save_loc(void __iomem *ccm_base)
+void imx25_boot_save_loc(void)
 {
+	enum bootsource src = BOOTSOURCE_UNKNOWN;
+	int instance = BOOTSOURCE_INSTANCE_UNKNOWN;
+
+	imx25_get_boot_source(&src, &instance);
+
+	bootsource_set(src);
+	bootsource_set_instance(instance);
+}
+
+void imx35_get_boot_source(enum bootsource *src, int *instance)
+{
+	void __iomem *ccm_base = IOMEM(MX35_CCM_BASE_ADDR);
 	uint32_t val;
 
 	val = readl(ccm_base + MX35_CCM_RCSR);
@@ -93,6 +109,17 @@ void imx35_boot_save_loc(void __iomem *ccm_base)
 			       (val >> MX35_CCM_RCSR_MEM_TYPE_SHIFT) & 0x3);
 }
 
+void imx35_boot_save_loc(void)
+{
+	enum bootsource src = BOOTSOURCE_UNKNOWN;
+	int instance = BOOTSOURCE_INSTANCE_UNKNOWN;
+
+	imx35_get_boot_source(&src, &instance);
+
+	bootsource_set(src);
+	bootsource_set_instance(instance);
+}
+
 #define IMX27_SYSCTRL_GPCR	0x18
 #define IMX27_GPCR_BOOT_SHIFT			16
 #define IMX27_GPCR_BOOT_MASK			(0xf << IMX27_GPCR_BOOT_SHIFT)
@@ -104,9 +131,9 @@ void imx35_boot_save_loc(void __iomem *ccm_base)
 #define IMX27_GPCR_BOOT_32BIT_CS0		6
 #define IMX27_GPCR_BOOT_8BIT_NAND_512		7
 
-void imx27_boot_save_loc(void __iomem *sysctrl_base)
+void imx27_get_boot_source(enum bootsource *src, int *instance)
 {
-	enum bootsource src;
+	void __iomem *sysctrl_base = IOMEM(MX27_SYSCTRL_BASE_ADDR);
 	uint32_t val;
 
 	val = readl(sysctrl_base + IMX27_SYSCTRL_GPCR);
@@ -115,20 +142,29 @@ void imx27_boot_save_loc(void __iomem *sysctrl_base)
 
 	switch (val) {
 	case IMX27_GPCR_BOOT_UART_USB:
-		src = BOOTSOURCE_SERIAL;
+		*src = BOOTSOURCE_SERIAL;
 		break;
 	case IMX27_GPCR_BOOT_8BIT_NAND_2k:
 	case IMX27_GPCR_BOOT_16BIT_NAND_2k:
 	case IMX27_GPCR_BOOT_16BIT_NAND_512:
 	case IMX27_GPCR_BOOT_8BIT_NAND_512:
-		src = BOOTSOURCE_NAND;
+		*src = BOOTSOURCE_NAND;
 		break;
 	default:
-		src = BOOTSOURCE_NOR;
+		*src = BOOTSOURCE_NOR;
 		break;
 	}
+}
+
+void imx27_boot_save_loc(void)
+{
+	enum bootsource src = BOOTSOURCE_UNKNOWN;
+	int instance = BOOTSOURCE_INSTANCE_UNKNOWN;
+
+	imx27_get_boot_source(&src, &instance);
 
 	bootsource_set(src);
+	bootsource_set_instance(instance);
 }
 
 #define IMX51_SRC_SBMR			0x4
@@ -136,9 +172,9 @@ void imx27_boot_save_loc(void __iomem *sysctrl_base)
 #define IMX51_SBMR_BT_MEM_CTL_SHIFT	0
 #define IMX51_SBMR_BMOD_SHIFT		14
 
-void imx51_boot_save_loc(void __iomem *src_base)
+void imx51_get_boot_source(enum bootsource *src, int *instance)
 {
-	enum bootsource src = BOOTSOURCE_UNKNOWN;
+	void __iomem *src_base = IOMEM(MX51_SRC_BASE_ADDR);
 	uint32_t reg;
 	unsigned int ctrl, type;
 
@@ -151,62 +187,84 @@ void imx51_boot_save_loc(void __iomem *src_base)
 		ctrl = (reg >> IMX51_SBMR_BT_MEM_CTL_SHIFT) & 0x3;
 		type = (reg >> IMX51_SBMR_BT_MEM_TYPE_SHIFT) & 0x3;
 
-		src = locations[ctrl][type];
+		*src = locations[ctrl][type];
 		break;
 	case 1:
 		/* reserved */
-		src = BOOTSOURCE_UNKNOWN;
+		*src = BOOTSOURCE_UNKNOWN;
 		break;
 	case 3:
-		src = BOOTSOURCE_SERIAL;
+		*src = BOOTSOURCE_SERIAL;
 		break;
 
 	}
+}
+
+void imx51_boot_save_loc(void)
+{
+	enum bootsource src = BOOTSOURCE_UNKNOWN;
+	int instance = BOOTSOURCE_INSTANCE_UNKNOWN;
+
+	imx51_get_boot_source(&src, &instance);
 
 	bootsource_set(src);
+	bootsource_set_instance(instance);
 }
 
 #define IMX53_SRC_SBMR	0x4
-void imx53_boot_save_loc(void __iomem *src_base)
+void imx53_get_boot_source(enum bootsource *src, int *instance)
 {
-	enum bootsource src = BOOTSOURCE_UNKNOWN;
-	int instance;
+	void __iomem *src_base = IOMEM(MX53_SRC_BASE_ADDR);
 	uint32_t cfg1 = readl(src_base + IMX53_SRC_SBMR);
 
+	if (((cfg1 >> 24) & 0x3) == 0x3) {
+		*src = BOOTSOURCE_USB;
+		*instance = 0;
+		return;
+	}
+
 	switch ((cfg1 & 0xff) >> 4) {
 	case 2:
-		src = BOOTSOURCE_HD;
+		*src = BOOTSOURCE_HD;
 		break;
 	case 3:
 		if (cfg1 & (1 << 3))
-			src = BOOTSOURCE_SPI;
+			*src = BOOTSOURCE_SPI;
 		else
-			src = BOOTSOURCE_I2C;
+			*src = BOOTSOURCE_I2C;
 		break;
 	case 4:
 	case 5:
 	case 6:
 	case 7:
-		src = BOOTSOURCE_MMC;
+		*src = BOOTSOURCE_MMC;
 		break;
 	default:
 		break;
 	}
 
 	if (cfg1 & (1 << 7))
-		src = BOOTSOURCE_NAND;
+		*src = BOOTSOURCE_NAND;
 
 
-	switch (src) {
+	switch (*src) {
 	case BOOTSOURCE_MMC:
 	case BOOTSOURCE_SPI:
 	case BOOTSOURCE_I2C:
-		instance = (cfg1 >> 20) & 0x3;
+		*instance = (cfg1 >> 20) & 0x3;
 		break;
 	default:
-		instance = 0;
+		*instance = 0;
 		break;
 	}
+}
+
+void imx53_boot_save_loc(void)
+{
+	enum bootsource src = BOOTSOURCE_UNKNOWN;
+	int instance = BOOTSOURCE_INSTANCE_UNKNOWN;
+
+	imx53_get_boot_source(&src, &instance);
 
 	bootsource_set(src);
 	bootsource_set_instance(instance);
@@ -278,7 +336,7 @@ internal_boot:
 	return;
 }
 
-void imx6_boot_save_loc(void __iomem *src_base)
+void imx6_boot_save_loc(void)
 {
 	enum bootsource src = BOOTSOURCE_UNKNOWN;
 	int instance = BOOTSOURCE_INSTANCE_UNKNOWN;
diff --git a/arch/arm/mach-imx/clk-imx5.c b/arch/arm/mach-imx/clk-imx5.c
index d3cde7e42f..c4c47a6d87 100644
--- a/arch/arm/mach-imx/clk-imx5.c
+++ b/arch/arm/mach-imx/clk-imx5.c
@@ -467,6 +467,7 @@ int __init mx53_clocks_init(struct device_d *dev, void __iomem *regs)
 	clkdev_add_physbase(clks[IMX5_CLK_UART_ROOT], MX53_UART2_BASE_ADDR, NULL);
 	clkdev_add_physbase(clks[IMX5_CLK_UART_ROOT], MX53_UART3_BASE_ADDR, NULL);
 	clkdev_add_physbase(clks[IMX5_CLK_UART_ROOT], MX53_UART4_BASE_ADDR, NULL);
+	clkdev_add_physbase(clks[IMX5_CLK_UART_ROOT], MX53_UART5_BASE_ADDR, NULL);
 	clkdev_add_physbase(clks[IMX5_CLK_PER_ROOT], MX53_I2C1_BASE_ADDR, NULL);
 	clkdev_add_physbase(clks[IMX5_CLK_PER_ROOT], MX53_I2C2_BASE_ADDR, NULL);
 	clkdev_add_physbase(clks[IMX5_CLK_PER_ROOT], MX53_I2C3_BASE_ADDR, NULL);
diff --git a/arch/arm/mach-imx/clk-imx6.c b/arch/arm/mach-imx/clk-imx6.c
index a634580c86..8ac43bebb0 100644
--- a/arch/arm/mach-imx/clk-imx6.c
+++ b/arch/arm/mach-imx/clk-imx6.c
@@ -495,10 +495,13 @@ static int imx6_ccm_probe(struct device_d *dev)
 	writel(0xf0ffffff, ccm_base + CCGR1); /* gate GPU3D, GPU2D */
 	writel(0xffffffff, ccm_base + CCGR2);
 	if (IS_ENABLED(CONFIG_DRIVER_VIDEO_IMX_IPUV3))
-		writel(0xffffffff, ccm_base + CCGR3); /* gate OpenVG */
+		writel(0x3fffffff, ccm_base + CCGR3); /* gate OpenVG */
 	else
-		writel(0x3fffffff, ccm_base + CCGR3); /* gate OpenVG, LDB, IPU1, IPU2 */
-	writel(0xffffffff, ccm_base + CCGR4);
+		writel(0x3fff0000, ccm_base + CCGR3); /* gate OpenVG, LDB, IPU1, IPU2 */
+	if (IS_ENABLED(CONFIG_PCI_IMX6))
+		writel(0xffffffff, ccm_base + CCGR4);
+	else
+		writel(0xfffffffc, ccm_base + CCGR4); /* gate PCIe */
 	writel(0xffffffff, ccm_base + CCGR5);
 	writel(0xffff3fff, ccm_base + CCGR6); /* gate VPU */
 	writel(0xffffffff, ccm_base + CCGR7);
diff --git a/arch/arm/mach-imx/imx25.c b/arch/arm/mach-imx/imx25.c
index 2534d75429..a4e27dd913 100644
--- a/arch/arm/mach-imx/imx25.c
+++ b/arch/arm/mach-imx/imx25.c
@@ -52,7 +52,7 @@ int imx25_init(void)
 {
 	int val;
 
-	imx25_boot_save_loc((void *)MX25_CCM_BASE_ADDR);
+	imx25_boot_save_loc();
 	add_generic_device("imx25-esdctl", 0, NULL, MX25_ESDCTL_BASE_ADDR, 0x1000, IORESOURCE_MEM, NULL);
 
 	/*
diff --git a/arch/arm/mach-imx/imx27.c b/arch/arm/mach-imx/imx27.c
index b99a4eae3f..81b9f539df 100644
--- a/arch/arm/mach-imx/imx27.c
+++ b/arch/arm/mach-imx/imx27.c
@@ -100,7 +100,7 @@ static void imx27_init_max(void)
 int imx27_init(void)
 {
 	imx27_silicon_revision();
-	imx27_boot_save_loc((void *)MX27_SYSCTRL_BASE_ADDR);
+	imx27_boot_save_loc();
 	add_generic_device("imx27-esdctl", DEVICE_ID_SINGLE, NULL,
 			   MX27_ESDCTL_BASE_ADDR, 0x1000, IORESOURCE_MEM, NULL);
 
diff --git a/arch/arm/mach-imx/imx35.c b/arch/arm/mach-imx/imx35.c
index 3e1aa97583..d37bdfda7b 100644
--- a/arch/arm/mach-imx/imx35.c
+++ b/arch/arm/mach-imx/imx35.c
@@ -55,7 +55,7 @@ int imx35_init(void)
 
 	imx35_silicon_revision();
 
-	imx35_boot_save_loc((void *)MX35_CCM_BASE_ADDR);
+	imx35_boot_save_loc();
 	add_generic_device("imx35-esdctl", 0, NULL, MX35_ESDCTL_BASE_ADDR, 0x1000, IORESOURCE_MEM, NULL);
 
 	return 0;
diff --git a/arch/arm/mach-imx/imx51.c b/arch/arm/mach-imx/imx51.c
index a6784d0a04..ffe6a7c651 100644
--- a/arch/arm/mach-imx/imx51.c
+++ b/arch/arm/mach-imx/imx51.c
@@ -58,7 +58,7 @@ static void imx51_ipu_mipi_setup(void)
 int imx51_init(void)
 {
 	imx_set_silicon_revision("i.MX51", imx51_silicon_revision());
-	imx51_boot_save_loc((void *)MX51_SRC_BASE_ADDR);
+	imx51_boot_save_loc();
 	add_generic_device("imx51-esdctl", 0, NULL, MX51_ESDCTL_BASE_ADDR, 0x1000, IORESOURCE_MEM, NULL);
 	imx51_ipu_mipi_setup();
 
diff --git a/arch/arm/mach-imx/imx53.c b/arch/arm/mach-imx/imx53.c
index 872d293640..2758f1bbcf 100644
--- a/arch/arm/mach-imx/imx53.c
+++ b/arch/arm/mach-imx/imx53.c
@@ -53,7 +53,7 @@ static int imx53_silicon_revision(void)
 int imx53_init(void)
 {
 	imx53_silicon_revision();
-	imx53_boot_save_loc((void *)MX53_SRC_BASE_ADDR);
+	imx53_boot_save_loc();
 	add_generic_device("imx53-esdctl", 0, NULL, MX53_ESDCTL_BASE_ADDR, 0x1000, IORESOURCE_MEM, NULL);
 
 	return 0;
diff --git a/arch/arm/mach-imx/imx6.c b/arch/arm/mach-imx/imx6.c
index 18509a7b51..a635ccd741 100644
--- a/arch/arm/mach-imx/imx6.c
+++ b/arch/arm/mach-imx/imx6.c
@@ -25,16 +25,12 @@
 #include <asm/mmu.h>
 #include <asm/cache-l2x0.h>
 
-#define SI_REV 0x260
-
 void imx6_init_lowlevel(void)
 {
 	void __iomem *aips1 = (void *)MX6_AIPS1_ON_BASE_ADDR;
 	void __iomem *aips2 = (void *)MX6_AIPS2_ON_BASE_ADDR;
-	void __iomem *iomux = (void *)MX6_IOMUXC_BASE_ADDR;
 	bool is_imx6q = __imx6_cpu_type() == IMX6_CPUTYPE_IMX6Q;
 	bool is_imx6d = __imx6_cpu_type() == IMX6_CPUTYPE_IMX6D;
-	uint32_t val;
 
 	/*
 	 * Set all MPROTx to be non-bufferable, trusted for R/W,
@@ -56,19 +52,10 @@ void imx6_init_lowlevel(void)
 	writel(0, aips2 + 0x4c);
 	writel(0, aips2 + 0x50);
 
-	/* enable all clocks */
-	writel(0xffffffff, 0x020c4068);
-	writel(0xffffffff, 0x020c406c);
-	writel(0xffffffff, 0x020c4070);
-	writel(0xffffffff, 0x020c4074);
-	writel(0xffffffff, 0x020c4078);
-	writel(0xffffffff, 0x020c407c);
-	writel(0xffffffff, 0x020c4080);
-
-	/*
-	 * Due to a hardware bug (related to errata ERR006282) on i.MX6DQ we
-	 * need to gate/ungate all PFDs to make sure PFD is working right,
-	 * otherwise PFDs may not output clock after reset.
+	/* Due to hardware limitation, on MX6Q we need to gate/ungate all PFDs
+	 * to make sure PFD is working right, otherwise, PFDs may
+	 * not output clock after reset, MX6DL and MX6SL have added 396M pfd
+	 * workaround in ROM code, as bus clock need it
 	 */
 	if (is_imx6q || is_imx6d) {
 		writel(BM_ANADIG_PFD_480_PFD3_CLKGATE |
@@ -94,6 +81,18 @@ void imx6_init_lowlevel(void)
 		       MX6_ANATOP_BASE_ADDR + HW_ANADIG_PFD_528_CLR);
 	}
 
+}
+
+void imx6_setup_ipu_qos(void)
+{
+	void __iomem *iomux = (void *)MX6_IOMUXC_BASE_ADDR;
+	void __iomem *fast2 = (void *)MX6_FAST2_BASE_ADDR;
+	uint32_t val;
+
+	if (!cpu_mx6_is_mx6q() && !cpu_mx6_is_mx6d() &&
+	    !cpu_mx6_is_mx6dl() && cpu_mx6_is_mx6s())
+		return;
+
 	val = readl(iomux + IOMUXC_GPR4);
 	val |= IMX6Q_GPR4_VPU_WR_CACHE_SEL | IMX6Q_GPR4_VPU_RD_CACHE_SEL |
 		IMX6Q_GPR4_VPU_P_WR_CACHE_VAL | IMX6Q_GPR4_VPU_P_RD_CACHE_VAL_MASK |
@@ -110,52 +109,28 @@ void imx6_init_lowlevel(void)
 	val &= ~(IMX6Q_GPR7_IPU2_ID00_RD_QOS_MASK | IMX6Q_GPR7_IPU2_ID01_RD_QOS_MASK);
 	val |= (0xf << 16) | (0x7 << 20);
 	writel(val, iomux + IOMUXC_GPR7);
+
+	/*
+	 * On i.MX6 QP/DP the NoC regulator for the IPU ports needs to be in
+	 * bypass mode for the above settings to take effect.
+	 */
+	if ((cpu_mx6_is_mx6q() || cpu_mx6_is_mx6d()) &&
+	    imx_silicon_revision() >= IMX_CHIP_REV_2_0) {
+		writel(0x2, fast2 + 0xb048c);
+		writel(0x2, fast2 + 0xb050c);
+	}
 }
 
 int imx6_init(void)
 {
 	const char *cputypestr;
-	u32 rev;
 	u32 mx6_silicon_revision;
 
 	imx6_init_lowlevel();
 
-	imx6_boot_save_loc((void *)MX6_SRC_BASE_ADDR);
-
-	rev = readl(MX6_ANATOP_BASE_ADDR + SI_REV);
+	imx6_boot_save_loc();
 
-	switch (rev & 0xfff) {
-	case 0x00:
-		mx6_silicon_revision = IMX_CHIP_REV_1_0;
-		break;
-
-	case 0x01:
-		mx6_silicon_revision = IMX_CHIP_REV_1_1;
-		break;
-
-	case 0x02:
-		mx6_silicon_revision = IMX_CHIP_REV_1_2;
-		break;
-
-	case 0x03:
-		mx6_silicon_revision = IMX_CHIP_REV_1_3;
-		break;
-
-	case 0x04:
-		mx6_silicon_revision = IMX_CHIP_REV_1_4;
-		break;
-
-	case 0x05:
-		mx6_silicon_revision = IMX_CHIP_REV_1_5;
-		break;
-
-	case 0x100:
-		mx6_silicon_revision = IMX_CHIP_REV_2_0;
-		break;
-
-	default:
-		mx6_silicon_revision = IMX_CHIP_REV_UNKNOWN;
-	}
+	mx6_silicon_revision = imx6_cpu_revision();
 
 	switch (imx6_cpu_type()) {
 	case IMX6_CPUTYPE_IMX6Q:
@@ -186,6 +161,8 @@ int imx6_init(void)
 
 	imx_set_silicon_revision(cputypestr, mx6_silicon_revision);
 
+	imx6_setup_ipu_qos();
+
 	return 0;
 }
 
@@ -291,7 +268,8 @@ static int imx6_fixup_cpus(struct device_node *root, void *context)
 
 static int imx6_fixup_cpus_register(void)
 {
-	if (!of_machine_is_compatible("fsl,imx6q") &&
+	if (!of_machine_is_compatible("fsl,imx6qp") &&
+	    !of_machine_is_compatible("fsl,imx6q")  &&
 	    !of_machine_is_compatible("fsl,imx6dl"))
 		return 0;
 
diff --git a/arch/arm/mach-imx/include/mach/generic.h b/arch/arm/mach-imx/include/mach/generic.h
index cadc501040..7c275dff84 100644
--- a/arch/arm/mach-imx/include/mach/generic.h
+++ b/arch/arm/mach-imx/include/mach/generic.h
@@ -8,13 +8,17 @@
 
 u64 imx_uid(void);
 
-void imx25_boot_save_loc(void __iomem *ccm_base);
-void imx35_boot_save_loc(void __iomem *ccm_base);
-void imx27_boot_save_loc(void __iomem *sysctrl_base);
-void imx50_boot_save_loc(void __iomem *src_base);
-void imx51_boot_save_loc(void __iomem *src_base);
-void imx53_boot_save_loc(void __iomem *src_base);
-void imx6_boot_save_loc(void __iomem *src_base);
+void imx25_boot_save_loc(void);
+void imx35_boot_save_loc(void);
+void imx27_boot_save_loc(void);
+void imx51_boot_save_loc(void);
+void imx53_boot_save_loc(void);
+void imx6_boot_save_loc(void);
+
+void imx25_get_boot_source(enum bootsource *src, int *instance);
+void imx35_get_boot_source(enum bootsource *src, int *instance);
+void imx51_get_boot_source(enum bootsource *src, int *instance);
+void imx53_get_boot_source(enum bootsource *src, int *instance);
 void imx6_get_boot_source(enum bootsource *src, int *instance);
 
 int imx1_init(void);
diff --git a/arch/arm/mach-imx/include/mach/imx-nand.h b/arch/arm/mach-imx/include/mach/imx-nand.h
index b9305e2e9d..0adba0989a 100644
--- a/arch/arm/mach-imx/include/mach/imx-nand.h
+++ b/arch/arm/mach-imx/include/mach/imx-nand.h
@@ -59,6 +59,55 @@ struct imx_nand_platform_data {
 
 #define NFC_V2_SPAS_SPARESIZE(spas)	((spas) >> 1)
 
+#define NFC_V3_FLASH_CMD		(host->regs_axi + 0x00)
+#define NFC_V3_FLASH_ADDR0		(host->regs_axi + 0x04)
+
+#define NFC_V3_CONFIG1			(host->regs_axi + 0x34)
+#define NFC_V3_CONFIG1_SP_EN		(1 << 0)
+#define NFC_V3_CONFIG1_RBA(x)		(((x) & 0x7 ) << 4)
+
+#define NFC_V3_ECC_STATUS_RESULT	(host->regs_axi + 0x38)
+
+#define NFC_V3_LAUNCH			(host->regs_axi + 0x40)
+
+#define NFC_V3_WRPROT			(host->regs_ip + 0x0)
+#define NFC_V3_WRPROT_LOCK_TIGHT	(1 << 0)
+#define NFC_V3_WRPROT_LOCK		(1 << 1)
+#define NFC_V3_WRPROT_UNLOCK		(1 << 2)
+#define NFC_V3_WRPROT_BLS_UNLOCK	(2 << 6)
+
+#define NFC_V3_WRPROT_UNLOCK_BLK_ADD0   (host->regs_ip + 0x04)
+
+#define NFC_V3_CONFIG2			(host->regs_ip + 0x24)
+#define NFC_V3_CONFIG2_PS_512			(0 << 0)
+#define NFC_V3_CONFIG2_PS_2048			(1 << 0)
+#define NFC_V3_CONFIG2_PS_4096			(2 << 0)
+#define NFC_V3_CONFIG2_ONE_CYCLE		(1 << 2)
+#define NFC_V3_CONFIG2_ECC_EN			(1 << 3)
+#define NFC_V3_CONFIG2_2CMD_PHASES		(1 << 4)
+#define NFC_V3_CONFIG2_NUM_ADDR_PHASE0		(1 << 5)
+#define NFC_V3_CONFIG2_ECC_MODE_8		(1 << 6)
+#define NFC_V3_MX51_CONFIG2_PPB(x)		(((x) & 0x3) << 7)
+#define NFC_V3_MX53_CONFIG2_PPB(x)		(((x) & 0x3) << 8)
+#define NFC_V3_CONFIG2_NUM_ADDR_PHASE1(x)	(((x) & 0x3) << 12)
+#define NFC_V3_CONFIG2_INT_MSK			(1 << 15)
+#define NFC_V3_CONFIG2_ST_CMD(x)		(((x) & 0xff) << 24)
+#define NFC_V3_CONFIG2_SPAS(x)			(((x) & 0xff) << 16)
+
+#define NFC_V3_CONFIG3				(host->regs_ip + 0x28)
+#define NFC_V3_CONFIG3_ADD_OP(x)		(((x) & 0x3) << 0)
+#define NFC_V3_CONFIG3_FW8			(1 << 3)
+#define NFC_V3_CONFIG3_SBB(x)			(((x) & 0x7) << 8)
+#define NFC_V3_CONFIG3_NUM_OF_DEVICES(x)	(((x) & 0x7) << 12)
+#define NFC_V3_CONFIG3_RBB_MODE			(1 << 15)
+#define NFC_V3_CONFIG3_NO_SDMA			(1 << 20)
+
+#define NFC_V3_IPC			(host->regs_ip + 0x2C)
+#define NFC_V3_IPC_CREQ			(1 << 0)
+#define NFC_V3_IPC_INT			(1 << 31)
+
+#define NFC_V3_DELAY_LINE		(host->regs_ip + 0x34)
+
 /*
  * Operation modes for the NFC. Valid for v1, v2 and v3
  * type controllers.
diff --git a/arch/arm/mach-imx/include/mach/imx6-regs.h b/arch/arm/mach-imx/include/mach/imx6-regs.h
index 68be43c9ab..e661c4ed12 100644
--- a/arch/arm/mach-imx/include/mach/imx6-regs.h
+++ b/arch/arm/mach-imx/include/mach/imx6-regs.h
@@ -3,6 +3,9 @@
 
 #define MX6_GPMI_BASE_ADDR		0x00112000
 
+#define MX6_FAST1_BASE_ADDR		0x00c00000
+#define MX6_FAST2_BASE_ADDR		0x00b00000
+
 #define MX6_AIPS1_ARB_BASE_ADDR		0x02000000
 #define MX6_AIPS2_ARB_BASE_ADDR		0x02100000
 
diff --git a/arch/arm/mach-imx/include/mach/imx6.h b/arch/arm/mach-imx/include/mach/imx6.h
index e8ffa47a7d..fb5eaf16b7 100644
--- a/arch/arm/mach-imx/include/mach/imx6.h
+++ b/arch/arm/mach-imx/include/mach/imx6.h
@@ -4,6 +4,7 @@
 #include <io.h>
 #include <mach/generic.h>
 #include <mach/imx6-regs.h>
+#include <mach/revision.h>
 
 void imx6_init_lowlevel(void);
 
@@ -48,6 +49,41 @@ static inline int imx6_cpu_type(void)
 	return __imx6_cpu_type();
 }
 
+static inline int __imx6_cpu_revision(void)
+{
+
+	uint32_t rev;
+
+	rev = readl(MX6_ANATOP_BASE_ADDR + IMX6_ANATOP_SI_REV);
+
+	switch (rev & 0xfff) {
+	case 0x00:
+		return IMX_CHIP_REV_1_0;
+	case 0x01:
+		return IMX_CHIP_REV_1_1;
+	case 0x02:
+		return IMX_CHIP_REV_1_2;
+	case 0x03:
+		return IMX_CHIP_REV_1_3;
+	case 0x04:
+		return IMX_CHIP_REV_1_4;
+	case 0x05:
+		return IMX_CHIP_REV_1_5;
+	case 0x100:
+		return IMX_CHIP_REV_2_0;
+	}
+
+	return IMX_CHIP_REV_UNKNOWN;
+}
+
+static inline int imx6_cpu_revision(void)
+{
+	if (!cpu_is_mx6())
+		return 0;
+
+	return __imx6_cpu_revision();
+}
+
 #define DEFINE_MX6_CPU_TYPE(str, type)					\
 	static inline int cpu_mx6_is_##str(void)			\
 	{								\
diff --git a/arch/arm/mach-imx/include/mach/xload.h b/arch/arm/mach-imx/include/mach/xload.h
index 997522e2d5..3898d664ef 100644
--- a/arch/arm/mach-imx/include/mach/xload.h
+++ b/arch/arm/mach-imx/include/mach/xload.h
@@ -1,6 +1,7 @@
 #ifndef __MACH_XLOAD_H
 #define __MACH_XLOAD_H
 
+int imx53_nand_start_image(void);
 int imx6_spi_load_image(int instance, unsigned int flash_offset, void *buf, int len);
 int imx6_spi_start_image(int instance);
 int imx6_esdhc_load_image(int instance, void *buf, int len);
diff --git a/arch/arm/mach-imx/xload-imx-nand.c b/arch/arm/mach-imx/xload-imx-nand.c
new file mode 100644
index 0000000000..22e41fac77
--- /dev/null
+++ b/arch/arm/mach-imx/xload-imx-nand.c
@@ -0,0 +1,308 @@
+/*
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#define pr_fmt(fmt)	"imx-nand-boot: " fmt
+
+#include <common.h>
+#include <init.h>
+#include <io.h>
+#include <linux/mtd/nand.h>
+#include <mach/imx-nand.h>
+#include <mach/generic.h>
+#include <mach/imx53-regs.h>
+#include <mach/xload.h>
+
+struct imx_nand {
+	void __iomem *base;
+	void __iomem *main_area0;
+	void __iomem *regs_ip;
+	void __iomem *regs_axi;
+	void *spare0;
+	int pagesize;
+	int v1;
+	int pages_per_block;
+};
+
+static void wait_op_done(struct imx_nand *host)
+{
+	u32 r;
+
+	while (1) {
+		r = readl(NFC_V3_IPC);
+		if (r & NFC_V3_IPC_INT)
+			break;
+	};
+
+	r &= ~NFC_V3_IPC_INT;
+
+	writel(r, NFC_V3_IPC);
+}
+
+/*
+ * This function issues the specified command to the NAND device and
+ * waits for completion.
+ *
+ * @param       cmd     command for NAND Flash
+ */
+static void imx_nandboot_send_cmd(struct imx_nand *host, u16 cmd)
+{
+	/* fill command */
+	writel(cmd, NFC_V3_FLASH_CMD);
+
+	/* send out command */
+	writel(NFC_CMD, NFC_V3_LAUNCH);
+
+	/* Wait for operation to complete */
+	wait_op_done(host);
+}
+
+/*
+ * This function sends an address (or partial address) to the
+ * NAND device.  The address is used to select the source/destination for
+ * a NAND command.
+ *
+ * @param       addr    address to be written to NFC.
+ * @param       islast  True if this is the last address cycle for command
+ */
+static void imx_nandboot_send_addr(struct imx_nand *host, u16 addr)
+{
+	/* fill address */
+	writel(addr, NFC_V3_FLASH_ADDR0);
+
+	/* send out address */
+	writel(NFC_ADDR, NFC_V3_LAUNCH);
+
+	wait_op_done(host);
+}
+
+static void imx_nandboot_nfc_addr(struct imx_nand *host, int page)
+{
+	imx_nandboot_send_addr(host, 0);
+
+	if (host->pagesize == 2048)
+		imx_nandboot_send_addr(host, 0);
+
+	imx_nandboot_send_addr(host, page & 0xff);
+	imx_nandboot_send_addr(host, (page >> 8) & 0xff);
+	imx_nandboot_send_addr(host, (page >> 16) & 0xff);
+
+	if (host->pagesize == 2048)
+		imx_nandboot_send_cmd(host, NAND_CMD_READSTART);
+}
+
+static void imx_nandboot_send_page(struct imx_nand *host, unsigned int ops)
+{
+	uint32_t tmp;
+
+	tmp = readl(NFC_V3_CONFIG1);
+	tmp &= ~(7 << 4);
+	writel(tmp, NFC_V3_CONFIG1);
+
+	/* transfer data from NFC ram to nand */
+	writel(ops, NFC_V3_LAUNCH);
+
+	wait_op_done(host);
+}
+
+static void __memcpy32(void *trg, const void *src, int size)
+{
+	int i;
+	unsigned int *t = trg;
+	unsigned const int *s = src;
+
+	for (i = 0; i < (size >> 2); i++)
+		*t++ = *s++;
+}
+
+static void imx_nandboot_get_page(struct imx_nand *host, unsigned int page)
+{
+	imx_nandboot_send_cmd(host, NAND_CMD_READ0);
+	imx_nandboot_nfc_addr(host, page);
+	imx_nandboot_send_page(host, NFC_OUTPUT);
+}
+
+static int imx_nandboot_read_page(struct imx_nand *host, unsigned int page,
+				   void *buf)
+{
+	int nsubpages;
+	u32 eccstat, err;
+
+	imx_nandboot_get_page(host, page);
+
+	__memcpy32(buf, host->main_area0, host->pagesize);
+
+	eccstat = readl(NFC_V3_ECC_STATUS_RESULT);
+	nsubpages = host->pagesize / 512;
+
+	do {
+		err = eccstat & 0xf;
+		if (err == 0xf)
+			return -EBADMSG;
+		eccstat >>= 4;
+	} while (--nsubpages);
+
+	return 0;
+}
+
+static int dbbt_block_is_bad(void *_dbbt, int block)
+{
+	int i;
+	u32 *dbbt = _dbbt;
+	int num_bad_blocks;
+
+	if (!_dbbt)
+		return false;
+
+	dbbt++; /* reserved */
+
+	num_bad_blocks = *dbbt++;
+
+	for (i = 0; i < num_bad_blocks; i++) {
+		if (*dbbt == block)
+			return true;
+		dbbt++;
+	}
+
+	return false;
+}
+
+static int read_firmware(struct imx_nand *host, void *dbbt, int page, void *buf,
+			 int npages)
+{
+	int ret;
+
+	if (dbbt_block_is_bad(dbbt, page / host->pages_per_block))
+		page = ALIGN(page, host->pages_per_block);
+
+	while (npages) {
+		if (!(page % host->pages_per_block)) {
+			if (dbbt_block_is_bad(NULL, page / host->pages_per_block)) {
+				page += host->pages_per_block;
+				continue;
+			}
+		}
+
+		ret = imx_nandboot_read_page(host, page, buf);
+		if (ret)
+			return ret;
+
+		buf += host->pagesize;
+		page++;
+		npages--;
+	}
+
+	return 0;
+}
+
+int imx53_nand_start_image(void)
+{
+	struct imx_nand host;
+	void *buf = IOMEM(MX53_CSD0_BASE_ADDR);
+	void *dbbt = NULL;
+	int page_firmware1, page_firmware2, page_dbbt, image_size, npages;
+	void (*firmware)(void);
+	int ret;
+	u32 cfg1 = readl(IOMEM(MX53_SRC_BASE_ADDR) + 0x4);
+
+	host.base = IOMEM(MX53_NFC_AXI_BASE_ADDR);
+	host.main_area0 = host.base;
+	host.regs_ip = IOMEM(MX53_NFC_BASE_ADDR);
+	host.regs_axi = host.base + 0x1e00;
+	host.spare0 = host.base + 0x1000;
+
+	switch ((cfg1 >> 14) & 0x3) {
+	case 0:
+		host.pagesize = 512;
+		break;
+	case 1:
+		host.pagesize = 2048;
+		break;
+	case 2:
+	case 3:
+		host.pagesize = 4096;
+		break;
+	}
+
+	switch ((cfg1 >> 17) & 0x3) {
+	case 0:
+		host.pages_per_block = 32;
+		break;
+	case 1:
+		host.pages_per_block = 64;
+		break;
+	case 2:
+		host.pages_per_block = 128;
+		break;
+	case 3:
+		host.pages_per_block = 256;
+		break;
+	}
+
+	pr_debug("Using pagesize %d, %d pages per block\n",
+		 host.pagesize, host.pages_per_block);
+
+	ret = imx_nandboot_read_page(&host, 0, buf);
+	if (ret)
+		return ret;
+
+	if (*(u32 *)(buf + 0x4) != 0x20424346) {
+		pr_err("No FCB Found on flash\n");
+		return -EINVAL;
+	}
+
+	page_firmware1 = *(u32 *)(buf + 0x68);
+	page_firmware2 = *(u32 *)(buf + 0x6c);
+	page_dbbt = *(u32 *)(buf + 0x78);
+
+	image_size = ALIGN(imx_image_size(), host.pagesize);
+	npages = image_size / host.pagesize;
+
+	if (page_dbbt) {
+		ret = imx_nandboot_read_page(&host, page_dbbt, buf);
+		if (!ret && *(u32 *)(buf + 0x4) == 0x44424254) {
+			ret = imx_nandboot_read_page(&host, page_dbbt + 4, buf);
+			if (!ret) {
+				pr_debug("Using DBBT from page %d\n", page_dbbt + 4);
+				dbbt = buf;
+				buf += host.pagesize;
+			}
+		}
+	}
+
+	pr_debug("Reading firmware from page %d, size %d\n",
+		 page_firmware1, image_size);
+
+	ret = read_firmware(&host, dbbt, page_firmware1, buf, npages);
+	if (ret) {
+		pr_debug("Reading primary firmware failed\n");
+		if (page_firmware2) {
+			pr_debug("Reading firmware from page %d, size %d\n",
+				 page_firmware2, image_size);
+			ret = read_firmware(&host, dbbt, page_firmware2, buf, npages);
+			if (ret) {
+				pr_err("Could not read firmware\n");
+				return -EINVAL;
+			}
+		} else {
+			pr_err("Reading primary firmware failed, no secondary firmware found\n");
+			return -EINVAL;
+		}
+	}
+
+	pr_debug("Firmware read, starting it\n");
+
+	firmware = buf;
+
+	firmware();
+
+	return 0;
+}