diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 69c1d7c..b6a9f93 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -123,6 +123,7 @@
 	select GPIOLIB
 	select HAS_DEBUG_LL
 	select HAVE_DEFAULT_ENVIRONMENT_NEW
+	select HAVE_MACH_ARM_HEAD
 	select HAVE_PBL_MULTI_IMAGES
 	select HW_HAS_PCI
 	select MVEBU_MBUS
diff --git a/arch/arm/boards/globalscale-guruplug/lowlevel.c b/arch/arm/boards/globalscale-guruplug/lowlevel.c
index 91bc1cf..67c3b23 100644
--- a/arch/arm/boards/globalscale-guruplug/lowlevel.c
+++ b/arch/arm/boards/globalscale-guruplug/lowlevel.c
@@ -31,5 +31,5 @@
 	fdt = __dtb_kirkwood_guruplug_server_plus_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	kirkwood_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/globalscale-mirabox/lowlevel.c b/arch/arm/boards/globalscale-mirabox/lowlevel.c
index 4f55d1a..7b070d7 100644
--- a/arch/arm/boards/globalscale-mirabox/lowlevel.c
+++ b/arch/arm/boards/globalscale-mirabox/lowlevel.c
@@ -31,5 +31,5 @@
 	fdt = __dtb_armada_370_mirabox_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	armada_370_xp_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/lenovo-ix4-300d/lowlevel.c b/arch/arm/boards/lenovo-ix4-300d/lowlevel.c
index 8cb8bd4..f8313cd 100644
--- a/arch/arm/boards/lenovo-ix4-300d/lowlevel.c
+++ b/arch/arm/boards/lenovo-ix4-300d/lowlevel.c
@@ -31,5 +31,5 @@
 	fdt = __dtb_armada_xp_lenovo_ix4_300d_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	armada_370_xp_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/marvell-armada-xp-gp/lowlevel.c b/arch/arm/boards/marvell-armada-xp-gp/lowlevel.c
index 59eaa29..da6d4aa 100644
--- a/arch/arm/boards/marvell-armada-xp-gp/lowlevel.c
+++ b/arch/arm/boards/marvell-armada-xp-gp/lowlevel.c
@@ -30,5 +30,5 @@
 
 	fdt = __dtb_armada_xp_gp_bb_start - get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	armada_370_xp_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/netgear-rn104/lowlevel.c b/arch/arm/boards/netgear-rn104/lowlevel.c
index f0d6df0..97590d8 100644
--- a/arch/arm/boards/netgear-rn104/lowlevel.c
+++ b/arch/arm/boards/netgear-rn104/lowlevel.c
@@ -18,5 +18,5 @@
 	fdt = __dtb_armada_370_rn104_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	armada_370_xp_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/netgear-rn2120/lowlevel.c b/arch/arm/boards/netgear-rn2120/lowlevel.c
index 29c8b43..59b9985 100644
--- a/arch/arm/boards/netgear-rn2120/lowlevel.c
+++ b/arch/arm/boards/netgear-rn2120/lowlevel.c
@@ -16,12 +16,14 @@
 #include <asm/barebox-arm-head.h>
 #include <asm/io.h>
 #include <mach/lowlevel.h>
+#include <mach/common.h>
 
 extern char __dtb_armada_xp_rn2120_bb_start[];
 
 ENTRY_FUNCTION(start_netgear_rn2120, r0, r1, r2)
 {
 	void *fdt;
+	void __iomem *base = mvebu_get_initial_int_reg_base();
 
 	arm_cpu_lowlevel_init();
 
@@ -31,11 +33,26 @@
 	 * SoC reset.
 	 * This is effectively gpio_direction_output(42, 1);
 	 */
-	writel((1 << 10) | readl((void *)0xd0018140), (void *)0xd0018140);
-	writel(~(1 << 10) & readl((void *)0xd0018144), (void *)0xd0018144);
+	writel((1 << 10) | readl(base + 0x18140), base + 0x18140);
+	writel(~(1 << 10) & readl(base + 0x18144), base + 0x18144);
+
+	/*
+	 * The vendor binary that initializes RAM doesn't program the SDRAM
+	 * Address Decoding registers to match the actual available RAM. But as
+	 * barebox later determines the RAM size from these, fix them up here.
+	 */
+
+	/* Win 1 Base Address Register: base=0x40000000 */
+	writel(0x40000000, base + 0x20188);
+	/* Win 1 Control Register: size=0x4000000, wincs=1, en=1*/
+	writel(0x3fffffe5, base + 0x2018c);
+
+	/* Win 0 Base Address Register is already 0, base=0x00000000 */
+	/* Win 0 Control Register: size=0x4000000, wincs=0, en=1 */
+	writel(0x3fffffe1, base + 0x20184);
 
 	fdt = __dtb_armada_xp_rn2120_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	armada_370_xp_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/plathome-openblocks-a6/lowlevel.c b/arch/arm/boards/plathome-openblocks-a6/lowlevel.c
index 71bf7aa..785ec21 100644
--- a/arch/arm/boards/plathome-openblocks-a6/lowlevel.c
+++ b/arch/arm/boards/plathome-openblocks-a6/lowlevel.c
@@ -28,5 +28,5 @@
 	fdt = __dtb_kirkwood_openblocks_a6_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	kirkwood_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/plathome-openblocks-ax3/lowlevel.c b/arch/arm/boards/plathome-openblocks-ax3/lowlevel.c
index 9030a5d..ae3664d 100644
--- a/arch/arm/boards/plathome-openblocks-ax3/lowlevel.c
+++ b/arch/arm/boards/plathome-openblocks-ax3/lowlevel.c
@@ -31,5 +31,5 @@
 	fdt = __dtb_armada_xp_openblocks_ax3_4_bb_start -
 		get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	armada_370_xp_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/solidrun-cubox/lowlevel.c b/arch/arm/boards/solidrun-cubox/lowlevel.c
index 08e31e8..071309e 100644
--- a/arch/arm/boards/solidrun-cubox/lowlevel.c
+++ b/arch/arm/boards/solidrun-cubox/lowlevel.c
@@ -31,5 +31,5 @@
 
 	fdt = __dtb_dove_cubox_bb_start - get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	dove_barebox_entry(fdt);
 }
diff --git a/arch/arm/boards/usi-topkick/lowlevel.c b/arch/arm/boards/usi-topkick/lowlevel.c
index ed94ee6..4c731e5 100644
--- a/arch/arm/boards/usi-topkick/lowlevel.c
+++ b/arch/arm/boards/usi-topkick/lowlevel.c
@@ -30,5 +30,5 @@
 
 	fdt = __dtb_kirkwood_topkick_bb_start - get_runtime_offset();
 
