diff --git a/Makefile b/Makefile
index 4dbc2be..b4bebf1 100644
--- a/Makefile
+++ b/Makefile
@@ -746,6 +746,10 @@
     endif
 endif
 
+ifeq ($(PSA_FWU_SUPPORT),1)
+    $(info PSA_FWU_SUPPORT is an experimental feature)
+endif
+
 ifeq (${ARM_XLAT_TABLES_LIB_V1}, 1)
     ifeq (${ALLOW_RO_XLAT_TABLES}, 1)
         $(error "ALLOW_RO_XLAT_TABLES requires translation tables library v2")
@@ -959,6 +963,7 @@
         USE_SP804_TIMER \
         ENABLE_FEAT_RNG \
         ENABLE_FEAT_SB \
+        PSA_FWU_SUPPORT \
 )))
 
 $(eval $(call assert_numerics,\
@@ -967,6 +972,8 @@
         ARM_ARCH_MINOR \
         BRANCH_PROTECTION \
         FW_ENC_STATUS \
+        NR_OF_FW_BANKS \
+        NR_OF_IMAGES_IN_FW_BANK \
 )))
 
 ifdef KEY_SIZE
@@ -1054,6 +1061,9 @@
         USE_SP804_TIMER \
         ENABLE_FEAT_RNG \
         ENABLE_FEAT_SB \
+        NR_OF_FW_BANKS \
+        NR_OF_IMAGES_IN_FW_BANK \
+        PSA_FWU_SUPPORT \
 )))
 
 ifeq (${SANITIZE_UB},trap)
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index 203e1d4..d2de135 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 #include <common/debug.h>
 #include <drivers/auth/auth_mod.h>
 #include <drivers/console.h>
+#include <drivers/fwu/fwu.h>
 #if MEASURED_BOOT
 #include <drivers/measured_boot/measured_boot.h>
 #endif
@@ -88,6 +89,10 @@
 	/* Perform remaining generic architectural setup in S-EL1 */
 	bl2_arch_setup();
 
+#if PSA_FWU_SUPPORT
+	fwu_init();
+#endif /* PSA_FWU_SUPPORT */
+
 #if TRUSTED_BOARD_BOOT
 	/* Initialize authentication module */
 	auth_mod_init();
diff --git a/common/bl_common.c b/common/bl_common.c
index f17afcb..a7e2816 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -239,9 +239,18 @@
 {
 	int err;
 
+/*
+ * All firmware banks should be part of the same non-volatile storage as per
+ * PSA FWU specification, hence don't check for any alternate boot source
+ * when PSA FWU is enabled.
+ */
+#if PSA_FWU_SUPPORT
+	err = load_auth_image_internal(image_id, image_data);
+#else
 	do {
 		err = load_auth_image_internal(image_id, image_data);
 	} while ((err != 0) && (plat_try_next_boot_source() != 0));
+#endif /* PSA_FWU_SUPPORT */
 
 	return err;
 }
diff --git a/common/hw_crc32.c b/common/tf_crc32.c
similarity index 86%
rename from common/hw_crc32.c
rename to common/tf_crc32.c
index a8731da..b33d36e 100644
--- a/common/hw_crc32.c
+++ b/common/tf_crc32.c
@@ -9,8 +9,9 @@
 
 #include <arm_acle.h>
 #include <common/debug.h>
+#include <common/tf_crc32.h>
 
-/* hw_crc32 - compute CRC using Arm intrinsic function
+/* compute CRC using Arm intrinsic function
  *
  * This function is useful for the platforms with the CPU ARMv8.0
  * (with CRC instructions supported), and onwards.
@@ -23,7 +24,7 @@
  *
  * Return calculated CRC value
  */
-uint32_t hw_crc32(uint32_t crc, const unsigned char *buf, size_t size)
+uint32_t tf_crc32(uint32_t crc, const unsigned char *buf, size_t size)
 {
 	assert(buf != NULL);
 
diff --git a/common/tf_log.c b/common/tf_log.c
index 08d3cf4..68f1be4 100644
--- a/common/tf_log.c
+++ b/common/tf_log.c
@@ -49,6 +49,20 @@
 	va_end(args);
 }
 
+void tf_log_newline(const char log_fmt[2])
+{
+	unsigned int log_level = log_fmt[0];
+
+	/* Verify that log_level is one of LOG_MARKER_* macro defined in debug.h */
+	assert((log_level > 0U) && (log_level <= LOG_LEVEL_VERBOSE));
+	assert((log_level % 10U) == 0U);
+
+	if (log_level > max_log_level)
+		return;
+
+	putchar('\n');
+}
+
 /*
  * The helper function to set the log level dynamically by platform. The
  * maximum log level is determined by `LOG_LEVEL` build flag at compile time
diff --git a/common/uuid.c b/common/uuid.c
index dd3c7b0..ac6db50 100644
--- a/common/uuid.c
+++ b/common/uuid.c
@@ -73,6 +73,7 @@
 int read_uuid(uint8_t *dest, char *uuid)
 {
 	int err;
+	uint8_t *dest_start = dest;
 
 	/* Check that we have enough characters */
 	if (strnlen(uuid, UUID_STRING_LENGTH) != UUID_STRING_LENGTH) {
@@ -124,7 +125,7 @@
 	if (err < 0) {
 		WARN("Error parsing UUID\n");
 		/* Clear the buffer on error */
-		memset((void *)dest, '\0', UUID_BYTES_LENGTH * sizeof(uint8_t));
+		memset((void *)dest_start, '\0', UUID_BYTES_LENGTH * sizeof(uint8_t));
 		return -EINVAL;
 	}
 
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index d014fbd..07f258c 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -399,6 +399,7 @@
 :|G|: `vishnu-banavath`_
 :|F|: plat/arm/board/corstone700
 :|F|: plat/arm/board/a5ds
+:|F|: plat/arm/board/diphda
 
 Arm Reference Design platform ports
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -414,13 +415,13 @@
 :|F|: plat/arm/board/rdv1mc/
 :|F|: plat/arm/board/sgi575/
 
-Arm Total Compute(tc0) platform port
+Arm Total Compute platform port
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
 :|G|: `arugan02`_
 :|M|: Usama Arif <usama.arif@arm.com>
 :|G|: `uarif1`_
-:|F|: plat/arm/board/tc0
+:|F|: plat/arm/board/tc
 
 HiSilicon HiKey and HiKey960 platform ports
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -493,8 +494,8 @@
 
 NXP i.MX 8 platform port
 ^^^^^^^^^^^^^^^^^^^^^^^^
-:|M|: Anson Huang <Anson.Huang@nxp.com>
-:|G|: `Anson-Huang`_
+:|M|: Peng Fan <peng.fan@nxp.com>
+:|G|: `MrVan`_
 :|F|: docs/plat/imx8.rst
 :|F|: plat/imx/
 
@@ -594,6 +595,8 @@
 :|G|: `rockchip-linux`_
 :|M|: Heiko Stuebner <heiko@sntech.de>
 :|G|: `mmind`_
+:|M|: Julius Werner <jwerner@chromium.org>
+:|G|: `jwerner-chromium`_
 :|F|: plat/rockchip/
 
 STM32MP1 platform port
diff --git a/docs/components/ffa-manifest-binding.rst b/docs/components/ffa-manifest-binding.rst
index 9e3919d..437df67 100644
--- a/docs/components/ffa-manifest-binding.rst
+++ b/docs/components/ffa-manifest-binding.rst
@@ -106,14 +106,14 @@
      The "compatible" must be the string "arm,ffa-manifest-rx_tx-buffer".
 
 - messaging-method [mandatory]
-   - value type: <u32>
-   - Specifies which messaging methods are supported by the partition:
+   - value type: <u8>
+   - Specifies which messaging methods are supported by the partition, set bit
+     means the feature is supported, clear bit - not supported:
 
-      - 0x0: direct messaging method
-      - 0x1: indirect messaging method
-      - 0x2: both direct and indirect messaging methods
-      - 0x3: direct messaging method with managed exit support
-      - 0x4: both messaging methods with managed exit support
+      - Bit[0]: support for receiving direct message requests
+      - Bit[1]: support for sending direct messages
+      - Bit[2]: support for indirect messaging
+      - Bit[3]: support for managed exit
 
 - has-primary-scheduler
    - value type: <empty>
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index ab638fb..bc277a7 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -323,6 +323,36 @@
    CPU. This needs to be enabled for revisions r3p0, r3p1, r4p0, and r4p1, for
    revisions r0p0, r1p0, and r2p0 there is no workaround.
 
+For Neoverse V1, the following errata build flags are defined :
+
+-  ``ERRATA_V1_1774420``: This applies errata 1774420 workaround to Neoverse-V1
+   CPU. This needs to be enabled only for revisions r0p0 and r1p0, it is fixed
+   in r1p1.
+
+-  ``ERRATA_V1_1791573``: This applies errata 1791573 workaround to Neoverse-V1
+   CPU. This needs to be enabled only for revisions r0p0 and r1p0, it is fixed
+   in r1p1.
+
+-  ``ERRATA_V1_1852267``: This applies errata 1852267 workaround to Neoverse-V1
+   CPU. This needs to be enabled only for revisions r0p0 and r1p0, it is fixed
+   in r1p1.
+
+-  ``ERRATA_V1_1925756``: This applies errata 1925756 workaround to Neoverse-V1
+   CPU. This needs to be enabled for r0p0, r1p0, and r1p1, it is still open.
+
+-  ``ERRATA_V1_1940577``: This applies errata 1940577 workaround to Neoverse-V1
+   CPU. This needs to be enabled only for revision r1p0 and r1p1 of the
+   CPU.
+
+-  ``ERRATA_V1_1966096``: This applies errata 1966096 workaround to Neoverse-V1
+   CPU. This needs to be enabled for revisions r1p0 and r1p1 of the CPU, the
+   issue is present in r0p0 as well but there is no workaround for that
+   revision.  It is still open.
+
+-  ``ERRATA_V1_2139242``: This applies errata 2139242 workaround to Neoverse-V1
+   CPU. This needs to be enabled for revisions r0p0, r1p0, and r1p1 of the
+   CPU.  It is still open.
+
 DSU Errata Workarounds
 ----------------------
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 86618e4..901a72a 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -860,9 +860,31 @@
     # Resume execution
     continue
 
+Firmware update options
+-----------------------
+
+-  ``NR_OF_FW_BANKS``: Define the number of firmware banks. This flag is used
+   in defining the firmware update metadata structure. This flag is by default
+   set to '2'.
+
+-  ``NR_OF_IMAGES_IN_FW_BANK``: Define the number of firmware images in each
+   firmware bank. Each firmware bank must have the same number of images as per
+   the `PSA FW update specification`_.
+   This flag is used in defining the firmware update metadata structure. This
+   flag is by default set to '1'.
+
+-  ``PSA_FWU_SUPPORT``: Enable the firmware update mechanism as per the
+   `PSA FW update specification`_. The default value is 0, and this is an
+   experimental feature.
+   PSA firmware update implementation has some limitations, such as BL2 is
+   not part of the protocol-updatable images, if BL2 needs to be updated, then
+   it should be done through another platform-defined mechanism, and it assumes
+   that the platform's hardware supports CRC32 instructions.
+
 --------------
 
 *Copyright (c) 2019-2021, Arm Limited. All rights reserved.*
 
 .. _DEN0115: https://developer.arm.com/docs/den0115/latest
+.. _PSA FW update specification: https://developer.arm.com/documentation/den0118/a/
 
diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 906daf8..54754fe 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -894,6 +894,54 @@
 Note that this API depends on ``DECRYPTION_SUPPORT`` build flag which is
 marked as experimental.
 
+Function : plat_fwu_set_images_source() [when PSA_FWU_SUPPORT == 1]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : struct fwu_metadata *metadata
+    Return   : void
+
+This function is mandatory when PSA_FWU_SUPPORT is enabled.
+It provides a means to retrieve image specification (offset in
+non-volatile storage and length) of active/updated images using the passed
+FWU metadata, and update I/O policies of active/updated images using retrieved
+image specification information.
+Further I/O layer operations such as I/O open, I/O read, etc. on these
+images rely on this function call.
+
+In Arm platforms, this function is used to set an I/O policy of the FIP image,
+container of all active/updated secure and non-secure images.
+
+Function : plat_fwu_set_metadata_image_source() [when PSA_FWU_SUPPORT == 1]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : unsigned int image_id, uintptr_t *dev_handle,
+               uintptr_t *image_spec
+    Return   : int
+
+This function is mandatory when PSA_FWU_SUPPORT is enabled. It is
+responsible for setting up the platform I/O policy of the requested metadata
+image (either FWU_METADATA_IMAGE_ID or BKUP_FWU_METADATA_IMAGE_ID) that will
+be used to load this image from the platform's non-volatile storage.
+
+FWU metadata can not be always stored as a raw image in non-volatile storage
+to define its image specification (offset in non-volatile storage and length)
+statically in I/O policy.
+For example, the FWU metadata image is stored as a partition inside the GUID
+partition table image. Its specification is defined in the partition table
+that needs to be parsed dynamically.
+This function provides a means to retrieve such dynamic information to set
+the I/O policy of the FWU metadata image.
+Further I/O layer operations such as I/O open, I/O read, etc. on FWU metadata
+image relies on this function call.
+
+It returns '0' on success, otherwise a negative error value on error.
+Alongside, returns device handle and image specification from the I/O policy
+of the requested FWU metadata image.
+
 Common optional modifications
 -----------------------------
 
diff --git a/docs/index.rst b/docs/index.rst
index 29e5839..edc2535 100644
--- a/docs/index.rst
+++ b/docs/index.rst
@@ -30,6 +30,7 @@
 -  `SMC Calling Convention`_
 -  `System Control and Management Interface (SCMI)`_
 -  `Software Delegated Exception Interface (SDEI)`_
+-  `PSA FW update specification`_
 
 Where possible, the code is designed for reuse or porting to other Armv7-A and
 Armv8-A model and hardware platforms.
@@ -92,3 +93,4 @@
 .. _System Control and Management Interface (SCMI): http://infocenter.arm.com/help/topic/com.arm.doc.den0056a/DEN0056A_System_Control_and_Management_Interface.pdf
 .. _Software Delegated Exception Interface (SDEI): http://infocenter.arm.com/help/topic/com.arm.doc.den0054a/ARM_DEN0054A_Software_Delegated_Exception_Interface.pdf
 .. _SMC Calling Convention: https://developer.arm.com/docs/den0028/latest
+.. _PSA FW update specification: https://developer.arm.com/documentation/den0118/a/
diff --git a/docs/plat/arm/diphda/index.rst b/docs/plat/arm/diphda/index.rst
new file mode 100644
index 0000000..27afda4
--- /dev/null
+++ b/docs/plat/arm/diphda/index.rst
@@ -0,0 +1,61 @@
+Diphda Platform
+==========================
+
+Some of the features of the Diphda platform referenced in TF-A include:
+
+- Cortex-A35 application processor (64-bit mode)
+- Secure Enclave
+- GIC-400
+- Trusted Board Boot
+
+Boot Sequence
+-------------
+
+The board boot relies on CoT (chain of trust). The trusted-firmware-a
+BL2 is extracted from the FIP and verified by the Secure Enclave
+processor. BL2 verification relies on the signature area at the
+beginning of the BL2 image. This area is needed by the SecureEnclave
+bootloader.
+
+Then, the application processor is released from reset and starts by
+executing BL2.
+
+BL2 performs the actions described in the trusted-firmware-a TBB design
+document.
+
+Build Procedure (TF-A only)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+-  Obtain AArch64 ELF bare-metal target `toolchain <https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/gnu-a/downloads>`_.
+   Set the CROSS_COMPILE environment variable to point to the toolchain folder.
+
+-  Build TF-A:
+
+   .. code:: shell
+
+      make LD=aarch64-none-elf-ld \
+      CC=aarch64-none-elf-gcc \
+      V=1 \
+      BUILD_BASE=<path to the build folder> \
+      PLAT=diphda \
+      SPD=spmd \
+      SPMD_SPM_AT_SEL2=0 \
+      DEBUG=1 \
+      MBEDTLS_DIR=mbedtls \
+      OPENSSL_DIR=<path to openssl usr folder> \
+      RUNTIME_SYSROOT=<path to the sysroot> \
+      ARCH=aarch64 \
+      TARGET_PLATFORM=<fpga or fvp> \
+      ENABLE_PIE=1 \
+      BL2_AT_EL3=1 \
+      CREATE_KEYS=1 \
+      GENERATE_COT=1 \
+      TRUSTED_BOARD_BOOT=1 \
+      COT=tbbr \
+      ARM_ROTPK_LOCATION=devel_rsa \
+      ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem \
+      BL32=<path to optee binary> \
+      BL33=<path to u-boot binary> \
+      bl2
+
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/docs/plat/arm/fvp/index.rst b/docs/plat/arm/fvp/index.rst
index 710043e..d41982f 100644
--- a/docs/plat/arm/fvp/index.rst
+++ b/docs/plat/arm/fvp/index.rst
@@ -12,7 +12,7 @@
 (64-bit host machine only).
 
 .. note::
-   The FVP models used are Version 11.14 Build 21, unless otherwise stated.
+   The FVP models used are Version 11.15 Build 14, unless otherwise stated.
 
 -  ``FVP_Base_AEMvA``
 -  ``FVP_Base_AEMv8A-AEMv8A``
@@ -39,7 +39,7 @@
 -  ``FVP_Base_Cortex-A76AEx8``
 -  ``FVP_Base_Cortex-A77x4``
 -  ``FVP_Base_Cortex-A78x4``
--  ``FVP_Base_Matterhornx4``
+-  ``FVP_Base_Cortex-A710x4``
 -  ``FVP_Morello``         (Version 0.10 build 542)
 -  ``FVP_Base_Neoverse-E1x1``
 -  ``FVP_Base_Neoverse-E1x2``
diff --git a/docs/plat/arm/index.rst b/docs/plat/arm/index.rst
index f72992b..c834f6a 100644
--- a/docs/plat/arm/index.rst
+++ b/docs/plat/arm/index.rst
@@ -8,10 +8,11 @@
    juno/index
    fvp/index
    fvp-ve/index
-   tc0/index
+   tc/index
    arm_fpga/index
    arm-build-options
    morello/index
+   diphda/index
 
 This chapter holds documentation related to Arm's development platforms,
 including both software models (FVPs) and hardware development boards
@@ -19,4 +20,4 @@
 
 --------------
 
-*Copyright (c) 2019, Arm Limited. All rights reserved.*
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/docs/plat/arm/tc0/index.rst b/docs/plat/arm/tc/index.rst
similarity index 73%
rename from docs/plat/arm/tc0/index.rst
rename to docs/plat/arm/tc/index.rst
index 34d1f13..20d3e56 100644
--- a/docs/plat/arm/tc0/index.rst
+++ b/docs/plat/arm/tc/index.rst
@@ -1,7 +1,7 @@
-TC0 Total Compute Platform
+TC Total Compute Platform
 ==========================
 
-Some of the features of TC0 platform referenced in TF-A include:
+Some of the features of TC platform referenced in TF-A include:
 
 - A `System Control Processor <https://github.com/ARM-software/SCP-firmware>`_
   to abstract power and system management tasks away from application
@@ -13,6 +13,12 @@
 - SCMI
 - MHUv2
 
+Currently, the main difference between TC0 (TARGET_PLATFORM=0) and TC1
+(TARGET_PLATFORM=1) platforms w.r.t to TF-A is the CPUs supported. TC0 has
+support for Cortex A510, Cortex A710 and Cortex X2, while TC1 has support for
+Cortex A510, Cortex Makalu and Cortex Makalu ELP Arm CPUs.
+
+
 Boot Sequence
 -------------
 
@@ -34,8 +40,8 @@
 
    .. code:: shell
 
-      make PLAT=tc0 BL33=<path_to_uboot.bin> \
-      SCP_BL2=<path_to_scp_ramfw.bin>  all fip
+      make PLAT=tc BL33=<path_to_uboot.bin> \
+      SCP_BL2=<path_to_scp_ramfw.bin> TARGET_PLATFORM={0,1} all fip
 
    Enable TBBR by adding the following options to the make command:
 
@@ -47,4 +53,4 @@
       ARM_ROTPK_LOCATION=devel_rsa  \
       ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem
 
-*Copyright (c) 2020, Arm Limited. All rights reserved.*
+*Copyright (c) 2020-2021, Arm Limited. All rights reserved.*
diff --git a/docs/plat/deprecated.rst b/docs/plat/deprecated.rst
index 203ae00..7cc4258 100644
--- a/docs/plat/deprecated.rst
+++ b/docs/plat/deprecated.rst
@@ -16,3 +16,5 @@
 +================+================+====================+====================+
 |    sgm775      |      Arm       |        2.5         |       2.7          |
 +----------------+----------------+--------------------+--------------------+
+|    mt6795      |      MTK       |        2.5         |       2.7          |
++----------------+----------------+--------------------+--------------------+
diff --git a/docs/plat/marvell/armada/build.rst b/docs/plat/marvell/armada/build.rst
index c9f5e82..ca84be6 100644
--- a/docs/plat/marvell/armada/build.rst
+++ b/docs/plat/marvell/armada/build.rst
@@ -98,10 +98,51 @@
         There is no reason to enable this feature if OP-TEE OS built with CFG_WITH_PAGER=n.
         Only set LLC_SRAM=1 if OP-TEE OS is built with CFG_WITH_PAGER=y.
 
+- MARVELL_SECURE_BOOT
+
+        Build trusted(=1)/non trusted(=0) image, default is non trusted.
+        This parameter is used only for ``mrvl_flash`` and ``mrvl_uart`` targets.
+
+- MV_DDR_PATH
+
+        This parameter is required for ``mrvl_flash`` and ``mrvl_uart`` targets.
+        For A7K/8K/CN913x it is used for BLE build and for Armada37x0 it used
+        for ddr_tool build.
+
+        Specify path to the full checkout of Marvell mv-ddr-marvell git
+        repository. Checkout must contain also .git subdirectory because
+        mv-ddr build process calls git commands.
+
+        Do not remove any parts of git checkout becuase build process and other
+        applications need them for correct building and version determination.
+
+
+CN913x specific build options:
+
+- CP_NUM
+
+        Total amount of CPs (South Bridge) connected to AP. When the parameter is omitted,
+        the build uses the default number of CPs, which is a number of embedded CPs inside the
+        package: 1 or 2 depending on the SoC used. The parameter is valid for OcteonTX2 CN913x SoC
+        family (PLAT=t9130), which can have external CPs connected to the MCI ports. Valid
+        values with CP_NUM are in a range of 1 to 3.
+
+
+A7K/8K/CN913x specific build options:
+
+- BLE_PATH
+
+        Points to BLE (Binary ROM extension) sources folder.
+        The parameter is optional, its default value is ``plat/marvell/armada/a8k/common/ble``
+        which uses TF-A in-tree BLE implementation.
+
+
+Armada37x0 specific build options:
+
 - CM3_SYSTEM_RESET
 
-        For Armada37x0 only, when ``CM3_SYSTEM_RESET=1``, the Cortex-M3 secure coprocessor will
-        be used for system reset.
+        When ``CM3_SYSTEM_RESET=1``, the Cortex-M3 secure coprocessor will be used for system reset.
+
         TF-A will send command 0x0009 with a magic value via the rWTM mailbox interface to the
         Cortex-M3 secure coprocessor.
         The firmware running in the coprocessor must either implement this functionality or
@@ -114,46 +155,15 @@
 
 - A3720_DB_PM_WAKEUP_SRC
 
-        For Armada 3720 Develpment Board only, when ``A3720_DB_PM_WAKEUP_SRC=1``,
+        For Armada 3720 Development Board only, when ``A3720_DB_PM_WAKEUP_SRC=1``,
         TF-A will setup PM wake up src configuration. This option is disabled by default.
 
-- MARVELL_SECURE_BOOT
 
-        Build trusted(=1)/non trusted(=0) image, default is non trusted.
-
-- BLE_PATH
-
-        Points to BLE (Binary ROM extension) sources folder.
-        Only required for A7K/8K/CN913x builds.
-        The parameter is optional, its default value is ``plat/marvell/armada/a8k/common/ble``.
-
-- MV_DDR_PATH
-
-        For A7K/8K/CN913x, use this parameter to point to mv_ddr driver sources to allow BLE build. For A37x0,
-        it is used for ddr_tool build.
-
-        Usage example: MV_DDR_PATH=path/to/mv_ddr
-
-        The parameter is optional for A7K/8K/CN913x, when this parameter is not set, the mv_ddr
-        sources are expected to be located at: drivers/marvell/mv_ddr. However, the parameter
-        is necessary for A37x0.
-
-        For the mv_ddr source location, check the section "Tools and external components installation"
-
-        If MV_DDR_PATH source code is a git snapshot then provide path to the full git
-        repository (including .git subdir) because mv_ddr build process calls git commands.
-
-- CP_NUM
-
-        Total amount of CPs (South Bridge) connected to AP. When the parameter is omitted,
-        the build uses the default number of CPs, which is a number of embedded CPs inside the
-        package: 1 or 2 depending on the SoC used. The parameter is valid for OcteonTX2 CN913x SoC
-        family (PLAT=t9130), which can have external CPs connected to the MCI ports. Valid
-        values with CP_NUM are in a range of 1 to 3.
+Armada37x0 specific build options for ``mrvl_flash`` and ``mrvl_uart`` targets:
 
 - DDR_TOPOLOGY
 
-        For Armada37x0 only, the DDR topology map index/name, default is 0.
+        The DDR topology map index/name, default is 0.
 
         Supported Options:
             -    0 - DDR3 1CS 512MB (DB-88F3720-DDR3-Modular, EspressoBin V3-V5)
@@ -168,7 +178,7 @@
 
 - CLOCKSPRESET
 
-        For Armada37x0 only, the clock tree configuration preset including CPU and DDR frequency,
+        The clock tree configuration preset including CPU and DDR frequency,
         default is CPU_800_DDR_800.
 
             - CPU_600_DDR_600  - CPU at 600 MHz, DDR at 600 MHz
@@ -185,7 +195,7 @@
 
 - BOOTDEV
 
-        For Armada37x0 only, the flash boot device, default is ``SPINOR``.
+        The flash boot device, default is ``SPINOR``.
 
         Currently, Armada37x0 only supports ``SPINOR``, ``SPINAND``, ``EMMCNORM`` and ``SATA``:
 
@@ -204,7 +214,7 @@
 
 - PARTNUM
 
-        For Armada37x0 only, the boot partition number, default is 0.
+        The boot partition number, default is 0.
 
         To boot from eMMC, the value should be aligned with the parameter in
         U-Boot with name of ``CONFIG_SYS_MMC_ENV_PART``, whose value by default is
@@ -213,7 +223,7 @@
 
 - WTMI_IMG
 
-        For Armada37x0 only, the path of the binary can point to an image which
+        The path of the binary can point to an image which
         does nothing, an image which supports EFUSE or a customized CM3 firmware
         binary. The default image is ``fuse.bin`` that built from sources in WTP
         folder, which is the next option. If the default image is OK, then this
@@ -236,16 +246,19 @@
 
 - WTP
 
-        For Armada37x0 only, use this parameter to point to wtptools source code
-        directory, which can be found as a3700_utils.zip in the release. Usage
-        example: ``WTP=/path/to/a3700_utils``
+        Specify path to the full checkout of Marvell A3700-utils-marvell git
+        repository. Checkout must contain also .git subdirectory because WTP
+        build process calls git commands.
 
-        If WTP source code is a git snapshot then provide path to the full git
-        repository (including .git subdir) because WTP build process calls git commands.
+        WTP build process uses also Marvell mv-ddr-marvell git repository
+        specified in MV_DDR_PATH option.
+
+        Do not remove any parts of git checkout becuase build process and other
+        applications need them for correct building and version determination.
 
 - CRYPTOPP_PATH
 
-        For Armada37x0 only, use this parameter to point to Crypto++ source code
+        Use this parameter to point to Crypto++ source code
         directory. If this option is specified then Crypto++ source code in
         CRYPTOPP_PATH directory will be automatically compiled. Crypto++ library
         is required for building WTP image tool. Either CRYPTOPP_PATH or
@@ -253,12 +266,12 @@
 
 - CRYPTOPP_LIBDIR
 
-        For Armada37x0 only, use this parameter to point to the directory with
+        Use this parameter to point to the directory with
         compiled Crypto++ library. By default it points to the CRYPTOPP_PATH.
 
 - CRYPTOPP_INCDIR
 
-        For Armada37x0 only, use this parameter to point to the directory with
+        Use this parameter to point to the directory with
         header files of Crypto++ library. By default it points to the CRYPTOPP_PATH.
 
 
@@ -354,8 +367,8 @@
 Tools and external components installation
 ------------------------------------------
 
-Armada37x0 Builds require installation of 3 components
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Armada37x0 Builds require installation of additional components
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 (1) ARM cross compiler capable of building images for the service CPU (CM3).
     This component is usually included in the Linux host packages.
@@ -388,6 +401,10 @@
 
     https://github.com/weidai11/cryptopp.git
 
+(5) Optional CZ.NIC's Armada 3720 Secure Firmware:
+
+    https://gitlab.nic.cz/turris/mox-boot-builder.git
+
 Armada70x0 and Armada80x0 Builds require installation of an additional component
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index 668416c..b1139b5 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -332,6 +332,8 @@
 	write_icc_igrpen1_el3(read_icc_igrpen1_el3() |
 				IGRPEN1_EL3_ENABLE_G1S_BIT);
 	isb();
+	/* Add DSB to ensure visibility of System register writes */
+	dsb();
 }
 
 /*******************************************************************************
@@ -363,6 +365,8 @@
 
 	/* Synchronise accesses to group enable registers */
 	isb();
+	/* Add DSB to ensure visibility of System register writes */
+	dsb();
 
 	/* Mark the connected core as asleep */
 	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
diff --git a/drivers/auth/auth_mod.c b/drivers/auth/auth_mod.c
index c7f84af..917ee4a 100644
--- a/drivers/auth/auth_mod.c
+++ b/drivers/auth/auth_mod.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,6 +16,7 @@
 #include <drivers/auth/auth_mod.h>
 #include <drivers/auth/crypto_mod.h>
 #include <drivers/auth/img_parser_mod.h>
+#include <drivers/fwu/fwu.h>
 #include <lib/fconf/fconf_tbbr_getter.h>
 #include <plat/common/platform.h>
 
@@ -242,6 +243,7 @@
 	unsigned int data_len, len, i;
 	unsigned int plat_nv_ctr;
 	int rc = 0;
+	bool is_trial_run = false;
 
 	/* Get the counter value from current image. The AM expects the IPM
 	 * to return the counter value as a DER encoded integer */
@@ -284,7 +286,10 @@
 		/* Invalid NV-counter */
 		return 1;
 	} else if (*cert_nv_ctr > plat_nv_ctr) {
-		*need_nv_ctr_upgrade = true;
+#if PSA_FWU_SUPPORT && IMAGE_BL2
+		is_trial_run = fwu_is_trial_run_state();
+#endif /* PSA_FWU_SUPPORT && IMAGE_BL2 */
+		*need_nv_ctr_upgrade = !is_trial_run;
 	}
 
 	return 0;
