More message encoding and attempted send

circuitmatter/__init__.py

@@ -289,6 +289,7 @@ def send(self, protocol_id, protocol_opcode, application_payload=None):
         if self.pending_acknowledgement is not None:
             message.exchange_flags |= ExchangeFlags.A
             self.send_standalone_time = None
+            message.acknowledged_message_counter = self.pending_acknowledgement
             self.pending_acknowledgement = None
         message.protocol_id = protocol_id
         message.protocol_opcode = protocol_opcode
@@ -338,20 +339,31 @@ def receive(self, message) -> bool:
 
 
 class UnsecuredSessionContext:
-    def __init__(self, message_counter, initiator, ephemeral_initiator_node_id):
+    def __init__(
+        self,
+        socket,
+        message_counter,
+        initiator,
+        ephemeral_initiator_node_id,
+        node_ipaddress,
+    ):
+        self.socket = socket
         self.initiator = initiator
         self.ephemeral_initiator_node_id = ephemeral_initiator_node_id
         self.message_reception_state = None
         self.message_counter = message_counter
+        self.node_ipaddress = node_ipaddress
         self.exchanges = {}
 
     def send(self, message):
         message.destination_node_id = self.ephemeral_initiator_node_id
         if message.message_counter is None:
             message.message_counter = next(self.message_counter)
+        print(message)
         buf = memoryview(bytearray(1280))
         nbytes = message.encode_into(buf)
         print(nbytes, buf[:nbytes].hex(" "))
+        self.socket.sendto(buf[:nbytes], self.node_ipaddress)
 
 
 class SecureSessionContext:
@@ -402,7 +414,7 @@ def clear(self):
         self.flags: int = 0
         self.session_id: int = 0
         self.security_flags: SecurityFlags = SecurityFlags(0)
-        self.message_counter: int = 0
+        self.message_counter: Optional[int] = None
         self.source_node_id = None
         self.secure_session: Optional[bool] = None
         self.payload = None
@@ -419,6 +431,8 @@ def clear(self):
         self.acknowledged_message_counter = None
         self.application_payload = None
 
+        self.source_ipaddress = None
+
     def parse_protocol_header(self):
         self.exchange_flags, self.protocol_opcode, self.exchange_id = (
             struct.unpack_from("<BBH", self.payload)
@@ -454,10 +468,11 @@ def decode(self, buffer):
         )
         self.security_flags = SecurityFlags(self.security_flags)
         offset = 8
-        self.source_node_id = None
         if self.flags & (1 << 2):
             self.source_node_id = struct.unpack_from("<Q", buffer, 8)[0]
             offset += 8
+        else:
+            self.source_node_id = None
 
         if (self.flags >> 4) != 0:
             raise RuntimeError("Incorrect version")
@@ -474,6 +489,28 @@ def decode(self, buffer):
 
     def encode_into(self, buffer):
         offset = 0
