Merge git://git.denx.de/u-boot-fsl-qoriq

16 files changed, 1004 insertions, 79 deletions

diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
index 22dce88..0b3d98e 100644
--- a/arch/arm/cpu/armv8/Kconfig
+++ b/arch/arm/cpu/armv8/Kconfig
@@ -3,6 +3,24 @@ if ARM64
 config ARMV8_MULTIENTRY
         bool "Enable multiple CPUs to enter into U-Boot"
 
+config ARMV8_SET_SMPEN
+        bool "Enable data coherency with other cores in cluster"
+        help
+	  Say Y here if there is not any trust firmware to set
+	  CPUECTLR_EL1.SMPEN bit before U-Boot.
+
+	  For A53, it enables data coherency with other cores in the
+	  cluster, and for A57/A72, it enables receiving of instruction
+	  cache and TLB maintenance operations.
+	  Cortex A53/57/72 cores require CPUECTLR_EL1.SMPEN set even
+	  for single core systems. Unfortunately write access to this
+	  register may be controlled by EL3/EL2 firmware. To be more
+	  precise, by default (if there is EL2/EL3 firmware running)
+	  this register is RO for NS EL1.
+	  This switch can be used to avoid writing to CPUECTLR_EL1,
+	  it can be safely enabled when EL2/EL3 initialized SMPEN bit
+	  or when CPU implementation doesn't include that register.
+
 config ARMV8_SPIN_TABLE
 	bool "Support spin-table enable method"
 	depends on ARMV8_MULTIENTRY && OF_LIBFDT
@@ -21,6 +39,47 @@ config ARMV8_SPIN_TABLE
 	    - Reserve the code for the spin-table and the release address
 	      via a /memreserve/ region in the Device Tree.
 
+menu "ARMv8 secure monitor firmware"
+config ARMV8_SEC_FIRMWARE_SUPPORT
+	bool "Enable ARMv8 secure monitor firmware framework support"
+	select OF_LIBFDT
+	select FIT
+	help
+	  This framework is aimed at making secure monitor firmware load
+	  process brief.
+	  Note: Only FIT format image is supported.
+	  You should prepare and provide the below information:
+	    - Address of secure firmware.
+	    - Address to hold the return address from secure firmware.
+	    - Secure firmware FIT image related information.
+	      Such as: SEC_FIRMWARE_FIT_IMAGE and SEC_FIRMEWARE_FIT_CNF_NAME
+	    - The target exception level that secure monitor firmware will
+	      return to.
+
+config SPL_ARMV8_SEC_FIRMWARE_SUPPORT
+	bool "Enable ARMv8 secure monitor firmware framework support for SPL"
+	select SPL_OF_LIBFDT
+	select SPL_FIT
+	help
+	  Say Y here to support this framework in SPL phase.
+
+config SEC_FIRMWARE_ARMV8_PSCI
+	bool "PSCI implementation in secure monitor firmware"
+	depends on ARMV8_SEC_FIRMWARE_SUPPORT || SPL_ARMV8_SEC_FIRMWARE_SUPPORT
+	help
+	  This config enables the ARMv8 PSCI implementation in secure monitor
+	  firmware. This is a private PSCI implementation and different from
+	  those implemented under the common ARMv8 PSCI framework.
+
+config ARMV8_SEC_FIRMWARE_ERET_ADDR_REVERT
+	bool "ARMv8 secure monitor firmware ERET address byteorder swap"
+	depends on ARMV8_SEC_FIRMWARE_SUPPORT || SPL_ARMV8_SEC_FIRMWARE_SUPPORT
+	help
+	  Say Y here when the endianness of the register or memory holding the
+	  Secure firmware exception return address is different with core's.
+
+endmenu
+
 config PSCI_RESET
 	bool "Use PSCI for reset and shutdown"
 	default y
diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
index e780afc..65915ee 100644
--- a/arch/arm/cpu/armv8/Makefile
+++ b/arch/arm/cpu/armv8/Makefile
@@ -19,7 +19,7 @@ obj-y	+= cpu-dt.o
 ifndef CONFIG_SPL_BUILD
 obj-$(CONFIG_ARMV8_SPIN_TABLE) += spin_table.o spin_table_v8.o
 endif
-obj-$(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) += sec_firmware.o sec_firmware_asm.o
+obj-$(CONFIG_$(SPL_)ARMV8_SEC_FIRMWARE_SUPPORT) += sec_firmware.o sec_firmware_asm.o
 
 obj-$(CONFIG_FSL_LAYERSCAPE) += fsl-layerscape/
 obj-$(CONFIG_S32V234) += s32v234/