-	mvebu_barebox_entry(fdt);
+	kirkwood_barebox_entry(fdt);
 }
diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index 80b3947..fb5c4e6 100644
--- a/arch/arm/mach-mvebu/Makefile
+++ b/arch/arm/mach-mvebu/Makefile
@@ -1,6 +1,6 @@
-lwl-y				+= lowlevel.o
-obj-y				+= common.o
+obj-pbl-y			+= common.o
 obj-$(CONFIG_ARCH_ARMADA_370)	+= armada-370-xp.o
 obj-$(CONFIG_ARCH_ARMADA_XP)	+= armada-370-xp.o
 obj-$(CONFIG_ARCH_DOVE)		+= dove.o
 obj-$(CONFIG_ARCH_KIRKWOOD)	+= kirkwood.o
+obj-y				+= kwbootimage.o
diff --git a/arch/arm/mach-mvebu/armada-370-xp.c b/arch/arm/mach-mvebu/armada-370-xp.c
index c362cfd..93ad955 100644
--- a/arch/arm/mach-mvebu/armada-370-xp.c
+++ b/arch/arm/mach-mvebu/armada-370-xp.c
@@ -25,60 +25,30 @@
 #include <mach/armada-370-xp-regs.h>
 #include <mach/socid.h>
 
-static inline void armada_370_xp_memory_find(unsigned long *phys_base,
-					     unsigned long *phys_size)
-{
-	int cs;
-
-	*phys_base = ~0;
-	*phys_size = 0;
-
-	for (cs = 0; cs < 4; cs++) {
-		u32 base = readl(ARMADA_370_XP_SDRAM_BASE + DDR_BASE_CSn(cs));
-		u32 ctrl = readl(ARMADA_370_XP_SDRAM_BASE + DDR_SIZE_CSn(cs));
-
-		/* Skip non-enabled CS */
-		if ((ctrl & DDR_SIZE_ENABLED) != DDR_SIZE_ENABLED)
-			continue;
-
-		base &= DDR_BASE_CS_LOW_MASK;
-		if (base < *phys_base)
-			*phys_base = base;
-		*phys_size += (ctrl | ~DDR_SIZE_MASK) + 1;
-	}
-}
-
 static const struct of_device_id armada_370_xp_pcie_of_ids[] = {
 	{ .compatible = "marvell,armada-xp-pcie", },
 	{ .compatible = "marvell,armada-370-pcie", },
 	{ },
 };
 
-static int armada_370_xp_soc_id_fixup(void)
+/*
+ * Marvell Armada XP MV78230-A0 incorrectly identifies itself as
+ * MV78460. Check for DEVID_MV78460 but if there are only 2 CPUs
+ * present in Coherency Fabric, fixup PCIe PRODUCT_ID.
+ */
+static int armada_xp_soc_id_fixup(void)
 {
 	struct device_node *np, *cnp;
 	void __iomem *base;
-	u32 reg, ctrl, mask;
+	u32 reg, ctrl;
 	u32 socid, numcpus;
 
 	socid = readl(ARMADA_370_XP_CPU_SOC_ID) & CPU_SOC_ID_DEVICE_MASK;
 	numcpus = 1 + (readl(ARMADA_370_XP_FABRIC_CONF) & FABRIC_NUM_CPUS_MASK);
 
-	switch (socid) {
-	/*
-	 * Marvell Armada XP MV78230-A0 incorrectly identifies itself as
-	 * MV78460. Check for DEVID_MV78460 but if there are only 2 CPUs
-	 * present in Coherency Fabric, fixup PCIe PRODUCT_ID.
-	 */
-	case DEVID_MV78460:
-		if (numcpus != 2)
-			return 0;
-		socid = DEVID_MV78230;
-		mask = PCIE0_EN | PCIE1_EN | PCIE0_QUADX1_EN;
-		break;
-	default:
+	if (socid != DEVID_MV78460 || numcpus != 2)
+		/* not affected */
 		return 0;
-	}
 
 	np = of_find_matching_node(NULL, armada_370_xp_pcie_of_ids);
 	if (!np)
@@ -86,7 +56,7 @@
 
 	/* Enable all individual x1 ports */
 	ctrl = readl(ARMADA_370_XP_SOC_CTRL);
-	writel(ctrl | mask, ARMADA_370_XP_SOC_CTRL);
+	writel(ctrl | PCIE0_EN | PCIE1_EN | PCIE0_QUADX1_EN, ARMADA_370_XP_SOC_CTRL);
 
 	for_each_child_of_node(np, cnp) {
 		base = of_iomap(cnp, 0);
@@ -95,7 +65,7 @@
 
 		/* Fixup PCIe port DEVICE_ID */
 		reg = readl(base + PCIE_VEN_DEV_ID);
-		reg = (socid << 16) | (reg & 0xffff);
+		reg = (DEVID_MV78230 << 16) | (reg & 0xffff);
 		writel(reg, base + PCIE_VEN_DEV_ID);
 	}
 
@@ -113,23 +83,10 @@
 	hang();
 }
 
-static int armada_xp_init_soc(struct device_node *root)
+static int armada_370_xp_init_soc(void)
 {
 	u32 reg;
 
-	/* Enable GBE0, GBE1, LCD and NFC PUP */
-	reg = readl(ARMADA_XP_PUP_ENABLE);
-	reg |= GE0_PUP_EN | GE1_PUP_EN | LCD_PUP_EN | NAND_PUP_EN | SPI_PUP_EN;
-	writel(reg, ARMADA_XP_PUP_ENABLE);
-
-	return 0;
-}
-
-static int armada_370_xp_init_soc(struct device_node *root, void *context)
-{
-	unsigned long phys_base, phys_size;
-	u32 reg;
-
 	if (!of_machine_is_compatible("marvell,armada-370-xp"))
 		return 0;
 
@@ -143,23 +100,17 @@
 	reg &= ~MBUS_ERR_PROP_EN;
 	writel(reg, ARMADA_370_XP_FABRIC_CTRL);
 
-	armada_370_xp_memory_find(&phys_base, &phys_size);
-
-	mvebu_set_memory(phys_base, phys_size);
 	mvebu_mbus_init();
 
-	armada_370_xp_soc_id_fixup();
+	armada_xp_soc_id_fixup();
 
-	if (of_machine_is_compatible("marvell,armadaxp"))
-		armada_xp_init_soc(root);
+	if (of_machine_is_compatible("marvell,armadaxp")) {
+		/* Enable GBE0, GBE1, LCD and NFC PUP */
+		reg = readl(ARMADA_XP_PUP_ENABLE);
+		reg |= GE0_PUP_EN | GE1_PUP_EN | LCD_PUP_EN | NAND_PUP_EN | SPI_PUP_EN;
+		writel(reg, ARMADA_XP_PUP_ENABLE);
+	}
 
 	return 0;
 }
