diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.c
index bd5b76e9..2323f904 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.c
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.c
@@ -56,6 +56,9 @@ const u8 XFsbl_TPadSha2[] = {0x30, 0x31, 0x30, 0x0D, 0x06, 0x09, 0x60,
 	0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02,
 	0x01, 0x05, 0x00, 0x04, 0x20 };
 
+u32 CheckSum = 0U;
+static XSecure_Rsa SecureRsa;
+
 /*****************************************************************************/
 /**
  * Configure the RSA and SHA for the SPK
@@ -78,18 +81,19 @@ u32 XFsbl_SpkVer(u64 AcOffset, u32 HashLen)
 	u8 * AcPtr = (u8*) (PTRSIZE) AcOffset;
 	u32 Status = XFSBL_SUCCESS;
 	void * ShaCtx = (void * )NULL;
+	u8 XFsbl_RsaSha3Array[512];
 
+#ifdef XFSBL_SHA2
+	sha2_context ShaCtxObj;
+	ShaCtx = &ShaCtxObj;
+#endif
 
-	(void)XFsbl_ShaStart( ShaCtx,  HashLen);
+	(void)XFsbl_ShaStart(ShaCtx, HashLen);
 
-	/**
-	 * Hash the PPK + SPK choice
-	 */
-	XFsbl_ShaUpdate( ShaCtx, AcPtr, 8, HashLen);
+	/* Hash the PPK + SPK choice */
+	XFsbl_ShaUpdate(ShaCtx, AcPtr, 8, HashLen);
 
-	/**
-	 * Set PPK pointer
-	 */
+	/* Set PPK pointer */
 	AcPtr += XFSBL_RSA_AC_ALIGN;
 	PpkModular = (u8 *)AcPtr;
 	AcPtr += XFSBL_SPK_SIG_SIZE;
@@ -101,23 +105,21 @@ u32 XFsbl_SpkVer(u64 AcOffset, u32 HashLen)
 	XFsbl_Printf(DEBUG_DETAILED,
 		"XFsbl_SpkVer: Ppk Mod %0x, Ppk Mod Ex %0x, Ppk Exp %0x\r\n",
 		PpkModular, PpkModularEx, PpkExp);
-	/**
-	 * Calculate SPK + Auth header(PPK and SPK Selectoin both) Hash
-	 */
-	XFsbl_ShaUpdate( ShaCtx, (u8 *)AcPtr, XFSBL_PPK_SIZE, HashLen);
-	XFsbl_ShaFinish( ShaCtx, (u8 *)SpkHash, HashLen);
 
-	/**
-	 * Set SPK Signature pointer
-	 */
+	/* Calculate SPK + Auth header(PPK and SPK Selectoin both) Hash */
+	XFsbl_ShaUpdate(ShaCtx, (u8 *)AcPtr, XFSBL_PPK_SIZE, HashLen);
+	XFsbl_ShaFinish(ShaCtx, (u8 *)SpkHash, HashLen);
+
+	/* Set SPK Signature pointer */
 	AcPtr += XFSBL_SPK_SIZE;
 
-	/**
-	 * Decrypt SPK Signature.
-	 */
+	XSecure_RsaInitialize(&SecureRsa, AcPtr, PpkModular,
+			              PpkModularEx, (u8 *)&PpkExp);
+
+	/* Decrypt SPK Signature */
+
 	if(XFSBL_SUCCESS !=
-		XFsbl_RsaDecrypt(AcPtr, PpkModular,
-		PpkModularEx, (u8 *)&PpkExp, XFsbl_RsaSha3Array, 0) )
+		XSecure_RsaDecrypt(&SecureRsa, XFsbl_RsaSha3Array))
 	{
 		XFsbl_Printf(DEBUG_GENERAL,
 			"XFsbl_SpkVer: XFSBL_ERROR_SPK_RSA_DECRYPT\r\n");
@@ -125,11 +127,9 @@ u32 XFsbl_SpkVer(u64 AcOffset, u32 HashLen)
 		goto END;
 	}
 
