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,