Merge https://source.denx.de/u-boot/custodians/u-boot-marvellWIP/14Jan2022

- mvebu: Move PCIe code from serdes to PCIe driver (Pali)
- mtd: nand: pxa3xx: use marvell, prefix for custom DT properties
  (Pierre)
- Add PCIe support for Iomega iConnect board (Tony)
- ddr: marvell: a38x: Misc improvements / fixes (Marek)
- tools: kwbimage: Load address fixes (Pali)
- mvebu: db-88f6720: Fix CONFIG_SPL_TEXT_BASE and remove wrong memory
  layout (Pali)
- mvebu: Replace hardcoded values 0x0030/0x4030 by proper calculation
  (Pali)

47 files changed, 920 insertions, 330 deletions

diff --git a/arch/arm/dts/armada-375.dtsi b/arch/arm/dts/armada-375.dtsi
index 62a548a..ff0ad7a 100644
--- a/arch/arm/dts/armada-375.dtsi
+++ b/arch/arm/dts/armada-375.dtsi
@@ -384,9 +384,10 @@
 				interrupts = <GIC_SPI 62 IRQ_TYPE_LEVEL_HIGH>;
 			};
 
-			system-controller@18200 {
+			systemc: system-controller@18200 {
 				compatible = "marvell,armada-375-system-controller";
 				reg = <0x18200 0x100>;
+				#reset-cells = <2>;
 			};
 
 			gateclk: clock-gating-control@18220 {
@@ -616,6 +617,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -634,6 +636,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <1>;
 				clocks = <&gateclk 6>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
diff --git a/arch/arm/dts/armada-380.dtsi b/arch/arm/dts/armada-380.dtsi
index cff1269..f3d7f4b 100644
--- a/arch/arm/dts/armada-380.dtsi
+++ b/arch/arm/dts/armada-380.dtsi
@@ -73,6 +73,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 8>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -92,6 +93,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -111,6 +113,7 @@
 				marvell,pcie-port = <2>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 6>;
+				resets = <&systemc 0 2>;
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/dts/armada-385.dtsi b/arch/arm/dts/armada-385.dtsi
index f0022d1..581a7d9 100644
--- a/arch/arm/dts/armada-385.dtsi
+++ b/arch/arm/dts/armada-385.dtsi
@@ -78,6 +78,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 8>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -97,6 +98,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -116,6 +118,7 @@
 				marvell,pcie-port = <2>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 6>;
+				resets = <&systemc 0 2>;
 				status = "disabled";
 			};
 
@@ -138,6 +141,7 @@
 				marvell,pcie-port = <3>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 7>;
+				resets = <&systemc 0 3>;
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/dts/armada-38x.dtsi b/arch/arm/dts/armada-38x.dtsi
index 72c49be..061bd78 100644
--- a/arch/arm/dts/armada-38x.dtsi
+++ b/arch/arm/dts/armada-38x.dtsi
@@ -328,6 +328,7 @@
 				compatible = "marvell,armada-380-system-controller",
 					     "marvell,armada-370-xp-system-controller";
 				reg = <0x18200 0x100>;
+				#reset-cells = <2>;
 			};
 
 			gateclk: clock-gating-control@18220 {
diff --git a/arch/arm/dts/armada-cp110.dtsi b/arch/arm/dts/armada-cp110.dtsi
index abf1e4e..ddc10d2 100644
--- a/arch/arm/dts/armada-cp110.dtsi
+++ b/arch/arm/dts/armada-cp110.dtsi
@@ -231,7 +231,7 @@
 				#address-cells = <1>;
 				#size-cells = <0>;
 				clocks = <&CP110_LABEL(syscon0) 1 2>;
-				nand-enable-arbiter;
+				marvell,nand-enable-arbiter;
 				num-cs = <1>;
 				nand-ecc-strength = <8>;
 				nand-ecc-step-size = <512>;
diff --git a/arch/arm/dts/armada-xp-98dx3236.dtsi b/arch/arm/dts/armada-xp-98dx3236.dtsi
index 5df1d18..1a48ff3 100644
--- a/arch/arm/dts/armada-xp-98dx3236.dtsi
+++ b/arch/arm/dts/armada-xp-98dx3236.dtsi
@@ -85,6 +85,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 		};
@@ -136,6 +137,7 @@
 			systemc: system-controller@18200 {
 				compatible = "marvell,armada-370-xp-system-controller";
 				reg = <0x18200 0x500>;
+				#reset-cells = <2>;
 			};
 
 			gateclk: clock-gating-control@18220 {
diff --git a/arch/arm/dts/armada-xp-mv78230.dtsi b/arch/arm/dts/armada-xp-mv78230.dtsi
index 8558bf6..63d7f48 100644
--- a/arch/arm/dts/armada-xp-mv78230.dtsi
+++ b/arch/arm/dts/armada-xp-mv78230.dtsi
@@ -92,6 +92,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -110,6 +111,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <1>;
 				clocks = <&gateclk 6>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -128,6 +130,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <2>;
 				clocks = <&gateclk 7>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -146,6 +149,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <3>;
 				clocks = <&gateclk 8>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -164,6 +168,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 9>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/dts/armada-xp-mv78260.dtsi b/arch/arm/dts/armada-xp-mv78260.dtsi
index 2d85fe8..5dc413d 100644
--- a/arch/arm/dts/armada-xp-mv78260.dtsi
+++ b/arch/arm/dts/armada-xp-mv78260.dtsi
@@ -107,6 +107,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -125,6 +126,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <1>;
 				clocks = <&gateclk 6>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -143,6 +145,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <2>;
 				clocks = <&gateclk 7>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -161,6 +164,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <3>;
 				clocks = <&gateclk 8>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -179,6 +183,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 9>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -197,6 +202,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <1>;
 				clocks = <&gateclk 10>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -215,6 +221,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <2>;
 				clocks = <&gateclk 11>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -233,6 +240,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <3>;
 				clocks = <&gateclk 12>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -251,6 +259,7 @@
 				marvell,pcie-port = <2>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 26>;
+				resets = <&systemc 0 2>;
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/dts/armada-xp-mv78460.dtsi b/arch/arm/dts/armada-xp-mv78460.dtsi
index 230a3fd..6fbd0ce 100644
--- a/arch/arm/dts/armada-xp-mv78460.dtsi
+++ b/arch/arm/dts/armada-xp-mv78460.dtsi
@@ -128,6 +128,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 5>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -146,6 +147,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <1>;
 				clocks = <&gateclk 6>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -164,6 +166,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <2>;
 				clocks = <&gateclk 7>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -182,6 +185,7 @@
 				marvell,pcie-port = <0>;
 				marvell,pcie-lane = <3>;
 				clocks = <&gateclk 8>;
+				resets = <&systemc 0 0>;
 				status = "disabled";
 			};
 
@@ -200,6 +204,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 9>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -218,6 +223,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <1>;
 				clocks = <&gateclk 10>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -236,6 +242,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <2>;
 				clocks = <&gateclk 11>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -254,6 +261,7 @@
 				marvell,pcie-port = <1>;
 				marvell,pcie-lane = <3>;
 				clocks = <&gateclk 12>;
+				resets = <&systemc 0 1>;
 				status = "disabled";
 			};
 
@@ -272,6 +280,7 @@
 				marvell,pcie-port = <2>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 26>;
+				resets = <&systemc 0 2>;
 				status = "disabled";
 			};
 
@@ -290,6 +299,7 @@
 				marvell,pcie-port = <3>;
 				marvell,pcie-lane = <0>;
 				clocks = <&gateclk 27>;
+				resets = <&systemc 0 3>;
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/dts/armada-xp-synology-ds414.dts b/arch/arm/dts/armada-xp-synology-ds414.dts
index 861967c..35909e3 100644
--- a/arch/arm/dts/armada-xp-synology-ds414.dts
+++ b/arch/arm/dts/armada-xp-synology-ds414.dts
@@ -187,6 +187,7 @@
 	pcie@1,0 {
 		/* Port 0, Lane 0 */
 		status = "okay";
+		num-lanes = <4>;
 	};
 
 	/*
diff --git a/arch/arm/dts/armada-xp-theadorable.dts b/arch/arm/dts/armada-xp-theadorable.dts
index 24cc1cc..a06a65a 100644
--- a/arch/arm/dts/armada-xp-theadorable.dts
+++ b/arch/arm/dts/armada-xp-theadorable.dts
@@ -214,5 +214,6 @@
 	pcie@9,0 {
 		/* Port 2, Lane 0 */
 		status = "okay";
+		num-lanes = <4>;
 	};
 };