diff --git a/drivers/fwu/fwu.c b/drivers/fwu/fwu.c
new file mode 100644
index 0000000..7cb4c29
--- /dev/null
+++ b/drivers/fwu/fwu.c
@@ -0,0 +1,187 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <common/tf_crc32.h>
+#include <common/tbbr/tbbr_img_def.h>
+#include <drivers/fwu/fwu.h>
+#include <drivers/fwu/fwu_metadata.h>
+#include <drivers/io/io_storage.h>
+
+#include <plat/common/platform.h>
+
+/*
+ * Assert that crc_32 is the first member of fwu_metadata structure.
+ * It avoids accessing data outside of the metadata structure during
+ * CRC32 computation if the crc_32 field gets moved due the structure
+ * member(s) addition in the future.
+ */
+CASSERT((offsetof(struct fwu_metadata, crc_32) == 0),
+	crc_32_must_be_first_member_of_structure);
+
+static struct fwu_metadata metadata;
+static bool is_fwu_initialized;
+
+/*******************************************************************************
+ * Compute CRC32 of the FWU metadata, and check it against the CRC32 value
+ * present in the FWU metadata.
+ *
+ * return -1 on error, otherwise 0
+ ******************************************************************************/
+static int fwu_metadata_crc_check(void)
+{
+	unsigned char *data = (unsigned char *)&metadata;
+
+	uint32_t calc_crc = tf_crc32(0U, data + sizeof(metadata.crc_32),
+				     (sizeof(metadata) -
+				      sizeof(metadata.crc_32)));
+
+	if (metadata.crc_32 != calc_crc) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Check the sanity of FWU metadata.
+ *
+ * return -1 on error, otherwise 0
+ ******************************************************************************/
+static int fwu_metadata_sanity_check(void)
+{
+	/* ToDo: add more conditions for sanity check */
+	if ((metadata.active_index >= NR_OF_FW_BANKS) ||
+	    (metadata.previous_active_index >= NR_OF_FW_BANKS)) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Verify and load specified FWU metadata image to local FWU metadata structure.
+ *
+ * @image_id: FWU metadata image id (either FWU_METADATA_IMAGE_ID or
+ *				     BKUP_FWU_METADATA_IMAGE_ID)
+ *
+ * return a negative value on error, otherwise 0
+ ******************************************************************************/
+static int fwu_metadata_load(unsigned int image_id)
+{
+	int result;
+	uintptr_t dev_handle, image_handle, image_spec;
+	size_t bytes_read;
+
+	assert((image_id == FWU_METADATA_IMAGE_ID) ||
+	       (image_id == BKUP_FWU_METADATA_IMAGE_ID));
+
+	result = plat_fwu_set_metadata_image_source(image_id,
+						    &dev_handle,
+						    &image_spec);
+	if (result != 0) {
+		WARN("Failed to set reference to image id=%u (%i)\n",
+		     image_id, result);
+		return result;
+	}
+
+	result = io_open(dev_handle, image_spec, &image_handle);
+	if (result != 0) {
+		WARN("Failed to load image id id=%u (%i)\n",
+		     image_id, result);
+		return result;
+	}
+
+	result = io_read(image_handle, (uintptr_t)&metadata,
+			 sizeof(struct fwu_metadata), &bytes_read);
+
+	if (result != 0) {
+		WARN("Failed to read image id=%u (%i)\n", image_id, result);
+		goto exit;
+	}
+
+	if (sizeof(struct fwu_metadata) != bytes_read) {
+		/* return -1 in case of partial/no read */
+		result = -1;
+		WARN("Read bytes (%zu) instead of expected (%zu) bytes\n",
+		     bytes_read, sizeof(struct fwu_metadata));
+		goto exit;
+	}
+
+	/* sanity check on loaded parameters */
+	result = fwu_metadata_sanity_check();
+	if (result != 0) {
+		WARN("Sanity %s\n", "check failed on FWU metadata");
+		goto exit;
+	}
+
+	/* CRC check on loaded parameters */
+	result = fwu_metadata_crc_check();
+	if (result != 0) {
+		WARN("CRC %s\n", "check failed on FWU metadata");
+	}
+
+exit:
+	(void)io_close(image_handle);
+
+	return result;
+}
+
+/*******************************************************************************
+ * The system runs in the trial run state if any of the images in the active
+ * firmware bank has not been accepted yet.
+ *
+ * Returns true if the system is running in the trial state.
+ ******************************************************************************/
+bool fwu_is_trial_run_state(void)
+{
+	bool trial_run = false;
+
+	assert(is_fwu_initialized == true);
+
+	for (unsigned int i = 0U; i < NR_OF_IMAGES_IN_FW_BANK; i++) {
+		struct fwu_image_entry *entry = &metadata.img_entry[i];
+		struct fwu_image_properties *img_props =
+			&entry->img_props[metadata.active_index];
+		if (img_props->accepted == 0) {
+			trial_run = true;
+			break;
+		}
+	}
+
+	return trial_run;
+}
+
+/*******************************************************************************
+ * Load verified copy of FWU metadata image kept in the platform NV storage
+ * into local FWU metadata structure.
+ * Also, update platform I/O policies with the offset address and length of
+ * firmware-updated images kept in the platform NV storage.
+ ******************************************************************************/
+void fwu_init(void)
+{
+	/* Load FWU metadata which will be used to load the images in the
+	 * active bank as per PSA FWU specification
+	 */
+	int result = fwu_metadata_load(FWU_METADATA_IMAGE_ID);
+
+	if (result != 0) {
+		WARN("loading of FWU-Metadata failed, "
+		     "using Bkup-FWU-Metadata\n");
+
+		result = fwu_metadata_load(BKUP_FWU_METADATA_IMAGE_ID);
+		if (result != 0) {
+			ERROR("loading of Bkup-FWU-Metadata failed\n");
+			panic();
+		}
+	}
+
+	plat_fwu_set_images_source(&metadata);
+
+	is_fwu_initialized = true;
+}
diff --git a/drivers/fwu/fwu.mk b/drivers/fwu/fwu.mk
new file mode 100644
index 0000000..f4452e0
--- /dev/null
+++ b/drivers/fwu/fwu.mk
@@ -0,0 +1,11 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+FWU_SRC_DIR	:= drivers/fwu/
+
+FWU_SRCS	:= ${FWU_SRC_DIR}fwu.c
+
+BL2_SOURCES	+= ${FWU_SRCS}
diff --git a/drivers/io/io_mtd.c b/drivers/io/io_mtd.c
index 7575fa2..ba8cecd 100644
--- a/drivers/io/io_mtd.c
+++ b/drivers/io/io_mtd.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,8 +18,9 @@
 typedef struct {
 	io_mtd_dev_spec_t	*dev_spec;
 	uintptr_t		base;
-	unsigned long long	offset;		/* Offset in bytes */
-	unsigned long long	size;	/* Size of device in bytes */
+	unsigned long long	pos;		/* Offset in bytes */
+	unsigned long long	size;		/* Size of device in bytes */
+	unsigned long long	extra_offset;	/* Extra offset in bytes */
 } mtd_dev_state_t;
 
 io_type_t device_type_mtd(void);
@@ -110,16 +111,47 @@
 	return 0;
 }
 
+static int mtd_add_extra_offset(mtd_dev_state_t *cur, size_t *extra_offset)
+{
+	io_mtd_ops_t *ops = &cur->dev_spec->ops;
+	int ret;
+
+	if (ops->seek == NULL) {
+		return 0;
+	}
+
+	ret = ops->seek(cur->base, cur->pos, extra_offset);
+	if (ret != 0) {
+		ERROR("%s: Seek error %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 static int mtd_open(io_dev_info_t *dev_info, const uintptr_t spec,
 		    io_entity_t *entity)
 {
 	mtd_dev_state_t *cur;
+	io_block_spec_t *region;
+	size_t extra_offset = 0U;
+	int ret;
 
 	assert((dev_info->info != 0UL) && (entity->info == 0UL));
 
+	region = (io_block_spec_t *)spec;
 	cur = (mtd_dev_state_t *)dev_info->info;
 	entity->info = (uintptr_t)cur;
-	cur->offset = 0U;
+	cur->base = region->offset;
+	cur->pos = 0U;
+	cur->extra_offset = 0U;
+
+	ret = mtd_add_extra_offset(cur, &extra_offset);
+	if (ret != 0) {
+		return ret;
+	}
+
+	cur->base += extra_offset;
 
 	return 0;
 }
@@ -128,6 +160,8 @@
 static int mtd_seek(io_entity_t *entity, int mode, signed long long offset)
 {
 	mtd_dev_state_t *cur;
+	size_t extra_offset = 0U;
+	int ret;
 
 	assert((entity->info != (uintptr_t)NULL) && (offset >= 0));
 
@@ -140,22 +174,29 @@
 			return -EINVAL;
 		}
 
-		cur->offset = offset;
+		cur->pos = offset;
 		break;
 	case IO_SEEK_CUR:
-		if (((cur->offset + (unsigned long long)offset) >=
+		if (((cur->base + cur->pos + (unsigned long long)offset) >=
 		     cur->size) ||
-		    ((cur->offset + (unsigned long long)offset) <
-		     cur->offset)) {
+		    ((cur->base + cur->pos + (unsigned long long)offset) <
+		     cur->base + cur->pos)) {
 			return -EINVAL;
 		}
 
-		cur->offset += (unsigned long long)offset;
+		cur->pos += (unsigned long long)offset;
 		break;
 	default:
 		return -EINVAL;
 	}
 
+	ret = mtd_add_extra_offset(cur, &extra_offset);
+	if (ret != 0) {
+		return ret;
+	}
+
+	cur->extra_offset = extra_offset;
+
 	return 0;
 }
 
@@ -174,18 +215,19 @@
 	assert(ops->read != NULL);
 
 	VERBOSE("Read at %llx into %lx, length %zi\n",
-		cur->offset, buffer, length);
-	if ((cur->offset + length) > cur->dev_spec->device_size) {
+		cur->base + cur->pos, buffer, length);
+	if ((cur->base + cur->pos + length) > cur->dev_spec->device_size) {
 		return -EINVAL;
 	}
 
-	ret = ops->read(cur->offset, buffer, length, out_length);
+	ret = ops->read(cur->base + cur->pos + cur->extra_offset, buffer,
+			length, out_length);
 	if (ret < 0) {
 		return ret;
 	}
 
 	assert(*out_length == length);
-	cur->offset += *out_length;
+	cur->pos += *out_length;
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/core.c b/drivers/mtd/nand/core.c
index 44b001e..9f0331a 100644
--- a/drivers/mtd/nand/core.c
+++ b/drivers/mtd/nand/core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -112,6 +112,47 @@
 	return 0;
 }
 
+int nand_seek_bb(uintptr_t base, unsigned int offset, size_t *extra_offset)
+{
+	unsigned int block;
+	unsigned int offset_block;
+	unsigned int max_block;
+	int is_bad;
+	size_t count_bb = 0U;
+
+	block = base / nand_dev.block_size;
+
+	if (offset != 0U) {
+		offset_block = (base + offset - 1U) / nand_dev.block_size;
+	} else {
+		offset_block = block;
+	}
+
+	max_block = nand_dev.size / nand_dev.block_size;
+
+	while (block <= offset_block) {
+		if (offset_block >= max_block) {
+			return -EIO;
+		}
+
+		is_bad = nand_dev.mtd_block_is_bad(block);
+		if (is_bad < 0) {
+			return is_bad;
+		}
+
+		if (is_bad == 1) {
+			count_bb++;
+			offset_block++;
+		}
+
+		block++;
+	}
+
+	*extra_offset = count_bb * nand_dev.block_size;
+
+	return 0;
+}
+
 struct nand_device *get_nand_device(void)
 {
 	return &nand_dev;
diff --git a/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk b/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk
index d518dbb..1af51f8 100644
--- a/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk
+++ b/drivers/nxp/auth/csf_hdr_parser/csf_hdr.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -9,7 +9,7 @@
 
 CSF_HDR_SOURCES	+=  $(PLAT_DRIVERS_PATH)/auth/csf_hdr_parser/plat_img_parser.c
 
-PLAT_INCLUDES	+= -I$(PLAT_DRIVERS_PATH)/auth/csf_hdr_parser/
+PLAT_INCLUDES	+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/auth/csf_hdr_parser/
 
 $(eval $(call add_define, CSF_HEADER_PREPENDED))
 
diff --git a/drivers/nxp/console/console.mk b/drivers/nxp/console/console.mk
index 22d1336..6174650 100644
--- a/drivers/nxp/console/console.mk
+++ b/drivers/nxp/console/console.mk
@@ -14,7 +14,7 @@
 
 ADD_CONSOLE		:= 1
 
-PLAT_INCLUDES		+=	-I$(PLAT_DRIVERS_PATH)/console
+PLAT_INCLUDES		+=	-I$(PLAT_DRIVERS_INCLUDE_PATH)/console
 
 ifeq ($(CONSOLE), NS16550)
 NXP_CONSOLE		:=	NS16550
diff --git a/drivers/nxp/crypto/caam/caam.mk b/drivers/nxp/crypto/caam/caam.mk
index 548c7b1..f929f53 100644
--- a/drivers/nxp/crypto/caam/caam.mk
+++ b/drivers/nxp/crypto/caam/caam.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2020-2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,11 +8,10 @@
 ifeq (${ADD_CAAM},)
 
 ADD_CAAM		:= 1
-CAAM_DRIVER_PATH	:= drivers/nxp/crypto/caam
 
-CAAM_DRIVER_SOURCES	+=  $(wildcard $(CAAM_DRIVER_PATH)/src/*.c)
+CAAM_DRIVER_SOURCES	+=  $(wildcard $(PLAT_DRIVERS_PATH)/crypto/caam/src/*.c)
 
-PLAT_INCLUDES		+= -I$(CAAM_DRIVER_PATH)/include
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/crypto/caam
 
 ifeq (${BL_COMM_CRYPTO_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${CAAM_DRIVER_SOURCES}
diff --git a/drivers/nxp/csu/csu.mk b/drivers/nxp/csu/csu.mk
index ebdf674..bc16035 100644
--- a/drivers/nxp/csu/csu.mk
+++ b/drivers/nxp/csu/csu.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,11 +8,9 @@
 
 CSU_ADDED		:= 1
 
-CSU_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/csu
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/csu
 
-PLAT_INCLUDES		+= -I$(CSU_DRIVERS_PATH)
-
-CSU_SOURCES		+= $(CSU_DRIVERS_PATH)/csu.c
+CSU_SOURCES		+= $(PLAT_DRIVERS_PATH)/csu/csu.c
 
 ifeq (${BL_COMM_CSU_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${CSU_SOURCES}
diff --git a/drivers/nxp/dcfg/dcfg.mk b/drivers/nxp/dcfg/dcfg.mk
index 61d1850..206595f 100644
--- a/drivers/nxp/dcfg/dcfg.mk
+++ b/drivers/nxp/dcfg/dcfg.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,11 +8,9 @@
 
 ADD_DCFG		:= 1
 
-DCFG_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/dcfg
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/dcfg
 
-PLAT_INCLUDES		+= -I$(DCFG_DRIVERS_PATH)
-
-DCFG_SOURCES		+= $(DCFG_DRIVERS_PATH)/dcfg.c
+DCFG_SOURCES		+= $(PLAT_DRIVERS_PATH)/dcfg/dcfg.c
 
 ifeq (${BL_COMM_DCFG_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${DCFG_SOURCES}
diff --git a/drivers/nxp/ddr/fsl-mmdc/ddr.mk b/drivers/nxp/ddr/fsl-mmdc/ddr.mk
index e6cc7c1..afccb62 100644
--- a/drivers/nxp/ddr/fsl-mmdc/ddr.mk
+++ b/drivers/nxp/ddr/fsl-mmdc/ddr.mk
@@ -9,11 +9,11 @@
 
 DDR_DRIVERS_PATH	:=	drivers/nxp/ddr
 
-DDR_CNTLR_SOURCES	:=	${DDR_DRIVERS_PATH}/fsl-mmdc/fsl_mmdc.c \
-				${DDR_DRIVERS_PATH}/nxp-ddr/utility.c	\
-				${DDR_DRIVERS_PATH}/nxp-ddr/ddr.c	\
-				${DDR_DRIVERS_PATH}/nxp-ddr/ddrc.c
+DDR_CNTLR_SOURCES	:=	${PLAT_DRIVERS_PATH}/ddr/fsl-mmdc/fsl_mmdc.c \
+				${PLAT_DRIVERS_PATH}/ddr/nxp-ddr/utility.c	\
+				${PLAT_DRIVERS_PATH}/ddr/nxp-ddr/ddr.c	\
+				${PLAT_DRIVERS_PATH}/ddr/nxp-ddr/ddrc.c
 
-PLAT_INCLUDES		+=	-I$(DDR_DRIVERS_PATH)/include	\
-				-I$(DDR_DRIVERS_PATH)/fsl-mmdc
+PLAT_INCLUDES		+=	-I$(PLAT_DRIVERS_INCLUDE_PATH)/ddr	\
+				-I$(PLAT_DRIVERS_INCLUDE_PATH)/ddr/fsl-mmdc
 #------------------------------------------------
diff --git a/drivers/nxp/ddr/nxp-ddr/ddr.mk b/drivers/nxp/ddr/nxp-ddr/ddr.mk
index 866c092..6bdd947 100644
--- a/drivers/nxp/ddr/nxp-ddr/ddr.mk
+++ b/drivers/nxp/ddr/nxp-ddr/ddr.mk
@@ -4,8 +4,6 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-DDR_DRIVERS_PATH	:= ${PLAT_DRIVERS_PATH}/ddr
-
 ifeq ($(PLAT_DDR_PHY), PHY_GEN2)
 $(eval $(call add_define, PHY_GEN2))
 PLAT_DDR_PHY_DIR		:= phy-gen2
@@ -68,12 +66,11 @@
 $(eval $(call add_define, DEBUG_DDR_INPUT_CONFIG))
 endif
 
-DDR_CNTLR_SOURCES	:= $(DDR_DRIVERS_PATH)/nxp-ddr/ddr.c \
-			   $(DDR_DRIVERS_PATH)/nxp-ddr/ddrc.c \
-			   $(DDR_DRIVERS_PATH)/nxp-ddr/dimm.c \
-			   $(DDR_DRIVERS_PATH)/nxp-ddr/regs.c \
-			   $(DDR_DRIVERS_PATH)/nxp-ddr/utility.c \
-			   $(DDR_DRIVERS_PATH)/$(PLAT_DDR_PHY_DIR)/phy.c
+DDR_CNTLR_SOURCES	:= $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/ddr.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/ddrc.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/dimm.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/regs.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/nxp-ddr/utility.c \
+			   $(PLAT_DRIVERS_PATH)/ddr/$(PLAT_DDR_PHY_DIR)/phy.c
 
-PLAT_INCLUDES		+= -I$(DDR_DRIVERS_PATH)/nxp-ddr \
-			   -I$(DDR_DRIVERS_PATH)/include
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/ddr
diff --git a/drivers/nxp/drivers.mk b/drivers/nxp/drivers.mk
index c6d5541..c2db363 100644
--- a/drivers/nxp/drivers.mk
+++ b/drivers/nxp/drivers.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,7 +8,8 @@
 ###############################################################################
 
 
-PLAT_DRIVERS_PATH	:=	drivers/nxp
+PLAT_DRIVERS_PATH		:=	drivers/nxp
+PLAT_DRIVERS_INCLUDE_PATH	:=	include/drivers/nxp
 
 ifeq (${SMMU_NEEDED},yes)
 PLAT_INCLUDES	+= -Iinclude/drivers/nxp/smmu/
diff --git a/drivers/nxp/gic/gic.mk b/drivers/nxp/gic/gic.mk
index 68091e8..d75e071 100644
--- a/drivers/nxp/gic/gic.mk
+++ b/drivers/nxp/gic/gic.mk
@@ -17,7 +17,7 @@
 GIC_SOURCES		+=	${PLAT_DRIVERS_PATH}/gic/ls_gicv2.c	\
 				plat/common/plat_gicv2.c
 
-PLAT_INCLUDES		+=	-I${PLAT_DRIVERS_PATH}/gic/include/gicv2
+PLAT_INCLUDES		+=	-I${PLAT_DRIVERS_INCLUDE_PATH}/gic/gicv2
 else
 ifeq ($(GIC), GIC500)
 include drivers/arm/gic/v3/gicv3.mk
@@ -25,7 +25,7 @@
 GIC_SOURCES		+=	${PLAT_DRIVERS_PATH}/gic/ls_gicv3.c	\
 				plat/common/plat_gicv3.c
 
-PLAT_INCLUDES		+=	-I${PLAT_DRIVERS_PATH}/gic/include/gicv3
+PLAT_INCLUDES		+=	-I${PLAT_DRIVERS_INCLUDE_PATH}/gic/gicv3
 else
     $(error -> GIC type not set!)
 endif
diff --git a/drivers/nxp/gpio/gpio.mk b/drivers/nxp/gpio/gpio.mk
index 157c60a..74f0dc4 100644
--- a/drivers/nxp/gpio/gpio.mk
+++ b/drivers/nxp/gpio/gpio.mk
@@ -9,11 +9,9 @@
 
 GPIO_ADDED		:= 1
 
-GPIO_DRIVERS_PATH	:=  drivers/nxp/gpio
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/gpio
 
-PLAT_INCLUDES		+=  -I$(GPIO_DRIVERS_PATH)
-
-GPIO_SOURCES		:= $(GPIO_DRIVERS_PATH)/nxp_gpio.c
+GPIO_SOURCES		:= $(PLAT_DRIVERS_PATH)/gpio/nxp_gpio.c
 
 ifeq (${BL_COMM_GPIO_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${GPIO_SOURCES}
diff --git a/drivers/nxp/i2c/i2c.mk b/drivers/nxp/i2c/i2c.mk
index ae89115..716e14a 100644
--- a/drivers/nxp/i2c/i2c.mk
+++ b/drivers/nxp/i2c/i2c.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -7,10 +7,10 @@
 ifeq (${ADD_I2C},)
 
 ADD_I2C			:= 1
-I2C_DRIVERS_PATH        := ${PLAT_DRIVERS_PATH}/i2c
 
-I2C_SOURCES		+= $(I2C_DRIVERS_PATH)/i2c.c
-PLAT_INCLUDES		+= -I$(I2C_DRIVERS_PATH)
+I2C_SOURCES		+= $(PLAT_DRIVERS_PATH)/i2c/i2c.c
+
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/i2c
 
 ifeq (${BL_COMM_I2C_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${I2C_SOURCES}
diff --git a/drivers/nxp/interconnect/interconnect.mk b/drivers/nxp/interconnect/interconnect.mk
index 81e3fa9..aa51be4 100644
--- a/drivers/nxp/interconnect/interconnect.mk
+++ b/drivers/nxp/interconnect/interconnect.mk
@@ -1,4 +1,4 @@
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -12,7 +12,7 @@
 ifeq (${ADD_INTERCONNECT},)
 
 ADD_INTERCONNECT	:= 1
-PLAT_INCLUDES		+= -I${PLAT_DRIVERS_PATH}/interconnect
+PLAT_INCLUDES		+= -I${PLAT_DRIVERS_INCLUDE_PATH}/interconnect
 
 ifeq (, $(filter $(INTERCONNECT), CCI400 CCN502 CCN504 CCN508))
     $(error -> Interconnect type not set!)
diff --git a/drivers/nxp/pmu/pmu.mk b/drivers/nxp/pmu/pmu.mk
index 56b0422..8d2ef07 100644
--- a/drivers/nxp/pmu/pmu.mk
+++ b/drivers/nxp/pmu/pmu.mk
@@ -8,11 +8,9 @@
 
 PMU_ADDED		:= 1
 
-PMU_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/pmu
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/pmu
 
-PLAT_INCLUDES		+= -I$(PMU_DRIVERS_PATH)
-
-PMU_SOURCES		+= $(PMU_DRIVERS_PATH)/pmu.c
+PMU_SOURCES		+= $(PLAT_DRIVERS_PATH)/pmu/pmu.c
 
 ifeq (${BL_COMM_PMU_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${PMU_SOURCES}
diff --git a/drivers/nxp/qspi/qspi.mk b/drivers/nxp/qspi/qspi.mk
index 3e2c735..b83dee2 100644
--- a/drivers/nxp/qspi/qspi.mk
+++ b/drivers/nxp/qspi/qspi.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,11 +8,9 @@
 
 QSPI_ADDED		:= 1
 
-QSPI_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/qspi
+QSPI_SOURCES		:= $(PLAT_DRIVERS_PATH)/qspi/qspi.c
 
-QSPI_SOURCES		:=  $(QSPI_DRIVERS_PATH)/qspi.c
-
-PLAT_INCLUDES		+= -I$(QSPI_DRIVERS_PATH)
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_PATH)/qspi
 
 ifeq (${BL_COMM_QSPI_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${QSPI_SOURCES}
diff --git a/drivers/nxp/sd/sd_mmc.mk b/drivers/nxp/sd/sd_mmc.mk
index af91b1f..c83b1bd 100644
--- a/drivers/nxp/sd/sd_mmc.mk
+++ b/drivers/nxp/sd/sd_mmc.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,12 +8,10 @@
 
 ADD_SD_MMC	:= 1
 
-SD_DRIVERS_PATH		:=  ${PLAT_DRIVERS_PATH}/sd
-
-SD_MMC_BOOT_SOURCES	+= ${SD_DRIVERS_PATH}/sd_mmc.c \
+SD_MMC_BOOT_SOURCES	+= ${PLAT_DRIVERS_PATH}/sd/sd_mmc.c \
 			   drivers/io/io_block.c
 
-PLAT_INCLUDES		+= -I$(SD_DRIVERS_PATH)
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/sd
 
 ifeq (${BL_COMM_SD_MMC_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${SD_MMC_BOOT_SOURCES}
diff --git a/drivers/nxp/sec_mon/sec_mon.mk b/drivers/nxp/sec_mon/sec_mon.mk
index 51e3e86..aaac53f 100644
--- a/drivers/nxp/sec_mon/sec_mon.mk
+++ b/drivers/nxp/sec_mon/sec_mon.mk
@@ -8,11 +8,9 @@
 
 ADD_SNVS		:= 1
 
-SNVS_DRIVERS_PATH	:= ${PLAT_DRIVERS_PATH}/sec_mon
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/sec_mon
 
-PLAT_INCLUDES		+= -I$(SNVS_DRIVERS_PATH)
-
-SNVS_SOURCES		+= $(SNVS_DRIVERS_PATH)/snvs.c
+SNVS_SOURCES		+= $(PLAT_DRIVERS_PATH)/sec_mon/snvs.c
 
 ifeq (${BL_COMM_SNVS_NEEDED},yes)
 BL_COMMON_SOURCES	+= ${SNVS_SOURCES}
diff --git a/drivers/nxp/sfp/sfp.mk b/drivers/nxp/sfp/sfp.mk
index 2546dc2..de708c5 100644
--- a/drivers/nxp/sfp/sfp.mk
+++ b/drivers/nxp/sfp/sfp.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2020 NXP
+# Copyright 2021 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -9,14 +9,12 @@
 SFP_ADDED		:= 1
 $(eval $(call add_define, NXP_SFP_ENABLED))
 
-SFP_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/sfp
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/sfp
 
-PLAT_INCLUDES		+= -I$(SFP_DRIVERS_PATH)
-
-SFP_SOURCES		+= $(SFP_DRIVERS_PATH)/sfp.c
+SFP_SOURCES		+= $(PLAT_DRIVERS_PATH)/sfp/sfp.c
 
 ifeq (${FUSE_PROG}, 1)
-SFP_BL2_SOURCES		+= $(SFP_DRIVERS_PATH)/fuse_prov.c
+SFP_BL2_SOURCES		+= $(PLAT_DRIVERS_PATH)/sfp/fuse_prov.c
 endif
 
 ifeq (${BL_COMM_SFP_NEEDED},yes)
diff --git a/drivers/nxp/timer/timer.mk b/drivers/nxp/timer/timer.mk
index b9e298f..d658d19 100644
--- a/drivers/nxp/timer/timer.mk
+++ b/drivers/nxp/timer/timer.mk
@@ -8,10 +8,8 @@
 
 ADD_TIMER		:= 1
 
-TIMER_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/timer
-
-PLAT_INCLUDES		+= -I$(TIMER_DRIVERS_PATH)
-TIMER_SOURCES	+= drivers/delay_timer/delay_timer.c	\
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/timer
+TIMER_SOURCES		+= drivers/delay_timer/delay_timer.c	\
 			   $(PLAT_DRIVERS_PATH)/timer/nxp_timer.c
 
 ifeq (${BL_COMM_TIMER_NEEDED},yes)
diff --git a/drivers/nxp/tzc/tzc.mk b/drivers/nxp/tzc/tzc.mk
index 830d78e..3fba28f 100644
--- a/drivers/nxp/tzc/tzc.mk
+++ b/drivers/nxp/tzc/tzc.mk
@@ -8,13 +8,11 @@
 
 ADD_TZASC		:= 1
 
-TZASC_DRIVERS_PATH	:=  ${PLAT_DRIVERS_PATH}/tzc
-
-PLAT_INCLUDES		+= -I$(TZASC_DRIVERS_PATH)
+PLAT_INCLUDES		+= -I$(PLAT_DRIVERS_INCLUDE_PATH)/tzc
 
 ifeq ($(TZC_ID), TZC400)
 TZASC_SOURCES		+= drivers/arm/tzc/tzc400.c\
-			   $(TZASC_DRIVERS_PATH)/plat_tzc400.c
+			   $(PLAT_DRIVERS_PATH)/tzc/plat_tzc400.c
 else ifeq ($(TZC_ID), NONE)
     $(info -> No TZC present on platform)
 else
diff --git a/drivers/scmi-msg/common.h b/drivers/scmi-msg/common.h
index ef5953b..62f3087 100644
--- a/drivers/scmi-msg/common.h
+++ b/drivers/scmi-msg/common.h
@@ -13,6 +13,7 @@
 
 #include "base.h"
 #include "clock.h"
+#include "power_domain.h"
 #include "reset_domain.h"
 
 #define SCMI_VERSION			0x20000U
@@ -111,6 +112,13 @@
 scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg);
 
 /*
+ * scmi_msg_get_pd_handler - Return a handler for a power domain message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg);
+
+/*
  * Process Read, process and write response for input SCMI message
  *
  * @msg: SCMI message context
diff --git a/drivers/scmi-msg/entry.c b/drivers/scmi-msg/entry.c
index ea3efa2..3537fbe 100644
--- a/drivers/scmi-msg/entry.c
+++ b/drivers/scmi-msg/entry.c
@@ -11,6 +11,31 @@
 
 #include "common.h"
 
+#pragma weak scmi_msg_get_clock_handler
+#pragma weak scmi_msg_get_rstd_handler
+#pragma weak scmi_msg_get_pd_handler
+#pragma weak scmi_msg_get_voltage_handler
+
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_voltage_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
 void scmi_status_response(struct scmi_msg *msg, int32_t status)
 {
 	assert(msg->out && msg->out_size >= sizeof(int32_t));
@@ -47,6 +72,9 @@
 	case SCMI_PROTOCOL_ID_RESET_DOMAIN:
 		handler = scmi_msg_get_rstd_handler(msg);
 		break;
+	case SCMI_PROTOCOL_ID_POWER_DOMAIN:
+		handler = scmi_msg_get_pd_handler(msg);
+		break;
 	default:
 		break;
 	}
diff --git a/drivers/scmi-msg/power_domain.c b/drivers/scmi-msg/power_domain.c
new file mode 100644
index 0000000..c4e1289
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.c
@@ -0,0 +1,239 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+#pragma weak plat_scmi_pd_count
+#pragma weak plat_scmi_pd_get_name
+#pragma weak plat_scmi_pd_get_state
+#pragma weak plat_scmi_pd_set_state
+#pragma weak plat_scmi_pd_statistics
+#pragma weak plat_scmi_pd_get_attributes
+
+static bool message_id_is_supported(size_t message_id);
+
+size_t plat_scmi_pd_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_pd_get_name(unsigned int agent_id __unused,
+				  unsigned int pd_id __unused)
+{
+	return NULL;
+}
+
+unsigned int plat_scmi_pd_statistics(unsigned int agent_id __unused,
+				     unsigned long *pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_attributes(unsigned int agent_id __unused,
+					 unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_state(unsigned int agent_id __unused,
+				    unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+int32_t plat_scmi_pd_set_state(unsigned int agent_id __unused,
+			       unsigned int flags __unused,
+			       unsigned int pd_id __unused,
+			       unsigned int state __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_PD,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	unsigned long addr = 0UL;
+	unsigned int len;
+
+	struct scmi_protocol_attributes_p2a_pd return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	return_values.attributes = plat_scmi_pd_count(msg->agent_id);
+	len = plat_scmi_pd_statistics(msg->agent_id, &addr);
+	if (len != 0U) {
+		return_values.statistics_addr_low = (unsigned int)addr;
+		return_values.statistics_addr_high = (uint32_t)(addr >> 32);
+		return_values.statistics_len = len;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_attributes(struct scmi_msg *msg)
+{
+	const struct scmi_pd_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_pd_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	const char *name = NULL;
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	name = plat_scmi_pd_get_name(msg->agent_id, pd_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.pd_name, name);
+
+	return_values.attributes = plat_scmi_pd_get_attributes(msg->agent_id, pd_id);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_get(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_get_a2p *in_args = (void *)msg->in;
+	unsigned int state = 0U;
+	struct scmi_pd_state_get_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	state = plat_scmi_pd_get_state(msg->agent_id, pd_id);
+
+	return_values.power_state = state;
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_set(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_set_a2p *in_args = (void *)msg->in;
+	unsigned int flags = 0U;
+	int32_t status = 0;
+	unsigned int pd_id = 0U;
+	unsigned int state = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	flags = SPECULATION_SAFE_VALUE(in_args->flags);
+	state = SPECULATION_SAFE_VALUE(in_args->power_state);
+
+	status = plat_scmi_pd_set_state(msg->agent_id, flags, pd_id, state);
+
+	scmi_status_response(msg, status);
+}
+
+static const scmi_msg_handler_t scmi_pd_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_PD_ATTRIBUTES] = scmi_pd_attributes,
+	[SCMI_PD_STATE_SET] = scmi_pd_state_set,
+	[SCMI_PD_STATE_GET] = scmi_pd_state_get,
+};
+
+static bool message_id_is_supported(size_t message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_pd_handler_table)) &&
+	       (scmi_pd_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg)
+{
+	const size_t array_size = ARRAY_SIZE(scmi_pd_handler_table);
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= array_size) {
+		VERBOSE("pd handle not found %u", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_pd_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/power_domain.h b/drivers/scmi-msg/power_domain.h
new file mode 100644
index 0000000..48551fd
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright 2021 NXP
+ */
+
+#ifndef SCMI_MSG_PD_H
+#define SCMI_MSG_PD_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_PD	0x21000U
+
+/*
+ * Identifiers of the SCMI POWER DOMAIN Protocol commands
+ */
+enum scmi_pd_command_id {
+	SCMI_PD_ATTRIBUTES = 0x003,
+	SCMI_PD_STATE_SET = 0x004,
+	SCMI_PD_STATE_GET = 0x005,
+};
+
+/* Protocol attributes */
+struct scmi_pd_attributes_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_protocol_attributes_p2a_pd {
+	int32_t status;
+	uint32_t attributes;
+	uint32_t statistics_addr_low;
+	uint32_t statistics_addr_high;
+	uint32_t statistics_len;
+};
+
+#define SCMI_PD_NAME_LENGTH_MAX	16U
+
+struct scmi_pd_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+	char pd_name[SCMI_PD_NAME_LENGTH_MAX];
+};
+
+/*
+ * Power Domain State Get
+ */
+
+struct scmi_pd_state_get_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_pd_state_get_p2a {
+	int32_t status;
+	uint32_t power_state;
+};
+
+/*
+ * Power domain State Set
+ */
+
+struct scmi_pd_state_set_a2p {
+	uint32_t flags;
+	uint32_t pd_id;
+	uint32_t power_state;
+};
+
+struct scmi_pd_state_set_p2a {
+	int32_t status;
+};
+
+#endif /* SCMI_MSG_PD_H */
diff --git a/drivers/scmi-msg/smt.c b/drivers/scmi-msg/smt.c
index b08ee06..9b079c7 100644
--- a/drivers/scmi-msg/smt.c
+++ b/drivers/scmi-msg/smt.c
@@ -44,12 +44,12 @@
 	assert_scmi_message_max_length_fits_in_smt_buffer_slot);
 
 /* Flag set in smt_header::status when SMT does not contain pending message */
-#define SMT_STATUS_FREE			BIT(0)
+#define SMT_STATUS_FREE			BIT_32(0)
 /* Flag set in smt_header::status when SMT reports an error */
-#define SMT_STATUS_ERROR		BIT(1)
+#define SMT_STATUS_ERROR		BIT_32(1)
 
 /* Flag set in smt_header::flags when SMT uses interrupts */
-#define SMT_FLAG_INTR_ENABLED		BIT(1)
+#define SMT_FLAG_INTR_ENABLED		BIT_32(1)
 
 /* Bit fields packed in smt_header::message_header */
 #define SMT_MSG_ID_MASK			GENMASK_32(7, 0)
@@ -133,7 +133,7 @@
 			  sizeof(smt_hdr->message_header);
 
 	if (in_payload_size > SCMI_PLAYLOAD_MAX) {
-		VERBOSE("SCMI payload too big %u", in_payload_size);
+		VERBOSE("SCMI payload too big %zu", in_payload_size);
 		goto out;
 	}
 
diff --git a/fdts/tc0.dts b/fdts/tc.dts
similarity index 99%
rename from fdts/tc0.dts
rename to fdts/tc.dts
index 9051b7b..f66d556 100644
--- a/fdts/tc0.dts
+++ b/fdts/tc.dts
@@ -7,7 +7,7 @@
 /dts-v1/;
 
 / {
-	compatible = "arm,tc0";
+	compatible = "arm,tc";
 	interrupt-parent = <&gic>;
 	#address-cells = <2>;
 	#size-cells = <2>;
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 3383a3b..c12dbc4 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -263,6 +263,9 @@
 #define ID_AA64MMFR1_EL1_PAN2_SUPPORTED		ULL(0x2)
 #define ID_AA64MMFR1_EL1_PAN3_SUPPORTED		ULL(0x3)
 
+#define ID_AA64MMFR1_EL1_VHE_SHIFT		U(8)
+#define ID_AA64MMFR1_EL1_VHE_MASK		ULL(0xf)
+
 /* ID_AA64MMFR2_EL1 definitions */
 #define ID_AA64MMFR2_EL1		S3_0_C0_C7_2
 
@@ -390,7 +393,8 @@
 
 #define SCTLR_ATA0_BIT		(ULL(1) << 42)
 #define SCTLR_ATA_BIT		(ULL(1) << 43)
-#define SCTLR_DSSBS_BIT		(ULL(1) << 44)
+#define SCTLR_DSSBS_SHIFT	U(44)
+#define SCTLR_DSSBS_BIT		(ULL(1) << SCTLR_DSSBS_SHIFT)
 #define SCTLR_TWEDEn_BIT	(ULL(1) << 45)
 #define SCTLR_TWEDEL_SHIFT	U(46)
 #define SCTLR_TWEDEL_MASK	ULL(0xf)
@@ -570,8 +574,16 @@
 #define SPSR_EL_SHIFT		U(2)
 #define SPSR_EL_WIDTH		U(2)
 
-#define SPSR_SSBS_BIT_AARCH64	BIT_64(12)
-#define SPSR_SSBS_BIT_AARCH32	BIT_64(23)
+#define SPSR_SSBS_SHIFT_AARCH64 U(12)
+#define SPSR_SSBS_BIT_AARCH64	(ULL(1) << SPSR_SSBS_SHIFT_AARCH64)
+#define SPSR_SSBS_SHIFT_AARCH32 U(23)
+#define SPSR_SSBS_BIT_AARCH32	(ULL(1) << SPSR_SSBS_SHIFT_AARCH32)
+
+#define SPSR_PAN_BIT		BIT_64(22)
+
+#define SPSR_DIT_BIT		BIT(24)
+
+#define SPSR_TCO_BIT_AARCH64	BIT_64(25)
 
 #define DISABLE_ALL_EXCEPTIONS \
 		(DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT | DAIF_DBG_BIT)
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 47a797a..dc0b7f3 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -17,6 +17,18 @@
 	return true;
 }
 
+static inline bool is_armv8_1_pan_present(void)
+{
+	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_PAN_SHIFT) &
+		ID_AA64MMFR1_EL1_PAN_MASK) != 0U;
+}
+
+static inline bool is_armv8_1_vhe_present(void)
+{
+	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_VHE_SHIFT) &
+		ID_AA64MMFR1_EL1_VHE_MASK) != 0U;
+}
+
 static inline bool is_armv8_2_ttcnp_present(void)
 {
 	return ((read_id_aa64mmfr2_el1() >> ID_AA64MMFR2_EL1_CNP_SHIFT) &
diff --git a/include/arch/aarch64/asm_macros.S b/include/arch/aarch64/asm_macros.S
index 464c05b..7706cd8 100644
--- a/include/arch/aarch64/asm_macros.S
+++ b/include/arch/aarch64/asm_macros.S
@@ -10,10 +10,6 @@
 #include <common/asm_macros_common.S>
 #include <lib/spinlock.h>
 
-#if ENABLE_BTI && !ARM_ARCH_AT_LEAST(8, 5)
-#error Branch Target Identification requires ARM_ARCH_MINOR >= 5
-#endif
-
 /*
  * TLBI instruction with type specifier that implements the workaround for
  * errata 813419 of Cortex-A57 or errata 1286807 of Cortex-A76.
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 77fb1f6..e33840c 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -106,6 +106,10 @@
 IMPORT_SYM(uintptr_t, __RO_START__,		BL_CODE_BASE);
 IMPORT_SYM(uintptr_t, __RO_END__,		BL_CODE_END);
 #endif
+#if SEPARATE_NOBITS_REGION
+IMPORT_SYM(uintptr_t, __NOBITS_START__,		BL_NOBITS_BASE);
+IMPORT_SYM(uintptr_t, __NOBITS_END__,		BL_NOBITS_END);
+#endif
 IMPORT_SYM(uintptr_t, __RW_END__,		BL_END);
 
 #if defined(IMAGE_BL1)
diff --git a/include/common/debug.h b/include/common/debug.h
index ed0e8bf..a7ca0d7 100644
--- a/include/common/debug.h
+++ b/include/common/debug.h
@@ -61,8 +61,10 @@
 
 #if LOG_LEVEL >= LOG_LEVEL_ERROR
 # define ERROR(...)	tf_log(LOG_MARKER_ERROR __VA_ARGS__)
+# define ERROR_NL()	tf_log_newline(LOG_MARKER_ERROR)
 #else
 # define ERROR(...)	no_tf_log(LOG_MARKER_ERROR __VA_ARGS__)
+# define ERROR_NL()
 #endif
 
 #if LOG_LEVEL >= LOG_LEVEL_NOTICE
@@ -109,6 +111,7 @@
 void __dead2 __stack_chk_fail(void);
 
 void tf_log(const char *fmt, ...) __printflike(1, 2);
+void tf_log_newline(const char log_fmt[2]);
 void tf_log_set_max_level(unsigned int log_level);
 
 #endif /* __ASSEMBLER__ */
diff --git a/include/common/hw_crc32.h b/include/common/tf_crc32.h
similarity index 62%
rename from include/common/hw_crc32.h
rename to include/common/tf_crc32.h
index 0d14d57..38c56a5 100644
--- a/include/common/hw_crc32.h
+++ b/include/common/tf_crc32.h
@@ -4,13 +4,13 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef HW_CRC32_H
-#define HW_CRC32_H
+#ifndef TF_CRC32_H
+#define TF_CRC32_H
 
 #include <stddef.h>
 #include <stdint.h>
 
 /* compute CRC using Arm intrinsic function */
-uint32_t hw_crc32(uint32_t crc, const unsigned char *buf, size_t size);
+uint32_t tf_crc32(uint32_t crc, const unsigned char *buf, size_t size);
 
-#endif /* HW_CRC32_H */
+#endif /* TF_CRC32_H */
diff --git a/include/drivers/fwu/fwu.h b/include/drivers/fwu/fwu.h
new file mode 100644
index 0000000..ae06da9
--- /dev/null
+++ b/include/drivers/fwu/fwu.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FWU_H
+#define FWU_H
+
+#include <stdbool.h>
+
+void fwu_init(void);
+bool fwu_is_trial_run_state(void);
+
+#endif /* FWU_H */
diff --git a/include/drivers/fwu/fwu_metadata.h b/include/drivers/fwu/fwu_metadata.h
new file mode 100644
index 0000000..2e88de5
--- /dev/null
+++ b/include/drivers/fwu/fwu_metadata.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * FWU metadata information as per the specification section 4.1:
+ * https://developer.arm.com/documentation/den0118/a/
+ *
+ */
+
+#ifndef FWU_METADATA_H
+#define FWU_METADATA_H
+
+#include <stdint.h>
+#include <tools_share/uuid.h>
+
+/* Properties of image in a bank */
+struct fwu_image_properties {
+
+	/* UUID of the image in this bank */
+	uuid_t img_uuid;
+
+	/* [0]: bit describing the image acceptance status –
+	 *      1 means the image is accepted
+	 * [31:1]: MBZ
+	 */
+	uint32_t accepted;
+
+	/* reserved (MBZ) */
+	uint32_t reserved;
+
+} __packed;
+
+/* Image entry information */
+struct fwu_image_entry {
+
+	/* UUID identifying the image type */
+	uuid_t img_type_uuid;
+
+	/* UUID of the storage volume where the image is located */
+	uuid_t location_uuid;
+
+	/* Properties of images with img_type_uuid in the different FW banks */
+	struct fwu_image_properties img_props[NR_OF_FW_BANKS];
+
+} __packed;
+
+/*
+ * FWU metadata filled by the updater and consumed by TF-A for
+ * various purposes as below:
+ * 1. Get active FW bank.
+ * 2. Rollback to previous working FW bank.
+ * 3. Get properties of all images present in all banks.
+ */
+struct fwu_metadata {
+
+	/* Metadata CRC value */
+	uint32_t crc_32;
+
+	/* Metadata version */
+	uint32_t version;
+
+	/* Bank index with which device boots */
+	uint32_t active_index;
+
+	/* Previous bank index with which device booted successfully */
+	uint32_t previous_active_index;
+
+	/* Image entry information */
+	struct fwu_image_entry img_entry[NR_OF_IMAGES_IN_FW_BANK];
+
+} __packed;
+
+#endif /* FWU_METADATA_H */
diff --git a/include/drivers/io/io_mtd.h b/include/drivers/io/io_mtd.h
index 1395ff6..2b5d9b1 100644
--- a/include/drivers/io/io_mtd.h
+++ b/include/drivers/io/io_mtd.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,11 +44,22 @@
 	 * Return 0 on success, a negative error code otherwise.
 	 */
 	int (*write)(unsigned int offset, uintptr_t buffer, size_t length);
+
+	/*
+	 * Look for an offset to be added to the given offset.
+	 *
+	 * @base: Base address of the area.
+	 * @offset: Offset in bytes to start read operation.
+	 * @extra_offset: [out] Offset to be added to the previous offset.
+	 * Return 0 on success, a negative error code otherwise.
+	 */
+	int (*seek)(uintptr_t base, unsigned int offset, size_t *extra_offset);
 } io_mtd_ops_t;
 
 typedef struct io_mtd_dev_spec {
 	unsigned long long device_size;
 	unsigned int erase_size;
+	size_t offset;
 	io_mtd_ops_t ops;
 } io_mtd_dev_spec_t;
 
diff --git a/include/drivers/nand.h b/include/drivers/nand.h
index 1dbb008..1b78ad4 100644
--- a/include/drivers/nand.h
+++ b/include/drivers/nand.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -46,6 +46,16 @@
 	      size_t *length_read);
 
 /*
+ * Look for an extra offset to be added in case of bad blocks
+ *
+ * @base: Base address of the area
+ * @offset: Byte offset to read from in device
+ * @extra_offset: [out] Extra offset to be added if bad blocks are found
+ * Return: 0 on success, a negative errno on failure
+ */
+int nand_seek_bb(uintptr_t base, unsigned int offset, size_t *extra_offset);
+
+/*
  * Get NAND device instance
  *
  * Return: NAND device instance reference
diff --git a/drivers/nxp/auth/csf_hdr_parser/csf_hdr.h b/include/drivers/nxp/auth/csf_hdr_parser/csf_hdr.h
similarity index 98%
rename from drivers/nxp/auth/csf_hdr_parser/csf_hdr.h
rename to include/drivers/nxp/auth/csf_hdr_parser/csf_hdr.h
index eaead76..ae56d3b 100644
--- a/drivers/nxp/auth/csf_hdr_parser/csf_hdr.h
+++ b/include/drivers/nxp/auth/csf_hdr_parser/csf_hdr.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/console/plat_console.h b/include/drivers/nxp/console/plat_console.h
similarity index 100%
rename from drivers/nxp/console/plat_console.h
rename to include/drivers/nxp/console/plat_console.h
diff --git a/drivers/nxp/crypto/caam/include/caam.h b/include/drivers/nxp/crypto/caam/caam.h
similarity index 97%
rename from drivers/nxp/crypto/caam/include/caam.h
rename to include/drivers/nxp/crypto/caam/caam.h
index 580e133..4984b54 100644
--- a/drivers/nxp/crypto/caam/include/caam.h
+++ b/include/drivers/nxp/crypto/caam/caam.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/caam_io.h b/include/drivers/nxp/crypto/caam/caam_io.h
similarity index 97%
rename from drivers/nxp/crypto/caam/include/caam_io.h
rename to include/drivers/nxp/crypto/caam/caam_io.h
index 4fdb04d..b68f836 100644
--- a/drivers/nxp/crypto/caam/include/caam_io.h
+++ b/include/drivers/nxp/crypto/caam/caam_io.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2018-2020 NXP
+ * Copyright 2018-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/hash.h b/include/drivers/nxp/crypto/caam/hash.h
similarity index 98%
rename from drivers/nxp/crypto/caam/include/hash.h
rename to include/drivers/nxp/crypto/caam/hash.h
index 946087d..9136dca 100644
--- a/drivers/nxp/crypto/caam/include/hash.h
+++ b/include/drivers/nxp/crypto/caam/hash.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/jobdesc.h b/include/drivers/nxp/crypto/caam/jobdesc.h
similarity index 97%
rename from drivers/nxp/crypto/caam/include/jobdesc.h
rename to include/drivers/nxp/crypto/caam/jobdesc.h
index 5921f7b..efef228 100644
--- a/drivers/nxp/crypto/caam/include/jobdesc.h
+++ b/include/drivers/nxp/crypto/caam/jobdesc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/jr_driver_config.h b/include/drivers/nxp/crypto/caam/jr_driver_config.h
similarity index 99%
rename from drivers/nxp/crypto/caam/include/jr_driver_config.h
rename to include/drivers/nxp/crypto/caam/jr_driver_config.h
index f25c42e..1b3c447 100644
--- a/drivers/nxp/crypto/caam/include/jr_driver_config.h
+++ b/include/drivers/nxp/crypto/caam/jr_driver_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/rsa.h b/include/drivers/nxp/crypto/caam/rsa.h
similarity index 96%
rename from drivers/nxp/crypto/caam/include/rsa.h
rename to include/drivers/nxp/crypto/caam/rsa.h
index bd5dc71..dd9ecdc 100644
--- a/drivers/nxp/crypto/caam/include/rsa.h
+++ b/include/drivers/nxp/crypto/caam/rsa.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/sec_hw_specific.h b/include/drivers/nxp/crypto/caam/sec_hw_specific.h
similarity index 99%
rename from drivers/nxp/crypto/caam/include/sec_hw_specific.h
rename to include/drivers/nxp/crypto/caam/sec_hw_specific.h
index a82a1a0..a4fc022 100644
--- a/drivers/nxp/crypto/caam/include/sec_hw_specific.h
+++ b/include/drivers/nxp/crypto/caam/sec_hw_specific.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/crypto/caam/include/sec_jr_driver.h b/include/drivers/nxp/crypto/caam/sec_jr_driver.h
similarity index 99%
rename from drivers/nxp/crypto/caam/include/sec_jr_driver.h
rename to include/drivers/nxp/crypto/caam/sec_jr_driver.h
index 1381eab..57e0fa0 100644
--- a/drivers/nxp/crypto/caam/include/sec_jr_driver.h
+++ b/include/drivers/nxp/crypto/caam/sec_jr_driver.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/csu/csu.h b/include/drivers/nxp/csu/csu.h
similarity index 96%
rename from drivers/nxp/csu/csu.h
rename to include/drivers/nxp/csu/csu.h
index 9f82feb..3a43e45 100644
--- a/drivers/nxp/csu/csu.h
+++ b/include/drivers/nxp/csu/csu.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/dcfg/dcfg.h b/include/drivers/nxp/dcfg/dcfg.h
similarity index 98%
rename from drivers/nxp/dcfg/dcfg.h
rename to include/drivers/nxp/dcfg/dcfg.h
index 161e295..3f4855a 100644
--- a/drivers/nxp/dcfg/dcfg.h
+++ b/include/drivers/nxp/dcfg/dcfg.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2018-2020 NXP
+ * Copyright 2018-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/dcfg/dcfg_lsch2.h b/include/drivers/nxp/dcfg/dcfg_lsch2.h
similarity index 98%
rename from drivers/nxp/dcfg/dcfg_lsch2.h
rename to include/drivers/nxp/dcfg/dcfg_lsch2.h
index c021aa1..2838aca 100644
--- a/drivers/nxp/dcfg/dcfg_lsch2.h
+++ b/include/drivers/nxp/dcfg/dcfg_lsch2.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/dcfg/dcfg_lsch3.h b/include/drivers/nxp/dcfg/dcfg_lsch3.h
similarity index 98%
rename from drivers/nxp/dcfg/dcfg_lsch3.h
rename to include/drivers/nxp/dcfg/dcfg_lsch3.h
index 8144542..40f02c1 100644
--- a/drivers/nxp/dcfg/dcfg_lsch3.h
+++ b/include/drivers/nxp/dcfg/dcfg_lsch3.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/dcfg/scfg.h b/include/drivers/nxp/dcfg/scfg.h
similarity index 98%
rename from drivers/nxp/dcfg/scfg.h
rename to include/drivers/nxp/dcfg/scfg.h
index 81df9a6..b6e3df5 100644
--- a/drivers/nxp/dcfg/scfg.h
+++ b/include/drivers/nxp/dcfg/scfg.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/ddr/include/ddr.h b/include/drivers/nxp/ddr/ddr.h
similarity index 100%
rename from drivers/nxp/ddr/include/ddr.h
rename to include/drivers/nxp/ddr/ddr.h
diff --git a/drivers/nxp/ddr/include/ddr_io.h b/include/drivers/nxp/ddr/ddr_io.h
similarity index 100%
rename from drivers/nxp/ddr/include/ddr_io.h
rename to include/drivers/nxp/ddr/ddr_io.h
diff --git a/drivers/nxp/ddr/include/dimm.h b/include/drivers/nxp/ddr/dimm.h
similarity index 100%
rename from drivers/nxp/ddr/include/dimm.h
rename to include/drivers/nxp/ddr/dimm.h
diff --git a/drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.h b/include/drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.h
similarity index 100%
rename from drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.h
rename to include/drivers/nxp/ddr/fsl-mmdc/fsl_mmdc.h
diff --git a/drivers/nxp/ddr/include/immap.h b/include/drivers/nxp/ddr/immap.h
similarity index 100%
rename from drivers/nxp/ddr/include/immap.h
rename to include/drivers/nxp/ddr/immap.h
diff --git a/drivers/nxp/ddr/include/opts.h b/include/drivers/nxp/ddr/opts.h
similarity index 100%
rename from drivers/nxp/ddr/include/opts.h
rename to include/drivers/nxp/ddr/opts.h
diff --git a/drivers/nxp/ddr/include/regs.h b/include/drivers/nxp/ddr/regs.h
similarity index 100%
rename from drivers/nxp/ddr/include/regs.h
rename to include/drivers/nxp/ddr/regs.h
diff --git a/drivers/nxp/ddr/include/utility.h b/include/drivers/nxp/ddr/utility.h
similarity index 100%
rename from drivers/nxp/ddr/include/utility.h
rename to include/drivers/nxp/ddr/utility.h
diff --git a/drivers/nxp/gic/include/gicv2/plat_gic.h b/include/drivers/nxp/gic/gicv2/plat_gic.h
similarity index 100%
rename from drivers/nxp/gic/include/gicv2/plat_gic.h
rename to include/drivers/nxp/gic/gicv2/plat_gic.h
diff --git a/drivers/nxp/gic/include/gicv3/plat_gic.h b/include/drivers/nxp/gic/gicv3/plat_gic.h
similarity index 100%
rename from drivers/nxp/gic/include/gicv3/plat_gic.h
rename to include/drivers/nxp/gic/gicv3/plat_gic.h
diff --git a/drivers/nxp/gpio/nxp_gpio.h b/include/drivers/nxp/gpio/nxp_gpio.h
similarity index 100%
rename from drivers/nxp/gpio/nxp_gpio.h
rename to include/drivers/nxp/gpio/nxp_gpio.h
diff --git a/drivers/nxp/i2c/i2c.h b/include/drivers/nxp/i2c/i2c.h
similarity index 97%
rename from drivers/nxp/i2c/i2c.h
rename to include/drivers/nxp/i2c/i2c.h
index 925bbc0..85e6eb4 100644
--- a/drivers/nxp/i2c/i2c.h
+++ b/include/drivers/nxp/i2c/i2c.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2016-2020 NXP
+ * Copyright 2016-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/interconnect/ls_interconnect.h b/include/drivers/nxp/interconnect/ls_interconnect.h
similarity index 92%
rename from drivers/nxp/interconnect/ls_interconnect.h
rename to include/drivers/nxp/interconnect/ls_interconnect.h
index 26787fb..777089c 100644
--- a/drivers/nxp/interconnect/ls_interconnect.h
+++ b/include/drivers/nxp/interconnect/ls_interconnect.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/pmu/pmu.h b/include/drivers/nxp/pmu/pmu.h
similarity index 100%
rename from drivers/nxp/pmu/pmu.h
rename to include/drivers/nxp/pmu/pmu.h
diff --git a/drivers/nxp/qspi/qspi.h b/include/drivers/nxp/qspi/qspi.h
similarity index 100%
rename from drivers/nxp/qspi/qspi.h
rename to include/drivers/nxp/qspi/qspi.h
diff --git a/drivers/nxp/sd/sd_mmc.h b/include/drivers/nxp/sd/sd_mmc.h
similarity index 99%
rename from drivers/nxp/sd/sd_mmc.h
rename to include/drivers/nxp/sd/sd_mmc.h
index 29ad328..32b41f1 100644
--- a/drivers/nxp/sd/sd_mmc.h
+++ b/include/drivers/nxp/sd/sd_mmc.h
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2015, 2016 Freescale Semiconductor, Inc.
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2021 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
diff --git a/drivers/nxp/sec_mon/snvs.h b/include/drivers/nxp/sec_mon/snvs.h
similarity index 100%
rename from drivers/nxp/sec_mon/snvs.h
rename to include/drivers/nxp/sec_mon/snvs.h
diff --git a/drivers/nxp/sfp/fuse_prov.h b/include/drivers/nxp/sfp/fuse_prov.h
similarity index 100%
rename from drivers/nxp/sfp/fuse_prov.h
rename to include/drivers/nxp/sfp/fuse_prov.h
diff --git a/drivers/nxp/sfp/sfp.h b/include/drivers/nxp/sfp/sfp.h
similarity index 100%
rename from drivers/nxp/sfp/sfp.h
rename to include/drivers/nxp/sfp/sfp.h
diff --git a/drivers/nxp/sfp/sfp_error_codes.h b/include/drivers/nxp/sfp/sfp_error_codes.h
similarity index 100%
rename from drivers/nxp/sfp/sfp_error_codes.h
rename to include/drivers/nxp/sfp/sfp_error_codes.h
diff --git a/drivers/nxp/timer/nxp_timer.h b/include/drivers/nxp/timer/nxp_timer.h
similarity index 100%
rename from drivers/nxp/timer/nxp_timer.h
rename to include/drivers/nxp/timer/nxp_timer.h
diff --git a/drivers/nxp/tzc/plat_tzc400.h b/include/drivers/nxp/tzc/plat_tzc400.h
similarity index 100%
rename from drivers/nxp/tzc/plat_tzc400.h
rename to include/drivers/nxp/tzc/plat_tzc400.h
diff --git a/include/export/common/tbbr/tbbr_img_def_exp.h b/include/export/common/tbbr/tbbr_img_def_exp.h
index 18f0125..2623c75 100644
--- a/include/export/common/tbbr/tbbr_img_def_exp.h
+++ b/include/export/common/tbbr/tbbr_img_def_exp.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -91,7 +91,17 @@
 /* FW_CONFIG */
 #define FW_CONFIG_ID			U(31)
 