-
-static int armada_370_xp_register_soc_fixup(void)
-{
-	mvebu_mbus_add_range("marvell,armada-370-xp", 0xf0, 0x01,
-			     MVEBU_REMAP_INT_REG_BASE);
-	return of_register_fixup(armada_370_xp_init_soc, NULL);
-}
-pure_initcall(armada_370_xp_register_soc_fixup);
+postcore_initcall(armada_370_xp_init_soc);
diff --git a/arch/arm/mach-mvebu/common.c b/arch/arm/mach-mvebu/common.c
index cb40d0c..06bfb72 100644
--- a/arch/arm/mach-mvebu/common.c
+++ b/arch/arm/mach-mvebu/common.c
@@ -23,6 +23,31 @@
 #include <linux/clk.h>
 #include <mach/common.h>
 #include <mach/socid.h>
+#include <asm/barebox-arm.h>
+#include <asm/memory.h>
+
+/*
+ * The different SoC headers containing register definitions (mach/dove-regs.h,
+ * mach/kirkwood-regs.h and mach/armada-370-xp-regs.h) are pairwise
+ * incompatible. So some defines are reproduced here instead of just #included.
+ */
+
+#define DOVE_SDRAM_BASE			IOMEM(0xf1800000)
+#define DOVE_SDRAM_MAPn(n)		(0x100 + ((n) * 0x10))
+#define DOVE_SDRAM_MAP_VALID		BIT(0)
+#define DOVE_SDRAM_LENGTH_SHIFT		16
+#define DOVE_SDRAM_LENGTH_MASK		(0x00f << DOVE_SDRAM_LENGTH_SHIFT)
+
+#define KIRKWOOD_SDRAM_BASE		(IOMEM(MVEBU_REMAP_INT_REG_BASE) + 0x00000)
+#define KIRKWOOD_DDR_BASE_CSn(n)	(0x1500 + ((n) * 0x8))
+#define KIRKWOOD_DDR_SIZE_CSn(n)	(0x1504 + ((n) * 0x8))
+#define KIRKWOOD_DDR_SIZE_ENABLED	BIT(0)
+#define KIRKWOOD_DDR_SIZE_MASK		0xff000000
+
+#define ARMADA_370_XP_SDRAM_BASE	(IOMEM(MVEBU_REMAP_INT_REG_BASE) + 0x20000)
+#define ARMADA_370_XP_DDR_SIZE_CSn(n)	(0x184 + ((n) * 0x8))
+#define ARMADA_370_XP_DDR_SIZE_ENABLED	BIT(0)
+#define ARMADA_370_XP_DDR_SIZE_MASK	0xff000000
 
 /*
  * Marvell MVEBU SoC id and revision can be read from any PCIe
@@ -78,48 +103,110 @@
 }
 postcore_initcall(mvebu_soc_id_init);
 
-/*
- * Memory size is set up by BootROM and can be read from SoC's ram controller
- * registers. Fixup provided DTs to reflect accessible amount of directly
- * attached RAM. Removable RAM, e.g. SODIMM, should be added by a per-board
- * fixup.
- */
-int mvebu_set_memory(u64 phys_base, u64 phys_size)
+static unsigned long dove_memory_find(void)
 {
-	struct device_node *np, *root;
-	__be32 reg[4];
-	int na, ns;
+	int n;
+	unsigned long mem_size = 0;
 
-	root = of_get_root_node();
-	if (!root)
-		return -EINVAL;
+	for (n = 0; n < 2; n++) {
+		uint32_t map = readl(DOVE_SDRAM_BASE + DOVE_SDRAM_MAPn(n));
+		uint32_t size;
 
-	np = of_find_node_by_path("/memory");
-	if (!np)
-		np = of_create_node(root, "/memory");
-	if (!np)
-		return -EINVAL;
+		/* skip disabled areas */
+		if ((map & DOVE_SDRAM_MAP_VALID) != DOVE_SDRAM_MAP_VALID)
+			continue;
 
-	na = of_n_addr_cells(np);
-	ns = of_n_size_cells(np);
-
-	if (na == 2) {
-		reg[0] = cpu_to_be32(phys_base >> 32);
-		reg[1] = cpu_to_be32(phys_base & 0xffffffff);
-	} else {
-		reg[0] = cpu_to_be32(phys_base & 0xffffffff);
+		/* real size is encoded as ld(2^(16+length)) */
+		size = (map & DOVE_SDRAM_LENGTH_MASK) >> DOVE_SDRAM_LENGTH_SHIFT;
+		mem_size += 1 << (16 + size);
 	}
 
-	if (ns == 2) {
-		reg[2] = cpu_to_be32(phys_size >> 32);
-		reg[3] = cpu_to_be32(phys_size & 0xffffffff);
-	} else {
-		reg[1] = cpu_to_be32(phys_size & 0xffffffff);
+	return mem_size;
+}
+
+static unsigned long kirkwood_memory_find(void)
+{
+	int cs;
+	unsigned long mem_size = 0;
+
+	for (cs = 0; cs < 4; cs++) {
+		u32 ctrl = readl(KIRKWOOD_SDRAM_BASE +
+				 KIRKWOOD_DDR_SIZE_CSn(cs));
+
+		/* Skip non-enabled CS */
+		if ((ctrl & KIRKWOOD_DDR_SIZE_ENABLED) !=
+		    KIRKWOOD_DDR_SIZE_ENABLED)
+			continue;
+
+		mem_size += (ctrl | ~KIRKWOOD_DDR_SIZE_MASK) + 1;
 	}
 
-	if (of_set_property(np, "device_type", "memory", sizeof("memory"), 1) ||
-	    of_set_property(np, "reg", reg, sizeof(u32) * (na + ns), 1))
-		pr_err("Unable to fixup memory node\n");
+	return mem_size;
+}
+
+static unsigned long armada_370_xp_memory_find(void)
+{
+	int cs;
+	unsigned long mem_size = 0;
+
+	for (cs = 0; cs < 4; cs++) {
+		u32 ctrl = readl(ARMADA_370_XP_SDRAM_BASE + ARMADA_370_XP_DDR_SIZE_CSn(cs));
+
+		/* Skip non-enabled CS */
+		if ((ctrl & ARMADA_370_XP_DDR_SIZE_ENABLED) != ARMADA_370_XP_DDR_SIZE_ENABLED)
+			continue;
+
+		mem_size += (ctrl | ~ARMADA_370_XP_DDR_SIZE_MASK) + 1;
+	}
+
+	return mem_size;
+}
+
+static int mvebu_meminit(void)
+{
+	if (of_machine_is_compatible("marvell,armada-370-xp"))
+		arm_add_mem_device("ram0", 0, armada_370_xp_memory_find());
 
 	return 0;
 }
+mem_initcall(mvebu_meminit);
+
+/*
+ * All MVEBU SoCs start with internal registers at 0xd0000000.
+ * To get more contiguous address space and as Linux expects them
+ * there, we remap them early to 0xf1000000.
+ *
+ * There no way to determine internal registers base address
+ * safely later on, as the remap register itself is within the
+ * internal registers.
+ */
+#define MVEBU_BRIDGE_REG_BASE		0x20000
+#define DEVICE_INTERNAL_BASE_ADDR	(MVEBU_BRIDGE_REG_BASE + 0x80)
+
+static void mvebu_remap_registers(void)
+{
+	void __iomem *base = mvebu_get_initial_int_reg_base();
+
+	writel(MVEBU_REMAP_INT_REG_BASE, base + DEVICE_INTERNAL_BASE_ADDR);
+}
+
+void __naked __noreturn dove_barebox_entry(void *boarddata)
+{
+	mvebu_remap_registers();
+
+	barebox_arm_entry(0, dove_memory_find(), boarddata);
+}
+
+void __naked __noreturn kirkwood_barebox_entry(void *boarddata)
+{
+	mvebu_remap_registers();
+
+	barebox_arm_entry(0, kirkwood_memory_find(), boarddata);
+}
+
+void __naked __noreturn armada_370_xp_barebox_entry(void *boarddata)
+{
+	mvebu_remap_registers();
+
+	barebox_arm_entry(0, armada_370_xp_memory_find(), boarddata);
+}
diff --git a/arch/arm/mach-mvebu/dove.c b/arch/arm/mach-mvebu/dove.c
index ba4af3a..1cdb7e1 100644
--- a/arch/arm/mach-mvebu/dove.c
+++ b/arch/arm/mach-mvebu/dove.c
@@ -43,32 +43,6 @@
 	writel(val, mcboot + SDRAM_REGS_BASE_DECODE);
 }
 
-static inline void dove_memory_find(unsigned long *phys_base,
-				    unsigned long *phys_size)
-{
-	int n;
-
-	*phys_base = ~0;
-	*phys_size = 0;
-
-	for (n = 0; n < 2; n++) {
-		uint32_t map = readl(DOVE_SDRAM_BASE + SDRAM_MAPn(n));
-		uint32_t base, size;
-
-		/* skip disabled areas */
-		if ((map & SDRAM_MAP_VALID) != SDRAM_MAP_VALID)
-			continue;
-
-		base = map & SDRAM_START_MASK;
-		if (base < *phys_base)
-			*phys_base = base;
-
-		/* real size is encoded as ld(2^(16+length)) */
-		size = (map & SDRAM_LENGTH_MASK) >> SDRAM_LENGTH_SHIFT;
-		*phys_size += 1 << (16 + size);
-	}
-}
-
 static void __noreturn dove_restart_soc(struct restart_handler *rst)
 {
 	/* enable and assert RSTOUTn */
@@ -78,10 +52,8 @@
 	hang();
 }
 
-static int dove_init_soc(struct device_node *root, void *context)
+static int dove_init_soc(void)
 {
-	unsigned long phys_base, phys_size;
-
 	if (!of_machine_is_compatible("marvell,dove"))
 		return 0;
 
@@ -91,20 +63,8 @@
 	barebox_set_hostname("dove");
 
 	dove_remap_mc_regs();
-	dove_memory_find(&phys_base, &phys_size);
-
-	mvebu_set_memory(phys_base, phys_size);
 	mvebu_mbus_init();
 
 	return 0;
 }
-
-static int dove_register_soc_fixup(void)
-{
-	mvebu_mbus_add_range("marvell,dove", 0xf0, 0x01,
-			     MVEBU_REMAP_INT_REG_BASE);
-	mvebu_mbus_add_range("marvell,dove", 0xf0, 0x02,
-			     DOVE_REMAP_MC_REGS);
-	return of_register_fixup(dove_init_soc, NULL);
-}
-pure_initcall(dove_register_soc_fixup);
+postcore_initcall(dove_init_soc);
diff --git a/arch/arm/mach-mvebu/include/mach/barebox-arm-head.h b/arch/arm/mach-mvebu/include/mach/barebox-arm-head.h
new file mode 100644
index 0000000..3035f40
--- /dev/null
+++ b/arch/arm/mach-mvebu/include/mach/barebox-arm-head.h
@@ -0,0 +1,54 @@
+#include <linux/stringify.h>
+#include <mach/common.h>
+
+static inline void __barebox_arm_head(void)
+{
+	__asm__ __volatile__ (
+#ifdef CONFIG_THUMB2_BAREBOX
+		".arm\n"
+		"adr r9, 1f + 1\n"
+		"bx r9\n"
+		".thumb\n"
+		"1:\n"
+		"bl 2f\n"
+		".rept 10\n"
+		"1: b 1b\n"
+		".endr\n"
+#else
+		"b 2f\n"
+		"1: b 1b\n"
+		"1: b 1b\n"
+		"1: b 1b\n"
+		"1: b 1b\n"
+		"1: b 1b\n"
+		"1: b 1b\n"
+		"1: b 1b\n"
+#endif
+		".asciz \"barebox\"\n"
+		".word _text\n"				/* text base. If copied there,
+							 * barebox can skip relocation
+							 */
+		".word _barebox_image_size\n"		/* image size to copy */
+
+		/*
+		 * The following entry (at offset 0x30) is the only intended
+		 * difference to the original arm __barebox_arm_head. This value
+		 * holds the address of the internal register window when the
+		 * image is started. If the window is not at the reset default
+		 * position any more the caller can pass the actual value here.
+		 */
+		".word " __stringify(MVEBU_BOOTUP_INT_REG_BASE) "\n"
+		".rept 7\n"
+		".word 0x55555555\n"
+		".endr\n"
+		"2:\n"
+	);
+}
+
+static inline void barebox_arm_head(void)
+{
+	__barebox_arm_head();
+	__asm__ __volatile__ (
+		"b barebox_arm_reset_vector\n"
+	);
+}
diff --git a/arch/arm/mach-mvebu/include/mach/common.h b/arch/arm/mach-mvebu/include/mach/common.h
index 602af8f..529eb61 100644
--- a/arch/arm/mach-mvebu/include/mach/common.h
+++ b/arch/arm/mach-mvebu/include/mach/common.h
@@ -18,8 +18,23 @@
 #ifndef __MACH_COMMON_H__
 #define __MACH_COMMON_H__
 
+#include <asm/sections.h>
+#include <asm/unaligned.h>
+
+#define MVEBU_BOOTUP_INT_REG_BASE	0xd0000000
 #define MVEBU_REMAP_INT_REG_BASE	0xf1000000
 
-int mvebu_set_memory(u64 phys_base, u64 phys_size);
+/* #including <asm/barebox-arm.h> yields a circle, so we need a forward decl */
+uint32_t get_runtime_offset(void);
+
+static inline void __iomem *mvebu_get_initial_int_reg_base(void)
+{
+#ifdef __PBL__
+	u32 base = __get_unaligned_le32(_text - get_runtime_offset() + 0x30);
+	return (void __force __iomem *)base;
+#else
+	return (void __force __iomem *)MVEBU_REMAP_INT_REG_BASE;
+#endif
+}
 
 #endif
diff --git a/arch/arm/mach-mvebu/include/mach/lowlevel.h b/arch/arm/mach-mvebu/include/mach/lowlevel.h
index c922a27..3e639fc 100644
--- a/arch/arm/mach-mvebu/include/mach/lowlevel.h
+++ b/arch/arm/mach-mvebu/include/mach/lowlevel.h
@@ -19,5 +19,8 @@
 #define __MACH_LOWLEVEL_H__
 
 void mvebu_barebox_entry(void *boarddata);
+void dove_barebox_entry(void *boarddata);
+void kirkwood_barebox_entry(void *boarddata);
+void armada_370_xp_barebox_entry(void *boarddata);
 
 #endif
diff --git a/arch/arm/mach-mvebu/kirkwood.c b/arch/arm/mach-mvebu/kirkwood.c
index 72a6b0d..59fb95f 100644
--- a/arch/arm/mach-mvebu/kirkwood.c
+++ b/arch/arm/mach-mvebu/kirkwood.c
@@ -21,29 +21,6 @@
 #include <linux/mbus.h>
 #include <mach/kirkwood-regs.h>
 
-static inline void kirkwood_memory_find(unsigned long *phys_base,
-					unsigned long *phys_size)
-{
-	int cs;
-
-	*phys_base = ~0;
-	*phys_size = 0;
-
-	for (cs = 0; cs < 4; cs++) {
-		u32 base = readl(KIRKWOOD_SDRAM_BASE + DDR_BASE_CSn(cs));
-		u32 ctrl = readl(KIRKWOOD_SDRAM_BASE + DDR_SIZE_CSn(cs));
-
-		/* Skip non-enabled CS */
-		if ((ctrl & DDR_SIZE_ENABLED) != DDR_SIZE_ENABLED)
-			continue;
-
-		base &= DDR_BASE_CS_LOW_MASK;
-		if (base < *phys_base)
-			*phys_base = base;
-		*phys_size += (ctrl | ~DDR_SIZE_MASK) + 1;
-	}
-}
-
 static void __noreturn kirkwood_restart_soc(struct restart_handler *rst)
 {
 	writel(SOFT_RESET_OUT_EN, KIRKWOOD_BRIDGE_BASE + BRIDGE_RSTOUT_MASK);
@@ -52,10 +29,8 @@
 	hang();
 }
 
-static int kirkwood_init_soc(struct device_node *root, void *context)
+static int kirkwood_init_soc(void)
 {
-	unsigned long phys_base, phys_size;
-
 	if (!of_machine_is_compatible("marvell,kirkwood"))
 		return 0;
 
@@ -64,18 +39,8 @@
 	barebox_set_model("Marvell Kirkwood");
 	barebox_set_hostname("kirkwood");
 
-	kirkwood_memory_find(&phys_base, &phys_size);
-
-	mvebu_set_memory(phys_base, phys_size);
 	mvebu_mbus_init();
 
 	return 0;
 }
-
-static int kirkwood_register_soc_fixup(void)
-{
-	mvebu_mbus_add_range("marvell,kirkwood", 0xf0, 0x01,
-			     MVEBU_REMAP_INT_REG_BASE);
-	return of_register_fixup(kirkwood_init_soc, NULL);
-}
-pure_initcall(kirkwood_register_soc_fixup);
+postcore_initcall(kirkwood_init_soc);
diff --git a/arch/arm/mach-mvebu/kwbootimage.c b/arch/arm/mach-mvebu/kwbootimage.c
new file mode 100644
index 0000000..8d364ce
--- /dev/null
+++ b/arch/arm/mach-mvebu/kwbootimage.c
@@ -0,0 +1,84 @@
+#include <bootm.h>
+#include <common.h>
+#include <fcntl.h>
+#include <filetype.h>
+#include <fs.h>
+#include <init.h>
+#include <libfile.h>
+#include <restart.h>
+#include <unistd.h>
+#include <asm/unaligned.h>
+#include <mach/common.h>
+
+static int do_bootm_kwbimage_v1(struct image_data *data)
+{
+	int fd, ret;
+	loff_t offset;
+	char header[0x20];
+	u32 image_size, image_source;
+	void (*barebox)(void);
+
+	fd = open(data->os_file, O_RDONLY);
+	if (fd < 0)
+		return fd;
+
+	ret = read_full(fd, header, 0x20);
+	if (ret < 0x20) {
+		pr_err("Failed to read header\n");
+		if (ret >= 0)
+			return -EINVAL;
+		return -errno;
+	}
+
+	image_size = header[4] | header[5] << 8 | header[6] << 16 | header[7] << 24;
+	image_source = header[0xc] | header[0xd] << 8 |
+		header[0xe] << 16 | header[0xf] << 24;
+
+	if (data->verbose)
+		pr_info("size: %u\noffset: %u\n", image_size, image_source);
+
+	offset = lseek(fd, image_source, SEEK_SET);
+	if (offset < 0) {
+		pr_err("Failed to seek to image (%lld, %d)\n", offset, errno);
+		return -errno;
+	}
+
+	barebox = xzalloc(image_size);
+
+	ret = read_full(fd, barebox, image_size);
+	if (ret < image_size) {
+		pr_err("Failed to read image\n");
+		if (ret >= 0)
+			ret = -EINVAL;
+		else
+			ret = -errno;
+		goto out_free;
+	}
+
+	if (is_barebox_arm_head((void *)barebox))
+		put_unaligned_le32(MVEBU_REMAP_INT_REG_BASE, barebox + 0x30);
+
+	shutdown_barebox();
+
+	barebox();
+
+	restart_machine();
+
+out_free:
+	free(barebox);
+	return ret;
+}
+
+static struct image_handler image_handler_kwbimage_v1_handler = {
+	.name = "MVEBU kwbimage v1",
+	.bootm = do_bootm_kwbimage_v1,
+	.filetype = filetype_kwbimage_v1,
+};
+
+static int mvebu_register_kwbimage_image_handler(void)
+{
+	register_image_handler(&image_handler_kwbimage_v1_handler);
+
+	return 0;
+}
+late_initcall(mvebu_register_kwbimage_image_handler);
diff --git a/arch/arm/mach-mvebu/lowlevel.c b/arch/arm/mach-mvebu/lowlevel.c
deleted file mode 100644
index 8d0ac84..0000000
--- a/arch/arm/mach-mvebu/lowlevel.c
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * Copyright (C) 2013
- *  Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
- *  Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>
- *
- * 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.
- *
- */
-
-#include <common.h>
-#include <io.h>
-#include <linux/sizes.h>
-#include <asm/barebox-arm.h>
-#include <asm/barebox-arm-head.h>
-#include <mach/common.h>
-#include <mach/lowlevel.h>
-
-void __naked barebox_arm_reset_vector(void)
-{
-	arm_cpu_lowlevel_init();
-	mvebu_barebox_entry(NULL);
-}
-
-/*
- * All MVEBU SoCs start with internal registers at 0xd0000000.
- * To get more contiguous address space and as Linux expects them
- * there, we remap them early to 0xf1000000.
- *
- * There is no way to determine internal registers base address
- * safely later on, as the remap register itself is within the
- * internal registers.
- */
-#define MVEBU_BOOTUP_INT_REG_BASE	0xd0000000
-#define MVEBU_BRIDGE_REG_BASE		0x20000
-#define DEVICE_INTERNAL_BASE_ADDR	(MVEBU_BRIDGE_REG_BASE + 0x80)
-
-static void mvebu_remap_registers(void)
-{
-	writel(MVEBU_REMAP_INT_REG_BASE,
-	       IOMEM(MVEBU_BOOTUP_INT_REG_BASE) + DEVICE_INTERNAL_BASE_ADDR);
-}
-
-/*
- * Determining the actual memory size is highly SoC dependent,
- * but for all SoCs RAM starts at 0x00000000. Therefore, we start
- * with a minimal memory setup of 64M and probe correct memory size
- * later.
- */
-#define MVEBU_BOOTUP_MEMORY_BASE	0x00000000
-#define MVEBU_BOOTUP_MEMORY_SIZE	SZ_64M
-
-void __naked __noreturn mvebu_barebox_entry(void *boarddata)
-{
-	mvebu_remap_registers();
-	barebox_arm_entry(MVEBU_BOOTUP_MEMORY_BASE,
-			  MVEBU_BOOTUP_MEMORY_SIZE, boarddata);
-}
diff --git a/common/filetype.c b/common/filetype.c
index 8d72933..323da02 100644
--- a/common/filetype.c
+++ b/common/filetype.c
@@ -63,6 +63,7 @@
 	[filetype_exe] = { "MS-DOS executable", "exe" },
 	[filetype_mxs_bootstream] = { "Freescale MXS bootstream", "mxsbs" },
 	[filetype_socfpga_xload] = { "SoCFPGA prebootloader image", "socfpga-xload" },
+	[filetype_kwbimage_v1] = { "MVEBU kwbimage (v1)", "kwb" },
 };
 
 const char *file_type_to_string(enum filetype f)
