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 * *
  * 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
 *
 * 
 * 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