diff --git a/Makefile b/Makefile
index 4f3f261..f736a3b 100644
--- a/Makefile
+++ b/Makefile
@@ -646,6 +646,13 @@
 endif
 
 ################################################################################
+# Make 128-Bit sysreg read/writes availabe when FEAT_D128 is enabled.
+################################################################################
+ifneq (${ENABLE_FEAT_D128}, 0)
+        BL_COMMON_SOURCES       +=      lib/extensions/sysreg128/sysreg128.S
+endif
+
+################################################################################
 # Platform specific Makefile might provide us ARCH_MAJOR/MINOR use that to come
 # up with appropriate march values for compiler.
 ################################################################################
@@ -1263,6 +1270,7 @@
 	ENABLE_FEAT_S2POE \
 	ENABLE_FEAT_S1POE \
 	ENABLE_FEAT_SCTLR2 \
+	ENABLE_FEAT_D128 \
 	ENABLE_FEAT_GCS \
 	ENABLE_FEAT_VHE \
 	ENABLE_FEAT_MPAM \
@@ -1421,6 +1429,7 @@
 	ENABLE_FEAT_S2POE \
 	ENABLE_FEAT_S1POE \
 	ENABLE_FEAT_SCTLR2 \
+	ENABLE_FEAT_D128 \
 	ENABLE_FEAT_GCS \
 	ENABLE_FEAT_MTE2 \
 	FEATURE_DETECTION \
diff --git a/common/bl_common.c b/common/bl_common.c
index 2a9f32f..2c452aa 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -150,8 +150,7 @@
  * of trust.
  */
 static int load_auth_image_recursive(unsigned int image_id,
-				    image_info_t *image_data,
-				    int is_parent_image)
+				    image_info_t *image_data)
 {
 	int rc;
 	unsigned int parent_id;
@@ -159,7 +158,7 @@
 	/* Use recursion to authenticate parent images */
 	rc = auth_mod_get_parent_id(image_id, &parent_id);
 	if (rc == 0) {
-		rc = load_auth_image_recursive(parent_id, image_data, 1);
+		rc = load_auth_image_recursive(parent_id, image_data);
 		if (rc != 0) {
 			return rc;
 		}
@@ -193,7 +192,7 @@
 {
 #if TRUSTED_BOARD_BOOT
 	if (dyn_is_auth_disabled() == 0) {
-		return load_auth_image_recursive(image_id, image_data, 0);
+		return load_auth_image_recursive(image_id, image_data);
 	}
 #endif
 
diff --git a/common/feat_detect.c b/common/feat_detect.c
index e63eec4..6aa5e2e 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -269,6 +269,12 @@
 			     ID_AA64MMFR3_EL1_SCTLR2_MASK);
 }
 
+static unsigned int read_feat_d128_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr3_el1(), ID_AA64MMFR3_EL1_D128_SHIFT,
+			     ID_AA64MMFR3_EL1_D128_MASK);
+}
+
 /***********************************************************************************
  * TF-A supports many Arm architectural features starting from arch version
  * (8.0 till 8.7+). These features are mostly enabled through build flags. This
@@ -394,6 +400,10 @@
 	check_feature(ENABLE_SME2_FOR_NS, read_feat_sme_id_field(),
 		      "SME2", 2, 2);
 
+	/* v9.3 features */
+	check_feature(ENABLE_FEAT_D128, read_feat_d128_id_field(),
+		      "D128", 1, 1);
+
 	/* v9.4 features */
 	check_feature(ENABLE_FEAT_GCS, read_feat_gcs_id_field(), "GCS", 1, 1);
 	check_feature(ENABLE_RME, read_feat_rme_id_field(), "RME", 1, 1);
diff --git a/docs/design_documents/measured_boot.rst b/docs/design_documents/measured_boot.rst
index 9dfe494..005903e 100644
--- a/docs/design_documents/measured_boot.rst
+++ b/docs/design_documents/measured_boot.rst
@@ -91,10 +91,10 @@
    and the variable length crypto agile structure called TCG_PCR_EVENT2. Event
    Log driver implemented in TF-A covers later part.
 
-#. RSE
+#. |RSE|
 
-   It is one of physical backend to extend the measurements. Please refer this
-   document :ref:`Runtime Security Engine (RSE)` for more details.
+   It is one of the physical backends to extend the measurements. Please refer
+   this document :ref:`Runtime Security Engine (RSE)` for more details.
 
 Platform Interface
 ------------------
@@ -121,7 +121,7 @@
       void bl2_plat_mboot_init(void);
 
    Initialise all Measured Boot backends supported by the platform
-   (e.g. Event Log buffer, RSE). As these functions do not return any value,
+   (e.g. Event Log buffer, |RSE|). As these functions do not return any value,
    the platform should deal with error management, such as logging the error
    somewhere, or panicking the system if this is considered a fatal error.
 
@@ -147,8 +147,9 @@
 
      - If it is Event Log backend, then record the measurement in TCG Event Log
        format.
-     - If it is a secure crypto-processor (like RSE), then extend the designated
-       PCR (or slot) with the given measurement.
+     - If it is a secure crypto-processor (like |RSE|), then extend the
+       designated PCR (or store it in secure on-chip memory) with the given
+       measurement.
    - This function must return 0 on success, a signed integer error code
      otherwise.
    - On the Arm FVP port, this function measures the given image and then
@@ -223,7 +224,7 @@
    - This function must return 0 on success, a signed integer error code
      otherwise.
    - In TC2 platform, this function is used to calculate the hash of the given
-     key and forward this hash to RSE alongside the measurement of the image
+     key and forward this hash to |RSE| alongside the measurement of the image
      which the key signs.
 
 --------------
diff --git a/docs/design_documents/rse.rst b/docs/design_documents/rse.rst
index 57467f3..dd110ca 100644
--- a/docs/design_documents/rse.rst
+++ b/docs/design_documents/rse.rst
@@ -138,6 +138,11 @@
   process can be requested from RSE. Furthermore, AP can request RSE to
   increase a non-volatile counter. Please refer to the
   ``RSE key management`` [5]_ document for more details.
+- ``DICE Protection Environment``: Securely store the firmware measurements
+  which were computed during the boot process and the associated metadata. It is
+  also capable of representing the boot measurements in the form of a
+  certificate chain, which is queriable. Please refer to the
+  ``DICE Protection Environment (DPE)`` [8]_ document for more details.
 
 Runtime service API
 ^^^^^^^^^^^^^^^^^^^
@@ -355,9 +360,7 @@
 Build time config options
 ^^^^^^^^^^^^^^^^^^^^^^^^^
 
-- ``MEASURED_BOOT``: Enable measured boot. It depends on the platform
-  implementation whether RSE or TPM (or both) backend based measured boot is
-  enabled.
+- ``MEASURED_BOOT``: Enable measured boot.
 - ``MBOOT_RSE_HASH_ALG``: Determine the hash algorithm to measure the images.
   The default value is sha-256.
 
@@ -432,10 +435,6 @@
 need to go through BL31. The RMM dispatcher module of the BL31 is responsible
 for delivering the calls between the two parties.
 
-.. Note::
-     Currently the connection between the RMM dispatcher and the PSA/RSE layer
-     is not yet implemented. RMM dispatcher just returns hard coded data.
-
 Delegated Attestation API
 ^^^^^^^^^^^^^^^^^^^^^^^^^
 Defined here:
@@ -674,6 +673,63 @@
         ]
     }
 
+RSE based DICE Protection Environment
+-------------------------------------
+
+The ``DICE Protection Environment (DPE)`` [8]_ service makes it possible to
+execute |DICE| commands within an isolated execution environment. It provides
+clients with an interface to send DICE commands, encoded as CBOR objects,
+that act on opaque context handles. The |DPE| service performs |DICE|
+derivations and certification on its internal contexts, without exposing the
+|DICE| secrets (private keys and CDIs) outside of the isolated execution
+environment.
+
+|DPE| API
+^^^^^^^^^
+
+Defined here:
+
+- ``include/lib/psa/dice_protection_environment.h``
+
+.. code-block:: c
+
+    dpe_error_t
+    dpe_derive_context(int      context_handle,
+                       uint32_t cert_id,
+                       bool     retain_parent_context,
+                       bool     allow_new_context_to_derive,
+                       bool     create_certificate,
+                       const DiceInputValues *dice_inputs,
+                       int32_t  target_locality,
+                       bool     return_certificate,
+                       bool     allow_new_context_to_export,
+                       bool     export_cdi,
+                       int     *new_context_handle,
+                       int     *new_parent_context_handle,
+                       uint8_t *new_certificate_buf,
+                       size_t   new_certificate_buf_size,
+                       size_t  *new_certificate_actual_size,
+                       uint8_t *exported_cdi_buf,
+                       size_t   exported_cdi_buf_size,
+                       size_t  *exported_cdi_actual_size);
+
+Build time config options
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+- ``MEASURED_BOOT``: Enable measured boot.
+- ``DICE_PROTECTION_ENVIRONMENT``: Boolean flag to specify the measured boot
+  backend when |RSE| based ``MEASURED_BOOT`` is enabled. The default value is
+  ``0``. When set to ``1`` then measurements and additional metadata collected
+  during the measured boot process are sent to the |DPE| for storage and
+  processing.
+- ``DPE_ALG_ID``: Determine the hash algorithm to measure the images. The
+  default value is sha-256.
+
+Example certificate chain
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+``plat/arm/board/tc/tc_dpe.h``
+
 RSE OTP Assets Management
 -------------------------
 
@@ -728,13 +784,14 @@
 References
 ----------
 
-.. [1] https://tf-m-user-guide.trustedfirmware.org/platform/arm/rse/readme.html
-.. [2] https://tf-m-user-guide.trustedfirmware.org/platform/arm/rse/rse_comms.html
-.. [3] https://git.trustedfirmware.org/TF-M/tf-m-extras.git/tree/partitions/measured_boot/measured_boot_integration_guide.rst
-.. [4] https://git.trustedfirmware.org/TF-M/tf-m-extras.git/tree/partitions/delegated_attestation/delegated_attest_integration_guide.rst
-.. [5] https://tf-m-user-guide.trustedfirmware.org/platform/arm/rse/rse_key_management.html
+.. [1] https://trustedfirmware-m.readthedocs.io/en/latest/platform/arm/rse/index.html
+.. [2] https://trustedfirmware-m.readthedocs.io/en/latest/platform/arm/rse/rse_comms.html
+.. [3] https://trustedfirmware-m.readthedocs.io/projects/tf-m-extras/en/latest/partitions/measured_boot_integration_guide.html
+.. [4] https://trustedfirmware-m.readthedocs.io/projects/tf-m-extras/en/latest/partitions/delegated_attestation/delegated_attest_integration_guide.html
+.. [5] https://trustedfirmware-m.readthedocs.io/en/latest/platform/arm/rse/rse_key_management.html
 .. [6] https://developer.arm.com/-/media/Files/pdf/PlatformSecurityArchitecture/Architect/DEN0063-PSA_Firmware_Framework-1.0.0-2.pdf?revision=2d1429fa-4b5b-461a-a60e-4ef3d8f7f4b4&hash=3BFD6F3E687F324672F18E5BE9F08EDC48087C93
 .. [7] https://developer.arm.com/documentation/DEN0096/A_a/?lang=en
+.. [8] https://trustedfirmware-m.readthedocs.io/projects/tf-m-extras/en/latest/partitions/dice_protection_environment/dice_protection_environment.html
 
 --------------
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 7776f5b..772447a 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -471,6 +471,15 @@
    This flag can take the values 0 to 2, to align  with the ``ENABLE_FEAT``
    mechanism. Default value is ``0``.
 
+-  ``ENABLE_FEAT_D128``: Numeric value to enable support for FEAT_D128
+   at EL2 and below, setting the bit SCT_EL3.D128En in EL3 to allow access to
+   128 bit version of system registers like PAR_EL1, TTBR0_EL1, TTBR1_EL1,
+   TTBR0_EL2, TTBR1_EL2, TTBR0_EL12, TTBR1_EL12 , VTTBR_EL2, RCWMASK_EL1, and
+   RCWSMASK_EL1. Its an optional architectural feature and is available from
+   9.3 and upwards.
+   This flag can take the values 0 to 2, to align  with the ``ENABLE_FEAT``
+   mechanism. Default value is ``0``.
+
 -  ``ENABLE_LTO``: Boolean option to enable Link Time Optimization (LTO)
    support in GCC for TF-A. This option is currently only supported for
    AArch64. Default is 0.
diff --git a/docs/global_substitutions.txt b/docs/global_substitutions.txt
index 80012e7..23a91cd 100644
--- a/docs/global_substitutions.txt
+++ b/docs/global_substitutions.txt
@@ -8,6 +8,8 @@
 .. |COT| replace:: :term:`COT`
 .. |CSS| replace:: :term:`CSS`
 .. |CVE| replace:: :term:`CVE`
+.. |DICE| replace:: :term:`DICE`
+.. |DPE| replace:: :term:`DPE`
 .. |DTB| replace:: :term:`DTB`
 .. |DS-5| replace:: :term:`DS-5`
 .. |DSU| replace:: :term:`DSU`
@@ -21,6 +23,7 @@
 .. |FVP| replace:: :term:`FVP`
 .. |FWU| replace:: :term:`FWU`
 .. |GIC| replace:: :term:`GIC`
+.. |HES| replace:: :term:`HES`
 .. |ISA| replace:: :term:`ISA`
 .. |Linaro| replace:: :term:`Linaro`
 .. |MMU| replace:: :term:`MMU`
@@ -31,12 +34,14 @@
 .. |OEN| replace:: :term:`OEN`
 .. |OP-TEE| replace:: :term:`OP-TEE`
 .. |OTE| replace:: :term:`OTE`
+.. |PCR| replace:: :term:`PCR`
 .. |PDD| replace:: :term:`PDD`
 .. |PAUTH| replace:: :term:`PAUTH`
 .. |PMF| replace:: :term:`PMF`
 .. |PSCI| replace:: :term:`PSCI`
 .. |RAS| replace:: :term:`RAS`
 .. |ROT| replace:: :term:`ROT`
+.. |RSE| replace:: :term:`RSE`
 .. |SCMI| replace:: :term:`SCMI`
 .. |SCP| replace:: :term:`SCP`
 .. |SDEI| replace:: :term:`SDEI`
@@ -55,6 +60,7 @@
 .. |SVE| replace:: :term:`SVE`
 .. |TBB| replace:: :term:`TBB`
 .. |TBBR| replace:: :term:`TBBR`
+.. |TCB| replace:: :term:`TCB`
 .. |TCG| replace:: :term:`TCG`
 .. |TEE| replace:: :term:`TEE`
 .. |TF-A| replace:: :term:`TF-A`
diff --git a/docs/glossary.rst b/docs/glossary.rst
index c02e938..f19897c 100644
--- a/docs/glossary.rst
+++ b/docs/glossary.rst
@@ -100,6 +100,9 @@
    GIC
       Generic Interrupt Controller
 
+   HES
+      Arm CCA Hardware Enforced Security
+
    ISA
       Instruction Set Architecture
 
@@ -136,6 +139,9 @@
    OTE
       Open-source Trusted Execution Environment
 
+   PCR
+      Platform Configuration Register
+
    PDD
       Platform Design Document
 
@@ -162,6 +168,9 @@
    ROT
       Root of Trust
 
+   RSE
+      Runtime Security Engine
+
    SCMI
       System Control and Management Interface
 
diff --git a/docs/threat_model/firmware_threat_model/threat_model.rst b/docs/threat_model/firmware_threat_model/threat_model.rst
index f8e4f7d..ae0219e 100644
--- a/docs/threat_model/firmware_threat_model/threat_model.rst
+++ b/docs/threat_model/firmware_threat_model/threat_model.rst
@@ -892,29 +892,66 @@
 
 .. topic:: Measured Boot Threats (or lack of)
 
- In the current Measured Boot design, BL1, BL2, and BL31, as well as the
- secure world components, form the |SRTM|. Measurement data is currently
- considered an asset to be protected against attack, and this is achieved
- by storing them in the Secure Memory.
- Beyond the measurements stored inside the TCG-compliant Event Log buffer,
- there are no other assets to protect or threats to defend against that
- could compromise |TF-A| execution environment's security.
+ In the current Measured Boot design the following components form the |TCB|:
+
+   - BL1, BL2, BL31
+   - Secure world components
+   - RMM (if RME extension is implemented)
+   - The configuration data of the above components
+
+ Across various Measured Boot backends, the data recorded during the flow as
+ well as the criticality of this data can vary. In most cases, these attributes
+ are considered valuable assets and are protected against potential attacks:
+
+   - Image measurement: the digest value of a component produced by a hash
+     function.
+   - Signer-id: the digest value of the image verification publiy key. The
+     verification public key is part of the image metadata.
+
+ In addition to these, other metadata attributes (image version, hash algorithm
+ identifier, etc) could be recorded during the Measured Boot process. But these
+ are not critical data.
+
+ In this context, an attack means modifying the measurement data (image or
+ public key hash) or recording arbitrary data as valid measurements.
+
+ The current Measured Boot design consists of two main parts. A frontend, which
+ is responsible for taking the measurements, and a backend which is responsible
+ for storing them. |TF-A| makes it possible to integrate various backends. Some
+ of these are implemented by the |TF-A| projects, while others are part of
+ different projects, and |TF-A| provides an integration layer.
+
+   - TCG-compliant Event Log: Implemented by |TF-A|. Measurements are stored in
+     the Event Log which is located on the secure on-chip memory of the AP. The
+     address of the Event Log buffer is handed off between boot stages and new
+     measurements are appended to the Event Log. A limitation of the current
+     Measured Boot implementation in |TF-A| is that it does not extend the
+     measurements into a |PCR| of a Discrete |TPM|, where measurements would
+     be securely stored and protected against tampering.
+   - `CCA Measured Boot`_: Implemented by |TF-M|. Measurements are stored in
+     |HES| secure on-chip memory. |HES| implements protection against tampering
+     its on-chip memory. |HES| interface is available for BL1 and BL2.
+   - `DICE Protection Environment`_ (DPE): Implemented by |TF-M|. Measurements
+     are stored in |RSE| secure on-chip memory. |RSE| implements protection
+     against tampering its on-chip memory. DPE provides additional protection
+     against unauthorized access by malicious actors through the use of one-time
+     context handles and the identification of the client's target locality
+     (location of the client).
+
+ Beyond the measurements (image digest and signer-id) there are no other assets
+ to protect or threats to defend against that could compromise |TF-A| execution
+ environment's security.
 
  There are general security assets and threats associated with remote/delegated
  attestation. However, these are outside the |TF-A| security boundary and
  should be dealt with by the appropriate agent in the platform/system.
  Since current Measured Boot design does not use local attestation, there would
- be no further assets to protect(like unsealed keys).
+ be no further assets to protect (like unsealed keys).
 
- A limitation of the current Measured Boot design is that it is dependent upon
- Secure Boot as implementation of Measured Boot does not extend measurements
- into a discrete |TPM|, where they would be securely stored and protected
- against tampering. This implies that if Secure-Boot is compromised, Measured
- Boot may also be compromised.
+ System integrators must carefully evaluate the security requirement and
+ capabilities of their platform and choose an appropriate Measured Boot
+ solution.
 
- Platforms must carefully evaluate the security of the default implementation
- since the |SRTM| includes all secure world components.
-
 
 .. _Runtime Firmware Threats:
 
@@ -1169,3 +1206,5 @@
 .. _Trusted Firmware-A Tests: https://git.trustedfirmware.org/TF-A/tf-a-tests.git/about/
 .. _OP-TEE Dispatcher: https://github.com/ARM-software/arm-trusted-firmware/blob/master/docs/components/spd/optee-dispatcher.rst
 .. _PSR Specification: https://developer.arm.com/documentation/den0106/0100
+.. _CCA Measured Boot: https://trustedfirmware-m.readthedocs.io/projects/tf-m-extras/en/latest/partitions/measured_boot_integration_guide.html
+.. _DICE Protection Environment: https://trustedfirmware-m.readthedocs.io/projects/tf-m-extras/en/latest/partitions/dice_protection_environment/dice_protection_environment.html
diff --git a/drivers/cadence/emmc/cdns_sdmmc.c b/drivers/cadence/emmc/cdns_sdmmc.c
index d2cd4d6..892d333 100644
--- a/drivers/cadence/emmc/cdns_sdmmc.c
+++ b/drivers/cadence/emmc/cdns_sdmmc.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ * Copyright (c) 2024, Altera Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,35 +19,6 @@
 #include <lib/mmio.h>
 #include <lib/utils.h>
 
-/* Card busy and present */
-#define CARD_BUSY					1
-#define CARD_NOT_BUSY					0
-
-/* 500 ms delay to read the RINST register */
-#define DELAY_MS_SRS_READ				500
-#define DELAY_RES					10
-
-/* SRS12 error mask */
-#define SRS12_ERR_MASK					0xFFFF8000
-
-/* Check DV dfi_init val=0 */
-#define IO_MASK_END_DATA				0x0
-
-/* Check DV dfi_init val=2; DDR Mode */
-#define IO_MASK_END_DATA_DDR				0x2
-#define IO_MASK_START_DATA				0x0
-#define DATA_SELECT_OE_END_DATA				0x1
-
-#define TIMEOUT						100000
-
-/* General define */
-#define SDHC_REG_MASK					UINT_MAX
-#define SD_HOST_BLOCK_SIZE				0x200
-#define DTCVVAL_DEFAULT_VAL				0xE
-#define CDMMC_DMA_MAX_BUFFER_SIZE			64*1024
-#define CDNSMMC_ADDRESS_MASK				U(0x0f)
-#define CONFIG_CDNS_DESC_COUNT				8
-
 void cdns_init(void);
 int cdns_send_cmd(struct mmc_cmd *cmd);
 int cdns_set_ios(unsigned int clk, unsigned int width);
@@ -62,7 +34,8 @@
 	.read			= cdns_read,
 	.write			= cdns_write,
 };
-
+void sd_host_adma_prepare(struct cdns_idmac_desc *desc_ptr, uintptr_t buf,
+			  size_t size);
 struct cdns_sdmmc_params cdns_params;
 struct cdns_sdmmc_combo_phy sdmmc_combo_phy_reg;
 struct cdns_sdmmc_sdhc sdmmc_sdhc_reg;
@@ -89,45 +62,22 @@
 		}
 	} while ((data & (1 << SDMMC_CDN_ICS)) == 0);
 
-	return 0;
-}
-
-int cdns_busy(void)
-{
-	unsigned int data;
-
-	data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS09);
-	return (data & STATUS_DATA_BUSY) ? CARD_BUSY : CARD_NOT_BUSY;
-}
-
-int cdns_vol_reset(void)
-{
-	/* Reset embedded card */
-	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), (7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
-	udelay(250);
-	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), (7 << SDMMC_CDN_BVS) | (0 << SDMMC_CDN_BP));
-	udelay(500);
-
-	/* Turn on supply voltage */
-	/* BVS = 7, BP = 1, BP2 only in UHS2 mode */
-	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), (7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
-	udelay(250);
 	return 0;
 }
 
 void cdns_set_sdmmc_var(struct cdns_sdmmc_combo_phy *combo_phy_reg,
-	struct cdns_sdmmc_sdhc *sdhc_reg)
+			struct cdns_sdmmc_sdhc *sdhc_reg)
 {
 	/* Values are taken by the reference of cadence IP documents */
 	combo_phy_reg->cp_clk_wr_delay = 0;
 	combo_phy_reg->cp_clk_wrdqs_delay = 0;
-	combo_phy_reg->cp_data_select_oe_end = 0;
+	combo_phy_reg->cp_data_select_oe_end = 1;
 	combo_phy_reg->cp_dll_bypass_mode = 1;
 	combo_phy_reg->cp_dll_locked_mode = 0;
-	combo_phy_reg->cp_dll_start_point = 0;
+	combo_phy_reg->cp_dll_start_point = 254;
 	combo_phy_reg->cp_gate_cfg_always_on = 1;
 	combo_phy_reg->cp_io_mask_always_on = 0;
-	combo_phy_reg->cp_io_mask_end = 0;
+	combo_phy_reg->cp_io_mask_end = 5;
 	combo_phy_reg->cp_io_mask_start = 0;
 	combo_phy_reg->cp_rd_del_sel = 52;
 	combo_phy_reg->cp_read_dqs_cmd_delay = 0;
@@ -142,38 +92,58 @@
 
 	sdhc_reg->sdhc_extended_rd_mode = 1;
 	sdhc_reg->sdhc_extended_wr_mode = 1;
-	sdhc_reg->sdhc_hcsdclkadj = 0;
+	sdhc_reg->sdhc_hcsdclkadj = 3;
 	sdhc_reg->sdhc_idelay_val = 0;
 	sdhc_reg->sdhc_rdcmd_en = 1;
 	sdhc_reg->sdhc_rddata_en = 1;
-	sdhc_reg->sdhc_rw_compensate = 9;
+	sdhc_reg->sdhc_rw_compensate = 10;
 	sdhc_reg->sdhc_sdcfsh = 0;
-	sdhc_reg->sdhc_sdcfsl = 1;
+	sdhc_reg->sdhc_sdcfsl = 0;
 	sdhc_reg->sdhc_wrcmd0_dly = 1;
 	sdhc_reg->sdhc_wrcmd0_sdclk_dly = 0;
 	sdhc_reg->sdhc_wrcmd1_dly = 0;
 	sdhc_reg->sdhc_wrcmd1_sdclk_dly = 0;
-	sdhc_reg->sdhc_wrdata0_dly = 1;
+	sdhc_reg->sdhc_wrdata0_dly = 0;
 	sdhc_reg->sdhc_wrdata0_sdclk_dly = 0;
 	sdhc_reg->sdhc_wrdata1_dly = 0;
 	sdhc_reg->sdhc_wrdata1_sdclk_dly = 0;
 }
 
-static int cdns_program_phy_reg(struct cdns_sdmmc_combo_phy *combo_phy_reg,
-	struct cdns_sdmmc_sdhc *sdhc_reg)
+int cdns_program_phy_reg(struct cdns_sdmmc_combo_phy *combo_phy_reg,
+				struct cdns_sdmmc_sdhc *sdhc_reg)
 {
 	uint32_t value = 0;
 	int ret = 0;
+	uint32_t timeout = 0;
+
+	/* HRS00 - Software Reset */
+	mmio_write_32((cdns_params.reg_base + SDHC_CDNS_HRS00), SDHC_CDNS_HRS00_SWR);
+
+	/* Waiting for SDHC_CDNS_HRS00_SWR reset */
+	timeout = TIMEOUT;
+	do {
+		udelay(250);
+		if (--timeout <= 0) {
+			NOTICE(" SDHC Software Reset failed!!!\n");
+			panic();
+		}
+	} while (((mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS00) &
+		SDHC_CDNS_HRS00_SWR) == 1));
+
+	/* Step 1, switch on DLL_RESET */
+	value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09);
+	value &= ~SDHC_PHY_SW_RESET;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09, value);
 
 	/* program PHY_DQS_TIMING_REG */
 	value = (CP_USE_EXT_LPBK_DQS(combo_phy_reg->cp_use_ext_lpbk_dqs)) |
 		(CP_USE_LPBK_DQS(combo_phy_reg->cp_use_lpbk_dqs)) |
 		(CP_USE_PHONY_DQS(combo_phy_reg->cp_use_phony_dqs)) |
 		(CP_USE_PHONY_DQS_CMD(combo_phy_reg->cp_use_phony_dqs_cmd));
-	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
-			COMBO_PHY_REG + PHY_DQS_TIMING_REG, MMC_REG_BASE +
-			SDHC_CDNS_HRS05, value);
-	if (ret != 0) {
+	ret = cdns_sdmmc_write_phy_reg(cdns_params.reg_base + SDHC_CDNS_HRS04,
+					COMBO_PHY_REG + PHY_DQS_TIMING_REG,
+					cdns_params.reg_base + SDHC_CDNS_HRS05, value);
+	if (ret != 0U) {
 		return ret;
 	}
 
@@ -183,73 +153,90 @@
 		(CP_RD_DEL_SEL(combo_phy_reg->cp_rd_del_sel)) |
 		(CP_UNDERRUN_SUPPRESS(combo_phy_reg->cp_underrun_suppress)) |
 		(CP_GATE_CFG_ALWAYS_ON(combo_phy_reg->cp_gate_cfg_always_on));
-	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
-			COMBO_PHY_REG + PHY_GATE_LPBK_CTRL_REG, MMC_REG_BASE +
-			SDHC_CDNS_HRS05, value);
-	if (ret != 0) {
-		return ret;
+	ret = cdns_sdmmc_write_phy_reg(cdns_params.reg_base + SDHC_CDNS_HRS04,
+				 COMBO_PHY_REG + PHY_GATE_LPBK_CTRL_REG,
+				 cdns_params.reg_base + SDHC_CDNS_HRS05, value);
+	if (ret != 0U) {
+		return -ret;
 	}
 
 	/* program PHY_DLL_MASTER_CTRL_REG */
-	value = (CP_DLL_BYPASS_MODE(combo_phy_reg->cp_dll_bypass_mode))
-			| (CP_DLL_START_POINT(combo_phy_reg->cp_dll_start_point));
-	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
-			COMBO_PHY_REG + PHY_DLL_MASTER_CTRL_REG, MMC_REG_BASE
-			+ SDHC_CDNS_HRS05, value);
-	if (ret != 0) {
+	value = (CP_DLL_BYPASS_MODE(combo_phy_reg->cp_dll_bypass_mode)) | (2 << 20) |
+		(CP_DLL_START_POINT(combo_phy_reg->cp_dll_start_point));
+	ret = cdns_sdmmc_write_phy_reg(cdns_params.reg_base + SDHC_CDNS_HRS04,
+					COMBO_PHY_REG + PHY_DLL_MASTER_CTRL_REG,
+					cdns_params.reg_base + SDHC_CDNS_HRS05, value);
+	if (ret != 0U) {
 		return ret;
 	}
 
 	/* program PHY_DLL_SLAVE_CTRL_REG */
-	value = (CP_READ_DQS_CMD_DELAY(combo_phy_reg->cp_read_dqs_cmd_delay))
-		| (CP_CLK_WRDQS_DELAY(combo_phy_reg->cp_clk_wrdqs_delay))
-		| (CP_CLK_WR_DELAY(combo_phy_reg->cp_clk_wr_delay))
-		| (CP_READ_DQS_DELAY(combo_phy_reg->cp_read_dqs_delay));
-	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
-			COMBO_PHY_REG + PHY_DLL_SLAVE_CTRL_REG, MMC_REG_BASE
-			+ SDHC_CDNS_HRS05, value);
-	if (ret != 0) {
+	value = (CP_READ_DQS_CMD_DELAY(combo_phy_reg->cp_read_dqs_cmd_delay)) |
+		(CP_CLK_WRDQS_DELAY(combo_phy_reg->cp_clk_wrdqs_delay)) |
+		(CP_CLK_WR_DELAY(combo_phy_reg->cp_clk_wr_delay)) |
+		(CP_READ_DQS_DELAY(combo_phy_reg->cp_read_dqs_delay));
+	ret = cdns_sdmmc_write_phy_reg(cdns_params.reg_base + SDHC_CDNS_HRS04,
+					COMBO_PHY_REG + PHY_DLL_SLAVE_CTRL_REG,
+					cdns_params.reg_base + SDHC_CDNS_HRS05, value);
+	if (ret != 0U) {
 		return ret;
 	}
 
 	/* program PHY_CTRL_REG */
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS04, COMBO_PHY_REG
-			+ PHY_CTRL_REG);
-	value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS05);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS04, COMBO_PHY_REG + PHY_CTRL_REG);
+	value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS05);
 
 	/* phony_dqs_timing=0 */
 	value &= ~(CP_PHONY_DQS_TIMING_MASK << CP_PHONY_DQS_TIMING_SHIFT);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS05, value);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS05, value);
 
 	/* switch off DLL_RESET */
 	do {
-		value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+		value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09);
 		value |= SDHC_PHY_SW_RESET;
-		mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, value);
-		value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09, value);
+		value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09);
 	/* polling PHY_INIT_COMPLETE */
 	} while ((value & SDHC_PHY_INIT_COMPLETE) != SDHC_PHY_INIT_COMPLETE);
 
 	/* program PHY_DQ_TIMING_REG */
