1 files changed, 145 insertions, 32 deletions

diff --git a/bsps/shared/dev/spi/xqspipsu-flash-helper.c b/bsps/shared/dev/spi/xqspipsu-flash-helper.c
index 43dc700507..10e1066173 100644
--- a/bsps/shared/dev/spi/xqspipsu-flash-helper.c
+++ b/bsps/shared/dev/spi/xqspipsu-flash-helper.c
@@ -274,22 +274,57 @@ static void QspiPsuHandler(
   }
 }
 
-/*****************************************************************************/
-/**
- *
- * Reads the flash ID and identifies the flash in FCT table.
- *
- * @param	QspiPsuPtr is a pointer to the QSPIPSU driver component to use.
- *
- * @return	XST_SUCCESS if successful, else XST_FAILURE.
- *
- * @note	None.
- *
- *****************************************************************************/
-static int FlashReadID(XQspiPsu *QspiPsuPtr)
+int QspiPsu_NOR_RDSFDP(
+  XQspiPsu *QspiPsuPtr,
+  u32 Address,
+  u32 ByteCount,
+  u8 **ReadBfrPtr
+)
+{
+  int Status;
+
+  *ReadBfrPtr = ReadBuffer;
+
+  CmdBfr[COMMAND_OFFSET]   = READ_SFDP;
+  CmdBfr[ADDRESS_1_OFFSET] =
+      (u8)((Address & 0xFF0000) >> 16);
+  CmdBfr[ADDRESS_2_OFFSET] =
+      (u8)((Address & 0xFF00) >> 8);
+  CmdBfr[ADDRESS_3_OFFSET] =
+      (u8)(Address & 0xFF);
+
+  FlashMsg[0].BusWidth = XQSPIPSU_SELECT_MODE_SPI;
+  FlashMsg[0].TxBfrPtr = CmdBfr;
+  FlashMsg[0].RxBfrPtr = NULL;
+  FlashMsg[0].ByteCount = 4;
+  FlashMsg[0].Flags = XQSPIPSU_MSG_FLAG_TX;
+
+  FlashMsg[1].BusWidth = XQSPIPSU_SELECT_MODE_SPI;
+  FlashMsg[1].TxBfrPtr = NULL;
+  FlashMsg[1].RxBfrPtr = NULL;
+  FlashMsg[1].ByteCount = DUMMY_CLOCKS;
+  FlashMsg[1].Flags = 0;
+
+  FlashMsg[2].BusWidth = XQSPIPSU_SELECT_MODE_SPI;
+  FlashMsg[2].TxBfrPtr = NULL;
+  FlashMsg[2].RxBfrPtr = *ReadBfrPtr;
+  FlashMsg[2].ByteCount = ByteCount;
+  FlashMsg[2].Flags = XQSPIPSU_MSG_FLAG_RX;
+
+  TransferInProgress = TRUE;
+  Status = XQspiPsu_InterruptTransfer(QspiPsuPtr, FlashMsg, 3);
+  if (Status != XST_SUCCESS)
+    return XST_FAILURE;
+
+  while (TransferInProgress);
+
+  rtems_cache_invalidate_multiple_data_lines(ReadBuffer, ByteCount);
+  return 0;
+}
+
+int QspiPsu_NOR_RDID(XQspiPsu *QspiPsuPtr, u8 *ReadBfrPtr, u32 ReadLen)
 {
   int Status;
-  u32 ReadId = 0;
 
   /*
    * Read ID
@@ -303,7 +338,7 @@ static int FlashReadID(XQspiPsu *QspiPsuPtr)
 
   FlashMsg[1].TxBfrPtr = NULL;
   FlashMsg[1].RxBfrPtr = ReadBfrPtr;
-  FlashMsg[1].ByteCount = 3;
+  FlashMsg[1].ByteCount = ReadLen;
   FlashMsg[1].BusWidth = XQSPIPSU_SELECT_MODE_SPI;
   FlashMsg[1].Flags = XQSPIPSU_MSG_FLAG_RX;
 
@@ -314,6 +349,33 @@ static int FlashReadID(XQspiPsu *QspiPsuPtr)
   }
   while (TransferInProgress);
 
+  rtems_cache_invalidate_multiple_data_lines(ReadBfrPtr, ReadLen);
+  return XST_SUCCESS;
+}
+
+/*****************************************************************************/
+/**
+ *
+ * Reads the flash ID and identifies the flash in FCT table.
+ *
+ * @param	QspiPsuPtr is a pointer to the QSPIPSU driver component to use.
+ *
+ * @return	XST_SUCCESS if successful, else XST_FAILURE.
+ *
+ * @note	None.
+ *
+ *****************************************************************************/
+static int FlashReadID(XQspiPsu *QspiPsuPtr)
+{
+  u32 ReadId = 0;
+  u32 ReadLen = 3;
+  int Status;
+
+  Status = QspiPsu_NOR_RDID(QspiPsuPtr, ReadBfrPtr, ReadLen);
+  if (Status != XST_SUCCESS) {
+    return XST_FAILURE;
+  }
+
   /* In case of dual, read both and ensure they are same make/size */
 
   /*
@@ -333,7 +395,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 +537,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,
@@ -489,22 +596,18 @@ int QspiPsu_NOR_Erase(
   u32 NumSect;
   int Status;
   u32 SectSize;
-  u32 SectMask;
 
   WriteEnableCmd = WRITE_ENABLE_CMD;
 
   if(QspiPsuPtr->Config.ConnectionMode == XQSPIPSU_CONNECTION_MODE_PARALLEL) {
-    SectMask = (Flash_Config_Table[FCTIndex]).SectMask - (Flash_Config_Table[FCTIndex]).SectSize;
     SectSize = (Flash_Config_Table[FCTIndex]).SectSize * 2;
     NumSect = (Flash_Config_Table[FCTIndex]).NumSect;
   } else if (QspiPsuPtr->Config.ConnectionMode == XQSPIPSU_CONNECTION_MODE_STACKED) {
     NumSect = (Flash_Config_Table[FCTIndex]).NumSect * 2;
-    SectMask = (Flash_Config_Table[FCTIndex]).SectMask;
     SectSize = (Flash_Config_Table[FCTIndex]).SectSize;
   } else {
     SectSize = (Flash_Config_Table[FCTIndex]).SectSize;
     NumSect = (Flash_Config_Table[FCTIndex]).NumSect;
-    SectMask = (Flash_Config_Table[FCTIndex]).SectMask;
   }
 
   /*
@@ -569,18 +672,9 @@ int QspiPsu_NOR_Erase(
   /*
    * Calculate no. of sectors to erase based on byte count
    */
-  NumSect = (ByteCount / SectSize) + 1;
-
-  /*
-   * If ByteCount to k sectors,
-   * but the address range spans from N to N+k+1 sectors, then
-   * increment no. of sectors to be erased
-   */
-
-  if (((Address + ByteCount) & SectMask) ==
-    ((Address + (NumSect * SectSize)) & SectMask)) {
-    NumSect++;
-  }
+  u32 SectorStartBase = RTEMS_ALIGN_DOWN(Address, SectSize);
+  u32 SectorEndTop = RTEMS_ALIGN_UP(Address + ByteCount, SectSize);
+  NumSect = (SectorEndTop - SectorStartBase)/SectSize;
 
   for (Sector = 0; Sector < NumSect; Sector++) {
 
@@ -828,6 +922,7 @@ int QspiPsu_NOR_Read(
     while (TransferInProgress);
 
   }
+  rtems_cache_invalidate_multiple_data_lines(ReadBuffer, ByteCount);
   return 0;
 }
 
@@ -1015,6 +1110,7 @@ static int MultiDieRead(
     Address += data_len;
     remain_len -= data_len;
   }
+  rtems_cache_invalidate_multiple_data_lines(ReadBfrPtr, ByteCount);
   return 0;
 }
 
@@ -2226,3 +2322,20 @@ static int MultiDieReadEcc(
   }
   return 0;
 }
+
+u32 QspiPsu_NOR_Get_Sector_Size(XQspiPsu *QspiPsuPtr)
+{
+  if(QspiPsuPtr->Config.ConnectionMode == XQSPIPSU_CONNECTION_MODE_PARALLEL) {
+    return Flash_Config_Table[FCTIndex].SectSize * 2;
+  }
+  return Flash_Config_Table[FCTIndex].SectSize;
+}
+
+u32 QspiPsu_NOR_Get_Device_Size(XQspiPsu *QspiPsuPtr)
+{
+  if(QspiPsuPtr->Config.ConnectionMode == XQSPIPSU_CONNECTION_MODE_STACKED
+     || QspiPsuPtr->Config.ConnectionMode == XQSPIPSU_CONNECTION_MODE_PARALLEL) {
+    return Flash_Config_Table[FCTIndex].FlashDeviceSize * 2;
+  }
+  return Flash_Config_Table[FCTIndex].FlashDeviceSize;
+}