diff --git a/core/drivers/zynqmp_csudma.c b/core/drivers/zynqmp_csudma.c
index 8809635b05e..4e3b59c9c29 100644
--- a/core/drivers/zynqmp_csudma.c
+++ b/core/drivers/zynqmp_csudma.c
@@ -10,6 +10,7 @@
 #include <kernel/cache_helpers.h>
 #include <kernel/delay.h>
 #include <mm/core_memprot.h>
+#include <tee/cache.h>
 #include <util.h>
 
 #define CSUDMA_ADDR_OFFSET		0x00
@@ -112,6 +113,7 @@ TEE_Result zynqmp_csudma_transfer(enum zynqmp_csudma_channel channel,
 				      CSUDMA_SIZE);
 	paddr_t phys = virt_to_phys(addr);
 	uint32_t addr_offset = 0;
+	TEE_Result res = TEE_ERROR_GENERIC;
 
 	if (!dma)
 		return TEE_ERROR_GENERIC;
@@ -129,9 +131,13 @@ TEE_Result zynqmp_csudma_transfer(enum zynqmp_csudma_channel channel,
 
 	if (channel == ZYNQMP_CSUDMA_DST_CHANNEL) {
 		dma = dma + CSUDMA_OFFSET_DIFF;
-		dcache_inv_range(addr, SHIFT_U64(len, CSUDMA_SIZE_SHIFT));
+		res = cache_operation(TEE_CACHEINVALIDATE, addr, len);
+		if (res)
+			return res;
 	} else {
-		dcache_clean_range(addr, SHIFT_U64(len, CSUDMA_SIZE_SHIFT));
+		res = cache_operation(TEE_CACHECLEAN, addr, len);
+		if (res)
+			return res;
 	}
 
 	addr_offset = phys & CSUDMA_ADDR_MASK;
diff --git a/core/drivers/zynqmp_huk.c b/core/drivers/zynqmp_huk.c
index bb6727c5484..6b48f84c9cf 100644
--- a/core/drivers/zynqmp_huk.c
+++ b/core/drivers/zynqmp_huk.c
@@ -10,9 +10,11 @@
 #include <drivers/zynqmp_huk.h>
 #include <drivers/zynqmp_pm.h>
 #include <io.h>
+#include <kernel/delay.h>
 #include <kernel/tee_common_otp.h>
 #include <mm/core_memprot.h>
 #include <string_ext.h>
+#include <tee/cache.h>
 #include <tee/tee_cryp_utl.h>
 #include <trace.h>
 #include <utee_defines.h>
@@ -114,6 +116,7 @@ TEE_Result tee_otp_get_hw_unique_key(struct tee_hw_unique_key *hwkey)
 	uint8_t dst[ZYNQMP_CSU_AES_DST_LEN(sizeof(src))]
 		__aligned_csuaes = { 0 };
 	TEE_Result ret = TEE_ERROR_GENERIC;
+	uint32_t retry_counter = 0;
 	uint32_t status = 0;
 
 	static_assert(sizeof(device_dna) == ZYNQMP_GCM_IV_SIZE);
@@ -194,19 +197,54 @@ TEE_Result tee_otp_get_hw_unique_key(struct tee_hw_unique_key *hwkey)
 	memset(src, 0, sizeof(src));
 	/* Ignore the tag data from the dst buffer - pass a smaller size */
 	ret = zynqmp_csu_aes_decrypt_data(dst, sizeof(src), src, sizeof(src),
-					  tag, sizeof(tag), iv,
-					  ZYNQMP_EFUSE_LEN(DNA),
+					  tag, sizeof(tag), iv, sizeof(iv),
 					  ZYNQMP_CSU_AES_KEY_SRC_DEV);
 	if (ret) {
 		EMSG("Can't decrypt DNA, please make sure PUF was registered");
 		goto cleanup;
 	}
 
-	if (memcmp(src, sha, sizeof(sha))) {
-		EMSG("PUF not ready, can't create HUK");
-		ret = TEE_ERROR_GENERIC;
-		goto cleanup;
-	}
+	/*
+	 * This retry loop here is a workaround for hardware issue where
+	 * CSU-DMA transfer is not yet delivered to DDR memory (in variable
+	 * 'src').
+	 *
+	 * CSU-AES/CSU-DMA code is implemented in a way that it waits for DONE
+	 * signal which indicates that memory transfer has left the DMA engine.
+	 */
+	retry_counter = 0;
+	do {
+		if (memcmp(src, sha, sizeof(sha)) == 0)
+			break;
+
+		/*
+		 * If the transfer is not completed within 1 second then there
+		 * is a real failure.
+		 */
+		if (retry_counter >= 1000) {
+			EMSG("PUF not ready, can't create HUK");
+			ret = TEE_ERROR_GENERIC;
+			goto cleanup;
+		}
+
+		/*
+		 * Give CPU's internal fifos some time to transfer data to the
+		 * DDR memory.
+		 */
+		mdelay(1);
+
+		/*
+		 * Invalidate src buffer from caches as memcmp above has
+		 * already loaded data from DDR to cache so that next compare
+		 * will be with fresh data.
+		 */
+		ret = cache_operation(TEE_CACHEINVALIDATE, src, sizeof(src));
+		if (ret) {
+			EMSG("Failed to invalidate cache");
+			goto cleanup;
+		}
+		retry_counter++;
+	} while (1);
 
 	IMSG("HUK ready");