-	combo_phy_reg->cp_io_mask_end = 0U;
-	value = (CP_IO_MASK_ALWAYS_ON(combo_phy_reg->cp_io_mask_always_on))
-		| (CP_IO_MASK_END(combo_phy_reg->cp_io_mask_end))
-		| (CP_IO_MASK_START(combo_phy_reg->cp_io_mask_start))
-		| (CP_DATA_SELECT_OE_END(combo_phy_reg->cp_data_select_oe_end));
+	value = (CP_IO_MASK_ALWAYS_ON(combo_phy_reg->cp_io_mask_always_on)) |
+		(CP_IO_MASK_END(combo_phy_reg->cp_io_mask_end)) |
+		(CP_IO_MASK_START(combo_phy_reg->cp_io_mask_start)) |
+		(CP_DATA_SELECT_OE_END(combo_phy_reg->cp_data_select_oe_end));
 
-	ret = cdns_sdmmc_write_phy_reg(MMC_REG_BASE + SDHC_CDNS_HRS04,
-			COMBO_PHY_REG + PHY_DQ_TIMING_REG, MMC_REG_BASE
-			+ SDHC_CDNS_HRS05, value);
-	if (ret != 0) {
+	ret = cdns_sdmmc_write_phy_reg(cdns_params.reg_base + SDHC_CDNS_HRS04,
+				 COMBO_PHY_REG + PHY_DQ_TIMING_REG,
+				 cdns_params.reg_base + SDHC_CDNS_HRS05, value);
+	if (ret != 0U) {
 		return ret;
 	}
+
+	value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09);
+	value |= (HRS_09_EXTENDED_RD_MODE | HRS_09_EXTENDED_WR_MODE |
+		HRS_09_RDCMD_EN | HRS_09_RDDATA_EN);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09, value);
+
+	value = 0;
+	value = SDHC_HCSDCLKADJ(HRS_10_HCSDCLKADJ_VAL);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS10, value);
+
+	value = 0;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS16, value);
+
+	value = (10 << 16);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS07, value);
+
 	return 0;
 }
 
 int cdns_read(int lba, uintptr_t buf, size_t size)
 {
-	inv_dcache_range(buf, size);
+	return 0;
+}
 
+int cdns_write(int lba, uintptr_t buf, size_t size)
+{
 	return 0;
 }
 
@@ -260,120 +247,79 @@
 
 int cdns_prepare(int dma_start_addr, uintptr_t dma_buff, size_t size)
 {
-	data_cmd = true;
-	struct cdns_idmac_desc *desc;
-	uint32_t desc_cnt, i;
-	uint64_t desc_base;
-
+	struct cdns_idmac_desc *cdns_desc_data;
 	assert(((dma_buff & CDNSMMC_ADDRESS_MASK) == 0) &&
-			(cdns_params.desc_size > 0) &&
-			((MMC_REG_BASE & MMC_BLOCK_MASK) == 0) &&
-			((cdns_params.desc_base & MMC_BLOCK_MASK) == 0) &&
-			((cdns_params.desc_size & MMC_BLOCK_MASK) == 0));
-
-	flush_dcache_range(dma_buff, size);
-
-	desc_cnt = (size + (CDMMC_DMA_MAX_BUFFER_SIZE) - 1) / (CDMMC_DMA_MAX_BUFFER_SIZE);
-	assert(desc_cnt * sizeof(struct cdns_idmac_desc) < cdns_params.desc_size);
-
-	if (desc_cnt > CONFIG_CDNS_DESC_COUNT) {
-		ERROR("Requested data transfer length %ld is greater than configured length %d",
-				size, (CONFIG_CDNS_DESC_COUNT * CDMMC_DMA_MAX_BUFFER_SIZE));
-		return -EINVAL;
-	}
-
-	desc = (struct cdns_idmac_desc *)cdns_params.desc_base;
-	desc_base = (uint64_t)desc;
-	i = 0;
-
-	while ((i + 1) < desc_cnt) {
-		desc->attr = ADMA_DESC_ATTR_VALID | ADMA_DESC_TRANSFER_DATA;
-		desc->reserved = 0;
-		desc->len = MAX_64KB_PAGE;
-		desc->addr_lo = (dma_buff & UINT_MAX) + (CDMMC_DMA_MAX_BUFFER_SIZE * i);
-#if CONFIG_DMA_ADDR_T_64BIT == 1
-		desc->addr_hi = (dma_buff >> 32) & 0xffffffff;
-#endif
-		size -= CDMMC_DMA_MAX_BUFFER_SIZE;
-		desc++;
-		i++;
-	}
+	 (cdns_params.desc_size > 0));
 
-	desc->attr = ADMA_DESC_ATTR_VALID | ADMA_DESC_TRANSFER_DATA |
-			ADMA_DESC_ATTR_END;
-	desc->reserved = 0;
-	desc->len = size;
-#if CONFIG_DMA_ADDR_T_64BIT == 1
-	desc->addr_lo = (dma_buff & UINT_MAX) + (CDMMC_DMA_MAX_BUFFER_SIZE * i);
-	desc->addr_hi = (dma_buff >> 32) & UINT_MAX;
-#else
-	desc->addr_lo = (dma_buff & UINT_MAX);
-#endif
+	cdns_desc_data = (struct cdns_idmac_desc *)cdns_params.desc_base;
+	sd_host_adma_prepare(cdns_desc_data, dma_buff, size);
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS22, (uint32_t)desc_base);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS23, (uint32_t)(desc_base >> 32));
-	flush_dcache_range(cdns_params.desc_base,
-				desc_cnt * CDMMC_DMA_MAX_BUFFER_SIZE);
-
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS01,
-			((512 << BLOCK_SIZE) | ((size/512) << BLK_COUNT_CT) | SDMA_BUF));
 	return 0;
 }
 
-static void cdns_host_set_clk(int clk)
+void cdns_host_set_clk(uint32_t clk)
 {
 	uint32_t ret = 0;
 	uint32_t sdclkfsval = 0;
-	uint32_t dtcvval = DTCVVAL_DEFAULT_VAL;
+	uint32_t dtcvval = 0xE;
 
-	sdclkfsval = (cdns_params.clk_rate / 2000) / clk;
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, 0);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
-			(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE));
+	sdclkfsval = (SD_HOST_CLK / 2) / clk;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS11, 0);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS11,
+			(dtcvval << SDMMC_CDN_DTCV) | (sdclkfsval << SDMMC_CDN_SDCLKFS) |
+			(1 << SDMMC_CDN_ICE));
 
-	ret = cdns_wait_ics(5000, MMC_REG_BASE + SDHC_CDNS_SRS11);
-	if (ret != 0U) {
-		ERROR("Waiting SDMMC_CDN_ICS timeout");
+	ret = cdns_wait_ics(5000, cdns_params.reg_base + SDHC_CDNS_SRS11);
+	if (ret != 0) {
+		ERROR("Waiting ICS timeout");
 	}
-
 	/* Enable DLL reset */
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) &
-			~SDHC_DLL_RESET_MASK);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09,
+		mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09) & ~0x00000001);
 	/* Set extended_wr_mode */
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, (mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09)
-			& SDHC_EXTENDED_WR_MODE_MASK) | (1 << EXTENDED_WR_MODE));
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09,
+		(mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09) & 0xFFFFFFF7) |
+			(1 << EXTENDED_WR_MODE));
 	/* Release DLL reset */
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
-			+ SDHC_CDNS_HRS09) | 1);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
-			+ SDHC_CDNS_HRS09) | (3 << RDCMD_EN));
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09,
+		mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09) | PHY_SW_RESET_EN);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_HRS09,
+		mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09) | RDCMD_EN);
 
 	do {
-		mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
-	} while (~mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) & (1 << 1));
+		mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09);
+	} while (~mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS09) &
+		(PHY_INIT_COMPLETE_BIT));
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
-	(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE) | (1 << SDMMC_CDN_SDCE));
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS13, UINT_MAX);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
+			(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE) |
+			(1 << SDMMC_CDN_SDCE));
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS13, 0xFFFFFFFF);
 }
 
 int cdns_set_ios(unsigned int clk, unsigned int width)
 {
+	uint32_t _status = 0;
 
+	_status = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
 	switch (width) {
 	case MMC_BUS_WIDTH_1:
-		mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), LEDC_OFF);
+		_status &= ~(BIT4);
 		break;
+
 	case MMC_BUS_WIDTH_4:
-		mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), DTW_4BIT);
+		_status |= BIT4;
 		break;
+
 	case MMC_BUS_WIDTH_8:
-		mmio_write_32((MMC_REG_BASE + SDHC_CDNS_SRS10), EDTW_8BIT);
+		_status |= BIT8;
 		break;
+
 	default:
 		assert(0);
 		break;
 	}
+	mmio_write_32((cdns_params.reg_base + SDHC_CDNS_SRS10), _status);
 	cdns_host_set_clk(clk);
 
 	return 0;
@@ -388,6 +334,7 @@
 	value |= data;
 	mmio_write_32(addr, value);
 	value = mmio_read_32(addr);
+
 	if (value != data) {
 		ERROR("SD host address is not set properly\n");
 		return -ENXIO;
@@ -396,429 +343,403 @@
 	return 0;
 }
 
-int cdns_write(int lba, uintptr_t buf, size_t size)
-{
-	return 0;
-}
 
-static int cdns_init_hrs_io(struct cdns_sdmmc_combo_phy *combo_phy_reg,
-	struct cdns_sdmmc_sdhc *sdhc_reg)
+
+void sd_host_oper_mode(enum sd_opr_modes opr_mode)
 {
-	uint32_t value = 0;
-	int ret = 0;
 
-	/* program HRS09, register 42 */
-	value = (SDHC_RDDATA_EN(sdhc_reg->sdhc_rddata_en))
-		| (SDHC_RDCMD_EN(sdhc_reg->sdhc_rdcmd_en))
-		| (SDHC_EXTENDED_WR_MODE(sdhc_reg->sdhc_extended_wr_mode))
-		| (SDHC_EXTENDED_RD_MODE(sdhc_reg->sdhc_extended_rd_mode));
-	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS09, value);
-	if (ret != 0) {
-		ERROR("Program HRS09 failed");
-		return ret;
-	}
+	uint32_t reg = 0;
 
-	/* program HRS10, register 43 */
-	value = (SDHC_HCSDCLKADJ(sdhc_reg->sdhc_hcsdclkadj));
-	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS10, value);
-	if (ret != 0) {
-		ERROR("Program HRS10 failed");
-		return ret;
-	}
+	switch (opr_mode) {
+	case SD_HOST_OPR_MODE_HV4E_0_SDMA_32:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg &= ~(HV4E | BIT_AD_64);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
 
-	/* program HRS16, register 48 */
-	value = (SDHC_WRDATA1_SDCLK_DLY(sdhc_reg->sdhc_wrdata1_sdclk_dly))
-		| (SDHC_WRDATA0_SDCLK_DLY(sdhc_reg->sdhc_wrdata0_sdclk_dly))
-		| (SDHC_WRCMD1_SDCLK_DLY(sdhc_reg->sdhc_wrcmd1_sdclk_dly))
-		| (SDHC_WRCMD0_SDCLK_DLY(sdhc_reg->sdhc_wrcmd0_sdclk_dly))
-		| (SDHC_WRDATA1_DLY(sdhc_reg->sdhc_wrdata1_dly))
-		| (SDHC_WRDATA0_DLY(sdhc_reg->sdhc_wrdata0_dly))
-		| (SDHC_WRCMD1_DLY(sdhc_reg->sdhc_wrcmd1_dly))
-		| (SDHC_WRCMD0_DLY(sdhc_reg->sdhc_wrcmd0_dly));
-	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS16, value);
-	if (ret != 0) {
-		ERROR("Program HRS16 failed");
-		return ret;
-	}
+	case SD_HOST_OPR_MODE_HV4E_1_SDMA_32:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg &= ~(HV4E | BIT_AD_64);
+		reg |= (HV4E);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
 
-	/* program HRS07, register 40 */
-	value = (SDHC_RW_COMPENSATE(sdhc_reg->sdhc_rw_compensate))
-		| (SDHC_IDELAY_VAL(sdhc_reg->sdhc_idelay_val));
-	ret = cdns_sdmmc_write_sd_host_reg(MMC_REG_BASE + SDHC_CDNS_HRS07, value);
-	if (ret != 0) {
-		ERROR("Program HRS07 failed");
-		return ret;
-	}
+	case SD_HOST_OPR_MODE_HV4E_1_SDMA_64:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg |= (HV4E | BIT_AD_64);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
 
-	return ret;
+	case SD_HOST_OPR_MODE_HV4E_0_ADMA_32:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		reg |= DMA_SEL_BIT_2;
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg &= ~(HV4E | BIT_AD_64);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
+
+	case SD_HOST_OPR_MODE_HV4E_0_ADMA_64:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		reg |= DMA_SEL_BIT_3;
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg &= ~(HV4E | BIT_AD_64);
+		reg |= BIT_AD_64;
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
+
+	case SD_HOST_OPR_MODE_HV4E_1_ADMA_32:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		reg |= DMA_SEL_BIT_2;
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg &= ~(HV4E | BIT_AD_64);
+		reg |= HV4E;
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
+
+	case SD_HOST_OPR_MODE_HV4E_1_ADMA_64:
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
+		reg &= ~(DMA_SEL_BIT);
+		reg |= DMA_SEL_BIT_2;
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg);
+		reg = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS15);
+		reg |= (HV4E | BIT_AD_64);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS15, reg);
+		break;
+	}
 }
 
-static int cdns_hc_set_clk(struct cdns_sdmmc_params *cdn_sdmmc_dev_mode_params)
+void card_reset(bool power_enable)
 {
-	uint32_t ret = 0;
-	uint32_t dtcvval, sdclkfsval;
+	uint32_t reg_value = 0;
 
-	dtcvval = DTC_VAL;
-	sdclkfsval = 0;
+	/* Reading SRS10 value before writing */
+	reg_value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
 
-	if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_DS) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR12) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_SDR_BC)) {
-		sdclkfsval = 4;
-	} else if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_HS) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR25) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_DDR50) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_SDR)) {
-		sdclkfsval = 2;
-	} else if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR50) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_DDR) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_HS400) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_HS400es)) {
-		sdclkfsval = 1;
-	} else if ((cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == SD_UHS_SDR104) ||
-		(cdn_sdmmc_dev_mode_params->cdn_sdmmc_dev_mode == EMMC_HS200)) {
-		sdclkfsval = 0;
+	if (power_enable == true) {
+		reg_value &= ~((7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
+		reg_value = ((1 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
+	} else {
+		reg_value &= ~((7 << SDMMC_CDN_BVS) | (1 << SDMMC_CDN_BP));
 	}
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg_value);
+}
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, 0);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
-		(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE));
-	ret = cdns_wait_ics(5000, MMC_REG_BASE + SDHC_CDNS_SRS11);
-	if (ret != 0U) {
-		ERROR("Waiting SDMMC_CDN_ICS timeout");
-		return ret;
-	}
+void high_speed_enable(bool mode)
+{
 
-	/* Enable DLL reset */
-	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_HRS09), mmio_read_32(MMC_REG_BASE
-			+ SDHC_CDNS_HRS09) & ~SDHC_DLL_RESET_MASK);
-	/* Set extended_wr_mode */
-	mmio_write_32((MMC_REG_BASE + SDHC_CDNS_HRS09),
-	(mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) &	SDHC_EXTENDED_WR_MODE_MASK) |
-			(1 << EXTENDED_WR_MODE));
-	/* Release DLL reset */
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
-			+ SDHC_CDNS_HRS09) | 1);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, mmio_read_32(MMC_REG_BASE
-			+ SDHC_CDNS_HRS09) | (3 << RDCMD_EN));
-	do {
-		mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
-	} while (~mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09) & (1 << 1));
+	uint32_t reg_value = 0;
+	/* Reading SRS10 value before writing */
+	reg_value = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS10);
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (dtcvval << SDMMC_CDN_DTCV) |
-		(sdclkfsval << SDMMC_CDN_SDCLKFS) | (1 << SDMMC_CDN_ICE) | (1 << SDMMC_CDN_SDCE));
+	if (mode == true) {
+		reg_value |= HS_EN;
+	} else {
+		reg_value &= ~HS_EN;
+	}
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS13, UINT_MAX);
-	return 0;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS10, reg_value);
 }
 
 int cdns_reset(void)
 {
-	uint32_t data = 0;
+	volatile uint32_t data = 0;
 	uint32_t count = 0;
-	uint32_t value = 0;
-
-	value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS11);
-	value &= ~(0xFFFF);
-	value |= 0x0;
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, value);
-	udelay(500);
 
 	/* Software reset */
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS00, 1);
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS11, SRS11_SRFA);
 	/* Wait status command response ready */
 	do {
-		data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS00);
+		data = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_HRS00);
 		count++;
-		if (count >= 5000) {
+		if (count >= CDNS_TIMEOUT) {
 			return -ETIMEDOUT;
 		}
-	/* Wait for HRS00.SWR */
-	} while ((data & 1) == 1);
-
-	/* Step 1, switch on DLL_RESET */
-	value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_HRS09);
-	value &= ~SDHC_PHY_SW_RESET;
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_HRS09, value);
+	/* Wait for SRS11 */
+	} while (((SRS11_SRFA_CHK(data)) & 1) == 1);
 
 	return 0;
 }
 
+void sdmmc_host_init(bool uhs2_enable)
+{
+	uint32_t timeout;
+
+	/* SRS11 - Host Control  default value set */
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS11, 0x0);
+
+	/* Waiting for detect card */
+	timeout = TIMEOUT;
+	do {
+		udelay(250);
+		if (--timeout <= 0) {
+			NOTICE(" SDHC Card Detecion failed!!!\n");
+			panic();
+		}
+	} while (((mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS09) & CHECK_CARD) == 0));
+
+	/* UHS2 Host setting */
+	if (uhs2_enable == true) {
+	/** need to implement*/
+	}
+
+	/* Card reset */
+
+	card_reset(1);
+	udelay(2500);
+	card_reset(0);
+	udelay(2500);
+	card_reset(1);
+	udelay(2500);
+
+	/* Enable Interrupt Flags*/
+	mmio_write_32((cdns_params.reg_base + SDHC_CDNS_SRS13), ~0);
+	high_speed_enable(true);
+}
+
 int cdns_sd_host_init(struct cdns_sdmmc_combo_phy *mmc_combo_phy_reg,
-struct cdns_sdmmc_sdhc *mmc_sdhc_reg)
+		      struct cdns_sdmmc_sdhc *mmc_sdhc_reg)
 {
 	int ret = 0;
 
 	ret = cdns_reset();
-	if (ret != 0) {
+	if (ret != 0U) {
 		ERROR("Program phy reg init failed");
 		return ret;
 	}
 
 	ret = cdns_program_phy_reg(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
-	if (ret != 0) {
+	if (ret != 0U) {
 		ERROR("Program phy reg init failed");
 		return ret;
 	}
-
-	ret = cdns_init_hrs_io(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
-	if (ret != 0) {
-		ERROR("Program init for HRS reg is failed");
-		return ret;
-	}
-
-	ret = cdns_sd_card_detect();
-	if (ret != 0) {
-		ERROR("SD card does not detect");
-		return ret;
-	}
+	sdmmc_host_init(0);
+	cdns_host_set_clk(100000);
 
-	ret = cdns_vol_reset();
-	if (ret != 0) {
-		ERROR("eMMC card reset failed");
-		return ret;
-	}
-
-	ret = cdns_hc_set_clk(&cdns_params);
-	if (ret != 0) {
-		ERROR("hc set clk failed");
-		return ret;
-	}
+	sd_host_oper_mode(SD_HOST_OPR_MODE_HV4E_0_ADMA_64);
 
 	return 0;
 }
 
-void cdns_srs10_value_toggle(uint8_t write_val, uint8_t prev_val)
-{
-	uint32_t data_op = 0U;
-
-	data_op = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS10);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS10, (data_op & (prev_val << 0)));
-	mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS10);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS10, data_op | (write_val << 0));
-}
-
-void cdns_srs11_srs15_config(uint32_t srs11_val, uint32_t srs15_val)
-{
-	uint32_t data = 0U;
-
-	data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS11);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS11, (data | srs11_val));
-	data = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS15);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS15, (data | srs15_val));
-}
-
 int cdns_send_cmd(struct mmc_cmd *cmd)
 {
-	uint32_t op = 0, ret = 0;
-	uint8_t write_value = 0, prev_val = 0;
-	uint32_t value;
-	int32_t timeout;
-	uint32_t cmd_indx;
-	uint32_t status = 0, srs15_val = 0, srs11_val = 0;
+	uint32_t cmd_flags = 0;
+	uint32_t timeout = 0;
 	uint32_t status_check = 0;
+	uint32_t mode = 0;
+	uint32_t status;
 
 	assert(cmd);
-	cmd_indx = (cmd->cmd_idx) << COM_IDX;
-
-	if (data_cmd) {
-		switch (cmd->cmd_idx) {
-		case SD_SWITCH:
-			op = DATA_PRESENT;
-			write_value = ADMA2_32 | DT_WIDTH;
-			prev_val = ADMA2_32 | DT_WIDTH;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			srs15_val = BIT_AD_64 | HV4E | V18SE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			break;
 
-		case SD_WRITE_SINGLE_BLOCK:
-		case SD_READ_SINGLE_BLOCK:
-			op = DATA_PRESENT;
-			write_value = ADMA2_32 | HS_EN | DT_WIDTH | LEDC;
-			prev_val = ADMA2_32 | HS_EN | DT_WIDTH;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = PVE | BIT_AD_64 | HV4E | SDR104_MODE | V18SE;
-			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS00, SAAR);
-			break;
+	cmd_flags = CDNS_HOST_CMD_INHIBIT | CDNS_HOST_DATA_INHIBIT;
 
-		case SD_WRITE_MULTIPLE_BLOCK:
-		case SD_READ_MULTIPLE_BLOCK:
-			op = DATA_PRESENT | AUTO_CMD_EN | MULTI_BLK_READ;
-			write_value = ADMA2_32 | HS_EN | DT_WIDTH | LEDC;
-			prev_val = ADMA2_32 | HS_EN | DT_WIDTH;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = PVE | BIT_AD_64 | HV4E | SDR104_MODE | V18SE;
-			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS00, SAAR);
-			break;
+	if ((cmd->cmd_idx == SD_STOP_TRANSMISSION) && (!data_cmd)) {
+		cmd_flags &= ~CDNS_HOST_DATA_INHIBIT;
+	}
 
-		case SD_APP_SEND_SCR:
-			op = DATA_PRESENT;
-			write_value = ADMA2_32 | LEDC;
-			prev_val = LEDC;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = BIT_AD_64 | HV4E | V18SE;
-			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			break;
+	timeout = TIMEOUT;
+	do {
+		udelay(100);
+		if (--timeout <= 0) {
+			udelay(50);
+			NOTICE("Timeout occur data and cmd line %x\n",
+			 mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS09));
+			panic();
+		}
+	} while ((mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS09) & (cmd_flags)));
 
-		case SD_SEND_IF_COND:
-			op = DATA_PRESENT | CMD_IDX_CHK_ENABLE;
-			write_value = LEDC;
-			prev_val = 0x0;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = HV4E;
-			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			break;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS12, 0xFFFFFFFF);
+	cmd_flags = 0;
+	cmd_flags = (cmd->cmd_idx) << COM_IDX;
 
-		default:
-			write_value = LEDC;
-			prev_val = 0x0;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			op = 0;
-			break;
-		}
+	if ((cmd->resp_type & MMC_RSP_136) != 0) {
+		cmd_flags |= RES_TYPE_SEL_136;
+	} else if (((cmd->resp_type & MMC_RSP_48) != 0) &&
+			((cmd->resp_type & MMC_RSP_BUSY) != 0)) {
+		cmd_flags |= RES_TYPE_SEL_48_B;
+	} else if ((cmd->resp_type & MMC_RSP_48) != 0) {
+		cmd_flags |= RES_TYPE_SEL_48;
 	} else {
-		switch (cmd->cmd_idx) {
-		case SD_GO_IDLE_STATE:
-			write_value = LEDC;
-			prev_val = 0x0;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = HV4E;
-			srs11_val = SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			break;
+		cmd_flags &= ~RES_TYPE_SEL_NO;
+	}
+
+	if ((cmd->resp_type & MMC_RSP_CRC) != 0) {
+		cmd_flags |= CMD_CHECK_RESP_CRC;
+	}
 
-		case SD_ALL_SEND_CID:
-			write_value = LEDC;
-			prev_val = 0x0;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = HV4E | V18SE;
-			srs11_val = SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			break;
+	if ((cmd->resp_type & MMC_RSP_CMD_IDX) != 0) {
+		cmd_flags |= CMD_IDX_CHK_ENABLE;
+	}
+
+	if ((cmd->cmd_idx == MMC_ACMD(51)) || (cmd->cmd_idx == MMC_CMD(17)) ||
+		(cmd->cmd_idx == MMC_CMD(18)) || (cmd->cmd_idx == MMC_CMD(24)) ||
+		(cmd->cmd_idx == MMC_CMD(25))) {
+		mmio_write_8((cdns_params.reg_base + DTCV_OFFSET), DTCV_VAL);
+		cmd_flags |= DATA_PRESENT;
+		mode |= BLK_CNT_EN;
+
+		mode |= (DMA_ENABLED);
+		if ((cmd->cmd_idx == SD_WRITE_MULTIPLE_BLOCK) ||
+		(cmd->cmd_idx == SD_READ_MULTIPLE_BLOCK)) {
+			mode |= (MULTI_BLK_READ);
+		} else {
+			mode &= ~(MULTI_BLK_READ);
+		}
+		if ((cmd->cmd_idx == SD_WRITE_MULTIPLE_BLOCK) ||
+		(cmd->cmd_idx == SD_WRITE_SINGLE_BLOCK)) {
+			mode &= ~CMD_READ;
+		} else {
+			mode |= CMD_READ;
+		}
+		mmio_write_16(cdns_params.reg_base + SDHC_CDNS_SRS03, mode);
 
-		case SD_SEND_IF_COND:
-			op = CMD_IDX_CHK_ENABLE;
-			write_value = LEDC;
-			prev_val = 0x0;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			srs15_val = HV4E;
-			srs11_val = READ_CLK | SDMMC_CDN_ICE | SDMMC_CDN_ICS | SDMMC_CDN_SDCE;
-			cdns_srs11_srs15_config(srs11_val, srs15_val);
-			break;
+	} else {
+		mmio_write_8((cdns_params.reg_base + DTCV_OFFSET), DTCV_VAL);
+	}
 
-		case SD_STOP_TRANSMISSION:
-			op = CMD_STOP_ABORT_CMD;
-			break;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS02, cmd->cmd_arg);
+	mmio_write_16((cdns_params.reg_base + CICE_OFFSET),
+		SDHCI_MAKE_CMD(cmd->cmd_idx, cmd_flags));
 
-		case SD_SEND_STATUS:
-			break;
+	timeout = TIMEOUT;
 
-		case 1:
-			cmd->cmd_arg = 0;
-			break;
+	do {
+		udelay(CDNS_TIMEOUT);
+		status = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS12);
+	} while (((status & (INT_CMD_DONE | ERROR_INT)) == 0) && (timeout-- > 0));
 
-		case SD_SELECT_CARD:
-			op = MULTI_BLK_READ;
-			break;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS12, (SRS_12_CC_EN));
+	status_check = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS12) & 0xffff8000;
+	if (status_check != 0U) {
+		timeout = TIMEOUT;
+		ERROR("SD host controller send command failed, SRS12 = %x", status_check);
+		return -1;
+	}
 
-		case SD_APP_CMD:
-		default:
-			write_value = LEDC;
-			prev_val = 0x0;
-			cdns_srs10_value_toggle(write_value, prev_val);
-			op = 0;
-			break;
+	if (!((cmd_flags & RES_TYPE_SEL_NO) == 0)) {
+		cmd->resp_data[0] = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS04);
+		if ((cmd_flags & RES_TYPE_SEL_NO) == RES_TYPE_SEL_136) {
+			cmd->resp_data[1] = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS05);
+			cmd->resp_data[2] = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS06);
+			cmd->resp_data[3] = mmio_read_32(cdns_params.reg_base + SDHC_CDNS_SRS07);
+			/* 136-bit: RTS=01b, Response field R[127:8] - RESP3[23:0],
+			 * RESP2[31:0], RESP1[31:0], RESP0[31:0]
+			 * Subsystem expects 128 bits response but cadence SDHC sends
+			 * 120 bits response from R[127:8]. Bits manupulation to address
+			 * the correct responses for the 136 bit response type.
+			 */
+			cmd->resp_data[3] = ((cmd->resp_data[3] << 8) |
+						((cmd->resp_data[2] >> 24) &
+						CDNS_CSD_BYTE_MASK));
+			cmd->resp_data[2] = ((cmd->resp_data[2] << 8) |
+						((cmd->resp_data[1] >> 24) &
+						CDNS_CSD_BYTE_MASK));
+			cmd->resp_data[1] = ((cmd->resp_data[1] << 8) |
+						((cmd->resp_data[0] >> 24) &
+						CDNS_CSD_BYTE_MASK));
+			cmd->resp_data[0] = (cmd->resp_data[0] << 8);
 		}
 	}
 
-	switch (cmd->resp_type) {
-	case MMC_RESPONSE_NONE:
-		op |= CMD_READ | MULTI_BLK_READ | DMA_ENABLED | BLK_CNT_EN;
-		break;
+	mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS12, (SRS_12_CC_EN));
 
-	case MMC_RESPONSE_R2:
-		op |= CMD_READ | MULTI_BLK_READ | DMA_ENABLED | BLK_CNT_EN |
-			RES_TYPE_SEL_136 | CMD_CHECK_RESP_CRC;
-		break;
+	return 0;
+}
 
-	case MMC_RESPONSE_R3:
-		op |= CMD_READ | MULTI_BLK_READ | DMA_ENABLED | BLK_CNT_EN |
-			RES_TYPE_SEL_48;
-		break;
+void sd_host_adma_prepare(struct cdns_idmac_desc *desc_ptr, uint64_t buf,
+			  size_t size)
+{
+	uint32_t full_desc_cnt = 0;
+	uint32_t non_full_desc_cnt = 0;
+	uint64_t desc_address;
+	uint32_t block_count;
+	uint32_t transfer_block_size;
 
-	case MMC_RESPONSE_R1:
-		if ((cmd->cmd_idx == SD_WRITE_SINGLE_BLOCK) || (cmd->cmd_idx
-			== SD_WRITE_MULTIPLE_BLOCK)) {
-			op |= DMA_ENABLED | BLK_CNT_EN | RES_TYPE_SEL_48
-			| CMD_CHECK_RESP_CRC | CMD_IDX_CHK_ENABLE;
-		} else {
-			op |= DMA_ENABLED | BLK_CNT_EN | CMD_READ | RES_TYPE_SEL_48
-			| CMD_CHECK_RESP_CRC | CMD_IDX_CHK_ENABLE;
+	full_desc_cnt = (size / PAGE_BUFFER_LEN);
+	non_full_desc_cnt = (size % PAGE_BUFFER_LEN);
+	for (int i = 0; i < full_desc_cnt; i++) {
+		desc_ptr->attr = (ADMA_DESC_TRANSFER_DATA | ADMA_DESC_ATTR_VALID);
+		desc_ptr->len = 0; // 0 means 64kb page size it will take
+		desc_ptr->addr_lo = 0;
+#if CONFIG_DMA_ADDR_T_64BIT == 1
+		desc_ptr->addr_hi = (uint32_t)((buf >> 32) & 0xffffffff);
+#endif
+		if (non_full_desc_cnt == 0) {
+			desc_ptr->attr |= (ADMA_DESC_ATTR_END);
 		}
-		break;
-
-	default:
-		op |= DMA_ENABLED | BLK_CNT_EN | CMD_READ | MULTI_BLK_READ |
-			RES_TYPE_SEL_48 | CMD_CHECK_RESP_CRC | CMD_IDX_CHK_ENABLE;
-		break;
+	buf += PAGE_BUFFER_LEN;
 	}
 
-	timeout = TIMEOUT;
-	do {
-		udelay(100);
-		ret = cdns_busy();
-		if (--timeout <= 0) {
-			udelay(50);
-			panic();
+	if (non_full_desc_cnt != 0) {
+		desc_ptr->attr =
+		(ADMA_DESC_TRANSFER_DATA | ADMA_DESC_ATTR_END | ADMA_DESC_ATTR_VALID);
+		desc_ptr->addr_lo = buf & 0xffffffff;
+		desc_ptr->len = size;
+#if CONFIG_DMA_ADDR_T_64BIT == 1
+		desc_ptr->addr_hi = (uint32_t)((buf >> 32) & 0xffffffff);
+#endif
+		desc_address = (uint64_t)desc_ptr;
+		if (size > MMC_MAX_BLOCK_LEN) {
+			transfer_block_size = MMC_MAX_BLOCK_LEN;
+		} else {
+			transfer_block_size = size;
 		}
-	} while (ret);
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS12, UINT_MAX);
+		block_count = (size / transfer_block_size);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS01,
+				((transfer_block_size << BLOCK_SIZE) | SDMA_BUF |
+				(block_count << BLK_COUNT_CT)));
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS22,
+				(uint32_t)desc_address & 0xFFFFFFFF);
+		mmio_write_32(cdns_params.reg_base + SDHC_CDNS_SRS23,
+				(uint32_t)(desc_address >> 32 & 0xFFFFFFFF));
+	}
+}
 
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS02, cmd->cmd_arg);
-	mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS14, 0x00000000);
-	if (cmd_indx == 1)
-		mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS03, SDHC_CDNS_SRS03_VALUE);
-	else
-		mmio_write_32(MMC_REG_BASE + SDHC_CDNS_SRS03, op | cmd_indx);
+int cdns_mmc_init(struct cdns_sdmmc_params *params,
+		  struct mmc_device_info *info)
+{
 
-	timeout = TIMEOUT;
-	do {
-		udelay(500);
-		value = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS12);
-	} while (((value & (INT_CMD_DONE | ERROR_INT)) == 0) && (timeout-- > 0));
+	int result = 0;
 