+/*
+ * Primary FWU metadata image ID
+ */
+#define FWU_METADATA_IMAGE_ID		U(32)
+
+/*
+ * Backup FWU metadata image ID
+ */
+#define BKUP_FWU_METADATA_IMAGE_ID	U(33)
+
 /* Max Images */
-#define MAX_IMAGE_IDS			U(32)
+#define MAX_IMAGE_IDS			U(34)
 
 #endif /* ARM_TRUSTED_FIRMWARE_EXPORT_COMMON_TBBR_TBBR_IMG_DEF_EXP_H */
diff --git a/include/lib/cpus/aarch64/neoverse_v1.h b/include/lib/cpus/aarch64/neoverse_v1.h
index 650eb4d..cfb26ab 100644
--- a/include/lib/cpus/aarch64/neoverse_v1.h
+++ b/include/lib/cpus/aarch64/neoverse_v1.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2021, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,6 +13,8 @@
  * CPU Extended Control register specific definitions.
  ******************************************************************************/
 #define NEOVERSE_V1_CPUECTLR_EL1				S3_0_C15_C1_4
+#define NEOVERSE_V1_CPUECTLR_EL1_BIT_8				(ULL(1) << 8)
+#define NEOVERSE_V1_CPUECTLR_EL1_BIT_53				(ULL(1) << 53)
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
@@ -20,4 +22,11 @@
 #define NEOVERSE_V1_CPUPWRCTLR_EL1				S3_0_C15_C2_7
 #define NEOVERSE_V1_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
 
+/*******************************************************************************
+ * CPU Auxiliary Control register specific definitions.
+ ******************************************************************************/
+#define NEOVERSE_V1_ACTLR2_EL1					S3_0_C15_C1_1
+#define NEOVERSE_V1_ACTLR2_EL1_BIT_2				(ULL(1) << 2)
+#define NEOVERSE_V1_ACTLR2_EL1_BIT_28				(ULL(1) << 28)
+
 #endif /* NEOVERSE_V1_H */
diff --git a/include/lib/extensions/amu.h b/include/lib/extensions/amu.h
index 3a70e4f..3a254c9 100644
--- a/include/lib/extensions/amu.h
+++ b/include/lib/extensions/amu.h
@@ -13,6 +13,7 @@
 #include <lib/cassert.h>
 #include <lib/utils_def.h>
 
+#include <context.h>
 #include <platform_def.h>
 
 /* All group 0 counters */
@@ -80,7 +81,11 @@
 };
 
 unsigned int amu_get_version(void);
+#if __aarch64__
+void amu_enable(bool el2_unused, cpu_context_t *ctx);
+#else
 void amu_enable(bool el2_unused);
+#endif
 
 /* Group 0 configuration helpers */
 uint64_t amu_group0_cnt_read(unsigned int idx);
diff --git a/include/lib/extensions/mpam.h b/include/lib/extensions/mpam.h
index ac8c00a..414adcb 100644
--- a/include/lib/extensions/mpam.h
+++ b/include/lib/extensions/mpam.h
@@ -9,7 +9,6 @@
 
 #include <stdbool.h>
 
-bool mpam_supported(void);
 void mpam_enable(bool el2_unused);
 
 #endif /* MPAM_H */
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 846c9a4..0a19d8b 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -153,7 +153,9 @@
 int arm_io_setup(void);
 
 /* Set image specification in IO block policy */
-int arm_set_image_source(unsigned int image_id, const char *part_name);
+int arm_set_image_source(unsigned int image_id, const char *part_name,
+			 uintptr_t *dev_handle, uintptr_t *image_spec);
+void arm_set_fip_addr(uint32_t active_fw_bank_idx);
 
 /* Security utility functions */
 void arm_tzc400_setup(uintptr_t tzc_base,
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 1def86e..2d5c521 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -16,6 +16,7 @@
 #if TRNG_SUPPORT
 #include "plat_trng.h"
 #endif
+#include <drivers/fwu/fwu_metadata.h>
 
 /*******************************************************************************
  * Forward declarations
@@ -349,4 +350,12 @@
  */
 int32_t plat_is_smccc_feature_available(u_register_t fid);
 
+/*******************************************************************************
+ * FWU platform specific functions
+ ******************************************************************************/
+int plat_fwu_set_metadata_image_source(unsigned int image_id,
+				       uintptr_t *dev_handle,
+				       uintptr_t *image_spec);
+void plat_fwu_set_images_source(struct fwu_metadata *metadata);
+
 #endif /* PLATFORM_H */
diff --git a/include/services/ffa_svc.h b/include/services/ffa_svc.h
index 5b39c42..ab36d9e 100644
--- a/include/services/ffa_svc.h
+++ b/include/services/ffa_svc.h
@@ -22,7 +22,7 @@
 
 /* The macros below are used to identify FFA calls from the SMC function ID */
 #define FFA_FNUM_MIN_VALUE	U(0x60)
-#define FFA_FNUM_MAX_VALUE	U(0x85)
+#define FFA_FNUM_MAX_VALUE	U(0x87)
 #define is_ffa_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
 	((GET_SMC_NUM(_fid) >= FFA_FNUM_MIN_VALUE) &&	\
@@ -32,7 +32,7 @@
 #define FFA_VERSION_MAJOR		U(1)
 #define FFA_VERSION_MAJOR_SHIFT		16
 #define FFA_VERSION_MAJOR_MASK		U(0x7FFF)
-#define FFA_VERSION_MINOR		U(0)
+#define FFA_VERSION_MINOR		U(1)
 #define FFA_VERSION_MINOR_SHIFT		0
 #define FFA_VERSION_MINOR_MASK		U(0xFFFF)
 #define FFA_VERSION_BIT31_MASK 		U(0x1u << 31)
@@ -61,32 +61,44 @@
 		 ((func_num) << FUNCID_NUM_SHIFT))
 
 /* FFA function numbers */
-#define FFA_FNUM_ERROR			U(0x60)
-#define FFA_FNUM_SUCCESS		U(0x61)
-#define FFA_FNUM_INTERRUPT		U(0x62)
-#define FFA_FNUM_VERSION		U(0x63)
-#define FFA_FNUM_FEATURES		U(0x64)
-#define FFA_FNUM_RX_RELEASE		U(0x65)
-#define FFA_FNUM_RXTX_MAP		U(0x66)
-#define FFA_FNUM_RXTX_UNMAP		U(0x67)
-#define FFA_FNUM_PARTITION_INFO_GET	U(0x68)
-#define FFA_FNUM_ID_GET			U(0x69)
-#define FFA_FNUM_MSG_POLL		U(0x6A)
-#define FFA_FNUM_MSG_WAIT		U(0x6B)
-#define FFA_FNUM_MSG_YIELD		U(0x6C)
-#define FFA_FNUM_MSG_RUN		U(0x6D)
-#define FFA_FNUM_MSG_SEND		U(0x6E)
-#define FFA_FNUM_MSG_SEND_DIRECT_REQ	U(0x6F)
-#define FFA_FNUM_MSG_SEND_DIRECT_RESP	U(0x70)
-#define FFA_FNUM_MEM_DONATE		U(0x71)
-#define FFA_FNUM_MEM_LEND		U(0x72)
-#define FFA_FNUM_MEM_SHARE		U(0x73)
-#define FFA_FNUM_MEM_RETRIEVE_REQ	U(0x74)
-#define FFA_FNUM_MEM_RETRIEVE_RESP	U(0x75)
-#define FFA_FNUM_MEM_RELINQUISH	U(0x76)
-#define FFA_FNUM_MEM_RECLAIM		U(0x77)
-#define FFA_FNUM_SECONDARY_EP_REGISTER	U(0x84)
-#define FFA_FNUM_SPM_ID_GET		U(0x85)
+#define FFA_FNUM_ERROR				U(0x60)
+#define FFA_FNUM_SUCCESS			U(0x61)
+#define FFA_FNUM_INTERRUPT			U(0x62)
+#define FFA_FNUM_VERSION			U(0x63)
+#define FFA_FNUM_FEATURES			U(0x64)
+#define FFA_FNUM_RX_RELEASE			U(0x65)
+#define FFA_FNUM_RXTX_MAP			U(0x66)
+#define FFA_FNUM_RXTX_UNMAP			U(0x67)
+#define FFA_FNUM_PARTITION_INFO_GET		U(0x68)
+#define FFA_FNUM_ID_GET				U(0x69)
+#define FFA_FNUM_MSG_POLL			U(0x6A) /* Legacy FF-A v1.0 */
+#define FFA_FNUM_MSG_WAIT			U(0x6B)
+#define FFA_FNUM_MSG_YIELD			U(0x6C)
+#define FFA_FNUM_MSG_RUN			U(0x6D)
+#define FFA_FNUM_MSG_SEND			U(0x6E) /* Legacy FF-A v1.0 */
+#define FFA_FNUM_MSG_SEND_DIRECT_REQ		U(0x6F)
+#define FFA_FNUM_MSG_SEND_DIRECT_RESP		U(0x70)
+#define FFA_FNUM_MEM_DONATE			U(0x71)
+#define FFA_FNUM_MEM_LEND			U(0x72)
+#define FFA_FNUM_MEM_SHARE			U(0x73)
+#define FFA_FNUM_MEM_RETRIEVE_REQ		U(0x74)
+#define FFA_FNUM_MEM_RETRIEVE_RESP		U(0x75)
+#define FFA_FNUM_MEM_RELINQUISH			U(0x76)
+#define FFA_FNUM_MEM_RECLAIM			U(0x77)
+#define FFA_FNUM_NORMAL_WORLD_RESUME		U(0x7C)
+
+/* FF-A v1.1 */
+#define FFA_FNUM_NOTIFICATION_BITMAP_CREATE	U(0x7D)
+#define FFA_FNUM_NOTIFICATION_BITMAP_DESTROY	U(0x7E)
+#define FFA_FNUM_NOTIFICATION_BIND		U(0x7F)
+#define FFA_FNUM_NOTIFICATION_UNBIND		U(0x80)
+#define FFA_FNUM_NOTIFICATION_SET		U(0x81)
+#define FFA_FNUM_NOTIFICATION_GET		U(0x82)
+#define FFA_FNUM_NOTIFICATION_INFO_GET		U(0x83)
+#define FFA_FNUM_RX_ACQUIRE			U(0x84)
+#define FFA_FNUM_SPM_ID_GET			U(0x85)
+#define FFA_FNUM_MSG_SEND2			U(0x86)
+#define FFA_FNUM_SECONDARY_EP_REGISTER		U(0x87)
 
 /* FFA SMC32 FIDs */
 #define FFA_ERROR		FFA_FID(SMC_32, FFA_FNUM_ERROR)
diff --git a/lib/cpus/aarch64/neoverse_v1.S b/lib/cpus/aarch64/neoverse_v1.S
index 7336294..0bcf52a 100644
--- a/lib/cpus/aarch64/neoverse_v1.S
+++ b/lib/cpus/aarch64/neoverse_v1.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,6 +21,244 @@
 #error "Neoverse-V1 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #1774420.
+	 * This applies to revisions r0p0 and r1p0, fixed in r1p1.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1774420_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_1774420
+	cbz	x0, 1f
+
+	/* Set bit 53 in CPUECTLR_EL1 */
+	mrs     x1, NEOVERSE_V1_CPUECTLR_EL1
+	orr	x1, x1, #NEOVERSE_V1_CPUECTLR_EL1_BIT_53
+	msr     NEOVERSE_V1_CPUECTLR_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_1774420_wa
+
+func check_errata_1774420
+	/* Applies to r0p0 and r1p0. */
+	mov	x1, #0x10
+	b	cpu_rev_var_ls
+endfunc check_errata_1774420
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #1791573.
+	 * This applies to revisions r0p0 and r1p0, fixed in r1p1.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1791573_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_1791573
+	cbz	x0, 1f
+
+	/* Set bit 2 in ACTLR2_EL1 */
+	mrs	x1, NEOVERSE_V1_ACTLR2_EL1
+	orr	x1, x1, #NEOVERSE_V1_ACTLR2_EL1_BIT_2
+	msr	NEOVERSE_V1_ACTLR2_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_1791573_wa
+
+func check_errata_1791573
+	/* Applies to r0p0 and r1p0. */
+	mov	x1, #0x10
+	b	cpu_rev_var_ls
+endfunc check_errata_1791573
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #1852267.
+	 * This applies to revisions r0p0 and r1p0, fixed in r1p1.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1852267_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_1852267
+	cbz	x0, 1f
+
+	/* Set bit 28 in ACTLR2_EL1 */
+	mrs	x1, NEOVERSE_V1_ACTLR2_EL1
+	orr	x1, x1, #NEOVERSE_V1_ACTLR2_EL1_BIT_28
+	msr	NEOVERSE_V1_ACTLR2_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_1852267_wa
+
+func check_errata_1852267
+	/* Applies to r0p0 and r1p0. */
+	mov	x1, #0x10
+	b	cpu_rev_var_ls
+endfunc check_errata_1852267
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #1925756.
+	 * This applies to revisions <= r1p1.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1925756_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_1925756
+	cbz	x0, 1f
+
+	/* Set bit 8 in CPUECTLR_EL1 */
+	mrs	x1, NEOVERSE_V1_CPUECTLR_EL1
+	orr	x1, x1, #NEOVERSE_V1_CPUECTLR_EL1_BIT_8
+	msr	NEOVERSE_V1_CPUECTLR_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_1925756_wa
+
+func check_errata_1925756
+	/* Applies to <= r1p1. */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_1925756
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Erratum #1940577
+	 * This applies to revisions r1p0 - r1p1 and is open.
+	 * It also exists in r0p0 but there is no fix in that
+	 * revision.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1940577_wa
+	/* Compare x0 against revisions r1p0 - r1p1 */
+	mov	x17, x30
+	bl	check_errata_1940577
+	cbz	x0, 1f
+
+	mov	x0, #0
+	msr	S3_6_C15_C8_0, x0
+	ldr	x0, =0x10E3900002
+	msr	S3_6_C15_C8_2, x0
+	ldr	x0, =0x10FFF00083
+	msr	S3_6_C15_C8_3, x0
+	ldr	x0, =0x2001003FF
+	msr	S3_6_C15_C8_1, x0
+
+	mov	x0, #1
+	msr	S3_6_C15_C8_0, x0
+	ldr	x0, =0x10E3800082
+	msr	S3_6_C15_C8_2, x0
+	ldr	x0, =0x10FFF00083
+	msr	S3_6_C15_C8_3, x0
+	ldr	x0, =0x2001003FF
+	msr	S3_6_C15_C8_1, x0
+
+	mov	x0, #2
+	msr	S3_6_C15_C8_0, x0
+	ldr	x0, =0x10E3800200
+	msr	S3_6_C15_C8_2, x0
+	ldr	x0, =0x10FFF003E0
+	msr	S3_6_C15_C8_3, x0
+	ldr	x0, =0x2001003FF
+	msr	S3_6_C15_C8_1, x0
+
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_1940577_wa
+
+func check_errata_1940577
+	/* Applies to revisions r1p0 - r1p1. */
+	mov	x1, #0x10
+	mov	x2, #0x11
+	b	cpu_rev_var_range
+endfunc check_errata_1940577
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #1966096
+	 * This applies to revisions r1p0 - r1p1 and is open.
+	 * It also exists in r0p0 but there is no workaround
+	 * for that revision.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1966096_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_1966096
+	cbz	x0, 1f
+
+	/* Apply the workaround. */
+	mov	x0, #0x3
+	msr	S3_6_C15_C8_0, x0
+	ldr	x0, =0xEE010F12
+	msr	S3_6_C15_C8_2, x0
+	ldr	x0, =0xFFFF0FFF
+	msr	S3_6_C15_C8_3, x0
+	ldr	x0, =0x80000000003FF
+	msr	S3_6_C15_C8_1, x0
+	isb
+
+1:
+	ret	x17
+endfunc errata_neoverse_v1_1966096_wa
+
+func check_errata_1966096
+	mov	x1, #0x10
+	mov	x2, #0x11
+	b	cpu_rev_var_range
+endfunc check_errata_1966096
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #2139242.
+	 * This applies to revisions r0p0, r1p0, and r1p1, it
+	 * is still open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_2139242_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2139242
+	cbz	x0, 1f
+
+	/* Apply the workaround. */
+	mov	x0, #0x3
+	msr	S3_6_C15_C8_0, x0
+	ldr	x0, =0xEE720F14
+	msr	S3_6_C15_C8_2, x0
+	ldr	x0, =0xFFFF0FDF
+	msr	S3_6_C15_C8_3, x0
+	ldr	x0, =0x40000005003FF
+	msr	S3_6_C15_C8_1, x0
+	isb
+
+1:
+	ret	x17
+endfunc errata_neoverse_v1_2139242_wa
+
+func check_errata_2139242
+	/* Applies to r0p0, r1p0, r1p1 */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2139242
+
 	/* ---------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ---------------------------------------------
@@ -42,6 +280,24 @@
 	 */
 #if REPORT_ERRATA
 func neoverse_v1_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata ERRATA_V1_1774420, neoverse_v1, 1774420
+	report_errata ERRATA_V1_1791573, neoverse_v1, 1791573
+	report_errata ERRATA_V1_1852267, neoverse_v1, 1852267
+	report_errata ERRATA_V1_1925756, neoverse_v1, 1925756
+	report_errata ERRATA_V1_1940577, neoverse_v1, 1940577
+	report_errata ERRATA_V1_1966096, neoverse_v1, 1966096
+	report_errata ERRATA_V1_2139242, neoverse_v1, 2139242
+
+	ldp	x8, x30, [sp], #16
 	ret
 endfunc neoverse_v1_errata_report
 #endif
@@ -51,8 +307,43 @@
 
 	/* Disable speculative loads */
 	msr	SSBS, xzr
-
 	isb
+
+#if ERRATA_V1_1774420
+	mov	x0, x18
+	bl	errata_neoverse_v1_1774420_wa
+#endif
+
+#if ERRATA_V1_1791573
+	mov	x0, x18
+	bl	errata_neoverse_v1_1791573_wa
+#endif
+
+#if ERRATA_V1_1852267
+	mov	x0, x18
+	bl	errata_neoverse_v1_1852267_wa
+#endif
+
+#if ERRATA_V1_1925756
+	mov	x0, x18
+	bl	errata_neoverse_v1_1925756_wa
+#endif
+
+#if ERRATA_V1_1940577
+	mov	x0, x18
+	bl	errata_neoverse_v1_1940577_wa
+#endif
+
+#if ERRATA_V1_1966096
+	mov	x0, x18
+	bl	errata_neoverse_v1_1966096_wa
+#endif
+
+#if ERRATA_V1_2139242
+	mov	x0, x18
+	bl	errata_neoverse_v1_2139242_wa
+#endif
+
 	ret	x19
 endfunc neoverse_v1_reset_func
 
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index b1747af..050a56e 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -372,6 +372,35 @@
 # exists in revisions r0p0, r1p0, and r2p0 as well but there is no workaround.
 ERRATA_N1_1946160	?=0
 
+# Flag to apply erratum 1774420 workaround during reset.  This erratum applies
+# to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
+ERRATA_V1_1774420	?=0
+
+# Flag to apply erratum 1791573 workaround during reset.  This erratum applies
+# to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
+ERRATA_V1_1791573	?=0
+
+# Flag to apply erratum 1852267 workaround during reset.  This erratum applies
+# to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
+ERRATA_V1_1852267	?=0
+
+# Flag to apply erratum 1925756 workaround during reset.  This needs to be
+# enabled for r0p0, r1p0, and r1p1 of the Neoverse V1 core, it is still open.
+ERRATA_V1_1925756	?=0
+
+# Flag to apply erratum 1940577 workaround during reset. This erratum applies
+# to revisions r1p0 and r1p1 of the Neoverse V1 cpu.
+ERRATA_V1_1940577	?=0
+
+# Flag to apply erratum 1966096 workaround during reset. This erratum applies
+# to revisions r1p0 and r1p1 of the Neoverse V1 CPU and is open.  This issue
+# exists in r0p0 as well but there is no workaround for that revision.
+ERRATA_V1_1966096   ?=0
+
+# Flag to apply erratum 2139242 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r1p1 of the Neoverse V1 cpu and is still open.
+ERRATA_V1_2139242   ?=0
+
 # Flag to apply DSU erratum 798953. This erratum applies to DSUs revision r0p0.
 # Applying the workaround results in higher DSU power consumption on idle.
 ERRATA_DSU_798953	?=0
@@ -677,6 +706,34 @@
 $(eval $(call assert_boolean,ERRATA_N1_1946160))
 $(eval $(call add_define,ERRATA_N1_1946160))
 
+# Process ERRATA_V1_1774420 flag
+$(eval $(call assert_boolean,ERRATA_V1_1774420))
+$(eval $(call add_define,ERRATA_V1_1774420))
+
+# Process ERRATA_V1_1791573 flag
+$(eval $(call assert_boolean,ERRATA_V1_1791573))
+$(eval $(call add_define,ERRATA_V1_1791573))
+
+# Process ERRATA_V1_1852267 flag
+$(eval $(call assert_boolean,ERRATA_V1_1852267))
+$(eval $(call add_define,ERRATA_V1_1852267))
+
+# Process ERRATA_V1_1925756 flag
+$(eval $(call assert_boolean,ERRATA_V1_1925756))
+$(eval $(call add_define,ERRATA_V1_1925756))
+
+# Process ERRATA_V1_1940577 flag
+$(eval $(call assert_boolean,ERRATA_V1_1940577))
+$(eval $(call add_define,ERRATA_V1_1940577))
+
+# Process ERRATA_V1_1966096 flag
+$(eval $(call assert_boolean,ERRATA_V1_1966096))
+$(eval $(call add_define,ERRATA_V1_1966096))
+
+# Process ERRATA_V1_2139242 flag
+$(eval $(call assert_boolean,ERRATA_V1_2139242))
+$(eval $(call add_define,ERRATA_V1_2139242))
+
 # Process ERRATA_DSU_798953 flag
 $(eval $(call assert_boolean,ERRATA_DSU_798953))
 $(eval $(call add_define,ERRATA_DSU_798953))
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index d610fd4..40e7ddf 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -903,16 +903,11 @@
 
 #if IMAGE_BL31
 	/* ----------------------------------------------------------
-	 * Restore CPTR_EL3, ZCR_EL3 for SVE support.
-	 * If SVE is not supported - skip the restoration.
+	 * Restore CPTR_EL3.
 	 * ZCR is only restored if SVE is supported and enabled.
 	 * Synchronization is required before zcr_el3 is addressed.
 	 * ----------------------------------------------------------
 	 */
-	mrs	x17, id_aa64pfr0_el1
-	ubfx	x17, x17, ID_AA64PFR0_SVE_SHIFT, ID_AA64PFR0_SVE_LENGTH
-	cbz	x17, sve_not_enabled
-
 	ldp	x19, x20, [sp, #CTX_EL3STATE_OFFSET + CTX_CPTR_EL3]
 	msr	cptr_el3, x19
 
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 7a25151..7c6f953 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -25,6 +25,7 @@
 #include <lib/extensions/twed.h>
 #include <lib/utils.h>
 
+static void enable_extensions_secure(cpu_context_t *ctx);
 
 /*******************************************************************************
  * Context management library initialisation routine. This library is used by
@@ -178,19 +179,13 @@
 	 *  indicated by the interrupt routing model for BL31.
 	 */
 	scr_el3 |= get_scr_el3_from_routing_model(security_state);
-
-#if ENABLE_SVE_FOR_NS
-	if (security_state == NON_SECURE) {
-		sve_enable(ctx);
-	}
 #endif
-#if ENABLE_SVE_FOR_SWD
+
+	/* Save the initialized value of CPTR_EL3 register */
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, read_cptr_el3());
 	if (security_state == SECURE) {
-		sve_enable(ctx);
+		enable_extensions_secure(ctx);
 	}
-#endif
-
-#endif
 
 	/*
 	 * SCR_EL3.HCE: Enable HVC instructions if next execution state is
@@ -335,7 +330,7 @@
  * When EL2 is implemented but unused `el2_unused` is non-zero, otherwise
  * it is zero.
  ******************************************************************************/
-static void enable_extensions_nonsecure(bool el2_unused)
+static void enable_extensions_nonsecure(bool el2_unused, cpu_context_t *ctx)
 {
 #if IMAGE_BL31
 #if ENABLE_SPE_FOR_LOWER_ELS
@@ -343,7 +338,11 @@
 #endif
 
 #if ENABLE_AMU
-	amu_enable(el2_unused);
+	amu_enable(el2_unused, ctx);
+#endif
+
+#if ENABLE_SVE_FOR_NS
+	sve_enable(ctx);
 #endif
 
 #if ENABLE_MPAM_FOR_LOWER_ELS
@@ -353,6 +352,18 @@
 }
 
 /*******************************************************************************
+ * Enable architecture extensions on first entry to Secure world.
+ ******************************************************************************/
+static void enable_extensions_secure(cpu_context_t *ctx)
+{
+#if IMAGE_BL31
+#if ENABLE_SVE_FOR_SWD
+	sve_enable(ctx);
+#endif
+#endif
+}
+
+/*******************************************************************************
  * The following function initializes the cpu_context for a CPU specified by
  * its `cpu_idx` for first use, and sets the initial entrypoint state as
  * specified by the entry_point_info structure.
@@ -586,7 +597,7 @@
 			write_cnthp_ctl_el2(CNTHP_CTL_RESET_VAL &
 						~(CNTHP_CTL_ENABLE_BIT));
 		}
-		enable_extensions_nonsecure(el2_unused);
+		enable_extensions_nonsecure(el2_unused, ctx);
 	}
 
 	cm_el1_sysregs_context_restore(security_state);
diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
index 24c3737..295c0d5 100644
--- a/lib/extensions/amu/aarch64/amu.c
+++ b/lib/extensions/amu/aarch64/amu.c
@@ -46,7 +46,7 @@
  * Enable counters. This function is meant to be invoked
  * by the context management library before exiting from EL3.
  */
-void amu_enable(bool el2_unused)
+void amu_enable(bool el2_unused, cpu_context_t *ctx)
 {
 	uint64_t v;
 	unsigned int amu_version = amu_get_version();
@@ -88,12 +88,13 @@
 	}
 
 	/*
-	 * CPTR_EL3.TAM: Set to zero so that any accesses to
+	 * Retrieve and update the CPTR_EL3 value from the context mentioned
+	 * in 'ctx'. Set CPTR_EL3.TAM to zero so that any accesses to
 	 * the Activity Monitor registers do not trap to EL3.
 	 */
-	v = read_cptr_el3();
+	v = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
 	v &= ~TAM_BIT;
-	write_cptr_el3(v);
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, v);
 
 	/* Enable group 0 counters */
 	write_amcntenset0_el0(AMU_GROUP0_COUNTERS_MASK);
