From 0716635808df8c2bdf05e31e4130f1b9a985c2f7 Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Thu, 18 Nov 2021 11:25:21 +0800
Subject: [PATCH] boardd: fix a panda may be connected by multiple panda
 instance (#22932)

* fix bugs in main

* no auto
---
 selfdrive/boardd/boardd.cc | 59 ++++++++++++++++----------------------
 1 file changed, 24 insertions(+), 35 deletions(-)

diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index d15dc1bbe9..ce73662e45 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -627,11 +627,7 @@ void pigeon_thread(Panda *panda) {
   delete pigeon;
 }
 
-int main(int argc, char* argv[]) {
-  std::vector<std::thread> threads;
-  std::vector<Panda *> pandas;
-  Panda *peripheral_panda;
-
+int main(int argc, char *argv[]) {
   LOGW("starting boardd");
 
   if (!Hardware::PC()) {
@@ -645,49 +641,42 @@ int main(int argc, char* argv[]) {
   LOGW("attempting to connect");
   PubMaster pm({"pandaStates", "peripheralState"});
 
-  // connect loop
-  while (!do_exit) {
-    std::vector<std::string> serials(argv + 1, argv + argc);
-    if (serials.size() == 0) serials.push_back("");
-
-    // connect to all provided serials
-    for (int i=0; i<serials.size(); i++) {
-      Panda *p = usb_connect(serials[i], i);
-      if (p != NULL) {
-        pandas.push_back(p);
-      }
-    }
+  std::vector<std::string> serials(argv + 1, argv + argc);
+  if (serials.size() == 0) serials.push_back("");
 
-    // send empty pandaState & peripheralState and try again
-    if (pandas.size() != serials.size()) {
+  // connect to all provided serials
+  std::vector<Panda *> pandas;
+  for (int i = 0; i < serials.size() && !do_exit; /**/) {
+    Panda *p = usb_connect(serials[i], i);
+    if (!p) {
+      // send empty pandaState & peripheralState and try again
       send_empty_panda_state(&pm);
       send_empty_peripheral_state(&pm);
       util::sleep_for(500);
-    } else {
-      break;
+      continue;
     }
-  }
 
-  if (pandas.size() == 0) {
-    // do_exit was set while not connected to a panda
-    return 0;
+    pandas.push_back(p);
+    ++i;
   }
 
-  peripheral_panda = pandas[0];
+  if (!do_exit) {
+    LOGW("connected to board");
+    Panda *peripheral_panda = pandas[0];
+    std::vector<std::thread> threads;
 
-  LOGW("connected to board");
+    threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
+    threads.emplace_back(peripheral_control_thread, peripheral_panda);
+    threads.emplace_back(pigeon_thread, peripheral_panda);
 
-  threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
-  threads.emplace_back(peripheral_control_thread, peripheral_panda);
-  threads.emplace_back(pigeon_thread, peripheral_panda);
+    threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
+    threads.emplace_back(can_recv_thread, pandas);
 
-  threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
-  threads.emplace_back(can_recv_thread, pandas);
-
-  for (auto &t : threads) t.join();
+    for (auto &t : threads) t.join();
+  }
 
   // we have exited, clean up pandas
-  for (const auto& panda : pandas){
+  for (Panda *panda : pandas) {
     delete panda;
   }
 }