Add the re-order processing right after receiving the PAF packets

Cancel the publish if the previous fabric information has existed

Signed-off-by: Lo,Chin-Ran <[email protected]>

examples/platform/linux/AppMain.cpp

@@ -540,15 +540,16 @@ int ChipLinuxAppInit(int argc, char * const argv[], OptionSet * customOptions,
 #if CHIP_DEVICE_CONFIG_ENABLE_WPA && CHIP_DEVICE_CONFIG_ENABLE_WIFIPAF
     if (LinuxDeviceOptions::GetInstance().mWiFi && LinuxDeviceOptions::GetInstance().mWiFiPAF)
     {
-        ChipLogProgress(NotSpecified, "WiFi-PAF: initialzing");
+        ChipLogProgress(WiFiPAF, "WiFi-PAF: initialzing");
         if (EnsureWiFiIsStarted())
         {
-            ChipLogProgress(NotSpecified, "Wi-Fi Management started");
+            ChipLogProgress(WiFiPAF, "Wi-Fi Management started");
             DeviceLayer::ConnectivityManager::WiFiPAFAdvertiseParam args;
 
             args.enable        = LinuxDeviceOptions::GetInstance().mWiFiPAF;
             args.freq_list_len = WiFiPAFGet_FreqList(LinuxDeviceOptions::GetInstance().mWiFiPAFExtCmds, args.freq_list);
             DeviceLayer::ConnectivityMgr().WiFiPAFPublish(args);
+            LinuxDeviceOptions::GetInstance().mPublishId = args.publish_id;
         }
     }
 #endif
@@ -709,6 +710,14 @@ void ChipLinuxAppMainLoop(AppMainLoopImplementation * impl)
         exampleAccessRestrictionProvider->SetEntries(1, LinuxDeviceOptions::GetInstance().arlEntries.Value());
     }
 #endif
+#if CHIP_DEVICE_CONFIG_ENABLE_WIFIPAF
+    if (Server::GetInstance().GetFabricTable().FabricCount() != 0)
+    {
+        ChipLogProgress(AppServer, "Fabric already commissioned. Canceling publishing");
+        DeviceLayer::ConnectivityMgr().WiFiPAFShutdown(LinuxDeviceOptions::GetInstance().mPublishId,
+                                                       chip::WiFiPAF::WiFiPafRole::kWiFiPafRole_Publisher);
+    }
+#endif
 
 #if CONFIG_BUILD_FOR_HOST_UNIT_TEST
     // Set ReadHandler Capacity for Subscriptions

examples/platform/linux/Options.h

@@ -59,6 +59,7 @@ struct LinuxDeviceOptions
 #if CHIP_DEVICE_CONFIG_ENABLE_WIFIPAF
     bool mWiFiPAF                = false;
     const char * mWiFiPAFExtCmds = nullptr;
+    uint32_t mPublishId          = 0;
 #endif
 #if CHIP_DEVICE_CONFIG_ENABLE_BOTH_COMMISSIONER_AND_COMMISSIONEE || CHIP_DEVICE_ENABLE_PORT_PARAMS
     uint16_t securedDevicePort         = CHIP_PORT;

src/include/platform/CHIPDeviceEvent.h

@@ -501,6 +501,7 @@ struct ChipDeviceEvent final
         {
             chip::System::PacketBuffer * Data;
             chip::WiFiPAF::WiFiPAFSession SessionInfo;
+            bool result;
         } CHIPoWiFiPAFReceived;
 #endif
         struct

src/include/platform/ConnectivityManager.h

@@ -299,6 +299,7 @@ struct ConnectivityManager::WiFiPAFAdvertiseParam
     /* Frequency list */
     uint16_t freq_list_len;
     std::unique_ptr<uint16_t[]> freq_list;
+    uint32_t publish_id;
 };
 #endif
 

src/platform/Linux/ConnectivityManagerImpl.cpp

