Fix thread commissioning fail where there are many Thread Border Routers

src/app/clusters/network-commissioning/network-commissioning.cpp

@@ -26,6 +26,7 @@
 #include <app/server/Server.h>
 #include <app/util/attribute-storage.h>
 #include <lib/support/SafeInt.h>
+#include <lib/support/SortUtils.h>
 #include <lib/support/ThreadOperationalDataset.h>
 #include <platform/DeviceControlServer.h>
 #include <platform/PlatformManager.h>
@@ -528,7 +529,8 @@ void Instance::OnFinished(Status status, CharSpan debugText, ThreadScanResponseI
     TLV::TLVWriter * writer;
     TLV::TLVType listContainerType;
     ThreadScanResponse scanResponse;
-    size_t networksEncoded = 0;
+    chip::Platform::ScopedMemoryBuffer<ThreadScanResponse> scanResponseArray;
+    size_t scanResponseArrayLength = 0;
     uint8_t extendedAddressBuffer[Thread::kSizeExtendedPanId];
 
     SuccessOrExit(err = commandHandle->PrepareCommand(
@@ -546,18 +548,63 @@ void Instance::OnFinished(Status status, CharSpan debugText, ThreadScanResponseI
         err = writer->StartContainer(TLV::ContextTag(to_underlying(Commands::ScanNetworksResponse::Fields::kThreadScanResults)),
                                      TLV::TLVType::kTLVType_Array, listContainerType));
 
-    for (; networks != nullptr && networks->Next(scanResponse) && networksEncoded < kMaxNetworksInScanResponse; networksEncoded++)
+    VerifyOrExit(
+        scanResponseArray.Alloc(networks->Count() > kMaxNetworksInScanResponse ? kMaxNetworksInScanResponse : networks->Count()),
+        err = CHIP_ERROR_NO_MEMORY);
+    for (; networks != nullptr && networks->Next(scanResponse);)
+    {
+        if ((scanResponseArrayLength == kMaxNetworksInScanResponse) &&
+            (scanResponseArray[scanResponseArrayLength - 1].rssi > scanResponse.rssi))
+        {
+            continue;
+        }
+
+        bool isDuplicated = false;
+
+        for (size_t i = 0; i < scanResponseArrayLength; i++)
+        {
+            if ((scanResponseArray[i].panId == scanResponse.panId) &&
+                (scanResponseArray[i].extendedPanId == scanResponse.extendedPanId))
+            {
+                isDuplicated = true;
+                if (scanResponseArray[i].rssi < scanResponse.rssi)
+                {
+                    scanResponseArray[i] = scanResponse;
+                    Sorting::InsertionSort(
+                        scanResponseArray.Get(), scanResponseArrayLength,
+                        [](const ThreadScanResponse & a, const ThreadScanResponse & b) -> bool { return a.rssi > b.rssi; });
+                }
+                break;
+            }
+        }
+
+        if (isDuplicated)
+        {
+            continue;
+        }
+
+        if (scanResponseArrayLength < kMaxNetworksInScanResponse)
+        {
+            scanResponseArrayLength++;
+        }
+        scanResponseArray[scanResponseArrayLength - 1] = scanResponse;
+        Sorting::InsertionSort(scanResponseArray.Get(), scanResponseArrayLength,
+                               [](const ThreadScanResponse & a, const ThreadScanResponse & b) -> bool { return a.rssi > b.rssi; });
+    }
+
+    for (size_t i = 0; i < scanResponseArrayLength; i++)
     {
         Structs::ThreadInterfaceScanResult::Type result;
-        Encoding::BigEndian::Put64(extendedAddressBuffer, scanResponse.extendedAddress);
-        result.panId           = scanResponse.panId;
-        result.extendedPanId   = scanResponse.extendedPanId;
-        result.networkName     = CharSpan(scanResponse.networkName, scanResponse.networkNameLen);
-        result.channel         = scanResponse.channel;
-        result.version         = scanResponse.version;
+        Encoding::BigEndian::Put64(extendedAddressBuffer, scanResponseArray[i].extendedAddress);
+        result.panId           = scanResponseArray[i].panId;
+        result.extendedPanId   = scanResponseArray[i].extendedPanId;
+        result.networkName     = CharSpan(scanResponseArray[i].networkName, scanResponseArray[i].networkNameLen);
+        result.channel         = scanResponseArray[i].channel;
+        result.version         = scanResponseArray[i].version;
         result.extendedAddress = ByteSpan(extendedAddressBuffer);
-        result.rssi            = scanResponse.rssi;
-        result.lqi             = scanResponse.lqi;
+        result.rssi            = scanResponseArray[i].rssi;
+        result.lqi             = scanResponseArray[i].lqi;
+
         SuccessOrExit(err = DataModel::Encode(*writer, TLV::AnonymousTag(), result));
     }
 

src/lib/support/SortUtils.h

@@ -66,6 +66,38 @@ void BubbleSort(T * items, size_t n, CompareFunc f)
     }
 }
 
+/**
+ *
+ * Impements the insertion sort algorithm to sort an array
+ * of items of size 'n'.
+ *
+ * The provided comparison function SHALL have the following signature:
+ *
+ *      bool Compare(const T& a, const T& b);
+ *
+ * If a is deemed less than (i.e is ordered before) b, return true.
+ * Else, return false.
+ *
+ * This is a stable sort.
+ *
+ */
+template <typename T, typename CompareFunc>
+void InsertionSort(T * items, size_t n, CompareFunc f)
+{
+    for (size_t i = 1; i < n; i++)
+    {
+        const T key = items[i];
+        int j       = i - 1;
+
+        while (j >= 0 && f(key, items[j]))
+        {
+            items[j + 1] = items[j];
+            j--;
+        }
+        items[j + 1] = key;
+    }
+}
+
 } // namespace Sorting
 
 } // namespace chip