-	/**
-	 * Authenticate SPK Signature.
-	 */
-	if(XFSBL_SUCCESS != XFsbl_CheckPadding(XFsbl_RsaSha3Array,
-					SpkHash, HashLen))
+	/* Authenticate SPK Signature */
+	if(XFSBL_SUCCESS != XSecure_RsaCheckPadding(XFsbl_RsaSha3Array,
+					                            SpkHash, HashLen))
 	{
 		XFsbl_PrintArray(DEBUG_INFO, SpkHash,
 				HashLen, "Calculated Partition Hash");
@@ -158,7 +158,7 @@ END:
  *
  ******************************************************************************/
 u32 XFsbl_PartitionSignVer(u64 PartitionOffset, u32 PartitionLen,
-	u64 AcOffset, u32 HashLen)
+							    u64 AcOffset, u32 HashLen)
 {
 
 	u8 PartitionHash[XFSBL_HASH_TYPE_SHA3] __attribute__ ((aligned (4)));
@@ -168,18 +168,24 @@ u32 XFsbl_PartitionSignVer(u64 PartitionOffset, u32 PartitionLen,
 	u8 * AcPtr = (u8*)(PTRSIZE) AcOffset;
 	u32 Status = XFSBL_SUCCESS;
 	u32 HashDataLen=0U;
+	u8 XFsbl_RsaSha3Array[512];
 
 	XFsbl_Printf(DEBUG_INFO,"Doing Partition Sign verification\r\n");
 
+	/* Reset CSU DMA. This is a workaround and need to be removed */
+	XCsuDma_Reset();
+
 	/**
 	 * hash to be calculated will be total length with AC minus
 	 * signature size
 	 */
 	HashDataLen = PartitionLen - XFSBL_FSBL_SIG_SIZE;
 
-	/**
-	 * Set SPK pointer
-	 */
+	/* Calculate Partition Hash */
+	XFsbl_ShaDigest((const u8 *)(PTRSIZE)PartitionOffset,
+			HashDataLen, PartitionHash, HashLen);
+
+	/* Set SPK pointer */
 	AcPtr += (XFSBL_RSA_AC_ALIGN + XFSBL_PPK_SIZE);
 	SpkModular = (u8 *)AcPtr;
 	AcPtr += XFSBL_BHDR_SIG_SIZE;
@@ -188,31 +194,23 @@ u32 XFsbl_PartitionSignVer(u64 PartitionOffset, u32 PartitionLen,
 	SpkExp = *((u32 *)AcPtr);
 	AcPtr += XFSBL_RSA_AC_ALIGN;
 
-	/**
-	 * Calculate Partition Hash
-	 */
-	XFsbl_ShaDigest((const u8 *)(PTRSIZE)PartitionOffset, HashDataLen,
-			PartitionHash, HashLen);
-
-	/**
-	 * Increment by  SPK Signature pointer
-	 */
+	/* Increment by  SPK Signature pointer */
 	AcPtr += XFSBL_SPK_SIG_SIZE;
-	/**
-	 * Increment by  BHDR Signature pointer
-	 */
+	/* Increment by  BHDR Signature pointer */
 	AcPtr += XFSBL_BHDR_SIG_SIZE;
 
 	XFsbl_Printf(DEBUG_DETAILED,
 		"XFsbl_PartVer: Spk Mod %0x, Spk Mod Ex %0x, Spk Exp %0x\r\n",
 		SpkModular, SpkModularEx, SpkExp);
 
-	/**
-	 * Decrypt Partition Signature.
-	 */
+	XFsbl_Printf(DEBUG_INFO,
+			"Partition Verification done \r\n");
+
+	XSecure_RsaInitialize(&SecureRsa, AcPtr, SpkModular,
+			              SpkModularEx, (u8 *)&SpkExp);
+	/* Decrypt Partition Signature. */
 	if(XFSBL_SUCCESS !=
-		XFsbl_RsaDecrypt(AcPtr, SpkModular,
-		SpkModularEx, (u8 *)&SpkExp, XFsbl_RsaSha3Array, 0) )
+		XSecure_RsaDecrypt(&SecureRsa, XFsbl_RsaSha3Array))
 	{
 		XFsbl_Printf(DEBUG_GENERAL,
                         "XFsbl_SpkVer: XFSBL_ERROR_PART_RSA_DECRYPT\r\n");
@@ -220,10 +218,8 @@ u32 XFsbl_PartitionSignVer(u64 PartitionOffset, u32 PartitionLen,
 		goto END;
 	}
 
-	/**
-	 * Authenticate Partition Signature.
-	 */
-	if(XFSBL_SUCCESS != XFsbl_CheckPadding(XFsbl_RsaSha3Array,
+	/* Authenticate Partition Signature */
+	if(XFSBL_SUCCESS != XSecure_RsaCheckPadding(XFsbl_RsaSha3Array,
 				PartitionHash, HashLen))
 	{
 		XFsbl_PrintArray(DEBUG_INFO, PartitionHash,
@@ -231,7 +227,7 @@ u32 XFsbl_PartitionSignVer(u64 PartitionOffset, u32 PartitionLen,
 		XFsbl_PrintArray(DEBUG_INFO, XFsbl_RsaSha3Array,
 				512, "RSA decrypted Hash");
 		XFsbl_Printf(DEBUG_GENERAL,
-                        "XFsbl_SpkVer: XFSBL_ERROR_PART_SIGNATURE\r\n");
+                        "XFsbl_PartVer: XFSBL_ERROR_PART_SIGNATURE\r\n");
 		Status = XFSBL_ERROR_PART_SIGNATURE;
 		goto END;
 	}
@@ -240,145 +236,6 @@ END:
 	return Status;
 }
 
-/*****************************************************************************/
-/**
- * DT 764500(Add feature for basic integrity check of
- the FSBL Image by FSBL.)
- *
- * @param
- *
- * @return
- *
- ******************************************************************************/
-u32 XFsbl_IntegrityCheck(u64 PartitionOffset, u32 PartitionLen,
-			u64 CheckSumOffset, u32 HashLen)
-{
-	u32 Status = XFSBL_SUCCESS;
-	u8 PartitionHash[XFSBL_HASH_TYPE_SHA3] __attribute__ ((aligned (4)));
-	u32 ii=0;
-	u8 *HashPtr = (u8 *)NULL;
-	void * ShaCtx = (void * )NULL;
-
-	XFsbl_Printf(DEBUG_INFO,"XFsbl_IntegrityCheck: Doing Partition"
-			" Integrity Check	HashLen=%d\r\n",HashLen);
-
-	HashPtr = (u8 *)(PTRSIZE)CheckSumOffset;
-	(void)XFsbl_ShaStart(ShaCtx,  HashLen);
-
-	/**
-	 * Calculate FSBL Hash
-	 */
-	XFsbl_ShaUpdate( ShaCtx, (u8 *)(PTRSIZE)PartitionOffset,
-			PartitionLen, HashLen);
-
-	XFsbl_ShaFinish( ShaCtx, (u8 *)PartitionHash, HashLen);
-
-	/**
-	 * Compare the calculated and builtin hash
-	 */
-	for (ii = 0; ii < HashLen; ii++)
-	{
-		if (HashPtr[ii] != PartitionHash[ii])
-		{
-			Status = XFSBL_FAILURE;
-			XFsbl_Printf(DEBUG_INFO,"Check Failed!!!!!\r\n");
-			goto END;
-		}
-	}
-	XFsbl_Printf(DEBUG_INFO,"Check Passed!!!!!\r\n");
-
-END:
-	return Status;
-}
-
-/*****************************************************************************/
-/**
- *
- * @param
- *
- * @return
- *
- ******************************************************************************/
-u32 XFsbl_CheckPadding(u8 *Signature, u8 *Hash, u32 HashLen)
-{
-
-	u8 * Tpadding = (u8 *)NULL;
-	u32 Pad = XFSBL_FSBL_SIG_SIZE - 3 - 19 - HashLen;
-	u8 * PadPtr = Signature;
-	u32 ii;
-	u32 Status = 0;
-
-	if(XFSBL_HASH_TYPE_SHA3 == HashLen)
-	{
-		Tpadding = (u8 *)XFsbl_TPadSha3;
-	}
-	else
-	{
-		Tpadding = (u8 *)XFsbl_TPadSha2;
-	}
-
-	/**
-	 * Re-Create PKCS#1v1.5 Padding
-	 * MSB  ----------------------------------------------------------------LSB
-	 * 0x0 || 0x1 || 0xFF(for 202 bytes) || 0x0 || T_padding || SHA256/384 Hash
-	 */
-
-	if (0x00 != *PadPtr)
-	{
-		Status = 1;
-		goto END;
-	}
-	PadPtr++;
-
-	if (0x01 != *PadPtr)
-	{
-		Status = 2;
-		goto END;
-	}
-	PadPtr++;
-
-	for (ii = 0; ii < Pad; ii++)
-	{
-		if (0xFF != *PadPtr)
-		{
-			Status = 3;
-			goto END;
-		}
-		PadPtr++;
-	}
-
-	if (0x00 != *PadPtr)
-	{
-		Status = 4;
-		goto END;
-	}
-	PadPtr++;
-
-	for (ii = 0; ii < 19; ii++)
-	{
-		if (*PadPtr != Tpadding[ii])
-		{
-			Status = 5;
-			goto END;
-		}
-		PadPtr++;
-	}
-
-	for (ii = 0; ii < HashLen; ii++)
-	{
-		if (*PadPtr != Hash[ii])
-		{
-			Status = 6;
-			goto END;
-		}
-		PadPtr++;
-	}
-
-END:
-	XFsbl_Printf(DEBUG_INFO,"XFsbl_CheckPadding: Error:0x%x\r\n",Status);
-	return Status;
-}
-
 /*****************************************************************************/
 /**
  *
@@ -387,8 +244,8 @@ END:
  * @return      None
  *
  ******************************************************************************/
-u32 XFsbl_Authentication(u64 PartitionOffset, u32 PartitionLen,
-        u64 AcOffset, u32 HashLen)
+u32 XFsbl_Authentication(XFsblPs * FsblInstancePtr, u64 PartitionOffset,
+						 u32 PartitionLen, u64 AcOffset, u32 HashLen)
 {
         u32 Status = XFSBL_SUCCESS;
 
@@ -399,20 +256,18 @@ u32 XFsbl_Authentication(u64 PartitionOffset, u32 PartitionLen,
 		(PTRSIZE )PartitionOffset, PartitionLen,
 		(PTRSIZE )AcOffset, HashLen);
 
-        /**
-         * Do SPK Signature verification using PPK
-         */
+        /* Do SPK Signature verification using PPK */
         Status = XFsbl_SpkVer(AcOffset, HashLen);
+
         if(XFSBL_SUCCESS != Status)
         {
                 goto END;
         }
 
-        /**
-         * Do Partition Signature verification using SPK
-         */
+        /* Do Partition Signature verification using SPK */
         Status = XFsbl_PartitionSignVer(PartitionOffset, PartitionLen,
-                        AcOffset, HashLen);
+			                        AcOffset, HashLen);
+
         if(XFSBL_SUCCESS != Status)
         {
                 goto END;
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.h
index 43bb591e..1646b2a0 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.h
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_authentication.h
@@ -50,7 +50,19 @@ extern "C" {
 
 /***************************** Include Files *********************************/
 #include "xfsbl_hw.h"
-
+#include "xfsbl_main.h"
+#ifdef XFSBL_RSA
+#include "xsecure_sha.h"
+#include "xsecure_rsa.h"
+#endif
+#ifdef XFSBL_AES
+#include "xsecure_aes.h"
+#endif
+#include "xcsudma.h"
+#include "xparameters.h"
+#ifdef XFSBL_SHA2
+#include "xilrsa.h"
+#endif
 /***************************** Type defines *********************************/
 #define XFSBL_HASH_TYPE_SHA3					(48U)
 #define XFSBL_HASH_TYPE_SHA2					(32U)
@@ -78,7 +90,7 @@ extern "C" {
 
 #define XFSBL_AUTH_CERT_MAX_SIZE	(XFSBL_AUTH_CERT_MIN_SIZE + 60)
 
-#define XFSBL_PARTIAL_AC_SIZE	(XFSBL_AUTH_CERT_MIN_SIZE - XFSBL_FSBL_SIG_SIZE)
+#define XFSBL_PARTIAL_AC_SIZE  (XFSBL_AUTH_CERT_MIN_SIZE - XFSBL_FSBL_SIG_SIZE)
 
 /**
 * CSU RSA Register Map
@@ -90,7 +102,8 @@ extern "C" {
 #define XFSBL_CSU_RSA_CONTROL_NOP         (0x00U)
 #define XFSBL_CSU_RSA_CONTROL_EXP         (0x01U)
 #define XFSBL_CSU_RSA_CONTROL_EXP_PRE     (0x05U)
-#define XFSBL_CSU_RSA_CONTROL_MASK		 (XFSBL_CSU_RSA_CONTROL_4096 + XFSBL_CSU_RSA_CONTROL_EXP_PRE)
+#define XFSBL_CSU_RSA_CONTROL_MASK		 (XFSBL_CSU_RSA_CONTROL_4096 \
+											+ XFSBL_CSU_RSA_CONTROL_EXP_PRE)
 
 #define XFSBL_CSU_RSA_RAM_EXPO			 (0)
 #define XFSBL_CSU_RSA_RAM_MOD			 (1)
@@ -109,21 +122,23 @@ extern "C" {
 
 #define	XFSBL_SHA3_LAST_PACKET			 (0x1)
 
-u32 XFsbl_Authentication(u64 PartitionOffset, u32 PartitionLen,
-        u64 AcOffset, u32 HashLen);
+#ifdef XFSBL_RSA
+u32 XFsbl_Authentication(XFsblPs * FsblInstancePtr, u64 PartitionOffset,
+						u32 PartitionLen, u64 AcOffset, u32 HashLen);
 u32 XFsbl_PartitionSignVer(u64 PartitionOffset, u32 PartitionLen,
-	u64 AcOffset, u32 HashLen);
+							    u64 AcOffset, u32 HashLen);
 u32 XFsbl_SpkVer(u64 AcOffset, u32 HashLen);
-//u32 XFsbl_IntegrityCheck(XCbrBootRom *InstancePtr);
+
 u32 XFsbl_CheckPadding(u8 *Signature, u8 *Hash, u32 HashLen);
 
-u32 XFsbl_RsaDecrypt(u8* EncText, u8* Mod, u8* ModExt, u8* ModExpo, u8* Result, u8 Reuse);
 void XFsbl_ShaDigest(const u8 *In, const u32 Size, u8 *Out, u32 HashLen);
-void XFsbl_ShaStart( void * Ctx, u32 HashLen);
+void XFsbl_ShaStart(void * Ctx, u32 HashLen);
 void XFsbl_ShaUpdate(void * Ctx, u8 * Data, u32 Size, u32 HashLen);
 void XFsbl_ShaFinish(void * Ctx, u8 * Hash, u32 HashLen);
+#endif
+
+extern XCsuDma CsuDma;  /* CSU DMA instance */
 
-extern u8 XFsbl_RsaSha3Array[512];
 #ifdef __cplusplus
 extern "C" }
 #endif
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_config.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_config.h
index 7fcf7ce3..91676ec0 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_config.h
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_config.h
@@ -116,12 +116,15 @@ extern "C" {
  *     - FSBL_NAND_EXCLUDE NAND code will be excluded
  *     - FSBL_QSPI_EXCLUDE QSPI code will be excluded
  *     - FSBL_SD_EXCLUDE SD code will be excluded
- *     - FSBL_RSA_EXCLUDE RSA code will be excluded
+ *     - FSBL_RSA_EXCLUDE RSA (authentication) code will be excluded
+ *     - FSBL_AES_EXCLUDE AES (decryption) code will be excluded
+ *     - FSBL_SHA2_EXCLUDE SHA2 code will be excluded
  */
 #define FSBL_NAND_EXCLUDE_VAL			(0U)
 #define FSBL_QSPI_EXCLUDE_VAL			(0U)
 #define FSBL_SD_EXCLUDE_VAL			(0U)
 #define FSBL_RSA_EXCLUDE_VAL			(0U)
+#define FSBL_AES_EXCLUDE_VAL			(0U)
 #define FSBL_SHA2_EXCLUDE_VAL			(1U)
 
 #if FSBL_NAND_EXCLUDE_VAL
@@ -140,6 +143,10 @@ extern "C" {
 #define FSBL_RSA_EXCLUDE
 #endif
 
+#if FSBL_AES_EXCLUDE_VAL
+#define FSBL_AES_EXCLUDE
+#endif
+
 #if FSBL_SHA2_EXCLUDE_VAL
 #define FSBL_SHA2_EXCLUDE
 #endif
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.c
index caa4b368..3357af4e 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.c
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.c
@@ -34,7 +34,7 @@
  *
  * @file xfsbl_csu_dma.c
  *
- * This contains code for the CSU DMA driver.
+ * Contains code for the CSU DMA intialization
  *
  * <pre>
  * MODIFICATION HISTORY:
@@ -50,6 +50,7 @@
  ******************************************************************************/
 
 /***************************** Include Files *********************************/
+#include "xcsudma.h"
 #include "xfsbl_csu_dma.h"
 /************************** Constant Definitions *****************************/
 
@@ -60,123 +61,37 @@
 /************************** Function Prototypes ******************************/
 
 /************************** Variable Definitions *****************************/
-
+XCsuDma CsuDma;
 
 /*****************************************************************************/
 /**
- *
- * This function will configure dst/src DMA address
- * and size and also start the CSU DMA.
+ * This function is used to initialize the DMA driver
  *
  * @param	None
  *
- * @return	None
+ * @return	returns the error codes described in xfsbl_error.h on any error
+ * 		returns XFSBL_SUCCESS on success
  *
- ******************************************************************************/
-void XFsbl_CsuDmaStart(XFSBL_CSU_DMA_CHANNEL Channel, u32 Addr, u32 Size)
+ *****************************************************************************/
+u32 XFsbl_CsuDmaInit()
 {
-	XASSERT_VOID((Addr & 3) == 0);
+	u32 Status = XFSBL_SUCCESS;
+	XCsuDma_Config * CsuDmaConfig;
 
-	if(XFSBL_CSU_DMA_SRC == Channel)
-	{
-		XASSERT_VOID((Size & 2) == 0);
-	}
-	else
-	{
-		XASSERT_VOID((Size & 3) == 0);
+	CsuDmaConfig = XCsuDma_LookupConfig(0);
+	if (NULL == CsuDmaConfig) {
+		XFsbl_Printf(DEBUG_GENERAL, "XFSBL_ERROR_CSUDMA_INIT_FAIL \n\r");
+		Status = XFSBL_ERROR_CSUDMA_INIT_FAIL;
+		goto END;
 	}
 
-	XFsbl_Out32(CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_ADDR_REG, Addr);
-
-	/**
-	 *  Writes to SIZE to start the channel, this starts
-	 *  the DMA.
-	 */
-	/** ASM Code */
-	mb();
-	XFsbl_Out32(CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_SIZE_REG, Size);
-}
-
-/*****************************************************************************/
-/**
- *
- * This function will stop/pause the CSU DMA.
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_CsuDmaStop(u32 Flags)
-{
-	/**
-	 * This function is TBD.
-	 */
-
-}
-
-/*****************************************************************************/
-/**
- *
- * This function will wait for CSU DMA done.
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_CsuDmaWaitForDone(XFSBL_CSU_DMA_CHANNEL Channel)
-{
-	volatile u32 Status = 0;
-
-	/** ASM Code */
-	mb();
-	/* Busy wait for the DMA.  */
-	/**
-	 * TBD: May need to have some timeout for this loop.
-	 */
-	do
-	{
-		Status = XFsbl_In32(CSUDMA_BASEADDR + Channel +
-				XFSBL_CSU_DMA_STATUS_REG);
-
-	} while (Status & XFSBL_CSU_DMA_STATUS_BUSY);
-
-}
-
-/*****************************************************************************/
-/**
- *
- * This function will start the dma transfer
- * and wait for DST DMA done.
- *
- * @param       None
- *
- * @return      None
- *
- ******************************************************************************/
-void XFsbl_CsuDmaXfer(u32 SrcAddr, u32 DestAddr, u32 ImgLen)
-{
-	XFsbl_Printf(DEBUG_INFO,"In XCbr_CsuDmaXfer: SrcAddr:0x%x "
-		"DestAddr:0x%x  ImgLen:0x%x\r\n",SrcAddr,DestAddr,ImgLen);
-
-	XFsbl_CsuDmaStart(XFSBL_CSU_DMA_DST, DestAddr, ((ImgLen/4) << 2) );
-
-	/**
-	 * In case of PSTP, the Src DMA channel is not used.
-	 * For this function SrcAddr == 0 means we are calling
-	 * this function for PS_TEST boot mode
-	 */
-	if(0 != SrcAddr)
-	{
-		XFsbl_CsuDmaStart(XFSBL_CSU_DMA_SRC, SrcAddr, (ImgLen/4) << 2);
-		XFsbl_CsuDmaWaitForDone(XFSBL_CSU_DMA_SRC);
+	Status = XCsuDma_CfgInitialize(&CsuDma, CsuDmaConfig,
+			CsuDmaConfig->BaseAddress);
+	if (Status != XFSBL_SUCCESS) {
+		XFsbl_Printf(DEBUG_GENERAL, "XFSBL_ERROR_CSUDMA_INIT_FAIL \n\r");
+		Status = XFSBL_ERROR_CSUDMA_INIT_FAIL;
+		goto END;
 	}
-
-	/**
-	 * TBD: Need to enhance this to return
-	 * timeout error
-	 */
-	XFsbl_CsuDmaWaitForDone(XFSBL_CSU_DMA_DST);
-
+END:
+	return Status;
 }
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.h
index d73d2cb9..9acf9ce9 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.h
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_csu_dma.h
@@ -35,7 +35,7 @@
 *
 * @file xcbr_csu_dma.h
 *
-* Contains enums/typedefs and macros for CSU DMA driver.
+* Contains declarations for CSU DMA initialization
 *
 * <pre>
 * MODIFICATION HISTORY:
@@ -59,217 +59,13 @@ extern "C" {
 
 /***************************** Include Files *********************************/
 #include "xfsbl_hw.h"
-
 /**************************** Macros Definitions *****************************/
-#define XASSERT_VOID(expression)
-#define mb() 	asm("dsb sy")
 
-u8 XFsbl_RsaSha3Array[512];
 /**************************** Type Definitions *******************************/
 
-/**
-* Definition for DMA channels.
-*/
-typedef enum
-{
-	XFSBL_CSU_DMA_SRC = 0x000U,
-    XFSBL_CSU_DMA_DST = 0x800U
-}XFSBL_CSU_DMA_CHANNEL;
-
-/**
-* Definition for DMA registers.
-*/
-typedef enum
-{
-    XFSBL_CSU_DMA_ADDR_REG	 	  = 0x00U,
-    XFSBL_CSU_DMA_SIZE_REG  	 	  = 0x04U,
-    XFSBL_CSU_DMA_STATUS_REG	 	  = 0x08U,
-    XFSBL_CSU_DMA_CTRL_REG         = 0x0CU,
-    XFSBL_CSU_DMA_CRC_REG          = 0x10U,
-    XFSBL_CSU_DMA_INT_STATUS_REG	  = 0x14U,
-    XFSBL_CSU_DMA_INT_ENABLE_REG   = 0x18U,
-    XFSBL_CSU_DMA_INT_DISABLE_REG  = 0x1CU,
-    XFSBL_CSU_DMA_INT_MASK_REG     = 0x20U,
-}XFSBL_CSU_DMA_REG_OFFSET;
-
-/**
-* Definition for DMA EOP.
-*/
-enum
-{
-    XFSBL_CSU_DMA_SIZE_EOP         = 0x1U
-};
-
-/**
-* Definition for DMA Status reg bits.
-*/
-typedef enum
-{
-    XFSBL_CSU_DMA_STATUS_BUSY      		= (0x1U << 0x0U),
-    XFSBL_CSU_DMA_STATUS_DONE_CNT_SHIFT 	= (13U),
-    XFSBL_CSU_DMA_STATUS_DONE_CNT_MASK 	= (0x7U << XFSBL_CSU_DMA_STATUS_DONE_CNT_SHIFT)
-}XFSBL_CSU_DMA_STATUS;
-
-/**
-* Definition for DMA Control reg bits.
-*/
-typedef enum
-{
-	XFSBL_CSU_DMA_CTRL_PAUSE_MEM              = (0x1U << 0x0U),
-    XFSBL_CSU_DMA_CTRL_PAUSE_STREAM           = (0x1U << 0x1U),
-    XFSBL_CSU_DMA_CTRL_FIFO_THRESH_SHIFT      = 0x2U,
-    XFSBL_CSU_DMA_CTRL_TIMEOUT_SHIFT          = 10U,
-    XFSBL_CSU_DMA_CTRL_AXI_BURST_INCR         = (0x0U << 22U),
-    XFSBL_CSU_DMA_CTRL_AXI_BURST_FIXED        = (0x1U << 22U),
-    XFSBL_CSU_DMA_CTRL_ENDIANNESS             = (0x1U << 23U),
-    XFSBL_CSU_DMA_CTRL_ERR_RESPONSE           = (0x1U << 24U),
-    XFSBL_CSU_DMA_CTRL_IF_FIFO_THRESH_SHIFT   = 25U
-}XFSBL_CSU_DMA_CTRL;
-
-/**
-* Definition for DMA Interrupt reg bits.
-*/
-typedef enum
-{
-    XFSBL_CSU_DMA_INT_INVALID_APB_ACCESS          = (0x1U << 0x5U),
-    XFSBL_CSU_DMA_INT_FIFO_THRESH_HIT             = (0x1U << 0x4U),
-    XFSBL_CSU_DMA_INT_DMA_TIMEOUT                 = (0x1U << 0x3U),
-    XFSBL_CSU_DMA_INT_AXI_RD__ERR                 = (0x1U << 0x2U),
-    XFSBL_CSU_DMA_INT_DMA_DONE                    = (0x1U << 0x1U),
-    XFSBL_CSU_DMA_INT_DMA_MEM_DONE                = (0x1U << 0x0U)
-}XFSBL_CSU_DMA_INT;
-
-/*
- * Definition for SSS reg Source bits.
- */
-typedef enum
-{
-    XFSBL_CSU_SSS_SRC_PCAP 		= 0x3U,
-    XFSBL_CSU_SSS_SRC_SRC_DMA 	= 0x5U,
-    XFSBL_CSU_SSS_SRC_AES  		= 0xAU,
-    XFSBL_CSU_SSS_SRC_PSTP 		= 0xCU,
-    XFSBL_CSU_SSS_SRC_NONE 		= 0x0U,
-	XFSBL_CSU_SSS_SRC_MASK 		= 0xFU
-}XFSBL_CSU_SSS_SRC;
-
-/**
-* Definition for SSS reg Destination bits.
-*/
-typedef enum
-{
-    XFSBL_CSU_SSS_PCAP_SHIFT = 0U,
-    XFSBL_CSU_SSS_DMA_SHIFT = 4U,
-    XFSBL_CSU_SSS_AES_SHIFT = 8U,
-    XFSBL_CSU_SSS_SHA_SHIFT = 12U,
-    XFSBL_CSU_SSS_PSTP_SHIFT = 16U
-}XFSBL_CSU_SSS_DEST_SHIFT;
-
-
 /**************************** Macros Definitions *****************************/
 
-/**
-* Definition for DMA inline functions
-*/
-static inline void XFsbl_DmaControl(XFSBL_CSU_DMA_CHANNEL Channel, u32 Val)
-{
-	XFsbl_Out32((CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_CTRL_REG), Val);
-}
-
-static inline void XFsbl_DmaEndian(XFSBL_CSU_DMA_CHANNEL Channel, u32 Enable)
-{
-	u32 Value = XFsbl_In32(CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_CTRL_REG);
-	if(Enable)
-	{
-		Value |= XFSBL_CSU_DMA_CTRL_ENDIANNESS;
-	}
-	else
-	{
-		Value &= ~XFSBL_CSU_DMA_CTRL_ENDIANNESS;
-	}
-
-	XFsbl_Out32((CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_CTRL_REG), Value);
-}
-
-static inline int XFsbl_DmaIsBusy(XFSBL_CSU_DMA_CHANNEL Channel)
-{
-	return (XFsbl_In32(CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_STATUS_REG) &
-			XFSBL_CSU_DMA_STATUS_BUSY);
-}
-
-static inline unsigned int XFsbl_DmaDoneCount(XFSBL_CSU_DMA_CHANNEL Channel)
-{
-	u32 Count;
-
-	Count = XFsbl_In32(CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_STATUS_REG);
-	Count &= XFSBL_CSU_DMA_STATUS_DONE_CNT_MASK;
-	Count >>= XFSBL_CSU_DMA_STATUS_DONE_CNT_SHIFT;
-	return Count;
-}
-
-static inline void XFsbl_DmaZeroDoneCount(XFSBL_CSU_DMA_CHANNEL Channel)
-{
-	XFsbl_Out32(CSUDMA_BASEADDR + Channel + XFSBL_CSU_DMA_STATUS_REG,
-			XFSBL_CSU_DMA_STATUS_DONE_CNT_MASK);
-	/** ASM Code */
-	mb();
-}
-
-/* The CRC function is only available on SRC channels.  */
-static inline u32 XFsbl_DmaSrcGetCrc(void)
-{
-	return (XFsbl_In32(CSUDMA_BASEADDR + XFSBL_CSU_DMA_SRC +
-				XFSBL_CSU_DMA_CRC_REG));
-}
-
-static inline void XFsbl_DmaSrcSetCrc(u32 Crc)
-{
-	XFsbl_Out32(CSUDMA_BASEADDR + XFSBL_CSU_DMA_SRC +
-					XFSBL_CSU_DMA_CRC_REG, Crc);
-	/** ASM Code */
-	mb();
-}
-
-void XFsbl_CsuDmaWaitForDone(XFSBL_CSU_DMA_CHANNEL Channel);
-void XFsbl_CsuDmaStart(XFSBL_CSU_DMA_CHANNEL Channel, u32 Addr, u32 Size);
-void XFsbl_CsuDmaXfer(u32 SrcAddr, u32 DestAddr, u32 ImgLen);
-
-
-/**
-* Definition for SSS inline functions
-*/
-static inline u32 XFsbl_SssInputPcap(XFSBL_CSU_SSS_SRC Src)
-{
-    Src &= XFSBL_CSU_SSS_SRC_MASK;
-    return (Src << XFSBL_CSU_SSS_PCAP_SHIFT);
-}
-
-static inline u32 XFsbl_SssInputDstDma(XFSBL_CSU_SSS_SRC Src)
-{
-    Src &= XFSBL_CSU_SSS_SRC_MASK;
-    return (Src << XFSBL_CSU_SSS_DMA_SHIFT);
-}
-
-static inline u32 XFsbl_SssInputAes(XFSBL_CSU_SSS_SRC Src)
-{
-    Src &= XFSBL_CSU_SSS_SRC_MASK;
-    return (Src << XFSBL_CSU_SSS_AES_SHIFT);
-}
-
-static inline u32 XFsbl_SssInputSha3(XFSBL_CSU_SSS_SRC Src)
-{
-    Src &= XFSBL_CSU_SSS_SRC_MASK;
-    return (Src << XFSBL_CSU_SSS_SHA_SHIFT);
-}
-
-static inline u32 XFsbl_SssInputPstp(XFSBL_CSU_SSS_SRC Src)
-{
-    Src &= XFSBL_CSU_SSS_SRC_MASK;
-    return (Src << XFSBL_CSU_SSS_PSTP_SHIFT);
-}
-
-static inline void XFsbl_SssSetup(u32 Cfg)
-{
-    XFsbl_Out32(CSU_CSU_SSS_CFG, Cfg);
-}
+/************************** Function Prototypes ******************************/
+u32 XFsbl_CsuDmaInit();
 
 #endif /* XFSBL_CSU_DMA_H*/
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_error.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_error.h
index baa7f6bf..024cbe35 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_error.h
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_error.h
@@ -139,13 +139,23 @@ extern "C" {
 #define XFSBL_ERROR_NAND_INIT			        (0x2CU)
 #define XFSBL_ERROR_NAND_READ			        (0x2DU)
 #define XFSBL_ERROR_ADDRESS				(0x2EU)
-#define XFSBL_ERROR_RSA_NOT_ENABLED			(0x2DU)
 #define XFSBL_ERROR_SPK_RSA_DECRYPT			(0x2FU)
 #define XFSBL_ERROR_SPK_SIGNATURE			(0x30U)
 #define XFSBL_ERROR_PART_RSA_DECRYPT			(0x31U)
 #define XFSBL_ERROR_PART_SIGNATURE			(0x32U)
 #define XFSBL_ERROR_DDR_INIT_FAIL			(0x33U)
 
+#define XFSBL_ERROR_CSUDMA_INIT_FAIL                (0x34U)
+#define XFSBL_ERROR_DECRYPTION_IV_COPY_FAIL         (0x35U)
+#define XFSBL_ERROR_DECRYPTION_FAIL                 (0x36U)
+#define XFSBL_ERROR_BITSTREAM_LOAD_FAIL             (0x37U)
+#define XFSBL_ERROR_BITSTREAM_GCM_TAG_MISMATCH      (0x38U)
+#define XFSBL_ERROR_DECRYPTION_IMAGE_LENGTH_MISMATCH    (0x39U)
+#define XFSBL_ERROR_BITSTREAM_DECRYPTION_FAIL       (0x40U)
+#define XFSBL_ERROR_DECRYPTION_FAILED               (0x41U)
+#define XFSBL_ERROR_RSA_NOT_ENABLED                 (0x42U)
+#define XFSBL_ERROR_AES_NOT_ENABLED                 (0x43U)
+
 #define XFSBL_FAILURE					(0x3FFU)
 
 /**************************** Type Definitions *******************************/
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h
index 65af9ddd..a7eef859 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h
@@ -474,6 +474,19 @@ extern "C" {
 #define XFSBL_RSA
 #endif
 
+/**
+ * Definition for AES to be included
+ */
+#if !defined(FSBL_AES_EXCLUDE)
+#define XFSBL_AES
+#endif
+
+/**
+ * Definition for SHA2 to be included
+ */
+#if !defined(FSBL_SHA2_EXCLUDE)
+#define XFSBL_SHA2
+#endif
 
 #define XFSBL_QSPI_LINEAR_BASE_ADDRESS_START		(0xC0000000U)
 #define XFSBL_QSPI_LINEAR_BASE_ADDRESS_END		(0xDFFFFFFFU)
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_image_header.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_image_header.h
index 5b52f5a0..88cab46f 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_image_header.h
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_image_header.h
@@ -78,6 +78,8 @@ extern "C" {
 
 #define XIH_BH_IMAGE_ATTRB_RSA_MASK	(0xC000U)
 #define XIH_BH_IMAGE_ATTRB_SHA2_MASK	(0x3000U)
+#define XIH_BH_IV_OFFSET       		(0xA0U)
+#define XIH_BH_IV_LENGTH   			(0x10U)
 
 /**
  * Defines for length of the headers
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_initialization.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_initialization.c
index 93cdebaf..f2384a98 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_initialization.c
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_initialization.c
@@ -56,8 +56,8 @@
 #include "xfsbl_main.h"
 #include "xfsbl_misc_drivers.h"
 #include "psu_init.h"
-
 #include "xfsbl_qspi.h"
+#include "xfsbl_csu_dma.h"
 
 /************************** Constant Definitions *****************************/
 #define XFSBL_R5_VECTOR_VALUE 	0xEAFEFFFEU
@@ -87,6 +87,7 @@ void XFsbl_RegisterHandlers(void);
 
 /************************** Variable Definitions *****************************/
 
+
 /****************************************************************************/
 /**
  * This function is used to initialize the FsblInstance with the
@@ -442,6 +443,12 @@ static u32 XFsbl_PrimaryBootDeviceInit(XFsblPs * FsblInstancePtr)
 			goto END;
 		}
 #endif
+
+		/* Initialize CSUDMA driver */
+		Status = XFsbl_CsuDmaInit();
+		if (XFSBL_SUCCESS != Status) {
+			goto END;
+		}
 	}
 
 	switch(BootMode)
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_partition_load.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_partition_load.c
index e866c688..8ffe7ede 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_partition_load.c
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_partition_load.c
@@ -92,6 +92,9 @@ static int IsR5LTcmEccInitialized = FALSE;
 u8 TcmVectorArray[32];
 u32 TcmSkipLength=0U;
 PTRSIZE TcmSkipAddress=0U;
+#ifdef XFSBL_AES
+static XSecure_Aes SecureAes;
+#endif
 /*****************************************************************************/
 /**
  * This function loads the partition
@@ -882,12 +885,16 @@ static u32 XFsbl_PartitionValidation(XFsblPs * FsblInstancePtr,
 	u32 DestinationCpu=0U;
 	u32 ExecState=0U;
 	u32 CpuNo=0U;
+	u32 ImageOffset = 0U;
 #ifdef XFSBL_RSA
 	u32 Length=0U;
 	u32 HashLen=0U;
-	u64 LoadAddress=0U;
 #endif
+	u64 LoadAddress=0U;
 	XFsblPs_PartitionHeader * PartitionHeader;
+	u32 FsblIv[XIH_BH_IV_LENGTH / 4U];
+	u32 UnencryptedLength;
+	u32 IvLocation;
 
 	/**
 	 * Update the variables
@@ -903,6 +910,25 @@ static u32 XFsbl_PartitionValidation(XFsblPs * FsblInstancePtr,
 			XIH_PH_ATTRB_ENCRYPTION )
 	{
 		IsEncryptionEnabled = TRUE;
+
+#ifdef XFSBL_AES
+		/* Copy the Iv from Flash into local memory */
+		IvLocation = ImageOffset + XIH_BH_IV_OFFSET;
+
+		Status = FsblInstancePtr->DeviceOps.DeviceCopy(IvLocation,
+				(PTRSIZE) FsblIv, XIH_BH_IV_LENGTH);
+
+		if (Status != XFSBL_SUCCESS) {
+			XFsbl_Printf(DEBUG_GENERAL,
+					"XFSBL_ERROR_DECRYPTION_IV_COPY_FAIL \r\n");
+			Status = XFSBL_ERROR_DECRYPTION_IV_COPY_FAIL;
+			goto END;
+		}
+#else
+		XFsbl_Printf(DEBUG_GENERAL,"XFSBL_ERROR_AES_NOT_ENABLED \r\n");
+		Status = XFSBL_ERROR_AES_NOT_ENABLED;
+		goto END;
+#endif
 	}
 
 	/**
@@ -979,19 +1005,16 @@ static u32 XFsbl_PartitionValidation(XFsblPs * FsblInstancePtr,
 		/**
 		 * Do the authentication validation
 		 */
-		Status = XFsbl_Authentication( LoadAddress, Length,
+		Status = XFsbl_Authentication(FsblInstancePtr, LoadAddress, Length,
 			(LoadAddress + Length) - XFSBL_AUTH_CERT_MIN_SIZE,
 			HashLen);
 
-		/* cache enable can be removed */
-		Xil_DCacheEnable();
-
 		if (Status != XFSBL_SUCCESS)
                 {
                         goto END;
                 }
 #else
-		XFsbl_Printf(DEBUG_GENERAL,"Rsa code not Enabled\r\n");
+		XFsbl_Printf(DEBUG_GENERAL,"XFSBL_ERROR_RSA_NOT_ENABLED \r\n");
 		Status = XFSBL_ERROR_RSA_NOT_ENABLED;
 		goto END;
 #endif
@@ -1004,13 +1027,37 @@ static u32 XFsbl_PartitionValidation(XFsblPs * FsblInstancePtr,
 	    (DestinationDevice == XIH_PH_ATTRB_DEST_DEVICE_PMU)) &&
 	    (IsEncryptionEnabled == TRUE))
 	{
-		/**
-		 * configure SSS
-		 */
+		XFsbl_Printf(DEBUG_INFO, "Decryption Enabled\r\n");
+#ifdef XFSBL_AES
 
-		/**
-		 * Use CSU DMA and decrypt the image
-		 */
+		/* AES expects IV in big endian form */
+		FsblIv[0] = Xil_Htonl(FsblIv[0]);
+		FsblIv[1] = Xil_Htonl(FsblIv[1]);
+		FsblIv[2] = Xil_Htonl(FsblIv[2]);
+
+		/* Initialize the Aes Instance so that it's ready to use */
+		XSecure_AesInitialize(&SecureAes, &CsuDma,
+		 XSECURE_CSU_AES_KEY_SRC_DEV, FsblIv, NULL);
+
+		XFsbl_Printf(DEBUG_INFO, " Aes initialized \r\n");
+
+		UnencryptedLength = PartitionHeader->UnEncryptedDataWordLength * 4U;
+
+		Status = XSecure_AesDecrypt(&SecureAes, (u8 *) LoadAddress,
+				(u8 *) LoadAddress, UnencryptedLength);
+
+		if (Status != XFSBL_SUCCESS) {
+			Status = XFSBL_ERROR_DECRYPTION_FAILED;
+			XFsbl_Printf(DEBUG_GENERAL, "XFSBL_ERROR_DECRYPTION_FAILED\r\n");
+			goto END;
+		} else {
+			XFsbl_Printf(DEBUG_GENERAL, "Decryption Successful\r\n");
+		}
+#else
+		XFsbl_Printf(DEBUG_GENERAL,"XFSBL_ERROR_AES_NOT_ENABLED \r\n");
+		Status = XFSBL_ERROR_AES_NOT_ENABLED;
+		goto END;
+#endif
 	}
 
 	/**
diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_rsa_sha.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_rsa_sha.c
index 73d6e45e..e27e5613 100644
--- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_rsa_sha.c
+++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_rsa_sha.c
@@ -53,7 +53,8 @@
 
 /***************************** Include Files *********************************/
 #include "xfsbl_authentication.h"
-#include "xfsbl_csu_dma.h"
+#ifdef XFSBL_RSA
+
 /************************** Constant Definitions *****************************/
 
 /**************************** Type Definitions *******************************/
@@ -63,374 +64,7 @@
 /************************** Function Prototypes ******************************/
 
 /************************** Variable Definitions *****************************/
-
-static u32 XFsbl_Sha3Len;
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_Sha3Padd(u8 *Dst, u32 MsgLen)
-{
-	/**
-	 * SHA3 HASH value is not generated correctly
-	 * 		when used 2nd time on REMUS 1.9
-	 */
-	XFsbl_MemSet(Dst, 0, MsgLen);
-	Dst[0] = 0x1;
-	Dst[MsgLen -1] |= 0x80;
-}
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_Sha3Start(void)
-{
-
-	XFsbl_Sha3Len = 0;
-
-	/**
-	 * Reset SHA3 engine.
-	 */
-	XFsbl_Out32(CSU_SHA_RESET, CSU_SHA_RESET_RESET_MASK);
-	XFsbl_Out32(CSU_SHA_RESET, 0);
-
-	/**
-	 * Configure the SSS for SHA3 hashing.
-	 */
-	XFsbl_SssSetup(XFsbl_SssInputSha3(XFSBL_CSU_SSS_SRC_SRC_DMA));
-
-	/**
-	 * Start SHA3 engine.
-	 */
-	XFsbl_Out32(CSU_SHA_START, CSU_SHA_START_START_MSG_MASK);
-}
-
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_Sha3Update(u8 * Data, u32 Size)
-{
-
-	XFsbl_Sha3Len += Size;
-
-	XASSERT_VOID((Size & 3) == 0);
-
-	XFsbl_CsuDmaStart(XFSBL_CSU_DMA_SRC, (u32 )(PTRSIZE)Data, Size);
-
-	/* Checking the SHA3 done bit should be enough.  */
-	XFsbl_CsuDmaWaitForDone(XFSBL_CSU_DMA_SRC);
-
-}
-
-
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_Sha3WaitForDone(void)
-{
-	volatile u32 Status;
-
-	do
-	{
-		Status = XFsbl_In32(CSU_SHA_DONE);
-	} while (CSU_SHA_DONE_SHA_DONE_MASK !=
-			(Status & CSU_SHA_DONE_SHA_DONE_MASK));
-}
-
-
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_Sha3Finish(u8 *Hash)
-{
-	u32 *HashPtr = (u32 *)Hash;
-	u32 PartialLen = XFsbl_Sha3Len % XFSBL_SHA3_BLOCK_LEN;
-
-	/**
-	 * DT 780552 (SHA3 HASH value is not generated correctly
-	 * 		when used 2nd time on REMUS 1.9)
-	 */
-	PartialLen = (PartialLen == 0)?(XFSBL_SHA3_BLOCK_LEN) :
-		(XFSBL_SHA3_BLOCK_LEN - PartialLen);
-
-	XFsbl_Sha3Padd(XFsbl_RsaSha3Array, PartialLen);
-
-	XFsbl_CsuDmaStart(XFSBL_CSU_DMA_SRC, (u32 )(PTRSIZE)XFsbl_RsaSha3Array,
-			PartialLen | XFSBL_CSU_DMA_SIZE_EOP);
-
-	/* Check the SHA3 DONE bit.  */
-	XFsbl_Sha3WaitForDone();
-
-
-	/* If requested, read out the Hash in reverse order.  */
-	if (Hash)
-	{
-		u32 Index = 0;
-		u32 Val = 0;
-		for (Index=0; Index < 12; Index++)
-		{
-			Val = XFsbl_In32(CSU_SHA_DIGEST_0 + (Index * 4));
-			HashPtr[11 - Index] = Val;
-		}
-	}
-
-}
-
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-void XFsbl_Sha3Digest(const u8 *In, const u32 Size, u8 *Out)
-{
-
-	XFsbl_Sha3Start();
-	XFsbl_Sha3Update((u8 *)In, Size);
-	XFsbl_Sha3Finish(Out);
-
-}
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-static void XFsbl_RsaPutData(u32 * WrData, u8 RamOffset)
-{
-	u32 Index = 0;
-	u32 DataOffset = 0;
-	u32 TmpIndex = 0;
-	u32 Data = 0;
-
-
-	/** Each of this loop will write 192 bits of data*/
-	for (DataOffset = 0; DataOffset < 22; DataOffset++)
-	{
-		for (Index = 0; Index < 6; Index++)
-		{
-			TmpIndex = (DataOffset*6) + Index;
-			/**
-			 * DT 776670: CSU Boot ROM fails with error code 0x30E
-			 * (RSA status register give done with error 0x5)
-			 * Added this condition as we need only 4 bytes
-			 * and rest of the data needs to be 0
-			 */
-			if(XFSBL_CSU_RSA_RAM_EXPO == RamOffset)
-			{
-				if(0 == TmpIndex )
-				{
-					Data = XFsbl_Htonl(*WrData);
-				}
-				else
-				{
-					Data = 0x0;
-				}
-			}
-			else
-			{
-				if(TmpIndex >=128)
-				{
-					Data = 0x0;
-				}
-				else
-				{
-					/**
-					 * The RSA data in Image is in Big Endian.
-					 * So reverse it before putting in RSA memory,
-					 * becasue RSA h/w expects it in Little endian.
-					 */
-
-					Data = XFsbl_Htonl(WrData[127-TmpIndex]);
-				}
-			}
-			XFsbl_Out32((RSA_WR_DATA_0 + Index * 4), Data);
-		}
-		XFsbl_Out32(RSA_WR_ADDR, ((RamOffset * 22) + DataOffset));
-	}
-}
-
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-static void XFsbl_RsaGetData(u32 * RdData)
-{
-	u32 Index = 0;
-	u32 DataOffset = 0;
-	int TmpIndex = 0;
-
-
-	/** Each of this loop will write 192 bits of data*/
-	for (DataOffset = 0; DataOffset < 22; DataOffset++)
-	{
-		XFsbl_Out32(RSA_RD_ADDR,
-				(XFSBL_CSU_RSA_RAM_RES_Y * 22) + DataOffset);
-
-		Index = (DataOffset == 0) ? 2: 0;
-		for (; Index < 6; Index++)
-		{
-			TmpIndex = 129 - ((DataOffset*6) + Index);
-			if(TmpIndex < 0)
-			{
-				break;
-			}
-			/**
-			 * The Signature digest is compared in Big endian.
-			 * So becasue RSA h/w results in Little endian,
-			 * reverse it after reading it from RSA memory,
-			 */
-			RdData[TmpIndex] = XFsbl_Htonl(
-						XFsbl_In32(RSA_RD_DATA_0
-							+ (Index * 4)));
-		}
-	}
-
-}
-
-/*****************************************************************************
- *
- * @param	u32 *Mod
- *
- * @return	None
- *
- * @notes	MINV is the 32-bit value of "-M mod 2**32"
- *			where M is LSB 32 bits of the original modulus
- * 			DT 776670: CSU Boot ROM fails with error code 0x30E
- * 			(RSA status register give done with error 0x5)
- *
- ******************************************************************************/
-
-static void XFsbl_Mod32Inverse(u32 *Mod)
-{
-	/** Calculate the MINV*/
-	u8 Count = 0;
-	u32 ModVal = XFsbl_Htonl(Mod[127]);
-	u32 Inv = 2 - ModVal;
-
-	for (Count = 0; Count < 4; ++Count)
-	{
-		Inv = (Inv * (2- ( ModVal * Inv ) ) );
-	}
-
-	Inv = -Inv;
-
-	/** Put the value in MINV registers*/
-	XFsbl_Out32(RSA_CORE_MINV0, (Inv & 0xFF ));
-	XFsbl_Out32(RSA_CORE_MINV1, ((Inv >> 8) & 0xFF ));
-	XFsbl_Out32(RSA_CORE_MINV2, ((Inv >> 16) & 0xFF ));
-	XFsbl_Out32(RSA_CORE_MINV3, ((Inv >> 24) & 0xFF ));
-}
-
-/*****************************************************************************
- *
- * @param	None
- *
- * @return	None
- *
- ******************************************************************************/
-u32 XFsbl_RsaDecrypt(u8* EncText, u8* Mod, u8* ModExt, u8* ModExpo, u8* Result, u8 Reuse)
-{
-	volatile u32 Status = XFSBL_SUCCESS;
-	u32 ErrorCode = XFSBL_SUCCESS;
-
-	/**
-	 * Populate the RSA parameters in h/w.
-	 */
-
-	/**
-	 * We will reuse the RSA config values,
-	 * incase of FSBL Signature Authentication,
-	 * because Boot header Signautre Authentication,
-	 * also uses the same SPK Key.
-	 */
-	if(0 == Reuse)
-	{
-		/**
-		 * DT 776670: CSU Boot ROM fails with error code 0x30E
-		 * (RSA status register give done with error 0x5)
-		 * Initialize Modular exponentiation
-		 */
-		XFsbl_RsaPutData((u32 *)ModExpo, XFSBL_CSU_RSA_RAM_EXPO);
-
-
-		/**
-		 * Initialize Modular.
-		 */
-		XFsbl_RsaPutData((u32 *)Mod, XFSBL_CSU_RSA_RAM_MOD);
-
-
-		/**
-		 * DT 776670: CSU Boot ROM fails with error code 0x30E
-		 * (RSA status register give done with error 0x5)
-		 * Initialize MINV values from Mod.
-		 */
-		XFsbl_Mod32Inverse((u32 *)Mod);
-	}
-
-	/**
-	 * Initialize Modular extension (R*R Mod M)
-	 */
-	XFsbl_RsaPutData((u32 *)ModExt, XFSBL_CSU_RSA_RAM_RES_Y);
-
-	/**
-	 * Initialize Digest
-	 */
-	XFsbl_RsaPutData((u32 *)EncText, XFSBL_CSU_RSA_RAM_DIGEST);
-
-
-	/**
-	 * Start the RSA operation.
-	 */
-	XFsbl_Out32(RSA_CORE_CTRL, XFSBL_CSU_RSA_CONTROL_MASK);
-
-	/**
-	 * Check and wait for status
-	 */
-	do
-	{
-		Status = XFsbl_In32(RSA_CORE_STATUS);
-		if(RSA_CORE_STATUS_ERROR_MASK ==
-			(Status & RSA_CORE_STATUS_ERROR_MASK))
-		{
-			ErrorCode = XFSBL_FAILURE;
-			goto END;
-		}
-	}while (RSA_CORE_STATUS_DONE_MASK !=
-		(Status & RSA_CORE_STATUS_DONE_MASK));
-
-
-	/**
-	 * Copy the result
-	 */
-	XFsbl_RsaGetData((u32 *)Result);
-
-END:
-	return ErrorCode;
-}
+static XSecure_Sha3 SecureSha3;
 
 /*****************************************************************************
  *
@@ -444,7 +78,13 @@ void XFsbl_ShaDigest(const u8 *In, const u32 Size, u8 *Out, u32 HashLen)
 
 	if(XFSBL_HASH_TYPE_SHA3 == HashLen)
 	{
-		XFsbl_Sha3Digest(In, Size, Out);
+		XSecure_Sha3Digest(&SecureSha3, In, Size, Out);
+	}
+	else
+	{
+#ifdef XFSBL_SHA2
+		sha_256(In, Size, Out);
+#endif
 	}
 }
 
@@ -456,11 +96,19 @@ void XFsbl_ShaDigest(const u8 *In, const u32 Size, u8 *Out, u32 HashLen)
  * @return	None
  *
  ******************************************************************************/
-void XFsbl_ShaStart( void * Ctx, u32 HashLen)
+void XFsbl_ShaStart(void * Ctx, u32 HashLen)
 {
 	if(XFSBL_HASH_TYPE_SHA3 == HashLen)
 	{
-		XFsbl_Sha3Start();
+		XSecure_Sha3Initialize(&SecureSha3, &CsuDma);
+
+		XSecure_Sha3Start(&SecureSha3);
+	}
+	else
+	{
+#ifdef XFSBL_SHA2
+		sha2_starts(Ctx);
+#endif
 	}
 }
 
@@ -472,11 +120,17 @@ void XFsbl_ShaStart( void * Ctx, u32 HashLen)
  * @return	None
  *
  ******************************************************************************/
-void XFsbl_ShaUpdate( void * Ctx, u8 * Data, u32 Size, u32 HashLen)
+void XFsbl_ShaUpdate(void * Ctx, u8 * Data, u32 Size, u32 HashLen)
 {
 	if(XFSBL_HASH_TYPE_SHA3 == HashLen)
 	{
-		XFsbl_Sha3Update(Data, Size);
+		XSecure_Sha3Update(&SecureSha3, Data, Size);
+	}
+	else
+	{
+#ifdef XFSBL_SHA2
+		sha2_update(Ctx, Data, Size);
+#endif
 	}
 }
 
@@ -487,11 +141,19 @@ void XFsbl_ShaUpdate( void * Ctx, u8 * Data, u32 Size, u32 HashLen)
  * @return	None
  *
  ******************************************************************************/
-void XFsbl_ShaFinish( void * Ctx, u8 * Hash, u32 HashLen)
+void XFsbl_ShaFinish(void * Ctx, u8 * Hash, u32 HashLen)
 {
 
 	if(XFSBL_HASH_TYPE_SHA3 == HashLen)
 	{
-		XFsbl_Sha3Finish(Hash);
+		XSecure_Sha3Finish(&SecureSha3, Hash);
+	}
+	else
+	{
+#ifdef XFSBL_SHA2
+		sha2_finish(Ctx, Hash);
+#endif
 	}
 }
+
+#endif