Add feature to detect socketcand beacon

can/interfaces/socketcand/socketcand.py

@@ -13,12 +13,96 @@
 import socket
 import time
 import traceback
+import urllib.parse as urlparselib
+import xml.etree.ElementTree as ET
 from collections import deque
+from typing import List
 
 import can
 
 log = logging.getLogger(__name__)
 
+DEFAULT_SOCKETCAND_DISCOVERY_ADDRESS = ""
+DEFAULT_SOCKETCAND_DISCOVERY_PORT = 42000
+
+
+def detect_beacon():
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.bind((DEFAULT_SOCKETCAND_DISCOVERY_ADDRESS, DEFAULT_SOCKETCAND_DISCOVERY_PORT))
+    log.info(
+        f"Listening on for socketcand UDP advertisement on {DEFAULT_SOCKETCAND_DISCOVERY_ADDRESS}:{DEFAULT_SOCKETCAND_DISCOVERY_PORT}"
+    )
+
+    # Time between beacons no more than 3 seconds. Allow for at least 3
+    timeout_ms = 12000
+    now = time.time() * 1000
+    end_time = now + timeout_ms
+    while (time.time() * 1000) < end_time:
+        try:
+            # get all sockets that are ready (can be a list with a single value
+            # being self.socket or an empty list if self.socket is not ready)
+            ready_receive_sockets, _, _ = select.select([sock], [], [], 1)
+
+            if not ready_receive_sockets:
+                log.debug("No advertisement received")
+                continue
+
+            msg = sock.recv(1024).decode("utf-8")
+            root = ET.fromstring(msg)
+            if root.tag != "CANBeacon":
+                log.debug("Unexpected message received over UDP")
+                continue
+
+            det_devs = []
+            det_host = None
+            det_port = None
+            for child in root:
+                if child.tag == "Bus":
+                    bus_name = child.attrib["name"]
+                    det_devs.append(bus_name)
+                elif child.tag == "URL":
+                    url = urlparselib.urlparse(child.text)
+                    det_host = url.hostname
+                    det_port = url.port
+
+            if not det_devs:
+                log.debug(
+                    "Got advertisement, but no SocketCAN devices advertised by socketcand"
+                )
+                continue
+
+            if (det_host is None) or (det_port is None):
+                det_host = None
+                det_port = None
+                log.debug(
+                    "Got advertisement, but no SocketCAN URL advertised by socketcand"
+                )
+                continue
+
+            log.info(f"Found SocketCAN devices: {det_devs}")
+            return [
+                {
+                    "interface": "socketcand",
+                    "host": det_host,
+                    "port": det_port,
+                    "channel": channel,
+                }
+                for channel in det_devs
+            ]
+
+        except ET.ParseError:
+            log.debug("Unexpected message received over UDP")
+            continue
+
+        except Exception as exc:
+            # something bad happened (e.g. the interface went down)
+            log.error(f"Failed to detect beacon: {exc}  {traceback.format_exc()}")
+            raise OSError(f"Failed to detect beacon: {exc} {traceback.format_exc()}")
+
+    raise TimeoutError(
+        f"detect_beacon: Failed to detect udp beacon for {timeout_ms} ms"
+    )
+
 
 def convert_ascii_message_to_can_message(ascii_msg: str) -> can.Message:
     if not ascii_msg.startswith("< frame ") or not ascii_msg.endswith(" >"):
@@ -79,6 +163,9 @@ class SocketCanDaemonBus(can.BusABC):
     def __init__(self, channel, host, port, tcp_tune=False, can_filters=None, **kwargs):
         """Connects to a CAN bus served by socketcand.
 
+        It implements :meth:`can.BusABC._detect_available_configs` to search for
+        available interfaces.
+
         It will attempt to connect to the server for up to 10s, after which a
         TimeoutError exception will be thrown.
 
@@ -231,3 +318,11 @@ def shutdown(self):
         """Stops all active periodic tasks and closes the socket."""
         super().shutdown()
         self.__socket.close()
+
+    @staticmethod
+    def _detect_available_configs() -> List[can.typechecking.AutoDetectedConfig]:
+        try:
+            return detect_beacon()
+        except Exception as e:
+            log.warning(f"Could not detect socketcand beacon: {e}")
+            return []