diff --git a/Makefile b/Makefile
index ec6f885..2a1d4d8 100644
--- a/Makefile
+++ b/Makefile
@@ -150,8 +150,6 @@
 ifeq (${USE_SPINLOCK_CAS},1)
 ifneq (${ARCH},aarch64)
         $(error USE_SPINLOCK_CAS requires AArch64)
-else
-        $(info USE_SPINLOCK_CAS is an experimental feature)
 endif
 endif
 
@@ -525,7 +523,6 @@
     endif
 
     ifeq (${SPD},spmd)
-        $(warning "SPMD is an experimental feature")
         # SPMD is located in std_svc directory
         SPD_DIR := std_svc
 
@@ -731,12 +728,7 @@
 
 # SDEI_IN_FCONF is only supported when SDEI_SUPPORT is enabled.
 ifeq ($(SDEI_SUPPORT)-$(SDEI_IN_FCONF),0-1)
-$(error "SDEI_IN_FCONF is an experimental feature and is only supported when \
-	SDEI_SUPPORT is enabled")
-endif
-
-ifeq ($(COT_DESC_IN_DTB),1)
-    $(info CoT in device tree is an experimental feature)
+$(error "SDEI_IN_FCONF is only supported when SDEI_SUPPORT is enabled")
 endif
 
 # If pointer authentication is used in the firmware, make sure that all the
@@ -751,24 +743,12 @@
 ifeq ($(CTX_INCLUDE_PAUTH_REGS),1)
     ifneq (${ARCH},aarch64)
         $(error CTX_INCLUDE_PAUTH_REGS requires AArch64)
-    else
-        $(info CTX_INCLUDE_PAUTH_REGS is an experimental feature)
     endif
 endif
 
-ifeq ($(ENABLE_PAUTH),1)
-    $(info Pointer Authentication is an experimental feature)
-endif
-
-ifeq ($(ENABLE_BTI),1)
-    $(info Branch Protection is an experimental feature)
-endif
-
 ifeq ($(CTX_INCLUDE_MTE_REGS),1)
     ifneq (${ARCH},aarch64)
         $(error CTX_INCLUDE_MTE_REGS requires AArch64)
-    else
-        $(info CTX_INCLUDE_MTE_REGS is an experimental feature)
     endif
 endif
 
@@ -778,8 +758,6 @@
 ifeq ($(MEASURED_BOOT),1)
     ifneq (${TRUSTED_BOARD_BOOT},1)
         $(error MEASURED_BOOT requires TRUSTED_BOARD_BOOT=1)
-    else
-        $(info MEASURED_BOOT is an experimental feature)
     endif
 endif
 
@@ -796,8 +774,6 @@
 ifneq (${DECRYPTION_SUPPORT},none)
     ifeq (${TRUSTED_BOARD_BOOT}, 0)
         $(error TRUSTED_BOARD_BOOT must be enabled for DECRYPTION_SUPPORT to be set)
-    else
-        $(info DECRYPTION_SUPPORT is an experimental feature)
     endif
 endif
 
@@ -955,6 +931,8 @@
         DYN_DISABLE_AUTH \
         EL3_EXCEPTION_HANDLING \
         ENABLE_AMU \
+        ENABLE_AMU_AUXILIARY_COUNTERS \
+        ENABLE_AMU_FCONF \
         AMU_RESTRICT_COUNTERS \
         ENABLE_ASSERTIONS \
         ENABLE_MPAM_FOR_LOWER_ELS \
@@ -1013,6 +991,8 @@
         ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_TRF_FOR_NS \
         ENABLE_FEAT_HCX \
+        ENABLE_MPMM \
+        ENABLE_MPMM_FCONF \
 )))
 
 $(eval $(call assert_numerics,\
@@ -1056,6 +1036,8 @@
         DECRYPTION_SUPPORT_${DECRYPTION_SUPPORT} \
         DISABLE_MTPMU \
         ENABLE_AMU \
+        ENABLE_AMU_AUXILIARY_COUNTERS \
+        ENABLE_AMU_FCONF \
         AMU_RESTRICT_COUNTERS \
         ENABLE_ASSERTIONS \
         ENABLE_BTI \
@@ -1119,6 +1101,8 @@
         ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_TRF_FOR_NS \
         ENABLE_FEAT_HCX \
+        ENABLE_MPMM \
+        ENABLE_MPMM_FCONF \
 )))
 
 ifeq (${SANITIZE_UB},trap)
@@ -1185,6 +1169,8 @@
 
 # Expand build macros for the different images
 ifeq (${NEED_BL1},yes)
+BL1_SOURCES := $(sort ${BL1_SOURCES})
+
 $(eval $(call MAKE_BL,bl1))
 endif
 
@@ -1193,6 +1179,8 @@
 FIP_BL2_ARGS := tb-fw
 endif
 
+BL2_SOURCES := $(sort ${BL2_SOURCES})
+
 $(if ${BL2}, $(eval $(call TOOL_ADD_IMG,bl2,--${FIP_BL2_ARGS})),\
 	$(eval $(call MAKE_BL,bl2,${FIP_BL2_ARGS})))
 endif
@@ -1330,7 +1318,8 @@
 		echo "    with ${CHECKPATCH_OPTS} option(s)";		\
 	fi
 	${Q}COMMON_COMMIT=$$(git merge-base HEAD ${BASE_COMMIT});	\
-	for commit in `git rev-list $$COMMON_COMMIT..HEAD`; do		\
+	for commit in `git rev-list --no-merges $$COMMON_COMMIT..HEAD`;	\
+	do								\
 		printf "\n[*] Checking style of '$$commit'\n\n";	\
 		git log --format=email "$$commit~..$$commit"		\
 			-- ${CHECK_PATHS} |				\
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
index fd60232..663ec64 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_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
  */
@@ -126,6 +126,9 @@
 	auth_mod_init();
 #endif /* TRUSTED_BOARD_BOOT */
 
+	/* Initialize the measured boot */
+	bl1_plat_mboot_init();
+
 	/* Perform platform setup in BL1. */
 	bl1_platform_setup();
 
@@ -147,6 +150,9 @@
 	else
 		NOTICE("BL1-FWU: *******FWU Process Started*******\n");
 
+	/* Teardown the measured boot driver */
+	bl1_plat_mboot_finish();
+
 	bl1_prepare_next_image(image_id);
 
 	console_flush();
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index 197c057..90fe39b 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -15,9 +15,6 @@
 #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
 #include <lib/extensions/pauth.h>
 #include <plat/common/platform.h>
 
@@ -95,24 +92,19 @@
 #if TRUSTED_BOARD_BOOT
 	/* Initialize authentication module */
 	auth_mod_init();
-
-#if MEASURED_BOOT
-	/* Initialize measured boot module */
-	measured_boot_init();
-
-#endif /* MEASURED_BOOT */
 #endif /* TRUSTED_BOARD_BOOT */
 
+	/* Initialize the Measured Boot backend */
+	bl2_plat_mboot_init();
+
 	/* Initialize boot source */
 	bl2_plat_preload_setup();
 
 	/* Load the subsequent bootloader images. */
 	next_bl_ep_info = bl2_load_images();
 
-#if MEASURED_BOOT
-	/* Finalize measured boot */
-	measured_boot_finish();
-#endif /* MEASURED_BOOT */
+	/* Teardown the Measured Boot backend */
+	bl2_plat_mboot_finish();
 
 #if !BL2_AT_EL3 && !ENABLE_RME
 #ifndef __aarch64__
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 106d410..9baa0c2 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -22,6 +22,8 @@
   endif
 endif
 
+include lib/extensions/amu/amu.mk
+include lib/mpmm/mpmm.mk
 include lib/psci/psci_lib.mk
 
 BL31_SOURCES		+=	bl31/bl31_main.c				\
@@ -78,8 +80,11 @@
 endif
 
 ifeq (${ENABLE_AMU},1)
-BL31_SOURCES		+=	lib/extensions/amu/aarch64/amu.c		\
-				lib/extensions/amu/aarch64/amu_helpers.S
+BL31_SOURCES		+=	${AMU_SOURCES}
+endif
+
+ifeq (${ENABLE_MPMM},1)
+BL31_SOURCES		+=	${MPMM_SOURCES}
 endif
 
 ifeq (${ENABLE_SVE_FOR_NS},1)
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 6339cf8..590b032 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,6 +8,7 @@
 	$(error SP_MIN is only supported on AArch32 platforms)
 endif
 
+include lib/extensions/amu/amu.mk
 include lib/psci/psci_lib.mk
 
 INCLUDES		+=	-Iinclude/bl32/sp_min
@@ -27,9 +28,8 @@
 BL32_SOURCES		+=	lib/pmf/pmf_main.c
 endif
 
-ifeq (${ENABLE_AMU}, 1)
-BL32_SOURCES		+=	lib/extensions/amu/aarch32/amu.c\
-				lib/extensions/amu/aarch32/amu_helpers.S
+ifeq (${ENABLE_AMU},1)
+BL32_SOURCES		+=	${AMU_SOURCES}
 endif
 
 ifeq (${WORKAROUND_CVE_2017_5715},1)
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 795c586..7d77f47 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -100,11 +100,27 @@
 	 * sections. This is done to safeguard against
 	 * possible corruption of this memory by dirty
 	 * cache lines in a system cache as a result of
-	 * use by an earlier boot loader stage.
+	 * use by an earlier boot loader stage. If PIE
+	 * is enabled however, RO sections including the
+	 * GOT may be modified during pie fixup.
+	 * Therefore, to be on the safe side, invalidate
+	 * the entire image region if PIE is enabled.
 	 * ---------------------------------------------
 	 */
-	adr	x0, __RW_START__
-	adr	x1, __RW_END__
+#if ENABLE_PIE
+#if SEPARATE_CODE_AND_RODATA
+	adrp	x0, __TEXT_START__
+	add	x0, x0, :lo12:__TEXT_START__
+#else
+	adrp	x0, __RO_START__
+	add	x0, x0, :lo12:__RO_START__
+#endif /* SEPARATE_CODE_AND_RODATA */
+#else
+	adrp	x0, __RW_START__
+	add	x0, x0, :lo12:__RW_START__
+#endif /* ENABLE_PIE */
+	adrp	x1, __RW_END__
+	add     x1, x1, :lo12:__RW_END__
 	sub	x1, x1, x0
 	bl	inv_dcache_range
 
diff --git a/common/bl_common.c b/common/bl_common.c
index a7e2816..eb2352a 100644
--- a/common/bl_common.c
+++ b/common/bl_common.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
  */
@@ -202,12 +202,26 @@
 		return -EAUTH;
 	}
 
-	/*
-	 * Flush the image to main memory so that it can be executed later by
-	 * any CPU, regardless of cache and MMU state. This is only needed for
-	 * child images, not for the parents (certificates).
-	 */
 	if (is_parent_image == 0) {
+		/*
+		 * Measure the image.
+		 * We do not measure its parents because these only play a role
+		 * in authentication, which is orthogonal to measured boot.
+		 *
+		 * TODO: Change this code if we change our minds about measuring
+		 * certificates.
+		 */
+		rc = plat_mboot_measure_image(image_id, image_data);
+		if (rc != 0) {
+			return rc;
+		}
+
+		/*
+		 * Flush the image to main memory so that it can be executed
+		 * later by any CPU, regardless of cache and MMU state. This
+		 * is only needed for child images, not for the parents
+		 * (certificates).
+		 */
 		flush_dcache_range(image_data->image_base,
 				   image_data->image_size);
 	}
diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c
index dd7a0fa..64e01ea 100644
--- a/common/fdt_wrappers.c
+++ b/common/fdt_wrappers.c
@@ -572,3 +572,47 @@
 	/* Translate the local device address recursively */
 	return fdtw_translate_address(dtb, local_bus_node, global_address);
 }
+
+/*
+ * For every CPU node (`/cpus/cpu@n`) in an FDT, execute a callback passing a
+ * pointer to the FDT and the offset of the CPU node. If the return value of the
+ * callback is negative, it is treated as an error and the loop is aborted. In
+ * this situation, the value of the callback is returned from the function.
+ *
+ * Returns `0` on success, or a negative integer representing an error code.
+ */
+int fdtw_for_each_cpu(const void *dtb,
+		      int (*callback)(const void *dtb, int node, uintptr_t mpidr))
+{
+	int ret = 0;
+	int parent, node = 0;
+
+	parent = fdt_path_offset(dtb, "/cpus");
+	if (parent < 0) {
+		return parent;
+	}
+
+	fdt_for_each_subnode(node, dtb, parent) {
+		const char *name;
+		int len;
+
+		uintptr_t mpidr = 0U;
+
+		name = fdt_get_name(dtb, node, &len);
+		if (strncmp(name, "cpu@", 4) != 0) {
+			continue;
+		}
+
+		ret = fdt_get_reg_props_by_index(dtb, node, 0, &mpidr, NULL);
+		if (ret < 0) {
+			break;
+		}
+
+		ret = callback(dtb, node, mpidr);
+		if (ret < 0) {
+			break;
+		}
+	}
+
+	return ret;
+}
diff --git a/common/fdt_wrappers.mk b/common/fdt_wrappers.mk
new file mode 100644
index 0000000..62b8c6e
--- /dev/null
+++ b/common/fdt_wrappers.mk
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+FDT_WRAPPERS_SOURCES	:=	common/fdt_wrappers.c
diff --git a/docs/about/features.rst b/docs/about/features.rst
index f5fc1e0..4b7fbe5 100644
--- a/docs/about/features.rst
+++ b/docs/about/features.rst
@@ -74,7 +74,7 @@
    loading of a hardware configuration (for example, a kernel device tree)
    as part of the FIP, to be passed through the firmware stages.
    This feature is now incorporated inside the firmware configuration framework
-   (fconf), which is still flagged as experimental.
+   (fconf).
 
 -  Support for alternative boot flows, for example to support platforms where
    the EL3 Runtime Software is loaded using other firmware or a separate
@@ -94,9 +94,7 @@
 -  Support for ARMv8.3 pointer authentication in the normal and secure worlds.
    The use of pointer authentication in the normal world is enabled whenever
    architectural support is available, without the need for additional build
-   flags. Use of pointer authentication in the secure world remains an
-   experimental configuration at this time and requires the
-   ``BRANCH_PROTECTION`` option to be set to non-zero.
+   flags.
 
 -  Position-Independent Executable (PIE) support. Currently for BL2, BL31, and
    TSP, with further support to be added in a future release.
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index 7db81fc..337dde6 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -217,6 +217,8 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Alexei Fedorov <Alexei.Fedorov@arm.com>
 :|G|: `AlexeiFedorov`_
+:|M|: Chris Kay <chris.kay@arm.com>
+:|G|: `CJKay`_
 :|F|: lib/extensions/amu/
 
 Memory Partitioning And Monitoring (MPAM) extensions
@@ -326,6 +328,13 @@
 :|F|: drivers/scmi-msg
 :|F|: include/drivers/scmi\*
 
+Max Power Mitigation Mechanism (MPMM)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Chris Kay <chris.kay@arm.com>
+:|G|: `CJKay`_
+:|F|: include/lib/mpmm/
+:|F|: lib/mpmm/
+
 Platform Ports
 ~~~~~~~~~~~~~~
 
diff --git a/docs/components/activity-monitors.rst b/docs/components/activity-monitors.rst
new file mode 100644
index 0000000..dd45c43
--- /dev/null
+++ b/docs/components/activity-monitors.rst
@@ -0,0 +1,34 @@
+Activity Monitors
+=================
+
+FEAT_AMUv1 of the Armv8-A architecture introduces the Activity Monitors
+extension. This extension describes the architecture for the Activity Monitor
+Unit (|AMU|), an optional non-invasive component for monitoring core events
+through a set of 64-bit counters.
+
+When the ``ENABLE_AMU=1`` build option is provided, Trusted Firmware-A sets up
+the |AMU| prior to its exit from EL3, and will save and restore architected
+|AMU| counters as necessary upon suspend and resume.
+
+.. _Activity Monitor Auxiliary Counters:
+
+Auxiliary counters
+------------------
+
+FEAT_AMUv1 describes a set of implementation-defined auxiliary counters (also
+known as group 1 counters), controlled by the ``ENABLE_AMU_AUXILIARY_COUNTERS``
+build option.
+
+As a security precaution, Trusted Firmware-A does not enable these by default.
+Instead, platforms may configure their auxiliary counters through one of two
+possible mechanisms:
+
+- |FCONF|, controlled by the ``ENABLE_AMU_FCONF`` build option.
+- A platform implementation of the ``plat_amu_topology`` function (the default).
+
+See :ref:`Activity Monitor Unit (AMU) Bindings` for documentation on the |FCONF|
+device tree bindings.
+
+--------------
+
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/docs/components/fconf/amu-bindings.rst b/docs/components/fconf/amu-bindings.rst
new file mode 100644
index 0000000..047f75e
--- /dev/null
+++ b/docs/components/fconf/amu-bindings.rst
@@ -0,0 +1,142 @@
+Activity Monitor Unit (AMU) Bindings
+====================================
+
+To support platform-defined Activity Monitor Unit (|AMU|) auxiliary counters
+through FCONF, the ``HW_CONFIG`` device tree accepts several |AMU|-specific
+nodes and properties.
+
+Bindings
+^^^^^^^^
+
+.. contents::
+    :local:
+
+``/cpus/cpus/cpu*`` node properties
+"""""""""""""""""""""""""""""""""""
+
+The ``cpu`` node has been augmented to support a handle to an associated |AMU|
+view, which should describe the counters offered by the core.
+
++---------------+-------+---------------+-------------------------------------+
+| Property name | Usage | Value type    | Description                         |
++===============+=======+===============+=====================================+
+| ``amu``       | O     | ``<phandle>`` | If present, indicates that an |AMU| |
+|               |       |               | is available and its counters are   |
+|               |       |               | described by the node provided.     |
++---------------+-------+---------------+-------------------------------------+
+
+``/cpus/amus`` node properties
+""""""""""""""""""""""""""""""
+
+The ``amus`` node describes the |AMUs| implemented by the cores in the system.
+This node does not have any properties.
+
+``/cpus/amus/amu*`` node properties
+"""""""""""""""""""""""""""""""""""
+
+An ``amu`` node describes the layout and meaning of the auxiliary counter
+registers of one or more |AMUs|, and may be shared by multiple cores.
+
++--------------------+-------+------------+------------------------------------+
+| Property name      | Usage | Value type | Description                        |
++====================+=======+============+====================================+
+| ``#address-cells`` | R     | ``<u32>``  | Value shall be 1. Specifies that   |
+|                    |       |            | the ``reg`` property array of      |
+|                    |       |            | children of this node uses a       |
+|                    |       |            | single cell.                       |
++--------------------+-------+------------+------------------------------------+
+| ``#size-cells``    | R     | ``<u32>``  | Value shall be 0. Specifies that   |
+|                    |       |            | no size is required in the ``reg`` |
+|                    |       |            | property in children of this node. |
++--------------------+-------+------------+------------------------------------+
+
+``/cpus/amus/amu*/counter*`` node properties
+""""""""""""""""""""""""""""""""""""""""""""
+
+A ``counter`` node describes an auxiliary counter belonging to the parent |AMU|
+view.
+
++-------------------+-------+-------------+------------------------------------+
+| Property name     | Usage | Value type  | Description                        |
++===================+=======+=============+====================================+
+| ``reg``           | R     | array       | Represents the counter register    |
+|                   |       |             | index, and must be a single cell.  |
++-------------------+-------+-------------+------------------------------------+
+| ``enable-at-el3`` | O     | ``<empty>`` | The presence of this property      |
+|                   |       |             | indicates that this counter should |
+|                   |       |             | be enabled prior to EL3 exit.      |
++-------------------+-------+-------------+------------------------------------+
+
+Example
+^^^^^^^
+
+An example system offering four cores made up of two clusters, where the cores
+of each cluster share different |AMUs|, may use something like the following:
+
+.. code-block::
+
+    cpus {
+        #address-cells = <2>;
+        #size-cells = <0>;
+
+        amus {
+            amu0: amu-0 {
+                #address-cells = <1>;
+                #size-cells = <0>;
+
+                counterX: counter@0 {
+                    reg = <0>;
+
+                    enable-at-el3;
+                };
+
+                counterY: counter@1 {
+                    reg = <1>;
+
+                    enable-at-el3;
+                };
+            };
+
+            amu1: amu-1 {
+                #address-cells = <1>;
+                #size-cells = <0>;
+
+                counterZ: counter@0 {
+                    reg = <0>;
+
+                    enable-at-el3;
+                };
+            };
+        };
+
+        cpu0@00000 {
+            ...
+
+            amu = <&amu0>;
+        };
+
+        cpu1@00100 {
+            ...
+
+            amu = <&amu0>;
+        };
+
+        cpu2@10000 {
+            ...
+
+            amu = <&amu1>;
+        };
+
+        cpu3@10100 {
+            ...
+
+            amu = <&amu1>;
+        };
+    }
+
+In this situation, ``cpu0`` and ``cpu1`` (the two cores in the first cluster),
+share the view of their AMUs defined by ``amu0``. Likewise, ``cpu2`` and
+``cpu3`` (the two cores in the second cluster), share the view of their |AMUs|
+defined by ``amu1``. This will cause ``counterX`` and ``counterY`` to be enabled
+for both ``cpu0`` and ``cpu1``, and ``counterZ`` to be enabled for both ``cpu2``
+and ``cpu3``.
diff --git a/docs/components/fconf/index.rst b/docs/components/fconf/index.rst
index 9020633..029f324 100644
--- a/docs/components/fconf/index.rst
+++ b/docs/components/fconf/index.rst
@@ -145,3 +145,5 @@
   :maxdepth: 1
 
   fconf_properties
+  amu-bindings
+  mpmm-bindings
diff --git a/docs/components/fconf/mpmm-bindings.rst b/docs/components/fconf/mpmm-bindings.rst
new file mode 100644
index 0000000..d3cc857
--- /dev/null
+++ b/docs/components/fconf/mpmm-bindings.rst
@@ -0,0 +1,48 @@
+Maximum Power Mitigation Mechanism (MPMM) Bindings
+==================================================
+
+|MPMM| support cannot be determined at runtime by the firmware. Instead, these
+DTB bindings allow the platform to communicate per-core support for |MPMM| via
+the ``HW_CONFIG`` device tree blob.
+
+Bindings
+^^^^^^^^
+
+.. contents::
+    :local:
+
+``/cpus/cpus/cpu*`` node properties
+"""""""""""""""""""""""""""""""""""
+
+The ``cpu`` node has been augmented to allow the platform to indicate support
+for |MPMM| on a given core.
+
++-------------------+-------+-------------+------------------------------------+
+| Property name     | Usage | Value type  | Description                        |
++===================+=======+=============+====================================+
+| ``supports-mpmm`` | O     | ``<empty>`` | If present, indicates that |MPMM|  |
+|                   |       |             | is available on this core.         |
++-------------------+-------+-------------+------------------------------------+
+
+Example
+^^^^^^^
+
+An example system offering two cores, one with support for |MPMM| and one
+without, can be described as follows:
+
+.. code-block::
+
+    cpus {
+        #address-cells = <2>;
+        #size-cells = <0>;
+
+        cpu0@00000 {
+            ...
+
+            supports-mpmm;
+        };
+
+        cpu1@00100 {
+            ...
+        };
+    }
diff --git a/docs/components/index.rst b/docs/components/index.rst
index f349d8d..754526d 100644
--- a/docs/components/index.rst
+++ b/docs/components/index.rst
@@ -7,12 +7,14 @@
    :numbered:
 
    spd/index
+   activity-monitors
    arm-sip-service
    debugfs-design
    exception-handling
    fconf/index
    firmware-update
    measured_boot/index
+   mpmm
    platform-interrupt-controller-API
    ras
    romlib-design
diff --git a/docs/components/measured_boot/event_log.rst b/docs/components/measured_boot/event_log.rst
index 5347dcc..0881248 100644
--- a/docs/components/measured_boot/event_log.rst
+++ b/docs/components/measured_boot/event_log.rst
@@ -9,7 +9,7 @@
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 Measured Boot driver expects a *tpm_event_log* node with the following field
-in 'nt_fw_config' and 'tsp_fw_config' DTS files:
+in 'tb_fw_config', 'nt_fw_config' and 'tsp_fw_config' DTS files:
 
 - compatible [mandatory]
    - value type: <string>
diff --git a/docs/components/mpmm.rst b/docs/components/mpmm.rst
new file mode 100644
index 0000000..1b1c6d8
--- /dev/null
+++ b/docs/components/mpmm.rst
@@ -0,0 +1,30 @@
+Maximum Power Mitigation Mechanism (MPMM)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+|MPMM| is an optional microarchitectural power management mechanism supported by
+some Arm Armv9-A cores, beginning with the Cortex-X2, Cortex-A710 and
+Cortex-A510 cores. This mechanism detects and limits high-activity events to
+assist in |SoC| processor power domain dynamic power budgeting and limit the
+triggering of whole-rail (i.e. clock chopping) responses to overcurrent
+conditions.
+
+|MPMM| is enabled on a per-core basis by the EL3 runtime firmware. The presence
+of |MPMM| cannot be determined at runtime by the firmware, and therefore the
+platform must expose this information through one of two possible mechanisms:
+
+- |FCONF|, controlled by the ``ENABLE_MPMM_FCONF`` build option.
+- A platform implementation of the ``plat_mpmm_topology`` function (the
+  default).
+
+See :ref:`Maximum Power Mitigation Mechanism (MPMM) Bindings` for documentation
+on the |FCONF| device tree bindings.
+
+.. warning::
+
+    |MPMM| exposes gear metrics through the auxiliary |AMU| counters. An
+    external power controller can use these metrics to budget SoC power by
+    limiting the number of cores that can execute higher-activity workloads or
+    switching to a different DVFS operating point. When this is the case, the
+    |AMU| counters that make up the |MPMM| gears must be enabled by the EL3
+    runtime firmware - please see :ref:`Activity Monitor Auxiliary Counters` for
+    documentation on enabling auxiliary |AMU| counters.
diff --git a/docs/design/firmware-design.rst b/docs/design/firmware-design.rst
index c12e73f..ef500ff 100644
--- a/docs/design/firmware-design.rst
+++ b/docs/design/firmware-design.rst
@@ -2616,8 +2616,6 @@
    ``CTX_INCLUDE_PAUTH_REGS`` to 1. This enables pointer authentication in BL1,
    BL2, BL31, and the TSP if it is used.
 
-   These options are experimental features.
-
    Note that Pointer Authentication is enabled for Non-secure world irrespective
    of the value of these build flags if the CPU supports it.
 
@@ -2629,8 +2627,7 @@
 ~~~~~~~~~
 
 -  Branch Target Identification feature is selected by ``BRANCH_PROTECTION``
-   option set to 1. This option defaults to 0 and this is an experimental
-   feature.
+   option set to 1. This option defaults to 0.
 
 -  Memory Tagging Extension feature is unconditionally enabled for both worlds
    (at EL0 and S-EL0) if it is only supported at EL0. If instead it is
diff --git a/docs/design/trusted-board-boot.rst b/docs/design/trusted-board-boot.rst
index 96cf24c..46177d7 100644
--- a/docs/design/trusted-board-boot.rst
+++ b/docs/design/trusted-board-boot.rst
@@ -239,9 +239,6 @@
 R060_TBBR_FUNCTION as specified in the `Trusted Board Boot Requirements (TBBR)`_
 document.
 
-Note that due to security considerations and complexity of this feature, it is
-marked as experimental.
-
 Firmware Encryption Tool
 ------------------------
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 1259881..04e3c0b 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -120,7 +120,7 @@
    |   4   |     bti      |   N   |  Y  |
    +-------+--------------+-------+-----+
 
-   This option defaults to 0 and this is an experimental feature.
+   This option defaults to 0.
    Note that Pointer Authentication is enabled for Non-secure world
    irrespective of the value of this option if the CPU supports it.
 
@@ -181,7 +181,7 @@
 -  ``CTX_INCLUDE_PAUTH_REGS``: Boolean option that, when set to 1, enables
    Pointer Authentication for Secure world. This will cause the ARMv8.3-PAuth
    registers to be included when saving and restoring the CPU context as
-   part of world switch. Default value is 0 and this is an experimental feature.
+   part of world switch. Default value is 0.
    Note that Pointer Authentication is enabled for Non-secure world irrespective
    of the value of this flag if the CPU supports it.
 
@@ -192,7 +192,7 @@
    authenticated decryption algorithm to be used to decrypt firmware/s during
    boot. It accepts 2 values: ``aes_gcm`` and ``none``. The default value of
    this flag is ``none`` to disable firmware decryption which is an optional
-   feature as per TBBR. Also, it is an experimental feature.
+   feature as per TBBR.
 
 -  ``DISABLE_BIN_GENERATION``: Boolean option to disable the generation
    of the binary image. If set to 1, then only the ELF image is built.
@@ -220,6 +220,14 @@
    v8.2 implementations also implement an AMU and this option can be used to
    enable this feature on those systems as well. Default is 0.
 
+-  ``ENABLE_AMU_AUXILIARY_COUNTERS``: Enables support for AMU auxiliary counters
+   (also known as group 1 counters). These are implementation-defined counters,
+   and as such require additional platform configuration. Default is 0.
+
+-  ``ENABLE_AMU_FCONF``: Enables configuration of the AMU through FCONF, which
+   allows platforms with auxiliary counters to describe them via the
+   ``HW_CONFIG`` device tree blob. Default is 0.
+
 -  ``ENABLE_ASSERTIONS``: This option controls whether or not calls to ``assert()``
    are compiled out. For debug builds, this option defaults to 1, and calls to
    ``assert()`` are left in place. For release builds, this option defaults to 0
@@ -257,6 +265,16 @@
    partitioning in EL3, however. Platform initialisation code should configure
    and use partitions in EL3 as required. This option defaults to ``0``.
 
+-  ``ENABLE_MPMM``: Boolean option to enable support for the Maximum Power
+   Mitigation Mechanism supported by certain Arm cores, which allows the SoC
+   firmware to detect and limit high activity events to assist in SoC processor
+   power domain dynamic power budgeting and limit the triggering of whole-rail
+   (i.e. clock chopping) responses to overcurrent conditions. Defaults to ``0``.
+
+-  ``ENABLE_MPMM_FCONF``: Enables configuration of MPMM through FCONF, which
+   allows platforms with cores supporting MPMM to describe them via the
+   ``HW_CONFIG`` device tree blob. Default is 0.
+
 -  ``ENABLE_PIE``: Boolean option to enable Position Independent Executable(PIE)
    support within generic code in TF-A. This option is currently only supported
    in BL2_AT_EL3, BL31, and BL32 (TSP) for AARCH64 binaries, and in BL32
@@ -312,20 +330,18 @@
    component of the option ``-fstack-protector-$ENABLE_STACK_PROTECTOR``.
 
 -  ``ENCRYPT_BL31``: Binary flag to enable encryption of BL31 firmware. This
-   flag depends on ``DECRYPTION_SUPPORT`` build flag which is marked as
-   experimental.
+   flag depends on ``DECRYPTION_SUPPORT`` build flag.
 
 -  ``ENCRYPT_BL32``: Binary flag to enable encryption of Secure BL32 payload.
-   This flag depends on ``DECRYPTION_SUPPORT`` build flag which is marked as
-   experimental.
+   This flag depends on ``DECRYPTION_SUPPORT`` build flag.
 
 -  ``ENC_KEY``: A 32-byte (256-bit) symmetric key in hex string format. It could
    either be SSK or BSSK depending on ``FW_ENC_STATUS`` flag. This value depends
-   on ``DECRYPTION_SUPPORT`` build flag which is marked as experimental.
+   on ``DECRYPTION_SUPPORT`` build flag.
 
 -  ``ENC_NONCE``: A 12-byte (96-bit) encryption nonce or Initialization Vector
    (IV) in hex string format. This value depends on ``DECRYPTION_SUPPORT``
-   build flag which is marked as experimental.
+   build flag.
 
 -  ``ERROR_DEPRECATED``: This option decides whether to treat the usage of
    deprecated platform APIs, helper functions or drivers within Trusted
@@ -364,8 +380,7 @@
      1: Encryption is done with Binding Secret Symmetric Key (BSSK) which is
         unique per device.
 
-   This flag depends on ``DECRYPTION_SUPPORT`` build flag which is marked as
-   experimental.
+   This flag depends on ``DECRYPTION_SUPPORT`` build flag.
 
 -  ``GENERATE_COT``: Boolean flag used to build and execute the ``cert_create``
    tool to create certificates as per the Chain of Trust described in
@@ -484,8 +499,7 @@
    in order to provide trust that the code taking the measurements and recording
    them has not been tampered with.
 
-   This option defaults to 0 and is an experimental feature in the stage of
-   development.
+   This option defaults to 0.
 
 -  ``NON_TRUSTED_WORLD_KEY``: This option is used when ``GENERATE_COT=1``. It
    specifies the file that contains the Non-Trusted World private key in PEM
@@ -695,26 +709,25 @@
 -  ``ARM_IO_IN_DTB``: This flag determines whether to use IO based on the
    firmware configuration framework. This will move the io_policies into a
    configuration device tree, instead of static structure in the code base.
-   This is currently an experimental feature.
 
 -  ``COT_DESC_IN_DTB``: This flag determines whether to create COT descriptors
    at runtime using fconf. If this flag is enabled, COT descriptors are
    statically captured in tb_fw_config file in the form of device tree nodes
    and properties. Currently, COT descriptors used by BL2 are moved to the
    device tree and COT descriptors used by BL1 are retained in the code
-   base statically. This is currently an experimental feature.
+   base statically.
 
 -  ``SDEI_IN_FCONF``: This flag determines whether to configure SDEI setup in
    runtime using firmware configuration framework. The platform specific SDEI
    shared and private events configuration is retrieved from device tree rather
-   than static C structures at compile time. This is currently an experimental
-   feature and is only supported if SDEI_SUPPORT build flag is enabled.
+   than static C structures at compile time. This is only supported if
+   SDEI_SUPPORT build flag is enabled.
 
 -  ``SEC_INT_DESC_IN_FCONF``: This flag determines whether to configure Group 0
    and Group1 secure interrupts using the firmware configuration framework. The
    platform specific secure interrupt property descriptor is retrieved from
    device tree in runtime rather than depending on static C structure at compile
-   time. This is currently an experimental feature.
+   time.
 
 -  ``USE_ROMLIB``: This flag determines whether library at ROM will be used.
    This feature creates a library of functions to be placed in ROM and thus
@@ -918,4 +931,3 @@
 
 .. _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 54754fe..92ff39f 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -562,15 +562,6 @@
    doesn't print anything to the console. If ``PLAT_LOG_LEVEL_ASSERT`` isn't
    defined, it defaults to ``LOG_LEVEL``.
 
-If the platform port uses the Activity Monitor Unit, the following constant
-may be defined:
-
--  **PLAT_AMU_GROUP1_COUNTERS_MASK**
-   This mask reflects the set of group counters that should be enabled.  The
-   maximum number of group 1 counters supported by AMUv1 is 16 so the mask
-   can be at most 0xffff. If the platform does not define this mask, no group 1
-   counters are enabled.
-
 File : plat_macros.S [mandatory]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -891,8 +882,7 @@
 
 On success the function should return 0 and a negative error code otherwise.
 
-Note that this API depends on ``DECRYPTION_SUPPORT`` build flag which is
-marked as experimental.
+Note that this API depends on ``DECRYPTION_SUPPORT`` build flag.
 
 Function : plat_fwu_set_images_source() [when PSA_FWU_SUPPORT == 1]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -1199,6 +1189,25 @@
 the SMCCC function specified in the argument; otherwise returns
 SMC_ARCH_CALL_NOT_SUPPORTED.
 
+Function : plat_mboot_measure_image()
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : unsigned int, image_info_t *
+    Return   : void
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function measures the given image and records its measurement using
+   the measured boot backend driver.
+-  On the Arm FVP port, this function measures the given image using its
+   passed id and information and then records that measurement in the
+   Event Log buffer.
+-  This function must return 0 on success, a negative error code otherwise.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
 Modifications specific to a Boot Loader stage
 ---------------------------------------------
 
@@ -1450,6 +1459,42 @@
 The default implementation of this function asserts therefore platforms must
 override it when using the FWU feature.
 
+Function : bl1_plat_mboot_init() [optional]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : void
+    Return   : void
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function is used to initialize the backend driver(s) of measured boot.
+-  On the Arm FVP port, this function is used to initialize the Event Log
+   backend driver, and also to write header information in the Event Log buffer.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
+Function : bl1_plat_mboot_finish() [optional]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : void
+    Return   : void
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function is used to finalize the measured boot backend driver(s),
+   and also, set the information for the next bootloader component to
+   extend the measurement if needed.
+-  On the Arm FVP port, this function is used to pass the base address of
+   the Event Log buffer and its size to BL2 via tb_fw_config to extend the
+   Event Log buffer with the measurement of various images loaded by BL2.
+   It results in panic on error.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
 Boot Loader Stage 2 (BL2)
 -------------------------
 
@@ -1738,6 +1783,42 @@
 This function returns 0 on success, a negative error code otherwise.
 This function is included if SCP_BL2U_BASE is defined.
 
+Function : bl2_plat_mboot_init() [optional]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : void
+    Return   : void
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function is used to initialize the backend driver(s) of measured boot.
+-  On the Arm FVP port, this function is used to initialize the Event Log
+   backend driver with the Event Log buffer information (base address and
+   size) received from BL1. It results in panic on error.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
+Function : bl2_plat_mboot_finish() [optional]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : void
+    Return   : void
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function is used to finalize the measured boot backend driver(s),
+   and also, set the information for the next bootloader component to extend
+   the measurement if needed.
+-  On the Arm FVP port, this function is used to pass the Event Log buffer
+   information (base address and size) to non-secure(BL33) and trusted OS(BL32)
+   via nt_fw and tos_fw config respectively. It results in panic on error.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
 Boot Loader Stage 3-1 (BL31)
 ----------------------------
 
diff --git a/docs/getting_started/prerequisites.rst b/docs/getting_started/prerequisites.rst
index aa1ae67..f45193a 100644
--- a/docs/getting_started/prerequisites.rst
+++ b/docs/getting_started/prerequisites.rst
@@ -26,7 +26,7 @@
 |TF-A| can be built with any of the following *cross-compiler* toolchains that
 target the Armv7-A or Armv8-A architectures:
 
-- GCC >= 10.2-2020.11 (from the `Arm Developer website`_)
+- GCC >= 10.3-2021.07 (from the `Arm Developer website`_)
 - Clang >= 4.0
 - Arm Compiler >= 6.0
 
diff --git a/docs/global_substitutions.txt b/docs/global_substitutions.txt
index 24ac830..af15146 100644
--- a/docs/global_substitutions.txt
+++ b/docs/global_substitutions.txt
@@ -1,5 +1,7 @@
 .. |AArch32| replace:: :term:`AArch32`
 .. |AArch64| replace:: :term:`AArch64`
+.. |AMU| replace:: :term:`AMU`
+.. |AMUs| replace:: :term:`AMUs <AMU>`
 .. |API| replace:: :term:`API`
 .. |BTI| replace:: :term:`BTI`
 .. |CoT| replace:: :term:`CoT`
@@ -23,6 +25,7 @@
 .. |Linaro| replace:: :term:`Linaro`
 .. |MMU| replace:: :term:`MMU`
 .. |MPAM| replace:: :term:`MPAM`
+.. |MPMM| replace:: :term:`MPMM`
 .. |MPIDR| replace:: :term:`MPIDR`
 .. |MTE| replace:: :term:`MTE`
 .. |OEN| replace:: :term:`OEN`
diff --git a/docs/glossary.rst b/docs/glossary.rst
index f4912f5..aeeb133 100644
--- a/docs/glossary.rst
+++ b/docs/glossary.rst
@@ -15,6 +15,10 @@
    AArch64
       64-bit execution state of the ARMv8 ISA
 
+   AMU
+      Activity Monitor Unit, a hardware monitoring unit introduced by FEAT_AMUv1
+      that exposes CPU core runtime metrics as a set of counter registers.
+
    API
       Application Programming Interface
 
@@ -88,6 +92,10 @@
    MPAM
       Memory Partitioning And Monitoring. An optional Armv8.4 extension.
 
+   MPMM
+     Maximum Power Mitigation Mechanism, an optional power management mechanism
+     supported by some Arm Armv9-A cores.
+
    MPIDR
       Multiprocessor Affinity Register
 
diff --git a/docs/plat/stm32mp1.rst b/docs/plat/stm32mp1.rst
index 17f7a86..af302c6 100644
--- a/docs/plat/stm32mp1.rst
+++ b/docs/plat/stm32mp1.rst
@@ -181,6 +181,7 @@
 .. code:: bash
 
     make CROSS_COMPILE=arm-none-eabi- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        AARCH32_SP=optee \
         DTB_FILE_NAME=stm32mp157c-ev1.dtb \
         BL33=<u-boot_directory>/u-boot-nodtb.bin \
         BL33_CFG=<u-boot_directory>/u-boot.dtb \
diff --git a/drivers/arm/css/scp/css_pm_scmi.c b/drivers/arm/css/scp/css_pm_scmi.c
index aeb7eda..5de2604 100644
--- a/drivers/arm/css/scp/css_pm_scmi.c
+++ b/drivers/arm/css/scp/css_pm_scmi.c
@@ -357,7 +357,7 @@
 	unsigned int composite_id, idx;
 
 	for (idx = 0; idx < PLAT_ARM_SCMI_CHANNEL_COUNT; idx++) {
-		INFO("Initializing driver on Channel %d\n", idx);
+		INFO("Initializing SCMI driver on channel %d\n", idx);
 
 		scmi_channels[idx].info = plat_css_get_scmi_info(idx);
 		scmi_channels[idx].lock = ARM_SCMI_LOCK_GET_INSTANCE;
diff --git a/drivers/arm/gic/v3/gic600_multichip.c b/drivers/arm/gic/v3/gic600_multichip.c
index fd3d8c2..5f42ad9 100644
--- a/drivers/arm/gic/v3/gic600_multichip.c
+++ b/drivers/arm/gic/v3/gic600_multichip.c
@@ -18,8 +18,6 @@
 #include "../common/gic_common_private.h"
 #include "gic600_multichip_private.h"
 
-#warning "GIC-600 Multichip driver is currently experimental and the API may change in future."
-
 /*******************************************************************************
  * GIC-600 multichip operation related helper functions
  ******************************************************************************/
@@ -212,8 +210,6 @@
 
 	gic600_multichip_validate_data(multichip_data);
 
-	INFO("GIC-600 Multichip driver is experimental\n");
-
 	/*
 	 * Ensure that G0/G1S/G1NS interrupts are disabled. This also ensures
 	 * that GIC-600 Multichip configuration is done first.
diff --git a/drivers/auth/cryptocell/713/cryptocell_crypto.c b/drivers/auth/cryptocell/713/cryptocell_crypto.c
index 5f390a2..077317e 100644
--- a/drivers/auth/cryptocell/713/cryptocell_crypto.c
+++ b/drivers/auth/cryptocell/713/cryptocell_crypto.c
@@ -13,6 +13,7 @@
 #include <drivers/auth/crypto_mod.h>
 
 #include <mbedtls/oid.h>
+#include <mbedtls/x509.h>
 
 #define LIB_NAME		"CryptoCell 713 SBROM"
 #define RSA_SALT_LEN		32
diff --git a/drivers/marvell/comphy/phy-comphy-3700.c b/drivers/marvell/comphy/phy-comphy-3700.c
index 0ad14a8..a3e414c 100644
--- a/drivers/marvell/comphy/phy-comphy-3700.c
+++ b/drivers/marvell/comphy/phy-comphy-3700.c
@@ -815,6 +815,12 @@
 
 	debug_enter();
 
+	/* Configure phy selector for PCIe */
+	ret = mvebu_a3700_comphy_set_phy_selector(comphy_index, comphy_mode);
+	if (ret) {
+		return ret;
+	}
+
 	/* 1. Enable max PLL. */
 	reg_set16(LANE_CFG1_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  USE_MAX_PLL_RATE_EN, USE_MAX_PLL_RATE_EN);
diff --git a/drivers/measured_boot/event_log.c b/drivers/measured_boot/event_log.c
deleted file mode 100644
index 0157b03..0000000
--- a/drivers/measured_boot/event_log.c
+++ /dev/null
@@ -1,359 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <errno.h>
-#include <string.h>
-#include <arch_helpers.h>
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <drivers/auth/crypto_mod.h>
-#include <drivers/measured_boot/event_log.h>
-#include <mbedtls/md.h>
-
-#include <plat/common/platform.h>
-
-/* Event Log data */
-static uint8_t event_log[EVENT_LOG_SIZE];
-
-/* End of Event Log */
-#define	EVENT_LOG_END	((uintptr_t)event_log + sizeof(event_log) - 1U)
-
-CASSERT(sizeof(event_log) >= LOG_MIN_SIZE, assert_event_log_size);
-
-/* Pointer in event_log[] */
-static uint8_t *log_ptr = event_log;
-
-/* Pointer to measured_boot_data_t */
-const static measured_boot_data_t *plat_data_ptr;
-
-static uintptr_t tos_fw_config_base;
-static uintptr_t nt_fw_config_base;
-
-/* TCG_EfiSpecIdEvent */
-static const id_event_headers_t id_event_header = {
-	.header = {
-		.pcr_index = PCR_0,
-		.event_type = EV_NO_ACTION,
-		.digest = {0},
-		.event_size = (uint32_t)(sizeof(id_event_struct_t) +
-				(sizeof(id_event_algorithm_size_t) *
-				HASH_ALG_COUNT))
-	},
-
-	.struct_header = {
-		.signature = TCG_ID_EVENT_SIGNATURE_03,
-		.platform_class = PLATFORM_CLASS_CLIENT,
-		.spec_version_minor = TCG_SPEC_VERSION_MINOR_TPM2,
-		.spec_version_major = TCG_SPEC_VERSION_MAJOR_TPM2,
-		.spec_errata = TCG_SPEC_ERRATA_TPM2,
-		.uintn_size = (uint8_t)(sizeof(unsigned int) /
-					sizeof(uint32_t)),
-		.number_of_algorithms = HASH_ALG_COUNT
-	}
-};
-
-static const event2_header_t locality_event_header = {
-	/*
-	 * All EV_NO_ACTION events SHALL set
-	 * TCG_PCR_EVENT2.pcrIndex = 0, unless otherwise specified
-	 */
-	.pcr_index = PCR_0,
-
-	/*
-	 * All EV_NO_ACTION events SHALL set
-	 * TCG_PCR_EVENT2.eventType = 03h
-	 */
-	.event_type = EV_NO_ACTION,
-
-	/*
-	 * All EV_NO_ACTION events SHALL set TCG_PCR_EVENT2.digests to all
-	 * 0x00's for each allocated Hash algorithm
-	 */
-	.digests = {
-		.count = HASH_ALG_COUNT
-	}
-};
-
-/*
- * Add TCG_PCR_EVENT2 event
- *
- * @param[in] hash	Pointer to hash data of TCG_DIGEST_SIZE bytes
- * @param[in] image_ptr	Pointer to image_data_t structure
- *
- * There must be room for storing this new event into the event log buffer.
- */
-static void add_event2(const uint8_t *hash, const image_data_t *image_ptr)
-{
-	void *ptr = log_ptr;
-	uint32_t name_len;
-
-	assert(image_ptr != NULL);
-	assert(image_ptr->name != NULL);
-
-	name_len = (uint32_t)strlen(image_ptr->name) + 1U;
-
-	/* Check for space in Event Log buffer */
-	assert(((uintptr_t)ptr + (uint32_t)EVENT2_HDR_SIZE + name_len) <=
-	       EVENT_LOG_END);
-
-	/*
-	 * As per TCG specifications, firmware components that are measured
-	 * into PCR[0] must be logged in the event log using the event type
-	 * EV_POST_CODE.
-	 */
-	/* TCG_PCR_EVENT2.PCRIndex */
-	((event2_header_t *)ptr)->pcr_index = image_ptr->pcr;
-
-	/* TCG_PCR_EVENT2.EventType */
-	((event2_header_t *)ptr)->event_type = EV_POST_CODE;
-
-	/* TCG_PCR_EVENT2.Digests.Count */
-	ptr = (uint8_t *)ptr + offsetof(event2_header_t, digests);
-	((tpml_digest_values *)ptr)->count = HASH_ALG_COUNT;
-
-	/* TCG_PCR_EVENT2.Digests[] */
-	ptr = (uint8_t *)((uintptr_t)ptr +
-			offsetof(tpml_digest_values, digests));
-
-	/* TCG_PCR_EVENT2.Digests[].AlgorithmId */
-	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
-
-	/* TCG_PCR_EVENT2.Digests[].Digest[] */
-	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(tpmt_ha, digest));
-
-	if (hash == NULL) {
-		/* Get BL2 hash from DTB */
-		bl2_plat_get_hash(ptr);
-	} else {
-		/* Copy digest */
-		(void)memcpy(ptr, (const void *)hash, TCG_DIGEST_SIZE);
-	}
-
-	/* TCG_PCR_EVENT2.EventSize */
-	ptr = (uint8_t *)((uintptr_t)ptr + TCG_DIGEST_SIZE);
-	((event2_data_t *)ptr)->event_size = name_len;
-
-	/* Copy event data to TCG_PCR_EVENT2.Event */
-	(void)memcpy((void *)(((event2_data_t *)ptr)->event),
-			(const void *)image_ptr->name, name_len);
-
-	/* End of event data */
-	log_ptr = (uint8_t *)((uintptr_t)ptr +
-			offsetof(event2_data_t, event) + name_len);
-}
-
-/*
- * Init Event Log
- *
- * Initialises Event Log by writing Specification ID and
- * Startup Locality events.
- */
-void event_log_init(void)
-{
-	const char locality_signature[] = TCG_STARTUP_LOCALITY_SIGNATURE;
-	void *ptr = event_log;
-
-	/* Get pointer to platform's measured_boot_data_t structure */
-	plat_data_ptr = plat_get_measured_boot_data();
-
-	/*
-	 * Add Specification ID Event first
-	 *
-	 * Copy TCG_EfiSpecIDEventStruct structure header
-	 */
-	(void)memcpy(ptr, (const void *)&id_event_header,
-			sizeof(id_event_header));
-	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(id_event_header));
-
-	/* TCG_EfiSpecIdEventAlgorithmSize structure */
-	((id_event_algorithm_size_t *)ptr)->algorithm_id = TPM_ALG_ID;
-	((id_event_algorithm_size_t *)ptr)->digest_size = TCG_DIGEST_SIZE;
-	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(id_event_algorithm_size_t));
-
-	/*
-	 * TCG_EfiSpecIDEventStruct.vendorInfoSize
-	 * No vendor data
-	 */
-	((id_event_struct_data_t *)ptr)->vendor_info_size = 0;
-	ptr = (uint8_t *)((uintptr_t)ptr +
-			offsetof(id_event_struct_data_t, vendor_info));
-
-	/*
-	 * The Startup Locality event should be placed in the log before
-	 * any event which extends PCR[0].
-	 *
-	 * Ref. TCG PC Client Platform Firmware Profile 9.4.5.3
-	 */
-
-	/* Copy Startup Locality Event Header */
-	(void)memcpy(ptr, (const void *)&locality_event_header,
-			sizeof(locality_event_header));
-	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(locality_event_header));
-
-	/* TCG_PCR_EVENT2.Digests[].AlgorithmId */
-	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
-
-	/* TCG_PCR_EVENT2.Digests[].Digest[] */
-	(void)memset(&((tpmt_ha *)ptr)->digest, 0, TPM_ALG_ID);
-	ptr = (uint8_t *)((uintptr_t)ptr +
-			offsetof(tpmt_ha, digest) + TCG_DIGEST_SIZE);
-
-	/* TCG_PCR_EVENT2.EventSize */
-	((event2_data_t *)ptr)->event_size =
-		(uint32_t)sizeof(startup_locality_event_t);
-	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(event2_data_t, event));
-
-	/* TCG_EfiStartupLocalityEvent.Signature */
-	(void)memcpy(ptr, (const void *)locality_signature,
-		sizeof(TCG_STARTUP_LOCALITY_SIGNATURE));
-
-	/*
-	 * TCG_EfiStartupLocalityEvent.StartupLocality = 0:
-	 * the platform's boot firmware
-	 */
-	((startup_locality_event_t *)ptr)->startup_locality = 0U;
-	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(startup_locality_event_t));
-
-	log_ptr = (uint8_t *)ptr;
-
-	/* Add BL2 event */
-	add_event2(NULL, plat_data_ptr->images_data);
-}
-
-/*
- * Calculate and write hash of image, configuration data, etc.
- * to Event Log.
- *
- * @param[in] data_base		Address of data
- * @param[in] data_size		Size of data
- * @param[in] data_id		Data ID
- * @return:
- *	0 = success
- *    < 0 = error
- */
-int tpm_record_measurement(uintptr_t data_base, uint32_t data_size,
-			   uint32_t data_id)
-{
-	const image_data_t *data_ptr = plat_data_ptr->images_data;
-	unsigned char hash_data[MBEDTLS_MD_MAX_SIZE];
-	int rc;
-
-	/* Get the metadata associated with this image. */
-	while ((data_ptr->id != INVALID_ID) && (data_ptr->id != data_id)) {
-		data_ptr++;
-	}
-	assert(data_ptr->id != INVALID_ID);
-
-	if (data_id == TOS_FW_CONFIG_ID) {
-		tos_fw_config_base = data_base;
-	} else if (data_id == NT_FW_CONFIG_ID) {
-		nt_fw_config_base = data_base;
-	} else {
-		/* No action */
-	}
-
-	/* Calculate hash */
-	rc = crypto_mod_calc_hash((unsigned int)MBEDTLS_MD_ID,
-				(void *)data_base, data_size, hash_data);
-	if (rc != 0) {
-		return rc;
-	}
-
-	add_event2(hash_data, data_ptr);
-	return 0;
-}
-
-/*
- * Finalise Event Log
- *
- * @param[out] log_addr	Pointer to return Event Log address
- * @param[out] log_size	Pointer to return Event Log size
- * @return:
- *	0 = success
- *    < 0 = error code
- */
-int event_log_finalise(uint8_t **log_addr, size_t *log_size)
-{
-	/* Event Log size */
-	size_t num_bytes = (uintptr_t)log_ptr - (uintptr_t)event_log;
-	int rc;
-
-	assert(log_addr != NULL);
-	assert(log_size != NULL);
-
-	if (nt_fw_config_base == 0UL) {
-		ERROR("%s(): %s_FW_CONFIG not loaded\n", __func__, "NT");
-		return -ENOENT;
-	}
-
-	/*
-	 * Set Event Log data in NT_FW_CONFIG and
-	 * get Event Log address in Non-Secure memory
-	 */
-	if (plat_data_ptr->set_nt_fw_info != NULL) {
-
-		/* Event Log address in Non-Secure memory */
-		uintptr_t ns_log_addr;
-
-		rc = plat_data_ptr->set_nt_fw_info(
-				nt_fw_config_base,
-#ifdef SPD_opteed
-				(uintptr_t)event_log,
-#endif
-				num_bytes, &ns_log_addr);
-		if (rc != 0) {
-			ERROR("%s(): Unable to update %s_FW_CONFIG\n",
-						__func__, "NT");
-			return rc;
-		}
-
-		/* Copy Event Log to Non-secure memory */
-		(void)memcpy((void *)ns_log_addr, (const void *)event_log,
-				num_bytes);
-
-		/* Ensure that the Event Log is visible in Non-secure memory */
-		flush_dcache_range(ns_log_addr, num_bytes);
-
-		/* Return Event Log address in Non-Secure memory */
-		*log_addr = (uint8_t *)ns_log_addr;
-
-	} else {
-		INFO("%s(): set_%s_fw_info not set\n", __func__, "nt");
-
-		/* Return Event Log address in Secure memory */
-		*log_addr = event_log;
-	}
-
-	if (tos_fw_config_base != 0UL) {
-		if (plat_data_ptr->set_tos_fw_info != NULL) {
-
-			/* Set Event Log data in TOS_FW_CONFIG */
-			rc = plat_data_ptr->set_tos_fw_info(
-						tos_fw_config_base,
-						(uintptr_t)event_log,
-						num_bytes);
-			if (rc != 0) {
-				ERROR("%s(): Unable to update %s_FW_CONFIG\n",
-						__func__, "TOS");
-				return rc;
-			}
-		} else {
-			INFO("%s(): set_%s_fw_info not set\n", __func__, "tos");
-		}
-	} else {
-		INFO("%s(): %s_FW_CONFIG not loaded\n", __func__, "TOS");
-	}
-
-	/* Ensure that the Event Log is visible in Secure memory */
-	flush_dcache_range((uintptr_t)event_log, num_bytes);
-
-	/* Return Event Log size */
-	*log_size = num_bytes;
-
-	return 0;
-}
diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
new file mode 100644
index 0000000..1755dd9
--- /dev/null
+++ b/drivers/measured_boot/event_log/event_log.c
@@ -0,0 +1,284 @@
+/*
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <arch_helpers.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <mbedtls/md.h>
+
+#include <plat/common/platform.h>
+
+/* Running Event Log Pointer */
+static uint8_t *log_ptr;
+
+/* Pointer to the first byte past end of the Event Log buffer */
+static uintptr_t log_end;
+
+/* Pointer to event_log_metadata_t */
+static const event_log_metadata_t *plat_metadata_ptr;
+
+/* TCG_EfiSpecIdEvent */
+static const id_event_headers_t id_event_header = {
+	.header = {
+		.pcr_index = PCR_0,
+		.event_type = EV_NO_ACTION,
+		.digest = {0},
+		.event_size = (uint32_t)(sizeof(id_event_struct_t) +
+				(sizeof(id_event_algorithm_size_t) *
+				HASH_ALG_COUNT))
+	},
+
+	.struct_header = {
+		.signature = TCG_ID_EVENT_SIGNATURE_03,
+		.platform_class = PLATFORM_CLASS_CLIENT,
+		.spec_version_minor = TCG_SPEC_VERSION_MINOR_TPM2,
+		.spec_version_major = TCG_SPEC_VERSION_MAJOR_TPM2,
+		.spec_errata = TCG_SPEC_ERRATA_TPM2,
+		.uintn_size = (uint8_t)(sizeof(unsigned int) /
+					sizeof(uint32_t)),
+		.number_of_algorithms = HASH_ALG_COUNT
+	}
+};
+
+static const event2_header_t locality_event_header = {
+	/*
+	 * All EV_NO_ACTION events SHALL set
+	 * TCG_PCR_EVENT2.pcrIndex = 0, unless otherwise specified
+	 */
+	.pcr_index = PCR_0,
+
+	/*
+	 * All EV_NO_ACTION events SHALL set
+	 * TCG_PCR_EVENT2.eventType = 03h
+	 */
+	.event_type = EV_NO_ACTION,
+
+	/*
+	 * All EV_NO_ACTION events SHALL set TCG_PCR_EVENT2.digests to all
+	 * 0x00's for each allocated Hash algorithm
+	 */
+	.digests = {
+		.count = HASH_ALG_COUNT
+	}
+};
+
+/*
+ * Record a measurement as a TCG_PCR_EVENT2 event
+ *
+ * @param[in] hash		Pointer to hash data of TCG_DIGEST_SIZE bytes
+ * @param[in] metadata_ptr	Pointer to event_log_metadata_t structure
+ *
+ * There must be room for storing this new event into the event log buffer.
+ */
+static void event_log_record(const uint8_t *hash,
+			     const event_log_metadata_t *metadata_ptr)
+{
+	void *ptr = log_ptr;
+	uint32_t name_len;
+
+	assert(hash != NULL);
+	assert(metadata_ptr != NULL);
+	assert(metadata_ptr->name != NULL);
+	/* event_log_init() must have been called prior to this. */
+	assert(log_ptr != NULL);
+
+	name_len = (uint32_t)strlen(metadata_ptr->name) + 1U;
+
+	/* Check for space in Event Log buffer */
+	assert(((uintptr_t)ptr + (uint32_t)EVENT2_HDR_SIZE + name_len) <
+	       log_end);
+
+	/*
+	 * As per TCG specifications, firmware components that are measured
+	 * into PCR[0] must be logged in the event log using the event type
+	 * EV_POST_CODE.
+	 */
+	/* TCG_PCR_EVENT2.PCRIndex */
+	((event2_header_t *)ptr)->pcr_index = metadata_ptr->pcr;
+
+	/* TCG_PCR_EVENT2.EventType */
+	((event2_header_t *)ptr)->event_type = EV_POST_CODE;
+
+	/* TCG_PCR_EVENT2.Digests.Count */
+	ptr = (uint8_t *)ptr + offsetof(event2_header_t, digests);
+	((tpml_digest_values *)ptr)->count = HASH_ALG_COUNT;
+
+	/* TCG_PCR_EVENT2.Digests[] */
+	ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(tpml_digest_values, digests));
+
+	/* TCG_PCR_EVENT2.Digests[].AlgorithmId */
+	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
+
+	/* TCG_PCR_EVENT2.Digests[].Digest[] */
+	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(tpmt_ha, digest));
+
+	/* Copy digest */
+	(void)memcpy(ptr, (const void *)hash, TCG_DIGEST_SIZE);
+
+	/* TCG_PCR_EVENT2.EventSize */
+	ptr = (uint8_t *)((uintptr_t)ptr + TCG_DIGEST_SIZE);
+	((event2_data_t *)ptr)->event_size = name_len;
+
+	/* Copy event data to TCG_PCR_EVENT2.Event */
+	(void)memcpy((void *)(((event2_data_t *)ptr)->event),
+			(const void *)metadata_ptr->name, name_len);
+
+	/* End of event data */
+	log_ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(event2_data_t, event) + name_len);
+}
+
+/*
+ * Initialise Event Log global variables, used during the recording
+ * of various payload measurements into the Event Log buffer
+ *
+ * @param[in] event_log_start		Base address of Event Log buffer
+ * @param[in] event_log_finish		End address of Event Log buffer,
+ * 					it is a first byte past end of the
+ * 					buffer
+ */
+void event_log_init(uint8_t *event_log_start, uint8_t *event_log_finish)
+{
+	assert(event_log_start != NULL);
+	assert(event_log_finish > event_log_start);
+
+	log_ptr = event_log_start;
+	log_end = (uintptr_t)event_log_finish;
+
+	/* Get pointer to platform's event_log_metadata_t structure */
+	plat_metadata_ptr = plat_event_log_get_metadata();
+	assert(plat_metadata_ptr != NULL);
+}
+
+/*
+ * Initialises Event Log by writing Specification ID and
+ * Startup Locality events
+ */
+void event_log_write_header(void)
+{
+	const char locality_signature[] = TCG_STARTUP_LOCALITY_SIGNATURE;
+	void *ptr = log_ptr;
+
+	/* event_log_init() must have been called prior to this. */
+	assert(log_ptr != NULL);
+
+	/*
+	 * Add Specification ID Event first
+	 *
+	 * Copy TCG_EfiSpecIDEventStruct structure header
+	 */
+	(void)memcpy(ptr, (const void *)&id_event_header,
+			sizeof(id_event_header));
+	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(id_event_header));
+
+	/* TCG_EfiSpecIdEventAlgorithmSize structure */
+	((id_event_algorithm_size_t *)ptr)->algorithm_id = TPM_ALG_ID;
+	((id_event_algorithm_size_t *)ptr)->digest_size = TCG_DIGEST_SIZE;
+	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(id_event_algorithm_size_t));
+
+	/*
+	 * TCG_EfiSpecIDEventStruct.vendorInfoSize
+	 * No vendor data
+	 */
+	((id_event_struct_data_t *)ptr)->vendor_info_size = 0;
+	ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(id_event_struct_data_t, vendor_info));
+
+	/*
+	 * The Startup Locality event should be placed in the log before
+	 * any event which extends PCR[0].
+	 *
+	 * Ref. TCG PC Client Platform Firmware Profile 9.4.5.3
+	 */
+
+	/* Copy Startup Locality Event Header */
+	(void)memcpy(ptr, (const void *)&locality_event_header,
+			sizeof(locality_event_header));
+	ptr = (uint8_t *)((uintptr_t)ptr + sizeof(locality_event_header));
+
+	/* TCG_PCR_EVENT2.Digests[].AlgorithmId */
+	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
+
+	/* TCG_PCR_EVENT2.Digests[].Digest[] */
+	(void)memset(&((tpmt_ha *)ptr)->digest, 0, TPM_ALG_ID);
+	ptr = (uint8_t *)((uintptr_t)ptr +
+			offsetof(tpmt_ha, digest) + TCG_DIGEST_SIZE);
+
+	/* TCG_PCR_EVENT2.EventSize */
+	((event2_data_t *)ptr)->event_size =
+		(uint32_t)sizeof(startup_locality_event_t);
+	ptr = (uint8_t *)((uintptr_t)ptr + offsetof(event2_data_t, event));
+
+	/* TCG_EfiStartupLocalityEvent.Signature */
+	(void)memcpy(ptr, (const void *)locality_signature,
+		sizeof(TCG_STARTUP_LOCALITY_SIGNATURE));
+
+	/*
+	 * TCG_EfiStartupLocalityEvent.StartupLocality = 0:
+	 * the platform's boot firmware
+	 */
+	((startup_locality_event_t *)ptr)->startup_locality = 0U;
+	log_ptr = (uint8_t *)((uintptr_t)ptr + sizeof(startup_locality_event_t));
+}
+
+/*
+ * Calculate and write hash of image, configuration data, etc.
+ * to Event Log.
+ *
+ * @param[in] data_base		Address of data
+ * @param[in] data_size		Size of data
+ * @param[in] data_id		Data ID
+ * @return:
+ *	0 = success
+ *    < 0 = error
+ */
+int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
+				 uint32_t data_id)
+{
+	unsigned char hash_data[MBEDTLS_MD_MAX_SIZE];
+	int rc;
+	const event_log_metadata_t *metadata_ptr = plat_metadata_ptr;
+
+	/* Get the metadata associated with this image. */
+	while ((metadata_ptr->id != INVALID_ID) &&
+		(metadata_ptr->id != data_id)) {
+		metadata_ptr++;
+	}
+	assert(metadata_ptr->id != INVALID_ID);
+
+	/* Calculate hash */
+	rc = crypto_mod_calc_hash((unsigned int)MBEDTLS_MD_ID,
+				(void *)data_base, data_size, hash_data);
+	if (rc != 0) {
+		return rc;
+	}
+
+	event_log_record(hash_data, metadata_ptr);
+
+	return 0;
+}
+
+/*
+ * Get current Event Log buffer size i.e. used space of Event Log buffer
+ *
+ * @param[in]  event_log_start		Base Pointer to Event Log buffer
+ *
+ * @return: current Size of Event Log buffer
+ */
+size_t event_log_get_cur_size(uint8_t *event_log_start)
+{
+	assert(event_log_start != NULL);
+	assert(log_ptr >= event_log_start);
+
+	return (size_t)((uintptr_t)log_ptr - (uintptr_t)event_log_start);
+}
diff --git a/drivers/measured_boot/measured_boot.mk b/drivers/measured_boot/event_log/event_log.mk
similarity index 63%
rename from drivers/measured_boot/measured_boot.mk
rename to drivers/measured_boot/event_log/event_log.mk
index 497fdba..37e5e29 100644
--- a/drivers/measured_boot/measured_boot.mk
+++ b/drivers/measured_boot/event_log/event_log.mk
@@ -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
 #
@@ -7,7 +7,8 @@
 # Default log level to dump the event log (LOG_LEVEL_INFO)
 EVENT_LOG_LEVEL         ?= 40
 
-# TPM hash algorithm
+# TPM hash algorithm.
+# SHA-256 (or stronger) is required for all devices that are TPM 2.0 compliant.
 TPM_HASH_ALG			:=	sha256
 
 ifeq (${TPM_HASH_ALG}, sha512)
@@ -24,8 +25,6 @@
     TCG_DIGEST_SIZE		:=	32U
 endif
 
-# Event Log length in bytes
-EVENT_LOG_SIZE			:= 1024
 
 # Set definitions for mbed TLS library and Measured Boot driver
 $(eval $(call add_defines,\
@@ -33,20 +32,19 @@
         MBEDTLS_MD_ID \
         TPM_ALG_ID \
         TCG_DIGEST_SIZE \
-        EVENT_LOG_SIZE \
         EVENT_LOG_LEVEL \
 )))
 
 ifeq (${HASH_ALG}, sha256)
-ifneq (${TPM_HASH_ALG}, sha256)
-$(eval $(call add_define,MBEDTLS_SHA512_C))
-endif
+    ifneq (${TPM_HASH_ALG}, sha256)
+        $(eval $(call add_define,MBEDTLS_SHA512_C))
+    endif
 endif
 
-MEASURED_BOOT_SRC_DIR	:= drivers/measured_boot/
+MEASURED_BOOT_SRC_DIR	:= drivers/measured_boot/event_log/
 
-MEASURED_BOOT_SOURCES	:= ${MEASURED_BOOT_SRC_DIR}measured_boot.c	\
-    			   ${MEASURED_BOOT_SRC_DIR}event_log.c		\
-    			   ${MEASURED_BOOT_SRC_DIR}event_print.c
+MEASURED_BOOT_SOURCES	:= ${MEASURED_BOOT_SRC_DIR}event_log.c		\
+			   ${MEASURED_BOOT_SRC_DIR}event_print.c
 
 BL2_SOURCES		+= ${MEASURED_BOOT_SOURCES}
+BL1_SOURCES             += ${MEASURED_BOOT_SOURCES}
diff --git a/drivers/measured_boot/event_print.c b/drivers/measured_boot/event_log/event_print.c
similarity index 98%
rename from drivers/measured_boot/event_print.c
rename to drivers/measured_boot/event_log/event_print.c
index 84ed4b1..e2ba174 100644
--- a/drivers/measured_boot/event_print.c
+++ b/drivers/measured_boot/event_log/event_print.c
@@ -8,7 +8,7 @@
 #include <string.h>
 
 #include <common/debug.h>
-#include <drivers/measured_boot/event_log.h>
+#include <drivers/measured_boot/event_log/event_log.h>
 
 #if LOG_LEVEL >= EVENT_LOG_LEVEL
 
diff --git a/drivers/measured_boot/measured_boot.c b/drivers/measured_boot/measured_boot.c
deleted file mode 100644
index 37fddfb..0000000
--- a/drivers/measured_boot/measured_boot.c
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-
-#include <common/debug.h>
-#include <drivers/measured_boot/measured_boot.h>
-
-/*
- * Init Measured Boot driver
- *
- * Initialises Event Log.
- */
-void measured_boot_init(void)
-{
-	event_log_init();
-}
-
-/*
- * Finish Measured Boot driver
- *
- * Finalises Event Log and dumps the records to the debug console.
- */
-void measured_boot_finish(void)
-{
-	uint8_t *log_addr;
-	size_t log_size;
-	int rc;
-
-	rc = event_log_finalise(&log_addr, &log_size);
-	if (rc != 0) {
-		panic();
-	}
-
-	dump_event_log(log_addr, log_size);
-}
diff --git a/drivers/nxp/sfp/fuse_prov.c b/drivers/nxp/sfp/fuse_prov.c
index 4d30f5f..165474f 100644
--- a/drivers/nxp/sfp/fuse_prov.c
+++ b/drivers/nxp/sfp/fuse_prov.c
@@ -326,7 +326,7 @@
 		      struct sfp_ccsr_regs_t *sfp_ccsr_regs)
 {
 	int ret;
-	uint32_t mask;
+	uint32_t mask = 0;
 
 #ifdef NXP_SFP_VER_3_4
 	if (((fuse_hdr->flags >> FLAG_MC_SHIFT) & 0x1) != 0) {
diff --git a/drivers/renesas/common/io/io_rcar.c b/drivers/renesas/common/io/io_rcar.c
index 17d7aaa..45ef386 100644
--- a/drivers/renesas/common/io/io_rcar.c
+++ b/drivers/renesas/common/io/io_rcar.c
@@ -380,7 +380,7 @@
 
 static int32_t rcar_dev_init(io_dev_info_t *dev_info, const uintptr_t name)
 {
-	uint64_t header[64] __aligned(FLASH_TRANS_SIZE_UNIT) = {0UL};
+	static uint64_t header[64] __aligned(FLASH_TRANS_SIZE_UNIT) = {0UL};
 	uintptr_t handle;
 	ssize_t offset;
 	uint32_t i;
@@ -423,15 +423,17 @@
 		WARN("Firmware Image Package header failed to seek\n");
 		goto error;
 	}
-#if RCAR_BL2_DCACHE == 1
-	inv_dcache_range((uint64_t) header, sizeof(header));
-#endif
+
 	rc = io_read(handle, (uintptr_t) &header, sizeof(header), &cnt);
 	if (rc != IO_SUCCESS) {
 		WARN("Firmware Image Package header failed to read\n");
 		goto error;
 	}
 
+#if RCAR_BL2_DCACHE == 1
+	inv_dcache_range((uint64_t) header, sizeof(header));
+#endif
+
 	rcar_image_number = header[0];
 	for (i = 0; i < rcar_image_number + 2; i++) {
 		rcar_image_header[i] = header[i * 2 + 1];
@@ -440,6 +442,7 @@
 
 	if (rcar_image_number == 0 || rcar_image_number > RCAR_MAX_BL3X_IMAGE) {
 		WARN("Firmware Image Package header check failed.\n");
+		rc = IO_FAIL;
 		goto error;
 	}
 
@@ -448,10 +451,7 @@
 		WARN("Firmware Image Package header failed to seek cert\n");
 		goto error;
 	}
-#if RCAR_BL2_DCACHE == 1
-	inv_dcache_range(RCAR_SDRAM_certESS,
-			 RCAR_CERT_SIZE * (2 + rcar_image_number));
-#endif
+
 	rc = io_read(handle, RCAR_SDRAM_certESS,
 		     RCAR_CERT_SIZE * (2 + rcar_image_number), &cnt);
 	if (rc != IO_SUCCESS) {
@@ -459,6 +459,11 @@
 		goto error;
 	}
 
+#if RCAR_BL2_DCACHE == 1
+	inv_dcache_range(RCAR_SDRAM_certESS,
+			 RCAR_CERT_SIZE * (2 + rcar_image_number));
+#endif
+
 	rcar_cert_load = RCAR_CERT_LOAD;
 error:
 
diff --git a/drivers/renesas/common/pwrc/pwrc.c b/drivers/renesas/common/pwrc/pwrc.c
index 4ebf049..4e175eb 100644
--- a/drivers/renesas/common/pwrc/pwrc.c
+++ b/drivers/renesas/common/pwrc/pwrc.c
@@ -776,14 +776,14 @@
 	memcpy((void *)sram.base, code.base, code.len);
 	flush_dcache_range((uint64_t) sram.base, code.len);
 
+	attr = MT_MEMORY | MT_RO | MT_SECURE | MT_EXECUTE;
+	ret = xlat_change_mem_attributes(sram.base, sram.len, attr);
+	assert(ret == 0);
+
 	/* Invalidate instruction cache */
 	plat_invalidate_icache();
 	dsb();
 	isb();
-
-	attr = MT_MEMORY | MT_RO | MT_SECURE | MT_EXECUTE;
-	ret = xlat_change_mem_attributes(sram.base, sram.len, attr);
-	assert(ret == 0);
 }
 
 uint32_t rcar_pwrc_get_cluster(void)
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 80b6408..3ebc376 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -467,12 +467,12 @@
 	_ACLK, _PLL3_R, _PLL4_P, _CK_PER
 };
 
-static const uint8_t ass_parents[] = {
-	_HSI, _HSE, _PLL2
+static const uint8_t axiss_parents[] = {
+	_HSI, _HSE, _PLL2_P
 };
 
-static const uint8_t mss_parents[] = {
-	_HSI, _HSE, _CSI, _PLL3
+static const uint8_t mcuss_parents[] = {
+	_HSI, _HSE, _CSI, _PLL3_P
 };
 
 static const uint8_t usbphy_parents[] = {
@@ -514,8 +514,8 @@
 	_CLK_PARENT_SEL(SDMMC3, RCC_SDMMC3CKSELR, sdmmc3_parents),
 	_CLK_PARENT_SEL(QSPI, RCC_QSPICKSELR, qspi_parents),
 	_CLK_PARENT_SEL(FMC, RCC_FMCCKSELR, fmc_parents),
-	_CLK_PARENT_SEL(AXIS, RCC_ASSCKSELR, ass_parents),
-	_CLK_PARENT_SEL(MCUS, RCC_MSSCKSELR, mss_parents),
+	_CLK_PARENT_SEL(AXIS, RCC_ASSCKSELR, axiss_parents),
+	_CLK_PARENT_SEL(MCUS, RCC_MSSCKSELR, mcuss_parents),
 	_CLK_PARENT_SEL(USBPHY, RCC_USBCKSELR, usbphy_parents),
 	_CLK_PARENT_SEL(USBO, RCC_USBCKSELR, usbo_parents),
 };
@@ -850,9 +850,7 @@
 
 			reg = mmio_read_32(rcc_base + RCC_MPCKDIVR);
 			clkdiv = reg & RCC_MPUDIV_MASK;
-			if (clkdiv != 0U) {
-				clock /= stm32mp1_mpu_div[clkdiv];
-			}
+			clock >>= stm32mp1_mpu_div[clkdiv];
 			break;
 		default:
 			break;
@@ -1093,6 +1091,10 @@
 	case PLL3_P:
 	case PLL3_Q:
 	case PLL3_R:
+	case CK_AXI:
+	case CK_MPU:
+	case CK_MCU:
+	case RTC:
 		return true;
 	default:
 		return false;
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
index 2101171..d57f120 100644
--- a/drivers/st/clk/stm32mp_clkfunc.c
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -163,7 +163,13 @@
  */
 static int fdt_get_rcc_node(void *fdt)
 {
-	return fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	static int node;
+
+	if (node <= 0) {
+		node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	}
+
+	return node;
 }
 
 /*
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index cff3a34..d3adeab 100644
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -628,6 +628,7 @@
 	int sdmmc_node;
 	void *fdt = NULL;
 	const fdt32_t *cuint;
+	struct dt_node_info dt_info;
 
 	if (fdt_get_address(&fdt) == 0) {
 		return -FDT_ERR_NOTFOUND;
@@ -637,27 +638,14 @@
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	sdmmc_node = fdt_node_offset_by_compatible(fdt, -1, DT_SDMMC2_COMPAT);
-
-	while (sdmmc_node != -FDT_ERR_NOTFOUND) {
-		cuint = fdt_getprop(fdt, sdmmc_node, "reg", NULL);
-		if (cuint == NULL) {
-			continue;
-		}
-
-		if (fdt32_to_cpu(*cuint) == sdmmc2_params.reg_base) {
-			break;
-		}
-
-		sdmmc_node = fdt_node_offset_by_compatible(fdt, sdmmc_node,
-							   DT_SDMMC2_COMPAT);
-	}
-
+	sdmmc_node = dt_match_instance_by_compatible(DT_SDMMC2_COMPAT,
+						     sdmmc2_params.reg_base);
 	if (sdmmc_node == -FDT_ERR_NOTFOUND) {
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	if (fdt_get_status(sdmmc_node) == DT_DISABLED) {
+	dt_fill_device_info(&dt_info, sdmmc_node);
+	if (dt_info.status == DT_DISABLED) {
 		return -FDT_ERR_NOTFOUND;
 	}
 
@@ -665,21 +653,8 @@
 		return -FDT_ERR_BADVALUE;
 	}
 
-	cuint = fdt_getprop(fdt, sdmmc_node, "clocks", NULL);
-	if (cuint == NULL) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	cuint++;
-	sdmmc2_params.clock_id = fdt32_to_cpu(*cuint);
-
-	cuint = fdt_getprop(fdt, sdmmc_node, "resets", NULL);
-	if (cuint == NULL) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	cuint++;
-	sdmmc2_params.reset_id = fdt32_to_cpu(*cuint);
+	sdmmc2_params.clock_id = dt_info.clock;
+	sdmmc2_params.reset_id = dt_info.reset;
 
 	if ((fdt_getprop(fdt, sdmmc_node, "st,use-ckin", NULL)) != NULL) {
 		sdmmc2_params.pin_ckin = SDMMC_CLKCR_SELCLKRX_0;
diff --git a/drivers/st/pmic/stpmic1.c b/drivers/st/pmic/stpmic1.c
index 9999630..0a35df3 100644
--- a/drivers/st/pmic/stpmic1.c
+++ b/drivers/st/pmic/stpmic1.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <errno.h>
 #include <string.h>
 
 #include <common/debug.h>
@@ -16,6 +17,7 @@
 	const uint16_t *voltage_table;
 	uint8_t voltage_table_size;
 	uint8_t control_reg;
+	uint8_t enable_mask;
 	uint8_t low_power_reg;
 	uint8_t pull_down_reg;
 	uint8_t pull_down;
@@ -426,6 +428,7 @@
 		.voltage_table	= buck1_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(buck1_voltage_table),
 		.control_reg	= BUCK1_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= BUCK1_PWRCTRL_REG,
 		.pull_down_reg	= BUCK_PULL_DOWN_REG,
 		.pull_down	= BUCK1_PULL_DOWN_SHIFT,
@@ -437,6 +440,7 @@
 		.voltage_table	= buck2_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(buck2_voltage_table),
 		.control_reg	= BUCK2_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= BUCK2_PWRCTRL_REG,
 		.pull_down_reg	= BUCK_PULL_DOWN_REG,
 		.pull_down	= BUCK2_PULL_DOWN_SHIFT,
@@ -448,6 +452,7 @@
 		.voltage_table	= buck3_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(buck3_voltage_table),
 		.control_reg	= BUCK3_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= BUCK3_PWRCTRL_REG,
 		.pull_down_reg	= BUCK_PULL_DOWN_REG,
 		.pull_down	= BUCK3_PULL_DOWN_SHIFT,
@@ -459,6 +464,7 @@
 		.voltage_table	= buck4_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(buck4_voltage_table),
 		.control_reg	= BUCK4_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= BUCK4_PWRCTRL_REG,
 		.pull_down_reg	= BUCK_PULL_DOWN_REG,
 		.pull_down	= BUCK4_PULL_DOWN_SHIFT,
@@ -470,6 +476,7 @@
 		.voltage_table	= ldo1_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(ldo1_voltage_table),
 		.control_reg	= LDO1_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= LDO1_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO1_MASK_RESET,
@@ -479,6 +486,7 @@
 		.voltage_table	= ldo2_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(ldo2_voltage_table),
 		.control_reg	= LDO2_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= LDO2_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO2_MASK_RESET,
@@ -488,6 +496,7 @@
 		.voltage_table	= ldo3_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(ldo3_voltage_table),
 		.control_reg	= LDO3_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= LDO3_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO3_MASK_RESET,
@@ -497,6 +506,7 @@
 		.voltage_table	= ldo4_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(ldo4_voltage_table),
 		.control_reg	= LDO4_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= LDO4_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO4_MASK_RESET,
@@ -506,6 +516,7 @@
 		.voltage_table	= ldo5_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(ldo5_voltage_table),
 		.control_reg	= LDO5_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= LDO5_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO5_MASK_RESET,
@@ -515,6 +526,7 @@
 		.voltage_table	= ldo6_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(ldo6_voltage_table),
 		.control_reg	= LDO6_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= LDO6_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= LDO6_MASK_RESET,
@@ -524,6 +536,7 @@
 		.voltage_table	= vref_ddr_voltage_table,
 		.voltage_table_size = ARRAY_SIZE(vref_ddr_voltage_table),
 		.control_reg	= VREF_DDR_CONTROL_REG,
+		.enable_mask	= LDO_BUCK_ENABLE_MASK,
 		.low_power_reg	= VREF_DDR_PWRCTRL_REG,
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= VREF_DDR_MASK_RESET,
@@ -581,14 +594,16 @@
 {
 	const struct regul_struct *regul = get_regulator_data(name);
 
-	return stpmic1_register_update(regul->control_reg, BIT(0), BIT(0));
+	return stpmic1_register_update(regul->control_reg, regul->enable_mask,
+				       regul->enable_mask);
 }
 
 int stpmic1_regulator_disable(const char *name)
 {
 	const struct regul_struct *regul = get_regulator_data(name);
 
-	return stpmic1_register_update(regul->control_reg, 0, BIT(0));
+	return stpmic1_register_update(regul->control_reg, 0,
+				       regul->enable_mask);
 }
 
 uint8_t stpmic1_is_regulator_enabled(const char *name)
@@ -600,7 +615,7 @@
 		panic();
 	}
 
-	return (val & 0x1U);
+	return (val & regul->enable_mask);
 }
 
 int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
@@ -653,6 +668,7 @@
 	const struct regul_struct *regul = get_regulator_data(name);
 	uint8_t value;
 	uint8_t mask;
+	int status;
 
 	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
 	if (strncmp(name, "buck", 4) == 0) {
@@ -664,13 +680,16 @@
 		return 0;
 	}
 
-	if (stpmic1_register_read(regul->control_reg, &value))
-		return -1;
+	status = stpmic1_register_read(regul->control_reg, &value);
+	if (status < 0) {
+		return status;
+	}
 
 	value = (value & mask) >> LDO_BUCK_VOLTAGE_SHIFT;
 
-	if (value > regul->voltage_table_size)
-		return -1;
+	if (value > regul->voltage_table_size) {
+		return -ERANGE;
+	}
 
 	return (int)regul->voltage_table[value];
 }
@@ -706,7 +725,7 @@
 		}
 
 		if (readval != value) {
-			return -1;
+			return -EIO;
 		}
 	}
 #endif
@@ -751,12 +770,12 @@
 
 int stpmic1_get_version(unsigned long *version)
 {
-	int rc;
 	uint8_t read_val;
+	int status;
 
-	rc = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
-	if (rc) {
-		return -1;
+	status = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
+	if (status < 0) {
+		return status;
 	}
 
 	*version = (unsigned long)read_val;
diff --git a/drivers/st/usb/stm32mp1_usb.c b/drivers/st/usb/stm32mp1_usb.c
new file mode 100644
index 0000000..9a49690
--- /dev/null
+++ b/drivers/st/usb/stm32mp1_usb.c
@@ -0,0 +1,1091 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_usb.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+#define USB_OTG_MODE_DEVICE			0U
+#define USB_OTG_MODE_HOST			1U
+#define USB_OTG_MODE_DRD			2U
+
+#define EP_TYPE_CTRL				0U
+#define EP_TYPE_ISOC				1U
+#define EP_TYPE_BULK				2U
+#define EP_TYPE_INTR				3U
+
+#define USBD_FIFO_FLUSH_TIMEOUT_US		1000U
+#define EP0_FIFO_SIZE				64U
+
+/* OTG registers offsets */
+#define OTG_GOTGINT				0x004U
+#define OTG_GAHBCFG				0x008U
+#define OTG_GUSBCFG				0x00CU
+#define OTG_GRSTCTL				0x010U
+#define OTG_GINTSTS				0x014U
+#define OTG_GINTMSK				0x018U
+#define OTG_GRXSTSP				0x020U
+#define OTG_GLPMCFG				0x054U
+#define OTG_DCFG				0x800U
+#define OTG_DCTL				0x804U
+#define OTG_DSTS				0x808U
+#define OTG_DIEPMSK				0x810U
+#define OTG_DOEPMSK				0x814U
+#define OTG_DAINT				0x818U
+#define OTG_DAINTMSK				0x81CU
+#define OTG_DIEPEMPMSK				0x834U
+
+/* Definitions for OTG_DIEPx registers */
+#define OTG_DIEP_BASE				0x900U
+#define OTG_DIEP_SIZE				0x20U
+#define OTG_DIEPCTL				0x00U
+#define OTG_DIEPINT				0x08U
+#define OTG_DIEPTSIZ				0x10U
+#define OTG_DIEPDMA				0x14U
+#define OTG_DTXFSTS				0x18U
+#define OTG_DIEP_MAX_NB				9U
+
+/* Definitions for OTG_DOEPx registers */
+#define OTG_DOEP_BASE				0xB00U
+#define OTG_DOEP_SIZE				0x20U
+#define OTG_DOEPCTL				0x00U
+#define OTG_DOEPINT				0x08U
+#define OTG_DOEPTSIZ				0x10U
+#define OTG_DOEPDMA				0x14U
+#define OTG_D0EP_MAX_NB				9U
+
+/* Definitions for OTG_DAINT registers */
+#define OTG_DAINT_OUT_MASK			GENMASK(31, 16)
+#define OTG_DAINT_OUT_SHIFT			16U
+#define OTG_DAINT_IN_MASK			GENMASK(15, 0)
+#define OTG_DAINT_IN_SHIFT			0U
+
+#define OTG_DAINT_EP0_IN			BIT(16)
+#define OTG_DAINT_EP0_OUT			BIT(0)
+
+/* Definitions for FIFOs */
+#define OTG_FIFO_BASE				0x1000U
+#define OTG_FIFO_SIZE				0x1000U
+
+/* Bit definitions for OTG_GOTGINT register */
+#define OTG_GOTGINT_SEDET			BIT(2)
+
+/* Bit definitions for OTG_GAHBCFG register */
+#define OTG_GAHBCFG_GINT			BIT(0)
+
+/* Bit definitions for OTG_GUSBCFG register */
+#define OTG_GUSBCFG_TRDT			GENMASK(13, 10)
+#define OTG_GUSBCFG_TRDT_SHIFT			10U
+
+#define USBD_HS_TRDT_VALUE			9U
+
+/* Bit definitions for OTG_GRSTCTL register */
+#define OTG_GRSTCTL_RXFFLSH			BIT(4)
+#define OTG_GRSTCTL_TXFFLSH			BIT(5)
+#define OTG_GRSTCTL_TXFNUM_SHIFT		6U
+
+/* Bit definitions for OTG_GINTSTS register */
+#define OTG_GINTSTS_CMOD			BIT(0)
+#define OTG_GINTSTS_MMIS			BIT(1)
+#define OTG_GINTSTS_OTGINT			BIT(2)
+#define OTG_GINTSTS_SOF				BIT(3)
+#define OTG_GINTSTS_RXFLVL			BIT(4)
+#define OTG_GINTSTS_USBSUSP			BIT(11)
+#define OTG_GINTSTS_USBRST			BIT(12)
+#define OTG_GINTSTS_ENUMDNE			BIT(13)
+#define OTG_GINTSTS_IEPINT			BIT(18)
+#define OTG_GINTSTS_OEPINT			BIT(19)
+#define OTG_GINTSTS_IISOIXFR			BIT(20)
+#define OTG_GINTSTS_IPXFR_INCOMPISOOUT		BIT(21)
+#define OTG_GINTSTS_LPMINT			BIT(27)
+#define OTG_GINTSTS_SRQINT			BIT(30)
+#define OTG_GINTSTS_WKUPINT			BIT(31)
+
+/* Bit definitions for OTG_GRXSTSP register */
+#define OTG_GRXSTSP_EPNUM			GENMASK(3, 0)
+#define OTG_GRXSTSP_BCNT			GENMASK(14, 4)
+#define OTG_GRXSTSP_BCNT_SHIFT			4U
+#define OTG_GRXSTSP_PKTSTS			GENMASK(20, 17)
+#define OTG_GRXSTSP_PKTSTS_SHIFT		17U
+
+#define STS_GOUT_NAK				1U
+#define STS_DATA_UPDT				2U
+#define STS_XFER_COMP				3U
+#define STS_SETUP_COMP				4U
+#define STS_SETUP_UPDT				6U
+
+/* Bit definitions for OTG_GLPMCFG register */
+#define OTG_GLPMCFG_BESL			GENMASK(5, 2)
+
+/* Bit definitions for OTG_DCFG register */
+#define OTG_DCFG_DAD				GENMASK(10, 4)
+#define OTG_DCFG_DAD_SHIFT			4U
+
+/* Bit definitions for OTG_DCTL register */
+#define OTG_DCTL_RWUSIG				BIT(0)
+#define OTG_DCTL_SDIS				BIT(1)
+#define OTG_DCTL_CGINAK				BIT(8)
+
+/* Bit definitions for OTG_DSTS register */
+#define OTG_DSTS_SUSPSTS			BIT(0)
+#define OTG_DSTS_ENUMSPD_MASK			GENMASK(2, 1)
+#define OTG_DSTS_FNSOF0				BIT(8)
+
+#define OTG_DSTS_ENUMSPD(val)			((val) << 1)
+#define OTG_DSTS_ENUMSPD_HS_PHY_30MHZ_OR_60MHZ	OTG_DSTS_ENUMSPD(0U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_30MHZ_OR_60MHZ	OTG_DSTS_ENUMSPD(1U)
+#define OTG_DSTS_ENUMSPD_LS_PHY_6MHZ		OTG_DSTS_ENUMSPD(2U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_48MHZ		OTG_DSTS_ENUMSPD(3U)
+
+/* Bit definitions for OTG_DIEPMSK register */
+#define OTG_DIEPMSK_XFRCM			BIT(0)
+#define OTG_DIEPMSK_EPDM			BIT(1)
+#define OTG_DIEPMSK_TOM				BIT(3)
+
+/* Bit definitions for OTG_DOEPMSK register */
+#define OTG_DOEPMSK_XFRCM			BIT(0)
+#define OTG_DOEPMSK_EPDM			BIT(1)
+#define OTG_DOEPMSK_STUPM			BIT(3)
+
+/* Bit definitions for OTG_DIEPCTLx registers */
+#define OTG_DIEPCTL_MPSIZ			GENMASK(10, 0)
+#define OTG_DIEPCTL_STALL			BIT(21)
+#define OTG_DIEPCTL_CNAK			BIT(26)
+#define OTG_DIEPCTL_SD0PID_SEVNFRM		BIT(28)
+#define OTG_DIEPCTL_SODDFRM			BIT(29)
+#define OTG_DIEPCTL_EPDIS			BIT(30)
+#define OTG_DIEPCTL_EPENA			BIT(31)
+
+/* Bit definitions for OTG_DIEPINTx registers */
+#define OTG_DIEPINT_XFRC			BIT(0)
+#define OTG_DIEPINT_EPDISD			BIT(1)
+#define OTG_DIEPINT_TOC				BIT(3)
+#define OTG_DIEPINT_ITTXFE			BIT(4)
+#define OTG_DIEPINT_INEPNE			BIT(6)
+#define OTG_DIEPINT_TXFE			BIT(7)
+#define OTG_DIEPINT_TXFE_SHIFT			7U
+
+#define OTG_DIEPINT_MASK			(BIT(13) | BIT(11) | GENMASK(9, 0))
+
+/* Bit definitions for OTG_DIEPTSIZx registers */
+#define OTG_DIEPTSIZ_XFRSIZ			GENMASK(18, 0)
+#define OTG_DIEPTSIZ_PKTCNT			GENMASK(28, 19)
+#define OTG_DIEPTSIZ_PKTCNT_SHIFT		19U
+#define OTG_DIEPTSIZ_MCNT_MASK			GENMASK(30, 29)
+#define OTG_DIEPTSIZ_MCNT_DATA0			BIT(29)
+
+#define OTG_DIEPTSIZ_PKTCNT_1			BIT(19)
+
+/* Bit definitions for OTG_DTXFSTSx registers */
+#define OTG_DTXFSTS_INEPTFSAV			GENMASK(15, 0)
+
+/* Bit definitions for OTG_DOEPCTLx registers */
+#define OTG_DOEPCTL_STALL			BIT(21)
+#define OTG_DOEPCTL_CNAK			BIT(26)
+#define OTG_DOEPCTL_SD0PID_SEVNFRM		BIT(28) /* other than endpoint 0 */
+#define OTG_DOEPCTL_SD1PID_SODDFRM		BIT(29) /* other than endpoint 0 */
+#define OTG_DOEPCTL_EPDIS			BIT(30)
+#define OTG_DOEPCTL_EPENA			BIT(31)
+
+/* Bit definitions for OTG_DOEPTSIZx registers */
+#define OTG_DOEPTSIZ_XFRSIZ			GENMASK(18, 0)
+#define OTG_DOEPTSIZ_PKTCNT			GENMASK(28, 19)
+#define OTG_DOEPTSIZ_RXDPID_STUPCNT		GENMASK(30, 29)
+
+/* Bit definitions for OTG_DOEPINTx registers */
+#define OTG_DOEPINT_XFRC			BIT(0)
+#define OTG_DOEPINT_STUP			BIT(3)
+#define OTG_DOEPINT_OTEPDIS			BIT(4)
+
+#define OTG_DOEPINT_MASK			(GENMASK(15, 12) | GENMASK(9, 8) | GENMASK(6, 0))
+
+#define EP_NB					15U
+#define EP_ALL					0x10U
+
+/*
+ * Flush TX FIFO.
+ * handle: PCD handle.
+ * num: FIFO number.
+ *	   This parameter can be a value from 1 to 15 or EP_ALL.
+ *	   EP_ALL= 0x10 means Flush all TX FIFOs
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_flush_tx_fifo(void *handle, uint32_t num)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+	mmio_write_32(usb_base_addr + OTG_GRSTCTL,
+		      OTG_GRSTCTL_TXFFLSH | (uint32_t)(num << OTG_GRSTCTL_TXFNUM_SHIFT));
+
+	while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+		OTG_GRSTCTL_TXFFLSH) == OTG_GRSTCTL_TXFFLSH) {
+		if (timeout_elapsed(timeout)) {
+			return USBD_TIMEOUT;
+		}
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Flush RX FIFO.
+ * handle: PCD handle.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_flush_rx_fifo(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+	mmio_write_32(usb_base_addr + OTG_GRSTCTL, OTG_GRSTCTL_RXFFLSH);
+
+	while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+		 OTG_GRSTCTL_RXFFLSH) == OTG_GRSTCTL_RXFFLSH) {
+		if (timeout_elapsed(timeout)) {
+			return USBD_TIMEOUT;
+		}
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Return the global USB interrupt status.
+ * handle: PCD handle.
+ * return: Interrupt register value.
+ */
+static uint32_t usb_dwc2_read_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_GINTSTS) &
+	       mmio_read_32(usb_base_addr + OTG_GINTMSK);
+}
+
+/*
+ * Return the USB device OUT endpoints interrupt.
+ * handle: PCD handle.
+ * return: Device OUT endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_out_ep_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+		 mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+		OTG_DAINT_OUT_MASK) >> OTG_DAINT_OUT_SHIFT;
+}
+
+/*
+ * Return the USB device IN endpoints interrupt.
+ * handle: PCD handle.
+ * return: Device IN endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_in_ep_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+		 mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+		OTG_DAINT_IN_MASK) >> OTG_DAINT_IN_SHIFT;
+}
+
+/*
+ * Return Device OUT EP interrupt register.
+ * handle: PCD handle.
+ * epnum: Endpoint number.
+ *         This parameter can be a value from 0 to 15.
+ * return: Device OUT EP Interrupt register.
+ */
+static uint32_t usb_dwc2_out_ep_int(void *handle, uint8_t epnum)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_DOEP_BASE +
+			    (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT) &
+	       mmio_read_32(usb_base_addr + OTG_DOEPMSK);
+}
+
+/*
+ * Return Device IN EP interrupt register.
+ * handle: PCD handle.
+ * epnum: Endpoint number.
+ *         This parameter can be a value from 0 to 15.
+ * return: Device IN EP Interrupt register.
+ */
+static uint32_t usb_dwc2_in_ep_int(void *handle, uint8_t epnum)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t msk;
+	uint32_t emp;
+
+	msk = mmio_read_32(usb_base_addr + OTG_DIEPMSK);
+	emp = mmio_read_32(usb_base_addr + OTG_DIEPEMPMSK);
+	msk |= ((emp >> epnum) << OTG_DIEPINT_TXFE_SHIFT) & OTG_DIEPINT_TXFE;
+
+	return mmio_read_32(usb_base_addr + OTG_DIEP_BASE +
+			    (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT) & msk;
+}
+
+/*
+ * Return USB core mode.
+ * handle: PCD handle.
+ * return: Core mode.
+ *         This parameter can be 0 (host) or 1 (device).
+ */
+static uint32_t usb_dwc2_get_mode(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_GINTSTS) & OTG_GINTSTS_CMOD;
+}
+
+/*
+ * Activate EP0 for detup transactions.
+ * handle: PCD handle.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_activate_setup(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE;
+
+	/* Set the MPS of the IN EP based on the enumeration speed */
+	mmio_clrbits_32(reg_offset + OTG_DIEPCTL, OTG_DIEPCTL_MPSIZ);
+
+	if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_ENUMSPD_MASK) ==
+	    OTG_DSTS_ENUMSPD_LS_PHY_6MHZ) {
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL, 3U);
+	}
+
+	mmio_setbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_CGINAK);
+
+	return USBD_OK;
+}
+
+/*
+ * Prepare the EP0 to start the first control setup.
+ * handle: Selected device.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep0_out_start(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE + OTG_DIEPTSIZ;
+	uint32_t reg_value = 0U;
+
+	/* PKTCNT = 1 and XFRSIZ = 24 bytes for endpoint 0 */
+	reg_value |= OTG_DIEPTSIZ_PKTCNT_1;
+	reg_value |= (EP0_FIFO_SIZE & OTG_DIEPTSIZ_XFRSIZ);
+	reg_value |= OTG_DOEPTSIZ_RXDPID_STUPCNT;
+
+	mmio_write_32(reg_offset, reg_value);
+
+	return USBD_OK;
+}
+
+/*
+ * Write a packet into the TX FIFO associated with the EP/channel.
+ * handle: Selected device.
+ * src: Pointer to source buffer.
+ * ch_ep_num: Endpoint or host channel number.
+ * len: Number of bytes to write.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_write_packet(void *handle, uint8_t *src,
+					  uint8_t ch_ep_num, uint16_t len)
+{
+	uint32_t reg_offset;
+	uint32_t count32b = (len + 3U) / 4U;
+	uint32_t i;
+
+	reg_offset = (uintptr_t)handle + OTG_FIFO_BASE +
+		     (ch_ep_num * OTG_FIFO_SIZE);
+
+	for (i = 0U; i < count32b; i++) {
+		uint32_t src_copy = 0U;
+		uint32_t j;
+
+		/* Data written to FIFO need to be 4 bytes aligned */
+		for (j = 0U; j < 4U; j++) {
+			src_copy += (*(src + j)) << (8U * j);
+		}
+
+		mmio_write_32(reg_offset, src_copy);
+		src += 4U;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Read a packet from the RX FIFO associated with the EP/channel.
+ * handle: Selected device.
+ * dst: Destination pointer.
+ * len: Number of bytes to read.
+ * return: Pointer to destination buffer.
+ */
+static void *usb_dwc2_read_packet(void *handle, uint8_t *dest, uint16_t len)
+{
+	uint32_t reg_offset;
+	uint32_t count32b = (len + 3U) / 4U;
+	uint32_t i;
+
+	VERBOSE("read packet length %i to 0x%lx\n", len, (uintptr_t)dest);
+
+	reg_offset = (uintptr_t)handle + OTG_FIFO_BASE;
+
+	for (i = 0U; i < count32b; i++) {
+		*(uint32_t *)dest = mmio_read_32(reg_offset);
+		dest += 4U;
+		dsb();
+	}
+
+	return (void *)dest;
+}
+
+/*
+ * Setup and start a transfer over an EP.
+ * handle: Selected device
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep_start_xfer(void *handle, struct usbd_ep *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+	uint32_t clear_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE + (ep->num * OTG_DIEP_SIZE);
+		clear_value = OTG_DIEPTSIZ_PKTCNT | OTG_DIEPTSIZ_XFRSIZ;
+		if (ep->xfer_len == 0U) {
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			/*
+			 * Program the transfer size and packet count
+			 * as follows:
+			 * xfersize = N * maxpacket + short_packet
+			 * pktcnt = N + (short_packet exist ? 1 : 0)
+			 */
+			reg_value = (OTG_DIEPTSIZ_PKTCNT &
+				     (((ep->xfer_len + ep->maxpacket - 1U) /
+				       ep->maxpacket) << OTG_DIEPTSIZ_PKTCNT_SHIFT))
+				    | ep->xfer_len;
+
+			if (ep->type == EP_TYPE_ISOC) {
+				clear_value |= OTG_DIEPTSIZ_MCNT_MASK;
+				reg_value |= OTG_DIEPTSIZ_MCNT_DATA0;
+			}
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ, clear_value, reg_value);
+
+		if ((ep->type != EP_TYPE_ISOC) && (ep->xfer_len > 0U)) {
+			/* Enable the TX FIFO empty interrupt for this EP */
+			mmio_setbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(ep->num));
+		}
+
+		/* EP enable, IN data in FIFO */
+		reg_value = OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA;
+
+		if (ep->type == EP_TYPE_ISOC) {
+			if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+				reg_value |= OTG_DIEPCTL_SODDFRM;
+			} else {
+				reg_value |= OTG_DIEPCTL_SD0PID_SEVNFRM;
+			}
+		}
+
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL, reg_value);
+
+		if (ep->type == EP_TYPE_ISOC) {
+			usb_dwc2_write_packet(handle, ep->xfer_buff, ep->num, ep->xfer_len);
+		}
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE + (ep->num * OTG_DOEP_SIZE);
+		/*
+		 * Program the transfer size and packet count as follows:
+		 * pktcnt = N
+		 * xfersize = N * maxpacket
+		 */
+		if (ep->xfer_len == 0U) {
+			reg_value = ep->maxpacket | OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			uint16_t pktcnt = (ep->xfer_len + ep->maxpacket - 1U) / ep->maxpacket;
+
+			reg_value = (pktcnt << OTG_DIEPTSIZ_PKTCNT_SHIFT) |
+				    (ep->maxpacket * pktcnt);
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DOEPTSIZ,
+				   OTG_DOEPTSIZ_XFRSIZ & OTG_DOEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* EP enable */
+		reg_value = OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA;
+
+		if (ep->type == EP_TYPE_ISOC) {
+			if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+				reg_value |= OTG_DOEPCTL_SD1PID_SODDFRM;
+			} else {
+				reg_value |= OTG_DOEPCTL_SD0PID_SEVNFRM;
+			}
+		}
+
+		mmio_setbits_32(reg_offset + OTG_DOEPCTL, reg_value);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Setup and start a transfer over the EP0.
+ * handle: Selected device.
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep0_start_xfer(void *handle, struct usbd_ep *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE +
+			     (ep->num * OTG_DIEP_SIZE);
+
+		if (ep->xfer_len == 0U) {
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			/*
+			 * Program the transfer size and packet count
+			 * as follows:
+			 * xfersize = N * maxpacket + short_packet
+			 * pktcnt = N + (short_packet exist ? 1 : 0)
+			 */
+
+			if (ep->xfer_len > ep->maxpacket) {
+				ep->xfer_len = ep->maxpacket;
+			}
+
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->xfer_len;
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+				   OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* Enable the TX FIFO empty interrupt for this EP */
+		if (ep->xfer_len > 0U) {
+			mmio_setbits_32(usb_base_addr +	OTG_DIEPEMPMSK,
+					BIT(ep->num));
+		}
+
+		/* EP enable, IN data in FIFO */
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL,
+				OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA);
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE +
+			     (ep->num * OTG_DOEP_SIZE);
+
+		/*
+		 * Program the transfer size and packet count as follows:
+		 * pktcnt = N
+		 * xfersize = N * maxpacket
+		 */
+		if (ep->xfer_len > 0U) {
+			ep->xfer_len = ep->maxpacket;
+		}
+
+		reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->maxpacket;
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+				   OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* EP enable */
+		mmio_setbits_32(reg_offset + OTG_DOEPCTL,
+				OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Set a stall condition over an EP.
+ * handle: Selected device.
+ * ep: Pointer to endpoint structure.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_ep_set_stall(void *handle, struct usbd_ep *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE +
+			     (ep->num * OTG_DIEP_SIZE);
+		reg_value = mmio_read_32(reg_offset + OTG_DIEPCTL);
+
+		if ((reg_value & OTG_DIEPCTL_EPENA) == 0U) {
+			reg_value &= ~OTG_DIEPCTL_EPDIS;
+		}
+
+		reg_value |= OTG_DIEPCTL_STALL;
+
+		mmio_write_32(reg_offset + OTG_DIEPCTL, reg_value);
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE +
+			     (ep->num * OTG_DOEP_SIZE);
+		reg_value = mmio_read_32(reg_offset + OTG_DOEPCTL);
+
+		if ((reg_value & OTG_DOEPCTL_EPENA) == 0U) {
+			reg_value &= ~OTG_DOEPCTL_EPDIS;
+		}
+
+		reg_value |= OTG_DOEPCTL_STALL;
+
+		mmio_write_32(reg_offset + OTG_DOEPCTL, reg_value);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Stop the USB device mode.
+ * handle: Selected device.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_stop_device(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t i;
+
+	/* Disable Int */
+	mmio_clrbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+	/* Clear pending interrupts */
+	for (i = 0U; i < EP_NB; i++) {
+		mmio_write_32(usb_base_addr + OTG_DIEP_BASE + (i * OTG_DIEP_SIZE) + OTG_DIEPINT,
+			      OTG_DIEPINT_MASK);
+		mmio_write_32(usb_base_addr + OTG_DOEP_BASE + (i * OTG_DOEP_SIZE) + OTG_DOEPINT,
+			      OTG_DOEPINT_MASK);
+	}
+
+	mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+
+	/* Clear interrupt masks */
+	mmio_write_32(usb_base_addr + OTG_DIEPMSK, 0U);
+	mmio_write_32(usb_base_addr + OTG_DOEPMSK, 0U);
+	mmio_write_32(usb_base_addr + OTG_DAINTMSK, 0U);
+
+	/* Flush the FIFO */
+	usb_dwc2_flush_rx_fifo(handle);
+	usb_dwc2_flush_tx_fifo(handle, EP_ALL);
+
+	/* Disconnect the USB device by disabling the pull-up/pull-down */
+	mmio_setbits_32((uintptr_t)handle + OTG_DCTL, OTG_DCTL_SDIS);
+
+	return USBD_OK;
+}
+
+/*
+ * Stop the USB device mode.
+ * handle: Selected device.
+ * address: New device address to be assigned.
+ *         This parameter can be a value from 0 to 255.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_set_address(void *handle, uint8_t address)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	mmio_clrsetbits_32(usb_base_addr + OTG_DCFG,
+			   OTG_DCFG_DAD,
+			   address << OTG_DCFG_DAD_SHIFT);
+
+	return USBD_OK;
+}
+
+/*
+ * Check FIFO for the next packet to be loaded.
+ * handle: Selected device.
+ * epnum : Endpoint number.
+ * xfer_len: Block length.
+ * xfer_count: Number of blocks.
+ * maxpacket: Max packet length.
+ * xfer_buff: Buffer pointer.
+ * return: USB status.
+ */
+static enum usb_status usb_dwc2_write_empty_tx_fifo(void *handle,
+						    uint32_t epnum,
+						    uint32_t xfer_len,
+						    uint32_t *xfer_count,
+						    uint32_t maxpacket,
+						    uint8_t **xfer_buff)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	int32_t len;
+	uint32_t len32b;
+	enum usb_status ret;
+
+	len = xfer_len - *xfer_count;
+
+	if ((len > 0) && ((uint32_t)len > maxpacket)) {
+		len = maxpacket;
+	}
+
+	len32b = (len + 3U) / 4U;
+
+	reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE);
+
+	while (((mmio_read_32(reg_offset + OTG_DTXFSTS) &
+		OTG_DTXFSTS_INEPTFSAV) > len32b) &&
+	       (*xfer_count < xfer_len) && (xfer_len != 0U)) {
+		/* Write the FIFO */
+		len = xfer_len - *xfer_count;
+
+		if ((len > 0) && ((uint32_t)len > maxpacket)) {
+			len = maxpacket;
+		}
+
+		len32b = (len + 3U) / 4U;
+
+		ret = usb_dwc2_write_packet(handle, *xfer_buff, epnum, len);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		*xfer_buff  += len;
+		*xfer_count += len;
+	}
+
+	if (len <= 0) {
+		mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * Handle PCD interrupt request.
+ * handle: PCD handle.
+ * param: Pointer to information updated by the IT handling.
+ * return: Action to do after IT handling.
+ */
+static enum usb_action usb_dwc2_it_handler(void *handle, uint32_t *param)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t ep_intr;
+	uint32_t epint;
+	uint32_t epnum;
+	uint32_t temp;
+	enum usb_status ret;
+
+	if (usb_dwc2_get_mode(handle) != USB_OTG_MODE_DEVICE) {
+		return USB_NOTHING;
+	}
+
+	/* Avoid spurious interrupt */
+	if (usb_dwc2_read_int(handle) == 0U) {
+		return USB_NOTHING;
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_MMIS) != 0U) {
+		/* Incorrect mode, acknowledge the interrupt */
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_MMIS);
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OEPINT) != 0U) {
+		uint32_t reg_offset;
+
+		/* Read in the device interrupt bits */
+		ep_intr = usb_dwc2_all_out_ep_int(handle);
+		epnum = 0U;
+		while ((ep_intr & BIT(0)) != BIT(0)) {
+			epnum++;
+			ep_intr >>= 1;
+		}
+
+		reg_offset = usb_base_addr + OTG_DOEP_BASE + (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT;
+
+		epint = usb_dwc2_out_ep_int(handle, epnum);
+
+		if ((epint & OTG_DOEPINT_XFRC) == OTG_DOEPINT_XFRC) {
+			mmio_write_32(reg_offset, OTG_DOEPINT_XFRC);
+			*param = epnum;
+
+			return USB_DATA_OUT;
+		}
+
+		if ((epint & OTG_DOEPINT_STUP) == OTG_DOEPINT_STUP) {
+			/* Inform  that a setup packet is available */
+			mmio_write_32(reg_offset, OTG_DOEPINT_STUP);
+
+			return USB_SETUP;
+		}
+
+		if ((epint & OTG_DOEPINT_OTEPDIS) == OTG_DOEPINT_OTEPDIS) {
+			mmio_write_32(reg_offset, OTG_DOEPINT_OTEPDIS);
+		}
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IEPINT) != 0U) {
+		uint32_t reg_offset;
+
+		/* Read in the device interrupt bits */
+		ep_intr = usb_dwc2_all_in_ep_int(handle);
+		epnum = 0U;
+		while ((ep_intr & BIT(0)) != BIT(0)) {
+			epnum++;
+			ep_intr >>= 1;
+		}
+
+		reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT;
+
+		epint = usb_dwc2_in_ep_int(handle, epnum);
+
+		if ((epint & OTG_DIEPINT_XFRC) == OTG_DIEPINT_XFRC) {
+			mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+			mmio_write_32(reg_offset, OTG_DIEPINT_XFRC);
+			*param = epnum;
+
+			return USB_DATA_IN;
+		}
+
+		if ((epint & OTG_DIEPINT_TOC) == OTG_DIEPINT_TOC) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_TOC);
+		}
+
+		if ((epint & OTG_DIEPINT_ITTXFE) == OTG_DIEPINT_ITTXFE) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_ITTXFE);
+		}
+
+		if ((epint & OTG_DIEPINT_INEPNE) == OTG_DIEPINT_INEPNE) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_INEPNE);
+		}
+
+		if ((epint & OTG_DIEPINT_EPDISD) == OTG_DIEPINT_EPDISD) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_EPDISD);
+		}
+
+		if ((epint & OTG_DIEPINT_TXFE) == OTG_DIEPINT_TXFE) {
+			*param = epnum;
+
+			return USB_WRITE_EMPTY;
+		}
+	}
+
+	/* Handle resume interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_WKUPINT) != 0U) {
+		INFO("handle USB : Resume\n");
+
+		/* Clear the remote wake-up signaling */
+		mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_WKUPINT);
+
+		return USB_RESUME;
+	}
+
+	/* Handle suspend interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBSUSP) != 0U) {
+		INFO("handle USB : Suspend int\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBSUSP);
+
+		if ((mmio_read_32(usb_base_addr + OTG_DSTS) &
+		     OTG_DSTS_SUSPSTS) == OTG_DSTS_SUSPSTS) {
+			return USB_SUSPEND;
+		}
+	}
+
+	/* Handle LPM interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_LPMINT) != 0U) {
+		INFO("handle USB : LPM int enter in suspend\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_LPMINT);
+		*param = (mmio_read_32(usb_base_addr + OTG_GLPMCFG) &
+			  OTG_GLPMCFG_BESL) >> 2;
+
+		return USB_LPM;
+	}
+
+	/* Handle reset interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBRST) != 0U) {
+		INFO("handle USB : Reset\n");
+
+		mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+
+		usb_dwc2_flush_tx_fifo(handle, 0U);
+
+		mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+		mmio_setbits_32(usb_base_addr + OTG_DAINTMSK, OTG_DAINT_EP0_IN | OTG_DAINT_EP0_OUT);
+
+		mmio_setbits_32(usb_base_addr + OTG_DOEPMSK, OTG_DOEPMSK_STUPM |
+							     OTG_DOEPMSK_XFRCM |
+							     OTG_DOEPMSK_EPDM);
+		mmio_setbits_32(usb_base_addr + OTG_DIEPMSK, OTG_DIEPMSK_TOM |
+							     OTG_DIEPMSK_XFRCM |
+							     OTG_DIEPMSK_EPDM);
+
+		/* Set default address to 0 */
+		mmio_clrbits_32(usb_base_addr + OTG_DCFG, OTG_DCFG_DAD);
+
+		/* Setup EP0 to receive SETUP packets */
+		ret = usb_dwc2_ep0_out_start(handle);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBRST);
+
+		return USB_RESET;
+	}
+
+	/* Handle enumeration done interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_ENUMDNE) != 0U) {
+		ret = usb_dwc2_activate_setup(handle);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		mmio_clrbits_32(usb_base_addr + OTG_GUSBCFG, OTG_GUSBCFG_TRDT);
+
+		mmio_setbits_32(usb_base_addr + OTG_GUSBCFG,
+				(USBD_HS_TRDT_VALUE << OTG_GUSBCFG_TRDT_SHIFT) & OTG_GUSBCFG_TRDT);
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_ENUMDNE);
+
+		return USB_ENUM_DONE;
+	}
+
+	/* Handle RXQLevel interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_RXFLVL) != 0U) {
+		mmio_clrbits_32(usb_base_addr + OTG_GINTMSK,
+				OTG_GINTSTS_RXFLVL);
+
+		temp = mmio_read_32(usb_base_addr + OTG_GRXSTSP);
+
+		*param = temp & OTG_GRXSTSP_EPNUM;
+		*param |= (temp & OTG_GRXSTSP_BCNT) << (USBD_OUT_COUNT_SHIFT -
+							OTG_GRXSTSP_BCNT_SHIFT);
+
+		if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) == STS_DATA_UPDT) {
+			if ((temp & OTG_GRXSTSP_BCNT) != 0U) {
+				mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+				return USB_READ_DATA_PACKET;
+			}
+		} else if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) ==
+			    STS_SETUP_UPDT) {
+			mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+			return USB_READ_SETUP_PACKET;
+		}
+
+		mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+	}
+
+	/* Handle SOF interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SOF) != 0U) {
+		INFO("handle USB : SOF\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SOF);
+
+		return USB_SOF;
+	}
+
+	/* Handle incomplete ISO IN interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IISOIXFR) != 0U) {
+		INFO("handle USB : ISO IN\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS,
+			      OTG_GINTSTS_IISOIXFR);
+	}
+
+	/* Handle incomplete ISO OUT interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IPXFR_INCOMPISOOUT) !=
+	    0U) {
+		INFO("handle USB : ISO OUT\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS,
+			      OTG_GINTSTS_IPXFR_INCOMPISOOUT);
+	}
+
+	/* Handle connection event interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SRQINT) != 0U) {
+		INFO("handle USB : Connect\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SRQINT);
+	}
+
+	/* Handle disconnection event interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OTGINT) != 0U) {
+		INFO("handle USB : Disconnect\n");
+
+		temp = mmio_read_32(usb_base_addr + OTG_GOTGINT);
+
+		if ((temp & OTG_GOTGINT_SEDET) == OTG_GOTGINT_SEDET) {
+			return USB_DISCONNECT;
+		}
+	}
+
+	return USB_NOTHING;
+}
+
+/*
+ * Start the usb device mode
+ * usb_core_handle: USB core driver handle.
+ * return  USB status.
+ */
+static enum usb_status usb_dwc2_start_device(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_SDIS);
+	mmio_setbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+	return USBD_OK;
+}
+
+static const struct usb_driver usb_dwc2driver = {
+	.ep0_out_start = usb_dwc2_ep0_out_start,
+	.ep_start_xfer = usb_dwc2_ep_start_xfer,
+	.ep0_start_xfer = usb_dwc2_ep0_start_xfer,
+	.write_packet = usb_dwc2_write_packet,
+	.read_packet = usb_dwc2_read_packet,
+	.ep_set_stall = usb_dwc2_ep_set_stall,
+	.start_device = usb_dwc2_start_device,
+	.stop_device = usb_dwc2_stop_device,
+	.set_address = usb_dwc2_set_address,
+	.write_empty_tx_fifo = usb_dwc2_write_empty_tx_fifo,
+	.it_handler = usb_dwc2_it_handler
+};
+
+/*
+ * Initialize USB DWC2 driver.
+ * usb_core_handle: USB core driver handle.
+ * pcd_handle: PCD handle.
+ * base_register: USB global register base address.
+ */
+void stm32mp1_usb_init_driver(struct usb_handle *usb_core_handle,
+			      struct pcd_handle *pcd_handle,
+			      void *base_register)
+{
+	register_usb_driver(usb_core_handle, pcd_handle, &usb_dwc2driver,
+			    base_register);
+}
diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index 6dbf372..ae42e32 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c
@@ -34,6 +34,9 @@
 {
 	unsigned int data;
 
+	if (base == 0 || cmd == NULL)
+		return -EINVAL;
+
 	data = mmio_read_32(base + HCS);
 	if ((data & HCS_UCRDY) == 0)
 		return -EBUSY;
@@ -54,9 +57,13 @@
 {
 	uintptr_t base;
 	unsigned int data;
-	int retries;
+	int result, retries;
+	uic_cmd_t cmd;
 
-	assert((ufs_params.reg_base != 0) && (val != NULL));
+	assert(ufs_params.reg_base != 0);
+
+	if (val == NULL)
+		return -EINVAL;
 
 	base = ufs_params.reg_base;
 	for (retries = 0; retries < 100; retries++) {
@@ -68,19 +75,20 @@
 	if (retries >= 100)
 		return -EBUSY;
 
-	mmio_write_32(base + IS, ~0);
-	mmio_write_32(base + UCMDARG1, (attr << 16) | GEN_SELECTOR_IDX(idx));
-	mmio_write_32(base + UCMDARG2, 0);
-	mmio_write_32(base + UCMDARG3, 0);
-	mmio_write_32(base + UICCMD, DME_GET);
-	do {
+	cmd.arg1 = (attr << 16) | GEN_SELECTOR_IDX(idx);
+	cmd.arg2 = 0;
+	cmd.arg3 = 0;
+	cmd.op = DME_GET;
+	for (retries = 0; retries < UFS_UIC_COMMAND_RETRIES; ++retries) {
+		result = ufshc_send_uic_cmd(base, &cmd);
+		if (result == 0)
+			break;
 		data = mmio_read_32(base + IS);
 		if (data & UFS_INT_UE)
 			return -EINVAL;
-	} while ((data & UFS_INT_UCCS) == 0);
-	mmio_write_32(base + IS, UFS_INT_UCCS);
-	data = mmio_read_32(base + UCMDARG2) & CONFIG_RESULT_CODE_MASK;
-	assert(data == 0);
+	}
+	if (retries >= UFS_UIC_COMMAND_RETRIES)
+		return -EIO;
 
 	*val = mmio_read_32(base + UCMDARG3);
 	return 0;
@@ -90,58 +98,101 @@
 {
 	uintptr_t base;
 	unsigned int data;
+	int result, retries;
+	uic_cmd_t cmd;
 
 	assert((ufs_params.reg_base != 0));
 
 	base = ufs_params.reg_base;
-	data = mmio_read_32(base + HCS);
-	if ((data & HCS_UCRDY) == 0)
-		return -EBUSY;
-	mmio_write_32(base + IS, ~0);
-	mmio_write_32(base + UCMDARG1, (attr << 16) | GEN_SELECTOR_IDX(idx));
-	mmio_write_32(base + UCMDARG2, 0);
-	mmio_write_32(base + UCMDARG3, val);
-	mmio_write_32(base + UICCMD, DME_SET);
-	do {
+	cmd.arg1 = (attr << 16) | GEN_SELECTOR_IDX(idx);
+	cmd.arg2 = 0;
+	cmd.arg3 = val;
+	cmd.op = DME_SET;
+
+	for (retries = 0; retries < UFS_UIC_COMMAND_RETRIES; ++retries) {
+		result = ufshc_send_uic_cmd(base, &cmd);
+		if (result == 0)
+			break;
 		data = mmio_read_32(base + IS);
 		if (data & UFS_INT_UE)
 			return -EINVAL;
-	} while ((data & UFS_INT_UCCS) == 0);
-	mmio_write_32(base + IS, UFS_INT_UCCS);
-	data = mmio_read_32(base + UCMDARG2) & CONFIG_RESULT_CODE_MASK;
-	assert(data == 0);
+	}
+	if (retries >= UFS_UIC_COMMAND_RETRIES)
+		return -EIO;
+
 	return 0;
 }
 
-static void ufshc_reset(uintptr_t base)
+static int ufshc_hce_enable(uintptr_t base)
 {
 	unsigned int data;
+	int retries;
 
 	/* Enable Host Controller */
 	mmio_write_32(base + HCE, HCE_ENABLE);
+
 	/* Wait until basic initialization sequence completed */
-	do {
+	for (retries = 0; retries < HCE_ENABLE_INNER_RETRIES; ++retries) {
 		data = mmio_read_32(base + HCE);
-	} while ((data & HCE_ENABLE) == 0);
+		if (data & HCE_ENABLE) {
+			break;
+		}
+		udelay(HCE_ENABLE_TIMEOUT_US);
+	}
+	if (retries >= HCE_ENABLE_INNER_RETRIES) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int ufshc_reset(uintptr_t base)
+{
+	unsigned int data;
+	int retries, result;
+
+	for (retries = 0; retries < HCE_ENABLE_OUTER_RETRIES; ++retries) {
+		result = ufshc_hce_enable(base);
+		if (result == 0) {
+			break;
+		}
+	}
+	if (retries >= HCE_ENABLE_OUTER_RETRIES) {
+		return -EIO;
+	}
 
 	/* Enable Interrupts */
 	data = UFS_INT_UCCS | UFS_INT_ULSS | UFS_INT_UE | UFS_INT_UTPES |
 	       UFS_INT_DFES | UFS_INT_HCFES | UFS_INT_SBFES;
 	mmio_write_32(base + IE, data);
+
+	return 0;
+}
+
+static int ufshc_dme_link_startup(uintptr_t base)
+{
+	uic_cmd_t cmd;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.op = DME_LINKSTARTUP;
+	return ufshc_send_uic_cmd(base, &cmd);
 }
 
 static int ufshc_link_startup(uintptr_t base)
 {
-	uic_cmd_t cmd;
 	int data, result;
 	int retries;
 
-	for (retries = 10; retries > 0; retries--) {
-		memset(&cmd, 0, sizeof(cmd));
-		cmd.op = DME_LINKSTARTUP;
-		result = ufshc_send_uic_cmd(base, &cmd);
-		if (result != 0)
+	for (retries = DME_LINKSTARTUP_RETRIES; retries > 0; retries--) {
+		result = ufshc_dme_link_startup(base);
+		if (result != 0) {
+			/* Reset controller before trying again */
+			result = ufshc_reset(base);
+			if (result != 0) {
+				return result;
+			}
 			continue;
+		}
 		while ((mmio_read_32(base + HCS) & HCS_DP) == 0)
 			;
 		data = mmio_read_32(base + IS);
@@ -772,7 +823,8 @@
 		assert((ops != NULL) && (ops->phy_init != NULL) &&
 		       (ops->phy_set_pwr_mode != NULL));
 
-		ufshc_reset(ufs_params.reg_base);
+		result = ufshc_reset(ufs_params.reg_base);
+		assert(result == 0);
 		ops->phy_init(&ufs_params);
 		result = ufshc_link_startup(ufs_params.reg_base);
 		assert(result == 0);
diff --git a/drivers/usb/usb_device.c b/drivers/usb/usb_device.c
new file mode 100644
index 0000000..ce02d4f
--- /dev/null
+++ b/drivers/usb/usb_device.c
@@ -0,0 +1,840 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/usb_device.h>
+
+/* Define for EP address */
+#define EP_DIR_MASK		BIT(7)
+#define EP_DIR_IN		BIT(7)
+#define EP_NUM_MASK		GENMASK(3, 0)
+
+#define EP0_IN			(0U | EP_DIR_IN)
+#define EP0_OUT			0U
+
+/* USB address between 1 through 127 = 0x7F mask */
+#define ADDRESS_MASK		GENMASK(6, 0)
+
+/*
+ * Set a STALL condition over an endpoint
+ * pdev: USB handle
+ * ep_addr: endpoint address
+ * return : status
+ */
+static enum usb_status usb_core_set_stall(struct usb_handle *pdev, uint8_t ep_addr)
+{
+	struct usbd_ep *ep;
+	struct pcd_handle *hpcd = (struct pcd_handle *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if (num >= USBD_EP_NB) {
+		return USBD_FAIL;
+	}
+	if ((EP_DIR_MASK & ep_addr) == EP_DIR_IN) {
+		ep = &hpcd->in_ep[num];
+		ep->is_in = true;
+	} else {
+		ep = &hpcd->out_ep[num];
+		ep->is_in = false;
+	}
+	ep->num = num;
+
+	pdev->driver->ep_set_stall(hpcd->instance, ep);
+	if (num == 0U) {
+		pdev->driver->ep0_out_start(hpcd->instance);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_get_desc
+ *         Handle Get Descriptor requests
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_get_desc(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	uint16_t len;
+	uint8_t *pbuf;
+	uint8_t desc_type = HIBYTE(req->value);
+	uint8_t desc_idx = LOBYTE(req->value);
+
+	switch (desc_type) {
+	case USB_DESC_TYPE_DEVICE:
+		pbuf = pdev->desc->get_device_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_CONFIGURATION:
+		pbuf = (uint8_t *)pdev->desc->get_config_desc(&len);
+		pbuf[1] = USB_DESC_TYPE_CONFIGURATION;
+		break;
+
+	case USB_DESC_TYPE_STRING:
+		switch (desc_idx) {
+		case USBD_IDX_LANGID_STR:
+			pbuf = pdev->desc->get_lang_id_desc(&len);
+			break;
+
+		case USBD_IDX_MFC_STR:
+			pbuf = pdev->desc->get_manufacturer_desc(&len);
+			break;
+
+		case USBD_IDX_PRODUCT_STR:
+			pbuf = pdev->desc->get_product_desc(&len);
+			break;
+
+		case USBD_IDX_SERIAL_STR:
+			pbuf = pdev->desc->get_serial_desc(&len);
+			break;
+
+		case USBD_IDX_CONFIG_STR:
+			pbuf = pdev->desc->get_configuration_desc(&len);
+			break;
+
+		case USBD_IDX_INTERFACE_STR:
+			pbuf = pdev->desc->get_interface_desc(&len);
+			break;
+
+		/* For all USER string */
+		case USBD_IDX_USER0_STR:
+		default:
+			pbuf = pdev->desc->get_usr_desc(desc_idx - USBD_IDX_USER0_STR, &len);
+			break;
+		}
+		break;
+
+	case USB_DESC_TYPE_DEVICE_QUALIFIER:
+		pbuf = (uint8_t *)pdev->desc->get_device_qualifier_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION:
+		pbuf = (uint8_t *)pdev->desc->get_config_desc(&len);
+		pbuf[1] = USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION;
+		break;
+
+	default:
+		ERROR("Unknown request %i\n", desc_type);
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	if ((len != 0U) && (req->length != 0U)) {
+		len = MIN(len, req->length);
+
+		/* Start the transfer */
+		usb_core_transmit_ep0(pdev, pbuf, len);
+	}
+}
+
+/*
+ * usb_core_set_config
+ *         Handle Set device configuration request
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_set_config(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	static uint8_t cfgidx;
+
+	cfgidx = LOBYTE(req->value);
+
+	if (cfgidx > USBD_MAX_NUM_CONFIGURATION) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	switch (pdev->dev_state) {
+	case USBD_STATE_ADDRESSED:
+		if (cfgidx != 0U) {
+			pdev->dev_config = cfgidx;
+			pdev->dev_state = USBD_STATE_CONFIGURED;
+			if (!pdev->class) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+			/* Set configuration and Start the Class */
+			if (pdev->class->init(pdev, cfgidx) != 0U) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+		}
+		break;
+
+	case USBD_STATE_CONFIGURED:
+		if (cfgidx == 0U) {
+			pdev->dev_state = USBD_STATE_ADDRESSED;
+			pdev->dev_config = cfgidx;
+			pdev->class->de_init(pdev, cfgidx);
+		} else if (cfgidx != pdev->dev_config) {
+			if (pdev->class != NULL) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+			/* Clear old configuration */
+			pdev->class->de_init(pdev, pdev->dev_config);
+			/* Set new configuration */
+			pdev->dev_config = cfgidx;
+			/* Set configuration and start the USB class */
+			if (pdev->class->init(pdev, cfgidx) != 0U) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+		}
+		break;
+
+	default:
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	/* Send status */
+	usb_core_transmit_ep0(pdev, NULL, 0U);
+}
+
+/*
+ * usb_core_get_status
+ *         Handle Get Status request
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_get_status(struct usb_handle *pdev,
+				struct usb_setup_req *req)
+{
+	if ((pdev->dev_state != USBD_STATE_ADDRESSED) &&
+	    (pdev->dev_state != USBD_STATE_CONFIGURED)) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	pdev->dev_config_status = USB_CONFIG_SELF_POWERED;
+
+	if (pdev->dev_remote_wakeup != 0U) {
+		pdev->dev_config_status |= USB_CONFIG_REMOTE_WAKEUP;
+	}
+
+	/* Start the transfer */
+	usb_core_transmit_ep0(pdev, (uint8_t *)&pdev->dev_config_status, 2U);
+}
+
+/*
+ * usb_core_set_address
+ *         Set device address
+ * pdev : device instance
+ * req : usb request
+ */
+static void usb_core_set_address(struct usb_handle *pdev,
+				 struct usb_setup_req *req)
+{
+	uint8_t dev_addr;
+
+	if ((req->index != 0U) || (req->length != 0U)) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	dev_addr = req->value & ADDRESS_MASK;
+	if (pdev->dev_state != USBD_STATE_DEFAULT) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	pdev->dev_address = dev_addr;
+	pdev->driver->set_address(((struct pcd_handle *)(pdev->data))->instance, dev_addr);
+
+	/* Send status */
+	usb_core_transmit_ep0(pdev, NULL, 0U);
+
+	if (dev_addr != 0U) {
+		pdev->dev_state  = USBD_STATE_ADDRESSED;
+	} else {
+		pdev->dev_state  = USBD_STATE_DEFAULT;
+	}
+}
+
+/*
+ * usb_core_dev_req
+ *         Handle standard usb device requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static enum usb_status usb_core_dev_req(struct usb_handle *pdev,
+					struct usb_setup_req *req)
+{
+	VERBOSE("receive request %i\n", req->b_request);
+	switch (req->b_request) {
+	case USB_REQ_GET_DESCRIPTOR:
+		usb_core_get_desc(pdev, req);
+		break;
+
+	case USB_REQ_SET_CONFIGURATION:
+		usb_core_set_config(pdev, req);
+		break;
+
+	case USB_REQ_GET_STATUS:
+		usb_core_get_status(pdev, req);
+		break;
+
+	case USB_REQ_SET_ADDRESS:
+		usb_core_set_address(pdev, req);
+		break;
+
+	case USB_REQ_GET_CONFIGURATION:
+	case USB_REQ_SET_FEATURE:
+	case USB_REQ_CLEAR_FEATURE:
+	default:
+		ERROR("NOT SUPPORTED %i\n", req->b_request);
+		usb_core_ctl_error(pdev);
+		break;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_itf_req
+ *         Handle standard usb interface requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static enum usb_status usb_core_itf_req(struct usb_handle *pdev,
+					struct usb_setup_req *req)
+{
+	if (pdev->dev_state != USBD_STATE_CONFIGURED) {
+		usb_core_ctl_error(pdev);
+		return USBD_OK;
+	}
+
+	if (LOBYTE(req->index) <= USBD_MAX_NUM_INTERFACES) {
+		pdev->class->setup(pdev, req);
+
+		if (req->length == 0U) {
+			usb_core_transmit_ep0(pdev, NULL, 0U);
+		}
+	} else {
+		usb_core_ctl_error(pdev);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_setup_stage
+ *         Handle the setup stage
+ * pdev: device instance
+ * psetup : setup buffer
+ * return : status
+ */
+static enum usb_status usb_core_setup_stage(struct usb_handle *pdev,
+					    uint8_t *psetup)
+{
+	struct usb_setup_req *req = &pdev->request;
+
+	/* Copy setup buffer into req structure */
+	req->bm_request = psetup[0];
+	req->b_request = psetup[1];
+	req->value = psetup[2] + (psetup[3] << 8);
+	req->index = psetup[4] + (psetup[5] << 8);
+	req->length = psetup[6] + (psetup[7] << 8);
+
+	pdev->ep0_state = USBD_EP0_SETUP;
+	pdev->ep0_data_len = pdev->request.length;
+
+	switch (pdev->request.bm_request & USB_REQ_RECIPIENT_MASK) {
+	case USB_REQ_RECIPIENT_DEVICE:
+		usb_core_dev_req(pdev, &pdev->request);
+		break;
+
+	case USB_REQ_RECIPIENT_INTERFACE:
+		usb_core_itf_req(pdev, &pdev->request);
+		break;
+
+	case USB_REQ_RECIPIENT_ENDPOINT:
+	default:
+		ERROR("receive unsupported request %i",
+		      pdev->request.bm_request & USB_REQ_RECIPIENT_MASK);
+		usb_core_set_stall(pdev, pdev->request.bm_request & USB_REQ_DIRECTION);
+		return USBD_FAIL;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_data_out
+ *         Handle data OUT stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * pdata: buffer to sent
+ * return : status
+ */
+static enum usb_status usb_core_data_out(struct usb_handle *pdev, uint8_t epnum,
+					 uint8_t *pdata)
+{
+	struct usb_endpoint *pep;
+
+	if (epnum == 0U) {
+		pep = &pdev->ep_out[0];
+		if (pdev->ep0_state == USBD_EP0_DATA_OUT) {
+			if (pep->rem_length > pep->maxpacket) {
+				pep->rem_length -= pep->maxpacket;
+
+				usb_core_receive(pdev, 0U, pdata,
+						 MIN(pep->rem_length,
+						     pep->maxpacket));
+			} else {
+				if (pdev->class->ep0_rx_ready &&
+				    (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+					pdev->class->ep0_rx_ready(pdev);
+				}
+
+				usb_core_transmit_ep0(pdev, NULL, 0U);
+			}
+		}
+	} else if (pdev->class->data_out != NULL &&
+		   (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+		pdev->class->data_out(pdev, epnum);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_data_in
+ *         Handle data in stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * pdata: buffer to fill
+ * return : status
+ */
+static enum usb_status usb_core_data_in(struct usb_handle *pdev, uint8_t epnum,
+					uint8_t *pdata)
+{
+	if (epnum == 0U) {
+		struct usb_endpoint *pep = &pdev->ep_in[0];
+
+		if (pdev->ep0_state == USBD_EP0_DATA_IN) {
+			if (pep->rem_length > pep->maxpacket) {
+				pep->rem_length -= pep->maxpacket;
+
+				usb_core_transmit(pdev, 0U, pdata,
+						  pep->rem_length);
+
+				/* Prepare EP for premature end of transfer */
+				usb_core_receive(pdev, 0U, NULL, 0U);
+			} else {
+				/* Last packet is MPS multiple, send ZLP packet */
+				if ((pep->total_length % pep->maxpacket == 0U) &&
+				    (pep->total_length >= pep->maxpacket) &&
+				    (pep->total_length < pdev->ep0_data_len)) {
+					usb_core_transmit(pdev, 0U, NULL, 0U);
+
+					pdev->ep0_data_len = 0U;
+
+					/* Prepare endpoint for premature end of transfer */
+					usb_core_receive(pdev, 0U, NULL, 0U);
+				} else {
+					if (pdev->class->ep0_tx_sent != NULL &&
+					    (pdev->dev_state ==
+					     USBD_STATE_CONFIGURED)) {
+						pdev->class->ep0_tx_sent(pdev);
+					}
+					/* Start the transfer */
+					usb_core_receive_ep0(pdev, NULL, 0U);
+				}
+			}
+		}
+	} else if ((pdev->class->data_in != NULL) &&
+		  (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+		pdev->class->data_in(pdev, epnum);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_suspend
+ *         Handle suspend event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_suspend(struct usb_handle  *pdev)
+{
+	INFO("USB Suspend mode\n");
+	pdev->dev_old_state =  pdev->dev_state;
+	pdev->dev_state  = USBD_STATE_SUSPENDED;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_resume
+ *         Handle resume event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_resume(struct usb_handle *pdev)
+{
+	INFO("USB Resume\n");
+	pdev->dev_state = pdev->dev_old_state;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_sof
+ *         Handle SOF event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_sof(struct usb_handle *pdev)
+{
+	if (pdev->dev_state == USBD_STATE_CONFIGURED) {
+		if (pdev->class->sof != NULL) {
+			pdev->class->sof(pdev);
+		}
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_disconnect
+ *         Handle device disconnection event
+ * pdev : device instance
+ * return : status
+ */
+static enum usb_status usb_core_disconnect(struct usb_handle *pdev)
+{
+	/* Free class resources */
+	pdev->dev_state = USBD_STATE_DEFAULT;
+	pdev->class->de_init(pdev, pdev->dev_config);
+
+	return USBD_OK;
+}
+
+enum usb_status usb_core_handle_it(struct usb_handle *pdev)
+{
+	uint32_t param = 0U;
+	uint32_t len = 0U;
+	struct usbd_ep *ep;
+
+	switch (pdev->driver->it_handler(pdev->data->instance, &param)) {
+	case USB_DATA_OUT:
+		usb_core_data_out(pdev, param,
+				  pdev->data->out_ep[param].xfer_buff);
+		break;
+
+	case USB_DATA_IN:
+		usb_core_data_in(pdev, param,
+				 pdev->data->in_ep[param].xfer_buff);
+		break;
+
+	case USB_SETUP:
+		usb_core_setup_stage(pdev, (uint8_t *)pdev->data->setup);
+		break;
+
+	case USB_ENUM_DONE:
+		break;
+
+	case USB_READ_DATA_PACKET:
+		ep = &pdev->data->out_ep[param &  USBD_OUT_EPNUM_MASK];
+		len = (param &  USBD_OUT_COUNT_MASK) >> USBD_OUT_COUNT_SHIFT;
+		pdev->driver->read_packet(pdev->data->instance,
+					  ep->xfer_buff, len);
+		ep->xfer_buff += len;
+		ep->xfer_count += len;
+		break;
+
+	case USB_READ_SETUP_PACKET:
+		ep = &pdev->data->out_ep[param &  USBD_OUT_EPNUM_MASK];
+		len = (param &  USBD_OUT_COUNT_MASK) >> 0x10;
+		pdev->driver->read_packet(pdev->data->instance,
+					  (uint8_t *)pdev->data->setup, 8);
+		ep->xfer_count += len;
+		break;
+
+	case USB_RESET:
+		pdev->dev_state = USBD_STATE_DEFAULT;
+		break;
+
+	case USB_RESUME:
+		if (pdev->data->lpm_state == LPM_L1) {
+			pdev->data->lpm_state = LPM_L0;
+		} else {
+			usb_core_resume(pdev);
+		}
+		break;
+
+	case USB_SUSPEND:
+		usb_core_suspend(pdev);
+		break;
+
+	case USB_LPM:
+		if (pdev->data->lpm_state == LPM_L0) {
+			pdev->data->lpm_state = LPM_L1;
+		} else {
+			usb_core_suspend(pdev);
+		}
+		break;
+
+	case USB_SOF:
+		usb_core_sof(pdev);
+		break;
+
+	case USB_DISCONNECT:
+		usb_core_disconnect(pdev);
+		break;
+
+	case USB_WRITE_EMPTY:
+		pdev->driver->write_empty_tx_fifo(pdev->data->instance, param,
+				     pdev->data->in_ep[param].xfer_len,
+				     (uint32_t *)&pdev->data->in_ep[param].xfer_count,
+				     pdev->data->in_ep[param].maxpacket,
+				     &pdev->data->in_ep[param].xfer_buff);
+		break;
+
+	case USB_NOTHING:
+	default:
+		break;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_receive
+ *          Receive an amount of data
+ * pdev: USB handle
+ * ep_addr: endpoint address
+ * buf: pointer to the reception buffer
+ * len: amount of data to be received
+ * return : status
+ */
+enum usb_status usb_core_receive(struct usb_handle *pdev, uint8_t ep_addr,
+				 uint8_t *buf, uint32_t len)
+{
+	struct usbd_ep *ep;
+	struct pcd_handle *hpcd = (struct pcd_handle *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if (num >= USBD_EP_NB) {
+		return USBD_FAIL;
+	}
+	ep = &hpcd->out_ep[num];
+
+	/* Setup and start the Xfer */
+	ep->xfer_buff = buf;
+	ep->xfer_len = len;
+	ep->xfer_count = 0U;
+	ep->is_in = false;
+	ep->num = num;
+
+	if (num == 0U) {
+		pdev->driver->ep0_start_xfer(hpcd->instance, ep);
+	} else {
+		pdev->driver->ep_start_xfer(hpcd->instance, ep);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_transmit
+ *          Send an amount of data
+ * pdev: USB handle
+ * ep_addr: endpoint address
+ * buf: pointer to the transmission buffer
+ * len: amount of data to be sent
+ * return : status
+ */
+enum usb_status usb_core_transmit(struct usb_handle *pdev, uint8_t ep_addr,
+				  uint8_t *buf, uint32_t len)
+{
+	struct usbd_ep *ep;
+	struct pcd_handle *hpcd = (struct pcd_handle *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if (num >= USBD_EP_NB) {
+		return USBD_FAIL;
+	}
+	ep = &hpcd->in_ep[num];
+
+	/* Setup and start the Xfer */
+	ep->xfer_buff = buf;
+	ep->xfer_len = len;
+	ep->xfer_count = 0U;
+	ep->is_in = true;
+	ep->num = num;
+
+	if (num == 0U) {
+		pdev->driver->ep0_start_xfer(hpcd->instance, ep);
+	} else {
+		pdev->driver->ep_start_xfer(hpcd->instance, ep);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_receive_ep0
+ *          Receive an amount of data on ep0
+ * pdev: USB handle
+ * buf: pointer to the reception buffer
+ * len: amount of data to be received
+ * return : status
+ */
+enum usb_status usb_core_receive_ep0(struct usb_handle *pdev, uint8_t *buf,
+				     uint32_t len)
+{
+	/* Prepare the reception of the buffer over EP0 */
+	if (len != 0U) {
+		pdev->ep0_state = USBD_EP0_DATA_OUT;
+	} else {
+		pdev->ep0_state = USBD_EP0_STATUS_OUT;
+	}
+
+	pdev->ep_out[0].total_length = len;
+	pdev->ep_out[0].rem_length = len;
+
+	/* Start the transfer */
+	return usb_core_receive(pdev, 0U, buf, len);
+}
+
+/*
+ * usb_core_transmit_ep0
+ *          Send an amount of data on ep0
+ * pdev: USB handle
+ * buf: pointer to the transmission buffer
+ * len: amount of data to be sent
+ * return : status
+ */
+enum usb_status usb_core_transmit_ep0(struct usb_handle *pdev, uint8_t *buf,
+				      uint32_t len)
+{
+	/* Set EP0 State */
+	if (len != 0U) {
+		pdev->ep0_state = USBD_EP0_DATA_IN;
+	} else {
+		pdev->ep0_state = USBD_EP0_STATUS_IN;
+	}
+
+	pdev->ep_in[0].total_length = len;
+	pdev->ep_in[0].rem_length = len;
+
+	/* Start the transfer */
+	return usb_core_transmit(pdev, 0U, buf, len);
+}
+
+/*
+ * usb_core_ctl_error
+ *         Handle USB low level error
+ * pdev: device instance
+ * req: usb request
+ * return : None
+ */
+
+void usb_core_ctl_error(struct usb_handle *pdev)
+{
+	ERROR("%s : Send an ERROR\n", __func__);
+	usb_core_set_stall(pdev, EP0_IN);
+	usb_core_set_stall(pdev, EP0_OUT);
+}
+
+/*
+ * usb_core_start
+ *         Start the USB device core.
+ * pdev: Device Handle
+ * return : USBD Status
+ */
+enum usb_status usb_core_start(struct usb_handle *pdev)
+{
+	/* Start the low level driver */
+	pdev->driver->start_device(pdev->data->instance);
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_stop
+ *         Stop the USB device core.
+ * pdev: Device Handle
+ * return : USBD Status
+ */
+enum usb_status usb_core_stop(struct usb_handle *pdev)
+{
+	/* Free class resources */
+	pdev->class->de_init(pdev, pdev->dev_config);
+
+	/* Stop the low level driver */
+	pdev->driver->stop_device(pdev->data->instance);
+
+	return USBD_OK;
+}
+
+/*
+ * register_usb_driver
+ *         Stop the USB device core.
+ * pdev: Device Handle
+ * pcd_handle: PCD handle
+ * driver: USB driver
+ * driver_handle: USB driver handle
+ * return : USBD Status
+ */
+enum usb_status register_usb_driver(struct usb_handle *pdev,
+				    struct pcd_handle *pcd_handle,
+				    const struct usb_driver *driver,
+				    void *driver_handle)
+{
+	uint8_t i;
+
+	assert(pdev != NULL);
+	assert(pcd_handle != NULL);
+	assert(driver != NULL);
+	assert(driver_handle != NULL);
+
+	/* Free class resources */
+	pdev->driver = driver;
+	pdev->data = pcd_handle;
+	pdev->data->instance = driver_handle;
+	pdev->dev_state = USBD_STATE_DEFAULT;
+	pdev->ep0_state = USBD_EP0_IDLE;
+
+	/* Copy endpoint information */
+	for (i = 0U; i < USBD_EP_NB; i++) {
+		pdev->ep_in[i].maxpacket = pdev->data->in_ep[i].maxpacket;
+		pdev->ep_out[i].maxpacket = pdev->data->out_ep[i].maxpacket;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * register_platform
+ *         Register the USB device core.
+ * pdev: Device Handle
+ * plat_call_back: callback
+ * return : USBD Status
+ */
+enum usb_status register_platform(struct usb_handle *pdev,
+			       const struct usb_desc *plat_call_back)
+{
+	assert(pdev != NULL);
+	assert(plat_call_back != NULL);
+
+	/* Save platform info in class resources */
+	pdev->desc = plat_call_back;
+
+	return USBD_OK;
+}
diff --git a/fdts/stm32mp15-bl2.dtsi b/fdts/stm32mp15-bl2.dtsi
index da95b25..074414b 100644
--- a/fdts/stm32mp15-bl2.dtsi
+++ b/fdts/stm32mp15-bl2.dtsi
@@ -4,6 +4,12 @@
  */
 
 / {
+#if !STM32MP_EMMC && !STM32MP_SDMMC
+	aliases {
+		/delete-property/ mmc0;
+	};
+#endif
+
 	cpus {
 		/delete-node/ cpu@1;
 	};
@@ -13,9 +19,25 @@
 	soc {
 		/delete-node/ timer@40006000;
 		/delete-node/ timer@44006000;
+#if !STM32MP_USB_PROGRAMMER
+		/delete-node/ usb-otg@49000000;
+#endif
 		/delete-node/ pwr_mcu@50001014;
 		/delete-node/ cryp@54001000;
 		/delete-node/ rng@54003000;
+#if !STM32MP_RAW_NAND
+		/delete-node/ memory-controller@58002000;
+#endif
+#if !STM32MP_SPI_NAND && !STM32MP_SPI_NOR
+		/delete-node/ spi@58003000;
+#endif
+#if !STM32MP_EMMC && !STM32MP_SDMMC
+		/delete-node/ mmc@58005000;
+		/delete-node/ mmc@58007000;
+#endif
+#if !STM32MP_USB_PROGRAMMER
+		/delete-node/ usbphyc@5a006000;
+#endif
 		/delete-node/ spi@5c001000;
 		/delete-node/ rtc@5c004000;
 		/delete-node/ etzpc@5c007000;
@@ -24,7 +46,25 @@
 		/delete-node/ tamp@5c00a000;
 
 		pin-controller@50002000 {
-			/delete-node/ rtc-out2-rmp-pins-0;
+#if !STM32MP_RAW_NAND
+			/delete-node/ fmc-0;
+#endif
+#if !STM32MP_SPI_NAND && !STM32MP_SPI_NOR
+			/delete-node/ qspi-clk-0;
+			/delete-node/ qspi-bk1-0;
+			/delete-node/ qspi-bk2-0;
+#endif
+#if !STM32MP_EMMC && !STM32MP_SDMMC
+			/delete-node/ sdmmc1-b4-0;
+			/delete-node/ sdmmc1-dir-0;
+			/delete-node/ sdmmc2-b4-0;
+			/delete-node/ sdmmc2-b4-1;
+			/delete-node/ sdmmc2-d47-0;
+#endif
+#if !STM32MP_USB_PROGRAMMER
+			/delete-node/ usbotg_hs-0;
+			/delete-node/ usbotg-fs-dp-dm-0;
+#endif
 		};
 	};
 
diff --git a/fdts/stm32mp15-bl32.dtsi b/fdts/stm32mp15-bl32.dtsi
index f005d56..ca4bb3e 100644
--- a/fdts/stm32mp15-bl32.dtsi
+++ b/fdts/stm32mp15-bl32.dtsi
@@ -20,8 +20,8 @@
 		/delete-node/ hash@54002000;
 		/delete-node/ memory-controller@58002000;
 		/delete-node/ spi@58003000;
-		/delete-node/ sdmmc@58005000;
-		/delete-node/ sdmmc@58007000;
+		/delete-node/ mmc@58005000;
+		/delete-node/ mmc@58007000;
 		/delete-node/ usbphyc@5a006000;
 		/delete-node/ spi@5c001000;
 		/delete-node/ stgen@5c008000;
@@ -37,6 +37,8 @@
 			/delete-node/ sdmmc2-b4-0;
 			/delete-node/ sdmmc2-b4-1;
 			/delete-node/ sdmmc2-d47-0;
+			/delete-node/ sdmmc2-d47-1;
+			/delete-node/ sdmmc2-d47-3;
 			/delete-node/ usbotg_hs-0;
 			/delete-node/ usbotg-fs-dp-dm-0;
 		};
diff --git a/fdts/stm32mp15-ddr.dtsi b/fdts/stm32mp15-ddr.dtsi
index 4825691..e5efd92 100644
--- a/fdts/stm32mp15-ddr.dtsi
+++ b/fdts/stm32mp15-ddr.dtsi
@@ -1,153 +1,127 @@
 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
 /*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved
  */
 
-/ {
-	soc {
-		ddr: ddr@5a003000{
+&ddr {
+	st,mem-name = DDR_MEM_NAME;
+	st,mem-speed = <DDR_MEM_SPEED>;
+	st,mem-size = <DDR_MEM_SIZE>;
 
-			compatible = "st,stm32mp1-ddr";
+	st,ctl-reg = <
+		DDR_MSTR
+		DDR_MRCTRL0
+		DDR_MRCTRL1
+		DDR_DERATEEN
+		DDR_DERATEINT
+		DDR_PWRCTL
+		DDR_PWRTMG
+		DDR_HWLPCTL
+		DDR_RFSHCTL0
+		DDR_RFSHCTL3
+		DDR_CRCPARCTL0
+		DDR_ZQCTL0
+		DDR_DFITMG0
+		DDR_DFITMG1
+		DDR_DFILPCFG0
+		DDR_DFIUPD0
+		DDR_DFIUPD1
+		DDR_DFIUPD2
+		DDR_DFIPHYMSTR
+		DDR_ODTMAP
+		DDR_DBG0
+		DDR_DBG1
+		DDR_DBGCMD
+		DDR_POISONCFG
+		DDR_PCCFG
+	>;
 
-			reg = <0x5A003000 0x550
-			       0x5A004000 0x234>;
+	st,ctl-timing = <
+		DDR_RFSHTMG
+		DDR_DRAMTMG0
+		DDR_DRAMTMG1
+		DDR_DRAMTMG2
+		DDR_DRAMTMG3
+		DDR_DRAMTMG4
+		DDR_DRAMTMG5
+		DDR_DRAMTMG6
+		DDR_DRAMTMG7
+		DDR_DRAMTMG8
+		DDR_DRAMTMG14
+		DDR_ODTCFG
+	>;
 
-			clocks = <&rcc AXIDCG>,
-				 <&rcc DDRC1>,
-				 <&rcc DDRC2>,
-				 <&rcc DDRPHYC>,
-				 <&rcc DDRCAPB>,
-				 <&rcc DDRPHYCAPB>;
+	st,ctl-map = <
+		DDR_ADDRMAP1
+		DDR_ADDRMAP2
+		DDR_ADDRMAP3
+		DDR_ADDRMAP4
+		DDR_ADDRMAP5
+		DDR_ADDRMAP6
+		DDR_ADDRMAP9
+		DDR_ADDRMAP10
+		DDR_ADDRMAP11
+	>;
 
-			clock-names = "axidcg",
-				      "ddrc1",
-				      "ddrc2",
-				      "ddrphyc",
-				      "ddrcapb",
-				      "ddrphycapb";
+	st,ctl-perf = <
+		DDR_SCHED
+		DDR_SCHED1
+		DDR_PERFHPR1
+		DDR_PERFLPR1
+		DDR_PERFWR1
+		DDR_PCFGR_0
+		DDR_PCFGW_0
+		DDR_PCFGQOS0_0
+		DDR_PCFGQOS1_0
+		DDR_PCFGWQOS0_0
+		DDR_PCFGWQOS1_0
+		DDR_PCFGR_1
+		DDR_PCFGW_1
+		DDR_PCFGQOS0_1
+		DDR_PCFGQOS1_1
+		DDR_PCFGWQOS0_1
+		DDR_PCFGWQOS1_1
+	>;
 
-			st,mem-name = DDR_MEM_NAME;
-			st,mem-speed = <DDR_MEM_SPEED>;
-			st,mem-size = <DDR_MEM_SIZE>;
+	st,phy-reg = <
+		DDR_PGCR
+		DDR_ACIOCR
+		DDR_DXCCR
+		DDR_DSGCR
+		DDR_DCR
+		DDR_ODTCR
+		DDR_ZQ0CR1
+		DDR_DX0GCR
+		DDR_DX1GCR
+		DDR_DX2GCR
+		DDR_DX3GCR
+	>;
 
-			st,ctl-reg = <
-				DDR_MSTR
-				DDR_MRCTRL0
-				DDR_MRCTRL1
-				DDR_DERATEEN
-				DDR_DERATEINT
-				DDR_PWRCTL
-				DDR_PWRTMG
-				DDR_HWLPCTL
-				DDR_RFSHCTL0
-				DDR_RFSHCTL3
-				DDR_CRCPARCTL0
-				DDR_ZQCTL0
-				DDR_DFITMG0
-				DDR_DFITMG1
-				DDR_DFILPCFG0
-				DDR_DFIUPD0
-				DDR_DFIUPD1
-				DDR_DFIUPD2
-				DDR_DFIPHYMSTR
-				DDR_ODTMAP
-				DDR_DBG0
-				DDR_DBG1
-				DDR_DBGCMD
-				DDR_POISONCFG
-				DDR_PCCFG
-			>;
+	st,phy-timing = <
+		DDR_PTR0
+		DDR_PTR1
+		DDR_PTR2
+		DDR_DTPR0
+		DDR_DTPR1
+		DDR_DTPR2
+		DDR_MR0
+		DDR_MR1
+		DDR_MR2
+		DDR_MR3
+	>;
 
-			st,ctl-timing = <
-				DDR_RFSHTMG
-				DDR_DRAMTMG0
-				DDR_DRAMTMG1
-				DDR_DRAMTMG2
-				DDR_DRAMTMG3
-				DDR_DRAMTMG4
-				DDR_DRAMTMG5
-				DDR_DRAMTMG6
-				DDR_DRAMTMG7
-				DDR_DRAMTMG8
-				DDR_DRAMTMG14
-				DDR_ODTCFG
-			>;
-
-			st,ctl-map = <
-				DDR_ADDRMAP1
-				DDR_ADDRMAP2
-				DDR_ADDRMAP3
-				DDR_ADDRMAP4
-				DDR_ADDRMAP5
-				DDR_ADDRMAP6
-				DDR_ADDRMAP9
-				DDR_ADDRMAP10
-				DDR_ADDRMAP11
-			>;
-
-			st,ctl-perf = <
-				DDR_SCHED
-				DDR_SCHED1
-				DDR_PERFHPR1
-				DDR_PERFLPR1
-				DDR_PERFWR1
-				DDR_PCFGR_0
-				DDR_PCFGW_0
-				DDR_PCFGQOS0_0
-				DDR_PCFGQOS1_0
-				DDR_PCFGWQOS0_0
-				DDR_PCFGWQOS1_0
-				DDR_PCFGR_1
-				DDR_PCFGW_1
-				DDR_PCFGQOS0_1
-				DDR_PCFGQOS1_1
-				DDR_PCFGWQOS0_1
-				DDR_PCFGWQOS1_1
-			>;
-
-			st,phy-reg = <
-				DDR_PGCR
-				DDR_ACIOCR
-				DDR_DXCCR
-				DDR_DSGCR
-				DDR_DCR
-				DDR_ODTCR
-				DDR_ZQ0CR1
-				DDR_DX0GCR
-				DDR_DX1GCR
-				DDR_DX2GCR
-				DDR_DX3GCR
-			>;
-
-			st,phy-timing = <
-				DDR_PTR0
-				DDR_PTR1
-				DDR_PTR2
-				DDR_DTPR0
-				DDR_DTPR1
-				DDR_DTPR2
-				DDR_MR0
-				DDR_MR1
-				DDR_MR2
-				DDR_MR3
-			>;
-
-			st,phy-cal = <
-				DDR_DX0DLLCR
-				DDR_DX0DQTR
-				DDR_DX0DQSTR
-				DDR_DX1DLLCR
-				DDR_DX1DQTR
-				DDR_DX1DQSTR
-				DDR_DX2DLLCR
-				DDR_DX2DQTR
-				DDR_DX2DQSTR
-				DDR_DX3DLLCR
-				DDR_DX3DQTR
-				DDR_DX3DQSTR
-			>;
-
-			status = "okay";
-		};
-	};
+	st,phy-cal = <
+		DDR_DX0DLLCR
+		DDR_DX0DQTR
+		DDR_DX0DQSTR
+		DDR_DX1DLLCR
+		DDR_DX1DQTR
+		DDR_DX1DQSTR
+		DDR_DX2DLLCR
+		DDR_DX2DQTR
+		DDR_DX2DQSTR
+		DDR_DX3DLLCR
+		DDR_DX3DQTR
+		DDR_DX3DQSTR
+	>;
 };
diff --git a/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi b/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
index c0fc1f7..6ca6293 100644
--- a/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
+++ b/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
@@ -15,7 +15,7 @@
  * Save Date: 2020.02.20, save Time: 18:45:20
  */
 
-#define DDR_MEM_NAME	"DDR3-DDR3L 16bits 533000Khz"
+#define DDR_MEM_NAME	"DDR3-DDR3L 16bits 533000kHz"
 #define DDR_MEM_SPEED	533000
 #define DDR_MEM_SIZE	0x20000000
 
diff --git a/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi b/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
index fc226d2..548f69a 100644
--- a/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
+++ b/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
@@ -15,7 +15,7 @@
  * Save Date: 2020.02.20, save Time: 18:49:33
  */
 
-#define DDR_MEM_NAME	"DDR3-DDR3L 32bits 533000Khz"
+#define DDR_MEM_NAME	"DDR3-DDR3L 32bits 533000kHz"
 #define DDR_MEM_SPEED	533000
 #define DDR_MEM_SIZE	0x40000000
 
diff --git a/fdts/stm32mp15-pinctrl.dtsi b/fdts/stm32mp15-pinctrl.dtsi
index 058cde2..e8e6b9f 100644
--- a/fdts/stm32mp15-pinctrl.dtsi
+++ b/fdts/stm32mp15-pinctrl.dtsi
@@ -86,12 +86,6 @@
 		};
 	};
 
-	rtc_out2_rmp_pins_a: rtc-out2-rmp-pins-0 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 8, ANALOG)>; /* RTC_OUT2_RMP */
-		};
-	};
-
 	sdmmc1_b4_pins_a: sdmmc1-b4-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('C', 8, AF12)>, /* SDMMC1_D0 */
@@ -176,6 +170,18 @@
 		};
 	};
 
+	sdmmc2_d47_pins_b: sdmmc2-d47-1 {
+		pins {
+			pinmux = <STM32_PINMUX('A', 8, AF9)>,  /* SDMMC2_D4 */
+				 <STM32_PINMUX('A', 9, AF10)>, /* SDMMC2_D5 */
+				 <STM32_PINMUX('C', 6, AF10)>, /* SDMMC2_D6 */
+				 <STM32_PINMUX('C', 7, AF10)>; /* SDMMC2_D7 */
+			slew-rate = <1>;
+			drive-push-pull;
+			bias-disable;
+		};
+	};
+
 	sdmmc2_d47_pins_d: sdmmc2-d47-3 {
 		pins {
 			pinmux = <STM32_PINMUX('A', 8, AF9)>, /* SDMMC2_D4 */
@@ -213,34 +219,90 @@
 
 	uart7_pins_a: uart7-0 {
 		pins1 {
-			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART4_TX */
+			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART7_TX */
 			bias-disable;
 			drive-push-pull;
 			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('E', 7, AF7)>, /* UART4_RX */
-				 <STM32_PINMUX('E', 10, AF7)>, /* UART4_CTS */
-				 <STM32_PINMUX('E', 9, AF7)>; /* UART4_RTS */
+			pinmux = <STM32_PINMUX('E', 7, AF7)>, /* UART7_RX */
+				 <STM32_PINMUX('E', 10, AF7)>, /* UART7_CTS */
+				 <STM32_PINMUX('E', 9, AF7)>; /* UART7_RTS */
 			bias-disable;
 		};
 	};
 
 	uart7_pins_b: uart7-1 {
 		pins1 {
-			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* USART7_TX */
+			pinmux = <STM32_PINMUX('F', 7, AF7)>; /* UART7_TX */
 			bias-disable;
 			drive-push-pull;
 			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* USART7_RX */
+			pinmux = <STM32_PINMUX('F', 6, AF7)>; /* UART7_RX */
+			bias-disable;
+		};
+	};
+
+	uart7_pins_c: uart7-2 {
+		pins1 {
+			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART7_TX */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* UART7_RX */
+			bias-disable;
+		};
+	};
+
+	uart8_pins_a: uart8-0 {
+		pins1 {
+			pinmux = <STM32_PINMUX('E', 1, AF8)>; /* UART8_TX */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('E', 0, AF8)>; /* UART8_RX */
 			bias-disable;
 		};
 	};
 
 	usart2_pins_a: usart2-0 {
 		pins1 {
+			pinmux = <STM32_PINMUX('F', 5, AF7)>, /* USART2_TX */
+				 <STM32_PINMUX('D', 4, AF7)>; /* USART2_RTS */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('D', 6, AF7)>, /* USART2_RX */
+				 <STM32_PINMUX('D', 3, AF7)>; /* USART2_CTS_NSS */
+			bias-disable;
+		};
+	};
+
+	usart2_pins_b: usart2-1 {
+		pins1 {
+			pinmux = <STM32_PINMUX('F', 5, AF7)>, /* USART2_TX */
+				 <STM32_PINMUX('A', 1, AF7)>; /* USART2_RTS */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('F', 4, AF7)>, /* USART2_RX */
+				 <STM32_PINMUX('E', 15, AF7)>; /* USART2_CTS_NSS */
+			bias-disable;
+		};
+	};
+
+	usart2_pins_c: usart2-2 {
+		pins1 {
 			pinmux = <STM32_PINMUX('D', 5, AF7)>, /* USART2_TX */
 				 <STM32_PINMUX('D', 4, AF7)>; /* USART2_RTS */
 			bias-disable;
@@ -256,15 +318,13 @@
 
 	usart3_pins_a: usart3-0 {
 		pins1 {
-			pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
-				 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
+			pinmux = <STM32_PINMUX('B', 10, AF7)>; /* USART3_TX */
 			bias-disable;
 			drive-push-pull;
 			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
-				 <STM32_PINMUX('I', 10, AF8)>; /* USART3_CTS_NSS */
+			pinmux = <STM32_PINMUX('B', 12, AF8)>; /* USART3_RX */
 			bias-disable;
 		};
 	};
@@ -279,12 +339,27 @@
 		};
 		pins2 {
 			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
+				 <STM32_PINMUX('I', 10, AF8)>; /* USART3_CTS_NSS */
+			bias-disable;
+		};
+	};
+
+	usart3_pins_c: usart3-2 {
+		pins1 {
+			pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
+				 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
 				 <STM32_PINMUX('B', 13, AF7)>; /* USART3_CTS_NSS */
 			bias-disable;
 		};
 	};
 
-	usbotg_hs_pins_a: usbotg_hs-0 {
+	usbotg_hs_pins_a: usbotg-hs-0 {
 		pins {
 			pinmux = <STM32_PINMUX('A', 10, ANALOG)>; /* OTG_ID */
 		};
diff --git a/fdts/stm32mp151.dtsi b/fdts/stm32mp151.dtsi
index c350c66..3beabbb 100644
--- a/fdts/stm32mp151.dtsi
+++ b/fdts/stm32mp151.dtsi
@@ -174,7 +174,7 @@
 		};
 
 		usbotg_hs: usb-otg@49000000 {
-			compatible = "st,stm32mp1-hsotg", "snps,dwc2";
+			compatible = "st,stm32mp15-hsotg", "snps,dwc2";
 			reg = <0x49000000 0x10000>;
 			clocks = <&rcc USBO_K>;
 			clock-names = "otg";
@@ -319,7 +319,7 @@
 			status = "disabled";
 		};
 
-		sdmmc1: sdmmc@58005000 {
+		sdmmc1: mmc@58005000 {
 			compatible = "st,stm32-sdmmc2", "arm,pl18x", "arm,primecell";
 			arm,primecell-periphid = <0x00253180>;
 			reg = <0x58005000 0x1000>, <0x58006000 0x1000>;
@@ -334,7 +334,7 @@
 			status = "disabled";
 		};
 
-		sdmmc2: sdmmc@58007000 {
+		sdmmc2: mmc@58007000 {
 			compatible = "st,stm32-sdmmc2", "arm,pl18x", "arm,primecell";
 			arm,primecell-periphid = <0x00253180>;
 			reg = <0x58007000 0x1000>, <0x58008000 0x1000>;
@@ -358,6 +358,24 @@
 			status = "disabled";
 		};
 
+		ddr: ddr@5a003000{
+			compatible = "st,stm32mp1-ddr";
+			reg = <0x5A003000 0x550 0x5A004000 0x234>;
+			clocks = <&rcc AXIDCG>,
+				 <&rcc DDRC1>,
+				 <&rcc DDRC2>,
+				 <&rcc DDRPHYC>,
+				 <&rcc DDRCAPB>,
+				 <&rcc DDRPHYCAPB>;
+			clock-names = "axidcg",
+				      "ddrc1",
+				      "ddrc2",
+				      "ddrphyc",
+				      "ddrcapb",
+				      "ddrphycapb";
+			status = "okay";
+		};
+
 		usbphyc: usbphyc@5a006000 {
 			#address-cells = <1>;
 			#size-cells = <0>;
@@ -434,7 +452,7 @@
 			status = "disabled";
 		};
 
-		bsec: nvmem@5c005000 {
+		bsec: efuse@5c005000 {
 			compatible = "st,stm32mp15-bsec";
 			reg = <0x5c005000 0x400>;
 			#address-cells = <1>;
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index a6b98b7..a09c66a 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -20,7 +20,6 @@
 		stdout-path = "serial0:115200n8";
 	};
 
-
 	memory@c0000000 {
 		device_type = "memory";
 		reg = <0xC0000000 0x40000000>;
@@ -52,7 +51,7 @@
 };
 
 &cryp1 {
-	status="okay";
+	status = "okay";
 };
 
 &hash1 {
@@ -233,7 +232,7 @@
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
-		CLK_ETH_DISABLED
+		CLK_ETH_PLL4P
 		CLK_SDMMC12_PLL4P
 		CLK_DSI_DSIPLL
 		CLK_STGEN_HSE
@@ -269,25 +268,33 @@
 
 	/* VCO = 1300.0 MHz => P = 650 (CPU) */
 	pll1: st,pll@0 {
-		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
-		frac = < 0x800 >;
+		compatible = "st,stm32mp1-pll";
+		reg = <0>;
+		cfg = <2 80 0 0 0 PQR(1,0,0)>;
+		frac = <0x800>;
 	};
 
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
-		cfg = < 2 65 1 0 0 PQR(1,1,1) >;
-		frac = < 0x1400 >;
+		compatible = "st,stm32mp1-pll";
+		reg = <1>;
+		cfg = <2 65 1 0 0 PQR(1,1,1)>;
+		frac = <0x1400>;
 	};
 
 	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
 	pll3: st,pll@2 {
-		cfg = < 1 33 1 16 36 PQR(1,1,1) >;
-		frac = < 0x1a04 >;
+		compatible = "st,stm32mp1-pll";
+		reg = <2>;
+		cfg = <1 33 1 16 36 PQR(1,1,1)>;
+		frac = <0x1a04>;
 	};
 
 	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
 	pll4: st,pll@3 {
-		cfg = < 3 98 5 7 7 PQR(1,1,1) >;
+		compatible = "st,stm32mp1-pll";
+		reg = <3>;
+		cfg = <3 98 5 7 7 PQR(1,1,1)>;
 	};
 };
 
diff --git a/fdts/stm32mp157c-ev1.dts b/fdts/stm32mp157c-ev1.dts
index c5d12e3..4937514 100644
--- a/fdts/stm32mp157c-ev1.dts
+++ b/fdts/stm32mp157c-ev1.dts
@@ -57,6 +57,7 @@
 
 &usart3 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&usart3_pins_a>;
+	pinctrl-0 = <&usart3_pins_b>;
+	uart-has-rtscts;
 	status = "disabled";
 };
diff --git a/fdts/stm32mp157c-lxa-mc1.dts b/fdts/stm32mp157c-lxa-mc1.dts
index 7b8e481..31f1382 100644
--- a/fdts/stm32mp157c-lxa-mc1.dts
+++ b/fdts/stm32mp157c-lxa-mc1.dts
@@ -75,7 +75,7 @@
 
 &sdmmc2 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&sdmmc2_b4_pins_a &mc1_sdmmc2_d47_pins_b>;
+	pinctrl-0 = <&sdmmc2_b4_pins_a &sdmmc2_d47_pins_b>;
 	bus-width = <8>;
 	no-1-8-v;
 	no-sd;
@@ -91,17 +91,3 @@
 	pinctrl-0 = <&uart4_pins_a>;
 	status = "okay";
 };
-
-&pinctrl {
-	mc1_sdmmc2_d47_pins_b: mc1-sdmmc2-d47-1 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 8, AF9)>,  /* SDMMC2_D4 */
-				 <STM32_PINMUX('A', 9, AF10)>, /* SDMMC2_D5 */
-				 <STM32_PINMUX('C', 6, AF10)>, /* SDMMC2_D6 */
-				 <STM32_PINMUX('C', 7, AF10)>; /* SDMMC2_D7 */
-			slew-rate = <1>;
-			drive-push-pull;
-			bias-disable;
-		};
-	};
-};
diff --git a/fdts/stm32mp15xx-dkx.dtsi b/fdts/stm32mp15xx-dkx.dtsi
index 52b914b..69b4828 100644
--- a/fdts/stm32mp15xx-dkx.dtsi
+++ b/fdts/stm32mp15xx-dkx.dtsi
@@ -141,7 +141,6 @@
 				regulator-name = "vdd_usb";
 				regulator-min-microvolt = <3300000>;
 				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
 			};
 
 			vdda: ldo5 {
@@ -223,7 +222,7 @@
 		CLK_CKPER_HSE
 		CLK_FMC_ACLK
 		CLK_QSPI_ACLK
-		CLK_ETH_DISABLED
+		CLK_ETH_PLL4P
 		CLK_SDMMC12_PLL4P
 		CLK_DSI_DSIPLL
 		CLK_STGEN_HSE
@@ -319,13 +318,13 @@
 
 &uart7 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&uart7_pins_b>;
+	pinctrl-0 = <&uart7_pins_c>;
 	status = "disabled";
 };
 
 &usart3 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&usart3_pins_b>;
+	pinctrl-0 = <&usart3_pins_c>;
 	uart-has-rtscts;
 	status = "disabled";
 };
diff --git a/fdts/stm32mp15xxaa-pinctrl.dtsi b/fdts/stm32mp15xxaa-pinctrl.dtsi
index 64e566b..04f7a43 100644
--- a/fdts/stm32mp15xxaa-pinctrl.dtsi
+++ b/fdts/stm32mp15xxaa-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
diff --git a/fdts/stm32mp15xxab-pinctrl.dtsi b/fdts/stm32mp15xxab-pinctrl.dtsi
index d29af89..328dad1 100644
--- a/fdts/stm32mp15xxab-pinctrl.dtsi
+++ b/fdts/stm32mp15xxab-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
diff --git a/fdts/stm32mp15xxac-pinctrl.dtsi b/fdts/stm32mp15xxac-pinctrl.dtsi
index 5d8199f..7eaa245 100644
--- a/fdts/stm32mp15xxac-pinctrl.dtsi
+++ b/fdts/stm32mp15xxac-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
diff --git a/fdts/stm32mp15xxad-pinctrl.dtsi b/fdts/stm32mp15xxad-pinctrl.dtsi
index 023f540..b63e207 100644
--- a/fdts/stm32mp15xxad-pinctrl.dtsi
+++ b/fdts/stm32mp15xxad-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
diff --git a/fdts/tc.dts b/fdts/tc.dts
index f66d556..13c9e16 100644
--- a/fdts/tc.dts
+++ b/fdts/tc.dts
@@ -79,6 +79,31 @@
 			};
 		};
 
+		amus {
+			amu: amu-0 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				mpmm_gear0: counter@0 {
+					reg = <0>;
+
+					enable-at-el3;
+				};
+
+				mpmm_gear1: counter@1 {
+					reg = <1>;
+
+					enable-at-el3;
+				};
+
+				mpmm_gear2: counter@2 {
+					reg = <2>;
+
+					enable-at-el3;
+				};
+			};
+		};
+
 		CPU0:cpu@0 {
 			device_type = "cpu";
 			compatible = "arm,armv8";
@@ -87,6 +112,8 @@
 			clocks = <&scmi_dvfs 0>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <406>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU1:cpu@100 {
@@ -97,6 +124,8 @@
 			clocks = <&scmi_dvfs 0>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <406>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU2:cpu@200 {
@@ -107,6 +136,8 @@
 			clocks = <&scmi_dvfs 0>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <406>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU3:cpu@300 {
@@ -117,6 +148,8 @@
 			clocks = <&scmi_dvfs 0>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <406>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU4:cpu@400 {
@@ -127,6 +160,8 @@
 			clocks = <&scmi_dvfs 1>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <912>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU5:cpu@500 {
@@ -137,6 +172,8 @@
 			clocks = <&scmi_dvfs 1>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <912>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU6:cpu@600 {
@@ -147,6 +184,8 @@
 			clocks = <&scmi_dvfs 1>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <912>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 		CPU7:cpu@700 {
@@ -157,15 +196,12 @@
 			clocks = <&scmi_dvfs 2>;
 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
 			capacity-dmips-mhz = <1024>;
+			amu = <&amu>;
+			supports-mpmm;
 		};
 
 	};
 
-	memory@80000000 {
-		device_type = "memory";
-		reg = <0x0 0x80000000 0x0 0x7d000000>;
-	};
-
 	reserved-memory {
 		#address-cells = <2>;
 		#size-cells = <2>;
diff --git a/include/arch/aarch32/arch.h b/include/arch/aarch32/arch.h
index 7221b62..a1bd942 100644
--- a/include/arch/aarch32/arch.h
+++ b/include/arch/aarch32/arch.h
@@ -253,7 +253,8 @@
 /* HCPTR definitions */
 #define HCPTR_RES1		((U(1) << 13) | (U(1) << 12) | U(0x3ff))
 #define TCPAC_BIT		(U(1) << 31)
-#define TAM_BIT			(U(1) << 30)
+#define TAM_SHIFT		U(30)
+#define TAM_BIT			(U(1) << TAM_SHIFT)
 #define TTA_BIT			(U(1) << 20)
 #define TCP11_BIT		(U(1) << 11)
 #define TCP10_BIT		(U(1) << 10)
@@ -727,8 +728,25 @@
 #define AMEVTYPER1E	p15, 0, c13, c15, 6
 #define AMEVTYPER1F	p15, 0, c13, c15, 7
 
+/* AMCNTENSET0 definitions */
+#define AMCNTENSET0_Pn_SHIFT	U(0)
+#define AMCNTENSET0_Pn_MASK	U(0xffff)
+
+/* AMCNTENSET1 definitions */
+#define AMCNTENSET1_Pn_SHIFT	U(0)
+#define AMCNTENSET1_Pn_MASK	U(0xffff)
+
+/* AMCNTENCLR0 definitions */
+#define AMCNTENCLR0_Pn_SHIFT	U(0)
+#define AMCNTENCLR0_Pn_MASK	U(0xffff)
+
+/* AMCNTENCLR1 definitions */
+#define AMCNTENCLR1_Pn_SHIFT	U(0)
+#define AMCNTENCLR1_Pn_MASK	U(0xffff)
+
 /* AMCR definitions */
-#define AMCR_CG1RZ_BIT		(ULL(1) << 17)
+#define AMCR_CG1RZ_SHIFT	U(17)
+#define AMCR_CG1RZ_BIT		(ULL(1) << AMCR_CG1RZ_SHIFT)
 
 /* AMCFGR definitions */
 #define AMCFGR_NCG_SHIFT	U(28)
@@ -737,6 +755,8 @@
 #define AMCFGR_N_MASK		U(0xff)
 
 /* AMCGCR definitions */
+#define AMCGCR_CG0NC_SHIFT	U(0)
+#define AMCGCR_CG0NC_MASK	U(0xff)
 #define AMCGCR_CG1NC_SHIFT	U(8)
 #define AMCGCR_CG1NC_MASK	U(0xff)
 
diff --git a/include/arch/aarch32/el3_common_macros.S b/include/arch/aarch32/el3_common_macros.S
index 65f9a8e..ad2a039 100644
--- a/include/arch/aarch32/el3_common_macros.S
+++ b/include/arch/aarch32/el3_common_macros.S
@@ -380,10 +380,21 @@
 		 * includes the data and NOBITS sections. This is done to
 		 * safeguard against possible corruption of this memory by
 		 * dirty cache lines in a system cache as a result of use by
-		 * an earlier boot loader stage.
+		 * an earlier boot loader stage. If PIE is enabled however,
+		 * RO sections including the GOT may be modified during
+		 * pie fixup. Therefore, to be on the safe side, invalidate
+		 * the entire image region if PIE is enabled.
 		 * -----------------------------------------------------------------
 		 */
+#if ENABLE_PIE
+#if SEPARATE_CODE_AND_RODATA
+		ldr	r0, =__TEXT_START__
+#else
+		ldr	r0, =__RO_START__
+#endif /* SEPARATE_CODE_AND_RODATA */
+#else
 		ldr	r0, =__RW_START__
+#endif /* ENABLE_PIE */
 		ldr	r1, =__RW_END__
 		sub	r1, r1, r0
 		bl	inv_dcache_range
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 74bc8cb..5408acf 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -532,7 +532,8 @@
 
 /* HCR definitions */
 #define HCR_RESET_VAL		ULL(0x0)
-#define HCR_AMVOFFEN_BIT	(ULL(1) << 51)
+#define HCR_AMVOFFEN_SHIFT	U(51)
+#define HCR_AMVOFFEN_BIT	(ULL(1) << HCR_AMVOFFEN_SHIFT)
 #define HCR_TEA_BIT		(ULL(1) << 47)
 #define HCR_API_BIT		(ULL(1) << 41)
 #define HCR_APK_BIT		(ULL(1) << 40)
@@ -570,7 +571,8 @@
 
 /* CPTR_EL3 definitions */
 #define TCPAC_BIT		(U(1) << 31)
-#define TAM_BIT			(U(1) << 30)
+#define TAM_SHIFT		U(30)
+#define TAM_BIT			(U(1) << TAM_SHIFT)
 #define TTA_BIT			(U(1) << 20)
 #define TFP_BIT			(U(1) << 10)
 #define CPTR_EZ_BIT		(U(1) << 8)
@@ -579,7 +581,8 @@
 /* CPTR_EL2 definitions */
 #define CPTR_EL2_RES1		((U(1) << 13) | (U(1) << 12) | (U(0x3ff)))
 #define CPTR_EL2_TCPAC_BIT	(U(1) << 31)
-#define CPTR_EL2_TAM_BIT	(U(1) << 30)
+#define CPTR_EL2_TAM_SHIFT	U(30)
+#define CPTR_EL2_TAM_BIT	(U(1) << CPTR_EL2_TAM_SHIFT)
 #define CPTR_EL2_TTA_BIT	(U(1) << 20)
 #define CPTR_EL2_TFP_BIT	(U(1) << 10)
 #define CPTR_EL2_TZ_BIT		(U(1) << 8)
@@ -1043,6 +1046,22 @@
 #define AMEVTYPER1E_EL0		S3_3_C13_C15_6
 #define AMEVTYPER1F_EL0		S3_3_C13_C15_7
 
+/* AMCNTENSET0_EL0 definitions */
+#define AMCNTENSET0_EL0_Pn_SHIFT	U(0)
+#define AMCNTENSET0_EL0_Pn_MASK		ULL(0xffff)
+
+/* AMCNTENSET1_EL0 definitions */
+#define AMCNTENSET1_EL0_Pn_SHIFT	U(0)
+#define AMCNTENSET1_EL0_Pn_MASK		ULL(0xffff)
+
+/* AMCNTENCLR0_EL0 definitions */
+#define AMCNTENCLR0_EL0_Pn_SHIFT	U(0)
+#define AMCNTENCLR0_EL0_Pn_MASK		ULL(0xffff)
+
+/* AMCNTENCLR1_EL0 definitions */
+#define AMCNTENCLR1_EL0_Pn_SHIFT	U(0)
+#define AMCNTENCLR1_EL0_Pn_MASK		ULL(0xffff)
+
 /* AMCFGR_EL0 definitions */
 #define AMCFGR_EL0_NCG_SHIFT	U(28)
 #define AMCFGR_EL0_NCG_MASK	U(0xf)
@@ -1050,6 +1069,8 @@
 #define AMCFGR_EL0_N_MASK	U(0xff)
 
 /* AMCGCR_EL0 definitions */
+#define AMCGCR_EL0_CG0NC_SHIFT	U(0)
+#define AMCGCR_EL0_CG0NC_MASK	U(0xff)
 #define AMCGCR_EL0_CG1NC_SHIFT	U(8)
 #define AMCGCR_EL0_CG1NC_MASK	U(0xff)
 
@@ -1074,7 +1095,8 @@
 #define AMCG1IDR_VOFF_SHIFT	U(16)
 
 /* New bit added to AMCR_EL0 */
-#define AMCR_CG1RZ_BIT		(ULL(0x1) << 17)
+#define AMCR_CG1RZ_SHIFT	U(17)
+#define AMCR_CG1RZ_BIT		(ULL(0x1) << AMCR_CG1RZ_SHIFT)
 
 /*
  * Definitions for virtual offset registers for architected activity monitor
@@ -1194,4 +1216,16 @@
 #define DSU_CLUSTER_PWR_ON	1
 #define DSU_CLUSTER_PWR_MASK	U(1)
 
+/*******************************************************************************
+ * Definitions for CPU Power/Performance Management registers
+ ******************************************************************************/
+
+#define CPUPPMCR_EL3			S3_6_C15_C2_0
+#define CPUPPMCR_EL3_MPMMPINCTL_SHIFT	UINT64_C(0)
+#define CPUPPMCR_EL3_MPMMPINCTL_MASK	UINT64_C(0x1)
+
+#define CPUMPMMCR_EL3			S3_6_C15_C2_1
+#define CPUMPMMCR_EL3_MPMM_EN_SHIFT	UINT64_C(0)
+#define CPUMPMMCR_EL3_MPMM_EN_MASK	UINT64_C(0x1)
+
 #endif /* ARCH_H */
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index cae05dc..37fa047 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -542,6 +542,10 @@
 /* DynamIQ Shared Unit power management */
 DEFINE_RENAME_SYSREG_RW_FUNCS(clusterpwrdn_el1, CLUSTERPWRDN_EL1)
 
+/* CPU Power/Performance Management registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(cpuppmcr_el3, CPUPPMCR_EL3)
+DEFINE_RENAME_SYSREG_RW_FUNCS(cpumpmmcr_el3, CPUMPMMCR_EL3)
+
 /* Armv9.2 RME Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(gptbr_el3, GPTBR_EL3)
 DEFINE_RENAME_SYSREG_RW_FUNCS(gpccr_el3, GPCCR_EL3)
diff --git a/include/arch/aarch64/el2_common_macros.S b/include/arch/aarch64/el2_common_macros.S
index c57a1ec..7bf4806 100644
--- a/include/arch/aarch64/el2_common_macros.S
+++ b/include/arch/aarch64/el2_common_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -82,10 +82,6 @@
 	 * Initialise MDCR_EL2, setting all fields rather than relying on
 	 * hw. Some fields are architecturally UNKNOWN on reset.
 	 *
-	 * MDCR_EL2.SDD: Set to one to disable AArch64 Secure self-hosted
-	 *  debug. Debug exceptions, other than Breakpoint Instruction
-	 *  exceptions, are disabled from all ELs in Secure state.
-	 *
 	 * MDCR_EL2.TDOSA: Set to zero so that EL2 and EL2 System register
 	 *  access to the powerdown debug registers do not trap to EL2.
 	 *
@@ -97,17 +93,7 @@
 	 *  register accesses to all Performance Monitors registers do not trap
 	 *  to EL2.
 	 *
-	 * MDCR_EL2.SCCD: Set to one so that cycle counting by PMCCNTR_EL0
-	 *  is prohibited in Secure state. This bit is RES0 in versions of the
-	 *  architecture with FEAT_PMUv3p5 not implemented, setting it to 1
-	 *  doesn't have any effect on them.
-	 *
-	 * MDCR_EL2.MCCD: Set to one so that cycle counting by PMCCNTR_EL0
-	 *  is prohibited in EL2. This bit is RES0 in versions of the
-	 *  architecture with FEAT_PMUv3p7 not implemented, setting it to 1
-	 *  doesn't have any effect on them.
-	 *
-	 * MDCR_EL2.SPME: Set to zero so that event counting by the program-
+	 * MDCR_EL2.HPMD: Set to zero so that event counting by the program-
 	 *  mable counters PMEVCNTR<n>_EL0 is prohibited in Secure state. If
 	 *  ARMv8.2 Debug is not implemented this bit does not have any effect
 	 *  on the counters unless there is support for the implementation
@@ -115,9 +101,9 @@
 	 *  ExternalSecureNoninvasiveDebugEnabled().
 	 * ---------------------------------------------------------------------
 	 */
-	mov_imm	x0, ((MDCR_EL2_RESET_VAL | MDCR_SDD_BIT | \
-		      MDCR_SPD32(MDCR_SPD32_DISABLE) | MDCR_SCCD_BIT | \
-		      MDCR_MCCD_BIT) & ~(MDCR_SPME_BIT | MDCR_TDOSA_BIT | \
+	mov_imm	x0, ((MDCR_EL2_RESET_VAL | \
+		      MDCR_SPD32(MDCR_SPD32_DISABLE)) \
+		      & ~(MDCR_EL2_HPMD | MDCR_TDOSA_BIT | \
 		      MDCR_TDA_BIT | MDCR_TPM_BIT))
 
 	msr	mdcr_el2, x0
@@ -126,17 +112,6 @@
 	 * Initialise PMCR_EL0 setting all fields rather than relying
 	 * on hw. Some fields are architecturally UNKNOWN on reset.
 	 *
-	 * PMCR_EL0.LP: Set to one so that event counter overflow, that
-	 *  is recorded in PMOVSCLR_EL0[0-30], occurs on the increment
-	 *  that changes PMEVCNTR<n>_EL0[63] from 1 to 0, when ARMv8.5-PMU
-	 *  is implemented. This bit is RES0 in versions of the architecture
-	 *  earlier than ARMv8.5, setting it to 1 doesn't have any effect
-	 *  on them.
-	 *
-	 * PMCR_EL0.LC: Set to one so that cycle counter overflow, that
-	 *  is recorded in PMOVSCLR_EL0[31], occurs on the increment
-	 *  that changes PMCCNTR_EL0[63] from 1 to 0.
-	 *
 	 * PMCR_EL0.DP: Set to one so that the cycle counter,
 	 *  PMCCNTR_EL0 does not count when event counting is prohibited.
 	 *
@@ -146,8 +121,7 @@
 	 *  counts on every clock cycle.
 	 * ---------------------------------------------------------------------
 	 */
-	mov_imm	x0, ((PMCR_EL0_RESET_VAL | PMCR_EL0_LP_BIT | \
-		      PMCR_EL0_LC_BIT | PMCR_EL0_DP_BIT) & \
+	mov_imm	x0, ((PMCR_EL0_RESET_VAL | PMCR_EL0_DP_BIT) & \
 		    ~(PMCR_EL0_X_BIT | PMCR_EL0_D_BIT))
 
 	msr	pmcr_el0, x0
diff --git a/include/arch/aarch64/el3_common_macros.S b/include/arch/aarch64/el3_common_macros.S
index 7d6a963..8e8d334 100644
--- a/include/arch/aarch64/el3_common_macros.S
+++ b/include/arch/aarch64/el3_common_macros.S
@@ -430,11 +430,24 @@
 		 * includes the data and NOBITS sections. This is done to
 		 * safeguard against possible corruption of this memory by
 		 * dirty cache lines in a system cache as a result of use by
-		 * an earlier boot loader stage.
+		 * an earlier boot loader stage. If PIE is enabled however,
+		 * RO sections including the GOT may be modified during
+                 * pie fixup. Therefore, to be on the safe side, invalidate
+		 * the entire image region if PIE is enabled.
 		 * -------------------------------------------------------------
 		 */
+#if ENABLE_PIE
+#if SEPARATE_CODE_AND_RODATA
+		adrp	x0, __TEXT_START__
+		add	x0, x0, :lo12:__TEXT_START__
+#else
+		adrp	x0, __RO_START__
+		add	x0, x0, :lo12:__RO_START__
+#endif /* SEPARATE_CODE_AND_RODATA */
+#else
 		adrp	x0, __RW_START__
 		add	x0, x0, :lo12:__RW_START__
+#endif /* ENABLE_PIE */
 		adrp	x1, __RW_END__
 		add	x1, x1, :lo12:__RW_END__
 		sub	x1, x1, x0
diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h
index 98e7a3e..9c7180c 100644
--- a/include/common/fdt_wrappers.h
+++ b/include/common/fdt_wrappers.h
@@ -41,6 +41,9 @@
 uint64_t fdtw_translate_address(const void *dtb, int bus_node,
 				uint64_t base_address);
 
+int fdtw_for_each_cpu(const void *fdt,
+		      int (*callback)(const void *dtb, int node, uintptr_t mpidr));
+
 static inline uint32_t fdt_blob_size(const void *dtb)
 {
 	const uint32_t *dtb_header = dtb;
diff --git a/include/drivers/arm/css/scmi.h b/include/drivers/arm/css/scmi.h
index adce7a6..9dd08e5 100644
--- a/include/drivers/arm/css/scmi.h
+++ b/include/drivers/arm/css/scmi.h
@@ -25,10 +25,16 @@
 #define MAKE_SCMI_VERSION(maj, min)	\
 			((((maj) & 0xffff) << 16) | ((min) & 0xffff))
 
-/* Macro to check if the driver is compatible with the SCMI version reported */
+/*
+ * Check that the driver's version is same or higher than the reported SCMI
+ * version. We accept lower major version numbers, as all affected protocols
+ * so far stay backwards compatible. This might need to be revisited in the
+ * future.
+ */
 #define is_scmi_version_compatible(drv, scmi)				\
+	((GET_SCMI_MAJOR_VER(drv) > GET_SCMI_MAJOR_VER(scmi)) ||	\
 	((GET_SCMI_MAJOR_VER(drv) == GET_SCMI_MAJOR_VER(scmi)) &&	\
-	(GET_SCMI_MINOR_VER(drv) <= GET_SCMI_MINOR_VER(scmi)))
+	(GET_SCMI_MINOR_VER(drv) <= GET_SCMI_MINOR_VER(scmi))))
 
 /* SCMI Protocol identifiers */
 #define SCMI_PWR_DMN_PROTO_ID			0x11
diff --git a/include/drivers/measured_boot/event_log.h b/include/drivers/measured_boot/event_log.h
deleted file mode 100644
index efde117..0000000
--- a/include/drivers/measured_boot/event_log.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef EVENT_LOG_H
-#define EVENT_LOG_H
-
-#include <stdint.h>
-
-#include <common/debug.h>
-#include <drivers/measured_boot/tcg.h>
-
-/*
- * Set Event Log debug level to one of:
- *
- * LOG_LEVEL_ERROR
- * LOG_LEVEL_INFO
- * LOG_LEVEL_WARNING
- * LOG_LEVEL_VERBOSE
- */
-#if EVENT_LOG_LEVEL   == LOG_LEVEL_ERROR
-#define	LOG_EVENT	ERROR
-#elif EVENT_LOG_LEVEL == LOG_LEVEL_NOTICE
-#define	LOG_EVENT	NOTICE
-#elif EVENT_LOG_LEVEL == LOG_LEVEL_WARNING
-#define	LOG_EVENT	WARN
-#elif EVENT_LOG_LEVEL == LOG_LEVEL_INFO
-#define	LOG_EVENT	INFO
-#elif EVENT_LOG_LEVEL == LOG_LEVEL_VERBOSE
-#define	LOG_EVENT	VERBOSE
-#else
-#error "Not supported EVENT_LOG_LEVEL"
-#endif
-
-/* Number of hashing algorithms supported */
-#define HASH_ALG_COUNT	1U
-
-#define	INVALID_ID	MAX_NUMBER_IDS
-
-#define MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
-
-#define BL2_STRING			"BL_2"
-#define BL31_STRING			"BL_31"
-#define BL32_STRING			"BL_32"
-#define	BL32_EXTRA1_IMAGE_STRING	"BL32_EXTRA1_IMAGE"
-#define	BL32_EXTRA2_IMAGE_STRING	"BL32_EXTRA2_IMAGE"
-#define BL33_STRING			"BL_33"
-#define GPT_IMAGE_STRING		"GPT"
-#define HW_CONFIG_STRING		"HW_CONFIG"
-#define NT_FW_CONFIG_STRING		"NT_FW_CONFIG"
-#define SCP_BL2_IMAGE_STRING		"SCP_BL2_IMAGE"
-#define SOC_FW_CONFIG_STRING		"SOC_FW_CONFIG"
-#define STM32_IMAGE_STRING		"STM32"
-#define	TOS_FW_CONFIG_STRING		"TOS_FW_CONFIG"
-
-typedef struct {
-	unsigned int id;
-	const char *name;
-	unsigned int pcr;
-} image_data_t;
-
-typedef struct {
-	const image_data_t *images_data;
-	int (*set_nt_fw_info)(uintptr_t config_base,
-#ifdef SPD_opteed
-				uintptr_t log_addr,
-#endif
-				size_t log_size, uintptr_t *ns_log_addr);
-	int (*set_tos_fw_info)(uintptr_t config_base, uintptr_t log_addr,
-				size_t log_size);
-} measured_boot_data_t;
-
-#define	ID_EVENT_SIZE	(sizeof(id_event_headers_t) + \
-			(sizeof(id_event_algorithm_size_t) * HASH_ALG_COUNT) + \
-			sizeof(id_event_struct_data_t))
-
-#define	LOC_EVENT_SIZE	(sizeof(event2_header_t) + \
-			sizeof(tpmt_ha) + TCG_DIGEST_SIZE + \
-			sizeof(event2_data_t) + \
-			sizeof(startup_locality_event_t))
-
-#define	LOG_MIN_SIZE	(ID_EVENT_SIZE + LOC_EVENT_SIZE)
-
-#define EVENT2_HDR_SIZE	(sizeof(event2_header_t) + \
-			sizeof(tpmt_ha) + TCG_DIGEST_SIZE + \
-			sizeof(event2_data_t))
-
-/* Functions' declarations */
-void event_log_init(void);
-int event_log_finalise(uint8_t **log_addr, size_t *log_size);
-void dump_event_log(uint8_t *log_addr, size_t log_size);
-const measured_boot_data_t *plat_get_measured_boot_data(void);
-int tpm_record_measurement(uintptr_t data_base, uint32_t data_size,
-			   uint32_t data_id);
-#endif /* EVENT_LOG_H */
diff --git a/include/drivers/measured_boot/event_log/event_log.h b/include/drivers/measured_boot/event_log/event_log.h
new file mode 100644
index 0000000..c6eb29c
--- /dev/null
+++ b/include/drivers/measured_boot/event_log/event_log.h
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EVENT_LOG_H
+#define EVENT_LOG_H
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <common/tbbr/tbbr_img_def.h>
+#include <drivers/measured_boot/event_log/tcg.h>
+
+/*
+ * Set Event Log debug level to one of:
+ *
+ * LOG_LEVEL_ERROR
+ * LOG_LEVEL_INFO
+ * LOG_LEVEL_WARNING
+ * LOG_LEVEL_VERBOSE
+ */
+#if EVENT_LOG_LEVEL   == LOG_LEVEL_ERROR
+#define	LOG_EVENT	ERROR
+#elif EVENT_LOG_LEVEL == LOG_LEVEL_NOTICE
+#define	LOG_EVENT	NOTICE
+#elif EVENT_LOG_LEVEL == LOG_LEVEL_WARNING
+#define	LOG_EVENT	WARN
+#elif EVENT_LOG_LEVEL == LOG_LEVEL_INFO
+#define	LOG_EVENT	INFO
+#elif EVENT_LOG_LEVEL == LOG_LEVEL_VERBOSE
+#define	LOG_EVENT	VERBOSE
+#else
+#error "Not supported EVENT_LOG_LEVEL"
+#endif
+
+/* Number of hashing algorithms supported */
+#define HASH_ALG_COUNT	1U
+
+#define	INVALID_ID	MAX_NUMBER_IDS
+
+#define MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
+
+/*
+ * Each event log entry has some metadata (i.e. a string) that identifies
+ * what is measured.These macros define these strings.
+ * Note that these strings follow the standardization recommendations
+ * defined in the Arm Server Base Security Guide (a.k.a. SBSG, Arm DEN 0086),
+ * where applicable. They should not be changed in the code.
+ * Where the SBSG does not make recommendations, we are free to choose any
+ * naming convention.
+ * The key thing is to choose meaningful strings so that when the TPM event
+ * log is used in attestation, the different components can be identified.
+ */
+#define EVLOG_BL2_STRING		"BL_2"
+#define EVLOG_BL31_STRING		"SECURE_RT_EL3"
+#if defined(SPD_opteed)
+#define EVLOG_BL32_STRING		"SECURE_RT_EL1_OPTEE"
+#elif defined(SPD_tspd)
+#define EVLOG_BL32_STRING		"SECURE_RT_EL1_TSPD"
+#elif defined(SPD_tlkd)
+#define EVLOG_BL32_STRING		"SECURE_RT_EL1_TLKD"
+#elif defined(SPD_trusty)
+#define EVLOG_BL32_STRING		"SECURE_RT_EL1_TRUSTY"
+#else
+#define EVLOG_BL32_STRING		"SECURE_RT_EL1_UNKNOWN"
+#endif
+#define	EVLOG_BL32_EXTRA1_STRING	"SECURE_RT_EL1_OPTEE_EXTRA1"
+#define	EVLOG_BL32_EXTRA2_STRING	"SECURE_RT_EL1_OPTEE_EXTRA2"
+#define EVLOG_BL33_STRING		"BL_33"
+#define EVLOG_FW_CONFIG_STRING		"FW_CONFIG"
+#define EVLOG_HW_CONFIG_STRING		"HW_CONFIG"
+#define EVLOG_NT_FW_CONFIG_STRING	"NT_FW_CONFIG"
+#define EVLOG_SCP_BL2_STRING		"SYS_CTRL_2"
+#define EVLOG_SOC_FW_CONFIG_STRING	"SOC_FW_CONFIG"
+#define EVLOG_STM32_STRING		"STM32"
+#define EVLOG_TB_FW_CONFIG_STRING	"TB_FW_CONFIG"
+#define	EVLOG_TOS_FW_CONFIG_STRING	"TOS_FW_CONFIG"
+
+typedef struct {
+	unsigned int id;
+	const char *name;
+	unsigned int pcr;
+} event_log_metadata_t;
+
+#define	ID_EVENT_SIZE	(sizeof(id_event_headers_t) + \
+			(sizeof(id_event_algorithm_size_t) * HASH_ALG_COUNT) + \
+			sizeof(id_event_struct_data_t))
+
+#define	LOC_EVENT_SIZE	(sizeof(event2_header_t) + \
+			sizeof(tpmt_ha) + TCG_DIGEST_SIZE + \
+			sizeof(event2_data_t) + \
+			sizeof(startup_locality_event_t))
+
+#define	LOG_MIN_SIZE	(ID_EVENT_SIZE + LOC_EVENT_SIZE)
+
+#define EVENT2_HDR_SIZE	(sizeof(event2_header_t) + \
+			sizeof(tpmt_ha) + TCG_DIGEST_SIZE + \
+			sizeof(event2_data_t))
+
+/* Functions' declarations */
+void event_log_init(uint8_t *event_log_start, uint8_t *event_log_finish);
+void event_log_write_header(void);
+void dump_event_log(uint8_t *log_addr, size_t log_size);
+const event_log_metadata_t *plat_event_log_get_metadata(void);
+int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
+				 uint32_t data_id);
+size_t event_log_get_cur_size(uint8_t *event_log_start);
+
+#endif /* EVENT_LOG_H */
diff --git a/include/drivers/measured_boot/tcg.h b/include/drivers/measured_boot/event_log/tcg.h
similarity index 100%
rename from include/drivers/measured_boot/tcg.h
rename to include/drivers/measured_boot/event_log/tcg.h
diff --git a/include/drivers/measured_boot/measured_boot.h b/include/drivers/measured_boot/measured_boot.h
deleted file mode 100644
index 05be4a9..0000000
--- a/include/drivers/measured_boot/measured_boot.h
+++ /dev/null
@@ -1,18 +0,0 @@
-/*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef MEASURED_BOOT_H
-#define MEASURED_BOOT_H
-
-#include <stdint.h>
-
-#include <drivers/measured_boot/event_log.h>
-
-/* Functions' declarations */
-void measured_boot_init(void);
-void measured_boot_finish(void);
-
-#endif /* MEASURED_BOOT_H */
diff --git a/include/drivers/nxp/dcfg/scfg.h b/include/drivers/nxp/dcfg/scfg.h
index b6e3df5..ef6ed6b 100644
--- a/include/drivers/nxp/dcfg/scfg.h
+++ b/include/drivers/nxp/dcfg/scfg.h
@@ -44,7 +44,7 @@
 #define scfg_clrbits32(a, v)	mmio_clrbits_32((uintptr_t)(a), v)
 #define scfg_clrsetbits32(a, clear, set)	\
 				mmio_clrsetbits_32((uintptr_t)(a), clear, set)
-#elif defined(NXP_GUR_LE)
+#elif defined(NXP_SCFG_LE)
 #define scfg_in32(a)		mmio_read_32((uintptr_t)(a))
 #define scfg_out32(a, v)	mmio_write_32((uintptr_t)(a), v)
 #define scfg_setbits32(a, v)	mmio_setbits_32((uintptr_t)(a), v)
diff --git a/include/drivers/st/stm32mp1_usb.h b/include/drivers/st/stm32mp1_usb.h
new file mode 100644
index 0000000..06a34cb
--- /dev/null
+++ b/include/drivers/st/stm32mp1_usb.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP1_USB_H
+#define STM32MP1_USB_H
+
+#include <drivers/usb_device.h>
+
+void stm32mp1_usb_init_driver(struct usb_handle *usb_core_handle,
+			      struct pcd_handle *pcd_handle,
+			      void *base_register);
+
+#endif /* STM32MP1_USB_H */
diff --git a/include/drivers/st/stpmic1.h b/include/drivers/st/stpmic1.h
index f7e293b..dc096cd 100644
--- a/include/drivers/st/stpmic1.h
+++ b/include/drivers/st/stpmic1.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -86,15 +86,15 @@
 #define ITSOURCE4_REG			0xB3U
 
 /* Registers masks */
-#define LDO_VOLTAGE_MASK		0x7CU
-#define BUCK_VOLTAGE_MASK		0xFCU
+#define LDO_VOLTAGE_MASK		GENMASK(6, 2)
+#define BUCK_VOLTAGE_MASK		GENMASK(7, 2)
 #define LDO_BUCK_VOLTAGE_SHIFT		2
-#define LDO_BUCK_ENABLE_MASK		0x01U
-#define LDO_BUCK_HPLP_ENABLE_MASK	0x02U
+#define LDO_BUCK_ENABLE_MASK		BIT(0)
+#define LDO_BUCK_HPLP_ENABLE_MASK	BIT(1)
 #define LDO_BUCK_HPLP_SHIFT		1
-#define LDO_BUCK_RANK_MASK		0x01U
-#define LDO_BUCK_RESET_MASK		0x01U
-#define LDO_BUCK_PULL_DOWN_MASK		0x03U
+#define LDO_BUCK_RANK_MASK		BIT(0)
+#define LDO_BUCK_RESET_MASK		BIT(0)
+#define LDO_BUCK_PULL_DOWN_MASK		GENMASK(1, 0)
 
 /* Pull down register */
 #define BUCK1_PULL_DOWN_SHIFT		0
@@ -135,12 +135,12 @@
 /* Main PMIC VINLOW Control Register (VIN_CONTROL_REGC DMSC) */
 #define SWIN_DETECTOR_ENABLED		BIT(7)
 #define SWOUT_DETECTOR_ENABLED          BIT(6)
-#define VINLOW_HYST_MASK		0x3
+#define VINLOW_HYST_MASK		GENMASK(1, 0)
 #define VINLOW_HYST_SHIFT		4
-#define VINLOW_THRESHOLD_MASK		0x7
+#define VINLOW_THRESHOLD_MASK		GENMASK(2, 0)
 #define VINLOW_THRESHOLD_SHIFT		1
-#define VINLOW_ENABLED			0x01
-#define VINLOW_CTRL_REG_MASK		0xFF
+#define VINLOW_ENABLED			BIT(0)
+#define VINLOW_CTRL_REG_MASK		GENMASK(7, 0)
 
 /* USB Control Register */
 #define BOOST_OVP_DISABLED		BIT(7)
diff --git a/include/drivers/ufs.h b/include/drivers/ufs.h
index 574c4ea..c074e85 100644
--- a/include/drivers/ufs.h
+++ b/include/drivers/ufs.h
@@ -254,6 +254,17 @@
 #define UFS_VENDOR_SKHYNIX		U(0x1AD)
 
 #define MAX_MODEL_LEN 16
+
+/* maximum number of retries for a general UIC command  */
+#define UFS_UIC_COMMAND_RETRIES		3
+
+/* maximum number of link-startup retries */
+#define DME_LINKSTARTUP_RETRIES		10
+
+#define HCE_ENABLE_OUTER_RETRIES	3
+#define HCE_ENABLE_INNER_RETRIES	50
+#define HCE_ENABLE_TIMEOUT_US		100
+
 /**
  * ufs_dev_desc - ufs device details from the device descriptor
  * @wmanufacturerid: card details
diff --git a/include/drivers/usb_device.h b/include/drivers/usb_device.h
new file mode 100644
index 0000000..e21e315
--- /dev/null
+++ b/include/drivers/usb_device.h
@@ -0,0 +1,276 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef USB_DEVICE_H
+#define USB_DEVICE_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define USBD_MAX_NUM_INTERFACES			1U
+#define USBD_MAX_NUM_CONFIGURATION		1U
+
+#define USB_LEN_DEV_QUALIFIER_DESC		0x0AU
+#define USB_LEN_DEV_DESC			0x12U
+#define USB_LEN_CFG_DESC			0x09U
+#define USB_LEN_IF_DESC				0x09U
+#define USB_LEN_EP_DESC				0x07U
+#define USB_LEN_OTG_DESC			0x03U
+#define USB_LEN_LANGID_STR_DESC			0x04U
+#define USB_LEN_OTHER_SPEED_DESC_SIZ		0x09U
+
+#define USBD_IDX_LANGID_STR			0x00U
+#define USBD_IDX_MFC_STR			0x01U
+#define USBD_IDX_PRODUCT_STR			0x02U
+#define USBD_IDX_SERIAL_STR			0x03U
+#define USBD_IDX_CONFIG_STR			0x04U
+#define USBD_IDX_INTERFACE_STR			0x05U
+#define USBD_IDX_USER0_STR			0x06U
+
+#define USB_REQ_TYPE_STANDARD			0x00U
+#define USB_REQ_TYPE_CLASS			0x20U
+#define USB_REQ_TYPE_VENDOR			0x40U
+#define USB_REQ_TYPE_MASK			0x60U
+
+#define USB_REQ_RECIPIENT_DEVICE		0x00U
+#define USB_REQ_RECIPIENT_INTERFACE		0x01U
+#define USB_REQ_RECIPIENT_ENDPOINT		0x02U
+#define USB_REQ_RECIPIENT_MASK			0x1FU
+
+#define USB_REQ_DIRECTION			0x80U
+
+#define USB_REQ_GET_STATUS			0x00U
+#define USB_REQ_CLEAR_FEATURE			0x01U
+#define USB_REQ_SET_FEATURE			0x03U
+#define USB_REQ_SET_ADDRESS			0x05U
+#define USB_REQ_GET_DESCRIPTOR			0x06U
+#define USB_REQ_SET_DESCRIPTOR			0x07U
+#define USB_REQ_GET_CONFIGURATION		0x08U
+#define USB_REQ_SET_CONFIGURATION		0x09U
+#define USB_REQ_GET_INTERFACE			0x0AU
+#define USB_REQ_SET_INTERFACE			0x0BU
+#define USB_REQ_SYNCH_FRAME			0x0CU
+
+#define USB_DESC_TYPE_DEVICE			0x01U
+#define USB_DESC_TYPE_CONFIGURATION		0x02U
+#define USB_DESC_TYPE_STRING			0x03U
+#define USB_DESC_TYPE_INTERFACE			0x04U
+#define USB_DESC_TYPE_ENDPOINT			0x05U
+#define USB_DESC_TYPE_DEVICE_QUALIFIER		0x06U
+#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION	0x07U
+#define USB_DESC_TYPE_BOS			0x0FU
+
+#define USB_CONFIG_REMOTE_WAKEUP		2U
+#define USB_CONFIG_SELF_POWERED			1U
+
+#define USB_MAX_EP0_SIZE			64U
+
+/* Device Status */
+#define USBD_STATE_DEFAULT			1U
+#define USBD_STATE_ADDRESSED			2U
+#define USBD_STATE_CONFIGURED			3U
+#define USBD_STATE_SUSPENDED			4U
+
+/* EP0 State */
+#define USBD_EP0_IDLE				0U
+#define USBD_EP0_SETUP				1U
+#define USBD_EP0_DATA_IN			2U
+#define USBD_EP0_DATA_OUT			3U
+#define USBD_EP0_STATUS_IN			4U
+#define USBD_EP0_STATUS_OUT			5U
+#define USBD_EP0_STALL				6U
+
+#define USBD_EP_TYPE_CTRL			0U
+#define USBD_EP_TYPE_ISOC			1U
+#define USBD_EP_TYPE_BULK			2U
+#define USBD_EP_TYPE_INTR			3U
+
+#define  USBD_OUT_EPNUM_MASK			GENMASK(15, 0)
+#define  USBD_OUT_COUNT_MASK			GENMASK(31, 16)
+#define  USBD_OUT_COUNT_SHIFT			16U
+
+/* Number of EP supported, allow to reduce footprint: default max = 15 */
+#ifndef CONFIG_USBD_EP_NB
+#define  USBD_EP_NB				15U
+#else
+#define  USBD_EP_NB				CONFIG_USBD_EP_NB
+#endif
+
+#define LOBYTE(x)	((uint8_t)((x) & 0x00FF))
+#define HIBYTE(x)	((uint8_t)(((x) & 0xFF00) >> 8))
+
+struct usb_setup_req {
+	uint8_t bm_request;
+	uint8_t b_request;
+	uint16_t value;
+	uint16_t index;
+	uint16_t length;
+};
+
+struct usb_handle;
+
+struct usb_class {
+	uint8_t (*init)(struct usb_handle *pdev, uint8_t cfgidx);
+	uint8_t (*de_init)(struct usb_handle *pdev, uint8_t cfgidx);
+	/* Control Endpoints */
+	uint8_t (*setup)(struct usb_handle *pdev, struct usb_setup_req *req);
+	uint8_t (*ep0_tx_sent)(struct usb_handle *pdev);
+	uint8_t (*ep0_rx_ready)(struct usb_handle *pdev);
+	/* Class Specific Endpoints */
+	uint8_t (*data_in)(struct usb_handle *pdev, uint8_t epnum);
+	uint8_t (*data_out)(struct usb_handle *pdev, uint8_t epnum);
+	uint8_t (*sof)(struct usb_handle *pdev);
+	uint8_t (*iso_in_incomplete)(struct usb_handle *pdev, uint8_t epnum);
+	uint8_t (*iso_out_incomplete)(struct usb_handle *pdev, uint8_t epnum);
+};
+
+/* Following USB Device status */
+enum usb_status {
+	USBD_OK = 0U,
+	USBD_BUSY,
+	USBD_FAIL,
+	USBD_TIMEOUT
+};
+
+/* Action to do after IT handling */
+enum usb_action {
+	USB_NOTHING = 0U,
+	USB_DATA_OUT,
+	USB_DATA_IN,
+	USB_SETUP,
+	USB_ENUM_DONE,
+	USB_READ_DATA_PACKET,
+	USB_READ_SETUP_PACKET,
+	USB_RESET,
+	USB_RESUME,
+	USB_SUSPEND,
+	USB_LPM,
+	USB_SOF,
+	USB_DISCONNECT,
+	USB_WRITE_EMPTY
+};
+
+/* USB Device descriptors structure */
+struct usb_desc {
+	uint8_t *(*get_device_desc)(uint16_t *length);
+	uint8_t *(*get_lang_id_desc)(uint16_t *length);
+	uint8_t *(*get_manufacturer_desc)(uint16_t *length);
+	uint8_t *(*get_product_desc)(uint16_t *length);
+	uint8_t *(*get_serial_desc)(uint16_t *length);
+	uint8_t *(*get_configuration_desc)(uint16_t *length);
+	uint8_t *(*get_interface_desc)(uint16_t *length);
+	uint8_t *(*get_usr_desc)(uint8_t index, uint16_t *length);
+	uint8_t *(*get_config_desc)(uint16_t *length);
+	uint8_t *(*get_device_qualifier_desc)(uint16_t *length);
+};
+
+/* USB Device handle structure */
+struct usb_endpoint {
+	uint32_t status;
+	uint32_t total_length;
+	uint32_t rem_length;
+	uint32_t maxpacket;
+};
+
+/*
+ * EndPoint descriptor
+ * num : Endpoint number, between 0 and 15 (limited by USBD_EP_NB)
+ * is_in: Endpoint direction
+ * type : Endpoint type
+ * maxpacket:  Endpoint Max packet size: between 0 and 64KB
+ * xfer_buff: Pointer to transfer buffer
+ * xfer_len: Current transfer lengt
+ * hxfer_count: Partial transfer length in case of multi packet transfer
+ */
+struct usbd_ep {
+	uint8_t num;
+	bool is_in;
+	uint8_t type;
+	uint32_t maxpacket;
+	uint8_t *xfer_buff;
+	uint32_t xfer_len;
+	uint32_t xfer_count;
+};
+
+enum pcd_lpm_state {
+	LPM_L0 = 0x00U, /* on */
+	LPM_L1 = 0x01U, /* LPM L1 sleep */
+	LPM_L2 = 0x02U, /* suspend */
+	LPM_L3 = 0x03U, /* off */
+};
+
+/* USB Device descriptors structure */
+struct usb_driver {
+	enum usb_status (*ep0_out_start)(void *handle);
+	enum usb_status (*ep_start_xfer)(void *handle, struct usbd_ep *ep);
+	enum usb_status (*ep0_start_xfer)(void *handle, struct usbd_ep *ep);
+	enum usb_status (*write_packet)(void *handle, uint8_t *src,
+				     uint8_t ch_ep_num, uint16_t len);
+	void *(*read_packet)(void *handle, uint8_t *dest, uint16_t len);
+	enum usb_status (*ep_set_stall)(void *handle, struct usbd_ep *ep);
+	enum usb_status (*start_device)(void *handle);
+	enum usb_status (*stop_device)(void *handle);
+	enum usb_status (*set_address)(void *handle, uint8_t address);
+	enum usb_status (*write_empty_tx_fifo)(void *handle,
+					    uint32_t epnum, uint32_t xfer_len,
+					    uint32_t *xfer_count,
+					    uint32_t maxpacket,
+					    uint8_t **xfer_buff);
+	enum usb_action (*it_handler)(void *handle, uint32_t *param);
+};
+
+/* USB Peripheral Controller Drivers */
+struct pcd_handle {
+	void *instance; /* Register base address */
+	struct usbd_ep in_ep[USBD_EP_NB]; /* IN endpoint parameters */
+	struct usbd_ep out_ep[USBD_EP_NB]; /* OUT endpoint parameters */
+	uint32_t setup[12]; /* Setup packet buffer */
+	enum pcd_lpm_state lpm_state; /* LPM State */
+};
+
+/* USB Device handle structure */
+struct usb_handle {
+	uint8_t id;
+	uint32_t dev_config;
+	uint32_t dev_config_status;
+	struct usb_endpoint ep_in[USBD_EP_NB];
+	struct usb_endpoint ep_out[USBD_EP_NB];
+	uint32_t ep0_state;
+	uint32_t ep0_data_len;
+	uint8_t dev_state;
+	uint8_t dev_old_state;
+	uint8_t dev_address;
+	uint32_t dev_remote_wakeup;
+	struct usb_setup_req request;
+	const struct usb_desc *desc;
+	struct usb_class *class;
+	void *class_data;
+	void *user_data;
+	struct pcd_handle *data;
+	const struct usb_driver *driver;
+};
+
+enum usb_status usb_core_handle_it(struct usb_handle *pdev);
+enum usb_status usb_core_receive(struct usb_handle *pdev, uint8_t ep_addr,
+				 uint8_t *p_buf, uint32_t len);
+enum usb_status usb_core_transmit(struct usb_handle *pdev, uint8_t ep_addr,
+				  uint8_t *p_buf, uint32_t len);
+enum usb_status usb_core_receive_ep0(struct usb_handle *pdev, uint8_t *p_buf,
+				     uint32_t len);
+enum usb_status usb_core_transmit_ep0(struct usb_handle *pdev, uint8_t *p_buf,
+				      uint32_t len);
+void usb_core_ctl_error(struct usb_handle *pdev);
+enum usb_status usb_core_start(struct usb_handle *pdev);
+enum usb_status usb_core_stop(struct usb_handle *pdev);
+enum usb_status register_usb_driver(struct usb_handle *pdev,
+				    struct pcd_handle *pcd_handle,
+				    const struct usb_driver *driver,
+				    void *driver_handle);
+enum usb_status register_platform(struct usb_handle *pdev,
+				  const struct usb_desc *plat_call_back);
+
+#endif /* USB_DEVICE_H */
diff --git a/include/lib/cpus/aarch64/cortex_demeter.h b/include/lib/cpus/aarch64/cortex_hunter.h
similarity index 65%
rename from include/lib/cpus/aarch64/cortex_demeter.h
rename to include/lib/cpus/aarch64/cortex_hunter.h
index 9dd0987..8b59fd9 100644
--- a/include/lib/cpus/aarch64/cortex_demeter.h
+++ b/include/lib/cpus/aarch64/cortex_hunter.h
@@ -4,20 +4,20 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef CORTEX_DEMETER_H
-#define CORTEX_DEMETER_H
+#ifndef CORTEX_HUNTER_H
+#define CORTEX_HUNTER_H
 
-#define CORTEX_DEMETER_MIDR				U(0x410FD4F0)
+#define CORTEX_HUNTER_MIDR					U(0x410FD810)
 
 /*******************************************************************************
  * CPU Extended Control register specific definitions
  ******************************************************************************/
-#define CORTEX_DEMETER_CPUECTLR_EL1			S3_0_C15_C1_4
+#define CORTEX_HUNTER_CPUECTLR_EL1				S3_0_C15_C1_4
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
  ******************************************************************************/
-#define CORTEX_DEMETER_CPUPWRCTLR_EL1			S3_0_C15_C2_7
-#define CORTEX_DEMETER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT	U(1)
+#define CORTEX_HUNTER_CPUPWRCTLR_EL1				S3_0_C15_C2_7
+#define CORTEX_HUNTER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT		U(1)
 
-#endif /* CORTEX_DEMETER_H */
+#endif /* CORTEX_HUNTER_H */
diff --git a/include/lib/cpus/aarch64/cortex_demeter.h b/include/lib/cpus/aarch64/neoverse_demeter.h
similarity index 64%
copy from include/lib/cpus/aarch64/cortex_demeter.h
copy to include/lib/cpus/aarch64/neoverse_demeter.h
index 9dd0987..230ed66 100644
--- a/include/lib/cpus/aarch64/cortex_demeter.h
+++ b/include/lib/cpus/aarch64/neoverse_demeter.h
@@ -4,20 +4,20 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef CORTEX_DEMETER_H
-#define CORTEX_DEMETER_H
+#ifndef NEOVERSE_DEMETER_H
+#define NEOVERSE_DEMETER_H
 
-#define CORTEX_DEMETER_MIDR				U(0x410FD4F0)
+#define NEOVERSE_DEMETER_MIDR				U(0x410FD4F0)
 
 /*******************************************************************************
  * CPU Extended Control register specific definitions
  ******************************************************************************/
-#define CORTEX_DEMETER_CPUECTLR_EL1			S3_0_C15_C1_4
+#define NEOVERSE_DEMETER_CPUECTLR_EL1			S3_0_C15_C1_4
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
  ******************************************************************************/
-#define CORTEX_DEMETER_CPUPWRCTLR_EL1			S3_0_C15_C2_7
-#define CORTEX_DEMETER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT	U(1)
+#define NEOVERSE_DEMETER_CPUPWRCTLR_EL1			S3_0_C15_C2_7
+#define NEOVERSE_DEMETER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT	U(1)
 
-#endif /* CORTEX_DEMETER_H */
+#endif /* NEOVERSE_DEMETER_H */
diff --git a/include/lib/extensions/amu.h b/include/lib/extensions/amu.h
index 3a254c9..6452f7e 100644
--- a/include/lib/extensions/amu.h
+++ b/include/lib/extensions/amu.h
@@ -10,105 +10,38 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <lib/cassert.h>
-#include <lib/utils_def.h>
-
 #include <context.h>
+
 #include <platform_def.h>
 
-/* All group 0 counters */
-#define AMU_GROUP0_COUNTERS_MASK	U(0xf)
-#define AMU_GROUP0_NR_COUNTERS		U(4)
-
-#ifdef PLAT_AMU_GROUP1_COUNTERS_MASK
-#define AMU_GROUP1_COUNTERS_MASK	PLAT_AMU_GROUP1_COUNTERS_MASK
-#else
-#define AMU_GROUP1_COUNTERS_MASK	U(0)
-#endif
-
-/* Calculate number of group 1 counters */
-#if (AMU_GROUP1_COUNTERS_MASK	& (1 << 15))
-#define	AMU_GROUP1_NR_COUNTERS		16U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 14))
-#define	AMU_GROUP1_NR_COUNTERS		15U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 13))
-#define	AMU_GROUP1_NR_COUNTERS		14U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 12))
-#define	AMU_GROUP1_NR_COUNTERS		13U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 11))
-#define	AMU_GROUP1_NR_COUNTERS		12U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 10))
-#define	AMU_GROUP1_NR_COUNTERS		11U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 9))
-#define	AMU_GROUP1_NR_COUNTERS		10U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 8))
-#define	AMU_GROUP1_NR_COUNTERS		9U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 7))
-#define	AMU_GROUP1_NR_COUNTERS		8U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 6))
-#define	AMU_GROUP1_NR_COUNTERS		7U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 5))
-#define	AMU_GROUP1_NR_COUNTERS		6U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 4))
-#define	AMU_GROUP1_NR_COUNTERS		5U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 3))
-#define	AMU_GROUP1_NR_COUNTERS		4U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 2))
-#define	AMU_GROUP1_NR_COUNTERS		3U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 1))
-#define	AMU_GROUP1_NR_COUNTERS		2U
-#elif (AMU_GROUP1_COUNTERS_MASK	& (1 << 0))
-#define	AMU_GROUP1_NR_COUNTERS		1U
-#else
-#define	AMU_GROUP1_NR_COUNTERS		0U
-#endif
-
-CASSERT(AMU_GROUP1_COUNTERS_MASK <= 0xffff, invalid_amu_group1_counters_mask);
-
-struct amu_ctx {
-	uint64_t group0_cnts[AMU_GROUP0_NR_COUNTERS];
-#if __aarch64__
-	/* Architected event counter 1 does not have an offset register. */
-	uint64_t group0_voffsets[AMU_GROUP0_NR_COUNTERS-1];
-#endif
-
-#if AMU_GROUP1_NR_COUNTERS
-	uint64_t group1_cnts[AMU_GROUP1_NR_COUNTERS];
-#if __aarch64__
-	uint64_t group1_voffsets[AMU_GROUP1_NR_COUNTERS];
-#endif
-#endif
-};
-
-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);
-void amu_group0_cnt_write(unsigned int idx, uint64_t val);
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+/*
+ * AMU data for a single core.
+ */
+struct amu_core {
+	uint16_t enable; /* Mask of auxiliary counters to enable */
+};
 
-#if __aarch64__
-uint64_t amu_group0_voffset_read(unsigned int idx);
-void amu_group0_voffset_write(unsigned int idx, uint64_t val);
-#endif
+/*
+ * Topological platform data specific to the AMU.
+ */
+struct amu_topology {
+	struct amu_core cores[PLATFORM_CORE_COUNT]; /* Per-core data */
+};
 
-#if AMU_GROUP1_NR_COUNTERS
-bool amu_group1_supported(void);
-
-/* Group 1 configuration helpers */
-uint64_t amu_group1_cnt_read(unsigned int idx);
-void amu_group1_cnt_write(unsigned int idx, uint64_t val);
-void amu_group1_set_evtype(unsigned int idx, unsigned int val);
-
-#if __aarch64__
-uint64_t amu_group1_voffset_read(unsigned int idx);
-void amu_group1_voffset_write(unsigned int idx, uint64_t val);
-#endif
-
-#endif
+#if !ENABLE_AMU_FCONF
+/*
+ * Retrieve the platform's AMU topology. A `NULL` return value is treated as a
+ * non-fatal error, in which case no auxiliary counters will be enabled.
+ */
+const struct amu_topology *plat_amu_topology(void);
+#endif /* ENABLE_AMU_FCONF */
+#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
 
 #endif /* AMU_H */
diff --git a/include/lib/fconf/fconf_amu_getter.h b/include/lib/fconf/fconf_amu_getter.h
new file mode 100644
index 0000000..2faee73
--- /dev/null
+++ b/include/lib/fconf/fconf_amu_getter.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FCONF_AMU_GETTER_H
+#define FCONF_AMU_GETTER_H
+
+#include <lib/extensions/amu.h>
+
+#define amu__config_getter(id)	fconf_amu_config.id
+
+struct fconf_amu_config {
+	const struct amu_topology *topology;
+};
+
+extern struct fconf_amu_config fconf_amu_config;
+
+#endif /* FCONF_AMU_GETTER_H */
diff --git a/include/lib/fconf/fconf_mpmm_getter.h b/include/lib/fconf/fconf_mpmm_getter.h
new file mode 100644
index 0000000..50d991a
--- /dev/null
+++ b/include/lib/fconf/fconf_mpmm_getter.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FCONF_MPMM_GETTER_H
+#define FCONF_MPMM_GETTER_H
+
+#include <lib/mpmm/mpmm.h>
+
+#define mpmm__config_getter(id)	fconf_mpmm_config.id
+
+struct fconf_mpmm_config {
+	const struct mpmm_topology *topology;
+};
+
+extern struct fconf_mpmm_config fconf_mpmm_config;
+
+#endif /* FCONF_MPMM_GETTER_H */
diff --git a/include/lib/fconf/fconf_tbbr_getter.h b/include/lib/fconf/fconf_tbbr_getter.h
index 6066af6..db98b68 100644
--- a/include/lib/fconf/fconf_tbbr_getter.h
+++ b/include/lib/fconf/fconf_tbbr_getter.h
@@ -23,9 +23,6 @@
 	uint32_t disable_auth;
 	void *mbedtls_heap_addr;
 	size_t mbedtls_heap_size;
-#if MEASURED_BOOT
-	uint8_t bl2_hash_data[TCG_DIGEST_SIZE];
-#endif
 };
 
 extern struct tbbr_dyn_config_t tbbr_dyn_config;
diff --git a/include/lib/mpmm/mpmm.h b/include/lib/mpmm/mpmm.h
new file mode 100644
index 0000000..955c530
--- /dev/null
+++ b/include/lib/mpmm/mpmm.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MPMM_H
+#define MPMM_H
+
+#include <stdbool.h>
+
+#include <platform_def.h>
+
+/*
+ * Enable the Maximum Power Mitigation Mechanism.
+ *
+ * This function will enable MPMM for the current core. The AMU counters
+ * representing the MPMM gears must have been configured and enabled prior to
+ * calling this function.
+ */
+void mpmm_enable(void);
+
+/*
+ * MPMM core data.
+ *
+ * This structure represents per-core data retrieved from the hardware
+ * configuration device tree.
+ */
+struct mpmm_core {
+	/*
+	 * Whether MPMM is supported.
+	 *
+	 * Cores with support for MPMM offer one or more auxiliary AMU counters
+	 * representing MPMM gears.
+	 */
+	bool supported;
+};
+
+/*
+ * MPMM topology.
+ *
+ * This topology structure describes the system-wide representation of the
+ * information retrieved from the hardware configuration device tree.
+ */
+struct mpmm_topology {
+	struct mpmm_core cores[PLATFORM_CORE_COUNT]; /* Per-core data */
+};
+
+#if !ENABLE_MPMM_FCONF
+/*
+ * Retrieve the platform's MPMM topology. A `NULL` return value is treated as a
+ * non-fatal error, in which case MPMM will not be enabled for any core.
+ */
+const struct mpmm_topology *plat_mpmm_topology(void);
+#endif /* ENABLE_MPMM_FCONF */
+
+#endif /* MPMM_H */
diff --git a/include/plat/arm/common/arm_dyn_cfg_helpers.h b/include/plat/arm/common/arm_dyn_cfg_helpers.h
index 34bf07c..ff00fe7 100644
--- a/include/plat/arm/common/arm_dyn_cfg_helpers.h
+++ b/include/plat/arm/common/arm_dyn_cfg_helpers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,8 +14,4 @@
 int arm_set_dtb_mbedtls_heap_info(void *dtb, void *heap_addr,
 	size_t heap_size);
 
-#if MEASURED_BOOT
-int arm_set_bl2_hash_info(void *dtb, void *data);
-#endif
-
 #endif /* ARM_DYN_CFG_HELPERS_H */
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 1500ed3..9618700 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -250,12 +250,8 @@
 int arm_get_mbedtls_heap(void **heap_addr, size_t *heap_size);
 
 #if MEASURED_BOOT
-/* Measured boot related functions */
-void arm_bl1_set_bl2_hash(const image_desc_t *image_desc);
-void arm_bl2_get_hash(void *data);
-int arm_set_tos_fw_info(uintptr_t config_base, uintptr_t log_addr,
-			size_t log_size);
-int arm_set_nt_fw_info(uintptr_t config_base,
+int arm_set_tos_fw_info(uintptr_t log_addr, size_t log_size);
+int arm_set_nt_fw_info(
 /*
  * Currently OP-TEE does not support reading DTBs from Secure memory
  * and this option should be removed when feature is supported.
@@ -264,6 +260,8 @@
 			uintptr_t log_addr,
 #endif
 			size_t log_size, uintptr_t *ns_log_addr);
+int arm_set_tb_fw_info(uintptr_t log_addr, size_t log_size);
+int arm_get_tb_fw_info(uint64_t *log_addr, size_t *log_size);
 #endif /* MEASURED_BOOT */
 
 /*
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 434835e..3fa63f5 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -122,6 +122,16 @@
 void bl2_plat_preload_setup(void);
 int plat_try_next_boot_source(void);
 
+#if MEASURED_BOOT
+int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data);
+#else
+static inline int plat_mboot_measure_image(unsigned int image_id __unused,
+					   image_info_t *image_data __unused)
+{
+	return 0;
+}
+#endif /* MEASURED_BOOT */
+
 /*******************************************************************************
  * Mandatory BL1 functions
  ******************************************************************************/
@@ -182,12 +192,16 @@
 int bl1_plat_handle_post_image_load(unsigned int image_id);
 
 #if MEASURED_BOOT
-/*
- * Calculates and writes BL2 hash data to the platform's defined location.
- * For ARM platforms the data are written to TB_FW_CONFIG DTB.
- */
-void bl1_plat_set_bl2_hash(const image_desc_t *image_desc);
-#endif
+void bl1_plat_mboot_init(void);
+void bl1_plat_mboot_finish(void);
+#else
+static inline void bl1_plat_mboot_init(void)
+{
+}
+static inline void bl1_plat_mboot_finish(void)
+{
+}
+#endif /* MEASURED_BOOT */
 
 /*******************************************************************************
  * Mandatory BL2 functions
@@ -208,9 +222,16 @@
  * Optional BL2 functions (may be overridden)
  ******************************************************************************/
 #if MEASURED_BOOT
-/* Read TCG_DIGEST_SIZE bytes of BL2 hash data */
-void bl2_plat_get_hash(void *data);
-#endif
+void bl2_plat_mboot_init(void);
+void bl2_plat_mboot_finish(void);
+#else
+static inline void bl2_plat_mboot_init(void)
+{
+}
+static inline void bl2_plat_mboot_finish(void)
+{
+}
+#endif /* MEASURED_BOOT */
 
 /*******************************************************************************
  * Mandatory BL2 at EL3 functions: Must be implemented if BL2_AT_EL3 image is
diff --git a/lib/cpus/aarch64/cortex_demeter.S b/lib/cpus/aarch64/cortex_demeter.S
deleted file mode 100644
index 9ad8b86..0000000
--- a/lib/cpus/aarch64/cortex_demeter.S
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <common/bl_common.h>
-#include <cortex_demeter.h>
-#include <cpu_macros.S>
-#include <plat_macros.S>
-
-/* Hardware handled coherency */
-#if HW_ASSISTED_COHERENCY == 0
-#error "Cortex Demeter must be compiled with HW_ASSISTED_COHERENCY enabled"
-#endif
-
-/* 64-bit only core */
-#if CTX_INCLUDE_AARCH32_REGS == 1
-#error "Cortex Demeter supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
-#endif
-
-	/* ----------------------------------------------------
-	 * HW will do the cache maintenance while powering down
-	 * ----------------------------------------------------
-	 */
-func cortex_demeter_core_pwr_dwn
-	/* ---------------------------------------------------
-	 * Enable CPU power down bit in power control register
-	 * ---------------------------------------------------
-	 */
-	mrs	x0, CORTEX_DEMETER_CPUPWRCTLR_EL1
-	orr	x0, x0, #CORTEX_DEMETER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
-	msr	CORTEX_DEMETER_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-endfunc cortex_demeter_core_pwr_dwn
-
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex Demeter. Must follow AAPCS.
- */
-func cortex_demeter_errata_report
-	ret
-endfunc cortex_demeter_errata_report
-#endif
-
-func cortex_demeter_reset_func
-	/* Disable speculative loads */
-	msr	SSBS, xzr
-	isb
-	ret
-endfunc cortex_demeter_reset_func
-
-	/* ---------------------------------------------
-	 * This function provides Cortex Demeter-
-	 * specific register information for crash
-	 * reporting. It needs to return with x6
-	 * pointing to a list of register names in ascii
-	 * and x8 - x15 having values of registers to be
-	 * reported.
-	 * ---------------------------------------------
-	 */
-.section .rodata.cortex_demeter_regs, "aS"
-cortex_demeter_regs:  /* The ascii list of register names to be reported */
-	.asciz	"cpuectlr_el1", ""
-
-func cortex_demeter_cpu_reg_dump
-	adr	x6, cortex_demeter_regs
-	mrs	x8, CORTEX_DEMETER_CPUECTLR_EL1
-	ret
-endfunc cortex_demeter_cpu_reg_dump
-
-declare_cpu_ops cortex_demeter, CORTEX_DEMETER_MIDR, \
-	cortex_demeter_reset_func, \
-	cortex_demeter_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_hunter.S b/lib/cpus/aarch64/cortex_hunter.S
new file mode 100644
index 0000000..2ab4296
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_hunter.S
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <cortex_hunter.h>
+#include <cpu_macros.S>
+#include <plat_macros.S>
+
+/* Hardware handled coherency */
+#if HW_ASSISTED_COHERENCY == 0
+#error "Cortex Hunter must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Cortex Hunter supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+func cortex_hunter_reset_func
+	/* Disable speculative loads */
+	msr	SSBS, xzr
+	isb
+	ret
+endfunc cortex_hunter_reset_func
+
+	/* ----------------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ----------------------------------------------------
+	 */
+func cortex_hunter_core_pwr_dwn
+	/* ---------------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------------
+	 */
+	mrs	x0, CORTEX_HUNTER_CPUPWRCTLR_EL1
+	orr	x0, x0, #CORTEX_HUNTER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+	msr	CORTEX_HUNTER_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc cortex_hunter_core_pwr_dwn
+
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Cortex Hunter. Must follow AAPCS.
+ */
+func cortex_hunter_errata_report
+	ret
+endfunc cortex_hunter_errata_report
+#endif
+
+	/* ---------------------------------------------
+	 * This function provides Cortex Hunter-specific
+	 * register information for crash reporting.
+	 * It needs to return with x6 pointing to
+	 * a list of register names in ascii and
+	 * x8 - x15 having values of registers to be
+	 * reported.
+	 * ---------------------------------------------
+	 */
+.section .rodata.cortex_hunter_regs, "aS"
+cortex_hunter_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func cortex_hunter_cpu_reg_dump
+	adr	x6, cortex_hunter_regs
+	mrs	x8, CORTEX_HUNTER_CPUECTLR_EL1
+	ret
+endfunc cortex_hunter_cpu_reg_dump
+
+declare_cpu_ops cortex_hunter, CORTEX_HUNTER_MIDR, \
+	cortex_hunter_reset_func, \
+	cortex_hunter_core_pwr_dwn
diff --git a/lib/cpus/aarch64/neoverse_demeter.S b/lib/cpus/aarch64/neoverse_demeter.S
new file mode 100644
index 0000000..f43c18b
--- /dev/null
+++ b/lib/cpus/aarch64/neoverse_demeter.S
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <neoverse_demeter.h>
+#include <cpu_macros.S>
+#include <plat_macros.S>
+
+/* Hardware handled coherency */
+#if HW_ASSISTED_COHERENCY == 0
+#error "Neoverse Demeter must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Neoverse Demeter supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+	/* ----------------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ----------------------------------------------------
+	 */
+func neoverse_demeter_core_pwr_dwn
+	/* ---------------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------------
+	 */
+	mrs	x0, NEOVERSE_DEMETER_CPUPWRCTLR_EL1
+	orr	x0, x0, #NEOVERSE_DEMETER_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
+	msr	NEOVERSE_DEMETER_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc neoverse_demeter_core_pwr_dwn
+
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Neoverse Demeter. Must follow AAPCS.
+ */
+func neoverse_demeter_errata_report
+	ret
+endfunc neoverse_demeter_errata_report
+#endif
+
+func neoverse_demeter_reset_func
+	/* Disable speculative loads */
+	msr	SSBS, xzr
+	isb
+	ret
+endfunc neoverse_demeter_reset_func
+
+	/* ---------------------------------------------
+	 * This function provides Neoverse Demeter-
+	 * specific register information for crash
+	 * reporting. It needs to return with x6
+	 * pointing to a list of register names in ascii
+	 * and x8 - x15 having values of registers to be
+	 * reported.
+	 * ---------------------------------------------
+	 */
+.section .rodata.neoverse_demeter_regs, "aS"
+neoverse_demeter_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func neoverse_demeter_cpu_reg_dump
+	adr	x6, neoverse_demeter_regs
+	mrs	x8, NEOVERSE_DEMETER_CPUECTLR_EL1
+	ret
+endfunc neoverse_demeter_cpu_reg_dump
+
+declare_cpu_ops neoverse_demeter, NEOVERSE_DEMETER_MIDR, \
+	neoverse_demeter_reset_func, \
+	neoverse_demeter_core_pwr_dwn
diff --git a/lib/cpus/errata_report.c b/lib/cpus/errata_report.c
index 5d1e3c5..93b2744 100644
--- a/lib/cpus/errata_report.c
+++ b/lib/cpus/errata_report.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 # define BL_STRING	"BL1"
 #elif defined(__aarch64__) && defined(IMAGE_BL31)
 # define BL_STRING	"BL31"
-#elif !defined(__arch64__) && defined(IMAGE_BL32)
+#elif !defined(__aarch64__) && defined(IMAGE_BL32)
 # define BL_STRING	"BL32"
 #elif defined(IMAGE_BL2) && BL2_AT_EL3
 # define BL_STRING "BL2"
diff --git a/lib/extensions/amu/aarch32/amu.c b/lib/extensions/amu/aarch32/amu.c
index ed56ddd..57b1158 100644
--- a/lib/extensions/amu/aarch32/amu.c
+++ b/lib/extensions/amu/aarch32/amu.c
@@ -5,95 +5,224 @@
  */
 
 #include <assert.h>
+#include <cdefs.h>
 #include <stdbool.h>
 
+#include "../amu_private.h"
 #include <arch.h>
 #include <arch_helpers.h>
-
+#include <common/debug.h>
 #include <lib/el3_runtime/pubsub_events.h>
 #include <lib/extensions/amu.h>
-#include <lib/extensions/amu_private.h>
 
 #include <plat/common/platform.h>
 
-static struct amu_ctx amu_ctxs[PLATFORM_CORE_COUNT];
+struct amu_ctx {
+	uint64_t group0_cnts[AMU_GROUP0_MAX_COUNTERS];
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint64_t group1_cnts[AMU_GROUP1_MAX_COUNTERS];
+#endif
 
-/*
- * Get AMU version value from pfr0.
- * Return values
- *   ID_PFR0_AMU_V1: FEAT_AMUv1 supported (introduced in ARM v8.4)
- *   ID_PFR0_AMU_V1P1: FEAT_AMUv1p1 supported (introduced in ARM v8.6)
- *   ID_PFR0_AMU_NOT_SUPPORTED: not supported
- */
-unsigned int amu_get_version(void)
+	uint16_t group0_enable;
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint16_t group1_enable;
+#endif
+};
+
+static struct amu_ctx amu_ctxs_[PLATFORM_CORE_COUNT];
+
+CASSERT((sizeof(amu_ctxs_[0].group0_enable) * CHAR_BIT) <= AMU_GROUP0_MAX_COUNTERS,
+	amu_ctx_group0_enable_cannot_represent_all_group0_counters);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+CASSERT((sizeof(amu_ctxs_[0].group1_enable) * CHAR_BIT) <= AMU_GROUP1_MAX_COUNTERS,
+	amu_ctx_group1_enable_cannot_represent_all_group1_counters);
+#endif
+
+static inline __unused uint32_t read_id_pfr0_amu(void)
 {
-	return (unsigned int)(read_id_pfr0() >> ID_PFR0_AMU_SHIFT) &
+	return (read_id_pfr0() >> ID_PFR0_AMU_SHIFT) &
 		ID_PFR0_AMU_MASK;
 }
 
-#if AMU_GROUP1_NR_COUNTERS
-/* Check if group 1 counters is implemented */
-bool amu_group1_supported(void)
+static inline __unused void write_hcptr_tam(uint32_t value)
 {
-	uint32_t features = read_amcfgr() >> AMCFGR_NCG_SHIFT;
+	write_hcptr((read_hcptr() & ~TAM_BIT) |
+		((value << TAM_SHIFT) & TAM_BIT));
+}
 
-	return (features & AMCFGR_NCG_MASK) == 1U;
+static inline __unused void write_amcr_cg1rz(uint32_t value)
+{
+	write_amcr((read_amcr() & ~AMCR_CG1RZ_BIT) |
+		((value << AMCR_CG1RZ_SHIFT) & AMCR_CG1RZ_BIT));
+}
+
+static inline __unused uint32_t read_amcfgr_ncg(void)
+{
+	return (read_amcfgr() >> AMCFGR_NCG_SHIFT) &
+		AMCFGR_NCG_MASK;
+}
+
+static inline __unused uint32_t read_amcgcr_cg0nc(void)
+{
+	return (read_amcgcr() >> AMCGCR_CG0NC_SHIFT) &
+		AMCGCR_CG0NC_MASK;
+}
+
+static inline __unused uint32_t read_amcgcr_cg1nc(void)
+{
+	return (read_amcgcr() >> AMCGCR_CG1NC_SHIFT) &
+		AMCGCR_CG1NC_MASK;
+}
+
+static inline __unused uint32_t read_amcntenset0_px(void)
+{
+	return (read_amcntenset0() >> AMCNTENSET0_Pn_SHIFT) &
+		AMCNTENSET0_Pn_MASK;
+}
+
+static inline __unused uint32_t read_amcntenset1_px(void)
+{
+	return (read_amcntenset1() >> AMCNTENSET1_Pn_SHIFT) &
+		AMCNTENSET1_Pn_MASK;
+}
+
+static inline __unused void write_amcntenset0_px(uint32_t px)
+{
+	uint32_t value = read_amcntenset0();
+
+	value &= ~AMCNTENSET0_Pn_MASK;
+	value |= (px << AMCNTENSET0_Pn_SHIFT) &
+		AMCNTENSET0_Pn_MASK;
+
+	write_amcntenset0(value);
+}
+
+static inline __unused void write_amcntenset1_px(uint32_t px)
+{
+	uint32_t value = read_amcntenset1();
+
+	value &= ~AMCNTENSET1_Pn_MASK;
+	value |= (px << AMCNTENSET1_Pn_SHIFT) &
+		AMCNTENSET1_Pn_MASK;
+
+	write_amcntenset1(value);
+}
+
+static inline __unused void write_amcntenclr0_px(uint32_t px)
+{
+	uint32_t value = read_amcntenclr0();
+
+	value &= ~AMCNTENCLR0_Pn_MASK;
+	value |= (px << AMCNTENCLR0_Pn_SHIFT) & AMCNTENCLR0_Pn_MASK;
+
+	write_amcntenclr0(value);
+}
+
+static inline __unused void write_amcntenclr1_px(uint32_t px)
+{
+	uint32_t value = read_amcntenclr1();
+
+	value &= ~AMCNTENCLR1_Pn_MASK;
+	value |= (px << AMCNTENCLR1_Pn_SHIFT) & AMCNTENCLR1_Pn_MASK;
+
+	write_amcntenclr1(value);
+}
+
+static __unused bool amu_supported(void)
+{
+	return read_id_pfr0_amu() >= ID_PFR0_AMU_V1;
+}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+static __unused bool amu_group1_supported(void)
+{
+	return read_amcfgr_ncg() > 0U;
 }
 #endif
 
 /*
- * Enable counters. This function is meant to be invoked
- * by the context management library before exiting from EL3.
+ * Enable counters. This function is meant to be invoked by the context
+ * management library before exiting from EL3.
  */
 void amu_enable(bool el2_unused)
 {
-	if (amu_get_version() == ID_PFR0_AMU_NOT_SUPPORTED) {
+	uint32_t id_pfr0_amu;		/* AMU version */
+
+	uint32_t amcfgr_ncg;		/* Number of counter groups */
+	uint32_t amcgcr_cg0nc;		/* Number of group 0 counters */
+
+	uint32_t amcntenset0_px = 0x0;	/* Group 0 enable mask */
+	uint32_t amcntenset1_px = 0x0;	/* Group 1 enable mask */
+
+	id_pfr0_amu = read_id_pfr0_amu();
+	if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+		/*
+		 * If the AMU is unsupported, nothing needs to be done.
+		 */
+
 		return;
 	}
 
-#if AMU_GROUP1_NR_COUNTERS
-	/* Check and set presence of group 1 counters */
-	if (!amu_group1_supported()) {
-		ERROR("AMU Counter Group 1 is not implemented\n");
-		panic();
-	}
-
-	/* Check number of group 1 counters */
-	uint32_t cnt_num = (read_amcgcr() >> AMCGCR_CG1NC_SHIFT) &
-				AMCGCR_CG1NC_MASK;
-	VERBOSE("%s%u. %s%u\n",
-		"Number of AMU Group 1 Counters ", cnt_num,
-		"Requested number ", AMU_GROUP1_NR_COUNTERS);
-
-	if (cnt_num < AMU_GROUP1_NR_COUNTERS) {
-		ERROR("%s%u is less than %s%u\n",
-		"Number of AMU Group 1 Counters ", cnt_num,
-		"Requested number ", AMU_GROUP1_NR_COUNTERS);
-		panic();
-	}
-#endif
-
 	if (el2_unused) {
-		uint64_t v;
 		/*
-		 * Non-secure access from EL0 or EL1 to the Activity Monitor
-		 * registers do not trap to EL2.
+		 * HCPTR.TAM: Set to zero so any accesses to the Activity
+		 * Monitor registers do not trap to EL2.
 		 */
-		v = read_hcptr();
-		v &= ~TAM_BIT;
-		write_hcptr(v);
+		write_hcptr_tam(0U);
 	}
 
-	/* Enable group 0 counters */
-	write_amcntenset0(AMU_GROUP0_COUNTERS_MASK);
+	/*
+	 * Retrieve the number of architected counters. All of these counters
+	 * are enabled by default.
+	 */
 
-#if AMU_GROUP1_NR_COUNTERS
-	/* Enable group 1 counters */
-	write_amcntenset1(AMU_GROUP1_COUNTERS_MASK);
+	amcgcr_cg0nc = read_amcgcr_cg0nc();
+	amcntenset0_px = (UINT32_C(1) << (amcgcr_cg0nc)) - 1U;
+
+	assert(amcgcr_cg0nc <= AMU_AMCGCR_CG0NC_MAX);
+
+	/*
+	 * The platform may opt to enable specific auxiliary counters. This can
+	 * be done via the common FCONF getter, or via the platform-implemented
+	 * function.
+	 */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	const struct amu_topology *topology;
+
+#if ENABLE_AMU_FCONF
+	topology = FCONF_GET_PROPERTY(amu, config, topology);
+#else
+	topology = plat_amu_topology();
+#endif /* ENABLE_AMU_FCONF */
+
+	if (topology != NULL) {
+		unsigned int core_pos = plat_my_core_pos();
+
+		amcntenset1_el0_px = topology->cores[core_pos].enable;
+	} else {
+		ERROR("AMU: failed to generate AMU topology\n");
+	}
+#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
+
+	/*
+	 * Enable the requested counters.
+	 */
+
+	write_amcntenset0_px(amcntenset0_px);
+
+	amcfgr_ncg = read_amcfgr_ncg();
+	if (amcfgr_ncg > 0U) {
+		write_amcntenset1_px(amcntenset1_px);
+
+#if !ENABLE_AMU_AUXILIARY_COUNTERS
+		VERBOSE("AMU: auxiliary counters detected but support is disabled\n");
 #endif
+	}
 
 	/* Initialize FEAT_AMUv1p1 features if present. */
-	if (amu_get_version() < ID_PFR0_AMU_V1P1) {
+	if (id_pfr0_amu < ID_PFR0_AMU_V1P1) {
 		return;
 	}
 
@@ -106,154 +235,183 @@
 	 * mapped view are unaffected.
 	 */
 	VERBOSE("AMU group 1 counter access restricted.\n");
-	write_amcr(read_amcr() | AMCR_CG1RZ_BIT);
+	write_amcr_cg1rz(1U);
 #else
-	write_amcr(read_amcr() & ~AMCR_CG1RZ_BIT);
+	write_amcr_cg1rz(0U);
 #endif
 }
 
 /* Read the group 0 counter identified by the given `idx`. */
-uint64_t amu_group0_cnt_read(unsigned int idx)
+static uint64_t amu_group0_cnt_read(unsigned int idx)
 {
-	assert(amu_get_version() != ID_PFR0_AMU_NOT_SUPPORTED);
-	assert(idx < AMU_GROUP0_NR_COUNTERS);
+	assert(amu_supported());
+	assert(idx < read_amcgcr_cg0nc());
 
 	return amu_group0_cnt_read_internal(idx);
 }
 
 /* Write the group 0 counter identified by the given `idx` with `val` */
-void amu_group0_cnt_write(unsigned  int idx, uint64_t val)
+static void amu_group0_cnt_write(unsigned  int idx, uint64_t val)
 {
-	assert(amu_get_version() != ID_PFR0_AMU_NOT_SUPPORTED);
-	assert(idx < AMU_GROUP0_NR_COUNTERS);
+	assert(amu_supported());
+	assert(idx < read_amcgcr_cg0nc());
 
 	amu_group0_cnt_write_internal(idx, val);
 	isb();
 }
 
-#if AMU_GROUP1_NR_COUNTERS
+#if ENABLE_AMU_AUXILIARY_COUNTERS
 /* Read the group 1 counter identified by the given `idx` */
-uint64_t amu_group1_cnt_read(unsigned  int idx)
+static uint64_t amu_group1_cnt_read(unsigned  int idx)
 {
-	assert(amu_get_version() != ID_PFR0_AMU_NOT_SUPPORTED);
+	assert(amu_supported());
 	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
+	assert(idx < read_amcgcr_cg1nc());
 
 	return amu_group1_cnt_read_internal(idx);
 }
 
 /* Write the group 1 counter identified by the given `idx` with `val` */
-void amu_group1_cnt_write(unsigned  int idx, uint64_t val)
+static void amu_group1_cnt_write(unsigned  int idx, uint64_t val)
 {
-	assert(amu_get_version() != ID_PFR0_AMU_NOT_SUPPORTED);
+	assert(amu_supported());
 	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
+	assert(idx < read_amcgcr_cg1nc());
 
 	amu_group1_cnt_write_internal(idx, val);
 	isb();
 }
-
-/*
- * Program the event type register for the given `idx` with
- * the event number `val`
- */
-void amu_group1_set_evtype(unsigned int idx, unsigned int val)
-{
-	assert(amu_get_version() != ID_PFR0_AMU_NOT_SUPPORTED);
-	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
-
-	amu_group1_set_evtype_internal(idx, val);
-	isb();
-}
-#endif	/* AMU_GROUP1_NR_COUNTERS */
+#endif
 
 static void *amu_context_save(const void *arg)
 {
-	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
-	unsigned int i;
+	uint32_t i;
 
-	if (amu_get_version() == ID_PFR0_AMU_NOT_SUPPORTED) {
-		return (void *)-1;
+	unsigned int core_pos;
+	struct amu_ctx *ctx;
+
+	uint32_t id_pfr0_amu;	/* AMU version */
+	uint32_t amcgcr_cg0nc;	/* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint32_t amcfgr_ncg;	/* Number of counter groups */
+	uint32_t amcgcr_cg1nc;	/* Number of group 1 counters */
+#endif
+
+	id_pfr0_amu = read_id_pfr0_amu();
+	if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+		return (void *)0;
 	}
 
-#if AMU_GROUP1_NR_COUNTERS
-	if (!amu_group1_supported()) {
-		return (void *)-1;
-	}
-#endif
-	/* Assert that group 0/1 counter configuration is what we expect */
-	assert(read_amcntenset0_el0() == AMU_GROUP0_COUNTERS_MASK);
+	core_pos = plat_my_core_pos();
+	ctx = &amu_ctxs_[core_pos];
 
-#if AMU_GROUP1_NR_COUNTERS
-	assert(read_amcntenset1_el0() == AMU_GROUP1_COUNTERS_MASK);
+	amcgcr_cg0nc = read_amcgcr_cg0nc();
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	amcfgr_ncg = read_amcfgr_ncg();
+	amcgcr_cg1nc = (amcfgr_ncg > 0U) ? read_amcgcr_cg1nc() : 0U;
 #endif
+
 	/*
-	 * Disable group 0/1 counters to avoid other observers like SCP sampling
-	 * counter values from the future via the memory mapped view.
+	 * Disable all AMU counters.
 	 */
-	write_amcntenclr0(AMU_GROUP0_COUNTERS_MASK);
 
-#if AMU_GROUP1_NR_COUNTERS
-	write_amcntenclr1(AMU_GROUP1_COUNTERS_MASK);
+	ctx->group0_enable = read_amcntenset0_px();
+	write_amcntenclr0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	if (amcfgr_ncg > 0U) {
+		ctx->group1_enable = read_amcntenset1_px();
+		write_amcntenclr1_px(ctx->group1_enable);
+	}
 #endif
-	isb();
 
-	/* Save all group 0 counters */
-	for (i = 0U; i < AMU_GROUP0_NR_COUNTERS; i++) {
+	/*
+	 * Save the counters to the local context.
+	 */
+
+	isb(); /* Ensure counters have been stopped */
+
+	for (i = 0U; i < amcgcr_cg0nc; i++) {
 		ctx->group0_cnts[i] = amu_group0_cnt_read(i);
 	}
 
-#if AMU_GROUP1_NR_COUNTERS
-	/* Save group 1 counters */
-	for (i = 0U; i < AMU_GROUP1_NR_COUNTERS; i++) {
-		if ((AMU_GROUP1_COUNTERS_MASK & (1U << i)) != 0U) {
-			ctx->group1_cnts[i] = amu_group1_cnt_read(i);
-		}
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	for (i = 0U; i < amcgcr_cg1nc; i++) {
+		ctx->group1_cnts[i] = amu_group1_cnt_read(i);
 	}
 #endif
+
 	return (void *)0;
 }
 
 static void *amu_context_restore(const void *arg)
 {
-	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
-	unsigned int i;
+	uint32_t i;
 
-	if (amu_get_version() == ID_PFR0_AMU_NOT_SUPPORTED) {
-		return (void *)-1;
-	}
+	unsigned int core_pos;
+	struct amu_ctx *ctx;
 
-#if AMU_GROUP1_NR_COUNTERS
-	if (!amu_group1_supported()) {
-		return (void *)-1;
-	}
-#endif
-	/* Counters were disabled in `amu_context_save()` */
-	assert(read_amcntenset0_el0() == 0U);
+	uint32_t id_pfr0_amu;	/* AMU version */
 
-#if AMU_GROUP1_NR_COUNTERS
-	assert(read_amcntenset1_el0() == 0U);
+	uint32_t amcfgr_ncg;	/* Number of counter groups */
+	uint32_t amcgcr_cg0nc;	/* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint32_t amcgcr_cg1nc;	/* Number of group 1 counters */
 #endif
 
-	/* Restore all group 0 counters */
-	for (i = 0U; i < AMU_GROUP0_NR_COUNTERS; i++) {
+	id_pfr0_amu = read_id_pfr0_amu();
+	if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+		return (void *)0;
+	}
+
+	core_pos = plat_my_core_pos();
+	ctx = &amu_ctxs_[core_pos];
+
+	amcfgr_ncg = read_amcfgr_ncg();
+	amcgcr_cg0nc = read_amcgcr_cg0nc();
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	amcgcr_cg1nc = (amcfgr_ncg > 0U) ? read_amcgcr_cg1nc() : 0U;
+#endif
+
+	/*
+	 * Sanity check that all counters were disabled when the context was
+	 * previously saved.
+	 */
+
+	assert(read_amcntenset0_px() == 0U);
+
+	if (amcfgr_ncg > 0U) {
+		assert(read_amcntenset1_px() == 0U);
+	}
+
+	/*
+	 * Restore the counter values from the local context.
+	 */
+
+	for (i = 0U; i < amcgcr_cg0nc; i++) {
 		amu_group0_cnt_write(i, ctx->group0_cnts[i]);
 	}
 
-	/* Restore group 0 counter configuration */
-	write_amcntenset0(AMU_GROUP0_COUNTERS_MASK);
-
-#if AMU_GROUP1_NR_COUNTERS
-	/* Restore group 1 counters */
-	for (i = 0U; i < AMU_GROUP1_NR_COUNTERS; i++) {
-		if ((AMU_GROUP1_COUNTERS_MASK & (1U << i)) != 0U) {
-			amu_group1_cnt_write(i, ctx->group1_cnts[i]);
-		}
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	for (i = 0U; i < amcgcr_cg1nc; i++) {
+		amu_group1_cnt_write(i, ctx->group1_cnts[i]);
 	}
+#endif
 
-	/* Restore group 1 counter configuration */
-	write_amcntenset1(AMU_GROUP1_COUNTERS_MASK);
+	/*
+	 * Re-enable counters that were disabled during context save.
+	 */
+
+	write_amcntenset0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	if (amcfgr_ncg > 0U) {
+		write_amcntenset1_px(ctx->group1_enable);
+	}
 #endif
 
 	return (void *)0;
diff --git a/lib/extensions/amu/aarch32/amu_helpers.S b/lib/extensions/amu/aarch32/amu_helpers.S
index d387341..8ac7678 100644
--- a/lib/extensions/amu/aarch32/amu_helpers.S
+++ b/lib/extensions/amu/aarch32/amu_helpers.S
@@ -84,6 +84,7 @@
 	bx		lr
 endfunc amu_group0_cnt_write_internal
 
+#if ENABLE_AMU_AUXILIARY_COUNTERS
 /*
  * uint64_t amu_group1_cnt_read_internal(int idx);
  *
@@ -267,3 +268,4 @@
 	stcopr	r1, AMEVTYPER1F /* index 15 */
 	bx	lr
 endfunc amu_group1_set_evtype_internal
+#endif
diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
index 295c0d5..35efd21 100644
--- a/lib/extensions/amu/aarch64/amu.c
+++ b/lib/extensions/amu/aarch64/amu.c
@@ -5,86 +5,218 @@
  */
 
 #include <assert.h>
+#include <cdefs.h>
 #include <stdbool.h>
 
+#include "../amu_private.h"
 #include <arch.h>
 #include <arch_features.h>
 #include <arch_helpers.h>
-
+#include <common/debug.h>
 #include <lib/el3_runtime/pubsub_events.h>
 #include <lib/extensions/amu.h>
-#include <lib/extensions/amu_private.h>
 
 #include <plat/common/platform.h>
 
-static struct amu_ctx amu_ctxs[PLATFORM_CORE_COUNT];
+#if ENABLE_AMU_FCONF
+#	include <lib/fconf/fconf.h>
+#	include <lib/fconf/fconf_amu_getter.h>
+#endif
 
-/*
- * Get AMU version value from aa64pfr0.
- * Return values
- *   ID_AA64PFR0_AMU_V1: FEAT_AMUv1 supported (introduced in ARM v8.4)
- *   ID_AA64PFR0_AMU_V1P1: FEAT_AMUv1p1 supported (introduced in ARM v8.6)
- *   ID_AA64PFR0_AMU_NOT_SUPPORTED: not supported
- */
-unsigned int amu_get_version(void)
+#if ENABLE_MPMM
+#	include <lib/mpmm/mpmm.h>
+#endif
+
+struct amu_ctx {
+	uint64_t group0_cnts[AMU_GROUP0_MAX_COUNTERS];
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint64_t group1_cnts[AMU_GROUP1_MAX_COUNTERS];
+#endif
+
+	/* Architected event counter 1 does not have an offset register */
+	uint64_t group0_voffsets[AMU_GROUP0_MAX_COUNTERS - 1U];
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint64_t group1_voffsets[AMU_GROUP1_MAX_COUNTERS];
+#endif
+
+	uint16_t group0_enable;
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint16_t group1_enable;
+#endif
+};
+
+static struct amu_ctx amu_ctxs_[PLATFORM_CORE_COUNT];
+
+CASSERT((sizeof(amu_ctxs_[0].group0_enable) * CHAR_BIT) <= AMU_GROUP0_MAX_COUNTERS,
+	amu_ctx_group0_enable_cannot_represent_all_group0_counters);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+CASSERT((sizeof(amu_ctxs_[0].group1_enable) * CHAR_BIT) <= AMU_GROUP1_MAX_COUNTERS,
+	amu_ctx_group1_enable_cannot_represent_all_group1_counters);
+#endif
+
+static inline __unused uint64_t read_id_aa64pfr0_el1_amu(void)
 {
-	return (unsigned int)(read_id_aa64pfr0_el1() >> ID_AA64PFR0_AMU_SHIFT) &
+	return (read_id_aa64pfr0_el1() >> ID_AA64PFR0_AMU_SHIFT) &
 		ID_AA64PFR0_AMU_MASK;
 }
 
-#if AMU_GROUP1_NR_COUNTERS
-/* Check if group 1 counters is implemented */
-bool amu_group1_supported(void)
+static inline __unused uint64_t read_hcr_el2_amvoffen(void)
 {
-	uint64_t features = read_amcfgr_el0() >> AMCFGR_EL0_NCG_SHIFT;
+	return (read_hcr_el2() & HCR_AMVOFFEN_BIT) >>
+		HCR_AMVOFFEN_SHIFT;
+}
 
-	return (features & AMCFGR_EL0_NCG_MASK) == 1U;
+static inline __unused void write_cptr_el2_tam(uint64_t value)
+{
+	write_cptr_el2((read_cptr_el2() & ~CPTR_EL2_TAM_BIT) |
+		((value << CPTR_EL2_TAM_SHIFT) & CPTR_EL2_TAM_BIT));
+}
+
+static inline __unused void write_cptr_el3_tam(cpu_context_t *ctx, uint64_t tam)
+{
+	uint64_t value = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+
+	value &= ~TAM_BIT;
+	value |= (tam << TAM_SHIFT) & TAM_BIT;
+
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, value);
+}
+
+static inline __unused void write_hcr_el2_amvoffen(uint64_t value)
+{
+	write_hcr_el2((read_hcr_el2() & ~HCR_AMVOFFEN_BIT) |
+		((value << HCR_AMVOFFEN_SHIFT) & HCR_AMVOFFEN_BIT));
+}
+
+static inline __unused void write_amcr_el0_cg1rz(uint64_t value)
+{
+	write_amcr_el0((read_amcr_el0() & ~AMCR_CG1RZ_BIT) |
+		((value << AMCR_CG1RZ_SHIFT) & AMCR_CG1RZ_BIT));
+}
+
+static inline __unused uint64_t read_amcfgr_el0_ncg(void)
+{
+	return (read_amcfgr_el0() >> AMCFGR_EL0_NCG_SHIFT) &
+		AMCFGR_EL0_NCG_MASK;
+}
+
+static inline __unused uint64_t read_amcgcr_el0_cg0nc(void)
+{
+	return (read_amcgcr_el0() >> AMCGCR_EL0_CG0NC_SHIFT) &
+		AMCGCR_EL0_CG0NC_MASK;
+}
+
+static inline __unused uint64_t read_amcg1idr_el0_voff(void)
+{
+	return (read_amcg1idr_el0() >> AMCG1IDR_VOFF_SHIFT) &
+		AMCG1IDR_VOFF_MASK;
+}
+
+static inline __unused uint64_t read_amcgcr_el0_cg1nc(void)
+{
+	return (read_amcgcr_el0() >> AMCGCR_EL0_CG1NC_SHIFT) &
+		AMCGCR_EL0_CG1NC_MASK;
+}
+
+static inline __unused uint64_t read_amcntenset0_el0_px(void)
+{
+	return (read_amcntenset0_el0() >> AMCNTENSET0_EL0_Pn_SHIFT) &
+		AMCNTENSET0_EL0_Pn_MASK;
+}
+
+static inline __unused uint64_t read_amcntenset1_el0_px(void)
+{
+	return (read_amcntenset1_el0() >> AMCNTENSET1_EL0_Pn_SHIFT) &
+		AMCNTENSET1_EL0_Pn_MASK;
+}
+
+static inline __unused void write_amcntenset0_el0_px(uint64_t px)
+{
+	uint64_t value = read_amcntenset0_el0();
+
+	value &= ~AMCNTENSET0_EL0_Pn_MASK;
+	value |= (px << AMCNTENSET0_EL0_Pn_SHIFT) & AMCNTENSET0_EL0_Pn_MASK;
+
+	write_amcntenset0_el0(value);
+}
+
+static inline __unused void write_amcntenset1_el0_px(uint64_t px)
+{
+	uint64_t value = read_amcntenset1_el0();
+
+	value &= ~AMCNTENSET1_EL0_Pn_MASK;
+	value |= (px << AMCNTENSET1_EL0_Pn_SHIFT) & AMCNTENSET1_EL0_Pn_MASK;
+
+	write_amcntenset1_el0(value);
+}
+
+static inline __unused void write_amcntenclr0_el0_px(uint64_t px)
+{
+	uint64_t value = read_amcntenclr0_el0();
+
+	value &= ~AMCNTENCLR0_EL0_Pn_MASK;
+	value |= (px << AMCNTENCLR0_EL0_Pn_SHIFT) & AMCNTENCLR0_EL0_Pn_MASK;
+
+	write_amcntenclr0_el0(value);
+}
+
+static inline __unused void write_amcntenclr1_el0_px(uint64_t px)
+{
+	uint64_t value = read_amcntenclr1_el0();
+
+	value &= ~AMCNTENCLR1_EL0_Pn_MASK;
+	value |= (px << AMCNTENCLR1_EL0_Pn_SHIFT) & AMCNTENCLR1_EL0_Pn_MASK;
+
+	write_amcntenclr1_el0(value);
+}
+
+static __unused bool amu_supported(void)
+{
+	return read_id_aa64pfr0_el1_amu() >= ID_AA64PFR0_AMU_V1;
+}
+
+static __unused bool amu_v1p1_supported(void)
+{
+	return read_id_aa64pfr0_el1_amu() >= ID_AA64PFR0_AMU_V1P1;
+}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+static __unused bool amu_group1_supported(void)
+{
+	return read_amcfgr_el0_ncg() > 0U;
 }
 #endif
 
 /*
- * Enable counters. This function is meant to be invoked
- * by the context management library before exiting from EL3.
+ * Enable counters. This function is meant to be invoked by the context
+ * management library before exiting from EL3.
  */
 void amu_enable(bool el2_unused, cpu_context_t *ctx)
 {
-	uint64_t v;
-	unsigned int amu_version = amu_get_version();
+	uint64_t id_aa64pfr0_el1_amu;		/* AMU version */
 
-	if (amu_version == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+	uint64_t amcfgr_el0_ncg;		/* Number of counter groups */
+	uint64_t amcgcr_el0_cg0nc;		/* Number of group 0 counters */
+
+	uint64_t amcntenset0_el0_px = 0x0;	/* Group 0 enable mask */
+	uint64_t amcntenset1_el0_px = 0x0;	/* Group 1 enable mask */
+
+	id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
+	if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+		/*
+		 * If the AMU is unsupported, nothing needs to be done.
+		 */
+
 		return;
 	}
 
-#if AMU_GROUP1_NR_COUNTERS
-	/* Check and set presence of group 1 counters */
-	if (!amu_group1_supported()) {
-		ERROR("AMU Counter Group 1 is not implemented\n");
-		panic();
-	}
-
-	/* Check number of group 1 counters */
-	uint64_t cnt_num = (read_amcgcr_el0() >> AMCGCR_EL0_CG1NC_SHIFT) &
-				AMCGCR_EL0_CG1NC_MASK;
-	VERBOSE("%s%llu. %s%u\n",
-		"Number of AMU Group 1 Counters ", cnt_num,
-		"Requested number ", AMU_GROUP1_NR_COUNTERS);
-
-	if (cnt_num < AMU_GROUP1_NR_COUNTERS) {
-		ERROR("%s%llu is less than %s%u\n",
-		"Number of AMU Group 1 Counters ", cnt_num,
-		"Requested number ", AMU_GROUP1_NR_COUNTERS);
-		panic();
-	}
-#endif
-
 	if (el2_unused) {
 		/*
-		 * CPTR_EL2.TAM: Set to zero so any accesses to
-		 * the Activity Monitor registers do not trap to EL2.
+		 * CPTR_EL2.TAM: Set to zero so any accesses to the Activity
+		 * Monitor registers do not trap to EL2.
 		 */
-		v = read_cptr_el2();
-		v &= ~CPTR_EL2_TAM_BIT;
-		write_cptr_el2(v);
+		write_cptr_el2_tam(0U);
 	}
 
 	/*
@@ -92,72 +224,141 @@
 	 * in 'ctx'. Set CPTR_EL3.TAM to zero so that any accesses to
 	 * the Activity Monitor registers do not trap to EL3.
 	 */
-	v = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
-	v &= ~TAM_BIT;
-	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, v);
+	write_cptr_el3_tam(ctx, 0U);
 
-	/* Enable group 0 counters */
-	write_amcntenset0_el0(AMU_GROUP0_COUNTERS_MASK);
+	/*
+	 * Retrieve the number of architected counters. All of these counters
+	 * are enabled by default.
+	 */
 
-#if AMU_GROUP1_NR_COUNTERS
-	/* Enable group 1 counters */
-	write_amcntenset1_el0(AMU_GROUP1_COUNTERS_MASK);
+	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
+	amcntenset0_el0_px = (UINT64_C(1) << (amcgcr_el0_cg0nc)) - 1U;
+
+	assert(amcgcr_el0_cg0nc <= AMU_AMCGCR_CG0NC_MAX);
+
+	/*
+	 * The platform may opt to enable specific auxiliary counters. This can
+	 * be done via the common FCONF getter, or via the platform-implemented
+	 * function.
+	 */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	const struct amu_topology *topology;
+
+#if ENABLE_AMU_FCONF
+	topology = FCONF_GET_PROPERTY(amu, config, topology);
+#else
+	topology = plat_amu_topology();
+#endif /* ENABLE_AMU_FCONF */
+
+	if (topology != NULL) {
+		unsigned int core_pos = plat_my_core_pos();
+
+		amcntenset1_el0_px = topology->cores[core_pos].enable;
+	} else {
+		ERROR("AMU: failed to generate AMU topology\n");
+	}
+#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
+
+	/*
+	 * Enable the requested counters.
+	 */
+
+	write_amcntenset0_el0_px(amcntenset0_el0_px);
+
+	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
+	if (amcfgr_el0_ncg > 0U) {
+		write_amcntenset1_el0_px(amcntenset1_el0_px);
+
+#if !ENABLE_AMU_AUXILIARY_COUNTERS
+		VERBOSE("AMU: auxiliary counters detected but support is disabled\n");
 #endif
+	}
 
 	/* Initialize FEAT_AMUv1p1 features if present. */
-	if (amu_version < ID_AA64PFR0_AMU_V1P1) {
-		return;
-	}
-
-	if (el2_unused) {
-		/* Make sure virtual offsets are disabled if EL2 not used. */
-		write_hcr_el2(read_hcr_el2() & ~HCR_AMVOFFEN_BIT);
-	}
+	if (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) {
+		if (el2_unused) {
+			/*
+			 * Make sure virtual offsets are disabled if EL2 not
+			 * used.
+			 */
+			write_hcr_el2_amvoffen(0U);
+		}
 
 #if AMU_RESTRICT_COUNTERS
-	/*
-	 * FEAT_AMUv1p1 adds a register field to restrict access to group 1
-	 * counters at all but the highest implemented EL.  This is controlled
-	 * with the AMU_RESTRICT_COUNTERS compile time flag, when set, system
-	 * register reads at lower ELs return zero.  Reads from the memory
-	 * mapped view are unaffected.
-	 */
-	VERBOSE("AMU group 1 counter access restricted.\n");
-	write_amcr_el0(read_amcr_el0() | AMCR_CG1RZ_BIT);
+		/*
+		 * FEAT_AMUv1p1 adds a register field to restrict access to
+		 * group 1 counters at all but the highest implemented EL. This
+		 * is controlled with the `AMU_RESTRICT_COUNTERS` compile time
+		 * flag, when set, system register reads at lower ELs return
+		 * zero. Reads from the memory mapped view are unaffected.
+		 */
+		VERBOSE("AMU group 1 counter access restricted.\n");
+		write_amcr_el0_cg1rz(1U);
 #else
-	write_amcr_el0(read_amcr_el0() & ~AMCR_CG1RZ_BIT);
+		write_amcr_el0_cg1rz(0U);
+#endif
+	}
+
+#if ENABLE_MPMM
+	mpmm_enable();
 #endif
 }
 
 /* Read the group 0 counter identified by the given `idx`. */
-uint64_t amu_group0_cnt_read(unsigned int idx)
+static uint64_t amu_group0_cnt_read(unsigned int idx)
 {
-	assert(amu_get_version() != ID_AA64PFR0_AMU_NOT_SUPPORTED);
-	assert(idx < AMU_GROUP0_NR_COUNTERS);
+	assert(amu_supported());
+	assert(idx < read_amcgcr_el0_cg0nc());
 
 	return amu_group0_cnt_read_internal(idx);
 }
 
 /* Write the group 0 counter identified by the given `idx` with `val` */
-void amu_group0_cnt_write(unsigned  int idx, uint64_t val)
+static void amu_group0_cnt_write(unsigned  int idx, uint64_t val)
 {
-	assert(amu_get_version() != ID_AA64PFR0_AMU_NOT_SUPPORTED);
-	assert(idx < AMU_GROUP0_NR_COUNTERS);
+	assert(amu_supported());
+	assert(idx < read_amcgcr_el0_cg0nc());
 
 	amu_group0_cnt_write_internal(idx, val);
 	isb();
 }
 
 /*
+ * Unlike with auxiliary counters, we cannot detect at runtime whether an
+ * architected counter supports a virtual offset. These are instead fixed
+ * according to FEAT_AMUv1p1, but this switch will need to be updated if later
+ * revisions of FEAT_AMU add additional architected counters.
+ */
+static bool amu_group0_voffset_supported(uint64_t idx)
+{
+	switch (idx) {
+	case 0U:
+	case 2U:
+	case 3U:
+		return true;
+
+	case 1U:
+		return false;
+
+	default:
+		ERROR("AMU: can't set up virtual offset for unknown "
+		      "architected counter %llu!\n", idx);
+
+		panic();
+	}
+}
+
+/*
  * Read the group 0 offset register for a given index. Index must be 0, 2,
  * or 3, the register for 1 does not exist.
  *
  * Using this function requires FEAT_AMUv1p1 support.
  */
-uint64_t amu_group0_voffset_read(unsigned int idx)
+static uint64_t amu_group0_voffset_read(unsigned int idx)
 {
-	assert(amu_get_version() >= ID_AA64PFR0_AMU_V1P1);
-	assert(idx < AMU_GROUP0_NR_COUNTERS);
+	assert(amu_v1p1_supported());
+	assert(idx < read_amcgcr_el0_cg0nc());
 	assert(idx != 1U);
 
 	return amu_group0_voffset_read_internal(idx);
@@ -169,33 +370,33 @@
  *
  * Using this function requires FEAT_AMUv1p1 support.
  */
-void amu_group0_voffset_write(unsigned int idx, uint64_t val)
+static void amu_group0_voffset_write(unsigned int idx, uint64_t val)
 {
-	assert(amu_get_version() >= ID_AA64PFR0_AMU_V1P1);
-	assert(idx < AMU_GROUP0_NR_COUNTERS);
+	assert(amu_v1p1_supported());
+	assert(idx < read_amcgcr_el0_cg0nc());
 	assert(idx != 1U);
 
 	amu_group0_voffset_write_internal(idx, val);
 	isb();
 }
 
-#if AMU_GROUP1_NR_COUNTERS
+#if ENABLE_AMU_AUXILIARY_COUNTERS
 /* Read the group 1 counter identified by the given `idx` */
-uint64_t amu_group1_cnt_read(unsigned int idx)
+static uint64_t amu_group1_cnt_read(unsigned int idx)
 {
-	assert(amu_get_version() != ID_AA64PFR0_AMU_NOT_SUPPORTED);
+	assert(amu_supported());
 	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
+	assert(idx < read_amcgcr_el0_cg1nc());
 
 	return amu_group1_cnt_read_internal(idx);
 }
 
 /* Write the group 1 counter identified by the given `idx` with `val` */
-void amu_group1_cnt_write(unsigned int idx, uint64_t val)
+static void amu_group1_cnt_write(unsigned int idx, uint64_t val)
 {
-	assert(amu_get_version() != ID_AA64PFR0_AMU_NOT_SUPPORTED);
+	assert(amu_supported());
 	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
+	assert(idx < read_amcgcr_el0_cg1nc());
 
 	amu_group1_cnt_write_internal(idx, val);
 	isb();
@@ -206,13 +407,12 @@
  *
  * Using this function requires FEAT_AMUv1p1 support.
  */
-uint64_t amu_group1_voffset_read(unsigned int idx)
+static uint64_t amu_group1_voffset_read(unsigned int idx)
 {
-	assert(amu_get_version() >= ID_AA64PFR0_AMU_V1P1);
+	assert(amu_v1p1_supported());
 	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
-	assert(((read_amcg1idr_el0() >> AMCG1IDR_VOFF_SHIFT) &
-		(1ULL << idx)) != 0ULL);
+	assert(idx < read_amcgcr_el0_cg1nc());
+	assert((read_amcg1idr_el0_voff() & (UINT64_C(1) << idx)) != 0U);
 
 	return amu_group1_voffset_read_internal(idx);
 }
@@ -222,167 +422,211 @@
  *
  * Using this function requires FEAT_AMUv1p1 support.
  */
-void amu_group1_voffset_write(unsigned int idx, uint64_t val)
+static void amu_group1_voffset_write(unsigned int idx, uint64_t val)
 {
-	assert(amu_get_version() >= ID_AA64PFR0_AMU_V1P1);
+	assert(amu_v1p1_supported());
 	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
-	assert(((read_amcg1idr_el0() >> AMCG1IDR_VOFF_SHIFT) &
-		(1ULL << idx)) != 0ULL);
+	assert(idx < read_amcgcr_el0_cg1nc());
+	assert((read_amcg1idr_el0_voff() & (UINT64_C(1) << idx)) != 0U);
 
 	amu_group1_voffset_write_internal(idx, val);
 	isb();
 }
-
-/*
- * Program the event type register for the given `idx` with
- * the event number `val`
- */
-void amu_group1_set_evtype(unsigned int idx, unsigned int val)
-{
-	assert(amu_get_version() != ID_AA64PFR0_AMU_NOT_SUPPORTED);
-	assert(amu_group1_supported());
-	assert(idx < AMU_GROUP1_NR_COUNTERS);
-
-	amu_group1_set_evtype_internal(idx, val);
-	isb();
-}
-#endif	/* AMU_GROUP1_NR_COUNTERS */
+#endif
 
 static void *amu_context_save(const void *arg)
 {
-	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
-	unsigned int i;
+	uint64_t i, j;
 
-	if (amu_get_version() == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
-		return (void *)-1;
+	unsigned int core_pos;
+	struct amu_ctx *ctx;
+
+	uint64_t id_aa64pfr0_el1_amu;	/* AMU version */
+	uint64_t hcr_el2_amvoffen;	/* AMU virtual offsets enabled */
+	uint64_t amcgcr_el0_cg0nc;	/* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint64_t amcg1idr_el0_voff;	/* Auxiliary counters with virtual offsets */
+	uint64_t amcfgr_el0_ncg;	/* Number of counter groups */
+	uint64_t amcgcr_el0_cg1nc;	/* Number of group 1 counters */
+#endif
+
+	id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
+	if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+		return (void *)0;
 	}
 
-#if AMU_GROUP1_NR_COUNTERS
-	if (!amu_group1_supported()) {
-		return (void *)-1;
-	}
-#endif
-	/* Assert that group 0/1 counter configuration is what we expect */
-	assert(read_amcntenset0_el0() == AMU_GROUP0_COUNTERS_MASK);
+	core_pos = plat_my_core_pos();
+	ctx = &amu_ctxs_[core_pos];
 
-#if AMU_GROUP1_NR_COUNTERS
-	assert(read_amcntenset1_el0() == AMU_GROUP1_COUNTERS_MASK);
+	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
+	hcr_el2_amvoffen = (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) ?
+		read_hcr_el2_amvoffen() : 0U;
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
+	amcgcr_el0_cg1nc = (amcfgr_el0_ncg > 0U) ? read_amcgcr_el0_cg1nc() : 0U;
+	amcg1idr_el0_voff = (hcr_el2_amvoffen != 0U) ? read_amcg1idr_el0_voff() : 0U;
 #endif
+
 	/*
-	 * Disable group 0/1 counters to avoid other observers like SCP sampling
-	 * counter values from the future via the memory mapped view.
+	 * Disable all AMU counters.
 	 */
-	write_amcntenclr0_el0(AMU_GROUP0_COUNTERS_MASK);
 
-#if AMU_GROUP1_NR_COUNTERS
-	write_amcntenclr1_el0(AMU_GROUP1_COUNTERS_MASK);
+	ctx->group0_enable = read_amcntenset0_el0_px();
+	write_amcntenclr0_el0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	if (amcfgr_el0_ncg > 0U) {
+		ctx->group1_enable = read_amcntenset1_el0_px();
+		write_amcntenclr1_el0_px(ctx->group1_enable);
+	}
 #endif
-	isb();
 
-	/* Save all group 0 counters */
-	for (i = 0U; i < AMU_GROUP0_NR_COUNTERS; i++) {
+	/*
+	 * Save the counters to the local context.
+	 */
+
+	isb(); /* Ensure counters have been stopped */
+
+	for (i = 0U; i < amcgcr_el0_cg0nc; i++) {
 		ctx->group0_cnts[i] = amu_group0_cnt_read(i);
 	}
 
-	/* Save group 0 virtual offsets if supported and enabled. */
-	if ((amu_get_version() >= ID_AA64PFR0_AMU_V1P1) &&
-			((read_hcr_el2() & HCR_AMVOFFEN_BIT) != 0ULL)) {
-		/* Not using a loop because count is fixed and index 1 DNE. */
-		ctx->group0_voffsets[0U] = amu_group0_voffset_read(0U);
-		ctx->group0_voffsets[1U] = amu_group0_voffset_read(2U);
-		ctx->group0_voffsets[2U] = amu_group0_voffset_read(3U);
-	}
-
-#if AMU_GROUP1_NR_COUNTERS
-	/* Save group 1 counters */
-	for (i = 0U; i < AMU_GROUP1_NR_COUNTERS; i++) {
-		if ((AMU_GROUP1_COUNTERS_MASK & (1UL << i)) != 0U) {
-			ctx->group1_cnts[i] = amu_group1_cnt_read(i);
-		}
-	}
-
-	/* Save group 1 virtual offsets if supported and enabled. */
-	if ((amu_get_version() >= ID_AA64PFR0_AMU_V1P1) &&
-			((read_hcr_el2() & HCR_AMVOFFEN_BIT) != 0ULL)) {
-		u_register_t amcg1idr = read_amcg1idr_el0() >>
-			AMCG1IDR_VOFF_SHIFT;
-		amcg1idr = amcg1idr & AMU_GROUP1_COUNTERS_MASK;
-
-		for (i = 0U; i < AMU_GROUP1_NR_COUNTERS; i++) {
-			if (((amcg1idr >> i) & 1ULL) != 0ULL) {
-				ctx->group1_voffsets[i] =
-					amu_group1_voffset_read(i);
-			}
-		}
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	for (i = 0U; i < amcgcr_el0_cg1nc; i++) {
+		ctx->group1_cnts[i] = amu_group1_cnt_read(i);
 	}
 #endif
+
+	/*
+	 * Save virtual offsets for counters that offer them.
+	 */
+
+	if (hcr_el2_amvoffen != 0U) {
+		for (i = 0U, j = 0U; i < amcgcr_el0_cg0nc; i++) {
+			if (!amu_group0_voffset_supported(i)) {
+				continue; /* No virtual offset */
+			}
+
+			ctx->group0_voffsets[j++] = amu_group0_voffset_read(i);
+		}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+		for (i = 0U, j = 0U; i < amcgcr_el0_cg1nc; i++) {
+			if ((amcg1idr_el0_voff >> i) & 1U) {
+				continue; /* No virtual offset */
+			}
+
+			ctx->group1_voffsets[j++] = amu_group1_voffset_read(i);
+		}
+#endif
+	}
+
 	return (void *)0;
 }
 
 static void *amu_context_restore(const void *arg)
 {
-	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
-	unsigned int i;
+	uint64_t i, j;
 
-	if (amu_get_version() == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
-		return (void *)-1;
-	}
+	unsigned int core_pos;
+	struct amu_ctx *ctx;
 
-#if AMU_GROUP1_NR_COUNTERS
-	if (!amu_group1_supported()) {
-		return (void *)-1;
-	}
-#endif
-	/* Counters were disabled in `amu_context_save()` */
-	assert(read_amcntenset0_el0() == 0U);
+	uint64_t id_aa64pfr0_el1_amu;	/* AMU version */
 
-#if AMU_GROUP1_NR_COUNTERS
-	assert(read_amcntenset1_el0() == 0U);
+	uint64_t hcr_el2_amvoffen;	/* AMU virtual offsets enabled */
+
+	uint64_t amcfgr_el0_ncg;	/* Number of counter groups */
+	uint64_t amcgcr_el0_cg0nc;	/* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	uint64_t amcgcr_el0_cg1nc;	/* Number of group 1 counters */
+	uint64_t amcg1idr_el0_voff;	/* Auxiliary counters with virtual offsets */
 #endif
 
-	/* Restore all group 0 counters */
-	for (i = 0U; i < AMU_GROUP0_NR_COUNTERS; i++) {
+	id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
+	if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+		return (void *)0;
+	}
+
+	core_pos = plat_my_core_pos();
+	ctx = &amu_ctxs_[core_pos];
+
+	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
+	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
+
+	hcr_el2_amvoffen = (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) ?
+		read_hcr_el2_amvoffen() : 0U;
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	amcgcr_el0_cg1nc = (amcfgr_el0_ncg > 0U) ? read_amcgcr_el0_cg1nc() : 0U;
+	amcg1idr_el0_voff = (hcr_el2_amvoffen != 0U) ? read_amcg1idr_el0_voff() : 0U;
+#endif
+
+	/*
+	 * Sanity check that all counters were disabled when the context was
+	 * previously saved.
+	 */
+
+	assert(read_amcntenset0_el0_px() == 0U);
+
+	if (amcfgr_el0_ncg > 0U) {
+		assert(read_amcntenset1_el0_px() == 0U);
+	}
+
+	/*
+	 * Restore the counter values from the local context.
+	 */
+
+	for (i = 0U; i < amcgcr_el0_cg0nc; i++) {
 		amu_group0_cnt_write(i, ctx->group0_cnts[i]);
 	}
 
-	/* Restore group 0 virtual offsets if supported and enabled. */
-	if ((amu_get_version() >= ID_AA64PFR0_AMU_V1P1) &&
-			((read_hcr_el2() & HCR_AMVOFFEN_BIT) != 0ULL)) {
-		/* Not using a loop because count is fixed and index 1 DNE. */
-		amu_group0_voffset_write(0U, ctx->group0_voffsets[0U]);
-		amu_group0_voffset_write(2U, ctx->group0_voffsets[1U]);
-		amu_group0_voffset_write(3U, ctx->group0_voffsets[2U]);
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	for (i = 0U; i < amcgcr_el0_cg1nc; i++) {
+		amu_group1_cnt_write(i, ctx->group1_cnts[i]);
 	}
+#endif
 
-	/* Restore group 0 counter configuration */
-	write_amcntenset0_el0(AMU_GROUP0_COUNTERS_MASK);
+	/*
+	 * Restore virtual offsets for counters that offer them.
+	 */
 
-#if AMU_GROUP1_NR_COUNTERS
-	/* Restore group 1 counters */
-	for (i = 0U; i < AMU_GROUP1_NR_COUNTERS; i++) {
-		if ((AMU_GROUP1_COUNTERS_MASK & (1UL << i)) != 0U) {
-			amu_group1_cnt_write(i, ctx->group1_cnts[i]);
-		}
-	}
-
-	/* Restore group 1 virtual offsets if supported and enabled. */
-	if ((amu_get_version() >= ID_AA64PFR0_AMU_V1P1) &&
-			((read_hcr_el2() & HCR_AMVOFFEN_BIT) != 0ULL)) {
-		u_register_t amcg1idr = read_amcg1idr_el0() >>
-			AMCG1IDR_VOFF_SHIFT;
-		amcg1idr = amcg1idr & AMU_GROUP1_COUNTERS_MASK;
-
-		for (i = 0U; i < AMU_GROUP1_NR_COUNTERS; i++) {
-			if (((amcg1idr >> i) & 1ULL) != 0ULL) {
-				amu_group1_voffset_write(i,
-					ctx->group1_voffsets[i]);
+	if (hcr_el2_amvoffen != 0U) {
+		for (i = 0U, j = 0U; i < amcgcr_el0_cg0nc; i++) {
+			if (!amu_group0_voffset_supported(i)) {
+				continue; /* No virtual offset */
 			}
+
+			amu_group0_voffset_write(i, ctx->group0_voffsets[j++]);
 		}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+		for (i = 0U, j = 0U; i < amcgcr_el0_cg1nc; i++) {
+			if ((amcg1idr_el0_voff >> i) & 1U) {
+				continue; /* No virtual offset */
+			}
+
+			amu_group1_voffset_write(i, ctx->group1_voffsets[j++]);
+		}
+#endif
 	}
 
-	/* Restore group 1 counter configuration */
-	write_amcntenset1_el0(AMU_GROUP1_COUNTERS_MASK);
+	/*
+	 * Re-enable counters that were disabled during context save.
+	 */
+
+	write_amcntenset0_el0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+	if (amcfgr_el0_ncg > 0) {
+		write_amcntenset1_el0_px(ctx->group1_enable);
+	}
+#endif
+
+#if ENABLE_MPMM
+	mpmm_enable();
 #endif
 
 	return (void *)0;
diff --git a/lib/extensions/amu/aarch64/amu_helpers.S b/lib/extensions/amu/aarch64/amu_helpers.S
index 9989abd..0f6d799 100644
--- a/lib/extensions/amu/aarch64/amu_helpers.S
+++ b/lib/extensions/amu/aarch64/amu_helpers.S
@@ -83,6 +83,7 @@
 	write	AMEVCNTR03_EL0		/* index 3 */
 endfunc amu_group0_cnt_write_internal
 
+#if ENABLE_AMU_AUXILIARY_COUNTERS
 /*
  * uint64_t amu_group1_cnt_read_internal(int idx);
  *
@@ -217,6 +218,7 @@
 	write	AMEVTYPER1E_EL0		/* index 14 */
 	write	AMEVTYPER1F_EL0		/* index 15 */
 endfunc amu_group1_set_evtype_internal
+#endif
 
 /*
  * Accessor functions for virtual offset registers added with FEAT_AMUv1p1
@@ -297,6 +299,7 @@
 	write	AMEVCNTVOFF03_EL2	/* index 3 */
 endfunc amu_group0_voffset_write_internal
 
+#if ENABLE_AMU_AUXILIARY_COUNTERS
 /*
  * uint64_t amu_group1_voffset_read_internal(int idx);
  *
@@ -383,3 +386,4 @@
 	write	AMEVCNTVOFF1E_EL2	/* index 14 */
 	write	AMEVCNTVOFF1F_EL2	/* index 15 */
 endfunc amu_group1_voffset_write_internal
+#endif
diff --git a/lib/extensions/amu/amu.mk b/lib/extensions/amu/amu.mk
new file mode 100644
index 0000000..0d203cb
--- /dev/null
+++ b/lib/extensions/amu/amu.mk
@@ -0,0 +1,24 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include lib/fconf/fconf.mk
+
+AMU_SOURCES	:=	lib/extensions/amu/${ARCH}/amu.c \
+			lib/extensions/amu/${ARCH}/amu_helpers.S
+
+ifneq (${ENABLE_AMU_AUXILIARY_COUNTERS},0)
+        ifeq (${ENABLE_AMU},0)
+                $(error AMU auxiliary counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`) requires AMU support (`ENABLE_AMU`))
+        endif
+endif
+
+ifneq (${ENABLE_AMU_FCONF},0)
+        ifeq (${ENABLE_AMU_AUXILIARY_COUNTERS},0)
+                $(error AMU FCONF support (`ENABLE_AMU_FCONF`) is not necessary when auxiliary counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`) is disabled)
+        endif
+
+        AMU_SOURCES	+=	${FCONF_AMU_SOURCES}
+endif
diff --git a/include/lib/extensions/amu_private.h b/lib/extensions/amu/amu_private.h
similarity index 78%
rename from include/lib/extensions/amu_private.h
rename to lib/extensions/amu/amu_private.h
index 3b4b47c..eb7ff0e 100644
--- a/include/lib/extensions/amu_private.h
+++ b/lib/extensions/amu/amu_private.h
@@ -9,6 +9,17 @@
 
 #include <stdint.h>
 
+#include <lib/cassert.h>
+#include <lib/extensions/amu.h>
+#include <lib/utils_def.h>
+
+#include <platform_def.h>
+
+#define AMU_GROUP0_MAX_COUNTERS		U(16)
+#define AMU_GROUP1_MAX_COUNTERS		U(16)
+
+#define AMU_AMCGCR_CG0NC_MAX		U(16)
+
 uint64_t amu_group0_cnt_read_internal(unsigned int idx);
 void amu_group0_cnt_write_internal(unsigned int idx, uint64_t val);
 
diff --git a/lib/fconf/fconf.mk b/lib/fconf/fconf.mk
index b01dc6f..fb88910 100644
--- a/lib/fconf/fconf.mk
+++ b/lib/fconf/fconf.mk
@@ -1,12 +1,19 @@
 #
-# Copyright (c) 2019-2020, ARM Limited. All rights reserved.
+# Copyright (c) 2019-2021, ARM Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-# Add Firmware Configuration files
-FCONF_SOURCES		:=	lib/fconf/fconf.c
-FCONF_DYN_SOURCES	:=	lib/fconf/fconf_dyn_cfg_getter.c
+include common/fdt_wrappers.mk
 
-BL1_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
-BL2_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+FCONF_SOURCES		:=	lib/fconf/fconf.c
+FCONF_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
+FCONF_DYN_SOURCES	:=	lib/fconf/fconf_dyn_cfg_getter.c
+FCONF_DYN_SOURCES	+=	${FDT_WRAPPERS_SOURCES}
+
+FCONF_AMU_SOURCES	:=	lib/fconf/fconf_amu_getter.c
+FCONF_AMU_SOURCES	+=	${FDT_WRAPPERS_SOURCES}
+
+FCONF_MPMM_SOURCES	:=	lib/fconf/fconf_mpmm_getter.c
+FCONF_MPMM_SOURCES	+=	${FDT_WRAPPERS_SOURCES}
diff --git a/lib/fconf/fconf_amu_getter.c b/lib/fconf/fconf_amu_getter.c
new file mode 100644
index 0000000..eff309c
--- /dev/null
+++ b/lib/fconf/fconf_amu_getter.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_amu_getter.h>
+#include <libfdt.h>
+
+#include <plat/common/platform.h>
+
+struct fconf_amu_config fconf_amu_config;
+static struct amu_topology fconf_amu_topology_;
+
+/*
+ * Populate the core-specific AMU structure with information retrieved from a
+ * device tree.
+ *
+ * Returns `0` on success, or a negative integer representing an error code.
+ */
+static int fconf_populate_amu_cpu_amu(const void *fdt, int parent,
+				      struct amu_core *amu)
+{
+	int ret = 0;
+	int node = 0;
+
+	fdt_for_each_subnode(node, fdt, parent) {
+		const char *name;
+		const char *value;
+		int len;
+
+		uintptr_t idx = 0U;
+
+		name = fdt_get_name(fdt, node, &len);
+		if (strncmp(name, "counter@", 8) != 0) {
+			continue;
+		}
+
+		ret = fdt_get_reg_props_by_index(fdt, node, 0, &idx, NULL);
+		if (ret < 0) {
+			break;
+		}
+
+		value = fdt_getprop(fdt, node, "enable-at-el3", &len);
+		if ((value == NULL) && (len != -FDT_ERR_NOTFOUND)) {
+			break;
+		}
+
+		if (len != -FDT_ERR_NOTFOUND) {
+			amu->enable |= (1 << idx);
+		}
+	}
+
+	if ((node < 0) && (node != -FDT_ERR_NOTFOUND)) {
+		return node;
+	}
+
+	return ret;
+}
+
+/*
+ * Within a `cpu` node, attempt to dereference the `amu` property, and populate
+ * the AMU information for the core.
+ *
+ * Returns `0` on success, or a negative integer representing an error code.
+ */
+static int fconf_populate_amu_cpu(const void *fdt, int node, uintptr_t mpidr)
+{
+	int ret;
+	int idx;
+
+	uint32_t amu_phandle;
+	struct amu_core *amu;
+
+	ret = fdt_read_uint32(fdt, node, "amu", &amu_phandle);
+	if (ret < 0) {
+		if (ret == -FDT_ERR_NOTFOUND) {
+			ret = 0;
+		}
+
+		return ret;
+	}
+
+	node = fdt_node_offset_by_phandle(fdt, amu_phandle);
+	if (node < 0) {
+		return node;
+	}
+
+	idx = plat_core_pos_by_mpidr(mpidr);
+	if (idx < 0) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	amu = &fconf_amu_topology_.cores[idx];
+
+	return fconf_populate_amu_cpu_amu(fdt, node, amu);
+}
+
+/*
+ * Populates the global `amu_topology` structure based on what's described by
+ * the hardware configuration device tree blob.
+ *
+ * The device tree is expected to provide an `amu` property for each `cpu` node,
+ * like so:
+ *
+ *     cpu@0 {
+ *         amu = <&cpu0_amu>;
+ *     };
+ *
+ *     amus {
+ *         cpu0_amu: amu-0 {
+ *             counters {
+ *                 #address-cells = <2>;
+ *                 #size-cells = <0>;
+ *
+ *                 counter@x,y {
+ *                     reg = <x y>; // Group x, counter y
+ *                 };
+ *             };
+ *         };
+ *     };
+ */
+static int fconf_populate_amu(uintptr_t config)
+{
+	int ret = fdtw_for_each_cpu(
+		(const void *)config, fconf_populate_amu_cpu);
+	if (ret == 0) {
+		fconf_amu_config.topology = &fconf_amu_topology_;
+	} else {
+		ERROR("FCONF: failed to parse AMU information: %d\n", ret);
+	}
+
+	return ret;
+}
+
+FCONF_REGISTER_POPULATOR(HW_CONFIG, amu, fconf_populate_amu);
diff --git a/lib/fconf/fconf_mpmm_getter.c b/lib/fconf/fconf_mpmm_getter.c
new file mode 100644
index 0000000..02a566d
--- /dev/null
+++ b/lib/fconf/fconf_mpmm_getter.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_mpmm_getter.h>
+#include <libfdt.h>
+
+#include <plat/common/platform.h>
+
+struct fconf_mpmm_config fconf_mpmm_config;
+static struct mpmm_topology fconf_mpmm_topology;
+
+/*
+ * Within a `cpu` node, determine support for MPMM via the `supports-mpmm`
+ * property.
+ *
+ * Returns `0` on success, or a negative integer representing an error code.
+ */
+static int fconf_populate_mpmm_cpu(const void *fdt, int off, uintptr_t mpidr)
+{
+	int ret, len;
+
+	int core_pos;
+	struct mpmm_core *core;
+
+	core_pos = plat_core_pos_by_mpidr(mpidr);
+	if (core_pos < 0) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	core = &fconf_mpmm_topology.cores[core_pos];
+
+	fdt_getprop(fdt, off, "supports-mpmm", &len);
+	if (len >= 0) {
+		core->supported = true;
+		ret = 0;
+	} else {
+		core->supported = false;
+		ret = len;
+	}
+
+	return ret;
+}
+
+/*
+ * Populates the global `fconf_mpmm_config` structure based on what's described
+ * by the hardware configuration device tree blob.
+ *
+ * The device tree is expected to provide a `supports-mpmm` property for each
+ * `cpu` node, like so:
+ *
+ *     cpu@0 {
+ *       supports-mpmm;
+ *     };
+ *
+ * This property indicates whether the core implements MPMM, as we cannot detect
+ * support for it dynamically.
+ */
+static int fconf_populate_mpmm(uintptr_t config)
+{
+	int ret = fdtw_for_each_cpu(
+		(const void *)config, fconf_populate_mpmm_cpu);
+	if (ret == 0) {
+		fconf_mpmm_config.topology = &fconf_mpmm_topology;
+	} else {
+		ERROR("FCONF: failed to configure MPMM: %d\n", ret);
+	}
+
+	return ret;
+}
+
+FCONF_REGISTER_POPULATOR(HW_CONFIG, mpmm, fconf_populate_mpmm);
diff --git a/lib/fconf/fconf_tbbr_getter.c b/lib/fconf/fconf_tbbr_getter.c
index 9a20ced..6f043e6 100644
--- a/lib/fconf/fconf_tbbr_getter.c
+++ b/lib/fconf/fconf_tbbr_getter.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
  */
@@ -71,26 +71,13 @@
 	}
 	tbbr_dyn_config.mbedtls_heap_size = val32;
 
-#if MEASURED_BOOT
-	/* Retrieve BL2 hash data details from the DTB */
-	err = fdtw_read_bytes(dtb, node, "bl2_hash_data", TCG_DIGEST_SIZE,
-				&tbbr_dyn_config.bl2_hash_data);
-	if (err < 0) {
-		ERROR("FCONF: Read %s failed for '%s'\n",
-				"bytes", "bl2_hash_data");
-		return err;
-	}
-#endif
 	VERBOSE("%s%s%s %d\n", "FCONF: `tbbr.", "disable_auth",
 		"` cell found with value =", tbbr_dyn_config.disable_auth);
 	VERBOSE("%s%s%s %p\n", "FCONF: `tbbr.", "mbedtls_heap_addr",
 		"` cell found with value =", tbbr_dyn_config.mbedtls_heap_addr);
 	VERBOSE("%s%s%s %zu\n", "FCONF: `tbbr.", "mbedtls_heap_size",
 		"` cell found with value =", tbbr_dyn_config.mbedtls_heap_size);
-#if MEASURED_BOOT
-	VERBOSE("%s%s%s %p\n", "FCONF: `tbbr.", "bl2_hash_data",
-		"` array found at address =", tbbr_dyn_config.bl2_hash_data);
-#endif
+
 	return 0;
 }
 
diff --git a/lib/mpmm/mpmm.c b/lib/mpmm/mpmm.c
new file mode 100644
index 0000000..a66f2aa
--- /dev/null
+++ b/lib/mpmm/mpmm.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <common/debug.h>
+#include <lib/mpmm/mpmm.h>
+
+#include <plat/common/platform.h>
+
+#if ENABLE_MPMM_FCONF
+#	include <lib/fconf/fconf.h>
+#	include <lib/fconf/fconf_mpmm_getter.h>
+#endif
+
+static uint64_t read_cpuppmcr_el3_mpmmpinctl(void)
+{
+	return (read_cpuppmcr_el3() >> CPUPPMCR_EL3_MPMMPINCTL_SHIFT) &
+		CPUPPMCR_EL3_MPMMPINCTL_MASK;
+}
+
+static void write_cpumpmmcr_el3_mpmm_en(uint64_t mpmm_en)
+{
+	uint64_t value = read_cpumpmmcr_el3();
+
+	value &= ~(CPUMPMMCR_EL3_MPMM_EN_MASK << CPUMPMMCR_EL3_MPMM_EN_SHIFT);
+	value |= (mpmm_en & CPUMPMMCR_EL3_MPMM_EN_MASK) <<
+		CPUMPMMCR_EL3_MPMM_EN_SHIFT;
+
+	write_cpumpmmcr_el3(value);
+}
+
+static bool mpmm_supported(void)
+{
+	bool supported = false;
+	const struct mpmm_topology *topology;
+
+#if ENABLE_MPMM_FCONF
+	topology = FCONF_GET_PROPERTY(mpmm, config, topology);
+#else
+	topology = plat_mpmm_topology();
+#endif /* ENABLE_MPMM_FCONF */
+
+	/*
+	 * For the current core firstly try to find out if the platform
+	 * configuration has claimed support for MPMM, then make sure that MPMM
+	 * is controllable through the system registers.
+	 */
+
+	if (topology != NULL) {
+		unsigned int core_pos = plat_my_core_pos();
+
+		supported = topology->cores[core_pos].supported &&
+			(read_cpuppmcr_el3_mpmmpinctl() == 0U);
+	} else {
+		ERROR("MPMM: failed to generate MPMM topology\n");
+	}
+
+	return supported;
+}
+
+void mpmm_enable(void)
+{
+	bool supported = mpmm_supported();
+
+	if (supported) {
+		write_cpumpmmcr_el3_mpmm_en(1U);
+	}
+}
diff --git a/lib/mpmm/mpmm.mk b/lib/mpmm/mpmm.mk
new file mode 100644
index 0000000..826f925
--- /dev/null
+++ b/lib/mpmm/mpmm.mk
@@ -0,0 +1,29 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include lib/extensions/amu/amu.mk
+include lib/fconf/fconf.mk
+
+ifneq (${ENABLE_MPMM},0)
+        ifneq ($(ARCH),aarch64)
+                $(error MPMM support (`ENABLE_MPMM`) can only be enabled in AArch64 images (`ARCH`))
+        endif
+
+        ifeq (${ENABLE_AMU_AUXILIARY_COUNTERS},0) # For MPMM gear AMU counters
+                $(error MPMM support (`ENABLE_MPM`) requires auxiliary AMU counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`))
+        endif
+endif
+
+MPMM_SOURCES	:=	lib/mpmm/mpmm.c
+MPMM_SOURCES	+=	${AMU_SOURCES}
+
+ifneq (${ENABLE_MPMM_FCONF},0)
+        ifeq (${ENABLE_MPMM},0)
+                $(error MPMM FCONF support (`ENABLE_MPMM_FCONF`) requires MPMM support (`ENABLE_MPMM`))
+        endif
+
+        MPMM_SOURCES	+= ${FCONF_MPMM_SOURCES}
+endif
diff --git a/lib/optee/optee_utils.c b/lib/optee/optee_utils.c
index d090b38..72979cd 100644
--- a/lib/optee/optee_utils.c
+++ b/lib/optee/optee_utils.c
@@ -82,11 +82,14 @@
 	init_size = image->size;
 
 	/*
-	 * -1 indicates loader decided address; take our pre-mapped area
-	 * for current image since arm-tf could not allocate memory dynamically
+	 * image->load_addr_hi & image->load_addr_lo set to UINT32_MAX indicate
+	 * loader decided address; take our pre-mapped area for current image
+	 * since arm-tf could not allocate memory dynamically
 	 */
-	if (init_load_addr == -1)
+	if ((image->load_addr_hi == UINT32_MAX) &&
+	    (image->load_addr_lo == UINT32_MAX)) {
 		init_load_addr = image_info->image_base;
+	}
 
 	/* Check that the default end address doesn't overflow */
 	if (check_uptr_overflow(image_info->image_base,
@@ -138,7 +141,8 @@
 
 {
 	optee_header_t *header;
-	int num, ret;
+	uint32_t num;
+	int ret;
 
 	assert(header_ep);
 	header = (optee_header_t *)header_ep->pc;
@@ -181,7 +185,7 @@
 	}
 
 	/* Parse OPTEE image */
-	for (num = 0; num < header->nb_images; num++) {
+	for (num = 0U; num < header->nb_images; num++) {
 		if (header->optee_image_list[num].image_id ==
 				OPTEE_PAGER_IMAGE_ID) {
 			ret = parse_optee_image(pager_image_info,
diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c
index 9c37d63..3cb4f7e 100644
--- a/lib/psci/psci_setup.c
+++ b/lib/psci/psci_setup.c
@@ -250,7 +250,8 @@
 		psci_caps |=  define_psci_cap(PSCI_CPU_ON_AARCH64);
 	if ((psci_plat_pm_ops->pwr_domain_suspend != NULL) &&
 	    (psci_plat_pm_ops->pwr_domain_suspend_finish != NULL)) {
-		psci_caps |=  define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
+		if (psci_plat_pm_ops->validate_power_state != NULL)
+			psci_caps |=  define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
 		if (psci_plat_pm_ops->get_sys_suspend_power_state != NULL)
 			psci_caps |=  define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64);
 	}
diff --git a/lib/xlat_mpu/xlat_mpu_utils.c b/lib/xlat_mpu/xlat_mpu_utils.c
index f305632..5400875 100644
--- a/lib/xlat_mpu/xlat_mpu_utils.c
+++ b/lib/xlat_mpu/xlat_mpu_utils.c
@@ -22,13 +22,13 @@
 #warning "xlat_mpu library is currently experimental and its API may change in future."
 
 
-#if LOG_LEVEL < LOG_LEVEL_VERBOSE
-
 void xlat_mmap_print(__unused const mmap_region_t *mmap)
 {
 	/* Empty */
 }
 
+#if LOG_LEVEL < LOG_LEVEL_VERBOSE
+
 void xlat_tables_print(__unused xlat_ctx_t *ctx)
 {
 	/* Empty */
@@ -36,36 +36,48 @@
 
 #else /* if LOG_LEVEL >= LOG_LEVEL_VERBOSE */
 
-static const char *invalid_descriptors_ommited =
-		"%s(%d invalid descriptors omitted)\n";
-
-void xlat_tables_print(xlat_ctx_t *ctx)
+static void xlat_tables_print_internal(__unused xlat_ctx_t *ctx)
 {
-	const char *xlat_regime_str;
-	int used_page_tables;
+	int region_to_use = 0;
+	uintptr_t region_base;
+	size_t region_size;
+	uint64_t prenr_el2_value = 0U;
 
-	if (ctx->xlat_regime == EL1_EL0_REGIME) {
-		xlat_regime_str = "1&0";
-	} else if (ctx->xlat_regime == EL2_REGIME) {
-		xlat_regime_str = "2";
-	} else {
-		assert(ctx->xlat_regime == EL3_REGIME);
-		xlat_regime_str = "3";
-		/* If no EL3 and EL3 tables generated, then need to know. */
+	/*
+	 * Keep track of how many invalid descriptors are counted in a row.
+	 * Whenever multiple invalid descriptors are found, only the first one
+	 * is printed, and a line is added to inform about how many descriptors
+	 * have been omitted.
+	 */
+
+	/*
+	 * TODO:  Remove this WARN() and comment when these API calls are more
+	 *        completely implemented and tested!
+	 */
+	WARN("%s in this early version of xlat_mpu library may not produce reliable results!",
+	     __func__);
+
+	/*
+	 * Sequence through all regions and print those in-use (PRENR has an
+	 * enable bit for each MPU region, 1 for in-use or 0 for unused):
+	 */
+	prenr_el2_value = read_prenr_el2();
+	for (region_to_use = 0;  region_to_use < N_MPU_REGIONS;
+	     region_to_use++) {
+		if (((prenr_el2_value >> region_to_use) & 1U) == 0U) {
+			continue;
+		}
+		region_base = read_prbar_el2() & PRBAR_PRLAR_ADDR_MASK;
+		region_size = read_prlar_el2() & PRBAR_PRLAR_ADDR_MASK;
+		printf("Address:  0x%llx, size:  0x%llx ",
+			(long long) region_base,
+			(long long) region_size);
 	}
-	VERBOSE("Translation tables state:\n");
-	VERBOSE("  Xlat regime:     EL%s\n", xlat_regime_str);
-	VERBOSE("  Max allowed PA:  0x%llx\n", ctx->pa_max_address);
-	VERBOSE("  Max allowed VA:  0x%lx\n", ctx->va_max_address);
-	VERBOSE("  Max mapped PA:   0x%llx\n", ctx->max_pa);
-	VERBOSE("  Max mapped VA:   0x%lx\n", ctx->max_va);
+}
 
-	VERBOSE("  Initial lookup level: %u\n", ctx->base_level);
-	VERBOSE("  Entries @initial lookup level: %u\n",
-		ctx->base_table_entries);
-
-	xlat_tables_print_internal(ctx, 0U, ctx->base_table,
-				   ctx->base_table_entries, ctx->base_level);
+void xlat_tables_print(__unused xlat_ctx_t *ctx)
+{
+	xlat_tables_print_internal(ctx);
 }
 
 #endif /* LOG_LEVEL >= LOG_LEVEL_VERBOSE */
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 819c536..45f5fa8 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -96,6 +96,12 @@
 # Build option to enable MPAM for lower ELs
 ENABLE_MPAM_FOR_LOWER_ELS	:= 0
 
+# Enable the Maximum Power Mitigation Mechanism on supporting cores.
+ENABLE_MPMM			:= 0
+
+# Enable MPMM configuration via FCONF.
+ENABLE_MPMM_FCONF		:= 0
+
 # Flag to Enable Position Independant support (PIE)
 ENABLE_PIE			:= 0
 
@@ -306,6 +312,8 @@
 CTX_INCLUDE_MTE_REGS		:= 0
 
 ENABLE_AMU			:= 0
+ENABLE_AMU_AUXILIARY_COUNTERS	:= 0
+ENABLE_AMU_FCONF		:= 0
 AMU_RESTRICT_COUNTERS		:= 0
 
 # By default, enable Scalable Vector Extension if implemented only for Non-secure
diff --git a/plat/arm/board/a5ds/platform.mk b/plat/arm/board/a5ds/platform.mk
index 8b0dc5c..4f87306 100644
--- a/plat/arm/board/a5ds/platform.mk
+++ b/plat/arm/board/a5ds/platform.mk
@@ -1,18 +1,23 @@
 #
-# Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+# Copyright (c) 2019-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
 # Firmware Configuration Framework sources
+include common/fdt_wrappers.mk
 include lib/fconf/fconf.mk
 
+BL1_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+BL2_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+
 # Add `libfdt` and Arm common helpers required for Dynamic Config
 include lib/libfdt/libfdt.mk
 
 DYN_CFG_SOURCES		+=	plat/arm/common/arm_dyn_cfg.c		\
-				plat/arm/common/arm_dyn_cfg_helpers.c	\
-				common/fdt_wrappers.c
+				plat/arm/common/arm_dyn_cfg_helpers.c
+
+DYN_CFG_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
 
 # Include GICv2 driver files
 include drivers/arm/gic/v2/gicv2.mk
diff --git a/plat/arm/board/arm_fpga/platform.mk b/plat/arm/board/arm_fpga/platform.mk
index 901fabf..084532c 100644
--- a/plat/arm/board/arm_fpga/platform.mk
+++ b/plat/arm/board/arm_fpga/platform.mk
@@ -4,6 +4,7 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
 include lib/libfdt/libfdt.mk
 
 RESET_TO_BL31 := 1
@@ -104,8 +105,7 @@
 
 PLAT_BL_COMMON_SOURCES	:=	plat/arm/board/arm_fpga/${ARCH}/fpga_helpers.S
 
-BL31_SOURCES		+=	common/fdt_wrappers.c				\
-				common/fdt_fixup.c				\
+BL31_SOURCES		+=	common/fdt_fixup.c				\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
 				drivers/arm/pl011/${ARCH}/pl011_console.S	\
@@ -117,6 +117,8 @@
 				${FPGA_CPU_LIBS}				\
 				${FPGA_GIC_SOURCES}
 
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 $(eval $(call MAKE_S,$(BUILD_PLAT),plat/arm/board/arm_fpga/rom_trampoline.S,bl31))
 $(eval $(call MAKE_S,$(BUILD_PLAT),plat/arm/board/arm_fpga/kernel_trampoline.S,bl31))
 $(eval $(call MAKE_LD,$(BUILD_PLAT)/build_axf.ld,plat/arm/board/arm_fpga/build_axf.ld.S,bl31))
diff --git a/plat/arm/board/fvp/fdts/fvp_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
index cad888f..c26b519 100644
--- a/plat/arm/board/fvp/fdts/fvp_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
@@ -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
  */
@@ -45,12 +45,10 @@
 		};
 #endif
 
-#if !defined(SPD_spmd)
 		nt_fw-config {
 			load-address = <0x0 0x80000000>;
 			max-size = <0x200>;
 			id = <NT_FW_CONFIG_ID>;
 		};
-#endif
 	};
 };
diff --git a/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
index 08d3c32..cf4ef2d 100644
--- a/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
@@ -26,19 +26,6 @@
 		 */
 		mbedtls_heap_addr = <0x0 0x0>;
 		mbedtls_heap_size = <0x0>;
-
-#if MEASURED_BOOT
-		/* BL2 image hash calculated by BL1 */
-		bl2_hash_data = [
-			00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
-#if BL2_HASH_SIZE > 32
-			00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
-#if BL2_HASH_SIZE > 48
-			00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
-#endif /* > 48 */
-#endif /* > 32 */
-			00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00];
-#endif /* MEASURED_BOOT */
 	};
 
 	/*
@@ -117,6 +104,10 @@
 	#include "cot_descriptors.dtsi"
 #endif
 
+#if MEASURED_BOOT
+	#include "event_log.dtsi"
+#endif
+
 };
 
 #if COT_DESC_IN_DTB
diff --git a/plat/arm/board/fvp/fvp_bl1_measured_boot.c b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
new file mode 100644
index 0000000..47af1f5
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/arm/common/plat_arm.h>
+
+/* Event Log data */
+static uint8_t event_log[PLAT_ARM_EVENT_LOG_MAX_SIZE];
+
+/* FVP table with platform specific image IDs, names and PCRs */
+const event_log_metadata_t fvp_event_log_metadata[] = {
+	{ FW_CONFIG_ID, EVLOG_FW_CONFIG_STRING, PCR_0 },
+	{ TB_FW_CONFIG_ID, EVLOG_TB_FW_CONFIG_STRING, PCR_0 },
+	{ BL2_IMAGE_ID, EVLOG_BL2_STRING, PCR_0 },
+	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+};
+
+void bl1_plat_mboot_init(void)
+{
+	event_log_init(event_log, event_log + sizeof(event_log));
+	event_log_write_header();
+}
+
+void bl1_plat_mboot_finish(void)
+{
+	size_t event_log_cur_size;
+
+	event_log_cur_size = event_log_get_cur_size(event_log);
+	int rc = arm_set_tb_fw_info((uintptr_t)event_log,
+				    event_log_cur_size);
+	if (rc != 0) {
+		/*
+		 * It is a fatal error because on FVP platform, BL2 software
+		 * assumes that a valid Event Log buffer exist and it will use
+		 * same Event Log buffer to append image measurements.
+		 */
+		panic();
+	}
+}
diff --git a/plat/arm/board/fvp/fvp_bl1_setup.c b/plat/arm/board/fvp/fvp_bl1_setup.c
index 06ee037..59fc0f3 100644
--- a/plat/arm/board/fvp/fvp_bl1_setup.c
+++ b/plat/arm/board/fvp/fvp_bl1_setup.c
@@ -76,63 +76,6 @@
 		wfi();
 }
 
-#if MEASURED_BOOT
-/*
- * Calculates and writes BL2 hash data to TB_FW_CONFIG DTB.
- */
-void bl1_plat_set_bl2_hash(const image_desc_t *image_desc)
-{
-	arm_bl1_set_bl2_hash(image_desc);
-}
-
-/*
- * Implementation for bl1_plat_handle_post_image_load(). This function
- * populates the default arguments to BL2. The BL2 memory layout structure
- * is allocated and the calculated layout is populated in arg1 to BL2.
- */
-int bl1_plat_handle_post_image_load(unsigned int image_id)
-{
-	meminfo_t *bl2_tzram_layout;
-	meminfo_t *bl1_tzram_layout;
-	image_desc_t *image_desc;
-	entry_point_info_t *ep_info;
-
-	if (image_id != BL2_IMAGE_ID) {
-		return 0;
-	}
-
-	/* Get the image descriptor */
-	image_desc = bl1_plat_get_image_desc(BL2_IMAGE_ID);
-	assert(image_desc != NULL);
-
-	/* Calculate BL2 hash and set it in TB_FW_CONFIG */
-	bl1_plat_set_bl2_hash(image_desc);
-
-	/* Get the entry point info */
-	ep_info = &image_desc->ep_info;
-
-	/* Find out how much free trusted ram remains after BL1 load */
-	bl1_tzram_layout = bl1_plat_sec_mem_layout();
-
-	/*
-	 * Create a new layout of memory for BL2 as seen by BL1 i.e.
-	 * tell it the amount of total and free memory available.
-	 * This layout is created at the first free address visible
-	 * to BL2. BL2 will read the memory layout before using its
-	 * memory for other purposes.
-	 */
-	bl2_tzram_layout = (meminfo_t *)bl1_tzram_layout->total_base;
-
-	bl1_calc_bl2_mem_layout(bl1_tzram_layout, bl2_tzram_layout);
-
-	ep_info->args.arg1 = (uintptr_t)bl2_tzram_layout;
-
-	VERBOSE("BL1: BL2 memory layout address = %p\n",
-		(void *)bl2_tzram_layout);
-	return 0;
-}
-#endif /* MEASURED_BOOT */
-
 /*******************************************************************************
  * The following function checks if Firmware update is needed by checking error
  * reported in NV flag.
diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
new file mode 100644
index 0000000..5ebfede
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/arm/common/plat_arm.h>
+
+/* Event Log data */
+static uint64_t event_log_base;
+
+/* FVP table with platform specific image IDs, names and PCRs */
+const event_log_metadata_t fvp_event_log_metadata[] = {
+	{ BL31_IMAGE_ID, EVLOG_BL31_STRING, PCR_0 },
+	{ BL32_IMAGE_ID, EVLOG_BL32_STRING, PCR_0 },
+	{ BL32_EXTRA1_IMAGE_ID, EVLOG_BL32_EXTRA1_STRING, PCR_0 },
+	{ BL32_EXTRA2_IMAGE_ID, EVLOG_BL32_EXTRA2_STRING, PCR_0 },
+	{ BL33_IMAGE_ID, EVLOG_BL33_STRING, PCR_0 },
+	{ HW_CONFIG_ID, EVLOG_HW_CONFIG_STRING, PCR_0 },
+	{ NT_FW_CONFIG_ID, EVLOG_NT_FW_CONFIG_STRING, PCR_0 },
+	{ SCP_BL2_IMAGE_ID, EVLOG_SCP_BL2_STRING, PCR_0 },
+	{ SOC_FW_CONFIG_ID, EVLOG_SOC_FW_CONFIG_STRING, PCR_0 },
+	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
+	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+};
+
+void bl2_plat_mboot_init(void)
+{
+	uint8_t *event_log_start;
+	uint8_t *event_log_finish;
+	size_t bl1_event_log_size;
+	int rc;
+
+	rc = arm_get_tb_fw_info(&event_log_base, &bl1_event_log_size);
+	if (rc != 0) {
+		ERROR("%s(): Unable to get Event Log info from TB_FW_CONFIG\n",
+		      __func__);
+		/*
+		 * It is a fatal error because on FVP platform, BL2 software
+		 * assumes that a valid Event Log buffer exist and it will use
+		 * same Event Log buffer to append image measurements.
+		 */
+		panic();
+	}
+
+	/*
+	 * BL1 and BL2 share the same Event Log buffer and that BL2 will
+	 * append its measurements after BL1's
+	 */
+	event_log_start = (uint8_t *)((uintptr_t)event_log_base +
+				      bl1_event_log_size);
+	event_log_finish = (uint8_t *)((uintptr_t)event_log_base +
+				       PLAT_ARM_EVENT_LOG_MAX_SIZE);
+
+	event_log_init((uint8_t *)event_log_start, event_log_finish);
+}
+
+void bl2_plat_mboot_finish(void)
+{
+	int rc;
+
+	/* Event Log address in Non-Secure memory */
+	uintptr_t ns_log_addr;
+
+	/* Event Log filled size */
+	size_t event_log_cur_size;
+
+	event_log_cur_size = event_log_get_cur_size((uint8_t *)event_log_base);
+
+	rc = arm_set_nt_fw_info(
+#ifdef SPD_opteed
+			    (uintptr_t)event_log_base,
+#endif
+			    event_log_cur_size, &ns_log_addr);
+	if (rc != 0) {
+		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
+		      __func__, "NT");
+		/*
+		 * It is a fatal error because on FVP secure world software
+		 * assumes that a valid event log exists and will use it to
+		 * record the measurements into the fTPM.
+		 * Note: In FVP platform, OP-TEE uses nt_fw_config to get the
+		 * secure Event Log buffer address.
+		 */
+		panic();
+	}
+
+	/* Copy Event Log to Non-secure memory */
+	(void)memcpy((void *)ns_log_addr, (const void *)event_log_base,
+		     event_log_cur_size);
+
+	/* Ensure that the Event Log is visible in Non-secure memory */
+	flush_dcache_range(ns_log_addr, event_log_cur_size);
+
+#if defined(SPD_tspd) || defined(SPD_spmd)
+	/* Set Event Log data in TOS_FW_CONFIG */
+	rc = arm_set_tos_fw_info((uintptr_t)event_log_base,
+				 event_log_cur_size);
+	if (rc != 0) {
+		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
+		      __func__, "TOS");
+		panic();
+	}
+#endif /* defined(SPD_tspd) || defined(SPD_spmd) */
+
+	dump_event_log((uint8_t *)event_log_base, event_log_cur_size);
+}
diff --git a/plat/arm/board/fvp/fvp_bl2_setup.c b/plat/arm/board/fvp/fvp_bl2_setup.c
index f2f2143..5a17a0d 100644
--- a/plat/arm/board/fvp/fvp_bl2_setup.c
+++ b/plat/arm/board/fvp/fvp_bl2_setup.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
  */
@@ -9,9 +9,6 @@
 #include <common/debug.h>
 #include <common/desc_image_load.h>
 #include <drivers/arm/sp804_delay_timer.h>
-#if MEASURED_BOOT
-#include <drivers/measured_boot/measured_boot.h>
-#endif
 #include <lib/fconf/fconf.h>
 #include <lib/fconf/fconf_dyn_cfg_getter.h>
 
@@ -73,45 +70,3 @@
 
 	return arm_bl_params;
 }
-#if MEASURED_BOOT
-static int fvp_bl2_plat_handle_post_image_load(unsigned int image_id)
-{
-	const bl_mem_params_node_t *bl_mem_params =
-				get_bl_mem_params_node(image_id);
-
-	assert(bl_mem_params != NULL);
-
-	image_info_t info = bl_mem_params->image_info;
-	int err;
-
-	if ((info.h.attr & IMAGE_ATTRIB_SKIP_LOADING) == 0U) {
-		/* Calculate image hash and record data in Event Log */
-		err = tpm_record_measurement(info.image_base,
-					     info.image_size, image_id);
-		if (err != 0) {
-			ERROR("%s%s image id %u (%i)\n",
-				"BL2: Failed to ", "record", image_id, err);
-			return err;
-		}
-	}
-
-	err = arm_bl2_handle_post_image_load(image_id);
-	if (err != 0) {
-		ERROR("%s%s image id %u (%i)\n",
-			"BL2: Failed to ", "handle", image_id, err);
-	}
-
-	return err;
-}
-
-int arm_bl2_plat_handle_post_image_load(unsigned int image_id)
-{
-	int err = fvp_bl2_plat_handle_post_image_load(image_id);
-
-	if (err != 0) {
-		ERROR("%s() returns %i\n", __func__, err);
-	}
-
-	return err;
-}
-#endif	/* MEASURED_BOOT */
diff --git a/plat/arm/board/fvp/fvp_common_measured_boot.c b/plat/arm/board/fvp/fvp_common_measured_boot.c
new file mode 100644
index 0000000..6a403d9
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_common_measured_boot.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/desc_image_load.h>
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+
+extern event_log_metadata_t fvp_event_log_metadata[];
+
+const event_log_metadata_t *plat_event_log_get_metadata(void)
+{
+	return fvp_event_log_metadata;
+}
+
+int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
+{
+	/* Calculate image hash and record data in Event Log */
+	int err = event_log_measure_and_record(image_data->image_base,
+					       image_data->image_size,
+					       image_id);
+	if (err != 0) {
+		ERROR("%s%s image id %u (%i)\n",
+		      "Failed to ", "record", image_id, err);
+		return err;
+	}
+
+	return 0;
+}
diff --git a/plat/arm/board/fvp/fvp_measured_boot.c b/plat/arm/board/fvp/fvp_measured_boot.c
deleted file mode 100644
index 5dcadba..0000000
--- a/plat/arm/board/fvp/fvp_measured_boot.c
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <drivers/measured_boot/event_log.h>
-#include <plat/arm/common/plat_arm.h>
-
-/* FVP table with platform specific image IDs, names and PCRs */
-static const image_data_t fvp_images_data[] = {
-	{ BL2_IMAGE_ID, BL2_STRING, PCR_0 },		/* Reserved for BL2 */
-	{ BL31_IMAGE_ID, BL31_STRING, PCR_0 },
-	{ BL32_IMAGE_ID, BL32_STRING, PCR_0 },
-	{ BL32_EXTRA1_IMAGE_ID, BL32_EXTRA1_IMAGE_STRING, PCR_0 },
-	{ BL32_EXTRA2_IMAGE_ID, BL32_EXTRA2_IMAGE_STRING, PCR_0 },
-	{ BL33_IMAGE_ID, BL33_STRING, PCR_0 },
-	{ HW_CONFIG_ID, HW_CONFIG_STRING, PCR_0 },
-	{ NT_FW_CONFIG_ID, NT_FW_CONFIG_STRING, PCR_0 },
-	{ SCP_BL2_IMAGE_ID, SCP_BL2_IMAGE_STRING, PCR_0 },
-	{ SOC_FW_CONFIG_ID, SOC_FW_CONFIG_STRING, PCR_0 },
-	{ TOS_FW_CONFIG_ID, TOS_FW_CONFIG_STRING, PCR_0 },
-	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
-};
-
-static const measured_boot_data_t fvp_measured_boot_data = {
-	fvp_images_data,
-	arm_set_nt_fw_info,
-	arm_set_tos_fw_info
-};
-
-/*
- * Function retuns pointer to FVP plat_measured_boot_data_t structure
- */
-const measured_boot_data_t *plat_get_measured_boot_data(void)
-{
-	return &fvp_measured_boot_data;
-}
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 96574e5..d89e122 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -341,4 +341,9 @@
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
 #endif
 
+/*
+ * Maximum size of Event Log buffer used in Measured Boot Event Log driver
+ */
+#define	PLAT_ARM_EVENT_LOG_MAX_SIZE		UL(0x400)
+
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index b375146..0d2c319 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -4,6 +4,8 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 # Use the GICv3 driver on the FVP by default
 FVP_USE_GIC_DRIVER	:= FVP_GICV3
 
@@ -130,16 +132,17 @@
 					lib/cpus/aarch64/neoverse_n2.S		\
 					lib/cpus/aarch64/neoverse_e1.S		\
 					lib/cpus/aarch64/neoverse_v1.S		\
+					lib/cpus/aarch64/neoverse_demeter.S	\
 					lib/cpus/aarch64/cortex_a78_ae.S	\
 					lib/cpus/aarch64/cortex_a510.S		\
 					lib/cpus/aarch64/cortex_a710.S	\
 					lib/cpus/aarch64/cortex_makalu.S	\
 					lib/cpus/aarch64/cortex_makalu_elp_arm.S \
-					lib/cpus/aarch64/cortex_demeter.S	\
 					lib/cpus/aarch64/cortex_a65.S		\
 					lib/cpus/aarch64/cortex_a65ae.S		\
 					lib/cpus/aarch64/cortex_a78c.S		\
-					lib/cpus/aarch64/cortex_hayes.S
+					lib/cpus/aarch64/cortex_hayes.S		\
+					lib/cpus/aarch64/cortex_hunter.S
 	endif
 	# AArch64/AArch32 cores
 	FVP_CPU_LIBS	+=	lib/cpus/aarch64/cortex_a55.S		\
@@ -227,11 +230,12 @@
 # Support for fconf in BL31
 # Added separately from the above list for better readability
 ifeq ($(filter 1,${BL2_AT_EL3} ${RESET_TO_BL31}),)
-BL31_SOURCES		+=	common/fdt_wrappers.c				\
-				lib/fconf/fconf.c				\
+BL31_SOURCES		+=	lib/fconf/fconf.c				\
 				lib/fconf/fconf_dyn_cfg_getter.c		\
 				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c
 
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 ifeq (${SEC_INT_DESC_IN_FCONF},1)
 BL31_SOURCES		+=	plat/arm/common/fconf/fconf_sec_intr_config.c
 endif
@@ -376,7 +380,10 @@
 BL2_SOURCES		+=	plat/arm/board/fvp/fvp_trusted_boot.c
 
 ifeq (${MEASURED_BOOT},1)
-BL2_SOURCES		+=	plat/arm/board/fvp/fvp_measured_boot.c
+BL1_SOURCES		+=	plat/arm/board/fvp/fvp_common_measured_boot.c	\
+				plat/arm/board/fvp/fvp_bl1_measured_boot.c
+BL2_SOURCES		+=	plat/arm/board/fvp/fvp_common_measured_boot.c	\
+				plat/arm/board/fvp/fvp_bl2_measured_boot.c
 endif
 
 # FVP being a development platform, enable capability to disable Authentication
diff --git a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
index 64cb7ad..0d8cca5 100644
--- a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
+++ b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
@@ -1,9 +1,11 @@
 #
-# Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 # SP_MIN source files specific to FVP platform
 BL32_SOURCES		+=	drivers/arm/fvp/fvp_pwrc.c			\
 				drivers/cfi/v2m/v2m_flash.c			\
@@ -22,10 +24,11 @@
 # Support for fconf in SP_MIN(BL32)
 # Added separately from the above list for better readability
 ifeq ($(filter 1,${BL2_AT_EL3} ${RESET_TO_SP_MIN}),)
-BL32_SOURCES		+=	common/fdt_wrappers.c				\
-				lib/fconf/fconf.c				\
+BL32_SOURCES		+=	lib/fconf/fconf.c				\
 				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c
 
+BL32_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 ifeq (${SEC_INT_DESC_IN_FCONF},1)
 BL32_SOURCES		+=	plat/arm/common/fconf/fconf_sec_intr_config.c
 endif
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c b/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c
deleted file mode 100644
index c6544b4..0000000
--- a/plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-
-#include "../../../../bl1/bl1_private.h"
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <context.h>
-#include <lib/el3_runtime/context_mgmt.h>
-
-#include <plat/common/platform.h>
-
-
-void cm_prepare_el2_exit(void);
-
-/* Following contains the cpu context pointers. */
-static void *bl1_cpu_context_ptr[2];
-
-void *cm_get_context(uint32_t security_state)
-{
-	assert(sec_state_is_valid(security_state));
-	return bl1_cpu_context_ptr[security_state];
-}
-
-void cm_set_context(void *context, uint32_t security_state)
-{
-	assert(sec_state_is_valid(security_state));
-	bl1_cpu_context_ptr[security_state] = context;
-}
-
-/*******************************************************************************
- * This function prepares the context for Secure/Normal world images.
- * Normal world images are transitioned to EL2(if supported) else EL1.
- ******************************************************************************/
-void bl1_prepare_next_image(unsigned int image_id)
-{
-	/*
-	 * Following array will be used for context management.
-	 * There are 2 instances, for the Secure and Non-Secure contexts.
-	 */
-	static cpu_context_t bl1_cpu_context[2];
-
-	unsigned int security_state, mode = MODE_EL1;
-	image_desc_t *desc;
-	entry_point_info_t *next_bl_ep;
-
-#if CTX_INCLUDE_AARCH32_REGS
-	/*
-	 * Ensure that the build flag to save AArch32 system registers in CPU
-	 * context is not set for AArch64-only platforms.
-	 */
-	if (el_implemented(1) == EL_IMPL_A64ONLY) {
-		ERROR("EL1 supports AArch64-only. Please set build flag %s",
-				"CTX_INCLUDE_AARCH32_REGS = 0\n");
-		panic();
-	}
-#endif
-
-	/* Get the image descriptor. */
-	desc = bl1_plat_get_image_desc(image_id);
-	assert(desc != NULL);
-
-	/* Get the entry point info. */
-	next_bl_ep = &desc->ep_info;
-
-	/* Get the image security state. */
-	security_state = GET_SECURITY_STATE(next_bl_ep->h.attr);
-
-	/* Setup the Secure/Non-Secure context if not done already. */
-	if (cm_get_context(security_state) == NULL) {
-		cm_set_context(&bl1_cpu_context[security_state], security_state);
-	}
-	/* Prepare the SPSR for the next BL image. */
-	next_bl_ep->spsr = (uint32_t)SPSR_64((uint64_t) mode,
-		(uint64_t)MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
-
-	/* Allow platform to make change */
-	bl1_plat_set_ep_info(image_id, next_bl_ep);
-
-	/* Prepare context for the next EL */
-	cm_prepare_el2_exit();
-
-	/* Indicate that image is in execution state. */
-	desc->state = IMAGE_STATE_EXECUTED;
-
-	print_entry_point_info(next_bl_ep);
-}
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_entrypoint.S b/plat/arm/board/fvp_r/fvp_r_bl1_entrypoint.S
index 19a685c..15f4c43 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_entrypoint.S
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_entrypoint.S
@@ -43,14 +43,6 @@
 	 */
 	bl	bl1_setup
 
-#if ENABLE_PAUTH
-	/* --------------------------------------------------------------------
-	 * Program APIAKey_EL1 and enable pointer authentication.
-	 * --------------------------------------------------------------------
-	 */
-	bl	pauth_init_enable_el2
-#endif /* ENABLE_PAUTH */
-
 	/* --------------------------------------------------------------------
 	 * Initialize platform and jump to our c-entry point
 	 * for this type of reset.
@@ -85,15 +77,6 @@
 	 */
 	bl	clear_all_mpu_regions
 
-#if ENABLE_PAUTH
-	/* ---------------------------------------------
-	 * Disable pointer authentication before jumping
-	 * to next boot image.
-	 * ---------------------------------------------
-	 */
-	bl	pauth_disable_el2
-#endif /* ENABLE_PAUTH */
-
 	/* --------------------------------------------------
 	 * Do the transition to next boot image.
 	 * --------------------------------------------------
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_main.c b/plat/arm/board/fvp_r/fvp_r_bl1_main.c
index 2fd0e97..841a176 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_main.c
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_main.c
@@ -25,6 +25,8 @@
 #include <platform_def.h>
 
 
+void cm_prepare_el2_exit(void);
+
 void bl1_run_next_image(const struct entry_point_info *bl_ep_info);
 
 /*******************************************************************************
@@ -39,13 +41,6 @@
 	/* Get the image id of next image to load and run. */
 	image_id = bl1_plat_get_next_image_id();
 
-#if ENABLE_PAUTH
-	/*
-	 * Disable pointer authentication before running next boot image
-	 */
-	pauth_disable_el2();
-#endif /* ENABLE_PAUTH */
-
 #if !ARM_DISABLE_TRUSTED_WDOG
 	/* Disable watchdog before leaving BL1 */
 	plat_arm_secure_wdt_stop();
@@ -96,12 +91,6 @@
 	NOTICE("BL1: Booting BL33\n");
 }
 
-static void bl1_load_bl2(void);
-
-#if ENABLE_PAUTH
-uint64_t bl1_apiakey[2];
-#endif
-
 /*******************************************************************************
  * Helper utility to calculate the BL2 memory layout taking into consideration
  * the BL1 RW data assuming that it is at the top of the memory layout.
@@ -124,6 +113,53 @@
 }
 
 /*******************************************************************************
+ * This function prepares for entry to BL33
+ ******************************************************************************/
+void bl1_prepare_next_image(unsigned int image_id)
+{
+	unsigned int mode = MODE_EL1;
+	image_desc_t *desc;
+	entry_point_info_t *next_bl_ep;
+
+#if CTX_INCLUDE_AARCH32_REGS
+	/*
+	 * Ensure that the build flag to save AArch32 system registers in CPU
+	 * context is not set for AArch64-only platforms.
+	 */
+	if (el_implemented(1) == EL_IMPL_A64ONLY) {
+		ERROR("EL1 supports AArch64-only. Please set build flag %s",
+				"CTX_INCLUDE_AARCH32_REGS = 0\n");
+		panic();
+	}
+#endif
+
+	/* Get the image descriptor. */
+	desc = bl1_plat_get_image_desc(image_id);
+	assert(desc != NULL);
+
+	/* Get the entry point info. */
+	next_bl_ep = &desc->ep_info;
+
+	/* FVP-R is only secure */
+	assert(GET_SECURITY_STATE(next_bl_ep->h.attr) == SECURE);
+
+	/* Prepare the SPSR for the next BL image. */
+	next_bl_ep->spsr = (uint32_t)SPSR_64((uint64_t) mode,
+		(uint64_t)MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+
+	/* Allow platform to make change */
+	bl1_plat_set_ep_info(image_id, next_bl_ep);
+
+	/* Prepare context for the next EL */
+	cm_prepare_el2_exit();
+
+	/* Indicate that image is in execution state. */
+	desc->state = IMAGE_STATE_EXECUTED;
+
+	print_entry_point_info(next_bl_ep);
+}
+
+/*******************************************************************************
  * Setup function for BL1.
  ******************************************************************************/
 void bl1_setup(void)
@@ -133,14 +169,6 @@
 
 	/* Perform late platform-specific setup */
 	bl1_plat_arch_setup();
-
-#if CTX_INCLUDE_PAUTH_REGS
-	/*
-	 * Assert that the ARMv8.3-PAuth registers are present or an access
-	 * fault will be triggered when they are being saved or restored.
-	 */
-	assert(is_armv8_3_pauth_present());
-#endif /* CTX_INCLUDE_PAUTH_REGS */
 }
 
 /*******************************************************************************
@@ -199,12 +227,6 @@
 	/* Perform platform setup in BL1. */
 	bl1_platform_setup();
 
-#if ENABLE_PAUTH
-	/* Store APIAKey_EL1 key */
-	bl1_apiakey[0] = read_apiakeylo_el1();
-	bl1_apiakey[1] = read_apiakeyhi_el1();
-#endif /* ENABLE_PAUTH */
-
 	/* Get the image id of next image to load and run. */
 	image_id = bl1_plat_get_next_image_id();
 
@@ -212,9 +234,7 @@
 	 * We currently interpret any image id other than
 	 * BL2_IMAGE_ID as the start of firmware update.
 	 */
-	if (image_id == BL2_IMAGE_ID) {
-		bl1_load_bl2();
-	} else if (image_id == BL33_IMAGE_ID) {
+	if (image_id == BL33_IMAGE_ID) {
 		bl1_load_bl33();
 	} else {
 		NOTICE("BL1-FWU: *******FWU Process Started*******\n");
@@ -228,48 +248,6 @@
 }
 
 /*******************************************************************************
- * This function locates and loads the BL2 raw binary image in the trusted SRAM.
- * Called by the primary cpu after a cold boot.
- * TODO: Add support for alternative image load mechanism e.g using virtio/elf
- * loader etc.
- ******************************************************************************/
-static void bl1_load_bl2(void)
-{
-	image_desc_t *desc;
-	image_info_t *info;
-	int err;
-
-	/* Get the image descriptor */
-	desc = bl1_plat_get_image_desc(BL2_IMAGE_ID);
-	assert(desc != NULL);
-
-	/* Get the image info */
-	info = &desc->image_info;
-	INFO("BL1: Loading BL2\n");
-
-	err = bl1_plat_handle_pre_image_load(BL2_IMAGE_ID);
-	if (err != 0) {
-		ERROR("Failure in pre image load handling of BL2 (%d)\n", err);
-		plat_error_handler(err);
-	}
-
-	err = load_auth_image(BL2_IMAGE_ID, info);
-	if (err != 0) {
-		ERROR("Failed to load BL2 firmware.\n");
-		plat_error_handler(err);
-	}
-
-	/* Allow platform to handle image information. */
-	err = bl1_plat_handle_post_image_load(BL2_IMAGE_ID);
-	if (err != 0) {
-		ERROR("Failure in post image load handling of BL2 (%d)\n", err);
-		plat_error_handler(err);
-	}
-
-	NOTICE("BL1: Booting BL2\n");
-}
-
-/*******************************************************************************
  * Function called just before handing over to the next BL to inform the user
  * about the boot progress. In debug mode, also print details about the BL
  * image's execution context.
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
index 3f3fd5e..68872c1 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
@@ -140,7 +140,6 @@
  */
 void arm_bl1_platform_setup(void)
 {
-	image_desc_t *desc;
 	uint32_t fw_config_max_size;
 
 	/* Initialise the IO layer and register platform IO devices */
@@ -157,8 +156,7 @@
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
 	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
 
-	desc = bl1_plat_get_image_desc(BL33_IMAGE_ID);
-	assert(desc != NULL);
+	assert(bl1_plat_get_image_desc(BL33_IMAGE_ID) != NULL);
 
 	/*
 	 * Allow access to the System counter timer module and program
diff --git a/plat/arm/board/fvp_r/fvp_r_common.c b/plat/arm/board/fvp_r/fvp_r_common.c
index bce943d..edcf658 100644
--- a/plat/arm/board/fvp_r/fvp_r_common.c
+++ b/plat/arm/board/fvp_r/fvp_r_common.c
@@ -11,7 +11,6 @@
 #include <common/debug.h>
 
 #include <drivers/arm/cci.h>
-#include <drivers/arm/ccn.h>
 #include <drivers/arm/gicv2.h>
 #include <drivers/arm/sp804_delay_timer.h>
 #include <drivers/generic_delay_timer.h>
@@ -80,7 +79,6 @@
 
 ARM_CASSERT_MMAP
 
-#if FVP_R_INTERCONNECT_DRIVER != FVP_R_CCN
 static const int fvp_cci400_map[] = {
 	PLAT_FVP_R_CCI400_CLUS0_SL_PORT,
 	PLAT_FVP_R_CCI400_CLUS1_SL_PORT,
@@ -103,7 +101,6 @@
 	assert(master < FVP_R_CLUSTER_COUNT);
 	return master;
 }
-#endif
 
 /*******************************************************************************
  * Initialize the platform config for future decision making
@@ -198,14 +195,6 @@
 
 void __init fvp_interconnect_init(void)
 {
-#if FVP_R_INTERCONNECT_DRIVER == FVP_R_CCN
-	if (ccn_get_part0_id(PLAT_ARM_CCN_BASE) != CCN_502_PART0_ID) {
-		ERROR("Unrecognized CCN variant detected. Only CCN-502 is supported");
-		panic();
-	}
-
-	plat_arm_interconnect_init();
-#else
 	uintptr_t cci_base = 0U;
 	const int *cci_map = NULL;
 	unsigned int map_size = 0U;
@@ -226,14 +215,10 @@
 	assert(cci_base != 0U);
 	assert(cci_map != NULL);
 	cci_init(cci_base, cci_map, map_size);
-#endif
 }
 
 void fvp_interconnect_enable(void)
 {
-#if FVP_R_INTERCONNECT_DRIVER == FVP_R_CCN
-	plat_arm_interconnect_enter_coherency();
-#else
 	unsigned int master;
 
 	if ((arm_config.flags & (ARM_CONFIG_FVP_HAS_CCI400 |
@@ -241,14 +226,10 @@
 		master = get_interconnect_master();
 		cci_enable_snoop_dvm_reqs(master);
 	}
-#endif
 }
 
 void fvp_interconnect_disable(void)
 {
-#if FVP_R_INTERCONNECT_DRIVER == FVP_R_CCN
-	plat_arm_interconnect_exit_coherency();
-#else
 	unsigned int master;
 
 	if ((arm_config.flags & (ARM_CONFIG_FVP_HAS_CCI400 |
@@ -256,7 +237,6 @@
 		master = get_interconnect_master();
 		cci_disable_snoop_dvm_reqs(master);
 	}
-#endif
 }
 
 #if TRUSTED_BOARD_BOOT
diff --git a/plat/arm/board/fvp_r/fvp_r_context.S b/plat/arm/board/fvp_r/fvp_r_context.S
deleted file mode 100644
index 2746c2e..0000000
--- a/plat/arm/board/fvp_r/fvp_r_context.S
+++ /dev/null
@@ -1,17 +0,0 @@
-/*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <asm_macros.S>
-
-	.global	el2_exit
-
-/* ------------------------------------------------------------------
- * The mechanism, from el3_exit, is not used in this v8-R64 implementation.
- * ------------------------------------------------------------------
- */
-func el2_exit
-	exception_return
-endfunc el2_exit
diff --git a/plat/arm/board/fvp_r/fvp_r_debug.S b/plat/arm/board/fvp_r/fvp_r_debug.S
index 8db1b09..88f0a29 100644
--- a/plat/arm/board/fvp_r/fvp_r_debug.S
+++ b/plat/arm/board/fvp_r/fvp_r_debug.S
@@ -41,6 +41,7 @@
 
 _panic_handler:
 	/* Pass to plat_panic_handler the address from where el2_panic was
-	 * called, not the address of the call from el2_panic. */
+	 * called, not the address of the call from el2_panic.
+	 */
 	mov	x30, x6
 	b	plat_panic_handler
diff --git a/plat/arm/board/fvp_r/fvp_r_def.h b/plat/arm/board/fvp_r/fvp_r_def.h
index b9f6989..eda39cf 100644
--- a/plat/arm/board/fvp_r/fvp_r_def.h
+++ b/plat/arm/board/fvp_r/fvp_r_def.h
@@ -9,28 +9,18 @@
 
 #include <lib/utils_def.h>
 
-#ifndef FVP_R_CLUSTER_COUNT
-#error "FVP_R_CLUSTER_COUNT is not set in makefile"
-#endif
-
-#ifndef FVP_R_MAX_CPUS_PER_CLUSTER
-#error "FVP_R_MAX_CPUS_PER_CLUSTER is not set in makefile"
-#endif
-
-#ifndef FVP_R_MAX_PE_PER_CPU
-#error "FVP_R_MAX_PE_PER_CPU is not set in makefile"
-#endif
-
-#define FVP_R_PRIMARY_CPU			0x0
-
-/* Defines for the Interconnect build selection */
-#define FVP_R_CCI			1
-#define FVP_R_CCN			2
+/******************************************************************************
+ * FVP-R topology constants
+ *****************************************************************************/
+#define FVP_R_CLUSTER_COUNT		2
+#define FVP_R_MAX_CPUS_PER_CLUSTER	4
+#define FVP_R_MAX_PE_PER_CPU		1
+#define FVP_R_PRIMARY_CPU		0x0
 
 /******************************************************************************
  * Definition of platform soc id
  *****************************************************************************/
-#define FVP_R_SOC_ID      0
+#define FVP_R_SOC_ID			0
 
 /*******************************************************************************
  * FVP_R memory map related constants
diff --git a/plat/arm/board/fvp_r/fvp_r_helpers.S b/plat/arm/board/fvp_r/fvp_r_helpers.S
index f7a04d8..ba85777 100644
--- a/plat/arm/board/fvp_r/fvp_r_helpers.S
+++ b/plat/arm/board/fvp_r/fvp_r_helpers.S
@@ -16,7 +16,6 @@
 	.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);
@@ -127,40 +126,3 @@
 	cset	w0, eq
 	ret
 endfunc plat_is_my_cpu_primary
-
-	/* ---------------------------------------------------------------------
-	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
-	 *
-	 * Function to calculate the core position on FVP_R.
-	 *
-	 * (ClusterId * FVP_R_MAX_CPUS_PER_CLUSTER * FVP_R_MAX_PE_PER_CPU) +
-	 * (CPUId * FVP_R_MAX_PE_PER_CPU) +
-	 * ThreadId
-	 *
-	 * which can be simplified as:
-	 *
-	 * ((ClusterId * FVP_R_MAX_CPUS_PER_CLUSTER + CPUId) * FVP_R_MAX_PE_PER_CPU)
-	 * + ThreadId
-	 * ---------------------------------------------------------------------
-	 */
-func plat_arm_calc_core_pos
-	/*
-	 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it
-	 * look as if in a multi-threaded implementation.
-	 */
-	tst	x0, #MPIDR_MT_MASK
-	lsl	x3, x0, #MPIDR_AFFINITY_BITS
-	csel	x3, x3, x0, eq
-
-	/* Extract individual affinity fields from MPIDR */
-	ubfx	x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
-	ubfx	x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
-	ubfx	x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
-
-	/* Compute linear position */
-	mov	x4, #FVP_R_MAX_CPUS_PER_CLUSTER
-	madd	x1, x2, x4, x1
-	mov	x5, #FVP_R_MAX_PE_PER_CPU
-	madd	x0, x1, x5, x0
-	ret
-endfunc plat_arm_calc_core_pos
diff --git a/plat/arm/board/fvp_r/fvp_r_pauth_helpers.S b/plat/arm/board/fvp_r/fvp_r_pauth_helpers.S
deleted file mode 100644
index 7e6bc3d..0000000
--- a/plat/arm/board/fvp_r/fvp_r_pauth_helpers.S
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <lib/el3_runtime/cpu_data.h>
-
-	.global	pauth_init_enable_el2
-	.global	pauth_disable_el2
-
-/* -------------------------------------------------------------
- * File contains EL2 versions of EL3 funcs in:
- * 	.../lib/extensions/pauth/pauth_helpers.S
- * -------------------------------------------------------------
- */
-
-/* -------------------------------------------------------------
- * Program APIAKey_EL1 and enable pointer authentication in EL2
- * -------------------------------------------------------------
- */
-func pauth_init_enable_el2
-	stp	x29, x30, [sp, #-16]!
-
-	/* Initialize platform key */
-	bl	plat_init_apkey
-
-	/* Program instruction key A used by the Trusted Firmware */
-	msr	APIAKeyLo_EL1, x0
-	msr	APIAKeyHi_EL1, x1
-
-	/* Enable pointer authentication */
-	mrs	x0, sctlr_el2
-	orr	x0, x0, #SCTLR_EnIA_BIT
-
-#if ENABLE_BTI
-	 /* Enable PAC branch type compatibility */
-	bic	x0, x0, #SCTLR_BT_BIT
-#endif
-	msr	sctlr_el2, x0
-	isb
-
-	ldp	x29, x30, [sp], #16
-	ret
-endfunc pauth_init_enable_el2
-
-/* -------------------------------------------------------------
- * Disable pointer authentication in EL2
- * -------------------------------------------------------------
- */
-func pauth_disable_el2
-	mrs	x0, sctlr_el2
-	bic	x0, x0, #SCTLR_EnIA_BIT
-	msr	sctlr_el2, x0
-	isb
-	ret
-endfunc pauth_disable_el2
diff --git a/plat/arm/board/fvp_r/include/plat.ld.S b/plat/arm/board/fvp_r/include/plat.ld.S
deleted file mode 100644
index e91a5a0..0000000
--- a/plat/arm/board/fvp_r/include/plat.ld.S
+++ /dev/null
@@ -1,11 +0,0 @@
-/*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-#ifndef PLAT_LD_S
-#define PLAT_LD_S
-
-#include <plat/arm/common/arm_tzc_dram.ld.S>
-
-#endif /* PLAT_LD_S */
diff --git a/plat/arm/board/fvp_r/include/platform_def.h b/plat/arm/board/fvp_r/include/platform_def.h
index 4a6b441..ea3a258 100644
--- a/plat/arm/board/fvp_r/include/platform_def.h
+++ b/plat/arm/board/fvp_r/include/platform_def.h
@@ -210,10 +210,6 @@
 #define PLAT_FVP_R_CCI5XX_CLUS0_SL_PORT	5
 #define PLAT_FVP_R_CCI5XX_CLUS1_SL_PORT	6
 
-/* CCN related constants. Only CCN 502 is currently supported */
-#define PLAT_ARM_CCN_BASE		UL(0xae000000)
-#define PLAT_ARM_CLUSTER_TO_CCN_ID_MAP	1, 5, 7, 11
-
 /* System timer related constants */
 #define PLAT_ARM_NSTIMER_FRAME_ID	U(1)
 
diff --git a/plat/arm/board/fvp_r/platform.mk b/plat/arm/board/fvp_r/platform.mk
index 8f5878f..93b5cf2 100644
--- a/plat/arm/board/fvp_r/platform.mk
+++ b/plat/arm/board/fvp_r/platform.mk
@@ -15,47 +15,12 @@
 
 override CTX_INCLUDE_AARCH32_REGS	:=	0
 
-# Default cluster count for FVP_R
-FVP_R_CLUSTER_COUNT	:= 2
-
-# Default number of CPUs per cluster on FVP_R
-FVP_R_MAX_CPUS_PER_CLUSTER	:= 4
-
-# Default number of threads per CPU on FVP_R
-FVP_R_MAX_PE_PER_CPU	:= 1
-
 # Use MPU-based memory management:
 XLAT_MPU_LIB_V1		:=	1
 
-# Pass FVP_R_CLUSTER_COUNT to the build system.
-$(eval $(call add_define,FVP_R_CLUSTER_COUNT))
-
-# Pass FVP_R_MAX_CPUS_PER_CLUSTER to the build system.
-$(eval $(call add_define,FVP_R_MAX_CPUS_PER_CLUSTER))
-
-# Pass FVP_R_MAX_PE_PER_CPU to the build system.
-$(eval $(call add_define,FVP_R_MAX_PE_PER_CPU))
-
-# Sanity check the cluster count and if FVP_R_CLUSTER_COUNT <= 2,
-# choose the CCI driver , else the CCN driver
-ifeq ($(FVP_R_CLUSTER_COUNT), 0)
-$(error "Incorrect cluster count specified for FVP_R port")
-else ifeq ($(FVP_R_CLUSTER_COUNT),$(filter $(FVP_R_CLUSTER_COUNT),1 2))
-FVP_R_INTERCONNECT_DRIVER := FVP_R_CCI
-else
-FVP_R_INTERCONNECT_DRIVER := FVP_R_CCN
-endif
-
-$(eval $(call add_define,FVP_R_INTERCONNECT_DRIVER))
-
-ifeq (${FVP_R_INTERCONNECT_DRIVER}, FVP_R_CCI)
+# FVP R will not have more than 2 clusters so just use CCI interconnect
 FVP_R_INTERCONNECT_SOURCES	:= 	drivers/arm/cci/cci.c
-else ifeq (${FVP_R_INTERCONNECT_DRIVER}, FVP_R_CCN)
-FVP_R_INTERCONNECT_SOURCES	:= 	drivers/arm/ccn/ccn.c		\
-					plat/arm/common/arm_ccn.c
-else
-$(error "Incorrect CCN driver chosen on FVP_R port")
-endif
+
 
 include plat/arm/board/common/board_common.mk
 include plat/arm/common/arm_common.mk
@@ -64,17 +29,14 @@
 
 FVP_R_BL_COMMON_SOURCES	:=	plat/arm/board/fvp_r/fvp_r_common.c		\
 				plat/arm/board/fvp_r/fvp_r_context_mgmt.c	\
-				plat/arm/board/fvp_r/fvp_r_context.S		\
 				plat/arm/board/fvp_r/fvp_r_debug.S		\
 				plat/arm/board/fvp_r/fvp_r_err.c		\
 				plat/arm/board/fvp_r/fvp_r_helpers.S		\
-				plat/arm/board/fvp_r/fvp_r_misc_helpers.S	\
-				plat/arm/board/fvp_r/fvp_r_pauth_helpers.S
+				plat/arm/board/fvp_r/fvp_r_misc_helpers.S
 
 FVP_R_BL1_SOURCES	:=	plat/arm/board/fvp_r/fvp_r_bl1_arch_setup.c	\
 				plat/arm/board/fvp_r/fvp_r_bl1_setup.c		\
 				plat/arm/board/fvp_r/fvp_r_io_storage.c		\
-				plat/arm/board/fvp_r/fvp_r_bl1_context_mgmt.c	\
 				plat/arm/board/fvp_r/fvp_r_bl1_entrypoint.S	\
 				plat/arm/board/fvp_r/fvp_r_bl1_exceptions.S	\
 				plat/arm/board/fvp_r/fvp_r_bl1_main.c
@@ -82,17 +44,13 @@
 FVP_R_CPU_LIBS		:=	lib/cpus/${ARCH}/aem_generic.S
 
 FVP_R_DYNC_CFG_SOURCES	:=	common/fdt_wrappers.c				\
-				common/uuid.c					\
-				plat/arm/common/arm_dyn_cfg.c			\
-				plat/arm/common/arm_dyn_cfg_helpers.c
+				plat/arm/common/arm_dyn_cfg.c
 
 ifeq (${TRUSTED_BOARD_BOOT},1)
 FVP_R_AUTH_SOURCES	:=	drivers/auth/auth_mod.c				\
 				drivers/auth/crypto_mod.c			\
 				drivers/auth/img_parser_mod.c			\
 				lib/fconf/fconf_tbbr_getter.c			\
-				bl1/tbbr/tbbr_img_desc.c			\
-				plat/arm/common/arm_bl1_fwu.c			\
 				plat/common/tbbr/plat_tbbr.c			\
 				drivers/auth/tbbr/tbbr_cot_bl1_r64.c		\
 				drivers/auth/tbbr/tbbr_cot_common.c		\
@@ -125,11 +83,6 @@
 				drivers/io/io_storage.c				\
 				drivers/io/io_semihosting.c			\
 				lib/cpus/aarch64/cpu_helpers.S			\
-				lib/cpus/errata_report.c			\
-				lib/cpus/aarch64/dsu_helpers.S			\
-				lib/el3_runtime/aarch64/context.S		\
-				lib/el3_runtime/aarch64/context_mgmt.c		\
-				lib/fconf/fconf.c				\
 				lib/fconf/fconf_dyn_cfg_getter.c		\
 				lib/semihosting/semihosting.c			\
 				lib/semihosting/${ARCH}/semihosting_call.S	\
diff --git a/plat/arm/board/fvp_ve/platform.mk b/plat/arm/board/fvp_ve/platform.mk
index ac45d57..f7eace8 100644
--- a/plat/arm/board/fvp_ve/platform.mk
+++ b/plat/arm/board/fvp_ve/platform.mk
@@ -1,9 +1,11 @@
 #
-# Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+# Copyright (c) 2019-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 ifdef ARM_CORTEX_A5
 # Use the SP804 timer instead of the generic one
 USE_SP804_TIMER	:= 1
@@ -125,10 +127,13 @@
 # Firmware Configuration Framework sources
 include lib/fconf/fconf.mk
 
+BL1_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+BL2_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+
 # Add `libfdt` and Arm common helpers required for Dynamic Config
 include lib/libfdt/libfdt.mk
 
 DYN_CFG_SOURCES		+=	plat/arm/common/arm_dyn_cfg.c		\
-				plat/arm/common/arm_dyn_cfg_helpers.c	\
-				common/fdt_wrappers.c
+				plat/arm/common/arm_dyn_cfg_helpers.c
 
+DYN_CFG_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
diff --git a/plat/arm/board/juno/platform.mk b/plat/arm/board/juno/platform.mk
index 92fbf35..2c84eb3 100644
--- a/plat/arm/board/juno/platform.mk
+++ b/plat/arm/board/juno/platform.mk
@@ -4,6 +4,8 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 # Include GICv2 driver files
 include drivers/arm/gic/v2/gicv2.mk
 
@@ -83,7 +85,6 @@
 				lib/cpus/aarch64/cortex_a57.S		\
 				lib/cpus/aarch64/cortex_a72.S		\
 				lib/utils/mem_region.c			\
-				common/fdt_wrappers.c			\
 				lib/fconf/fconf.c			\
 				lib/fconf/fconf_dyn_cfg_getter.c	\
 				plat/arm/board/juno/juno_bl31_setup.c	\
@@ -94,6 +95,8 @@
 				${JUNO_INTERCONNECT_SOURCES}		\
 				${JUNO_SECURITY_SOURCES}
 
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 ifeq (${CSS_USE_SCMI_SDS_DRIVER},1)
 BL1_SOURCES		+=	drivers/arm/css/sds/sds.c
 endif
diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
index ccabced..745d91c 100644
--- a/plat/arm/board/tc/include/platform_def.h
+++ b/plat/arm/board/tc/include/platform_def.h
@@ -185,6 +185,7 @@
 
 #define PLAT_ARM_DRAM2_BASE		ULL(0x8080000000)
 #define PLAT_ARM_DRAM2_SIZE		ULL(0x180000000)
+#define PLAT_ARM_DRAM2_END		(PLAT_ARM_DRAM2_BASE + PLAT_ARM_DRAM2_SIZE - 1ULL)
 
 #define PLAT_ARM_G1S_IRQ_PROPS(grp)	CSS_G1S_IRQ_PROPS(grp)
 #define PLAT_ARM_G0_IRQ_PROPS(grp)	ARM_G0_IRQ_PROPS(grp)
@@ -260,13 +261,15 @@
 /*
  * 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.
+ * secure. The second and third regions gives non secure access to rest of DRAM.
  */
-#define TC_TZC_REGIONS_DEF						\
-	{TC_TZC_DRAM1_BASE, ARM_SCP_TZC_DRAM1_END,			\
-		TZC_REGION_S_RDWR, PLAT_ARM_TZC_NS_DEV_ACCESS},		\
-	{TC_NS_DRAM1_BASE, TC_NS_DRAM1_END, ARM_TZC_NS_DRAM_S_ACCESS, \
-		PLAT_ARM_TZC_NS_DEV_ACCESS}
+#define TC_TZC_REGIONS_DEF	\
+	{TC_TZC_DRAM1_BASE, ARM_SCP_TZC_DRAM1_END,	\
+		TZC_REGION_S_RDWR, PLAT_ARM_TZC_NS_DEV_ACCESS},	\
+	{TC_NS_DRAM1_BASE, TC_NS_DRAM1_END, ARM_TZC_NS_DRAM_S_ACCESS,	\
+		PLAT_ARM_TZC_NS_DEV_ACCESS},	\
+	{PLAT_ARM_DRAM2_BASE, PLAT_ARM_DRAM2_END,	\
+		ARM_TZC_NS_DRAM_S_ACCESS, PLAT_ARM_TZC_NS_DEV_ACCESS}
 
 /* virtual address used by dynamic mem_protect for chunk_base */
 #define PLAT_ARM_MEM_PROTEC_VA_FRAME	UL(0xc0000000)
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
index 7ebf639..8765fa2 100644
--- a/plat/arm/board/tc/platform.mk
+++ b/plat/arm/board/tc/platform.mk
@@ -3,6 +3,8 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 ifeq ($(filter ${TARGET_PLATFORM}, 0 1),)
         $(error TARGET_PLATFORM must be 0 or 1)
 endif
@@ -91,13 +93,14 @@
 				${ENT_GIC_SOURCES}			\
 				${TC_BASE}/tc_bl31_setup.c	\
 				${TC_BASE}/tc_topology.c	\
-				common/fdt_wrappers.c			\
 				lib/fconf/fconf.c			\
 				lib/fconf/fconf_dyn_cfg_getter.c	\
 				drivers/cfi/v2m/v2m_flash.c		\
 				lib/utils/mem_region.c			\
 				plat/arm/common/arm_nor_psci_mem_protect.c
 
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 # 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
@@ -137,6 +140,11 @@
 override ENABLE_SPE_FOR_LOWER_ELS	:= 0
 
 override ENABLE_AMU := 1
+override ENABLE_AMU_AUXILIARY_COUNTERS := 1
+override ENABLE_AMU_FCONF := 1
+
+override ENABLE_MPMM := 1
+override ENABLE_MPMM_FCONF := 1
 
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
diff --git a/plat/arm/common/arm_bl2_setup.c b/plat/arm/common/arm_bl2_setup.c
index 2871b1b..08c014d 100644
--- a/plat/arm/common/arm_bl2_setup.c
+++ b/plat/arm/common/arm_bl2_setup.c
@@ -48,9 +48,6 @@
 #pragma weak bl2_platform_setup
 #pragma weak bl2_plat_arch_setup
 #pragma weak bl2_plat_sec_mem_layout
-#if MEASURED_BOOT
-#pragma weak bl2_plat_get_hash
-#endif
 
 #if ENABLE_RME
 #define MAP_BL2_TOTAL		MAP_REGION_FLAT(			\
@@ -323,11 +320,3 @@
 {
 	return arm_bl2_plat_handle_post_image_load(image_id);
 }
-
-#if MEASURED_BOOT
-/* Read TCG_DIGEST_SIZE bytes of BL2 hash data */
-void bl2_plat_get_hash(void *data)
-{
-	arm_bl2_get_hash(data);
-}
-#endif
diff --git a/plat/arm/common/arm_bl31_setup.c b/plat/arm/common/arm_bl31_setup.c
index 6472590..a6f7df5 100644
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c
@@ -218,6 +218,10 @@
 	 * Linux kernel tree, Linux expects the physical address of the device
 	 * tree blob (DTB) in x0, while x1-x3 are reserved for future use and
 	 * must be 0.
+	 * Repurpose the option to load Hafnium hypervisor in the normal world.
+	 * It expects its manifest address in x0. This is essentially the linux
+	 * dts (passed to the primary VM) by adding 'hypervisor' and chosen
+	 * nodes specifying the Hypervisor configuration.
 	 */
 #if RESET_TO_BL31
 	bl33_image_ep_info.args.arg0 = (u_register_t)ARM_PRELOADED_DTB_BASE;
@@ -228,14 +232,6 @@
 	bl33_image_ep_info.args.arg2 = 0U;
 	bl33_image_ep_info.args.arg3 = 0U;
 # endif
-
-#if defined(SPD_spmd)
-	/*
-	 * Hafnium in normal world expects its manifest address in x0, In CI
-	 * configuration manifest is preloaded at 0x80000000(start of DRAM).
-	 */
-	bl33_image_ep_info.args.arg0 = (u_register_t)ARM_DRAM1_BASE;
-#endif
 }
 
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index f82392c..78efb0f 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -4,6 +4,8 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 ifeq (${ARCH}, aarch64)
   # On ARM standard platorms, the TSP can execute from Trusted SRAM, Trusted
   # DRAM (if available) or the TZC secured area of DRAM.
@@ -256,14 +258,18 @@
 # Firmware Configuration Framework sources
 include lib/fconf/fconf.mk
 
+BL1_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+BL2_SOURCES		+=	${FCONF_SOURCES} ${FCONF_DYN_SOURCES}
+
 # Add `libfdt` and Arm common helpers required for Dynamic Config
 include lib/libfdt/libfdt.mk
 
 DYN_CFG_SOURCES		+=	plat/arm/common/arm_dyn_cfg.c		\
 				plat/arm/common/arm_dyn_cfg_helpers.c	\
-				common/fdt_wrappers.c			\
 				common/uuid.c
 
+DYN_CFG_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 BL1_SOURCES		+=	${DYN_CFG_SOURCES}
 BL2_SOURCES		+=	${DYN_CFG_SOURCES}
 
@@ -343,10 +349,10 @@
 
 ifeq (${SPD},spmd)
 BL31_SOURCES		+=	plat/common/plat_spmd_manifest.c	\
-				common/fdt_wrappers.c			\
 				common/uuid.c				\
 				${LIBFDT_SRCS}
 
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
 endif
 
 ifneq (${TRUSTED_BOARD_BOOT},0)
@@ -406,7 +412,7 @@
 endif
 
 ifeq (${MEASURED_BOOT},1)
-    MEASURED_BOOT_MK := drivers/measured_boot/measured_boot.mk
+    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
     $(info Including ${MEASURED_BOOT_MK})
     include ${MEASURED_BOOT_MK}
 endif
diff --git a/plat/arm/common/arm_dyn_cfg.c b/plat/arm/common/arm_dyn_cfg.c
index 30473be..6aae9ae 100644
--- a/plat/arm/common/arm_dyn_cfg.c
+++ b/plat/arm/common/arm_dyn_cfg.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,10 +15,6 @@
 #include <common/tbbr/tbbr_img_def.h>
 #if TRUSTED_BOARD_BOOT
 #include <drivers/auth/mbedtls/mbedtls_config.h>
-#if MEASURED_BOOT
-#include <drivers/auth/crypto_mod.h>
-#include <mbedtls/md.h>
-#endif
 #endif
 #include <lib/fconf/fconf.h>
 #include <lib/fconf/fconf_dyn_cfg_getter.h>
@@ -115,82 +111,13 @@
 		 * images. It's critical because BL2 won't be able to proceed
 		 * without the heap info.
 		 *
-		 * In MEASURED_BOOT case flushing is done in
-		 * arm_bl1_set_bl2_hash() function which is called after heap
-		 * information is written in the DTB.
+		 * In MEASURED_BOOT case flushing is done in a function which
+		 * is called after heap information is written in the DTB.
 		 */
 		flush_dcache_range(tb_fw_cfg_dtb, fdt_totalsize(dtb));
 #endif /* !MEASURED_BOOT */
 	}
 }
-
-#if MEASURED_BOOT
-/*
- * Calculates and writes BL2 hash data to TB_FW_CONFIG DTB.
- * Executed only from BL1.
- */
-void arm_bl1_set_bl2_hash(const image_desc_t *image_desc)
-{
-	unsigned char hash_data[MBEDTLS_MD_MAX_SIZE];
-	const image_info_t image_info = image_desc->image_info;
-	uintptr_t tb_fw_cfg_dtb;
-	int err;
-	const struct dyn_cfg_dtb_info_t *tb_fw_config_info;
-
-	tb_fw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, TB_FW_CONFIG_ID);
-	assert(tb_fw_config_info != NULL);
-
-	tb_fw_cfg_dtb = tb_fw_config_info->config_addr;
-
-	/*
-	 * If tb_fw_cfg_dtb==NULL then DTB is not present for the current
-	 * platform. As such, we cannot write to the DTB at all and pass
-	 * measured data.
-	 */
-	if (tb_fw_cfg_dtb == 0UL) {
-		panic();
-	}
-
-	/* Calculate hash */
-	err = crypto_mod_calc_hash(MBEDTLS_MD_ID,
-					(void *)image_info.image_base,
-					image_info.image_size, hash_data);
-	if (err != 0) {
-		ERROR("%scalculate%s\n", "BL1: unable to ",
-						" BL2 hash");
-		panic();
-	}
-
-	err = arm_set_bl2_hash_info((void *)tb_fw_cfg_dtb, hash_data);
-	if (err < 0) {
-		ERROR("%swrite%sdata%s\n", "BL1: unable to ",
-					" BL2 hash ", "to DTB\n");
-		panic();
-	}
-
-	/*
-	 * Ensure that the info written to the DTB is visible to other
-	 * images. It's critical because BL2 won't be able to proceed
-	 * without the heap info and its hash data.
-	 */
-	flush_dcache_range(tb_fw_cfg_dtb, fdt_totalsize((void *)tb_fw_cfg_dtb));
-}
-
-/*
- * Reads TCG_DIGEST_SIZE bytes of BL2 hash data from the DTB.
- * Executed only from BL2.
- */
-void arm_bl2_get_hash(void *data)
-{
-	const void *bl2_hash;
-
-	assert(data != NULL);
-
-	/* Retrieve TCG_DIGEST_SIZE bytes of BL2 hash data from the DTB */
-	bl2_hash = FCONF_GET_PROPERTY(tbbr, dyn_config, bl2_hash_data);
-	(void)memcpy(data, bl2_hash, TCG_DIGEST_SIZE);
-}
-#endif /* MEASURED_BOOT */
 #endif /* TRUSTED_BOARD_BOOT */
 
 /*
diff --git a/plat/arm/common/arm_dyn_cfg_helpers.c b/plat/arm/common/arm_dyn_cfg_helpers.c
index 5f20c8d..6a2a6f8 100644
--- a/plat/arm/common/arm_dyn_cfg_helpers.c
+++ b/plat/arm/common/arm_dyn_cfg_helpers.c
@@ -11,6 +11,8 @@
 #endif
 #include <common/fdt_wrappers.h>
 
+#include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <libfdt.h>
 
 #include <plat/arm/common/arm_dyn_cfg_helpers.h>
@@ -20,18 +22,15 @@
 #define DTB_PROP_MBEDTLS_HEAP_SIZE "mbedtls_heap_size"
 
 #if MEASURED_BOOT
-#define DTB_PROP_BL2_HASH_DATA	"bl2_hash_data"
 #ifdef SPD_opteed
 /*
  * Currently OP-TEE does not support reading DTBs from Secure memory
  * and this property should be removed when this feature is supported.
  */
 #define DTB_PROP_HW_SM_LOG_ADDR	"tpm_event_log_sm_addr"
-#endif
+#endif /* SPD_opteed */
 #define DTB_PROP_HW_LOG_ADDR	"tpm_event_log_addr"
 #define DTB_PROP_HW_LOG_SIZE    "tpm_event_log_size"
-
-static int dtb_root = -1;
 #endif /* MEASURED_BOOT */
 
 /*******************************************************************************
@@ -81,9 +80,8 @@
  */
 int arm_set_dtb_mbedtls_heap_info(void *dtb, void *heap_addr, size_t heap_size)
 {
-#if !MEASURED_BOOT
 	int dtb_root;
-#endif
+
 	/*
 	 * Verify that the DTB is valid, before attempting to write to it,
 	 * and get the DTB root node.
@@ -123,32 +121,8 @@
 
 #if MEASURED_BOOT
 /*
- * This function writes the BL2 hash data in HW_FW_CONFIG DTB.
- * When it is called, it is guaranteed that a DTB is available.
- *
- * This function is supposed to be called only by BL1.
- *
- * Returns:
- *	0 = success
- *    < 0 = error
- */
-int arm_set_bl2_hash_info(void *dtb, void *data)
-{
-	assert(dtb_root >= 0);
-
-	/*
-	 * Write the BL2 hash data in the DTB.
-	 */
-	return fdtw_write_inplace_bytes(dtb, dtb_root,
-					DTB_PROP_BL2_HASH_DATA,
-					TCG_DIGEST_SIZE, data);
-}
-
-/*
  * Write the Event Log address and its size in the DTB.
  *
- * This function is supposed to be called only by BL2.
- *
  * Returns:
  *	0 = success
  *    < 0 = error
@@ -231,14 +205,20 @@
  *	0 = success
  *    < 0 = error
  */
-int arm_set_tos_fw_info(uintptr_t config_base, uintptr_t log_addr,
-			size_t log_size)
+int arm_set_tos_fw_info(uintptr_t log_addr, size_t log_size)
 {
+	uintptr_t config_base;
+	const bl_mem_params_node_t *cfg_mem_params;
 	int err;
 
-	assert(config_base != 0UL);
 	assert(log_addr != 0UL);
 
+	/* Get the config load address and size of TOS_FW_CONFIG */
+	cfg_mem_params = get_bl_mem_params_node(TOS_FW_CONFIG_ID);
+	assert(cfg_mem_params != NULL);
+
+	config_base = cfg_mem_params->image_info.image_base;
+
 	/* Write the Event Log address and its size in the DTB */
 	err = arm_set_event_log_info(config_base,
 #ifdef SPD_opteed
@@ -263,23 +243,25 @@
  *	0 = success
  *    < 0 = error
  */
-int arm_set_nt_fw_info(uintptr_t config_base,
+int arm_set_nt_fw_info(
 #ifdef SPD_opteed
 			uintptr_t log_addr,
 #endif
 			size_t log_size, uintptr_t *ns_log_addr)
 {
+	uintptr_t config_base;
 	uintptr_t ns_addr;
 	const bl_mem_params_node_t *cfg_mem_params;
 	int err;
 
-	assert(config_base != 0UL);
 	assert(ns_log_addr != NULL);
 
 	/* Get the config load address and size from NT_FW_CONFIG */
 	cfg_mem_params = get_bl_mem_params_node(NT_FW_CONFIG_ID);
 	assert(cfg_mem_params != NULL);
 
+	config_base = cfg_mem_params->image_info.image_base;
+
 	/* Calculate Event Log address in Non-secure memory */
 	ns_addr = cfg_mem_params->image_info.image_base +
 			cfg_mem_params->image_info.image_max_size;
@@ -300,4 +282,87 @@
 	*ns_log_addr = (err < 0) ? 0UL : ns_addr;
 	return err;
 }
+
+/*
+ * This function writes the Event Log address and its size
+ * in the TB_FW_CONFIG DTB.
+ *
+ * This function is supposed to be called only by BL1.
+ *
+ * Returns:
+ *     0 = success
+ *   < 0 = error
+ */
+int arm_set_tb_fw_info(uintptr_t log_addr, size_t log_size)
+{
+	/*
+	 * Read tb_fw_config device tree for Event Log properties
+	 * and write the Event Log address and its size in the DTB
+	 */
+	const struct dyn_cfg_dtb_info_t *tb_fw_config_info;
+	uintptr_t tb_fw_cfg_dtb;
+	int err;
+
+	tb_fw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, TB_FW_CONFIG_ID);
+	assert(tb_fw_config_info != NULL);
+
+	tb_fw_cfg_dtb = tb_fw_config_info->config_addr;
+
+	err = arm_set_event_log_info(tb_fw_cfg_dtb,
+#ifdef SPD_opteed
+				     0UL,
+#endif
+				     log_addr, log_size);
+	return err;
+}
+
+/*
+ * This function reads the Event Log address and its size
+ * properties present in TB_FW_CONFIG DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *     0 = success
+ *   < 0 = error
+ * Alongside returns Event Log address and its size.
+ */
+
+int arm_get_tb_fw_info(uint64_t *log_addr, size_t *log_size)
+{
+	/* As libfdt uses void *, we can't avoid this cast */
+	const struct dyn_cfg_dtb_info_t *tb_fw_config_info;
+	int node, rc;
+
+	tb_fw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, TB_FW_CONFIG_ID);
+	assert(tb_fw_config_info != NULL);
+
+	void *dtb = (void *)tb_fw_config_info->config_addr;
+	const char *compatible = "arm,tpm_event_log";
+
+	/* Assert the node offset point to compatible property */
+	node = fdt_node_offset_by_compatible(dtb, -1, compatible);
+	if (node < 0) {
+		WARN("The compatible property '%s'%s", compatible,
+		     " not specified in TB_FW config.\n");
+		return node;
+	}
+
+	VERBOSE("Dyn cfg: '%s'%s", compatible, " found in the config\n");
+
+	rc = fdt_read_uint64(dtb, node, DTB_PROP_HW_LOG_ADDR, log_addr);
+	if (rc != 0) {
+		ERROR("%s%s", DTB_PROP_HW_LOG_ADDR,
+		      " not specified in TB_FW config.\n");
+		return rc;
+	}
+
+	rc = fdt_read_uint32(dtb, node, DTB_PROP_HW_LOG_SIZE, (uint32_t *)log_size);
+	if (rc != 0) {
+		ERROR("%s%s", DTB_PROP_HW_LOG_SIZE,
+		      " not specified in TB_FW config.\n");
+	}
+
+	return rc;
+}
 #endif /* MEASURED_BOOT */
diff --git a/plat/arm/css/sgi/include/sgi_base_platform_def.h b/plat/arm/css/sgi/include/sgi_base_platform_def.h
index d795f25..c9c8c04 100644
--- a/plat/arm/css/sgi/include/sgi_base_platform_def.h
+++ b/plat/arm/css/sgi/include/sgi_base_platform_def.h
@@ -89,7 +89,7 @@
  *
  */
 #if TRUSTED_BOARD_BOOT
-# define PLAT_ARM_MAX_BL2_SIZE		(0x1D000 + ((CSS_SGI_CHIP_COUNT - 1) * \
+# define PLAT_ARM_MAX_BL2_SIZE		(0x20000 + ((CSS_SGI_CHIP_COUNT - 1) * \
 							0x2000))
 #else
 # define PLAT_ARM_MAX_BL2_SIZE		(0x14000 + ((CSS_SGI_CHIP_COUNT - 1) * \
diff --git a/plat/common/plat_bl1_common.c b/plat/common/plat_bl1_common.c
index 1c6d68b..bcf9f89 100644
--- a/plat/common/plat_bl1_common.c
+++ b/plat/common/plat_bl1_common.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
  */
@@ -27,9 +27,6 @@
 #pragma weak bl1_plat_fwu_done
 #pragma weak bl1_plat_handle_pre_image_load
 #pragma weak bl1_plat_handle_post_image_load
-#if MEASURED_BOOT
-#pragma weak bl1_plat_set_bl2_hash
-#endif
 
 unsigned int bl1_plat_get_next_image_id(void)
 {
@@ -118,12 +115,3 @@
 		(void *) bl2_secram_layout);
 	return 0;
 }
-
-#if MEASURED_BOOT
-/*
- * Calculates and writes BL2 hash data to TB_FW_CONFIG DTB.
- */
-void bl1_plat_set_bl2_hash(const image_desc_t *image_desc)
-{
-}
-#endif
diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index e86938b..6709678 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/include/platform_def.h
@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <arch.h>
 #include <common/tbbr/tbbr_img_def.h>
 
 #define PLATFORM_LINKER_FORMAT		"elf64-littleaarch64"
diff --git a/plat/marvell/armada/a3k/common/a3700_common.mk b/plat/marvell/armada/a3k/common/a3700_common.mk
index 9550452..d0e8688 100644
--- a/plat/marvell/armada/a3k/common/a3700_common.mk
+++ b/plat/marvell/armada/a3k/common/a3700_common.mk
@@ -76,6 +76,7 @@
 
 ifdef WTP
 
+# Do not remove! Following checks are required to ensure correct TF-A builds, removing these checks leads to broken TF-A builds
 $(if $(wildcard $(value WTP)/*),,$(error "'WTP=$(value WTP)' was specified, but '$(value WTP)' directory does not exist"))
 $(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"))
 
@@ -143,6 +144,7 @@
 CRYPTOPP_INCDIR		?= $(CRYPTOPP_PATH)
 
 $(TBB): FORCE
+#	Do not remove! Following checks are required to ensure correct TF-A builds, removing these checks leads to broken TF-A builds
 	$(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"))
@@ -159,6 +161,7 @@
 	$(Q)cp -a $(WTMI_MULTI_IMG) $(BUILD_PLAT)/wtmi.bin
 
 $(TIMDDRTOOL): FORCE
+#	Do not remove! Following checks are required to ensure correct TF-A builds, removing these checks leads to broken TF-A builds
 	$(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 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"))
diff --git a/plat/marvell/armada/a8k/common/ble/ble.mk b/plat/marvell/armada/a8k/common/ble/ble.mk
index 87e2ce0..160e98f 100644
--- a/plat/marvell/armada/a8k/common/ble/ble.mk
+++ b/plat/marvell/armada/a8k/common/ble/ble.mk
@@ -28,6 +28,7 @@
 $(BLE_OBJS): $(MV_DDR_LIB)
 
 $(MV_DDR_LIB): FORCE
+#	Do not remove! Following checks are required to ensure correct TF-A builds, removing these checks leads to broken TF-A builds
 	$(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"))
diff --git a/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c b/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c
index b40fa87..d018953 100644
--- a/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c
+++ b/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c
@@ -46,7 +46,6 @@
 	 R12_CCIF0_EVENT_B |				\
 	 R12_SSPM2SPM_WAKEUP_B |			\
 	 R12_SCP2SPM_WAKEUP_B |				\
-	 R12_ADSP2SPM_WAKEUP_B |			\
 	 R12_USBX_CDSC_B |				\
 	 R12_USBX_POWERDWN_B |				\
 	 R12_SYS_TIMER_EVENT_B |			\
diff --git a/plat/nvidia/tegra/soc/t194/platform_t194.mk b/plat/nvidia/tegra/soc/t194/platform_t194.mk
index 339375f..7583833 100644
--- a/plat/nvidia/tegra/soc/t194/platform_t194.mk
+++ b/plat/nvidia/tegra/soc/t194/platform_t194.mk
@@ -1,9 +1,11 @@
 #
-# Copyright (c) 2019-2020, NVIDIA CORPORATION. All rights reserved.
+# Copyright (c) 2019-2021, NVIDIA CORPORATION. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 # platform configs
 ENABLE_CONSOLE_SPE			:= 1
 $(eval $(call add_define,ENABLE_CONSOLE_SPE))
@@ -74,10 +76,10 @@
 
 # SPM dispatcher
 ifeq (${SPD},spmd)
-# include device tree helper library
 include lib/libfdt/libfdt.mk
 # sources to support spmd
 BL31_SOURCES		+=	plat/common/plat_spmd_manifest.c	\
-				common/fdt_wrappers.c			\
 				${LIBFDT_SRCS}
+
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
 endif
diff --git a/plat/nxp/common/include/default/ch_2/soc_default_helper_macros.h b/plat/nxp/common/include/default/ch_2/soc_default_helper_macros.h
index 789b112..84f07e6 100644
--- a/plat/nxp/common/include/default/ch_2/soc_default_helper_macros.h
+++ b/plat/nxp/common/include/default/ch_2/soc_default_helper_macros.h
@@ -56,6 +56,11 @@
 #define RCPM_POWMGTCSR_OFFSET		0x130
 #define RCPM_IPPDEXPCR0_OFFSET		0x140
 #define RCPM_POWMGTCSR_LPM20_REQ	0x00100000
-#endif
+#endif /* NXP_RCPM_ADDR */
+
+#define DCFG_SBEESR2_ADDR		0x20140534
+#define DCFG_MBEESR2_ADDR		0x20140544
+/* SBEESR and MBEESR bit mask */
+#define OCRAM_EESR_MASK			0x00000060
 
 #endif	/*	SOC_DEFAULT_HELPER_MACROS_H	*/
diff --git a/plat/nxp/common/include/default/ch_3_2/soc_default_helper_macros.h b/plat/nxp/common/include/default/ch_3_2/soc_default_helper_macros.h
index 8de516e..1edd28d 100644
--- a/plat/nxp/common/include/default/ch_3_2/soc_default_helper_macros.h
+++ b/plat/nxp/common/include/default/ch_3_2/soc_default_helper_macros.h
@@ -79,4 +79,9 @@
 #define ENABLE_WUO			0x10
 #endif /* NXP_CCN_ADDR */
 
+#define DCFG_SBEESR2_ADDR		0x00100534
+#define DCFG_MBEESR2_ADDR		0x00100544
+/* SBEESR and MBEESR bit mask */
+#define OCRAM_EESR_MASK			0x00000008
+
 #endif	/*	SOC_DEFAULT_HELPER_MACROS_H	*/
diff --git a/plat/nxp/common/ocram/aarch64/ocram.S b/plat/nxp/common/ocram/aarch64/ocram.S
new file mode 100644
index 0000000..ec53341
--- /dev/null
+++ b/plat/nxp/common/ocram/aarch64/ocram.S
@@ -0,0 +1,71 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+
+#include <soc_default_base_addr.h>
+#include <soc_default_helper_macros.h>
+
+.global ocram_init
+
+/*
+ * void ocram_init(uintptr_t start_addr, size_t size)
+ *
+ * This function will do OCRAM ECC.
+ * OCRAM is initialized with 64-bit writes and then a write
+ * performed to address 0x0010_0534 with the value 0x0000_0008.
+ *
+ * x0: start_addr
+ * x1: size in bytes
+ * Called from C
+ */
+
+func ocram_init
+	/* save the aarch32/64 non-volatile registers */
+	stp	x4,  x5,  [sp, #-16]!
+	stp	x6,  x7,  [sp, #-16]!
+	stp	x8,  x9,  [sp, #-16]!
+	stp	x10, x11, [sp, #-16]!
+	stp	x12, x13, [sp, #-16]!
+	stp	x18, x30, [sp, #-16]!
+
+	/* convert bytes to 64-byte chunks */
+	lsr	x1, x1, #6
+1:
+	/* for each location, read and write-back */
+	dc	ivac, x0
+	dsb	sy
+	ldp	x4, x5, [x0]
+	ldp	x6, x7, [x0, #16]
+	ldp	x8, x9, [x0, #32]
+	ldp	x10, x11, [x0, #48]
+	stp	x4, x5, [x0]
+	stp	x6, x7, [x0, #16]
+	stp	x8, x9, [x0, #32]
+	stp	x10, x11, [x0, #48]
+	dc	cvac, x0
+
+	sub	x1, x1, #1
+	cbz	x1, 2f
+	add	x0, x0, #64
+	b	1b
+2:
+	/* Clear OCRAM ECC status bit in SBEESR2 and MBEESR2 */
+	ldr	w1, =OCRAM_EESR_MASK
+	ldr	x0, =DCFG_SBEESR2_ADDR
+	str	w1, [x0]
+	ldr	x0, =DCFG_MBEESR2_ADDR
+	str	w1, [x0]
+
+	/* restore the aarch32/64 non-volatile registers */
+	ldp	x18, x30, [sp], #16
+	ldp	x12, x13, [sp], #16
+	ldp	x10, x11, [sp], #16
+	ldp	x8,  x9,  [sp], #16
+	ldp	x6,  x7,  [sp], #16
+	ldp	x4,  x5,  [sp], #16
+	ret
+endfunc ocram_init
diff --git a/plat/nxp/common/ocram/ocram.h b/plat/nxp/common/ocram/ocram.h
new file mode 100644
index 0000000..479de61
--- /dev/null
+++ b/plat/nxp/common/ocram/ocram.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright 2021 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef OCRAM_H
+#define OCRAM_H
+
+void ocram_init(uintptr_t start_addr, size_t size);
+
+#endif /* OCRAM_H */
diff --git a/plat/nxp/common/ocram/ocram.mk b/plat/nxp/common/ocram/ocram.mk
new file mode 100644
index 0000000..c77bd4a
--- /dev/null
+++ b/plat/nxp/common/ocram/ocram.mk
@@ -0,0 +1,14 @@
+#
+# Copyright 2021 NXP
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+PLAT_OCRAM_PATH		:= $(PLAT_COMMON_PATH)/ocram
+
+OCRAM_SOURCES		:= ${PLAT_OCRAM_PATH}/$(ARCH)/ocram.S
+
+BL2_SOURCES		+= ${OCRAM_SOURCES}
+
+PLAT_INCLUDES           += -I${PLAT_COMMON_PATH}/ocram
diff --git a/plat/nxp/common/plat_make_helper/soc_common_def.mk b/plat/nxp/common/plat_make_helper/soc_common_def.mk
index fdd7249..22cd39a 100644
--- a/plat/nxp/common/plat_make_helper/soc_common_def.mk
+++ b/plat/nxp/common/plat_make_helper/soc_common_def.mk
@@ -112,3 +112,8 @@
 ifneq (${PLAT_XLAT_TABLES_DYNAMIC},)
 $(eval $(call add_define,PLAT_XLAT_TABLES_DYNAMIC))
 endif
+
+ifeq (${OCRAM_ECC_EN},yes)
+$(eval $(call add_define,CONFIG_OCRAM_ECC_EN))
+include ${PLAT_COMMON_PATH}/ocram/ocram.mk
+endif
diff --git a/plat/nxp/soc-ls1028a/soc.c b/plat/nxp/soc-ls1028a/soc.c
index 4f67154..edfd657 100644
--- a/plat/nxp/soc-ls1028a/soc.c
+++ b/plat/nxp/soc-ls1028a/soc.c
@@ -16,6 +16,9 @@
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <ls_interconnect.h>
 #include <mmio.h>
+#ifdef POLICY_FUSE_PROVISION
+#include <nxp_gpio.h>
+#endif
 #if TRUSTED_BOARD_BOOT
 #include <nxp_smmu.h>
 #endif
@@ -81,6 +84,15 @@
 }
 
 #ifdef IMAGE_BL2
+
+#ifdef POLICY_FUSE_PROVISION
+static gpio_init_info_t gpio_init_data = {
+	.gpio1_base_addr = NXP_GPIO1_ADDR,
+	.gpio2_base_addr = NXP_GPIO2_ADDR,
+	.gpio3_base_addr = NXP_GPIO3_ADDR,
+};
+#endif
+
 void soc_preload_setup(void)
 {
 }
diff --git a/plat/nxp/soc-ls1028a/soc.def b/plat/nxp/soc-ls1028a/soc.def
index e133982..c23c1bb 100644
--- a/plat/nxp/soc-ls1028a/soc.def
+++ b/plat/nxp/soc-ls1028a/soc.def
@@ -88,6 +88,8 @@
 NXP_ESDHC_ENDIANNESS	:=	LE
 NXP_QSPI_ENDIANNESS	:=	LE
 NXP_FSPI_ENDIANNESS	:=	LE
+NXP_SCFG_ENDIANNESS	:=	LE
+NXP_GPIO_ENDIANNESS	:=	LE
 
 NXP_SFP_VER		:=	3_4
 
diff --git a/plat/qemu/qemu_sbsa/platform.mk b/plat/qemu/qemu_sbsa/platform.mk
index 9fb30ad..5a6b1e1 100644
--- a/plat/qemu/qemu_sbsa/platform.mk
+++ b/plat/qemu/qemu_sbsa/platform.mk
@@ -1,9 +1,11 @@
 #
-# Copyright (c) 2019-2020, Linaro Limited and Contributors. All rights reserved.
+# Copyright (c) 2019-2021, Linaro Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include common/fdt_wrappers.mk
+
 CRASH_REPORTING	:=	1
 
 include lib/libfdt/libfdt.mk
@@ -86,8 +88,10 @@
 				${PLAT_QEMU_COMMON_PATH}/aarch64/plat_helpers.S	\
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl31_setup.c	\
 				common/fdt_fixup.c				\
-				common/fdt_wrappers.c				\
 				${QEMU_GIC_SOURCES}
+
+BL31_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
+
 ifeq (${SPM_MM},1)
 	BL31_SOURCES		+=	${PLAT_QEMU_COMMON_PATH}/qemu_spm.c
 endif
diff --git a/plat/qti/common/src/pm8998.c b/plat/qti/common/src/pm_ps_hold.c
similarity index 91%
rename from plat/qti/common/src/pm8998.c
rename to plat/qti/common/src/pm_ps_hold.c
index b189a8b..208345c 100644
--- a/plat/qti/common/src/pm8998.c
+++ b/plat/qti/common/src/pm_ps_hold.c
@@ -14,11 +14,9 @@
  * include other part numbers like PM6150.
  */
 
-#define PON_PS_HOLD_RESET_CTL		0x85a
 #define RESET_TYPE_WARM_RESET		1
 #define RESET_TYPE_SHUTDOWN		4
 
-#define PON_PS_HOLD_RESET_CTL2		0x85b
 #define S2_RESET_EN			BIT(7)
 
 static void configure_ps_hold(uint32_t reset_type)
diff --git a/plat/qti/sc7180/inc/platform_def.h b/plat/qti/sc7180/inc/platform_def.h
index b0798a6..e3dc811 100644
--- a/plat/qti/sc7180/inc/platform_def.h
+++ b/plat/qti/sc7180/inc/platform_def.h
@@ -190,5 +190,10 @@
 #define QTI_SOC_REVISION_REG			0x1FC8000
 #define QTI_SOC_REVISION_MASK			U(0xFFFF)
 /*----------------------------------------------------------------------------*/
+/* LC PON register offsets */
+/*----------------------------------------------------------------------------*/
+#define PON_PS_HOLD_RESET_CTL			0x85a
+#define PON_PS_HOLD_RESET_CTL2			0x85b
+/*----------------------------------------------------------------------------*/
 
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/qti/sc7180/platform.mk b/plat/qti/sc7180/platform.mk
index ec560d0..141e2c3 100644
--- a/plat/qti/sc7180/platform.mk
+++ b/plat/qti/sc7180/platform.mk
@@ -51,7 +51,7 @@
 				$(QTI_PLAT_PATH)/common/src/$(ARCH)/qti_kryo4_silver.S	\
 				$(QTI_PLAT_PATH)/common/src/$(ARCH)/qti_kryo4_gold.S	\
 				$(QTI_PLAT_PATH)/common/src/$(ARCH)/qti_uart_console.S	\
-				$(QTI_PLAT_PATH)/common/src/pm8998.c			\
+				$(QTI_PLAT_PATH)/common/src/pm_ps_hold.c			\
 				$(QTI_PLAT_PATH)/common/src/qti_stack_protector.c	\
 				$(QTI_PLAT_PATH)/common/src/qti_common.c		\
 				$(QTI_PLAT_PATH)/common/src/qti_bl31_setup.c		\
diff --git a/plat/qti/sc7280/inc/platform_def.h b/plat/qti/sc7280/inc/platform_def.h
index 660cb33..da7eddc 100644
--- a/plat/qti/sc7280/inc/platform_def.h
+++ b/plat/qti/sc7280/inc/platform_def.h
@@ -190,5 +190,10 @@
 #define QTI_SOC_REVISION_REG			0x1FC8000
 #define QTI_SOC_REVISION_MASK			U(0xFFFF)
 /*----------------------------------------------------------------------------*/
+/* LC PON register offsets */
+/*----------------------------------------------------------------------------*/
+#define PON_PS_HOLD_RESET_CTL			0x852
+#define PON_PS_HOLD_RESET_CTL2			0x853
+/*----------------------------------------------------------------------------*/
 
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/qti/sc7280/platform.mk b/plat/qti/sc7280/platform.mk
index 6e26781..bc2c221 100644
--- a/plat/qti/sc7280/platform.mk
+++ b/plat/qti/sc7280/platform.mk
@@ -51,7 +51,7 @@
 				$(QTI_PLAT_PATH)/common/src/$(ARCH)/qti_kryo6_silver.S	\
 				$(QTI_PLAT_PATH)/common/src/$(ARCH)/qti_kryo6_gold.S	\
 				$(QTI_PLAT_PATH)/common/src/$(ARCH)/qti_uart_console.S	\
-				$(QTI_PLAT_PATH)/common/src/pm8998.c			\
+				$(QTI_PLAT_PATH)/common/src/pm_ps_hold.c			\
 				$(QTI_PLAT_PATH)/common/src/qti_stack_protector.c	\
 				$(QTI_PLAT_PATH)/common/src/qti_common.c		\
 				$(QTI_PLAT_PATH)/common/src/qti_bl31_setup.c		\
diff --git a/plat/renesas/common/plat_pm.c b/plat/renesas/common/plat_pm.c
index 6a9ad45..1d4a7f6 100644
--- a/plat/renesas/common/plat_pm.c
+++ b/plat/renesas/common/plat_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, Renesas Electronics Corporation. All rights reserved.
+ * Copyright (c) 2015-2021, Renesas Electronics Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -128,11 +128,6 @@
 
 		rcar_pwrc_clusteroff(mpidr);
 	}
-
-#if RCAR_SYSTEM_SUSPEND
-	if (SYSTEM_PWR_STATE(target_state) == PLAT_MAX_OFF_STATE)
-		rcar_pwrc_suspend_to_ram();
-#endif
 }
 
 static void rcar_pwr_domain_suspend_finish(const psci_power_state_t
@@ -160,6 +155,18 @@
 	rcar_pwr_domain_on_finish(target_state);
 }
 
+static void __dead2 rcar_pwr_domain_pwr_down_wfi(const psci_power_state_t *target_state)
+{
+#if RCAR_SYSTEM_SUSPEND
+	if (SYSTEM_PWR_STATE(target_state) == PLAT_MAX_OFF_STATE)
+		rcar_pwrc_suspend_to_ram();
+#endif
+	wfi();
+
+	ERROR("RCAR Power Down: operation not handled.\n");
+	panic();
+}
+
 static void __dead2 rcar_system_off(void)
 {
 #if PMIC_ROHM_BD9571
@@ -292,6 +299,7 @@
 	.system_off			= rcar_system_off,
 	.system_reset			= rcar_system_reset,
 	.validate_power_state		= rcar_validate_power_state,
+	.pwr_domain_pwr_down_wfi	= rcar_pwr_domain_pwr_down_wfi,
 #if RCAR_SYSTEM_SUSPEND
 	.get_sys_suspend_power_state	= rcar_get_sys_suspend_power_state,
 #endif
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index 7e76083..b0314d2 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -13,6 +13,7 @@
 #include <drivers/io/io_block.h>
 #include <drivers/io/io_driver.h>
 #include <drivers/io/io_fip.h>
+#include <drivers/io/io_memmap.h>
 #include <drivers/io/io_mtd.h>
 #include <drivers/io/io_storage.h>
 #include <drivers/mmc.h>
@@ -24,6 +25,7 @@
 #include <drivers/st/stm32_fmc2_nand.h>
 #include <drivers/st/stm32_qspi.h>
 #include <drivers/st/stm32_sdmmc2.h>
+#include <drivers/usb_device.h>
 #include <lib/fconf/fconf.h>
 #include <lib/mmio.h>
 #include <lib/utils.h>
@@ -31,7 +33,9 @@
 #include <tools_share/firmware_image_package.h>
 
 #include <platform_def.h>
+#include <stm32cubeprogrammer.h>
 #include <stm32mp_fconf_getter.h>
+#include <usb_dfu.h>
 
 /* IO devices */
 uintptr_t fip_dev_handle;
@@ -95,6 +99,10 @@
 static const io_dev_connector_t *spi_dev_con;
 #endif
 
+#if STM32MP_USB_PROGRAMMER
+static const io_dev_connector_t *memmap_dev_con;
+#endif
+
 io_block_spec_t image_block_spec = {
 	.offset = 0U,
 	.length = 0U,
@@ -128,6 +136,9 @@
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
 		INFO("Using SPI NAND\n");
 		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		INFO("Using USB\n");
+		break;
 	default:
 		ERROR("Boot interface %u not found\n",
 		      boot_context->boot_interface_selected);
@@ -246,6 +257,32 @@
 }
 #endif /* STM32MP_SPI_NAND */
 
+#if STM32MP_USB_PROGRAMMER
+static void mmap_io_setup(void)
+{
+	int io_result __unused;
+
+	io_result = register_io_dev_memmap(&memmap_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(memmap_dev_con, (uintptr_t)NULL,
+				&storage_dev_handle);
+	assert(io_result == 0);
+}
+
+static void stm32cubeprogrammer_usb(void)
+{
+	int ret __unused;
+	struct usb_handle *pdev;
+
+	/* Init USB on platform */
+	pdev = usb_dfu_plat_init();
+
+	ret = stm32cubeprog_usb_load(pdev, DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+	assert(ret == 0);
+}
+#endif
+
 void stm32mp_io_setup(void)
 {
 	int io_result __unused;
@@ -297,6 +334,12 @@
 		boot_spi_nand(boot_context);
 		break;
 #endif
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		dmbsy();
+		mmap_io_setup();
+		break;
+#endif
 
 	default:
 		ERROR("Boot interface %d not supported\n",
@@ -357,6 +400,17 @@
 		break;
 #endif
 
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		if (image_id == FW_CONFIG_ID) {
+			stm32cubeprogrammer_usb();
+			/* FIP loaded at DWL address */
+			image_block_spec.offset = DWL_BUFFER_BASE;
+			image_block_spec.length = DWL_BUFFER_SIZE;
+		}
+		break;
+#endif
+
 	default:
 		ERROR("FIP Not found\n");
 		panic();
diff --git a/plat/st/common/include/stm32cubeprogrammer.h b/plat/st/common/include/stm32cubeprogrammer.h
new file mode 100644
index 0000000..503d919
--- /dev/null
+++ b/plat/st/common/include/stm32cubeprogrammer.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32CUBEPROGRAMMER_H
+#define STM32CUBEPROGRAMMER_H
+
+#include <stdint.h>
+
+#include <usb_dfu.h>
+
+/* Phase definition */
+#define PHASE_FLASHLAYOUT	0U
+#define PHASE_SSBL		3U
+#define PHASE_CMD		0xF1U
+#define PHASE_RESET		0xFFU
+
+/* Functions provided by plat */
+uint8_t usb_dfu_get_phase(uint8_t alt);
+
+int stm32cubeprog_usb_load(struct usb_handle *usb_core_handle,
+			   uintptr_t ssbl_base,
+			   size_t ssbl_len);
+
+#endif /* STM32CUBEPROGRAMMER_H */
diff --git a/plat/st/common/include/stm32mp_dt.h b/plat/st/common/include/stm32mp_dt.h
index 299c0b1..f7201c0 100644
--- a/plat/st/common/include/stm32mp_dt.h
+++ b/plat/st/common/include/stm32mp_dt.h
@@ -34,6 +34,7 @@
 void dt_fill_device_info(struct dt_node_info *info, int node);
 int dt_get_node(struct dt_node_info *info, int offset, const char *compat);
 int dt_get_stdout_uart_info(struct dt_node_info *info);
+int dt_match_instance_by_compatible(const char *compatible, uintptr_t address);
 uint32_t dt_get_ddr_size(void);
 uint32_t dt_get_pwr_vdd_voltage(void);
 const char *dt_get_board_model(void);
diff --git a/plat/st/common/include/usb_dfu.h b/plat/st/common/include/usb_dfu.h
new file mode 100644
index 0000000..f7d4245
--- /dev/null
+++ b/plat/st/common/include/usb_dfu.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef USB_DFU_H
+#define USB_DFU_H
+
+#include <stdint.h>
+
+#include <drivers/usb_device.h>
+
+#define DFU_DESCRIPTOR_TYPE		0x21U
+
+/* Max DFU Packet Size = 1024 bytes */
+#define USBD_DFU_XFER_SIZE		1024U
+
+#define TRANSFER_SIZE_BYTES(size) \
+	((uint8_t)((size) & 0xFF)), /* XFERSIZEB0 */\
+	((uint8_t)((size) >> 8))    /* XFERSIZEB1 */
+
+/*
+ * helper for descriptor of DFU interface 0 Alternate setting n
+ * with iInterface = index of string descriptor, assumed Nth user string
+ */
+#define USBD_DFU_IF_DESC(n)	0x09U, /* Interface Descriptor size */\
+				USB_DESC_TYPE_INTERFACE, /* descriptor type */\
+				0x00U, /* Number of Interface */\
+				(n), /* Alternate setting */\
+				0x00U, /* bNumEndpoints*/\
+				0xFEU, /* Application Specific Class Code */\
+				0x01U, /* Device Firmware Upgrade Code */\
+				0x02U, /* DFU mode protocol */ \
+				USBD_IDX_USER0_STR + (n) /* iInterface */
+
+/* DFU1.1 Standard */
+#define USB_DFU_VERSION			0x0110U
+#define USB_DFU_ITF_SIZ			9U
+#define USB_DFU_DESC_SIZ(itf)		(USB_DFU_ITF_SIZ * ((itf) + 2U))
+
+/*
+ * bmAttribute value for DFU:
+ * bitCanDnload = 1(bit 0)
+ * bitCanUpload = 1(bit 1)
+ * bitManifestationTolerant = 1 (bit 2)
+ * bitWillDetach = 1(bit 3)
+ * Reserved (bit4-6)
+ * bitAcceleratedST = 0(bit 7)
+ */
+#define DFU_BM_ATTRIBUTE		0x0FU
+
+#define DFU_STATUS_SIZE			6U
+
+/* Callback for media access */
+struct usb_dfu_media {
+	int (*upload)(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+		      void *user_data);
+	int (*download)(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+			void *user_data);
+	int (*manifestation)(uint8_t alt, void *user_data);
+};
+
+/* Internal DFU handle */
+struct usb_dfu_handle {
+	uint8_t status[DFU_STATUS_SIZE];
+	uint8_t dev_state;
+	uint8_t dev_status;
+	uint8_t alt_setting;
+	const struct usb_dfu_media *callback;
+};
+
+void usb_dfu_register(struct usb_handle *pdev, struct usb_dfu_handle *phandle);
+
+int usb_dfu_loop(struct usb_handle *pdev, const struct usb_dfu_media *pmedia);
+
+/* Function provided by plat */
+struct usb_handle *usb_dfu_plat_init(void);
+
+#endif /* USB_DFU_H */
diff --git a/plat/st/common/stm32cubeprogrammer_usb.c b/plat/st/common/stm32cubeprogrammer_usb.c
new file mode 100644
index 0000000..19a6bba
--- /dev/null
+++ b/plat/st/common/stm32cubeprogrammer_usb.c
@@ -0,0 +1,196 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <tools_share/firmware_image_package.h>
+
+#include <stm32cubeprogrammer.h>
+#include <usb_dfu.h>
+
+/* Undefined download address */
+#define UNDEFINED_DOWN_ADDR	0xFFFFFFFF
+
+struct dfu_state {
+	uint8_t phase;
+	uintptr_t base;
+	size_t len;
+	uintptr_t address;
+	/* working buffer */
+	uint8_t buffer[UCHAR_MAX];
+};
+
+static struct dfu_state dfu_state;
+
+/* minimal size of Get Pḧase = offset for additionnl information */
+#define	GET_PHASE_LEN	9
+
+#define DFU_ERROR(...) \
+	{ \
+		ERROR(__VA_ARGS__); \
+		if (dfu->phase != PHASE_RESET) { \
+			snprintf((char *)&dfu->buffer[GET_PHASE_LEN], \
+				 sizeof(dfu->buffer) - GET_PHASE_LEN, \
+				 __VA_ARGS__); \
+			dfu->phase = PHASE_RESET; \
+			dfu->address = UNDEFINED_DOWN_ADDR; \
+			dfu->len = 0; \
+		} \
+	}
+
+static bool is_valid_header(fip_toc_header_t *header)
+{
+	if ((header->name == TOC_HEADER_NAME) && (header->serial_number != 0U)) {
+		return true;
+	}
+
+	return false;
+}
+
+static int dfu_callback_upload(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+			       void *user_data)
+{
+	int result = 0;
+	uint32_t length = 0;
+	struct dfu_state *dfu = (struct dfu_state *)user_data;
+
+	switch (usb_dfu_get_phase(alt)) {
+	case PHASE_CMD:
+		/* Get Pḧase */
+		dfu->buffer[0] = dfu->phase;
+		dfu->buffer[1] = (uint8_t)(dfu->address);
+		dfu->buffer[2] = (uint8_t)(dfu->address >> 8);
+		dfu->buffer[3] = (uint8_t)(dfu->address >> 16);
+		dfu->buffer[4] = (uint8_t)(dfu->address >> 24);
+		dfu->buffer[5] = 0x00;
+		dfu->buffer[6] = 0x00;
+		dfu->buffer[7] = 0x00;
+		dfu->buffer[8] = 0x00;
+		length = GET_PHASE_LEN;
+		if (dfu->phase == PHASE_FLASHLAYOUT &&
+		    dfu->address == UNDEFINED_DOWN_ADDR) {
+			INFO("Send detach request\n");
+			dfu->buffer[length++] = 0x01;
+		}
+		if (dfu->phase == PHASE_RESET) {
+			/* error information is added by DFU_ERROR macro */
+			length += strnlen((char *)&dfu->buffer[GET_PHASE_LEN],
+					  sizeof(dfu->buffer) - GET_PHASE_LEN)
+				  - 1;
+		}
+		break;
+
+	default:
+		DFU_ERROR("phase ID :%i, alternate %i for phase %i\n",
+			  dfu->phase, alt, usb_dfu_get_phase(alt));
+		result = -EIO;
+		break;
+	}
+
+	if (result == 0) {
+		*len = length;
+		*buffer = (uintptr_t)dfu->buffer;
+	}
+
+	return result;
+}
+
+static int dfu_callback_download(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+				 void *user_data)
+{
+	struct dfu_state *dfu = (struct dfu_state *)user_data;
+
+	if ((dfu->phase != usb_dfu_get_phase(alt)) ||
+	    (dfu->address == UNDEFINED_DOWN_ADDR)) {
+		DFU_ERROR("phase ID :%i, alternate %i, address %x\n",
+			  dfu->phase, alt, (uint32_t)dfu->address);
+		return -EIO;
+	}
+
+	VERBOSE("Download %d %lx %x\n", alt, dfu->address, *len);
+	*buffer = dfu->address;
+	dfu->address += *len;
+
+	if (dfu->address - dfu->base > dfu->len) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int dfu_callback_manifestation(uint8_t alt, void *user_data)
+{
+	struct dfu_state *dfu = (struct dfu_state *)user_data;
+
+	if (dfu->phase != usb_dfu_get_phase(alt)) {
+		ERROR("Manifestation phase ID :%i, alternate %i, address %lx\n",
+		      dfu->phase, alt, dfu->address);
+		return -EIO;
+	}
+
+	INFO("phase ID :%i, Manifestation %d at %lx\n",
+	     dfu->phase, alt, dfu->address);
+
+	switch (dfu->phase) {
+	case PHASE_SSBL:
+		if (!is_valid_header((fip_toc_header_t *)dfu->base)) {
+			DFU_ERROR("FIP Header check failed for phase %d\n", alt);
+			return -EIO;
+		}
+		VERBOSE("FIP header looks OK.\n");
+
+		/* Configure End with request detach */
+		dfu->phase = PHASE_FLASHLAYOUT;
+		dfu->address = UNDEFINED_DOWN_ADDR;
+		dfu->len = 0;
+		break;
+	default:
+		DFU_ERROR("Unknown phase\n");
+	}
+
+	return 0;
+}
+
+/* Open a connection to the USB device */
+static const struct usb_dfu_media usb_dfu_fops = {
+	.upload = dfu_callback_upload,
+	.download = dfu_callback_download,
+	.manifestation = dfu_callback_manifestation,
+};
+
+int stm32cubeprog_usb_load(struct usb_handle *usb_core_handle,
+			   uintptr_t base,
+			   size_t len)
+{
+	int ret;
+
+	usb_core_handle->user_data = (void *)&dfu_state;
+
+	INFO("DFU USB START...\n");
+	ret = usb_core_start(usb_core_handle);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	dfu_state.phase = PHASE_SSBL;
+	dfu_state.address = base;
+	dfu_state.base = base;
+	dfu_state.len = len;
+
+	ret = usb_dfu_loop(usb_core_handle, &usb_dfu_fops);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	INFO("DFU USB STOP...\n");
+	ret = usb_core_stop(usb_core_handle);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	return 0;
+}
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 17ac145..9120408 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -17,6 +17,8 @@
 #include <plat/common/platform.h>
 #include <services/arm_arch_svc.h>
 
+#define HEADER_VERSION_MAJOR_MASK	GENMASK(23, 16)
+
 uintptr_t plat_get_ns_image_entrypoint(void)
 {
 	return BL33_BASE;
@@ -93,7 +95,8 @@
 		return -EINVAL;
 	}
 
-	if (header->header_version != BOOT_API_HEADER_VERSION) {
+	if ((header->header_version & HEADER_VERSION_MAJOR_MASK) !=
+	    (BOOT_API_HEADER_VERSION & HEADER_VERSION_MAJOR_MASK)) {
 		ERROR("Header version\n");
 		return -EINVAL;
 	}
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index 0b35646..4dc9908 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -204,6 +204,33 @@
 }
 
 /*******************************************************************************
+ * This function returns the node offset matching compatible string in the DT,
+ * and also matching the reg property with the given address.
+ * Returns value on success, and error value on failure.
+ ******************************************************************************/
+int dt_match_instance_by_compatible(const char *compatible, uintptr_t address)
+{
+	int node;
+
+	fdt_for_each_compatible_node(fdt, node, compatible) {
+		const fdt32_t *cuint;
+
+		assert(fdt_get_node_parent_address_cells(node) == 1);
+
+		cuint = fdt_getprop(fdt, node, "reg", NULL);
+		if (cuint == NULL) {
+			continue;
+		}
+
+		if ((uintptr_t)fdt32_to_cpu(*cuint) == address) {
+			return node;
+		}
+	}
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+/*******************************************************************************
  * This function gets DDR size information from the DT.
  * Returns value in bytes on success, and 0 on failure.
  ******************************************************************************/
diff --git a/plat/st/common/usb_dfu.c b/plat/st/common/usb_dfu.c
new file mode 100644
index 0000000..8bb0994
--- /dev/null
+++ b/plat/st/common/usb_dfu.c
@@ -0,0 +1,538 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+
+#include <platform_def.h>
+#include <usb_dfu.h>
+
+/* Device states as defined in DFU spec */
+#define STATE_APP_IDLE			0
+#define STATE_APP_DETACH		1
+#define STATE_DFU_IDLE			2
+#define STATE_DFU_DNLOAD_SYNC		3
+#define STATE_DFU_DNLOAD_BUSY		4
+#define STATE_DFU_DNLOAD_IDLE		5
+#define STATE_DFU_MANIFEST_SYNC		6
+#define STATE_DFU_MANIFEST		7
+#define STATE_DFU_MANIFEST_WAIT_RESET	8
+#define STATE_DFU_UPLOAD_IDLE		9
+#define STATE_DFU_ERROR			10
+
+/* DFU errors */
+#define DFU_ERROR_NONE			0x00
+#define DFU_ERROR_TARGET		0x01
+#define DFU_ERROR_FILE			0x02
+#define DFU_ERROR_WRITE			0x03
+#define DFU_ERROR_ERASE			0x04
+#define DFU_ERROR_CHECK_ERASED		0x05
+#define DFU_ERROR_PROG			0x06
+#define DFU_ERROR_VERIFY		0x07
+#define DFU_ERROR_ADDRESS		0x08
+#define DFU_ERROR_NOTDONE		0x09
+#define DFU_ERROR_FIRMWARE		0x0A
+#define DFU_ERROR_VENDOR		0x0B
+#define DFU_ERROR_USB			0x0C
+#define DFU_ERROR_POR			0x0D
+#define DFU_ERROR_UNKNOWN		0x0E
+#define DFU_ERROR_STALLEDPKT		0x0F
+
+/* DFU request */
+#define DFU_DETACH			0
+#define DFU_DNLOAD			1
+#define DFU_UPLOAD			2
+#define DFU_GETSTATUS			3
+#define DFU_CLRSTATUS			4
+#define DFU_GETSTATE			5
+#define DFU_ABORT			6
+
+static bool usb_dfu_detach_req;
+
+/*
+ * usb_dfu_init
+ *         Initialize the DFU interface
+ * pdev: device instance
+ * cfgidx: Configuration index
+ * return: status
+ */
+static uint8_t usb_dfu_init(struct usb_handle *pdev, uint8_t cfgidx)
+{
+	(void)pdev;
+	(void)cfgidx;
+
+	/* Nothing to do in this stage */
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_de_init
+ *         De-Initialize the DFU layer
+ * pdev: device instance
+ * cfgidx: Configuration index
+ * return: status
+ */
+static uint8_t usb_dfu_de_init(struct usb_handle *pdev, uint8_t cfgidx)
+{
+	(void)pdev;
+	(void)cfgidx;
+
+	/* Nothing to do in this stage */
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_data_in
+ *         handle data IN Stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * return: status
+ */
+static uint8_t usb_dfu_data_in(struct usb_handle *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_ep0_rx_ready
+ *         handle EP0 Rx Ready event
+ * pdev: device
+ * return: status
+ */
+static uint8_t usb_dfu_ep0_rx_ready(struct usb_handle *pdev)
+{
+	(void)pdev;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_ep0_tx_ready
+ *         handle EP0 TRx Ready event
+ * pdev: device instance
+ * return: status
+ */
+static uint8_t usb_dfu_ep0_tx_ready(struct usb_handle *pdev)
+{
+	(void)pdev;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_sof
+ *         handle SOF event
+ * pdev: device instance
+ * return: status
+ */
+static uint8_t usb_dfu_sof(struct usb_handle *pdev)
+{
+	(void)pdev;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_iso_in_incomplete
+ *         handle data ISO IN Incomplete event
+ * pdev: device instance
+ * epnum: endpoint index
+ * return: status
+ */
+static uint8_t usb_dfu_iso_in_incomplete(struct usb_handle *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_iso_out_incomplete
+ *         handle data ISO OUT Incomplete event
+ * pdev: device instance
+ * epnum: endpoint index
+ * return: status
+ */
+static uint8_t usb_dfu_iso_out_incomplete(struct usb_handle *pdev,
+					  uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_data_out
+ *         handle data OUT Stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * return: status
+ */
+static uint8_t usb_dfu_data_out(struct usb_handle *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_dfu_detach
+ *         Handles the DFU DETACH request.
+ * pdev: device instance
+ * req: pointer to the request structure.
+ */
+static void usb_dfu_detach(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	INFO("Receive DFU Detach\n");
+
+	if ((hdfu->dev_state == STATE_DFU_IDLE) ||
+	    (hdfu->dev_state == STATE_DFU_DNLOAD_SYNC) ||
+	    (hdfu->dev_state == STATE_DFU_DNLOAD_IDLE) ||
+	    (hdfu->dev_state == STATE_DFU_MANIFEST_SYNC) ||
+	    (hdfu->dev_state == STATE_DFU_UPLOAD_IDLE)) {
+		/* Update the state machine */
+		hdfu->dev_state = STATE_DFU_IDLE;
+		hdfu->dev_status = DFU_ERROR_NONE;
+	}
+
+	usb_dfu_detach_req = true;
+}
+
+/*
+ * usb_dfu_download
+ *         Handles the DFU DNLOAD request.
+ * pdev: device instance
+ * req: pointer to the request structure
+ */
+static void usb_dfu_download(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+	uintptr_t data_ptr;
+	uint32_t length;
+	int ret;
+
+	/* Data setup request */
+	if (req->length > 0) {
+		/* Unsupported state */
+		if ((hdfu->dev_state != STATE_DFU_IDLE) &&
+		    (hdfu->dev_state != STATE_DFU_DNLOAD_IDLE)) {
+			/* Call the error management function (command will be nacked) */
+			usb_core_ctl_error(pdev);
+			return;
+		}
+
+		/* Get the data address */
+		length = req->length;
+		ret = hdfu->callback->download(hdfu->alt_setting, &data_ptr,
+					       &length, pdev->user_data);
+		if (ret == 0U) {
+			/* Update the state machine */
+			hdfu->dev_state = STATE_DFU_DNLOAD_SYNC;
+			/* Start the transfer */
+			usb_core_receive_ep0(pdev, (uint8_t *)data_ptr, length);
+		} else {
+			usb_core_ctl_error(pdev);
+		}
+	} else {
+		/* End of DNLOAD operation*/
+		if (hdfu->dev_state != STATE_DFU_DNLOAD_IDLE) {
+			/* Call the error management function (command will be nacked) */
+			usb_core_ctl_error(pdev);
+			return;
+		}
+		/* End of DNLOAD operation*/
+		hdfu->dev_state = STATE_DFU_MANIFEST_SYNC;
+		ret = hdfu->callback->manifestation(hdfu->alt_setting, pdev->user_data);
+		if (ret == 0U) {
+			hdfu->dev_state = STATE_DFU_MANIFEST_SYNC;
+		} else {
+			usb_core_ctl_error(pdev);
+		}
+	}
+}
+
+/*
+ * usb_dfu_upload
+ *         Handles the DFU UPLOAD request.
+ * pdev: instance
+ * req: pointer to the request structure
+ */
+static void usb_dfu_upload(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+	uintptr_t data_ptr;
+	uint32_t length;
+	int ret;
+
+	/* Data setup request */
+	if (req->length == 0) {
+		/* No Data setup request */
+		hdfu->dev_state = STATE_DFU_IDLE;
+		return;
+	}
+
+	/* Unsupported state */
+	if ((hdfu->dev_state != STATE_DFU_IDLE) && (hdfu->dev_state != STATE_DFU_UPLOAD_IDLE)) {
+		ERROR("UPLOAD : Unsupported State\n");
+		/* Call the error management function (command will be nacked) */
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	/* Update the data address */
+	length = req->length;
+	ret = hdfu->callback->upload(hdfu->alt_setting, &data_ptr, &length, pdev->user_data);
+	if (ret == 0U) {
+		/* Short frame */
+		hdfu->dev_state = (req->length > length) ? STATE_DFU_IDLE : STATE_DFU_UPLOAD_IDLE;
+
+		/* Start the transfer */
+		usb_core_transmit_ep0(pdev, (uint8_t *)data_ptr, length);
+	} else {
+		ERROR("UPLOAD : bad block %i on alt %i\n", req->value, req->index);
+		hdfu->dev_state = STATE_DFU_ERROR;
+		hdfu->dev_status = DFU_ERROR_STALLEDPKT;
+
+		/* Call the error management function (command will be nacked) */
+		usb_core_ctl_error(pdev);
+	}
+}
+
+/*
+ * usb_dfu_get_status
+ *         Handles the DFU GETSTATUS request.
+ * pdev: instance
+ */
+static void usb_dfu_get_status(struct usb_handle *pdev)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	hdfu->status[0] = hdfu->dev_status;	/* bStatus */
+	hdfu->status[1] = 0;			/* bwPollTimeout[3] */
+	hdfu->status[2] = 0;
+	hdfu->status[3] = 0;
+	hdfu->status[4] = hdfu->dev_state;	/* bState */
+	hdfu->status[5] = 0;			/* iString */
+
+	/* next step */
+	switch (hdfu->dev_state) {
+	case STATE_DFU_DNLOAD_SYNC:
+		hdfu->dev_state = STATE_DFU_DNLOAD_IDLE;
+		break;
+	case STATE_DFU_MANIFEST_SYNC:
+		/* the device is 'ManifestationTolerant' */
+		hdfu->status[4] = STATE_DFU_MANIFEST;
+		hdfu->status[1] = 1U; /* bwPollTimeout = 1ms */
+		hdfu->dev_state = STATE_DFU_IDLE;
+		break;
+
+	default:
+		break;
+	}
+
+	/* Start the transfer */
+	usb_core_transmit_ep0(pdev, (uint8_t *)&hdfu->status[0], sizeof(hdfu->status));
+}
+
+/*
+ * usb_dfu_clear_status
+ *         Handles the DFU CLRSTATUS request.
+ * pdev: device instance
+ */
+static void usb_dfu_clear_status(struct usb_handle *pdev)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	if (hdfu->dev_state == STATE_DFU_ERROR) {
+		hdfu->dev_state = STATE_DFU_IDLE;
+		hdfu->dev_status = DFU_ERROR_NONE;
+	} else {
+		/* State Error */
+		hdfu->dev_state = STATE_DFU_ERROR;
+		hdfu->dev_status = DFU_ERROR_UNKNOWN;
+	}
+}
+
+/*
+ * usb_dfu_get_state
+ *         Handles the DFU GETSTATE request.
+ * pdev: device instance
+ */
+static void usb_dfu_get_state(struct usb_handle *pdev)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	/* Return the current state of the DFU interface */
+	usb_core_transmit_ep0(pdev, &hdfu->dev_state, 1);
+}
+
+/*
+ * usb_dfu_abort
+ *         Handles the DFU ABORT request.
+ * pdev: device instance
+ */
+static void usb_dfu_abort(struct usb_handle *pdev)
+{
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	if ((hdfu->dev_state == STATE_DFU_IDLE) ||
+	    (hdfu->dev_state == STATE_DFU_DNLOAD_SYNC) ||
+	    (hdfu->dev_state == STATE_DFU_DNLOAD_IDLE) ||
+	    (hdfu->dev_state == STATE_DFU_MANIFEST_SYNC) ||
+	    (hdfu->dev_state == STATE_DFU_UPLOAD_IDLE)) {
+		hdfu->dev_state = STATE_DFU_IDLE;
+		hdfu->dev_status = DFU_ERROR_NONE;
+	}
+}
+
+/*
+ * usb_dfu_setup
+ *         Handle the DFU specific requests
+ * pdev: instance
+ * req: usb requests
+ * return: status
+ */
+static uint8_t usb_dfu_setup(struct usb_handle *pdev, struct usb_setup_req *req)
+{
+	uint8_t *pbuf = NULL;
+	uint16_t len = 0U;
+	uint8_t ret = USBD_OK;
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	switch (req->bm_request & USB_REQ_TYPE_MASK) {
+	case USB_REQ_TYPE_CLASS:
+		switch (req->b_request) {
+		case DFU_DNLOAD:
+			usb_dfu_download(pdev, req);
+			break;
+
+		case DFU_UPLOAD:
+			usb_dfu_upload(pdev, req);
+			break;
+
+		case DFU_GETSTATUS:
+			usb_dfu_get_status(pdev);
+			break;
+
+		case DFU_CLRSTATUS:
+			usb_dfu_clear_status(pdev);
+			break;
+
+		case DFU_GETSTATE:
+			usb_dfu_get_state(pdev);
+			break;
+
+		case DFU_ABORT:
+			usb_dfu_abort(pdev);
+			break;
+
+		case DFU_DETACH:
+			usb_dfu_detach(pdev, req);
+			break;
+
+		default:
+			ERROR("unknown request %x on alternate %i\n",
+			      req->b_request, hdfu->alt_setting);
+			usb_core_ctl_error(pdev);
+			ret = USBD_FAIL;
+			break;
+		}
+		break;
+	case USB_REQ_TYPE_STANDARD:
+		switch (req->b_request) {
+		case USB_REQ_GET_DESCRIPTOR:
+			if (HIBYTE(req->value) == DFU_DESCRIPTOR_TYPE) {
+				pbuf = pdev->desc->get_config_desc(&len);
+				/* DFU descriptor at the end of the USB */
+				pbuf += len - 9U;
+				len = 9U;
+				len = MIN(len, req->length);
+			}
+
+			/* Start the transfer */
+			usb_core_transmit_ep0(pdev, pbuf, len);
+
+			break;
+
+		case USB_REQ_GET_INTERFACE:
+			/* Start the transfer */
+			usb_core_transmit_ep0(pdev, (uint8_t *)&hdfu->alt_setting, 1U);
+			break;
+
+		case USB_REQ_SET_INTERFACE:
+			hdfu->alt_setting = LOBYTE(req->value);
+			break;
+
+		default:
+			usb_core_ctl_error(pdev);
+			ret = USBD_FAIL;
+			break;
+		}
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static const struct usb_class usb_dfu = {
+	.init = usb_dfu_init,
+	.de_init = usb_dfu_de_init,
+	.setup = usb_dfu_setup,
+	.ep0_tx_sent = usb_dfu_ep0_tx_ready,
+	.ep0_rx_ready = usb_dfu_ep0_rx_ready,
+	.data_in = usb_dfu_data_in,
+	.data_out = usb_dfu_data_out,
+	.sof = usb_dfu_sof,
+	.iso_in_incomplete = usb_dfu_iso_in_incomplete,
+	.iso_out_incomplete = usb_dfu_iso_out_incomplete,
+};
+
+void usb_dfu_register(struct usb_handle *pdev, struct usb_dfu_handle *phandle)
+{
+	pdev->class = (struct usb_class *)&usb_dfu;
+	pdev->class_data = phandle;
+
+	phandle->dev_state = STATE_DFU_IDLE;
+	phandle->dev_status = DFU_ERROR_NONE;
+}
+
+int usb_dfu_loop(struct usb_handle *pdev, const struct usb_dfu_media *pmedia)
+{
+	uint32_t it_count;
+	enum usb_status ret;
+	struct usb_dfu_handle *hdfu = (struct usb_dfu_handle *)pdev->class_data;
+
+	hdfu->callback = pmedia;
+	usb_dfu_detach_req = false;
+	/* Continue to handle USB core IT to assure complete data transmission */
+	it_count = 100U;
+
+	/* DFU infinite loop until DETACH_REQ */
+	while (it_count != 0U) {
+		ret = usb_core_handle_it(pdev);
+		if (ret != USBD_OK) {
+			return -EIO;
+		}
+
+		/* Detach request received */
+		if (usb_dfu_detach_req) {
+			it_count--;
+		}
+	}
+
+	return 0;
+}
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index e87c529..7eaf0ed 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -490,5 +490,19 @@
 
 void bl2_el3_plat_prepare_exit(void)
 {
+	uint16_t boot_itf = stm32mp_get_boot_itf_selected();
+
+	switch (boot_itf) {
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		/* Invalidate the downloaded buffer used with io_memmap */
+		inv_dcache_range(DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+		break;
+#endif
+	default:
+		/* Do nothing in default case */
+		break;
+	}
+
 	stm32mp1_security_setup();
 }
diff --git a/plat/st/stm32mp1/include/boot_api.h b/plat/st/stm32mp1/include/boot_api.h
index c16639a..52b1d1a 100644
--- a/plat/st/stm32mp1/include/boot_api.h
+++ b/plat/st/stm32mp1/include/boot_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -39,6 +39,9 @@
 /* Boot occurred on QSPI NOR */
 #define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI		0x4U
 
+/* Boot occurred on USB */
+#define BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB		0x6U
+
 /* Boot occurred on QSPI NAND */
 #define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI		0x7U
 
diff --git a/plat/st/stm32mp1/include/platform_def.h b/plat/st/stm32mp1/include/platform_def.h
index 8a065bf..1e9443e 100644
--- a/plat/st/stm32mp1/include/platform_def.h
+++ b/plat/st/stm32mp1/include/platform_def.h
@@ -92,6 +92,10 @@
  */
 #define PLAT_STM32MP_NS_IMAGE_OFFSET	BL33_BASE
 
+/* Needed by STM32CubeProgrammer support */
+#define DWL_BUFFER_BASE			(STM32MP_DDR_BASE + U(0x08000000))
+#define DWL_BUFFER_SIZE			U(0x08000000)
+
 /*
  * SSBL offset in case it's stored in eMMC boot partition.
  * We can fix it to 256K because TF-A size can't be bigger than SRAM
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 1c2c9f0..c8c2e5f 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -53,6 +53,9 @@
 STM32MP_SPI_NOR		?=	0
 STM32MP_EMMC_BOOT	?=	0
 
+# Serial boot devices
+STM32MP_USB_PROGRAMMER	?=	0
+
 # Device tree
 DTB_FILE_NAME		?=	stm32mp157c-ev1.dtb
 ifeq ($(STM32MP_USE_STM32IMAGE),1)
@@ -127,6 +130,7 @@
 		STM32MP_SPI_NOR \
 		STM32MP_EMMC_BOOT \
 		PLAT_XLAT_TABLES_DYNAMIC \
+		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
 )))
 
@@ -147,6 +151,7 @@
 		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32_TF_A_COPIES \
 		PLAT_PARTITION_MAX_ENTRIES \
+		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
 )))
 
@@ -154,13 +159,15 @@
 PLAT_INCLUDES		:=	-Iplat/st/common/include/
 PLAT_INCLUDES		+=	-Iplat/st/stm32mp1/include/
 
+include common/fdt_wrappers.mk
 include lib/libfdt/libfdt.mk
 
-PLAT_BL_COMMON_SOURCES	:=	common/fdt_wrappers.c					\
-				common/uuid.c						\
+PLAT_BL_COMMON_SOURCES	:=	common/uuid.c						\
 				plat/st/common/stm32mp_common.c				\
 				plat/st/stm32mp1/stm32mp1_private.c
 
+PLAT_BL_COMMON_SOURCES	+=	${FDT_WRAPPERS_SOURCES}
+
 PLAT_BL_COMMON_SOURCES	+=	drivers/st/uart/aarch32/stm32_console.S
 
 ifneq (${ENABLE_STACK_PROTECTOR},0)
@@ -249,6 +256,17 @@
 BL2_SOURCES		+=	plat/st/stm32mp1/stm32mp1_boot_device.c
 endif
 
+ifeq (${STM32MP_USB_PROGRAMMER},1)
+#The DFU stack uses only one end point, reduce the USB stack footprint
+$(eval $(call add_define_val,CONFIG_USBD_EP_NB,1U))
+BL2_SOURCES		+=	drivers/io/io_memmap.c					\
+				drivers/st/usb/stm32mp1_usb.c				\
+				drivers/usb/usb_device.c				\
+				plat/st/common/stm32cubeprogrammer_usb.c		\
+				plat/st/common/usb_dfu.c					\
+				plat/st/stm32mp1/stm32mp1_usb_dfu.c
+endif
+
 BL2_SOURCES		+=	drivers/st/ddr/stm32mp1_ddr.c				\
 				drivers/st/ddr/stm32mp1_ram.c
 
@@ -272,7 +290,8 @@
 	    [ ${STM32MP_SDMMC} != 1 ] && \
 	    [ ${STM32MP_RAW_NAND} != 1 ] && \
 	    [ ${STM32MP_SPI_NAND} != 1 ] && \
-	    [ ${STM32MP_SPI_NOR} != 1 ]; then \
+	    [ ${STM32MP_SPI_NOR} != 1 ] && \
+	    [ ${STM32MP_USB_PROGRAMMER} != 1 ]; then \
 		echo "No boot device driver is enabled"; \
 		false; \
 	fi
@@ -323,7 +342,7 @@
 		-c plat/st/stm32mp1/stm32mp1.S -o $@
 endif
 
-$(eval $(call MAKE_LD,${STM32_TF_LINKERFILE},plat/st/stm32mp1/stm32mp1.ld.S,2))
+$(eval $(call MAKE_LD,${STM32_TF_LINKERFILE},plat/st/stm32mp1/stm32mp1.ld.S,bl2))
 
 tf-a-%.elf: stm32mp1-%.o ${STM32_TF_LINKERFILE}
 	@echo "  LDS     $<"
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index 469c2d9..f5d4b2f 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -339,6 +339,9 @@
 #define DATA0_OTP			U(0)
 #define PART_NUMBER_OTP			U(1)
 #define NAND_OTP			U(9)
+#define UID0_OTP			U(13)
+#define UID1_OTP			U(14)
+#define UID2_OTP			U(15)
 #define PACKAGE_OTP			U(16)
 #define HW2_OTP				U(18)
 
@@ -401,6 +404,9 @@
 /* NAND number of planes */
 #define NAND_PLANE_BIT_NB_MASK		BIT(14)
 
+/* UID OTP */
+#define UID_WORD_NB			U(3)
+
 /*******************************************************************************
  * STM32MP1 TAMP
  ******************************************************************************/
@@ -415,6 +421,11 @@
 #endif
 
 /*******************************************************************************
+ * STM32MP1 USB
+ ******************************************************************************/
+#define USB_OTG_BASE			U(0x49000000)
+
+/*******************************************************************************
  * STM32MP1 DDRCTRL
  ******************************************************************************/
 #define DDRCTRL_BASE			U(0x5A003000)
diff --git a/plat/st/stm32mp1/stm32mp1_shared_resources.c b/plat/st/stm32mp1/stm32mp1_shared_resources.c
index 208e34a..6b1bcaa 100644
--- a/plat/st/stm32mp1/stm32mp1_shared_resources.c
+++ b/plat/st/stm32mp1/stm32mp1_shared_resources.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -114,7 +114,7 @@
 	if (bank != GPIO_BANK_Z) {
 		int count = fdt_get_gpio_bank_pin_count(bank);
 
-		assert((count >= 0) || (count <= (GPIO_PIN_MAX + 1)));
+		assert((count >= 0) && ((unsigned int)count <= (GPIO_PIN_MAX + 1)));
 
 		return (unsigned int)count;
 	}
@@ -163,7 +163,7 @@
 
 	if ((id >= STM32MP1_SHRES_GPIOZ(0)) &&
 	    (id <= STM32MP1_SHRES_GPIOZ(7)) &&
-	    ((id - STM32MP1_SHRES_GPIOZ(0)) >= get_gpioz_nbpin())) {
+	    ((unsigned int)(id - STM32MP1_SHRES_GPIOZ(0)) >= get_gpioz_nbpin())) {
 		ERROR("Invalid GPIO pin %u, %u pin(s) available\n",
 		      id - STM32MP1_SHRES_GPIOZ(0), get_gpioz_nbpin());
 		panic();
diff --git a/plat/st/stm32mp1/stm32mp1_usb_dfu.c b/plat/st/stm32mp1/stm32mp1_usb_dfu.c
new file mode 100644
index 0000000..051d435
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_usb_dfu.c
@@ -0,0 +1,388 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <limits.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/stm32mp1_usb.h>
+#include <drivers/usb_device.h>
+
+#include <platform_def.h>
+#include <stm32cubeprogrammer.h>
+#include <stm32mp_common.h>
+#include <usb_dfu.h>
+
+/*  String size (1 byte) + type (1 byte) + 24 UTF16 characters: 2 bytes each */
+#define SIZ_STRING_SERIAL		U(24)
+#define USB_SIZ_STRING_SERIAL		(1U + 1U + (SIZ_STRING_SERIAL * 2U))
+#define USBD_MAX_STR_DESC_SIZ		0x100
+#define USBD_VID			0x0483
+#define USBD_PID			0xDF11
+#define USBD_LANGID_STRING		0x409
+#define USBD_MANUFACTURER_STRING	"STMicroelectronics"
+#define USBD_CONFIGURATION_STRING	"DFU Config"
+#define USBD_INTERFACE_STRING		"DFU Interface"
+
+#define USB_DFU_ITF_NUM			6
+
+#define USB_DFU_CONFIG_DESC_SIZ		USB_DFU_DESC_SIZ(USB_DFU_ITF_NUM)
+
+/* DFU devices */
+static struct usb_dfu_handle usb_dfu_handle;
+
+/* USB Standard Device Descriptor */
+static const uint8_t usb_stm32mp1_desc[USB_LEN_DEV_DESC] = {
+	USB_LEN_DEV_DESC,           /* bLength */
+	USB_DESC_TYPE_DEVICE,       /* bDescriptorType */
+	0x00,                       /* bcdUSB */
+	0x02,                       /* version */
+	0x00,                       /* bDeviceClass */
+	0x00,                       /* bDeviceSubClass */
+	0x00,                       /* bDeviceProtocol */
+	USB_MAX_EP0_SIZE,           /* bMaxPacketSize */
+	LOBYTE(USBD_VID),           /* idVendor */
+	HIBYTE(USBD_VID),           /* idVendor */
+	LOBYTE(USBD_PID),           /* idVendor */
+	HIBYTE(USBD_PID),           /* idVendor */
+	0x00,                       /* bcdDevice rel. 2.00 */
+	0x02,
+	USBD_IDX_MFC_STR,           /* Index of manufacturer string */
+	USBD_IDX_PRODUCT_STR,       /* Index of product string */
+	USBD_IDX_SERIAL_STR,        /* Index of serial number string */
+	USBD_MAX_NUM_CONFIGURATION  /* bNumConfigurations */
+}; /* USB_DeviceDescriptor */
+
+/* USB Standard String Descriptor */
+static const uint8_t usb_stm32mp1_lang_id_desc[USB_LEN_LANGID_STR_DESC] = {
+	USB_LEN_LANGID_STR_DESC,
+	USB_DESC_TYPE_STRING,
+	LOBYTE(USBD_LANGID_STRING),
+	HIBYTE(USBD_LANGID_STRING),
+};
+
+/* USB Standard Device Descriptor */
+static const uint8_t
+usbd_stm32mp1_qualifier_desc[USB_LEN_DEV_QUALIFIER_DESC] = {
+	USB_LEN_DEV_QUALIFIER_DESC,
+	USB_DESC_TYPE_DEVICE_QUALIFIER,
+	0x00,
+	0x02,
+	0x00,
+	0x00,
+	0x00,
+	0x40,
+	0x01,
+	0x00,
+};
+
+/* USB serial number: build dynamically */
+static uint8_t usb_stm32mp1_serial[USB_SIZ_STRING_SERIAL + 1];
+
+/* USB DFU device Configuration Descriptor */
+static const uint8_t usb_stm32mp1_config_desc[USB_DFU_CONFIG_DESC_SIZ] = {
+	0x09, /* bLength: Configuration Descriptor size */
+	USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */
+	USB_DFU_CONFIG_DESC_SIZ, /* wTotalLength: Bytes returned */
+	0x00,
+	0x01, /* bNumInterfaces: 1 interface */
+	0x01, /* bConfigurationValue: Configuration value */
+	0x02, /* iConfiguration: Index of string descriptor for configuration */
+	0xC0, /* bmAttributes: bus powered and Supprts Remote Wakeup */
+	0x32, /* MaxPower 100 mA: this current is used for detecting Vbus */
+
+	/* Descriptor of DFU interface 0 Alternate setting 0..N */
+	USBD_DFU_IF_DESC(0),
+	USBD_DFU_IF_DESC(1),
+	USBD_DFU_IF_DESC(2),
+	USBD_DFU_IF_DESC(3),
+	USBD_DFU_IF_DESC(4),
+	USBD_DFU_IF_DESC(5),
+
+	/* DFU Functional Descriptor */
+	0x09, /* blength = 9 Bytes */
+	DFU_DESCRIPTOR_TYPE, /* DFU Functional Descriptor */
+	DFU_BM_ATTRIBUTE, /* bmAttribute for DFU */
+	0xFF, /* DetachTimeOut = 255 ms */
+	0x00,
+	TRANSFER_SIZE_BYTES(USBD_DFU_XFER_SIZE), /* TransferSize = 1024 Byte */
+	((USB_DFU_VERSION >> 0) & 0xFF), /* bcdDFUVersion */
+	((USB_DFU_VERSION >> 8) & 0xFF)
+};
+
+/* The user strings: one by alternate as defined in USBD_DFU_IF_DESC */
+const char *const if_desc_string[USB_DFU_ITF_NUM] = {
+	"@Partition0 /0x00/1*256Ke",
+	"@FSBL /0x01/1*1Me",
+	"@Partition2 /0x02/1*1Me",
+	"@Partition3 /0x03/1*16Me",
+	"@Partition4 /0x04/1*16Me",
+	"@virtual /0xF1/1*512Ba"
+};
+
+/* Buffer to build the unicode string provided to USB device stack */
+static uint8_t usb_str_dec[USBD_MAX_STR_DESC_SIZ];
+
+/*
+ * Convert Ascii string into unicode one
+ * desc : descriptor buffer
+ * unicode : Formatted string buffer (unicode)
+ * len : descriptor length
+ */
+static void stm32mp1_get_string(const char *desc, uint8_t *unicode, uint16_t *len)
+{
+	uint8_t idx = 0U;
+
+	if (desc == NULL) {
+		return;
+	}
+
+	*len =  strlen(desc) * 2U + 2U;
+	unicode[idx++] = *len;
+	unicode[idx++] =  USB_DESC_TYPE_STRING;
+
+	while (*desc != '\0') {
+		unicode[idx++] = *desc++;
+		unicode[idx++] =  0x00U;
+	}
+}
+
+/*
+ * Create the serial number string descriptor
+ */
+static void update_serial_num_string(void)
+{
+	uint8_t i;
+	uint32_t result;
+	char serial_string[SIZ_STRING_SERIAL + 2U];
+	uint32_t deviceserial[UID_WORD_NB];
+	uint16_t length;
+
+	for (i = 0U; i < UID_WORD_NB; i++) {
+		result = bsec_shadow_register(i + UID0_OTP);
+		if (result != BSEC_OK) {
+			ERROR("BSEC: UID%d Shadowing Error\n", i);
+			break;
+		}
+		result = bsec_read_otp(&deviceserial[i], i + UID0_OTP);
+		if (result != BSEC_OK) {
+			ERROR("BSEC: UID%d Read Error\n", i);
+			break;
+		}
+	}
+	/* On bsec error: serial number is set to 0 */
+	if (result != BSEC_OK) {
+		for (i = 0; i < UID_WORD_NB; i++) {
+			deviceserial[i] = 0U;
+		}
+	}
+	/* build serial number with OTP value as in ROM code */
+	snprintf(serial_string, sizeof(serial_string), "%08X%08X%08X",
+		 deviceserial[0], deviceserial[1], deviceserial[2]);
+
+	length = USB_SIZ_STRING_SERIAL;
+	stm32mp1_get_string(serial_string, usb_stm32mp1_serial, &length);
+}
+
+/*
+ * Return Device Qualifier descriptor
+ * length : pointer data length
+ * return : pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_get_qualifier_desc(uint16_t *length)
+{
+	*length = sizeof(usbd_stm32mp1_qualifier_desc);
+
+	return (uint8_t *)usbd_stm32mp1_qualifier_desc;
+}
+
+/*
+ * Return configuration descriptor
+ * length : pointer data length
+ * return : pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_get_config_desc(uint16_t *length)
+{
+	*length = sizeof(usb_stm32mp1_config_desc);
+
+	return (uint8_t *)usb_stm32mp1_config_desc;
+}
+
+/*
+ * Returns the device descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_device_desc(uint16_t *length)
+{
+	*length = sizeof(usb_stm32mp1_desc);
+
+	return (uint8_t *)usb_stm32mp1_desc;
+}
+
+/*
+ * Returns the LangID string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_lang_id_desc(uint16_t *length)
+{
+	*length = sizeof(usb_stm32mp1_lang_id_desc);
+
+	return (uint8_t *)usb_stm32mp1_lang_id_desc;
+}
+
+/*
+ *  Returns the product string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_product_desc(uint16_t *length)
+{
+	char name[STM32_SOC_NAME_SIZE];
+	char product[128];
+	uint32_t chip_id;
+	uint32_t chip_version;
+
+	stm32mp_get_soc_name(name);
+	chip_id = stm32mp_get_chip_dev_id();
+	chip_version = stm32mp_get_chip_version();
+
+	snprintf(product, sizeof(product),
+		 "DFU @Device ID /0x%03X, @Revision ID /0x%04X, @Name /%s,",
+		 chip_id, chip_version, name);
+
+	stm32mp1_get_string(product, usb_str_dec, length);
+
+	return usb_str_dec;
+}
+
+/*
+ * Returns the manufacturer string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_manufacturer_desc(uint16_t *length)
+{
+	stm32mp1_get_string(USBD_MANUFACTURER_STRING, usb_str_dec, length);
+
+	return usb_str_dec;
+}
+
+/*
+ * Returns the serial number string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_serial_desc(uint16_t *length)
+{
+	*length = USB_SIZ_STRING_SERIAL;
+
+	return (uint8_t *)usb_stm32mp1_serial;
+}
+
+/*
+ * Returns the configuration string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_config_desc(uint16_t *length)
+{
+	stm32mp1_get_string(USBD_CONFIGURATION_STRING, usb_str_dec, length);
+
+	return usb_str_dec;
+}
+
+/*
+ * Returns the interface string descriptor.
+ * length : Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_interface_desc(uint16_t *length)
+{
+	stm32mp1_get_string(USBD_INTERFACE_STRING, usb_str_dec, length);
+
+	return usb_str_dec;
+}
+
+/*
+ * Manages the transfer of memory interfaces string descriptors.
+ * index: descriptor index
+ * length : pointer data length
+ * return : pointer to the descriptor table or NULL if the descriptor
+ *          is not supported.
+ */
+static uint8_t *stm32mp1_get_usr_desc(uint8_t index, uint16_t *length)
+{
+	if (index >= ARRAY_SIZE(if_desc_string)) {
+		return NULL;
+	}
+
+	stm32mp1_get_string(if_desc_string[index], usb_str_dec, length);
+
+	return usb_str_dec;
+}
+
+static const struct usb_desc dfu_desc = {
+	.get_device_desc = stm32mp1_device_desc,
+	.get_lang_id_desc = stm32mp1_lang_id_desc,
+	.get_manufacturer_desc = stm32mp1_manufacturer_desc,
+	.get_product_desc = stm32mp1_product_desc,
+	.get_configuration_desc = stm32mp1_config_desc,
+	.get_serial_desc = stm32mp1_serial_desc,
+	.get_interface_desc = stm32mp1_interface_desc,
+	.get_usr_desc = stm32mp1_get_usr_desc,
+	.get_config_desc = stm32mp1_get_config_desc,
+	.get_device_qualifier_desc = stm32mp1_get_qualifier_desc,
+};
+
+static struct usb_handle usb_core_handle;
+static struct pcd_handle pcd_handle;
+
+struct usb_handle *usb_dfu_plat_init(void)
+{
+	/* Prepare USB Driver */
+	pcd_handle.in_ep[0].maxpacket = USB_MAX_EP0_SIZE;
+	pcd_handle.out_ep[0].maxpacket = USB_MAX_EP0_SIZE;
+	stm32mp1_usb_init_driver(&usb_core_handle, &pcd_handle,
+				 (uint32_t *)USB_OTG_BASE);
+
+	/* STM32MP15 = keep the configuration from ROM code */
+	usb_core_handle.ep0_state = USBD_EP0_DATA_IN;
+	usb_core_handle.dev_state = USBD_STATE_CONFIGURED;
+
+	/* Update the serial number string descriptor from the unique ID */
+	update_serial_num_string();
+
+	/* Prepare USB DFU stack */
+	usb_dfu_register(&usb_core_handle, &usb_dfu_handle);
+
+	/* Register DFU descriptor in USB stack */
+	register_platform(&usb_core_handle, &dfu_desc);
+
+	return &usb_core_handle;
+}
+
+/* Link between USB alternate and STM32CubeProgramer phase */
+uint8_t usb_dfu_get_phase(uint8_t alt)
+{
+	uint8_t ret;
+
+	switch (alt) {
+	case 3:
+		ret = PHASE_SSBL;
+		break;
+	case 5:
+		ret = PHASE_CMD;
+		break;
+	default:
+		ret = PHASE_RESET;
+		break;
+	}
+
+	return ret;
+}
diff --git a/services/std_svc/sdei/sdei_intr_mgmt.c b/services/std_svc/sdei/sdei_intr_mgmt.c
index 5d176c2..399c2ec 100644
--- a/services/std_svc/sdei/sdei_intr_mgmt.c
+++ b/services/std_svc/sdei/sdei_intr_mgmt.c
@@ -531,7 +531,7 @@
 	if (is_event_shared(map))
 		sdei_map_unlock(map);
 
-	SDEI_LOG("ACK %llx, ev:%d ss:%d spsr:%lx ELR:%lx\n", mpidr, map->ev_num,
+	SDEI_LOG("ACK %llx, ev:0x%x ss:%d spsr:%lx ELR:%lx\n", mpidr, map->ev_num,
 			sec_state, read_spsr_el3(), read_elr_el3());
 
 	ctx = handle;
@@ -568,7 +568,7 @@
 	 * interrupt.
 	 */
 	if ((map->ev_num != SDEI_EVENT_0) && !is_map_bound(map)) {
-		ERROR("Invalid SDEI mapping: ev=%u\n", map->ev_num);
+		ERROR("Invalid SDEI mapping: ev=0x%x\n", map->ev_num);
 		panic();
 	}
 	plat_ic_end_of_interrupt(intr_raw);
@@ -703,7 +703,7 @@
 	/* Having done sanity checks, pop dispatch */
 	(void) pop_dispatch();
 
-	SDEI_LOG("EOI:%lx, %d spsr:%lx elr:%lx\n", read_mpidr_el1(),
+	SDEI_LOG("EOI:%lx, 0x%x spsr:%lx elr:%lx\n", read_mpidr_el1(),
 			map->ev_num, read_spsr_el3(), read_elr_el3());
 
 	/*
diff --git a/services/std_svc/sdei/sdei_main.c b/services/std_svc/sdei/sdei_main.c
index 5371df1..4ceaae8 100644
--- a/services/std_svc/sdei/sdei_main.c
+++ b/services/std_svc/sdei/sdei_main.c
@@ -966,7 +966,7 @@
 
 	case SDEI_EVENT_REGISTER:
 		x5 = SMC_GET_GP(ctx, CTX_GPREG_X5);
-		SDEI_LOG("> REG(n:%d e:%llx a:%llx f:%x m:%llx)\n", ev_num,
+		SDEI_LOG("> REG(n:0x%x e:%llx a:%llx f:%x m:%llx)\n", ev_num,
 				x2, x3, (int) x4, x5);
 		ret = sdei_event_register(ev_num, x2, x3, x4, x5);
 		SDEI_LOG("< REG:%lld\n", ret);
@@ -979,7 +979,7 @@
 		SMC_RET1(ctx, ret);
 
 	case SDEI_EVENT_DISABLE:
-		SDEI_LOG("> DISABLE(n:%d)\n", ev_num);
+		SDEI_LOG("> DISABLE(n:0x%x)\n", ev_num);
 		ret = sdei_event_disable(ev_num);
 		SDEI_LOG("< DISABLE:%lld\n", ret);
 		SMC_RET1(ctx, ret);
@@ -1013,19 +1013,19 @@
 		SMC_RET0(ctx);
 
 	case SDEI_EVENT_STATUS:
-		SDEI_LOG("> STAT(n:%d)\n", ev_num);
+		SDEI_LOG("> STAT(n:0x%x)\n", ev_num);
 		ret = sdei_event_status(ev_num);
 		SDEI_LOG("< STAT:%lld\n", ret);
 		SMC_RET1(ctx, ret);
 
 	case SDEI_EVENT_GET_INFO:
-		SDEI_LOG("> INFO(n:%d, %d)\n", ev_num, (int) x2);
+		SDEI_LOG("> INFO(n:0x%x, %d)\n", ev_num, (int) x2);
 		ret = sdei_event_get_info(ev_num, (int) x2);
 		SDEI_LOG("< INFO:%lld\n", ret);
 		SMC_RET1(ctx, ret);
 
 	case SDEI_EVENT_UNREGISTER:
-		SDEI_LOG("> UNREG(n:%d)\n", ev_num);
+		SDEI_LOG("> UNREG(n:0x%x)\n", ev_num);
 		ret = sdei_event_unregister(ev_num);
 		SDEI_LOG("< UNREG:%lld\n", ret);
 		SMC_RET1(ctx, ret);
@@ -1049,7 +1049,7 @@
 		SMC_RET1(ctx, ret);
 
 	case SDEI_INTERRUPT_RELEASE:
-		SDEI_LOG("> REL(%d)\n", ev_num);
+		SDEI_LOG("> REL(0x%x)\n", ev_num);
 		ret = sdei_interrupt_release(ev_num);
 		SDEI_LOG("< REL:%lld\n", ret);
 		SMC_RET1(ctx, ret);
@@ -1067,7 +1067,7 @@
 		SMC_RET1(ctx, ret);
 
 	case SDEI_EVENT_ROUTING_SET:
-		SDEI_LOG("> ROUTE_SET(n:%d f:%llx aff:%llx)\n", ev_num, x2, x3);
+		SDEI_LOG("> ROUTE_SET(n:0x%x f:%llx aff:%llx)\n", ev_num, x2, x3);
 		ret = sdei_event_routing_set(ev_num, x2, x3);
 		SDEI_LOG("< ROUTE_SET:%lld\n", ret);
 		SMC_RET1(ctx, ret);
@@ -1079,7 +1079,7 @@
 		SMC_RET1(ctx, ret);
 
 	case SDEI_EVENT_SIGNAL:
-		SDEI_LOG("> SIGNAL(e:%d t:%llx)\n", ev_num, x2);
+		SDEI_LOG("> SIGNAL(e:0x%x t:%llx)\n", ev_num, x2);
 		ret = sdei_signal(ev_num, x2);
 		SDEI_LOG("< SIGNAL:%lld\n", ret);
 		SMC_RET1(ctx, ret);