diff --git a/arch/arm/cpu/armv8/cpu-dt.c b/arch/arm/cpu/armv8/cpu-dt.c
index 3a5afe8..5156a15 100644
--- a/arch/arm/cpu/armv8/cpu-dt.c
+++ b/arch/arm/cpu/armv8/cpu-dt.c
@@ -14,7 +14,7 @@
 int psci_update_dt(void *fdt)
 {
 #ifdef CONFIG_MP
-#if defined(CONFIG_ARMV8_PSCI) || defined(CONFIG_FSL_PPA_ARMV8_PSCI)
+#if defined(CONFIG_ARMV8_PSCI) || defined(CONFIG_SEC_FIRMWARE_ARMV8_PSCI)
 
 #ifdef CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT
 	/*
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/Kconfig b/arch/arm/cpu/armv8/fsl-layerscape/Kconfig
index de0b580..ba411e2 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/Kconfig
+++ b/arch/arm/cpu/armv8/fsl-layerscape/Kconfig
@@ -1,5 +1,6 @@
 config ARCH_LS1012A
 	bool
+	select ARMV8_SET_SMPEN
 	select FSL_LSCH2
 	select SYS_FSL_DDR_BE
 	select SYS_FSL_MMDC
@@ -7,6 +8,7 @@ config ARCH_LS1012A
 
 config ARCH_LS1043A
 	bool
+	select ARMV8_SET_SMPEN
 	select FSL_LSCH2
 	select SYS_FSL_DDR
 	select SYS_FSL_DDR_BE
@@ -23,6 +25,7 @@ config ARCH_LS1043A
 
 config ARCH_LS1046A
 	bool
+	select ARMV8_SET_SMPEN
 	select FSL_LSCH2
 	select SYS_FSL_DDR
 	select SYS_FSL_DDR_BE
@@ -38,6 +41,7 @@ config ARCH_LS1046A
 
 config ARCH_LS2080A
 	bool
+	select ARMV8_SET_SMPEN
 	select FSL_LSCH3
 	select SYS_FSL_DDR
 	select SYS_FSL_DDR_LE
@@ -75,25 +79,60 @@ config FSL_LSCH3
 menu "Layerscape architecture"
 	depends on FSL_LSCH2 || FSL_LSCH3
 
+config FSL_PCIE_COMPAT
+	string "PCIe compatible of Kernel DT"
+	depends on PCIE_LAYERSCAPE
+	default "fsl,ls1012a-pcie" if ARCH_LS1012A
+	default "fsl,ls1043a-pcie" if ARCH_LS1043A
+	default "fsl,ls1046a-pcie" if ARCH_LS1046A
+	default "fsl,ls2080a-pcie" if ARCH_LS2080A
+	help
+	  This compatible is used to find pci controller node in Kernel DT
+	  to complete fixup.
+
+config HAS_FEATURE_GIC64K_ALIGN
+	bool
+	default y if ARCH_LS1043A
+
+config HAS_FEATURE_ENHANCED_MSI
+	bool
+	default y if ARCH_LS1043A
+
 menu "Layerscape PPA"
 config FSL_LS_PPA
 	bool "FSL Layerscape PPA firmware support"
 	depends on !ARMV8_PSCI
-	depends on ARCH_LS1043A || ARCH_LS1046A
-	select FSL_PPA_ARMV8_PSCI
+	select ARMV8_SEC_FIRMWARE_SUPPORT
+	select SEC_FIRMWARE_ARMV8_PSCI
+	select ARMV8_SEC_FIRMWARE_ERET_ADDR_REVERT if FSL_LSCH2
 	help
 	  The FSL Primary Protected Application (PPA) is a software component
 	  which is loaded during boot stage, and then remains resident in RAM
 	  and runs in the TrustZone after boot.
 	  Say y to enable it.
+choice
+	prompt "FSL Layerscape PPA firmware loading-media select"
+	depends on FSL_LS_PPA
+	default SYS_LS_PPA_FW_IN_XIP
+
+config SYS_LS_PPA_FW_IN_XIP
+	bool "XIP"
+	help
+	  Say Y here if the PPA firmware locate at XIP flash, such
+	  as NOR or QSPI flash.
+
+endchoice
 
-config FSL_PPA_ARMV8_PSCI
-	bool "PSCI implementation in PPA firmware"
+config SYS_LS_PPA_FW_ADDR
+	hex "Address of PPA firmware loading from"
 	depends on FSL_LS_PPA
+	default 0x40500000 if SYS_LS_PPA_FW_IN_XIP && QSPI_BOOT
+	default 0x60500000 if SYS_LS_PPA_FW_IN_XIP
 	help
-	  This config enables the ARMv8 PSCI implementation in PPA firmware.
-	  This is a private PSCI implementation and different from those
-	  implemented under the common ARMv8 PSCI framework.
+	  If the PPA firmware locate at XIP flash, such as NOR or
+	  QSPI flash, this address is a directly memory-mapped.
+	  If it is in a serial accessed flash, such as NAND and SD
+	  card, it is a byte offset.
 endmenu
 
 config SYS_FSL_ERRATUM_A010315
@@ -116,7 +155,7 @@ config MAX_CPUS
 	  in spin table to properly handle all cores.
 
 config SECURE_BOOT
-	bool
+	bool "Secure Boot"
 	help
 		Enable Freescale Secure Boot feature
 
@@ -148,6 +187,83 @@ config SYS_HAS_SERDES
 
 endmenu
 
+menu "Layerscape clock tree configuration"
+	depends on FSL_LSCH2 || FSL_LSCH3
+
+config SYS_FSL_CLK
+	bool "Enable clock tree initialization"
+	default y
+
+config CLUSTER_CLK_FREQ
+	int "Reference clock of core cluster"
+	depends on ARCH_LS1012A
+	default 100000000
+	help
+	  This number is the reference clock frequency of core PLL.
+	  For most platforms, the core PLL and Platform PLL have the same
+	  reference clock, but for some platforms, LS1012A for instance,
+	  they are provided sepatately.
+
+config SYS_FSL_PCLK_DIV
+	int "Platform clock divider"
+	default 1 if ARCH_LS1043A
+	default 1 if ARCH_LS1046A
+	default 2
+	help
+	  This is the divider that is used to derive Platform clock from
+	  Platform PLL, in another word:
+		Platform_clk = Platform_PLL_freq / this_divider
+
+config SYS_FSL_DSPI_CLK_DIV
+	int "DSPI clock divider"
+	default 1 if ARCH_LS1043A
+	default 2
+	help
+	  This is the divider that is used to derive DSPI clock from Platform
+	  PLL, in another word DSPI_clk = Platform_PLL_freq / this_divider.
+
+config SYS_FSL_DUART_CLK_DIV
+	int "DUART clock divider"
+	default 1 if ARCH_LS1043A
+	default 2
+	help
+	  This is the divider that is used to derive DUART clock from Platform
+	  clock, in another word DUART_clk = Platform_clk / this_divider.
+
+config SYS_FSL_I2C_CLK_DIV
+	int "I2C clock divider"
+	default 1 if ARCH_LS1043A
+	default 2
+	help
+	  This is the divider that is used to derive I2C clock from Platform
+	  clock, in another word I2C_clk = Platform_clk / this_divider.
+
+config SYS_FSL_IFC_CLK_DIV
+	int "IFC clock divider"
+	default 1 if ARCH_LS1043A
+	default 2
+	help
+	  This is the divider that is used to derive IFC clock from Platform
+	  clock, in another word IFC_clk = Platform_clk / this_divider.
+
+config SYS_FSL_LPUART_CLK_DIV
+	int "LPUART clock divider"
+	default 1 if ARCH_LS1043A
+	default 2
+	help
+	  This is the divider that is used to derive LPUART clock from Platform
+	  clock, in another word LPUART_clk = Platform_clk / this_divider.
+
+config SYS_FSL_SDHC_CLK_DIV
+	int "SDHC clock divider"
+	default 1 if ARCH_LS1043A
+	default 1 if ARCH_LS1012A
+	default 2
+	help
+	  This is the divider that is used to derive SDHC clock from Platform
+	  clock, in another word SDHC_clk = Platform_clk / this_divider.
+endmenu
+
 config SYS_FSL_ERRATUM_A008336
 	bool
 
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/Makefile b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
index 423b4b3..c9ab93e 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/Makefile
+++ b/arch/arm/cpu/armv8/fsl-layerscape/Makefile
@@ -10,7 +10,7 @@ obj-y += soc.o
 obj-$(CONFIG_MP) += mp.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SPL) += spl.o
-obj-$(CONFIG_FSL_LS_PPA) += ppa.o
+obj-$(CONFIG_$(SPL_)FSL_LS_PPA) += ppa.o
 
 ifneq ($(CONFIG_FSL_LSCH3),)
 obj-y += fsl_lsch3_speed.o
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 467d9af..335f225 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -345,8 +345,9 @@ int print_cpuinfo(void)
 		       (type == TY_ITYP_VER_A72 ? "A72" : "   "))),
 		       strmhz(buf, sysinfo.freq_processor[core]));
 	}
+	/* Display platform clock as Bus frequency. */
 	printf("\n       Bus:      %-4s MHz  ",
-	       strmhz(buf, sysinfo.freq_systembus));
+	       strmhz(buf, sysinfo.freq_systembus / CONFIG_SYS_FSL_PCLK_DIV));
 	printf("DDR:      %-4s MT/s", strmhz(buf, sysinfo.freq_ddrbus));
 #ifdef CONFIG_SYS_DPAA_FMAN
 	printf("  FMAN:     %-4s MHz", strmhz(buf, sysinfo.freq_fman[0]));
@@ -411,7 +412,7 @@ int arch_early_init_r(void)
 #endif
 #ifdef CONFIG_MP
 #if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && \
-	defined(CONFIG_FSL_PPA_ARMV8_PSCI)
+	defined(CONFIG_SEC_FIRMWARE_ARMV8_PSCI)
 	/* Check the psci version to determine if the psci is supported */
 	psci_ver = sec_firmware_support_psci_version();
 #endif
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
index c10ccf9..26d4a30 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fdt.c
@@ -43,7 +43,7 @@ void ft_fixup_cpu(void *blob)
 	u64 val, core_id;
 	size_t *boot_code_size = &(__secondary_boot_code_size);
 #if defined(CONFIG_ARMV8_SEC_FIRMWARE_SUPPORT) && \
-	defined(CONFIG_FSL_PPA_ARMV8_PSCI)
+	defined(CONFIG_SEC_FIRMWARE_ARMV8_PSCI)
 	int node;
 	u32 psci_ver;
 
@@ -133,6 +133,218 @@ void fsl_fdt_disable_usb(void *blob)
 	}
 }
 
