Add feature to detect socketcand beacon

can/interfaces/socketcand/socketcand.py

@@ -18,6 +18,76 @@
 
 log = logging.getLogger(__name__)
 
+DEFAULT_SOCKETCAND_PORT = 42000
+
+
+def detect_beacon(host, port):
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.bind((host, port))
+    log.info(f"Listening on for socketcand UDP advertisement on {host}:{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 det_devs, det_host, det_port
+
+        except ET.ParseError as exc:
+            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(" >"):
@@ -75,15 +145,41 @@ def connect_to_server(s, host, port):
 
 
 class SocketCanDaemonBus(can.BusABC):
-    def __init__(self, channel, host, port, can_filters=None, **kwargs):
-        self.__host = host
-        self.__port = port
+    def __init__(
+        self,
+        channel,
+        address=None,
+        beacon_address=("", DEFAULT_SOCKETCAND_PORT),
+        can_filters=None,
+        **kwargs,
+    ):
+        if not address:
+            devs, host, port = detect_beacon(beacon_address[0], beacon_address[1])
+            if not (devs and host and port):
+                log.error("Failed to detect socketcand beacon")
+                raise TimeoutError(
+                    f"Failed to detect socketcand beacon: {traceback.format_exc()}"
+                )
+            address = (host, port)
+            if not channel:
+                channel = devs[0]
+        else:
+            if not channel:
+                log.error(
+                    f"Channel must be provided if address is specified: {traceback.format_exc()}"
+                )
+                raise ValueError(
+                    f"Channel must be provided if address is specified: {traceback.format_exc()}"
+                )
+            host = address[0]
+            port = address[1]
+
         self.__socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
         self.__message_buffer = deque()
         self.__receive_buffer = ""  # i know string is not the most efficient here
         self.channel = channel
         self.channel_info = f"socketcand on {channel}@{host}:{port}"
-        connect_to_server(self.__socket, self.__host, self.__port)
+        connect_to_server(self.__socket, host, port)
         self._expect_msg("< hi >")
 
         log.info(