[U-Boot] [PATCH] qspi: Add code to send only aligned data to TxFIFO

[Rajat Srivastava]

Some Freescale QSPI controllers require driver to send only 16 bytes
aligned data to TxFIFO while performing flash write operation. The extra
data is not actually written on flash. The patch enables driver to send
16 bytes aligned data to TxFIFO, provided the config is enabled.

The reason behind this behaviour of controller is still not clear and
discussion with hardware team is ongoing. The patch will be updated
before sending it to upstream.

Signed-off-by: Rajat Srivastava <rajat.srivastava at nxp.com>
---
 drivers/spi/fsl_qspi.c | 29 ++++++++++++++++++++++-------
 1 file changed, 22 insertions(+), 7 deletions(-)

diff --git a/drivers/spi/fsl_qspi.c b/drivers/spi/fsl_qspi.c
index 1d26c6344b..b93e89ff32 100644
--- a/drivers/spi/fsl_qspi.c
+++ b/drivers/spi/fsl_qspi.c
@@ -676,20 +676,35 @@ static void qspi_op_write(struct fsl_qspi_priv *priv, u8 *txbuf, u32 len)
 	tx_size = (len > TX_BUFFER_SIZE) ?
 		TX_BUFFER_SIZE : len;
 
-	size = tx_size / 16;
+	size = tx_size / 4;
+	for (i = 0; i < size; i++) {
+		memcpy(&data, txbuf, 4);
+		data = qspi_endian_xchg(data);
+		qspi_write32(priv->flags, &regs->tbdr, data);
+		txbuf += 4;
+	}
+#if defined(CONFIG_FSL_SPI_ALIGNED_TXFIFO)
 	/*
-	 * There must be atleast 128bit data
-	 * available in TX FIFO for any pop operation
+	 * There must be at least 16 bytes of data
+	 * available in TX FIFO for any POP operation
 	 */
-	if (tx_size % 16)
-		size++;
-	for (i = 0; i < size * 4; i++) {
+	size = (((tx_size/16) + 1) * 4) - size;
+	for (i = 0; i < size; i++) {
+		data = 0;
 		memcpy(&data, txbuf, 4);
 		data = qspi_endian_xchg(data);
 		qspi_write32(priv->flags, &regs->tbdr, data);
 		txbuf += 4;
 	}
-
+#else
+	size = tx_size % 4;
+	if (size) {
+		data = 0;
+		memcpy(&data, txbuf, size);
+		data = qspi_endian_xchg(data);
+		qspi_write32(priv->flags, &regs->tbdr, data);
+	}
+#endif
 	qspi_write32(priv->flags, &regs->ipcr,
 		     (seqid << QSPI_IPCR_SEQID_SHIFT) | tx_size);
 	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
-- 
2.17.1