bsps: Import Xilinx NAND driver

This adds Xilinx's driver for the Xilinx NAND controller embedded in the
ZynqMP SoC. Within that device alone, it is possible to access this
peripheral from MicroBlaze, ARMv7, and ARMv8 cores. This has been added
to the hardware ZynqMP BSPs since QEMU does not support emulation of
this peripheral. This driver supports polled operation only. The
imported files are and should be able to remain unmodified. Import
information is kept in bsps/shared/dev/nand/VERSION.

6 files changed, 4062 insertions, 0 deletions

diff --git a/bsps/shared/dev/nand/VERSION b/bsps/shared/dev/nand/VERSION
new file mode 100644
index 0000000000..c0afe6e031
--- /dev/null
+++ b/bsps/shared/dev/nand/VERSION
@@ -0,0 +1,20 @@
+The information in this file describes the source of files in
+bsps/shared/dev/nand/ and bsps/include/dev/nand/.
+
+Import from:
+
+https://github.com/Xilinx/embeddedsw.git
+
+commit 8a89579489c88ea5acd23d7d439ac928659c26cf
+Author:     msreeram <manikanta.sreeram@xilinx.com>
+AuthorDate: Wed Apr 6 23:24:38 2022 -0600
+Commit:     Siva Addepalli <sivaprasad.addepalli@xilinx.com>
+CommitDate: Fri Apr 8 16:47:15 2022 +0530
+
+    update license file for EmbeddedSW 2022.1 release
+
+    Update license file for EmbeddedSW 2022.1 release
+
+    Signed-off-by: Manikanta Sreeram <msreeram@xilinx.com>
+
+    Acked-by : Meena Paleti <meena.paleti@xilinx.com>
diff --git a/bsps/shared/dev/nand/xnandpsu.c b/bsps/shared/dev/nand/xnandpsu.c
new file mode 100644
index 0000000000..a2e2235906
--- /dev/null
+++ b/bsps/shared/dev/nand/xnandpsu.c
@@ -0,0 +1,2922 @@
+/******************************************************************************
+* Copyright (C) 2015 - 2022 Xilinx, Inc.  All rights reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xnandpsu.c
+* @addtogroup nandpsu_v1_10
+* @{
+*
+* This file contains the implementation of the interface functions for
+* XNandPsu driver. Refer to the header file xnandpsu.h for more detailed
+* information.
+*
+* This module supports for NAND flash memory devices that conform to the
+* "Open NAND Flash Interface" (ONFI) 3.0 Specification. This modules
+* implements basic flash operations like read, write and erase.
+*
+* @note		Driver has been renamed to nandpsu after change in
+*		naming convention.
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date	   Changes
+* ----- ----   ----------  -----------------------------------------------
+* 1.0   nm     05/06/2014  First release
+* 2.0   sb     01/12/2015  Removed Null checks for Buffer passed
+*			   as parameter to Read API's
+*			   - XNandPsu_Read()
+*			   - XNandPsu_ReadPage
+*			   Modified
+*			   - XNandPsu_SetFeature()
+*			   - XNandPsu_GetFeature()
+*			   and made them public.
+*			   Removed Failure Return for BCF Error check in
+*			   XNandPsu_ReadPage() and added BCH_Error counter
+*			   in the instance pointer structure.
+* 			   Added XNandPsu_Prepare_Cmd API
+*			   Replaced
+*			   - XNandPsu_IntrStsEnable
+*			   - XNandPsu_IntrStsClear
+*			   - XNandPsu_IntrClear
+*			   - XNandPsu_SetProgramReg
+*			   with XNandPsu_WriteReg call
+*			   Modified xnandpsu.c file API's with above changes.
+*  			   Corrected the program command for Set Feature API.
+*			   Modified
+*			   - XNandPsu_OnfiReadStatus
+*			   - XNandPsu_GetFeature
+*			   - XNandPsu_SetFeature
+*			   to add support for DDR mode.
+*			   Changed Convention for SLC/MLC
+*			   SLC --> HAMMING
+*			   MLC --> BCH
+*			   SlcMlc --> IsBCH
+*			   Removed extra DMA mode initialization from
+*			   the XNandPsu_CfgInitialize API.
+*			   Modified
+*			   - XNandPsu_SetEccAddrSize
+*			   ECC address now is calculated based upon the
+*			   size of spare area
+*			   Modified Block Erase API, removed clearing of
+*			   packet register before erase.
+*			   Clearing Data Interface Register before
+*			   XNandPsu_OnfiReset call.
+*			   Modified XNandPsu_ChangeTimingMode API supporting
+*			   SDR and NVDDR interface for timing modes 0 to 5.
+*			   Modified Bbt Signature and Version Offset value for
+*			   Oob and No-Oob region.
+* 1.0   kpc    17/6/2015   Added timer based timeout intsead of sw counter.
+* 1.1   mi     09/16/16 Removed compilation warnings with extra compiler flags.
+* 1.1	nsk    11/07/16    Change memcpy to Xil_MemCpy to handle word aligned
+*	                   data access.
+* 1.2	nsk    01/19/17    Fix for the failure of reading nand first redundant
+* 	                   parameter page. CR#966603
+* 1.3	nsk    08/14/17    Added CCI support
+* 1.4	nsk    04/10/18    Added ICCARM compiler support. CR#997552.
+* 1.5   mus    11/05/18 Support 64 bit DMA addresses for Microblaze-X platform.
+* 1.5   mus    11/05/18 Updated XNandPsu_ChangeClockFreq to fix compilation
+*                       warnings.
+* 1.6	sd     06/02/20    Added Clock support
+* 1.6	sd     20/03/20    Added compilation flag
+* 1.8   sg     03/18/21	   Added validation check for parameter page.
+* 1.9   akm    07/15/21    Initialize NandInstPtr with Data Interface & Timing mode info.
+* 1.10  akm    10/20/21    Fix gcc warnings.
+* 1.10  akm    12/21/21    Validate input parameters before use.
+* 1.10  akm    01/05/22    Remove assert checks form static and internal APIs.
+*
+* </pre>
+*
+******************************************************************************/
+
+/***************************** Include Files *********************************/
+#include "xnandpsu.h"
+#include "xnandpsu_bbm.h"
+#include "sleep.h"
+#include "xil_mem.h"
+/************************** Constant Definitions *****************************/
+
+static const XNandPsu_EccMatrix EccMatrix[] = {
+	/* 512 byte page */
+	{XNANDPSU_PAGE_SIZE_512, 9U, 1U, XNANDPSU_HAMMING, 0x20DU, 0x3U},
+	{XNANDPSU_PAGE_SIZE_512, 9U, 4U, XNANDPSU_BCH, 0x209U, 0x7U},
+	{XNANDPSU_PAGE_SIZE_512, 9U, 8U, XNANDPSU_BCH, 0x203U, 0xDU},
+	/* 2K byte page */
+	{XNANDPSU_PAGE_SIZE_2K, 9U, 1U, XNANDPSU_HAMMING, 0x834U, 0xCU},
+	{XNANDPSU_PAGE_SIZE_2K, 9U, 4U, XNANDPSU_BCH, 0x826U, 0x1AU},
+	{XNANDPSU_PAGE_SIZE_2K, 9U, 8U, XNANDPSU_BCH, 0x80cU, 0x34U},
+	{XNANDPSU_PAGE_SIZE_2K, 9U, 12U, XNANDPSU_BCH, 0x822U, 0x4EU},
+	{XNANDPSU_PAGE_SIZE_2K, 10U, 24U, XNANDPSU_BCH, 0x81cU, 0x54U},
+	/* 4K byte page */
+	{XNANDPSU_PAGE_SIZE_4K, 9U, 1U, XNANDPSU_HAMMING, 0x1068U, 0x18U},
+	{XNANDPSU_PAGE_SIZE_4K, 9U, 4U, XNANDPSU_BCH, 0x104cU, 0x34U},
+	{XNANDPSU_PAGE_SIZE_4K, 9U, 8U, XNANDPSU_BCH, 0x1018U, 0x68U},
+	{XNANDPSU_PAGE_SIZE_4K, 9U, 12U, XNANDPSU_BCH, 0x1044U, 0x9CU},
+	{XNANDPSU_PAGE_SIZE_4K, 10U, 24U, XNANDPSU_BCH, 0x1038U, 0xA8U},
+	/* 8K byte page */
+	{XNANDPSU_PAGE_SIZE_8K, 9U, 1U, XNANDPSU_HAMMING, 0x20d0U, 0x30U},
+	{XNANDPSU_PAGE_SIZE_8K, 9U, 4U, XNANDPSU_BCH, 0x2098U, 0x68U},
+	{XNANDPSU_PAGE_SIZE_8K, 9U, 8U, XNANDPSU_BCH, 0x2030U, 0xD0U},
+	{XNANDPSU_PAGE_SIZE_8K, 9U, 12U, XNANDPSU_BCH, 0x2088U, 0x138U},
+	{XNANDPSU_PAGE_SIZE_8K, 10U, 24U, XNANDPSU_BCH, 0x2070U, 0x150U},
+	/* 16K byte page */
+	{XNANDPSU_PAGE_SIZE_16K, 9U, 1U, XNANDPSU_HAMMING, 0x4460U, 0x60U},
+	{XNANDPSU_PAGE_SIZE_16K, 9U, 4U, XNANDPSU_BCH, 0x43f0U, 0xD0U},
+	{XNANDPSU_PAGE_SIZE_16K, 9U, 8U, XNANDPSU_BCH, 0x4320U, 0x1A0U},
+	{XNANDPSU_PAGE_SIZE_16K, 9U, 12U, XNANDPSU_BCH, 0x4250U, 0x270U},
+	{XNANDPSU_PAGE_SIZE_16K, 10U, 24U, XNANDPSU_BCH, 0x4220U, 0x2A0U}
+};
+
+/**************************** Type Definitions *******************************/
+/***************** Macros (Inline Functions) Definitions *********************/
+
+/************************** Function Prototypes ******************************/
+
+static s32 XNandPsu_FlashInit(XNandPsu *InstancePtr);
+
+static s32 XNandPsu_InitGeometry(XNandPsu *InstancePtr, OnfiParamPage *Param);
+
+static void XNandPsu_InitFeatures(XNandPsu *InstancePtr, OnfiParamPage *Param);
+
+static void XNandPsu_InitDataInterface(XNandPsu *InstancePtr, OnfiParamPage *Param);
+
+static void XNandPsu_InitTimingMode(XNandPsu *InstancePtr, OnfiParamPage *Param);
+
+static s32 XNandPsu_PollRegTimeout(XNandPsu *InstancePtr, u32 RegOffset,
+					u32 Mask, u32 Timeout);
+
+static void XNandPsu_SetPktSzCnt(XNandPsu *InstancePtr, u32 PktSize,
+						u32 PktCount);
+
+static void XNandPsu_SetPageColAddr(XNandPsu *InstancePtr, u32 Page, u16 Col);
+
+static void XNandPsu_SetPageSize(XNandPsu *InstancePtr);
+
+static void XNandPsu_SelectChip(XNandPsu *InstancePtr, u32 Target);
+
+static s32 XNandPsu_OnfiReset(XNandPsu *InstancePtr, u32 Target);
+
+static s32 XNandPsu_OnfiReadStatus(XNandPsu *InstancePtr, u32 Target,
+							u16 *OnfiStatus);
+
+static s32 XNandPsu_OnfiReadId(XNandPsu *InstancePtr, u32 Target, u8 IdAddr,
+							u32 IdLen, u8 *Buf);
+
+static s32 XNandPsu_OnfiReadParamPage(XNandPsu *InstancePtr, u32 Target,
+						u8 *Buf);
+
+static s32 XNandPsu_ProgramPage(XNandPsu *InstancePtr, u32 Target, u32 Page,
+							u32 Col, u8 *Buf);
+
+static s32 XNandPsu_ReadPage(XNandPsu *InstancePtr, u32 Target, u32 Page,
+							u32 Col, u8 *Buf);
+
+static s32 XNandPsu_CheckOnDie(XNandPsu *InstancePtr, OnfiParamPage *Param);
+
+static void XNandPsu_SetEccAddrSize(XNandPsu *InstancePtr);
+
+static s32 XNandPsu_ChangeReadColumn(XNandPsu *InstancePtr, u32 Target,
+					u32 Col, u32 PktSize, u32 PktCount,
+					u8 *Buf);
+
+static s32 XNandPsu_ChangeWriteColumn(XNandPsu *InstancePtr, u32 Target,
+					u32 Col, u32 PktSize, u32 PktCount,
+					u8 *Buf);
+
+static s32 XNandPsu_InitExtEcc(XNandPsu *InstancePtr, OnfiExtPrmPage *ExtPrm);
+
+static s32 XNandPsu_Data_ReadWrite(XNandPsu *InstancePtr, u8* Buf, u32 PktCount,
+				u32 PktSize, u32 Operation, u8 DmaMode);
+
+static s32 XNandPsu_WaitFor_Transfer_Complete(XNandPsu *InstancePtr);
+
+static s32 XNandPsu_Device_Ready(XNandPsu *InstancePtr, u32 Target);
+
+static void XNandPsu_Fifo_Read(XNandPsu *InstancePtr, u8* Buf, u32 Size);
+
+static void XNandPsu_Fifo_Write(XNandPsu *InstancePtr, u8* Buf, u32 Size);
+
+static void XNandPsu_Update_DmaAddr(XNandPsu *InstancePtr, u8* Buf);
+/*****************************************************************************/
+/**
+*
+* This function initializes a specific XNandPsu instance. This function must
+* be called prior to using the NAND flash device to read or write any data.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	ConfigPtr points to XNandPsu device configuration structure.
+* @param	EffectiveAddr is the base address of NAND flash controller.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+* @note		The user needs to first call the XNandPsu_LookupConfig() API
+*		which returns the Configuration structure pointer which is
+*		passed as a parameter to the XNandPsu_CfgInitialize() API.
+*
+******************************************************************************/
+s32 XNandPsu_CfgInitialize(XNandPsu *InstancePtr, XNandPsu_Config *ConfigPtr,
+				u32 EffectiveAddr)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(ConfigPtr != NULL);
+
+	s32 Status = XST_FAILURE;
+
+	/* Initialize InstancePtr Config structure */
+	InstancePtr->Config.DeviceId = ConfigPtr->DeviceId;
+	InstancePtr->Config.BaseAddress = EffectiveAddr;
+	InstancePtr->Config.IsCacheCoherent = ConfigPtr->IsCacheCoherent;
+#if defined  (XCLOCKING)
+	InstancePtr->Config.RefClk = ConfigPtr->RefClk;
+#endif
+	/* Operate in Polling Mode */
+	InstancePtr->Mode = XNANDPSU_POLLING;
+	/* Enable MDMA mode by default */
+	InstancePtr->DmaMode = XNANDPSU_MDMA;
+	InstancePtr->IsReady = XIL_COMPONENT_IS_READY;
+
+	/* Initialize the NAND flash targets */
+	Status = XNandPsu_FlashInit(InstancePtr);
+	if (Status != XST_SUCCESS) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: Flash init failed\r\n",__func__);
+#endif
+		goto Out;
+	}
+	/* Set ECC mode */
+	if (InstancePtr->Features.EzNand != 0U) {
+		InstancePtr->EccMode = XNANDPSU_EZNAND;
+	} else if (InstancePtr->Features.OnDie != 0U) {
+		InstancePtr->EccMode = XNANDPSU_ONDIE;
+	} else {
+		InstancePtr->EccMode = XNANDPSU_HWECC;
+	}
+
+	/* Initialize Ecc Error flip counters */
+	 InstancePtr->Ecc_Stat_PerPage_flips = 0U;
+	 InstancePtr->Ecc_Stats_total_flips = 0U;
+
+	/*
+	 * Scan for the bad block table(bbt) stored in the flash & load it in
+	 * memory(RAM).  If bbt is not found, create bbt by scanning factory
+	 * marked bad blocks and store it in last good blocks of flash.
+	 */
+	XNandPsu_InitBbtDesc(InstancePtr);
+	Status = XNandPsu_ScanBbt(InstancePtr);
+	if (Status != XST_SUCCESS) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: BBT scan failed\r\n",__func__);
+#endif
+		goto Out;
+	}
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function initializes the NAND flash and gets the geometry information.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_FlashInit(XNandPsu *InstancePtr)
+{
+	u32 Target;
+	u8 Id[ONFI_SIG_LEN] = {0U};
+	OnfiParamPage Param[ONFI_MND_PRM_PGS] = {0U};
+	s32 Status = XST_FAILURE;
+	u32 Index;
+	u32 Crc;
+	u32 PrmPgOff;
+	u32 PrmPgLen;
+#ifdef __ICCARM__
+#pragma pack(push, 1)
+	OnfiExtPrmPage ExtParam = {0U};
+#pragma pack(pop)
+#else
+	OnfiExtPrmPage ExtParam __attribute__ ((aligned(64))) = {0U};
+#endif
+
+	/* Clear Data Interface Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_DATA_INTF_OFFSET, 0U);
+
+	/* Clear DMA Buffer Boundary Register */
+	XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+			XNANDPSU_DMA_BUF_BND_OFFSET, 0U);
+
+	for (Target = 0U; Target < XNANDPSU_MAX_TARGETS; Target++) {
+		/* Reset the Target */
+		Status = XNandPsu_OnfiReset(InstancePtr, Target);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+		/* Read ONFI ID */
+		Status = XNandPsu_OnfiReadId(InstancePtr, Target,
+						ONFI_READ_ID_ADDR,
+						ONFI_SIG_LEN,
+						(u8 *)&Id[0]);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+
+		if (!IS_ONFI(Id)) {
+			if (Target == 0U) {
+#ifdef XNANDPSU_DEBUG
+				xil_printf("%s: ONFI ID doesn't match\r\n",
+								__func__);
+#endif
+				Status = XST_FAILURE;
+				goto Out;
+			}
+		}
+
+		/* Read Parameter Page */
+		Status = XNandPsu_OnfiReadParamPage(InstancePtr,
+						Target, (u8 *)&Param[0]);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+		for(Index = 0U; Index < ONFI_MND_PRM_PGS; Index++){
+			/* Check CRC */
+			Crc = XNandPsu_OnfiParamPageCrc((u8*)&Param[Index], 0U,
+								ONFI_CRC_LEN);
+			if (Crc != Param[Index].Crc) {
+#ifdef XNANDPSU_DEBUG
+				xil_printf("%s: ONFI parameter page (%d) crc check failed\r\n",
+							__func__, Index);
+#endif
+				continue;
+			} else {
+				break;
+			}
+		}
+		if (Index >= ONFI_MND_PRM_PGS) {
+			Status = XST_FAILURE;
+			goto Out;
+		}
+		/* Fill Geometry for the first target */
+		if (Target == 0U) {
+			Status = XNandPsu_InitGeometry(InstancePtr, &Param[Index]);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			XNandPsu_InitDataInterface(InstancePtr, &Param[Index]);
+			XNandPsu_InitTimingMode(InstancePtr, &Param[Index]);
+			XNandPsu_InitFeatures(InstancePtr, &Param[Index]);
+			if ((!InstancePtr->Features.EzNand) != 0U) {
+				Status =XNandPsu_CheckOnDie(InstancePtr,&Param[Index]);
+				if (Status != XST_SUCCESS) {
+					InstancePtr->Features.OnDie = 0U;
+				}
+			}
+			if ((InstancePtr->Geometry.NumBitsECC == 0xFFU) &&
+				(InstancePtr->Features.ExtPrmPage != 0U)) {
+					/* ONFI 3.1 section 5.7.1.6 & 5.7.1.7 */
+				PrmPgLen = (u32)Param[Index].ExtParamPageLen * 16U;
+				PrmPgOff = (u32)((u32)Param[Index].NumOfParamPages *
+						ONFI_PRM_PG_LEN) + (Index * (u32)PrmPgLen);
+
+				Status = XNandPsu_ChangeReadColumn(
+						InstancePtr, Target,
+						PrmPgOff, PrmPgLen, 1U,
+						(u8 *)(void *)&ExtParam);
+				if (Status != XST_SUCCESS) {
+					goto Out;
+				}
+				/* Check CRC */
+				Crc = XNandPsu_OnfiParamPageCrc(
+						(u8 *)&ExtParam,
+						2U, PrmPgLen);
+				if (Crc != ExtParam.Crc) {
+#ifdef XNANDPSU_DEBUG
+	xil_printf("%s: ONFI extended parameter page (%d) crc check failed\r\n",
+							__func__, Index);
+#endif
+					Status = XST_FAILURE;
+					goto Out;
+				}
+				/* Initialize Extended ECC info */
+				Status = XNandPsu_InitExtEcc(
+						InstancePtr,
+						&ExtParam);
+				if (Status != XST_SUCCESS) {
+#ifdef XNANDPSU_DEBUG
+	xil_printf("%s: Init extended ecc failed\r\n",__func__);
+#endif
+					goto Out;
+				}
+			}
+			/* Configure ECC settings */
+			XNandPsu_SetEccAddrSize(InstancePtr);
+		}
+		InstancePtr->Geometry.NumTargets++;
+	}
+	/* Calculate total number of blocks and total size of flash */
+	InstancePtr->Geometry.NumPages = InstancePtr->Geometry.NumTargets *
+					InstancePtr->Geometry.NumTargetPages;
+	InstancePtr->Geometry.NumBlocks = InstancePtr->Geometry.NumTargets *
+					InstancePtr->Geometry.NumTargetBlocks;
+	InstancePtr->Geometry.DeviceSize =
+					(u64)InstancePtr->Geometry.NumTargets *
+					InstancePtr->Geometry.TargetSize;
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function initializes the geometry information from ONFI parameter page.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Param is pointer to the ONFI parameter page.
+*
+* @return
+*               - XST_SUCCESS if successful.
+*               - XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_InitGeometry(XNandPsu *InstancePtr, OnfiParamPage *Param)
+{
+	s32 Status = XST_FAILURE;
+
+	if (Param->BytesPerPage > XNANDPSU_MAX_PAGE_SIZE) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Invalid Bytes Per Page %d\r\n",
+					__func__, Param->BytesPerPage);
+#endif
+			goto Out;
+	}
+	InstancePtr->Geometry.BytesPerPage = Param->BytesPerPage;
+
+
+	if (Param->SpareBytesPerPage > XNANDPSU_MAX_SPARE_SIZE) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Invalid Spare Bytes Per Page %d\r\n",
+					__func__, Param->SpareBytesPerPage);
+#endif
+			goto Out;
+	}
+	InstancePtr->Geometry.SpareBytesPerPage = Param->SpareBytesPerPage;
+
+	if (Param->PagesPerBlock > XNANDPSU_MAX_PAGES_PER_BLOCK) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Invalid Page Count Per Block %d\r\n",
+					__func__, Param->PagesPerBlock);
+#endif
+			goto Out;
+	}
+	InstancePtr->Geometry.PagesPerBlock = Param->PagesPerBlock;
+
+
+	if (Param->BlocksPerLun > XNANDPSU_MAX_BLOCKS) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Invalid block count per LUN %d\r\n",
+					__func__, Param->BlocksPerLun);
+#endif
+			goto Out;
+	}
+	InstancePtr->Geometry.BlocksPerLun = Param->BlocksPerLun;
+
+	if (Param->NumLuns > XNANDPSU_MAX_LUNS) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Invalid LUN count %d\r\n",
+					__func__, Param->NumLuns);
+#endif
+			goto Out;
+	}
+	InstancePtr->Geometry.NumLuns = Param->NumLuns;
+
+	InstancePtr->Geometry.RowAddrCycles = Param->AddrCycles & 0xFU;
+	InstancePtr->Geometry.ColAddrCycles = (Param->AddrCycles >> 4U) & 0xFU;
+	InstancePtr->Geometry.NumBitsPerCell = Param->BitsPerCell;
+	InstancePtr->Geometry.NumBitsECC = Param->EccBits;
+	InstancePtr->Geometry.BlockSize = (Param->PagesPerBlock *
+						Param->BytesPerPage);
+	InstancePtr->Geometry.NumTargetBlocks = (Param->BlocksPerLun *
+						(u32)Param->NumLuns);
+	InstancePtr->Geometry.NumTargetPages = (Param->BlocksPerLun *
+						(u32)Param->NumLuns *
+						Param->PagesPerBlock);
+	InstancePtr->Geometry.TargetSize = ((u64)Param->BlocksPerLun *
+						(u64)Param->NumLuns *
+						(u64)Param->PagesPerBlock *
+						(u64)Param->BytesPerPage);
+	InstancePtr->Geometry.EccCodeWordSize = 9U; /* 2 power of 9 = 512 */
+	if (InstancePtr->Geometry.NumTargetBlocks > XNANDPSU_MAX_BLOCKS)
+		xil_printf("!!! Device contains more blocks than the max defined blocks in driver\r\n");
+
+#ifdef XNANDPSU_DEBUG
+	xil_printf("Manufacturer: %s\r\n", Param->DeviceManufacturer);
+	xil_printf("Device Model: %s\r\n", Param->DeviceModel);
+	xil_printf("Jedec ID: 0x%x\r\n", Param->JedecManufacturerId);
+	xil_printf("Bytes Per Page: 0x%x\r\n", Param->BytesPerPage);
+	xil_printf("Spare Bytes Per Page: 0x%x\r\n", Param->SpareBytesPerPage);
+	xil_printf("Pages Per Block: 0x%x\r\n", Param->PagesPerBlock);
+	xil_printf("Blocks Per LUN: 0x%x\r\n", Param->BlocksPerLun);
+	xil_printf("Number of LUNs: 0x%x\r\n", Param->NumLuns);
+	xil_printf("Number of bits per cell: 0x%x\r\n", Param->BitsPerCell);
+	xil_printf("Number of ECC bits: 0x%x\r\n", Param->EccBits);
+	xil_printf("Block Size: 0x%x\r\n", InstancePtr->Geometry.BlockSize);
+
+	xil_printf("Number of Target Blocks: 0x%x\r\n",
+					InstancePtr->Geometry.NumTargetBlocks);
+	xil_printf("Number of Target Pages: 0x%x\r\n",
+					InstancePtr->Geometry.NumTargetPages);
+
+#endif
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function initializes the feature list from ONFI parameter page.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Param is pointer to ONFI parameter page buffer.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_InitFeatures(XNandPsu *InstancePtr, OnfiParamPage *Param)
+{
+	InstancePtr->Features.NvDdr = ((Param->Features & (1U << 5)) != 0U) ?
+								1U : 0U;
+	InstancePtr->Features.EzNand = ((Param->Features & (1U << 9)) != 0U) ?
+								1U : 0U;
+	InstancePtr->Features.ExtPrmPage = ((Param->Features & (1U << 7)) != 0U) ?
+								1U : 0U;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function initializes the Data Interface from ONFI parameter page.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Param is pointer to ONFI parameter page buffer.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_InitDataInterface(XNandPsu *InstancePtr, OnfiParamPage *Param)
+{
+	if (Param->NVDDRTimingMode)
+		InstancePtr->DataInterface = XNANDPSU_NVDDR;
+	else if (Param->SDRTimingMode)
+		InstancePtr->DataInterface = XNANDPSU_SDR;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function initializes the Timing mode from ONFI parameter page.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Param is pointer to ONFI parameter page buffer.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_InitTimingMode(XNandPsu *InstancePtr, OnfiParamPage *Param)
+{
+	s8 Mode;
+	u8 TimingMode =  (u8)(Param->SDRTimingMode);
+
+	if (InstancePtr->DataInterface == XNANDPSU_NVDDR)
+		TimingMode = Param->NVDDRTimingMode;
+
+	for(Mode = XNANDPSU_MAX_TIMING_MODE; Mode >= 0; Mode--) {
+		if (TimingMode & (0x01 << Mode)) {
+			InstancePtr->TimingMode = Mode;
+			break;
+		} else {
+			continue;
+		}
+	}
+}
+
+/*****************************************************************************/
+/**
+*
+* This function checks if the flash supports on-die ECC.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Param is pointer to ONFI parameter page.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_CheckOnDie(XNandPsu *InstancePtr, OnfiParamPage *Param)
+{
+	s32 Status = XST_FAILURE;
+	u8 JedecId[2] = {0U};
+	u8 EccSetFeature[4] = {0x08U, 0x00U, 0x00U, 0x00U};
+	u8 EccGetFeature[4] ={0U};
+
+	/*
+	 * Check if this flash supports On-Die ECC.
+	 * For more information, refer to Micron TN2945.
+	 * Micron Flash: MT29F1G08ABADA, MT29F1G08ABBDA
+	 *		 MT29F1G16ABBDA,
+	 *		 MT29F2G08ABBEA, MT29F2G16ABBEA,
+	 *		 MT29F2G08ABAEA, MT29F2G16ABAEA,
+	 *		 MT29F4G08ABBDA, MT29F4G16ABBDA,
+	 *		 MT29F4G08ABADA, MT29F4G16ABADA,
+	 *		 MT29F8G08ADBDA, MT29F8G16ADBDA,
+	 *		 MT29F8G08ADADA, MT29F8G16ADADA
+	 */
+
+	/* Read JEDEC ID */
+	Status = XNandPsu_OnfiReadId(InstancePtr, 0U, 0x00U, 2U, &JedecId[0]);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	if ((JedecId[0] == 0x2CU) &&
+	/* 1 Gb flash devices */
+	((JedecId[1] == 0xF1U) ||
+	(JedecId[1] == 0xA1U) ||
+	(JedecId[1] == 0xB1U) ||
+	/* 2 Gb flash devices */
+	(JedecId[1] == 0xAAU) ||
+	(JedecId[1] == 0xBAU) ||
+	(JedecId[1] == 0xDAU) ||
+	(JedecId[1] == 0xCAU) ||
+	/* 4 Gb flash devices */
+	(JedecId[1] == 0xACU) ||
+	(JedecId[1] == 0xBCU) ||
+	(JedecId[1] == 0xDCU) ||
+	(JedecId[1] == 0xCCU) ||
+	/* 8 Gb flash devices */
+	(JedecId[1] == 0xA3U) ||
+	(JedecId[1] == 0xB3U) ||
+	(JedecId[1] == 0xD3U) ||
+	(JedecId[1] == 0xC3U))) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: Ondie flash detected, jedec id 0x%x 0x%x\r\n",
+					__func__, JedecId[0], JedecId[1]);
+#endif
+		/* On-Die Set Feature */
+		Status = XNandPsu_SetFeature(InstancePtr, 0U, 0x90U,
+						&EccSetFeature[0]);
+		if (Status != XST_SUCCESS) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Ondie set_feature failed\r\n",
+								__func__);
+#endif
+			goto Out;
+		}
+		/* Check to see if ECC feature is set */
+		Status = XNandPsu_GetFeature(InstancePtr, 0U, 0x90U,
+						&EccGetFeature[0]);
+		if (Status != XST_SUCCESS) {
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: Ondie get_feature failed\r\n",
+								__func__);
+#endif
+			goto Out;
+		}
+		if ((EccGetFeature[0] & 0x08U) != 0U) {
+			InstancePtr->Features.OnDie = 1U;
+			Status = XST_SUCCESS;
+		}
+	} else {
+		/* On-Die flash not found */
+		Status = XST_FAILURE;
+	}
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function enables DMA mode of controller operation.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+void XNandPsu_EnableDmaMode(XNandPsu *InstancePtr)
+{
+	/* Assert the input arguments. */
+	Xil_AssertVoid(InstancePtr != NULL);
+	Xil_AssertVoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+
+	InstancePtr->DmaMode = XNANDPSU_MDMA;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function disables DMA mode of driver/controller operation.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+void XNandPsu_DisableDmaMode(XNandPsu *InstancePtr)
+{
+	/* Assert the input arguments. */
+	Xil_AssertVoid(InstancePtr != NULL);
+	Xil_AssertVoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+
+	InstancePtr->DmaMode = XNANDPSU_PIO;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function enables ECC mode of driver/controller operation.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+void XNandPsu_EnableEccMode(XNandPsu *InstancePtr)
+{
+	/* Assert the input arguments. */
+	Xil_AssertVoid(InstancePtr != NULL);
+	Xil_AssertVoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+
+	InstancePtr->EccMode = XNANDPSU_HWECC;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function disables ECC mode of driver/controller operation.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+void XNandPsu_DisableEccMode(XNandPsu *InstancePtr)
+{
+	/* Assert the input arguments. */
+	Xil_AssertVoid(InstancePtr != NULL);
+	Xil_AssertVoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+
+	InstancePtr->EccMode = XNANDPSU_NONE;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function polls for a register bit set status till the timeout.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	RegOffset is the offset of register.
+* @param	Mask is the bitmask.
+* @param	Timeout is the timeout value.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_PollRegTimeout(XNandPsu *InstancePtr, u32 RegOffset,
+					u32 Mask, u32 Timeout)
+{
+	s32 Status = XST_FAILURE;
+	volatile u32 RegVal;
+	u32 TimeoutVar = Timeout;
+
+	while (TimeoutVar > 0U) {
+		RegVal = XNandPsu_ReadReg(InstancePtr->Config.BaseAddress,
+						RegOffset) & Mask;
+		if (RegVal != 0U) {
+			break;
+		}
+		TimeoutVar--;
+		usleep(1);
+	}
+
+	if (TimeoutVar <= 0U) {
+		Status = XST_FAILURE;
+	} else {
+		Status = XST_SUCCESS;
+	}
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sets packet size and packet count values in packet register.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	PktSize is the packet size.
+* @param	PktCount is the packet count.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_SetPktSzCnt(XNandPsu *InstancePtr, u32 PktSize,
+						u32 PktCount)
+{
+	/* Update Packet Register with pkt size and count */
+	XNandPsu_ReadModifyWrite(InstancePtr, XNANDPSU_PKT_OFFSET,
+				((u32)XNANDPSU_PKT_PKT_SIZE_MASK |
+				(u32)XNANDPSU_PKT_PKT_CNT_MASK),
+				((PktSize & XNANDPSU_PKT_PKT_SIZE_MASK) |
+				((PktCount << XNANDPSU_PKT_PKT_CNT_SHIFT) &
+				XNANDPSU_PKT_PKT_CNT_MASK)));
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sets Page and Column values in the Memory address registers.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Page is the page value.
+* @param	Col is the column value.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_SetPageColAddr(XNandPsu *InstancePtr, u32 Page, u16 Col)
+{
+	/* Program Memory Address Register 1 */
+	XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_MEM_ADDR1_OFFSET,
+				(((u32)Col & XNANDPSU_MEM_ADDR1_COL_ADDR_MASK) |
+				((Page << (u32)XNANDPSU_MEM_ADDR1_PG_ADDR_SHIFT) &
+				XNANDPSU_MEM_ADDR1_PG_ADDR_MASK)));
+	/* Program Memory Address Register 2 */
+	XNandPsu_ReadModifyWrite(InstancePtr, XNANDPSU_MEM_ADDR2_OFFSET,
+				XNANDPSU_MEM_ADDR2_MEM_ADDR_MASK,
+				((Page >> XNANDPSU_MEM_ADDR1_PG_ADDR_SHIFT) &
+				XNANDPSU_MEM_ADDR2_MEM_ADDR_MASK));
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sets the size of page in Command Register.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_SetPageSize(XNandPsu *InstancePtr)
+{
+	u32 PageSizeMask = 0;
+	u32 PageSize = InstancePtr->Geometry.BytesPerPage;
+
+	/* Calculate page size mask */
+	switch(PageSize) {
+		case XNANDPSU_PAGE_SIZE_512:
+			PageSizeMask = (0U << XNANDPSU_CMD_PG_SIZE_SHIFT);
+			break;
+		case XNANDPSU_PAGE_SIZE_2K:
+			PageSizeMask = (1U << XNANDPSU_CMD_PG_SIZE_SHIFT);
+			break;
+		case XNANDPSU_PAGE_SIZE_4K:
+			PageSizeMask = (2U << XNANDPSU_CMD_PG_SIZE_SHIFT);
+			break;
+		case XNANDPSU_PAGE_SIZE_8K:
+			PageSizeMask = (3U << XNANDPSU_CMD_PG_SIZE_SHIFT);
+			break;
+		case XNANDPSU_PAGE_SIZE_16K:
+			PageSizeMask = (4U << XNANDPSU_CMD_PG_SIZE_SHIFT);
+			break;
+		case XNANDPSU_PAGE_SIZE_1K_16BIT:
+			PageSizeMask = (5U << XNANDPSU_CMD_PG_SIZE_SHIFT);
+			break;
+		default:
+			/* Not supported */
+			break;
+	}
+	/* Update Command Register */
+	XNandPsu_ReadModifyWrite(InstancePtr, XNANDPSU_CMD_OFFSET,
+				XNANDPSU_CMD_PG_SIZE_MASK, PageSizeMask);
+}
+
+/*****************************************************************************/
+/**
+*
+* This function setup the Ecc Register.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_SetEccAddrSize(XNandPsu *InstancePtr)
+{
+	u32 PageSize = InstancePtr->Geometry.BytesPerPage;
+	u32 CodeWordSize = InstancePtr->Geometry.EccCodeWordSize;
+	u32 NumEccBits = InstancePtr->Geometry.NumBitsECC;
+	u32 Index;
+	u32 Found = 0U;
+	u8 BchModeVal;
+
+	for (Index = 0U; Index < (sizeof(EccMatrix)/sizeof(XNandPsu_EccMatrix));
+						Index++) {
+		if ((EccMatrix[Index].PageSize == PageSize) &&
+			(EccMatrix[Index].CodeWordSize >= CodeWordSize)) {
+			if (EccMatrix[Index].NumEccBits >= NumEccBits) {
+				Found = Index;
+				break;
+			}
+			else {
+				Found = Index;
+			}
+		}
+	}
+
+	if (Found != 0U) {
+		if(InstancePtr->Geometry.SpareBytesPerPage < 64U) {
+			InstancePtr->EccCfg.EccAddr = (u16)PageSize;
+		}
+		else {
+			InstancePtr->EccCfg.EccAddr = ((u16)PageSize +
+				(InstancePtr->Geometry.SpareBytesPerPage
+						- EccMatrix[Found].EccSize));
+		}
+		InstancePtr->EccCfg.EccSize = EccMatrix[Found].EccSize;
+		InstancePtr->EccCfg.NumEccBits = EccMatrix[Found].NumEccBits;
+		InstancePtr->EccCfg.CodeWordSize =
+						EccMatrix[Found].CodeWordSize;
+#ifdef XNANDPSU_DEBUG
+		xil_printf("ECC: addr 0x%x size 0x%x numbits %d "
+				   "codesz %d\r\n",
+				   InstancePtr->EccCfg.EccAddr,
+				   InstancePtr->EccCfg.EccSize,
+				   InstancePtr->EccCfg.NumEccBits,
+				   InstancePtr->EccCfg.CodeWordSize);
+#endif
+		if (EccMatrix[Found].IsBCH == XNANDPSU_HAMMING) {
+			InstancePtr->EccCfg.IsBCH = 0U;
+		} else {
+			InstancePtr->EccCfg.IsBCH = 1U;
+		}
+		/* Write ECC register */
+		XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				(u32)XNANDPSU_ECC_OFFSET,
+				((u32)InstancePtr->EccCfg.EccAddr |
+				((u32)InstancePtr->EccCfg.EccSize << (u32)16) |
+				((u32)InstancePtr->EccCfg.IsBCH << (u32)27)));
+
+		if (EccMatrix[Found].IsBCH == XNANDPSU_BCH) {
+			/* Write memory address register 2 */
+			switch(InstancePtr->EccCfg.NumEccBits) {
+				case 16U:
+					BchModeVal = 0x0U;
+					break;
+				case 12U:
+					BchModeVal = 0x1U;
+					break;
+				case 8U:
+					BchModeVal = 0x2U;
+					break;
+				case 4U:
+					BchModeVal = 0x3U;
+					break;
+				case 24U:
+					BchModeVal = 0x4U;
+					break;
+				default:
+					BchModeVal = 0x0U;
+					break;
+			}
+			XNandPsu_ReadModifyWrite(InstancePtr,
+				XNANDPSU_MEM_ADDR2_OFFSET,
+				XNANDPSU_MEM_ADDR2_NFC_BCH_MODE_MASK,
+				((u32)BchModeVal <<
+				(u32)XNANDPSU_MEM_ADDR2_NFC_BCH_MODE_SHIFT));
+		}
+	}
+}
+
+/*****************************************************************************/
+/**
+*
+* This function setup the Ecc Spare Command Register.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_SetEccSpareCmd(XNandPsu *InstancePtr, u16 SpareCmd,
+								u8 AddrCycles)
+{
+	XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				(u32)XNANDPSU_ECC_SPR_CMD_OFFSET,
+				(u32)SpareCmd | ((u32)AddrCycles << 28U));
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sets the chip select value in memory address2 register.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_SelectChip(XNandPsu *InstancePtr, u32 Target)
+{
+#if defined  (XCLOCKING)
+	Xil_ClockEnable(InstancePtr->Config.RefClk);
+#endif
+	/* Update Memory Address2 register with chip select */
+	XNandPsu_ReadModifyWrite(InstancePtr, XNANDPSU_MEM_ADDR2_OFFSET,
+			XNANDPSU_MEM_ADDR2_CHIP_SEL_MASK,
+			((Target << XNANDPSU_MEM_ADDR2_CHIP_SEL_SHIFT) &
+			XNANDPSU_MEM_ADDR2_CHIP_SEL_MASK));
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Reset command to the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_OnfiReset(XNandPsu *InstancePtr, u32 Target)
+{
+	s32 Status = XST_FAILURE;
+
+	/* Enable Transfer Complete Interrupt in Interrupt Status Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_INTR_STS_EN_OFFSET,
+		XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK);
+	/* Program Command Register */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_RST, ONFI_CMD_INVALID, 0U,
+			0U, 0U);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set Reset in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_PROG_OFFSET, XNANDPSU_PROG_RST_MASK);
+
+	/* Poll for Transfer Complete event */
+	Status = XNandPsu_WaitFor_Transfer_Complete(InstancePtr);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Read Status command to the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	OnfiStatus is the ONFI status value to return.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_OnfiReadStatus(XNandPsu *InstancePtr, u32 Target,
+							u16 *OnfiStatus)
+{
+	s32 Status = XST_FAILURE;
+
+	/* Enable Transfer Complete Interrupt in Interrupt Status Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_INTR_STS_EN_OFFSET,
+		XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK);
+	/* Program Command Register */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_RD_STS, ONFI_CMD_INVALID,
+				0U, 0U, 0U);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Program Packet Size and Packet Count */
+	if(InstancePtr->DataInterface == XNANDPSU_SDR)
+		XNandPsu_SetPktSzCnt(InstancePtr, 1U, 1U);
+	else
+		XNandPsu_SetPktSzCnt(InstancePtr, 2U, 1U);
+
+	/* Set Read Status in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_RD_STS_MASK);
+	/* Poll for Transfer Complete event */
+	Status = XNandPsu_WaitFor_Transfer_Complete(InstancePtr);
+	/* Read Flash Status */
+	*OnfiStatus = (u16) XNandPsu_ReadReg(InstancePtr->Config.BaseAddress,
+						XNANDPSU_FLASH_STS_OFFSET);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Read ID command to the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Buf is the ONFI ID value to return.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_OnfiReadId(XNandPsu *InstancePtr, u32 Target, u8 IdAddr,
+							u32 IdLen, u8 *Buf)
+{
+	s32 Status = XST_FAILURE;
+	u32 Index;
+	u32 Rem;
+	u32 RegVal;
+	u32 RemIdx;
+
+	u32 *BufPtr = (u32 *)(void *)Buf;
+
+	/*
+	 * Enable Buffer Read Ready Interrupt in Interrupt Status Enable
+	 * Register
+	 */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_INTR_STS_EN_OFFSET,
+		XNANDPSU_INTR_STS_EN_BUFF_RD_RDY_STS_EN_MASK);
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_RD_ID, ONFI_CMD_INVALID, 0U,
+					0U, ONFI_READ_ID_ADDR_CYCLES);
+
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, 0U, IdAddr);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, IdLen, 1U);
+	/* Set Read ID in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_RD_ID_MASK);
+
+	/* Poll for Buffer Read Ready event */
+	Status = XNandPsu_PollRegTimeout(
+		InstancePtr,
+		XNANDPSU_INTR_STS_OFFSET,
+		XNANDPSU_INTR_STS_BUFF_RD_RDY_STS_EN_MASK,
+		XNANDPSU_INTR_POLL_TIMEOUT);
+	if (Status != XST_SUCCESS) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: Poll for buf read ready timeout\r\n",
+							__func__);
+#endif
+		goto Out;
+	}
+	/*
+	 * Enable Transfer Complete Interrupt in Interrupt
+	 * Status Enable Register
+	 */
+
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_INTR_STS_EN_OFFSET,
+		XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK);
+
+	/*
+	 * Clear Buffer Read Ready Interrupt in Interrupt Status
+	 * Register
+	 */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_OFFSET,
+			XNANDPSU_INTR_STS_BUFF_RD_RDY_STS_EN_MASK);
+	/* Read Packet Data from Data Port Register */
+	for (Index = 0U; Index < (IdLen/4); Index++) {
+		*(BufPtr+Index) = XNandPsu_ReadReg(
+					InstancePtr->Config.BaseAddress,
+					XNANDPSU_BUF_DATA_PORT_OFFSET);
+	}
+	Rem = IdLen % 4;
+	if (Rem != 0U) {
+		RegVal = XNandPsu_ReadReg(
+					InstancePtr->Config.BaseAddress,
+					XNANDPSU_BUF_DATA_PORT_OFFSET);
+		for (RemIdx = 0U; RemIdx < Rem; RemIdx++) {
+			*(Buf + (Index * 4U) + RemIdx) = (u8) (RegVal >>
+						(RemIdx * 8U)) & 0xFFU;
+		}
+	}
+
+	Status = XNandPsu_WaitFor_Transfer_Complete(InstancePtr);
+
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends the ONFI Read Parameter Page command to flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	PrmIndex is the index of parameter page.
+* @param	Buf is the parameter page information to return.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_OnfiReadParamPage(XNandPsu *InstancePtr, u32 Target,
+						u8 *Buf)
+{
+	s32 Status = XST_FAILURE;
+
+	/*
+	 * Enable Buffer Read Ready Interrupt in Interrupt Status Enable
+	 * Register
+	 */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_INTR_STS_EN_OFFSET,
+		XNANDPSU_INTR_STS_EN_BUFF_RD_RDY_STS_EN_MASK);
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_RD_PRM_PG, ONFI_CMD_INVALID,
+					0U, 0U, ONFI_PRM_PG_ADDR_CYCLES);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, 0U, 0U);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, ONFI_MND_PRM_PGS*ONFI_PRM_PG_LEN, 1U);
+	/* Set Read Parameter Page in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_RD_PRM_PG_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, 1U, ONFI_MND_PRM_PGS*ONFI_PRM_PG_LEN, 0, 0);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function returns the length including bad blocks from a given offset and
+* length.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @param	Offset is the flash data address to read from.
+* @param	Length is number of bytes to read.
+*
+* @return
+*		- Return actual length including bad blocks.
+*
+* @note		None.
+*
+******************************************************************************/
+static s32 XNandPsu_CalculateLength(XNandPsu *InstancePtr, u64 Offset,
+							u64 Length)
+{
+	s32 Status;
+	u32 BlockSize;
+	u32 BlockLen;
+	u32 Block;
+	u64 TempLen = 0;
+	u64 OffsetVar = Offset;
+
+	BlockSize = InstancePtr->Geometry.BlockSize;
+
+	while (TempLen < Length) {
+		Block = (u32)(OffsetVar/BlockSize);
+		BlockLen = BlockSize - (u32)(OffsetVar % BlockSize);
+		if (OffsetVar >= InstancePtr->Geometry.DeviceSize) {
+			Status = XST_FAILURE;
+			goto Out;
+		}
+		/* Check if the block is bad */
+		Status = XNandPsu_IsBlockBad(InstancePtr, Block);
+		if (Status != XST_SUCCESS) {
+			/* Good block */
+			TempLen += BlockLen;
+		}
+		OffsetVar += BlockLen;
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function writes to the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Offset is the starting offset of flash to write.
+* @param	Length is the number of bytes to write.
+* @param	SrcBuf is the source data buffer to write.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_Write(XNandPsu *InstancePtr, u64 Offset, u64 Length, u8 *SrcBuf)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(SrcBuf != NULL);
+	Xil_AssertNonvoid(Length != 0U);
+	Xil_AssertNonvoid((Offset + Length) <=
+				InstancePtr->Geometry.DeviceSize);
+
+	s32 Status = XST_FAILURE;
+	u32 Page;
+	u32 Col;
+	u32 Target;
+	u32 Block;
+	u32 PartialBytes = 0;
+	u32 NumBytes;
+	u32 RemLen;
+	u8 *BufPtr;
+	u8 *SrcBufPtr = (u8 *)SrcBuf;
+	u64 OffsetVar = Offset;
+	u64 LengthVar = Length;
+
+	/*
+	 * Check if write operation exceeds flash size when including
+	 * bad blocks.
+	 */
+	Status = XNandPsu_CalculateLength(InstancePtr, OffsetVar, LengthVar);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	while (LengthVar > 0U) {
+		Block = (u32) (OffsetVar/InstancePtr->Geometry.BlockSize);
+		/*
+		 * Skip the bad blocks. Increment the offset by block size.
+		 * For better results, always program the flash starting at
+		 * a block boundary.
+		 */
+		if (XNandPsu_IsBlockBad(InstancePtr, Block) == XST_SUCCESS) {
+			OffsetVar += (u64)InstancePtr->Geometry.BlockSize;
+			continue;
+		}
+		/* Calculate Page and Column address values */
+		Page = (u32) (OffsetVar/InstancePtr->Geometry.BytesPerPage);
+		Col = (u32) (OffsetVar &
+				(InstancePtr->Geometry.BytesPerPage - 1U));
+		PartialBytes = 0U;
+		/*
+		 * Check if partial write.
+		 * If column address is > 0 or Length is < page size
+		 */
+		if ((Col > 0U) ||
+			(LengthVar < InstancePtr->Geometry.BytesPerPage)) {
+			RemLen = InstancePtr->Geometry.BytesPerPage - Col;
+			PartialBytes = (RemLen < (u32)LengthVar) ?
+					RemLen : (u32)LengthVar;
+		}
+
+		Target = (u32) (OffsetVar/InstancePtr->Geometry.TargetSize);
+		if (Page > InstancePtr->Geometry.NumTargetPages) {
+			Page %= InstancePtr->Geometry.NumTargetPages;
+		}
+
+		/* Check if partial write */
+		if (PartialBytes > 0U) {
+			BufPtr = &InstancePtr->PartialDataBuf[0];
+			(void)memset(BufPtr, 0xFF,
+					InstancePtr->Geometry.BytesPerPage);
+			(void)Xil_MemCpy(BufPtr + Col, SrcBufPtr, PartialBytes);
+
+			NumBytes = PartialBytes;
+		} else {
+			BufPtr = (u8 *)SrcBufPtr;
+			NumBytes = (InstancePtr->Geometry.BytesPerPage <
+					(u32)LengthVar) ?
+					InstancePtr->Geometry.BytesPerPage :
+					(u32)LengthVar;
+		}
+		/* Program page */
+		Status = XNandPsu_ProgramPage(InstancePtr, Target, Page, 0U,
+								BufPtr);
+		if (Status != XST_SUCCESS)
+			goto Out;
+
+		Status = XNandPsu_Device_Ready(InstancePtr, Target);
+		if (Status != XST_SUCCESS)
+			goto Out;
+
+		SrcBufPtr += NumBytes;
+		OffsetVar += NumBytes;
+		LengthVar -= NumBytes;
+	}
+
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function reads from the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Offset is the starting offset of flash to read.
+* @param	Length is the number of bytes to read.
+* @param	DestBuf is the destination data buffer to fill in.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_Read(XNandPsu *InstancePtr, u64 Offset, u64 Length, u8 *DestBuf)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(DestBuf != NULL);
+	Xil_AssertNonvoid(Length != 0U);
+	Xil_AssertNonvoid((Offset + Length) <=
+				InstancePtr->Geometry.DeviceSize);
+
+	s32 Status = XST_FAILURE;
+	u32 Page;
+	u32 Col;
+	u32 Target;
+	u32 Block;
+	u32 PartialBytes = 0U;
+	u32 RemLen;
+	u32 NumBytes;
+	u8 *BufPtr;
+	u8 *DestBufPtr = (u8 *)DestBuf;
+	u64 OffsetVar = Offset;
+	u64 LengthVar = Length;
+
+	/*
+	 * Check if read operation exceeds flash size when including
+	 * bad blocks.
+	 */
+	Status = XNandPsu_CalculateLength(InstancePtr, OffsetVar, LengthVar);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	while (LengthVar > 0U) {
+		Block = (u32)(OffsetVar/InstancePtr->Geometry.BlockSize);
+		/*
+		 * Skip the bad block. Increment the offset by block size.
+		 * The flash programming utility must make sure to start
+		 * writing always at a block boundary and skip blocks if any.
+		 */
+		if (XNandPsu_IsBlockBad(InstancePtr, Block) == XST_SUCCESS) {
+			OffsetVar += (u64)InstancePtr->Geometry.BlockSize;
+			continue;
+		}
+		/* Calculate Page and Column address values */
+		Page = (u32) (OffsetVar/InstancePtr->Geometry.BytesPerPage);
+		Col = (u32) (OffsetVar &
+				(InstancePtr->Geometry.BytesPerPage - 1U));
+		PartialBytes = 0U;
+		/*
+		 * Check if partial write.
+		 * If column address is > 0 or Length is < page size
+		 */
+		if ((Col > 0U) ||
+			(LengthVar < InstancePtr->Geometry.BytesPerPage)) {
+			RemLen = InstancePtr->Geometry.BytesPerPage - Col;
+			PartialBytes = ((u32)RemLen < (u32)LengthVar) ?
+						(u32)RemLen : (u32)LengthVar;
+		}
+
+		Target = (u32) (OffsetVar/InstancePtr->Geometry.TargetSize);
+		if (Page > InstancePtr->Geometry.NumTargetPages) {
+			Page %= InstancePtr->Geometry.NumTargetPages;
+		}
+		/* Check if partial read */
+		if (PartialBytes > 0U) {
+			BufPtr = &InstancePtr->PartialDataBuf[0];
+			NumBytes = PartialBytes;
+		} else {
+			BufPtr = DestBufPtr;
+			NumBytes = (InstancePtr->Geometry.BytesPerPage <
+					(u32)LengthVar) ?
+					InstancePtr->Geometry.BytesPerPage :
+					(u32)LengthVar;
+		}
+		/* Read page */
+		Status = XNandPsu_ReadPage(InstancePtr, Target, Page, 0U,
+								BufPtr);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+		if (PartialBytes > 0U) {
+			(void)Xil_MemCpy(DestBufPtr, BufPtr + Col, NumBytes);
+		}
+		DestBufPtr += NumBytes;
+		OffsetVar += NumBytes;
+		LengthVar -= NumBytes;
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function erases the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Offset is the starting offset of flash to erase.
+* @param	Length is the number of bytes to erase.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note
+*		The Offset and Length should be aligned to block size boundary
+*		to get better results.
+*
+******************************************************************************/
+s32 XNandPsu_Erase(XNandPsu *InstancePtr, u64 Offset, u64 Length)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(Length != 0U);
+	Xil_AssertNonvoid((Offset + Length) <=
+			InstancePtr->Geometry.DeviceSize);
+
+	s32 Status = XST_FAILURE;
+	u32 Target = 0;
+	u32 StartBlock;
+	u32 NumBlocks = 0;
+	u32 Block;
+	u32 AlignOff;
+	u32 EraseLen;
+	u32 BlockRemLen;
+	u64 OffsetVar = Offset;
+	u64 LengthVar = Length;
+
+	/*
+	 * Check if erase operation exceeds flash size when including
+	 * bad blocks.
+	 */
+	Status = XNandPsu_CalculateLength(InstancePtr, OffsetVar, LengthVar);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+	/* Calculate number of blocks to erase */
+	StartBlock = (u32) (OffsetVar/InstancePtr->Geometry.BlockSize);
+
+	while (LengthVar > 0U) {
+		Block = (u32) (OffsetVar/InstancePtr->Geometry.BlockSize);
+		if (XNandPsu_IsBlockBad(InstancePtr, Block) ==
+							XST_SUCCESS) {
+			OffsetVar += (u64)InstancePtr->Geometry.BlockSize;
+			NumBlocks++;
+			continue;
+		}
+
+		AlignOff = (u32)OffsetVar &
+				(InstancePtr->Geometry.BlockSize - (u32)1);
+		if (AlignOff > 0U) {
+			BlockRemLen = InstancePtr->Geometry.BlockSize -
+								AlignOff;
+			EraseLen = (BlockRemLen < (u32)LengthVar) ?
+						BlockRemLen :(u32)LengthVar;
+		} else {
+			EraseLen = (InstancePtr->Geometry.BlockSize <
+						(u32)LengthVar) ?
+					InstancePtr->Geometry.BlockSize:
+						(u32)LengthVar;
+		}
+		NumBlocks++;
+		OffsetVar += EraseLen;
+		LengthVar -= EraseLen;
+	}
+
+	for (Block = StartBlock; Block < (StartBlock + NumBlocks); Block++) {
+		Target = Block/InstancePtr->Geometry.NumTargetBlocks;
+		Block %= InstancePtr->Geometry.NumTargetBlocks;
+		/* Don't erase bad block */
+		if (XNandPsu_IsBlockBad(InstancePtr, Block) ==
+							XST_SUCCESS)
+			continue;
+		/* Block Erase */
+		Status = XNandPsu_EraseBlock(InstancePtr, Target, Block);
+		if (Status != XST_SUCCESS)
+			goto Out;
+
+		Status = XNandPsu_Device_Ready(InstancePtr, Target);
+		if (Status != XST_SUCCESS)
+					goto Out;
+
+				}
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Program Page command to flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Page is the page address value to program.
+* @param	Col is the column address value to program.
+* @param	Buf is the data buffer to program.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_ProgramPage(XNandPsu *InstancePtr, u32 Target, u32 Page,
+							u32 Col, u8 *Buf)
+{
+	u32 PktSize;
+	u32 PktCount;
+	s32 Status = XST_FAILURE;
+	u32 IsrValue;
+	u32 AddrCycles = InstancePtr->Geometry.RowAddrCycles +
+				InstancePtr->Geometry.ColAddrCycles;
+
+	if (InstancePtr->EccCfg.CodeWordSize > 9U) {
+		PktSize = 1024U;
+	} else {
+		PktSize = 512U;
+	}
+	PktCount = InstancePtr->Geometry.BytesPerPage/PktSize;
+
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_PG_PROG1, ONFI_CMD_PG_PROG2,
+					1U, 1U, (u8)AddrCycles);
+
+	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
+		IsrValue = XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK |
+			   XNANDPSU_INTR_STS_EN_DMA_INT_STS_EN_MASK;
+		if (InstancePtr->Config.IsCacheCoherent == 0) {
+			Xil_DCacheFlushRange((INTPTR)(void *)Buf, (PktSize * PktCount));
+		}
+		XNandPsu_Update_DmaAddr(InstancePtr, Buf);
+	} else {
+		IsrValue = XNANDPSU_INTR_STS_EN_BUFF_WR_RDY_STS_EN_MASK;
+	}
+
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			   XNANDPSU_INTR_STS_EN_OFFSET, IsrValue);
+	/* Program Page Size */
+	XNandPsu_SetPageSize(InstancePtr);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, Page, (u16)Col);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set ECC */
+	if (InstancePtr->EccMode == XNANDPSU_HWECC) {
+		XNandPsu_SetEccSpareCmd(InstancePtr, ONFI_CMD_CHNG_WR_COL,
+					InstancePtr->Geometry.ColAddrCycles);
+	}
+	/* Set Page Program in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_PG_PROG_MASK);
+
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 1, 1);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Program Page command to flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Page is the page address value to program.
+* @param	Buf is the data buffer to program.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_WriteSpareBytes(XNandPsu *InstancePtr, u32 Page, u8 *Buf)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(Page < InstancePtr->Geometry.NumPages);
+	Xil_AssertNonvoid(Buf != NULL);
+
+	u32 PktCount = 1U;
+	u16 PreEccSpareCol = 0U;
+	u16 PreEccSpareWrCnt = 0U;
+	u16 PostEccSpareCol = 0U;
+	u16 PostEccSpareWrCnt = 0U;
+	u32 PostWrite = 0U;
+	OnfiCmdFormat Cmd;
+	s32 Status = XST_FAILURE;
+	u32 RegVal;
+	u32 AddrCycles = InstancePtr->Geometry.RowAddrCycles +
+				InstancePtr->Geometry.ColAddrCycles;
+	u32 Col = InstancePtr->Geometry.BytesPerPage;
+	u32 Target = Page/InstancePtr->Geometry.NumTargetPages;
+	u32 PktSize = InstancePtr->Geometry.SpareBytesPerPage;
+	u32 *BufPtr = (u32 *)(void *)Buf;
+	u32 PageVar = Page;
+
+	PageVar %= InstancePtr->Geometry.NumTargetPages;
+
+	if (InstancePtr->EccMode == XNANDPSU_HWECC) {
+		/* Calculate ECC free positions before and after ECC code */
+		PreEccSpareCol = 0x0U;
+		PreEccSpareWrCnt = InstancePtr->EccCfg.EccAddr -
+				(u16)InstancePtr->Geometry.BytesPerPage;
+
+		PostEccSpareCol = PreEccSpareWrCnt +
+					InstancePtr->EccCfg.EccSize;
+		PostEccSpareWrCnt = InstancePtr->Geometry.SpareBytesPerPage -
+					PostEccSpareCol;
+
+		PreEccSpareWrCnt = (PreEccSpareWrCnt/4U) * 4U;
+		PostEccSpareWrCnt = (PostEccSpareWrCnt/4U) * 4U;
+
+		if (PreEccSpareWrCnt > 0U) {
+			PktSize = PreEccSpareWrCnt;
+			PktCount = 1U;
+			Col = InstancePtr->Geometry.BytesPerPage +
+							PreEccSpareCol;
+			BufPtr = (u32 *)(void *)Buf;
+			if (PostEccSpareWrCnt > 0U) {
+				PostWrite = 1U;
+			}
+		} else if (PostEccSpareWrCnt > 0U) {
+			PktSize = PostEccSpareWrCnt;
+			PktCount = 1U;
+			Col = InstancePtr->Geometry.BytesPerPage +
+							PostEccSpareCol;
+			BufPtr = (u32 *)(Buf + Col);
+		} else {
+			/* No free spare bytes available for writing */
+			Status = XST_FAILURE;
+			goto Out;
+		}
+	}
+
+	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
+		RegVal = XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK;
+		if (InstancePtr->Config.IsCacheCoherent == 0) {
+			Xil_DCacheFlushRange((INTPTR)(void *)BufPtr, (PktSize * PktCount));
+		}
+		XNandPsu_Update_DmaAddr(InstancePtr, (u8 *)BufPtr);
+	} else {
+		RegVal = XNANDPSU_INTR_STS_EN_BUFF_WR_RDY_STS_EN_MASK;
+	}
+
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			   XNANDPSU_INTR_STS_EN_OFFSET, RegVal);
+	/* Program Command hack for change write column */
+	if (PostWrite > 0U) {
+		Cmd.Command1 = 0x80U;
+		Cmd.Command2 = 0x00U;
+		XNandPsu_Prepare_Cmd(InstancePtr, Cmd.Command1, Cmd.Command2,
+				0U , 1U, (u8)AddrCycles);
+
+	} else {
+		XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_PG_PROG1,
+				ONFI_CMD_PG_PROG2, 0U , 1U, (u8)AddrCycles);
+	}
+	/* Program Page Size */
+	XNandPsu_SetPageSize(InstancePtr);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, PageVar, (u16)Col);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set Page Program in Program Register */
+	if (PostWrite > 0U) {
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,((u32)XNANDPSU_PROG_PG_PROG_MASK |
+				(u32)XNANDPSU_PROG_CHNG_ROW_ADDR_MASK));
+	} else {
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_PG_PROG_MASK);
+	}
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, (u8 *)BufPtr, PktCount,
+					 PktSize, 1, 1);
+
+	if (InstancePtr->EccMode == XNANDPSU_HWECC) {
+		if (PostWrite > 0U) {
+			BufPtr = (u32 *)(Buf + PostEccSpareCol);
+			Status = XNandPsu_ChangeWriteColumn(InstancePtr,
+					Target,
+					PostEccSpareCol, PostEccSpareWrCnt, 1U,
+					(u8 *)(void *)BufPtr);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+		}
+	}
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Read Page command to flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Page is the page address value to read.
+* @param	Col is the column address value to read.
+* @param	Buf is the data buffer to fill in.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_ReadPage(XNandPsu *InstancePtr, u32 Target, u32 Page,
+							u32 Col, u8 *Buf)
+{
+	u32 PktSize;
+	u32 PktCount;
+	s32 Status = XST_FAILURE;
+	u32 RegVal;
+	u32 AddrCycles = InstancePtr->Geometry.RowAddrCycles +
+				InstancePtr->Geometry.ColAddrCycles;
+
+	if (InstancePtr->EccCfg.CodeWordSize > 9U) {
+		PktSize = 1024U;
+	} else {
+		PktSize = 512U;
+	}
+	PktCount = InstancePtr->Geometry.BytesPerPage/PktSize;
+
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_RD1, ONFI_CMD_RD2,
+					1U, 1U, (u8)AddrCycles);
+
+	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
+		RegVal = XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK |
+			 XNANDPSU_INTR_STS_EN_DMA_INT_STS_EN_MASK;
+		if (InstancePtr->Config.IsCacheCoherent == 0) {
+			Xil_DCacheInvalidateRange((INTPTR)(void *)Buf, (PktSize * PktCount));
+		}
+		XNandPsu_Update_DmaAddr(InstancePtr, Buf);
+	} else {
+		RegVal = XNANDPSU_INTR_STS_EN_BUFF_RD_RDY_STS_EN_MASK;
+	}
+	/* Enable Single bit error and Multi bit error */
+	if (InstancePtr->EccMode == XNANDPSU_HWECC)
+		RegVal |= XNANDPSU_INTR_STS_EN_MUL_BIT_ERR_STS_EN_MASK |
+			 XNANDPSU_INTR_STS_EN_ERR_INTR_STS_EN_MASK;
+
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			   XNANDPSU_INTR_STS_EN_OFFSET, RegVal);
+	/* Program Page Size */
+	XNandPsu_SetPageSize(InstancePtr);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, Page, (u16)Col);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set ECC */
+	if (InstancePtr->EccMode == XNANDPSU_HWECC) {
+		XNandPsu_SetEccSpareCmd(InstancePtr,
+					(ONFI_CMD_CHNG_RD_COL1 |
+					(ONFI_CMD_CHNG_RD_COL2 << (u8)8U)),
+					InstancePtr->Geometry.ColAddrCycles);
+	}
+
+	/* Set Read command in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+				XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_RD_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 0, 1);
+
+	/* Check ECC Errors */
+	if (InstancePtr->EccMode == XNANDPSU_HWECC) {
+		/* Hamming Multi Bit Errors */
+		if (((u32)XNandPsu_ReadReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_INTR_STS_OFFSET) &
+			(u32)XNANDPSU_INTR_STS_MUL_BIT_ERR_STS_EN_MASK) != 0U) {
+
+			XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+				XNANDPSU_INTR_STS_OFFSET,
+				XNANDPSU_INTR_STS_MUL_BIT_ERR_STS_EN_MASK);
+
+#ifdef XNANDPSU_DEBUG
+			xil_printf("%s: ECC Hamming multi bit error\r\n",
+							__func__);
+#endif
+			InstancePtr->Ecc_Stat_PerPage_flips =
+					((XNandPsu_ReadReg(
+					InstancePtr->Config.BaseAddress,
+					XNANDPSU_ECC_ERR_CNT_OFFSET) &
+					0x1FF00U) >> 8U);
+			InstancePtr->Ecc_Stats_total_flips +=
+					InstancePtr->Ecc_Stat_PerPage_flips;
+			Status = XST_FAILURE;
+		}
+		/* Hamming Single Bit or BCH Errors */
+		if (((u32)XNandPsu_ReadReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_INTR_STS_OFFSET) &
+			(u32)XNANDPSU_INTR_STS_ERR_INTR_STS_EN_MASK) != 0U) {
+
+			XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+					XNANDPSU_INTR_STS_OFFSET,
+					XNANDPSU_INTR_STS_ERR_INTR_STS_EN_MASK);
+
+			if (InstancePtr->EccCfg.IsBCH == 1U) {
+				InstancePtr->Ecc_Stat_PerPage_flips =
+						((XNandPsu_ReadReg(
+						InstancePtr->Config.BaseAddress,
+						XNANDPSU_ECC_ERR_CNT_OFFSET)&
+						0x1FF00U) >> 8U);
+				InstancePtr->Ecc_Stats_total_flips +=
+					InstancePtr->Ecc_Stat_PerPage_flips;
+				Status = XST_SUCCESS;
+			}
+		}
+	}
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function reads spare bytes from flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Page is the page address value to read.
+* @param	Buf is the data buffer to fill in.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_ReadSpareBytes(XNandPsu *InstancePtr, u32 Page, u8 *Buf)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(Page < InstancePtr->Geometry.NumPages);
+	Xil_AssertNonvoid(Buf != NULL);
+
+	u32 PktCount = 1U;
+	s32 Status = XST_FAILURE;
+	u32 RegVal;
+	u32 AddrCycles = InstancePtr->Geometry.RowAddrCycles +
+				InstancePtr->Geometry.ColAddrCycles;
+	u32 Col = InstancePtr->Geometry.BytesPerPage;
+	u32 Target = Page/InstancePtr->Geometry.NumTargetPages;
+	u32 PktSize = InstancePtr->Geometry.SpareBytesPerPage;
+	u32 PageVar = Page;
+
+	PageVar %= InstancePtr->Geometry.NumTargetPages;
+
+	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
+		RegVal = XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK;
+		if (InstancePtr->Config.IsCacheCoherent == 0) {
+			Xil_DCacheInvalidateRange((INTPTR)(void *)Buf, (PktSize * PktCount));
+		}
+		XNandPsu_Update_DmaAddr(InstancePtr, Buf);
+	} else {
+		RegVal = XNANDPSU_INTR_STS_EN_BUFF_RD_RDY_STS_EN_MASK;
+	}
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			   XNANDPSU_INTR_STS_EN_OFFSET, RegVal);
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_RD1, ONFI_CMD_RD2, 0U,
+						1U, (u8)AddrCycles);
+	/* Program Page Size */
+	XNandPsu_SetPageSize(InstancePtr);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, PageVar, (u16)Col);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set Read command in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+				XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_RD_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 0, 1);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI block erase command to the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Block is the block to erase.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_EraseBlock(XNandPsu *InstancePtr, u32 Target, u32 Block)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(Target < XNANDPSU_MAX_TARGETS);
+	Xil_AssertNonvoid(Block < InstancePtr->Geometry.NumBlocks);
+
+	s32 Status = XST_FAILURE;
+	u32 Page;
+	u32 ErasePage;
+	u32 EraseCol;
+	u32 AddrCycles = InstancePtr->Geometry.RowAddrCycles;
+
+	Page = Block * InstancePtr->Geometry.PagesPerBlock;
+	ErasePage = (Page >> 16U) & 0xFFFFU;
+	EraseCol = Page & 0xFFFFU;
+
+	/*
+	 * Enable Transfer Complete Interrupt in Interrupt Status Enable
+	 * Register
+	 */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_EN_OFFSET,
+			XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK);
+
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_BLK_ERASE1,
+			ONFI_CMD_BLK_ERASE2, 0U , 0U, (u8)AddrCycles);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, ErasePage, (u16)EraseCol);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set Block Erase in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_BLK_ERASE_MASK);
+	/* Poll for Transfer Complete event */
+	Status = XNandPsu_WaitFor_Transfer_Complete(InstancePtr);
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Get Feature command to flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Feature is the feature selector.
+* @param	Buf is the buffer to fill feature value.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_GetFeature(XNandPsu *InstancePtr, u32 Target, u8 Feature,
+								u8 *Buf)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY)
+	Xil_AssertNonvoid(Buf != NULL);
+	Xil_AssertNonvoid(Target < XNANDPSU_MAX_TARGETS);
+
+	s32 Status;
+	u32 PktSize = 4;
+	u32 PktCount = 1;
+
+	if (InstancePtr->DataInterface == XNANDPSU_NVDDR) {
+		PktSize = 8U;
+	}
+
+	/*
+	 * Enable Buffer Read Ready Interrupt in Interrupt Status
+	 * Enable Register
+	 */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_EN_OFFSET,
+			XNANDPSU_INTR_STS_EN_BUFF_RD_RDY_STS_EN_MASK);
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_GET_FEATURES,
+				ONFI_CMD_INVALID, 0U, 0U, 1U);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, 0x0U, Feature);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Set Read Parameter Page in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_GET_FEATURES_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 0, 0);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function sends ONFI Set Feature command to flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Feature is the feature selector.
+* @param	Buf is the feature value to send.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_SetFeature(XNandPsu *InstancePtr, u32 Target, u8 Feature,
+								u8 *Buf)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY)
+	Xil_AssertNonvoid(Buf != NULL);
+	Xil_AssertNonvoid(Target < XNANDPSU_MAX_TARGETS);
+
+	s32 Status;
+	u32 PktSize = 4U;
+	u32 PktCount = 1U;
+
+	if (InstancePtr->DataInterface == XNANDPSU_NVDDR) {
+		PktSize = 8U;
+	}
+
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_EN_OFFSET, 0U);
+
+	/*
+	 * Enable Buffer Write Ready Interrupt in Interrupt Status
+	 * Enable Register
+	 */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_EN_OFFSET,
+			XNANDPSU_INTR_STS_EN_BUFF_WR_RDY_STS_EN_MASK);
+
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_SET_FEATURES,
+				ONFI_CMD_INVALID, 0U , 0U, 1U);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, 0x0U, Feature);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Set Read Parameter Page in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_SET_FEATURES_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 1, 0);
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function changes clock frequency of flash controller.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	ClockFreq is the clock frequency to change.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_ChangeClockFreq(XNandPsu *InstancePtr, u32 ClockFreq)
+{
+	(void) InstancePtr;
+	(void) ClockFreq;
+
+	/* Not implemented */
+}
+/*****************************************************************************/
+/**
+*
+* This function changes the data interface and timing mode.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	NewIntf is the new data interface.
+* @param	NewMode is the new timing mode.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+s32 XNandPsu_ChangeTimingMode(XNandPsu *InstancePtr,
+				XNandPsu_DataInterface NewIntf,
+				XNandPsu_TimingMode NewMode)
+{
+	/* Assert the input arguments. */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+
+	s32 Status = XST_SUCCESS;
+	u32 Target;
+	u32 RegVal;
+	u8 Buf[4] = {0U};
+	u32 *Feature = (u32 *)(void *)&Buf[0];
+	u32 SetFeature = 0U;
+	u32 NewModeVar = (u32)NewMode;
+
+	/* Check for valid input arguments */
+	if(((NewIntf != XNANDPSU_SDR) && (NewIntf != XNANDPSU_NVDDR)) ||
+			(NewModeVar > 5U)){
+		Status = XST_FAILURE;
+		goto Out;
+	}
+
+	if(NewIntf == XNANDPSU_NVDDR){
+		NewModeVar = NewModeVar | (u32)0x10;
+	}
+	/* Get current data interface type and timing mode */
+	XNandPsu_DataInterface CurIntf = InstancePtr->DataInterface;
+	XNandPsu_TimingMode CurMode = InstancePtr->TimingMode;
+
+	/* Check if the flash is in same mode */
+	if ((CurIntf == NewIntf) && (CurMode == NewModeVar)) {
+		Status = XST_SUCCESS;
+		goto Out;
+	}
+
+	if ((CurIntf == XNANDPSU_NVDDR) && (NewIntf == XNANDPSU_SDR)) {
+
+		NewModeVar = XNANDPSU_SDR0;
+
+		/* Change the clock frequency */
+		XNandPsu_ChangeClockFreq(InstancePtr, XNANDPSU_SDR_CLK);
+
+		/* Update Data Interface Register */
+		RegVal = ((NewModeVar % 6U) << ((NewIntf == XNANDPSU_NVDDR) ? 3U : 0U)) |
+				((u32)NewIntf << XNANDPSU_DATA_INTF_DATA_INTF_SHIFT);
+		XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+					XNANDPSU_DATA_INTF_OFFSET, RegVal);
+
+		for (Target = 0U; Target < InstancePtr->Geometry.NumTargets;
+							Target++) {
+			Status = XNandPsu_OnfiReset(InstancePtr, Target);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+		}
+
+		/* Set Feature */
+		for (Target = 0U; Target < InstancePtr->Geometry.NumTargets;
+								Target++) {
+			Status = XNandPsu_SetFeature(InstancePtr, Target, 0x01U,
+							(u8 *)(void *)&NewModeVar);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+		}
+
+		InstancePtr->DataInterface = NewIntf;
+		InstancePtr->TimingMode = NewModeVar;
+
+		for (Target = 0U; Target < InstancePtr->Geometry.NumTargets;
+								Target++) {
+			Status = XNandPsu_GetFeature(InstancePtr, Target, 0x01U,
+								&Buf[0]);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			/* Check if set_feature was successful */
+			if (*Feature != NewModeVar) {
+				Status = XST_FAILURE;
+				goto Out;
+			}
+		}
+
+		goto Out;
+	}
+
+	SetFeature = NewModeVar;
+	if((CurIntf == XNANDPSU_NVDDR) && (NewIntf == XNANDPSU_NVDDR)){
+		SetFeature |= SetFeature << 8U;
+	}
+	/* Set Feature */
+	for (Target = 0U; Target < InstancePtr->Geometry.NumTargets;
+							Target++) {
+		Status = XNandPsu_SetFeature(InstancePtr, Target, 0x01U,
+						(u8 *)(void *)&SetFeature);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+	}
+
+	InstancePtr->DataInterface = NewIntf;
+	InstancePtr->TimingMode = NewModeVar;
+	/* Update Data Interface Register */
+	RegVal = ((NewMode % 6U) << ((NewIntf == XNANDPSU_NVDDR) ? 3U : 0U)) |
+			((u32)NewIntf << XNANDPSU_DATA_INTF_DATA_INTF_SHIFT);
+	XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_DATA_INTF_OFFSET, RegVal);
+
+	/* Get Feature */
+	for (Target = 0U; Target < InstancePtr->Geometry.NumTargets;
+							Target++) {
+		Status = XNandPsu_GetFeature(InstancePtr, Target, 0x01U,
+							&Buf[0]);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+
+		/* Check if set_feature was successful */
+		if (*Feature != NewModeVar) {
+			Status = XST_FAILURE;
+			goto Out;
+		}
+	}
+
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function issues change read column and reads the data into buffer
+* specified by user.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Col is the coulmn address.
+* @param	PktSize is the number of bytes to read.
+* @param	PktCount is the number of transactions to read.
+* @param	Buf is the data buffer to fill in.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_ChangeReadColumn(XNandPsu *InstancePtr, u32 Target,
+					u32 Col, u32 PktSize, u32 PktCount,
+					u8 *Buf)
+{
+	s32 Status = XST_FAILURE;
+	u32 RegVal;
+	u32 AddrCycles = InstancePtr->Geometry.ColAddrCycles;
+
+	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
+		RegVal = XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK |
+			 XNANDPSU_INTR_STS_EN_DMA_INT_STS_EN_MASK;
+		Xil_DCacheInvalidateRange((INTPTR)(void *)Buf, (PktSize * PktCount));
+		XNandPsu_Update_DmaAddr(InstancePtr, Buf);
+	} else {
+		RegVal = XNANDPSU_INTR_STS_EN_BUFF_RD_RDY_STS_EN_MASK;
+	}
+
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			   XNANDPSU_INTR_STS_EN_OFFSET, RegVal);
+	/* Program Command */
+	XNandPsu_Prepare_Cmd(InstancePtr, ONFI_CMD_CHNG_RD_COL1,
+			ONFI_CMD_CHNG_RD_COL2, 0U , 1U, (u8)AddrCycles);
+	/* Program Page Size */
+	XNandPsu_SetPageSize(InstancePtr);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, 0U, (u16)Col);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set Read command in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_RD_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 0, 1);
+
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function issues change read column and reads the data into buffer
+* specified by user.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chip select value.
+* @param	Col is the coulmn address.
+* @param	PktSize is the number of bytes to read.
+* @param	PktCount is the number of transactions to read.
+* @param	Buf is the data buffer to fill in.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_ChangeWriteColumn(XNandPsu *InstancePtr, u32 Target,
+					u32 Col, u32 PktSize, u32 PktCount,
+					u8 *Buf)
+{
+	s32 Status = XST_FAILURE;
+	OnfiCmdFormat OnfiCommand;
+	u32 RegVal;
+	u32 AddrCycles = InstancePtr->Geometry.ColAddrCycles;
+
+	if (PktCount == 0U) {
+		return XST_SUCCESS;
+	}
+
+	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
+		RegVal = XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK |
+			 XNANDPSU_INTR_STS_EN_DMA_INT_STS_EN_MASK;
+		XNandPsu_Update_DmaAddr(InstancePtr, Buf);
+	} else {
+		RegVal = XNANDPSU_INTR_STS_EN_BUFF_WR_RDY_STS_EN_MASK;
+	}
+
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			   XNANDPSU_INTR_STS_EN_OFFSET, RegVal);
+	/* Change write column hack */
+	OnfiCommand.Command1 = 0x85U;
+	OnfiCommand.Command2 = 0x10U;
+	XNandPsu_Prepare_Cmd(InstancePtr, OnfiCommand.Command1,
+				OnfiCommand.Command2, 0U , 0U, (u8)AddrCycles);
+
+	/* Program Page Size */
+	XNandPsu_SetPageSize(InstancePtr);
+	/* Program Column, Page, Block address */
+	XNandPsu_SetPageColAddr(InstancePtr, 0U, (u16)Col);
+	/* Program Packet Size and Packet Count */
+	XNandPsu_SetPktSzCnt(InstancePtr, PktSize, PktCount);
+	/* Program Memory Address Register2 for chip select */
+	XNandPsu_SelectChip(InstancePtr, Target);
+	/* Set Page Program in Program Register */
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_PROG_OFFSET,XNANDPSU_PROG_CHNG_ROW_ADDR_END_MASK);
+
+	Status = XNandPsu_Data_ReadWrite(InstancePtr, Buf, PktCount, PktSize, 1, 0);
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function initializes extended parameter page ECC information.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	ExtPrm is the Extended parameter page buffer.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_InitExtEcc(XNandPsu *InstancePtr, OnfiExtPrmPage *ExtPrm)
+{
+	s32 Status = XST_FAILURE;
+	u32 Offset = 0U;
+	u32 Found = 0U;
+	OnfiExtEccBlock *EccBlock;
+
+	if (ExtPrm->Section0Type != 0x2U) {
+		Offset += (u32)ExtPrm->Section0Len;
+		if (ExtPrm->Section1Type != 0x2U) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: Extended ECC section not found\r\n",__func__);
+#endif
+			Status = XST_FAILURE;
+		} else {
+			Found = 1U;
+		}
+	} else {
+		Found = 1U;
+	}
+
+	if (Found != 0U) {
+		EccBlock = (OnfiExtEccBlock *)&ExtPrm->SectionData[Offset];
+		Xil_AssertNonvoid(EccBlock != NULL);
+		if (EccBlock->CodeWordSize == 0U) {
+			Status = XST_FAILURE;
+		} else {
+			InstancePtr->Geometry.NumBitsECC =
+						EccBlock->NumEccBits;
+			InstancePtr->Geometry.EccCodeWordSize =
+						(u32)EccBlock->CodeWordSize;
+			Status = XST_SUCCESS;
+		}
+	}
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function prepares command to be written into command register.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Cmd1 is the first Onfi Command.
+* @param	Cmd2 is the second Onfi Command.
+* @param	EccState is the flag to set Ecc State.
+* @param	DmaMode is the flag to set DMA mode.
+* @param	AddrCycles is the number of Address Cycles.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+void XNandPsu_Prepare_Cmd(XNandPsu *InstancePtr, u8 Cmd1, u8 Cmd2, u8 EccState,
+			u8 DmaMode, u8 AddrCycles)
+{
+	Xil_AssertVoid(InstancePtr != NULL);
+
+	u32 RegValue = 0U;
+
+	RegValue = (u32)Cmd1 | (((u32)Cmd2 << (u32)XNANDPSU_CMD_CMD2_SHIFT) &
+			(u32)XNANDPSU_CMD_CMD2_MASK);
+
+	if ((EccState != 0U) && (InstancePtr->EccMode == XNANDPSU_HWECC)) {
+		RegValue |= 1U << XNANDPSU_CMD_ECC_ON_SHIFT;
+	}
+
+	if ((DmaMode != 0U) && (InstancePtr->DmaMode == XNANDPSU_MDMA)) {
+		RegValue |= XNANDPSU_MDMA << XNANDPSU_CMD_DMA_EN_SHIFT;
+	}
+
+	if (AddrCycles != 0U) {
+		RegValue |= (u32)AddrCycles <<
+				(u32)XNANDPSU_CMD_ADDR_CYCLES_SHIFT;
+	}
+
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_CMD_OFFSET, RegValue);
+}
+
+/*****************************************************************************/
+/**
+*
+* This function Read/Writes data from the nand controller.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Buf is the data buffer.
+* @param	PktCount is the number packet chunks.
+* @param	PktSize is the size of the packet.
+* @param	Operation is 1 for write and 0 for read.
+* @param	DmaMode is 1 for Dma and 0 for PIO.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_Data_ReadWrite(XNandPsu *InstancePtr, u8* Buf, u32 PktCount,
+				u32 PktSize, u32 Operation, u8 DmaMode)
+{
+	u32 BufRwCnt = 0U;
+	s32 Status = XST_FAILURE;
+	u32 Event = XNANDPSU_INTR_STS_BUFF_RD_RDY_STS_EN_MASK;
+
+	if ((DmaMode != 0U) && (InstancePtr->DmaMode == XNANDPSU_MDMA))
+		goto DmaDone;
+
+	if (Operation)
+		Event = XNANDPSU_INTR_STS_BUFF_WR_RDY_STS_EN_MASK;
+
+	while (BufRwCnt < PktCount) {
+		/* Poll for Buffer Write Ready event */
+		Status = XNandPsu_PollRegTimeout(InstancePtr,
+				XNANDPSU_INTR_STS_OFFSET, Event,
+				XNANDPSU_INTR_POLL_TIMEOUT);
+		if (Status != XST_SUCCESS) {
+			xil_printf("%s: Poll for buf write ready timeout\r\n",
+				    __func__);
+			goto Out;
+		}
+
+		/* Increment Buffer Write Interrupt Count */
+		BufRwCnt++;
+
+		if (BufRwCnt == PktCount)
+			XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+				XNANDPSU_INTR_STS_EN_OFFSET,
+				XNANDPSU_INTR_STS_EN_TRANS_COMP_STS_EN_MASK);
+
+		else
+			XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+				XNANDPSU_INTR_STS_EN_OFFSET, 0U);
+		/*
+	         * Clear Buffer Write Ready Interrupt in Interrupt Status
+		 * Register
+		 */
+		XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_OFFSET, Event);
+		/* Write Packet Data to Data Port Register */
+		if (Operation)
+			XNandPsu_Fifo_Write(InstancePtr, Buf, PktSize);
+		else
+			XNandPsu_Fifo_Read(InstancePtr, Buf, PktSize);
+
+		Buf += PktSize;
+
+		if (BufRwCnt < PktCount)
+			XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+				XNANDPSU_INTR_STS_EN_OFFSET, Event);
+		else
+			break;
+	}
+
+DmaDone:
+	Status = XNandPsu_WaitFor_Transfer_Complete(InstancePtr);
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function writes data to the fifo.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Buf is the buffer pointer.
+* @param	Size of the Buffer.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_Fifo_Write(XNandPsu *InstancePtr, u8* Buffer, u32 Size)
+{
+	u32 *BufPtr = (u32 *)(void *)Buffer;
+	u32 Index;
+
+	for (Index = 0U; Index < Size/4U; Index++)
+		XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_BUF_DATA_PORT_OFFSET,
+				BufPtr[Index]);
+}
+
+/*****************************************************************************/
+/**
+*
+* This function reads data from the fifo.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Buf is the buffer pointer.
+* @param	Size of the Buffer.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_Fifo_Read(XNandPsu *InstancePtr, u8* Buf, u32 Size)
+{
+	u32 *BufPtr = (u32 *)(void *)Buf;
+	u32 Index;
+
+	for (Index = 0U; Index < Size/4U; Index++)
+		BufPtr[Index] = XNandPsu_ReadReg(InstancePtr->Config.BaseAddress,
+					XNANDPSU_BUF_DATA_PORT_OFFSET);
+}
+
+/*****************************************************************************/
+/**
+*
+* This function configures the given dma address to the controller.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Buf is the buffer pointer.
+*
+* @return
+*		None
+*
+* @note		None
+*
+******************************************************************************/
+static void XNandPsu_Update_DmaAddr(XNandPsu *InstancePtr, u8* Buf)
+{
+#if defined(__aarch64__) || defined(__arch64__)
+		XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_DMA_SYS_ADDR1_OFFSET,
+				(u32) (((INTPTR)Buf >> 32U) & 0xFFFFFFFFU));
+#endif
+		XNandPsu_WriteReg(InstancePtr->Config.BaseAddress,
+				XNANDPSU_DMA_SYS_ADDR0_OFFSET,
+				(u32) ((INTPTR)(void *)Buf & 0xFFFFFFFFU));
+
+}
+
+/*****************************************************************************/
+/**
+*
+* This function waits for the device ready stataus.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Target is the chipselect value.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		None
+*
+******************************************************************************/
+static s32 XNandPsu_Device_Ready(XNandPsu *InstancePtr, u32 Target)
+{
+	s32 Status = XST_SUCCESS;
+	u16 OnfiStatus = 0U;
+
+	do {
+		Status = XNandPsu_OnfiReadStatus(InstancePtr, Target,
+							&OnfiStatus);
+		if (Status != XST_SUCCESS)
+			goto Out;
+		if ((OnfiStatus & (1U << 6U)) != 0U) {
+			if ((OnfiStatus & (1U << 0U)) != 0U) {
+				Status = XST_FAILURE;
+				goto Out;
+			}
+		}
+	} while (((OnfiStatus >> 6U) & 0x1U) == 0U);
+
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function waits for the  transfer complete event.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if failed.
+*
+* @note		Expects that transfer complete event was set before calling
+* 		this function.
+*
+******************************************************************************/
+static s32 XNandPsu_WaitFor_Transfer_Complete(XNandPsu *InstancePtr)
+{
+s32 Status = XST_FAILURE;
+
+	 /* Poll for Transfer Complete event */
+	Status = XNandPsu_PollRegTimeout(
+			InstancePtr,
+			XNANDPSU_INTR_STS_OFFSET,
+			XNANDPSU_INTR_STS_TRANS_COMP_STS_EN_MASK,
+			XNANDPSU_INTR_POLL_TIMEOUT);
+	if (Status != XST_SUCCESS) {
+		xil_printf("%s: Poll for xfer complete timeout\r\n", __func__);
+		goto Out;
+	}
+
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+		XNANDPSU_INTR_STS_EN_OFFSET, 0U);
+
+	XNandPsu_WriteReg((InstancePtr)->Config.BaseAddress,
+			XNANDPSU_INTR_STS_OFFSET,
+			XNANDPSU_INTR_STS_TRANS_COMP_STS_EN_MASK);
+#if defined  (XCLOCKING)
+	Xil_ClockDisable(InstancePtr->Config.RefClk);
+#endif
+Out:
+	return Status;
+}
+/** @} */
diff --git a/bsps/shared/dev/nand/xnandpsu_bbm.c b/bsps/shared/dev/nand/xnandpsu_bbm.c
new file mode 100644
index 0000000000..c43a2ba62f
--- /dev/null
+++ b/bsps/shared/dev/nand/xnandpsu_bbm.c
@@ -0,0 +1,912 @@
+/******************************************************************************
+* Copyright (C) 2015 - 2022 Xilinx, Inc.  All rights reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xnandpsu_bbm.c
+* @addtogroup nandpsu_v1_10
+* @{
+*
+* This file implements the Bad Block Management (BBM) functionality.
+* See xnandpsu_bbm.h for more details.
+*
+* @note		None
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date        Changes
+* ----- ----   ----------  -----------------------------------------------
+* 1.0   nm     05/06/2014  First release
+* 2.0   sb     01/12/2015  Added support for writing BBT signature and version
+*			   in page section by enabling XNANDPSU_BBT_NO_OOB.
+*			   Modified Bbt Signature and Version Offset value for
+*			   Oob and No-Oob region.
+* 1.1	nsk    11/07/16    Change memcpy to Xil_MemCpy to handle word aligned
+*	                   data access.
+* 1.4	nsk    04/10/18    Added ICCARM compiler support.
+* 1.10	akm    01/05/22    Remove assert checks form static and internal APIs.
+* </pre>
+*
+******************************************************************************/
+
+/***************************** Include Files *********************************/
+#include <string.h>	/**< For Xil_MemCpy and memset */
+#include "xil_types.h"
+#include "xnandpsu.h"
+#include "xnandpsu_bbm.h"
+#include "xil_mem.h"
+
+/************************** Constant Definitions *****************************/
+
+/**************************** Type Definitions *******************************/
+
+/***************** Macros (Inline Functions) Definitions *********************/
+
+/************************** Function Prototypes ******************************/
+static s32 XNandPsu_ReadBbt(XNandPsu *InstancePtr, u32 Target);
+
+static s32 XNandPsu_SearchBbt(XNandPsu *InstancePtr, XNandPsu_BbtDesc *Desc,
+							u32 Target);
+
+static void XNandPsu_CreateBbt(XNandPsu *InstancePtr, u32 Target);
+
+static void XNandPsu_ConvertBbt(XNandPsu *InstancePtr, u8 *Buf, u32 Target);
+
+static s32 XNandPsu_WriteBbt(XNandPsu *InstancePtr, XNandPsu_BbtDesc *Desc,
+				XNandPsu_BbtDesc *MirrorDesc, u32 Target);
+
+static s32 XNandPsu_MarkBbt(XNandPsu* InstancePtr, XNandPsu_BbtDesc *Desc,
+							u32 Target);
+
+static s32 XNandPsu_UpdateBbt(XNandPsu *InstancePtr, u32 Target);
+
+/************************** Variable Definitions *****************************/
+
+/*****************************************************************************/
+/**
+* This function initializes the Bad Block Table(BBT) descriptors with a
+* predefined pattern for searching Bad Block Table(BBT) in flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		- NONE
+*
+******************************************************************************/
+void XNandPsu_InitBbtDesc(XNandPsu *InstancePtr)
+{
+	u32 Index;
+
+	/* Initialize primary Bad Block Table(BBT) */
+	for (Index = 0U; Index < XNANDPSU_MAX_TARGETS; Index++) {
+		InstancePtr->BbtDesc.PageOffset[Index] =
+						XNANDPSU_BBT_DESC_PAGE_OFFSET;
+	}
+	if (InstancePtr->EccMode == XNANDPSU_ONDIE) {
+		InstancePtr->BbtDesc.SigOffset = XNANDPSU_ONDIE_SIG_OFFSET;
+		InstancePtr->BbtDesc.VerOffset = XNANDPSU_ONDIE_VER_OFFSET;
+	} else {
+		InstancePtr->BbtDesc.SigOffset = XNANDPSU_BBT_DESC_SIG_OFFSET;
+		InstancePtr->BbtDesc.VerOffset = XNANDPSU_BBT_DESC_VER_OFFSET;
+	}
+	InstancePtr->BbtDesc.SigLength = XNANDPSU_BBT_DESC_SIG_LEN;
+	InstancePtr->BbtDesc.MaxBlocks = XNANDPSU_BBT_DESC_MAX_BLOCKS;
+	(void)strcpy(&InstancePtr->BbtDesc.Signature[0], "Bbt0");
+	for (Index = 0U; Index < XNANDPSU_MAX_TARGETS; Index++) {
+		InstancePtr->BbtDesc.Version[Index] = 0U;
+	}
+	InstancePtr->BbtDesc.Valid = 0U;
+
+	/* Assuming that the flash device will have at least 4 blocks. */
+	if (InstancePtr->Geometry.NumTargetBlocks <= InstancePtr->
+							BbtDesc.MaxBlocks){
+		InstancePtr->BbtDesc.MaxBlocks = 4U;
+	}
+
+	/* Initialize mirror Bad Block Table(BBT) */
+	for (Index = 0U; Index < XNANDPSU_MAX_TARGETS; Index++) {
+		InstancePtr->BbtMirrorDesc.PageOffset[Index] =
+						XNANDPSU_BBT_DESC_PAGE_OFFSET;
+	}
+	if (InstancePtr->EccMode == XNANDPSU_ONDIE) {
+		InstancePtr->BbtMirrorDesc.SigOffset =
+					XNANDPSU_ONDIE_SIG_OFFSET;
+		InstancePtr->BbtMirrorDesc.VerOffset =
+					XNANDPSU_ONDIE_VER_OFFSET;
+	} else {
+		InstancePtr->BbtMirrorDesc.SigOffset =
+					XNANDPSU_BBT_DESC_SIG_OFFSET;
+		InstancePtr->BbtMirrorDesc.VerOffset =
+					XNANDPSU_BBT_DESC_VER_OFFSET;
+	}
+	InstancePtr->BbtMirrorDesc.SigLength = XNANDPSU_BBT_DESC_SIG_LEN;
+	InstancePtr->BbtMirrorDesc.MaxBlocks = XNANDPSU_BBT_DESC_MAX_BLOCKS;
+	(void)strcpy(&InstancePtr->BbtMirrorDesc.Signature[0], "1tbB");
+	for (Index = 0U; Index < XNANDPSU_MAX_TARGETS; Index++) {
+		InstancePtr->BbtMirrorDesc.Version[Index] = 0U;
+	}
+	InstancePtr->BbtMirrorDesc.Valid = 0U;
+
+	/* Assuming that the flash device will have at least 4 blocks. */
+	if (InstancePtr->Geometry.NumTargetBlocks <= InstancePtr->
+						BbtMirrorDesc.MaxBlocks){
+		InstancePtr->BbtMirrorDesc.MaxBlocks = 4U;
+	}
+
+	/* Initialize Bad block search pattern structure */
+	if (InstancePtr->Geometry.BytesPerPage > 512U) {
+		/* For flash page size > 512 bytes */
+		InstancePtr->BbPattern.Options = XNANDPSU_BBT_SCAN_2ND_PAGE;
+		InstancePtr->BbPattern.Offset =
+			XNANDPSU_BB_PTRN_OFF_LARGE_PAGE;
+		InstancePtr->BbPattern.Length =
+			XNANDPSU_BB_PTRN_LEN_LARGE_PAGE;
+	} else {
+		InstancePtr->BbPattern.Options = XNANDPSU_BBT_SCAN_2ND_PAGE;
+		InstancePtr->BbPattern.Offset =
+			XNANDPSU_BB_PTRN_OFF_SML_PAGE;
+		InstancePtr->BbPattern.Length =
+			XNANDPSU_BB_PTRN_LEN_SML_PAGE;
+	}
+	for(Index = 0U; Index < XNANDPSU_BB_PTRN_LEN_LARGE_PAGE; Index++) {
+		InstancePtr->BbPattern.Pattern[Index] = XNANDPSU_BB_PATTERN;
+	}
+}
+
+/*****************************************************************************/
+/**
+* This function scans the NAND flash for factory marked bad blocks and creates
+* a RAM based Bad Block Table(BBT).
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		- NONE
+*
+******************************************************************************/
+static void XNandPsu_CreateBbt(XNandPsu *InstancePtr, u32 Target)
+{
+	u32 BlockIndex;
+	u32 PageIndex;
+	u32 Length;
+	u32 BlockOffset;
+	u8 BlockShift;
+	u32 NumPages;
+	u32 Page;
+#ifdef __ICCARM__
+#pragma pack(push, 1)
+	u8 Buf[XNANDPSU_MAX_SPARE_SIZE] = {0U};
+#pragma pack(pop)
+#else
+	u8 Buf[XNANDPSU_MAX_SPARE_SIZE] __attribute__ ((aligned(64))) = {0U};
+#endif
+	u32 StartBlock = Target * InstancePtr->Geometry.NumTargetBlocks;
+	u32 NumBlocks = InstancePtr->Geometry.NumTargetBlocks;
+	s32 Status;
+
+	/* Number of pages to search for bad block pattern */
+	if ((InstancePtr->BbPattern.Options & XNANDPSU_BBT_SCAN_2ND_PAGE) != 0U)
+	{
+		NumPages = 2U;
+	} else {
+		NumPages = 1U;
+	}
+	/* Scan all the blocks for factory marked bad blocks */
+	for(BlockIndex = StartBlock; BlockIndex < (StartBlock + NumBlocks);
+							BlockIndex++) {
+		/* Block offset in Bad Block Table(BBT) entry */
+		BlockOffset = BlockIndex >> XNANDPSU_BBT_BLOCK_SHIFT;
+		/* Block shift value in the byte */
+		BlockShift = XNandPsu_BbtBlockShift(BlockIndex);
+		Page = BlockIndex * InstancePtr->Geometry.PagesPerBlock;
+		/* Search for the bad block pattern */
+		for(PageIndex = 0U; PageIndex < NumPages; PageIndex++) {
+			Status = XNandPsu_ReadSpareBytes(InstancePtr,
+					(Page + PageIndex), &Buf[0]);
+
+			if (Status != XST_SUCCESS) {
+				/* Marking as bad block */
+				InstancePtr->Bbt[BlockOffset] |=
+					(u8)(XNANDPSU_BLOCK_FACTORY_BAD <<
+					 BlockShift);
+				break;
+			}
+			/*
+			 * Read the spare bytes to check for bad block
+			 * pattern
+			 */
+			for(Length = 0U; Length <
+				InstancePtr->BbPattern.Length; Length++) {
+				if (Buf[InstancePtr->BbPattern.Offset + Length]
+						!=
+					InstancePtr->BbPattern.Pattern[Length])
+				{
+					/* Bad block found */
+					InstancePtr->Bbt[BlockOffset] |=
+						(u8)
+						(XNANDPSU_BLOCK_FACTORY_BAD <<
+						 BlockShift);
+					break;
+				}
+			}
+		}
+	}
+}
+
+/*****************************************************************************/
+/**
+* This function reads the Bad Block Table(BBT) if present in flash. If not it
+* scans the flash for detecting factory marked bad blocks and creates a bad
+* block table and write the Bad Block Table(BBT) into the flash.
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+s32 XNandPsu_ScanBbt(XNandPsu *InstancePtr)
+{
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY)
+
+	s32 Status;
+	u32 Index;
+	u32 BbtLen;
+
+	/* Zero the RAM based Bad Block Table(BBT) entries */
+	BbtLen = InstancePtr->Geometry.NumBlocks >>
+					XNANDPSU_BBT_BLOCK_SHIFT;
+	(void)memset(&InstancePtr->Bbt[0], 0, BbtLen);
+
+	for (Index = 0U; Index < InstancePtr->Geometry.NumTargets; Index++) {
+
+		if (XNandPsu_ReadBbt(InstancePtr, Index) != XST_SUCCESS) {
+			/* Create memory based Bad Block Table(BBT) */
+			XNandPsu_CreateBbt(InstancePtr, Index);
+			/* Write the Bad Block Table(BBT) to the flash */
+			Status = XNandPsu_WriteBbt(InstancePtr,
+					&InstancePtr->BbtDesc,
+					&InstancePtr->BbtMirrorDesc, Index);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			/* Write the Mirror Bad Block Table(BBT) to the flash */
+			Status = XNandPsu_WriteBbt(InstancePtr,
+					&InstancePtr->BbtMirrorDesc,
+					&InstancePtr->BbtDesc, Index);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			/*
+			 * Mark the blocks containing Bad Block Table
+			 * (BBT) as Reserved
+			 */
+			Status = XNandPsu_MarkBbt(InstancePtr,
+							&InstancePtr->BbtDesc,
+							Index);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			Status = XNandPsu_MarkBbt(InstancePtr,
+						&InstancePtr->BbtMirrorDesc,
+							Index);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+		}
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+* This function converts the Bad Block Table(BBT) read from the flash to the
+* RAM based Bad Block Table(BBT).
+*
+* @param	InstancePtr is a pointer to the XNandPsu instance.
+* @param	Buf is the buffer which contains BBT read from flash.
+*
+* @return
+*		- NONE.
+*
+******************************************************************************/
+static void XNandPsu_ConvertBbt(XNandPsu *InstancePtr, u8 *Buf, u32 Target)
+{
+	u32 BlockOffset;
+	u8 BlockShift;
+	u32 Data;
+	u8 BlockType;
+	u32 BlockIndex;
+	u32 BbtLen = InstancePtr->Geometry.NumTargetBlocks >>
+					XNANDPSU_BBT_BLOCK_SHIFT;
+	u32 StartBlock = Target * InstancePtr->Geometry.NumTargetBlocks;
+
+	for(BlockOffset = StartBlock; BlockOffset < (StartBlock + BbtLen);
+						BlockOffset++) {
+		Data = *(Buf + BlockOffset);
+		/* Clear the RAM based Bad Block Table(BBT) contents */
+		InstancePtr->Bbt[BlockOffset] = 0x0U;
+		/* Loop through the every 4 blocks in the bitmap */
+		for(BlockIndex = 0U; BlockIndex < XNANDPSU_BBT_ENTRY_NUM_BLOCKS;
+				BlockIndex++) {
+			BlockShift = XNandPsu_BbtBlockShift(BlockIndex);
+			BlockType = (u8) ((Data >> BlockShift) &
+				XNANDPSU_BLOCK_TYPE_MASK);
+			switch(BlockType) {
+				case XNANDPSU_FLASH_BLOCK_FAC_BAD:
+					/* Factory bad block */
+					InstancePtr->Bbt[BlockOffset] |=
+						(u8)
+						(XNANDPSU_BLOCK_FACTORY_BAD <<
+						BlockShift);
+					break;
+				case XNANDPSU_FLASH_BLOCK_RESERVED:
+					/* Reserved block */
+					InstancePtr->Bbt[BlockOffset] |=
+						(u8)
+						(XNANDPSU_BLOCK_RESERVED <<
+						BlockShift);
+					break;
+				case XNANDPSU_FLASH_BLOCK_BAD:
+					/* Bad block due to wear */
+					InstancePtr->Bbt[BlockOffset] |=
+						(u8)(XNANDPSU_BLOCK_BAD <<
+						BlockShift);
+					break;
+				default:
+					/* Good block */
+					/* The BBT entry already defaults to
+					 * zero */
+					break;
+			}
+		}
+	}
+}
+
+/*****************************************************************************/
+/**
+* This function searches the Bad Bloock Table(BBT) in flash and loads into the
+* memory based Bad Block Table(BBT).
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+static s32 XNandPsu_ReadBbt(XNandPsu *InstancePtr, u32 Target)
+{
+	u64 Offset;
+#ifdef __ICCARM__
+#pragma pack(push, 1)
+	u8 Buf[XNANDPSU_BBT_BUF_LENGTH]= {0U};
+#pragma pack(pop)
+#else
+	u8 Buf[XNANDPSU_BBT_BUF_LENGTH] __attribute__ ((aligned(64))) = {0U};
+#endif
+	s32 Status1;
+	s32 Status2;
+	s32 Status;
+	u32 BufLen;
+
+	XNandPsu_BbtDesc *Desc = &InstancePtr->BbtDesc;
+	XNandPsu_BbtDesc *MirrorDesc = &InstancePtr->BbtMirrorDesc;
+	BufLen = InstancePtr->Geometry.NumBlocks >>
+			XNANDPSU_BBT_BLOCK_SHIFT;
+	/* Search the Bad Block Table(BBT) in flash */
+	Status1 = XNandPsu_SearchBbt(InstancePtr, Desc, Target);
+	Status2 = XNandPsu_SearchBbt(InstancePtr, MirrorDesc, Target);
+	if ((Status1 != XST_SUCCESS) && (Status2 != XST_SUCCESS)) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: Bad block table not found\r\n",__func__);
+#endif
+		Status = XST_FAILURE;
+		goto Out;
+	}
+#ifdef XNANDPSU_DEBUG
+	xil_printf("%s: Bad block table found\r\n",__func__);
+#endif
+	/* Bad Block Table found */
+	if ((Desc->Valid != 0U) && (MirrorDesc->Valid != 0U)) {
+		/* Valid BBT & Mirror BBT found */
+		if (Desc->Version[Target] > MirrorDesc->Version[Target]) {
+			Offset = (u64)Desc->PageOffset[Target] *
+				(u64)InstancePtr->Geometry.BytesPerPage;
+			Status = XNandPsu_Read(InstancePtr, Offset, BufLen,
+								&Buf[0]);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			/* Convert flash BBT to memory based BBT */
+			XNandPsu_ConvertBbt(InstancePtr, &Buf[0], Target);
+			MirrorDesc->Version[Target] = Desc->Version[Target];
+
+			/* Write the BBT to Mirror BBT location in flash */
+			Status = XNandPsu_WriteBbt(InstancePtr, MirrorDesc,
+							Desc, Target);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+		} else if (Desc->Version[Target] <
+						MirrorDesc->Version[Target]) {
+			Offset = (u64)MirrorDesc->PageOffset[Target] *
+				(u64)InstancePtr->Geometry.BytesPerPage;
+			Status = XNandPsu_Read(InstancePtr, Offset, BufLen,
+								&Buf[0]);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			/* Convert flash BBT to memory based BBT */
+			XNandPsu_ConvertBbt(InstancePtr, &Buf[0], Target);
+			Desc->Version[Target] = MirrorDesc->Version[Target];
+
+			/* Write the Mirror BBT to BBT location in flash */
+			Status = XNandPsu_WriteBbt(InstancePtr, Desc,
+							MirrorDesc, Target);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+		} else {
+			/* Both are up-to-date */
+			Offset = (u64)Desc->PageOffset[Target] *
+				(u64)InstancePtr->Geometry.BytesPerPage;
+			Status = XNandPsu_Read(InstancePtr, Offset, BufLen,
+								&Buf[0]);
+			if (Status != XST_SUCCESS) {
+				goto Out;
+			}
+			/* Convert flash BBT to memory based BBT */
+			XNandPsu_ConvertBbt(InstancePtr, &Buf[0], Target);
+		}
+	} else if (Desc->Valid != 0U) {
+		/* Valid Primary BBT found */
+		Offset = (u64)Desc->PageOffset[Target] *
+			(u64)InstancePtr->Geometry.BytesPerPage;
+		Status = XNandPsu_Read(InstancePtr, Offset, BufLen, &Buf[0]);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+		/* Convert flash BBT to memory based BBT */
+		XNandPsu_ConvertBbt(InstancePtr, &Buf[0], Target);
+		MirrorDesc->Version[Target] = Desc->Version[Target];
+
+		/* Write the BBT to Mirror BBT location in flash */
+		Status = XNandPsu_WriteBbt(InstancePtr, MirrorDesc, Desc,
+								Target);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+	} else {
+		/* Valid Mirror BBT found */
+		Offset = (u64)MirrorDesc->PageOffset[Target] *
+			(u64)InstancePtr->Geometry.BytesPerPage;
+		Status = XNandPsu_Read(InstancePtr, Offset, BufLen, &Buf[0]);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+		/* Convert flash BBT to memory based BBT */
+		XNandPsu_ConvertBbt(InstancePtr, &Buf[0], Target);
+		Desc->Version[Target] = MirrorDesc->Version[Target];
+
+		/* Write the Mirror BBT to BBT location in flash */
+		Status = XNandPsu_WriteBbt(InstancePtr, Desc, MirrorDesc,
+								Target);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+* This function searches the BBT in flash.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @param	Desc is the BBT descriptor pattern to search.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+static s32 XNandPsu_SearchBbt(XNandPsu *InstancePtr, XNandPsu_BbtDesc *Desc,
+								u32 Target)
+{
+	u32 StartBlock;
+	u32 SigOffset;
+	u32 VerOffset;
+	u32 MaxBlocks;
+	u32 PageOff;
+	u32 SigLength;
+#ifdef __ICCARM__
+#pragma pack(push, 1)
+	u8 Buf[XNANDPSU_MAX_SPARE_SIZE] = {0U};
+#pragma pack(pop)
+#else
+	u8 Buf[XNANDPSU_MAX_SPARE_SIZE] __attribute__ ((aligned(64))) = {0U};
+#endif
+	u32 Block;
+	u32 Offset;
+	s32 Status;
+
+	StartBlock = ((Target + (u32)1) *
+				InstancePtr->Geometry.NumTargetBlocks) - (u32)1;
+	SigOffset = Desc->SigOffset;
+	VerOffset = Desc->VerOffset;
+	MaxBlocks = Desc->MaxBlocks;
+	SigLength = Desc->SigLength;
+
+	/* Read the last 4 blocks for Bad Block Table(BBT) signature */
+	for(Block = 0U; Block < MaxBlocks; Block++) {
+		PageOff = (StartBlock - Block) *
+			InstancePtr->Geometry.PagesPerBlock;
+
+			Status = XNandPsu_ReadSpareBytes(InstancePtr, PageOff, &Buf[0]);
+		if (Status != XST_SUCCESS) {
+			continue;
+		}
+		/* Check the Bad Block Table(BBT) signature */
+		for(Offset = 0U; Offset < SigLength; Offset++) {
+			if (Buf[Offset + SigOffset] !=
+				(u8)(Desc->Signature[Offset]))
+			{
+				break; /* Check the next blocks */
+			}
+		}
+		if (Offset >= SigLength) {
+			/* Bad Block Table(BBT) found */
+			Desc->PageOffset[Target] = PageOff;
+			Desc->Version[Target] = Buf[VerOffset];
+			Desc->Valid = 1U;
+
+			Status = XST_SUCCESS;
+			goto Out;
+		}
+	}
+	/* Bad Block Table(BBT) not found */
+	Status = XST_FAILURE;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+* This function writes Bad Block Table(BBT) from RAM to flash.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @param	Desc is the BBT descriptor to be written to flash.
+* @param	MirrorDesc is the mirror BBT descriptor.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+static s32 XNandPsu_WriteBbt(XNandPsu *InstancePtr, XNandPsu_BbtDesc *Desc,
+				XNandPsu_BbtDesc *MirrorDesc, u32 Target)
+{
+	u64 Offset;
+	u32 Block = {0U};
+	u32 EndBlock = ((Target + (u32)1) *
+			InstancePtr->Geometry.NumTargetBlocks) - (u32)1;
+#ifdef __ICCARM__
+#pragma pack(push, 1)
+	u8 Buf[XNANDPSU_BBT_BUF_LENGTH]= {0U};
+	u8 SpareBuf[XNANDPSU_MAX_SPARE_SIZE]= {0U};
+#pragma pack(pop)
+#else
+	u8 Buf[XNANDPSU_BBT_BUF_LENGTH] __attribute__ ((aligned(64))) = {0U};
+	u8 SpareBuf[XNANDPSU_MAX_SPARE_SIZE] __attribute__ ((aligned(64))) = {0U};
+#endif
+
+	u8 Mask[4] = {0x00U, 0x01U, 0x02U, 0x03U};
+	u8 Data;
+	u32 BlockOffset;
+	u8 BlockShift;
+	s32 Status;
+	u32 BlockIndex;
+	u32 Index;
+	u8 BlockType;
+	u32 BbtLen = InstancePtr->Geometry.NumBlocks >>
+						XNANDPSU_BBT_BLOCK_SHIFT;
+	/* Find a valid block to write the Bad Block Table(BBT) */
+	if ((!Desc->Valid) != 0U) {
+		for(Index = 0U; Index < Desc->MaxBlocks; Index++) {
+			Block  = (EndBlock - Index);
+			BlockOffset = Block >> XNANDPSU_BBT_BLOCK_SHIFT;
+			BlockShift = XNandPsu_BbtBlockShift(Block);
+			BlockType = (InstancePtr->Bbt[BlockOffset] >>
+					BlockShift) & XNANDPSU_BLOCK_TYPE_MASK;
+			switch(BlockType)
+			{
+				case XNANDPSU_BLOCK_BAD:
+				case XNANDPSU_BLOCK_FACTORY_BAD:
+					continue;
+				default:
+					/* Good Block */
+					break;
+			}
+			Desc->PageOffset[Target] = Block *
+				InstancePtr->Geometry.PagesPerBlock;
+			if (Desc->PageOffset[Target] !=
+					MirrorDesc->PageOffset[Target]) {
+				/* Free block found */
+				Desc->Valid = 1U;
+				break;
+			}
+		}
+
+
+		/* Block not found for writing Bad Block Table(BBT) */
+		if (Index >= Desc->MaxBlocks) {
+#ifdef XNANDPSU_DEBUG
+		xil_printf("%s: Blocks unavailable for writing BBT\r\n",
+								__func__);
+#endif
+			Status = XST_FAILURE;
+			goto Out;
+		}
+	} else {
+		Block = Desc->PageOffset[Target] /
+					InstancePtr->Geometry.PagesPerBlock;
+	}
+	/* Convert the memory based BBT to flash based table */
+	(void)memset(Buf, 0xff, BbtLen);
+
+	/* Loop through the number of blocks */
+	for(BlockOffset = 0U; BlockOffset < BbtLen; BlockOffset++) {
+		Data = InstancePtr->Bbt[BlockOffset];
+		/* Calculate the bit mask for 4 blocks at a time in loop */
+		for(BlockIndex = 0U; BlockIndex < XNANDPSU_BBT_ENTRY_NUM_BLOCKS;
+				BlockIndex++) {
+			BlockShift = XNandPsu_BbtBlockShift(BlockIndex);
+			Buf[BlockOffset] &= ~(Mask[Data &
+					XNANDPSU_BLOCK_TYPE_MASK] <<
+					BlockShift);
+			Data >>= XNANDPSU_BBT_BLOCK_SHIFT;
+		}
+	}
+	/* Write the Bad Block Table(BBT) to flash */
+	Status = XNandPsu_EraseBlock(InstancePtr, 0U, Block);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	/* Write the BBT to page offset */
+	Offset = (u64)Desc->PageOffset[Target] *
+			(u64)InstancePtr->Geometry.BytesPerPage;
+	Status = XNandPsu_Write(InstancePtr, Offset, BbtLen, &Buf[0]);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+	/* Write the signature and version in the spare data area */
+	(void)memset(SpareBuf, 0xff, InstancePtr->Geometry.SpareBytesPerPage);
+	Status = XNandPsu_ReadSpareBytes(InstancePtr, Desc->PageOffset[Target],
+					 &SpareBuf[0]);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	(void)Xil_MemCpy(SpareBuf + Desc->SigOffset, &Desc->Signature[0],
+							Desc->SigLength);
+	(void)memcpy(SpareBuf + Desc->VerOffset, &Desc->Version[Target], 1U);
+
+	Status = XNandPsu_WriteSpareBytes(InstancePtr,
+				Desc->PageOffset[Target], &SpareBuf[0]);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+* This function updates the primary and mirror Bad Block Table(BBT) in the
+* flash.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+static s32 XNandPsu_UpdateBbt(XNandPsu *InstancePtr, u32 Target)
+{
+	s32 Status;
+	u8 Version;
+
+	/* Update the version number */
+	Version = InstancePtr->BbtDesc.Version[Target];
+	InstancePtr->BbtDesc.Version[Target] = (u8)(((u16)Version +
+							(u16)1) % (u16)256U);
+
+	Version = InstancePtr->BbtMirrorDesc.Version[Target];
+	InstancePtr->BbtMirrorDesc.Version[Target] = (u8)(((u16)Version +
+							(u16)1) % (u16)256);
+	/* Update the primary Bad Block Table(BBT) in flash */
+	Status = XNandPsu_WriteBbt(InstancePtr, &InstancePtr->BbtDesc,
+						&InstancePtr->BbtMirrorDesc,
+						Target);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	/* Update the mirrored Bad Block Table(BBT) in flash */
+	Status = XNandPsu_WriteBbt(InstancePtr, &InstancePtr->BbtMirrorDesc,
+						&InstancePtr->BbtDesc,
+						Target);
+	if (Status != XST_SUCCESS) {
+		goto Out;
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+* This function marks the block containing Bad Block Table as reserved
+* and updates the BBT.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @param	Desc is the BBT descriptor pointer.
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+static s32 XNandPsu_MarkBbt(XNandPsu* InstancePtr, XNandPsu_BbtDesc *Desc,
+								u32 Target)
+{
+	u32 BlockIndex;
+	u32 BlockOffset;
+	u8 BlockShift;
+	u8 OldVal;
+	u8 NewVal;
+	s32 Status;
+	u32 UpdateBbt = 0U;
+	u32 Index;
+
+	/* Mark the last four blocks as Reserved */
+	BlockIndex = ((Target + (u32)1) * InstancePtr->Geometry.NumTargetBlocks) -
+						Desc->MaxBlocks - (u32)1;
+
+	for(Index = 0U; Index < Desc->MaxBlocks; Index++) {
+
+		BlockOffset = BlockIndex >> XNANDPSU_BBT_BLOCK_SHIFT;
+		BlockShift = XNandPsu_BbtBlockShift(BlockIndex);
+		OldVal = InstancePtr->Bbt[BlockOffset];
+		NewVal = (u8) (OldVal | (XNANDPSU_BLOCK_RESERVED <<
+							BlockShift));
+		InstancePtr->Bbt[BlockOffset] = NewVal;
+
+		if (OldVal != NewVal) {
+			UpdateBbt = 1U;
+		}
+		BlockIndex++;
+	}
+
+	/* Update the BBT to flash */
+	if (UpdateBbt != 0U) {
+		Status = XNandPsu_UpdateBbt(InstancePtr, Target);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+*
+* This function checks whether a block is bad or not.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+*
+* @param	Block is the block number.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+s32 XNandPsu_IsBlockBad(XNandPsu *InstancePtr, u32 Block)
+{
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY)
+	Xil_AssertNonvoid(Block < InstancePtr->Geometry.NumBlocks);
+
+	u8 Data;
+	u8 BlockShift;
+	u8 BlockType;
+	u32 BlockOffset;
+	s32 Status;
+
+	BlockOffset = Block >> XNANDPSU_BBT_BLOCK_SHIFT;
+	BlockShift = XNandPsu_BbtBlockShift(Block);
+	Data = InstancePtr->Bbt[BlockOffset];	/* Block information in BBT */
+	BlockType = (Data >> BlockShift) & XNANDPSU_BLOCK_TYPE_MASK;
+
+	if ((BlockType != XNANDPSU_BLOCK_GOOD) &&
+		(BlockType != XNANDPSU_BLOCK_RESERVED)) {
+		Status = XST_SUCCESS;
+	}
+	else {
+		Status = XST_FAILURE;
+	}
+	return Status;
+}
+
+/*****************************************************************************/
+/**
+* This function marks a block as bad in the RAM based Bad Block Table(BBT). It
+* also updates the Bad Block Table(BBT) in the flash.
+*
+* @param	InstancePtr is the pointer to the XNandPsu instance.
+* @param	Block is the block number.
+*
+* @return
+*		- XST_SUCCESS if successful.
+*		- XST_FAILURE if fail.
+*
+******************************************************************************/
+s32 XNandPsu_MarkBlockBad(XNandPsu *InstancePtr, u32 Block)
+{
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY)
+	Xil_AssertNonvoid(Block < InstancePtr->Geometry.NumBlocks);
+
+	u8 Data;
+	u8 BlockShift;
+	u32 BlockOffset;
+	u8 OldVal;
+	u8 NewVal;
+	s32 Status;
+	u32 Target;
+
+	Target = Block / InstancePtr->Geometry.NumTargetBlocks;
+
+	BlockOffset = Block >> XNANDPSU_BBT_BLOCK_SHIFT;
+	BlockShift = XNandPsu_BbtBlockShift(Block);
+	Data = InstancePtr->Bbt[BlockOffset];	/* Block information in BBT */
+
+	/* Mark the block as bad in the RAM based Bad Block Table */
+	OldVal = Data;
+	Data &= ~(XNANDPSU_BLOCK_TYPE_MASK << BlockShift);
+	Data |= (XNANDPSU_BLOCK_BAD << BlockShift);
+	NewVal = Data;
+	InstancePtr->Bbt[BlockOffset] = Data;
+
+	/* Update the Bad Block Table(BBT) in flash */
+	if (OldVal != NewVal) {
+		Status = XNandPsu_UpdateBbt(InstancePtr, Target);
+		if (Status != XST_SUCCESS) {
+			goto Out;
+		}
+	}
+
+	Status = XST_SUCCESS;
+Out:
+	return Status;
+}
+/** @} */
diff --git a/bsps/shared/dev/nand/xnandpsu_g.c b/bsps/shared/dev/nand/xnandpsu_g.c
new file mode 100644
index 0000000000..2fb2a2a26e
--- /dev/null
+++ b/bsps/shared/dev/nand/xnandpsu_g.c
@@ -0,0 +1,47 @@
+/******************************************************************************
+* Copyright (C) 2015 - 2022 Xilinx, Inc.  All rights reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xnandpsu_g.c
+* @addtogroup nandpsu_v1_10
+* @{
+*
+* This file contains a configuration table where each entry is a configuration
+* structure for an XNandPsu device in the system.
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date	   Changes
+* ----- ----   ----------  -----------------------------------------------
+* 1.0   nm     05/06/2014  First release
+* 1.0   nm     06/02/2014  Changed the copyright to new copyright
+* </pre>
+*
+******************************************************************************/
+
+/***************************** Include Files ********************************/
+#include "xparameters.h"
+#include "xnandpsu.h"
+/************************** Constant Definitions ****************************/
+
+/**************************** Type Definitions ******************************/
+
+/***************** Macros (Inline Functions) Definitions ********************/
+
+/************************** Variable Definitions ****************************/
+
+/**
+ * Each XNandPsu device in the system has an entry in this table.
+ */
+XNandPsu_Config XNandPsu_ConfigTable[] = {
+	{
+		0U,
+		(u32)XPAR_XNANDPSU_0_BASEADDR
+	}
+};
+/** @} */
diff --git a/bsps/shared/dev/nand/xnandpsu_onfi.c b/bsps/shared/dev/nand/xnandpsu_onfi.c
new file mode 100644
index 0000000000..cd230f247f
--- /dev/null
+++ b/bsps/shared/dev/nand/xnandpsu_onfi.c
@@ -0,0 +1,91 @@
+/******************************************************************************
+* Copyright (C) 2015 - 2022 Xilinx, Inc.  All rights reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xnandpsu_onfi.c
+* @addtogroup nandpsu_v1_10
+* @{
+*
+* This file contains the implementation of ONFI specific functions.
+*
+* @note		None
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date	   Changes
+* ----- ----   ----------  -----------------------------------------------
+* 1.0   nm     05/06/2014  First release
+* </pre>
+*
+******************************************************************************/
+
+/***************************** Include Files *********************************/
+#include "xnandpsu_onfi.h"
+#include "xnandpsu.h"
+
+/************************** Constant Definitions *****************************/
+
+/**************************** Type Definitions *******************************/
+
+/***************** Macros (Inline Functions) Definitions *********************/
+
+/************************** Function Prototypes ******************************/
+
+/*****************************************************************************/
+/**
+*
+* This function calculates ONFI parameter page CRC.
+*
+* @param	ParamBuf is a pointer to the ONFI parameter page buffer.
+* @param	StartOff is the starting offset in buffer to calculate CRC.
+* @param	Length is the number of bytes for which CRC is calculated.
+*
+* @return
+*		CRC value.
+* @note
+*		None.
+*
+******************************************************************************/
+u32 XNandPsu_OnfiParamPageCrc(u8 *ParamBuf, u32 StartOff, u32 Length)
+{
+	const u32 CrcInit = 0x4F4EU;
+	const u32 Order = 16U;
+	const u32 Polynom = 0x8005U;
+	u32 i, j, c, Bit;
+	u32 Crc = CrcInit;
+	u32 DataIn;
+	u32 DataByteCount = 0U;
+	u32 CrcMask, CrcHighBit;
+
+	CrcMask = ((u32)(((u32)1 << (Order - (u32)1)) -(u32)1) << (u32)1) | (u32)1;
+	CrcHighBit = (u32)((u32)1 << (Order - (u32)1));
+	/*
+	 * CRC covers the data bytes between byte 0 and byte 253
+	 * (ONFI 1.0, section 5.4.1.36)
+	 */
+	for(i = StartOff; i < Length; i++) {
+		DataIn = *(ParamBuf + i);
+		c = (u32)DataIn;
+		DataByteCount++;
+		j = 0x80U;
+		while(j != 0U) {
+			Bit = Crc & CrcHighBit;
+			Crc <<= 1U;
+			if ((c & j) != 0U) {
+				Bit ^= CrcHighBit;
+			}
+			if (Bit != 0U) {
+				Crc ^= Polynom;
+			}
+			j >>= 1U;
+		}
+		Crc &= CrcMask;
+	}
+	return Crc;
+}
+/** @} */
diff --git a/bsps/shared/dev/nand/xnandpsu_sinit.c b/bsps/shared/dev/nand/xnandpsu_sinit.c
new file mode 100644
index 0000000000..dd6b14ef10
--- /dev/null
+++ b/bsps/shared/dev/nand/xnandpsu_sinit.c
@@ -0,0 +1,70 @@
+/******************************************************************************
+* Copyright (C) 2015 - 2022 Xilinx, Inc.  All rights reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xnandpsu_sinit.c
+* @addtogroup nandpsu_v1_10
+* @{
+*
+* The implementation of the XNandPsu driver's static initialization
+* functionality.
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date	   Changes
+* ----- ----   ----------  -----------------------------------------------
+* 1.0   nm     05/06/2014  First release
+* </pre>
+*
+******************************************************************************/
+
+/***************************** Include Files ********************************/
+#include "xstatus.h"
+#include "xparameters.h"
+#include "xnandpsu.h"
+/************************** Constant Definitions ****************************/
+
+/**************************** Type Definitions ******************************/
+
+/***************** Macros (Inline Functions) Definitions ********************/
+
+/************************** Variable Definitions ****************************/
+
+extern XNandPsu_Config XNandPsu_ConfigTable[];
+
+/************************** Function Prototypes *****************************/
+
+/****************************************************************************/
+/**
+*
+* Looks up the controller configuration based on the unique controller ID. A
+* table contains the configuration info for each controller in the system.
+*
+* @param	DevID is the ID of the controller to look up the
+*		configuration for.
+*
+* @return
+*		A pointer to the configuration found or NULL if the specified
+*		controller ID was not found.
+*
+******************************************************************************/
+XNandPsu_Config *XNandPsu_LookupConfig(u16 DevID)
+{
+	XNandPsu_Config *CfgPtr = NULL;
+	u32 Index;
+
+	for (Index = 0U; Index < (u32)XPAR_XNANDPSU_NUM_INSTANCES; Index++) {
+		if (XNandPsu_ConfigTable[Index].DeviceId == DevID) {
+			CfgPtr = &XNandPsu_ConfigTable[Index];
+			break;
+		}
+	}
+
+	return (XNandPsu_Config *)CfgPtr;
+}
+/** @} */