-	timeout = TIMEOUT;
+	assert((params != NULL) &&
+		((params->reg_base & MMC_BLOCK_MASK) == 0) &&
+		((params->desc_size & MMC_BLOCK_MASK) == 0) &&
+		((params->reg_pinmux & MMC_BLOCK_MASK) == 0) &&
+		((params->reg_phy & MMC_BLOCK_MASK) == 0) &&
+		(params->desc_size > 0) &&
+		(params->clk_rate > 0) &&
+		((params->bus_width == MMC_BUS_WIDTH_1) ||
+		(params->bus_width == MMC_BUS_WIDTH_4) ||
+		(params->bus_width == MMC_BUS_WIDTH_8)));
 
-	if (data_cmd) {
-		data_cmd = false;
-		do {
-			udelay(250);
-		} while (((value & TRAN_COMP) == 0) && (timeout-- > 0));
-	}
+	memcpy(&cdns_params, params, sizeof(struct cdns_sdmmc_params));
 
-	status_check = value & SRS12_ERR_MASK;
-	if (status_check != 0U) {
-		ERROR("SD host controller send command failed, SRS12 = %x", status);
-		return -1;
+	cdns_set_sdmmc_var(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	result = cdns_sd_host_init(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
+	if (result < 0) {
+		return result;
 	}
 
-	if ((op & RES_TYPE_SEL_48) || (op & RES_TYPE_SEL_136)) {
-		cmd->resp_data[0] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS04);
-		if (op & RES_TYPE_SEL_136) {
-			cmd->resp_data[1] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS05);
-			cmd->resp_data[2] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS06);
-			cmd->resp_data[3] = mmio_read_32(MMC_REG_BASE + SDHC_CDNS_SRS07);
-		}
-	}
+	cdns_params.cdn_sdmmc_dev_type = info->mmc_dev_type;
+	cdns_params.cdn_sdmmc_dev_mode = SD_DS;
 
-	return 0;
+	result = mmc_init(&cdns_sdmmc_ops, params->clk_rate, params->bus_width,
+			params->flags, info);
+
+	return result;
 }