+#ifdef CONFIG_HAS_FEATURE_GIC64K_ALIGN
+static void fdt_fixup_gic(void *blob)
+{
+	int offset, err;
+	u64 reg[8];
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	unsigned int val;
+	struct ccsr_scfg __iomem *scfg = (void *)CONFIG_SYS_FSL_SCFG_ADDR;
+	int align_64k = 0;
+
+	val = gur_in32(&gur->svr);
+
+	if (SVR_SOC_VER(val) != SVR_LS1043A) {
+		align_64k = 1;
+	} else if (SVR_REV(val) != REV1_0) {
+		val = scfg_in32(&scfg->gic_align) & (0x01 << GIC_ADDR_BIT);
+		if (!val)
+			align_64k = 1;
+	}
+
+	offset = fdt_subnode_offset(blob, 0, "interrupt-controller@1400000");
+	if (offset < 0) {
+		printf("WARNING: fdt_subnode_offset can't find node %s: %s\n",
+		       "interrupt-controller@1400000", fdt_strerror(offset));
+		return;
+	}
+
+	/* Fixup gic node align with 64K */
+	if (align_64k) {
+		reg[0] = cpu_to_fdt64(GICD_BASE_64K);
+		reg[1] = cpu_to_fdt64(GICD_SIZE_64K);
+		reg[2] = cpu_to_fdt64(GICC_BASE_64K);
+		reg[3] = cpu_to_fdt64(GICC_SIZE_64K);
+		reg[4] = cpu_to_fdt64(GICH_BASE_64K);
+		reg[5] = cpu_to_fdt64(GICH_SIZE_64K);
+		reg[6] = cpu_to_fdt64(GICV_BASE_64K);
+		reg[7] = cpu_to_fdt64(GICV_SIZE_64K);
+	} else {
+	/* Fixup gic node align with default */
+		reg[0] = cpu_to_fdt64(GICD_BASE);
+		reg[1] = cpu_to_fdt64(GICD_SIZE);
+		reg[2] = cpu_to_fdt64(GICC_BASE);
+		reg[3] = cpu_to_fdt64(GICC_SIZE);
+		reg[4] = cpu_to_fdt64(GICH_BASE);
+		reg[5] = cpu_to_fdt64(GICH_SIZE);
+		reg[6] = cpu_to_fdt64(GICV_BASE);
+		reg[7] = cpu_to_fdt64(GICV_SIZE);
+	}
+
+	err = fdt_setprop(blob, offset, "reg", reg, sizeof(reg));
+	if (err < 0) {
+		printf("WARNING: fdt_setprop can't set %s from node %s: %s\n",
+		       "reg", "interrupt-controller@1400000",
+		       fdt_strerror(err));
+		return;
+	}
+
+	return;
+}
+#endif
+
+#ifdef CONFIG_HAS_FEATURE_ENHANCED_MSI
+static int _fdt_fixup_msi_node(void *blob, const char *name,
+				  int irq_0, int irq_1, int rev)
+{
+	int err, offset, len;
+	u32 tmp[4][3];
+	void *p;
+
+	offset = fdt_path_offset(blob, name);
+	if (offset < 0) {
+		printf("WARNING: fdt_path_offset can't find path %s: %s\n",
+		       name, fdt_strerror(offset));
+		return 0;
+	}
+
+	/*fixup the property of interrupts*/
+
+	tmp[0][0] = cpu_to_fdt32(0x0);
+	tmp[0][1] = cpu_to_fdt32(irq_0);
+	tmp[0][2] = cpu_to_fdt32(0x4);
+
+	if (rev > REV1_0) {
+		tmp[1][0] = cpu_to_fdt32(0x0);
+		tmp[1][1] = cpu_to_fdt32(irq_1);
+		tmp[1][2] = cpu_to_fdt32(0x4);
+		tmp[2][0] = cpu_to_fdt32(0x0);
+		tmp[2][1] = cpu_to_fdt32(irq_1 + 1);
+		tmp[2][2] = cpu_to_fdt32(0x4);
+		tmp[3][0] = cpu_to_fdt32(0x0);
+		tmp[3][1] = cpu_to_fdt32(irq_1 + 2);
+		tmp[3][2] = cpu_to_fdt32(0x4);
+		len = sizeof(tmp);
+	} else {
+		len = sizeof(tmp[0]);
+	}
+
+	err = fdt_setprop(blob, offset, "interrupts", tmp, len);
+	if (err < 0) {
+		printf("WARNING: fdt_setprop can't set %s from node %s: %s\n",
+		       "interrupts", name, fdt_strerror(err));
+		return 0;
+	}
+
+	/*fixup the property of reg*/
+	p = (char *)fdt_getprop(blob, offset, "reg", &len);
+	if (!p) {
+		printf("WARNING: fdt_getprop can't get %s from node %s\n",
+		       "reg", name);
+		return 0;
+	}
+
+	memcpy((char *)tmp, p, len);
+
+	if (rev > REV1_0)
+		*((u32 *)tmp + 3) = cpu_to_fdt32(0x1000);
+	else
+		*((u32 *)tmp + 3) = cpu_to_fdt32(0x8);
+
+	err = fdt_setprop(blob, offset, "reg", tmp, len);
+	if (err < 0) {
+		printf("WARNING: fdt_setprop can't set %s from node %s: %s\n",
+		       "reg", name, fdt_strerror(err));
+		return 0;
+	}
+
+	/*fixup the property of compatible*/
+	if (rev > REV1_0)
+		err = fdt_setprop_string(blob, offset, "compatible",
+					 "fsl,ls1043a-v1.1-msi");
+	else
+		err = fdt_setprop_string(blob, offset, "compatible",
+					 "fsl,ls1043a-msi");
+	if (err < 0) {
+		printf("WARNING: fdt_setprop can't set %s from node %s: %s\n",
+		       "compatible", name, fdt_strerror(err));
+		return 0;
+	}
+
+	return 1;
+}
+
+static int _fdt_fixup_pci_msi(void *blob, const char *name, int rev)
+{
+	int offset, len, err;
+	void *p;
+	int val;
+	u32 tmp[4][8];
+
+	offset = fdt_path_offset(blob, name);
+	if (offset < 0) {
+		printf("WARNING: fdt_path_offset can't find path %s: %s\n",
+		       name, fdt_strerror(offset));
+		return 0;
+	}
+
+	p = (char *)fdt_getprop(blob, offset, "interrupt-map", &len);
+	if (!p || len != sizeof(tmp)) {
+		printf("WARNING: fdt_getprop can't get %s from node %s\n",
+		       "interrupt-map", name);
+		return 0;
+	}
+
+	memcpy((char *)tmp, p, len);
+
+	val = fdt32_to_cpu(tmp[0][6]);
+	if (rev > REV1_0) {
+		tmp[1][6] = cpu_to_fdt32(val + 1);
+		tmp[2][6] = cpu_to_fdt32(val + 2);
+		tmp[3][6] = cpu_to_fdt32(val + 3);
+	} else {
+		tmp[1][6] = cpu_to_fdt32(val);
+		tmp[2][6] = cpu_to_fdt32(val);
+		tmp[3][6] = cpu_to_fdt32(val);
+	}
+
+	err = fdt_setprop(blob, offset, "interrupt-map", tmp, sizeof(tmp));
+	if (err < 0) {
+		printf("WARNING: fdt_setprop can't set %s from node %s: %s.\n",
+		       "interrupt-map", name, fdt_strerror(err));
+		return 0;
+	}
+	return 1;
+}
+
+/* Fixup msi node for ls1043a rev1.1*/
+
+static void fdt_fixup_msi(void *blob)
+{
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	unsigned int rev;
+
+	rev = gur_in32(&gur->svr);
+
+	if (SVR_SOC_VER(rev) != SVR_LS1043A)
+		return;
+
+	rev = SVR_REV(rev);
+
+	_fdt_fixup_msi_node(blob, "/soc/msi-controller1@1571000",
+			    116, 111, rev);
+	_fdt_fixup_msi_node(blob, "/soc/msi-controller2@1572000",
+			    126, 121, rev);
+	_fdt_fixup_msi_node(blob, "/soc/msi-controller3@1573000",
+			    160, 155, rev);
+
+	_fdt_fixup_pci_msi(blob, "/soc/pcie@3400000", rev);
+	_fdt_fixup_pci_msi(blob, "/soc/pcie@3500000", rev);
+	_fdt_fixup_pci_msi(blob, "/soc/pcie@3600000", rev);
+}
+#endif
+
 void ft_cpu_setup(void *blob, bd_t *bd)
 {
 #ifdef CONFIG_FSL_LSCH2
@@ -177,4 +389,10 @@ void ft_cpu_setup(void *blob, bd_t *bd)
 #endif
 	fsl_fdt_disable_usb(blob);
 
+#ifdef CONFIG_HAS_FEATURE_GIC64K_ALIGN
+	fdt_fixup_gic(blob);
+#endif
+#ifdef CONFIG_HAS_FEATURE_ENHANCED_MSI
+	fdt_fixup_msi(blob);
+#endif
 }
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c
index e06b063..c0b4d0a 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_serdes.c
@@ -129,6 +129,278 @@ void serdes_init(u32 sd, u32 sd_addr, u32 sd_prctl_mask, u32 sd_prctl_shift,
 	serdes_prtcl_map[NONE] = 1;
 }
 