@@ -292,6 +293,13 @@
 		return filetype_mips_barebox;
 	if (buf[0] == be32_to_cpu(0x534F4659))
 		return filetype_bpk;
+	if ((buf8[0] == 0x5a || buf8[0] == 0x69 || buf8[0] == 0x78 ||
+	     buf8[0] == 0x8b || buf8[0] == 0x9c) &&
+	    buf8[0x1] == 0 && buf8[0x2] == 0 && buf8[0x3] == 0 &&
+	    buf8[0x8] == 1 && buf8[0x18] == 0 && buf8[0x1b] == 0 &&
+	    buf8[0x1c] == 0 && buf8[0x1d] == 0 &&
+	    (buf8[0x1e] == 0 || buf8[0x1e] == 1))
+		return filetype_kwbimage_v1;
 
 	if (bufsize < 64)
 		return filetype_unknown;
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 7850e4a..0234c66 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -42,6 +42,11 @@
 	depends on DISK_AHCI
 	bool "i.MX AHCI support"
 
+config DISK_SATA_MV
+	depends on ARCH_MVEBU
+	select DISK_IDE_SFF
+	bool "Marvell SATA support"
+
 comment "interface types"
 
 config DISK_INTF_PLATFORM_IDE
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index c444c4d..6b83ae2 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_DISK_ATA) += disk_ata_drive.o
 obj-$(CONFIG_DISK_AHCI) += ahci.o
 obj-$(CONFIG_DISK_AHCI_IMX) += sata-imx.o
