[cru] Make sure the superpage size FIFO is parsed correctly

Author: kostorr

src/Cru/CruBar.cxx

@@ -133,7 +133,19 @@ uint32_t CruBar::getSuperpageSize(uint32_t link)
   writeRegister(Cru::Registers::LINK_SUPERPAGE_SIZE.get(link).index, 0xbadcafe); // write a dummy value to update the FIFO
   uint32_t superpageSizeFifo = readRegister(Cru::Registers::LINK_SUPERPAGE_SIZE.get(link).index);
   uint32_t superpageSize = Utilities::getBits(superpageSizeFifo, 0, 23); // [0-23] -> superpage size (in bytes)
-  //uint32_t superpageIndex = Utilities::getBits(superpageSizeFifo, 24, 31); // [24-31] -> superpage index (0-255) _for testing_
+  if (superpageSize == 0) {                                              // No reason to check for index -> superpageSize == 0 -> CRU FW < v3.4.0
+    return 0;
+  }
+  uint32_t superpageSizeIndex = Utilities::getBits(superpageSizeFifo, 24, 31); // [24-31] -> superpage index (0-255)
+
+  while (superpageSizeIndex != mSuperpageSizeIndexCounter[link]) { // In case the PCIe bus wasn't fast enough
+    //std::cout << "[SP INDEX] link " << link << " : " << superpageSizeIndex << " instead of " << mSuperpageSizeIndexCounter[link] << std::endl;
+    superpageSizeFifo = readRegister(Cru::Registers::LINK_SUPERPAGE_SIZE.get(link).index);
+    superpageSize = Utilities::getBits(superpageSizeFifo, 0, 23);
+    superpageSizeIndex = Utilities::getBits(superpageSizeFifo, 24, 31);
+  }
+
+  mSuperpageSizeIndexCounter[link] = (superpageSizeIndex + 1) % 256;
 
   return superpageSize;
 }

src/Cru/CruBar.h

@@ -132,6 +132,9 @@ class CruBar final : public BarInterfaceBase
   uint32_t mOnuAddress;
   bool mDynamicOffset;
 
+  /// Per-link counter to verify superpage sizes received are valid
+  uint32_t mSuperpageSizeIndexCounter[Cru::MAX_LINKS] = { 0 };
+
   /// Checks if this is the correct BAR. Used to check for BAR 2 for special functions.
   void assertBarIndex(int index, std::string message) const
   {

src/Cru/CruDmaChannel.cxx

@@ -173,8 +173,13 @@ void CruDmaChannel::deviceStopDma()
       if (mReadyQueue.size() >= READY_QUEUE_CAPACITY) {
         break;
       }
-      if (!link.queue.empty()) { // care for the extra filled superpage
-        transferSuperpageFromLinkToReady(link, true);
+
+      if (!link.queue.empty()) {    // care for the extra filled superpage
+        if (i == amountAvailable) { // Propagate that it is the last popped to set the size only to the RDH (eox)
+          transferSuperpageFromLinkToReady(link, true);
+        } else {
+          transferSuperpageFromLinkToReady(link);
+        }
         moved++;
       }
     }
@@ -278,14 +283,18 @@ void CruDmaChannel::transferSuperpageFromLinkToReady(Link& link, bool isPopped)
   }
 
   link.queue.front().setReady(true);
-  uint32_t superpageSize = getBar()->getSuperpageSize(link.id);
-  if (isPopped) { // Only RDH in case it is popped
-    link.queue.front().setReceived(0x40);
-  } else if (superpageSize == 0) {                                //backwards compatible in case the superpage size register is empty
-    link.queue.front().setReceived(link.queue.front().getSize()); // force the full superpage size
+
+  if (isPopped) {
+    link.queue.front().setReceived(0x40); // Only RDH in case it's popped
   } else {
-    link.queue.front().setReceived(superpageSize);
+    uint32_t superpageSize = getBar()->getSuperpageSize(link.id);
+    if (superpageSize == 0) {
+      link.queue.front().setReceived(link.queue.front().getSize()); // force the full superpage size for backwards compatibility
+    } else {
+      link.queue.front().setReceived(superpageSize);
+    }
   }
+
   mReadyQueue.push_back(link.queue.front());
   link.queue.pop_front();
   link.superpageCounter++;