+__weak int get_serdes_volt(void)
+{
+	return -1;
+}
+
+__weak int set_serdes_volt(int svdd)
+{
+	return -1;
+}
+
+int setup_serdes_volt(u32 svdd)
+{
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	struct ccsr_serdes *serdes1_base;
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	struct ccsr_serdes *serdes2_base;
+#endif
+	u32 cfg_rcw4 = gur_in32(&gur->rcwsr[4]);
+	u32 cfg_rcw5 = gur_in32(&gur->rcwsr[5]);
+	u32 cfg_tmp, reg = 0;
+	int svdd_cur, svdd_tar;
+	int ret;
+	int i;
+
+	/* Only support switch SVDD to 900mV/1000mV */
+	if (svdd != 900 && svdd != 1000)
+		return -EINVAL;
+
+	svdd_tar = svdd;
+	svdd_cur = get_serdes_volt();
+	if (svdd_cur < 0)
+		return -EINVAL;
+
+	debug("%s: current SVDD: %dmV; target SVDD: %dmV\n",
+	      __func__, svdd_cur, svdd_tar);
+	if (svdd_cur == svdd_tar)
+		return 0;
+
+	serdes1_base = (void *)CONFIG_SYS_FSL_SERDES_ADDR;
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	serdes2_base = (void *)serdes1_base + 0x10000;
+#endif
+
+	/* Put the all enabled lanes in reset */
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	cfg_tmp = cfg_rcw4 & FSL_CHASSIS2_RCWSR4_SRDS1_PRTCL_MASK;
+	cfg_tmp >>= FSL_CHASSIS2_RCWSR4_SRDS1_PRTCL_SHIFT;
+
+	for (i = 0; i < 4 && cfg_tmp & (0xf << (3 - i)); i++) {
+		reg = in_be32(&serdes1_base->lane[i].gcr0);
+		reg &= 0xFF9FFFFF;
+		out_be32(&serdes1_base->lane[i].gcr0, reg);
+	}
+#endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	cfg_tmp = cfg_rcw4 & FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_MASK;
+	cfg_tmp >>= FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_SHIFT;
+
+	for (i = 0; i < 4 && cfg_tmp & (0xf << (3 - i)); i++) {
+		reg = in_be32(&serdes2_base->lane[i].gcr0);
+		reg &= 0xFF9FFFFF;
+		out_be32(&serdes2_base->lane[i].gcr0, reg);
+	}
+#endif
+
+	/* Put the all enabled PLL in reset */
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	cfg_tmp = (cfg_rcw5 >> 22) & 0x3;
+	for (i = 0; i < 2 && !(cfg_tmp & (0x1 << (1 - i))); i++) {
+		reg = in_be32(&serdes1_base->bank[i].rstctl);
+		reg &= 0xFFFFFFBF;
+		reg |= 0x10000000;
+		out_be32(&serdes1_base->bank[i].rstctl, reg);
+		udelay(1);
+
+		reg = in_be32(&serdes1_base->bank[i].rstctl);
+		reg &= 0xFFFFFF1F;
+		out_be32(&serdes1_base->bank[i].rstctl, reg);
+	}
+	udelay(1);
+#endif
+
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	cfg_tmp = (cfg_rcw5 >> 20) & 0x3;
+	for (i = 0; i < 2 && !(cfg_tmp & (0x1 << (1 - i))); i++) {
+		reg = in_be32(&serdes2_base->bank[i].rstctl);
+		reg &= 0xFFFFFFBF;
+		reg |= 0x10000000;
+		out_be32(&serdes2_base->bank[i].rstctl, reg);
+		udelay(1);
+
+		reg = in_be32(&serdes2_base->bank[i].rstctl);
+		reg &= 0xFFFFFF1F;
+		out_be32(&serdes2_base->bank[i].rstctl, reg);
+	}
+	udelay(1);
+#endif
+
+	/* Put the Rx/Tx calibration into reset */
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	reg = in_be32(&serdes1_base->srdstcalcr);
+	reg &= 0xF7FFFFFF;
+	out_be32(&serdes1_base->srdstcalcr, reg);
+	reg = in_be32(&serdes1_base->srdsrcalcr);
+	reg &= 0xF7FFFFFF;
+	out_be32(&serdes1_base->srdsrcalcr, reg);
+
+#endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	reg = in_be32(&serdes2_base->srdstcalcr);
+	reg &= 0xF7FFFFFF;
+	out_be32(&serdes2_base->srdstcalcr, reg);
+	reg = in_be32(&serdes2_base->srdsrcalcr);
+	reg &= 0xF7FFFFFF;
+	out_be32(&serdes2_base->srdsrcalcr, reg);
+#endif
+
+	/*
+	 * If SVDD set failed, will not return directly, so that the
+	 * serdes lanes can complete reseting.
+	 */
+	ret = set_serdes_volt(svdd_tar);
+	if (ret)
+		printf("%s: Failed to set SVDD\n", __func__);
+
+	/* Wait for SVDD to stabilize */
+	udelay(100);
+
+	/* For each PLL that’s not disabled via RCW */
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	cfg_tmp = (cfg_rcw5 >> 22) & 0x3;
+	for (i = 0; i < 2 && !(cfg_tmp & (0x1 << (1 - i))); i++) {
+		reg = in_be32(&serdes1_base->bank[i].rstctl);
+		reg |= 0x00000020;
+		out_be32(&serdes1_base->bank[i].rstctl, reg);
+		udelay(1);
+
+		reg = in_be32(&serdes1_base->bank[i].rstctl);
+		reg |= 0x00000080;
+		out_be32(&serdes1_base->bank[i].rstctl, reg);
+
+		/* Take the Rx/Tx calibration out of reset */
+		if (!(cfg_tmp == 0x3 && i == 1)) {
+			udelay(1);
+			reg = in_be32(&serdes1_base->srdstcalcr);
+			reg |= 0x08000000;
+			out_be32(&serdes1_base->srdstcalcr, reg);
+			reg = in_be32(&serdes1_base->srdsrcalcr);
+			reg |= 0x08000000;
+			out_be32(&serdes1_base->srdsrcalcr, reg);
+		}
+	}
+	udelay(1);
+#endif
+
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	cfg_tmp = (cfg_rcw5 >> 20) & 0x3;
+	for (i = 0; i < 2 && !(cfg_tmp & (0x1 << (1 - i))); i++) {
+		reg = in_be32(&serdes2_base->bank[i].rstctl);
+		reg |= 0x00000020;
+		out_be32(&serdes2_base->bank[i].rstctl, reg);
+		udelay(1);
+
+		reg = in_be32(&serdes2_base->bank[i].rstctl);
+		reg |= 0x00000080;
+		out_be32(&serdes2_base->bank[i].rstctl, reg);
+
+		/* Take the Rx/Tx calibration out of reset */
+		if (!(cfg_tmp == 0x3 && i == 1)) {
+			udelay(1);
+			reg = in_be32(&serdes2_base->srdstcalcr);
+			reg |= 0x08000000;
+			out_be32(&serdes2_base->srdstcalcr, reg);
+			reg = in_be32(&serdes2_base->srdsrcalcr);
+			reg |= 0x08000000;
+			out_be32(&serdes2_base->srdsrcalcr, reg);
+		}
+	}
+	udelay(1);
+
+#endif
+
+	/* Wait for at lesat 625us to ensure the PLLs being reset are locked */
+	udelay(800);
+
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	cfg_tmp = (cfg_rcw5 >> 22) & 0x3;
+	for (i = 0; i < 2 && !(cfg_tmp & (0x1 << (1 - i))); i++) {
+		/* if the PLL is not locked, set RST_ERR */
+		reg = in_be32(&serdes1_base->bank[i].pllcr0);
+		if (!((reg >> 23) & 0x1)) {
+			reg = in_be32(&serdes1_base->bank[i].rstctl);
+			reg |= 0x20000000;
+			out_be32(&serdes1_base->bank[i].rstctl, reg);
+		} else {
+			udelay(1);
+			reg = in_be32(&serdes1_base->bank[i].rstctl);
+			reg &= 0xFFFFFFEF;
+			reg |= 0x00000040;
+			out_be32(&serdes1_base->bank[i].rstctl, reg);
+			udelay(1);
+		}
+	}
+#endif
+
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	cfg_tmp = (cfg_rcw5 >> 20) & 0x3;
+	for (i = 0; i < 2 && !(cfg_tmp & (0x1 << (1 - i))); i++) {
+		reg = in_be32(&serdes2_base->bank[i].pllcr0);
+		if (!((reg >> 23) & 0x1)) {
+			reg = in_be32(&serdes2_base->bank[i].rstctl);
+			reg |= 0x20000000;
+			out_be32(&serdes2_base->bank[i].rstctl, reg);
+		} else {
+			udelay(1);
+			reg = in_be32(&serdes2_base->bank[i].rstctl);
+			reg &= 0xFFFFFFEF;
+			reg |= 0x00000040;
+			out_be32(&serdes2_base->bank[i].rstctl, reg);
+			udelay(1);
+		}
+	}
+#endif
+
+	/* Take the all enabled lanes out of reset */
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	cfg_tmp = cfg_rcw4 & FSL_CHASSIS2_RCWSR4_SRDS1_PRTCL_MASK;
+	cfg_tmp >>= FSL_CHASSIS2_RCWSR4_SRDS1_PRTCL_SHIFT;
+
+	for (i = 0; i < 4 && cfg_tmp & (0xf << (3 - i)); i++) {
+		reg = in_be32(&serdes1_base->lane[i].gcr0);
+		reg |= 0x00600000;
+		out_be32(&serdes1_base->lane[i].gcr0, reg);
+	}
+#endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	cfg_tmp = cfg_rcw4 & FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_MASK;
+	cfg_tmp >>= FSL_CHASSIS2_RCWSR4_SRDS2_PRTCL_SHIFT;
+
+	for (i = 0; i < 4 && cfg_tmp & (0xf << (3 - i)); i++) {
+		reg = in_be32(&serdes2_base->lane[i].gcr0);
+		reg |= 0x00600000;
+		out_be32(&serdes2_base->lane[i].gcr0, reg);
+	}
+#endif
+	/* For each PLL being reset, and achieved PLL lock set RST_DONE */
+#ifdef CONFIG_SYS_FSL_SRDS_1
+	cfg_tmp = (cfg_rcw5 >> 22) & 0x3;
+	for (i = 0; i < 2; i++) {
+		reg = in_be32(&serdes1_base->bank[i].pllcr0);
+		if (!(cfg_tmp & (0x1 << (1 - i))) && ((reg >> 23) & 0x1)) {
+			reg = in_be32(&serdes1_base->bank[i].rstctl);
+			reg |= 0x40000000;
+			out_be32(&serdes1_base->bank[i].rstctl, reg);
+		}
+	}
+#endif
+#ifdef CONFIG_SYS_FSL_SRDS_2
+	cfg_tmp = (cfg_rcw5 >> 20) & 0x3;
+	for (i = 0; i < 2; i++) {
+		reg = in_be32(&serdes2_base->bank[i].pllcr0);
+		if (!(cfg_tmp & (0x1 << (1 - i))) && ((reg >> 23) & 0x1)) {
+			reg = in_be32(&serdes2_base->bank[i].rstctl);
+			reg |= 0x40000000;
+			out_be32(&serdes2_base->bank[i].rstctl, reg);
+		}
+	}
+#endif
+
+	return ret;
+}
+
 void fsl_serdes_init(void)
 {
 #ifdef CONFIG_SYS_FSL_SRDS_1
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c
index 55005f0..3da7037 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch2_speed.c
@@ -52,22 +52,28 @@ void get_sys_info(struct sys_info *sys_info)
 	uint freq_c_pll[CONFIG_SYS_FSL_NUM_CC_PLLS];
 	uint ratio[CONFIG_SYS_FSL_NUM_CC_PLLS];
 	unsigned long sysclk = CONFIG_SYS_CLK_FREQ;
+	unsigned long cluster_clk;
 
 	sys_info->freq_systembus = sysclk;
+#ifndef CONFIG_CLUSTER_CLK_FREQ
+#define CONFIG_CLUSTER_CLK_FREQ	CONFIG_SYS_CLK_FREQ
+#endif
+	cluster_clk = CONFIG_CLUSTER_CLK_FREQ;
+
 #ifdef CONFIG_DDR_CLK_FREQ
 	sys_info->freq_ddrbus = CONFIG_DDR_CLK_FREQ;
 #else
 	sys_info->freq_ddrbus = sysclk;
 #endif
 
-#ifdef CONFIG_ARCH_LS1012A
-	sys_info->freq_ddrbus *= (gur_in32(&gur->rcwsr[0]) >>
-			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_SHIFT) &
-			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_MASK;
-#else
+	/* The freq_systembus is used to record frequency of platform PLL */
 	sys_info->freq_systembus *= (gur_in32(&gur->rcwsr[0]) >>
 			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_SHIFT) &
 			FSL_CHASSIS2_RCWSR0_SYS_PLL_RAT_MASK;
+
+#ifdef CONFIG_ARCH_LS1012A
+	sys_info->freq_ddrbus = 2 * sys_info->freq_systembus;
+#else
 	sys_info->freq_ddrbus *= (gur_in32(&gur->rcwsr[0]) >>
 			FSL_CHASSIS2_RCWSR0_MEM_PLL_RAT_SHIFT) &
 			FSL_CHASSIS2_RCWSR0_MEM_PLL_RAT_MASK;
@@ -76,7 +82,7 @@ void get_sys_info(struct sys_info *sys_info)
 	for (i = 0; i < CONFIG_SYS_FSL_NUM_CC_PLLS; i++) {
 		ratio[i] = (in_be32(&clk->pllcgsr[i].pllcngsr) >> 1) & 0xff;
 		if (ratio[i] > 4)
-			freq_c_pll[i] = sysclk * ratio[i];
+			freq_c_pll[i] = cluster_clk * ratio[i];
 		else
 			freq_c_pll[i] = sys_info->freq_systembus * ratio[i];
 	}
@@ -91,11 +97,6 @@ void get_sys_info(struct sys_info *sys_info)
 			freq_c_pll[cplx_pll] / core_cplx_pll_div[c_pll_sel];
 	}
 