@@ -188,7 +188,7 @@ void ConnectivityManagerImpl::_OnPlatformEvent(const ChipDeviceEvent * event)
         ChipLogProgress(DeviceLayer, "WiFi-PAF: event: kCHIPoWiFiPAFWriteDone");
         WiFiPAF::WiFiPAFSession TxInfo;
         memcpy(&TxInfo, &event->CHIPoWiFiPAFReceived.SessionInfo, sizeof(WiFiPAF::WiFiPAFSession));
-        _GetWiFiPAF()->HandleWriteConfirmed(TxInfo);
+        _GetWiFiPAF()->HandleWriteConfirmed(TxInfo, event->CHIPoWiFiPAFReceived.result);
         break;
     }
     }
@@ -916,6 +916,7 @@ CHIP_ERROR ConnectivityManagerImpl::_WiFiPAFPublish(ConnectivityManager::WiFiPAF
 
     ChipLogProgress(DeviceLayer, "WiFi-PAF: publish_id: %u ! ", publish_id);
     _GetWiFiPAF()->AddPafSession(publish_id);
+    InArgs.publish_id = publish_id;
 
     g_signal_connect(mWpaSupplicant.iface, "nanreplied",
                      G_CALLBACK(+[](WpaFiW1Wpa_supplicant1Interface * proxy, GVariant * obj, ConnectivityManagerImpl * self) {
@@ -1781,6 +1782,7 @@ CHIP_ERROR ConnectivityManagerImpl::_WiFiPAFSend(const WiFiPAF::WiFiPAFSession &
     // Post an event to the Chip queue to deliver the data into the Chip stack.
     ChipDeviceEvent event{ .Type = DeviceEventType::kCHIPoWiFiPAFWriteDone };
     memcpy(&event.CHIPoWiFiPAFReceived.SessionInfo, &TxInfo, sizeof(chip::WiFiPAF::WiFiPAFSession));
+    event.CHIPoWiFiPAFReceived.result = result;
     PlatformMgr().PostEventOrDie(&event);
     return ret;
 }

src/wifipaf/WiFiPAFEndPoint.cpp

@@ -181,6 +181,16 @@ void WiFiPAFEndPoint::DoClose(uint8_t flags, CHIP_ERROR err)
             StopConnectTimer();
         }
 
+        // Free the packets in re-order queue if ones exist
+        for (uint8_t qidx = 0; qidx < PAFTP_REORDER_QUEUE_SIZE; qidx++)
+        {
+            if (ReorderQueue[qidx] != nullptr)
+            {
+                ReorderQueue[qidx] = nullptr;
+                ItemsInReorderQueue--;
+            }
+        }
+
         // If transmit buffer is empty or a transmission abort was specified...
         if (mPafTP.TxState() == WiFiPAFTP::kState_Idle || (flags & kWiFiPAFCloseFlag_AbortTransmission))
         {
@@ -469,7 +479,7 @@ CHIP_ERROR WiFiPAFEndPoint::HandleHandshakeConfirmationReceived()
     return CHIP_NO_ERROR;
 }
 
-CHIP_ERROR WiFiPAFEndPoint::HandleFragmentConfirmationReceived()
+CHIP_ERROR WiFiPAFEndPoint::HandleFragmentConfirmationReceived(bool result)
 {
     CHIP_ERROR err = CHIP_NO_ERROR;
 
@@ -485,6 +495,15 @@ CHIP_ERROR WiFiPAFEndPoint::HandleFragmentConfirmationReceived()
         mConnStateFlags.Clear(ConnectionStateFlag::kStandAloneAckInFlight);
     }
 
+    if (result != true)
+    {
+        // Something wrong in writing packets
+        ChipLogError(WiFiPAF, "Failed to send PAF packet");
+        err = CHIP_ERROR_SENDING_BLOCKED;
+        StopAckReceivedTimer();
+        SuccessOrExit(err);
+    }
+
     // If local receive window size has shrunk to or below immediate ack threshold, AND a message fragment is not
     // pending on which to piggyback an ack, send immediate stand-alone ack.
     //
@@ -512,7 +531,7 @@ CHIP_ERROR WiFiPAFEndPoint::HandleFragmentConfirmationReceived()
     return err;
 }
 
-CHIP_ERROR WiFiPAFEndPoint::HandleSendConfirmationReceived()
+CHIP_ERROR WiFiPAFEndPoint::HandleSendConfirmationReceived(bool result)
 {
     ChipLogDebugWiFiPAFEndPoint(WiFiPAF, "entered HandleSendConfirmationReceived");
 
@@ -526,7 +545,7 @@ CHIP_ERROR WiFiPAFEndPoint::HandleSendConfirmationReceived()
         return HandleHandshakeConfirmationReceived();
     }
 
-    return HandleFragmentConfirmationReceived();
+    return HandleFragmentConfirmationReceived(result);
 }
 
 CHIP_ERROR WiFiPAFEndPoint::DriveStandAloneAck()
@@ -792,6 +811,34 @@ SequenceNumber_t WiFiPAFEndPoint::AdjustRemoteReceiveWindow(SequenceNumber_t las
     return static_cast<uint8_t>(newRemoteWindowBoundary - newestUnackedSentSeqNum);
 }
 
+CHIP_ERROR WiFiPAFEndPoint::GetPktSn(Encoding::LittleEndian::Reader & reader, uint8_t * pHead, SequenceNumber_t & seqNum)
+{
+    CHIP_ERROR err;
+    BitFlags<WiFiPAFTP::HeaderFlags> rx_flags;
+    uint8_t SnOffset = 0;
+    SequenceNumber_t * pSn;
+    err = reader.Read8(rx_flags.RawStorage()).StatusCode();
+    if (rx_flags.Has(WiFiPAFTP::HeaderFlags::kHankshake))
+    {
+        // Handkshake message => No ack/sn
+        return CHIP_ERROR_INTERNAL;
+    }
+    // Always has header flag
+    SnOffset += kTransferProtocolHeaderFlagsSize;
+    if (rx_flags.Has(WiFiPAFTP::HeaderFlags::kManagementOpcode)) // Has Mgmt_Op
+    {
+        SnOffset += kTransferProtocolMgmtOpSize;
+    }
+    if (rx_flags.Has(WiFiPAFTP::HeaderFlags::kFragmentAck)) // Has ack
+    {
+        SnOffset += kTransferProtocolAckSize;
+    }
+    pSn    = pHead + SnOffset;
+    seqNum = *pSn;
+
+    return CHIP_NO_ERROR;
+}
+
 CHIP_ERROR WiFiPAFEndPoint::DebugPktAckSn(const PktDirect_t PktDirect, Encoding::LittleEndian::Reader & reader, uint8_t * pHead)
 {
 #ifdef CHIP_WIFIPAF_END_POINT_DEBUG_LOGGING_ENABLED
@@ -845,6 +892,81 @@ CHIP_ERROR WiFiPAFEndPoint::DebugPktAckSn(const PktDirect_t PktDirect, Encoding:
 }
 
 CHIP_ERROR WiFiPAFEndPoint::Receive(PacketBufferHandle && data)
+{
+    SequenceNumber_t ExpRxNextSeqNum = mPafTP.GetRxNextSeqNum();
+    SequenceNumber_t seqNum;
+    BitFlags<WiFiPAFTP::HeaderFlags> rx_flags;
+    Encoding::LittleEndian::Reader reader(data->Start(), data->DataLength());
+    CHIP_ERROR err = CHIP_NO_ERROR;
+
+    err = GetPktSn(reader, data->Start(), seqNum);
+    if (err != CHIP_NO_ERROR)
+    {
+        // Failed to get SeqNum. => Pass down to PAFTP engine directly
+        return _Receive(std::move(data));
+    }
+    /*
+        If reorder-queue is not empty => Need to queue the packet whose SeqNum is the next one at
+        offset 0 to fill the hole.
+    */
+    if ((ExpRxNextSeqNum == seqNum) && (ItemsInReorderQueue == 0))
+        return _Receive(std::move(data));
+
+    ChipLogError(WiFiPAF, "Reorder the packet: [%u, %u]", ExpRxNextSeqNum, seqNum);
+    // Start reordering packets
+    SequenceNumber_t offset = OffsetSeqNum(seqNum, ExpRxNextSeqNum);
+    if (offset >= PAFTP_REORDER_QUEUE_SIZE)
+    {
+        // Offset is too big
+        // => It may be the unexpected packet or duplicate packet => drop it
+        ChipLogError(WiFiPAF, "Offset (%u) is too big => drop the packet", offset);
+        ChipLogDebugBufferWiFiPAFEndPoint(WiFiPAF, data);
+        return CHIP_NO_ERROR;
+    }
+
+    // Save the packet to the reorder-queue
+    if (ReorderQueue[offset] == nullptr)
+    {
+        ReorderQueue[offset] = std::move(data).UnsafeRelease();
+        ItemsInReorderQueue++;
+    }
+
+    // Consume the packets in the reorder queue if no hole exists
+    if (ReorderQueue[0] == nullptr)
+    {
+        // The hole still exists => Can't continue
+        ChipLogError(WiFiPAF, "The hole still exists. Packets in reorder-queue: %u", ItemsInReorderQueue);
+        return CHIP_NO_ERROR;
+    }
+    uint8_t qidx;
+    for (qidx = 0; qidx < PAFTP_REORDER_QUEUE_SIZE; qidx++)
+    {
+        // The head slots should have been filled. => Do rx processing
+        if (ReorderQueue[qidx] == nullptr)
+        {
+            // Stop consuming packets until the hole or no packets
+            break;
+        }
+        // Consume the saved packets
+        ChipLogProgress(WiFiPAF, "Rx processing from the re-order queue [%u]", qidx);
+        err                = _Receive(System::PacketBufferHandle::Adopt(ReorderQueue[qidx]));
+        ReorderQueue[qidx] = nullptr;
+        ItemsInReorderQueue--;
+    }
+    // Has reached the 1st hole in the queue => move the rest items forward
+    // Note: It's to continue => No need to reinit "i"
+    for (uint8_t newId = 0; qidx < PAFTP_REORDER_QUEUE_SIZE; qidx++, newId++)
+    {
+        if (ReorderQueue[qidx] != nullptr)
+        {
+            ReorderQueue[newId] = ReorderQueue[qidx];
+            ReorderQueue[qidx]  = nullptr;
+        }
+    }
+    return err;
+}
+
+CHIP_ERROR WiFiPAFEndPoint::_Receive(PacketBufferHandle && data)
 {
     ChipLogDebugWiFiPAFEndPoint(WiFiPAF, "+++++++++++++++++++++ entered receive");
     ChipLogDebugBufferWiFiPAFEndPoint(WiFiPAF, data);

src/wifipaf/WiFiPAFEndPoint.h

@@ -89,15 +89,21 @@ class DLL_EXPORT WiFiPAFEndPoint
     ~WiFiPAFEndPoint() = default;
 
 private:
+    CHIP_ERROR _Receive(PacketBufferHandle && data);
     enum class PktDirect_t : uint8_t
     {
         kTx,
         kRx
     };
     CHIP_ERROR DebugPktAckSn(const PktDirect_t PktDirect, Encoding::LittleEndian::Reader & reader, uint8_t * pHead);
+    CHIP_ERROR GetPktSn(Encoding::LittleEndian::Reader & reader, uint8_t * pHead, SequenceNumber_t & seqNum);
 
     WiFiPAFLayer * mWiFiPafLayer; ///< [READ-ONLY] Pointer to the WiFiPAFLayer object that owns this object.
     WiFiPAFSession mSessionInfo;
+    SequenceNumber_t mRxAck;
+#define PAFTP_REORDER_QUEUE_SIZE PAF_MAX_RECEIVE_WINDOW_SIZE
+    System::PacketBuffer * ReorderQueue[PAFTP_REORDER_QUEUE_SIZE];
+    uint8_t ItemsInReorderQueue;
 
     enum class ConnectionStateFlag : uint8_t
     {
@@ -148,9 +154,9 @@ class DLL_EXPORT WiFiPAFEndPoint
 
     // Receive path:
     CHIP_ERROR HandleConnectComplete();
-    CHIP_ERROR HandleSendConfirmationReceived();
+    CHIP_ERROR HandleSendConfirmationReceived(bool result);
     CHIP_ERROR HandleHandshakeConfirmationReceived();
-    CHIP_ERROR HandleFragmentConfirmationReceived();
+    CHIP_ERROR HandleFragmentConfirmationReceived(bool result);
     CHIP_ERROR HandleCapabilitiesRequestReceived(PacketBufferHandle && data);
     CHIP_ERROR HandleCapabilitiesResponseReceived(PacketBufferHandle && data);
     SequenceNumber_t AdjustRemoteReceiveWindow(SequenceNumber_t lastReceivedAck, SequenceNumber_t maxRemoteWindowSize,

src/wifipaf/WiFiPAFLayer.cpp

@@ -248,6 +248,11 @@ void WiFiPAFLayer::Shutdown(OnCancelDeviceHandle OnCancelDevice)
         }
         ChipLogProgress(WiFiPAF, "WiFiPAF: Canceling id: %u", PafInfoElm.id);
         OnCancelDevice(PafInfoElm.id, PafInfoElm.role);
+        WiFiPAFEndPoint * endPoint = sWiFiPAFEndPointPool.Find(reinterpret_cast<WIFIPAF_CONNECTION_OBJECT>(&PafInfoElm));
+        if (endPoint != nullptr)
+        {
+            endPoint->DoClose(kWiFiPAFCloseFlag_AbortTransmission, WIFIPAF_ERROR_APP_CLOSED_CONNECTION);
+        }
     }
     PafInfoVect.clear();
 }
@@ -288,11 +293,11 @@ CHIP_ERROR WiFiPAFLayer::SendMessage(WiFiPAF::WiFiPAFSession & TxInfo, chip::Sys
     return CHIP_NO_ERROR;
 }
 
-CHIP_ERROR WiFiPAFLayer::HandleWriteConfirmed(WiFiPAF::WiFiPAFSession & TxInfo)
+CHIP_ERROR WiFiPAFLayer::HandleWriteConfirmed(WiFiPAF::WiFiPAFSession & TxInfo, bool result)
 {
     WiFiPAFEndPoint * endPoint = sWiFiPAFEndPointPool.Find(reinterpret_cast<WIFIPAF_CONNECTION_OBJECT>(&TxInfo));
     VerifyOrReturnError(endPoint != nullptr, CHIP_ERROR_INCORRECT_STATE, ChipLogDetail(WiFiPAF, "No endpoint to send packets"));
-    CHIP_ERROR err = endPoint->HandleSendConfirmationReceived();
+    CHIP_ERROR err = endPoint->HandleSendConfirmationReceived(result);
     VerifyOrReturnError(err == CHIP_NO_ERROR, CHIP_ERROR_INCORRECT_STATE,
                         ChipLogError(WiFiPAF, "Write_Confirm, Send pakets failed, err = %" CHIP_ERROR_FORMAT, err.Format()));
 

src/wifipaf/WiFiPAFLayer.h

@@ -170,7 +170,7 @@ class DLL_EXPORT WiFiPAFLayer
     State GetWiFiPAFState() { return mAppState; };
     void SetWiFiPAFState(State state);
     CHIP_ERROR SendMessage(WiFiPAF::WiFiPAFSession & TxInfo, chip::System::PacketBufferHandle && msg);
-    CHIP_ERROR HandleWriteConfirmed(WiFiPAF::WiFiPAFSession & TxInfo);
+    CHIP_ERROR HandleWriteConfirmed(WiFiPAF::WiFiPAFSession & TxInfo, bool result);
     CHIP_ERROR NewEndPoint(WiFiPAFEndPoint ** retEndPoint, WiFiPAFSession & SessionInfo, WiFiPafRole role);
     typedef void (*OnSubscribeCompleteFunct)(void * appState);
     typedef void (*OnSubscribeErrorFunct)(void * appState, CHIP_ERROR err);

src/wifipaf/WiFiPAFTP.cpp

@@ -53,10 +53,11 @@
 
 namespace chip {
 namespace WiFiPAF {
-
-static inline void IncSeqNum(SequenceNumber_t & a_seq_num)
+SequenceNumber_t OffsetSeqNum(SequenceNumber_t & tgtSeqNum, SequenceNumber_t & baseSeqNum)
 {
-    a_seq_num = static_cast<SequenceNumber_t>(0xff & ((a_seq_num) + 1));
+    if (tgtSeqNum >= baseSeqNum)
+        return static_cast<SequenceNumber_t>(tgtSeqNum - baseSeqNum);
+    return static_cast<SequenceNumber_t>((0xff - baseSeqNum) + tgtSeqNum + 1);
 }
 
 static inline bool DidReceiveData(BitFlags<WiFiPAFTP::HeaderFlags> rx_flags)
@@ -118,7 +119,7 @@ SequenceNumber_t WiFiPAFTP::GetAndIncrementNextTxSeqNum()
     mTxNewestUnackedSeqNum = mTxNextSeqNum;
 
     // Increment mTxNextSeqNum.
-    IncSeqNum(mTxNextSeqNum);
+    mTxNextSeqNum = IncSeqNum(mTxNextSeqNum);
 
     return ret;
 }
@@ -177,7 +178,7 @@ CHIP_ERROR WiFiPAFTP::HandleAckReceived(SequenceNumber_t ack_num)
     {
         // Update newest unacknowledged fragment to one past that which was just acknowledged.
         mTxOldestUnackedSeqNum = ack_num;
-        IncSeqNum(mTxOldestUnackedSeqNum);
+        mTxOldestUnackedSeqNum = IncSeqNum(mTxOldestUnackedSeqNum);
     }
 
     return CHIP_NO_ERROR;
@@ -258,7 +259,7 @@ CHIP_ERROR WiFiPAFTP::HandleCharacteristicReceived(System::PacketBufferHandle &&
                 SuccessOrExit(err);
                 ChipLogDebugWiFiPAFTP(WiFiPAF, "Update the seq_n: %u", mRxNewestUnackedSeqNum);
                 mRxNextSeqNum = mRxNewestUnackedSeqNum;
-                IncSeqNum(mRxNextSeqNum);
+                mRxNextSeqNum = IncSeqNum(mRxNextSeqNum);
                 LogState();
                 return CHIP_NO_ERROR;
             }
@@ -282,7 +283,7 @@ CHIP_ERROR WiFiPAFTP::HandleCharacteristicReceived(System::PacketBufferHandle &&
         VerifyOrExit(mRxNewestUnackedSeqNum == mRxNextSeqNum, err = WIFIPAF_ERROR_INVALID_PAFTP_SEQUENCE_NUMBER);
 
         // Increment next expected rx sequence number.
-        IncSeqNum(mRxNextSeqNum);
+        mRxNextSeqNum = IncSeqNum(mRxNextSeqNum);
 
         // If fragment was stand-alone ack, we're done here; no payload for message reassembler.
         if (!DidReceiveData(rx_flags))