/******************************************************************************
* Copyright (C) 2015 - 2022 Xilinx, Inc.  All rights reserved.
* SPDX-License-Identifier: MIT
******************************************************************************/

/*****************************************************************************/
/**
*
* @file xnandpsu.c
* @addtogroup Overview
* @{
*
* 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.
* 1.11  akm    03/31/22    Fix unused parameter warning.
* 1.11  akm    03/31/22    Fix misleading-indentation warning.
*
* </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);

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;

#ifdef __rtems__
	/* Set page cache to unavailable */
	InstancePtr->PartialDataPageIndex = XNANDPSU_PAGE_CACHE_UNAVAILABLE;
#endif

	/* 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;
	}

#ifdef __rtems__
	/* Set page cache to none */
	InstancePtr->PartialDataPageIndex = XNANDPSU_PAGE_CACHE_NONE;
#endif
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);
				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)
{
	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;
}

#ifdef __rtems__
#include <rtems/rtems/clock.h>
static void udelay( void )
{
	uint64_t time = rtems_clock_get_uptime_nanoseconds() + 1000;
	while (1) {
		uint64_t newtime = rtems_clock_get_uptime_nanoseconds();
		if (newtime > time) {
			break;
		}
	}
}
#define usleep(x) udelay()
#endif

/*****************************************************************************/
/**
*
* 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;
	}

#ifdef __rtems__
	if (InstancePtr->PartialDataPageIndex != XNANDPSU_PAGE_CACHE_UNAVAILABLE) {
		/* All writes invalidate the page cache */
		InstancePtr->PartialDataPageIndex = XNANDPSU_PAGE_CACHE_NONE;
	}
#endif
	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);
#ifdef __rtems__
		{
#else
		if (Page > InstancePtr->Geometry.NumTargetPages) {
#endif
			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);
#ifdef __rtems__
		{
#else
		if (Page > InstancePtr->Geometry.NumTargetPages) {
#endif
			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;
		}
#ifdef __rtems__
		if (Page == InstancePtr->PartialDataPageIndex) {
			/*
			 * This is a whole page read for the currently cached
			 * page. It will not be taken care of below, so perform
			 * the copy here.
			 */
			if (PartialBytes == 0U) {
				(void)Xil_MemCpy(DestBufPtr,
						&InstancePtr->PartialDataBuf[0],
						NumBytes);
			}
		} else {
#endif
		/* Read page */
		Status = XNandPsu_ReadPage(InstancePtr, Target, Page, 0U,
								BufPtr);
#ifdef __rtems__
			if (PartialBytes > 0U &&
				InstancePtr->PartialDataPageIndex != XNANDPSU_PAGE_CACHE_UNAVAILABLE) {
				/*
				 * Partial read into page cache. Update the
				 * cached page index.
				 */
				InstancePtr->PartialDataPageIndex = Page;
			}
		}
#endif
		if (Status != XST_SUCCESS) {
			goto Out;
		}
		if (PartialBytes > 0U) {
			(void)Xil_MemCpy(DestBufPtr, BufPtr + Col, NumBytes);
#ifdef __rtems__
			/* The destination buffer is touched by hardware, synchronize */
			if (InstancePtr->Config.IsCacheCoherent == 0) {
				Xil_DCacheFlushRange((INTPTR)(void *)DestBufPtr, NumBytes);
			}
#endif
		}
		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;
#ifdef __rtems__
		u32 ModBlock = Block % InstancePtr->Geometry.NumTargetBlocks;
#else
		Block %= InstancePtr->Geometry.NumTargetBlocks;
#endif
		/* Don't erase bad block */
		if (XNandPsu_IsBlockBad(InstancePtr, Block) ==
							XST_SUCCESS)
			continue;
		/* Block Erase */
#ifdef __rtems__
		Status = XNandPsu_EraseBlock(InstancePtr, Target, ModBlock);
#else
		Status = XNandPsu_EraseBlock(InstancePtr, Target, Block);
#endif
		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);

#ifdef __rtems__
	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
		if (InstancePtr->Config.IsCacheCoherent == 0) {
			Xil_DCacheInvalidateRange((INTPTR)(void *)Buf, (PktSize * PktCount));
		}
	}
#endif

	/* 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);

#ifdef __rtems__
	if (InstancePtr->DmaMode == XNANDPSU_MDMA) {
		if (InstancePtr->Config.IsCacheCoherent == 0) {
			Xil_DCacheInvalidateRange((INTPTR)(void *)Buf, (PktSize * PktCount));
		}
	}
#endif

	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);
#ifdef __rtems__
	Xil_AssertNonvoid(Block < InstancePtr->Geometry.NumTargetBlocks);
#else
	Xil_AssertNonvoid(Block < InstancePtr->Geometry.NumBlocks);
#endif

	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;
#ifdef __rtems__
		if (InstancePtr->Config.IsCacheCoherent == 0) {
			Xil_DCacheFlushRange((INTPTR)(void *)Buf, (PktSize * PktCount));
		}
#endif
		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;
}
/** @} */