-#ifdef CONFIG_ARCH_LS1012A
-	sys_info->freq_systembus = sys_info->freq_ddrbus / 2;
-	sys_info->freq_ddrbus *= 2;
-#endif
-
 #define HWA_CGA_M1_CLK_SEL	0xe0000000
 #define HWA_CGA_M1_CLK_SHIFT	29
 #ifdef CONFIG_SYS_DPAA_FMAN
@@ -148,7 +149,9 @@ void get_sys_info(struct sys_info *sys_info)
 		break;
 	}
 #else
-	sys_info->freq_sdhc = sys_info->freq_systembus;
+	sys_info->freq_sdhc = (sys_info->freq_systembus /
+				CONFIG_SYS_FSL_PCLK_DIV) /
+				CONFIG_SYS_FSL_SDHC_CLK_DIV;
 #endif
 #endif
 
@@ -166,7 +169,7 @@ int get_clocks(void)
 
 	get_sys_info(&sys_info);
 	gd->cpu_clk = sys_info.freq_processor[0];
-	gd->bus_clk = sys_info.freq_systembus;
+	gd->bus_clk = sys_info.freq_systembus / CONFIG_SYS_FSL_PCLK_DIV;
 	gd->mem_clk = sys_info.freq_ddrbus;
 
 #ifdef CONFIG_FSL_ESDHC
@@ -179,41 +182,73 @@ int get_clocks(void)
 		return 1;
 }
 
+/********************************************
+ * get_bus_freq
+ * return platform clock in Hz
+ *********************************************/
 ulong get_bus_freq(ulong dummy)
 {
+	if (!gd->bus_clk)
+		get_clocks();
+
 	return gd->bus_clk;
 }
 
 ulong get_ddr_freq(ulong dummy)
 {
+	if (!gd->mem_clk)
+		get_clocks();
+
 	return gd->mem_clk;
 }
 
 #ifdef CONFIG_FSL_ESDHC
 int get_sdhc_freq(ulong dummy)
 {
+	if (!gd->arch.sdhc_clk)
+		get_clocks();
+
 	return gd->arch.sdhc_clk;
 }
 #endif
 
 int get_serial_clock(void)
 {
-	return gd->bus_clk;
+	return get_bus_freq(0) / CONFIG_SYS_FSL_DUART_CLK_DIV;
+}
+
+int get_i2c_freq(ulong dummy)
+{
+	return get_bus_freq(0) / CONFIG_SYS_FSL_I2C_CLK_DIV;
+}
+
+int get_dspi_freq(ulong dummy)
+{
+	return get_bus_freq(0) / CONFIG_SYS_FSL_DSPI_CLK_DIV;
 }
 