+        struct.pack_into(
+            "<BHBI",
+            buffer,
+            offset,
+            self.flags,
+            self.session_id,
+            self.security_flags,
+            self.message_counter,
+        )
+        offset += 8
+        if self.source_node_id is not None:
+            struct.pack_into("<Q", buffer, offset, self.source_node_id)
+            offset += 8
+        if self.destination_node_id is not None:
+            if self.destination_node_id > 0xFFFF_FFFF_FFFF_0000:
+                struct.pack_into(
+                    "<H", buffer, offset, self.destination_node_id & 0xFFFF
+                )
+                offset += 2
+            else:
+                struct.pack_into("<Q", buffer, offset, self.destination_node_id)
+                offset += 8
         struct.pack_into(
             "BBHH",
             buffer,
@@ -486,32 +523,92 @@ def encode_into(self, buffer):
         offset += 6
         if self.acknowledged_message_counter is not None:
             struct.pack_into("I", buffer, offset, self.acknowledged_message_counter)
-            offset += struct.calcsize(4)
+            offset += 4
+        if self.application_payload is not None:
+            if isinstance(self.application_payload, tlv.TLVStructure):
+                offset = self.application_payload.encode_into(buffer, offset)
+            else:
+                buffer[offset : offset + len(self.application_payload)] = (
+                    self.application_payload
+                )
+                offset += len(self.application_payload)
         return offset
 
+    @property
+    def source_node_id(self):
+        return self._source_node_id
+
+    @source_node_id.setter
+    def source_node_id(self, value):
+        self._source_node_id = value
+        if value is not None:
+            self.flags |= 1 << 2
+        else:
+            self.flags &= ~(1 << 2)
+
+    @property
+    def destination_node_id(self):
+        return self._destination_node_id
+
+    @destination_node_id.setter
+    def destination_node_id(self, value):
+        self._destination_node_id = value
+        # Clear the field
+        self.flags &= ~0x3
+        if value > 0xFFFF_FFFF_FFFF_0000:
+            self.flags |= 2
+        elif value > 0:
+            self.flags |= 1
+
+    def __str__(self):
+        pieces = ["Message:"]
+        pieces.append(f"Message Flags: {self.flags}")
+        pieces.append(f"Session ID: {self.session_id}")
+        pieces.append(f"Security Flags: {self.security_flags}")
+        pieces.append(f"Message Counter: {self.message_counter}")
+        if self.source_node_id is not None:
+            pieces.append(f"Source Node ID: {self.source_node_id:x}")
+        if self.destination_node_id is not None:
+            pieces.append(f"Destination Node ID: {self.destination_node_id:x}")
+        payload_info = ["Payload: "]
+        payload_info.append(f"Exchange Flags: {self.exchange_flags!r}")
+        payload_info.append(f"Protocol Opcode: {self.protocol_opcode!r}")
+        payload_info.append(f"Exchange ID: {self.exchange_id}")
+        if self.protocol_vendor_id:
+            payload_info.append(f"Protocol Vendor ID: {self.protocol_vendor_id}")
+        payload_info.append(f"Protocol ID: {self.protocol_id!r}")
+        if self.acknowledged_message_counter is not None:
+            payload_info.append(
+                f"Acknowledged Message Counter: {self.acknowledged_message_counter}"
+            )
+        if self.application_payload is not None:
+            application_payload = str(self.application_payload).replace("\n", "\n    ")
+            payload_info.append(f"Application Payload: {application_payload}")
+        pieces.append("\n    ".join(payload_info))
+        return "\n  ".join(pieces)
+
 
 class SessionManager:
-    def __init__(self):
+    def __init__(self, socket):
         persist_path = pathlib.Path("counters.json")
         if persist_path.exists():
             self.nonvolatile = json.loads(persist_path.read_text())
         else:
             self.nonvolatile = {}
-            self.nonvolatile["unencrypted_message_counter"] = 0
-            self.nonvolatile["group_encrypted_data_message_counter"] = 0
-            self.nonvolatile["group_encrypted_control_message_counter"] = 0
-        self.unencrypted_message_counter = MessageCounter(
-            self.nonvolatile["unencrypted_message_counter"]
-        )
+            self.nonvolatile["check_in_counter"] = None
+            self.nonvolatile["group_encrypted_data_message_counter"] = None
+            self.nonvolatile["group_encrypted_control_message_counter"] = None
+        self.unencrypted_message_counter = MessageCounter()
         self.group_encrypted_data_message_counter = MessageCounter(
             self.nonvolatile["group_encrypted_data_message_counter"]
         )
         self.group_encrypted_control_message_counter = MessageCounter(
             self.nonvolatile["group_encrypted_control_message_counter"]
         )
-        self.check_in_counter = 0
+        self.check_in_counter = MessageCounter(self.nonvolatile["check_in_counter"])
         self.unsecured_session_context = {}
         self.secure_session_contexts = ["reserved"]
+        self.socket = socket
 
     def _increment(self, value):
         return (value + 1) % 0xFFFFFFFF
@@ -528,9 +625,11 @@ def get_session(self, message):
             if message.source_node_id not in self.unsecured_session_context:
                 self.unsecured_session_context[message.source_node_id] = (
                     UnsecuredSessionContext(
+                        self.socket,
                         self.unencrypted_message_counter,
                         initiator=False,
                         ephemeral_initiator_node_id=message.source_node_id,
+                        node_ipaddress=message.source_ipaddress,
                     )
                 )
             session_context = self.unsecured_session_context[message.source_node_id]
@@ -636,7 +735,6 @@ def __init__(self, socketpool, mdns_server, state_filename, record_to=None):
             self.recorded_packets = []
         else:
             self.recorded_packets = None
-        self.manager = SessionManager()
 
         with open(state_filename, "r") as state_file:
             self.nonvolatile = json.load(state_file)
@@ -662,6 +760,8 @@ def __init__(self, socketpool, mdns_server, state_filename, record_to=None):
         self.socket.bind((UDP_IP, self.UDP_PORT))
         self.socket.setblocking(False)
 
+        self.manager = SessionManager(self.socket)
+
         print(f"Listening on UDP port {self.UDP_PORT}")
 
         if commission:
@@ -721,6 +821,7 @@ def process_packet(self, address, data):
         # This is section 4.7.2
         message = Message()
         message.decode(data)
+        message.source_ipaddress = address
         if message.secure_session:
             # Decrypt the payload
             pass
@@ -769,7 +870,7 @@ def process_packet(self, address, data):
                 exchange.send(
                     ProtocolId.SECURE_CHANNEL,
                     SecureProtocolOpcode.PBKDF_PARAM_RESPONSE,
-                    response.encode(),
+                    response,
                 )
 
             elif protocol_opcode == SecureProtocolOpcode.PBKDF_PARAM_RESPONSE:

circuitmatter/__main__.py

@@ -30,6 +30,12 @@ def recvfrom_into(self, buffer, nbytes=None):
         buffer[: len(decoded)] = decoded
         return len(decoded), address
 
+    def sendto(self, data, address):
+        if address is None:
+            raise ValueError("Address must be set")
+        print("sendto", address, data.hex(" "))
+        return len(data)
+
 
 class ReplaySocketPool:
     AF_INET6 = 0
@@ -78,16 +84,16 @@ def advertise_service(
         if service_type in self.active_services:
             self.active_services[service_type].kill()
             del self.active_services[service_type]
-        self.active_services[service_type] = subprocess.Popen(
-            [
-                "avahi-publish-service",
-                *subtypes,
-                instance_name,
-                f"{service_type}.{protocol}",
-                str(port),
-                *txt_records,
-            ]
-        )
+        command = [
+            "avahi-publish-service",
+            *subtypes,
+            instance_name,
+            f"{service_type}.{protocol}",
+            str(port),
+            *txt_records,
+        ]
+        print("running avahi", command)
+        self.active_services[service_type] = subprocess.Popen(command)
 
     def __del__(self):
         for active_service in self.active_services.values():

test_data/recorded_packets.json

@@ -1 +1 @@
-[["receive", 12949804778404, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHceHgahu2bHT7198QUgHGgAABUwASCUfOKTiSkZChWo80IKLeAJrQP4QjlyHzXAU4XY17COnyUCKh4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12950144513768, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHceHgahu2bHT7198QUgHGgAABUwASCUfOKTiSkZChWo80IKLeAJrQP4QjlyHzXAU4XY17COnyUCKh4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12950540781536, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHceHgahu2bHT7198QUgHGgAABUwASCUfOKTiSkZChWo80IKLeAJrQP4QjlyHzXAU4XY17COnyUCKh4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12951088813409, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHceHgahu2bHT7198QUgHGgAABUwASCUfOKTiSkZChWo80IKLeAJrQP4QjlyHzXAU4XY17COnyUCKh4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12951967655249, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHceHgahu2bHT7198QUgHGgAABUwASCUfOKTiSkZChWo80IKLeAJrQP4QjlyHzXAU4XY17COnyUCKh4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12960266757845, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHgeHga1UXD8lhA2VgUgHWgAABUwASDq88BY4o1KMEOhTYxvmhCnLE9CmI3HRGI6lRdcwMBSIiUCKx4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12960629245872, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHgeHga1UXD8lhA2VgUgHWgAABUwASDq88BY4o1KMEOhTYxvmhCnLE9CmI3HRGI6lRdcwMBSIiUCKx4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12961001635012, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHgeHga1UXD8lhA2VgUgHWgAABUwASDq88BY4o1KMEOhTYxvmhCnLE9CmI3HRGI6lRdcwMBSIiUCKx4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12961511504230, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHgeHga1UXD8lhA2VgUgHWgAABUwASDq88BY4o1KMEOhTYxvmhCnLE9CmI3HRGI6lRdcwMBSIiUCKx4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12962460950320, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHgeHga1UXD8lhA2VgUgHWgAABUwASDq88BY4o1KMEOhTYxvmhCnLE9CmI3HRGI6lRdcwMBSIiUCKx4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12979816719673, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHkeHgZIpSIef7tBqwUgHmgAABUwASA08hQ29Dg4r6zjmGrLG+5OkYeOa3GfIRz8zBmirb36RSUCLB4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12980145876216, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHkeHgZIpSIef7tBqwUgHmgAABUwASA08hQ29Dg4r6zjmGrLG+5OkYeOa3GfIRz8zBmirb36RSUCLB4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12980522150543, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHkeHgZIpSIef7tBqwUgHmgAABUwASA08hQ29Dg4r6zjmGrLG+5OkYeOa3GfIRz8zBmirb36RSUCLB4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12981185733919, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHkeHgZIpSIef7tBqwUgHmgAABUwASA08hQ29Dg4r6zjmGrLG+5OkYeOa3GfIRz8zBmirb36RSUCLB4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12982043582071, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHkeHgZIpSIef7tBqwUgHmgAABUwASA08hQ29Dg4r6zjmGrLG+5OkYeOa3GfIRz8zBmirb36RSUCLB4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12990263927610, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHoeHgbGNfGaA6ZMXAUgH2gAABUwASDFi1+HqR/BaN0S18wlXX48EJPsXU9EVpumnLaR8sGEIiUCLR4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12990600095933, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHoeHgbGNfGaA6ZMXAUgH2gAABUwASDFi1+HqR/BaN0S18wlXX48EJPsXU9EVpumnLaR8sGEIiUCLR4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12990995425710, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHoeHgbGNfGaA6ZMXAUgH2gAABUwASDFi1+HqR/BaN0S18wlXX48EJPsXU9EVpumnLaR8sGEIiUCLR4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12991659186503, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHoeHgbGNfGaA6ZMXAUgH2gAABUwASDFi1+HqR/BaN0S18wlXX48EJPsXU9EVpumnLaR8sGEIiUCLR4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 12992397890936, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAHoeHgbGNfGaA6ZMXAUgH2gAABUwASDFi1+HqR/BaN0S18wlXX48EJPsXU9EVpumnLaR8sGEIiUCLR4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="]]
+[["receive", 277634700589145, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHseHgbighJRIqWzsAUgIGgAABUwASCFzR7wiM4L8C3VEpSXe5UK/1tjpinMKPaEtLYTbkR9kyUCLh4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 277634706760287, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHweHgbighJRIqWzsAVAIGgAAAEAAAAAAAIA"], ["receive", 277634708186275, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAH0eHgahFRe9xqs9aAUgIWgAABUwASAQJZk9pu15Dip5poydFQr50ZInKaRJg9hFd7lFMiV0QCUCLx4kAwAoBDUFJQH0ASUCLAElA6APJAQSJAULJgYAAAQBJAcBGBg="], ["receive", 277634727394981, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAH4eHgahFRe9xqs9aAVAIWgAAAEAAAAAAAIA"], ["receive", 277635098871160, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAH4eHgahFRe9xqs9aAVAIWgAAAEAAAAAAAIA"], ["receive", 277635109360371, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHweHgbighJRIqWzsAVAIGgAAAEAAAAAAAIA"], ["receive", 277635473211167, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAH4eHgahFRe9xqs9aAVAIWgAAAEAAAAAAAIA"], ["receive", 277635519678001, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHweHgbighJRIqWzsAVAIGgAAAEAAAAAAAIA"], ["receive", 277636049081875, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAH4eHgahFRe9xqs9aAVAIWgAAAEAAAAAAAIA"], ["receive", 277636061715619, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHweHgbighJRIqWzsAVAIGgAAAEAAAAAAAIA"], ["receive", 277637055974480, ["::ffff:192.168.0.37", 53917, 0, 0], "BAAAAH4eHgahFRe9xqs9aAVAIWgAAAEAAAAAAAIA"], ["receive", 277637087415383, ["fd98:bbab:bd61:8040:14ae:fe13:c814:3bc2", 58700, 0, 0], "BAAAAHweHgbighJRIqWzsAVAIGgAAAEAAAAAAAIA"]]