diff --git a/lib/extensions/sve/sve.c b/lib/extensions/sve/sve.c
index 7043cc2..2702c30 100644
--- a/lib/extensions/sve/sve.c
+++ b/lib/extensions/sve/sve.c
@@ -27,11 +27,13 @@
 
 void sve_enable(cpu_context_t *context)
 {
+	u_register_t cptr_el3;
+
 	if (!sve_supported()) {
 		return;
 	}
 
-	u_register_t cptr_el3 = read_cptr_el3();
+	cptr_el3 = read_ctx_reg(get_el3state_ctx(context), CTX_CPTR_EL3);
 
 	/* Enable access to SVE functionality for all ELs. */
 	cptr_el3 = (cptr_el3 | CPTR_EZ_BIT) & ~(TFP_BIT);
diff --git a/lib/zlib/tf_gunzip.c b/lib/zlib/tf_gunzip.c
index fd56dfc..3ac80bc 100644
--- a/lib/zlib/tf_gunzip.c
+++ b/lib/zlib/tf_gunzip.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <string.h>
 
 #include <common/debug.h>
+#include <common/tf_crc32.h>
 #include <lib/utils.h>
 #include <tf_gunzip.h>
 
@@ -100,3 +101,15 @@
 
 	return ret;
 }
+
+/* Wrapper function to calculate CRC
+ * @crc: previous accumulated CRC
+ * @buf: buffer base address
+ * @size: size of the buffer
+ *
+ * Return calculated CRC32 value
+ */
+uint32_t tf_crc32(uint32_t crc, const unsigned char *buf, size_t size)
+{
+	return (uint32_t)crc32((unsigned long)crc, buf, size);
+}
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index b2d1ee2..72f84b5 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -344,3 +344,14 @@
 
 # Build option to use the SP804 timer instead of the generic one
 USE_SP804_TIMER			:= 0
+
+# Build option to define number of firmware banks, used in firmware update
+# metadata structure.
+NR_OF_FW_BANKS			:= 2
+
+# Build option to define number of images in firmware bank, used in firmware
+# update metadata structure.
+NR_OF_IMAGES_IN_FW_BANK		:= 1
+
+# Disable Firmware update support by default
+PSA_FWU_SUPPORT			:= 0
diff --git a/plat/allwinner/common/include/platform_def.h b/plat/allwinner/common/include/platform_def.h
index 4893368..56a2ad6 100644
--- a/plat/allwinner/common/include/platform_def.h
+++ b/plat/allwinner/common/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,9 +13,6 @@
 
 #include <sunxi_mmap.h>
 
-/* The SCP firmware is allocated the last 16KiB of SRAM A2. */
-#define SUNXI_SCP_SIZE			0x4000
-
 #ifdef SUNXI_BL31_IN_DRAM
 
 #define BL31_BASE			SUNXI_DRAM_BASE
@@ -31,7 +28,6 @@
 #define BL31_BASE			(SUNXI_SRAM_A2_BASE + 0x4000)
 #define BL31_LIMIT			(SUNXI_SRAM_A2_BASE + \
 					 SUNXI_SRAM_A2_SIZE - SUNXI_SCP_SIZE)
-#define SUNXI_SCP_BASE			BL31_LIMIT
 
 /* Overwrite U-Boot SPL, but reserve the first page for the SPL header. */
 #define BL31_NOBITS_BASE		(SUNXI_SRAM_A1_BASE + 0x1000)
@@ -39,20 +35,23 @@
 
 #define MAX_XLAT_TABLES			1
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 28)
-#define SUNXI_BL33_VIRT_BASE		(SUNXI_DRAM_VIRT_BASE + SUNXI_DRAM_SEC_SIZE)
+
+#define SUNXI_BL33_VIRT_BASE		SUNXI_DRAM_VIRT_BASE
+
+/* The SCP firmware is allocated the last 16KiB of SRAM A2. */
+#define SUNXI_SCP_BASE			BL31_LIMIT
+#define SUNXI_SCP_SIZE			0x4000
 
 #endif /* SUNXI_BL31_IN_DRAM */
 
-/* How much memory to reserve as secure for BL32, if configured */
-#define SUNXI_DRAM_SEC_SIZE		(32U << 20)
-
 /* How much DRAM to map (to map BL33, for fetching the DTB from U-Boot) */
 #define SUNXI_DRAM_MAP_SIZE		(64U << 20)
 
 #define CACHE_WRITEBACK_SHIFT		6
 #define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
 
-#define MAX_MMAP_REGIONS		(3 + PLATFORM_MMAP_REGIONS)
+#define MAX_STATIC_MMAP_REGIONS		3
+#define MAX_MMAP_REGIONS		(5 + MAX_STATIC_MMAP_REGIONS)
 
 #define PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
 	(SUNXI_SRAM_A2_BASE + SUNXI_SRAM_A2_SIZE - 0x200)
@@ -72,7 +71,6 @@
 #define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER_COUNT * \
 					 PLATFORM_MAX_CPUS_PER_CLUSTER)
 #define PLATFORM_MAX_CPUS_PER_CLUSTER	U(4)
-#define PLATFORM_MMAP_REGIONS		5
 #define PLATFORM_STACK_SIZE		(0x1000 / PLATFORM_CORE_COUNT)
 
 #ifndef SPD_none
diff --git a/plat/allwinner/common/sunxi_common.c b/plat/allwinner/common/sunxi_common.c
index d47d360..82410b1 100644
--- a/plat/allwinner/common/sunxi_common.c
+++ b/plat/allwinner/common/sunxi_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,17 +14,11 @@
 #include <sunxi_mmap.h>
 #include <sunxi_private.h>
 
-static const mmap_region_t sunxi_mmap[PLATFORM_MMAP_REGIONS + 1] = {
+static const mmap_region_t sunxi_mmap[MAX_STATIC_MMAP_REGIONS + 1] = {
 	MAP_REGION_FLAT(SUNXI_SRAM_BASE, SUNXI_SRAM_SIZE,
-			MT_RW_DATA | MT_SECURE),
-#ifdef SUNXI_SCP_BASE
-	MAP_REGION_FLAT(SUNXI_SCP_BASE, SUNXI_SCP_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
-#endif
 	MAP_REGION_FLAT(SUNXI_DEV_BASE, SUNXI_DEV_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
-	MAP_REGION(SUNXI_DRAM_BASE, SUNXI_DRAM_VIRT_BASE, SUNXI_DRAM_SEC_SIZE,
-		   MT_RW_DATA | MT_SECURE),
 	MAP_REGION(PRELOADED_BL33_BASE, SUNXI_BL33_VIRT_BASE,
 		   SUNXI_DRAM_MAP_SIZE, MT_RW_DATA | MT_NS),
 	{},
@@ -40,12 +34,24 @@
 	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
 			BL_CODE_END - BL_CODE_BASE,
 			MT_CODE | MT_SECURE);
+	mmap_add_region(BL_CODE_END, BL_CODE_END,
+			BL_END - BL_CODE_END,
+			MT_RW_DATA | MT_SECURE);
+#if SEPARATE_CODE_AND_RODATA
 	mmap_add_region(BL_RO_DATA_BASE, BL_RO_DATA_BASE,
 			BL_RO_DATA_END - BL_RO_DATA_BASE,
 			MT_RO_DATA | MT_SECURE);
+#endif
+#if SEPARATE_NOBITS_REGION
+	mmap_add_region(BL_NOBITS_BASE, BL_NOBITS_BASE,
+			BL_NOBITS_END - BL_NOBITS_BASE,
+			MT_RW_DATA | MT_SECURE);
+#endif
+#if USE_COHERENT_MEM
 	mmap_add_region(BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_BASE,
 			BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
 			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER);
+#endif
 
 	mmap_add(sunxi_mmap);
 	init_xlat_tables();
diff --git a/plat/allwinner/common/sunxi_scpi_pm.c b/plat/allwinner/common/sunxi_scpi_pm.c
index 74763ef..eb37daa 100644
--- a/plat/allwinner/common/sunxi_scpi_pm.c
+++ b/plat/allwinner/common/sunxi_scpi_pm.c
@@ -212,7 +212,6 @@
 		uint32_t offset = SUNXI_SCP_BASE - vector;
 
 		mmio_write_32(vector, offset >> 2);
-		clean_dcache_range(vector, sizeof(uint32_t));
 	}
 
 	/* Take the SCP out of reset. */
diff --git a/plat/allwinner/sun50i_a64/sunxi_power.c b/plat/allwinner/sun50i_a64/sunxi_power.c
index 0fdb62d..a35b9dd 100644
--- a/plat/allwinner/sun50i_a64/sunxi_power.c
+++ b/plat/allwinner/sun50i_a64/sunxi_power.c
@@ -244,7 +244,6 @@
 	 * in instruction granularity (32 bits).
 	 */
 	mmio_write_32(arisc_reset_vec, ((uintptr_t)code - arisc_reset_vec) / 4);
-	clean_dcache_range(arisc_reset_vec, 4);
 
 	/* De-assert the arisc reset line to let it run. */
 	mmio_setbits_32(SUNXI_R_CPUCFG_BASE, BIT(0));
diff --git a/plat/arm/board/common/rotpk/arm_dev_rotpk.S b/plat/arm/board/common/rotpk/arm_dev_rotpk.S
index 80f2192..38f91fe 100644
--- a/plat/arm/board/common/rotpk/arm_dev_rotpk.S
+++ b/plat/arm/board/common/rotpk/arm_dev_rotpk.S
@@ -1,10 +1,17 @@
 /*
- * Copyright (c) 2020, ARM Limited. All rights reserved.
+ * Copyright (c) 2021, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+/* diphda platform provides custom values for the macros defined in
+ * arm_def.h , so only platform_def.h needs to be included
+ */
+#if !defined(TARGET_PLATFORM_FVP) && !defined(TARGET_PLATFORM_FPGA)
 #include "plat/arm/common/arm_def.h"
+#else
+#include <platform_def.h>
+#endif
 
 	.global arm_rotpk_header
 	.global arm_rotpk_header_end
diff --git a/plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c b/plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c
new file mode 100644
index 0000000..916c868
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/desc_image_load.h>
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Following descriptor provides BL image/ep information that gets used
+ * by BL2 to load the images and also subset of this information is
+ * passed to next BL image. The image loading sequence is managed by
+ * populating the images in required loading order. The image execution
+ * sequence is managed by populating the `next_handoff_image_id` with
+ * the next executable image id.
+ ******************************************************************************/
+static bl_mem_params_node_t bl2_mem_params_descs[] = {
+
+	/* Fill BL31 related information */
+	{
+		.image_id = BL31_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
+			.ep_info.args.arg3 = ARM_BL31_PLAT_PARAM_VAL,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+
+		.next_handoff_image_id = BL32_IMAGE_ID,
+	},
+
+	/* Fill BL32 related information */
+	{
+		.image_id = BL32_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
+			.ep_info.args.arg0 = DIPHDA_TOS_FW_CONFIG_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
+
+	/* Fill TOS_FW_CONFIG related information */
+	{
+		.image_id = TOS_FW_CONFIG_ID,
+		.image_info.image_base = DIPHDA_TOS_FW_CONFIG_BASE,
+		.image_info.image_max_size = DIPHDA_TOS_FW_CONFIG_LIMIT - \
+			DIPHDA_TOS_FW_CONFIG_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		VERSION_2, image_info_t, 0),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/* Fill BL33 related information */
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+		.ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
+		.image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
+			- PLAT_ARM_NS_IMAGE_BASE,
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+};
+
+REGISTER_BL_IMAGE_DESCS(bl2_mem_params_descs)
diff --git a/plat/arm/board/tc0/tc0_err.c b/plat/arm/board/diphda/common/diphda_err.c
similarity index 62%
copy from plat/arm/board/tc0/tc0_err.c
copy to plat/arm/board/diphda/common/diphda_err.c
index 83f2e9f..89a3b82 100644
--- a/plat/arm/board/tc0/tc0_err.c
+++ b/plat/arm/board/diphda/common/diphda_err.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,11 +7,11 @@
 #include <plat/arm/common/plat_arm.h>
 
 /*
- * tc0 error handler
+ * diphda error handler
  */
 void __dead2 plat_arm_error_handler(int err)
 {
-	while (true) {
+	while (1) {
 		wfi();
 	}
 }
diff --git a/plat/arm/board/diphda/common/diphda_helpers.S b/plat/arm/board/diphda/common/diphda_helpers.S
new file mode 100644
index 0000000..c9d2a88
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_helpers.S
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <platform_def.h>
+
+	.globl	plat_secondary_cold_boot_setup
+	.globl	plat_get_my_entrypoint
+	.globl	plat_is_my_cpu_primary
+	.globl	plat_arm_calc_core_pos
+
+	/* --------------------------------------------------------------------
+	 * void plat_secondary_cold_boot_setup (void);
+	 *
+	 * For AArch32, cold-booting secondary CPUs is not yet
+	 * implemented and they panic.
+	 * --------------------------------------------------------------------
+	 */
+func plat_secondary_cold_boot_setup
+cb_panic:
+	b	cb_panic
+endfunc plat_secondary_cold_boot_setup
+
+	/* ---------------------------------------------------------------------
+	 * unsigned long plat_get_my_entrypoint (void);
+	 *
+	 * Main job of this routine is to distinguish between a cold and warm
+	 * boot. On diphda, this information can be queried from the power
+	 * controller. The Power Control SYS Status Register (PSYSR) indicates
+	 * the wake-up reason for the CPU.
+	 *
+	 * For a cold boot, return 0.
+	 * For a warm boot, Not yet supported.
+	 *
+	 * TODO: PSYSR is a common register and should be
+	 * 	accessed using locks. Since it is not possible
+	 * 	to use locks immediately after a cold reset
+	 * 	we are relying on the fact that after a cold
+	 * 	reset all cpus will read the same WK field
+	 * ---------------------------------------------------------------------
+	 */
+func plat_get_my_entrypoint
+	/* TODO support warm boot */
+	/* Cold reset */
+	mov	x0, #0
+	ret
+endfunc plat_get_my_entrypoint
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_is_my_cpu_primary (void);
+	 *
+	 * Find out whether the current CPU is the primary
+	 * CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_is_my_cpu_primary
+	mrs	x0, mpidr_el1
+	mov_imm	x1, MPIDR_AFFINITY_MASK
+	and	x0, x0, x1
+	cmp	x0, #DIPHDA_PRIMARY_CPU
+	cset	w0, eq
+	ret
+endfunc plat_is_my_cpu_primary
diff --git a/plat/arm/board/diphda/common/diphda_plat.c b/plat/arm/board/diphda/common/diphda_plat.c
new file mode 100644
index 0000000..28d15a5
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_plat.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/bl_common.h>
+
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+/*
+ * Table of regions to map using the MMU.
+ * Replace or extend the below regions as required
+ */
+
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	ARM_MAP_NS_SHARED_RAM,
+	ARM_MAP_NS_DRAM1,
+	DIPHDA_MAP_DEVICE,
+	DIPHDA_EXTERNAL_FLASH,
+	{0}
+};
+
+/* diphda only has one always-on power domain and there
+ * is no power control present
+ */
+void __init plat_arm_pwrc_setup(void)
+{
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	/* Returning the Generic Timer Frequency */
+	return SYS_COUNTER_FREQ_IN_TICKS;
+}
+
+
+/*
+ * Helper function to initialize ARM interconnect driver.
+ */
+void plat_arm_interconnect_init(void)
+{
+}
+
+/*
+ * Helper function to place current master into coherency
+ */
+void plat_arm_interconnect_enter_coherency(void)
+{
+}
+
+/*
+ * Helper function to remove current master from coherency
+ */
+void plat_arm_interconnect_exit_coherency(void)
+{
+}
+
+/*
+ * This function is invoked during Mbed TLS library initialisation to get a heap
+ * The function simply returns the default allocated heap.
+ */
+
+#if TRUSTED_BOARD_BOOT
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	assert(heap_addr != NULL);
+	assert(heap_size != NULL);
+
+	return arm_get_mbedtls_heap(heap_addr, heap_size);
+}
+#endif
diff --git a/plat/arm/board/diphda/common/diphda_pm.c b/plat/arm/board/diphda/common/diphda_pm.c
new file mode 100644
index 0000000..12b322e
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_pm.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/psci/psci.h>
+#include <plat/arm/common/plat_arm.h>
+
+/*******************************************************************************
+ * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
+ * platform layer will take care of registering the handlers with PSCI.
+ ******************************************************************************/
+plat_psci_ops_t plat_arm_psci_pm_ops = {
+	/* dummy struct */
+	.validate_ns_entrypoint = NULL
+};
+
+const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
+{
+	return ops;
+}
diff --git a/plat/arm/board/diphda/common/diphda_security.c b/plat/arm/board/diphda/common/diphda_security.c
new file mode 100644
index 0000000..bf172af
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_security.c
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * We assume that all security programming is done by the primary core.
+ */
+void plat_arm_security_setup(void)
+{
+	/*
+	 * If the platform had additional peripheral specific security
+	 * configurations, those would be configured here.
+	 */
+}
diff --git a/plat/arm/board/diphda/common/diphda_stack_protector.c b/plat/arm/board/diphda/common/diphda_stack_protector.c
new file mode 100644
index 0000000..6228b63
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_stack_protector.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <plat/common/platform.h>
+
+static uint32_t plat_generate_random_number(void)
+{
+	uintptr_t return_addr = (uintptr_t)__builtin_return_address(0U);
+	uintptr_t frame_addr = (uintptr_t)__builtin_frame_address(0U);
+	uint64_t cntpct = read_cntpct_el0();
+
+	/* Generate 32-bit pattern: saving the 2 least significant bytes
+	 * in random_lo and random_hi
+	 */
+	uint16_t random_lo = (uint16_t)(
+			(((uint64_t)return_addr) << 13) ^ frame_addr ^ cntpct
+			);
+
+	uint16_t random_hi = (uint16_t)(
+			(((uint64_t)frame_addr) << 15) ^ return_addr ^ cntpct
+			);
+
+	return (((uint32_t)random_hi) << 16) | random_lo;
+}
+
+u_register_t plat_get_stack_protector_canary(void)
+{
+	return  plat_generate_random_number(); /* a 32-bit pattern returned */
+}
diff --git a/plat/arm/board/diphda/common/diphda_topology.c b/plat/arm/board/diphda/common/diphda_topology.c
new file mode 100644
index 0000000..9dfd05d
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_topology.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+
+/* The diphda power domain tree descriptor */
+static unsigned char diphda_power_domain_tree_desc[PLAT_ARM_CLUSTER_COUNT
+							+ 2];
+/*******************************************************************************
+ * This function dynamically constructs the topology according to
+ * CLUSTER_COUNT and returns it.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	int i;
+
+	/*
+	 * The highest level is the system level. The next level is constituted
+	 * by clusters and then cores in clusters.
+	 */
+	diphda_power_domain_tree_desc[0] = 1;
+	diphda_power_domain_tree_desc[1] = PLAT_ARM_CLUSTER_COUNT;
+
+	for (i = 0; i < PLAT_ARM_CLUSTER_COUNT; i++)
+		diphda_power_domain_tree_desc[i + 2] = PLATFORM_CORE_COUNT;
+
+	return diphda_power_domain_tree_desc;
+}
+
+/******************************************************************************
+ * This function implements a part of the critical interface between the PSCI
+ * generic layer and the platform that allows the former to query the platform
+ * to convert an MPIDR to a unique linear index. An error code (-1) is
+ * returned in case the MPIDR is invalid.
+ *****************************************************************************/
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	return plat_arm_calc_core_pos(mpidr);
+}
diff --git a/plat/arm/board/diphda/common/diphda_trusted_boot.c b/plat/arm/board/diphda/common/diphda_trusted_boot.c
new file mode 100644
index 0000000..ddb41fa
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_trusted_boot.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * Return the ROTPK hash in the following ASN.1 structure in DER format:
+ *
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm         OBJECT IDENTIFIER,
+ *     parameters        ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm   AlgorithmIdentifier,
+ *     digest            OCTET STRING
+ * }
+ *
+ * The function returns 0 on success. Any other value is treated as error by the
+ * Trusted Board Boot. The function also reports extra information related
+ * to the ROTPK in the flags parameter: ROTPK_IS_HASH, ROTPK_NOT_DEPLOYED.
+ *
+ * Refer to the TF-A porting-guide document for more details.
+ */
+int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+			unsigned int *flags)
+{
+	return arm_get_rotpk_info(cookie, key_ptr, key_len, flags);
+}
+
+/*
+ * STUB overriding the non-volatile counter reading.
+ * NV counters are not implemented at this stage of development.
+ * Return: 0 = success
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+    *nv_ctr = DIPHDA_FW_NVCTR_VAL;
+    return 0;
+}
+
+/*
+ * STUB overriding the non-volatile counter updating.
+ * NV counters are not implemented at this stage of development.
+ * Return: 0 = success
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+    return 0;
+}
diff --git a/plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts b/plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts
new file mode 100644
index 0000000..536bdc3
--- /dev/null
+++ b/plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+/dts-v1/;
+
+/ {
+	compatible = "arm,ffa-core-manifest-1.0";
+	#address-cells = <2>;
+	#size-cells = <1>;
+
+	/*
+	 * BL32 image details needed by SPMC
+	 *
+	 * Note:
+	 * binary_size: size of BL32 + TOS_FW_CONFIG
+	 */
+
+	attribute {
+		spmc_id = <0x8000>;
+		maj_ver = <0x1>;
+		min_ver = <0x1>;
+		exec_state = <0x0>;
+		load_address = <0x0 0x2002000>;
+		entrypoint = <0x0 0x2002000>;
+		binary_size = <0xae000>;
+	};
+
+};
diff --git a/plat/arm/board/diphda/common/include/platform_def.h b/plat/arm/board/diphda/common/include/platform_def.h
new file mode 100644
index 0000000..37fd71b
--- /dev/null
+++ b/plat/arm/board/diphda/common/include/platform_def.h
@@ -0,0 +1,416 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
+
+#include <common/tbbr/tbbr_img_def.h>
+#include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+#include <plat/arm/board/common/v2m_def.h>
+#include <plat/arm/common/arm_spm_def.h>
+#include <plat/arm/common/smccc_def.h>
+#include <plat/common/common_def.h>
+#include <plat/arm/soc/common/soc_css_def.h>
+
+#define ARM_ROTPK_HEADER_LEN			19
+#define ARM_ROTPK_HASH_LEN			32
+
+/* Special value used to verify platform parameters from BL2 to BL31 */
+#define ARM_BL31_PLAT_PARAM_VAL		ULL(0x0f1e2d3c4b5a6978)
+
+/* PL011 UART related constants */
+#ifdef V2M_IOFPGA_UART0_CLK_IN_HZ
+#undef V2M_IOFPGA_UART0_CLK_IN_HZ
+#endif
+
+#ifdef V2M_IOFPGA_UART1_CLK_IN_HZ
+#undef V2M_IOFPGA_UART1_CLK_IN_HZ
+#endif
+
+#define V2M_IOFPGA_UART0_CLK_IN_HZ		50000000
+#define V2M_IOFPGA_UART1_CLK_IN_HZ		50000000
+
+/* Core/Cluster/Thread counts for diphda */
+#define DIPHDA_CLUSTER_COUNT			U(1)
+#define DIPHDA_MAX_CPUS_PER_CLUSTER		U(4)
+#define DIPHDA_MAX_PE_PER_CPU			U(1)
+#define DIPHDA_PRIMARY_CPU			U(0)
+
+#define PLAT_ARM_CLUSTER_COUNT		DIPHDA_CLUSTER_COUNT
+
+#define PLATFORM_CORE_COUNT			(PLAT_ARM_CLUSTER_COUNT *      \
+						DIPHDA_MAX_CPUS_PER_CLUSTER *  \
+						DIPHDA_MAX_PE_PER_CPU)
+
+/* UART related constants */
+#define PLAT_ARM_BOOT_UART_BASE		0x1a510000
+#define PLAT_ARM_BOOT_UART_CLK_IN_HZ		V2M_IOFPGA_UART0_CLK_IN_HZ
+#define PLAT_ARM_RUN_UART_BASE		0x1a520000
+#define PLAT_ARM_RUN_UART_CLK_IN_HZ		V2M_IOFPGA_UART1_CLK_IN_HZ
+#define ARM_CONSOLE_BAUDRATE			115200
+#define PLAT_ARM_CRASH_UART_BASE		PLAT_ARM_RUN_UART_BASE
+#define PLAT_ARM_CRASH_UART_CLK_IN_HZ		PLAT_ARM_RUN_UART_CLK_IN_HZ
+
+/* Memory related constants */
+
+/* SRAM (CVM) memory layout
+ *
+ * <ARM_TRUSTED_SRAM_BASE>
+ *
+ *         partition size: sizeof(meminfo_t) = 16 bytes
+ *
+ *         content: memory info area used by the next BL
+ *
+ * <ARM_FW_CONFIG_BASE>
+ *
+ *         partition size: 4080 bytes
+ *
+ * <ARM_BL2_MEM_DESC_BASE>
+ *
+ *         partition size: 4 KB
+ *
+ *         content:
+ *
+ *             Area where BL2 copies the images descriptors
+ *
+ * <ARM_BL_RAM_BASE> = <BL32_BASE>
+ *
+ *         partition size: 688 KB
+ *
+ *         content:
+ *
+ *             BL32 (optee-os)
+ *
+ * <DIPHDA_TOS_FW_CONFIG_BASE> = 0x20ae000
+ *
+ *         partition size: 8 KB
+ *
+ *         content:
+ *
+ *             BL32 config (TOS_FW_CONFIG)
+ *
+ * <BL31_BASE>
+ *
+ *         partition size: 140 KB
+ *
+ *         content:
+ *
+ *             BL31
+ *
+ * <BL2_SIGNATURE_BASE>
+ *
+ *     partition size: 4 KB
+ *
+ *     content:
+ *
+ *         MCUBOOT data needed to verify TF-A BL2
+ *
+ * <BL2_BASE>
+ *
+ *     partition size: 176 KB
+ *
+ *         content:
+ *
+ *             BL2
+ *
+ * <ARM_NS_SHARED_RAM_BASE> = <ARM_TRUSTED_SRAM_BASE> + 1 MB
+ *
+ *         partition size: 3 MB
+ *
+ *         content:
+ *
+ *             BL33 (u-boot)
+ */
+
+/* DDR memory */
+#define ARM_DRAM1_BASE			UL(0x80000000)
+#define ARM_DRAM1_SIZE			UL(0x80000000)
+#define ARM_DRAM1_END				(ARM_DRAM1_BASE +	\
+						ARM_DRAM1_SIZE - 1)
+
+/* DRAM1 and DRAM2 are the same for diphda */
+#define ARM_DRAM2_BASE			ARM_DRAM1_BASE
+#define ARM_DRAM2_SIZE			ARM_DRAM1_SIZE
+#define ARM_DRAM2_END				ARM_DRAM1_END
+
+#define ARM_NS_DRAM1_BASE			ARM_DRAM1_BASE
+#define ARM_NS_DRAM1_SIZE			ARM_DRAM1_SIZE
+#define ARM_NS_DRAM1_END			(ARM_NS_DRAM1_BASE +	\
+						ARM_NS_DRAM1_SIZE - 1)
+
+/* The first 8 KB of Trusted SRAM are used as shared memory */
+#define ARM_TRUSTED_SRAM_BASE			UL(0x02000000)
+#define ARM_SHARED_RAM_SIZE			UL(0x00002000)  /* 8 KB */
+#define ARM_SHARED_RAM_BASE			ARM_TRUSTED_SRAM_BASE
+
+/* The remaining Trusted SRAM is used to load the BL images */
+
+#define PLAT_ARM_TRUSTED_SRAM_SIZE		UL(0x00100000)  /* 1 MB */
+
+#define PLAT_ARM_MAX_BL2_SIZE			UL(0x0002d000)  /* 180 KB */
+
+#define PLAT_ARM_MAX_BL31_SIZE		UL(0x00023000)  /* 140 KB */
+
+#define ARM_BL_RAM_BASE			(ARM_SHARED_RAM_BASE +	\
+						ARM_SHARED_RAM_SIZE)
+#define ARM_BL_RAM_SIZE			(PLAT_ARM_TRUSTED_SRAM_SIZE -	\
+						ARM_SHARED_RAM_SIZE)
+
+#define BL2_SIGNATURE_SIZE			UL(0x00001000)  /* 4 KB */
+#define BL2_SIGNATURE_BASE			(BL2_LIMIT - \
+						PLAT_ARM_MAX_BL2_SIZE)
+#define BL2_BASE				(BL2_LIMIT - \
+						PLAT_ARM_MAX_BL2_SIZE + \
+						BL2_SIGNATURE_SIZE)
+#define BL2_LIMIT				(ARM_BL_RAM_BASE + \
+						ARM_BL_RAM_SIZE)
+
+#define BL31_BASE				(BL2_SIGNATURE_BASE - \
+						PLAT_ARM_MAX_BL31_SIZE)
+#define BL31_LIMIT				BL2_SIGNATURE_BASE
+
+#define DIPHDA_TOS_FW_CONFIG_BASE		(BL31_BASE - \
+						DIPHDA_TOS_FW_CONFIG_SIZE)
+#define DIPHDA_TOS_FW_CONFIG_SIZE		UL(0x00002000)  /* 8 KB */
+#define DIPHDA_TOS_FW_CONFIG_LIMIT		BL31_BASE
+
+#define BL32_BASE				ARM_BL_RAM_BASE
+#define PLAT_ARM_MAX_BL32_SIZE		(DIPHDA_TOS_FW_CONFIG_BASE - \
+						BL32_BASE)     /* 688 KB */
+#define BL32_LIMIT				(BL32_BASE + \
+						PLAT_ARM_MAX_BL32_SIZE)
+
+/* SPD_spmd settings */
+
+#define PLAT_ARM_SPMC_BASE			BL32_BASE
+#define PLAT_ARM_SPMC_SIZE			PLAT_ARM_MAX_BL32_SIZE
+
+/* NS memory */
+
+/* The last 3 MB of the SRAM is allocated to the non secure area */
+#define ARM_NS_SHARED_RAM_BASE		(ARM_TRUSTED_SRAM_BASE + \
+						PLAT_ARM_TRUSTED_SRAM_SIZE)
+#define ARM_NS_SHARED_RAM_SIZE		UL(0x00300000)  /* 3 MB */
+
+/* end of the definition of SRAM memory layout */
+
+/* NOR Flash */
+
+#define PLAT_ARM_FIP_BASE			UL(0x08131000)
+#define PLAT_ARM_FIP_MAX_SIZE			UL(0x1ff000)  /* 1.996 MB */
+
+#define PLAT_ARM_NVM_BASE			V2M_FLASH0_BASE
+#define PLAT_ARM_NVM_SIZE			UL(0x02000000)  /* 32 MB */
+
+#define PLAT_ARM_FLASH_IMAGE_BASE		PLAT_ARM_FIP_BASE
+#define PLAT_ARM_FLASH_IMAGE_MAX_SIZE		PLAT_ARM_FIP_MAX_SIZE
+
+/*
+ * Some data must be aligned on the biggest cache line size in the platform.
+ * This is known only to the platform as it might have a combination of
+ * integrated and external caches.
+ */
+#define CACHE_WRITEBACK_GRANULE		(U(1) << ARM_CACHE_WRITEBACK_SHIFT)
+#define ARM_CACHE_WRITEBACK_SHIFT		6
+
+/*
+ * Define FW_CONFIG area base and limit. Leave enough space for BL2 meminfo.
+ * FW_CONFIG is intended to host the device tree. Currently, This area is not
+ * used because diphda platform doesn't use a device tree at TF-A level.
+ */
+#define ARM_FW_CONFIG_BASE			(ARM_SHARED_RAM_BASE \
+						+ sizeof(meminfo_t))
+#define ARM_FW_CONFIG_LIMIT			(ARM_SHARED_RAM_BASE \
+						+ (ARM_SHARED_RAM_SIZE >> 1))
+
+/*
+ * Boot parameters passed from BL2 to BL31/BL32 are stored here
+ */
+#define ARM_BL2_MEM_DESC_BASE			ARM_FW_CONFIG_LIMIT
+#define ARM_BL2_MEM_DESC_LIMIT		ARM_BL_RAM_BASE
+
+/*
+ * The max number of regions like RO(code), coherent and data required by
+ * different BL stages which need to be mapped in the MMU.
+ */
+#define ARM_BL_REGIONS			3
+#define PLAT_ARM_MMAP_ENTRIES			8
+#define MAX_XLAT_TABLES			5
+#define MAX_MMAP_REGIONS			(PLAT_ARM_MMAP_ENTRIES + \
+						ARM_BL_REGIONS)
+#define MAX_IO_DEVICES			2
+#define MAX_IO_HANDLES			3
+#define MAX_IO_BLOCK_DEVICES			1
+
+/* GIC related constants */
+#define PLAT_ARM_GICD_BASE			0x1C010000
+#define PLAT_ARM_GICC_BASE			0x1C02F000
+
+/* MHUv2 Secure Channel receiver and sender */
+#define PLAT_SDK700_MHU0_SEND			0x1B800000
+#define PLAT_SDK700_MHU0_RECV			0x1B810000
+
+/* Timer/watchdog related constants */
+#define ARM_SYS_CNTCTL_BASE			UL(0x1a200000)
+#define ARM_SYS_CNTREAD_BASE			UL(0x1a210000)
+#define ARM_SYS_TIMCTL_BASE			UL(0x1a220000)
+
+#define SYS_COUNTER_FREQ_IN_TICKS	UL(50000000) /* 50MHz */
+
+#define DIPHDA_IRQ_TZ_WDOG			32
+#define DIPHDA_IRQ_SEC_SYS_TIMER		34
+
+#define PLAT_MAX_PWR_LVL			2
+/*
+ * Macros mapping the MPIDR Affinity levels to ARM Platform Power levels. The
+ * power levels have a 1:1 mapping with the MPIDR affinity levels.
+ */
+#define ARM_PWR_LVL0				MPIDR_AFFLVL0
+#define ARM_PWR_LVL1				MPIDR_AFFLVL1
+#define ARM_PWR_LVL2				MPIDR_AFFLVL2
+
+/*
+ *  Macros for local power states in ARM platforms encoded by State-ID field
+ *  within the power-state parameter.
+ */
+/* Local power state for power domains in Run state. */
+#define ARM_LOCAL_STATE_RUN			U(0)
+/* Local power state for retention. Valid only for CPU power domains */
+#define ARM_LOCAL_STATE_RET			U(1)
+/* Local power state for OFF/power-down. Valid for CPU and cluster
+ * power domains
+ */
+#define ARM_LOCAL_STATE_OFF			U(2)
+
+#define PLAT_ARM_TRUSTED_MAILBOX_BASE		ARM_TRUSTED_SRAM_BASE
+#define PLAT_ARM_NSTIMER_FRAME_ID		U(1)
+
+#define PLAT_ARM_NS_IMAGE_BASE		(ARM_NS_SHARED_RAM_BASE)
+
+#define PLAT_PHY_ADDR_SPACE_SIZE		(1ULL << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE		(1ULL << 32)
+
+/*
+ * This macro defines the deepest retention state possible. A higher state
+ * ID will represent an invalid or a power down state.
+ */
+#define PLAT_MAX_RET_STATE			1
+
+/*
+ * This macro defines the deepest power down states possible. Any state ID
+ * higher than this is invalid.
+ */
+#define PLAT_MAX_OFF_STATE			2
+
+#define PLATFORM_STACK_SIZE			UL(0x440)
+
+#define DIPHDA_EXTERNAL_FLASH			MAP_REGION_FLAT(	\
+						PLAT_ARM_NVM_BASE,	\
+						PLAT_ARM_NVM_SIZE,	\
+						MT_DEVICE | MT_RO | MT_SECURE)
+
+#define ARM_MAP_SHARED_RAM			MAP_REGION_FLAT(	\
+						ARM_SHARED_RAM_BASE,	\
+						ARM_SHARED_RAM_SIZE,	\
+						MT_MEMORY | MT_RW | MT_SECURE)
+
+#define ARM_MAP_NS_SHARED_RAM			MAP_REGION_FLAT(	\
+						ARM_NS_SHARED_RAM_BASE, \
+						ARM_NS_SHARED_RAM_SIZE, \
+						MT_MEMORY | MT_RW | MT_NS)
+
+#define ARM_MAP_NS_DRAM1			MAP_REGION_FLAT(	\
+						ARM_NS_DRAM1_BASE,	\
+						ARM_NS_DRAM1_SIZE,	\
+						MT_MEMORY | MT_RW | MT_NS)
+
+#define ARM_MAP_BL_RO				MAP_REGION_FLAT(	\
+						BL_CODE_BASE,		\
+						BL_CODE_END		\
+							- BL_CODE_BASE, \
+						MT_CODE | MT_SECURE),	\
+						MAP_REGION_FLAT(	\
+						BL_RO_DATA_BASE,	\
+						BL_RO_DATA_END	\
+						- BL_RO_DATA_BASE,	\
+						MT_RO_DATA | MT_SECURE)
+#if USE_COHERENT_MEM
+#define ARM_MAP_BL_COHERENT_RAM		MAP_REGION_FLAT(	\
+						BL_COHERENT_RAM_BASE,	\
+						BL_COHERENT_RAM_END	\
+						- BL_COHERENT_RAM_BASE, \
+						MT_DEVICE | MT_RW | MT_SECURE)
+#endif
+
+/*
+ * Map the region for the optional device tree configuration with read and
+ * write permissions
+ */
+#define ARM_MAP_BL_CONFIG_REGION		MAP_REGION_FLAT(	\
+						ARM_FW_CONFIG_BASE,	\
+						(ARM_FW_CONFIG_LIMIT-   \
+						ARM_FW_CONFIG_BASE),   \
+						MT_MEMORY | MT_RW | MT_SECURE)
+
+#define DIPHDA_DEVICE_BASE			(0x1A000000)
+#define DIPHDA_DEVICE_SIZE			(0x26000000)
+#define DIPHDA_MAP_DEVICE			MAP_REGION_FLAT(	\
+						DIPHDA_DEVICE_BASE,	\
+						DIPHDA_DEVICE_SIZE,	\
+						MT_DEVICE | MT_RW | MT_SECURE)
+
+#define ARM_IRQ_SEC_PHY_TIMER			29
+
+#define ARM_IRQ_SEC_SGI_0			8
+#define ARM_IRQ_SEC_SGI_1			9
+#define ARM_IRQ_SEC_SGI_2			10
+#define ARM_IRQ_SEC_SGI_3			11
+#define ARM_IRQ_SEC_SGI_4			12
+#define ARM_IRQ_SEC_SGI_5			13
+#define ARM_IRQ_SEC_SGI_6			14
+#define ARM_IRQ_SEC_SGI_7			15
+
+/*
+ * Define a list of Group 1 Secure and Group 0 interrupt properties as per GICv3
+ * terminology. On a GICv2 system or mode, the lists will be merged and treated
+ * as Group 0 interrupts.
+ */
+#define ARM_G1S_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(ARM_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, \
+		(grp), GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_1, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_2, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_3, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_4, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_5, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE)
+
+#define ARM_G0_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_6, GIC_HIGHEST_SEC_PRIORITY, (grp), \
+		GIC_INTR_CFG_EDGE)
+
+/*
+ * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
+ * terminology. On a GICv2 system or mode, the lists will be merged and treated
+ * as Group 0 interrupts.
+ */
+#define PLAT_ARM_G1S_IRQ_PROPS(grp)	\
+	ARM_G1S_IRQ_PROPS(grp), \
+	INTR_PROP_DESC(DIPHDA_IRQ_TZ_WDOG, GIC_HIGHEST_SEC_PRIORITY, \
+		(grp), GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(DIPHDA_IRQ_SEC_SYS_TIMER, \
+		GIC_HIGHEST_SEC_PRIORITY, (grp), GIC_INTR_CFG_LEVEL)
+
+#define PLAT_ARM_G0_IRQ_PROPS(grp)	ARM_G0_IRQ_PROPS(grp)
+
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/diphda/include/plat_macros.S b/plat/arm/board/diphda/include/plat_macros.S
new file mode 100644
index 0000000..4de8f95
--- /dev/null
+++ b/plat/arm/board/diphda/include/plat_macros.S
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef PLAT_MACROS_S
+#define PLAT_MACROS_S
+
+#include <css_macros.S>
+
+/* ---------------------------------------------
+ * The below required platform porting macro
+ * prints out relevant platform registers
+ * whenever an unhandled exception is taken in
+ * BL31.
+ * ---------------------------------------------
+ */
+	.macro plat_crash_print_regs
+	css_print_gic_regs
+	.endm
+
+#endif /* PLAT_MACROS_S */
diff --git a/plat/arm/board/diphda/platform.mk b/plat/arm/board/diphda/platform.mk
new file mode 100644
index 0000000..5ff0862
--- /dev/null
+++ b/plat/arm/board/diphda/platform.mk
@@ -0,0 +1,76 @@
+#
+# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Making sure the diphda platform type is specified
+ifeq ($(filter ${TARGET_PLATFORM}, fpga fvp),)
+	$(error TARGET_PLATFORM must be fpga or fvp)
+endif
+
+DIPHDA_CPU_LIBS	+=lib/cpus/aarch64/cortex_a35.S
+
+PLAT_INCLUDES		:=	-Iplat/arm/board/diphda/common/include	\
+				-Iplat/arm/board/diphda/include		\
+				-Iinclude/plat/arm/common			\
+				-Iinclude/plat/arm/css/common/aarch64
+
+
+DIPHDA_FW_NVCTR_VAL	:=	255
+TFW_NVCTR_VAL		:=	${DIPHDA_FW_NVCTR_VAL}
+NTFW_NVCTR_VAL		:=	${DIPHDA_FW_NVCTR_VAL}
+
+override NEED_BL1	:=	no
+
+override NEED_BL2	:=	yes
+FIP_BL2_ARGS := tb-fw
+
+override NEED_BL2U	:=	no
+override NEED_BL31	:=	yes
+NEED_BL32		:=	yes
+override NEED_BL33	:=	yes
+
+# Include GICv2 driver files
+include drivers/arm/gic/v2/gicv2.mk
+
+DIPHDA_GIC_SOURCES	:=	${GICV2_SOURCES}			\
+				plat/common/plat_gicv2.c		\
+				plat/arm/common/arm_gicv2.c
+
+
+BL2_SOURCES		+=	plat/arm/board/diphda/common/diphda_security.c		\
+				plat/arm/board/diphda/common/diphda_err.c		\
+				plat/arm/board/diphda/common/diphda_trusted_boot.c	\
+				lib/utils/mem_region.c					\
+				plat/arm/board/diphda/common/diphda_helpers.S		\
+				plat/arm/board/diphda/common/diphda_plat.c		\
+				plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c \
+				${DIPHDA_CPU_LIBS}					\
+
+
+BL31_SOURCES	+=	drivers/cfi/v2m/v2m_flash.c				\
+			lib/utils/mem_region.c					\
+			plat/arm/board/diphda/common/diphda_helpers.S		\
+			plat/arm/board/diphda/common/diphda_topology.c		\
+			plat/arm/board/diphda/common/diphda_security.c		\
+			plat/arm/board/diphda/common/diphda_plat.c		\
+			plat/arm/board/diphda/common/diphda_pm.c		\
+			${DIPHDA_CPU_LIBS}					\
+			${DIPHDA_GIC_SOURCES}
+
+
+FDT_SOURCES		+=	plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts
+DIPHDA_TOS_FW_CONFIG	:=	${BUILD_PLAT}/fdts/diphda_spmc_manifest.dtb
+
+# Add the SPMC manifest to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${DIPHDA_TOS_FW_CONFIG},--tos-fw-config,${DIPHDA_TOS_FW_CONFIG}))
+
+# Adding TARGET_PLATFORM as a GCC define (-D option)
+$(eval $(call add_define,TARGET_PLATFORM_$(call uppercase,${TARGET_PLATFORM})))
+
+# Adding DIPHDA_FW_NVCTR_VAL as a GCC define (-D option)
+$(eval $(call add_define,DIPHDA_FW_NVCTR_VAL))
+
+include plat/arm/common/arm_common.mk
+include plat/arm/board/common/board_common.mk
diff --git a/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts b/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
index 67e5504..21a6073 100644
--- a/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
+++ b/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
@@ -20,7 +20,7 @@
 	attribute {
 		spmc_id = <0x8000>;
 		maj_ver = <0x1>;
-		min_ver = <0x0>;
+		min_ver = <0x1>;
 		exec_state = <0x0>;
 		load_address = <0x0 0x6000000>;
 		entrypoint = <0x0 0x6000000>;
diff --git a/plat/arm/board/fvp/fdts/fvp_spmc_optee_sp_manifest.dts b/plat/arm/board/fvp/fdts/fvp_spmc_optee_sp_manifest.dts
index 088179b..041dade 100644
--- a/plat/arm/board/fvp/fdts/fvp_spmc_optee_sp_manifest.dts
+++ b/plat/arm/board/fvp/fdts/fvp_spmc_optee_sp_manifest.dts
@@ -20,7 +20,7 @@
 	attribute {
 		spmc_id = <0x8000>;
 		maj_ver = <0x1>;
-		min_ver = <0x0>;
+		min_ver = <0x1>;
 		exec_state = <0x0>;
 		load_address = <0x0 0x6000000>;
 		entrypoint = <0x0 0x6000000>;
diff --git a/plat/arm/board/fvp/fdts/optee_sp_manifest.dts b/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
index 928d0d3..07235b0 100644
--- a/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
+++ b/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
@@ -25,7 +25,7 @@
 	entrypoint-offset = <0x1000>;
 	xlat-granule = <0>; /* 4KiB */
 	boot-order = <0>;
-	messaging-method = <0>; /* Direct messaging only */
+	messaging-method = <3>; /* Direct messaging only */
 	run-time-model = <1>; /* Run to completion */
 
 	/* Boot protocol */
diff --git a/plat/arm/board/fvp/fvp_bl1_setup.c b/plat/arm/board/fvp/fvp_bl1_setup.c
index e713bbc..06ee037 100644
--- a/plat/arm/board/fvp/fvp_bl1_setup.c
+++ b/plat/arm/board/fvp/fvp_bl1_setup.c
@@ -1,15 +1,17 @@
 /*
- * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <assert.h>
+#include <errno.h>
 
 #include <bl1/bl1.h>
 #include <common/tbbr/tbbr_img_def.h>
 #include <drivers/arm/smmu_v3.h>
 #include <drivers/arm/sp805.h>
+#include <lib/mmio.h>
 #include <plat/arm/common/arm_config.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/arm/common/arm_def.h>
@@ -61,6 +63,12 @@
 
 __dead2 void bl1_plat_fwu_done(void *client_cookie, void *reserved)
 {
+	uint32_t nv_flags = mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
+
+	/* Clear the NV flags register. */
+	mmio_write_32((V2M_SYSREGS_BASE + V2M_SYS_NVFLAGSCLR),
+		      nv_flags);
+
 	/* Setup the watchdog to reset the system as soon as possible */
 	sp805_refresh(ARM_SP805_TWDG_BASE, 1U);
 
@@ -124,3 +132,15 @@
 	return 0;
 }
 #endif /* MEASURED_BOOT */
+
+/*******************************************************************************
+ * The following function checks if Firmware update is needed by checking error
+ * reported in NV flag.
+ ******************************************************************************/
+bool plat_arm_bl1_fwu_needed(void)
+{
+	int32_t nv_flags = (int32_t)mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
+
+	/* if image load/authentication failed */
+	return ((nv_flags == -EAUTH) || (nv_flags == -ENOENT));
+}
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index fe0903b..9d3c031 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -72,14 +72,11 @@
  * Table of memory regions for various BL stages to map using the MMU.
  * This doesn't include Trusted SRAM as setup_page_tables() already takes care
  * of mapping it.
- *
- * The flash needs to be mapped as writable in order to erase the FIP's Table of
- * Contents in case of unrecoverable error (see plat_error_handler()).
  */
 #ifdef IMAGE_BL1
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
-	V2M_MAP_FLASH0_RW,
+	V2M_MAP_FLASH0_RO,
 	V2M_MAP_IOFPGA,
 	MAP_DEVICE0,
 #if FVP_INTERCONNECT_DRIVER == FVP_CCN
diff --git a/plat/arm/board/fvp/fvp_err.c b/plat/arm/board/fvp/fvp_err.c
index 2928b3a..1f9f0dd 100644
--- a/plat/arm/board/fvp/fvp_err.c
+++ b/plat/arm/board/fvp/fvp_err.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <common/debug.h>
 #include <drivers/arm/sp805.h>
 #include <drivers/cfi/v2m_flash.h>
+#include <lib/mmio.h>
 #include <plat/arm/common/plat_arm.h>
 #include <platform_def.h>
 
@@ -17,25 +18,8 @@
  */
 __dead2 void plat_arm_error_handler(int err)
 {
-	int ret;
-
-	switch (err) {
-	case -ENOENT:
-	case -EAUTH:
-		/* Image load or authentication error. Erase the ToC */
-		INFO("Erasing FIP ToC from flash...\n");
-		(void)nor_unlock(PLAT_ARM_FLASH_IMAGE_BASE);
-		ret = nor_word_program(PLAT_ARM_FLASH_IMAGE_BASE, 0);
-		if (ret != 0) {
-			ERROR("Cannot erase ToC\n");
-		} else {
-			INFO("Done\n");
-		}
-		break;
-	default:
-		/* Unexpected error */
-		break;
-	}
+	/* Propagate the err code in the NV-flags register */
+	mmio_write_32(V2M_SYS_NVFLAGS_ADDR, (uint32_t)err);
 
 	console_flush();
 
diff --git a/plat/arm/board/fvp/fvp_io_storage.c b/plat/arm/board/fvp/fvp_io_storage.c
index 109d321..4eef51c 100644
--- a/plat/arm/board/fvp/fvp_io_storage.c
+++ b/plat/arm/board/fvp/fvp_io_storage.c
@@ -20,6 +20,10 @@
 #define BL32_IMAGE_NAME			"bl32.bin"
 #define BL33_IMAGE_NAME			"bl33.bin"
 #define TB_FW_CONFIG_NAME		"fvp_tb_fw_config.dtb"
+#define SOC_FW_CONFIG_NAME		"fvp_soc_fw_config.dtb"
+#define TOS_FW_CONFIG_NAME		"fvp_tsp_fw_config.dtb"
+#define NT_FW_CONFIG_NAME		"fvp_nt_fw_config.dtb"
+#define FW_CONFIG_NAME			"fvp_fw_config.dtb"
 #define HW_CONFIG_NAME			"hw_config.dtb"
 
 #if TRUSTED_BOARD_BOOT
@@ -58,6 +62,22 @@
 		.path = TB_FW_CONFIG_NAME,
 		.mode = FOPEN_MODE_RB
 	},
+	[SOC_FW_CONFIG_ID] = {
+		.path = SOC_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[TOS_FW_CONFIG_ID] = {
+		.path = TOS_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[NT_FW_CONFIG_ID] = {
+		.path = NT_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[FW_CONFIG_ID] = {
+		.path = FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
 	[HW_CONFIG_ID] = {
 		.path = HW_CONFIG_NAME,
 		.mode = FOPEN_MODE_RB
diff --git a/plat/arm/board/juno/juno_bl1_setup.c b/plat/arm/board/juno/juno_bl1_setup.c
index 2234055..a9d5cc3 100644
--- a/plat/arm/board/juno/juno_bl1_setup.c
+++ b/plat/arm/board/juno/juno_bl1_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -62,11 +62,11 @@
  ******************************************************************************/
 bool plat_arm_bl1_fwu_needed(void)
 {
-	const int32_t *nv_flags_ptr = (const int32_t *)V2M_SYS_NVFLAGS_ADDR;
+	int32_t nv_flags = (int32_t)mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
 
 	/* Check if TOC is invalid or watchdog reset happened. */
-	return (!arm_io_is_toc_valid() || (((*nv_flags_ptr == -EAUTH) ||
-		(*nv_flags_ptr == -ENOENT)) && is_watchdog_reset()));
+	return (!arm_io_is_toc_valid() || (((nv_flags == -EAUTH) ||
+		(nv_flags == -ENOENT)) && is_watchdog_reset()));
 }
 
 /*******************************************************************************
@@ -86,13 +86,11 @@
  ******************************************************************************/
 __dead2 void bl1_plat_fwu_done(void *client_cookie, void *reserved)
 {
-	unsigned int *nv_flags_clr = (unsigned int *)
-			(V2M_SYSREGS_BASE + V2M_SYS_NVFLAGSCLR);
-	unsigned int *nv_flags_ptr = (unsigned int *)
-			(V2M_SYSREGS_BASE + V2M_SYS_NVFLAGS);
+	uint32_t nv_flags = mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
 
 	/* Clear the NV flags register. */
-	*nv_flags_clr = *nv_flags_ptr;
+	mmio_write_32((V2M_SYSREGS_BASE + V2M_SYS_NVFLAGSCLR),
+		      nv_flags);
 
 	/* Setup the watchdog to reset the system as soon as possible */
 	sp805_refresh(ARM_SP805_TWDG_BASE, 1U);
diff --git a/plat/arm/board/juno/juno_err.c b/plat/arm/board/juno/juno_err.c
index 60699cc..02d751e 100644
--- a/plat/arm/board/juno/juno_err.c
+++ b/plat/arm/board/juno/juno_err.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,10 +16,8 @@
  */
 void __dead2 plat_arm_error_handler(int err)
 {
-	uint32_t *flags_ptr = (uint32_t *)V2M_SYS_NVFLAGS_ADDR;
-
 	/* Propagate the err code in the NV-flags register */
-	*flags_ptr = err;
+	mmio_write_32(V2M_SYS_NVFLAGS_ADDR, (uint32_t)err);
 
 	/* Setup the watchdog to reset the system as soon as possible */
 	sp805_refresh(ARM_SP805_TWDG_BASE, 1U);
diff --git a/plat/arm/board/tc0/fdts/tc0_fw_config.dts b/plat/arm/board/tc/fdts/tc_fw_config.dts
similarity index 100%
rename from plat/arm/board/tc0/fdts/tc0_fw_config.dts
rename to plat/arm/board/tc/fdts/tc_fw_config.dts
diff --git a/plat/arm/board/tc0/fdts/tc0_spmc_manifest.dts b/plat/arm/board/tc/fdts/tc_spmc_manifest.dts
similarity index 97%
rename from plat/arm/board/tc0/fdts/tc0_spmc_manifest.dts
rename to plat/arm/board/tc/fdts/tc_spmc_manifest.dts
index 44c7008..a8592f6 100644
--- a/plat/arm/board/tc0/fdts/tc0_spmc_manifest.dts
+++ b/plat/arm/board/tc/fdts/tc_spmc_manifest.dts
@@ -13,7 +13,7 @@
 	attribute {
 		spmc_id = <0x8000>;
 		maj_ver = <0x1>;
-		min_ver = <0x0>;
+		min_ver = <0x1>;
 		exec_state = <0x0>;
 		load_address = <0x0 0xfd000000>;
 		entrypoint = <0x0 0xfd000000>;
@@ -110,7 +110,7 @@
 		};
 	};
 
-	/* 32MB of TC0_TZC_DRAM1_BASE */
+	/* 32MB of TC_TZC_DRAM1_BASE */
 	memory@fd000000 {
 		device_type = "memory";
 		reg = <0x0 0xfd000000 0x2000000>;
diff --git a/plat/arm/board/tc0/fdts/tc0_spmc_optee_sp_manifest.dts b/plat/arm/board/tc/fdts/tc_spmc_optee_sp_manifest.dts
similarity index 94%
rename from plat/arm/board/tc0/fdts/tc0_spmc_optee_sp_manifest.dts
rename to plat/arm/board/tc/fdts/tc_spmc_optee_sp_manifest.dts
index 0830d5c..34b4e74 100644
--- a/plat/arm/board/tc0/fdts/tc0_spmc_optee_sp_manifest.dts
+++ b/plat/arm/board/tc/fdts/tc_spmc_optee_sp_manifest.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,7 +13,7 @@
 	attribute {
 		spmc_id = <0x8000>;
 		maj_ver = <0x1>;
-		min_ver = <0x0>;
+		min_ver = <0x1>;
 		exec_state = <0x0>;
 		load_address = <0x0 0xfd000000>;
 		entrypoint = <0x0 0xfd000000>;
@@ -116,7 +116,7 @@
 		};
 	};
 
-	/* 32MB of TC0_TZC_DRAM1_BASE */
+	/* 32MB of TC_TZC_DRAM1_BASE */
 	memory@fd000000 {
 		device_type = "memory";
 		reg = <0x0 0xfd000000 0x2000000>;
diff --git a/plat/arm/board/tc0/fdts/tc0_tb_fw_config.dts b/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
similarity index 100%
rename from plat/arm/board/tc0/fdts/tc0_tb_fw_config.dts
rename to plat/arm/board/tc/fdts/tc_tb_fw_config.dts
diff --git a/plat/arm/board/tc0/include/plat_macros.S b/plat/arm/board/tc/include/plat_macros.S
similarity index 100%
rename from plat/arm/board/tc0/include/plat_macros.S
rename to plat/arm/board/tc/include/plat_macros.S
diff --git a/plat/arm/board/tc0/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
similarity index 82%
rename from plat/arm/board/tc0/include/platform_def.h
rename to plat/arm/board/tc/include/platform_def.h
index b169d77..c8edd2f 100644
--- a/plat/arm/board/tc0/include/platform_def.h
+++ b/plat/arm/board/tc/include/platform_def.h
@@ -29,38 +29,38 @@
  *   - BL32_BASE when SPD_spmd is enabled
  *   - Region to load Trusted OS
  */
-#define TC0_TZC_DRAM1_BASE		(ARM_AP_TZC_DRAM1_BASE -	\
-					 TC0_TZC_DRAM1_SIZE)
-#define TC0_TZC_DRAM1_SIZE		UL(0x02000000)	/* 32 MB */
-#define TC0_TZC_DRAM1_END		(TC0_TZC_DRAM1_BASE +		\
-					 TC0_TZC_DRAM1_SIZE - 1)
+#define TC_TZC_DRAM1_BASE		(ARM_AP_TZC_DRAM1_BASE -	\
+					 TC_TZC_DRAM1_SIZE)
+#define TC_TZC_DRAM1_SIZE		UL(0x02000000)	/* 32 MB */
+#define TC_TZC_DRAM1_END		(TC_TZC_DRAM1_BASE +		\
+					 TC_TZC_DRAM1_SIZE - 1)
 
-#define TC0_NS_DRAM1_BASE		ARM_DRAM1_BASE
-#define TC0_NS_DRAM1_SIZE		(ARM_DRAM1_SIZE -		\
+#define TC_NS_DRAM1_BASE		ARM_DRAM1_BASE
+#define TC_NS_DRAM1_SIZE		(ARM_DRAM1_SIZE -		\
 					 ARM_TZC_DRAM1_SIZE -		\
-					 TC0_TZC_DRAM1_SIZE)
-#define TC0_NS_DRAM1_END		(TC0_NS_DRAM1_BASE +		\
-					 TC0_NS_DRAM1_SIZE - 1)
+					 TC_TZC_DRAM1_SIZE)
+#define TC_NS_DRAM1_END		(TC_NS_DRAM1_BASE +		\
+					 TC_NS_DRAM1_SIZE - 1)
 
 /*
- * Mappings for TC0 DRAM1 (non-secure) and TC0 TZC DRAM1 (secure)
+ * Mappings for TC DRAM1 (non-secure) and TC TZC DRAM1 (secure)
  */
-#define TC0_MAP_NS_DRAM1		MAP_REGION_FLAT(		\
-						TC0_NS_DRAM1_BASE,	\
-						TC0_NS_DRAM1_SIZE,	\
+#define TC_MAP_NS_DRAM1		MAP_REGION_FLAT(		\
+						TC_NS_DRAM1_BASE,	\
+						TC_NS_DRAM1_SIZE,	\
 						MT_MEMORY | MT_RW | MT_NS)
 
 