diff --git a/drivers/st/ddr/phy/phyinit/src/ddrphy_phyinit_c_initphyconfig.c b/drivers/st/ddr/phy/phyinit/src/ddrphy_phyinit_c_initphyconfig.c
index a0712b5..e5c8258 100644
--- a/drivers/st/ddr/phy/phyinit/src/ddrphy_phyinit_c_initphyconfig.c
+++ b/drivers/st/ddr/phy/phyinit/src/ddrphy_phyinit_c_initphyconfig.c
@@ -761,17 +761,9 @@
 {
 	uint16_t loopvector;
 	uint16_t pllbypass_dat = 0U;
-	uint16_t skipddc_dat = 0U;	/*
-					 * Set to vector offset based on frequency to disable dram
-					 * drift compensation.
-					 */
 
 	pllbypass_dat |= (uint16_t)config->uib.pllbypass;
 
-	if (config->uib.frequency < 333U) {
-		skipddc_dat |= 0x5U;
-	}
-
 	for (loopvector = 0U; loopvector < 8U; loopvector++) {
 		uint16_t dfifreqxlat_dat;
 		uintptr_t reg = (uintptr_t)(DDRPHYC_BASE + (4U * (C0 | TMASTER |
@@ -799,6 +791,15 @@
 		}
 #else /* STM32MP_LPDDR4_TYPE */
 		if (loopvector == 0U) {
+			uint16_t skipddc_dat = 0U;	/*
+							 * Set to vector offset based on frequency
+							 * to disable dram drift compensation.
+							 */
+
+			if (config->uib.frequency < 333U) {
+				skipddc_dat |= 0x5U;
+			}
+
 			/*
 			 * Retrain & Relock DfiFreq = 00,01,02,03)  Use StartVec 0 (pll_enabled) or
 			 * StartVec 1 (pll_bypassed).
diff --git a/include/arch/aarch32/arch.h b/include/arch/aarch32/arch.h
index d32ead4..d2591dd 100644
--- a/include/arch/aarch32/arch.h
+++ b/include/arch/aarch32/arch.h
@@ -697,8 +697,7 @@
 /* PAR fields */
 #define PAR_F_SHIFT	U(0)
 #define PAR_F_MASK	ULL(0x1)
-#define PAR_ADDR_SHIFT	U(12)
-#define PAR_ADDR_MASK	(BIT_64(40) - ULL(1)) /* 40-bits-wide page address */
+#define PAR_ADDR_MASK	GENMASK_64(39, 12) /* 28-bits-wide page address */
 
 /*******************************************************************************
  * Definitions for system register interface to AMU for FEAT_AMUv1
diff --git a/include/arch/aarch32/arch_features.h b/include/arch/aarch32/arch_features.h
index abe34a4..a29b672 100644
--- a/include/arch/aarch32/arch_features.h
+++ b/include/arch/aarch32/arch_features.h
@@ -194,5 +194,7 @@
 static inline bool is_feat_ebep_present(void) { return false; }
 __attribute__((always_inline))
 static inline bool is_feat_sebep_present(void) { return false; }
+__attribute__((always_inline))
+static inline bool is_feat_d128_present(void) { return false; }
 
 #endif /* ARCH_FEATURES_H */
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 6a19822..3f0120c 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -331,6 +331,7 @@
 #define PARANGE_0100	U(44)
 #define PARANGE_0101	U(48)
 #define PARANGE_0110	U(52)
+#define PARANGE_0111	U(56)
 
 #define ID_AA64MMFR0_EL1_ECV_SHIFT		U(60)
 #define ID_AA64MMFR0_EL1_ECV_MASK		ULL(0xf)
@@ -394,6 +395,10 @@
 /* ID_AA64MMFR3_EL1 definitions */
 #define ID_AA64MMFR3_EL1			S3_0_C0_C7_3
 
+#define ID_AA64MMFR3_EL1_D128_SHIFT		U(32)
+#define ID_AA64MMFR3_EL1_D128_MASK		ULL(0xf)
+#define D128_IMPLEMENTED			ULL(0x1)
+
 #define ID_AA64MMFR3_EL1_S2POE_SHIFT		U(20)
 #define ID_AA64MMFR3_EL1_S2POE_MASK		ULL(0xf)
 
@@ -594,6 +599,7 @@
 #define SCR_FGTEN2_BIT		(UL(1) << 59)
 #define SCR_NSE_BIT		(ULL(1) << SCR_NSE_SHIFT)
 #define SCR_GPF_BIT		(UL(1) << 48)
+#define SCR_D128En_BIT		(UL(1) << 47)
 #define SCR_TWEDEL_SHIFT	U(30)
 #define SCR_TWEDEL_MASK		ULL(0xf)
 #define SCR_PIEN_BIT		(UL(1) << 45)
@@ -1179,8 +1185,9 @@
 /* PAR_EL1 fields */
 #define PAR_F_SHIFT	U(0)
 #define PAR_F_MASK	ULL(0x1)
-#define PAR_ADDR_SHIFT	U(12)
-#define PAR_ADDR_MASK	(BIT(40) - ULL(1)) /* 40-bits-wide page address */
+
+#define PAR_D128_ADDR_MASK	GENMASK(55, 12) /* 44-bits-wide page address */
+#define PAR_ADDR_MASK		GENMASK(51, 12) /* 40-bits-wide page address */
 
 /*******************************************************************************
  * Definitions for system register interface to SPE
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index de21fea..ec38d76 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -138,6 +138,8 @@
  * +----------------------------+
  * |	FEAT_SCTLR2		|
  * +----------------------------+
+ * |	FEAT_D128		|
+ * +----------------------------+
  */
 
 __attribute__((always_inline))
@@ -275,6 +277,11 @@
 		     ID_AA64MMFR3_EL1_SCTLR2_MASK, SCTLR2_IMPLEMENTED,
 		     ENABLE_FEAT_SCTLR2)
 
+/* FEAT_D128 */
+CREATE_FEATURE_FUNCS(feat_d128, id_aa64mmfr3_el1, ID_AA64MMFR3_EL1_D128_SHIFT,
+		     ID_AA64MMFR3_EL1_D128_MASK, D128_IMPLEMENTED,
+		     ENABLE_FEAT_D128)
+
 __attribute__((always_inline))
 static inline bool is_feat_sxpie_supported(void)
 {
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index acaa1b8..9c36e4b 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -13,6 +13,7 @@
 #include <string.h>
 
 #include <arch.h>
+#include <lib/extensions/sysreg128.h>
 
 /**********************************************************************
  * Macros which create inline functions to read or write CPU system
@@ -263,7 +264,12 @@
 #define write_daifclr(val) SYSREG_WRITE_CONST(daifclr, val)
 #define write_daifset(val) SYSREG_WRITE_CONST(daifset, val)
 
+#if ENABLE_FEAT_D128
+DECLARE_SYSREG128_RW_FUNCS(par_el1)
+#else
 DEFINE_SYSREG_RW_FUNCS(par_el1)
+#endif
+
 DEFINE_IDREG_READ_FUNC(id_pfr1_el1)
 DEFINE_IDREG_READ_FUNC(id_aa64isar0_el1)
 DEFINE_IDREG_READ_FUNC(id_aa64isar1_el1)
@@ -443,13 +449,21 @@
 DEFINE_SYSREG_RW_FUNCS(tcr_el2)
 DEFINE_SYSREG_RW_FUNCS(tcr_el3)
 
+#if ENABLE_FEAT_D128
+DECLARE_SYSREG128_RW_FUNCS(ttbr0_el1)
+DECLARE_SYSREG128_RW_FUNCS(ttbr1_el1)
+DECLARE_SYSREG128_RW_FUNCS(ttbr0_el2)
+DECLARE_SYSREG128_RW_FUNCS(ttbr1_el2)
+DECLARE_SYSREG128_RW_FUNCS(vttbr_el2)
+#else
 DEFINE_SYSREG_RW_FUNCS(ttbr0_el1)
-DEFINE_SYSREG_RW_FUNCS(ttbr0_el2)
-DEFINE_SYSREG_RW_FUNCS(ttbr0_el3)
-
 DEFINE_SYSREG_RW_FUNCS(ttbr1_el1)
-
+DEFINE_SYSREG_RW_FUNCS(ttbr0_el2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(ttbr1_el2, TTBR1_EL2)
 DEFINE_SYSREG_RW_FUNCS(vttbr_el2)
+#endif
+
+DEFINE_SYSREG_RW_FUNCS(ttbr0_el3)
 
 DEFINE_SYSREG_RW_FUNCS(cptr_el2)
 DEFINE_SYSREG_RW_FUNCS(cptr_el3)
@@ -574,7 +588,6 @@
 
 /* Armv8.1 VHE Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(contextidr_el2, CONTEXTIDR_EL2)
-DEFINE_RENAME_SYSREG_RW_FUNCS(ttbr1_el2, TTBR1_EL2)
 
 /* Armv8.2 ID Registers */
 DEFINE_RENAME_IDREG_READ_FUNC(id_aa64mmfr2_el1, ID_AA64MMFR2_EL1)
@@ -671,8 +684,13 @@
 DEFINE_RENAME_SYSREG_RW_FUNCS(gcspr_el0, GCSPR_EL0)
 
 /* FEAT_THE Registers */
+#if ENABLE_FEAT_D128
+DECLARE_SYSREG128_RW_FUNCS(rcwmask_el1)
+DECLARE_SYSREG128_RW_FUNCS(rcwsmask_el1)
+#else
 DEFINE_RENAME_SYSREG_RW_FUNCS(rcwmask_el1, RCWMASK_EL1)
 DEFINE_RENAME_SYSREG_RW_FUNCS(rcwsmask_el1, RCWSMASK_EL1)
+#endif
 
 /* FEAT_SCTLR2 Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(sctlr2_el1, SCTLR2_EL1)
diff --git a/include/common/par.h b/include/common/par.h
new file mode 100644
index 0000000..c8d67a3
--- /dev/null
+++ b/include/common/par.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PAR_H
+#define PAR_H
+
+#include<arch_features.h>
+#include<lib/extensions/sysreg128.h>
+
+static inline uint64_t get_par_el1_pa(sysreg_t par)
+{
+	uint64_t pa = par & UINT64_MAX;
+	/* PA, bits [51:12] is Output address */
+	uint64_t mask = PAR_ADDR_MASK;
+
+#if ENABLE_FEAT_D128
+	/* If D128 is in use, the PA is in the upper 64-bit word of PAR_EL1 */
+	if (is_feat_d128_supported() && (par & PAR_EL1_D128)) {
+		pa = (par >> 64) & UINT64_MAX;
+		/* PA, bits [55:12] is Output address */
+		mask = PAR_D128_ADDR_MASK;
+	}
+#endif
+	return pa & mask;
+}
+
+#endif /* PAR_H */
diff --git a/include/drivers/cadence/cdns_sdmmc.h b/include/drivers/cadence/cdns_sdmmc.h
index 4be7414..f8d616f 100644
--- a/include/drivers/cadence/cdns_sdmmc.h
+++ b/include/drivers/cadence/cdns_sdmmc.h
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
  * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ * Copyright (c) 2024, Altera Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,23 +11,26 @@
 
 #include <drivers/cadence/cdns_combo_phy.h>
 #include <drivers/mmc.h>
-#include "socfpga_plat_def.h"
 
 #if MMC_DEVICE_TYPE == 0
-#define CONFIG_DMA_ADDR_T_64BIT		0
+#define CONFIG_DMA_ADDR_T_64BIT			0
 #endif
 
-#define MMC_REG_BASE			SOCFPGA_MMC_REG_BASE
-#define COMBO_PHY_REG		0x0
-#define SDHC_EXTENDED_WR_MODE_MASK	0xFFFFFFF7
-#define SDHC_DLL_RESET_MASK	0x00000001
+#define MMC_REG_BASE				SOCFPGA_MMC_REG_BASE
+#define COMBO_PHY_REG				0x0
+#define SDHC_EXTENDED_WR_MODE_MASK		0xFFFFFFF7
+#define SDHC_DLL_RESET_MASK			0x00000001
+#define MMC_MAX_BLOCK_LEN 512U
+
 /* HRS09 */
 #define SDHC_PHY_SW_RESET			BIT(0)
-#define SDHC_PHY_INIT_COMPLETE		BIT(1)
-#define SDHC_EXTENDED_RD_MODE(x)	((x) << 2)
+#define SDHC_PHY_INIT_COMPLETE			BIT(1)
+#define SDHC_EXTENDED_RD_MODE(x)		((x) << 2)
 #define EXTENDED_WR_MODE			3
-#define SDHC_EXTENDED_WR_MODE(x)	((x) << 3)
-#define RDCMD_EN					15
+#define SDHC_EXTENDED_WR_MODE(x)		((x) << 3)
+#define RDCMD_EN				(3 << 15)
+#define PHY_SW_RESET_EN				(1 << 0)
+#define PHY_INIT_COMPLETE_BIT			(1 << 1)
 #define SDHC_RDCMD_EN(x)			((x) << 15)
 #define SDHC_RDDATA_EN(x)			((x) << 16)
 
@@ -38,9 +42,9 @@
 /* • 1111b - Reserved */
 /* • 1110b - t_sdmclk*2(27+2) */
 /* • 1101b - t_sdmclk*2(26+2) */
-#define READ_CLK					0xa << 16
-#define WRITE_CLK					0xe << 16
-#define DTC_VAL						0xE
+#define READ_CLK				0xa << 16
+#define WRITE_CLK				0xe << 16
+#define DTC_VAL					0xE
 
 /* SRS00 */
 /* System Address / Argument 2 / 32-bit block count
@@ -49,18 +53,18 @@
  * • SDMA system memory address
  * • Auto CMD23 Argument
  */
-#define SAAR						(1)
+#define SAAR					(1)
 
 /* SRS01 */
 /* Transfer Block Size
  * This field defines block size for block data transfers
  */
-#define BLOCK_SIZE					0
+#define BLOCK_SIZE				0
 
 /* SDMA Buffer Boundary
  * System address boundary can be set for SDMA engine.
  */
-#define SDMA_BUF					7 << 12
+#define SDMA_BUF				7 << 12
 
 /* Block Count For Current Transfer
  * To set the number of data blocks can be defined for next transfer
@@ -68,93 +72,108 @@
 #define BLK_COUNT_CT				16
 
 /* SRS03 */
-#define CMD_START					(U(1) << 31)
+#define CMD_START				(U(1) << 31)
 #define CMD_USE_HOLD_REG			(1 << 29)
 #define CMD_UPDATE_CLK_ONLY			(1 << 21)
 #define CMD_SEND_INIT				(1 << 15)
 #define CMD_STOP_ABORT_CMD			(4 << 22)
 #define CMD_RESUME_CMD				(2 << 22)
 #define CMD_SUSPEND_CMD				(1 << 22)
-#define DATA_PRESENT				(1 << 21)
-#define CMD_IDX_CHK_ENABLE			(1 << 20)
-#define CMD_WRITE					(0 << 4)
-#define CMD_READ					(1 << 4)
+#define DATA_PRESENT				(0x20)
+#define CMD_IDX_CHK_ENABLE			(0x10)
+#define CMD_WRITE				(0 << 4)
+#define CMD_READ				(1 << 4)
 #define	MULTI_BLK_READ				(1 << 5)
-#define RESP_ERR					(1 << 7)
-#define CMD_CHECK_RESP_CRC			(1 << 19)
-#define RES_TYPE_SEL_48				(2 << 16)
-#define RES_TYPE_SEL_136			(1 << 16)
-#define RES_TYPE_SEL_48_B			(3 << 16)
-#define RES_TYPE_SEL_NO				(0 << 16)
-#define DMA_ENABLED					(1 << 0)
-#define BLK_CNT_EN					(1 << 1)
-#define AUTO_CMD_EN					(2 << 2)
-#define COM_IDX						24
-#define ERROR_INT					(1 << 15)
-#define INT_SBE						(1 << 13)
-#define INT_HLE						(1 << 12)
-#define INT_FRUN					(1 << 11)
-#define INT_DRT						(1 << 9)
-#define INT_RTO						(1 << 8)
-#define INT_DCRC					(1 << 7)
-#define INT_RCRC					(1 << 6)
-#define INT_RXDR					(1 << 5)
-#define INT_TXDR					(1 << 4)
-#define INT_DTO						(1 << 3)
+#define RESP_ERR				(1 << 7)
+#define CMD_CHECK_RESP_CRC			(0x08)
+#define RES_TYPE_SEL_48				(0x2)
+#define RES_TYPE_SEL_136			(0x1)
+#define RES_TYPE_SEL_48_B			(0x3)
+#define RES_TYPE_SEL_NO				(0x3)
+#define DMA_ENABLED				(1 << 0)
+#define BLK_CNT_EN				(1 << 1)
+#define AUTO_CMD_EN				(2 << 2)
+#define COM_IDX					24
+#define ERROR_INT				(1 << 15)
+#define INT_SBE					(1 << 13)
+#define INT_HLE					(1 << 12)
+#define INT_FRUN				(1 << 11)
+#define INT_DRT					(1 << 9)
+#define INT_RTO					(1 << 8)
+#define INT_DCRC				(1 << 7)
+#define INT_RCRC				(1 << 6)
+#define INT_RXDR				(1 << 5)
+#define INT_TXDR				(1 << 4)
+#define INT_DTO					(1 << 3)
 #define INT_CMD_DONE				(1 << 0)
-#define TRAN_COMP					(1 << 1)
+#define TRAN_COMP				(1 << 1)
 
 /* SRS09 */
 #define STATUS_DATA_BUSY			BIT(2)
+#define CI					16
+#define CHECK_CARD				BIT(CI)
 
 /* SRS10 */
+#define BIT1					(0 << 1)
+#define BIT4					(1 << 1)
+#define BIT8					(1 << 5)
+
 /* LED Control
  * State of this bit directly drives led port of the host
  * in order to control the external LED diode
  * Default value 0 << 1
  */
-#define LEDC						BIT(0)
-#define LEDC_OFF					0 << 1
+#define LEDC					BIT(0)
+#define LEDC_OFF				(0 << 1)
 
 /* Data Transfer Width
  * Bit used to configure DAT bus width to 1 or 4
  * Default value 1 << 1
  */
-#define DT_WIDTH					BIT(1)
-#define DTW_4BIT					1 << 1
+#define DT_WIDTH				BIT(1)
+#define DTW_4BIT				(1 << 1)
 
 /* Extended Data Transfer Width
  * This bit is to enable/disable 8-bit DAT bus width mode
  * Default value 1 << 5
  */
-#define EDTW_8BIT					1 << 5
+#define EDTW_8BIT				BIT(5)
 
 /* High Speed Enable
  * Selects operating mode to Default Speed (HSE=0) or High Speed (HSE=1)
  */
-#define HS_EN						BIT(2)
+#define HS_EN					BIT(2)
 
 /* here 0 defines the 64 Kb size */
 #define MAX_64KB_PAGE				0
-#define EMMC_DESC_SIZE		(1<<20)
-
+#define EMMC_DESC_SIZE				(1<<20)
+#define DTCV_OFFSET				(0x22E)
+#define DTCV_VAL				(0xE)
+#define CICE_OFFSET				(0x20E)
+#define SRS_12_CC_EN				(1 << 0)
 /* SRS11 */
 /* Software Reset For All
  * When set to 1, the entire slot is reset
  * After completing the reset operation, SRFA bit is automatically cleared
  */
-#define SRFA						BIT(24)
+#define SRFA					BIT(24)
 
 /* Software Reset For CMD Line
  * When set to 1, resets the logic related to the command generation and response checking
  */
-#define SRCMD						BIT(25)
+#define SRCMD					BIT(25)
 
 /* Software Reset For DAT Line
  * When set to 1, resets the logic related to the data path,
  * including data buffers and the DMA logic
  */
-#define SRDAT						BIT(26)
+#define SRDAT					BIT(26)
+
+
+/* SRS12 */
+/* Error mask */
+#define SRS12_ERR_MASK				0xFFFF8000U
+#define CDNS_CSD_BYTE_MASK			0x000000FFU
 
 /* SRS15 */
 /* UHS Mode Select
@@ -165,40 +184,43 @@
  * • 011b - SDR104
  * • 100b - DDR50
  */
-#define SDR12_MODE					0 << 16
-#define SDR25_MODE					1 << 16
-#define SDR50_MODE					2 << 16
-#define SDR104_MODE					3 << 16
-#define DDR50_MODE					4 << 16
+#define SDR12_MODE				0 << 16
+#define SDR25_MODE				1 << 16
+#define SDR50_MODE				2 << 16
+#define SDR104_MODE				3 << 16
+#define DDR50_MODE				4 << 16
 /* 1.8V Signaling Enable
  * • 0 - for Default Speed, High Speed mode
  * • 1 - for UHS-I mode
  */
-#define V18SE						BIT(19)
+#define V18SE					BIT(19)
 
 /* CMD23 Enable
  * In result of Card Identification process,
  * Host Driver set this bit to 1 if Card supports CMD23
  */
-#define CMD23_EN					BIT(27)
+#define CMD23_EN				BIT(27)
 
 /* Host Version 4.00 Enable
  * • 0 - Version 3.00
  * • 1 - Version 4.00
  */
-#define HV4E						BIT(28)
+#define HV4E					BIT(28)
 /* Conf depends on SRS15.HV4E */
-#define SDMA						0 << 3
-#define ADMA2_32					2 << 3
-#define ADMA2_64					3 << 3
+#define SDMA					0 << 3
+#define ADMA2_32				2 << 3
+#define ADMA2_64				3 << 3
+#define DMA_SEL_BIT				3 << 3
+#define DMA_SEL_BIT_2				2 << 3
+#define DMA_SEL_BIT_3				3 << 3
 
 /* Preset Value Enable
  * Setting this bit to 1 triggers an automatically update of SRS11
  */
-#define PVE							BIT(31)
+#define PVE					BIT(31)
 
-#define BIT_AD_32					0 << 29
-#define BIT_AD_64					1 << 29
+#define BIT_AD_32				0 << 29
+#define BIT_AD_64				1 << 29
 
 /* SW RESET REG*/
 #define SDHC_CDNS_HRS00				(0x00)
@@ -206,7 +228,7 @@
 
 /* PHY access port */
 #define SDHC_CDNS_HRS04				0x10
-#define SDHC_CDNS_HRS04_ADDR		GENMASK(5, 0)
+#define SDHC_CDNS_HRS04_ADDR			GENMASK(5, 0)
 
 /* PHY data access port */
 #define SDHC_CDNS_HRS05				0x14
@@ -233,14 +255,51 @@
 #define SDHC_CDNS_SRS13				0x234
 #define SDHC_CDNS_SRS14				0x238
 #define SDHC_CDNS_SRS15				0x23c
+#define SDHC_CDNS_SRS16				0x240
 #define SDHC_CDNS_SRS21				0x254
 #define SDHC_CDNS_SRS22				0x258
 #define SDHC_CDNS_SRS23				0x25c
+#define SDHC_CDNS_SRS24				0x260
+#define SDHC_CDNS_SRS25				0x264
+
+/* SRS00 */
+#define SAAR					(1)
+
+/* SRS03 */
+#define CMD_START				(U(1) << 31)
+#define CMD_USE_HOLD_REG			(1 << 29)
+#define CMD_UPDATE_CLK_ONLY			(1 << 21)
+#define CMD_SEND_INIT				(1 << 15)
+#define CMD_STOP_ABORT_CMD			(4 << 22)
+#define CMD_RESUME_CMD				(2 << 22)
+#define CMD_SUSPEND_CMD				(1 << 22)
+#define DMA_ENABLED				(1 << 0)
+#define BLK_CNT_EN				(1 << 1)
+#define AUTO_CMD_EN				(2 << 2)
+#define COM_IDX					24
+#define ERROR_INT				(1 << 15)
+#define INT_SBE					(1 << 13)
+#define INT_HLE					(1 << 12)
+#define INT_FRUN				(1 << 11)
+#define INT_DRT					(1 << 9)
+#define INT_RTO					(1 << 8)
+#define INT_DCRC				(1 << 7)
+#define INT_RCRC				(1 << 6)
+#define INT_RXDR				(1 << 5)
+#define INT_TXDR				(1 << 4)
+#define INT_DTO					(1 << 3)
+#define INT_CMD_DONE				(1 << 0)
+#define TRAN_COMP				(1 << 1)
+#define CDNS_HOST_CMD_INHIBIT			(BIT(0))
+#define CDNS_HOST_DATA_INHIBIT			(BIT(1))
+#define ACE_CMD_12				(BIT(2))
+
+#define PAGE_BUFFER_LEN				(64 * 1024)
 
 /* HRS07 */
 #define SDHC_CDNS_HRS07				0x1c
 #define SDHC_IDELAY_VAL(x)			((x) << 0)
-#define SDHC_RW_COMPENSATE(x)		((x) << 16)
+#define SDHC_RW_COMPENSATE(x)			((x) << 16)
 
 /* PHY reset port */
 #define SDHC_CDNS_HRS09				0x24
@@ -254,49 +313,49 @@
 
 /* Pinmux headers will reomove after ATF driver implementation */
 #define PINMUX_SDMMC_SEL			0x0
-#define PIN0SEL						0x00
-#define PIN1SEL						0x04
-#define PIN2SEL						0x08
-#define PIN3SEL						0x0C
-#define PIN4SEL						0x10
-#define PIN5SEL						0x14
-#define PIN6SEL						0x18
-#define PIN7SEL						0x1C
-#define PIN8SEL						0x20
-#define PIN9SEL						0x24
-#define PIN10SEL					0x28
+#define PIN0SEL					0x00
+#define PIN1SEL					0x04
+#define PIN2SEL					0x08
+#define PIN3SEL					0x0C
+#define PIN4SEL					0x10
+#define PIN5SEL					0x14
+#define PIN6SEL					0x18
+#define PIN7SEL					0x1C
+#define PIN8SEL					0x20
+#define PIN9SEL					0x24
+#define PIN10SEL				0x28
 
 /* HRS16 */
 #define SDHC_WRCMD0_DLY(x)			((x) << 0)
 #define SDHC_WRCMD1_DLY(x)			((x) << 4)
 #define SDHC_WRDATA0_DLY(x)			((x) << 8)
 #define SDHC_WRDATA1_DLY(x)			((x) << 12)
-#define SDHC_WRCMD0_SDCLK_DLY(x)	((x) << 16)
-#define SDHC_WRCMD1_SDCLK_DLY(x)	((x) << 20)
-#define SDHC_WRDATA0_SDCLK_DLY(x)	((x) << 24)
-#define SDHC_WRDATA1_SDCLK_DLY(x)	((x) << 28)
+#define SDHC_WRCMD0_SDCLK_DLY(x)		((x) << 16)
+#define SDHC_WRCMD1_SDCLK_DLY(x)		((x) << 20)
+#define SDHC_WRDATA0_SDCLK_DLY(x)		((x) << 24)
+#define SDHC_WRDATA1_SDCLK_DLY(x)		((x) << 28)
 
 /* Shared Macros */
 #define SDMMC_CDN(_reg)				(SDMMC_CDN_REG_BASE + \
 								(SDMMC_CDN_##_reg))
 
 /* MMC Peripheral Definition */
-#define SOCFPGA_MMC_BLOCK_MASK		(SOCFPGA_MMC_BLOCK_SIZE - U(1))
-#define SOCFPGA_MMC_BOOT_CLK_RATE	(400 * 1000)
+#define SOCFPGA_MMC_BLOCK_MASK			(SOCFPGA_MMC_BLOCK_SIZE - U(1))
+#define SOCFPGA_MMC_BOOT_CLK_RATE		(400 * 1000)
 #define MMC_RESPONSE_NONE			0
-#define SDHC_CDNS_SRS03_VALUE		0x01020013
+#define SDHC_CDNS_SRS03_VALUE			0x01020013
 
 /* Value randomly chosen for eMMC RCA, it should be > 1 */
-#define MMC_FIX_RCA					6
+#define MMC_FIX_RCA				6
 #define RCA_SHIFT_OFFSET			16
 
-#define CMD_EXTCSD_PARTITION_CONFIG	179
-#define CMD_EXTCSD_BUS_WIDTH		183
-#define CMD_EXTCSD_HS_TIMING		185
+#define CMD_EXTCSD_PARTITION_CONFIG		179
+#define CMD_EXTCSD_BUS_WIDTH			183
+#define CMD_EXTCSD_HS_TIMING			185
 #define CMD_EXTCSD_SEC_CNT			212
 
-#define PART_CFG_BOOT_PARTITION1_ENABLE	(U(1) << 3)
-#define PART_CFG_PARTITION1_ACCESS	(U(1) << 0)
+#define PART_CFG_BOOT_PARTITION1_ENABLE		(U(1) << 3)
+#define PART_CFG_PARTITION1_ACCESS		(U(1) << 0)
 
 /* Values in EXT CSD register */
 #define MMC_BUS_WIDTH_1				U(0)
@@ -304,8 +363,8 @@
 #define MMC_BUS_WIDTH_8				U(2)
 #define MMC_BUS_WIDTH_DDR_4			U(5)
 #define MMC_BUS_WIDTH_DDR_8			U(6)
-#define MMC_BOOT_MODE_BACKWARD		(U(0) << 3)
-#define MMC_BOOT_MODE_HS_TIMING		(U(1) << 3)
+#define MMC_BOOT_MODE_BACKWARD			(U(0) << 3)
+#define MMC_BOOT_MODE_HS_TIMING			(U(1) << 3)
 #define MMC_BOOT_MODE_DDR			(U(2) << 3)
 
 #define EXTCSD_SET_CMD				(U(0) << 24)
@@ -314,14 +373,14 @@
 #define EXTCSD_WRITE_BYTES			(U(3) << 24)
 #define EXTCSD_CMD(x)				(((x) & 0xff) << 16)
 #define EXTCSD_VALUE(x)				(((x) & 0xff) << 8)
-#define EXTCSD_CMD_SET_NORMAL		U(1)
+#define EXTCSD_CMD_SET_NORMAL			U(1)
 
-#define CSD_TRAN_SPEED_UNIT_MASK	GENMASK(2, 0)
-#define CSD_TRAN_SPEED_MULT_MASK	GENMASK(6, 3)
-#define CSD_TRAN_SPEED_MULT_SHIFT	3
+#define CSD_TRAN_SPEED_UNIT_MASK		GENMASK(2, 0)
+#define CSD_TRAN_SPEED_MULT_MASK		GENMASK(6, 3)
+#define CSD_TRAN_SPEED_MULT_SHIFT		3
 
-#define STATUS_CURRENT_STATE(x)		(((x) & 0xf) << 9)
-#define STATUS_READY_FOR_DATA		BIT(8)
+#define STATUS_CURRENT_STATE(x)			(((x) & 0xf) << 9)
+#define STATUS_READY_FOR_DATA			BIT(8)
 #define STATUS_SWITCH_ERROR			BIT(7)
 #define MMC_GET_STATE(x)			(((x) >> 9) & 0xf)
 #define MMC_STATE_IDLE				0
@@ -342,12 +401,51 @@
 #define VHS_2_7_3_6_V				BIT(8)
 
 /*ADMA table component*/
-#define ADMA_DESC_ATTR_VALID		BIT(0)
+#define ADMA_DESC_ATTR_VALID			BIT(0)
 #define ADMA_DESC_ATTR_END			BIT(1)
 #define ADMA_DESC_ATTR_INT			BIT(2)
 #define ADMA_DESC_ATTR_ACT1			BIT(4)
 #define ADMA_DESC_ATTR_ACT2			BIT(5)
-#define ADMA_DESC_TRANSFER_DATA		ADMA_DESC_ATTR_ACT2
+#define ADMA_DESC_TRANSFER_DATA			ADMA_DESC_ATTR_ACT2
+
+#define HRS_09_EXTENDED_RD_MODE			(1 << 2)
+#define HRS_09_EXTENDED_WR_MODE			(1 << 3)
+#define HRS_09_RDCMD_EN				(1 << 15)
+#define HRS_09_RDDATA_EN			(1 << 16)
+#define HRS_10_HCSDCLKADJ_VAL			(3)
+
+#define SRS11_SRFA				(1 << 24)
+#define SRS11_SRFA_CHK(x)			(x >> 24)
+#define CDNS_TIMEOUT				(5000)
+
+#define SDHCI_MAKE_CMD(c, f) (((c & 0xff) << 8) | (f & 0xff))
+
+/* Card busy and present */
+#define CARD_BUSY				1
+#define CARD_NOT_BUSY				0
+
+/* 500 ms delay to read the RINST register */
+#define DELAY_MS_SRS_READ			500
+#define DELAY_RES				10
+
+/* Check DV dfi_init val=0 */
+#define IO_MASK_END_DATA			0x0
+
+/* Check DV dfi_init val=2; DDR Mode */
+#define IO_MASK_END_DATA_DDR			0x2
+#define IO_MASK_START_DATA			0x0
+#define DATA_SELECT_OE_END_DATA			0x1
+
+#define TIMEOUT					100000
+
+/* General define */
+#define SDHC_REG_MASK				UINT_MAX
+#define SD_HOST_BLOCK_SIZE			0x200
+#define DTCVVAL_DEFAULT_VAL			0xE
+#define CDMMC_DMA_MAX_BUFFER_SIZE		64*1024
+#define CDNSMMC_ADDRESS_MASK			U(0x0f)
+#define CONFIG_CDNS_DESC_COUNT			8
+#define SD_HOST_CLK				200000000
 
 enum sd_opcode {
 	SD_GO_IDLE_STATE = 0,
@@ -388,6 +486,16 @@
 	SD_APP_SEND_SCR = 51,
 };
 
+enum sd_opr_modes {
+	SD_HOST_OPR_MODE_HV4E_0_SDMA_32 = 0,
+	SD_HOST_OPR_MODE_HV4E_1_SDMA_32,
+	SD_HOST_OPR_MODE_HV4E_1_SDMA_64,
+	SD_HOST_OPR_MODE_HV4E_0_ADMA_32,
+	SD_HOST_OPR_MODE_HV4E_0_ADMA_64,
+	SD_HOST_OPR_MODE_HV4E_1_ADMA_32,
+	SD_HOST_OPR_MODE_HV4E_1_ADMA_64,
+};
+
 struct cdns_sdmmc_sdhc {
 	uint32_t	sdhc_extended_rd_mode;
 	uint32_t	sdhc_extended_wr_mode;
@@ -439,9 +547,6 @@
 	uint32_t	combophy;
 };
 
-/* read and write API */
-size_t sdmmc_read_blocks(int lba, uintptr_t buf, size_t size);
-size_t sdmmc_write_blocks(int lba, const uintptr_t buf, size_t size);
 
 struct cdns_idmac_desc {
 	/*8 bit attribute*/
@@ -467,4 +572,8 @@
 struct cdns_sdmmc_sdhc *mmc_sdhc_reg);
 void cdns_set_sdmmc_var(struct cdns_sdmmc_combo_phy *combo_phy_reg,
 struct cdns_sdmmc_sdhc *sdhc_reg);
+int cdns_mmc_init(struct cdns_sdmmc_params *params, struct mmc_device_info *info);
+int cdns_program_phy_reg(struct cdns_sdmmc_combo_phy *combo_phy_reg,
+				struct cdns_sdmmc_sdhc *sdhc_reg);
+void cdns_host_set_clk(uint32_t clk);
 #endif
diff --git a/include/lib/el3_runtime/context_el1.h b/include/lib/el3_runtime/context_el1.h
index 94af210..4379bcf 100644
--- a/include/lib/el3_runtime/context_el1.h
+++ b/include/lib/el3_runtime/context_el1.h
@@ -7,6 +7,8 @@
 #ifndef CONTEXT_EL1_H
 #define CONTEXT_EL1_H
 
+#include <lib/extensions/sysreg128.h>
+
 #ifndef __ASSEMBLER__
 
 /*******************************************************************************
@@ -28,15 +30,12 @@
 	uint64_t csselr_el1;
 	uint64_t sp_el1;
 	uint64_t esr_el1;
-	uint64_t ttbr0_el1;
-	uint64_t ttbr1_el1;
 	uint64_t mair_el1;
 	uint64_t amair_el1;
 	uint64_t actlr_el1;
 	uint64_t tpidr_el1;
 	uint64_t tpidr_el0;
 	uint64_t tpidrro_el0;
-	uint64_t par_el1;
 	uint64_t far_el1;
 	uint64_t afsr0_el1;
 	uint64_t afsr1_el1;
@@ -44,6 +43,9 @@
 	uint64_t vbar_el1;
 	uint64_t mdccint_el1;
 	uint64_t mdscr_el1;
+	sysreg_t par_el1;
+	sysreg_t ttbr0_el1;
+	sysreg_t ttbr1_el1;
 } el1_common_regs_t;
 
 typedef struct el1_aarch32_regs {
@@ -108,8 +110,8 @@
 } el1_gcs_regs_t;
 
 typedef struct el1_the_regs {
-	uint64_t rcwmask_el1;
-	uint64_t rcwsmask_el1;
+	sysreg_t rcwmask_el1;
+	sysreg_t rcwsmask_el1;
 } el1_the_regs_t;
 
 typedef struct el1_sctlr2_regs {
diff --git a/include/lib/el3_runtime/context_el2.h b/include/lib/el3_runtime/context_el2.h
index ad0b68f..7374e39 100644
--- a/include/lib/el3_runtime/context_el2.h
+++ b/include/lib/el3_runtime/context_el2.h
@@ -7,7 +7,10 @@
 #ifndef CONTEXT_EL2_H
 #define CONTEXT_EL2_H
 
+#include <lib/extensions/sysreg128.h>
+
 #ifndef __ASSEMBLER__
+
 /*******************************************************************************
  * EL2 Registers:
  * AArch64 EL2 system register context structure for preserving the
@@ -40,12 +43,12 @@
 	uint64_t sp_el2;
 	uint64_t tcr_el2;
 	uint64_t tpidr_el2;
-	uint64_t ttbr0_el2;
 	uint64_t vbar_el2;
 	uint64_t vmpidr_el2;
 	uint64_t vpidr_el2;
 	uint64_t vtcr_el2;
-	uint64_t vttbr_el2;
+	sysreg_t vttbr_el2;
+	sysreg_t ttbr0_el2;
 } el2_common_regs_t;
 
 typedef struct el2_mte2_regs {
@@ -75,7 +78,7 @@
 
 typedef struct el2_vhe_regs {
 	uint64_t contextidr_el2;
-	uint64_t ttbr1_el2;
+	sysreg_t ttbr1_el2;
 } el2_vhe_regs_t;
 
 typedef struct el2_ras_regs {
@@ -222,6 +225,9 @@
 #define write_el2_ctx_common(ctx, reg, val)	((((ctx)->common).reg)	\
 							= (uint64_t) (val))
 
+#define write_el2_ctx_sysreg128(ctx, reg, val)	((((ctx)->common).reg)	\
+							= (sysreg_t) (val))
+
 #if ENABLE_FEAT_MTE2
 #define read_el2_ctx_mte2(ctx, reg)		(((ctx)->mte2).reg)
 #define write_el2_ctx_mte2(ctx, reg, val)	((((ctx)->mte2).reg)	\
@@ -262,6 +268,9 @@
 #define read_el2_ctx_vhe(ctx, reg)		(((ctx)->vhe).reg)
 #define write_el2_ctx_vhe(ctx, reg, val)	((((ctx)->vhe).reg)	\
 							= (uint64_t) (val))
+
+#define write_el2_ctx_vhe_sysreg128(ctx, reg, val)	((((ctx)->vhe).reg)	\
+								= (sysreg_t) (val))
 #else
 #define read_el2_ctx_vhe(ctx, reg)		ULL(0)
 #define write_el2_ctx_vhe(ctx, reg, val)
diff --git a/include/lib/extensions/sysreg128.h b/include/lib/extensions/sysreg128.h
new file mode 100644
index 0000000..8854856
--- /dev/null
+++ b/include/lib/extensions/sysreg128.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SYSREG128_H
+#define SYSREG128_H
+
+#ifndef __ASSEMBLER__
+
+#if ENABLE_FEAT_D128
+#include <stdint.h>
+
+typedef uint128_t sysreg_t;
+
+#define PAR_EL1_D128	(((sysreg_t)(1ULL)) << (64))
+
+#define _DECLARE_SYSREG128_READ_FUNC(_name)	\
+uint128_t read_ ## _name(void);
+
+#define _DECLARE_SYSREG128_WRITE_FUNC(_name)	\
+void write_ ## _name(uint128_t v);
+
+#define DECLARE_SYSREG128_RW_FUNCS(_name)	\
+	_DECLARE_SYSREG128_READ_FUNC(_name)	\
+	_DECLARE_SYSREG128_WRITE_FUNC(_name)
+#else
+
+typedef uint64_t sysreg_t;
+
+#endif /* ENABLE_FEAT_D128 */
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* SYSREG128_H */
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index fa0660c..8aa2ccc 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -33,6 +33,7 @@
 #include <lib/extensions/sme.h>
 #include <lib/extensions/spe.h>
 #include <lib/extensions/sve.h>
+#include <lib/extensions/sysreg128.h>
 #include <lib/extensions/sys_reg_trace.h>
 #include <lib/extensions/tcr2.h>
 #include <lib/extensions/trbe.h>
@@ -282,6 +283,14 @@
 		scr_el3 |= SCR_SCTLR2En_BIT;
 	}
 
+	if (is_feat_d128_supported()) {
+		/* Set the D128En bit in SCR_EL3 to enable access to 128-bit
+		 * versions of TTBR0_EL1, TTBR1_EL1, RCWMASK_EL1, RCWSMASK_EL1,
+		 * PAR_EL1 and TTBR1_EL2, TTBR0_EL2 and VTTBR_EL2 registers.
+		 */
+		scr_el3 |= SCR_D128En_BIT;
+	}
+
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 
 	/* Initialize EL2 context registers */
@@ -1329,12 +1338,13 @@
 	write_el2_ctx_common(ctx, sp_el2, read_sp_el2());
 	write_el2_ctx_common(ctx, tcr_el2, read_tcr_el2());
 	write_el2_ctx_common(ctx, tpidr_el2, read_tpidr_el2());
-	write_el2_ctx_common(ctx, ttbr0_el2, read_ttbr0_el2());
 	write_el2_ctx_common(ctx, vbar_el2, read_vbar_el2());
 	write_el2_ctx_common(ctx, vmpidr_el2, read_vmpidr_el2());
 	write_el2_ctx_common(ctx, vpidr_el2, read_vpidr_el2());
 	write_el2_ctx_common(ctx, vtcr_el2, read_vtcr_el2());
-	write_el2_ctx_common(ctx, vttbr_el2, read_vttbr_el2());
+
+	write_el2_ctx_sysreg128(ctx, ttbr0_el2, read_ttbr0_el2());
+	write_el2_ctx_sysreg128(ctx, vttbr_el2, read_vttbr_el2());
 }
 
 static void el2_sysregs_context_restore_common(el2_sysregs_t *ctx)
@@ -1410,7 +1420,7 @@
 	if (is_feat_vhe_supported()) {
 		write_el2_ctx_vhe(el2_sysregs_ctx, contextidr_el2,
 					read_contextidr_el2());
-		write_el2_ctx_vhe(el2_sysregs_ctx, ttbr1_el2, read_ttbr1_el2());
+		write_el2_ctx_vhe_sysreg128(el2_sysregs_ctx, ttbr1_el2, read_ttbr1_el2());
 	}
 
 	if (is_feat_ras_supported()) {
diff --git a/lib/extensions/sysreg128/sysreg128.S b/lib/extensions/sysreg128/sysreg128.S
new file mode 100644
index 0000000..08cff2f
--- /dev/null
+++ b/lib/extensions/sysreg128/sysreg128.S
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <lib/extensions/sysreg128.h>
+
+        .global read_par_el1
+        .global write_par_el1
+        .global read_ttbr0_el1
+        .global write_ttbr0_el1
+        .global read_ttbr1_el1
+        .global write_ttbr1_el1
+        .global read_ttbr0_el2
+        .global write_ttbr0_el2
+        .global read_ttbr1_el2
+        .global write_ttbr1_el2
+        .global read_vttbr_el2
+        .global write_vttbr_el2
+        .global read_rcwmask_el1
+        .global write_rcwmask_el1
+        .global read_rcwsmask_el1
+        .global write_rcwsmask_el1
+
+/*
+ * _mrrs - Move System register to two adjacent general-purpose
+ * registers.
+ * Instruction: MRRS <Xt>, <Xt+1>, (<systemreg>|S<op0>_<op1>_<Cn>_<Cm>_<op2>)
+ *
+ * Arguments/Opcode bit field:
+ * regins: System register opcode.
+ *
+ * Clobbers: x0,x1,x2
+ */
+.macro _mrrs regins:req
+#if ENABLE_FEAT_D128 == 2
+        mrs     x0, ID_AA64MMFR3_EL1
+        tst     x0, #(ID_AA64MMFR3_EL1_D128_MASK << ID_AA64MMFR3_EL1_D128_SHIFT)
+        bne     1f
+        /* If FEAT_D128 is not implemented then use mrs */
+        .inst   0xD5300000 | (\regins)
+        ret
+#endif
+1:
+        .inst   0xD5700000 | (\regins)
+        ret
+.endm
+
+/*
+ * _msrr - Move two adjacent general-purpose registers to System register.
+ * Instruction: MSRR (<systemreg>|S<op0>_<op1>_<Cn>_<Cm>_<op2>), <Xt>, <Xt+1>
+ *
+ * Arguments/Opcode bit field:
+ * regins: System register opcode.
+ *
+ * Clobbers: x0,x1,x2
+ */
+.macro _msrr regins:req
+        /* If FEAT_D128 is not implemented use msr, dont tamper
+         * x0, x1 as they maybe used for mrrs */
+#if ENABLE_FEAT_D128 == 2
+        mrs     x2, ID_AA64MMFR3_EL1
+        tst     x2, #(ID_AA64MMFR3_EL1_D128_MASK << ID_AA64MMFR3_EL1_D128_SHIFT)
+        bne     1f
+        /* If FEAT_D128 is not implemented then use msr */
+        .inst   0xD5100000 | (\regins)
+        ret
+#endif
+1:
+        .inst   0xD5500000 | (\regins)
+        ret
+.endm
+
+func read_par_el1
+        _mrrs   0x87400 /* S3_0_C7_C4_0 */
+endfunc read_par_el1
+
+func write_par_el1
+        _msrr   0x87400
+endfunc write_par_el1
+
+func read_ttbr0_el1
+        _mrrs   0x82000 /* S3_0_C2_C0_0 */
+endfunc read_ttbr0_el1
+
+func write_ttbr0_el1
+        _msrr 0x82000
+endfunc write_ttbr0_el1
+
+func read_ttbr1_el1
+        _mrrs 0x82020 /* S3_0_C2_C0_1 */
+endfunc read_ttbr1_el1
+
+func write_ttbr1_el1
+        _msrr 0x82020
+endfunc write_ttbr1_el1
+
+func read_ttbr0_el2
+        _mrrs 0xC2000 /* S3_4_C2_C0_0 */
+endfunc read_ttbr0_el2
+
+func write_ttbr0_el2
+        _msrr 0xC2000
+endfunc write_ttbr0_el2
+
+func read_ttbr1_el2
+        _mrrs 0xC2020 /* S3_4_C2_C0_1 */
+endfunc read_ttbr1_el2
+
+func write_ttbr1_el2
+        _msrr 0xC2020
+endfunc write_ttbr1_el2
+
+func read_vttbr_el2
+        _mrrs 0xC2100 /* S3_4_C2_C1_0 */
+endfunc read_vttbr_el2
+
+func write_vttbr_el2
+        _msrr 0xC2100
+endfunc write_vttbr_el2
+
+func read_rcwmask_el1
+        _mrrs 0x8D0C0 /* S3_0_C13_C0_6 */
+endfunc read_rcwmask_el1
+
+func write_rcwmask_el1
+        _msrr 0x8D0C0
+endfunc write_rcwmask_el1
+
+func read_rcwsmask_el1
+        _mrrs 0x8D060 /* S3_0_C13_C0_3 */
+endfunc read_rcwsmask_el1
+
+func write_rcwsmask_el1
+        _msrr 0x8D060
+endfunc write_rcwsmask_el1
diff --git a/lib/romlib/Makefile b/lib/romlib/Makefile
index 3d2b850..4cac75b 100644
--- a/lib/romlib/Makefile
+++ b/lib/romlib/Makefile
@@ -73,7 +73,7 @@
 	$(s)echo "  PRE     $@"
 	$(q)$(ROMLIB_GEN) pre --output $@ --deps $(BUILD_DIR)/jmptbl.d $<
 
-$(WRAPPER_SOURCES) &: $(BUILD_DIR)/jmptbl.i | $$(@D)/
+$(WRAPPER_SOURCES) $&: $(BUILD_DIR)/jmptbl.i | $$(@D)/
 	$(s)echo "  WRP     $<"
 	$(q)$(ROMLIB_GEN) genwrappers --bti=$(ENABLE_BTI) -b $(WRAPPER_DIR) $<
 
diff --git a/lib/xlat_tables/aarch64/xlat_tables.c b/lib/xlat_tables/aarch64/xlat_tables.c
index f4195f4..f207266 100644
--- a/lib/xlat_tables/aarch64/xlat_tables.c
+++ b/lib/xlat_tables/aarch64/xlat_tables.c
@@ -66,7 +66,7 @@
  */
 static const unsigned int pa_range_bits_arr[] = {
 	PARANGE_0000, PARANGE_0001, PARANGE_0010, PARANGE_0011, PARANGE_0100,
-	PARANGE_0101, PARANGE_0110
+	PARANGE_0101, PARANGE_0110, PARANGE_0111
 };
 
 static unsigned long long get_max_supported_pa(void)
diff --git a/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c b/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c
index 18e001b..7321fd7 100644
--- a/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c
+++ b/lib/xlat_tables_v2/aarch64/xlat_tables_arch.c
@@ -109,7 +109,7 @@
  */
 static const unsigned int pa_range_bits_arr[] = {
 	PARANGE_0000, PARANGE_0001, PARANGE_0010, PARANGE_0011, PARANGE_0100,
-	PARANGE_0101, PARANGE_0110
+	PARANGE_0101, PARANGE_0110, PARANGE_0111
 };
 
 unsigned long long xlat_arch_get_max_supported_pa(void)
diff --git a/make_helpers/arch_features.mk b/make_helpers/arch_features.mk
index 6c893f4..39f6223 100644
--- a/make_helpers/arch_features.mk
+++ b/make_helpers/arch_features.mk
@@ -406,6 +406,12 @@
 ENABLE_BRBE_FOR_NS			?=	0
 
 #----
+# 9.3
+#----
+# Flag to enable access to Arm v9.3 FEAT_D128 extension
+ENABLE_FEAT_D128			?=	0
+
+#----
 #9.4
 #----
 
diff --git a/make_helpers/utilities.mk b/make_helpers/utilities.mk
index efa0ab9..fcccd24 100644
--- a/make_helpers/utilities.mk
+++ b/make_helpers/utilities.mk
@@ -22,6 +22,13 @@
 escape-shell = '$(subst ','\'',$(1))'
 
 #
+# The grouped-target symbol. Grouped targets are not supported on versions of
+# GNU Make <= 4.2, which was most recently packaged with Ubuntu 20.04.
+#
+
+& := $(if $(filter grouped-target,$(.FEATURES)),&)
+
+#
 # Upper-case a string value.
 #
 # Parameters:
diff --git a/plat/amd/versal2/bl31_setup.c b/plat/amd/versal2/bl31_setup.c
index 64c356a..47d4c2c 100644
--- a/plat/amd/versal2/bl31_setup.c
+++ b/plat/amd/versal2/bl31_setup.c
@@ -172,7 +172,7 @@
 	uint32_t i;
 
 	/* Validate 'handler' and 'id' parameters */
-	if (handler == NULL || index >= MAX_INTR_EL3) {
+	if ((handler == NULL) || (index >= MAX_INTR_EL3)) {
 		return -EINVAL;
 	}
 
diff --git a/plat/amd/versal2/plat_psci.c b/plat/amd/versal2/plat_psci.c
index a55042d..688b177 100644
--- a/plat/amd/versal2/plat_psci.c
+++ b/plat/amd/versal2/plat_psci.c
@@ -21,6 +21,7 @@
 
 #define PM_RET_ERROR_NOFEATURE U(19)
 #define ALWAYSTRUE true
+#define LINEAR_MODE BIT(1)
 
 static uintptr_t _sec_entry;
 
@@ -166,7 +167,12 @@
 
 	switch (ioctl_id) {
 	case IOCTL_OSPI_MUX_SELECT:
-		mmio_write_32(SLCR_OSPI_QSPI_IOU_AXI_MUX_SEL, arg1);
+		if ((arg1 == 0) || (arg1 == 1)) {
+			mmio_clrsetbits_32(SLCR_OSPI_QSPI_IOU_AXI_MUX_SEL, LINEAR_MODE,
+					(arg1 ? LINEAR_MODE : 0));
+		} else {
+			ret = PM_RET_ERROR_ARGS;
+		}
 		break;
 	case IOCTL_UFS_TXRX_CFGRDY_GET:
 		ret = (int32_t) mmio_read_32(PMXC_IOU_SLCR_TX_RX_CONFIG_RDY);
diff --git a/plat/amd/versal2/scmi.c b/plat/amd/versal2/scmi.c
index 59aff08..6375df3 100644
--- a/plat/amd/versal2/scmi.c
+++ b/plat/amd/versal2/scmi.c
@@ -269,7 +269,7 @@
 
 const char *plat_scmi_clock_get_name(unsigned int agent_id, unsigned int scmi_id)
 {
-	struct scmi_clk *clock = clk_find(agent_id, scmi_id);
+	const struct scmi_clk *clock = clk_find(agent_id, scmi_id);
 	const char *ret;
 
 	if (clock == NULL) {
@@ -287,7 +287,7 @@
 				    unsigned long *array, size_t *nb_elts,
 				    uint32_t start_idx)
 {
-	struct scmi_clk *clock = clk_find(agent_id, scmi_id);
+	const struct scmi_clk *clock = clk_find(agent_id, scmi_id);
 
 	if (clock == NULL) {
 		return SCMI_NOT_FOUND;
@@ -312,7 +312,7 @@
 
 unsigned long plat_scmi_clock_get_rate(unsigned int agent_id, unsigned int scmi_id)
 {
-	struct scmi_clk *clock = clk_find(agent_id, scmi_id);
+	const struct scmi_clk *clock = clk_find(agent_id, scmi_id);
 	unsigned long ret;
 
 	if ((clock == NULL)) {
@@ -341,7 +341,7 @@
 
 int32_t plat_scmi_clock_get_state(unsigned int agent_id, unsigned int scmi_id)
 {
-	struct scmi_clk *clock = clk_find(agent_id, scmi_id);
+	const struct scmi_clk *clock = clk_find(agent_id, scmi_id);
 	int32_t ret;
 
 	if ((clock == NULL)) {
@@ -647,7 +647,7 @@
 		for (i = 0U; i < ARRAY_SIZE(scmi0_clock); i++) {
 
 			/* Keep i2c on 100MHz to calculate rates properly */
-			if (i >= CLK_I2C0_0 && i <= CLK_I2C7_0)
+			if ((i >= CLK_I2C0_0) && (i <= CLK_I2C7_0))
 				continue;
 
 			/* Keep UFS clocks to default values to get the expected rates */
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index b3d503e..80dfd2a 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -323,13 +323,13 @@
 			     unsigned int power_level)
 {
 	unsigned int psysr;
-	int ret;
+	int ret = 0;
 
 	/*
 	 * The format of 'power_level' is implementation-defined, but 0 must
 	 * mean a CPU. We also allow 1 to denote the cluster
 	 */
-	if ((power_level != ARM_PWR_LVL0) && (power_level != ARM_PWR_LVL1))
+	if ((power_level < ARM_PWR_LVL0) || (power_level > ARM_PWR_LVL1))
 		return PSCI_E_INVALID_PARAMS;
 
 	/*
@@ -343,9 +343,14 @@
 
 	if (power_level == ARM_PWR_LVL0) {
 		ret = ((psysr & PSYSR_AFF_L0) != 0U) ? HW_ON : HW_OFF;
-	} else {
-		/* power_level == ARM_PWR_LVL1 */
-		ret = ((psysr & PSYSR_AFF_L1) != 0U) ? HW_ON : HW_OFF;
+	} else if (power_level == ARM_PWR_LVL1) {
+	/*
+	 * Use L1 affinity if MPIDR_EL1.MT bit is not set else use L2 affinity.
+	 */
+		if ((read_mpidr_el1() & MPIDR_MT_MASK) == 0U)
+			ret = ((psysr & PSYSR_AFF_L1) != 0U) ? HW_ON : HW_OFF;
+		else
+			ret = ((psysr & PSYSR_AFF_L2) != 0U) ? HW_ON : HW_OFF;
 	}
 
 	return ret;
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 7cb3982..6f53a81 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -56,6 +56,7 @@
 
       ENABLE_BRBE_FOR_NS	:= 2
       ENABLE_TRBE_FOR_NS	:= 2
+      ENABLE_FEAT_D128		:= 2
 endif
 
 ENABLE_SYS_REG_TRACE_FOR_NS	:= 2
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
index 3ef25de..9cd3011 100644
--- a/plat/arm/board/tc/platform.mk
+++ b/plat/arm/board/tc/platform.mk
@@ -61,6 +61,10 @@
 endif
 
 ifneq ($(shell expr $(TARGET_PLATFORM) \<= 1), 0)
+        $(error Platform ${PLAT}$(TARGET_PLATFORM) is no longer available.)
+endif
+
+ifneq ($(shell expr $(TARGET_PLATFORM) = 2), 0)
         $(warning Platform ${PLAT}$(TARGET_PLATFORM) is deprecated. \
           Some of the features might not work as expected)
 endif
diff --git a/plat/arm/common/arm_common.c b/plat/arm/common/arm_common.c
index 21cc39c..2d4165c 100644
--- a/plat/arm/common/arm_common.c
+++ b/plat/arm/common/arm_common.c
@@ -12,6 +12,8 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/romlib.h>
+#include <common/par.h>
+#include <lib/extensions/sysreg128.h>
 #include <lib/mmio.h>
 #include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables_compat.h>
@@ -196,7 +198,8 @@
  */
 int plat_sdei_validate_entry_point(uintptr_t ep, unsigned int client_mode)
 {
-	uint64_t par, pa;
+	uint64_t pa;
+	sysreg_t par;
 	u_register_t scr_el3;
 
 	/* Doing Non-secure address translation requires SCR_EL3.NS set */
@@ -230,7 +233,7 @@
 		return -1;
 
 	/* Extract Physical Address from PAR */
-	pa = (par & (PAR_ADDR_MASK << PAR_ADDR_SHIFT));
+	pa = get_par_el1_pa(par);
 
 	/* Perform NS entry point validation on the physical address */
 	return arm_validate_ns_entrypoint(pa);
diff --git a/plat/imx/imx8m/imx8m_ccm.c b/plat/imx/imx8m/imx8m_ccm.c
index 10a00c9..6b14446 100644
--- a/plat/imx/imx8m/imx8m_ccm.c
+++ b/plat/imx/imx8m/imx8m_ccm.c
@@ -17,16 +17,16 @@
 } imx8m_uart_info[] = {
 	{	/* UART 1 */
 		.ccm_reg = 0x4490,
-		.uart_base = 0x30860000,
+		.uart_base = IMX_UART1_BASE,
 	}, {	/* UART 2 */
 		.ccm_reg = 0x44a0,
-		.uart_base = 0x30890000,
+		.uart_base = IMX_UART2_BASE,
 	}, {	/* UART 3 */
 		.ccm_reg = 0x44b0,
-		.uart_base = 0x30880000,
+		.uart_base = IMX_UART3_BASE,
 	}, {	/* UART 4 */
 		.ccm_reg = 0x44c0,
-		.uart_base = 0x30a60000,
+		.uart_base = IMX_UART4_BASE,
 	}
 };
 
diff --git a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
index f6e46eb..03edc6e 100644
--- a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
@@ -62,7 +62,7 @@
 	{0},
 };
 
-static const struct imx_rdc_cfg rdc[] = {
+static struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
 	RDC_MDAn(RDC_MDA_M4, DID1),
 
@@ -164,14 +164,14 @@
 
 	imx_aipstz_init(aipstz);
 
-	imx_rdc_init(rdc);
-
-	imx_csu_init(csu_cfg);
-
 	if (console_base == 0U) {
 		console_base = imx8m_uart_get_base();
 	}
 
+	imx_rdc_init(rdc, console_base);
+
+	imx_csu_init(csu_cfg);
+
 	console_imx_uart_register(console_base, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index 2fa6199..e6ad8fe 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/include/platform_def.h
@@ -85,6 +85,11 @@
 #define PLAT_CRASH_UART_CLK_IN_HZ	24000000
 #define IMX_CONSOLE_BAUDRATE		115200
 
+#define IMX_UART1_BASE			U(0x30860000)
+#define IMX_UART2_BASE			U(0x30890000)
+#define IMX_UART3_BASE			U(0x30880000)
+#define IMX_UART4_BASE			U(0x30a60000)
+
 #define IMX_AIPSTZ1			U(0x301f0000)
 #define IMX_AIPSTZ2			U(0x305f0000)
 #define IMX_AIPSTZ3			U(0x309f0000)
diff --git a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
index befa769..42d173e 100644
--- a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
@@ -48,7 +48,7 @@
 	{0},
 };
 
-static const struct imx_rdc_cfg rdc[] = {
+static struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
 	RDC_MDAn(RDC_MDA_M7, DID1),
 
@@ -136,7 +136,11 @@
 
 	imx_aipstz_init(aipstz);
 
-	imx_rdc_init(rdc);
+	if (console_base == 0U) {
+		console_base = imx8m_uart_get_base();
+	}
+
+	imx_rdc_init(rdc, console_base);
 
 	imx_csu_init(csu_cfg);
 
@@ -152,10 +156,6 @@
 	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
 	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
 
-	if (console_base == 0U) {
-		console_base = imx8m_uart_get_base();
-	}
-
 	console_imx_uart_register(console_base, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
diff --git a/plat/imx/imx8m/imx8mn/include/platform_def.h b/plat/imx/imx8m/imx8mn/include/platform_def.h
index 569432d..b76bdbf 100644
--- a/plat/imx/imx8m/imx8mn/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mn/include/platform_def.h
@@ -68,6 +68,11 @@
 #define PLAT_CRASH_UART_CLK_IN_HZ	24000000
 #define IMX_CONSOLE_BAUDRATE		115200
 
+#define IMX_UART1_BASE			U(0x30860000)
+#define IMX_UART2_BASE			U(0x30890000)
+#define IMX_UART3_BASE			U(0x30880000)
+#define IMX_UART4_BASE			U(0x30a60000)
+
 #define IMX_AIPSTZ1			U(0x301f0000)
 #define IMX_AIPSTZ2			U(0x305f0000)
 #define IMX_AIPSTZ3			U(0x309f0000)
diff --git a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
index ffad3d1..141c94b 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
@@ -49,7 +49,7 @@
 	{0},
 };
 
-static const struct imx_rdc_cfg rdc[] = {
+static struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
 	RDC_MDAn(RDC_MDA_M7, DID1),
 
@@ -166,7 +166,11 @@
 
 	imx_aipstz_init(aipstz);
 
-	imx_rdc_init(rdc);
+	if (console_base == 0U) {
+		console_base = imx8m_uart_get_base();
+	}
+
+	imx_rdc_init(rdc, console_base);
 
 	imx_csu_init(csu_cfg);
 
@@ -175,10 +179,6 @@
 	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
 	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
 
-	if (console_base == 0U) {
-		console_base = imx8m_uart_get_base();
-	}
-
 	console_imx_uart_register(console_base, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
diff --git a/plat/imx/imx8m/imx8mp/include/platform_def.h b/plat/imx/imx8m/imx8mp/include/platform_def.h
index 84a7e00..78f3d5b 100644
--- a/plat/imx/imx8m/imx8mp/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mp/include/platform_def.h
@@ -86,6 +86,11 @@
 #define PLAT_CRASH_UART_CLK_IN_HZ	24000000
 #define IMX_CONSOLE_BAUDRATE		115200
 
+#define IMX_UART1_BASE			U(0x30860000)
+#define IMX_UART2_BASE			U(0x30890000)
+#define IMX_UART3_BASE			U(0x30880000)
+#define IMX_UART4_BASE			U(0x30a60000)
+
 #define IMX_AIPSTZ1			U(0x301f0000)
 #define IMX_AIPSTZ2			U(0x305f0000)
 #define IMX_AIPSTZ3			U(0x309f0000)
diff --git a/plat/imx/imx8m/imx8mq/include/platform_def.h b/plat/imx/imx8m/imx8mq/include/platform_def.h
index b04f391..61c0e8e 100644
--- a/plat/imx/imx8m/imx8mq/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mq/include/platform_def.h
@@ -65,6 +65,11 @@
 #define PLAT_CRASH_UART_CLK_IN_HZ	25000000
 #define IMX_CONSOLE_BAUDRATE		115200
 
+#define IMX_UART1_BASE			U(0x30860000)
+#define IMX_UART2_BASE			U(0x30890000)
+#define IMX_UART3_BASE			U(0x30880000)
+#define IMX_UART4_BASE			U(0x30a60000)
+
 #define IMX_AIPS_BASE			U(0x30200000)
 #define IMX_AIPS_SIZE			U(0xC00000)
 #define IMX_AIPS1_BASE			U(0x30200000)
diff --git a/plat/imx/imx8m/imx_rdc.c b/plat/imx/imx8m/imx_rdc.c
index 85de191..de15956 100644
--- a/plat/imx/imx8m/imx_rdc.c
+++ b/plat/imx/imx8m/imx_rdc.c
@@ -4,13 +4,78 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <errno.h>
+
 #include <lib/mmio.h>
 
 #include <imx_rdc.h>
 
+struct imx_uart {
+	int index;
+	unsigned int uart_base;
+};
+
+static const struct imx_uart imx8m_uart_info[] = {
+	{	/* UART 1 */
+		.index = RDC_PDAP_UART1,
+		.uart_base = IMX_UART1_BASE,
+	}, {	/* UART 2 */
+		.index = RDC_PDAP_UART2,
+		.uart_base = IMX_UART2_BASE,
+	}, {	/* UART 3 */
+		.index = RDC_PDAP_UART3,
+		.uart_base = IMX_UART3_BASE,
+	}, {	/* UART 4 */
+		.index = RDC_PDAP_UART4,
+		.uart_base = IMX_UART4_BASE,
+	}
+};
+
+static int imx_rdc_uart_get_pdap_index(unsigned int uart_base)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(imx8m_uart_info); i++) {
+		if (imx8m_uart_info[i].uart_base == uart_base) {
+			return imx8m_uart_info[i].index;
+		}
+	}
+
+	return -ENODEV;
+}
+
-void imx_rdc_init(const struct imx_rdc_cfg *rdc_cfg)
+static void imx_rdc_console_access_enable(struct imx_rdc_cfg *rdc_cfg,
+				   unsigned int console_base)
 {
-	const struct imx_rdc_cfg *rdc = rdc_cfg;
+	struct imx_rdc_cfg *rdc;
+	int console_pdap_index;
+
+	console_pdap_index = imx_rdc_uart_get_pdap_index(console_base);
+	if (console_pdap_index < 0) {
+		return;
+	}
+
+	for (rdc = rdc_cfg; rdc->type != RDC_INVALID; rdc++) {
+		if (rdc->type != RDC_PDAP || rdc->index != console_pdap_index) {
+			continue;
+		}
+
+		if (rdc->index == console_pdap_index &&
+		    rdc->setting.rdc_pdap == (D0R | D0W)) {
+			return;
+		}
+
+		if (rdc->index == console_pdap_index) {
+			rdc->setting.rdc_pdap = D0R | D0W;
+		}
+	}
+}
+
+void imx_rdc_init(struct imx_rdc_cfg *rdc_cfg, unsigned int console_base)
+{
+	struct imx_rdc_cfg *rdc = rdc_cfg;
+
+	imx_rdc_console_access_enable(rdc, console_base);
 
 	while (rdc->type != RDC_INVALID) {
 		switch (rdc->type) {
diff --git a/plat/imx/imx8m/include/imx_rdc.h b/plat/imx/imx8m/include/imx_rdc.h
index a6e10a7..fbdcbf2 100644
--- a/plat/imx/imx8m/include/imx_rdc.h
+++ b/plat/imx/imx8m/include/imx_rdc.h
@@ -67,7 +67,7 @@
 	  .setting.rdc_mem_region[2] = (mrc),	\
 	}
 
-void imx_rdc_init(const struct imx_rdc_cfg *cfg);
+void imx_rdc_init(struct imx_rdc_cfg *cfg, unsigned int console_base);
 
 #endif /* IMX_RDC_H */
 
diff --git a/plat/intel/soc/agilex/include/socfpga_plat_def.h b/plat/intel/soc/agilex/include/socfpga_plat_def.h
index 94fed3a..840ffdd 100644
--- a/plat/intel/soc/agilex/include/socfpga_plat_def.h
+++ b/plat/intel/soc/agilex/include/socfpga_plat_def.h
@@ -15,7 +15,6 @@
 
 /* Platform Setting */
 #define PLATFORM_MODEL				PLAT_SOCFPGA_AGILEX
-#define BOOT_SOURCE				BOOT_SOURCE_SDMMC
 /* 1 = Flush cache, 0 = No cache flush.
  * Default for Agilex is No cache flush.
  * For Agilex FP8, set to Flush cache.
@@ -35,12 +34,24 @@
 #define CAD_QSPIDATA_OFST			0xff900000
 #define CAD_QSPI_OFFSET				0xff8d2000
 
+/* FIP Setting */
+#define PLAT_FIP_BASE				(0)
+#if ARM_LINUX_KERNEL_AS_BL33
+#define PLAT_FIP_MAX_SIZE			(0x8000000)
+#else
+#define PLAT_FIP_MAX_SIZE			(0x1000000)
+#endif
+
 /* SDMMC Setting */
-# if ARM_LINUX_KERNEL_AS_BL33
+#if ARM_LINUX_KERNEL_AS_BL33
+#define PLAT_MMC_DATA_BASE			(0x10000000)
+#define PLAT_MMC_DATA_SIZE			(0x100000)
 #define SOCFPGA_MMC_BLOCK_SIZE			U(32768)
-# else
+#else
+#define PLAT_MMC_DATA_BASE			(0xffe3c000)
+#define PLAT_MMC_DATA_SIZE			(0x2000
 #define SOCFPGA_MMC_BLOCK_SIZE			U(8192)
-# endif
+#endif
 
 /* Register Mapping */
 #define SOCFPGA_CCU_NOC_REG_BASE		0xf7000000
@@ -112,7 +123,7 @@
 /*******************************************************************************
  * SDMMC related pointer function
  ******************************************************************************/
-#define SDMMC_READ_BLOCKS			mmc_read_blocks
+#define SDMMC_READ_BLOCKS			sdmmc_read_blocks
 #define SDMMC_WRITE_BLOCKS			mmc_write_blocks
 
 /*******************************************************************************
diff --git a/plat/intel/soc/agilex/platform.mk b/plat/intel/soc/agilex/platform.mk
index 21cc6a3..d534b2e 100644
--- a/plat/intel/soc/agilex/platform.mk
+++ b/plat/intel/soc/agilex/platform.mk
@@ -28,6 +28,7 @@
 			plat/intel/soc/common/aarch64/platform_common.c \
 			plat/intel/soc/common/aarch64/plat_helpers.S	\
 			plat/intel/soc/common/drivers/ccu/ncore_ccu.c	\
+			plat/intel/soc/common/drivers/sdmmc/sdmmc.c			\
 			plat/intel/soc/common/lib/sha/sha.c				\
 			plat/intel/soc/common/socfpga_delay_timer.c
 
@@ -87,6 +88,24 @@
 $(eval $(call add_define,ARM_LINUX_KERNEL_AS_BL33))
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
+# Configs for Boot Source
+SOCFPGA_BOOT_SOURCE_SDMMC		?=	0
+SOCFPGA_BOOT_SOURCE_QSPI		?=	0
+SOCFPGA_BOOT_SOURCE_NAND		?=	0
+
+$(eval $(call assert_booleans,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+$(eval $(call add_defines,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+
 # Configs for VAB Authentication
 SOCFPGA_SECURE_VAB_AUTH  := 	0
 $(eval $(call assert_boolean,SOCFPGA_SECURE_VAB_AUTH))
diff --git a/plat/intel/soc/agilex5/bl2_plat_setup.c b/plat/intel/soc/agilex5/bl2_plat_setup.c
index 78a4889..fe5dc6e 100644
--- a/plat/intel/soc/agilex5/bl2_plat_setup.c
+++ b/plat/intel/soc/agilex5/bl2_plat_setup.c
@@ -103,6 +103,15 @@
 	/* Configure the pinmux */
 	config_pinmux(&reverse_handoff_ptr);
 
+	/* Configure OCRAM to NON SECURE ACCESS */
+	mmio_write_32(OCRAM_REGION_0_REG_BASE, OCRAM_NON_SECURE_ENABLE);
+	mmio_write_32(SOCFPGA_L4_PER_SCR_REG_BASE + SOCFPGA_SDMMC_SECU_BIT,
+		SOCFPGA_SDMMC_SECU_BIT_ENABLE);
+	mmio_write_32(SOCFPGA_L4_SYS_SCR_REG_BASE + SOCFPGA_SDMMC_SECU_BIT,
+		SOCFPGA_SDMMC_SECU_BIT_ENABLE);
+	mmio_write_32(SOCFPGA_LWSOC2FPGA_SCR_REG_BASE,
+		SOCFPGA_LWSOC2FPGA_ENABLE);
+
 	/* Configure the clock manager */
 	if ((config_clkmgr_handoff(&reverse_handoff_ptr)) != 0) {
 		ERROR("SOCFPGA: Failed to initialize the clock manager\n");
@@ -157,7 +166,7 @@
 	switch (boot_source) {
 	case BOOT_SOURCE_SDMMC:
 		NOTICE("SDMMC boot\n");
-		sdmmc_init(&reverse_handoff_ptr, &params, &mmc_info);
+		cdns_mmc_init(&params, &mmc_info);
 		socfpga_io_setup(boot_source, PLAT_SDMMC_DATA_BASE);
 		break;
 
diff --git a/plat/intel/soc/agilex5/include/agilex5_system_manager.h b/plat/intel/soc/agilex5/include/agilex5_system_manager.h
index f61c479..ac4bf12 100644
--- a/plat/intel/soc/agilex5/include/agilex5_system_manager.h
+++ b/plat/intel/soc/agilex5/include/agilex5_system_manager.h
@@ -185,6 +185,21 @@
 #define SDM								0x000A000A
 #define CORE_SIGHT_DEBUG						0x000B000B
 
+/* JTAG ID value for Agilex5 */
+#define A590_JTAG_ID							0x9000
+#define A594_JTAG_ID							0x40009000
+#define A5C0_JTAG_ID							0xC000
+#define A5C4_JTAG_ID							0x4000C000
+#define A5D0_JTAG_ID							0xD000
+#define A5D4_JTAG_ID							0x4000D000
+#define A5F0_JTAG_ID							0xC000
+#define A5F4_JTAG_ID							0x4000F000
+#define A510_JTAG_ID							0x1000
+#define A514_JTAG_ID							0x40001000
+#define A530_JTAG_ID							0x3000
+#define A534_JTAG_ID							0x40003000
+#define JTAG_ID_MASK							0xC000F000
+
 /* Field Masking */
 #define SYSMGR_SDMMC_DRVSEL(x)						(((x) & 0x7) << 0)
 #define SYSMGR_SDMMC_SMPLSEL(x)						(((x) & 0x7) << 4)
diff --git a/plat/intel/soc/agilex5/include/socfpga_plat_def.h b/plat/intel/soc/agilex5/include/socfpga_plat_def.h
index 3a0aea1..c1f3cc5 100644
--- a/plat/intel/soc/agilex5/include/socfpga_plat_def.h
+++ b/plat/intel/soc/agilex5/include/socfpga_plat_def.h
@@ -16,7 +16,6 @@
 
 /* Platform Setting */
 #define PLATFORM_MODEL						PLAT_SOCFPGA_AGILEX5
-#define BOOT_SOURCE						BOOT_SOURCE_SDMMC
 /* 1 = Flush cache, 0 = No cache flush.
  * Default for Agilex5 is Cache flush.
  */
@@ -46,12 +45,24 @@
 #define CAD_QSPIDATA_OFST					0x10900000
 #define CAD_QSPI_OFFSET						0x108d2000
 
+/* FIP Setting */
+#define PLAT_FIP_BASE						(0)
+#if ARM_LINUX_KERNEL_AS_BL33
+#define PLAT_FIP_MAX_SIZE					(0x8000000)
+#else
+#define PLAT_FIP_MAX_SIZE					(0x1000000)
+#endif
+
 /* SDMMC Setting */
-# if ARM_LINUX_KERNEL_AS_BL33
+#if ARM_LINUX_KERNEL_AS_BL33
+#define PLAT_MMC_DATA_BASE					(0x90000000)
+#define PLAT_MMC_DATA_SIZE					(0x100000)
 #define SOCFPGA_MMC_BLOCK_SIZE					U(32768)
-# else
+#else
+#define PLAT_MMC_DATA_BASE					(0x0007D000)
+#define PLAT_MMC_DATA_SIZE					(0x2000)
 #define SOCFPGA_MMC_BLOCK_SIZE					U(8192)
-# endif
+#endif
 
 /* Register Mapping */
 #define SOCFPGA_CCU_NOC_REG_BASE				0x1c000000
@@ -69,10 +80,22 @@
 #define SOCFPGA_L4_SYS_SCR_REG_BASE				0x10d21100
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE				0x10d21200
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE				0x10d21300
+#define SOCFPGA_SDMMC_SECU_BIT					0x40
+#define SOCFPGA_LWSOC2FPGA_ENABLE				0xffe0301
+#define SOCFPGA_SDMMC_SECU_BIT_ENABLE				0x1010001
+
 
 /* Define maximum page size for NAND flash devices */
 #define PLATFORM_MTD_MAX_PAGE_SIZE				U(0x2000)
 
+/* OCRAM Register*/
+
+#define OCRAM_REG_BASE						0x108CC400
+#define OCRAM_REGION_0_OFFSET					0x18
+#define OCRAM_REGION_0_REG_BASE					(OCRAM_REG_BASE + \
+								OCRAM_REGION_0_OFFSET)
+#define OCRAM_NON_SECURE_ENABLE					0x0
+
 /*******************************************************************************
  * Platform memory map related constants
  ******************************************************************************/
diff --git a/plat/intel/soc/agilex5/platform.mk b/plat/intel/soc/agilex5/platform.mk
index 4bb90d5..a831c39 100644
--- a/plat/intel/soc/agilex5/platform.mk
+++ b/plat/intel/soc/agilex5/platform.mk
@@ -99,6 +99,7 @@
 		plat/intel/soc/common/sip/socfpga_sip_ecc.c		\
 		plat/intel/soc/common/sip/socfpga_sip_fcs.c		\
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
+		plat/intel/soc/common/soc/socfpga_system_manager.c	\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c
 
 # Configs for A76 and A55
@@ -113,6 +114,24 @@
 $(eval $(call add_define,ARM_LINUX_KERNEL_AS_BL33))
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
+# Configs for Boot Source
+SOCFPGA_BOOT_SOURCE_SDMMC		?=	0
+SOCFPGA_BOOT_SOURCE_QSPI		?=	0
+SOCFPGA_BOOT_SOURCE_NAND		?=	0
+
+$(eval $(call assert_booleans,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+$(eval $(call add_defines,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+
 # Configs for VAB Authentication
 SOCFPGA_SECURE_VAB_AUTH  := 	0
 $(eval $(call assert_boolean,SOCFPGA_SECURE_VAB_AUTH))
diff --git a/plat/intel/soc/common/drivers/sdmmc/sdmmc.c b/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
index 48f91eb..62698a9 100644
--- a/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
+++ b/plat/intel/soc/common/drivers/sdmmc/sdmmc.c
@@ -18,53 +18,26 @@
 #include <lib/mmio.h>
 #include <lib/utils.h>
 
-#include "agilex5_pinmux.h"
 #include "sdmmc.h"
 #include "socfpga_mailbox.h"
+#include "wdt/watchdog.h"
 
 static const struct mmc_ops *ops;
 static unsigned int mmc_ocr_value;
-static struct mmc_csd_emmc mmc_csd;
-static struct sd_switch_status sd_switch_func_status;
-static unsigned char mmc_ext_csd[512] __aligned(16);
 static unsigned int mmc_flags;
-static struct mmc_device_info *mmc_dev_info;
 static unsigned int rca;
-static unsigned int scr[2]__aligned(16) = { 0 };
 
 extern const struct mmc_ops cdns_sdmmc_ops;
 extern struct cdns_sdmmc_params cdns_params;
 extern struct cdns_sdmmc_combo_phy sdmmc_combo_phy_reg;
 extern struct cdns_sdmmc_sdhc sdmmc_sdhc_reg;
 
-static bool is_cmd23_enabled(void)
+bool is_cmd23_enabled(void)
 {
 	return ((mmc_flags & MMC_FLAG_CMD23) != 0U);
 }
 
-static bool is_sd_cmd6_enabled(void)
-{
-	return ((mmc_flags & MMC_FLAG_SD_CMD6) != 0U);
-}
-
-/* TODO: Will romove once ATF driver is developed */
-void sdmmc_pin_config(void)
-{
-	/* temp use base + addr. Official must change to common method */
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x00, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x04, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x08, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x0C, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x10, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x14, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x18, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x1C, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x20, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x24, 0x0);
-	mmio_write_32(AGX5_PINMUX_PIN0SEL+0x28, 0x0);
-}
-
-static int sdmmc_send_cmd(unsigned int idx, unsigned int arg,
+int sdmmc_send_cmd(unsigned int idx, unsigned int arg,
 			unsigned int r_type, unsigned int *r_data)
 {
 	struct mmc_cmd cmd;
@@ -94,7 +67,7 @@
 	return ret;
 }
 
-static int sdmmc_device_state(void)
+int sdmmc_device_state(void)
 {
 	int retries = DEFAULT_SDMMC_MAX_RETRIES;
 	unsigned int resp_data[4];
@@ -125,521 +98,12 @@
 	return MMC_GET_STATE(resp_data[0]);
 }
 
-static int sdmmc_set_ext_csd(unsigned int ext_cmd, unsigned int value)
-{
-	int ret;
-
-	ret = sdmmc_send_cmd(MMC_CMD(6),
-			   EXTCSD_WRITE_BYTES | EXTCSD_CMD(ext_cmd) |
-			   EXTCSD_VALUE(value) | EXTCSD_CMD_SET_NORMAL,
-			   MMC_RESPONSE_R1B, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	do {
-		ret = sdmmc_device_state();
-		if (ret < 0) {
-			return ret;
-		}
-	} while (ret == MMC_STATE_PRG);
-
-	return 0;
-}
-
-static int sdmmc_mmc_sd_switch(unsigned int bus_width)
-{
-	int ret;
-	int retries = DEFAULT_SDMMC_MAX_RETRIES;
-	unsigned int bus_width_arg = 0;
-
-	/* CMD55: Application Specific Command */
-	ret = sdmmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
-			   MMC_RESPONSE_R5, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	ret = ops->prepare(0, (uintptr_t)&scr, sizeof(scr));
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* ACMD51: SEND_SCR */
-	do {
-		ret = sdmmc_send_cmd(MMC_ACMD(51), 0, MMC_RESPONSE_R1, NULL);
-		if ((ret != 0) && (retries == 0)) {
-			ERROR("ACMD51 failed after %d retries (ret=%d)\n",
-			      DEFAULT_SDMMC_MAX_RETRIES, ret);
-			return ret;
-		}
-
-		retries--;
-	} while (ret != 0);
-
-	ret = ops->read(0, (uintptr_t)&scr, sizeof(scr));
-	if (ret != 0) {
-		return ret;
-	}
-
-	if (((scr[0] & SD_SCR_BUS_WIDTH_4) != 0U) &&
-	    (bus_width == MMC_BUS_WIDTH_4)) {
-		bus_width_arg = 2;
-	}
-
-	/* CMD55: Application Specific Command */
-	ret = sdmmc_send_cmd(MMC_CMD(55), rca << RCA_SHIFT_OFFSET,
-			   MMC_RESPONSE_R5, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* ACMD6: SET_BUS_WIDTH */
-	ret = sdmmc_send_cmd(MMC_ACMD(6), bus_width_arg, MMC_RESPONSE_R1, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	do {
-		ret = sdmmc_device_state();
-		if (ret < 0) {
-			return ret;
-		}
-	} while (ret == MMC_STATE_PRG);
-
-	return 0;
-}
-
-static int sdmmc_set_ios(unsigned int clk, unsigned int bus_width)
-{
-	int ret;
-	unsigned int width = bus_width;
-
-	if (mmc_dev_info->mmc_dev_type != MMC_IS_EMMC) {
-		if (width == MMC_BUS_WIDTH_8) {
-			WARN("Wrong bus config for SD-card, force to 4\n");
-			width = MMC_BUS_WIDTH_4;
-		}
-		ret = sdmmc_mmc_sd_switch(width);
-		if (ret != 0) {
-			return ret;
-		}
-	} else if (mmc_csd.spec_vers == 4U) {
-		ret = sdmmc_set_ext_csd(CMD_EXTCSD_BUS_WIDTH,
-				      (unsigned int)width);
-		if (ret != 0) {
-			return ret;
-		}
-	} else {
-		VERBOSE("Wrong MMC type or spec version\n");
-	}
-
-	return ops->set_ios(clk, width);
-}
-
-static int sdmmc_fill_device_info(void)
-{
-	unsigned long long c_size;
-	unsigned int speed_idx;
-	unsigned int nb_blocks;
-	unsigned int freq_unit;
-	int ret = 0;
-	struct mmc_csd_sd_v2 *csd_sd_v2;
-
-	switch (mmc_dev_info->mmc_dev_type) {
-	case MMC_IS_EMMC:
-		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
-
-		ret = ops->prepare(0, (uintptr_t)&mmc_ext_csd,
-				   sizeof(mmc_ext_csd));
-		if (ret != 0) {
-			return ret;
-		}
-
-		/* MMC CMD8: SEND_EXT_CSD */
-		ret = sdmmc_send_cmd(MMC_CMD(8), 0, MMC_RESPONSE_R1, NULL);
-		if (ret != 0) {
-			return ret;
-		}
-
-		ret = ops->read(0, (uintptr_t)&mmc_ext_csd,
-				sizeof(mmc_ext_csd));
-		if (ret != 0) {
-			return ret;
-		}
-
-		do {
-			ret = sdmmc_device_state();
-			if (ret < 0) {
-				return ret;
-			}
-		} while (ret != MMC_STATE_TRAN);
-
-		nb_blocks = (mmc_ext_csd[CMD_EXTCSD_SEC_CNT] << 0) |
-			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 1] << 8) |
-			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 2] << 16) |
-			    (mmc_ext_csd[CMD_EXTCSD_SEC_CNT + 3] << 24);
-
-		mmc_dev_info->device_size = (unsigned long long)nb_blocks *
-			mmc_dev_info->block_size;
-
-		break;
-
-	case MMC_IS_SD:
-		/*
-		 * Use the same mmc_csd struct, as required fields here
-		 * (READ_BL_LEN, C_SIZE, CSIZE_MULT) are common with eMMC.
-		 */
-		mmc_dev_info->block_size = BIT_32(mmc_csd.read_bl_len);
-
-		c_size = ((unsigned long long)mmc_csd.c_size_high << 2U) |
-			 (unsigned long long)mmc_csd.c_size_low;
-		assert(c_size != 0xFFFU);
-
-		mmc_dev_info->device_size = (c_size + 1U) *
-					    BIT_64(mmc_csd.c_size_mult + 2U) *
-					    mmc_dev_info->block_size;
-
-		break;
-
-	case MMC_IS_SD_HC:
-		assert(mmc_csd.csd_structure == 1U);
-
-		mmc_dev_info->block_size = MMC_BLOCK_SIZE;
-
-		/* Need to use mmc_csd_sd_v2 struct */
-		csd_sd_v2 = (struct mmc_csd_sd_v2 *)&mmc_csd;
-		c_size = ((unsigned long long)csd_sd_v2->c_size_high << 16) |
-			 (unsigned long long)csd_sd_v2->c_size_low;
-
-		mmc_dev_info->device_size = (c_size + 1U) << SDMMC_MULT_BY_512K_SHIFT;
-
-		break;
-
-	default:
-		ret = -EINVAL;
-		break;
-	}
-
-	if (ret < 0) {
-		return ret;
-	}
-
-	speed_idx = (mmc_csd.tran_speed & CSD_TRAN_SPEED_MULT_MASK) >>
-			 CSD_TRAN_SPEED_MULT_SHIFT;
-
-	assert(speed_idx > 0U);
-
-	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
-		mmc_dev_info->max_bus_freq = tran_speed_base[speed_idx];
-	} else {
-		mmc_dev_info->max_bus_freq = sd_tran_speed_base[speed_idx];
-	}
-
-	freq_unit = mmc_csd.tran_speed & CSD_TRAN_SPEED_UNIT_MASK;
-	while (freq_unit != 0U) {
-		mmc_dev_info->max_bus_freq *= 10U;
-		--freq_unit;
-	}
-
-	mmc_dev_info->max_bus_freq *= 10000U;
-
-	return 0;
-}
-
-static int sdmmc_sd_switch(unsigned int mode, unsigned char group,
-		     unsigned char func)
-{
-	unsigned int group_shift = (group - 1U) * 4U;
-	unsigned int group_mask = GENMASK(group_shift + 3U,  group_shift);
-	unsigned int arg;
-	int ret;
-
-	ret = ops->prepare(0, (uintptr_t)&sd_switch_func_status,
-			   sizeof(sd_switch_func_status));
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* MMC CMD6: SWITCH_FUNC */
-	arg = mode | SD_SWITCH_ALL_GROUPS_MASK;
-	arg &= ~group_mask;
-	arg |= func << group_shift;
-	ret = sdmmc_send_cmd(MMC_CMD(6), arg, MMC_RESPONSE_R1, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	return ops->read(0, (uintptr_t)&sd_switch_func_status,
-			 sizeof(sd_switch_func_status));
-}
-
-static int sdmmc_sd_send_op_cond(void)
-{
-	int n;
-	unsigned int resp_data[4];
-
-	for (n = 0; n < SEND_SDMMC_OP_COND_MAX_RETRIES; n++) {
-		int ret;
-
-		/* CMD55: Application Specific Command */
-		ret = sdmmc_send_cmd(MMC_CMD(55), 0, MMC_RESPONSE_R1, NULL);
-		if (ret != 0) {
-			return ret;
-		}
-
-		/* ACMD41: SD_SEND_OP_COND */
-		ret = sdmmc_send_cmd(MMC_ACMD(41), OCR_HCS |
-			mmc_dev_info->ocr_voltage, MMC_RESPONSE_R3,
-			&resp_data[0]);
-		if (ret != 0) {
-			return ret;
-		}
-
-		if ((resp_data[0] & OCR_POWERUP) != 0U) {
-			mmc_ocr_value = resp_data[0];
-
-			if ((mmc_ocr_value & OCR_HCS) != 0U) {
-				mmc_dev_info->mmc_dev_type = MMC_IS_SD_HC;
-			} else {
-				mmc_dev_info->mmc_dev_type = MMC_IS_SD;
-			}
-
-			return 0;
-		}
-
-		mdelay(10);
-	}
-
-	ERROR("ACMD41 failed after %d retries\n", SEND_SDMMC_OP_COND_MAX_RETRIES);
-
-	return -EIO;
-}
-
-static int sdmmc_reset_to_idle(void)
-{
-	int ret;
-
-	/* CMD0: reset to IDLE */
-	ret = sdmmc_send_cmd(MMC_CMD(0), 0, 0, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	mdelay(2);
-
-	return 0;
-}
-
-static int sdmmc_send_op_cond(void)
-{
-	int ret, n;
-	unsigned int resp_data[4];
-
-	ret = sdmmc_reset_to_idle();
-	if (ret != 0) {
-		return ret;
-	}
-
-	for (n = 0; n < SEND_SDMMC_OP_COND_MAX_RETRIES; n++) {
-		ret = sdmmc_send_cmd(MMC_CMD(1), OCR_SECTOR_MODE |
-				   OCR_VDD_MIN_2V7 | OCR_VDD_MIN_1V7,
-				   MMC_RESPONSE_R3, &resp_data[0]);
-		if (ret != 0) {
-			return ret;
-		}
-
-		if ((resp_data[0] & OCR_POWERUP) != 0U) {
-			mmc_ocr_value = resp_data[0];
-			return 0;
-		}
-
-		mdelay(10);
-	}
-
-	ERROR("CMD1 failed after %d retries\n", SEND_SDMMC_OP_COND_MAX_RETRIES);
-
-	return -EIO;
-}
-
-static int sdmmc_enumerate(unsigned int clk, unsigned int bus_width)
-{
-	int ret;
-	unsigned int resp_data[4];
-
-	ops->init();
-
-	ret = sdmmc_reset_to_idle();
-	if (ret != 0) {
-		return ret;
-	}
-
-	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
-		ret = sdmmc_send_op_cond();
-	} else {
-		/* CMD8: Send Interface Condition Command */
-		ret = sdmmc_send_cmd(MMC_CMD(8), VHS_2_7_3_6_V | CMD8_CHECK_PATTERN,
-				   MMC_RESPONSE_R5, &resp_data[0]);
-
-		if ((ret == 0) && ((resp_data[0] & 0xffU) == CMD8_CHECK_PATTERN)) {
-			ret = sdmmc_sd_send_op_cond();
-		}
-	}
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* CMD2: Card Identification */
-	ret = sdmmc_send_cmd(MMC_CMD(2), 0, MMC_RESPONSE_R2, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* CMD3: Set Relative Address */
-	if (mmc_dev_info->mmc_dev_type == MMC_IS_EMMC) {
-		rca = MMC_FIX_RCA;
-		ret = sdmmc_send_cmd(MMC_CMD(3), rca << RCA_SHIFT_OFFSET,
-				   MMC_RESPONSE_R1, NULL);
-		if (ret != 0) {
-			return ret;
-		}
-	} else {
-		ret = sdmmc_send_cmd(MMC_CMD(3), 0,
-				   MMC_RESPONSE_R6, &resp_data[0]);
-		if (ret != 0) {
-			return ret;
-		}
-
-		rca = (resp_data[0] & 0xFFFF0000U) >> 16;
-	}
-
-	/* CMD9: CSD Register */
-	ret = sdmmc_send_cmd(MMC_CMD(9), rca << RCA_SHIFT_OFFSET,
-			   MMC_RESPONSE_R2, &resp_data[0]);
-	if (ret != 0) {
-		return ret;
-	}
-
-	memcpy_s(&mmc_csd, sizeof(mmc_csd) / MBOX_WORD_BYTE,
-		&resp_data, sizeof(resp_data) / MBOX_WORD_BYTE);
-
-	/* CMD7: Select Card */
-	ret = sdmmc_send_cmd(MMC_CMD(7), rca << RCA_SHIFT_OFFSET,
-			   MMC_RESPONSE_R1, NULL);
-	if (ret != 0) {
-		return ret;
-	}
-
-	do {
-		ret = sdmmc_device_state();
-		if (ret < 0) {
-			return ret;
-		}
-	} while (ret != MMC_STATE_TRAN);
-
-	ret = sdmmc_set_ios(clk, bus_width);
-	if (ret != 0) {
-		return ret;
-	}
-
-	ret = sdmmc_fill_device_info();
-	if (ret != 0) {
-		return ret;
-	}
-
-	if (is_sd_cmd6_enabled() &&
-	    (mmc_dev_info->mmc_dev_type == MMC_IS_SD_HC)) {
-		/* Try to switch to High Speed Mode */
-		ret = sdmmc_sd_switch(SD_SWITCH_FUNC_CHECK, 1U, 1U);
-		if (ret != 0) {
-			return ret;
-		}
-
-		if ((sd_switch_func_status.support_g1 & BIT(9)) == 0U) {
-			/* High speed not supported, keep default speed */
-			return 0;
-		}
-
-		ret = sdmmc_sd_switch(SD_SWITCH_FUNC_SWITCH, 1U, 1U);
-		if (ret != 0) {
-			return ret;
-		}
-
-		if ((sd_switch_func_status.sel_g2_g1 & 0x1U) == 0U) {
-			/* Cannot switch to high speed, keep default speed */
-			return 0;
-		}
-
-		mmc_dev_info->max_bus_freq = 50000000U;
-		ret = ops->set_ios(clk, bus_width);
-	}
-
-	return ret;
-}
-
 size_t sdmmc_read_blocks(int lba, uintptr_t buf, size_t size)
 {
-	int ret;
-	unsigned int cmd_idx, cmd_arg;
-
-	assert((ops != NULL) &&
-	       (ops->read != NULL) &&
-	       (size != 0U) &&
-	       ((size & MMC_BLOCK_MASK) == 0U));
-
-	ret = ops->prepare(lba, buf, size);
-	if (ret != 0) {
-		return 0;
-	}
-
-	if (is_cmd23_enabled()) {
-		/* Set block count */
-		ret = sdmmc_send_cmd(MMC_CMD(23), size / MMC_BLOCK_SIZE,
-				   MMC_RESPONSE_R1, NULL);
-		if (ret != 0) {
-			return 0;
-		}
-
-		cmd_idx = MMC_CMD(18);
-	} else {
-		if (size > MMC_BLOCK_SIZE) {
-			cmd_idx = MMC_CMD(18);
-		} else {
-			cmd_idx = MMC_CMD(17);
-		}
-	}
-
-	if (((mmc_ocr_value & OCR_ACCESS_MODE_MASK) == OCR_BYTE_MODE) &&
-	    (mmc_dev_info->mmc_dev_type != MMC_IS_SD_HC)) {
-		cmd_arg = lba * MMC_BLOCK_SIZE;
-	} else {
-		cmd_arg = lba;
-	}
+	mmc_read_blocks(lba, buf, size);
 
-	ret = sdmmc_send_cmd(cmd_idx, cmd_arg, MMC_RESPONSE_R1, NULL);
-	if (ret != 0) {
-		return 0;
-	}
-
-	ret = ops->read(lba, buf, size);
-	if (ret != 0) {
-		return 0;
-	}
-
-	/* Wait buffer empty */
-	do {
-		ret = sdmmc_device_state();
-		if (ret < 0) {
-			return 0;
-		}
-	} while ((ret != MMC_STATE_TRAN) && (ret != MMC_STATE_DATA));
-
-	if (!is_cmd23_enabled() && (size > MMC_BLOCK_SIZE)) {
-		ret = sdmmc_send_cmd(MMC_CMD(12), 0, MMC_RESPONSE_R1B, NULL);
-		if (ret != 0) {
-			return 0;
-		}
-	}
+	/* Restart watchdog for reading each chunk byte */
+	watchdog_sw_rst();
 
 	return size;
 }
@@ -710,64 +174,3 @@
 
 	return size;
 }
-
-int sd_or_mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
-	     unsigned int width, unsigned int flags,
-	     struct mmc_device_info *device_info)
-{
-	assert((ops_ptr != NULL) &&
-	       (ops_ptr->init != NULL) &&
-	       (ops_ptr->send_cmd != NULL) &&
-	       (ops_ptr->set_ios != NULL) &&
-	       (ops_ptr->prepare != NULL) &&
-	       (ops_ptr->read != NULL) &&
-	       (ops_ptr->write != NULL) &&
-	       (device_info != NULL) &&
-	       (clk != 0) &&
-	       ((width == MMC_BUS_WIDTH_1) ||
-		(width == MMC_BUS_WIDTH_4) ||
-		(width == MMC_BUS_WIDTH_8) ||
-		(width == MMC_BUS_WIDTH_DDR_4) ||
-		(width == MMC_BUS_WIDTH_DDR_8)));
-
-	ops = ops_ptr;
-	mmc_flags = flags;
-	mmc_dev_info = device_info;
-
-	return sdmmc_enumerate(clk, width);
-}
-
-int sdmmc_init(handoff *hoff_ptr, struct cdns_sdmmc_params *params, struct mmc_device_info *info)
-{
-	int result = 0;
-
-	/* SDMMC pin mux configuration */
-	sdmmc_pin_config();
-	cdns_set_sdmmc_var(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
-	result = cdns_sd_host_init(&sdmmc_combo_phy_reg, &sdmmc_sdhc_reg);
-	if (result < 0) {
-		return result;
-	}
-
-	assert((params != NULL) &&
-	       ((params->reg_base & MMC_BLOCK_MASK) == 0) &&
-	       ((params->desc_base & MMC_BLOCK_MASK) == 0) &&
-	       ((params->desc_size & MMC_BLOCK_MASK) == 0) &&
-		   ((params->reg_pinmux & MMC_BLOCK_MASK) == 0) &&
-		   ((params->reg_phy & MMC_BLOCK_MASK) == 0) &&
-	       (params->desc_size > 0) &&
-	       (params->clk_rate > 0) &&
-	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
-		(params->bus_width == MMC_BUS_WIDTH_4) ||
-		(params->bus_width == MMC_BUS_WIDTH_8)));
-
-	memcpy_s(&cdns_params, sizeof(struct cdns_sdmmc_params) / MBOX_WORD_BYTE,
-		params, sizeof(struct cdns_sdmmc_params) / MBOX_WORD_BYTE);
-	cdns_params.cdn_sdmmc_dev_type = info->mmc_dev_type;
-	cdns_params.cdn_sdmmc_dev_mode = SD_DS;
-
-	result = sd_or_mmc_init(&cdns_sdmmc_ops, params->clk_rate, params->bus_width,
-		params->flags, info);
-
-	return result;
-}
diff --git a/plat/intel/soc/common/drivers/sdmmc/sdmmc.h b/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
index 16c6b04..3f6119c 100644
--- a/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
+++ b/plat/intel/soc/common/drivers/sdmmc/sdmmc.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2022-2023, Intel Corporation. All rights reserved.
+ * Copyright (c) 2024, Altera Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,10 +34,12 @@
  * @hoff_ptr: Pointer to the hand-off data
  * Return: 0 on success, a negative errno on failure
  */
-int sdmmc_init(handoff *hoff_ptr, struct cdns_sdmmc_params *params,
-	     struct mmc_device_info *info);
-int sd_or_mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
-	     unsigned int width, unsigned int flags,
-	     struct mmc_device_info *device_info);
 void sdmmc_pin_config(void);
+size_t sdmmc_read_blocks(int lba, uintptr_t buf, size_t size);
+size_t sdmmc_write_blocks(int lba, const uintptr_t buf, size_t size);
+int sdmmc_device_state(void);
+bool is_cmd23_enabled(void);
+int sdmmc_send_cmd(unsigned int idx, unsigned int arg,
+			unsigned int r_type, unsigned int *r_data);
+
 #endif
diff --git a/plat/intel/soc/common/include/platform_def.h b/plat/intel/soc/common/include/platform_def.h
index 08ab5df..a820e41 100644
--- a/plat/intel/soc/common/include/platform_def.h
+++ b/plat/intel/soc/common/include/platform_def.h
@@ -22,6 +22,18 @@
 #define PLAT_SOCFPGA_AGILEX5			4
 #define SIMICS_RUN				1
 #define MAX_IO_MTD_DEVICES			U(1)
+/* Boot Source configuration
+ * TODO: Shall consider "assert_numeric" in the future
+ */
+#if SOCFPGA_BOOT_SOURCE_NAND
+#define BOOT_SOURCE						BOOT_SOURCE_NAND
+#elif SOCFPGA_BOOT_SOURCE_SDMMC
+#define BOOT_SOURCE						BOOT_SOURCE_SDMMC
+#elif SOCFPGA_BOOT_SOURCE_QSPI
+#define BOOT_SOURCE						BOOT_SOURCE_QSPI
+#else
+#define BOOT_SOURCE						BOOT_SOURCE_SDMMC
+#endif
 
 /* sysmgr.boot_scratch_cold4 & 5 used for CPU release address for SPL */
 #define PLAT_CPU_RELEASE_ADDR			0xffd12210
diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index fcee101..82f9fd3 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -247,6 +247,7 @@
 
 #if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
 void intel_smmu_hps_remapper_init(uint64_t *mem);
+int intel_smmu_hps_remapper_config(uint32_t remapper_bypass);
 #endif
 
 int mailbox_rsu_get_spt_offset(uint32_t *resp_buf, uint32_t resp_buf_len);
diff --git a/plat/intel/soc/common/include/socfpga_private.h b/plat/intel/soc/common/include/socfpga_private.h
index a0d4180..fbe18c3 100644
--- a/plat/intel/soc/common/include/socfpga_private.h
+++ b/plat/intel/soc/common/include/socfpga_private.h
@@ -24,8 +24,8 @@
 	BOOT_SOURCE_FPGA = 0,
 	BOOT_SOURCE_SDMMC,
 	BOOT_SOURCE_NAND,
-	BOOT_SOURCE_RSVD,
-	BOOT_SOURCE_QSPI
+	BOOT_SOURCE_QSPI,
+	BOOT_SOURCE_RSVD
 } boot_source_type;
 
 /*******************************************************************************
diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index d2eceb9..31474c4 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -140,6 +140,7 @@
 #define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_FINALIZE			0xC200008B
 #define INTEL_SIP_SMC_FCS_ECDH_REQUEST_INIT				0xC200008C
 #define INTEL_SIP_SMC_FCS_ECDH_REQUEST_FINALIZE				0xC200008E
+#define INTEL_SIP_SMC_FCS_SDM_REMAPPER_CONFIG				0xC2000201
 
 /* SEU ERR */
 #define INTEL_SIP_SMC_SEU_ERR_STATUS					0xC2000099
diff --git a/plat/intel/soc/common/include/socfpga_system_manager.h b/plat/intel/soc/common/include/socfpga_system_manager.h
index 1327b81..346cfe1 100644
--- a/plat/intel/soc/common/include/socfpga_system_manager.h
+++ b/plat/intel/soc/common/include/socfpga_system_manager.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+ * Copyright (c) 2024, Altera Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,4 +33,8 @@
 #define SOCFPGA_SYSMGR(_reg)		(SOCFPGA_SYSMGR_REG_BASE \
 						+ (SOCFPGA_SYSMGR_##_reg))
 
+/* Function Prototype */
+uint32_t intel_hps_get_jtag_id(void);
+bool is_agilex5_A5F0(void);
+
 #endif /* SOCFPGA_SYSTEMMANAGER_H */
diff --git a/plat/intel/soc/common/soc/socfpga_system_manager.c b/plat/intel/soc/common/soc/socfpga_system_manager.c
new file mode 100644
index 0000000..4223b2b
--- /dev/null
+++ b/plat/intel/soc/common/soc/socfpga_system_manager.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2024, Altera Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#include "socfpga_system_manager.h"
+
+uint32_t intel_hps_get_jtag_id(void)
+{
+	uint32_t jtag_id = 0x00;
+
+	jtag_id = (mmio_read_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_4)));
+
+	INFO("%s: JTAG ID: 0x%x\n", __func__, jtag_id);
+
+	return jtag_id;
+}
+
+/* Check for Agilex5 SM4 */
+bool is_agilex5_A5F0(void)
+{
+	return ((intel_hps_get_jtag_id() & JTAG_ID_MASK) == A5F0_JTAG_ID);
+}
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index 5dfbc14..3c223c1 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -28,6 +28,9 @@
 static uint32_t send_id, rcv_id;
 static uint32_t bytes_per_block, blocks_submitted;
 static bool bridge_disable;
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+static uint32_t g_remapper_bypass;
+#endif
 
 /* RSU static variables */
 static uint32_t rsu_dcmf_ver[4] = {0};
@@ -758,7 +761,7 @@
 	/* Read out Bit 1 value */
 	uint32_t remap = (mmio_read_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_POR_1)) & 0x02);
 
-	if (remap == 0x00) {
+	if ((remap == 0x00) && (g_remapper_bypass == 0x00)) {
 		/* Update DRAM Base address for SDM SMMU */
 		mmio_write_32(SOCFPGA_SYSMGR(SDM_BE_ARADDR_REMAP), DRAM_BASE);
 		mmio_write_32(SOCFPGA_SYSMGR(SDM_BE_AWADDR_REMAP), DRAM_BASE);
@@ -766,6 +769,19 @@
 	} else {
 		*mem = *mem - DRAM_BASE;
 	}
+}
+
+int intel_smmu_hps_remapper_config(uint32_t remapper_bypass)
+{
+	/* Read out the JTAG-ID from boot scratch register */
+	if (is_agilex5_A5F0() != 0) {
+		if (remapper_bypass == 0x01) {
+			g_remapper_bypass = remapper_bypass;
+			mmio_write_32(SOCFPGA_SYSMGR(SDM_BE_ARADDR_REMAP), 0);
+			mmio_write_32(SOCFPGA_SYSMGR(SDM_BE_AWADDR_REMAP), 0);
+		}
+	}
+	return INTEL_SIP_SMC_STATUS_OK;
 }
 #endif
 
@@ -1291,6 +1307,12 @@
 		status = intel_fcs_aes_crypt_update_finalize(x1, x2, x3, x4,
 					x5, x6, true, &send_id);
 		SMC_RET1(handle, status);
+
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	case INTEL_SIP_SMC_FCS_SDM_REMAPPER_CONFIG:
+		status = intel_smmu_hps_remapper_config(x1);
+		SMC_RET1(handle, status);
+#endif
 
 	case INTEL_SIP_SMC_GET_ROM_PATCH_SHA384:
 		status = intel_fcs_get_rom_patch_sha384(x1, &retval64,
diff --git a/plat/intel/soc/common/socfpga_storage.c b/plat/intel/soc/common/socfpga_storage.c
index 7679f59..66b5216 100644
--- a/plat/intel/soc/common/socfpga_storage.c
+++ b/plat/intel/soc/common/socfpga_storage.c
@@ -22,22 +22,12 @@
 #include <drivers/partition/partition.h>
 #include <lib/mmio.h>
 #include <tools_share/firmware_image_package.h>
+
 #include "drivers/sdmmc/sdmmc.h"
 #include "socfpga_private.h"
 #include "socfpga_ros.h"
 
 
-#define PLAT_FIP_BASE		(0)
-# if ARM_LINUX_KERNEL_AS_BL33
-#define PLAT_FIP_MAX_SIZE	(0x8000000)
-#define PLAT_MMC_DATA_BASE	(0x10000000)
-#define PLAT_MMC_DATA_SIZE	(0x100000)
-# else
-#define PLAT_FIP_MAX_SIZE	(0x1000000)
-#define PLAT_MMC_DATA_BASE	(0xffe3c000)
-#define PLAT_MMC_DATA_SIZE	(0x2000)
-# endif
-
 static const io_dev_connector_t *fip_dev_con;
 static const io_dev_connector_t *boot_dev_con;
 
diff --git a/plat/intel/soc/n5x/include/socfpga_plat_def.h b/plat/intel/soc/n5x/include/socfpga_plat_def.h
index c477787..6f0a40b 100644
--- a/plat/intel/soc/n5x/include/socfpga_plat_def.h
+++ b/plat/intel/soc/n5x/include/socfpga_plat_def.h
@@ -15,7 +15,6 @@
 
 /* Platform Setting */
 #define PLATFORM_MODEL				PLAT_SOCFPGA_N5X
-#define BOOT_SOURCE				BOOT_SOURCE_SDMMC
 #define PLAT_PRIMARY_CPU			0
 #define PLAT_CLUSTER_ID_MPIDR_AFF_SHIFT		MPIDR_AFF1_SHIFT
 #define PLAT_CPU_ID_MPIDR_AFF_SHIFT		MPIDR_AFF0_SHIFT
@@ -30,12 +29,14 @@
 #define CAD_QSPIDATA_OFST			0xff900000
 #define CAD_QSPI_OFFSET				0xff8d2000
 
+/* FIP Setting */
+#define PLAT_FIP_BASE				(0)
+#define PLAT_FIP_MAX_SIZE			(0x1000000)
+
 /* SDMMC Setting */
-# if ARM_LINUX_KERNEL_AS_BL33
-#define SOCFPGA_MMC_BLOCK_SIZE			U(32768)
-# else
+#define PLAT_MMC_DATA_BASE			(0xffe3c000)
+#define PLAT_MMC_DATA_SIZE			(0x2000)
 #define SOCFPGA_MMC_BLOCK_SIZE			U(8192)
-# endif
 
 /* Register Mapping */
 #define SOCFPGA_CCU_NOC_REG_BASE		U(0xf7000000)
diff --git a/plat/intel/soc/n5x/platform.mk b/plat/intel/soc/n5x/platform.mk
index 29fd0eb..4770f8d 100644
--- a/plat/intel/soc/n5x/platform.mk
+++ b/plat/intel/soc/n5x/platform.mk
@@ -1,5 +1,6 @@
 #
-# Copyright (c) 2020-2022, Intel Corporation. All rights reserved.
+# Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
+# Copyright (c) 2024, Altera Corporation. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -46,8 +47,30 @@
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c
 
+# Don't have the Linux kernel as a BL33 image by default
+ARM_LINUX_KERNEL_AS_BL33	:=	0
+$(eval $(call assert_boolean,ARM_LINUX_KERNEL_AS_BL33))
+$(eval $(call add_define,ARM_LINUX_KERNEL_AS_BL33))
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
+# Configs for Boot Source
+SOCFPGA_BOOT_SOURCE_SDMMC		?=	0
+SOCFPGA_BOOT_SOURCE_QSPI		?=	0
+SOCFPGA_BOOT_SOURCE_NAND		?=	0
+
+$(eval $(call assert_booleans,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+$(eval $(call add_defines,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+
 PROGRAMMABLE_RESET_ADDRESS	:= 0
 RESET_TO_BL2			:= 1
 BL2_INV_DCACHE			:= 0
diff --git a/plat/intel/soc/stratix10/include/socfpga_plat_def.h b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
index cd5b076..90345c3 100644
--- a/plat/intel/soc/stratix10/include/socfpga_plat_def.h
+++ b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
@@ -14,7 +14,6 @@
 
 /* Platform Setting */
 #define PLATFORM_MODEL				PLAT_SOCFPGA_STRATIX10
-#define BOOT_SOURCE				BOOT_SOURCE_SDMMC
 #define PLAT_PRIMARY_CPU			0
 #define PLAT_CLUSTER_ID_MPIDR_AFF_SHIFT		MPIDR_AFF1_SHIFT
 #define PLAT_CPU_ID_MPIDR_AFF_SHIFT		MPIDR_AFF0_SHIFT
@@ -29,12 +28,14 @@
 #define CAD_QSPIDATA_OFST			0xff900000
 #define CAD_QSPI_OFFSET				0xff8d2000
 
+/* FIP Setting */
+#define PLAT_FIP_BASE				(0)
+#define PLAT_FIP_MAX_SIZE			(0x1000000)
+
 /* SDMMC Setting */
-# if ARM_LINUX_KERNEL_AS_BL33
-#define SOCFPGA_MMC_BLOCK_SIZE			U(32768)
-# else
+#define PLAT_MMC_DATA_BASE			(0xffe3c000)
+#define PLAT_MMC_DATA_SIZE			(0x2000)
 #define SOCFPGA_MMC_BLOCK_SIZE			U(8192)
-# endif
 
 /* Register Mapping */
 #define SOCFPGA_CCU_NOC_REG_BASE		0xf7000000
diff --git a/plat/intel/soc/stratix10/platform.mk b/plat/intel/soc/stratix10/platform.mk
index 0a33c3d..4cd7032 100644
--- a/plat/intel/soc/stratix10/platform.mk
+++ b/plat/intel/soc/stratix10/platform.mk
@@ -78,8 +78,30 @@
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c
 
+# Don't have the Linux kernel as a BL33 image by default
+ARM_LINUX_KERNEL_AS_BL33	:=	0
+$(eval $(call assert_boolean,ARM_LINUX_KERNEL_AS_BL33))
+$(eval $(call add_define,ARM_LINUX_KERNEL_AS_BL33))
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
+# Configs for Boot Source
+SOCFPGA_BOOT_SOURCE_SDMMC		?=	0
+SOCFPGA_BOOT_SOURCE_QSPI		?=	0
+SOCFPGA_BOOT_SOURCE_NAND		?=	0
+
+$(eval $(call assert_booleans,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+$(eval $(call add_defines,\
+	$(sort \
+		SOCFPGA_BOOT_SOURCE_SDMMC \
+		SOCFPGA_BOOT_SOURCE_QSPI \
+		SOCFPGA_BOOT_SOURCE_NAND \
+)))
+
 PROGRAMMABLE_RESET_ADDRESS	:= 0
 RESET_TO_BL2			:= 1
 USE_COHERENT_MEM		:= 1
diff --git a/plat/mediatek/drivers/rng/mt8186/rng_plat.c b/plat/mediatek/drivers/rng/mt8186/rng_plat.c
new file mode 100644
index 0000000..691b923
--- /dev/null
+++ b/plat/mediatek/drivers/rng/mt8186/rng_plat.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2024, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/smccc.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+#include <services/trng_svc.h>
+#include <smccc_helpers.h>
+
+#include <mtk_mmap_pool.h>
+#include <mtk_sip_svc.h>
+#include "rng_plat.h"
+
+static spinlock_t rng_lock;
+
+static int trng_wait(uint32_t reg, uint32_t expected_value)
+{
+	uint64_t timeout = timeout_init_us(TRNG_TIME_OUT);
+	uint32_t value = 0;
+
+	do {
+		value = mmio_read_32(reg);
+		if ((value & expected_value) == expected_value)
+			return 0;
+
+		udelay(10);
+	} while (!timeout_elapsed(timeout));
+
+	return -ETIMEDOUT;
+}
+
+static int trng_write(uint32_t reg, uint32_t value,
+			   uint32_t read_reg, uint32_t expected_value)
+{
+	int retry = MTK_TRNG_MAX_ROUND;
+	uint32_t read_value = 0;
+
+	do {
+		mmio_write_32(reg, value);
+
+		read_value = mmio_read_32(read_reg);
+		if ((read_value & value) == expected_value)
+			return 0;
+
+		udelay(10);
+	} while (--retry > 0);
+
+	return -ETIMEDOUT;
+}
+
+static uint32_t trng_prng(uint32_t *rand)
+{
+	int32_t ret = 0;
+	uint32_t seed[4] = {0};
+
+	if (rand == NULL)
+		return MTK_SIP_E_INVALID_PARAM;
+
+	/* ungate */
+	ret = trng_write(TRNG_PDN_CLR, TRNG_PDN_VALUE, TRNG_PDN_STATUS, 0);
+	if (ret) {
+		ERROR("%s: ungate fail\n", __func__);
+		return MTK_SIP_E_NOT_SUPPORTED;
+	}
+
+	/* read random data once and drop it */
+	seed[0] = mmio_read_32(TRNG_DATA);
+
+	/* enable von-neumann extractor */
+	mmio_setbits_32(TRNG_CONF, TRNG_CONF_VON_EN);
+
+	/* start */
+	mmio_setbits_32(TRNG_CTRL, TRNG_CTRL_START);
+
+	/* get seeds from trng */
+	for (int i = 0; i < ARRAY_SIZE(seed); i++) {
+		ret = trng_wait(TRNG_CTRL, TRNG_CTRL_RDY);
+		if (ret) {
+			ERROR("%s: trng NOT ready\n", __func__);
+			return MTK_SIP_E_NOT_SUPPORTED;
+		}
+
+		seed[i] = mmio_read_32(TRNG_DATA);
+	}
+
+	/* stop */
+	mmio_clrbits_32(TRNG_CTRL, TRNG_CTRL_START);
+
+	/* gate */
+	ret = trng_write(TRNG_PDN_SET, TRNG_PDN_VALUE, TRNG_PDN_STATUS, TRNG_PDN_VALUE);
+	if (ret) {
+		ERROR("%s: gate fail\n", __func__);
+		return MTK_SIP_E_NOT_SUPPORTED;
+	}
+
+	for (int i = 0; i < ARRAY_SIZE(seed); i++)
+		rand[i] = seed[i];
+
+	return 0;
+}
+
+static uint32_t get_true_rnd(uint32_t *val, uint32_t num)
+{
+	uint32_t rand[4] = {0};
+	uint32_t ret;
+
+	if (val == NULL || num > ARRAY_SIZE(rand))
+		return MTK_SIP_E_INVALID_PARAM;
+
+	spin_lock(&rng_lock);
+	ret = trng_prng(rand);
+	spin_unlock(&rng_lock);
+
+	for (int i = 0; i < num; i++)
+		val[i] = rand[i];
+
+	return ret;
+}
+
+/*
+ * plat_get_entropy - get 64-bit random number data which is used form
+ * atf early stage
+ * output - out: output 64-bit entropy combine with 2 32-bit random number
+ */
+bool plat_get_entropy(uint64_t *out)
+{
+	uint32_t entropy_pool[2] = {0};
+	uint32_t ret;
+
+	assert(out);
+	assert(!check_uptr_overflow((uintptr_t)out, sizeof(*out)));
+
+	/* Get 2 32-bits entropy */
+	ret = get_true_rnd(entropy_pool, ARRAY_SIZE(entropy_pool));
+	if (ret)
+		return false;
+
+	/* Output 8 bytes entropy combine with 2 32-bit random number. */
+	*out = ((uint64_t)entropy_pool[0] << 32) | entropy_pool[1];
+
+	return true;
+}
diff --git a/plat/mediatek/drivers/rng/mt8186/rng_plat.h b/plat/mediatek/drivers/rng/mt8186/rng_plat.h
new file mode 100644
index 0000000..ab22c45
--- /dev/null
+++ b/plat/mediatek/drivers/rng/mt8186/rng_plat.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2024, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RNG_PLAT_H
+#define RNG_PLAT_H
+
+#define TRNG_TIME_OUT		1000
+#define MTK_TRNG_MAX_ROUND	4
+
+/*******************************************************************************
+ * TRNG related constants
+ ******************************************************************************/
+#define TRNG_BASE_SIZE		0x1000
+#define TRNG_CTRL		(TRNG_BASE + 0x0000)
+#define TRNG_TIME		(TRNG_BASE + 0x0004)
+#define TRNG_DATA		(TRNG_BASE + 0x0008)
+#define TRNG_CONF		(TRNG_BASE + 0x000C)
+#define TRNG_CTRL_RDY		0x80000000
+#define TRNG_CTRL_START		0x00000001
+#define TRNG_CONF_VON_EN	0x00000020
+#define TRNG_PDN_BASE_SIZE	0x1000
+#define TRNG_PDN_SET		(INFRACFG_AO_BASE + 0x0088)
+#define TRNG_PDN_CLR		(INFRACFG_AO_BASE + 0x008C)
+#define TRNG_PDN_STATUS		(INFRACFG_AO_BASE + 0x0094)
+#define TRNG_PDN_VALUE		0x200
+
+#endif /* RNG_PLAT_H */
diff --git a/plat/mediatek/drivers/rng/mt8188/rng_plat.c b/plat/mediatek/drivers/rng/mt8188/rng_plat.c
new file mode 100644
index 0000000..361be22
--- /dev/null
+++ b/plat/mediatek/drivers/rng/mt8188/rng_plat.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2024, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/smccc.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+#include <services/trng_svc.h>
+#include <smccc_helpers.h>
+
+#include "rng_plat.h"
+
+static void trng_external_swrst(void)
+{
+	/* External swrst to reset whole rng module */
+	mmio_setbits_32(TRNG_SWRST_SET_REG, RNG_SWRST_B);
+	mmio_setbits_32(TRNG_SWRST_CLR_REG, RNG_SWRST_B);
+
+	/* Disable irq */
+	mmio_clrbits_32(RNG_IRQ_CFG, IRQ_EN);
+	/* Set default cutoff value */
+	mmio_write_32(RNG_HTEST, RNG_DEFAULT_CUTOFF);
+	/* Enable rng */
+	mmio_setbits_32(RNG_EN, DRBG_EN | NRBG_EN);
+}
+
+static bool get_entropy_32(uint32_t *out)
+{
+	uint64_t time = timeout_init_us(MTK_TIMEOUT_POLL);
+	int retry_times = 0;
+
+	while (!(mmio_read_32(RNG_STATUS) & DRBG_VALID)) {
+		if (mmio_read_32(RNG_STATUS) & (RNG_ERROR | APB_ERROR)) {
+			mmio_clrbits_32(RNG_EN, DRBG_EN | NRBG_EN);
+
+			mmio_clrbits_32(RNG_SWRST, SWRST_B);
+			mmio_setbits_32(RNG_SWRST, SWRST_B);
+
+			mmio_setbits_32(RNG_EN, DRBG_EN | NRBG_EN);
+		}
+
+		if (timeout_elapsed(time)) {
+			trng_external_swrst();
+			time = timeout_init_us(MTK_TIMEOUT_POLL);
+			retry_times++;
+		}
+
+		if (retry_times > MTK_RETRY_CNT) {
+			ERROR("%s: trng NOT ready\n", __func__);
+			return false;
+		}
+	}
+
+	*out = mmio_read_32(RNG_OUT);
+
+	return true;
+}
+
+/* Get random number from HWRNG and return 8 bytes of entropy.
+ * Return 'true' when random value generated successfully, otherwise return
+ * 'false'.
+ */
+bool plat_get_entropy(uint64_t *out)
+{
+	uint32_t seed[2] = { 0 };
+	int i = 0;
+
+	assert(out);
+	assert(!check_uptr_overflow((uintptr_t)out, sizeof(*out)));
+
+	/* Disable interrupt mode */
+	mmio_clrbits_32(RNG_IRQ_CFG, IRQ_EN);
+	/* Set rng health test cutoff value */
+	mmio_write_32(RNG_HTEST, RNG_DEFAULT_CUTOFF);
+	/* Enable rng module */
+	mmio_setbits_32(RNG_EN, DRBG_EN | NRBG_EN);
+
+	for (i = 0; i < ARRAY_SIZE(seed); i++) {
+		if (!get_entropy_32(&seed[i]))
+			return false;
+	}
+
+	/* Output 8 bytes entropy by combining 2 32-bit random numbers. */
+	*out = ((uint64_t)seed[0] << 32) | seed[1];
+
+	return true;
+}
diff --git a/plat/mediatek/drivers/rng/mt8188/rng_plat.h b/plat/mediatek/drivers/rng/mt8188/rng_plat.h
new file mode 100644
index 0000000..37ef271
--- /dev/null
+++ b/plat/mediatek/drivers/rng/mt8188/rng_plat.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2024, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RNG_PLAT_H
+#define RNG_PLAT_H
+
+#include <lib/utils_def.h>
+
+#define MTK_TIMEOUT_POLL	1000
+
+#define MTK_RETRY_CNT		10
+
+#define RNG_DEFAULT_CUTOFF	0x04871C0B
+
+/*******************************************************************************
+ * TRNG related constants
+ ******************************************************************************/
+#define RNG_STATUS		(TRNG_BASE + 0x0004)
+#define RNG_SWRST		(TRNG_BASE + 0x0010)
+#define RNG_IRQ_CFG		(TRNG_BASE + 0x0014)
+#define RNG_EN			(TRNG_BASE + 0x0020)
+#define RNG_HTEST		(TRNG_BASE + 0x0028)
+#define RNG_OUT			(TRNG_BASE + 0x0030)
+#define RNG_RAW			(TRNG_BASE + 0x0038)
+#define RNG_SRC			(TRNG_BASE + 0x0050)
+
+#define RAW_VALID		BIT(12)
+#define DRBG_VALID		BIT(4)
+#define RAW_EN			BIT(8)
+#define NRBG_EN			BIT(4)
+#define DRBG_EN			BIT(0)
+#define IRQ_EN			BIT(0)
+#define SWRST_B			BIT(0)
+/* Error conditions */
+#define RNG_ERROR		GENMASK_32(28, 24)
+#define APB_ERROR		BIT(16)
+
+/* External swrst */
+#define TRNG_SWRST_SET_REG	(INFRACFG_AO_BASE + 0x150)
+#define TRNG_SWRST_CLR_REG	(INFRACFG_AO_BASE + 0x154)
+#define RNG_SWRST_B		BIT(13)
+
+#endif /* RNG_PLAT_H */
diff --git a/plat/mediatek/drivers/rng/rng.c b/plat/mediatek/drivers/rng/rng.c
new file mode 100644
index 0000000..d611168
--- /dev/null
+++ b/plat/mediatek/drivers/rng/rng.c
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2024, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <lib/smccc.h>
+#include <plat/common/plat_trng.h>
+
+#include <mtk_sip_svc.h>
+
+DEFINE_SVC_UUID2(_plat_trng_uuid,
+	0xf6b2c8d9, 0x1abb, 0x4d83, 0xb2, 0x3f,
+	0x5c, 0x51, 0xb6, 0xef, 0xfc, 0xaf
+);
+uuid_t plat_trng_uuid;
+
+void plat_entropy_setup(void)
+{
+	uint64_t placeholder;
+
+	plat_trng_uuid = _plat_trng_uuid;
+
+	/* Initialise the entropy source and trigger RNG generation */
+	plat_get_entropy(&placeholder);
+}
diff --git a/plat/mediatek/drivers/rng/rules.mk b/plat/mediatek/drivers/rng/rules.mk
new file mode 100644
index 0000000..5bcd2cd
--- /dev/null
+++ b/plat/mediatek/drivers/rng/rules.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2024, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := rng
+
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/rng.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/${MTK_SOC}/rng_plat.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/mt8188/include/plat_helpers.h b/plat/mediatek/include/plat_helpers.h
similarity index 71%
rename from plat/mediatek/mt8188/include/plat_helpers.h
rename to plat/mediatek/include/plat_helpers.h
index eb78623..b86ed23 100644
--- a/plat/mediatek/mt8188/include/plat_helpers.h
+++ b/plat/mediatek/include/plat_helpers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2024, Mediatek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/mt8186/include/plat_helpers.h b/plat/mediatek/mt8186/include/plat_helpers.h
deleted file mode 100644
index ebc9fa0..0000000
--- a/plat/mediatek/mt8186/include/plat_helpers.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef __PLAT_HELPERS_H__
-#define __PLAT_HELPERS_H__
-
-unsigned int plat_mediatek_calc_core_pos(u_register_t mpidr);
-
-#endif /* __PLAT_HELPERS_H__ */
diff --git a/plat/mediatek/mt8186/include/platform_def.h b/plat/mediatek/mt8186/include/platform_def.h
index 98b88bd..707f4a5 100644
--- a/plat/mediatek/mt8186/include/platform_def.h
+++ b/plat/mediatek/mt8186/include/platform_def.h
@@ -77,6 +77,11 @@
 #define MSDC0_BASE		(IO_PHYS + 0x01230000)
 
 /*******************************************************************************
+ * TRNG related constants
+ ******************************************************************************/
+#define TRNG_BASE		(IO_PHYS + 0x0020F000)
+
+/*******************************************************************************
  * GIC-600 & interrupt handling related constants
  ******************************************************************************/
 /* Base MTK_platform compatible GIC memory map */
diff --git a/plat/mediatek/mt8186/platform.mk b/plat/mediatek/mt8186/platform.mk
index 2bd2fb4..9c03340 100644
--- a/plat/mediatek/mt8186/platform.mk
+++ b/plat/mediatek/mt8186/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2021-2023, MediaTek Inc. All rights reserved.
+# Copyright (c) 2021-2024, MediaTek Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -7,6 +7,9 @@
 MTK_PLAT     := plat/mediatek
 MTK_PLAT_SOC := ${MTK_PLAT}/${PLAT}
 
+# True Random Number Generator firmware Interface
+TRNG_SUPPORT := 1
+
 PLAT_INCLUDES := -I${MTK_PLAT}/common/                            \
                  -I${MTK_PLAT}/drivers/cirq/                      \
                  -I${MTK_PLAT}/drivers/gic600/                    \
@@ -79,6 +82,11 @@
                 ${MTK_PLAT_SOC}/plat_sip_calls.c                      \
                 ${MTK_PLAT_SOC}/plat_topology.c
 
+ifeq (${TRNG_SUPPORT},1)
+BL31_SOURCES += ${MTK_PLAT}/drivers/rng/rng.c                         \
+                ${MTK_PLAT}/drivers/rng/${PLAT}/rng_plat.c
+endif
+
 # Build SPM drivers
 include ${MTK_PLAT_SOC}/drivers/spm/build.mk
 
diff --git a/plat/mediatek/mt8188/include/platform_def.h b/plat/mediatek/mt8188/include/platform_def.h
index 8e0f5f9..dccb052 100644
--- a/plat/mediatek/mt8188/include/platform_def.h
+++ b/plat/mediatek/mt8188/include/platform_def.h
@@ -190,6 +190,11 @@
 #define SUB_EMI_MPU_BASE	(IO_PHYS + 0x00225000)
 
 /*******************************************************************************
+ * TRNG related constants
+ ******************************************************************************/
+#define TRNG_BASE		(IO_PHYS + 0x0020F000)
+
+/*******************************************************************************
  * System counter frequency related constants
  ******************************************************************************/
 #define SYS_COUNTER_FREQ_IN_HZ	(13000000)
diff --git a/plat/mediatek/mt8188/plat_config.mk b/plat/mediatek/mt8188/plat_config.mk
index 2e3392f..82ef7e8 100644
--- a/plat/mediatek/mt8188/plat_config.mk
+++ b/plat/mediatek/mt8188/plat_config.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2022-2023, MediaTek Inc. All rights reserved.
+# Copyright (c) 2022-2024, MediaTek Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -46,5 +46,8 @@
 CPU_PM_TINYSYS_SUPPORT := y
 MTK_PUBEVENT_ENABLE := y
 
+# True Random Number Generator firmware Interface
+TRNG_SUPPORT := 1
+
 MACH_MT8188 := 1
 $(eval $(call add_define,MACH_MT8188))
diff --git a/plat/mediatek/mt8188/platform.mk b/plat/mediatek/mt8188/platform.mk
index 5096e15..b776447 100644
--- a/plat/mediatek/mt8188/platform.mk
+++ b/plat/mediatek/mt8188/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2022-2023, MediaTek Inc. All rights reserved.
+# Copyright (c) 2022-2024, MediaTek Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -39,6 +39,9 @@
 MODULES-y += $(MTK_PLAT)/drivers/pmic
 MODULES-y += $(MTK_PLAT)/drivers/pmic_wrap
 MODULES-y += $(MTK_PLAT)/drivers/ptp3
+ifeq (${TRNG_SUPPORT},1)
+MODULES-y += $(MTK_PLAT)/drivers/rng
+endif
 MODULES-y += $(MTK_PLAT)/drivers/rtc
 MODULES-y += $(MTK_PLAT)/drivers/spm
 MODULES-y += $(MTK_PLAT)/drivers/timer
diff --git a/plat/mediatek/mt8192/include/plat_helpers.h b/plat/mediatek/mt8192/include/plat_helpers.h
deleted file mode 100644
index 9b550ee..0000000
--- a/plat/mediatek/mt8192/include/plat_helpers.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef __PLAT_HELPERS_H__
-#define __PLAT_HELPERS_H__
-
-unsigned int plat_mediatek_calc_core_pos(u_register_t mpidr);
-
-#endif /* __PLAT_HELPERS_H__ */
diff --git a/plat/mediatek/mt8195/include/plat_helpers.h b/plat/mediatek/mt8195/include/plat_helpers.h
deleted file mode 100644
index ebc9fa0..0000000
--- a/plat/mediatek/mt8195/include/plat_helpers.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef __PLAT_HELPERS_H__
-#define __PLAT_HELPERS_H__
-
-unsigned int plat_mediatek_calc_core_pos(u_register_t mpidr);
-
-#endif /* __PLAT_HELPERS_H__ */
diff --git a/plat/rockchip/common/include/plat_macros.S b/plat/rockchip/common/include/plat_macros.S
index 691beeb..548e3d9 100644
--- a/plat/rockchip/common/include/plat_macros.S
+++ b/plat/rockchip/common/include/plat_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2024, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -23,8 +23,8 @@
 
 /* Registers common to both GICv2 and GICv3 */
 gicd_pend_reg:
-	.asciz "gicd_ispendr regs (Offsets 0x200 - 0x278)\n"	\
-		" Offset:\t\t\tvalue\n"
+	.ascii "gicd_ispendr regs (Offsets 0x200 - 0x278)\n"	\
+		" Offset:\t\t\tvalue\n\0"
 newline:
 	.asciz "\n"
 spacer:
diff --git a/plat/st/common/common_rules.mk b/plat/st/common/common_rules.mk
index fba7783..9070a16 100644
--- a/plat/st/common/common_rules.mk
+++ b/plat/st/common/common_rules.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2023, STMicroelectronics - All Rights Reserved
+# Copyright (c) 2023-2024, STMicroelectronics - All Rights Reserved
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -58,9 +58,9 @@
 tf-a-%.elf: $(PLAT)-%.o ${STM32_TF_LINKERFILE}
 	$(s)echo "  LDS     $<"
 ifeq ($($(ARCH)-ld-id),gnu-gcc)
-	$(q)$($(ARCH)-ld) -o $@ $(subst --,-Wl$(comma)--,${STM32_TF_ELF_LDFLAGS}) -nostartfiles -Wl,-Map=$(@:.elf=.map) -Wl,-dT ${STM32_TF_LINKERFILE} $<
+	$(q)$($(ARCH)-ld) -o $@ $(subst --,-Wl$(comma)--,${STM32_TF_ELF_LDFLAGS}) -nostartfiles -no-pie -Wl,-Map=$(@:.elf=.map) -Wl,-dT ${STM32_TF_LINKERFILE} $<
 else
-	$(q)$($(ARCH)-ld) -o $@ ${STM32_TF_ELF_LDFLAGS} -Map=$(@:.elf=.map) --script ${STM32_TF_LINKERFILE} $<
+	$(q)$($(ARCH)-ld) -o $@ ${STM32_TF_ELF_LDFLAGS} -no-pie -Map=$(@:.elf=.map) --script ${STM32_TF_LINKERFILE} $<
 endif
 
 tf-a-%.bin: tf-a-%.elf
diff --git a/plat/st/stm32mp2/include/platform_def.h b/plat/st/stm32mp2/include/platform_def.h
index b98b56d..e720c02 100644
--- a/plat/st/stm32mp2/include/platform_def.h
+++ b/plat/st/stm32mp2/include/platform_def.h
@@ -33,7 +33,7 @@
 #define PLATFORM_CORE_COUNT		U(2)
 #define PLATFORM_MAX_CPUS_PER_CLUSTER	U(2)
 
-#define PLAT_MAX_PWR_LVL		U(3)
+#define PLAT_MAX_PWR_LVL		U(1)
 #define PLAT_MIN_SUSPEND_PWR_LVL	U(2)
 #define PLAT_NUM_PWR_DOMAINS		U(6)
 
diff --git a/plat/st/stm32mp2/plat_bl2_mem_params_desc.c b/plat/st/stm32mp2/plat_bl2_mem_params_desc.c
index 2513180..8ca582e 100644
--- a/plat/st/stm32mp2/plat_bl2_mem_params_desc.c
+++ b/plat/st/stm32mp2/plat_bl2_mem_params_desc.c
@@ -10,6 +10,12 @@
 
 #include <platform_def.h>
 
+#if STM32MP_BL33_EL1
+#define BL33_MODE MODE_EL1
+#else
+#define BL33_MODE MODE_EL2
+#endif
+
 /*******************************************************************************
  * Following descriptor provides BL image/ep information that gets used
  * by BL2 to load the images and also subset of this information is
@@ -138,7 +144,7 @@
 				      VERSION_2, entry_point_info_t,
 				      NON_SECURE | EXECUTABLE),
 
-		.ep_info.spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS),
+		.ep_info.spsr = SPSR_64(BL33_MODE, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS),
 
 		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
 				      VERSION_2, image_info_t,
diff --git a/plat/st/stm32mp2/platform.mk b/plat/st/stm32mp2/platform.mk
index df1cacd..2a9b6fd 100644
--- a/plat/st/stm32mp2/platform.mk
+++ b/plat/st/stm32mp2/platform.mk
@@ -15,6 +15,15 @@
 PROGRAMMABLE_RESET_ADDRESS	:=	1
 BL2_IN_XIP_MEM			:=	1
 
+STM32MP_BL33_EL1		?=	1
+ifeq ($(STM32MP_BL33_EL1),1)
+INIT_UNUSED_NS_EL2		:=	1
+endif
+
+# Disable features unsupported in ARMv8.0
+ENABLE_SPE_FOR_NS		:=	0
+ENABLE_SVE_FOR_NS		:=	0
+
 # Default Device tree
 DTB_FILE_NAME			?=	stm32mp257f-ev1.dtb
 
@@ -83,6 +92,7 @@
 		STM32MP_DDR4_TYPE \
 		STM32MP_LPDDR4_TYPE \
 		STM32MP25 \
+		STM32MP_BL33_EL1 \
 )))
 
 $(eval $(call assert_numerics,\
@@ -105,6 +115,7 @@
 		STM32MP_DDR4_TYPE \
 		STM32MP_LPDDR4_TYPE \
 		STM32MP25 \
+		STM32MP_BL33_EL1 \
 )))
 
 # STM32MP2x is based on Cortex-A35, which is Armv8.0, and does not support BTI
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
index 8b77050..5c6ba6b 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
@@ -1740,3 +1740,41 @@
 
 	return 0;
 }
+
+/**
+ * ti_sci_lpm_get_next_sys_mode() - Get next LPM system mode
+ *
+ * @next_mode:	pointer to a variable that will store the next mode
+ *
+ * Return: 0 if all goes well, else appropriate error message
+ */
+int ti_sci_lpm_get_next_sys_mode(uint8_t *next_mode)
+{
+	struct ti_sci_msg_req_lpm_get_next_sys_mode req;
+	struct ti_sci_msg_resp_lpm_get_next_sys_mode resp;
+	struct ti_sci_xfer xfer;
+	int ret;
+
+	if (next_mode == NULL) {
+		return -EINVAL;
+	}
+
+	ret = ti_sci_setup_one_xfer(TI_SCI_MSG_LPM_GET_NEXT_SYS_MODE, 0,
+				    &req, sizeof(req),
+				    &resp, sizeof(resp),
+				    &xfer);
+	if (ret != 0) {
+		ERROR("Message alloc failed (%d)\n", ret);
+		return ret;
+	}
+
+	ret = ti_sci_do_xfer(&xfer);
+	if (ret != 0) {
+		ERROR("Transfer send failed (%d)\n", ret);
+		return ret;
+	}
+
+	*next_mode = resp.mode;
+
+	return 0;
+}
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.h b/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
index acaca4d..06d1f8d 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
@@ -252,6 +252,11 @@
  *		@mode: Low power mode to enter.
  *		@core_resume_addr: Address that core should be resumed from
  *				   after low power transition.
+ * - ti_sci_lpm_get_next_sys_mode - Get next LPM system mode
+ *
+ * @next_mode:	pointer to a variable that will store the next mode
+ *
+ * Return: 0 if all goes well, else appropriate error message
  *
  * NOTE: for all these functions, the following are generic in nature:
  * Returns 0 for successful request, else returns corresponding error message.
@@ -259,5 +264,6 @@
 int ti_sci_enter_sleep(uint8_t proc_id,
 		       uint8_t mode,
 		       uint64_t core_resume_addr);
+int ti_sci_lpm_get_next_sys_mode(uint8_t *next_mode);
 
 #endif /* TI_SCI_H */
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h b/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
index 7f1c368..cc71eac 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
@@ -31,6 +31,7 @@
 
 /* Low Power Mode Requests */
 #define TI_SCI_MSG_ENTER_SLEEP		0x0301
+#define TI_SCI_MSG_LPM_GET_NEXT_SYS_MODE 0x030d
 
 /* Clock requests */
 #define TI_SCI_MSG_SET_CLOCK_STATE	0x0100
@@ -133,6 +134,7 @@
  *		MSG_FLAG_CAPS_LPM_MCU_ONLY: MCU only LPM
  *		MSG_FLAG_CAPS_LPM_STANDBY: Standby LPM
  *		MSG_FLAG_CAPS_LPM_PARTIAL_IO: Partial IO in LPM
+ *		MSG_FLAG_CAPS_LPM_DM_MANAGED: LPM can be managed by DM
  *
  * Response to a generic message with message type TI_SCI_MSG_QUERY_FW_CAPS
  * providing currently available SOC/firmware capabilities. SoC that don't
@@ -145,6 +147,7 @@
 #define MSG_FLAG_CAPS_LPM_MCU_ONLY	TI_SCI_MSG_FLAG(2)
 #define MSG_FLAG_CAPS_LPM_STANDBY	TI_SCI_MSG_FLAG(3)
 #define MSG_FLAG_CAPS_LPM_PARTIAL_IO	TI_SCI_MSG_FLAG(4)
+#define MSG_FLAG_CAPS_LPM_DM_MANAGED	TI_SCI_MSG_FLAG(5)
 	uint64_t fw_caps;
 } __packed;
 
@@ -764,10 +767,35 @@
  */
 struct ti_sci_msg_req_enter_sleep {
 	struct ti_sci_msg_hdr hdr;
+#define MSG_VALUE_SLEEP_MODE_DEEP_SLEEP 0x0
 	uint8_t mode;
 	uint8_t processor_id;
 	uint32_t core_resume_lo;
 	uint32_t core_resume_hi;
 } __packed;
 
+/**
+ * struct ti_sci_msg_req_lpm_get_next_sys_mode - Request for TI_SCI_MSG_LPM_GET_NEXT_SYS_MODE.
+ *
+ * @hdr Generic Header
+ *
+ * This message is used to enquire DM for selected system wide low power mode.
+ */
+struct ti_sci_msg_req_lpm_get_next_sys_mode {
+	struct ti_sci_msg_hdr hdr;
+} __packed;
+
+/**
+ * struct ti_sci_msg_resp_lpm_get_next_sys_mode - Response for TI_SCI_MSG_LPM_GET_NEXT_SYS_MODE.
+ *
+ * @hdr Generic Header
+ * @mode The selected system wide low power mode.
+ *
+ * Note: If the mode selection is not yet locked, this API returns "not selected" mode.
+ */
+struct ti_sci_msg_resp_lpm_get_next_sys_mode {
+	struct ti_sci_msg_hdr hdr;
+	uint8_t mode;
+} __packed;
+
 #endif /* TI_SCI_PROTOCOL_H */
diff --git a/plat/ti/k3/common/k3_psci.c b/plat/ti/k3/common/k3_psci.c
index e8d73db..df49f48 100644
--- a/plat/ti/k3/common/k3_psci.c
+++ b/plat/ti/k3/common/k3_psci.c
@@ -234,7 +234,7 @@
 	return PSCI_E_SUCCESS;
 }
 
-static void k3_pwr_domain_suspend(const psci_power_state_t *target_state)
+static void k3_pwr_domain_suspend_to_mode(const psci_power_state_t *target_state, uint8_t mode)
 {
 	unsigned int core, proc_id;
 
@@ -247,7 +247,25 @@
 
 	k3_pwr_domain_off(target_state);
 
+	ti_sci_enter_sleep(proc_id, mode, k3_sec_entrypoint);
+}
+
+static void k3_pwr_domain_suspend_dm_managed(const psci_power_state_t *target_state)
+{
+	uint8_t mode = MSG_VALUE_SLEEP_MODE_DEEP_SLEEP;
+	int ret;
+
-	ti_sci_enter_sleep(proc_id, 0, k3_sec_entrypoint);
+	ret = ti_sci_lpm_get_next_sys_mode(&mode);
+	if (ret != 0) {
+		ERROR("Failed to fetch next system mode\n");
+	}
+
+	k3_pwr_domain_suspend_to_mode(target_state, mode);
+}
+
+static void k3_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	k3_pwr_domain_suspend_to_mode(target_state, MSG_VALUE_SLEEP_MODE_DEEP_SLEEP);
 }
 
 static void k3_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
@@ -301,6 +319,8 @@
 		k3_plat_psci_ops.pwr_domain_suspend = NULL;
 		k3_plat_psci_ops.pwr_domain_suspend_finish = NULL;
 		k3_plat_psci_ops.get_sys_suspend_power_state = NULL;
+	} else if (fw_caps & MSG_FLAG_CAPS_LPM_DM_MANAGED) {
+		k3_plat_psci_ops.pwr_domain_suspend = k3_pwr_domain_suspend_dm_managed;
 	}
 
 	*psci_ops = &k3_plat_psci_ops;
diff --git a/plat/xilinx/common/ipi.c b/plat/xilinx/common/ipi.c
index 399d283..d7c70f3 100644
--- a/plat/xilinx/common/ipi.c
+++ b/plat/xilinx/common/ipi.c
@@ -70,7 +70,7 @@
 {
 	int ret = 1;
 
-	if (remote >= ipi_total || local >= ipi_total) {
+	if ((remote >= ipi_total) || (local >= ipi_total)) {
 		ret = 0;
 	}
 
@@ -144,11 +144,11 @@
 	uint32_t status;
 
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_OBR_OFFSET);
-	if (status & IPI_BIT_MASK(remote)) {
+	if ((status & IPI_BIT_MASK(remote)) != 0U) {
 		ret |= IPI_MB_STATUS_SEND_PENDING;
 	}
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_ISR_OFFSET);
-	if (status & IPI_BIT_MASK(remote)) {
+	if ((status & IPI_BIT_MASK(remote)) != 0U) {
 		ret |= IPI_MB_STATUS_RECV_PENDING;
 	}
 
@@ -170,11 +170,11 @@
 
 	mmio_write_32(IPI_REG_BASE(local) + IPI_TRIG_OFFSET,
 		      IPI_BIT_MASK(remote));
-	if (is_blocking) {
+	if (is_blocking != 0U) {
 		do {
 			status = mmio_read_32(IPI_REG_BASE(local) +
 					      IPI_OBR_OFFSET);
-		} while (status & IPI_BIT_MASK(remote));
+		} while ((status & IPI_BIT_MASK(remote)) != 0U);
 	}
 }
 
diff --git a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
index 0ea51f0..9a0149b 100644
--- a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
+++ b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
@@ -94,7 +94,7 @@
 
 	/* Validate IPI mailbox access */
 	ret = ipi_mb_validate(ipi_local_id, ipi_remote_id, is_secure);
-	if (ret)
+	if (ret != 0)
 		SMC_RET1(handle, ret);
 
 	switch (GET_SMC_NUM(smc_fid)) {
@@ -128,7 +128,7 @@
 
 		enable_interrupt = (x3 & IPI_SMC_ACK_EIRQ_MASK) ? 1 : 0;
 		ipi_mb_ack(ipi_local_id, ipi_remote_id);
-		if (enable_interrupt)
+		if (enable_interrupt != 0)
 			ipi_mb_enable_irq(ipi_local_id, ipi_remote_id);
 		SMC_RET1(handle, 0);
 	}
diff --git a/plat/xilinx/common/plat_startup.c b/plat/xilinx/common/plat_startup.c
index 5beb765..149ba2d 100644
--- a/plat/xilinx/common/plat_startup.c
+++ b/plat/xilinx/common/plat_startup.c
@@ -237,8 +237,8 @@
 		}
 
 		target_secure = get_xbl_ss(&HandoffParams->partition[i]);
-		if (target_secure == XBL_FLAGS_SECURE &&
-		    target_el == XBL_FLAGS_EL2) {
+		if ((target_secure == XBL_FLAGS_SECURE) &&
+		    (target_el == XBL_FLAGS_EL2)) {
 			WARN("BL31: invalid security state (%i) for exception level (%i)\n",
 			     target_secure, target_el);
 			continue;
@@ -284,7 +284,7 @@
 		}
 
 		VERBOSE("Setting up %s entry point to:%" PRIx64 ", el:%x\n",
-			target_secure == XBL_FLAGS_SECURE ? "BL32" : "BL33",
+			(target_secure == XBL_FLAGS_SECURE) ? "BL32" : "BL33",
 			HandoffParams->partition[i].entry_point,
 			target_el);
 		image->pc = HandoffParams->partition[i].entry_point;
diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index 425fdcb..c3872fc 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c
@@ -217,6 +217,7 @@
 enum pm_ret_status pm_ipi_buff_read_callb(uint32_t *value, size_t count)
 {
 	size_t i;
+	size_t local_count = count;
 #if IPI_CRC_CHECK
 	uint32_t crc;
 #endif
@@ -225,8 +226,8 @@
 				IPI_BUFFER_REQ_OFFSET;
 	enum pm_ret_status ret = PM_RET_SUCCESS;
 
-	if (count > IPI_BUFFER_MAX_WORDS) {
-		count = IPI_BUFFER_MAX_WORDS;
+	if (local_count > IPI_BUFFER_MAX_WORDS) {
+		local_count = IPI_BUFFER_MAX_WORDS;
 	}
 
 	for (i = 0; i < count; i++) {
@@ -240,7 +241,7 @@
 		/* Payload data is invalid as CRC validation failed
 		 * Clear the payload to avoid leakage of data to upper layers
 		 */
-		memset(value, 0, count);
+		memset(value, 0, local_count);
 	}
 #endif
 	return ret;
diff --git a/plat/xilinx/common/pm_service/pm_svc_main.c b/plat/xilinx/common/pm_service/pm_svc_main.c
index 861c5b3..afb9a96 100644
--- a/plat/xilinx/common/pm_service/pm_svc_main.c
+++ b/plat/xilinx/common/pm_service/pm_svc_main.c
@@ -478,9 +478,9 @@
 	 * than other eemi calls.
 	 */
 	if (api_id == (uint32_t)PM_QUERY_DATA) {
-		if ((pm_arg[0] == XPM_QID_CLOCK_GET_NAME ||
-		    pm_arg[0] == XPM_QID_PINCTRL_GET_FUNCTION_NAME) &&
-		    ret == PM_RET_SUCCESS) {
+		if (((pm_arg[0] == XPM_QID_CLOCK_GET_NAME) ||
+		    (pm_arg[0] == XPM_QID_PINCTRL_GET_FUNCTION_NAME)) &&
+		    (ret == PM_RET_SUCCESS)) {
 			SMC_RET2(handle, (uint64_t)buf[0] | ((uint64_t)buf[1] << 32U),
 				(uint64_t)buf[2] | ((uint64_t)buf[3] << 32U));
 		}
@@ -559,7 +559,7 @@
 	uint32_t security_flag = NON_SECURE_FLAG;
 	uint32_t api_id;
 	bool status = false, status_tmp = false;
-	uint64_t x[4] = {x1, x2, x3, x4};
+	const uint64_t x[4] = {x1, x2, x3, x4};
 
 	/* Handle case where PM wasn't initialized properly */
 	if (pm_up == false) {
diff --git a/plat/xilinx/versal/bl31_versal_setup.c b/plat/xilinx/versal/bl31_versal_setup.c
index 58589ad..819a55b 100644
--- a/plat/xilinx/versal/bl31_versal_setup.c
+++ b/plat/xilinx/versal/bl31_versal_setup.c
@@ -76,7 +76,7 @@
 	uint64_t tfa_handoff_addr;
 	uint32_t payload[PAYLOAD_ARG_CNT], max_size = HANDOFF_PARAMS_MAX_SIZE;
 	enum pm_ret_status ret_status;
-	uint64_t addr[HANDOFF_PARAMS_MAX_SIZE];
+	const uint64_t addr[HANDOFF_PARAMS_MAX_SIZE];
 
 	/*
 	 * Do initial security configuration to allow DRAM/device access. On
@@ -131,7 +131,7 @@
 	enum xbl_handoff ret = xbl_handover(&bl32_image_ep_info,
 						  &bl33_image_ep_info,
 						  tfa_handoff_addr);
-	if (ret == XBL_HANDOFF_NO_STRUCT || ret == XBL_HANDOFF_INVAL_STRUCT) {
+	if ((ret == XBL_HANDOFF_NO_STRUCT) || (ret == XBL_HANDOFF_INVAL_STRUCT)) {
 		bl31_set_default_config();
 	} else if (ret == XBL_HANDOFF_TOO_MANY_PARTS) {
 		ERROR("BL31: Error too many partitions %u\n", ret);
@@ -153,7 +153,7 @@
 	uint32_t i;
 
 	/* Validate 'handler' and 'id' parameters */
-	if (handler == NULL || index >= MAX_INTR_EL3) {
+	if ((handler == NULL) || (index >= MAX_INTR_EL3)) {
 		return -EINVAL;
 	}
 
diff --git a/plat/xilinx/versal/plat_psci.c b/plat/xilinx/versal/plat_psci.c
index a299d14..7571e9d 100644
--- a/plat/xilinx/versal/plat_psci.c
+++ b/plat/xilinx/versal/plat_psci.c
@@ -77,7 +77,7 @@
 		plat_versal_gic_save();
 	}
 
-	state = target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE ?
+	state = (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) ?
 		PM_STATE_SUSPEND_TO_RAM : PM_STATE_CPU_IDLE;
 
 	/* Send request to PMC to suspend this core */
@@ -146,7 +146,7 @@
 	(void)pm_system_shutdown(XPM_SHUTDOWN_TYPE_SHUTDOWN,
 				 pm_get_shutdown_scope(), SECURE_FLAG);
 
-	while (1) {
+	while (true) {
 		wfi();
 	}
 }
@@ -185,7 +185,7 @@
 
 	(void)psci_cpu_off();
 
-	while (1) {
+	while (true) {
 		wfi();
 	}
 }
diff --git a/plat/xilinx/versal/sip_svc_setup.c b/plat/xilinx/versal/sip_svc_setup.c
index 3c0bd63..d449f74 100644
--- a/plat/xilinx/versal/sip_svc_setup.c
+++ b/plat/xilinx/versal/sip_svc_setup.c
@@ -80,7 +80,7 @@
 	VERBOSE("SMCID: 0x%08x, x1: 0x%016" PRIx64 ", x2: 0x%016" PRIx64 ", x3: 0x%016" PRIx64 ", x4: 0x%016" PRIx64 "\n",
 		smc_fid, x1, x2, x3, x4);
 
-	if (smc_fid & SIP_FID_MASK) {
+	if ((smc_fid & SIP_FID_MASK) != 0U) {
 		WARN("SMC out of SiP assinged range: 0x%x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);
 	}
diff --git a/plat/xilinx/versal_net/bl31_versal_net_setup.c b/plat/xilinx/versal_net/bl31_versal_net_setup.c
index ebde49f..cf2368a 100644
--- a/plat/xilinx/versal_net/bl31_versal_net_setup.c
+++ b/plat/xilinx/versal_net/bl31_versal_net_setup.c
@@ -179,7 +179,7 @@
 	uint32_t i;
 
 	/* Validate 'handler' and 'id' parameters */
-	if (handler == NULL || index >= MAX_INTR_EL3) {
+	if ((handler == NULL) || (index >= MAX_INTR_EL3)) {
 		return -EINVAL;
 	}
 
@@ -214,7 +214,7 @@
 	}
 
 	if (handler != NULL) {
-		handler(intr_id, flags, handle, cookie);
+		(void)handler(intr_id, flags, handle, cookie);
 	}
 
 	return 0;
diff --git a/plat/xilinx/versal_net/plat_psci_pm.c b/plat/xilinx/versal_net/plat_psci_pm.c
index 7a653d4..a2bf0d9 100644
--- a/plat/xilinx/versal_net/plat_psci_pm.c
+++ b/plat/xilinx/versal_net/plat_psci_pm.c
@@ -42,7 +42,7 @@
 		return PSCI_E_INTERN_FAIL;
 	}
 
-	pm_req_wakeup(proc->node_id, (versal_net_sec_entry & 0xFFFFFFFFU) | 0x1U,
+	(void)pm_req_wakeup(proc->node_id, (versal_net_sec_entry & 0xFFFFFFFFU) | 0x1U,
 		      versal_net_sec_entry >> 32, 0, 0);
 
 	/* Clear power down request */
@@ -130,7 +130,7 @@
 
 	(void)psci_cpu_off();
 
-	while (1) {
+	while (true) {
 		wfi();
 	}
 }
@@ -162,11 +162,11 @@
 		plat_arm_gic_save();
 	}
 
-	state = target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE ?
+	state = (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) ?
 		PM_STATE_SUSPEND_TO_RAM : PM_STATE_CPU_IDLE;
 
 	/* Send request to PMC to suspend this core */
-	pm_self_suspend(proc->node_id, MAX_LATENCY, state, versal_net_sec_entry,
+	(void)pm_self_suspend(proc->node_id, MAX_LATENCY, state, versal_net_sec_entry,
 			SECURE_FLAG);
 
 	/* TODO: disable coherency */
@@ -223,10 +223,10 @@
 static void __dead2 versal_net_system_off(void)
 {
 	/* Send the power down request to the PMC */
-	pm_system_shutdown(XPM_SHUTDOWN_TYPE_SHUTDOWN,
+	(void)pm_system_shutdown(XPM_SHUTDOWN_TYPE_SHUTDOWN,
 			  pm_get_shutdown_scope(), SECURE_FLAG);
 
-	while (1) {
+	while (true) {
 		wfi();
 	}
 }
@@ -257,7 +257,7 @@
 	}
 
 	/* We expect the 'state id' to be zero */
-	if (psci_get_pstate_id(power_state)) {
+	if (psci_get_pstate_id(power_state) != 0U) {
 		return PSCI_E_INVALID_PARAMS;
 	}
 
diff --git a/plat/xilinx/versal_net/sip_svc_setup.c b/plat/xilinx/versal_net/sip_svc_setup.c
index c974810..bf06e2c 100644
--- a/plat/xilinx/versal_net/sip_svc_setup.c
+++ b/plat/xilinx/versal_net/sip_svc_setup.c
@@ -69,7 +69,7 @@
 	VERBOSE("SMCID: 0x%08x, x1: 0x%016" PRIx64 ", x2: 0x%016" PRIx64 ", x3: 0x%016" PRIx64 ", x4: 0x%016" PRIx64 "\n",
 		smc_fid, x1, x2, x3, x4);
 
-	if (smc_fid & SIP_FID_MASK) {
+	if ((smc_fid & SIP_FID_MASK) != 0U) {
 		WARN("SMC out of SiP assinged range: 0x%x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);
 	}
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index b0bd8a1..8b902c8 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -244,8 +244,8 @@
 	ver = chipid[1] >> ZYNQMP_EFUSE_IPDISABLE_SHIFT;
 
 	for (i = 0; i < ARRAY_SIZE(zynqmp_devices); i++) {
-		if (zynqmp_devices[i].id == id &&
-		    zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK)) {
+		if ((zynqmp_devices[i].id == id) &&
+		    (zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK))) {
 			break;
 		}
 	}
@@ -299,8 +299,8 @@
 	tmp = id;
 	tmp &= ZYNQMP_CSU_IDCODE_XILINX_ID_MASK |
 	       ZYNQMP_CSU_IDCODE_FAMILY_MASK;
-	maskid = ZYNQMP_CSU_IDCODE_XILINX_ID << ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT |
-		 ZYNQMP_CSU_IDCODE_FAMILY << ZYNQMP_CSU_IDCODE_FAMILY_SHIFT;
+	maskid = (ZYNQMP_CSU_IDCODE_XILINX_ID << ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT) |
+		 (ZYNQMP_CSU_IDCODE_FAMILY << ZYNQMP_CSU_IDCODE_FAMILY_SHIFT);
 	if (tmp != maskid) {
 		ERROR("Incorrect IDCODE 0x%x, maskid 0x%x\n", id, maskid);
 		return "UNKN";
@@ -348,7 +348,7 @@
 {
 	uint32_t ver = zynqmp_get_silicon_ver();
 	uint32_t rtl = zynqmp_get_rtl_ver();
-	char *label = "Unknown";
+	const char *label = "Unknown";
 
 	switch (ver) {
 	case ZYNQMP_CSU_VERSION_QEMU:
diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index 1e7df05..58db2e4 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -58,7 +58,7 @@
 	pm_client_wakeup(proc);
 
 	/* Send request to PMU to wake up selected APU CPU core */
-	pm_req_wakeup(proc->node_id, 1, zynqmp_sec_entry, REQ_ACK_BLOCKING);
+	(void)pm_req_wakeup(proc->node_id, 1, zynqmp_sec_entry, REQ_ACK_BLOCKING);
 
 	return PSCI_E_SUCCESS;
 }
@@ -88,7 +88,7 @@
 	 * invoking CPU_on function, during which resume address will
 	 * be set.
 	 */
-	pm_self_suspend(proc->node_id, MAX_LATENCY, PM_STATE_CPU_IDLE, 0);
+	(void)pm_self_suspend(proc->node_id, MAX_LATENCY, PM_STATE_CPU_IDLE, 0);
 }
 
 static void zynqmp_pwr_domain_suspend(const psci_power_state_t *target_state)
@@ -105,11 +105,11 @@
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
 
-	state = target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE ?
+	state = (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) ?
 		PM_STATE_SUSPEND_TO_RAM : PM_STATE_CPU_IDLE;
 
 	/* Send request to PMU to suspend this core */
-	pm_self_suspend(proc->node_id, MAX_LATENCY, state, zynqmp_sec_entry);
+	(void)pm_self_suspend(proc->node_id, MAX_LATENCY, state, zynqmp_sec_entry);
 
 	/* APU is to be turned off */
 	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
@@ -166,10 +166,10 @@
 	plat_arm_interconnect_exit_coherency();
 
 	/* Send the power down request to the PMU */
-	pm_system_shutdown(PMF_SHUTDOWN_TYPE_SHUTDOWN,
+	(void)pm_system_shutdown((uint32_t)PMF_SHUTDOWN_TYPE_SHUTDOWN,
 			   pm_get_shutdown_scope());
 
-	while (1) {
+	while (true) {
 		wfi();
 	}
 }
@@ -180,10 +180,10 @@
 	plat_arm_interconnect_exit_coherency();
 
 	/* Send the system reset request to the PMU */
-	pm_system_shutdown(PMF_SHUTDOWN_TYPE_RESET,
+	(void)pm_system_shutdown((uint32_t)PMF_SHUTDOWN_TYPE_RESET,
 			   pm_get_shutdown_scope());
 
-	while (1) {
+	while (true) {
 		wfi();
 	}
 }
@@ -204,7 +204,7 @@
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_OFF_STATE;
 	}
 	/* We expect the 'state id' to be zero */
-	if (psci_get_pstate_id(power_state)) {
+	if (psci_get_pstate_id(power_state) != 0U) {
 		return PSCI_E_INVALID_PARAMS;
 	}
 
diff --git a/plat/xilinx/zynqmp/plat_zynqmp.c b/plat/xilinx/zynqmp/plat_zynqmp.c
index e3a979e..65faa2f 100644
--- a/plat/xilinx/zynqmp/plat_zynqmp.c
+++ b/plat/xilinx/zynqmp/plat_zynqmp.c
@@ -10,7 +10,7 @@
 
 int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
 {
-	if (mpidr & MPIDR_CLUSTER_MASK) {
+	if ((mpidr & MPIDR_CLUSTER_MASK) != 0U) {
 		return -1;
 	}
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
index 5a1e218..91adb07 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
@@ -1226,7 +1226,7 @@
 		.control_reg = CRF_APB_ACPU_CTRL,
 		.status_reg = 0,
 		.parents = &((int32_t []) {
-			CLK_ACPU | PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN,
+			(CLK_ACPU | (PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN)),
 			CLK_NA_PARENT
 		}),
 		.nodes = &acpu_full_nodes,
@@ -2117,7 +2117,7 @@
 		.control_reg = CRF_APB_ACPU_CTRL,
 		.status_reg = 0,
 		.parents = &((int32_t []) {
-			CLK_ACPU | PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN,
+			(CLK_ACPU | (PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN)),
 			CLK_NA_PARENT
 		}),
 		.nodes = &acpu_half_nodes,
@@ -2140,7 +2140,7 @@
 		.control_reg = CRF_APB_GPU_REF_CTRL,
 		.status_reg = 0,
 		.parents = &((int32_t []) {
-			CLK_GPU_REF | PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN,
+			(CLK_GPU_REF | (PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN)),
 			CLK_NA_PARENT
 		}),
 		.nodes = &gpu_pp0_nodes,
@@ -2151,7 +2151,7 @@
 		.control_reg = CRF_APB_GPU_REF_CTRL,
 		.status_reg = 0,
 		.parents = &((int32_t []) {
-			CLK_GPU_REF | PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN,
+			(CLK_GPU_REF | (PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN)),
 			CLK_NA_PARENT
 		}),
 		.nodes = &gpu_pp1_nodes,
@@ -2176,7 +2176,7 @@
 		.control_reg = CRL_APB_CPU_R5_CTRL,
 		.status_reg = 0,
 		.parents = &((int32_t []) {
-			CLK_CPU_R5 | PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN,
+			(CLK_CPU_R5 | (PARENT_CLK_NODE2 << CLK_PARENTS_ID_LEN)),
 			CLK_DUMMY_PARENT,
 			CLK_NA_PARENT
 		}),
@@ -2456,14 +2456,14 @@
 void pm_api_clock_get_name(uint32_t clock_id, char *name)
 {
 	if (clock_id == CLK_MAX) {
-		memcpy(name, END_OF_CLK, sizeof(END_OF_CLK) > CLK_NAME_LEN ?
-					 CLK_NAME_LEN : sizeof(END_OF_CLK));
+		(void)memcpy(name, END_OF_CLK, ((sizeof(END_OF_CLK) > CLK_NAME_LEN) ?
+					 CLK_NAME_LEN : sizeof(END_OF_CLK)));
 	} else if ((clock_id > CLK_MAX) || (!pm_clock_valid(clock_id))) {
-		memset(name, 0, CLK_NAME_LEN);
+		(void)memset(name, 0, CLK_NAME_LEN);
 	} else if (clock_id < CLK_MAX_OUTPUT_CLK) {
-		memcpy(name, clocks[clock_id].name, CLK_NAME_LEN);
+		(void)memcpy(name, clocks[clock_id].name, CLK_NAME_LEN);
 	} else {
-		memcpy(name, ext_clocks[clock_id - CLK_MAX_OUTPUT_CLK].name,
+		(void)memcpy(name, ext_clocks[clock_id - CLK_MAX_OUTPUT_CLK].name,
 		       CLK_NAME_LEN);
 	}
 }
@@ -2486,7 +2486,7 @@
 					     uint32_t index,
 					     uint32_t *topology)
 {
-	struct pm_clock_node *clock_nodes;
+	const struct pm_clock_node *clock_nodes;
 	uint8_t num_nodes;
 	uint32_t i;
 	uint16_t typeflags;
@@ -2499,7 +2499,7 @@
 		return PM_RET_ERROR_NOTSUPPORTED;
 	}
 
-	memset(topology, 0, CLK_TOPOLOGY_PAYLOAD_LEN);
+	(void)memset(topology, 0, CLK_TOPOLOGY_PAYLOAD_LEN);
 	clock_nodes = *clocks[clock_id].nodes;
 	num_nodes = clocks[clock_id].num_nodes;
 
@@ -2543,7 +2543,7 @@
 						       uint32_t *mul,
 						       uint32_t *div)
 {
-	struct pm_clock_node *clock_nodes;
+	const struct pm_clock_node *clock_nodes;
 	uint8_t num_nodes;
 	uint32_t type, i;
 
@@ -2598,7 +2598,7 @@
 					    uint32_t *parents)
 {
 	uint32_t i;
-	int32_t *clk_parents;
+	const int32_t *clk_parents;
 
 	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
@@ -2613,7 +2613,7 @@
 		return PM_RET_ERROR_ARGS;
 	}
 
-	memset(parents, 0, CLK_PARENTS_PAYLOAD_LEN);
+	(void)memset(parents, 0, CLK_PARENTS_PAYLOAD_LEN);
 
 	/* Skip parent till index */
 	for (i = 0; i < index; i++) {
@@ -2675,7 +2675,7 @@
 						uint32_t *max_div)
 {
 	uint32_t i;
-	struct pm_clock_node *nodes;
+	const struct pm_clock_node *nodes;
 
 	if (clock_id >= CLK_MAX_OUTPUT_CLK) {
 		return PM_RET_ERROR_ARGS;
@@ -2684,8 +2684,8 @@
 	nodes = *clocks[clock_id].nodes;
 	for (i = 0; i < clocks[clock_id].num_nodes; i++) {
 		if (nodes[i].type == div_type) {
-			if (CLK_DIVIDER_POWER_OF_TWO &
-					nodes[i].typeflags) {
+			if ((CLK_DIVIDER_POWER_OF_TWO &
+					nodes[i].typeflags) != 0U) {
 				*max_div = (1U << (BIT(nodes[i].width) - 1U));
 			} else {
 				*max_div = BIT(nodes[i].width) - 1U;
@@ -2789,7 +2789,7 @@
 enum pm_ret_status pm_clock_get_pll_node_id(enum clock_id clock_id,
 					    enum pm_node_id *node_id)
 {
-	struct pm_pll *pll = pm_clock_get_pll(clock_id);
+	const struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
 	if (pll != NULL) {
 		*node_id = pll->nid;
@@ -2812,10 +2812,10 @@
 	uint32_t i;
 
 	for (i = 0; i < ARRAY_SIZE(pm_plls); i++) {
-		if (pm_plls[i].pre_src == clock_id ||
-		    pm_plls[i].post_src == clock_id ||
-		    pm_plls[i].div2 == clock_id ||
-		    pm_plls[i].bypass == clock_id) {
+		if ((pm_plls[i].pre_src == clock_id) ||
+		    (pm_plls[i].post_src == clock_id) ||
+		    (pm_plls[i].div2 == clock_id) ||
+		    (pm_plls[i].bypass == clock_id)) {
 			return &pm_plls[i];
 		}
 	}
@@ -2990,7 +2990,7 @@
 {
 	struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
-	if ((pll == NULL) || (mode != PLL_FRAC_MODE && mode != PLL_INT_MODE)) {
+	if ((pll == NULL) || ((mode != PLL_FRAC_MODE) && (mode != PLL_INT_MODE))) {
 		return PM_RET_ERROR_ARGS;
 	}
 	pll->mode = mode;
@@ -3011,7 +3011,7 @@
 enum pm_ret_status pm_clock_get_pll_mode(enum clock_id clock_id,
 					 uint32_t *mode)
 {
-	struct pm_pll *pll = pm_clock_get_pll(clock_id);
+	const struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
 	if ((pll == NULL) || (mode == NULL)) {
 		return PM_RET_ERROR_ARGS;
@@ -3052,7 +3052,7 @@
 uint8_t pm_clock_has_div(uint32_t clock_id, enum pm_clock_div_id div_id)
 {
 	uint32_t i;
-	struct pm_clock_node *nodes;
+	const struct pm_clock_node *nodes;
 
 	if (clock_id >= CLK_MAX_OUTPUT_CLK) {
 		return 0;
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
index dd21499..0dbfa57 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
@@ -62,7 +62,7 @@
 {
 	uint32_t val;
 
-	if (mmio_read_32(CRL_APB_RST_LPD_TOP) & CRL_APB_RPU_AMBA_RESET) {
+	if ((mmio_read_32(CRL_APB_RST_LPD_TOP) & CRL_APB_RPU_AMBA_RESET) != 0U) {
 		return PM_RET_ERROR_ACCESS;
 	}
 
@@ -165,8 +165,8 @@
 static enum pm_ret_status pm_ioctl_set_tapdelay_bypass(uint32_t type,
 						       uint32_t value)
 {
-	if ((value != PM_TAPDELAY_BYPASS_ENABLE &&
-	     value != PM_TAPDELAY_BYPASS_DISABLE) || type >= PM_TAPDELAY_MAX) {
+	if ((((value != PM_TAPDELAY_BYPASS_ENABLE) &&
+	     (value != PM_TAPDELAY_BYPASS_DISABLE)) || (type >= PM_TAPDELAY_MAX))) {
 		return PM_RET_ERROR_ARGS;
 	}
 
@@ -481,7 +481,7 @@
 					      uint32_t value)
 {
 	uint32_t mask;
-	uint32_t regarr[] = {0xFD360000U,
+	const uint32_t regarr[] = {0xFD360000U,
 				0xFD360014U,
 				0xFD370000U,
 				0xFD370014U,
@@ -682,7 +682,7 @@
  */
 enum pm_ret_status tfa_ioctl_bitmask(uint32_t *bit_mask)
 {
-	uint8_t supported_ids[] = {
+	const uint8_t supported_ids[] = {
 		IOCTL_GET_RPU_OPER_MODE,
 		IOCTL_SET_RPU_OPER_MODE,
 		IOCTL_RPU_BOOT_ADDR_CONFIG,
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
index 2d8c23b..1477e25 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
@@ -2012,9 +2012,9 @@
 void pm_api_pinctrl_get_function_name(uint32_t fid, char *name)
 {
 	if (fid >= MAX_FUNCTION) {
-		memcpy(name, END_OF_FUNCTION, FUNCTION_NAME_LEN);
+		(void)memcpy(name, END_OF_FUNCTION, FUNCTION_NAME_LEN);
 	} else {
-		memcpy(name, pinctrl_functions[fid].name, FUNCTION_NAME_LEN);
+		(void)memcpy(name, pinctrl_functions[fid].name, FUNCTION_NAME_LEN);
 	}
 }
 
@@ -2049,7 +2049,7 @@
 		return PM_RET_ERROR_ARGS;
 	}
 
-	memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
+	(void)memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
 
 	grps = pinctrl_functions[fid].group_base;
 	end_of_grp_offset = grps + pinctrl_functions[fid].group_size;
@@ -2088,13 +2088,13 @@
 						 uint16_t *groups)
 {
 	uint32_t i;
-	uint16_t *grps;
+	const uint16_t *grps;
 
 	if (pin >= MAX_PIN) {
 		return PM_RET_ERROR_ARGS;
 	}
 
-	memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
+	(void)memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
 
 	grps = *zynqmp_pin_groups[pin].groups;
 	if (grps == NULL) {
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index 9d0e2c4..a517257 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -204,7 +204,7 @@
 			continue;
 		}
 
-		while (reg) {
+		while (reg != 0U) {
 			enum pm_node_id node;
 			uint32_t idx, ret, irq, lowest_set = reg & (-reg);
 
@@ -218,7 +218,7 @@
 			node = irq_to_pm_node(irq);
 			reg &= ~lowest_set;
 
-			if (node > NODE_UNKNOWN && node < NODE_MAX) {
+			if ((node > NODE_UNKNOWN) && (node < NODE_MAX)) {
 				if (pm_wakeup_nodes_set[node] == 0U) {
 					ret = pm_set_wakeup_source(NODE_APU, node, 1U);
 					pm_wakeup_nodes_set[node] = (ret == PM_RET_SUCCESS) ? 1U : 0U;
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
index d7c9f24..079f89e 100644
--- a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
@@ -1115,7 +1115,7 @@
 		return status;
 	}
 
-	if (enable) {
+	if (enable != 0U) {
 		api_id = PM_CLOCK_ENABLE;
 	} else {
 		api_id = PM_CLOCK_DISABLE;
@@ -1297,7 +1297,7 @@
 		return status;
 	}
 
-	if (pm_clock_has_div(clock_id, PM_CLOCK_DIV0_ID)) {
+	if ((pm_clock_has_div(clock_id, PM_CLOCK_DIV0_ID)) != 0U) {
 		/* Send request to the PMU to get div0 */
 		PM_PACK_PAYLOAD3(payload, PM_CLOCK_GETDIVIDER, clock_id,
 				 PM_CLOCK_DIV0_ID);
@@ -1308,7 +1308,7 @@
 		*divider = val;
 	}
 
-	if (pm_clock_has_div(clock_id, PM_CLOCK_DIV1_ID)) {
+	if ((pm_clock_has_div(clock_id, PM_CLOCK_DIV1_ID)) != 0U) {
 		/* Send request to the PMU to get div1 */
 		PM_PACK_PAYLOAD3(payload, PM_CLOCK_GETDIVIDER, clock_id,
 				 PM_CLOCK_DIV1_ID);
@@ -1657,7 +1657,7 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL) {
+	if ((nid < NODE_APLL) || (nid > NODE_IOPLL)) {
 		return PM_RET_ERROR_ARGS;
 	}
 
@@ -1688,7 +1688,7 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL) {
+	if ((nid < NODE_APLL) || (nid > NODE_IOPLL)) {
 		return PM_RET_ERROR_ARGS;
 	}
 
@@ -1721,7 +1721,7 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL) {
+	if ((nid < NODE_APLL) || (nid > NODE_IOPLL)) {
 		return PM_RET_ERROR_ARGS;
 	}
 
@@ -1749,7 +1749,7 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL) {
+	if ((nid < NODE_APLL) || (nid > NODE_IOPLL)) {
 		return PM_RET_ERROR_ARGS;
 	}
 
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
index 65b2426..b3215f8 100644
--- a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
@@ -374,7 +374,7 @@
 		uint32_t value = 0U;
 
 		ret = pm_fpga_get_status(&value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32)));
 	}
 
 	case PM_SECURE_RSA_AES:
@@ -389,15 +389,15 @@
 		}
 
 		SMC_RET2(handle,
-			 (uint64_t)result[0] | ((uint64_t)result[1] << 32),
-			 (uint64_t)result[2] | ((uint64_t)result[3] << 32));
+			 ((uint64_t)result[0] | ((uint64_t)result[1] << 32)),
+			 ((uint64_t)result[2] | ((uint64_t)result[3] << 32)));
 	case PM_IOCTL:
 	{
 		uint32_t value = 0U;
 
 		ret = pm_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
 			       pm_arg[3], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32)));
 	}
 
 	case PM_QUERY_DATA:
@@ -406,8 +406,8 @@
 
 		pm_query_data(pm_arg[0], pm_arg[1], pm_arg[2],
 			      pm_arg[3], data);
-		SMC_RET2(handle, (uint64_t)data[0]  | ((uint64_t)data[1] << 32),
-			 (uint64_t)data[2] | ((uint64_t)data[3] << 32));
+		SMC_RET2(handle, ((uint64_t)data[0]  | ((uint64_t)data[1] << 32)),
+			 ((uint64_t)data[2] | ((uint64_t)data[3] << 32)));
 	}
 
 	case PM_CLOCK_ENABLE:
@@ -423,7 +423,7 @@
 		uint32_t value = 0U;
 
 		ret = pm_clock_getstate(pm_arg[0], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32)));
 	}
 
 	case PM_CLOCK_SETDIVIDER:
@@ -435,7 +435,7 @@
 		uint32_t value = 0U;
 
 		ret = pm_clock_getdivider(pm_arg[0], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32)));
 	}
 
 	case PM_CLOCK_SETPARENT:
@@ -447,7 +447,7 @@
 		uint32_t value = 0U;
 
 		ret = pm_clock_getparent(pm_arg[0], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32U)));
 	}
 
 	case PM_GET_TRUSTZONE_VERSION:
@@ -472,7 +472,7 @@
 	{
 		ret = pm_secure_image(pm_arg[0], pm_arg[1], pm_arg[2],
 				      pm_arg[3], &result[0]);
-		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)result[0] << 32U),
+		SMC_RET2(handle, ((uint64_t)ret | ((uint64_t)result[0] << 32U)),
 			 result[1]);
 	}
 
@@ -482,7 +482,7 @@
 
 		ret = pm_fpga_read(pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3],
 				   &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32U)));
 	}
 
 	case PM_SECURE_AES:
@@ -490,7 +490,7 @@
 		uint32_t value = 0U;
 
 		ret = pm_aes_engine(pm_arg[0], pm_arg[1], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32U)));
 	}
 
 	case PM_PLL_SET_PARAMETER:
@@ -502,7 +502,7 @@
 		uint32_t value = 0U;
 
 		ret = pm_pll_get_parameter(pm_arg[0], pm_arg[1], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value << 32U));
+		SMC_RET1(handle, ((uint64_t)ret | ((uint64_t)value << 32U)));
 	}
 
 	case PM_PLL_SET_MODE:
@@ -514,7 +514,7 @@
 		uint32_t mode = 0U;
 
 		ret = pm_pll_get_mode(pm_arg[0], &mode);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)mode << 32U));
+		SMC_RET1(handle, ((uint64_t)ret | ((uint64_t)mode << 32U)));
 	}
 
 	case PM_REGISTER_ACCESS:
@@ -523,7 +523,7 @@
 
 		ret = pm_register_access(pm_arg[0], pm_arg[1], pm_arg[2],
 					 pm_arg[3], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
+		SMC_RET1(handle, ((uint64_t)ret | (((uint64_t)value) << 32U)));
 	}
 
 	case PM_EFUSE_ACCESS:
@@ -538,7 +538,7 @@
 		}
 #endif
 		ret = pm_efuse_access(pm_arg[0], pm_arg[1], &value);
-		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
+		SMC_RET1(handle, (uint64_t)ret | (((uint64_t)value) << 32U));
 	}
 
 	case PM_FPGA_GET_VERSION:
@@ -549,8 +549,8 @@
 		PM_PACK_PAYLOAD5(payload, smc_fid & FUNCID_NUM_MASK,
 				 pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3]);
 		ret = pm_ipi_send_sync(primary_proc, payload, ret_payload, 3U);
-		SMC_RET2(handle, (uint64_t)ret | (uint64_t)ret_payload[0] << 32U,
-			 (uint64_t)ret_payload[1] | (uint64_t)ret_payload[2] << 32U);
+		SMC_RET2(handle, ((uint64_t)ret | ((uint64_t)ret_payload[0] << 32U)),
+			 ((uint64_t)ret_payload[1] | ((uint64_t)ret_payload[2] << 32U)));
 	}
 
 	case PM_FEATURE_CHECK:
@@ -560,8 +560,8 @@
 
 		ret = pm_feature_check(pm_arg[0], &version, bit_mask,
 				       ARRAY_SIZE(bit_mask));
-		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)version << 32U),
-			 (uint64_t)bit_mask[0] | ((uint64_t)bit_mask[1] << 32U));
+		SMC_RET2(handle, ((uint64_t)ret | ((uint64_t)version << 32U)),
+			 ((uint64_t)bit_mask[0] | ((uint64_t)bit_mask[1] << 32U)));
 	}
 
 	default:
@@ -570,7 +570,7 @@
 				 pm_arg[2], pm_arg[3], pm_arg[4]);
 		ret = pm_ipi_send_sync(primary_proc, payload, result,
 				       RET_PAYLOAD_ARG_CNT);
-		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)result[0] << 32U),
-			 (uint64_t)result[1] | ((uint64_t)result[2] << 32U));
+		SMC_RET2(handle, ((uint64_t)ret | ((uint64_t)result[0] << 32U)),
+			 ((uint64_t)result[1] | ((uint64_t)result[2] << 32U)));
 	}
 }
diff --git a/plat/xilinx/zynqmp/sip_svc_setup.c b/plat/xilinx/zynqmp/sip_svc_setup.c
index f5990ca..1baefb3 100644
--- a/plat/xilinx/zynqmp/sip_svc_setup.c
+++ b/plat/xilinx/zynqmp/sip_svc_setup.c
@@ -81,7 +81,7 @@
 	VERBOSE("SMCID: 0x%08x, x1: 0x%016" PRIx64 ", x2: 0x%016" PRIx64 ", x3: 0x%016" PRIx64 ", x4: 0x%016" PRIx64 "\n",
 		smc_fid, x1, x2, x3, x4);
 
-	if (smc_fid & SIP_FID_MASK) {
+	if ((smc_fid & (uint32_t)SIP_FID_MASK) != 0U) {
 		WARN("SMC out of SiP assinged range: 0x%x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);
 	}
diff --git a/services/spd/tlkd/tlkd.mk b/services/spd/tlkd/tlkd.mk
index 56de0a6..fc8840d 100644
--- a/services/spd/tlkd/tlkd.mk
+++ b/services/spd/tlkd/tlkd.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2024, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,7 +8,9 @@
 SPD_INCLUDES		:=	-Iinclude/bl32/payloads
 endif
 
+ifeq (${ENABLE_FEAT_D128}, 0)
 SPD_SOURCES		:=	services/spd/tlkd/tlkd_common.c		\
 				services/spd/tlkd/tlkd_helpers.S	\
 				services/spd/tlkd/tlkd_main.c		\
 				services/spd/tlkd/tlkd_pm.c
+endif
\ No newline at end of file