From 4b3db9b71001ea3fa59a9927b10ae0b9c223a50d Mon Sep 17 00:00:00 2001
From: P L Sai Krishna <lakshmi.sai.krishna.potthuri@xilinx.com>
Date: Fri, 13 Mar 2015 19:16:05 +0530
Subject: [PATCH] qspipsu_v1_0: Added support for unaligned data transfer in
 DMA mode.

This patch add support for unaligned data transfer support in DMA
mode.

Signed-off-by: P L Sai Krishna <lakshmis@xilinx.com>
---
 .../drivers/qspipsu/src/xqspipsu.c            | 70 +++++++++++++++++--
 .../drivers/qspipsu/src/xqspipsu.h            |  1 +
 2 files changed, 66 insertions(+), 5 deletions(-)

diff --git a/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.c b/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.c
index 1ce69cf2..0ea3454f 100644
--- a/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.c
+++ b/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.c
@@ -138,6 +138,7 @@ int XQspiPsu_CfgInitialize(XQspiPsu *InstancePtr, XQspiPsu_Config *ConfigPtr,
 	InstancePtr->ReadMode = XQSPIPSU_READMODE_DMA;
 	InstancePtr->GenFifoCS = XQSPIPSU_GENFIFO_CS_LOWER;
 	InstancePtr->GenFifoBus = XQSPIPSU_GENFIFO_BUS_LOWER;
+	InstancePtr->IsUnaligned = 0;
 
 	/* Select QSPIPSU */
 	XQspiPsu_Select(InstancePtr);
@@ -328,6 +329,7 @@ int XQspiPsu_PolledTransfer(XQspiPsu *InstancePtr, XQspiPsu_Msg *Msg,
 	/* list */
 	for (Index = 0; Index < NumMsg; Index++) {
 
+GENFIFO:
 		Status = XQspiPsu_GenFifoEntryData(InstancePtr, Msg, Index);
 		if (Status != XST_SUCCESS) {
 			return Status;
@@ -365,6 +367,21 @@ int XQspiPsu_PolledTransfer(XQspiPsu *InstancePtr, XQspiPsu_Msg *Msg,
 						XQspiPsu_ReadReg(BaseAddress,
 							XQSPIPSU_QSPIDMA_DST_I_STS_OFFSET) |
 							XQSPIPSU_QSPIDMA_DST_I_STS_DONE_MASK);
+					/* Read remaining bytes using IO mode */
+					if(InstancePtr->RxBytes % 4 != 0 ) {
+						XQspiPsu_WriteReg(BaseAddress,
+							XQSPIPSU_CFG_OFFSET,
+							(XQspiPsu_ReadReg(BaseAddress,
+							XQSPIPSU_CFG_OFFSET) &
+							~XQSPIPSU_CFG_MODE_EN_MASK));
+						InstancePtr->ReadMode = XQSPIPSU_READMODE_IO;
+						Msg[Index].ByteCount =
+							(InstancePtr->RxBytes % 4);
+						Msg[Index].RxBfrPtr += (InstancePtr->RxBytes -
+								(InstancePtr->RxBytes % 4));
+						InstancePtr->IsUnaligned = 1;
+						goto GENFIFO;
+					}
 					InstancePtr->RxBytes = 0;
 				}
 			} else if (Msg[Index].RxBfrPtr != NULL) {
@@ -386,6 +403,16 @@ int XQspiPsu_PolledTransfer(XQspiPsu *InstancePtr, XQspiPsu_Msg *Msg,
 			(InstancePtr->TxBytes != 0) ||
 			!(QspiPsuStatusReg & XQSPIPSU_ISR_TXEMPTY_MASK) ||
 			(InstancePtr->RxBytes != 0));
+
+		if(InstancePtr->IsUnaligned) {
+			InstancePtr->IsUnaligned = 0;
+			XQspiPsu_WriteReg(BaseAddress,
+				XQSPIPSU_CFG_OFFSET, (XQspiPsu_ReadReg(
+				BaseAddress,
+				XQSPIPSU_CFG_OFFSET) |
+				XQSPIPSU_CFG_MODE_EN_DMA_MASK));
+			InstancePtr->ReadMode = XQSPIPSU_READMODE_DMA;
+		}
 	}
 
 	/* De-select slave */
@@ -575,9 +602,33 @@ int XQspiPsu_InterruptHandler(XQspiPsu *InstancePtr)
 	if (InstancePtr->ReadMode == XQSPIPSU_READMODE_DMA &&
 		(MsgCnt < NumMsg) && (Msg[MsgCnt].RxBfrPtr != NULL)) {
 		if ((DmaIntrStatusReg & XQSPIPSU_QSPIDMA_DST_I_STS_DONE_MASK)) {
-		InstancePtr->RxBytes = 0;
-		MsgCnt += 1;
-		DeltaMsgCnt = 1;
+				/* Read remaining bytes using IO mode */
+			if(InstancePtr->RxBytes % 4 != 0 ) {
+				XQspiPsu_WriteReg(BaseAddress,
+					XQSPIPSU_CFG_OFFSET, (XQspiPsu_ReadReg(
+					BaseAddress, XQSPIPSU_CFG_OFFSET) &
+					~XQSPIPSU_CFG_MODE_EN_MASK));
+				InstancePtr->ReadMode = XQSPIPSU_READMODE_IO;
+				Msg[MsgCnt].ByteCount = (InstancePtr->RxBytes % 4);
+				Msg[MsgCnt].RxBfrPtr += (InstancePtr->RxBytes -
+						(InstancePtr->RxBytes % 4));
+				InstancePtr->IsUnaligned = 1;
+				XQspiPsu_GenFifoEntryData(InstancePtr, Msg,
+						MsgCnt);
+				if(IsManualStart) {
+					XQspiPsu_WriteReg(BaseAddress,
+						XQSPIPSU_CFG_OFFSET,
+						XQspiPsu_ReadReg(BaseAddress,
+						XQSPIPSU_CFG_OFFSET) |
+						XQSPIPSU_CFG_START_GEN_FIFO_MASK);
+				}
+			}
+			else {
+				InstancePtr->RxBytes = 0;
+				MsgCnt += 1;
+				DeltaMsgCnt = 1;
+			}
+		}
 	} else if ((MsgCnt < NumMsg) && (Msg[MsgCnt].RxBfrPtr != NULL)) {
 		RxThr = XQspiPsu_ReadReg(BaseAddress,
 					XQSPIPSU_RX_THRESHOLD_OFFSET);
@@ -621,6 +672,15 @@ int XQspiPsu_InterruptHandler(XQspiPsu *InstancePtr)
 	if ((QspiPsuStatusReg & XQSPIPSU_ISR_GENFIFOEMPTY_MASK) &&
 		(DeltaMsgCnt || (MsgCnt > NumMsg))) {
 		if (MsgCnt < NumMsg) {
+			if(InstancePtr->IsUnaligned) {
+				InstancePtr->IsUnaligned = 0;
+				XQspiPsu_WriteReg(InstancePtr->Config.
+					BaseAddress, XQSPIPSU_CFG_OFFSET,
+					(XQspiPsu_ReadReg(InstancePtr->Config.
+					BaseAddress, XQSPIPSU_CFG_OFFSET) |
+					XQSPIPSU_CFG_MODE_EN_DMA_MASK));
+				InstancePtr->ReadMode = XQSPIPSU_READMODE_DMA;
+			}
 			/* This might not work if not manual start */
 			XQspiPsu_GenFifoEntryData(InstancePtr, Msg, MsgCnt);
 
@@ -924,8 +984,8 @@ static inline void XQspiPsu_SetupRxDma(XQspiPsu *InstancePtr,
 	DmaRxBytes = InstancePtr->RxBytes;
 	if (Remainder != 0) {
 		/* This is done to make Dma bytes aligned */
-		DmaRxBytes = InstancePtr->RxBytes + 4 - Remainder;
-		/* Handle remaining bytes with IO or DMA to local buffer */
+		DmaRxBytes = InstancePtr->RxBytes - Remainder;
+		Msg->ByteCount = DmaRxBytes;
 	}
 
 	/* Write no. of words to DMA DST SIZE */
diff --git a/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.h b/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.h
index 2f6d9dd7..9e9c3909 100644
--- a/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.h
+++ b/XilinxProcessorIPLib/drivers/qspipsu/src/xqspipsu.h
@@ -165,6 +165,7 @@ typedef struct {
 	u32 GenFifoBus;
 	int NumMsg;
 	int MsgCnt;
+	int IsUnaligned;
 	XQspiPsu_Msg *Msg;
 	XQspiPsu_StatusHandler StatusHandler;
 	void *StatusRef;  	 /**< Callback reference for status handler */