-#define TC0_MAP_TZC_DRAM1		MAP_REGION_FLAT(		\
-						TC0_TZC_DRAM1_BASE,	\
-						TC0_TZC_DRAM1_SIZE,	\
+#define TC_MAP_TZC_DRAM1		MAP_REGION_FLAT(		\
+						TC_TZC_DRAM1_BASE,	\
+						TC_TZC_DRAM1_SIZE,	\
 						MT_MEMORY | MT_RW | MT_SECURE)
 /*
- * Max size of SPMC is 2MB for tc0. With SPMD enabled this value corresponds to
+ * Max size of SPMC is 2MB for tc. With SPMD enabled this value corresponds to
  * max size of BL32 image.
  */
 #if defined(SPD_spmd)
-#define PLAT_ARM_SPMC_BASE		TC0_TZC_DRAM1_BASE
+#define PLAT_ARM_SPMC_BASE		TC_TZC_DRAM1_BASE
 #define PLAT_ARM_SPMC_SIZE		UL(0x200000)  /* 2 MB */
 #endif
 
@@ -152,18 +152,18 @@
 #endif
 
 
-#define TC0_DEVICE_BASE			0x21000000
-#define TC0_DEVICE_SIZE			0x5f000000
+#define TC_DEVICE_BASE			0x21000000
+#define TC_DEVICE_SIZE			0x5f000000
 
-// TC0_MAP_DEVICE covers different peripherals
+// TC_MAP_DEVICE covers different peripherals
 // available to the platform
-#define TC0_MAP_DEVICE	MAP_REGION_FLAT(		\
-					TC0_DEVICE_BASE,	\
-					TC0_DEVICE_SIZE,	\
+#define TC_MAP_DEVICE	MAP_REGION_FLAT(		\
+					TC_DEVICE_BASE,	\
+					TC_DEVICE_SIZE,	\
 					MT_DEVICE | MT_RW | MT_SECURE)
 
 
-#define TC0_FLASH0_RO	MAP_REGION_FLAT(V2M_FLASH0_BASE,\
+#define TC_FLASH0_RO	MAP_REGION_FLAT(V2M_FLASH0_BASE,\
 						V2M_FLASH0_SIZE,	\
 						MT_DEVICE | MT_RO | MT_SECURE)
 
@@ -250,14 +250,14 @@
 		(TZC_REGION_ACCESS_RDWR(TZC_NSAID_DEFAULT))
 
 /*
- * The first region below, TC0_TZC_DRAM1_BASE (0xfd000000) to
+ * The first region below, TC_TZC_DRAM1_BASE (0xfd000000) to
  * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 48 MB of DRAM as
  * secure. The second region gives non secure access to rest of DRAM.
  */
-#define TC0_TZC_REGIONS_DEF						\
-	{TC0_TZC_DRAM1_BASE, ARM_SCP_TZC_DRAM1_END,			\
+#define TC_TZC_REGIONS_DEF						\
+	{TC_TZC_DRAM1_BASE, ARM_SCP_TZC_DRAM1_END,			\
 		TZC_REGION_S_RDWR, PLAT_ARM_TZC_NS_DEV_ACCESS},		\
-	{TC0_NS_DRAM1_BASE, TC0_NS_DRAM1_END, ARM_TZC_NS_DRAM_S_ACCESS, \
+	{TC_NS_DRAM1_BASE, TC_NS_DRAM1_END, ARM_TZC_NS_DRAM_S_ACCESS, \
 		PLAT_ARM_TZC_NS_DEV_ACCESS}
 
 /* virtual address used by dynamic mem_protect for chunk_base */
diff --git a/plat/arm/board/tc0/include/tc0_helpers.S b/plat/arm/board/tc/include/tc_helpers.S
similarity index 96%
rename from plat/arm/board/tc0/include/tc0_helpers.S
rename to plat/arm/board/tc/include/tc_helpers.S
index 90623a2..5f54856 100644
--- a/plat/arm/board/tc0/include/tc0_helpers.S
+++ b/plat/arm/board/tc/include/tc_helpers.S
@@ -15,7 +15,7 @@
 	/* ---------------------------------------------------------------------
 	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
 	 *
-	 * Function to calculate the core position on TC0.
+	 * Function to calculate the core position on TC.
 	 *
 	 * (ClusterId * PLAT_MAX_CPUS_PER_CLUSTER * PLAT_MAX_PE_PER_CPU) +
 	 * (CPUId * PLAT_MAX_PE_PER_CPU) +
diff --git a/plat/arm/board/tc/include/tc_plat.h b/plat/arm/board/tc/include/tc_plat.h
new file mode 100644
index 0000000..28c0308
--- /dev/null
+++ b/plat/arm/board/tc/include/tc_plat.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TC_PLAT_H
+#define TC_PLAT_H
+
+void tc_bl31_common_platform_setup(void);
+
+#endif /* TC_PLAT_H */
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
new file mode 100644
index 0000000..8db764c
--- /dev/null
+++ b/plat/arm/board/tc/platform.mk
@@ -0,0 +1,137 @@
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq ($(filter ${TARGET_PLATFORM}, 0 1),)
+        $(error TARGET_PLATFORM must be 0 or 1)
+endif
+
+CSS_LOAD_SCP_IMAGES	:=	1
+
+CSS_USE_SCMI_SDS_DRIVER	:=	1
+
+RAS_EXTENSION		:=	0
+
+SDEI_SUPPORT		:=	0
+
+EL3_EXCEPTION_HANDLING	:=	0
+
+HANDLE_EA_EL3_FIRST	:=	0
+
+# System coherency is managed in hardware
+HW_ASSISTED_COHERENCY	:=	1
+
+# When building for systems with hardware-assisted coherency, there's no need to
+# use USE_COHERENT_MEM. Require that USE_COHERENT_MEM must be set to 0 too.
+USE_COHERENT_MEM	:=	0
+
+GIC_ENABLE_V4_EXTN	:=      1
+
+# GIC-600 configuration
+GICV3_SUPPORT_GIC600	:=	1
+
+
+# Include GICv3 driver files
+include drivers/arm/gic/v3/gicv3.mk
+
+ENT_GIC_SOURCES		:=	${GICV3_SOURCES}		\
+				plat/common/plat_gicv3.c	\
+				plat/arm/common/arm_gicv3.c
+
+override NEED_BL2U	:=	no
+
+override ARM_PLAT_MT	:=	1
+
+TC_BASE	=	plat/arm/board/tc
+
+PLAT_INCLUDES		+=	-I${TC_BASE}/include/
+
+# Common CPU libraries
+TC_CPU_SOURCES	:=	lib/cpus/aarch64/cortex_a510.S
+
+# CPU libraries for TARGET_PLATFORM=0
+ifeq (${TARGET_PLATFORM}, 0)
+TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_a710.S \
+			lib/cpus/aarch64/cortex_x2.S
+endif
+
+# CPU libraries for TARGET_PLATFORM=1
+ifeq (${TARGET_PLATFORM}, 1)
+TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_makalu.S \
+			lib/cpus/aarch64/cortex_makalu_elp_arm.S
+endif
+
+INTERCONNECT_SOURCES	:=	${TC_BASE}/tc_interconnect.c
+
+PLAT_BL_COMMON_SOURCES	+=	${TC_BASE}/tc_plat.c	\
+				${TC_BASE}/include/tc_helpers.S
+
+BL1_SOURCES		+=	${INTERCONNECT_SOURCES}	\
+				${TC_CPU_SOURCES}	\
+				${TC_BASE}/tc_trusted_boot.c	\
+				${TC_BASE}/tc_err.c	\
+				drivers/arm/sbsa/sbsa.c
+
+
+BL2_SOURCES		+=	${TC_BASE}/tc_security.c	\
+				${TC_BASE}/tc_err.c		\
+				${TC_BASE}/tc_trusted_boot.c		\
+				lib/utils/mem_region.c			\
+				drivers/arm/tzc/tzc400.c		\
+				plat/arm/common/arm_tzc400.c		\
+				plat/arm/common/arm_nor_psci_mem_protect.c
+
+BL31_SOURCES		+=	${INTERCONNECT_SOURCES}	\
+				${TC_CPU_SOURCES}	\
+				${ENT_GIC_SOURCES}			\
+				${TC_BASE}/tc_bl31_setup.c	\
+				${TC_BASE}/tc_topology.c	\
+				drivers/cfi/v2m/v2m_flash.c		\
+				lib/utils/mem_region.c			\
+				plat/arm/common/arm_nor_psci_mem_protect.c
+
+# Add the FDT_SOURCES and options for Dynamic Config
+FDT_SOURCES		+=	${TC_BASE}/fdts/${PLAT}_fw_config.dts	\
+				${TC_BASE}/fdts/${PLAT}_tb_fw_config.dts
+FW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}_fw_config.dtb
+TB_FW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}_tb_fw_config.dtb
+
+# Add the FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
+# Add the TB_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
+
+ifeq (${SPD},spmd)
+ifeq ($(ARM_SPMC_MANIFEST_DTS),)
+ARM_SPMC_MANIFEST_DTS	:=	${TC_BASE}/fdts/${PLAT}_spmc_manifest.dts
+endif
+
+FDT_SOURCES		+=	${ARM_SPMC_MANIFEST_DTS}
+TC_TOS_FW_CONFIG	:=	${BUILD_PLAT}/fdts/$(notdir $(basename ${ARM_SPMC_MANIFEST_DTS})).dtb
+
+# Add the TOS_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TC_TOS_FW_CONFIG},--tos-fw-config,${TC_TOS_FW_CONFIG}))
+endif
+
+#Device tree
+TC_HW_CONFIG_DTS	:=	fdts/tc.dts
+TC_HW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}.dtb
+FDT_SOURCES		+=	${TC_HW_CONFIG_DTS}
+$(eval TC_HW_CONFIG	:=	${BUILD_PLAT}/$(patsubst %.dts,%.dtb,$(TC_HW_CONFIG_DTS)))
+
+# Add the HW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TC_HW_CONFIG},--hw-config,${TC_HW_CONFIG}))
+
+override CTX_INCLUDE_AARCH32_REGS	:= 0
+
+override CTX_INCLUDE_PAUTH_REGS	:= 1
+
+override ENABLE_SPE_FOR_LOWER_ELS	:= 0
+
+override ENABLE_AMU := 1
+
+include plat/arm/common/arm_common.mk
+include plat/arm/css/common/css_common.mk
+include plat/arm/soc/common/soc_css.mk
+include plat/arm/board/common/board_common.mk
diff --git a/plat/arm/board/tc0/tc0_bl31_setup.c b/plat/arm/board/tc/tc_bl31_setup.c
similarity index 83%
rename from plat/arm/board/tc0/tc0_bl31_setup.c
rename to plat/arm/board/tc/tc_bl31_setup.c
index b91b11c..ecec26c 100644
--- a/plat/arm/board/tc0/tc0_bl31_setup.c
+++ b/plat/arm/board/tc/tc_bl31_setup.c
@@ -7,7 +7,7 @@
 #include <assert.h>
 
 #include <libfdt.h>
-#include <tc0_plat.h>
+#include <tc_plat.h>
 
 #include <common/bl_common.h>
 #include <common/debug.h>
@@ -16,7 +16,7 @@
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
 