+obj-$(CONFIG_DISK_SATA_MV) += sata_mv.o
 
 # interface types
 
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
new file mode 100644
index 0000000..22b29d0
--- /dev/null
+++ b/drivers/ata/sata_mv.c
@@ -0,0 +1,126 @@
+#include <common.h>
+#include <clock.h>
+#include <driver.h>
+#include <init.h>
+#include <ata_drive.h>
+
+#include <asm/io.h>
+
+/* This can/should be moved to a more generic place */
+static void ata_ioports_init(struct ata_ioports *io,
+			     void *base_data, void *base_reg, void *base_alt,
+			     unsigned stride)
+{
+	/* io->cmd_addr is unused */;
+	io->data_addr = base_data;
+	io->error_addr = base_reg + 1 * stride;
+	/* io->feature_addr is unused */
+	io->nsect_addr = base_reg + 2 * stride;
+	io->lbal_addr = base_reg + 3 * stride;
+	io->lbam_addr = base_reg + 4 * stride;
+	io->lbah_addr = base_reg + 5 * stride;
+	io->device_addr = base_reg + 6 * stride;
+	io->status_addr = base_reg + 7 * stride;
+	io->command_addr = base_reg + 7 * stride;
+	/* io->altstatus_addr is unused */
+
+	if (base_alt)
+		io->ctl_addr = base_alt;
+	else
+		io->ctl_addr = io->status_addr;
+
+	/* io->alt_dev_addr is unused */
+}
+
+#define REG_WINDOW_CONTROL(n)	((n) * 0x10 + 0x30)
+#define REG_WINDOW_BASE(n)	((n) * 0x10 + 0x34)
+
+#define REG_EDMA_COMMAND(n)	((n) * 0x2000 + 0x2028)
+#define REG_EDMA_COMMAND__EATARST	0x00000004
+
+#define REG_ATA_BASE		0x2100
+#define REG_SSTATUS(n)		((n) * 0x2000 + 0x2300)
+#define REG_SCONTROL(n)		((n) * 0x2000 + 0x2308)
+#define REG_SCONTROL__DET		0x0000000f
+#define REG_SCONTROL__DET__INIT		0x00000001
+#define REG_SCONTROL__DET__PHYOK	0x00000002
+#define REG_SCONTROL__IPM		0x00000f00
+#define REG_SCONTROL__IPM__PARTIAL	0x00000100
+#define REG_SCONTROL__IPM__SLUMBER	0x00000200
+
+static int mv_sata_probe(struct device_d *dev)
+{
+	struct resource *iores;
+	void __iomem *base;
+	struct ide_port *ide;
+	u32 scontrol;
+	int ret, i;
+
+	iores = dev_request_mem_resource(dev, 0);
+	if (IS_ERR(iores)) {
+		dev_err(dev, "Failed to request mem resources\n");
+		return PTR_ERR(iores);
+	}
+	base = IOMEM(iores->start);
+
+	/* disable MBus windows */
+	for (i = 0; i < 4; ++i) {
+		writel(0, base + REG_WINDOW_CONTROL(i));
+		writel(0, base + REG_WINDOW_BASE(i));
+	}
+
+	/* enable first window */
+	writel(0x7fff0e01, base + REG_WINDOW_CONTROL(0));
+	writel(0, base + REG_WINDOW_BASE(0));
+
+	writel(REG_EDMA_COMMAND__EATARST, base + REG_EDMA_COMMAND(0));
+	udelay(25);
+	writel(0x0, base + REG_EDMA_COMMAND(0));
+
+	scontrol = readl(base + REG_SCONTROL(0));
+	scontrol &= ~(REG_SCONTROL__DET | REG_SCONTROL__IPM);
+	/* disable power management */
+	scontrol |= REG_SCONTROL__IPM__PARTIAL | REG_SCONTROL__IPM__SLUMBER;
+
+	/* perform interface communication initialization */
+	writel(scontrol | REG_SCONTROL__DET__INIT, base + REG_SCONTROL(0));
+	writel(scontrol, base + REG_SCONTROL(0));
+
+	ret = wait_on_timeout(10 * MSECOND,
+			      (readl(base + REG_SSTATUS(0)) & REG_SCONTROL__DET) == (REG_SCONTROL__DET__INIT | REG_SCONTROL__DET__PHYOK));
+	if (ret) {
+		dev_err(dev, "Failed to wait for phy (sstatus=0x%08x)\n",
+			readl(base + REG_SSTATUS(0)));
+		return ret;
+	}
+
+	ide = xzalloc(sizeof(*ide));
+
+	ide->port.dev = dev;
+
+	ata_ioports_init(&ide->io, base + REG_ATA_BASE, base + REG_ATA_BASE,
+			 NULL, 4);
+
+	dev->priv = ide;
+
+	ret = ide_port_register(ide);
+	if (ret)
+		free(ide);
+
+	return ret;
+}
+
+static const struct of_device_id mv_sata_dt_ids[] = {
+	{
+		.compatible = "marvell,armada-370-sata",
+	}, {
+		/* sentinel */
+	}
+};
+
+static struct driver_d mv_sata_driver = {
+	.name = "mv_sata",
+	.probe = mv_sata_probe,
+	.of_compatible = mv_sata_dt_ids,
+};
+device_platform_driver(mv_sata_driver);
diff --git a/drivers/bus/mvebu-mbus.c b/drivers/bus/mvebu-mbus.c
index df5f7a3..965a221 100644
--- a/drivers/bus/mvebu-mbus.c
+++ b/drivers/bus/mvebu-mbus.c
@@ -59,6 +59,7 @@
 #include <of.h>
 #include <of_address.h>
 #include <linux/mbus.h>
