diff --git a/src/app/clusters/network-commissioning/network-commissioning.cpp b/src/app/clusters/network-commissioning/network-commissioning.cpp index 13c890a61bd1bb..a63df5cb88321f 100644 --- a/src/app/clusters/network-commissioning/network-commissioning.cpp +++ b/src/app/clusters/network-commissioning/network-commissioning.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -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 scanResponseArray; + size_t scanResponseArrayLength = 0; uint8_t extendedAddressBuffer[Thread::kSizeExtendedPanId]; SuccessOrExit(err = commandHandle->PrepareCommand( @@ -546,18 +548,61 @@ 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(chip::min(networks->Count(), kMaxNetworksInScanResponse)), 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)) + { + if (scanResponseArray[i].rssi < scanResponse.rssi) + { + scanResponseArray[i] = scanResponseArray[--scanResponseArrayLength]; + } + else + { + isDuplicated = true; + } + 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)); } diff --git a/src/lib/support/SortUtils.h b/src/lib/support/SortUtils.h index 27ab1852d3da1a..9ebe3e368886ac 100644 --- a/src/lib/support/SortUtils.h +++ b/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 +void InsertionSort(T * items, size_t n, CompareFunc f) +{ + for (size_t i = 1; i < n; i++) + { + const T key = items[i]; + int j = static_cast(i) - 1; + + while (j >= 0 && f(key, items[j])) + { + items[j + 1] = items[j]; + j--; + } + items[j + 1] = key; + } +} + } // namespace Sorting } // namespace chip