-static scmi_channel_plat_info_t tc0_scmi_plat_info[] = {
+static scmi_channel_plat_info_t tc_scmi_plat_info[] = {
 	{
 		.scmi_mbx_mem = CSS_SCMI_PAYLOAD_BASE,
 		.db_reg_addr = PLAT_CSS_MHU_BASE + SENDER_REG_SET(0),
@@ -28,13 +28,13 @@
 
 void bl31_platform_setup(void)
 {
-	tc0_bl31_common_platform_setup();
+	tc_bl31_common_platform_setup();
 }
 
 scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
 {
 
-	return &tc0_scmi_plat_info[channel_id];
+	return &tc_scmi_plat_info[channel_id];
 
 }
 
@@ -44,7 +44,7 @@
 	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
 }
 
-void tc0_bl31_common_platform_setup(void)
+void tc_bl31_common_platform_setup(void)
 {
 	arm_bl31_platform_setup();
 }
diff --git a/plat/arm/board/tc0/tc0_err.c b/plat/arm/board/tc/tc_err.c
similarity index 91%
rename from plat/arm/board/tc0/tc0_err.c
rename to plat/arm/board/tc/tc_err.c
index 83f2e9f..9ed7e92 100644
--- a/plat/arm/board/tc0/tc0_err.c
+++ b/plat/arm/board/tc/tc_err.c
@@ -7,7 +7,7 @@
 #include <plat/arm/common/plat_arm.h>
 
 /*
- * tc0 error handler
+ * tc error handler
  */
 void __dead2 plat_arm_error_handler(int err)
 {
diff --git a/plat/arm/board/tc0/tc0_interconnect.c b/plat/arm/board/tc/tc_interconnect.c
similarity index 100%
rename from plat/arm/board/tc0/tc0_interconnect.c
rename to plat/arm/board/tc/tc_interconnect.c
diff --git a/plat/arm/board/tc0/tc0_plat.c b/plat/arm/board/tc/tc_plat.c
similarity index 96%
rename from plat/arm/board/tc0/tc0_plat.c
rename to plat/arm/board/tc/tc_plat.c
index b5698c0..3863a0a 100644
--- a/plat/arm/board/tc0/tc0_plat.c
+++ b/plat/arm/board/tc/tc_plat.c
@@ -28,19 +28,19 @@
 #if IMAGE_BL1
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
-	TC0_FLASH0_RO,
-	TC0_MAP_DEVICE,
+	TC_FLASH0_RO,
+	TC_MAP_DEVICE,
 	{0}
 };
 #endif
 #if IMAGE_BL2
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
-	TC0_FLASH0_RO,
-	TC0_MAP_DEVICE,
-	TC0_MAP_NS_DRAM1,
+	TC_FLASH0_RO,
+	TC_MAP_DEVICE,
+	TC_MAP_NS_DRAM1,
 #if defined(SPD_spmd)
-	TC0_MAP_TZC_DRAM1,
+	TC_MAP_TZC_DRAM1,
 #endif
 #if ARM_BL31_IN_DRAM
 	ARM_MAP_BL31_SEC_DRAM,
@@ -62,7 +62,7 @@
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
 	V2M_MAP_IOFPGA,
-	TC0_MAP_DEVICE,
+	TC_MAP_DEVICE,
 #if SPM_MM
 	ARM_SPM_BUF_EL3_MMAP,
 #endif
diff --git a/plat/arm/board/tc0/tc0_security.c b/plat/arm/board/tc/tc_security.c
similarity index 94%
rename from plat/arm/board/tc0/tc0_security.c
rename to plat/arm/board/tc/tc_security.c
index f543762..6a34501 100644
--- a/plat/arm/board/tc0/tc0_security.c
+++ b/plat/arm/board/tc/tc_security.c
@@ -8,7 +8,7 @@
 #include <platform_def.h>
 
 static const arm_tzc_regions_info_t tzc_regions[] = {
-	TC0_TZC_REGIONS_DEF,
+	TC_TZC_REGIONS_DEF,
 	{}
 };
 
diff --git a/plat/arm/board/tc0/tc0_topology.c b/plat/arm/board/tc/tc_topology.c
similarity index 96%
rename from plat/arm/board/tc0/tc0_topology.c
rename to plat/arm/board/tc/tc_topology.c
index 8cfc3b5..9e18da6 100644
--- a/plat/arm/board/tc0/tc0_topology.c
+++ b/plat/arm/board/tc/tc_topology.c
@@ -10,7 +10,7 @@
 /******************************************************************************
  * The power domain tree descriptor.
  ******************************************************************************/
-const unsigned char tc0_pd_tree_desc[] = {
+const unsigned char tc_pd_tree_desc[] = {
 	PLAT_ARM_CLUSTER_COUNT,
 	PLAT_MAX_CPUS_PER_CLUSTER,
 };
@@ -20,7 +20,7 @@
  ******************************************************************************/
 const unsigned char *plat_get_power_domain_tree_desc(void)
 {
-	return tc0_pd_tree_desc;
+	return tc_pd_tree_desc;
 }
 
 /*******************************************************************************
diff --git a/plat/arm/board/tc0/tc0_trusted_boot.c b/plat/arm/board/tc/tc_trusted_boot.c
similarity index 100%
rename from plat/arm/board/tc0/tc0_trusted_boot.c
rename to plat/arm/board/tc/tc_trusted_boot.c
diff --git a/plat/arm/board/tc0/include/tc0_plat.h b/plat/arm/board/tc0/include/tc0_plat.h
deleted file mode 100644
index f0cb431..0000000
--- a/plat/arm/board/tc0/include/tc0_plat.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef tc0_bl31_common_platform_setup_PLAT_H
-#define tc0_bl31_common_platform_setup_PLAT_H
-
-void tc0_bl31_common_platform_setup(void);
-
-#endif /* tc0_bl31_common_platform_setup_PLAT_H */
diff --git a/plat/arm/board/tc0/platform.mk b/plat/arm/board/tc0/platform.mk
deleted file mode 100644
index 814ccd3..0000000
--- a/plat/arm/board/tc0/platform.mk
+++ /dev/null
@@ -1,120 +0,0 @@
-# Copyright (c) 2020-2021, Arm Limited. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-CSS_LOAD_SCP_IMAGES	:=	1
-
-CSS_USE_SCMI_SDS_DRIVER	:=	1
-
-RAS_EXTENSION		:=	0
-
-SDEI_SUPPORT		:=	0
-
-EL3_EXCEPTION_HANDLING	:=	0
-
-HANDLE_EA_EL3_FIRST	:=	0
-
-# System coherency is managed in hardware
-HW_ASSISTED_COHERENCY	:=	1
-
-# When building for systems with hardware-assisted coherency, there's no need to
-# use USE_COHERENT_MEM. Require that USE_COHERENT_MEM must be set to 0 too.
-USE_COHERENT_MEM	:=	0
-
-GIC_ENABLE_V4_EXTN	:=      1
-
-# GIC-600 configuration
-GICV3_SUPPORT_GIC600	:=	1
-
-
-# Include GICv3 driver files
-include drivers/arm/gic/v3/gicv3.mk
-
-ENT_GIC_SOURCES		:=	${GICV3_SOURCES}		\
-				plat/common/plat_gicv3.c	\
-				plat/arm/common/arm_gicv3.c
-
-override NEED_BL2U	:=	no
-
-override ARM_PLAT_MT	:=	1
-
-TC0_BASE	=	plat/arm/board/tc0
-
-PLAT_INCLUDES		+=	-I${TC0_BASE}/include/
-
-TC0_CPU_SOURCES	:=	lib/cpus/aarch64/cortex_a510.S         \
-			lib/cpus/aarch64/cortex_a710.S \
-			lib/cpus/aarch64/cortex_x2.S
-
-INTERCONNECT_SOURCES	:=	${TC0_BASE}/tc0_interconnect.c
-
-PLAT_BL_COMMON_SOURCES	+=	${TC0_BASE}/tc0_plat.c	\
-				${TC0_BASE}/include/tc0_helpers.S
-
-BL1_SOURCES		+=	${INTERCONNECT_SOURCES}	\
-				${TC0_CPU_SOURCES}	\
-				${TC0_BASE}/tc0_trusted_boot.c	\
-				${TC0_BASE}/tc0_err.c	\
-				drivers/arm/sbsa/sbsa.c
-
-
-BL2_SOURCES		+=	${TC0_BASE}/tc0_security.c	\
-				${TC0_BASE}/tc0_err.c		\
-				${TC0_BASE}/tc0_trusted_boot.c		\
-				lib/utils/mem_region.c			\
-				drivers/arm/tzc/tzc400.c		\
-				plat/arm/common/arm_tzc400.c		\
-				plat/arm/common/arm_nor_psci_mem_protect.c
-
-BL31_SOURCES		+=	${INTERCONNECT_SOURCES}	\
-				${TC0_CPU_SOURCES}	\
-				${ENT_GIC_SOURCES}			\
-				${TC0_BASE}/tc0_bl31_setup.c	\
-				${TC0_BASE}/tc0_topology.c	\
-				drivers/cfi/v2m/v2m_flash.c		\
-				lib/utils/mem_region.c			\
-				plat/arm/common/arm_nor_psci_mem_protect.c
-
-# Add the FDT_SOURCES and options for Dynamic Config
-FDT_SOURCES		+=	${TC0_BASE}/fdts/${PLAT}_fw_config.dts	\
-				${TC0_BASE}/fdts/${PLAT}_tb_fw_config.dts
-FW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}_fw_config.dtb
-TB_FW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}_tb_fw_config.dtb
-
-# Add the FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
-# Add the TB_FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
-
-ifeq (${SPD},spmd)
-ifeq ($(ARM_SPMC_MANIFEST_DTS),)
-ARM_SPMC_MANIFEST_DTS	:=	${TC0_BASE}/fdts/${PLAT}_spmc_manifest.dts
-endif
-
-FDT_SOURCES		+=	${ARM_SPMC_MANIFEST_DTS}
-TC0_TOS_FW_CONFIG	:=	${BUILD_PLAT}/fdts/$(notdir $(basename ${ARM_SPMC_MANIFEST_DTS})).dtb
-
-# Add the TOS_FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${TC0_TOS_FW_CONFIG},--tos-fw-config,${TC0_TOS_FW_CONFIG}))
-endif
-
-#Device tree
-TC0_HW_CONFIG_DTS	:=	fdts/tc0.dts
-TC0_HW_CONFIG		:=	${BUILD_PLAT}/fdts/${PLAT}.dtb
-FDT_SOURCES		+=	${TC0_HW_CONFIG_DTS}
-$(eval TC0_HW_CONFIG	:=	${BUILD_PLAT}/$(patsubst %.dts,%.dtb,$(TC0_HW_CONFIG_DTS)))
-
-# Add the HW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${TC0_HW_CONFIG},--hw-config,${TC0_HW_CONFIG}))
-
-override CTX_INCLUDE_AARCH32_REGS	:= 0
-
-override CTX_INCLUDE_PAUTH_REGS	:= 1
-
-override ENABLE_SPE_FOR_LOWER_ELS	:= 0
-
-include plat/arm/common/arm_common.mk
-include plat/arm/css/common/css_common.mk
-include plat/arm/soc/common/soc_css.mk
-include plat/arm/board/common/board_common.mk
diff --git a/plat/arm/common/arm_bl2_setup.c b/plat/arm/common/arm_bl2_setup.c
index 63ed9fe..26af383 100644
--- a/plat/arm/common/arm_bl2_setup.c
+++ b/plat/arm/common/arm_bl2_setup.c
@@ -94,13 +94,10 @@
 {
 	arm_bl2_dyn_cfg_init();
 
-#if ARM_GPT_SUPPORT
-	int result = arm_set_image_source(FIP_IMAGE_ID, "FIP_A");
-
-	if (result != 0) {
-		panic();
-	}
-#endif /* ARM_GPT_SUPPORT */
+#if ARM_GPT_SUPPORT && !PSA_FWU_SUPPORT
+	/* Always use the FIP from bank 0 */
+	arm_set_fip_addr(0U);
+#endif /* ARM_GPT_SUPPORT && !PSA_FWU_SUPPORT */
 }
 
 /*
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index f1e4cf5..4d5e8b4 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -185,6 +185,18 @@
   BL2_CPPFLAGS += -march=armv8-a+crc
 endif
 
+ifeq ($(PSA_FWU_SUPPORT),1)
+    # GPT support is recommended as per PSA FWU specification hence
+    # PSA FWU implementation is tightly coupled with GPT support,
+    # and it does not support other formats.
+    ifneq ($(ARM_GPT_SUPPORT),1)
+      $(error For PSA_FWU_SUPPORT, ARM_GPT_SUPPORT must be enabled)
+    endif
+    FWU_MK := drivers/fwu/fwu.mk
+    $(info Including ${FWU_MK})
+    include ${FWU_MK}
+endif
+
 ifeq (${ARCH}, aarch64)
 PLAT_INCLUDES		+=	-Iinclude/plat/arm/common/aarch64
 endif
@@ -230,7 +242,7 @@
 				drivers/io/io_storage.c				\
 				plat/arm/common/arm_bl2_setup.c			\
 				plat/arm/common/arm_err.c			\
-				common/hw_crc32.c				\
+				common/tf_crc32.c				\
 				${ARM_IO_SOURCES}
 
 # Firmware Configuration Framework sources
@@ -256,8 +268,10 @@
 ifeq (${JUNO_AARCH32_EL3_RUNTIME},1)
 BL2_SOURCES		+=	plat/arm/common/aarch32/arm_bl2_mem_params_desc.c
 else
+ifeq ($(filter ${TARGET_PLATFORM}, fpga fvp),)
 BL2_SOURCES		+=	plat/arm/common/${ARCH}/arm_bl2_mem_params_desc.c
 endif
+endif
 BL2_SOURCES		+=	plat/arm/common/arm_image_load.c		\
 				common/desc_image_load.c
 ifeq (${SPD},opteed)
diff --git a/plat/arm/common/arm_io_storage.c b/plat/arm/common/arm_io_storage.c
index c5d913e..387086a 100644
--- a/plat/arm/common/arm_io_storage.c
+++ b/plat/arm/common/arm_io_storage.c
@@ -5,6 +5,7 @@
  */
 
 #include <common/debug.h>
+#include <drivers/fwu/fwu_metadata.h>
 #include <drivers/io/io_driver.h>
 #include <drivers/io/io_fip.h>
 #include <drivers/io/io_memmap.h>
@@ -24,6 +25,13 @@
 static const io_dev_connector_t *memmap_dev_con;
 uintptr_t memmap_dev_handle;
 
+#if ARM_GPT_SUPPORT
+/* fip partition names */
+static const char * const fip_part_names[] = {"FIP_A", "FIP_B"};
+CASSERT(sizeof(fip_part_names)/sizeof(char *) == NR_OF_FW_BANKS,
+	assert_fip_partition_names_missing);
+#endif /* ARM_GPT_SUPPORT */
+
 /* Weak definitions may be overridden in specific ARM standard platform */
 #pragma weak plat_arm_io_setup
 #pragma weak plat_arm_get_alt_image_source
@@ -139,17 +147,20 @@
 }
 
 #if ARM_GPT_SUPPORT
-/**********************************************************************
- * arm_set_image_source: Set image specification in IO policy
+/******************************************************************************
+ * Retrieve partition entry details such as offset and length, and set these
+ * details in the I/O policy of the requested image.
  *
- * @image_id: id of the image whose specification to be set
+ * @image_id: image id whose I/O policy to be updated
  *
- * @part_name: name of the partition that to be read for entry details
+ * @part_name: partition name whose details to be retrieved
  *
- * set the entry and offset details of partition in global IO policy
- * of the image
- *********************************************************************/
-int arm_set_image_source(unsigned int image_id, const char *part_name)
+ * Returns 0 on success, error otherwise
+ * Alongside, returns device handle and image specification of requested
+ * image.
+ ******************************************************************************/
+int arm_set_image_source(unsigned int image_id, const char *part_name,
+			 uintptr_t *dev_handle, uintptr_t *image_spec)
 {
 	const partition_entry_t *entry = get_partition_entry(part_name);
 
@@ -158,19 +169,82 @@
 		return -ENOENT;
 	}
 
-	const struct plat_io_policy *policy = FCONF_GET_PROPERTY(arm,
-								 io_policies,
-								 image_id);
+	struct plat_io_policy *policy = FCONF_GET_PROPERTY(arm,
+							   io_policies,
+							   image_id);
 
 	assert(policy != NULL);
 	assert(policy->image_spec != 0UL);
 
+	io_block_spec_t *spec = (io_block_spec_t *)policy->image_spec;
 	/* set offset and length of the image */
-	io_block_spec_t *image_spec = (io_block_spec_t *)policy->image_spec;
+	spec->offset = PLAT_ARM_FLASH_IMAGE_BASE + entry->start;
+	spec->length = entry->length;
 
-	image_spec->offset = PLAT_ARM_FLASH_IMAGE_BASE + entry->start;
-	image_spec->length = entry->length;
+	*dev_handle = *(policy->dev_handle);
+	*image_spec = policy->image_spec;
 
 	return 0;
 }
-#endif
+
+/*******************************************************************************
+ * Set the source offset and length of the FIP image in its I/O policy.
+ *
+ * @active_fw_bank_idx: active firmware bank index gathered from FWU metadata.
+ ******************************************************************************/
+void arm_set_fip_addr(uint32_t active_fw_bank_idx)
+{
+	uintptr_t dev_handle __unused;
+	uintptr_t image_spec __unused;
+
+	assert(active_fw_bank_idx < NR_OF_FW_BANKS);
+
+	INFO("Booting with partition %s\n", fip_part_names[active_fw_bank_idx]);
+
+	int result = arm_set_image_source(FIP_IMAGE_ID,
+					  fip_part_names[active_fw_bank_idx],
+					  &dev_handle,
+					  &image_spec);
+	if (result != 0) {
+		panic();
+	}
+}
+#endif /* ARM_GPT_SUPPORT */
+
+#if PSA_FWU_SUPPORT
+/*******************************************************************************
+ * Read the FIP partition of the GPT image corresponding to the active firmware
+ * bank to get its offset and length, and update these details in the I/O policy
+ * of the FIP image.
+ ******************************************************************************/
+void plat_fwu_set_images_source(struct fwu_metadata *metadata)
+{
+	arm_set_fip_addr(metadata->active_index);
+}
+
+/*******************************************************************************
+ * Read the requested FWU metadata partition of the GPT image to get its offset
+ * and length, and update these details in the I/O policy of the requested FWU
+ * metadata image.
+ ******************************************************************************/
+int plat_fwu_set_metadata_image_source(unsigned int image_id,
+				       uintptr_t *dev_handle,
+				       uintptr_t *image_spec)
+{
+	int result = -1;
+
+	if (image_id == FWU_METADATA_IMAGE_ID) {
+		result = arm_set_image_source(FWU_METADATA_IMAGE_ID,
+					      "FWU-Metadata",
+					      dev_handle,
+					      image_spec);
+	} else if (image_id == BKUP_FWU_METADATA_IMAGE_ID) {
+		result = arm_set_image_source(BKUP_FWU_METADATA_IMAGE_ID,
+					      "Bkup-FWU-Metadata",
+					      dev_handle,
+					      image_spec);
+	}
+
+	return result;
+}
+#endif /* PSA_FWU_SUPPORT */
diff --git a/plat/arm/common/fconf/arm_fconf_io.c b/plat/arm/common/fconf/arm_fconf_io.c
index 8e4469f..86fd6d5 100644
--- a/plat/arm/common/fconf/arm_fconf_io.c
+++ b/plat/arm/common/fconf/arm_fconf_io.c
@@ -18,6 +18,11 @@
 #include <plat/arm/common/arm_fconf_io_storage.h>
 #include <platform_def.h>
 
+#if PSA_FWU_SUPPORT
+/* metadata entry details */
+static io_block_spec_t fwu_metadata_spec;
+#endif /* PSA_FWU_SUPPORT */
+
 io_block_spec_t fip_block_spec = {
 /*
  * This is fixed FIP address used by BL1, BL2 loads partition table
@@ -92,6 +97,20 @@
 		open_memmap
 	},
 #endif /* ARM_GPT_SUPPORT */
+#if PSA_FWU_SUPPORT
+	[FWU_METADATA_IMAGE_ID] = {
+		&memmap_dev_handle,
+		/* filled runtime from partition information */
+		(uintptr_t)&fwu_metadata_spec,
+		open_memmap
+	},
+	[BKUP_FWU_METADATA_IMAGE_ID] = {
+		&memmap_dev_handle,
+		/* filled runtime from partition information */
+		(uintptr_t)&fwu_metadata_spec,
+		open_memmap
+	},
+#endif /* PSA_FWU_SUPPORT */
 	[FIP_IMAGE_ID] = {
 		&memmap_dev_handle,
 		(uintptr_t)&fip_block_spec,
diff --git a/plat/arm/css/sgi/include/sgi_ras.h b/plat/arm/css/sgi/include/sgi_ras.h
index 4b8a0d1..e69a684 100644
--- a/plat/arm/css/sgi/include/sgi_ras.h
+++ b/plat/arm/css/sgi/include/sgi_ras.h
@@ -7,16 +7,11 @@
 #ifndef SGI_RAS_H
 #define SGI_RAS_H
 
-/* Platform specific SMC FID's used for DMC-620 RAS error handling */
-#define SP_DMC_ERROR_OVERFLOW_EVENT_AARCH64	0xC4000043
-#define SP_DMC_ERROR_ECC_EVENT_AARCH64		0xC4000044
-
 /*
  * Mapping the RAS interrupt with SDEI event number and the event
  * id used with Standalone MM code
  */
 struct sgi_ras_ev_map {
-	int ras_ev_num;		/* RAS Event number */
 	int sdei_ev_num;	/* SDEI Event number */
 	int intr;		/* Physical intr number */
 };
diff --git a/plat/arm/css/sgi/sgi_ras.c b/plat/arm/css/sgi/sgi_ras.c
index a04972d..4f03ac4 100644
--- a/plat/arm/css/sgi/sgi_ras.c
+++ b/plat/arm/css/sgi/sgi_ras.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,44 +20,51 @@
 static int sgi_ras_intr_handler(const struct err_record_info *err_rec,
 				int probe_data,
 				const struct err_handler_data *const data);
-struct efi_guid {
-	uint32_t	data1;
-	uint16_t	data2;
-	uint16_t	data3;
-	uint8_t		data4[8];
-};
-
 typedef struct mm_communicate_header {
 	struct efi_guid	header_guid;
 	size_t		message_len;
 	uint8_t		data[8];
 } mm_communicate_header_t;
 
+/*
+ * GUID to indicate that the MM communication message is intended for DMC-620
+ * MM driver.
+ */
+const struct efi_guid dmc620_ecc_event_guid = {
+	0x5ef0afd5, 0xe01a, 0x4c30,
+	{0x86, 0x19, 0x45, 0x46, 0x26, 0x91, 0x80, 0x98}
+};
+
 struct sgi_ras_ev_map sgi575_ras_map[] = {
 
-	/* DMC620 error overflow interrupt*/
-	{SP_DMC_ERROR_OVERFLOW_EVENT_AARCH64, SGI_SDEI_DS_EVENT_1, 33},
+	/* DMC 0 error ECC error interrupt*/
+	{SGI_SDEI_DS_EVENT_0, 35},
 
-	/* DMC620 error ECC error interrupt*/
-	{SP_DMC_ERROR_ECC_EVENT_AARCH64, SGI_SDEI_DS_EVENT_0, 35},
+	/* DMC 1 error ECC error interrupt*/
+	{SGI_SDEI_DS_EVENT_1, 39},
 };
 
 #define SGI575_RAS_MAP_SIZE	ARRAY_SIZE(sgi575_ras_map)
 
 struct err_record_info sgi_err_records[] = {
 	{
+		/* DMC 0 error record info */
 		.handler = &sgi_ras_intr_handler,
+		.aux_data = (void *)0,
+	}, {
+		/* DMC 1 error record info */
+		.handler = &sgi_ras_intr_handler,
+		.aux_data = (void *)1,
 	},
 };
 
 struct ras_interrupt sgi_ras_interrupts[] = {
 	{
-		.intr_number = 33,
-		.err_record = &sgi_err_records[0],
-	},
-	{
 		.intr_number = 35,
 		.err_record = &sgi_err_records[0],
+	}, {
+		.intr_number = 39,
+		.err_record = &sgi_err_records[1],
 	}
 };
 
@@ -138,9 +145,10 @@
 	 */
 	header = (void *) PLAT_SPM_BUF_BASE;
 	memset(header, 0, sizeof(*header));
-	memcpy(&header->data, &ras_map->ras_ev_num,
-	       sizeof(ras_map->ras_ev_num));
-	header->message_len = 4;
+	memcpy(&header->data, &err_rec->aux_data, sizeof(err_rec->aux_data));
+	header->message_len = sizeof(err_rec->aux_data);
+	memcpy(&header->header_guid, (void *) &dmc620_ecc_event_guid,
+			sizeof(const struct efi_guid));
 
 	spm_mm_sp_call(MM_COMMUNICATE_AARCH64, (uint64_t)header, 0,
 		       plat_my_core_pos());
diff --git a/plat/common/aarch64/plat_common.c b/plat/common/aarch64/plat_common.c
index ba4c366..5b3262c 100644
--- a/plat/common/aarch64/plat_common.c
+++ b/plat/common/aarch64/plat_common.c
@@ -90,6 +90,7 @@
 #endif
 	unsigned int level = (unsigned int)GET_EL(read_spsr_el3());
 
+	ERROR_NL();
 	ERROR("Unhandled External Abort received on 0x%lx from %s\n",
 		read_mpidr_el1(), get_el_str(level));
 	ERROR("exception reason=%u syndrome=0x%llx\n", ea_reason, syndrome);
diff --git a/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c b/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c
index 3cf5c36..2df96ae 100644
--- a/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c
+++ b/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -43,6 +43,8 @@
 	 IOMUXC_SW_PAD_CTL_PAD_SD3_SLEW_SLOW         | \
 	 IOMUXC_SW_PAD_CTL_PAD_SD3_DSE_3_X6)
 
+static struct mmc_device_info mmc_info;
+
 static void picopi_setup_pinmux(void)
 {
 	/* Configure UART5 TX */
@@ -93,14 +95,13 @@
 static void picopi_usdhc_setup(void)
 {
 	imx_usdhc_params_t params;
-	struct mmc_device_info info;
 
 	zeromem(&params, sizeof(imx_usdhc_params_t));
 	params.reg_base = PLAT_PICOPI_BOOT_MMC_BASE;
 	params.clk_rate = 25000000;
 	params.bus_width = MMC_BUS_WIDTH_8;
-	info.mmc_dev_type = MMC_IS_EMMC;
-	imx_usdhc_init(&params, &info);
+	mmc_info.mmc_dev_type = MMC_IS_EMMC;
+	imx_usdhc_init(&params, &mmc_info);
 }
 
 static void picopi_setup_usb_clocks(void)
diff --git a/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c b/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c
index 935a411..ec13ade 100644
--- a/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c
+++ b/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -69,6 +69,8 @@
 	 IOMUXC_SW_PAD_CTL_PAD_ECSPI1_SCLK_HYS_EN		| \
 	 IOMUXC_SW_PAD_CTL_PAD_ECSPI1_SCLK_DSE_1_X4)
 
+static struct mmc_device_info mmc_info;
+
 static void warp7_setup_pinmux(void)
 {
 	/* Configure UART1 TX */
@@ -99,14 +101,13 @@
 static void warp7_usdhc_setup(void)
 {
 	imx_usdhc_params_t params;
-	struct mmc_device_info info;
 
 	zeromem(&params, sizeof(imx_usdhc_params_t));
 	params.reg_base = PLAT_WARP7_BOOT_MMC_BASE;
 	params.clk_rate = 25000000;
 	params.bus_width = MMC_BUS_WIDTH_8;
-	info.mmc_dev_type = MMC_IS_EMMC;
-	imx_usdhc_init(&params, &info);
+	mmc_info.mmc_dev_type = MMC_IS_EMMC;
+	imx_usdhc_init(&params, &mmc_info);
 }
 
 static void warp7_setup_usb_clocks(void)
diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index f8efa56..940d22b 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/include/platform_def.h
@@ -42,7 +42,7 @@
 #define BL31_BASE			U(0x900000)
 #define BL31_LIMIT			U(0x920000)
 #define IMX8MM_FIP_BASE			U(0x40310000)
-#define IMX8MM_FIP_SIZE			U(0x000200000)
+#define IMX8MM_FIP_SIZE			U(0x000300000)
 #define IMX8MM_FIP_LIMIT		U(FIP_BASE + FIP_SIZE)
 
 /* Define FIP image location on eMMC */
@@ -56,7 +56,7 @@
 
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
-#define PLAT_NS_IMAGE_SIZE		U(0x00100000)
+#define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
diff --git a/plat/marvell/armada/a3k/common/a3700_common.mk b/plat/marvell/armada/a3k/common/a3700_common.mk
index 7d95e48..0a89742 100644
--- a/plat/marvell/armada/a3k/common/a3700_common.mk
+++ b/plat/marvell/armada/a3k/common/a3700_common.mk
@@ -74,44 +74,45 @@
 ifdef WTP
 
 $(if $(wildcard $(value WTP)/*),,$(error "'WTP=$(value WTP)' was specified, but '$(value WTP)' directory does not exist"))
-$(if $(shell test -s "$(value WTP)/branch.txt" || git -C $(value WTP) rev-parse --show-cdup 2>&1),$(error "'WTP=$(value WTP)' was specified, but '$(value WTP)' does not contain valid Marvell a3700_utils release tarball nor git repository"))
+$(if $(shell git -C $(value WTP) rev-parse --show-cdup 2>&1),$(error "'WTP=$(value WTP)' was specified, but '$(value WTP)' does not contain valid A3700-utils-marvell git repository"))
 
-DOIMAGEPATH	:= $(WTP)
-DOIMAGETOOL	:= $(DOIMAGEPATH)/wtptp/src/TBB_Linux/release/TBB_linux
+TBB		:= $(WTP)/wtptp/src/TBB_Linux/release/TBB_linux
 
 BUILD_UART	:= uart-images
 UART_IMAGE	:= $(BUILD_UART).tgz.bin
 
 ifeq ($(MARVELL_SECURE_BOOT),1)
-DOIMAGE_CFG	:= $(BUILD_PLAT)/atf-tim.txt
-DOIMAGEUART_CFG	:= $(BUILD_PLAT)/$(BUILD_UART)/atf-tim.txt
-IMAGESPATH	:= $(DOIMAGEPATH)/tim/trusted
-TIMNCFG		:= $(BUILD_PLAT)/atf-timN.txt
-TIMNUARTCFG	:= $(BUILD_PLAT)/$(BUILD_UART)/atf-timN.txt
-TIMNSIG		:= $(IMAGESPATH)/timnsign.txt
-TIM2IMGARGS	:= -i $(DOIMAGE_CFG) -n $(TIMNCFG)
-TIMN_IMAGE	:= $$(grep "Image Filename:" -m 1 $(TIMNCFG) | cut -c 17-)
+TIM_CFG		:= $(BUILD_PLAT)/atf-tim.txt
+TIM_UART_CFG	:= $(BUILD_PLAT)/$(BUILD_UART)/atf-tim.txt
+IMAGESPATH	:= $(WTP)/tim/trusted
+TIMN_CFG	:= $(BUILD_PLAT)/atf-timN.txt
+TIMN_UART_CFG	:= $(BUILD_PLAT)/$(BUILD_UART)/atf-timN.txt
+TIMN_SIG	:= $(IMAGESPATH)/timnsign.txt
+TIM2IMGARGS	:= -i $(TIM_CFG) -n $(TIMN_CFG)
+TIMN_UART_IMAGE	:= $$(grep "Image Filename:" -m 1 $(TIMN_UART_CFG) | cut -c 17-)
 else #MARVELL_SECURE_BOOT
-DOIMAGE_CFG	:= $(BUILD_PLAT)/atf-ntim.txt
-DOIMAGEUART_CFG	:= $(BUILD_PLAT)/$(BUILD_UART)/atf-ntim.txt
-IMAGESPATH	:= $(DOIMAGEPATH)/tim/untrusted
-TIM2IMGARGS	:= -i $(DOIMAGE_CFG)
+TIM_CFG		:= $(BUILD_PLAT)/atf-ntim.txt
+TIM_UART_CFG	:= $(BUILD_PLAT)/$(BUILD_UART)/atf-ntim.txt
+IMAGESPATH	:= $(WTP)/tim/untrusted
+TIM2IMGARGS	:= -i $(TIM_CFG)
 endif #MARVELL_SECURE_BOOT
 
-TIMBUILD	:= $(DOIMAGEPATH)/script/buildtim.sh
-TIM2IMG		:= $(DOIMAGEPATH)/script/tim2img.pl
-TIMDDRTOOL	:= $(DOIMAGEPATH)/tim/ddr/ddr_tool
+TIM_UART_IMAGE	:= $$(grep "Image Filename:" -m 1 $(TIM_UART_CFG) | cut -c 17-)
+
+TIMBUILD	:= $(WTP)/script/buildtim.sh
+TIM2IMG		:= $(WTP)/script/tim2img.pl
+TIMDDRTOOL	:= $(WTP)/tim/ddr/ddr_tool
 
 $(TIMBUILD): $(TIMDDRTOOL)
 
 # WTMI_IMG is used to specify the customized RTOS image running over
 # Service CPU (CM3 processor). By the default, it points to a
 # baremetal binary of fuse programming in A3700_utils.
-WTMI_IMG	:= $(DOIMAGEPATH)/wtmi/fuse/build/fuse.bin
+WTMI_IMG	:= $(WTP)/wtmi/fuse/build/fuse.bin
 
 # WTMI_MULTI_IMG is composed of CM3 RTOS image (WTMI_IMG)
 # and sys-init image.
-WTMI_MULTI_IMG		:= $(DOIMAGEPATH)/wtmi/build/wtmi.bin
+WTMI_MULTI_IMG		:= $(WTP)/wtmi/build/wtmi.bin
 
 WTMI_ENC_IMG		:= wtmi-enc.bin
 
@@ -124,16 +125,21 @@
 BOOTDEV			?= SPINOR
 PARTNUM			?= 0
 
-TIM_IMAGE		:= $$(grep "Image Filename:" -m 1 $(DOIMAGE_CFG) | cut -c 17-)
-TIMBLDARGS		:= $(MARVELL_SECURE_BOOT) $(BOOTDEV) $(IMAGESPATH) $(DOIMAGEPATH) $(CLOCKSPRESET) \
-				$(DDR_TOPOLOGY) $(PARTNUM) $(DEBUG) $(DOIMAGE_CFG) $(TIMNCFG) $(TIMNSIG) 1
-TIMBLDUARTARGS		:= $(MARVELL_SECURE_BOOT) UART $(IMAGESPATH) $(DOIMAGEPATH) $(CLOCKSPRESET) \
-				$(DDR_TOPOLOGY) 0 0 $(DOIMAGEUART_CFG) $(TIMNUARTCFG) $(TIMNSIG) 0
+TIMBLDARGS		:= $(MARVELL_SECURE_BOOT) $(BOOTDEV) $(IMAGESPATH) $(WTP) $(CLOCKSPRESET) \
+				$(DDR_TOPOLOGY) $(PARTNUM) $(DEBUG) $(TIM_CFG) $(TIMN_CFG) $(TIMN_SIG) 1
+TIMBLDUARTARGS		:= $(MARVELL_SECURE_BOOT) UART $(IMAGESPATH) $(WTP) $(CLOCKSPRESET) \
+				$(DDR_TOPOLOGY) 0 0 $(TIM_UART_CFG) $(TIMN_UART_CFG) $(TIMN_SIG) 0
+
+UART_IMAGES		:= $(BUILD_UART)/$(TIM_UART_IMAGE)
+ifeq ($(MARVELL_SECURE_BOOT),1)
+UART_IMAGES		+= $(BUILD_UART)/$(TIMN_UART_IMAGE)
+endif
+UART_IMAGES		+= $(BUILD_UART)/wtmi_h.bin $(BUILD_UART)/boot-image_h.bin
 
 CRYPTOPP_LIBDIR		?= $(CRYPTOPP_PATH)
 CRYPTOPP_INCDIR		?= $(CRYPTOPP_PATH)
 
-$(DOIMAGETOOL): FORCE
+$(TBB): FORCE
 	$(if $(CRYPTOPP_LIBDIR),,$(error "Platform '$(PLAT)' for WTP image tool requires CRYPTOPP_PATH or CRYPTOPP_LIBDIR. Please set CRYPTOPP_PATH or CRYPTOPP_LIBDIR to point to the right directory"))
 	$(if $(CRYPTOPP_INCDIR),,$(error "Platform '$(PLAT)' for WTP image tool requires CRYPTOPP_PATH or CRYPTOPP_INCDIR. Please set CRYPTOPP_PATH or CRYPTOPP_INCDIR to point to the right directory"))
 	$(if $(wildcard $(CRYPTOPP_LIBDIR)/*),,$(error "Either 'CRYPTOPP_PATH' or 'CRYPTOPP_LIB' was set to '$(CRYPTOPP_LIBDIR)', but '$(CRYPTOPP_LIBDIR)' does not exist"))
@@ -141,10 +147,10 @@
 ifdef CRYPTOPP_PATH
 	$(Q)$(MAKE) --no-print-directory -C $(CRYPTOPP_PATH) -f GNUmakefile
 endif
-	$(Q)$(MAKE) --no-print-directory -C $(DOIMAGEPATH)/wtptp/src/TBB_Linux -f TBB_linux.mak LIBDIR=$(CRYPTOPP_LIBDIR) INCDIR=$(CRYPTOPP_INCDIR)
+	$(Q)$(MAKE) --no-print-directory -C $(WTP)/wtptp/src/TBB_Linux -f TBB_linux.mak LIBDIR=$(CRYPTOPP_LIBDIR) INCDIR=$(CRYPTOPP_INCDIR)
 
 $(WTMI_MULTI_IMG): FORCE
-	$(Q)$(MAKE) --no-print-directory -C $(DOIMAGEPATH) WTMI_IMG=$(WTMI_IMG) DDR_TOPOLOGY=$(DDR_TOPOLOGY) CLOCKSPRESET=$(CLOCKSPRESET) WTMI
+	$(Q)$(MAKE) --no-print-directory -C $(WTP) WTMI_IMG=$(WTMI_IMG) DDR_TOPOLOGY=$(DDR_TOPOLOGY) CLOCKSPRESET=$(CLOCKSPRESET) WTMI
 
 $(BUILD_PLAT)/wtmi.bin: $(WTMI_MULTI_IMG)
 	$(Q)cp -a $(WTMI_MULTI_IMG) $(BUILD_PLAT)/wtmi.bin
@@ -152,40 +158,40 @@
 $(TIMDDRTOOL): FORCE
 	$(if $(value MV_DDR_PATH),,$(error "Platform '${PLAT}' for ddr tool requires MV_DDR_PATH. Please set MV_DDR_PATH to point to the right directory"))
 	$(if $(wildcard $(value MV_DDR_PATH)/*),,$(error "'MV_DDR_PATH=$(value MV_DDR_PATH)' was specified, but '$(value MV_DDR_PATH)' directory does not exist"))
-	$(if $(shell test -s "$(value MV_DDR_PATH)/branch.txt" || git -C $(value MV_DDR_PATH) rev-parse --show-cdup 2>&1),$(error "'MV_DDR_PATH=$(value MV_DDR_PATH)' was specified, but '$(value MV_DDR_PATH)' does not contain valid Marvell mv_ddr release tarball nor git repository"))
-	$(Q)$(MAKE) --no-print-directory -C $(DOIMAGEPATH) MV_DDR_PATH=$(MV_DDR_PATH) DDR_TOPOLOGY=$(DDR_TOPOLOGY) mv_ddr
+	$(if $(shell git -C $(value MV_DDR_PATH) rev-parse --show-cdup 2>&1),$(error "'MV_DDR_PATH=$(value MV_DDR_PATH)' was specified, but '$(value MV_DDR_PATH)' does not contain valid mv-ddr-marvell git repository"))
+	$(Q)$(MAKE) --no-print-directory -C $(WTP) MV_DDR_PATH=$(MV_DDR_PATH) DDR_TOPOLOGY=$(DDR_TOPOLOGY) mv_ddr
 
-$(BUILD_PLAT)/$(UART_IMAGE): $(BUILD_PLAT)/$(BOOT_IMAGE) $(BUILD_PLAT)/wtmi.bin $(DOIMAGETOOL) $(TIMBUILD) $(TIMDDRTOOL)
+$(BUILD_PLAT)/$(UART_IMAGE): $(BUILD_PLAT)/$(BOOT_IMAGE) $(BUILD_PLAT)/wtmi.bin $(TBB) $(TIMBUILD) $(TIMDDRTOOL)
 	@$(ECHO_BLANK_LINE)
 	@echo "Building uart images"
 	$(Q)mkdir -p $(BUILD_PLAT)/$(BUILD_UART)
 	$(Q)cp -a $(BUILD_PLAT)/wtmi.bin $(BUILD_PLAT)/$(BUILD_UART)/wtmi.bin
 	$(Q)cp -a $(BUILD_PLAT)/$(BOOT_IMAGE) $(BUILD_PLAT)/$(BUILD_UART)/$(BOOT_IMAGE)
 	$(Q)cd $(BUILD_PLAT)/$(BUILD_UART) && $(TIMBUILD) $(TIMBLDUARTARGS)
-	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(DOIMAGEUART_CFG)
-	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(DOIMAGEUART_CFG)
+	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(TIM_UART_CFG)
+	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(TIM_UART_CFG)
 ifeq ($(MARVELL_SECURE_BOOT),1)
-	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(TIMNUARTCFG)
-	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(TIMNUARTCFG)
+	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(TIMN_UART_CFG)
+	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(TIMN_UART_CFG)
 endif
-	$(Q)cd $(BUILD_PLAT)/$(BUILD_UART) && $(DOIMAGETOOL) -r $(DOIMAGEUART_CFG) -v -D
+	$(Q)cd $(BUILD_PLAT)/$(BUILD_UART) && $(TBB) -r $(TIM_UART_CFG) -v -D
 ifeq ($(MARVELL_SECURE_BOOT),1)
-	$(Q)cd $(BUILD_PLAT)/$(BUILD_UART) && $(DOIMAGETOOL) -r $(TIMNUARTCFG)
+	$(Q)cd $(BUILD_PLAT)/$(BUILD_UART) && $(TBB) -r $(TIMN_UART_CFG)
 endif
-	$(Q)tar czf $(BUILD_PLAT)/$(UART_IMAGE) -C $(BUILD_PLAT) $(BUILD_UART)/$(TIM_IMAGE) $(BUILD_UART)/wtmi_h.bin $(BUILD_UART)/boot-image_h.bin
+	$(Q)tar czf $(BUILD_PLAT)/$(UART_IMAGE) -C $(BUILD_PLAT) $(UART_IMAGES)
 	@$(ECHO_BLANK_LINE)
 	@echo "Built $@ successfully"
 	@$(ECHO_BLANK_LINE)
 
-$(BUILD_PLAT)/$(FLASH_IMAGE): $(BUILD_PLAT)/$(BOOT_IMAGE) $(BUILD_PLAT)/wtmi.bin $(DOIMAGETOOL) $(TIMBUILD) $(TIMDDRTOOL) $(TIM2IMG)
+$(BUILD_PLAT)/$(FLASH_IMAGE): $(BUILD_PLAT)/$(BOOT_IMAGE) $(BUILD_PLAT)/wtmi.bin $(TBB) $(TIMBUILD) $(TIMDDRTOOL) $(TIM2IMG)
 	@$(ECHO_BLANK_LINE)
 	@echo "Building flash image"
 	$(Q)cd $(BUILD_PLAT) && $(TIMBUILD) $(TIMBLDARGS)
-	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(DOIMAGE_CFG)
-	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(DOIMAGE_CFG)
+	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(TIM_CFG)
+	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(TIM_CFG)
 ifeq ($(MARVELL_SECURE_BOOT),1)
-	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(TIMNCFG)
-	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(TIMNCFG)
+	$(Q)sed -i 's|WTMI_IMG|wtmi.bin|1' $(TIMN_CFG)
+	$(Q)sed -i 's|BOOT_IMAGE|$(BOOT_IMAGE)|1' $(TIMN_CFG)
 	@$(ECHO_BLANK_LINE)
 	@echo "=======================================================";
 	@echo "  Secure boot. Encrypting wtmi and boot-image";
@@ -203,11 +209,11 @@
 	-K `cat $(IMAGESPATH)/aes-256.txt` -nosalt \
 	-iv `cat $(IMAGESPATH)/iv.txt` -p
 endif
-	$(Q)cd $(BUILD_PLAT) && $(DOIMAGETOOL) -r $(DOIMAGE_CFG) -v -D
+	$(Q)cd $(BUILD_PLAT) && $(TBB) -r $(TIM_CFG) -v -D
 ifeq ($(MARVELL_SECURE_BOOT),1)
-	$(Q)cd $(BUILD_PLAT) && $(DOIMAGETOOL) -r $(TIMNCFG)
-	$(Q)sed -i 's|wtmi.bin|$(WTMI_ENC_IMG)|1' $(TIMNCFG)
-	$(Q)sed -i 's|$(BOOT_IMAGE)|$(BOOT_ENC_IMAGE)|1' $(TIMNCFG)
+	$(Q)cd $(BUILD_PLAT) && $(TBB) -r $(TIMN_CFG)
+	$(Q)sed -i 's|wtmi.bin|$(WTMI_ENC_IMG)|1' $(TIMN_CFG)
+	$(Q)sed -i 's|$(BOOT_IMAGE)|$(BOOT_ENC_IMAGE)|1' $(TIMN_CFG)
 endif
 	$(Q)cd $(BUILD_PLAT) && $(TIM2IMG) $(TIM2IMGARGS) -o $(BUILD_PLAT)/$(FLASH_IMAGE)
 	@$(ECHO_BLANK_LINE)
@@ -218,8 +224,8 @@
 
 .PHONY: mrvl_clean
 mrvl_clean:
-	-$(Q)$(MAKE) --no-print-directory -C $(DOIMAGEPATH) MV_DDR_PATH=$(MV_DDR_PATH) clean
-	-$(Q)$(MAKE) --no-print-directory -C $(DOIMAGEPATH)/wtptp/src/TBB_Linux -f TBB_linux.mak clean
+	-$(Q)$(MAKE) --no-print-directory -C $(WTP) MV_DDR_PATH=$(MV_DDR_PATH) clean
+	-$(Q)$(MAKE) --no-print-directory -C $(WTP)/wtptp/src/TBB_Linux -f TBB_linux.mak clean
 ifdef CRYPTOPP_PATH
 	-$(Q)$(MAKE) --no-print-directory -C $(CRYPTOPP_PATH) -f GNUmakefile clean
 endif
diff --git a/plat/marvell/armada/a3k/common/io_addr_dec.c b/plat/marvell/armada/a3k/common/io_addr_dec.c
index b27633c..fea7f81 100644
--- a/plat/marvell/armada/a3k/common/io_addr_dec.c
+++ b/plat/marvell/armada/a3k/common/io_addr_dec.c
@@ -67,17 +67,14 @@
 	mmio_write_32(MVEBU_DEC_WIN_CTRL_REG(dec_win->dec_reg_base,
 		      win_id, dec_win->win_offset), ctrl);
 
-	INFO("set_io_addr_dec %d result: ctrl(0x%x) base(0x%x)",
+	INFO("set_io_addr_dec %d result: ctrl(0x%x) base(0x%x) remap(0x%x)\n",
 	     win_id, mmio_read_32(MVEBU_DEC_WIN_CTRL_REG(dec_win->dec_reg_base,
 	     win_id, dec_win->win_offset)),
 	     mmio_read_32(MVEBU_DEC_WIN_BASE_REG(dec_win->dec_reg_base,
-			  win_id, dec_win->win_offset)));
-	if (win_id < dec_win->max_remap)
-		INFO(" remap(%x)\n",
-		     mmio_read_32(MVEBU_DEC_WIN_REMAP_REG(dec_win->dec_reg_base,
-		     win_id, dec_win->win_offset)));
-	else
-		INFO("\n");
+			  win_id, dec_win->win_offset)),
+	     (win_id < dec_win->max_remap) ?
+		mmio_read_32(MVEBU_DEC_WIN_REMAP_REG(dec_win->dec_reg_base,
+			     win_id, dec_win->win_offset)) : 0);
 }
 
 /* Set io decode window */
@@ -167,12 +164,11 @@
 			ERROR("Failed to set IO address decode\n");
 			return -1;
 		}
-		INFO("Set IO decode window successfully, base(0x%x)",
-		     io_dec_win->dec_reg_base);
-		INFO(" win_attr(%x) max_dram_win(%d) max_remap(%d)",
+		INFO("Set IO decode window successfully, base(0x%x)"
+		     " win_attr(%x) max_dram_win(%d) max_remap(%d)"
+		     " win_offset(%d)\n", io_dec_win->dec_reg_base,
 		     io_dec_win->win_attr, io_dec_win->max_dram_win,
-		     io_dec_win->max_remap);
-		INFO(" win_offset(%d)\n", io_dec_win->win_offset);
+		     io_dec_win->max_remap, io_dec_win->win_offset);
 	}
 
 	return 0;
diff --git a/plat/marvell/armada/a8k/common/a8k_common.mk b/plat/marvell/armada/a8k/common/a8k_common.mk
index 90883f2..773b912 100644
--- a/plat/marvell/armada/a8k/common/a8k_common.mk
+++ b/plat/marvell/armada/a8k/common/a8k_common.mk
@@ -82,6 +82,7 @@
 
 PLAT_INCLUDES		:=	-I$(BOARD_DIR)				\
 				-I$(BOARD_DIR)/board			\
+				-I$(CURDIR)/drivers/marvell		\
 				-I$(PLAT_COMMON_BASE)/include		\
 				-I$(PLAT_INCLUDE_BASE)/common
 
diff --git a/plat/marvell/armada/a8k/common/ble/ble.mk b/plat/marvell/armada/a8k/common/ble/ble.mk
index d6d72c1..87e2ce0 100644
--- a/plat/marvell/armada/a8k/common/ble/ble.mk
+++ b/plat/marvell/armada/a8k/common/ble/ble.mk
@@ -3,8 +3,6 @@
 # SPDX-License-Identifier:     BSD-3-Clause
 # https://spdx.org/licenses
 
-MV_DDR_PATH		?=	drivers/marvell/mv_ddr
-
 MV_DDR_LIB		=	$(BUILD_PLAT)/ble/mv_ddr_lib.a
 LIBC_LIB		=	$(BUILD_PLAT)/lib/libc.a
 BLE_LIBS		=	$(MV_DDR_LIB) $(LIBC_LIB)
@@ -18,14 +16,19 @@
 				$(PLAT_MARVELL)/common/plat_delay_timer.c	\
 				$(PLAT_MARVELL)/common/marvell_console.c
 
-PLAT_INCLUDES		+= 	-I$(MV_DDR_PATH)				\
-				-I$(CURDIR)/include				\
+MV_DDR_INCLUDES		:=	-I$(CURDIR)/include				\
 				-I$(CURDIR)/include/arch/aarch64		\
 				-I$(CURDIR)/include/lib/libc			\
-				-I$(CURDIR)/include/lib/libc/aarch64		\
-				-I$(CURDIR)/drivers/marvell
+				-I$(CURDIR)/include/lib/libc/aarch64
 
 BLE_LINKERFILE		:=	$(BLE_PATH)/ble.ld.S
 
+BLE_OBJS := $(addprefix $(BUILD_PLAT)/ble/,$(call SOURCES_TO_OBJS,$(BLE_SOURCES)))
+$(BLE_OBJS): PLAT_INCLUDES += -I$(MV_DDR_PATH)
+$(BLE_OBJS): $(MV_DDR_LIB)
+
 $(MV_DDR_LIB): FORCE
-	@+make -C $(MV_DDR_PATH) --no-print-directory PLAT_INCLUDES="$(PLAT_INCLUDES)" PLATFORM=$(PLAT) ARCH=AARCH64 OBJ_DIR=$(BUILD_PLAT)/ble
+	$(if $(value MV_DDR_PATH),,$(error "Platform '$(PLAT)' for BLE requires MV_DDR_PATH. Please set MV_DDR_PATH to point to the right directory"))
+	$(if $(wildcard $(value MV_DDR_PATH)/*),,$(error "'MV_DDR_PATH=$(value MV_DDR_PATH)' was specified, but '$(value MV_DDR_PATH)' directory does not exist"))
+	$(if $(shell git -C $(value MV_DDR_PATH) rev-parse --show-cdup 2>&1),$(error "'MV_DDR_PATH=$(value MV_DDR_PATH)' was specified, but '$(value MV_DDR_PATH)' does not contain valid mv-ddr-marvell git repository"))
+	@+make -C $(MV_DDR_PATH) --no-print-directory PLAT_INCLUDES="$(MV_DDR_INCLUDES)" PLATFORM=$(PLAT) ARCH=AARCH64 OBJ_DIR=$(BUILD_PLAT)/ble
diff --git a/plat/mediatek/mt8192/drivers/dfd/plat_dfd.c b/plat/mediatek/mt8192/drivers/dfd/plat_dfd.c
new file mode 100644
index 0000000..69c395e
--- /dev/null
+++ b/plat/mediatek/mt8192/drivers/dfd/plat_dfd.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mtk_sip_svc.h>
+#include <plat_dfd.h>
+
+static bool dfd_enabled;
+static uint64_t dfd_base_addr;
+static uint64_t dfd_chain_length;
+static uint64_t dfd_cache_dump;
+
+static void dfd_setup(uint64_t base_addr, uint64_t chain_length,
+		      uint64_t cache_dump)
+{
+	/* bit[0]: rg_rw_dfd_internal_dump_en -> 1 */
+	/* bit[2]: rg_rw_dfd_clock_stop_en -> 1 */
+	sync_writel(DFD_INTERNAL_CTL, 0x5);
+
+	/* bit[13]: xreset_b_update_disable */
+	mmio_setbits_32(DFD_INTERNAL_CTL, 0x1 << 13);
+
+	/*
+	 * bit[10:3]: DFD trigger selection mask
+	 * bit[3]: rg_rw_dfd_trigger_sel[0] = 1(enable wdt trigger)
+	 * bit[4]: rg_rw_dfd_trigger_sel[1] = 1(enable HW trigger)
+	 * bit[5]: rg_rw_dfd_trigger_sel[2] = 1(enable SW trigger)
+	 * bit[6]: rg_rw_dfd_trigger_sel[3] = 1(enable SW non-security trigger)
+	 * bit[7]: rg_rw_dfd_trigger_sel[4] = 1(enable timer trigger)
+	 */
+	mmio_setbits_32(DFD_INTERNAL_CTL, 0x1 << 3);
+
+	/* bit[20:19]: rg_dfd_armpll_div_mux_sel switch to PLL2 for DFD */
+	mmio_setbits_32(DFD_INTERNAL_CTL, 0x3 << 19);
+
+	/*
+	 * bit[0]: rg_rw_dfd_auto_power_on = 1
+	 * bit[2:1]: rg_rw_dfd_auto_power_on_dely = 1(10us)
+	 * bit[4:2]: rg_rw_dfd_power_on_wait_time = 1(20us)
+	 */
+	mmio_write_32(DFD_INTERNAL_PWR_ON, 0xB);
+
+	/* longest scan chain length */
+	mmio_write_32(DFD_CHAIN_LENGTH0, chain_length);
+
+	/* bit[1:0]: rg_rw_dfd_shift_clock_ratio */
+	mmio_write_32(DFD_INTERNAL_SHIFT_CLK_RATIO, 0x0);
+
+	/* rg_dfd_test_so_over_64 */
+	mmio_write_32(DFD_INTERNAL_TEST_SO_OVER_64, 0x1);
+
+	/* DFD3.0 */
+	mmio_write_32(DFD_TEST_SI_0, DFD_TEST_SI_0_CACHE_DIS_VAL);
+	mmio_write_32(DFD_TEST_SI_1, DFD_TEST_SI_1_VAL);
+	mmio_write_32(DFD_TEST_SI_2, DFD_TEST_SI_2_VAL);
+	mmio_write_32(DFD_TEST_SI_3, DFD_TEST_SI_3_VAL);
+
+	/* for iLDO feature */
+	sync_writel(DFD_POWER_CTL, 0xF9);
+
+	/* set base address */
+	mmio_write_32(DFD_O_SET_BASEADDR_REG, base_addr >> 24);
+
+	/*
+	 * disable sleep protect of DFD
+	 * 10001220[8]: protect_en_reg[8]
+	 * 10001a3c[2]: infra_mcu_pwr_ctl_mask[2]
+	 */
+	mmio_clrbits_32(DFD_O_PROTECT_EN_REG, 1 << 8);
+	mmio_clrbits_32(DFD_O_INTRF_MCU_PWR_CTL_MASK, 1 << 2);
+
+	/* clean DFD trigger status */
+	sync_writel(DFD_CLEAN_STATUS, 0x1);
+	sync_writel(DFD_CLEAN_STATUS, 0x0);
+
+	/* DFD-3.0 */
+	sync_writel(DFD_V30_CTL, 0x1);
+
+	/* setup global variables for suspend and resume */
+	dfd_enabled = true;
+	dfd_base_addr = base_addr;
+	dfd_chain_length = chain_length;
+	dfd_cache_dump = cache_dump;
+
+	if ((cache_dump & DFD_CACHE_DUMP_ENABLE) != 0UL) {
+		/* DFD3.5 */
+		mmio_write_32(DFD_TEST_SI_0, DFD_TEST_SI_0_CACHE_EN_VAL);
+		sync_writel(DFD_V35_ENALBE, 0x1);
+		sync_writel(DFD_V35_TAP_NUMBER, 0xB);
+		sync_writel(DFD_V35_TAP_EN, DFD_V35_TAP_EN_VAL);
+		sync_writel(DFD_V35_SEQ0_0, DFD_V35_SEQ0_0_VAL);
+
+		if (cache_dump & DFD_PARITY_ERR_TRIGGER) {
+			sync_writel(DFD_HW_TRIGGER_MASK, 0xC);
+			mmio_setbits_32(DFD_INTERNAL_CTL, 0x1 << 4);
+		}
+	}
+	dsbsy();
+}
+
+void dfd_resume(void)
+{
+	if (dfd_enabled == true) {
+		dfd_setup(dfd_base_addr, dfd_chain_length, dfd_cache_dump);
+	}
+}
+
+uint64_t dfd_smc_dispatcher(uint64_t arg0, uint64_t arg1,
+			    uint64_t arg2, uint64_t arg3)
+{
+	uint64_t ret = 0L;
+
+	switch (arg0) {
+	case PLAT_MTK_DFD_SETUP_MAGIC:
+		dfd_setup(arg1, arg2, arg3);
+		break;
+	case PLAT_MTK_DFD_READ_MAGIC:
+		/* only allow to access DFD register base + 0x200 */
+		if (arg1 <= 0x200) {
+			ret = mmio_read_32(MISC1_CFG_BASE + arg1);
+		}
+		break;
+	case PLAT_MTK_DFD_WRITE_MAGIC:
+		/* only allow to access DFD register base + 0x200 */
+		if (arg1 <= 0x200) {
+			sync_writel(MISC1_CFG_BASE + arg1, arg2);
+		}
+		break;
+	default:
+		ret = MTK_SIP_E_INVALID_PARAM;
+		break;
+	}
+
+	return ret;
+}
diff --git a/plat/mediatek/mt8192/drivers/dfd/plat_dfd.h b/plat/mediatek/mt8192/drivers/dfd/plat_dfd.h
new file mode 100644
index 0000000..7f0f4b5
--- /dev/null
+++ b/plat/mediatek/mt8192/drivers/dfd/plat_dfd.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_DFD_H
+#define PLAT_DFD_H
+
+#include <arch_helpers.h>
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define sync_writel(addr, val)	do { mmio_write_32((addr), (val)); \
+				dsbsy(); \
+				} while (0)
+
+#define PLAT_MTK_DFD_SETUP_MAGIC		(0x99716150)
+#define PLAT_MTK_DFD_READ_MAGIC			(0x99716151)
+#define PLAT_MTK_DFD_WRITE_MAGIC		(0x99716152)
+
+#define MCU_BIU_BASE				(MCUCFG_BASE)
+#define MISC1_CFG_BASE				(MCU_BIU_BASE + 0xE040)
+#define DFD_INTERNAL_CTL			(MISC1_CFG_BASE + 0x00)
+#define DFD_INTERNAL_PWR_ON			(MISC1_CFG_BASE + 0x08)
+#define DFD_CHAIN_LENGTH0			(MISC1_CFG_BASE + 0x0C)
+#define DFD_INTERNAL_SHIFT_CLK_RATIO		(MISC1_CFG_BASE + 0x10)
+#define DFD_CHAIN_LENGTH1			(MISC1_CFG_BASE + 0x1C)
+#define DFD_CHAIN_LENGTH2			(MISC1_CFG_BASE + 0x20)
+#define DFD_CHAIN_LENGTH3			(MISC1_CFG_BASE + 0x24)
+#define DFD_INTERNAL_TEST_SO_0			(MISC1_CFG_BASE + 0x28)
+#define DFD_INTERNAL_NUM_OF_TEST_SO_GROUP	(MISC1_CFG_BASE + 0x30)
+#define DFD_INTERNAL_TEST_SO_OVER_64		(MISC1_CFG_BASE + 0x34)
+#define DFD_V30_CTL				(MISC1_CFG_BASE + 0x48)
+#define DFD_V30_BASE_ADDR			(MISC1_CFG_BASE + 0x4C)
+#define DFD_POWER_CTL				(MISC1_CFG_BASE + 0x50)
+#define DFD_TEST_SI_0				(MISC1_CFG_BASE + 0x58)
+#define DFD_TEST_SI_1				(MISC1_CFG_BASE + 0x5C)
+#define DFD_CLEAN_STATUS			(MISC1_CFG_BASE + 0x60)
+#define DFD_TEST_SI_2				(MISC1_CFG_BASE + 0x1D8)
+#define DFD_TEST_SI_3				(MISC1_CFG_BASE + 0x1DC)
+#define DFD_HW_TRIGGER_MASK			(MISC1_CFG_BASE + 0xBC)
+
+#define DFD_V35_ENALBE				(MCU_BIU_BASE + 0xE0A8)
+#define DFD_V35_TAP_NUMBER			(MCU_BIU_BASE + 0xE0AC)
+#define DFD_V35_TAP_EN				(MCU_BIU_BASE + 0xE0B0)
+#define DFD_V35_CTL				(MCU_BIU_BASE + 0xE0B4)
+#define DFD_V35_SEQ0_0				(MCU_BIU_BASE + 0xE0C0)
+#define DFD_V35_SEQ0_1				(MCU_BIU_BASE + 0xE0C4)
+
+#define DFD_O_PROTECT_EN_REG			(0x10001220)
+#define DFD_O_INTRF_MCU_PWR_CTL_MASK		(0x10001A3C)
+#define DFD_O_SET_BASEADDR_REG			(0x10043034)
+
+#define DFD_CACHE_DUMP_ENABLE			1U
+#define DFD_PARITY_ERR_TRIGGER			2U
+
+#define DFD_TEST_SI_0_CACHE_DIS_VAL		(0x1E000202)
+#define DFD_TEST_SI_0_CACHE_EN_VAL		(0x1E000002)
+#define DFD_TEST_SI_1_VAL			(0x20408100)
+#define DFD_TEST_SI_2_VAL			(0x10101000)
+#define DFD_TEST_SI_3_VAL			(0x00000010)
+#define	DFD_V35_TAP_EN_VAL			(0x43FF)
+#define	DFD_V35_SEQ0_0_VAL			(0x63668820)
+
+void dfd_resume(void);
+uint64_t dfd_smc_dispatcher(uint64_t arg0, uint64_t arg1,
+			    uint64_t arg2, uint64_t arg3);
+
+#endif /* PLAT_DFD_H */
diff --git a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c
index 307862d..2d67fdf 100644
--- a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c
+++ b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c
@@ -143,6 +143,11 @@
 		blocked |= SPM_COND_CHECK_BLOCKED_PLL;
 	}
 
+	if (is_system_suspend && (blocked != 0U)) {
+		INFO("suspend: %s total blocked = 0x%08x\n",
+		     dest->name, blocked);
+	}
+
 	return blocked;
 }
 
diff --git a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h
index ba13fe3..91ebdd9 100644
--- a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h
+++ b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h
@@ -23,20 +23,11 @@
 	PLAT_SPM_COND_MAX,
 };
 
