project-chip · wilbur-se · Apr 2, 2024
diff --git a/Makefile b/Makefile
@@ -71,8 +71,8 @@ LDFLAGS += -Wl,-soname=$(PLUGIN_NAME).so -Wl,-Map -Wl,$(PLUGIN_NAME).map -Wl,--c
 endif
 
 DISSECTOR_SRCS := packet-matter.cpp packet-matter-decrypt.cpp packet-matter-echo.cpp packet-matter-common.cpp packet-matter-im.cpp packet-matter-security.cpp
-SRCS := $(DISSECTOR_SRCS) $(MATTER_SRCS) TLVDissector.cpp MatterMessageTracker.cpp MessageEncryptionKey.cpp UserEncryptionKeyPrefs.cpp HKDF.c
-HEADERS = moduleinfo.h  packet-matter.h packet-matter-decrypt.h TLVDissector.h MatterMessageTracker.h MessageEncryptionKey.h UserEncryptionKeyPrefs.h HKDF.h
+SRCS := $(DISSECTOR_SRCS) $(MATTER_SRCS) TLVDissector.cpp MatterMessageTracker.cpp MessageEncryptionKey.cpp UserEncryptionKeyPrefs.cpp UserNodeIdPrefs.cpp HKDF.c
+HEADERS = moduleinfo.h  packet-matter.h packet-matter-decrypt.h TLVDissector.h MatterMessageTracker.h MessageEncryptionKey.h UserEncryptionKeyPrefs.h UserNodeIdPrefs.h HKDF.h
 OBJS := $(foreach src, $(SRCS), $(src:.c=.o))
 OBJS := $(foreach src, $(OBJS), $(src:.cpp=.o))
 

