Vehicle Announcement Fixes (#11)

Experimental IPv6 support for awaiting multicast vehicle announcements. 

Fix regressions on IPV4
* bind needs "", not INADDRY_ANY constant
* The UDP state machine can get confused with bad packets. Spec only allows for single per packet, so reset parser between reads.

doipclient/client.py

@@ -5,7 +5,7 @@
 import time
 import ssl
 from enum import IntEnum
-from .constants import TCP_DATA_UNSECURED, UDP_DISCOVERY, A_PROCESSING_TIME
+from .constants import TCP_DATA_UNSECURED, UDP_DISCOVERY, A_PROCESSING_TIME, LINK_LOCAL_MULTICAST_ADDRESS
 from .messages import *
 
 logger = logging.getLogger("doipclient")
@@ -28,6 +28,9 @@ class ParserState(IntEnum):
         READ_PAYLOAD = 5
 
     def __init__(self):
+        self.reset()
+
+    def reset(self):
         self.rx_buffer = bytearray()
         self.protocol_version = None
         self.payload_type = None
@@ -198,7 +201,7 @@ def __exit__(self, type, value, traceback):
 
     @classmethod
     def await_vehicle_announcement(
-        cls, udp_port=UDP_DISCOVERY, timeout=None, ipv6=False
+        cls, udp_port=UDP_DISCOVERY, timeout=None, ipv6=False, source_interface=None
     ):
         """Receive Vehicle Announcement Message
 
@@ -215,19 +218,46 @@ def await_vehicle_announcement(
         :type ipv6: bool, optional
         :return: IP Address of ECU and VehicleAnnouncementMessage object
         :rtype: tuple
+        :param source_interface: Interface name (like "eth0") to bind to for use with IPv6. Defaults to None which
+            will use the default interface (which may not be the one connected to the ECU). Does nothing for IPv4,
+            which will bind to all interfaces uses INADDR_ANY.
+        :type source_interface: str, optional 
         :raises TimeoutError: If vehicle announcement not received in time
         """
         start_time = time.time()
-        if not ipv6:
-            sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        else:
+
+        if ipv6:
             sock = socket.socket(socket.AF_INET6, socket.SOCK_DGRAM)
 
+            # IPv6 version always uses link-local scope multicast address (FF02 16 ::1) 
+            sock.bind((LINK_LOCAL_MULTICAST_ADDRESS, udp_port))
+
+            if source_interface is None:
+                # 0 is the "default multicast interface" which is unlikely to be correct, but it will do
+                interface_index = 0
+            else:
+                interface_index = socket.if_nametoindex(source_interface)
+
+            # Join the group so that packets are delivered
+            mc_addr = ipaddress.IPv6Address(LINK_LOCAL_MULTICAST_ADDRESS)
+            join_data = struct.pack('16sI', mc_addr.packed, interface_index)
+            # IPV6_JOIN_GROUP is also known as IPV6_ADD_MEMBERSHIP, though older Python for Windows doesn't have it
+            # IPPROTO_IPV6 may be missing in older Windows builds
+            try:
+                from socket import IPPROTO_IPV6
+            except ImportError:
+                IPPROTO_IPV6 = 41
+            sock.setsockopt(IPPROTO_IPV6, socket.IPV6_JOIN_GROUP, join_data)
+        else:
+            sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            # IPv4, use INADDR_ANY to listen to all interfaces for broadcasts (not multicast)
+            sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+            sock.bind(("", udp_port))
+
         sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
-        sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
         if timeout is not None:
             sock.settimeout(timeout)
-        sock.bind(("", udp_port))
+
         parser = Parser()
 
         while True:
@@ -247,8 +277,11 @@ def await_vehicle_announcement(
                 raise TimeoutError(
                     "Timed out waiting for Vehicle Announcement broadcast"
                 )
+            # "Only one DoIP message shall be transmitted by any DoIP entity per datagram"
+            # So, reset the parser after each UDP read
+            parser.reset()
             result = parser.read_message(data)
-            if result:
+            if result and type(result) == VehicleIdentificationResponse:
                 return addr, result
 
     def empty_rxqueue(self):
@@ -309,6 +342,9 @@ def read_doip(
                                 logger.debug("Peer has closed the connection.")
                                 self._tcp_close_detected = True
                         else:
+                            # "Only one DoIP message shall be transmitted by any DoIP entity
+                            # per UDP datagram", so reset the UDP parser for each recv()
+                            self._udp_parser.reset()
                             data = self._udp_sock.recv(1024)
                     except socket.timeout:
                         pass

doipclient/constants.py

@@ -15,3 +15,6 @@
 # Table 39 - Supported TCP ports
 TCP_DATA_UNSECURED = 13400
 TCP_DATA_SECURED = 3496
+
+# link-local scope multicast address (FF02 16 ::1) 
+LINK_LOCAL_MULTICAST_ADDRESS = "ff02::1"

tests/test_client.py

@@ -5,6 +5,11 @@
 from doipclient.client import Parser
 from doipclient.messages import *
 
+try:
+    from socket import IPPROTO_IPV6
+except ImportError:
+    IPPROTO_IPV6 = 41
+
 test_logical_address = 1
 test_ip = "127.0.0.1"
 
@@ -148,6 +153,16 @@ def recv(self, bufflen):
         except IndexError:
             raise socket.timeout()
 
+    def recvfrom(self, bufflen):
+        try:
+            result = self.rx_queue.pop(0)
+            if type(result) == bytearray:
+                return result, None
+            else:
+                raise (result)
+        except IndexError:
+            raise socket.timeout()
+
     def send(self, buffer):
         self.tx_queue.append(buffer)
         return len(buffer)
@@ -581,3 +596,36 @@ def test_ipv6(mock_socket):
         socket.SOL_SOCKET: {socket.SO_REUSEADDR: True},
         socket.IPPROTO_TCP: {socket.TCP_NODELAY: True},
     }
+
+
+def test_await_ipv6(mock_socket):
+    mock_socket.rx_queue.clear()
+    try:
+        DoIPClient.await_vehicle_announcement(
+            udp_port=13400, timeout=0.1, ipv6=True, source_interface=None
+        )
+    except TimeoutError:
+        pass
+    assert mock_socket._network == socket.AF_INET6
+    assert mock_socket._bound_ip == 'ff02::1'
+    assert mock_socket._bound_port == 13400
+    assert mock_socket.opts == {
+        socket.SOL_SOCKET: {socket.SO_REUSEADDR: True},
+        IPPROTO_IPV6: {socket.IPV6_JOIN_GROUP: b'\xff\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00'},
+    }
+
+
+def test_await_ipv4(mock_socket):
+    mock_socket.rx_queue.clear()
+    try:
+        DoIPClient.await_vehicle_announcement(
+            udp_port=13400, timeout=0.1, ipv6=False, source_interface=None
+        )
+    except TimeoutError:
+        pass
+    assert mock_socket._network == socket.AF_INET
+    assert mock_socket._bound_ip == ""
+    assert mock_socket._bound_port == 13400
+    assert mock_socket.opts == {
+        socket.SOL_SOCKET: {socket.SO_REUSEADDR: True, socket.SO_BROADCAST: True},
+    }