-enum PLAT_SPM_COND_PLL {
-	PLAT_SPM_COND_PLL_UNIVPLL = 0,
-	PLAT_SPM_COND_PLL_MFGPLL,
-	PLAT_SPM_COND_PLL_MSDCPLL,
-	PLAT_SPM_COND_PLL_TVDPLL,
-	PLAT_SPM_COND_PLL_MMPLL,
-	PLAT_SPM_COND_PLL_MAX,
-};
-
-#define PLL_BIT_MFGPLL	(PLAT_SPM_COND_PLL_MFGPLL)
-#define PLL_BIT_MMPLL	(PLAT_SPM_COND_PLL_MMPLL)
-#define PLL_BIT_UNIVPLL	(PLAT_SPM_COND_PLL_UNIVPLL)
-#define PLL_BIT_MSDCPLL	(PLAT_SPM_COND_PLL_MSDCPLL)
-#define PLL_BIT_TVDPLL	(PLAT_SPM_COND_PLL_TVDPLL)
+#define PLL_BIT_UNIVPLL	BIT(0)
+#define PLL_BIT_MFGPLL	BIT(1)
+#define PLL_BIT_MSDCPLL	BIT(2)
+#define PLL_BIT_TVDPLL	BIT(3)
+#define PLL_BIT_MMPLL	BIT(4)
 
 /* Definition about SPM_COND_CHECK_BLOCKED
  * bit [00 ~ 15]: cg blocking index
diff --git a/plat/mediatek/mt8192/include/plat_sip_calls.h b/plat/mediatek/mt8192/include/plat_sip_calls.h
index 0e42322..f68a4ea 100644
--- a/plat/mediatek/mt8192/include/plat_sip_calls.h
+++ b/plat/mediatek/mt8192/include/plat_sip_calls.h
@@ -10,6 +10,10 @@
 /*******************************************************************************
  * Plat SiP function constants
  ******************************************************************************/
-#define MTK_PLAT_SIP_NUM_CALLS    0
+#define MTK_PLAT_SIP_NUM_CALLS    2
+
+/* DFD */
+#define MTK_SIP_KERNEL_DFD_AARCH32		0x82000205
+#define MTK_SIP_KERNEL_DFD_AARCH64		0xC2000205
 
 #endif /* PLAT_SIP_CALLS_H */
diff --git a/plat/mediatek/mt8192/plat_pm.c b/plat/mediatek/mt8192/plat_pm.c
index 6dfb6c9..018e418 100644
--- a/plat/mediatek/mt8192/plat_pm.c
+++ b/plat/mediatek/mt8192/plat_pm.c
@@ -17,6 +17,7 @@
 #include <mtk_ptp3_common.h>
 #include <mtspmc.h>
 #include <plat/common/platform.h>
+#include <plat_dfd.h>
 #include <plat_mtk_lpm.h>
 #include <plat_params.h>
 #include <plat_pm.h>
@@ -168,6 +169,8 @@
 	mt_gic_distif_restore();
 	gic_sgi_restore_all();
 
+	dfd_resume();
+
 	plat_mt_pm_invoke_no_check(pwr_mcusys_on_finished, cpu, state);
 }
 
diff --git a/plat/mediatek/mt8192/plat_sip_calls.c b/plat/mediatek/mt8192/plat_sip_calls.c
index f567f02..353faf8 100644
--- a/plat/mediatek/mt8192/plat_sip_calls.c
+++ b/plat/mediatek/mt8192/plat_sip_calls.c
@@ -9,6 +9,7 @@
 #include <mtk_apusys.h>
 #include <mtk_sip_svc.h>
 #include <mt_spm_vcorefs.h>