diff --git a/arch/arm/dts/armada-xp.dtsi b/arch/arm/dts/armada-xp.dtsi
index d856d96..fb5640b 100644
--- a/arch/arm/dts/armada-xp.dtsi
+++ b/arch/arm/dts/armada-xp.dtsi
@@ -78,6 +78,7 @@
 			systemc: system-controller@18200 {
 				compatible = "marvell,armada-370-xp-system-controller";
 				reg = <0x18200 0x500>;
+				#reset-cells = <2>;
 			};
 
 			gateclk: clock-gating-control@18220 {
diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index 7e9c206..a5a2087 100644
--- a/arch/arm/mach-mvebu/Makefile
+++ b/arch/arm/mach-mvebu/Makefile
@@ -21,6 +21,7 @@ else # CONFIG_ARCH_KIRKWOOD
 
 obj-y	= cpu.o
 obj-y	+= dram.o
+obj-$(CONFIG_DM_RESET) += system-controller.o
 ifndef CONFIG_SPL_BUILD
 obj-$(CONFIG_ARMADA_375) += ../../../drivers/ddr/marvell/axp/xor.o
 obj-$(CONFIG_ARMADA_38X) += ../../../drivers/ddr/marvell/a38x/xor.o
@@ -30,6 +31,17 @@ obj-$(CONFIG_MVEBU_EFUSE) += efuse.o
 
 extra-y += kwbimage.cfg
 
+ifneq ($(CONFIG_ARMADA_370)$(CONFIG_ARMADA_XP),)
+	KWB_REPLACE += CPU
+	KWB_CFG_CPU = SHEEVA
+else ifneq ($(CONFIG_ARMADA_375)$(CONFIG_ARMADA_38X)$(CONFIG_ARMADA_39X),)
+	KWB_REPLACE += CPU
+	KWB_CFG_CPU = A9
+endif
+
+KWB_REPLACE += LOAD_ADDRESS
+KWB_CFG_LOAD_ADDRESS = $(CONFIG_SPL_TEXT_BASE)
+
 KWB_REPLACE += BOOT_FROM
 ifneq ($(CONFIG_MVEBU_SPL_BOOT_DEVICE_SPI),)
 	KWB_CFG_BOOT_FROM=spi
@@ -58,10 +70,20 @@ KWB_REPLACE += SEC_FUSE_DUMP
 KWB_CFG_SEC_FUSE_DUMP = a38x
 endif
 
+ifdef CONFIG_ARMADA_38X
+# BootROM output is by default enabled on pre-A38x and disabled on A38x
+# DEBUG flag on A38x for non-UART boot source only enable BootROM output and nothing more
+KWB_REPLACE += DEBUG
+KWB_CFG_DEBUG = 1
+endif
+
+quiet_cmd_kwbcfg = KWBCFG  $@
+cmd_kwbcfg = sed -ne '$(foreach V,$(KWB_REPLACE),s/\#@$(V)/$(V) $(KWB_CFG_$(V))/;)p' \
+	<$< >$(dir $@)$(@F)
+
 $(obj)/kwbimage.cfg: $(src)/kwbimage.cfg.in include/autoconf.mk \
 		include/config/auto.conf
-	$(Q)sed -ne '$(foreach V,$(KWB_REPLACE),s/^#@$(V)/$(V) $(KWB_CFG_$(V))/;)p' \
-	<$< >$(dir $@)$(@F)
+	$(call cmd,kwbcfg)
 
 endif # CONFIG_SPL_BUILD
 obj-y	+= gpio.o
diff --git a/arch/arm/mach-mvebu/include/mach/cpu.h b/arch/arm/mach-mvebu/include/mach/cpu.h
index a7a62c7..b99d86a 100644
--- a/arch/arm/mach-mvebu/include/mach/cpu.h
+++ b/arch/arm/mach-mvebu/include/mach/cpu.h
@@ -74,10 +74,11 @@ enum {
 /*
  * Default Device Address MAP BAR values
  */
+#define MBUS_PCI_MAX_PORTS	6
 #define MBUS_PCI_MEM_BASE	MVEBU_SDRAM_SIZE_MAX
-#define MBUS_PCI_MEM_SIZE	(128 << 20)
+#define MBUS_PCI_MEM_SIZE	((MBUS_PCI_MAX_PORTS * 128) << 20)
 #define MBUS_PCI_IO_BASE	0xF1100000
-#define MBUS_PCI_IO_SIZE	(64 << 10)
+#define MBUS_PCI_IO_SIZE	((MBUS_PCI_MAX_PORTS * 64) << 10)
 #define MBUS_SPI_BASE		0xF4000000
 #define MBUS_SPI_SIZE		(8 << 20)
 #define MBUS_DFX_BASE		0xF6000000
diff --git a/arch/arm/mach-mvebu/kwbimage.cfg.in b/arch/arm/mach-mvebu/kwbimage.cfg.in
index 049d23c..ccb0997 100644
--- a/arch/arm/mach-mvebu/kwbimage.cfg.in
+++ b/arch/arm/mach-mvebu/kwbimage.cfg.in
@@ -5,8 +5,14 @@
 # Armada 38x uses version 1 image format
 VERSION		1
 
+# Type of the CPU core
+#@CPU
+
 # Boot Media configurations
 #@BOOT_FROM
 
-# Binary Header (bin_hdr) with DDR3 training code
-BINARY spl/u-boot-spl.bin
+# Enable BootROM output via DEBUG flag on SoCs which require it
+#@DEBUG
+
+# Include U-Boot SPL with DDR3 training code into Binary Header
+BINARY spl/u-boot-spl.bin #@LOAD_ADDRESS
diff --git a/arch/arm/mach-mvebu/serdes/a38x/Makefile b/arch/arm/mach-mvebu/serdes/a38x/Makefile
index 917fc13..5a70b37 100644
--- a/arch/arm/mach-mvebu/serdes/a38x/Makefile
+++ b/arch/arm/mach-mvebu/serdes/a38x/Makefile
@@ -1,6 +1,5 @@
 # SPDX-License-Identifier: GPL-2.0+
 
-obj-$(CONFIG_SPL_BUILD)	= ctrl_pex.o
 obj-$(CONFIG_SPL_BUILD)	+= high_speed_env_spec.o
 obj-$(CONFIG_SPL_BUILD)	+= high_speed_env_spec-38x.o
 obj-$(CONFIG_SPL_BUILD)	+= seq_exec.o
diff --git a/arch/arm/mach-mvebu/serdes/a38x/ctrl_pex.c b/arch/arm/mach-mvebu/serdes/a38x/ctrl_pex.c
deleted file mode 100644
index 55c3f9c..0000000
--- a/arch/arm/mach-mvebu/serdes/a38x/ctrl_pex.c
+++ /dev/null
@@ -1,64 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0
-/*
- * Copyright (C) Marvell International Ltd. and its affiliates
- */
-
-#include <common.h>
-#include <spl.h>
-#include <asm/io.h>
-#include <asm/arch/cpu.h>
-#include <asm/arch/soc.h>
-#include <linux/bitops.h>
-#include <linux/delay.h>
-
-#include "ctrl_pex.h"
-#include "sys_env_lib.h"
-
-__weak void board_pex_config(void)
-{
-	/* nothing in this weak default implementation */
-}
-
-int hws_pex_config(const struct serdes_map *serdes_map, u8 count)
-{
-	enum serdes_type serdes_type;
-	u32 idx, tmp;
-
-	DEBUG_INIT_FULL_S("\n### hws_pex_config ###\n");
-
-	tmp = reg_read(SOC_CONTROL_REG1);
-	tmp &= ~0x03;
-
-	for (idx = 0; idx < count; idx++) {
-		serdes_type = serdes_map[idx].serdes_type;
-		if ((serdes_type != PEX0) &&
-		    ((serdes_map[idx].serdes_mode == PEX_ROOT_COMPLEX_X4) ||
-		     (serdes_map[idx].serdes_mode == PEX_END_POINT_X4))) {
-			/* for PEX by4 - relevant for the first port only */
-			continue;
-		}
-
-		switch (serdes_type) {
-		case PEX0:
-			tmp |= 0x1 << PCIE0_ENABLE_OFFS;
-			break;
-		case PEX1:
-			tmp |= 0x1 << PCIE1_ENABLE_OFFS;
-			break;
-		case PEX2:
-			tmp |= 0x1 << PCIE2_ENABLE_OFFS;
-			break;
-		case PEX3:
-			tmp |= 0x1 << PCIE3_ENABLE_OFFS;
-			break;
-		default:
-			break;
-		}
-	}
-
-	reg_write(SOC_CONTROL_REG1, tmp);
-
-	board_pex_config();
-
-	return MV_OK;
-}
diff --git a/arch/arm/mach-mvebu/serdes/a38x/ctrl_pex.h b/arch/arm/mach-mvebu/serdes/a38x/ctrl_pex.h
deleted file mode 100644
index 64193d5..0000000
--- a/arch/arm/mach-mvebu/serdes/a38x/ctrl_pex.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0 */
-/*
- * Copyright (C) Marvell International Ltd. and its affiliates
- */
-
-#ifndef _CTRL_PEX_H
-#define _CTRL_PEX_H
-
-#include <pci.h>
-#include "high_speed_env_spec.h"
-
-/* Direct access to PEX0 Root Port's PCIe Capability structure */
-#define PEX0_RP_PCIE_CFG_OFFSET		(0x00080000 + 0x60)
-
-/* SOC_CONTROL_REG1 fields */
-#define PCIE0_ENABLE_OFFS		0
-#define PCIE0_ENABLE_MASK		(0x1 << PCIE0_ENABLE_OFFS)
-#define PCIE1_ENABLE_OFFS		1
-#define PCIE1_ENABLE_MASK		(0x1 << PCIE1_ENABLE_OFFS)
-#define PCIE2_ENABLE_OFFS		2
-#define PCIE2_ENABLE_MASK		(0x1 << PCIE2_ENABLE_OFFS)
-#define PCIE3_ENABLE_OFFS		3
-#define PCIE4_ENABLE_MASK		(0x1 << PCIE3_ENABLE_OFFS)
-
-int hws_pex_config(const struct serdes_map *serdes_map, u8 count);
-void board_pex_config(void);
-
-#endif
diff --git a/arch/arm/mach-mvebu/serdes/a38x/high_speed_env_spec.c b/arch/arm/mach-mvebu/serdes/a38x/high_speed_env_spec.c
index 9ba60b5..2e467b5 100644
--- a/arch/arm/mach-mvebu/serdes/a38x/high_speed_env_spec.c
+++ b/arch/arm/mach-mvebu/serdes/a38x/high_speed_env_spec.c
@@ -12,7 +12,6 @@
 
 #include "high_speed_env_spec.h"
 #include "sys_env_lib.h"
-#include "ctrl_pex.h"
 
 /*
  * serdes_seq_db - holds all serdes sequences, their size and the
@@ -1555,9 +1554,6 @@ int hws_power_up_serdes_lanes(struct serdes_map *serdes_map, u8 count)
 		   After finish the Power_up sequence for all lanes,
 		   the lanes should be released from reset state.       */
 		CHECK_STATUS(hws_pex_tx_config_seq(serdes_map, count));
-
-		/* PEX configuration */
-		CHECK_STATUS(hws_pex_config(serdes_map, count));
 	}
 
 	/* USB2 configuration */
@@ -1743,21 +1739,6 @@ int serdes_power_up_ctrl(u32 serdes_num, int serdes_power_up,
 				else
 					reg_data &= ~0x4000;
 				reg_write(SOC_CONTROL_REG1, reg_data);
-
-				/*
-				 * Set Maximum Link Width to X1 or X4 in Root
-				 * Port's PCIe Link Capability register.
-				 * This register is read-only but if is not set
-				 * correctly then access to PCI config space of
-				 * endpoint card behind this Root Port does not
-				 * work.
-				 */
-				reg_data = reg_read(PEX0_RP_PCIE_CFG_OFFSET +
-						    PCI_EXP_LNKCAP);
-				reg_data &= ~PCI_EXP_LNKCAP_MLW;
-				reg_data |= (is_pex_by1 ? 1 : 4) << 4;
-				reg_write(PEX0_RP_PCIE_CFG_OFFSET +
-					  PCI_EXP_LNKCAP, reg_data);
 			}
 
 			CHECK_STATUS(mv_seq_exec(serdes_num, PEX_POWER_UP_SEQ));
diff --git a/arch/arm/mach-mvebu/system-controller.c b/arch/arm/mach-mvebu/system-controller.c
new file mode 100644
index 0000000..ea858b2
--- /dev/null
+++ b/arch/arm/mach-mvebu/system-controller.c
@@ -0,0 +1,105 @@
+// SPDX-License-Identifier: GPL-2.0+
+// (C) 2021 Pali Rohár <pali@kernel.org>
+
+#include <common.h>
+#include <dm.h>
+#include <reset-uclass.h>
+#include <asm/io.h>
+
+#define MVEBU_SOC_CONTROL_1_REG 0x4
+
+#define MVEBU_PCIE_ID 0
+
+struct mvebu_reset_data {
+	void *base;
+};
+
+static int mvebu_reset_of_xlate(struct reset_ctl *rst,
+				struct ofnode_phandle_args *args)
+{
+	if (args->args_count < 2)
+		return -EINVAL;
+
+	rst->id = args->args[0];
+	rst->data = args->args[1];
+
+	/* Currently only PCIe is implemented */
+	if (rst->id != MVEBU_PCIE_ID)
+		return -EINVAL;
+
+	/* Four PCIe enable bits are shared across more PCIe links */
+	if (!(rst->data >= 0 && rst->data <= 3))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int mvebu_reset_request(struct reset_ctl *rst)
+{
+	return 0;
+}
+
+static int mvebu_reset_free(struct reset_ctl *rst)
+{
+	return 0;
+}
+
+static int mvebu_reset_assert(struct reset_ctl *rst)
+{
+	struct mvebu_reset_data *data = dev_get_priv(rst->dev);
+
+	clrbits_32(data->base + MVEBU_SOC_CONTROL_1_REG, BIT(rst->data));
+	return 0;
+}
+
+static int mvebu_reset_deassert(struct reset_ctl *rst)
+{
+	struct mvebu_reset_data *data = dev_get_priv(rst->dev);
+
+	setbits_32(data->base + MVEBU_SOC_CONTROL_1_REG, BIT(rst->data));
+	return 0;
+}
+
+static int mvebu_reset_status(struct reset_ctl *rst)
+{
+	struct mvebu_reset_data *data = dev_get_priv(rst->dev);
+
+	return !(readl(data->base + MVEBU_SOC_CONTROL_1_REG) & BIT(rst->data));
+}
+
+static int mvebu_reset_of_to_plat(struct udevice *dev)
+{
+	struct mvebu_reset_data *data = dev_get_priv(dev);
+
+	data->base = (void *)dev_read_addr(dev);
+	if ((fdt_addr_t)data->base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct udevice_id mvebu_reset_of_match[] = {
+	{ .compatible = "marvell,armada-370-xp-system-controller" },
+	{ .compatible = "marvell,armada-375-system-controller" },
+	{ .compatible = "marvell,armada-380-system-controller" },
+	{ .compatible = "marvell,armada-390-system-controller" },
+	{ },
+};
+
+static struct reset_ops mvebu_reset_ops = {
+	.of_xlate = mvebu_reset_of_xlate,
+	.request = mvebu_reset_request,
+	.rfree = mvebu_reset_free,
+	.rst_assert = mvebu_reset_assert,
+	.rst_deassert = mvebu_reset_deassert,
+	.rst_status = mvebu_reset_status,
+};
+
+U_BOOT_DRIVER(mvebu_reset) = {
+	.name = "mvebu-reset",
+	.id = UCLASS_RESET,
+	.of_match = mvebu_reset_of_match,
+	.of_to_plat = mvebu_reset_of_to_plat,
+	.priv_auto = sizeof(struct mvebu_reset_data),
+	.ops = &mvebu_reset_ops,
+};
diff --git a/board/gdsys/a38x/MAINTAINERS b/board/gdsys/a38x/MAINTAINERS
index d31e675..6492e79 100644
--- a/board/gdsys/a38x/MAINTAINERS
+++ b/board/gdsys/a38x/MAINTAINERS
@@ -1,5 +1,4 @@
 A38X BOARD
-M:	Dirk Eibach <dirk.eibach@gdsys.cc>
 M:	Mario Six <mario.six@gdsys.cc>
 S:	Maintained
 F:	board/gdsys/a38x/
diff --git a/board/gdsys/a38x/controlcenterdc.c b/board/gdsys/a38x/controlcenterdc.c
index dc424f2..7d65400 100644
--- a/board/gdsys/a38x/controlcenterdc.c
+++ b/board/gdsys/a38x/controlcenterdc.c
@@ -94,12 +94,16 @@ int hws_board_topology_load(struct serdes_map **serdes_map_array, u8 *count)
 	return 0;
 }
 
-void board_pex_config(void)
+void spl_board_init(void)
 {
 #ifdef CONFIG_SPL_BUILD
 	uint k;
 	struct gpio_desc gpio = {};
 
+	/* Enable PCIe link 2 */
+	setbits_32(MVEBU_REGISTER(0x18204), BIT(2));
+	mdelay(10);
+
 	if (!request_gpio_by_name(&gpio, "pca9698@22", 31, "fpga-program-gpio")) {
 		/* prepare FPGA reconfiguration */
 		dm_gpio_set_dir_flags(&gpio, GPIOD_IS_OUT);
diff --git a/board/gdsys/mpc8308/MAINTAINERS b/board/gdsys/mpc8308/MAINTAINERS
index dc0b389..57faba4 100644
--- a/board/gdsys/mpc8308/MAINTAINERS
+++ b/board/gdsys/mpc8308/MAINTAINERS
@@ -1,5 +1,5 @@
 MPC8308 BOARD
-M:	Dirk Eibach <dirk.eibach@gdsys.cc>
+M:	Mario Six <mario.six@gdsys.cc>
 S:	Maintained
 F:	board/gdsys/mpc8308/
 F:	include/configs/gazerbeam.h
diff --git a/board/iomega/iconnect/MAINTAINERS b/board/iomega/iconnect/MAINTAINERS
index 167cf07..a1b018e 100644
--- a/board/iomega/iconnect/MAINTAINERS
+++ b/board/iomega/iconnect/MAINTAINERS
@@ -1,4 +1,5 @@
 ICONNECT BOARD
+M:      Tony Dinh <mibodhi@gmail.com>
 M:	Luka Perkov <luka@openwrt.org>
 S:	Maintained
 F:	board/iomega/iconnect/
diff --git a/board/iomega/iconnect/iconnect.c b/board/iomega/iconnect/iconnect.c
index fe4be28..9e123aa 100644
--- a/board/iomega/iconnect/iconnect.c
+++ b/board/iomega/iconnect/iconnect.c
@@ -92,3 +92,10 @@ int board_init(void)
 
 	return 0;
 }
+
+int board_late_init(void)
+{
+	/* Do late init to ensure successful enumeration of PCIe devices */
+	pci_init();
+	return 0;
+}
diff --git a/configs/controlcenterdc_defconfig b/configs/controlcenterdc_defconfig
index d6844a4..b6eb955 100644
--- a/configs/controlcenterdc_defconfig
+++ b/configs/controlcenterdc_defconfig
@@ -33,6 +33,7 @@ CONFIG_SYS_CONSOLE_INFO_QUIET=y
 CONFIG_DISPLAY_BOARDINFO_LATE=y
 CONFIG_BOARD_LATE_INIT=y
 CONFIG_LAST_STAGE_INIT=y
+CONFIG_SPL_BOARD_INIT=y
 CONFIG_SPL_SYS_MALLOC_SIMPLE=y
 CONFIG_SPL_I2C=y
 CONFIG_HUSH_PARSER=y
diff --git a/configs/db-88f6720_defconfig b/configs/db-88f6720_defconfig
index 32a7349..2e8f4c0 100644
--- a/configs/db-88f6720_defconfig
+++ b/configs/db-88f6720_defconfig
@@ -11,7 +11,7 @@ CONFIG_ENV_SIZE=0x10000
 CONFIG_ENV_OFFSET=0x100000
 CONFIG_ENV_SECT_SIZE=0x10000
 CONFIG_DEFAULT_DEVICE_TREE="armada-375-db"
-CONFIG_SPL_TEXT_BASE=0x40004030
+CONFIG_SPL_TEXT_BASE=0x40000030
 CONFIG_SPL_SERIAL=y
 CONFIG_SPL=y
 CONFIG_DEBUG_UART_BASE=0xf1012000
diff --git a/configs/iconnect_defconfig b/configs/iconnect_defconfig
index 605b98b..1c9c77a 100644
--- a/configs/iconnect_defconfig
+++ b/configs/iconnect_defconfig
@@ -2,6 +2,7 @@ CONFIG_ARM=y
 CONFIG_SKIP_LOWLEVEL_INIT=y
 CONFIG_SYS_DCACHE_OFF=y
 CONFIG_ARCH_CPU_INIT=y
+CONFIG_SYS_THUMB_BUILD=y
 CONFIG_ARCH_KIRKWOOD=y
 CONFIG_SYS_KWD_CONFIG="board/iomega/iconnect/kwbimage.cfg"
 CONFIG_SYS_TEXT_BASE=0x600000
@@ -18,9 +19,11 @@ CONFIG_USE_BOOTCOMMAND=y
 CONFIG_BOOTCOMMAND="setenv bootargs ${console} ${mtdparts} ${bootargs_root}; ubi part rootfs; ubifsmount ubi:rootfs; ubifsload 0x800000 ${kernel}; bootm 0x800000"
 CONFIG_USE_PREBOOT=y
 # CONFIG_DISPLAY_BOARDINFO is not set
+CONFIG_BOARD_LATE_INIT=y
 CONFIG_SYS_PROMPT="iconnect => "
 # CONFIG_CMD_FLASH is not set
 CONFIG_CMD_NAND=y
+CONFIG_CMD_PCI=y
 CONFIG_CMD_USB=y
 # CONFIG_CMD_SETEXPR is not set
 CONFIG_CMD_MII=y
@@ -43,6 +46,8 @@ CONFIG_MTD=y
 CONFIG_MTD_RAW_NAND=y
 CONFIG_MVGBE=y
 CONFIG_MII=y
+CONFIG_PCI=y
+CONFIG_PCI_MVEBU=y
 CONFIG_SYS_NS16550=y
 CONFIG_USB=y
 CONFIG_USB_EHCI_HCD=y
diff --git a/drivers/ddr/marvell/a38x/ddr3_training_centralization.c b/drivers/ddr/marvell/a38x/ddr3_training_centralization.c
index 648b37e..42308b6 100644
--- a/drivers/ddr/marvell/a38x/ddr3_training_centralization.c
+++ b/drivers/ddr/marvell/a38x/ddr3_training_centralization.c
@@ -55,6 +55,7 @@ static int ddr3_tip_centralization(u32 dev_num, u32 mode)
 	enum hws_training_ip_stat training_result[MAX_INTERFACE_NUM];
 	u32 if_id, pattern_id, bit_id;
 	u8 bus_id;
+	u8 current_byte_status;
 	u8 cur_start_win[BUS_WIDTH_IN_BITS];
 	u8 centralization_result[MAX_INTERFACE_NUM][BUS_WIDTH_IN_BITS];
 	u8 cur_end_win[BUS_WIDTH_IN_BITS];
@@ -166,6 +167,10 @@ static int ddr3_tip_centralization(u32 dev_num, u32 mode)
 						  result[search_dir_id][7]));
 				}
 
+				current_byte_status =
+					mv_ddr_tip_sub_phy_byte_status_get(if_id,
+									   bus_id);
+
 				for (bit_id = 0; bit_id < BUS_WIDTH_IN_BITS;
 				     bit_id++) {
 					/* check if this code is valid for 2 edge, probably not :( */
@@ -174,11 +179,32 @@ static int ddr3_tip_centralization(u32 dev_num, u32 mode)
 							       [HWS_LOW2HIGH]
 							       [bit_id],
 							       EDGE_1);
+					if (current_byte_status &
+					    BYTE_SPLIT_OUT_MIX) {
+						if (cur_start_win[bit_id] >= 64)
+							cur_start_win[bit_id] -= 64;
+						else
+							cur_start_win[bit_id] = 0;
+						DEBUG_CENTRALIZATION_ENGINE
+							(DEBUG_LEVEL_INFO,
+							 ("pattern %d IF %d pup %d bit %d subtract 64 adll from start\n",
+							  pattern_id, if_id, bus_id, bit_id));
+					}
 					cur_end_win[bit_id] =
 						GET_TAP_RESULT(result
 							       [HWS_HIGH2LOW]
 							       [bit_id],
 							       EDGE_1);
+					if (cur_end_win[bit_id] >= 64 &&
+					    (current_byte_status &
+					     BYTE_SPLIT_OUT_MIX)) {
+						cur_end_win[bit_id] -= 64;
+						DEBUG_CENTRALIZATION_ENGINE
+							(DEBUG_LEVEL_INFO,
+							 ("pattern %d IF %d pup %d bit %d subtract 64 adll from end\n",
+							  pattern_id, if_id, bus_id, bit_id));
+					}
+
 					/* window length */
 					current_window[bit_id] =
 						cur_end_win[bit_id] -
diff --git a/drivers/ddr/marvell/a38x/mv_ddr_plat.c b/drivers/ddr/marvell/a38x/mv_ddr_plat.c
index faafc86..7c7bce7 100644
--- a/drivers/ddr/marvell/a38x/mv_ddr_plat.c
+++ b/drivers/ddr/marvell/a38x/mv_ddr_plat.c
@@ -167,8 +167,6 @@ static u16 a38x_vco_freq_per_sar_ref_clk_40_mhz[] = {
 };
 
 
-static u32 async_mode_at_tf;
-
 static u32 dq_bit_map_2_phy_pin[] = {
 	1, 0, 2, 6, 9, 8, 3, 7,	/* 0 */
 	8, 9, 1, 7, 2, 6, 3, 0,	/* 1 */
@@ -734,7 +732,8 @@ static int ddr3_tip_a38x_set_divider(u8 dev_num, u32 if_id,
 	u32 divider = 0;
 	u32 sar_val, ref_clk_satr;
 	u32 async_val;
-	u32 freq = mv_ddr_freq_get(frequency);
+	u32 cpu_freq;
+	u32 ddr_freq = mv_ddr_freq_get(frequency);
 
 	if (if_id != 0) {
 		DEBUG_TRAINING_ACCESS(DEBUG_LEVEL_ERROR,
@@ -751,11 +750,14 @@ static int ddr3_tip_a38x_set_divider(u8 dev_num, u32 if_id,
 	ref_clk_satr = reg_read(DEVICE_SAMPLE_AT_RESET2_REG);
 	if (((ref_clk_satr >> DEVICE_SAMPLE_AT_RESET2_REG_REFCLK_OFFSET) & 0x1) ==
 	    DEVICE_SAMPLE_AT_RESET2_REG_REFCLK_25MHZ)
-		divider = a38x_vco_freq_per_sar_ref_clk_25_mhz[sar_val] / freq;
+		cpu_freq = a38x_vco_freq_per_sar_ref_clk_25_mhz[sar_val];
 	else
-		divider = a38x_vco_freq_per_sar_ref_clk_40_mhz[sar_val] / freq;
+		cpu_freq = a38x_vco_freq_per_sar_ref_clk_40_mhz[sar_val];
+
+	divider = cpu_freq / ddr_freq;
 
-	if ((async_mode_at_tf == 1) && (freq > 400)) {
+	if (((cpu_freq % ddr_freq != 0) || (divider != 2 && divider != 3)) &&
+	    (ddr_freq > 400)) {
 		/* Set async mode */
 		dunit_write(0x20220, 0x1000, 0x1000);
 		dunit_write(0xe42f4, 0x200, 0x200);
@@ -869,8 +871,6 @@ int ddr3_tip_ext_write(u32 dev_num, u32 if_id, u32 reg_addr,
 
 int mv_ddr_early_init(void)
 {
-	struct mv_ddr_topology_map *tm = mv_ddr_topology_map_get();
-
 	/* FIXME: change this configuration per ddr type
 	 * configure a380 and a390 to work with receiver odt timing
 	 * the odt_config is defined:
@@ -882,9 +882,6 @@ int mv_ddr_early_init(void)
 
 	mv_ddr_sw_db_init(0, 0);
 
-	if (tm->interface_params[0].memory_freq != MV_DDR_FREQ_SAR)
-		async_mode_at_tf = 1;
-
 	return MV_OK;
 }
 
diff --git a/drivers/mtd/nand/raw/pxa3xx_nand.c b/drivers/mtd/nand/raw/pxa3xx_nand.c
index 8ff58a7..3a9c9ca 100644
--- a/drivers/mtd/nand/raw/pxa3xx_nand.c
+++ b/drivers/mtd/nand/raw/pxa3xx_nand.c
@@ -1862,10 +1862,10 @@ static int pxa3xx_nand_probe_dt(struct udevice *dev, struct pxa3xx_nand_info *in
 		return -EINVAL;
 	}
 
-	if (dev_read_bool(dev, "nand-enable-arbiter"))
+	if (dev_read_bool(dev, "marvell,nand-enable-arbiter"))
 		pdata->enable_arbiter = 1;
 
-	if (dev_read_bool(dev, "nand-keep-config"))
+	if (dev_read_bool(dev, "marvell,nand-keep-config"))
 		pdata->keep_config = 1;
 
 	/*
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index 42f8cb6..630d6e6 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -265,6 +265,7 @@ config PCI_MVEBU
 	bool "Enable Armada XP/38x PCIe driver"
 	depends on ARCH_MVEBU
 	select MISC
+	select DM_RESET
 	help
 	  Say Y here if you want to enable PCIe controller support on
 	  Armada XP/38x SoCs.
diff --git a/drivers/pci/pci_mvebu.c b/drivers/pci/pci_mvebu.c
index 18f79d2..b3ea034 100644
--- a/drivers/pci/pci_mvebu.c
+++ b/drivers/pci/pci_mvebu.c
@@ -18,13 +18,16 @@
 #include <dm/lists.h>
 #include <dm/of_access.h>
 #include <pci.h>
+#include <reset.h>
 #include <asm/io.h>
 #include <asm/arch/cpu.h>
 #include <asm/arch/soc.h>
 #include <linux/bitops.h>
+#include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/ioport.h>
 #include <linux/mbus.h>
+#include <linux/sizes.h>
 
 /* PCIe unit register offsets */
 #define SELECT(x, n)			((x >> n) & 1UL)
@@ -59,6 +62,9 @@
 #define PCIE_DEBUG_CTRL			0x1a60
 #define  PCIE_DEBUG_SOFT_RESET		BIT(20)
 
+#define LINK_WAIT_RETRIES	100
+#define LINK_WAIT_TIMEOUT	1000
+
 struct mvebu_pcie {
 	struct pci_controller hose;
 	void __iomem *base;
@@ -66,8 +72,10 @@ struct mvebu_pcie {
 	struct resource mem;
 	void __iomem *iobase;
 	struct resource io;
+	u32 intregs;
 	u32 port;
 	u32 lane;
+	bool is_x4;
 	int devfn;
 	u32 lane_mask;
 	int first_busno;
@@ -80,14 +88,6 @@ struct mvebu_pcie {
 	u32 cfgcache[(0x3c - 0x10) / 4];
 };
 
-/*
- * MVEBU PCIe controller needs MEMORY and I/O BARs to be mapped
- * into SoCs address space. Each controller will map 128M of MEM
- * and 64K of I/O space when registered.
- */
-static void __iomem *mvebu_pcie_membase = (void __iomem *)MBUS_PCI_MEM_BASE;
-static void __iomem *mvebu_pcie_iobase = (void __iomem *)MBUS_PCI_IO_BASE;
-
 static inline bool mvebu_pcie_link_up(struct mvebu_pcie *pcie)
 {
 	u32 val;
@@ -95,6 +95,23 @@ static inline bool mvebu_pcie_link_up(struct mvebu_pcie *pcie)
 	return !(val & PCIE_STAT_LINK_DOWN);
 }
 
+static void mvebu_pcie_wait_for_link(struct mvebu_pcie *pcie)
+{
+	int retries;
+
+	/* check if the link is up or not */
+	for (retries = 0; retries < LINK_WAIT_RETRIES; retries++) {
+		if (mvebu_pcie_link_up(pcie)) {
+			printf("%s: Link up\n", pcie->name);
+			return;
+		}
+
+		udelay(LINK_WAIT_TIMEOUT);
+	}
+
+	printf("%s: Link down\n", pcie->name);
+}
+
 static void mvebu_pcie_set_local_bus_nr(struct mvebu_pcie *pcie, int busno)
 {
 	u32 stat;
@@ -357,15 +374,43 @@ static void mvebu_pcie_setup_wins(struct mvebu_pcie *pcie)
 	       pcie->base + PCIE_BAR_CTRL_OFF(1));
 
 	/* Setup BAR[0] to internal registers. */
-	writel(SOC_REGS_PHY_BASE, pcie->base + PCIE_BAR_LO_OFF(0));
+	writel(pcie->intregs, pcie->base + PCIE_BAR_LO_OFF(0));
 	writel(0, pcie->base + PCIE_BAR_HI_OFF(0));
 }
 
-static int mvebu_pcie_probe(struct udevice *dev)
+/* Only enable PCIe link, do not setup it */
+static int mvebu_pcie_enable_link(struct mvebu_pcie *pcie, ofnode node)
+{
+	struct reset_ctl rst;
+	int ret;
+
+	ret = reset_get_by_index_nodev(node, 0, &rst);
+	if (ret == -ENOENT) {
+		return 0;
+	} else if (ret < 0) {
+		printf("%s: cannot get reset controller: %d\n", pcie->name, ret);
+		return ret;
+	}
+
+	ret = reset_request(&rst);
+	if (ret) {
+		printf("%s: cannot request reset controller: %d\n", pcie->name, ret);
+		return ret;
+	}
+
+	ret = reset_deassert(&rst);
+	reset_free(&rst);
+	if (ret) {
+		printf("%s: cannot enable PCIe port: %d\n", pcie->name, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Setup PCIe link but do not enable it */
+static void mvebu_pcie_setup_link(struct mvebu_pcie *pcie)
 {
-	struct mvebu_pcie *pcie = dev_get_plat(dev);
-	struct udevice *ctlr = pci_get_controller(dev);
-	struct pci_controller *hose = dev_get_uclass_priv(ctlr);
 	u32 reg;
 
 	/* Setup PCIe controller to Root Complex mode */
@@ -374,6 +419,26 @@ static int mvebu_pcie_probe(struct udevice *dev)
 	writel(reg, pcie->base + PCIE_CTRL_OFF);
 
 	/*
+	 * Set Maximum Link Width to X1 or X4 in Root Port's PCIe Link
+	 * Capability register. This register is defined by PCIe specification
+	 * as read-only but this mvebu controller has it as read-write and must
+	 * be set to number of SerDes PCIe lanes (1 or 4). If this register is
+	 * not set correctly then link with endpoint card is not established.
+	 */
+	reg = readl(pcie->base + PCIE_CAPAB_OFF + PCI_EXP_LNKCAP);
+	reg &= ~PCI_EXP_LNKCAP_MLW;
+	reg |= (pcie->is_x4 ? 4 : 1) << 4;
+	writel(reg, pcie->base + PCIE_CAPAB_OFF + PCI_EXP_LNKCAP);
+}
+
+static int mvebu_pcie_probe(struct udevice *dev)
+{
+	struct mvebu_pcie *pcie = dev_get_plat(dev);
+	struct udevice *ctlr = pci_get_controller(dev);
+	struct pci_controller *hose = dev_get_uclass_priv(ctlr);
+	u32 reg;
+
+	/*
 	 * Change Class Code of PCI Bridge device to PCI Bridge (0x600400)
 	 * because default value is Memory controller (0x508000) which
 	 * U-Boot cannot recognize as P2P Bridge.
@@ -433,26 +498,26 @@ static int mvebu_pcie_probe(struct udevice *dev)
 	mvebu_pcie_set_local_bus_nr(pcie, 0);
 	mvebu_pcie_set_local_dev_nr(pcie, 1);
 
-	pcie->mem.start = (u32)mvebu_pcie_membase;
-	pcie->mem.end = pcie->mem.start + MBUS_PCI_MEM_SIZE - 1;
-	mvebu_pcie_membase += MBUS_PCI_MEM_SIZE;
-
-	if (mvebu_mbus_add_window_by_id(pcie->mem_target, pcie->mem_attr,
+	if (resource_size(&pcie->mem) &&
+	    mvebu_mbus_add_window_by_id(pcie->mem_target, pcie->mem_attr,
 					(phys_addr_t)pcie->mem.start,
 					resource_size(&pcie->mem))) {
-		printf("PCIe unable to add mbus window for mem at %08x+%08x\n",
+		printf("%s: unable to add mbus window for mem at %08x+%08x\n",
+		       pcie->name,
 		       (u32)pcie->mem.start, (unsigned)resource_size(&pcie->mem));
+		pcie->mem.start = 0;
+		pcie->mem.end = -1;
 	}
 
-	pcie->io.start = (u32)mvebu_pcie_iobase;
-	pcie->io.end = pcie->io.start + MBUS_PCI_IO_SIZE - 1;
-	mvebu_pcie_iobase += MBUS_PCI_IO_SIZE;
-
-	if (mvebu_mbus_add_window_by_id(pcie->io_target, pcie->io_attr,
+	if (resource_size(&pcie->io) &&
+	    mvebu_mbus_add_window_by_id(pcie->io_target, pcie->io_attr,
 					(phys_addr_t)pcie->io.start,
 					resource_size(&pcie->io))) {
-		printf("PCIe unable to add mbus window for IO at %08x+%08x\n",
+		printf("%s: unable to add mbus window for IO at %08x+%08x\n",
+		       pcie->name,
 		       (u32)pcie->io.start, (unsigned)resource_size(&pcie->io));
+		pcie->io.start = 0;
+		pcie->io.end = -1;
 	}
 
 	/* Setup windows and configure host bridge */
@@ -461,13 +526,23 @@ static int mvebu_pcie_probe(struct udevice *dev)
 	/* PCI memory space */
 	pci_set_region(hose->regions + 0, pcie->mem.start,
 		       pcie->mem.start, resource_size(&pcie->mem), PCI_REGION_MEM);
-	pci_set_region(hose->regions + 1,
-		       0, 0,
-		       gd->ram_size,
-		       PCI_REGION_MEM | PCI_REGION_SYS_MEMORY);
-	pci_set_region(hose->regions + 2, pcie->io.start,
-		       pcie->io.start, resource_size(&pcie->io), PCI_REGION_IO);
-	hose->region_count = 3;
+	hose->region_count = 1;
+
+	if (resource_size(&pcie->mem)) {
+		pci_set_region(hose->regions + hose->region_count,
+			       pcie->mem.start, pcie->mem.start,
+			       resource_size(&pcie->mem),
+			       PCI_REGION_MEM);
+		hose->region_count++;
+	}
+
+	if (resource_size(&pcie->io)) {
+		pci_set_region(hose->regions + hose->region_count,
+			       pcie->io.start, pcie->io.start,
+			       resource_size(&pcie->io),
+			       PCI_REGION_IO);
+		hose->region_count++;
+	}
 
 	/* PCI Bridge support 32-bit I/O and 64-bit prefetch mem addressing */
 	pcie->cfgcache[(PCI_IO_BASE - 0x10) / 4] =
@@ -475,21 +550,7 @@ static int mvebu_pcie_probe(struct udevice *dev)
 	pcie->cfgcache[(PCI_PREF_MEMORY_BASE - 0x10) / 4] =
 		PCI_PREF_RANGE_TYPE_64 | (PCI_PREF_RANGE_TYPE_64 << 16);
 
-	return 0;
-}
-
-static int mvebu_pcie_port_parse_dt(ofnode node, struct mvebu_pcie *pcie)
-{
-	const u32 *addr;
-	int len;
-
-	addr = ofnode_get_property(node, "assigned-addresses", &len);
-	if (!addr) {
-		pr_err("property \"assigned-addresses\" not found");
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	pcie->base = (void *)(fdt32_to_cpu(addr[2]) + SOC_REGS_PHY_BASE);
+	mvebu_pcie_wait_for_link(pcie);
 
 	return 0;
 }
@@ -554,31 +615,38 @@ static int mvebu_get_tgt_attr(ofnode node, int devfn,
 	return -ENOENT;
 }
 
-static int mvebu_pcie_of_to_plat(struct udevice *dev)
+static int mvebu_pcie_port_parse_dt(ofnode node, ofnode parent, struct mvebu_pcie *pcie)
 {
-	struct mvebu_pcie *pcie = dev_get_plat(dev);
+	struct fdt_pci_addr pci_addr;
+	const u32 *addr;
+	u32 num_lanes;
 	int ret = 0;
+	int len;
 
 	/* Get port number, lane number and memory target / attr */
-	if (ofnode_read_u32(dev_ofnode(dev), "marvell,pcie-port",
+	if (ofnode_read_u32(node, "marvell,pcie-port",
 			    &pcie->port)) {
 		ret = -ENODEV;
 		goto err;
 	}
 
-	if (ofnode_read_u32(dev_ofnode(dev), "marvell,pcie-lane", &pcie->lane))
+	if (ofnode_read_u32(node, "marvell,pcie-lane", &pcie->lane))
 		pcie->lane = 0;
 
 	sprintf(pcie->name, "pcie%d.%d", pcie->port, pcie->lane);
 
-	/* pci_get_devfn() returns devfn in bits 15..8, see PCI_DEV usage */
-	pcie->devfn = pci_get_devfn(dev);
-	if (pcie->devfn < 0) {
-		ret = -ENODEV;
+	if (!ofnode_read_u32(node, "num-lanes", &num_lanes) && num_lanes == 4)
+		pcie->is_x4 = true;
+
+	/* devfn is in bits [15:8], see PCI_DEV usage */
+	ret = ofnode_read_pci_addr(node, FDT_PCI_SPACE_CONFIG, "reg", &pci_addr);
+	if (ret < 0) {
+		printf("%s: property \"reg\" is invalid\n", pcie->name);
 		goto err;
 	}
+	pcie->devfn = pci_addr.phys_hi & 0xff00;
 
-	ret = mvebu_get_tgt_attr(dev_ofnode(dev->parent), pcie->devfn,
+	ret = mvebu_get_tgt_attr(parent, pcie->devfn,
 				 IORESOURCE_MEM,
 				 &pcie->mem_target, &pcie->mem_attr);
 	if (ret < 0) {
@@ -586,7 +654,7 @@ static int mvebu_pcie_of_to_plat(struct udevice *dev)
 		goto err;
 	}
 
-	ret = mvebu_get_tgt_attr(dev_ofnode(dev->parent), pcie->devfn,
+	ret = mvebu_get_tgt_attr(parent, pcie->devfn,
 				 IORESOURCE_IO,
 				 &pcie->io_target, &pcie->io_attr);
 	if (ret < 0) {
@@ -595,9 +663,15 @@ static int mvebu_pcie_of_to_plat(struct udevice *dev)
 	}
 
 	/* Parse PCIe controller register base from DT */
-	ret = mvebu_pcie_port_parse_dt(dev_ofnode(dev), pcie);
-	if (ret < 0)
+	addr = ofnode_get_property(node, "assigned-addresses", &len);
+	if (!addr) {
+		printf("%s: property \"assigned-addresses\" not found\n", pcie->name);
+		ret = -FDT_ERR_NOTFOUND;
 		goto err;
+	}
+
+	pcie->base = (void *)(u32)ofnode_translate_address(node, addr);
+	pcie->intregs = (u32)pcie->base - fdt32_to_cpu(addr[2]);
 
 	return 0;
 
@@ -615,7 +689,6 @@ static struct driver pcie_mvebu_drv = {
 	.id			= UCLASS_PCI,
 	.ops			= &mvebu_pcie_ops,
 	.probe			= mvebu_pcie_probe,
-	.of_to_plat	= mvebu_pcie_of_to_plat,
 	.plat_auto	= sizeof(struct mvebu_pcie),
 };
 
@@ -625,9 +698,14 @@ static struct driver pcie_mvebu_drv = {
  */
 static int mvebu_pcie_bind(struct udevice *parent)
 {
+	struct mvebu_pcie **ports_pcie;
 	struct mvebu_pcie *pcie;
 	struct uclass_driver *drv;
 	struct udevice *dev;
+	struct resource mem;
+	struct resource io;
+	int ports_count, i;
+	ofnode *ports_nodes;
 	ofnode subnode;
 
 	/* Lookup pci driver */
@@ -637,19 +715,94 @@ static int mvebu_pcie_bind(struct udevice *parent)
 		return -ENOENT;
 	}
 
+	ports_count = ofnode_get_child_count(dev_ofnode(parent));
+	ports_pcie = calloc(ports_count, sizeof(*ports_pcie));
+	ports_nodes = calloc(ports_count, sizeof(*ports_nodes));
+	if (!ports_pcie || !ports_nodes) {
+		free(ports_pcie);
+		free(ports_nodes);
+		return -ENOMEM;
+	}
+	ports_count = 0;
+
+	mem.start = MBUS_PCI_MEM_BASE;
+	mem.end = MBUS_PCI_MEM_BASE + MBUS_PCI_MEM_SIZE - 1;
+	io.start = MBUS_PCI_IO_BASE;
+	io.end = MBUS_PCI_IO_BASE + MBUS_PCI_IO_SIZE - 1;
+
+	/* First phase: Fill mvebu_pcie struct for each port */
 	ofnode_for_each_subnode(subnode, dev_ofnode(parent)) {
 		if (!ofnode_is_available(subnode))
 			continue;
 
 		pcie = calloc(1, sizeof(*pcie));
 		if (!pcie)
-			return -ENOMEM;
+			continue;
+
+		if (mvebu_pcie_port_parse_dt(subnode, dev_ofnode(parent), pcie) < 0) {
+			free(pcie);
+			continue;
+		}
+
+		/*
+		 * MVEBU PCIe controller needs MEMORY and I/O BARs to be mapped
+		 * into SoCs address space. Each controller will map 128M of MEM
+		 * and 64K of I/O space when registered.
+		 */
+
+		if (resource_size(&mem) >= SZ_128M) {
+			pcie->mem.start = mem.start;
+			pcie->mem.end = mem.start + SZ_128M - 1;
+			mem.start += SZ_128M;
+		} else {
+			printf("%s: unable to assign mbus window for mem\n", pcie->name);
+			pcie->mem.start = 0;
+			pcie->mem.end = -1;
+		}
+
+		if (resource_size(&io) >= SZ_64K) {
+			pcie->io.start = io.start;
+			pcie->io.end = io.start + SZ_64K - 1;
+			io.start += SZ_64K;
+		} else {
+			printf("%s: unable to assign mbus window for io\n", pcie->name);
+			pcie->io.start = 0;
+			pcie->io.end = -1;
+		}
+
+		ports_pcie[ports_count] = pcie;
+		ports_nodes[ports_count] = subnode;
+		ports_count++;
+	}
+
+	/* Second phase: Setup all PCIe links (do not enable them yet) */
+	for (i = 0; i < ports_count; i++)
+		mvebu_pcie_setup_link(ports_pcie[i]);
+
+	/* Third phase: Enable all PCIe links and create for each UCLASS_PCI device */
+	for (i = 0; i < ports_count; i++) {
+		pcie = ports_pcie[i];
+		subnode = ports_nodes[i];
+
+		/*
+		 * PCIe link can be enabled only after all PCIe links were
+		 * properly configured. This is because more PCIe links shares
+		 * one enable bit and some PCIe links cannot be enabled
+		 * individually.
+		 */
+		if (mvebu_pcie_enable_link(pcie, subnode) < 0) {
+			free(pcie);
+			continue;
+		}
 
 		/* Create child device UCLASS_PCI and bind it */
 		device_bind(parent, &pcie_mvebu_drv, pcie->name, pcie, subnode,
 			    &dev);
 	}
 
+	free(ports_pcie);
+	free(ports_nodes);
+
 	return 0;
 }
 
diff --git a/include/configs/clearfog.h b/include/configs/clearfog.h
index a30bca5..c9af5a4 100644
--- a/include/configs/clearfog.h
+++ b/include/configs/clearfog.h
@@ -53,7 +53,7 @@
 
 /* Defines for SPL */
 #define CONFIG_SPL_SIZE			(140 << 10)
-#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - 0x0030)
+#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + CONFIG_SPL_SIZE)
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/db-88f6720.h b/include/configs/db-88f6720.h
index 19fc669..16c83a8 100644
--- a/include/configs/db-88f6720.h
+++ b/include/configs/db-88f6720.h
@@ -32,22 +32,9 @@
  */
 #include "mv-common.h"
 
-/*
- * Memory layout while starting into the bin_hdr via the
- * BootROM:
- *
- * 0x4000.4000 - 0x4003.4000	headers space (192KiB)
- * 0x4000.4030			bin_hdr start address
- * 0x4003.4000 - 0x4004.7c00	BootROM memory allocations (15KiB)
- * 0x4007.fffc			BootROM stack top
- *
- * The address space between 0x4007.fffc and 0x400f.fff is not locked in
- * L2 cache thus cannot be used.
- */
-
 /* SPL */
 /* Defines for SPL */
-#define CONFIG_SPL_MAX_SIZE		((128 << 10) - 0x4030)
+#define CONFIG_SPL_MAX_SIZE		((128 << 10) - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + (128 << 10))
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/db-88f6820-amc.h b/include/configs/db-88f6820-amc.h
index 1f70c60..6538e66 100644
--- a/include/configs/db-88f6820-amc.h
+++ b/include/configs/db-88f6820-amc.h
@@ -41,7 +41,7 @@
 
 /* Defines for SPL */
 #define CONFIG_SPL_SIZE			(140 << 10)
-#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - 0x0030)
+#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + CONFIG_SPL_SIZE)
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/db-88f6820-gp.h b/include/configs/db-88f6820-gp.h
index 41dadfe..5f26119 100644
--- a/include/configs/db-88f6820-gp.h
+++ b/include/configs/db-88f6820-gp.h
@@ -50,7 +50,7 @@
 
 /* Defines for SPL */
 #define CONFIG_SPL_SIZE			(140 << 10)
-#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - 0x0030)
+#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + CONFIG_SPL_SIZE)
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/db-mv784mp-gp.h b/include/configs/db-mv784mp-gp.h
index dbbc33e..449a567 100644
--- a/include/configs/db-mv784mp-gp.h
+++ b/include/configs/db-mv784mp-gp.h
@@ -59,7 +59,7 @@
 
 /* SPL */
 /* Defines for SPL */
-#define CONFIG_SPL_MAX_SIZE		((128 << 10) - 0x4030)
+#define CONFIG_SPL_MAX_SIZE		((128 << 10) - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + (128 << 10))
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/ds414.h b/include/configs/ds414.h
index 7fba2b4..dbccd46 100644
--- a/include/configs/ds414.h
+++ b/include/configs/ds414.h
@@ -45,7 +45,7 @@
 
 /* SPL */
 /* Defines for SPL */
-#define CONFIG_SPL_MAX_SIZE		((128 << 10) - 0x4030)
+#define CONFIG_SPL_MAX_SIZE		((128 << 10) - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + (128 << 10))
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/helios4.h b/include/configs/helios4.h
index 56d35d6..de1ebbf 100644
--- a/include/configs/helios4.h
+++ b/include/configs/helios4.h
@@ -53,7 +53,7 @@
 
 /* Defines for SPL */
 #define CONFIG_SPL_SIZE			(140 << 10)
-#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - 0x0030)
+#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + CONFIG_SPL_SIZE)
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/maxbcm.h b/include/configs/maxbcm.h
index 53ba649..073c5a5 100644
--- a/include/configs/maxbcm.h
+++ b/include/configs/maxbcm.h
@@ -48,7 +48,7 @@
 
 /* SPL */
 /* Defines for SPL */
-#define CONFIG_SPL_MAX_SIZE		((128 << 10) - 0x4030)
+#define CONFIG_SPL_MAX_SIZE		((128 << 10) - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + (128 << 10))
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/theadorable.h b/include/configs/theadorable.h
index b43c03d..3c942cc 100644
--- a/include/configs/theadorable.h
+++ b/include/configs/theadorable.h
@@ -77,7 +77,7 @@
 
 /* SPL */
 /* Defines for SPL */
-#define CONFIG_SPL_MAX_SIZE		((128 << 10) - 0x4030)
+#define CONFIG_SPL_MAX_SIZE		((128 << 10) - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + (128 << 10))
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/turris_omnia.h b/include/configs/turris_omnia.h
index 9436a62..b35299b 100644
--- a/include/configs/turris_omnia.h
+++ b/include/configs/turris_omnia.h
@@ -28,7 +28,7 @@
 
 /* Defines for SPL */
 #define CONFIG_SPL_SIZE			(140 << 10)
-#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - 0x0030)
+#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + CONFIG_SPL_SIZE)
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/include/configs/x530.h b/include/configs/x530.h
index e78e249..67ff01d 100644
--- a/include/configs/x530.h
+++ b/include/configs/x530.h
@@ -68,7 +68,7 @@
 
 /* Defines for SPL */
 #define CONFIG_SPL_SIZE			(140 << 10)
-#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - 0x0030)
+#define CONFIG_SPL_MAX_SIZE		(CONFIG_SPL_SIZE - (CONFIG_SPL_TEXT_BASE - 0x40000000))
 
 #define CONFIG_SPL_BSS_START_ADDR	(0x40000000 + CONFIG_SPL_SIZE)
 #define CONFIG_SPL_BSS_MAX_SIZE		(16 << 10)
diff --git a/tools/kwbimage.c b/tools/kwbimage.c
index 224d815..9b63ce8 100644
--- a/tools/kwbimage.c
+++ b/tools/kwbimage.c
@@ -99,6 +99,7 @@ enum image_cfg_type {
 	IMAGE_CFG_NAND_BADBLK_LOCATION,
 	IMAGE_CFG_NAND_ECC_MODE,
 	IMAGE_CFG_NAND_PAGESZ,
+	IMAGE_CFG_CPU,
 	IMAGE_CFG_BINARY,
 	IMAGE_CFG_DATA,
 	IMAGE_CFG_DATA_DELAY,
@@ -129,6 +130,7 @@ static const char * const id_strs[] = {
 	[IMAGE_CFG_NAND_BADBLK_LOCATION] = "NAND_BADBLK_LOCATION",
 	[IMAGE_CFG_NAND_ECC_MODE] = "NAND_ECC_MODE",
 	[IMAGE_CFG_NAND_PAGESZ] = "NAND_PAGE_SIZE",
+	[IMAGE_CFG_CPU] = "CPU",
 	[IMAGE_CFG_BINARY] = "BINARY",
 	[IMAGE_CFG_DATA] = "DATA",
 	[IMAGE_CFG_DATA_DELAY] = "DATA_DELAY",
@@ -152,9 +154,11 @@ struct image_cfg_element {
 	enum image_cfg_type type;
 	union {
 		unsigned int version;
+		unsigned int cpu_sheeva;
 		unsigned int bootfrom;
 		struct {
 			const char *file;
+			unsigned int loadaddr;
 			unsigned int args[BINARY_MAX_ARGS];
 			unsigned int nargs;
 		} binary;
@@ -199,7 +203,7 @@ static const char *image_boot_mode_name(unsigned int id)
 	return NULL;
 }
 
-int image_boot_mode_id(const char *boot_mode_name)
+static int image_boot_mode_id(const char *boot_mode_name)
 {
 	int i;
 
@@ -210,7 +214,18 @@ int image_boot_mode_id(const char *boot_mode_name)
 	return -1;
 }
 
-int image_nand_ecc_mode_id(const char *nand_ecc_mode_name)
+static const char *image_nand_ecc_mode_name(unsigned int id)
+{
+	int i;
+
+	for (i = 0; nand_ecc_modes[i].name; i++)
+		if (nand_ecc_modes[i].id == id)
+			return nand_ecc_modes[i].name;
+
+	return NULL;
+}
+
+static int image_nand_ecc_mode_id(const char *nand_ecc_mode_name)
 {
 	int i;
 
@@ -280,6 +295,17 @@ static int image_get_bootfrom(void)
 	return e->bootfrom;
 }
 
+static int image_is_cpu_sheeva(void)
+{
+	struct image_cfg_element *e;
+
+	e = image_find_option(IMAGE_CFG_CPU);
+	if (!e)
+		return 0;
+
+	return e->cpu_sheeva;
+}
+
 /*
  * Compute a 8-bit checksum of a memory area. This algorithm follows
  * the requirements of the Marvell SoC BootROM specifications.
@@ -344,6 +370,29 @@ static uint32_t image_checksum32(void *start, uint32_t len)
 	return csum;
 }
 
+static unsigned int options_to_baudrate(uint8_t options)
+{
+	switch (options & 0x7) {
+	case MAIN_HDR_V1_OPT_BAUD_2400:
+		return 2400;
+	case MAIN_HDR_V1_OPT_BAUD_4800:
+		return 4800;
+	case MAIN_HDR_V1_OPT_BAUD_9600:
+		return 9600;
+	case MAIN_HDR_V1_OPT_BAUD_19200:
+		return 19200;
+	case MAIN_HDR_V1_OPT_BAUD_38400:
+		return 38400;
+	case MAIN_HDR_V1_OPT_BAUD_57600:
+		return 57600;
+	case MAIN_HDR_V1_OPT_BAUD_115200:
+		return 115200;
+	case MAIN_HDR_V1_OPT_BAUD_DEFAULT:
+	default:
+		return 0;
+	}
+}
+
 static uint8_t baudrate_to_option(unsigned int baudrate)
 {
 	switch (baudrate) {
@@ -602,7 +651,8 @@ static int kwb_export_pubkey(RSA *key, struct pubkey_der_v1 *dst, FILE *hashf,
 	return 0;
 }
 
-int kwb_sign(RSA *key, void *data, int datasz, struct sig_v1 *sig, char *signame)
+static int kwb_sign(RSA *key, void *data, int datasz, struct sig_v1 *sig,
+		    char *signame)
 {
 	EVP_PKEY *evp_key;
 	EVP_MD_CTX *ctx;
@@ -662,8 +712,8 @@ err_key:
 	return ret;
 }
 
-int kwb_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
-	       char *signame)
+static int kwb_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
+		      char *signame)
 {
 	EVP_PKEY *evp_key;
 	EVP_MD_CTX *ctx;
@@ -722,8 +772,8 @@ err_key:
 	return ret;
 }
 
-int kwb_sign_and_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
-			char *signame)
+static int kwb_sign_and_verify(RSA *key, void *data, int datasz,
+			       struct sig_v1 *sig, char *signame)
 {
 	if (kwb_sign(key, data, datasz, sig, signame) < 0)
 		return -1;
@@ -735,7 +785,7 @@ int kwb_sign_and_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
 }
 
 
-int kwb_dump_fuse_cmds_38x(FILE *out, struct secure_hdr_v1 *sec_hdr)
+static int kwb_dump_fuse_cmds_38x(FILE *out, struct secure_hdr_v1 *sec_hdr)
 {
 	struct hash_v1 kak_pub_hash;
 	struct image_cfg_element *e;
@@ -992,10 +1042,13 @@ static void *image_create_v0(size_t *imagesz, struct image_tool_params *params,
 
 static size_t image_headersz_v1(int *hasext)
 {
-	struct image_cfg_element *binarye;
+	struct image_cfg_element *e;
 	unsigned int count;
 	size_t headersz;
+	int cpu_sheeva;
+	struct stat s;
 	int cfgi;
+	int ret;
 
 	/*
 	 * Calculate the size of the header and the size of the
@@ -1009,19 +1062,25 @@ static size_t image_headersz_v1(int *hasext)
 			*hasext = 1;
 	}
 
-	count = image_count_options(IMAGE_CFG_DATA);
-	if (count > 0)
-		headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
+	cpu_sheeva = image_is_cpu_sheeva();
 
+	count = 0;
 	for (cfgi = 0; cfgi < cfgn; cfgi++) {
-		int ret;
-		struct stat s;
+		e = &image_cfg[cfgi];
 
-		binarye = &image_cfg[cfgi];
-		if (binarye->type != IMAGE_CFG_BINARY)
+		if (e->type == IMAGE_CFG_DATA)
+			count++;
+
+		if (e->type == IMAGE_CFG_DATA_DELAY ||
+		    (e->type == IMAGE_CFG_BINARY && count > 0)) {
+			headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
+			count = 0;
+		}
+
+		if (e->type != IMAGE_CFG_BINARY)
 			continue;
 
-		ret = stat(binarye->binary.file, &s);
+		ret = stat(e->binary.file, &s);
 		if (ret < 0) {
 			char cwd[PATH_MAX];
 			char *dir = cwd;
@@ -1036,30 +1095,74 @@ static size_t image_headersz_v1(int *hasext)
 				"Didn't find the file '%s' in '%s' which is mandatory to generate the image\n"
 				"This file generally contains the DDR3 training code, and should be extracted from an existing bootable\n"
 				"image for your board. Use 'dumpimage -T kwbimage -p 0' to extract it from an existing image.\n",
-				binarye->binary.file, dir);
+				e->binary.file, dir);
 			return 0;
 		}
 
 		headersz += sizeof(struct opt_hdr_v1) + sizeof(uint32_t) +
-			(binarye->binary.nargs) * sizeof(uint32_t);
-		headersz = ALIGN(headersz, 16);
+			(e->binary.nargs) * sizeof(uint32_t);
+
+		if (e->binary.loadaddr) {
+			/*
+			 * BootROM loads kwbimage header (in which the
+			 * executable code is also stored) to address
+			 * 0x40004000 or 0x40000000. Thus there is
+			 * restriction for the load address of the N-th
+			 * BINARY image.
+			 */
+			unsigned int base_addr, low_addr, high_addr;
+
+			base_addr = cpu_sheeva ? 0x40004000 : 0x40000000;
+			low_addr = base_addr + headersz;
+			high_addr = low_addr +
+				    (BINARY_MAX_ARGS - e->binary.nargs) * sizeof(uint32_t);
+
+			if (cpu_sheeva && e->binary.loadaddr % 16) {
+				fprintf(stderr,
+					"Invalid LOAD_ADDRESS 0x%08x for BINARY %s with %d args.\n"
+					"Address for CPU SHEEVA must be 16-byte aligned.\n",
+					e->binary.loadaddr, e->binary.file, e->binary.nargs);
+				return 0;
+			}
+
+			if (e->binary.loadaddr % 4 || e->binary.loadaddr < low_addr ||
+			    e->binary.loadaddr > high_addr) {
+				fprintf(stderr,
+					"Invalid LOAD_ADDRESS 0x%08x for BINARY %s with %d args.\n"
+					"Address must be 4-byte aligned and in range 0x%08x-0x%08x.\n",
+					e->binary.loadaddr, e->binary.file,
+					e->binary.nargs, low_addr, high_addr);
+				return 0;
+			}
+			headersz = e->binary.loadaddr - base_addr;
+		} else if (cpu_sheeva) {
+			headersz = ALIGN(headersz, 16);
+		} else {
+			headersz = ALIGN(headersz, 4);
+		}
+
 		headersz += ALIGN(s.st_size, 4) + sizeof(uint32_t);
 		if (hasext)
 			*hasext = 1;
 	}
 
+	if (count > 0)
+		headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
+
 	return image_headersz_align(headersz, image_get_bootfrom());
 }
 
-int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
-			 struct image_cfg_element *binarye,
-			 struct main_hdr_v1 *main_hdr)
+static int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
+				struct image_cfg_element *binarye,
+				struct main_hdr_v1 *main_hdr)
 {
 	struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
+	uint32_t base_addr;
 	uint32_t add_args;
 	uint32_t offset;
 	uint32_t *args;
 	size_t binhdrsz;
+	int cpu_sheeva;
 	struct stat s;
 	int argi;
 	FILE *bin;
@@ -1091,13 +1194,22 @@ int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
 	*cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
 
 	/*
-	 * ARM executable code inside the BIN header on some mvebu platforms
-	 * (e.g. A370, AXP) must always be aligned with the 128-bit boundary.
+	 * ARM executable code inside the BIN header on platforms with Sheeva
+	 * CPU (A370 and AXP) must always be aligned with the 128-bit boundary.
+	 * In the case when this code is not position independent (e.g. ARM
+	 * SPL), it must be placed at fixed load and execute address.
 	 * This requirement can be met by inserting dummy arguments into
 	 * BIN header, if needed.
 	 */
+	cpu_sheeva = image_is_cpu_sheeva();
+	base_addr = cpu_sheeva ? 0x40004000 : 0x40000000;
 	offset = *cur - (uint8_t *)main_hdr;
-	add_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
+	if (binarye->binary.loadaddr)
+		add_args = (binarye->binary.loadaddr - base_addr - offset) / sizeof(uint32_t);
+	else if (cpu_sheeva)
+		add_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
+	else
+		add_args = 0;
 	if (add_args) {
 		*(args - 1) = cpu_to_le32(binarye->binary.nargs + add_args);
 		*cur += add_args * sizeof(uint32_t);
@@ -1135,7 +1247,7 @@ err_close:
 	return -1;
 }
 
-int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
+static int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
 {
 	FILE *hashf;
 	int res;
@@ -1154,8 +1266,8 @@ int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
 	return res < 0 ? 1 : 0;
 }
 
-int kwb_sign_csk_with_kak(struct image_tool_params *params,
-			  struct secure_hdr_v1 *secure_hdr, RSA *csk)
+static int kwb_sign_csk_with_kak(struct image_tool_params *params,
+				 struct secure_hdr_v1 *secure_hdr, RSA *csk)
 {
 	RSA *kak = NULL;
 	RSA *kak_pub = NULL;
@@ -1196,9 +1308,9 @@ int kwb_sign_csk_with_kak(struct image_tool_params *params,
 	return 0;
 }
 
-int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
-			 int payloadsz, size_t headersz, uint8_t *image,
-			 struct secure_hdr_v1 *secure_hdr)
+static int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
+				int payloadsz, size_t headersz, uint8_t *image,
+				struct secure_hdr_v1 *secure_hdr)
 {
 	struct image_cfg_element *e_jtagdelay;
 	struct image_cfg_element *e_boxid;
@@ -1248,6 +1360,22 @@ int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
 	return 0;
 }
 
+static void finish_register_set_header_v1(uint8_t **cur, uint8_t **next_ext,
+					  struct register_set_hdr_v1 *register_set_hdr,
+					  int *datai, uint8_t delay)
+{
+	int size = sizeof(struct register_set_hdr_v1) + 8 * (*datai) + 4;
+
+	register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
+	register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
+	register_set_hdr->headersz_msb = size >> 16;
+	register_set_hdr->data[*datai].last_entry.delay = delay;
+	*cur += size;
+	**next_ext = 1;
+	*next_ext = &register_set_hdr->data[*datai].last_entry.next;
+	*datai = 0;
+}
+
 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 			     uint8_t *ptr, int payloadsz)
 {
@@ -1260,7 +1388,8 @@ static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 	uint8_t *image, *cur;
 	int hasext = 0;
 	uint8_t *next_ext = NULL;
-	int cfgi, datai, size;
+	int cfgi, datai;
+	uint8_t delay;
 
 	/*
 	 * Calculate the size of the header and the size of the
@@ -1351,49 +1480,53 @@ static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 	}
 
 	datai = 0;
-	register_set_hdr = (struct register_set_hdr_v1 *)cur;
 	for (cfgi = 0; cfgi < cfgn; cfgi++) {
 		e = &image_cfg[cfgi];
 		if (e->type != IMAGE_CFG_DATA &&
-		    e->type != IMAGE_CFG_DATA_DELAY)
-			continue;
-		if (e->type == IMAGE_CFG_DATA_DELAY) {
-			size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
-			register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
-			register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
-			register_set_hdr->headersz_msb = size >> 16;
-			register_set_hdr->data[datai].last_entry.delay = e->regdata_delay;
-			cur += size;
-			*next_ext = 1;
-			next_ext = &register_set_hdr->data[datai].last_entry.next;
-			datai = 0;
+		    e->type != IMAGE_CFG_DATA_DELAY &&
+		    e->type != IMAGE_CFG_BINARY)
 			continue;
+
+		if (datai == 0)
+			register_set_hdr = (struct register_set_hdr_v1 *)cur;
+
+		/* If delay is not specified, use the smallest possible value. */
+		if (e->type == IMAGE_CFG_DATA_DELAY)
+			delay = e->regdata_delay;
+		else
+			delay = REGISTER_SET_HDR_OPT_DELAY_MS(0);
+
+		/*
+		 * DATA_DELAY command is the last entry in the register set
+		 * header and BINARY command inserts new binary header.
+		 * Therefore BINARY command requires to finish register set
+		 * header if some DATA command was specified. And DATA_DELAY
+		 * command automatically finish register set header even when
+		 * there was no DATA command.
+		 */
+		if (e->type == IMAGE_CFG_DATA_DELAY ||
+		    (e->type == IMAGE_CFG_BINARY && datai != 0))
+			finish_register_set_header_v1(&cur, &next_ext, register_set_hdr,
+						      &datai, delay);
+
+		if (e->type == IMAGE_CFG_DATA) {
+			register_set_hdr->data[datai].entry.address =
+				cpu_to_le32(e->regdata.raddr);
+			register_set_hdr->data[datai].entry.value =
+				cpu_to_le32(e->regdata.rdata);
+			datai++;
+		}
+
+		if (e->type == IMAGE_CFG_BINARY) {
+			if (add_binary_header_v1(&cur, &next_ext, e, main_hdr))
+				return NULL;
 		}
-		register_set_hdr->data[datai].entry.address =
-			cpu_to_le32(e->regdata.raddr);
-		register_set_hdr->data[datai].entry.value =
-			cpu_to_le32(e->regdata.rdata);
-		datai++;
 	}
 	if (datai != 0) {
-		size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
-		register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
-		register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
-		register_set_hdr->headersz_msb = size >> 16;
-		/* Set delay to the smallest possible value 1ms. */
-		register_set_hdr->data[datai].last_entry.delay = 1;
-		cur += size;
-		*next_ext = 1;
-		next_ext = &register_set_hdr->data[datai].last_entry.next;
-	}
-
-	for (cfgi = 0; cfgi < cfgn; cfgi++) {
-		e = &image_cfg[cfgi];
-		if (e->type != IMAGE_CFG_BINARY)
-			continue;
-
-		if (add_binary_header_v1(&cur, &next_ext, e, main_hdr))
-			return NULL;
+		/* Set delay to the smallest possible value. */
+		delay = REGISTER_SET_HDR_OPT_DELAY_MS(0);
+		finish_register_set_header_v1(&cur, &next_ext, register_set_hdr,
+					      &datai, delay);
 	}
 
 	if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz + headersz,
@@ -1415,7 +1548,7 @@ static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 	return image;
 }
 
-int recognize_keyword(char *keyword)
+static int recognize_keyword(char *keyword)
 {
 	int kw_id;
 
@@ -1455,6 +1588,18 @@ static int image_create_config_parse_oneline(char *line,
 	case IMAGE_CFG_VERSION:
 		el->version = atoi(value1);
 		break;
+	case IMAGE_CFG_CPU:
+		if (strcmp(value1, "FEROCEON") == 0)
+			el->cpu_sheeva = 0;
+		else if (strcmp(value1, "SHEEVA") == 0)
+			el->cpu_sheeva = 1;
+		else if (strcmp(value1, "A9") == 0)
+			el->cpu_sheeva = 0;
+		else {
+			fprintf(stderr, "Invalid CPU %s\n", value1);
+			return -1;
+		}
+		break;
 	case IMAGE_CFG_BOOT_FROM:
 		ret = image_boot_mode_id(value1);
 
@@ -1488,10 +1633,40 @@ static int image_create_config_parse_oneline(char *line,
 		el->binary.file = strdup(value1);
 		while (1) {
 			char *value = strtok_r(NULL, delimiters, &saveptr);
+			char *endptr;
 
 			if (!value)
 				break;
-			el->binary.args[argi] = strtoul(value, NULL, 16);
+
+			if (!strcmp(value, "LOAD_ADDRESS")) {
+				value = strtok_r(NULL, delimiters, &saveptr);
+				if (!value) {
+					fprintf(stderr,
+						"Missing address argument for BINARY LOAD_ADDRESS\n");
+					return -1;
+				}
+				el->binary.loadaddr = strtoul(value, &endptr, 16);
+				if (*endptr) {
+					fprintf(stderr,
+						"Invalid argument '%s' for BINARY LOAD_ADDRESS\n",
+						value);
+					return -1;
+				}
+				value = strtok_r(NULL, delimiters, &saveptr);
+				if (value) {
+					fprintf(stderr,
+						"Unexpected argument '%s' after BINARY LOAD_ADDRESS\n",
+						value);
+					return -1;
+				}
+				break;
+			}
+
+			el->binary.args[argi] = strtoul(value, &endptr, 16);
+			if (*endptr) {
+				fprintf(stderr, "Invalid argument '%s' for BINARY\n", value);
+				return -1;
+			}
 			argi++;
 			if (argi >= BINARY_MAX_ARGS) {
 				fprintf(stderr,
@@ -1518,6 +1693,10 @@ static int image_create_config_parse_oneline(char *line,
 			el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_SDRAM_SETUP;
 		else
 			el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_MS(strtoul(value1, NULL, 10));
+		if (el->regdata_delay > 255) {
+			fprintf(stderr, "Maximal DATA_DELAY is 255\n");
+			return -1;
+		}
 		break;
 	case IMAGE_CFG_BAUDRATE:
 		el->baudrate = strtoul(value1, NULL, 10);
@@ -1727,9 +1906,12 @@ static void kwbimage_print_header(const void *ptr)
 
 	for_each_opt_hdr_v1 (ohdr, mhdr) {
 		if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
-			printf("BIN Hdr Size: ");
+			printf("BIN Img Size: ");
 			genimg_print_size(opt_hdr_v1_size(ohdr) - 12 -
 					  4 * ohdr->data[0]);
+			printf("BIN Img Offs: %08x\n",
+				(unsigned)((uint8_t *)ohdr - (uint8_t *)mhdr) +
+				8 + 4 * ohdr->data[0]);
 		}
 	}
 
@@ -1766,7 +1948,7 @@ static int kwbimage_verify_header(unsigned char *ptr, int image_size,
 	if (kwbimage_version(ptr) == 0) {
 		struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
 
-		if (mhdr->ext & 0x1) {
+		if (mhdr->ext) {
 			struct ext_hdr_v0 *ext_hdr = (void *)(mhdr + 1);
 
 			csum = image_checksum8(ext_hdr, sizeof(*ext_hdr) - 1);
@@ -1892,6 +2074,15 @@ static int kwbimage_generate(struct image_tool_params *params,
 
 	case 1:
 		alloc_len = image_headersz_v1(NULL);
+		if (!alloc_len) {
+			free(image_cfg);
+			exit(EXIT_FAILURE);
+		}
+		if (alloc_len > 192*1024) {
+			fprintf(stderr, "Header is too big (%u bytes), maximal kwbimage header size is %u bytes\n", alloc_len, 192*1024);
+			free(image_cfg);
+			exit(EXIT_FAILURE);
+		}
 		break;
 
 	default:
@@ -1931,52 +2122,207 @@ static int kwbimage_generate(struct image_tool_params *params,
 		return 4 + (4 - s.st_size % 4) % 4;
 }
 
+static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
+{
+	struct main_hdr_v0 *mhdr0 = (struct main_hdr_v0 *)ptr;
+	struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
+	size_t header_size = kwbheader_size(ptr);
+	struct register_set_hdr_v1 *regset_hdr;
+	struct ext_hdr_v0_reg *regdata;
+	struct ext_hdr_v0 *ehdr0;
+	struct opt_hdr_v1 *ohdr;
+	unsigned offset;
+	int cur_idx;
+	int version;
+	FILE *f;
+	int i;
+
+	f = fopen(params->outfile, "w");
+	if (!f) {
+		fprintf(stderr, "Can't open \"%s\": %s\n", params->outfile, strerror(errno));
+		return -1;
+	}
+
+	version = kwbimage_version(ptr);
+
+	if (version != 0)
+		fprintf(f, "VERSION %d\n", version);
+
+	fprintf(f, "BOOT_FROM %s\n", image_boot_mode_name(mhdr->blockid) ?: "<unknown>");
+
+	if (version == 0 && mhdr->blockid == IBR_HDR_NAND_ID)
+		fprintf(f, "NAND_ECC_MODE %s\n", image_nand_ecc_mode_name(mhdr0->nandeccmode));
+
+	if (mhdr->blockid == IBR_HDR_NAND_ID)
+		fprintf(f, "NAND_PAGE_SIZE 0x%x\n", (unsigned)mhdr->nandpagesize);
+
+	if (version != 0 && mhdr->blockid == IBR_HDR_NAND_ID) {
+		fprintf(f, "NAND_BLKSZ 0x%x\n", (unsigned)mhdr->nandblocksize);
+		fprintf(f, "NAND_BADBLK_LOCATION 0x%x\n", (unsigned)mhdr->nandbadblklocation);
+	}
+
+	if (version == 0 && mhdr->blockid == IBR_HDR_SATA_ID)
+		fprintf(f, "SATA_PIO_MODE %u\n", (unsigned)mhdr0->satapiomode);
+
+	/*
+	 * Addresses and sizes which are specified by mkimage command line
+	 * arguments and not in kwbimage config file
+	 */
+
+	if (version != 0)
+		fprintf(f, "#HEADER_SIZE 0x%x\n",
+			((unsigned)mhdr->headersz_msb << 8) | le16_to_cpu(mhdr->headersz_lsb));
+
+	fprintf(f, "#SRC_ADDRESS 0x%x\n", le32_to_cpu(mhdr->srcaddr));
+	fprintf(f, "#BLOCK_SIZE 0x%x\n", le32_to_cpu(mhdr->blocksize));
+	fprintf(f, "#DEST_ADDRESS 0x%08x\n", le32_to_cpu(mhdr->destaddr));
+	fprintf(f, "#EXEC_ADDRESS 0x%08x\n", le32_to_cpu(mhdr->execaddr));
+
+	if (version != 0) {
+		if (options_to_baudrate(mhdr->options))
+			fprintf(f, "BAUDRATE %u\n", options_to_baudrate(mhdr->options));
+		if (options_to_baudrate(mhdr->options) ||
+		    ((mhdr->options >> 3) & 0x3) || ((mhdr->options >> 5) & 0x7)) {
+			fprintf(f, "UART_PORT %u\n", (unsigned)((mhdr->options >> 3) & 0x3));
+			fprintf(f, "UART_MPP 0x%x\n", (unsigned)((mhdr->options >> 5) & 0x7));
+		}
+		if (mhdr->flags & 0x1)
+			fprintf(f, "DEBUG 1\n");
+	}
+
+	cur_idx = 1;
+	for_each_opt_hdr_v1(ohdr, ptr) {
+		if (ohdr->headertype == OPT_HDR_V1_SECURE_TYPE) {
+			fprintf(f, "#SECURE_HEADER\n");
+		} else if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
+			fprintf(f, "BINARY binary%d.bin", cur_idx);
+			for (i = 0; i < ohdr->data[0]; i++)
+				fprintf(f, " 0x%x", le32_to_cpu(((uint32_t *)ohdr->data)[i + 1]));
+			offset = (unsigned)((uint8_t *)ohdr - (uint8_t *)mhdr) + 8 + 4 * ohdr->data[0];
+			fprintf(f, " LOAD_ADDRESS 0x%08x\n", 0x40000000 + offset);
+			fprintf(f, " # for CPU SHEEVA: LOAD_ADDRESS 0x%08x\n", 0x40004000 + offset);
+			cur_idx++;
+		} else if (ohdr->headertype == OPT_HDR_V1_REGISTER_TYPE) {
+			regset_hdr = (struct register_set_hdr_v1 *)ohdr;
+			for (i = 0;
+			     i < opt_hdr_v1_size(ohdr) - sizeof(struct opt_hdr_v1) -
+				 sizeof(regset_hdr->data[0].last_entry);
+			     i++)
+				fprintf(f, "DATA 0x%08x 0x%08x\n",
+					le32_to_cpu(regset_hdr->data[i].entry.address),
+					le32_to_cpu(regset_hdr->data[i].entry.value));
+			if (opt_hdr_v1_size(ohdr) - sizeof(struct opt_hdr_v1) >=
+			    sizeof(regset_hdr->data[0].last_entry)) {
+				if (regset_hdr->data[0].last_entry.delay)
+					fprintf(f, "DATA_DELAY %u\n",
+						(unsigned)regset_hdr->data[0].last_entry.delay);
+				else
+					fprintf(f, "DATA_DELAY SDRAM_SETUP\n");
+			}
+		}
+	}
+
+	if (version == 0 && mhdr0->ext) {
+		ehdr0 = (struct ext_hdr_v0 *)(mhdr0 + 1);
+		if (ehdr0->offset) {
+			for (regdata = (struct ext_hdr_v0_reg *)((uint8_t *)ptr + ehdr0->offset);
+			     (uint8_t *)regdata < (uint8_t *)ptr + header_size && regdata->raddr &&
+			     regdata->rdata;
+			     regdata++)
+				fprintf(f, "DATA 0x%08x 0x%08x\n", le32_to_cpu(regdata->raddr),
+					le32_to_cpu(regdata->rdata));
+		}
+	}
+
+	if (version == 0 && le16_to_cpu(mhdr0->ddrinitdelay))
+		fprintf(f, "DDR_INIT_DELAY %u\n", (unsigned)le16_to_cpu(mhdr0->ddrinitdelay));
+
+	/* Undocumented reserved fields */
+
+	if (version == 0 && (mhdr0->rsvd1[0] || mhdr0->rsvd1[1] || mhdr0->rsvd1[2]))
+		fprintf(f, "#RSVD1 0x%x 0x%x 0x%x\n", (unsigned)mhdr0->rsvd1[0],
+			(unsigned)mhdr0->rsvd1[1], (unsigned)mhdr0->rsvd1[2]);
+
+	if (version == 0 && mhdr0->rsvd3)
+		fprintf(f, "#RSVD3 0x%x\n", (unsigned)mhdr0->rsvd3);
+
+	if (version == 0 && le16_to_cpu(mhdr0->rsvd2))
+		fprintf(f, "#RSVD2 0x%x\n", (unsigned)le16_to_cpu(mhdr0->rsvd2));
+
+	if (version != 0 && mhdr->reserved4)
+		fprintf(f, "#RESERVED4 0x%x\n", (unsigned)mhdr->reserved4);
+
+	if (version != 0 && mhdr->reserved5)
+		fprintf(f, "#RESERVED5 0x%x\n", (unsigned)le16_to_cpu(mhdr->reserved5));
+
+	fclose(f);
+
+	return 0;
+}
+
 static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params)
 {
 	struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
 	size_t header_size = kwbheader_size(ptr);
 	struct opt_hdr_v1 *ohdr;
 	int idx = params->pflag;
-	int cur_idx = 0;
+	int cur_idx;
 	uint32_t offset;
 	ulong image;
 	ulong size;
 
-	for_each_opt_hdr_v1 (ohdr, ptr) {
-		if (ohdr->headertype != OPT_HDR_V1_BINARY_TYPE)
-			continue;
+	/* Generate kwbimage config file when '-p -1' is specified */
+	if (idx == -1)
+		return kwbimage_generate_config(ptr, params);
 
-		if (idx == cur_idx) {
-			image = (ulong)&ohdr->data[4 + 4 * ohdr->data[0]];
-			size = opt_hdr_v1_size(ohdr) - 12 - 4 * ohdr->data[0];
-			goto extract;
-		}
+	image = 0;
+	size = 0;
 
-		++cur_idx;
-	}
+	if (idx == 0) {
+		/* Extract data image when -p is not specified or when '-p 0' is specified */
+		offset = le32_to_cpu(mhdr->srcaddr);
 
-	if (idx != cur_idx) {
-		printf("Image %d is not present\n", idx);
-		return -1;
-	}
+		if (mhdr->blockid == IBR_HDR_SATA_ID) {
+			offset -= 1;
+			offset *= 512;
+		}
 
-	offset = le32_to_cpu(mhdr->srcaddr);
+		if (mhdr->blockid == IBR_HDR_SDIO_ID)
+			offset *= 512;
 
-	if (mhdr->blockid == IBR_HDR_SATA_ID) {
-		offset -= 1;
-		offset *= 512;
-	}
+		if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
+			offset = header_size;
 
-	if (mhdr->blockid == IBR_HDR_SDIO_ID)
-		offset *= 512;
+		image = (ulong)((uint8_t *)ptr + offset);
+		size = le32_to_cpu(mhdr->blocksize) - 4;
+	} else {
+		/* Extract N-th binary header executabe image when other '-p N' is specified */
+		cur_idx = 1;
+		for_each_opt_hdr_v1(ohdr, ptr) {
+			if (ohdr->headertype != OPT_HDR_V1_BINARY_TYPE)
+				continue;
 
-	if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
-		offset = header_size;
+			if (idx == cur_idx) {
+				image = (ulong)&ohdr->data[4 + 4 * ohdr->data[0]];
+				size = opt_hdr_v1_size(ohdr) - 12 - 4 * ohdr->data[0];
+				break;
+			}
 
-	image = (ulong)((uint8_t *)ptr + offset);
-	size = le32_to_cpu(mhdr->blocksize) - 4;
+			++cur_idx;
+		}
+
+		if (!image) {
+			fprintf(stderr, "Argument -p %d is invalid\n", idx);
+			fprintf(stderr, "Available subimages:\n");
+			fprintf(stderr, " -p -1  - kwbimage config file\n");
+			fprintf(stderr, " -p 0   - data image\n");
+			if (cur_idx - 1 > 0)
+				fprintf(stderr, " -p N   - Nth binary header image (totally: %d)\n",
+					cur_idx - 1);
+			return -1;
+		}
+	}
 
-extract:
 	return imagetool_save_subimage(params->outfile, image, size);
 }
 
@@ -1985,7 +2331,8 @@ extract:
  */
 static int kwbimage_check_params(struct image_tool_params *params)
 {
-	if (!params->iflag && (!params->imagename || !strlen(params->imagename))) {
+	if (!params->lflag && !params->iflag &&
+	    (!params->imagename || !strlen(params->imagename))) {
 		char *msg = "Configuration file for kwbimage creation omitted";
 
 		fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
diff --git a/tools/kwbimage.h b/tools/kwbimage.h
index 8d37357..9ebc7d7 100644
--- a/tools/kwbimage.h
+++ b/tools/kwbimage.h
@@ -208,7 +208,7 @@ static inline size_t kwbheader_size(const void *header)
 		const struct main_hdr_v0 *hdr = header;
 
 		return sizeof(*hdr) +
-		       (hdr->ext & 0x1) ? sizeof(struct ext_hdr_v0) : 0;
+		       hdr->ext ? sizeof(struct ext_hdr_v0) : 0;
 	} else {
 		const struct main_hdr_v1 *hdr = header;
 
@@ -235,11 +235,11 @@ static inline int opt_hdr_v1_valid_size(const struct opt_hdr_v1 *ohdr,
 {
 	uint32_t ohdr_size;
 
-	if ((void *)(ohdr + 1) > mhdr_end)
+	if ((const void *)(ohdr + 1) > mhdr_end)
 		return 0;
 
 	ohdr_size = opt_hdr_v1_size(ohdr);
-	if (ohdr_size < 8 || (void *)((uint8_t *)ohdr + ohdr_size) > mhdr_end)
+	if (ohdr_size < 8 || (const void *)((const uint8_t *)ohdr + ohdr_size) > mhdr_end)
 		return 0;
 
 	return 1;
@@ -252,7 +252,7 @@ static inline struct opt_hdr_v1 *opt_hdr_v1_first(void *img) {
 		return NULL;
 
 	mhdr = img;
-	if (mhdr->ext & 0x1)
+	if (mhdr->ext)
 		return (struct opt_hdr_v1 *)(mhdr + 1);
 	else
 		return NULL;
@@ -272,7 +272,7 @@ static inline struct opt_hdr_v1 *_opt_hdr_v1_next(struct opt_hdr_v1 *cur)
 
 static inline struct opt_hdr_v1 *opt_hdr_v1_next(struct opt_hdr_v1 *cur)
 {
-	if (*opt_hdr_v1_ext(cur) & 0x1)
+	if (*opt_hdr_v1_ext(cur))
 		return _opt_hdr_v1_next(cur);
 	else
 		return NULL;
diff --git a/tools/kwboot.c b/tools/kwboot.c
index d22e6ea..c3d8ab6 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -1398,7 +1398,7 @@ kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
 	uint32_t ohdrsz;
 	uint8_t *prev_ext;
 
-	if (hdr->ext & 0x1) {
+	if (hdr->ext) {
 		for_each_opt_hdr_v1 (ohdr, img)
 			if (opt_hdr_v1_next(ohdr) == NULL)
 				break;
@@ -1422,7 +1422,7 @@ kwboot_add_bin_ohdr_v1(void *img, size_t *size, uint32_t binsz)
 	ohdrsz = sizeof(*ohdr) + 4 + 4 * num_args + binsz + 4;
 	kwboot_img_grow_hdr(hdr, size, ohdrsz);
 
-	*prev_ext |= 1;
+	*prev_ext = 1;
 
 	ohdr->headertype = OPT_HDR_V1_BINARY_TYPE;
 	ohdr->headersz_msb = ohdrsz >> 16;