+#ifdef CONFIG_FSL_LPUART
+int get_uart_freq(ulong dummy)
+{
+	return get_bus_freq(0) / CONFIG_SYS_FSL_LPUART_CLK_DIV;
+}
+#endif
+
 unsigned int mxc_get_clock(enum mxc_clock clk)
 {
 	switch (clk) {
 	case MXC_I2C_CLK:
-		return get_bus_freq(0);
+		return get_i2c_freq(0);
 #if defined(CONFIG_FSL_ESDHC)
 	case MXC_ESDHC_CLK:
 		return get_sdhc_freq(0);
 #endif
 	case MXC_DSPI_CLK:
-		return get_bus_freq(0);
+		return get_dspi_freq(0);
+#ifdef CONFIG_FSL_LPUART
 	case MXC_UART_CLK:
-		return get_bus_freq(0);
+		return get_uart_freq(0);
+#endif
 	default:
 		printf("Unsupported clock\n");
 	}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c
index a9b12a4..f8fefc7 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/fsl_lsch3_speed.c
@@ -88,11 +88,10 @@ void get_sys_info(struct sys_info *sys_info)
 #endif
 #endif
 
+	/* The freq_systembus is used to record frequency of platform PLL */
 	sys_info->freq_systembus *= (gur_in32(&gur->rcwsr[0]) >>
 			FSL_CHASSIS3_RCWSR0_SYS_PLL_RAT_SHIFT) &
 			FSL_CHASSIS3_RCWSR0_SYS_PLL_RAT_MASK;
-	/* Platform clock is half of platform PLL */
-	sys_info->freq_systembus /= 2;
 	sys_info->freq_ddrbus *= (gur_in32(&gur->rcwsr[0]) >>
 			FSL_CHASSIS3_RCWSR0_MEM_PLL_RAT_SHIFT) &
 			FSL_CHASSIS3_RCWSR0_MEM_PLL_RAT_MASK;
@@ -142,13 +141,13 @@ int get_clocks(void)
 	struct sys_info sys_info;
 	get_sys_info(&sys_info);
 	gd->cpu_clk = sys_info.freq_processor[0];
-	gd->bus_clk = sys_info.freq_systembus;
+	gd->bus_clk = sys_info.freq_systembus / CONFIG_SYS_FSL_PCLK_DIV;
 	gd->mem_clk = sys_info.freq_ddrbus;
 #ifdef CONFIG_SYS_FSL_HAS_DP_DDR
 	gd->arch.mem2_clk = sys_info.freq_ddrbus2;
 #endif
 #if defined(CONFIG_FSL_ESDHC)
-	gd->arch.sdhc_clk = gd->bus_clk / 2;
+	gd->arch.sdhc_clk = gd->bus_clk / CONFIG_SYS_FSL_SDHC_CLK_DIV;
 #endif /* defined(CONFIG_FSL_ESDHC) */
 
 	if (gd->cpu_clk != 0)
@@ -159,7 +158,7 @@ int get_clocks(void)
 
 /********************************************
  * get_bus_freq
- * return system bus freq in Hz
+ * return platform clock in Hz
  *********************************************/
 ulong get_bus_freq(ulong dummy)
 {
@@ -190,13 +189,28 @@ ulong get_ddr_freq(ulong ctrl_num)
 	return gd->mem_clk;
 }
 
+int get_i2c_freq(ulong dummy)
+{
+	return get_bus_freq(0) / CONFIG_SYS_FSL_I2C_CLK_DIV;
+}
+
+int get_dspi_freq(ulong dummy)
+{
+	return get_bus_freq(0) / CONFIG_SYS_FSL_DSPI_CLK_DIV;
+}
+
+int get_serial_clock(void)
+{
+	return get_bus_freq(0) / CONFIG_SYS_FSL_DUART_CLK_DIV;
+}
+
 unsigned int mxc_get_clock(enum mxc_clock clk)
 {
 	switch (clk) {
 	case MXC_I2C_CLK:
-		return get_bus_freq(0) / 2;
+		return get_i2c_freq(0);
 	case MXC_DSPI_CLK:
-		return get_bus_freq(0) / 2;
+		return get_dspi_freq(0);
 	default:
 		printf("Unsupported clock\n");
 	}
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
index 72f2c11..a2185f2 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
+++ b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S
@@ -10,15 +10,66 @@
 #include <linux/linkage.h>
 #include <asm/gic.h>
 #include <asm/macro.h>
+#include <asm/arch-fsl-layerscape/soc.h>
 #ifdef CONFIG_MP
 #include <asm/arch/mp.h>
 #endif
 #ifdef CONFIG_FSL_LSCH3
 #include <asm/arch-fsl-layerscape/immap_lsch3.h>
-#include <asm/arch-fsl-layerscape/soc.h>
 #endif
 #include <asm/u-boot.h>
 
+/* Get GIC offset
+* For LS1043a rev1.0, GIC base address align with 4k.
+* For LS1043a rev1.1, if DCFG_GIC400_ALIGN[GIC_ADDR_BIT]
+* is set, GIC base address align with 4K, or else align
+* with 64k.
+* output:
+*	x0: the base address of GICD
+*	x1: the base address of GICC
+*/
+ENTRY(get_gic_offset)
+	ldr     x0, =GICD_BASE
+#ifdef CONFIG_GICV2
+	ldr     x1, =GICC_BASE
+#endif
+#ifdef CONFIG_HAS_FEATURE_GIC64K_ALIGN
+	ldr     x2, =DCFG_CCSR_SVR
+	ldr	w2, [x2]
+	rev	w2, w2
+	mov	w3, w2
+	ands	w3, w3, #SVR_WO_E << 8
+	mov	w4, #SVR_LS1043A << 8
+	cmp	w3, w4
+	b.ne	1f
+	ands	w2, w2, #0xff
+	cmp	w2, #REV1_0
+	b.eq	1f
+	ldr	x2, =SCFG_GIC400_ALIGN
+	ldr	w2, [x2]
+	rev	w2, w2
+	tbnz	w2, #GIC_ADDR_BIT, 1f
+	ldr     x0, =GICD_BASE_64K
+#ifdef CONFIG_GICV2
+	ldr     x1, =GICC_BASE_64K
+#endif
+1:
+#endif
+	ret
+ENDPROC(get_gic_offset)
+
+ENTRY(smp_kick_all_cpus)
+	/* Kick secondary cpus up by SGI 0 interrupt */
+#if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+	mov	x29, lr			/* Save LR */
+	bl	get_gic_offset
+	bl	gic_kick_secondary_cpus
+	mov	lr, x29			/* Restore LR */
+#endif
+	ret
+ENDPROC(smp_kick_all_cpus)
+
+
 ENTRY(lowlevel_init)
 	mov	x29, lr			/* Save LR */
 
@@ -29,6 +80,26 @@ ENTRY(lowlevel_init)
 	ldr	x0, =CCI_AUX_CONTROL_BASE(20)
 	ldr	x1, =0x00000010
 	bl	ccn504_set_aux
+
+	/*
+	 * Set forced-order mode in RNI-6, RNI-20
+	 * This is required for performance optimization on LS2088A
+	 * LS2080A family does not support setting forced-order mode,
+	 * so skip this operation for LS2080A family
+	 */
+	bl	get_svr
+	lsr	w0, w0, #16
+	ldr	w1, =SVR_DEV_LS2080A
+	cmp	w0, w1
+	b.eq	1f
+
+	ldr	x0, =CCI_AUX_CONTROL_BASE(6)
+	ldr	x1, =0x00000020
+	bl	ccn504_set_aux
+	ldr	x0, =CCI_AUX_CONTROL_BASE(20)
+	ldr	x1, =0x00000020
+	bl	ccn504_set_aux
+1:
 #endif
 
 	/* Add fully-coherent masters to DVM domain */
@@ -110,15 +181,14 @@ ENTRY(lowlevel_init)
 	/* Initialize GIC Secure Bank Status */
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
 	branch_if_slave x0, 1f
-	ldr	x0, =GICD_BASE
+	bl	get_gic_offset
 	bl	gic_init_secure
 1:
 #ifdef CONFIG_GICV3
 	ldr	x0, =GICR_BASE
 	bl	gic_init_secure_percpu
 #elif defined(CONFIG_GICV2)
-	ldr	x0, =GICD_BASE
-	ldr	x1, =GICC_BASE
+	bl	get_gic_offset
 	bl	gic_init_secure_percpu
 #endif
 #endif
@@ -209,10 +279,47 @@ ENTRY(lowlevel_init)
 	isb
 #endif
 
+#if defined(CONFIG_FSL_LSCH2) && !defined(CONFIG_SPL_BUILD)
+	bl	fsl_ocram_init
+#endif
+
 	mov	lr, x29			/* Restore LR */
 	ret
 ENDPROC(lowlevel_init)
 
+#if defined(CONFIG_FSL_LSCH2) && !defined(CONFIG_SPL_BUILD)
+ENTRY(fsl_ocram_init)
+	mov	x28, lr			/* Save LR */
+	bl	fsl_clear_ocram
+	bl	fsl_ocram_clear_ecc_err
+	mov	lr, x28			/* Restore LR */
+	ret
+ENDPROC(fsl_ocram_init)
+
+ENTRY(fsl_clear_ocram)
+/* Clear OCRAM */
+	ldr	x0, =CONFIG_SYS_FSL_OCRAM_BASE
+	ldr	x1, =(CONFIG_SYS_FSL_OCRAM_BASE + CONFIG_SYS_FSL_OCRAM_SIZE)
+	mov	x2, #0
+clear_loop:
+	str	x2, [x0]
+	add	x0, x0, #8
+	cmp	x0, x1
+	b.lo	clear_loop
+	ret
+ENDPROC(fsl_clear_ocram)
+
+ENTRY(fsl_ocram_clear_ecc_err)
+	/* OCRAM1/2 ECC status bit */
+	mov	w1, #0x60
+	ldr	x0, =DCSR_DCFG_SBEESR2
+	str	w1, [x0]
+	ldr	x0, =DCSR_DCFG_MBEESR2
+	str	w1, [x0]
+	ret
+ENDPROC(fsl_ocram_init)
+#endif
+
 #ifdef CONFIG_FSL_LSCH3
 	.globl get_svr
 get_svr:
@@ -356,7 +463,8 @@ ENTRY(secondary_boot_func)
 #if defined(CONFIG_GICV3)
 	gic_wait_for_interrupt_m x0
 #elif defined(CONFIG_GICV2)
-        ldr     x0, =GICC_BASE
+	bl	get_gic_offset
+	mov	x0, x1
         gic_wait_for_interrupt_m x0, w1
 #endif
 
@@ -378,29 +486,29 @@ cpu_is_le:
 	b.eq	1f
 
 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
-	adr	x3, secondary_switch_to_el1
-	ldr	x4, =ES_TO_AARCH64
+	adr	x4, secondary_switch_to_el1
+	ldr	x5, =ES_TO_AARCH64
 #else
-	ldr	x3, [x11]
-	ldr	x4, =ES_TO_AARCH32
+	ldr	x4, [x11]
+	ldr	x5, =ES_TO_AARCH32
 #endif
 	bl	secondary_switch_to_el2
 
 1:
 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
-	adr	x3, secondary_switch_to_el1
+	adr	x4, secondary_switch_to_el1
 #else
-	ldr	x3, [x11]
+	ldr	x4, [x11]
 #endif
-	ldr	x4, =ES_TO_AARCH64
+	ldr	x5, =ES_TO_AARCH64
 	bl	secondary_switch_to_el2
 
 ENDPROC(secondary_boot_func)
 
 ENTRY(secondary_switch_to_el2)
-	switch_el x5, 1f, 0f, 0f
+	switch_el x6, 1f, 0f, 0f
 0:	ret
-1:	armv8_switch_to_el2_m x3, x4, x5
+1:	armv8_switch_to_el2_m x4, x5, x6
 ENDPROC(secondary_switch_to_el2)
 
 ENTRY(secondary_switch_to_el1)
@@ -414,22 +522,22 @@ ENTRY(secondary_switch_to_el1)
 	/* physical address of this cpus spin table element */
 	add	x11, x1, x0
 
-	ldr	x3, [x11]
+	ldr	x4, [x11]
 
 	ldr	x5, [x11, #24]
 	ldr	x6, =IH_ARCH_DEFAULT
 	cmp	x6, x5
 	b.eq	2f
 
-	ldr	x4, =ES_TO_AARCH32
+	ldr	x5, =ES_TO_AARCH32
 	bl	switch_to_el1
 
-2:	ldr	x4, =ES_TO_AARCH64
+2:	ldr	x5, =ES_TO_AARCH64
 
 switch_to_el1:
-	switch_el x5, 0f, 1f, 0f
+	switch_el x6, 0f, 1f, 0f
 0:	ret
-1:	armv8_switch_to_el1_m x3, x4, x5
+1:	armv8_switch_to_el1_m x4, x5, x6
 ENDPROC(secondary_switch_to_el1)
 
 	/* Ensure that the literals used by the secondary boot code are
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/soc.c b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
index 2f54625..9489f85 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/soc.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
@@ -213,10 +213,12 @@ int sata_init(void)
 	ccsr_ahci  = (void *)CONFIG_SYS_SATA2;
 	out_le32(&ccsr_ahci->ppcfg, AHCI_PORT_PHY_1_CFG);
 	out_le32(&ccsr_ahci->ptc, AHCI_PORT_TRANS_CFG);
+	out_le32(&ccsr_ahci->axicc, AHCI_PORT_AXICC_CFG);
 
 	ccsr_ahci  = (void *)CONFIG_SYS_SATA1;
 	out_le32(&ccsr_ahci->ppcfg, AHCI_PORT_PHY_1_CFG);
 	out_le32(&ccsr_ahci->ptc, AHCI_PORT_TRANS_CFG);
+	out_le32(&ccsr_ahci->axicc, AHCI_PORT_AXICC_CFG);
 
 	ahci_init((void __iomem *)CONFIG_SYS_SATA1);
 	scsi_scan(0);
@@ -336,6 +338,95 @@ static void erratum_a010539(void)
 #endif
 }
 
+/* Get VDD in the unit mV from voltage ID */
+int get_core_volt_from_fuse(void)
+{
+	struct ccsr_gur *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	int vdd;
+	u32 fusesr;
+	u8 vid;
+
+	fusesr = in_be32(&gur->dcfg_fusesr);
+	debug("%s: fusesr = 0x%x\n", __func__, fusesr);
+	vid = (fusesr >> FSL_CHASSIS2_DCFG_FUSESR_ALTVID_SHIFT) &
+		FSL_CHASSIS2_DCFG_FUSESR_ALTVID_MASK;
+	if ((vid == 0) || (vid == FSL_CHASSIS2_DCFG_FUSESR_ALTVID_MASK)) {
+		vid = (fusesr >> FSL_CHASSIS2_DCFG_FUSESR_VID_SHIFT) &
+			FSL_CHASSIS2_DCFG_FUSESR_VID_MASK;
+	}
+	debug("%s: VID = 0x%x\n", __func__, vid);
+	switch (vid) {
+	case 0x00: /* VID isn't supported */
+		vdd = -EINVAL;
+		debug("%s: The VID feature is not supported\n", __func__);
+		break;
+	case 0x08: /* 0.9V silicon */
+		vdd = 900;
+		break;
+	case 0x10: /* 1.0V silicon */
+		vdd = 1000;
+		break;
+	default:  /* Other core voltage */
+		vdd = -EINVAL;
+		printf("%s: The VID(%x) isn't supported\n", __func__, vid);
+		break;
+	}
+	debug("%s: The required minimum volt of CORE is %dmV\n", __func__, vdd);
+
+	return vdd;
+}
+
+__weak int board_switch_core_volt(u32 vdd)
+{
+	return 0;
+}
+
+static int setup_core_volt(u32 vdd)
+{
+	return board_setup_core_volt(vdd);
+}
+
+#ifdef CONFIG_SYS_FSL_DDR
+static void ddr_enable_0v9_volt(bool en)
+{
+	struct ccsr_ddr __iomem *ddr = (void *)CONFIG_SYS_FSL_DDR_ADDR;
+	u32 tmp;
+
+	tmp = ddr_in32(&ddr->ddr_cdr1);
+
+	if (en)
+		tmp |= DDR_CDR1_V0PT9_EN;
+	else
+		tmp &= ~DDR_CDR1_V0PT9_EN;
+
+	ddr_out32(&ddr->ddr_cdr1, tmp);
+}
+#endif
+
+int setup_chip_volt(void)
+{
+	int vdd;
+
+	vdd = get_core_volt_from_fuse();
+	/* Nothing to do for silicons doesn't support VID */
+	if (vdd < 0)
+		return vdd;
+
+	if (setup_core_volt(vdd))
+		printf("%s: Switch core VDD to %dmV failed\n", __func__, vdd);
+#ifdef CONFIG_SYS_HAS_SERDES
+	if (setup_serdes_volt(vdd))
+		printf("%s: Switch SVDD to %dmV failed\n", __func__, vdd);
+#endif
+
+#ifdef CONFIG_SYS_FSL_DDR
+	if (vdd == 900)
+		ddr_enable_0v9_volt(true);
+#endif
+
+	return 0;
+}
+
 void fsl_lsch2_early_init_f(void)
 {
 	struct ccsr_cci400 *cci = (struct ccsr_cci400 *)CONFIG_SYS_CCI400_ADDR;
diff --git a/arch/arm/cpu/armv8/sec_firmware.c b/arch/arm/cpu/armv8/sec_firmware.c
index 0b973f0..ec9cf40 100644
--- a/arch/arm/cpu/armv8/sec_firmware.c
+++ b/arch/arm/cpu/armv8/sec_firmware.c
@@ -209,7 +209,7 @@ __weak bool sec_firmware_is_valid(const void *sec_firmware_img)
 	return true;
 }
 
-#ifdef CONFIG_FSL_PPA_ARMV8_PSCI
+#ifdef CONFIG_SEC_FIRMWARE_ARMV8_PSCI
 /*
  * The PSCI_VERSION function is added from PSCI v0.2. When the PSCI
  * v0.1 received this function, the NOT_SUPPORTED (0xffff_ffff) error
diff --git a/arch/arm/cpu/armv8/sec_firmware_asm.S b/arch/arm/cpu/armv8/sec_firmware_asm.S
index 903195d..30563eb 100644
--- a/arch/arm/cpu/armv8/sec_firmware_asm.S
+++ b/arch/arm/cpu/armv8/sec_firmware_asm.S
@@ -23,12 +23,12 @@ WEAK(_sec_firmware_entry)
 	/* Set exception return address hold pointer */
         adr	x4, 1f
         mov	x3, x4
-#ifdef SEC_FIRMWARE_ERET_ADDR_REVERT
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_ERET_ADDR_REVERT
         rev	w3, w3
 #endif
         str	w3, [x1]
         lsr	x3, x4, #32
-#ifdef SEC_FIRMWARE_ERET_ADDR_REVERT
+#ifdef CONFIG_ARMV8_SEC_FIRMWARE_ERET_ADDR_REVERT
         rev	w3, w3
 #endif
         str	w3, [x2]
@@ -41,7 +41,7 @@ WEAK(_sec_firmware_entry)
         ret
 ENDPROC(_sec_firmware_entry)
 
-#ifdef CONFIG_FSL_PPA_ARMV8_PSCI
+#ifdef CONFIG_SEC_FIRMWARE_ARMV8_PSCI
 ENTRY(_sec_firmware_support_psci_version)
 	mov	x0, 0x84000000
 	mov	x1, 0x0
@@ -57,7 +57,8 @@ ENDPROC(_sec_firmware_support_psci_version)
  * x0: argument, zero
  * x1: machine nr
  * x2: fdt address
- * x3: kernel entry point
+ * x3: input argument
+ * x4: kernel entry point
  * @param outputs for secure firmware:
  * x0: function id
  * x1: kernel entry point
@@ -65,10 +66,9 @@ ENDPROC(_sec_firmware_support_psci_version)
  * x3: fdt address
 */
 ENTRY(armv8_el2_to_aarch32)
-	mov	x0, x3
 	mov	x3, x2
 	mov	x2, x1
-	mov	x1, x0
+	mov	x1, x4
 	ldr	x0, =0xc000ff04
 	smc	#0
 	ret
diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index 9535057..62d97f7 100644
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -85,6 +85,17 @@ save_boot_params_ret:
 	msr	cpacr_el1, x0			/* Enable FP/SIMD */
 0:
 
+	/*
+	 * Enalbe SMPEN bit for coherency.
+	 * This register is not architectural but at the moment
+	 * this bit should be set for A53/A57/A72.
+	 */
+#ifdef CONFIG_ARMV8_SET_SMPEN
+	mrs     x0, S3_1_c15_c2_1               /* cpuactlr_el1 */
+	orr     x0, x0, #0x40
+	msr     S3_1_c15_c2_1, x0
+#endif
+
 	/* Apply ARM core specific erratas */
 	bl	apply_core_errata
 
@@ -250,14 +261,14 @@ WEAK(lowlevel_init)
 	/*
 	 * All slaves will enter EL2 and optionally EL1.
 	 */
-	adr	x3, lowlevel_in_el2
-	ldr	x4, =ES_TO_AARCH64
+	adr	x4, lowlevel_in_el2
+	ldr	x5, =ES_TO_AARCH64
 	bl	armv8_switch_to_el2
 
 lowlevel_in_el2:
 #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
-	adr	x3, lowlevel_in_el1
-	ldr	x4, =ES_TO_AARCH64
+	adr	x4, lowlevel_in_el1
+	ldr	x5, =ES_TO_AARCH64
 	bl	armv8_switch_to_el1
 
 lowlevel_in_el1:
diff --git a/arch/arm/cpu/armv8/transition.S b/arch/arm/cpu/armv8/transition.S
index adb9f35..ca07465 100644
--- a/arch/arm/cpu/armv8/transition.S
+++ b/arch/arm/cpu/armv8/transition.S
@@ -11,9 +11,9 @@
 #include <asm/macro.h>
 
 ENTRY(armv8_switch_to_el2)
-	switch_el x5, 1f, 0f, 0f
+	switch_el x6, 1f, 0f, 0f
 0:
-	cmp x4, #ES_TO_AARCH64
+	cmp x5, #ES_TO_AARCH64
 	b.eq 2f
 	/*
 	 * When loading 32-bit kernel, it will jump
@@ -22,23 +22,23 @@ ENTRY(armv8_switch_to_el2)
 	bl armv8_el2_to_aarch32
 2:
 	/*
-	 * x3 is kernel entry point or switch_to_el1
+	 * x4 is kernel entry point or switch_to_el1
 	 * if CONFIG_ARMV8_SWITCH_TO_EL1 is defined.
          * When running in EL2 now, jump to the
-	 * address saved in x3.
+	 * address saved in x4.
 	 */
-	br x3
-1:	armv8_switch_to_el2_m x3, x4, x5
+	br x4
+1:	armv8_switch_to_el2_m x4, x5, x6
 ENDPROC(armv8_switch_to_el2)
 
 ENTRY(armv8_switch_to_el1)
-	switch_el x5, 0f, 1f, 0f
+	switch_el x6, 0f, 1f, 0f
 0:
-	/* x3 is kernel entry point. When running in EL1
-	 * now, jump to the address saved in x3.
+	/* x4 is kernel entry point. When running in EL1
+	 * now, jump to the address saved in x4.
 	 */
-	br x3
-1:	armv8_switch_to_el1_m x3, x4, x5
+	br x4
+1:	armv8_switch_to_el1_m x4, x5, x6
 ENDPROC(armv8_switch_to_el1)
 
 WEAK(armv8_el2_to_aarch32)