+#include <mach/common.h>
 
 /* DDR target is the same on all platforms */
 #define TARGET_DDR		0
@@ -810,7 +811,26 @@
 	return 0;
 }
 
-static int mvebu_mbus_fixup_register(void) {
+#define DOVE_REMAP_MC_REGS      0xf1800000
+
+static int mvebu_mbus_fixup_register(void)
+{
+	if (IS_ENABLED(CONFIG_ARCH_ARMADA_370) ||
+	    IS_ENABLED(CONFIG_ARCH_ARMADA_XP))
+		mvebu_mbus_add_range("marvell,armada-370-xp", 0xf0, 0x01,
+				     MVEBU_REMAP_INT_REG_BASE);
+
+	if (IS_ENABLED(CONFIG_ARCH_DOVE)) {
+		mvebu_mbus_add_range("marvell,dove", 0xf0, 0x01,
+				     MVEBU_REMAP_INT_REG_BASE);
+		mvebu_mbus_add_range("marvell,dove", 0xf0, 0x02,
+				     DOVE_REMAP_MC_REGS);
+	}
+
+	if (IS_ENABLED(CONFIG_ARCH_KIRKWOOD))
+		mvebu_mbus_add_range("marvell,kirkwood", 0xf0, 0x01,
+				     MVEBU_REMAP_INT_REG_BASE);
+
 	return of_register_fixup(mvebu_mbus_of_fixup, NULL);
 }
 pure_initcall(mvebu_mbus_fixup_register);
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 9b9e6c9..caece3b 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -270,6 +270,7 @@
 		}
 		/* FALLTHRU */
 	case STATUS_MAST_RD_DATA_ACK: /* 0x50 */