+#include <plat_dfd.h>
 #include "plat_sip_calls.h"
 
 uintptr_t mediatek_plat_sip_handler(uint32_t smc_fid,
@@ -34,6 +35,11 @@
 		ret = apusys_kernel_ctrl(x1, x2, x3, x4, &rnd_val0);
 		SMC_RET2(handle, ret, rnd_val0);
 		break;
+	case MTK_SIP_KERNEL_DFD_AARCH32:
+	case MTK_SIP_KERNEL_DFD_AARCH64:
+		ret = dfd_smc_dispatcher(x1, x2, x3, x4);
+		SMC_RET1(handle, ret);
+		break;
 	default:
 		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
 		break;
diff --git a/plat/mediatek/mt8192/platform.mk b/plat/mediatek/mt8192/platform.mk
index 7761a55..cbdaadd 100644
--- a/plat/mediatek/mt8192/platform.mk
+++ b/plat/mediatek/mt8192/platform.mk
@@ -19,6 +19,7 @@
                  -I${MTK_PLAT_SOC}/drivers/apusys/                \
                  -I${MTK_PLAT_SOC}/drivers/dcm                    \
                  -I${MTK_PLAT_SOC}/drivers/devapc                 \
+                 -I${MTK_PLAT_SOC}/drivers/dfd                    \
                  -I${MTK_PLAT_SOC}/drivers/emi_mpu/               \
                  -I${MTK_PLAT_SOC}/drivers/gpio/                  \
                  -I${MTK_PLAT_SOC}/drivers/mcdi/                  \
@@ -68,6 +69,7 @@
                    ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm.c                 \
                    ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm_utils.c           \
                    ${MTK_PLAT_SOC}/drivers/devapc/devapc.c               \
+                   ${MTK_PLAT_SOC}/drivers/dfd/plat_dfd.c                \
                    ${MTK_PLAT_SOC}/drivers/emi_mpu/emi_mpu.c             \
                    ${MTK_PLAT_SOC}/drivers/gpio/mtgpio.c                 \
                    ${MTK_PLAT_SOC}/drivers/mcdi/mt_cpu_pm.c              \
diff --git a/plat/mediatek/mt8195/bl31_plat_setup.c b/plat/mediatek/mt8195/bl31_plat_setup.c
index 39c1918..8745454 100644
--- a/plat/mediatek/mt8195/bl31_plat_setup.c
+++ b/plat/mediatek/mt8195/bl31_plat_setup.c
@@ -19,6 +19,7 @@
 #include <mt_gic_v3.h>
 #include <mt_spm.h>
 #include <mt_timer.h>
+#include <mtk_dcm.h>
 #include <mtgpio.h>
 #include <plat_params.h>
 #include <plat_private.h>
@@ -84,6 +85,11 @@
  ******************************************************************************/
 void bl31_platform_setup(void)
 {
+	/* Set dcm on */
+	if (!dcm_set_default()) {
+		ERROR("Failed to set default dcm on!!\n");
+	}
+
 	/* Initialize the GIC driver, CPU and distributor interfaces */
 	mt_gic_driver_init();
 	mt_gic_init();
diff --git a/plat/mediatek/mt8195/drivers/dcm/mtk_dcm.c b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm.c
new file mode 100644
index 0000000..aed0833
--- /dev/null
+++ b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mtk_dcm.h>
+#include <mtk_dcm_utils.h>
+
+static void dcm_armcore(bool mode)
+{
+	dcm_mp_cpusys_top_bus_pll_div_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_0_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_1_dcm(mode);
+}
+
+static void dcm_mcusys(bool on)
+{
+	dcm_mp_cpusys_top_adb_dcm(on);
+	dcm_mp_cpusys_top_apb_dcm(on);
+	dcm_mp_cpusys_top_cpubiu_dcm(on);
+	dcm_mp_cpusys_top_misc_dcm(on);
+	dcm_mp_cpusys_top_mp0_qdcm(on);
+	dcm_cpccfg_reg_emi_wfifo(on);
+	dcm_mp_cpusys_top_last_cor_idle_dcm(on);
+}
+
+static void dcm_stall(bool on)
+{
+	dcm_mp_cpusys_top_core_stall_dcm(on);
+	dcm_mp_cpusys_top_fcm_stall_dcm(on);
+}
+
+static bool check_dcm_state(void)
+{
+	bool ret = true;
+
+	ret &= dcm_mp_cpusys_top_bus_pll_div_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_adb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_apb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpubiu_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_misc_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_mp0_qdcm_is_on();
+	ret &= dcm_cpccfg_reg_emi_wfifo_is_on();
+	ret &= dcm_mp_cpusys_top_last_cor_idle_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_core_stall_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_fcm_stall_dcm_is_on();
+
+	return ret;
+}
+
+bool dcm_set_default(void)
+{
+	dcm_armcore(true);
+	dcm_mcusys(true);
+	dcm_stall(true);
+
+	return check_dcm_state();
+}
diff --git a/plat/mediatek/mt8195/drivers/dcm/mtk_dcm.h b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm.h
new file mode 100644
index 0000000..cb65b85
--- /dev/null
+++ b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_H
+#define MTK_DCM_H
+
+#include <stdbool.h>
+
+bool dcm_set_default(void);
+
+#endif /* #ifndef MTK_DCM_H */
diff --git a/plat/mediatek/mt8195/drivers/dcm/mtk_dcm_utils.c b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm_utils.c
new file mode 100644
index 0000000..a1a3720
--- /dev/null
+++ b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm_utils.c
@@ -0,0 +1,483 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mtk_dcm_utils.h>
+
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_MASK (BIT(17))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_MASK (BIT(15) | \
+			BIT(16) | \
+			BIT(17) | \
+			BIT(18) | \
+			BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG2_MASK (BIT(15) | \
+			BIT(16) | \
+			BIT(17) | \
+			BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_ON (BIT(17))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_ON (BIT(15) | \
+			BIT(16) | \
+			BIT(17) | \
+			BIT(18) | \
+			BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG2_ON (BIT(15) | \
+			BIT(16) | \
+			BIT(17) | \
+			BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_OFF ((0x0 << 17))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_OFF ((0x0 << 15) | \
+			(0x0 << 16) | \
+			(0x0 << 17) | \
+			(0x0 << 18) | \
+			(0x0 << 21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG2_OFF ((0x0 << 15) | \
+			(0x0 << 16) | \
+			(0x0 << 17) | \
+			(0x0 << 18))
+
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG0) &
+		MP_CPUSYS_TOP_ADB_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4) &
+		MP_CPUSYS_TOP_ADB_DCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0) &
+		MP_CPUSYS_TOP_ADB_DCM_REG2_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG2_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_adb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG2_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG2_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_APB_DCM_REG0_MASK (BIT(5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_MASK (BIT(8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_MASK (BIT(16))
+#define MP_CPUSYS_TOP_APB_DCM_REG0_ON (BIT(5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_ON (BIT(8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_ON (BIT(16))
+#define MP_CPUSYS_TOP_APB_DCM_REG0_OFF ((0x0 << 5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_OFF ((0x0 << 8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_OFF ((0x0 << 16))
+
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG2_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_apb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG1_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG2_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK (BIT(11) | \
+			BIT(24) | \
+			BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON (BIT(11) | \
+			BIT(24) | \
+			BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF ((0x0 << 11) | \
+			(0x0 << 24) | \
+			(0x0 << 25))
+
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG) &
+		MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK (BIT(0))
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON (BIT(0))
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF ((0x0 << 0))
+
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG7) &
+		MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_core_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK ((0xffff << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON ((0xffff << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF ((0x0 << 0))
+
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCSIC_DCM0) &
+		MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK (BIT(24) | \
+			BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON (BIT(24) | \
+			BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF ((0x0 << 24) | \
+			(0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0) &
+		MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK (BIT(24) | \
+			BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON (BIT(24) | \
+			BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF ((0x0 << 24) | \
+			(0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1) &
+		MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK (BIT(4))
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON (BIT(4))
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF ((0x0 << 4))
+
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG7) &
+		MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK ((0x1U << 31))
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON ((0x1U << 31))
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF ((0x0U << 31))
+
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG) &
+		MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_MASK (BIT(1) | \
+			BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_ON (BIT(1) | \
+			BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_OFF ((0x0 << 1) | \
+			(0x0 << 4))
+
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_MISC_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_misc_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK (BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK (BIT(0) | \
+			BIT(1) | \
+			BIT(2) | \
+			BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_ON (BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG1_ON (BIT(0) | \
+			BIT(1) | \
+			BIT(2) | \
+			BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF ((0x0 << 3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG1_OFF ((0x0 << 0) | \
+			(0x0 << 1) | \
+			(0x0 << 2) | \
+			(0x0 << 3))
+
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG0) &
+		MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MP0_QDCM_REG1_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_mp0_qdcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG1_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG1_OFF);
+	}
+}
+
+#define CPCCFG_REG_EMI_WFIFO_REG0_MASK (BIT(0) | \
+			BIT(1) | \
+			BIT(2) | \
+			BIT(3))
+#define CPCCFG_REG_EMI_WFIFO_REG0_ON (BIT(0) | \
+			BIT(1) | \
+			BIT(2) | \
+			BIT(3))
+#define CPCCFG_REG_EMI_WFIFO_REG0_OFF ((0x0 << 0) | \
+			(0x0 << 1) | \
+			(0x0 << 2) | \
+			(0x0 << 3))
+
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(CPCCFG_REG_EMI_WFIFO) &
+		CPCCFG_REG_EMI_WFIFO_REG0_MASK) ==
+		(unsigned int) CPCCFG_REG_EMI_WFIFO_REG0_ON);
+
+	return ret;
+}
+
+void dcm_cpccfg_reg_emi_wfifo(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+			CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			CPCCFG_REG_EMI_WFIFO_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+			CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			CPCCFG_REG_EMI_WFIFO_REG0_OFF);
+	}
+}
diff --git a/plat/mediatek/mt8195/drivers/dcm/mtk_dcm_utils.h b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm_utils.h
new file mode 100644
index 0000000..e5743af
--- /dev/null
+++ b/plat/mediatek/mt8195/drivers/dcm/mtk_dcm_utils.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_UTILS_H
+#define MTK_DCM_UTILS_H
+
+#include <stdbool.h>
+
+#include <mtk_dcm.h>
+#include <platform_def.h>
+
+/* Base */
+#define MP_CPUSYS_TOP_BASE	(MCUCFG_BASE + 0x8000)
+#define CPCCFG_REG_BASE		(MCUCFG_BASE + 0xA800)
+
+/* Register Definition */
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG0 (MP_CPUSYS_TOP_BASE + 0x22a0)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG1 (MP_CPUSYS_TOP_BASE + 0x22a4)
+#define MP_CPUSYS_TOP_BUS_PLLDIV_CFG (MP_CPUSYS_TOP_BASE + 0x22e0)
+#define MP_CPUSYS_TOP_MCSIC_DCM0 (MP_CPUSYS_TOP_BASE + 0x2440)
+#define MP_CPUSYS_TOP_MP_ADB_DCM_CFG0 (MP_CPUSYS_TOP_BASE + 0x2500)
+#define MP_CPUSYS_TOP_MP_ADB_DCM_CFG4 (MP_CPUSYS_TOP_BASE + 0x2510)
+#define MP_CPUSYS_TOP_MP_MISC_DCM_CFG0 (MP_CPUSYS_TOP_BASE + 0x2518)
+#define MP_CPUSYS_TOP_MCUSYS_DCM_CFG0 (MP_CPUSYS_TOP_BASE + 0x25c0)
+#define CPCCFG_REG_EMI_WFIFO (CPCCFG_REG_BASE + 0x100)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG0 (MP_CPUSYS_TOP_BASE + 0x4880)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG7 (MP_CPUSYS_TOP_BASE + 0x489c)
+
+/* MP_CPUSYS_TOP */
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void);
+void dcm_mp_cpusys_top_adb_dcm(bool on);
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void);
+void dcm_mp_cpusys_top_apb_dcm(bool on);
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void);
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on);
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_core_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on);
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void);
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on);
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void);
+void dcm_mp_cpusys_top_misc_dcm(bool on);
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void);
+void dcm_mp_cpusys_top_mp0_qdcm(bool on);
+/* CPCCFG_REG */
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void);
+void dcm_cpccfg_reg_emi_wfifo(bool on);
+
+#endif
diff --git a/plat/mediatek/mt8195/platform.mk b/plat/mediatek/mt8195/platform.mk
index 481e9ad..f4604c4 100644
--- a/plat/mediatek/mt8195/platform.mk
+++ b/plat/mediatek/mt8195/platform.mk
@@ -14,7 +14,8 @@
                  -I${MTK_PLAT}/common/drivers/timer/              \
                  -I${MTK_PLAT}/common/drivers/uart/               \
                  -I${MTK_PLAT}/common/lpm/                        \
-                 -I${MTK_PLAT_SOC}/drivers/dp/                  \
+                 -I${MTK_PLAT_SOC}/drivers/dcm                    \
+                 -I${MTK_PLAT_SOC}/drivers/dp/                    \
                  -I${MTK_PLAT_SOC}/drivers/gpio/                  \
                  -I${MTK_PLAT_SOC}/drivers/mcdi/                  \
                  -I${MTK_PLAT_SOC}/drivers/pmic/                  \
@@ -56,6 +57,8 @@
                 ${MTK_PLAT_SOC}/aarch64/platform_common.c             \
                 ${MTK_PLAT_SOC}/aarch64/plat_helpers.S                \
                 ${MTK_PLAT_SOC}/bl31_plat_setup.c                     \
+                ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm.c                 \
+                ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm_utils.c           \
                 ${MTK_PLAT_SOC}/drivers/dp/mt_dp.c                    \
                 ${MTK_PLAT_SOC}/drivers/gpio/mtgpio.c                 \
                 ${MTK_PLAT_SOC}/drivers/mcdi/mt_cpu_pm.c              \
diff --git a/plat/nxp/soc-lx2160a/soc.def b/plat/nxp/soc-lx2160a/soc.def
index bd0dd15..24d1d13 100644
--- a/plat/nxp/soc-lx2160a/soc.def
+++ b/plat/nxp/soc-lx2160a/soc.def
@@ -91,7 +91,7 @@
 # Location of BL2 on OCRAM
 BL2_BASE_ADDR	:=	$(shell echo $$(( $(OCRAM_START_ADDR) + $(NXP_ROM_RSVD) + $(CSF_HDR_SZ) )))
 # Covert to HEX to be used by create_pbl.mk
-BL2_BASE	:=	$$(echo "obase=16; ${BL2_BASE_ADDR}" | bc)
+BL2_BASE	:=	$(shell echo "0x"$$(echo "obase=16; ${BL2_BASE_ADDR}" | bc))
 
 # BL2_HDR_LOC is at  (OCRAM_ADDR + NXP_ROM_RSVD)
 # This value BL2_HDR_LOC + CSF_HDR_SZ should not overalp with BL2_BASE
@@ -107,95 +107,5 @@
 # SoC Errata
 ERRATA_SOC_A050426	:= 1
 
-ifneq (${CACHE_LINE},)
-$(eval $(call add_define_val,PLATFORM_CACHE_LINE_SHIFT,${CACHE_LINE}))
-$(eval CACHE_WRITEBACK_GRANULE=$(shell echo $$((1 << $(CACHE_LINE)))))
-$(eval $(call add_define_val,CACHE_WRITEBACK_GRANULE,$(CACHE_WRITEBACK_GRANULE)))
-endif
-
-ifneq (${INTERCONNECT},)
-$(eval $(call add_define,NXP_HAS_CCN508))
-endif
-
-ifneq (${CHASSIS},)
-$(eval $(call add_define,CONFIG_CHASSIS_${CHASSIS}))
-endif
-
-ifneq (${PLAT_DDR_PHY},)
-$(eval $(call add_define,NXP_DDR_${PLAT_DDR_PHY}))
-endif
-
-ifneq (${PHYS_SYS},)
-$(eval $(call add_define,CONFIG_PHYS_64BIT))
-endif
-
-ifneq (${CSF_HDR_SZ},)
-$(eval $(call add_define_val,CSF_HDR_SZ,${CSF_HDR_SZ}))
-endif
-
-ifneq (${OCRAM_START_ADDR},)
-$(eval $(call add_define_val,NXP_OCRAM_ADDR,${OCRAM_START_ADDR}))
-endif
-
-ifneq (${OCRAM_SIZE},)
-$(eval $(call add_define_val,NXP_OCRAM_SIZE,${OCRAM_SIZE}))
-endif
-
-ifneq (${NXP_ROM_RSVD},)
-$(eval $(call add_define_val,NXP_ROM_RSVD,${NXP_ROM_RSVD}))
-endif
-
-ifneq (${BL2_BASE_ADDR},)
-$(eval $(call add_define_val,BL2_BASE,${BL2_BASE_ADDR}))
-endif
-
-ifeq (${SEC_MEM_NON_COHERENT},yes)
-$(eval $(call add_define,SEC_MEM_NON_COHERENT))
-endif
-
-ifneq (${NXP_ESDHC_ENDIANNESS},)
-$(eval $(call add_define,NXP_ESDHC_${NXP_ESDHC_ENDIANNESS}))
-endif
-
-ifneq (${NXP_SFP_VER},)
-$(eval $(call add_define,NXP_SFP_VER_${NXP_SFP_VER}))
-endif
-
-ifneq (${NXP_SFP_ENDIANNESS},)
-$(eval $(call add_define,NXP_SFP_${NXP_SFP_ENDIANNESS}))
-endif
-
-ifneq (${NXP_GPIO_ENDIANNESS},)
-$(eval $(call add_define,NXP_GPIO_${NXP_GPIO_ENDIANNESS}))
-endif
-
-ifneq (${NXP_SNVS_ENDIANNESS},)
-$(eval $(call add_define,NXP_SNVS_${NXP_SNVS_ENDIANNESS}))
-endif
-
-ifneq (${NXP_GUR_ENDIANNESS},)
-$(eval $(call add_define,NXP_GUR_${NXP_GUR_ENDIANNESS}))
-endif
-
-ifneq (${NXP_FSPI_ENDIANNESS},)
-$(eval $(call add_define,NXP_FSPI_${NXP_FSPI_ENDIANNESS}))
-endif
-
 # enable dynamic memory mapping
 PLAT_XLAT_TABLES_DYNAMIC :=	1
-
-ifneq (${NXP_SEC_ENDIANNESS},)
-$(eval $(call add_define,NXP_SEC_${NXP_SEC_ENDIANNESS}))
-endif
-
-ifneq (${NXP_DDR_ENDIANNESS},)
-$(eval $(call add_define,NXP_DDR_${NXP_DDR_ENDIANNESS}))
-endif
-
-ifneq (${NXP_DDR_INTLV_256B},)
-$(eval $(call add_define,NXP_DDR_INTLV_256B))
-endif
-
-ifneq (${PLAT_XLAT_TABLES_DYNAMIC},)
-$(eval $(call add_define,PLAT_XLAT_TABLES_DYNAMIC))
-endif
diff --git a/plat/nxp/soc-lx2160a/soc.mk b/plat/nxp/soc-lx2160a/soc.mk
index b9649b4..8ab1430 100644
--- a/plat/nxp/soc-lx2160a/soc.mk
+++ b/plat/nxp/soc-lx2160a/soc.mk
@@ -15,7 +15,7 @@
 
  # get SoC-specific defnitions
 include ${PLAT_SOC_PATH}/soc.def
-
+include ${PLAT_COMMON_PATH}/plat_make_helper/soc_common_def.mk
 include ${PLAT_COMMON_PATH}/plat_make_helper/plat_build_macros.mk
 
  # SoC-specific
diff --git a/plat/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index fbcaa63..0891d80 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h
@@ -81,7 +81,7 @@
 #define SEC_ROM_SIZE			0x00020000
 
 #define NS_DRAM0_BASE			0x40000000
-#define NS_DRAM0_SIZE			0x3de00000
+#define NS_DRAM0_SIZE			0xc0000000
 
 #define SEC_SRAM_BASE			0x0e000000
 #define SEC_SRAM_SIZE			0x00060000
diff --git a/plat/rockchip/rk3399/drivers/dram/dram.h b/plat/rockchip/rk3399/drivers/dram/dram.h
index 0eb12cf..5572b16 100644
--- a/plat/rockchip/rk3399/drivers/dram/dram.h
+++ b/plat/rockchip/rk3399/drivers/dram/dram.h
@@ -149,7 +149,7 @@
 	uint32_t rx_cal_dqs[2][4];
 };
 
-extern __sramdata struct rk3399_sdram_params sdram_config;
+extern struct rk3399_sdram_params sdram_config;
 
 void dram_init(void);
 
diff --git a/plat/rockchip/rk3399/drivers/dram/suspend.c b/plat/rockchip/rk3399/drivers/dram/suspend.c
index 7f9fad1..a8b1c32 100644
--- a/plat/rockchip/rk3399/drivers/dram/suspend.c
+++ b/plat/rockchip/rk3399/drivers/dram/suspend.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -49,6 +49,7 @@
 
 __pmusramdata uint32_t dpll_data[PLL_CON_COUNT];
 __pmusramdata uint32_t cru_clksel_con6;
+__pmusramdata uint8_t pmu_enable_watchdog0;
 
 /*
  * Copy @num registers from @src to @dst
@@ -562,8 +563,14 @@
 
 	/* LPDDR4 f2 cann't do training, all training will fail */
 	for (ch = 0; ch < ch_count; ch++) {
-		mmio_clrsetbits_32(PHY_REG(ch, 896), (0x3 << 8) | 1,
-				   fn << 8);
+		/*
+		 * Without this disabled for LPDDR4 we end up writing 0's
+		 * in place of real data in an interesting pattern.
+		 */
+		if (sdram_params->dramtype != LPDDR4) {
+			mmio_clrsetbits_32(PHY_REG(ch, 896), (0x3 << 8) | 1,
+					fn << 8);
+		}
 
 		/* data_training failed */
 		if (data_training(ch, sdram_params, PI_FULL_TRAINING))
@@ -748,13 +755,44 @@
 	phy_regs->phy896[0] &= ~(0x3 << 8);
 }
 
+__pmusramfunc void phy_dll_bypass_set(uint32_t ch, uint32_t freq)
+{
+	if (freq <= (125 * 1000 * 1000)) {
+		/* Set master mode to SW for slices*/
+		mmio_setbits_32(PHY_REG(ch, 86), 3 << 10);
+		mmio_setbits_32(PHY_REG(ch, 214), 3 << 10);
+		mmio_setbits_32(PHY_REG(ch, 342), 3 << 10);
+		mmio_setbits_32(PHY_REG(ch, 470), 3 << 10);
+		/* Set master mode to SW for address slices*/
+		mmio_setbits_32(PHY_REG(ch, 547), 3 << 18);
+		mmio_setbits_32(PHY_REG(ch, 675), 3 << 18);
+		mmio_setbits_32(PHY_REG(ch, 803), 3 << 18);
+	} else {
+		/* Clear SW master mode for slices*/
+		mmio_clrbits_32(PHY_REG(ch, 86), 3 << 10);
+		mmio_clrbits_32(PHY_REG(ch, 214), 3 << 10);
+		mmio_clrbits_32(PHY_REG(ch, 342), 3 << 10);
+		mmio_clrbits_32(PHY_REG(ch, 470), 3 << 10);
+		/* Clear SW master mode for address slices*/
+		mmio_clrbits_32(PHY_REG(ch, 547), 3 << 18);
+		mmio_clrbits_32(PHY_REG(ch, 675), 3 << 18);
+		mmio_clrbits_32(PHY_REG(ch, 803), 3 << 18);
+	}
+}
+
 __pmusramfunc void dmc_resume(void)
 {
 	struct rk3399_sdram_params *sdram_params = &sdram_config;
 	uint32_t channel_mask = 0;
 	uint32_t channel;
 
-	pmusram_enable_watchdog();
+	/*
+	 * We can't turn off the watchdog, so if we have not turned it on before
+	 * we should not turn it on here.
+	 */
+	if ((pmu_enable_watchdog0 & 0x1) == 0x1) {
+		pmusram_enable_watchdog();
+	}
 	pmu_sgrf_rst_hld_release();
 	restore_pmu_rsthold();
 	sram_secure_timer_init();
@@ -772,6 +810,13 @@
 retry:
 	for (channel = 0; channel < sdram_params->num_channels; channel++) {
 		phy_pctrl_reset(channel);
+		/*
+		 * Without this, LPDDR4 will write 0's in place of real data
+		 * in a strange pattern.
+		 */
+		if (sdram_params->dramtype == LPDDR4) {
+			phy_dll_bypass_set(channel, sdram_params->ddr_freq);
+		}
 		pctl_cfg(channel, sdram_params);
 	}
 
@@ -788,8 +833,12 @@
 		if (sdram_params->dramtype == LPDDR3)
 			sram_udelay(10);
 
-		/* If traning fail, retry to do it again. */
-		if (data_training(channel, sdram_params, PI_FULL_TRAINING))
+		/*
+		 * Training here will always fail for LPDDR4, so skip it
+		 * If traning fail, retry to do it again.
+		 */
+		if (sdram_params->dramtype != LPDDR4 &&
+		    data_training(channel, sdram_params, PI_FULL_TRAINING))
 			goto retry;
 
 		set_ddrconfig(sdram_params, channel,
diff --git a/plat/rockchip/rk3399/drivers/dram/suspend.h b/plat/rockchip/rk3399/drivers/dram/suspend.h
index b99a926..1389944 100644
--- a/plat/rockchip/rk3399/drivers/dram/suspend.h
+++ b/plat/rockchip/rk3399/drivers/dram/suspend.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #ifndef SUSPEND_H
 #define SUSPEND_H
 
+#include <stdint.h>
 #include <dram.h>
 
 #define KHz (1000)
@@ -22,5 +23,6 @@
 
 void dmc_suspend(void);
 __pmusramfunc void dmc_resume(void);
+extern __pmusramdata uint8_t pmu_enable_watchdog0;
 
 #endif /* SUSPEND_H */
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.c b/plat/rockchip/rk3399/drivers/pmu/pmu.c
index faee678..3084c4f 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -1324,6 +1324,7 @@
 		store_wdt0[i] = mmio_read_32(WDT0_BASE + i * 4);
 		store_wdt1[i] = mmio_read_32(WDT1_BASE + i * 4);
 	}
+	pmu_enable_watchdog0 = (uint8_t) store_wdt0[0] & 0x1;
 }
 
 void wdt_register_restore(void)
diff --git a/plat/rpi/rpi4/platform.mk b/plat/rpi/rpi4/platform.mk
index 99d51fb..528eb1d 100644
--- a/plat/rpi/rpi4/platform.mk
+++ b/plat/rpi/rpi4/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -86,6 +86,9 @@
 # Use normal memory mapping for ROM, FIP, SRAM and DRAM
 RPI3_USE_UEFI_MAP		:= 0
 
+# SMCCC PCI support (should be enabled for ACPI builds)
+SMC_PCI_SUPPORT            	:= 0
+
 # Process platform flags
 # ----------------------
 
@@ -96,6 +99,7 @@
 endif
 $(eval $(call add_define,RPI3_RUNTIME_UART))
 $(eval $(call add_define,RPI3_USE_UEFI_MAP))
+$(eval $(call add_define,SMC_PCI_SUPPORT))
 
 ifeq (${ARCH},aarch32)
   $(error Error: AArch32 not supported on rpi4)
@@ -105,3 +109,8 @@
 PLAT_BL_COMMON_SOURCES	+=	drivers/rpi3/rng/rpi3_rng.c		\
 				plat/rpi/common/rpi3_stack_protector.c
 endif
+
+ifeq ($(SMC_PCI_SUPPORT), 1)
+BL31_SOURCES            +=      plat/rpi/rpi4/rpi4_pci_svc.c
+endif
+
diff --git a/plat/rpi/rpi4/rpi4_pci_svc.c b/plat/rpi/rpi4/rpi4_pci_svc.c
new file mode 100644
index 0000000..7d1ca5c
--- /dev/null
+++ b/plat/rpi/rpi4/rpi4_pci_svc.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * The RPi4 has a single nonstandard PCI config region. It is broken into two
+ * pieces, the root port config registers and a window to a single device's
+ * config space which can move between devices. There isn't (yet) an
+ * authoritative public document on this since the available BCM2711 reference
+ * notes that there is a PCIe root port in the memory map but doesn't describe
+ * it. Given that it's not ECAM compliant yet reasonably simple, it makes for
+ * an excellent example of the PCI SMCCC interface.
+ *
+ * The PCI SMCCC interface is described in DEN0115 availabe from:
+ * https://developer.arm.com/documentation/den0115/latest
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/pmf/pmf.h>
+#include <lib/runtime_instr.h>
+#include <services/pci_svc.h>
+#include <services/sdei.h>
+#include <services/std_svc.h>
+#include <smccc_helpers.h>
+
+#include <lib/mmio.h>
+
+static spinlock_t pci_lock;
+
+#define PCIE_REG_BASE		U(RPI_IO_BASE + 0x01500000)
+#define PCIE_MISC_PCIE_STATUS	0x4068
+#define PCIE_EXT_CFG_INDEX	0x9000
+/* A small window pointing at the ECAM of the device selected by CFG_INDEX */
+#define PCIE_EXT_CFG_DATA	0x8000
+#define INVALID_PCI_ADDR	0xFFFFFFFF
+
+#define	PCIE_EXT_BUS_SHIFT	20
+#define	PCIE_EXT_DEV_SHIFT	15
+#define	PCIE_EXT_FUN_SHIFT	12
+
+
+static uint64_t pci_segment_lib_get_base(uint32_t address, uint32_t offset)
+{
+	uint64_t	base;
+	uint32_t	bus, dev, fun;
+	uint32_t	status;
+
+	base = PCIE_REG_BASE;
+
+	offset &= PCI_OFFSET_MASK;  /* Pick off the 4k register offset */
+
+	/* The root port is at the base of the PCIe register space */
+	if (address != 0U) {
+		/*
+		 * The current device must be at CFG_DATA, a 4K window mapped,
+		 * via CFG_INDEX, to the device we are accessing. At the same
+		 * time we must avoid accesses to certain areas of the cfg
+		 * space via CFG_DATA. Detect those accesses and report that
+		 * the address is invalid.
+		 */
+		base += PCIE_EXT_CFG_DATA;
+		bus = PCI_ADDR_BUS(address);
+		dev = PCI_ADDR_DEV(address);
+		fun = PCI_ADDR_FUN(address);
+		address = (bus << PCIE_EXT_BUS_SHIFT) |
+			  (dev << PCIE_EXT_DEV_SHIFT) |
+			  (fun << PCIE_EXT_FUN_SHIFT);
+
+		/* Allow only dev = 0 on root port and bus 1 */
+		if ((bus < 2U) && (dev > 0U)) {
+			return INVALID_PCI_ADDR;
+		}
+
+		/* Assure link up before reading bus 1 */
+		status = mmio_read_32(PCIE_REG_BASE + PCIE_MISC_PCIE_STATUS);
+		if ((status & 0x30) != 0x30) {
+			return INVALID_PCI_ADDR;
+		}
+
+		/* Adjust which device the CFG_DATA window is pointing at */
+		mmio_write_32(PCIE_REG_BASE + PCIE_EXT_CFG_INDEX, address);
+	}
+	return base + offset;
+}
+
+/**
+ * pci_read_config() - Performs a config space read at addr
+ * @addr: 32-bit, segment, BDF of requested function encoded per DEN0115
+ * @off:  register offset of function described by @addr to read
+ * @sz:	  size of read (8,16,32) bits.
+ * @val:  returned zero extended value read from config space
+ *
+ * sz bits of PCI config space is read at addr:offset, and the value
+ * is returned in val. Invalid segment/offset values return failure.
+ * Reads to valid functions that don't exist return INVALID_PCI_ADDR
+ * as is specified by PCI for requests that aren't completed by EPs.
+ * The boilerplate in pci_svc.c tends to do basic segment, off
+ * and sz validation. This routine should avoid duplicating those
+ * checks.
+ *
+ * This function maps directly to the PCI_READ function in DEN0115
+ * where detailed requirements may be found.
+ *
+ * Return: SMC_PCI_CALL_SUCCESS with val set
+ *	   SMC_PCI_CALL_INVAL_PARAM, on parameter error
+ */
+uint32_t pci_read_config(uint32_t addr, uint32_t off, uint32_t sz, uint32_t *val)
+{
+	uint32_t ret = SMC_PCI_CALL_SUCCESS;
+	uint64_t base;
+
+	spin_lock(&pci_lock);
+	base = pci_segment_lib_get_base(addr, off);
+
+	if (base == INVALID_PCI_ADDR) {
+		*val = base;
+	} else {
+		switch (sz) {
+		case SMC_PCI_SZ_8BIT:
+			*val = mmio_read_8(base);
+			break;
+		case SMC_PCI_SZ_16BIT:
+			*val = mmio_read_16(base);
+			break;
+		case SMC_PCI_SZ_32BIT:
+			*val = mmio_read_32(base);
+			break;
+		default: /* should be unreachable */
+			*val = 0;
+			ret = SMC_PCI_CALL_INVAL_PARAM;
+		}
+	}
+	spin_unlock(&pci_lock);
+	return ret;
+}
+
+/**
+ * pci_write_config() - Performs a config space write at addr
+ * @addr: 32-bit, segment, BDF of requested function encoded per DEN0115
+ * @off:  register offset of function described by @addr to write
+ * @sz:	  size of write (8,16,32) bits.
+ * @val:  value to be written
+ *
+ * sz bits of PCI config space is written at addr:offset. Invalid
+ * segment/BDF values return failure. Writes to valid functions
+ * without valid EPs are ignored, as is specified by PCI.
+ * The boilerplate in pci_svc.c tends to do basic segment, off
+ * and sz validation, so it shouldn't need to be repeated here.
+ *
+ * This function maps directly to the PCI_WRITE function in DEN0115
+ * where detailed requirements may be found.
+ *
+ * Return: SMC_PCI_CALL_SUCCESS
+ *	   SMC_PCI_CALL_INVAL_PARAM, on parameter error
+ */
+uint32_t pci_write_config(uint32_t addr, uint32_t off, uint32_t sz, uint32_t val)
+{
+	uint32_t ret = SMC_PCI_CALL_SUCCESS;
+	uint64_t base;
+
+	spin_lock(&pci_lock);
+	base = pci_segment_lib_get_base(addr, off);
+
+	if (base != INVALID_PCI_ADDR) {
+		switch (sz) {
+		case SMC_PCI_SZ_8BIT:
+			mmio_write_8(base, val);
+			break;
+		case SMC_PCI_SZ_16BIT:
+			mmio_write_16(base, val);
+			break;
+		case SMC_PCI_SZ_32BIT:
+			mmio_write_32(base, val);
+			break;
+		default: /* should be unreachable */
+			ret = SMC_PCI_CALL_INVAL_PARAM;
+		}
+	}
+	spin_unlock(&pci_lock);
+	return ret;
+}
+
+/**
+ * pci_get_bus_for_seg() - returns the start->end bus range for a segment
+ * @seg:  segment being queried
+ * @bus_range:	returned bus begin + (end << 8)
+ * @nseg: returns next segment in this machine or 0 for end
+ *
+ * pci_get_bus_for_seg is called to check if a given segment is
+ * valid on this machine. If it is valid, then its bus ranges are
+ * returned along with the next valid segment on the machine. If
+ * this is the last segment, then nseg must be 0.
+ *
+ * This function maps directly to the PCI_GET_SEG_INFO function
+ * in DEN0115 where detailed requirements may be found.
+ *
+ * Return: SMC_PCI_CALL_SUCCESS, and appropriate bus_range and nseg
+ *	   SMC_PCI_CALL_NOT_IMPL, if the segment is invalid
+ */
+uint32_t pci_get_bus_for_seg(uint32_t seg, uint32_t *bus_range, uint32_t *nseg)
+{
+	uint32_t ret = SMC_PCI_CALL_SUCCESS;
+	*nseg = 0U; /* only a single segment */
+	if (seg == 0U) {
+		*bus_range = 0xFF00; /* start 0, end 255 */
+	} else {
+		*bus_range = 0U;
+		ret = SMC_PCI_CALL_NOT_IMPL;
+	}
+	return ret;
+}
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index 42d3487..edced71 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -17,6 +17,7 @@
 /* Functions to save and get boot context address given by ROM code */
 void stm32mp_save_boot_ctx_address(uintptr_t address);
 uintptr_t stm32mp_get_boot_ctx_address(void);
+uint16_t stm32mp_get_boot_itf_selected(void);
 
 bool stm32mp_is_single_core(void);
 bool stm32mp_is_closed_device(void);
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index d3de1e1..5e5958b 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -28,10 +28,14 @@
 }
 
 static uintptr_t boot_ctx_address;
+static uint16_t boot_itf_selected;
 
 void stm32mp_save_boot_ctx_address(uintptr_t address)
 {
+	boot_api_context_t *boot_context = (boot_api_context_t *)address;
+
 	boot_ctx_address = address;
+	boot_itf_selected = boot_context->boot_interface_selected;
 }
 
 uintptr_t stm32mp_get_boot_ctx_address(void)
@@ -39,6 +43,11 @@
 	return boot_ctx_address;
 }
 
+uint16_t stm32mp_get_boot_itf_selected(void)
+{
+	return boot_itf_selected;
+}
+
 uintptr_t stm32mp_ddrctrl_base(void)
 {
 	return DDRCTRL_BASE;
@@ -105,7 +114,7 @@
 {
 	return  mmap_add_dynamic_region(STM32MP_DDR_BASE, STM32MP_DDR_BASE,
 					STM32MP_DDR_MAX_SIZE,
-					MT_NON_CACHEABLE | MT_RW | MT_NS);
+					MT_NON_CACHEABLE | MT_RW | MT_SECURE);
 }
 
 int stm32mp_unmap_ddr(void)
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index 6465c10..0b35646 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -209,15 +209,24 @@
  ******************************************************************************/
 uint32_t dt_get_ddr_size(void)
 {
+	static uint32_t size;
 	int node;
 
+	if (size != 0U) {
+		return size;
+	}
+
 	node = fdt_node_offset_by_compatible(fdt, -1, DT_DDR_COMPAT);
 	if (node < 0) {
 		INFO("%s: Cannot read DDR node in DT\n", __func__);
 		return 0;
 	}
 
-	return fdt_read_uint32_default(fdt, node, "st,mem-size", 0);
+	size = fdt_read_uint32_default(fdt, node, "st,mem-size", 0U);
+
+	flush_dcache_range((uintptr_t)&size, sizeof(uint32_t));
+
+	return size;
 }
 
 /*******************************************************************************
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 91073b8..3e179fb 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -132,7 +132,6 @@
 void bl2_platform_setup(void)
 {
 	int ret;
-	uint32_t ddr_ns_size;
 
 	if (dt_pmic_status() > 0) {
 		initialize_pmic();
@@ -144,24 +143,16 @@
 		panic();
 	}
 
-	ddr_ns_size = stm32mp_get_ddr_ns_size();
-	assert(ddr_ns_size > 0U);
-
-	/* Map non secure DDR for BL33 load, now with cacheable attribute */
+	/* Map DDR for binary load, now with cacheable attribute */
 	ret = mmap_add_dynamic_region(STM32MP_DDR_BASE, STM32MP_DDR_BASE,
-				      ddr_ns_size, MT_MEMORY | MT_RW | MT_NS);
-	assert(ret == 0);
+				      STM32MP_DDR_MAX_SIZE, MT_MEMORY | MT_RW | MT_SECURE);
+	if (ret < 0) {
+		ERROR("DDR mapping: error %d\n", ret);
+		panic();
+	}
 
 #ifdef AARCH32_SP_OPTEE
 	INFO("BL2 runs OP-TEE setup\n");
-
-	/* Map secure DDR for OP-TEE paged area */
-	ret = mmap_add_dynamic_region(STM32MP_DDR_BASE + ddr_ns_size,
-				      STM32MP_DDR_BASE + ddr_ns_size,
-				      STM32MP_DDR_S_SIZE,
-				      MT_MEMORY | MT_RW | MT_SECURE);
-	assert(ret == 0);
-
 	/* Initialize tzc400 after DDR initialization */
 	stm32mp1_security_setup();
 #else
diff --git a/plat/st/stm32mp1/stm32mp1_security.c b/plat/st/stm32mp1/stm32mp1_security.c
index 195b3a5..2ee5f4a 100644
--- a/plat/st/stm32mp1/stm32mp1_security.c
+++ b/plat/st/stm32mp1/stm32mp1_security.c
@@ -27,6 +27,45 @@
 	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_ETH_ID) | \
 	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_DAP_ID)
 
+static unsigned int region_nb;
+
+static void init_tzc400_begin(unsigned int region0_attr)
+{
+	tzc400_init(STM32MP1_TZC_BASE);
+	tzc400_disable_filters();
+
+	/* Region 0 set to cover all DRAM at 0xC000_0000 */
+	tzc400_configure_region0(region0_attr, 0);
+
+	region_nb = 1U;
+}
+
+static void init_tzc400_end(unsigned int action)
+{
+	tzc400_set_action(action);
+	tzc400_enable_filters();
+}
+
+static void tzc400_add_region(unsigned long long region_base,
+			      unsigned long long region_top, bool sec)
+{
+	unsigned int sec_attr;
+	unsigned int nsaid_permissions;
+
+	if (sec) {
+		sec_attr = TZC_REGION_S_RDWR;
+		nsaid_permissions = 0;
+	} else {
+		sec_attr = TZC_REGION_S_NONE;
+		nsaid_permissions = TZC_REGION_NSEC_ALL_ACCESS_RDWR;
+	}
+
+	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, region_nb, region_base,
+				region_top, sec_attr, nsaid_permissions);
+
+	region_nb++;
+}
+
 /*******************************************************************************
  * Initialize the TrustZone Controller. Configure Region 0 with Secure RW access
  * and allow Non-Secure masters full access.
@@ -38,10 +77,9 @@
 	unsigned long long ddr_ns_size =
 		(unsigned long long)stm32mp_get_ddr_ns_size();
 	unsigned long long ddr_ns_top = ddr_base + (ddr_ns_size - 1U);
+	unsigned long long ddr_top __unused;
 
-	tzc400_init(STM32MP1_TZC_BASE);
-
-	tzc400_disable_filters();
+	init_tzc400_begin(TZC_REGION_S_NONE);
 
 	/*
 	 * Region 1 set to cover all non-secure DRAM at 0xC000_0000. Apply the
@@ -49,35 +87,28 @@
 	 */
 	region_base = ddr_base;
 	region_top = ddr_ns_top;
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 1,
-				region_base,
-				region_top,
-				TZC_REGION_S_NONE,
-				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+	tzc400_add_region(region_base, region_top, false);
 
 #ifdef AARCH32_SP_OPTEE
 	/* Region 2 set to cover all secure DRAM. */
 	region_base = region_top + 1U;
 	region_top += STM32MP_DDR_S_SIZE;
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 2,
-				region_base,
-				region_top,
-				TZC_REGION_S_RDWR,
-				0);
+	tzc400_add_region(region_base, region_top, true);
 
-	/* Region 3 set to cover non-secure shared memory DRAM. */
-	region_base = region_top + 1U;
-	region_top += STM32MP_DDR_SHMEM_SIZE;
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 3,
-				region_base,
-				region_top,
-				TZC_REGION_S_NONE,
-				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+	ddr_top = STM32MP_DDR_BASE + dt_get_ddr_size() - 1U;
+	if (region_top < ddr_top) {
+		/* Region 3 set to cover non-secure memory DRAM after BL32. */
+		region_base = region_top + 1U;
+		region_top = ddr_top;
+		tzc400_add_region(region_base, region_top, false);
+	}
 #endif
 
-	tzc400_set_action(TZC_ACTION_INT);
-
-	tzc400_enable_filters();
+	/*
+	 * Raise an interrupt (secure FIQ) if a NS device tries to access
+	 * secure memory
+	 */
+	init_tzc400_end(TZC_ACTION_INT);
 }
 
 /*******************************************************************************
@@ -90,23 +121,11 @@
 	stm32mp_clk_enable(TZC1);
 	stm32mp_clk_enable(TZC2);
 
-	tzc400_init(STM32MP1_TZC_BASE);
-
-	tzc400_disable_filters();
-
-	/* Region 1 set to cover Non-Secure DRAM at 0xC000_0000 */
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 1,
-				STM32MP_DDR_BASE,
-				STM32MP_DDR_BASE +
-				(STM32MP_DDR_MAX_SIZE - 1U),
-				TZC_REGION_S_NONE,
-				TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID) |
-				TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_SDMMC_ID));
+	/* Region 0 set to cover all DRAM secure at 0xC000_0000 */
+	init_tzc400_begin(TZC_REGION_S_RDWR);
 
 	/* Raise an exception if a NS device tries to access secure memory */
-	tzc400_set_action(TZC_ACTION_ERR);
-
-	tzc400_enable_filters();
+	init_tzc400_end(TZC_ACTION_ERR);
 }
 
 /*******************************************************************************
diff --git a/plat/xilinx/common/include/ipi.h b/plat/xilinx/common/include/ipi.h
index 9c1d0f2..483902e 100644
--- a/plat/xilinx/common/include/ipi.h
+++ b/plat/xilinx/common/include/ipi.h
@@ -63,7 +63,7 @@
 int ipi_mb_enquire_status(uint32_t local, uint32_t remote);
 
 /* Trigger notification on the IPI mailbox */
-int ipi_mb_notify(uint32_t local, uint32_t remote, uint32_t is_blocking);
+void ipi_mb_notify(uint32_t local, uint32_t remote, uint32_t is_blocking);
 
 /* Ack IPI mailbox notification */
 void ipi_mb_ack(uint32_t local, uint32_t remote);
diff --git a/plat/xilinx/common/ipi.c b/plat/xilinx/common/ipi.c
index ca4146e..0b8020b 100644
--- a/plat/xilinx/common/ipi.c
+++ b/plat/xilinx/common/ipi.c
@@ -13,7 +13,6 @@
 
 #include <common/debug.h>
 #include <common/runtime_svc.h>
-#include <drivers/delay_timer.h>
 #include <lib/bakery_lock.h>
 #include <lib/mmio.h>
 
@@ -39,9 +38,6 @@
 /* IPI register bit mask */
 #define IPI_BIT_MASK(I) (ipi_table[(I)].ipi_bit_mask)
 
-/* IPI Timeout */
-#define TIMEOUT_COUNT_US	U(0x4000)
-
 /* IPI configuration table */
 const static struct ipi_config *ipi_table;
 
@@ -160,30 +156,21 @@
  * @remote - remote IPI ID
  * @is_blocking - if to trigger the notification in blocking mode or not.
  *
- * return - 0 - Success or Error incase of timeout
  * It sets the remote bit in the IPI agent trigger register.
  *
  */
-int ipi_mb_notify(uint32_t local, uint32_t remote, uint32_t is_blocking)
+void ipi_mb_notify(uint32_t local, uint32_t remote, uint32_t is_blocking)
 {
 	uint32_t status;
-	const unsigned int timeout_count = TIMEOUT_COUNT_US;
-	uint64_t timeout;
 
 	mmio_write_32(IPI_REG_BASE(local) + IPI_TRIG_OFFSET,
 		      IPI_BIT_MASK(remote));
 	if (is_blocking) {
-		timeout = timeout_init_us(timeout_count);
 		do {
 			status = mmio_read_32(IPI_REG_BASE(local) +
 					      IPI_OBR_OFFSET);
-			if (timeout_elapsed(timeout)) {
-				return -ETIMEDOUT;
-			}
 		} while (status & IPI_BIT_MASK(remote));
 	}
-
-	return 0;
 }
 
 /* ipi_mb_ack() - Ack IPI mailbox notification from the other end
diff --git a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
index cd5d830..f531158 100644
--- a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
+++ b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
@@ -107,8 +107,8 @@
 		uint32_t is_blocking;
 
 		is_blocking = (x3 & IPI_SMC_NOTIFY_BLOCK_MASK) ? 1 : 0;
-		ret = ipi_mb_notify(ipi_local_id, ipi_remote_id, is_blocking);
-		SMC_RET1(handle, ret);
+		ipi_mb_notify(ipi_local_id, ipi_remote_id, is_blocking);
+		SMC_RET1(handle, 0);
 	}
 	case IPI_MAILBOX_ACK:
 	{
diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index 7b5bd02..7b2c8ec 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c
@@ -55,7 +55,6 @@
 					     uint32_t payload[PAYLOAD_ARG_CNT],
 					     uint32_t is_blocking)
 {
-	int status;
 	unsigned int offset = 0;
 	uintptr_t buffer_base = proc->ipi->buffer_base +
 					IPI_BUFFER_TARGET_REMOTE_OFFSET +
@@ -71,13 +70,10 @@
 	}
 
 	/* Generate IPI to remote processor */
-	status = ipi_mb_notify(proc->ipi->local_ipi_id, proc->ipi->remote_ipi_id,
+	ipi_mb_notify(proc->ipi->local_ipi_id, proc->ipi->remote_ipi_id,
 		      is_blocking);
-	if (status == 0) {
-		return PM_RET_SUCCESS;
-	}
 
-	return PM_RET_ERROR_TIMEOUT;
+	return PM_RET_SUCCESS;
 }
 
 /**
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.c b/plat/xilinx/versal/pm_service/pm_api_sys.c
index 15fe187..912835a 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.c
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.c
@@ -173,7 +173,7 @@
 	/* Send request to the PLM */
 	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_ABORT_SUSPEND,
 			 reason, primary_proc->node_id);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
@@ -909,7 +909,7 @@
 
 	PM_PACK_PAYLOAD4(payload, LIBPM_MODULE_ID, flag, PM_SET_WAKEUP_SOURCE,
 			 target, wkup_device, enable);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
index 9a53408..62260bc 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
@@ -209,7 +209,7 @@
 	/* TODO: allow passing the node ID of the affected CPU */
 	PM_PACK_PAYLOAD3(payload, PM_ABORT_SUSPEND, reason,
 			 primary_proc->node_id);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
@@ -228,7 +228,7 @@
 
 	PM_PACK_PAYLOAD4(payload, PM_SET_WAKEUP_SOURCE, target, wkup_node,
 			 enable);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
@@ -316,7 +316,7 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	PM_PACK_PAYLOAD2(payload, PM_RELEASE_NODE, nid);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
@@ -332,7 +332,7 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	PM_PACK_PAYLOAD3(payload, PM_SET_MAX_LATENCY, nid, latency);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /* Miscellaneous API functions */
@@ -461,7 +461,7 @@
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_RESET_ASSERT, reset, assert);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
diff --git a/services/std_svc/sdei/sdei_intr_mgmt.c b/services/std_svc/sdei/sdei_intr_mgmt.c
index fa1d3d2..5d176c2 100644
--- a/services/std_svc/sdei/sdei_intr_mgmt.c
+++ b/services/std_svc/sdei/sdei_intr_mgmt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include <arch_helpers.h>
+#include <arch_features.h>
 #include <bl31/ehf.h>
 #include <bl31/interrupt_mgmt.h>
 #include <common/bl_common.h>
@@ -232,6 +233,77 @@
 }
 
 /*
+ * Prepare for ERET:
+ * - Set the ELR to the registered handler address
+ * - Set the SPSR register as described in the SDEI documentation and
+ *   the AArch64.TakeException() pseudocode function in
+ *   ARM DDI 0487F.c page J1-7635
+ */
+
+static void sdei_set_elr_spsr(sdei_entry_t *se, sdei_dispatch_context_t *disp_ctx)
+{
+	unsigned int client_el = sdei_client_el();
+	u_register_t sdei_spsr = SPSR_64(client_el, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+
+	u_register_t interrupted_pstate = disp_ctx->spsr_el3;
+
+	/* Check the SPAN bit in the client el SCTLR */
+	u_register_t client_el_sctlr;
+
+	if (client_el == MODE_EL2) {
+		client_el_sctlr = read_sctlr_el2();
+	} else {
+		client_el_sctlr = read_sctlr_el1();
+	}
+
+	/*
+	 * Check whether to force the PAN bit or use the value in the
+	 * interrupted EL according to the check described in
+	 * TakeException. Since the client can only be Non-Secure
+	 * EL2 or El1 some of the conditions in ElIsInHost() we know
+	 * will always be True.
+	 * When the client_el is EL2 we know that there will be a SPAN
+	 * bit in SCTLR_EL2 as we have already checked for the condition
+	 * HCR_EL2.E2H = 1 and HCR_EL2.TGE = 1
+	 */
+	u_register_t hcr_el2 = read_hcr();
+	bool el_is_in_host = is_armv8_1_vhe_present() &&
+			     (hcr_el2 & HCR_TGE_BIT) &&
+			     (hcr_el2 & HCR_E2H_BIT);
+
+	if (is_armv8_1_pan_present() &&
+	    ((client_el == MODE_EL1) ||
+		(client_el == MODE_EL2 && el_is_in_host)) &&
+	    ((client_el_sctlr & SCTLR_SPAN_BIT) == 0U)) {
+		sdei_spsr |=  SPSR_PAN_BIT;
+	} else {
+		sdei_spsr |= (interrupted_pstate & SPSR_PAN_BIT);
+	}
+
+	/* If SSBS is implemented, take the value from the client el SCTLR */
+	u_register_t ssbs_enabled = (read_id_aa64pfr1_el1()
+					>> ID_AA64PFR1_EL1_SSBS_SHIFT)
+					& ID_AA64PFR1_EL1_SSBS_MASK;
+	if (ssbs_enabled != SSBS_UNAVAILABLE) {
+		u_register_t  ssbs_bit = ((client_el_sctlr & SCTLR_DSSBS_BIT)
+						>> SCTLR_DSSBS_SHIFT)
+						<< SPSR_SSBS_SHIFT_AARCH64;
+		sdei_spsr |= ssbs_bit;
+	}
+
+	/* If MTE is implemented in the client el set the TCO bit */
+	if (get_armv8_5_mte_support() >= MTE_IMPLEMENTED_ELX) {
+		sdei_spsr |= SPSR_TCO_BIT_AARCH64;
+	}
+
+	/* Take the DIT field from the pstate of the interrupted el */
+	sdei_spsr |= (interrupted_pstate & SPSR_DIT_BIT);
+
+	cm_set_elr_spsr_el3(NON_SECURE, (uintptr_t) se->ep, sdei_spsr);
+}
+
+/*
  * Populate the Non-secure context so that the next ERET will dispatch to the
  * SDEI client.
  */
@@ -256,15 +328,8 @@
 	SMC_SET_GP(ctx, CTX_GPREG_X2, disp_ctx->elr_el3);
 	SMC_SET_GP(ctx, CTX_GPREG_X3, disp_ctx->spsr_el3);
 
-	/*
-	 * Prepare for ERET:
-	 *
-	 * - Set PC to the registered handler address
-	 * - Set SPSR to jump to client EL with exceptions masked
-	 */
-	cm_set_elr_spsr_el3(NON_SECURE, (uintptr_t) se->ep,
-			SPSR_64(sdei_client_el(), MODE_SP_ELX,
-				DISABLE_ALL_EXCEPTIONS));
+	/* Setup the elr and spsr register to prepare for ERET */
+	sdei_set_elr_spsr(se, disp_ctx);
 
 #if DYNAMIC_WORKAROUND_CVE_2018_3639
 	cve_2018_3639_t *tgt_cve_2018_3639;
@@ -571,15 +636,15 @@
 	if (!can_sdei_state_trans(se, DO_DISPATCH))
 		return -1;
 
-	/* Activate the priority corresponding to the event being dispatched */
-	ehf_activate_priority(sdei_event_priority(map));
-
 	/*
 	 * Prepare for NS dispatch by restoring the Non-secure context and
 	 * marking that as active.
 	 */
 	ns_ctx = restore_and_resume_ns_context();
 
+	/* Activate the priority corresponding to the event being dispatched */
+	ehf_activate_priority(sdei_event_priority(map));
+
 	/* Dispatch event synchronously */
 	setup_ns_dispatch(map, se, ns_ctx, &dispatch_jmp);
 	begin_sdei_synchronous_dispatch(&dispatch_jmp);