Refactor: Implement concurrent multi-factory device discovery (#1083)

This commit refactors the device discovery mechanism to support concurrent, non-blocking validation of multiple ports across multiple device factories.

The key changes include:

-   **`DeviceManager`:** `discoverDevices` is completely rewritten to perform concurrent validation. It uses a round-robin scheduler to give each factory fair time on each port, significantly speeding up discovery when multiple ports or factories (like serial, TCP, mDNS) are present. The total discovery time is now bounded by the longest single-factory timeout, rather than the sum of all timeouts.

-   **`DeviceFactory`:** A new three-phase validation protocol (`beginValidation`, `stepValidation`, `completeValidation`) is introduced. This allows `DeviceManager` to drive the discovery process in a non-blocking manner. The existing `locateDevice` method is reimplemented as a blocking wrapper around this new protocol for single-port discovery.

-   **`cltool`:**
    -   Added a `-use-mdns` command-line option to enable network device discovery via mDNS.

-   **Firmware Updater:** The post-update process is improved to more robustly wait for the device to reboot into application mode, with better progress reporting and timeout handling.

-   **Other:** Data types for message statistics in `message_stats.h` have been changed from `int` to `uint64_t` to prevent potential overflow.

Author: kylemallory

cltool/CMakeLists.txt

@@ -18,6 +18,8 @@ include_directories(
 # Link the InertialSenseSDK static library 
 link_directories(${IS_SDK_DIR})
 
+add_definitions(-DIS_LOG_LEVEL=IS_LOG_LEVEL_MORE_INFO)
+
 if(WIN32)
     # yamlcpp needs to know it's a static lib
     add_definitions(-DYAML_CPP_STATIC_DEFINE)

cltool/src/cltool.cpp

@@ -658,6 +658,10 @@ bool cltool_parseCommandLine(int argc, char* argv[])
             g_commandLineOptions.list_devices = true;
             g_commandLineOptions.displayMode = cInertialSenseDisplay::DMODE_QUIET;
         }
+        else if (startsWith(a, "-use-mdns"))
+        {
+            g_commandLineOptions.useMdns = true;
+        }
         else if (startsWith(a, "-lmf="))
         {
             g_commandLineOptions.maxLogFileSize = (uint32_t)strtoul(&a[5], NULL, 10);
@@ -927,6 +931,9 @@ bool cltool_parseCommandLine(int argc, char* argv[])
         }
     }
 