+		udelay(2);
 		if (status != STATUS_MAST_RD_DATA_ACK)
 			drv_data->action = ACTION_CONTINUE;
 		else {
@@ -280,6 +281,7 @@
 
 		if (drv_data->bytes_left == 1)
 			drv_data->cntl_bits &= ~REG_CONTROL_ACK;
+			udelay(2);
 		break;
 
 	case STATUS_MAST_RD_DATA_NO_ACK: /* 0x58 */
@@ -318,6 +320,8 @@
 	mv64xxx_i2c_prepare_for_io(drv_data, drv_data->msgs);
 	mv64xxx_write(drv_data, drv_data->cntl_bits | REG_CONTROL_START,
 		drv_data->reg_offsets.control);
+
+	udelay(2);
 }
 
 static void
@@ -333,7 +337,7 @@
 		mv64xxx_i2c_send_start(drv_data);
 
 		if (drv_data->errata_delay)
-			udelay(5);
+			udelay(3);
 
 		/*
 		 * We're never at the start of the message here, and by this
@@ -349,6 +353,7 @@
 		break;
 
 	case ACTION_SEND_ADDR_1:
+		udelay(2);
 		mv64xxx_write(drv_data, drv_data->addr1,
 			drv_data->reg_offsets.data);
 		mv64xxx_write(drv_data, drv_data->cntl_bits,
@@ -360,9 +365,11 @@
 			drv_data->reg_offsets.data);
 		mv64xxx_write(drv_data, drv_data->cntl_bits,
 			drv_data->reg_offsets.control);
+		udelay(2);
 		break;
 
 	case ACTION_SEND_DATA:
+		udelay(2);
 		mv64xxx_write(drv_data, drv_data->msg->buf[drv_data->byte_posn++],
 			drv_data->reg_offsets.data);
 		mv64xxx_write(drv_data, drv_data->cntl_bits,
@@ -383,8 +390,9 @@
 		mv64xxx_write(drv_data, drv_data->cntl_bits | REG_CONTROL_STOP,
 			drv_data->reg_offsets.control);
 		drv_data->block = false;
+		udelay(2);
 		if (drv_data->errata_delay)
-			udelay(5);
+			udelay(3);
 
 		break;
 
@@ -406,26 +414,6 @@
 	}
 }
 
-static void mv64xxx_i2c_intr(struct mv64xxx_i2c_data *drv_data)
-{
-	u32 status;
-	uint64_t start;
-
-	start = get_time_ns();
-
-	while (mv64xxx_read(drv_data, drv_data->reg_offsets.control) &
-						REG_CONTROL_IFLG) {
-		status = mv64xxx_read(drv_data, drv_data->reg_offsets.status);
-		mv64xxx_i2c_fsm(drv_data, status);
-		mv64xxx_i2c_do_action(drv_data);
-
-		if (is_timeout_non_interruptible(start, 3 * SECOND)) {
-			drv_data->rc = -EIO;
-			break;
-		}
-	}
-}
-
 /*
  *****************************************************************************
  *
@@ -436,8 +424,15 @@
 static void
 mv64xxx_i2c_wait_for_completion(struct mv64xxx_i2c_data *drv_data)
 {
+	u32 status;
 	do {
-		mv64xxx_i2c_intr(drv_data);
+		if (mv64xxx_read(drv_data, drv_data->reg_offsets.control) &
+							REG_CONTROL_IFLG) {
+			status = mv64xxx_read(drv_data,
+					      drv_data->reg_offsets.status);
+			mv64xxx_i2c_fsm(drv_data, status);
+			mv64xxx_i2c_do_action(drv_data);
+		}
 		if (drv_data->rc) {
 			drv_data->state = STATE_IDLE;
 			dev_err(&drv_data->adapter.dev, "I2C bus error\n");
diff --git a/include/filetype.h b/include/filetype.h
index 65bd6ef..709c186 100644
--- a/include/filetype.h
+++ b/include/filetype.h
@@ -38,6 +38,7 @@
 	filetype_xz_compressed,
 	filetype_mxs_bootstream,
 	filetype_socfpga_xload,
+	filetype_kwbimage_v1,
 	filetype_max,
 };
 
diff --git a/scripts/kwboot.c b/scripts/kwboot.c
index 9e4181e..4c6b19a 100644
--- a/scripts/kwboot.c
+++ b/scripts/kwboot.c
@@ -264,10 +264,11 @@
 }
 
 static int
-kwboot_bootmsg(int tty, void *msg)
+kwboot_bootmsg(int tty, void *msg, unsigned num_nacks)
 {
 	int rc;
 	char c;
+	unsigned saw_nacks = 0;
 
 	if (msg == NULL)
 		kwboot_printv("Please reboot the target into UART boot mode...");
@@ -295,11 +296,13 @@
 				kwboot_printv("\\x%02hhx", c);
 
 			rc = kwboot_tty_recv(tty, &c, 1, KWBOOT_MSG_RSP_TIMEO);
+
+			saw_nacks = 0;
 		}
 
-	} while (rc || c != NAK);
+	} while (rc || c != NAK || (++saw_nacks < num_nacks));
 
-	kwboot_printv("\nGot expected NAK\n");
+	kwboot_printv("\nGot expected NAKs\n");
 
 	return rc;
 }
@@ -603,7 +606,7 @@
 kwboot_check_image(unsigned char *img, size_t size)
 {
 	size_t i;
-	size_t header_size, image_size;
+	size_t header_size, image_size, image_offset;
 	unsigned char csum = 0;
 
 	if (size < 0x20) {
@@ -640,12 +643,20 @@
 
 	image_size = img[0x4] | (img[0x5] << 8) |
 		(img[0x6] << 16) | (img[0x7] << 24);
+	image_offset = img[0xc] | (img[0xd] << 8) |
+		(img[0xe] << 16) | (img[0xf] << 24);
 
 	header_size = (img[0x9] << 16) | img[0xa] | (img[0xb] << 8);
 
-	if (header_size + image_size != size) {
-		fprintf(stderr, "Size mismatch (%zu + %zu != %zu)\n",
-			header_size, image_size, size);
+	if (header_size > image_offset) {
+		fprintf(stderr, "Header (%zu) expands over image start (%zu)\n",
+			header_size, image_offset);
+		return 1;
+	}
+
+	if (image_offset + image_size != size) {
+		fprintf(stderr, "Image doesn't end at file end (%zu + %zu != %zu)\n",
+			image_offset, image_size, size);
 		return 1;
 	}
 
@@ -728,6 +739,7 @@
 	void *bootmsg;
 	void *debugmsg;
 	void *img;
+	unsigned num_nacks = 1;
 	size_t size;
 	speed_t speed;
 
@@ -744,7 +756,7 @@
 	kwboot_verbose = isatty(STDOUT_FILENO);
 
 	do {
-		int c = getopt(argc, argv, "b:dfhtB:D:");
+		int c = getopt(argc, argv, "b:dfhtn:B:D:");
 		if (c < 0)
 			break;
 
@@ -771,6 +783,10 @@
 			force = 1;
 			break;
 
+		case 'n':
+			num_nacks = atoi(optarg);
+			break;
+
 		case 'B':
 			speed = kwboot_tty_speed(atoi(optarg));
 			if (speed == -1)
@@ -820,7 +836,7 @@
 			goto out;
 		}
 	} else {
-		rc = kwboot_bootmsg(tty, bootmsg);
+		rc = kwboot_bootmsg(tty, bootmsg, num_nacks);
 		if (rc) {
 			perror("bootmsg");
 			goto out;