From a67aab6cd4490479b002082cacf3222c59b22c82 Mon Sep 17 00:00:00 2001
From: Kinsey Moore <kinsey.moore@oarcorp.com>
Date: Fri, 19 May 2023 12:47:32 -0500
Subject: bsps/xqspipsu: Ensure NOR writes align

This change causes NOR writes to be broken according to page boundaries.
Writes across page boundaries cause the writes beyond the boundary to
fail silently. This also introduces a new function that will explicitly
write pages.
---
 bsps/include/dev/spi/xqspipsu-flash-helper.h | 25 ++++++++++++++-
 bsps/shared/dev/spi/xqspipsu-flash-helper.c  | 47 +++++++++++++++++++++++++++-
 2 files changed, 70 insertions(+), 2 deletions(-)

diff --git a/bsps/include/dev/spi/xqspipsu-flash-helper.h b/bsps/include/dev/spi/xqspipsu-flash-helper.h
index 22f85f156c..1e16acaf06 100644
--- a/bsps/include/dev/spi/xqspipsu-flash-helper.h
+++ b/bsps/include/dev/spi/xqspipsu-flash-helper.h
@@ -49,11 +49,34 @@ int QspiPsu_NOR_Erase(
  * @note	None.
  *
  ******************************************************************************/
+int QspiPsu_NOR_Write_Page(
+  XQspiPsu *QspiPsuPtr,
+  u32 Address,
+  u32 ByteCount,
+  u8 *WriteBfrPtr
+);
+
+/*****************************************************************************/
+/**
+ *
+ * This function writes to the serial Flash connected to the QSPIPSU interface.
+ * Writes will be broken into device page sized and aligned writes as necessary.
+ *
+ * @param	QspiPsuPtr is a pointer to the QSPIPSU driver component to use.
+ * @param	Address contains the address to write data to in the Flash.
+ * @param	ByteCount contains the number of bytes to write.
+ * @param	WriteBfrPtr is pointer to the write buffer (which is to be transmitted)
+ *
+ * @return	XST_SUCCESS if successful, else XST_FAILURE.
+ *
+ * @note	None.
+ *
+ ******************************************************************************/
 int QspiPsu_NOR_Write(
   XQspiPsu *QspiPsuPtr,
   u32 Address,
   u32 ByteCount,
-	u8 *WriteBfrPtr
+  u8 *WriteBfrPtr
 );
 
 /*****************************************************************************/
diff --git a/bsps/shared/dev/spi/xqspipsu-flash-helper.c b/bsps/shared/dev/spi/xqspipsu-flash-helper.c
index 43dc700507..0c1a9ffd2a 100644
--- a/bsps/shared/dev/spi/xqspipsu-flash-helper.c
+++ b/bsps/shared/dev/spi/xqspipsu-flash-helper.c
@@ -333,7 +333,7 @@ static int FlashReadID(XQspiPsu *QspiPsuPtr)
   return XST_SUCCESS;
 }
 
-int QspiPsu_NOR_Write(
+int QspiPsu_NOR_Write_Page(
   XQspiPsu *QspiPsuPtr,
   u32 Address,
   u32 ByteCount,
@@ -475,6 +475,51 @@ int QspiPsu_NOR_Write(
   return 0;
 }
 
+int QspiPsu_NOR_Write(
+  XQspiPsu *QspiPsuPtr,
+  u32 Address,
+  u32 ByteCount,
+  u8 *WriteBfrPtr
+)
+{
+  int Status;
+  size_t ByteCountRemaining = ByteCount;
+  unsigned char *WriteBfrPartial = WriteBfrPtr;
+  uint32_t AddressPartial = Address;
+  uint32_t PageSize = Flash_Config_Table[FCTIndex].PageSize;
+  if(QspiPsuPtr->Config.ConnectionMode == XQSPIPSU_CONNECTION_MODE_PARALLEL) {
+    PageSize *= 2;
+  }
+
+  while (ByteCountRemaining > 0) {
+    /* Get write boundary */
+    size_t WriteChunkLen = RTEMS_ALIGN_UP(AddressPartial + 1, PageSize);
+
+    /* Get offset to write boundary */
+    WriteChunkLen -= (size_t)AddressPartial;
+
+    /* Cap short writes */
+    if (WriteChunkLen > ByteCountRemaining) {
+      WriteChunkLen = ByteCountRemaining;
+    }
+
+    Status = QspiPsu_NOR_Write_Page(
+      QspiPsuPtr,
+      AddressPartial,
+      WriteChunkLen,
+      WriteBfrPartial
+    );
+    if ( Status != XST_SUCCESS ) {
+      return Status;
+    }
+
+    ByteCountRemaining -= WriteChunkLen;
+    AddressPartial += WriteChunkLen;
+    WriteBfrPartial += WriteChunkLen;
+  }
+  return Status;
+}
+
 int QspiPsu_NOR_Erase(
   XQspiPsu *QspiPsuPtr,
   u32 Address,
-- 
cgit v1.2.3