+    // Always apply the verboseLevel to the SDK log system
+    IS_SET_LOG_LEVEL((eLogLevel)g_commandLineOptions.verboseLevel);
+
     // We are either using a serial port or replaying data
     if ((g_commandLineOptions.comPort.length() == 0) && !g_commandLineOptions.replayDataLog)
     {
@@ -1175,6 +1182,7 @@ void cltool_outputUsage()
 	cout << "    -dboc" << boldOff << "           Send stop-broadcast command `$STPB` on close." << endlbOn;
 	cout << "    -h --help" << boldOff << "       Display this help menu." << endlbOn;
     cout << "    -list-devices" << boldOff << "   Discovers and prints a list of discovered Inertial Sense devices and connected ports." << endlbOn;
+    cout << "    -use-mdns" << boldOff << "       Enable mDNS network discovery of remote devices (e.g., socat TCP-forwarded serial ports)." << endlbOn;
     cout << "    -lm" << boldOff << "             Listen mode for ISB. Disables device verification (-vd) and does not send stop-broadcast command on start." << endlbOn;
     cout << "    -magRecal[n]" << boldOff << "    Recalibrate magnetometers: 0=multi-axis, 1=single-axis" << endlbOn;
     cout << "    -nmea=[s]" << boldOff << "       Send NMEA message s with added checksum footer. Display rx messages. (`-nmea=ASCE,0,GxGGA,1`)" << endlbOn;

cltool/src/cltool.h

@@ -176,6 +176,7 @@ typedef struct cmd_options_s // we need to name this to make MSVC happy, since w
     uint32_t updateFirmwareSlot = 0;
     uint32_t runDurationMs = 0;             // Run for this many millis before exiting (0 = indefinitely)
     bool list_devices = false;              // if true, dumps results of findDevices() including port name.
+    bool useMdns = false;                   // if true, registers ISmDnsPortFactory for mDNS network device discovery
     EVFContainer_t evFCont = {0};
     EVMContainer_t evMCont = {0};
     EVOContainer_t evOCont;

cltool/src/cltool_main.cpp

@@ -41,6 +41,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
 #include "CorrectionService.h"
 #include "NtripCorrectionService.h"
 #include "TcpPortFactory.h"
+#include "ISmDnsPortFactory.h"
 #include "util/natsort.h"
 #include "util/uri.hpp"
 
@@ -956,13 +957,39 @@ void getMemoryEvent(InertialSense& inertialSenseInterface, uint32_t addrs, const
 
 static int cltool_dataStreaming()
 {
+    IS_LOG_OUTPUT(stdout);
+
     // [C++ COMM INSTRUCTION] STEP 1: Instantiate InertialSense Class
     // Create InertialSense object, passing in data callback function pointer.
-    InertialSense inertialSenseInterface({}, {&CltoolDeviceFactory::getInstance()});
+    // Build explicit port factory list — only include ISmDnsPortFactory when -use-mdns is specified
+    SerialPortFactory& spf = SerialPortFactory::getInstance();
+    spf.portOptions.defaultBaudRate = g_commandLineOptions.baudRate;
+    spf.portOptions.defaultBlocking = false;
+    TcpPortFactory& tpf = TcpPortFactory::getInstance();
+    tpf.portOptions.defaultBlocking = false;
+
+    std::vector<PortFactory*> portFactories = {&spf, &tpf};
+    if (g_commandLineOptions.useMdns) {
+        ISmDnsPortFactory& mdpf = ISmDnsPortFactory::getInstance();
+        mdpf.portOptions.defaultBlocking = false;
+        portFactories.push_back(&mdpf);
+    }
+
+    InertialSense inertialSenseInterface(portFactories, {&CltoolDeviceFactory::getInstance()});
     g_inertialSenseInterface = &inertialSenseInterface;
     inertialSenseInterface.setErrorHandler(cltool_errorCallback);
     inertialSenseInterface.EnableDeviceValidation(!g_commandLineOptions.disableDeviceValidation);
 
+    // Pre-warm mDNS cache so network devices are discoverable when Open() calls discoverPorts()
+    if (g_commandLineOptions.useMdns) {
+        printf("Discovering mDNS devices...\n");
+        uint32_t deadline = current_timeMs() + 3000;
+        while (current_timeMs() < deadline) {
+            ISmDnsPortFactory::tick();
+            SLEEP_MS(50);
+        }
+    }
+
     // [C++ COMM INSTRUCTION] STEP 2: Open serial port
     if (!inertialSenseInterface.Open(g_commandLineOptions.comPort.c_str(), g_commandLineOptions.baudRate, g_commandLineOptions.disableBroadcastsOnClose))
     {

src/DeviceFactory.cpp

@@ -1,5 +1,5 @@
 /**
- * @file DeviceFactory.cpp 
+ * @file DeviceFactory.cpp
  * @brief ${BRIEF_DESC}
  *
  * @author Kyle Mallory on 3/10/25.
@@ -24,63 +24,120 @@ void DeviceFactory::locateDevices(std::function<bool(DeviceFactory*, const dev_i
 }
 
 /**
- * Attempts to identify a specific type of device on the specified port, within the timeout period.
- * @param deviceCallback a function to be called if this Factory identified a possible/viable Inertial Sense device on the specified port
- * @param port the port to check for an Inertial Sense device. In most uses, the port specified should already be opened, however if the
- *   port is not opened, this function will attempt to open it in order for ensure discovery.
- * @param hdwId a hardware Id qualifier that can be used to narrow the type of device.  There is no direct indication that a hardware type
- *   failed to match.
- * @param timeout the maximum time to attempt to identify a device before giving up. There is no direct indication that a timeout occurred.
- * @return true if a device was detected, otherwise false. Note that a false can result for any number of reasons, including invalid port,
- *   hdwId mismatch, or a timeout.
+ * Blocking single-port discovery. Implemented as a wrapper around the three-phase validation protocol.
  */
 bool DeviceFactory::locateDevice(std::function<bool(DeviceFactory*, const dev_info_t&, port_handle_t)>& deviceCallback, port_handle_t port, uint16_t hdwId, uint16_t timeoutMs) {
-    if (!portIsValid(port))
-        return false;     // TODO: Should we do anything special if the port is invalid?  Really, we should never get here with an invalid port...
+    // Check for existing device on this port
+    device_handle_t dev = DeviceManager::getInstance().getDevice(port);
+    if (dev) {
+        if (dev->matchesHdwId(hdwId)) {
+            if (!dev->port)
+                dev->assignPort(port);
+            return dev->validate(timeoutMs);
+        }
+        return false;
+    }
+
+    // Phase 1: begin validation
+    auto ctx = beginValidation(port, hdwId, timeoutMs);
+    if (!ctx)
+        return false;
+
+    // Phase 2: blocking loop
+    while (!ctx->complete) {
+        stepValidation(*ctx);
+        if (!ctx->complete)
+            SLEEP_MS(2);
+    }
 
-    // can we open the port?
-    if (!portIsOpened(port)) {
-        log_debug(IS_LOG_DEVICE_FACTORY, "Opening serial port '%s'", portName(port));
+    // Phase 3: complete validation
+    dev_info_t devInfo;
+    if (completeValidation(*ctx, devInfo)) {
+        return deviceCallback(this, devInfo, port);
+    }
+    return false;
+}
+
+/**
+ * Phase 1: Opens the port, validates it, creates a base ISDevice for probing, and calls the
+ * onBeginValidation() hook to allow the factory to decline or do custom setup.
+ */
+std::unique_ptr<DeviceFactory::ValidationContext> DeviceFactory::beginValidation(port_handle_t port, uint16_t hdwId, uint32_t timeoutMs, std::shared_ptr<ISDevice> sharedDevice) {
+    if (!portIsValid(port)) {
+        log_more_debug(IS_LOG_DEVICE_FACTORY, "beginValidation: port '%s' is invalid, skipping.", portName(port));
+        return nullptr;
+    }
+
+    // Open port if needed (only when no shared device provided, i.e. standalone/blocking path)
+    if (!sharedDevice && !portIsOpened(port)) {
         if (!portValidate(port) || (portOpen(port) != PORT_ERROR__NONE)) {
-            log_debug(IS_LOG_DEVICE_FACTORY, "Error opening serial port '%s'.  Ignoring.  Error was: %s", portName(port), SERIAL_PORT(port)->error);
-            portClose(port);              // failed to open
+            portClose(port);
             portInvalidate(port);
-            return false;
-            // m_ignoredPorts.push_back(curPortName);     // record this port name as bad, so we don't try and reopen it again
+            return nullptr;
         }
     }
 
-    // We can only validate devices connected on COMM ports, since ISComm is needed to parse/communicate with the device
-    if (!(portType(port) & PORT_TYPE__COMM))
-        return false;
+    // Only COMM ports can be validated via ISComm protocol
+    if (!(portType(port) & PORT_TYPE__COMM)) {
+        log_more_debug(IS_LOG_DEVICE_FACTORY, "beginValidation: port '%s' is not a COMM port (type=0x%04X), skipping.", portName(port), portType(port));
+        return nullptr;
+    }
 
     if (timeoutMs <= 0)
         timeoutMs = deviceTimeout;
 
-    // at this point, the port should be opened...
-    device_handle_t dev = DeviceManager::getInstance().getDevice(port);
-    if (!dev) {
-        // no previous device exists, so identify the device and then register it with the manager
-        int validationResult = 0;
-        ISDevice localDev(hdwId, port);
-        do {
-            is_comm_port_parse_messages(port); // Read data directly into comm buffer and call callback functions
-            validationResult = localDev.validateAsync(timeoutMs);
-            SLEEP_MS(2);
-        } while (!validationResult);
+    // Let the factory decline this port
+    if (!onBeginValidation(port, hdwId)) {
+        log_more_debug(IS_LOG_DEVICE_FACTORY, "beginValidation: factory declined port '%s'.", portName(port));
+        return nullptr;
+    }
 
-        if (localDev.hasDeviceInfo() && localDev.matchesHdwId(hdwId)) {
-            // note that the deviceHandler callback can still reject the device for reasons
-            return deviceCallback(this, localDev.devInfo, port);
-        }
-    } else if (dev->matchesHdwId(hdwId)) {
-        if (!dev->port)
-            dev->assignPort(port);
-        else if (dev->port != port) {
-            // FIXME: this isn't good - it shouldn't happen, but it might... we should deal with it.
-        }
-        // a device exists associated with this port already, there isn't anything to do.
-        return dev->validate(timeoutMs);
+    log_more_debug(IS_LOG_DEVICE_FACTORY, "beginValidation: created validation context for port '%s' (hdwId=0x%04X, timeout=%dms)", portName(port), hdwId, timeoutMs);
+    auto ctx = std::make_unique<ValidationContext>();
+    ctx->port = port;
+    ctx->device = sharedDevice ? sharedDevice : std::make_shared<ISDevice>(hdwId, port);
+    ctx->hdwId = hdwId;
+    ctx->timeoutMs = timeoutMs;
+    return ctx;
+}
+
+/**
+ * Phase 2: Reads pending data from the port, then calls the onStepValidation() hook to
+ * advance validation state.
+ */
+int DeviceFactory::stepValidation(ValidationContext& ctx) {
+    if (ctx.complete)
+        return ctx.result;
+
+    is_comm_port_parse_messages(ctx.port);
+    ctx.result = onStepValidation(*ctx.device, ctx.timeoutMs);
+    ctx.complete = (ctx.result != 0);
+    if (ctx.result == 1) {
+        log_debug(IS_LOG_DEVICE_FACTORY, "stepValidation: port '%s' validated successfully.", portName(ctx.port));
+    } else if (ctx.result == -1) {
+        log_debug(IS_LOG_DEVICE_FACTORY, "stepValidation: port '%s' timed out.", portName(ctx.port));
     }
-    return false;
+    return ctx.result;
 }
+
+/**
+ * Phase 3: Checks that validation succeeded and the device has info, then calls the
+ * onCompleteValidation() hook to let the factory accept or reject.
+ */
+bool DeviceFactory::completeValidation(ValidationContext& ctx, dev_info_t& devInfoOut) {
+    if (ctx.result <= 0)
+        return false;
+    if (!ctx.device || !ctx.device->hasDeviceInfo()) {
+        log_debug(IS_LOG_DEVICE_FACTORY, "completeValidation: port '%s' validated but has no device info.", portName(ctx.port));
+        return false;
+    }
+    if (!onCompleteValidation(ctx.device->devInfo, ctx.hdwId)) {
+        log_debug(IS_LOG_DEVICE_FACTORY, "completeValidation: factory rejected device on port '%s' (hdwId=0x%04X).", portName(ctx.port), ctx.hdwId);
+        return false;
+    }
+
+    devInfoOut = ctx.device->devInfo;
+    log_debug(IS_LOG_DEVICE_FACTORY, "completeValidation: factory accepted device %s on port '%s'.",
+        ISDevice::getIdAsString(devInfoOut).c_str(), portName(ctx.port));
+    return true;
+}

src/DeviceFactory.h

@@ -22,11 +22,30 @@
 
 /**
  * An interface, to be implemented as a singleton, that is responsible for identifying various types of ISDevice and allocating and releasing them.
+ *
+ * Supports both blocking single-port discovery (locateDevice) and concurrent multi-port discovery
+ * via a three-phase protocol (beginValidation / stepValidation / completeValidation) driven by
+ * DeviceManager::discoverDevices(). Subclasses customize behavior by overriding the virtual hooks
+ * (onBeginValidation, onStepValidation, onCompleteValidation) without managing ValidationContext.
  */
 class DeviceFactory {
 public:
     virtual ~DeviceFactory() = default;
 
+    /**
+     * Per-port, per-factory state during concurrent validation. Managed by the base class
+     * beginValidation/stepValidation/completeValidation methods. Subclasses should not need
+     * to interact with this directly — override the onXxx hooks instead.
+     */
+    struct ValidationContext {
+        port_handle_t port = nullptr;
+        std::shared_ptr<ISDevice> device;
+        uint16_t hdwId = IS_HARDWARE_ANY;
+        uint32_t timeoutMs = 3000;
+        bool complete = false;
+        int result = 0;  // -1 = failed/timeout, 0 = in-progress, 1 = success
+    };
+
     /**
      * A function to be implemented in the factory responsible for allocating the underlying device type and returning a pointer to it
      * This function should NOT manipulate the underlying port, such as opening, etc.
@@ -66,6 +85,7 @@ class DeviceFactory {
 
     /**
      * Attempts to identify a specific type of device on the specified port, within the timeout period.
+     * Implemented as a blocking wrapper around beginValidation/stepValidation/completeValidation.
      * @param deviceCallback a function to be called if this Factory identified a possible/viable Inertial Sense device on the specified port
      * @param port the port to check for an Inertial Sense device. In most uses, the port specified should already be opened, however if the
      *   port is not opened, this function will attempt to open it in order for ensure discovery.
@@ -77,14 +97,71 @@ class DeviceFactory {
      */
     virtual bool locateDevice(std::function<bool(DeviceFactory*, const dev_info_t&, port_handle_t)>& deviceCallback, port_handle_t port, uint16_t hdwId, uint16_t timeout);
 
+    /**
+     * Phase 1: Prepare a ValidationContext for concurrent validation on this port.
+     * If a shared ISDevice probe is provided, it will be used (concurrent discovery shares one
+     * ISDevice per port across all factories). Otherwise, opens the port and creates a new probe.
+     * Calls onBeginValidation() to allow the factory to decline or do custom setup.
+     * @return a ValidationContext, or nullptr if this port cannot be validated
+     */
+    std::unique_ptr<ValidationContext> beginValidation(port_handle_t port, uint16_t hdwId, uint32_t timeoutMs, std::shared_ptr<ISDevice> sharedDevice = nullptr);
+
+    /**
+     * Phase 2: Non-blocking validation step. Reads pending data from the port, then calls
+     * onStepValidation() to advance validation state. Called repeatedly during the concurrent
+     * loop, only when this factory is the active round-robin slot for this port.
+     * @return -1 = failed/timeout, 0 = still in progress, 1 = validated successfully
+     */
+    int stepValidation(ValidationContext& ctx);
+
+    /**
+     * Phase 3: After stepValidation returns 1, checks that the device has valid info and calls
+     * onCompleteValidation() to let the factory accept or reject the device.
+     * @param ctx the completed validation context
+     * @param devInfoOut populated with the validated device info on success
+     * @return true if this factory claims the device, false otherwise
+     */
+    bool completeValidation(ValidationContext& ctx, dev_info_t& devInfoOut);
+
     /**
      * Assigns a Factory-specific timeout period for each port
      * @param timeout
      */
     void setPerDeviceTimeout(uint32_t timeout) { deviceTimeout = timeout; };
 
+    uint32_t getPerDeviceTimeout() const { return deviceTimeout; };
+
+protected:
+    /**
+     * Virtual hook called during Phase 1. Override to decline certain ports or perform
+     * custom port setup before validation begins.
+     * @param port the port about to be validated
+     * @param hdwId the hardware Id filter
+     * @return true to proceed with validation, false to skip this port
+     */
+    virtual bool onBeginValidation(port_handle_t port, uint16_t hdwId) { (void)port; (void)hdwId; return true; }
+
+    /**
+     * Virtual hook called during Phase 2. Override to inject custom validation logic
+     * (e.g., factory-specific queries or handshakes). The base class has already called
+     * is_comm_port_parse_messages() before invoking this hook.
+     * @param device the ISDevice probe to validate against
+     * @param timeoutMs the timeout for this validation attempt
+     * @return -1 = failed/timeout, 0 = still in progress, 1 = validated successfully
+     */
+    virtual int onStepValidation(ISDevice& device, uint32_t timeoutMs) { return device.validateAsync(timeoutMs); }
+
+    /**
+     * Virtual hook called during Phase 3. Override to accept or reject a validated device
+     * based on factory-specific criteria (e.g., hardware type, firmware version).
+     * @param devInfo the validated device info
+     * @param hdwId the hardware Id filter from the discovery request
+     * @return true to claim this device, false to pass to the next factory
+     */
+    virtual bool onCompleteValidation(const dev_info_t& devInfo, uint16_t hdwId) { (void)devInfo; (void)hdwId; return true; }
+
 private:
-    uint32_t deviceTimeout = 3000;
+    uint32_t deviceTimeout = 3000;  // should match DeviceManager::DISCOVERY__DEFAULT_TIMEOUT
 };
 
 class ImxDeviceFactory : public DeviceFactory {