diff --git a/MessageEncryptionKey.cpp b/MessageEncryptionKey.cpp
@@ -48,6 +48,7 @@ const MessageEncryptionKey *MessageEncryptionKeyTable::AddKey(const MessageEncry
     newKey->dataEncKey = (char *)wmem_alloc(wmem_epan_scope(), keyData.dataEncKeyLen);
     memcpy(newKey->dataEncKey, keyData.dataEncKey, keyData.dataEncKeyLen);
     newKey->dataEncKeyLen = keyData.dataEncKeyLen;
+    newKey->srcNodeId = keyData.srcNodeId;
     newKey->nextKey = existingKeys;
 
     wmem_tree_insert32(sKeys, newKey->keyId, newKey);

diff --git a/MessageEncryptionKey.h b/MessageEncryptionKey.h
@@ -1,3 +1,11 @@
+/*
+ * @Author       : wilbur
+ * @Date         : 2024-03-29 12:42:10
+ * @LastEditTime : 2024-03-29 21:12:48
+ * @LastEditors  : wilbur
+ * @Description  : Please enter file description
+ * Copyright (c) 2024, All Rights Reserved.
+ */
 /*
  *  Copyright (c) 2023 Project CHIP Authors.
  * 
@@ -18,6 +26,7 @@ class MessageEncryptionKey
     uint8_t sessionType;
     char *dataEncKey;
     guint dataEncKeyLen;
+    uint64_t srcNodeId;
     MessageEncryptionKey *nextKey;
 
     bool IsSameKey(const MessageEncryptionKey& otherKey) const;

diff --git a/UserEncryptionKeyPrefs.cpp b/UserEncryptionKeyPrefs.cpp
@@ -64,6 +64,7 @@ MessageEncryptionKey_update_cb(void *rec _U_, char **err _U_)
 
     r->keyId = matter::MatterSessionId::kNone;
     r->sessionType = matter::kMatterEncryptionType_AES128CCM;
+    r->srcNodeId = 0;
 
     *err = NULL;
 
@@ -123,5 +124,6 @@ void UserEncryptionKeyPrefs::Init(module_t* prefs)
     for (guint i = 0; i < sKeyCount; i++) {
         sKeyList[i].keyId = matter::MatterSessionId::kNone;
         sKeyList[i].sessionType = matter::kMatterEncryptionType_AES128CCM;
+        sKeyList[i].srcNodeId = 0;
     }
 }
diff --git a/UserNodeIdPrefs.cpp b/UserNodeIdPrefs.cpp
@@ -0,0 +1,112 @@
+/*
+ *  Copyright (c) 2023 Project CHIP Authors.
+ *
+ *  Use of this source code is governed by a BSD-style
+ *  license that can be found in the LICENSE file or at
+ *  https://opensource.org/license/bsd-3-clause
+ *
+ *  SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <Matter/Core/MatterCore.h>
+#include <Matter/Support/CodeUtils.h>
+#include <epan/packet.h>
+#include <epan/prefs.h>
+#include <epan/proto_data.h>
+#include <epan/uat.h>
+#include <glib.h>
+#include <stdint.h>
+
+#include "UserNodeIdPrefs.h"
+#include "config.h"
+#include "packet-matter.h"
+
+uat_t *UserNodeIdPrefs::sUAT = NULL;
+MessageNodeId *UserNodeIdPrefs::sNodeIdList = NULL;
+guint UserNodeIdPrefs::sNodeIdCount = 0;
+
+UAT_BUFFER_CB_DEF(MessageNodeId, nodeId, MessageNodeId, dataNodeId,
+                  dataNodeIdLen);
+
+static gboolean MessageNodeId_nodeId_check_cb(void *rec, const char *ptr,
+                                              unsigned len,
+                                              const void *chk_data,
+                                              const void *fld_data,
+                                              char **err) {
+  if (len != kNodeIdLength) {
+    *err = g_strdup("Invalid node id length");
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+static void *MessageNodeId_copy_cb(void *dest, const void *orig,
+                                   size_t len _U_) {
+  MessageNodeId *d = (MessageNodeId *)dest;
+  MessageNodeId *o = (MessageNodeId *)orig;
+
+  if (o->dataNodeIdLen) {
+    d->dataNodeId = (char *)g_memdup(o->dataNodeId, o->dataNodeIdLen);
+    d->dataNodeIdLen = o->dataNodeIdLen;
+    d->NodeId = ((uint64_t)(uint8_t)o->dataNodeId[0] << (7 * 8)) |
+                ((uint64_t)(uint8_t)o->dataNodeId[1] << (6 * 8)) |
+                ((uint64_t)(uint8_t)o->dataNodeId[2] << (5 * 8)) |
+                ((uint64_t)(uint8_t)o->dataNodeId[3] << (4 * 8)) |
+                ((uint64_t)(uint8_t)o->dataNodeId[4] << (3 * 8)) |
+                ((uint64_t)(uint8_t)o->dataNodeId[5] << (2 * 8)) |
+                ((uint64_t)(uint8_t)o->dataNodeId[6] << (1 * 8)) |
+                (uint64_t)(uint8_t)o->dataNodeId[7];
+  }
+  return dest;
+}
+
+static gboolean MessageNodeId_update_cb(void *rec _U_, char **err _U_) {
+  MessageNodeId *r = (MessageNodeId *)rec;
+
+  // r->NodeId = 0;
+
+  *err = NULL;
+
+  return TRUE;
+}
+
+static void MessageNodeId_free_cb(void *rec) {
+  MessageNodeId *r = (MessageNodeId *)rec;
+
+  g_free(r->dataNodeId);
+}
+
+static void MessageNodeId_post_update_cb(void) {
+  // No action required.
+}
+
+void UserNodeIdPrefs::Init(module_t *prefs) {
+#ifndef UAT_FLD_BUFFER_OTHER
+#define UAT_FLD_BUFFER_OTHER(basename, field_name, title, desc)                \
+  {                                                                            \
+    #field_name, title, PT_TXTMOD_HEXBYTES,                                    \
+        {basename##_##field_name##_check_cb, basename##_##field_name##_set_cb, \
+         basename##_##field_name##_tostr_cb},                                  \
+        {0, 0, 0}, 0, desc, FLDFILL                                            \
+  }
+#endif
+
+  static uat_field_t keyDataUATFields[] = {
+      UAT_FLD_BUFFER_OTHER(MessageNodeId, nodeId, "Node Id",
+                           "Matter message node id"),
+      UAT_END_FIELDS};
+
+  sUAT = uat_new("Message Node Ids", sizeof(MessageNodeId),
+                 "matter_message_node_ids", TRUE, &sNodeIdList, &sNodeIdCount,
+                 UAT_AFFECTS_DISSECTION, NULL, MessageNodeId_copy_cb,
+                 MessageNodeId_update_cb, MessageNodeId_free_cb,
+                 MessageNodeId_post_update_cb, NULL, keyDataUATFields);
+
+  prefs_register_uat_preference(prefs, "message_node_ids", "Message Node Ids",
+                                "A table of node ids for Matter messages",
+                                sUAT);
+
+  char *err;
+  uat_load(sUAT, nullptr, &err);
+}
diff --git a/UserNodeIdPrefs.h b/UserNodeIdPrefs.h
@@ -0,0 +1,46 @@
+/*
+ * @Author       : wilbur
+ * @Date         : 2024-03-29 13:17:12
+ * @LastEditTime : 2024-03-29 20:19:55
+ * @LastEditors  : wilbur
+ * @Description  : Please enter file description
+ * Copyright (c) 2024, All Rights Reserved.
+ */
+/*
+ *  Copyright (c) 2023 Project CHIP Authors.
+ *
+ *  Use of this source code is governed by a BSD-style
+ *  license that can be found in the LICENSE file or at
+ *  https://opensource.org/license/bsd-3-clause
+ *
+ *  SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef USERNODEIDPREFS_H_
+#define USERNODEIDPREFS_H_
+
+enum { kNodeIdLength = 8 };
+
+class MessageNodeId {
+ public:
+  char *dataNodeId;
+  guint dataNodeIdLen;
+  uint64_t NodeId;
+  MessageNodeId *nextId;
+};
+
+class UserNodeIdPrefs {
+ public:
+  static void Init(module_t *prefs);
+  static guint GetNodeIdCount() { return sNodeIdCount; }
+  static MessageNodeId *GetNodeId(size_t index) {
+    return (index < sNodeIdCount) ? &sNodeIdList[index] : 0;
+  }
+
+ private:
+  static uat_t *sUAT;
+  static MessageNodeId *sNodeIdList;
+  static guint sNodeIdCount;
+};
+
+#endif /* USERNODEIDPREFS_H_ */
diff --git a/packet-matter.cpp b/packet-matter.cpp
@@ -50,6 +50,7 @@
 #include "MessageEncryptionKey.h"
 #include "MatterMessageTracker.h"
 #include "UserEncryptionKeyPrefs.h"
+#include "UserNodeIdPrefs.h"
 
 using namespace matter;
 
@@ -151,6 +152,7 @@ TryDecryptMessage(tvbuff_t *tvb, int encDataOffset, packet_info *pinfo, const Ma
     bool success = false;
 
     if (MessageIsEncrypted(msgInfo)) {
+        MatterMessageInfo msgInfoCopy = msgInfo;
 
         dataLen = msgInfo.msgLen - encDataOffset;
         encData = (uint8_t *)tvb_memdup(pinfo->pool, tvb, encDataOffset, dataLen);
@@ -161,17 +163,35 @@ TryDecryptMessage(tvbuff_t *tvb, int encDataOffset, packet_info *pinfo, const Ma
         // successfully decrypts the message.
         const MessageEncryptionKey *keys = MessageEncryptionKeyTable::FindKeysById(msgInfo.sessionId);
         for (; keys != NULL && !success; keys = keys->nextKey) {
-            success = TryDecryptMessage_AES128CCM(encData, unencData, dataLen, aadData, encDataOffset, pinfo, msgInfo, *keys);
+            if (keys->srcNodeId && msgInfo.srcNodeId == 0) {
+                msgInfoCopy.srcNodeId = keys->srcNodeId;
+                success = TryDecryptMessage_AES128CCM(encData, unencData, dataLen, aadData, encDataOffset, pinfo, msgInfoCopy, *keys);
+            } else {
+                success = TryDecryptMessage_AES128CCM(encData, unencData, dataLen, aadData, encDataOffset, pinfo, msgInfo, *keys);
+            }
         }
 
         // If no matching key was found...
         if (!success) {
             MessageEncryptionKey userPrefKey;
+            MessageNodeId userPrefNodeId;
 
             // Search the keys in the user encryption key preferences table for a key that can decrypt the message.
             for (size_t i = 0; !success && i < UserEncryptionKeyPrefs::GetKeyCount(); i++) {
+                memset(&userPrefNodeId, 0, sizeof(userPrefNodeId));
                 userPrefKey = *UserEncryptionKeyPrefs::GetKey(i);
+                userPrefKey.srcNodeId = msgInfo.srcNodeId;
                 success = TryDecryptMessage_AES128CCM(encData, unencData, dataLen, aadData, encDataOffset, pinfo, msgInfo, userPrefKey);
+                if (!success && msgInfo.srcNodeId == 0) {
+                    for (size_t j = 0; !success && j < UserNodeIdPrefs::GetNodeIdCount(); j++) {
+                        userPrefNodeId = *UserNodeIdPrefs::GetNodeId(j);
+                        msgInfoCopy.srcNodeId = userPrefNodeId.NodeId;
+                        success = TryDecryptMessage_AES128CCM(encData, unencData, dataLen, aadData, encDataOffset, pinfo, msgInfoCopy, userPrefKey);
+                    }
+                    if (success) {
+                        userPrefKey.srcNodeId = userPrefNodeId.NodeId;
+                    }
+                }
             }
 
             // If a matching key was found, copy it to the main encryption key table, indexed by the key id.
@@ -729,6 +749,8 @@ proto_register_matter(void)
 
     UserEncryptionKeyPrefs::Init(prefs_matter);
 
+    UserNodeIdPrefs::Init(prefs_matter);
+
     MessageEncryptionKeyTable::Init();
 }