From 1fb11923aa56db4353c3bb5e0d1fe2bc30ae184a Mon Sep 17 00:00:00 2001
From: Vivien Nicolas <vnicolas@apple.com>
Date: Wed, 21 Sep 2022 12:35:37 +0200
Subject: [PATCH] Try to pair over all discovered ips instead of only the first
 one that has been discovered

---
 scripts/tools/check_includes_config.py |  4 ++--
 src/controller/SetUpCodePairer.cpp     | 25 +++++++++++++++----------
 src/controller/SetUpCodePairer.h       |  6 +++---
 3 files changed, 20 insertions(+), 15 deletions(-)

diff --git a/scripts/tools/check_includes_config.py b/scripts/tools/check_includes_config.py
index 3ab64a2f9250be..080e79d080590d 100644
--- a/scripts/tools/check_includes_config.py
+++ b/scripts/tools/check_includes_config.py
@@ -148,7 +148,7 @@
     # Uses platform-define to switch between list and array
     'src/lib/dnssd/minimal_mdns/ResponseSender.h': {'list'},
 
-    # Not really for embedded consumers; uses std::queue to keep track
+    # Not really for embedded consumers; uses std::deque to keep track
     # of a list of discovered things.
-    'src/controller/SetUpCodePairer.h': {'queue'},
+    'src/controller/SetUpCodePairer.h': {'deque'},
 }
diff --git a/src/controller/SetUpCodePairer.cpp b/src/controller/SetUpCodePairer.cpp
index 2f13f629a3e680..98528be064c3cd 100644
--- a/src/controller/SetUpCodePairer.cpp
+++ b/src/controller/SetUpCodePairer.cpp
@@ -224,7 +224,7 @@ bool SetUpCodePairer::ConnectToDiscoveredDevice()
         // connection attempt fails and calls right back into us to try the next
         // thing.
         RendezvousParameters params(mDiscoveredParameters.front());
-        mDiscoveredParameters.pop();
+        mDiscoveredParameters.pop_front();
 
         params.SetSetupPINCode(mSetUpPINCode);
 
@@ -268,8 +268,8 @@ void SetUpCodePairer::OnDiscoveredDeviceOverBle(BLE_CONNECTION_OBJECT connObj)
     mWaitingForDiscovery[kBLETransport] = false;
 
     Transport::PeerAddress peerAddress = Transport::PeerAddress::BLE();
-    mDiscoveredParameters.emplace();
-    mDiscoveredParameters.back().SetPeerAddress(peerAddress).SetConnectionObject(connObj);
+    mDiscoveredParameters.emplace_front();
+    mDiscoveredParameters.front().SetPeerAddress(peerAddress).SetConnectionObject(connObj);
     ConnectToDiscoveredDevice();
 }
 
@@ -338,12 +338,17 @@ void SetUpCodePairer::NotifyCommissionableDeviceDiscovered(const Dnssd::Discover
 
     ChipLogProgress(Controller, "Discovered device to be commissioned over DNS-SD");
 
-    Inet::InterfaceId interfaceId =
-        nodeData.resolutionData.ipAddress[0].IsIPv6LinkLocal() ? nodeData.resolutionData.interfaceId : Inet::InterfaceId::Null();
-    Transport::PeerAddress peerAddress =
-        Transport::PeerAddress::UDP(nodeData.resolutionData.ipAddress[0], nodeData.resolutionData.port, interfaceId);
-    mDiscoveredParameters.emplace();
-    mDiscoveredParameters.back().SetPeerAddress(peerAddress);
+    auto & resolutionData = nodeData.resolutionData;
+    for (size_t i = 0; i < resolutionData.numIPs; i++)
+    {
+        Inet::InterfaceId interfaceId =
+            resolutionData.ipAddress[i].IsIPv6LinkLocal() ? resolutionData.interfaceId : Inet::InterfaceId::Null();
+        Transport::PeerAddress peerAddress =
+            Transport::PeerAddress::UDP(resolutionData.ipAddress[i], resolutionData.port, interfaceId);
+        mDiscoveredParameters.emplace_back();
+        mDiscoveredParameters.back().SetPeerAddress(peerAddress);
+    }
+
     ConnectToDiscoveredDevice();
 }
 
@@ -396,7 +401,7 @@ void SetUpCodePairer::ResetDiscoveryState()
 
     while (!mDiscoveredParameters.empty())
     {
-        mDiscoveredParameters.pop();
+        mDiscoveredParameters.pop_front();
     }
 
     mLastPASEError = CHIP_NO_ERROR;
diff --git a/src/controller/SetUpCodePairer.h b/src/controller/SetUpCodePairer.h
index ed1c6f0c2ad5ac..354913a6320c88 100644
--- a/src/controller/SetUpCodePairer.h
+++ b/src/controller/SetUpCodePairer.h
@@ -41,7 +41,7 @@
 
 #include <controller/DeviceDiscoveryDelegate.h>
 
-#include <queue>
+#include <deque>
 
 namespace chip {
 namespace Controller {
@@ -177,10 +177,10 @@ class DLL_EXPORT SetUpCodePairer : public DevicePairingDelegate
     // process happening via the relevant transport.
     bool mWaitingForDiscovery[kTransportTypeCount] = { false };
 
-    // Queue of things we have discovered but not tried connecting to yet.  The
+    // Double ended-queue of things we have discovered but not tried connecting to yet.  The
     // general discovery/pairing process will terminate once this queue is empty
     // and all the booleans in mWaitingForDiscovery are false.
-    std::queue<RendezvousParameters> mDiscoveredParameters;
+    std::deque<RendezvousParameters> mDiscoveredParameters;
 
     // mWaitingForPASE is true if we have called either
     // EstablishPASEConnection or PairDevice on mCommissioner and are now just