Implement Manual and QR payload processors

SmartHomeOS · Nov 20, 2024 · 1f7f472 · 1f7f472
1 parent 8186072
commit 1f7f472
Show file tree

Hide file tree

Showing 7 changed files with 302 additions and 20 deletions.
diff --git a/ExampleConsole/ExampleConsole.csproj b/ExampleConsole/ExampleConsole.csproj
@@ -7,4 +7,8 @@
     <Nullable>enable</Nullable>
   </PropertyGroup>
 
+  <ItemGroup>
+    <ProjectReference Include="..\MatterDotNet\MatterDotNet.csproj" />
+  </ItemGroup>
+
 </Project>
diff --git a/MatterDotNet.sln b/MatterDotNet.sln
@@ -1,12 +1,14 @@
 
 Microsoft Visual Studio Solution File, Format Version 12.00
 # Visual Studio Version 17
-VisualStudioVersion = 17.12.35506.116 d17.12
+VisualStudioVersion = 17.12.35506.116
 MinimumVisualStudioVersion = 10.0.40219.1
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MatterDotNet", "MatterDotNet\MatterDotNet.csproj", "{EA1A2183-F755-48C1-A431-29C280D5D493}"
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ExampleConsole", "ExampleConsole\ExampleConsole.csproj", "{FB0B84C4-A10C-4911-9D79-36134B311B07}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Test", "Test\Test.csproj", "{DDE2325B-62EF-460A-8A4B-6ED3311AEFAF}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -21,6 +23,10 @@ Global
 		{FB0B84C4-A10C-4911-9D79-36134B311B07}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{FB0B84C4-A10C-4911-9D79-36134B311B07}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{FB0B84C4-A10C-4911-9D79-36134B311B07}.Release|Any CPU.Build.0 = Release|Any CPU
+		{DDE2325B-62EF-460A-8A4B-6ED3311AEFAF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{DDE2325B-62EF-460A-8A4B-6ED3311AEFAF}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{DDE2325B-62EF-460A-8A4B-6ED3311AEFAF}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{DDE2325B-62EF-460A-8A4B-6ED3311AEFAF}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

diff --git a/MatterDotNet/PayloadParser.cs b/MatterDotNet/PayloadParser.cs
@@ -0,0 +1,161 @@
+// MatterDotNet Copyright (C) 2024 
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Affero General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or any later version.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY, without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+// See the GNU Affero General Public License for more details.
+// You should have received a copy of the GNU Affero General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+using MatterDotNet.Security;
+
+namespace MatterDotNet
+{
+    public class PayloadParser
+    {
+        [Flags]
+        public enum DiscoveryCapabilities
+        {
+            RESERVED = 0x1,
+            BLE = 0x2,
+            IP = 0x4,
+        }
+        public enum FlowType
+        {
+            STANDARD = 0,
+            USER_INTENT = 1,
+            CUSTOM = 2,
+            RESERVED = 3
+        }
+
+        public DiscoveryCapabilities Capabiilities { get; set; }
+        public FlowType Flow {  get; set; }
+        public ushort VendorID { get; set; }
+        public ushort ProductID { get; set; }
+        public ushort Discriminator {  get; set; }
+        public uint Passcode { get; set; }
+        public byte DiscriminatorLength { get; set; }
+
+        public override string ToString()
+        {
+            return $"Vendor: {VendorID}, Product: {ProductID}, Passcode: {Passcode}, Discriminator: {Discriminator:X}, Flow: {Flow}, Caps: {Capabiilities}";
+        }
+
+        private PayloadParser() { }
+
+        private PayloadParser(string QRCode)
+        {
+            byte[] data = Decode(QRCode.Substring(3));
+            uint version = readBits(data, 0, 3);
+
+            VendorID = (ushort)readBits(data, 3, 16);
+            ProductID = (ushort)readBits(data, 19, 16);
+
+            Flow = (FlowType)readBits(data, 35, 2);
+            Capabiilities = (DiscoveryCapabilities)readBits(data, 37, 8);
+            DiscriminatorLength = 12;
+            Discriminator = (ushort)readBits(data, 45, DiscriminatorLength);
+            Passcode = readBits(data, 57, 27);
+            uint padding = readBits(data, 84, 4);
+            bool success = padding == 0;
+        }
+
+        public static PayloadParser FromQR(string QRCode)
+        {
+            if (!QRCode.StartsWith("MT:"))
+                throw new ArgumentException("Invalid QR Code");
+           return new PayloadParser(QRCode);
+        }
+
+        public static PayloadParser FromPIN(string pin)
+        {
+            PayloadParser ret = new PayloadParser();
+            if (pin.Length != 11 && pin.Length != 21)
+                throw new ArgumentException("Invalid PIN");
+            int actualChecksum = int.Parse(pin.Substring(pin.Length == 11 ? 10 : 20, 1));
+            int computedChecksum = Checksum.GenerateVerhoeff(pin.Substring(0, 10));
+            if (actualChecksum != computedChecksum)
+                throw new ArgumentException("Pin Checksum Invalid: Should be " + computedChecksum);
+
+            byte leading = byte.Parse(pin.Substring(0, 1));
+            int version = ((leading & 0x8) == 0) ? 0 : 1;
+            bool vidpid = (leading & 0x4) == 0x4;
+            ret.Discriminator = (ushort)((leading & 0x3) << 2);
+            ushort group1 = ushort.Parse(pin.Substring(1, 5));
+            ret.Discriminator |= (ushort)(group1 >> 14);
+            ret.Passcode = (uint)(group1 & 0x3FFF);
+            ushort group2 = ushort.Parse(pin.Substring(6, 4));
+            ret.Passcode |= (uint)(group2 << 14);
+            ret.DiscriminatorLength = 4;
+            if (vidpid)
+            {
+                if (pin.Length != 21)
+                    throw new ArgumentException("Truncated PIN code");
+                ret.VendorID = ushort.Parse(pin.Substring(10, 5));
+                ret.ProductID = ushort.Parse(pin.Substring(15, 5));
+                ret.Flow = FlowType.CUSTOM;
+            }
+            else
+                ret.Flow = FlowType.STANDARD;
+            return ret;
+        }
+
+        private static byte[] Decode(string str)
+        {
+            List<byte> data = new List<byte>();
+            for (int i = 0; i < str.Length; i += 5)
+                data.AddRange(Unpack(str.Substring(i, Math.Min(5, str.Length - i))));
+            return data.ToArray();
+        }
+
+        private static byte[] Unpack(string str)
+        {
+            uint digit = DecodeBase38(str);
+            if (str.Length == 5)
+            {
+                byte[] result = new byte[3];
+                result[0] = (byte)digit;
+                result[1] = (byte)(digit >> 8);
+                result[2] = (byte)(digit >> 16);
+                return result;
+            }
+            else if (str.Length == 4)
+            {
+                return [(byte)digit, (byte)(digit >> 8)];
+            }
+            else if (str.Length == 2)
+            {
+                return [(byte)(digit & 0xFF)];
+            }
+            else
+                throw new ArgumentException("Invalid QR String");
+        }
+
+
+        static uint readBits(byte[] buf, int index, int numberOfBitsToRead)
+        {
+            uint dest = 0;
+
+            int currentIndex = index;
+            for (int bitsRead = 0; bitsRead < numberOfBitsToRead; bitsRead++)
+            {
+                if ((buf[currentIndex / 8] & (1 << (currentIndex % 8))) != 0)
+                    dest |= (uint)(1 << bitsRead);
+                currentIndex++;
+            }
+            return dest;
+        }
+
+        private static uint DecodeBase38(string sIn)
+        {
+            const string map = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ-.";
+            uint ret = 0;
+            for (int i = sIn.Length - 1; i >= 0; i--)
+                ret = (uint)(ret * 38 + map.IndexOf(sIn[i]));
+            return ret;
+        }
+    }
+}
diff --git a/MatterDotNet/QRParser.cs b/MatterDotNet/QRParser.cs
diff --git a/MatterDotNet/Security/Checksum.cs b/MatterDotNet/Security/Checksum.cs
@@ -0,0 +1,56 @@
+// MatterDotNet Copyright (C) 2024 
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Affero General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or any later version.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY, without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+// See the GNU Affero General Public License for more details.
+// You should have received a copy of the GNU Affero General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+namespace MatterDotNet.Security
+{
+    internal static class Checksum
+    {
+        private static int[,] d = new int[,]
+         {
+            {0, 1, 2, 3, 4, 5, 6, 7, 8, 9},
+            {1, 2, 3, 4, 0, 6, 7, 8, 9, 5},
+            {2, 3, 4, 0, 1, 7, 8, 9, 5, 6},
+            {3, 4, 0, 1, 2, 8, 9, 5, 6, 7},
+            {4, 0, 1, 2, 3, 9, 5, 6, 7, 8},
+            {5, 9, 8, 7, 6, 0, 4, 3, 2, 1},
+            {6, 5, 9, 8, 7, 1, 0, 4, 3, 2},
+            {7, 6, 5, 9, 8, 2, 1, 0, 4, 3},
+            {8, 7, 6, 5, 9, 3, 2, 1, 0, 4},
+            {9, 8, 7, 6, 5, 4, 3, 2, 1, 0}
+         };
+
+        private static int[,] p = new int[,]
+        {
+            {0, 1, 2, 3, 4, 5, 6, 7, 8, 9},
+            {1, 5, 7, 6, 2, 8, 3, 0, 9, 4},
+            {5, 8, 0, 3, 7, 9, 6, 1, 4, 2},
+            {8, 9, 1, 6, 0, 4, 3, 5, 2, 7},
+            {9, 4, 5, 3, 1, 2, 6, 8, 7, 0},
+            {4, 2, 8, 6, 5, 7, 3, 9, 0, 1},
+            {2, 7, 9, 3, 8, 0, 6, 4, 1, 5},
+            {7, 0, 4, 6, 9, 1, 3, 2, 5, 8}
+        };
+
+        private static int[] inv = { 0, 4, 3, 2, 1, 5, 6, 7, 8, 9 };
+
+        public static int GenerateVerhoeff(string num)
+        {
+            int ret = 0;
+            int[] myArray = num.ToCharArray().Select(x => int.Parse(x.ToString())).Reverse().ToArray();
+
+            for (int i = 0; i < myArray.Length; i++)
+                ret = d[ret, p[((i + 1) % 8), myArray[i]]];
+
+            return inv[ret];
+        }
+    }
+}
diff --git a/Test/PayloadParsing.cs b/Test/PayloadParsing.cs
@@ -0,0 +1,46 @@
+using MatterDotNet;
+
+namespace Test
+{
+    public class PayloadParsing
+    {
+
+        [Test]
+        public void PIN_AllOnes()
+        {
+            string PIN = "765535819165535655359";
+            PayloadParser parser = PayloadParser.FromPIN(PIN);
+            Assert.That(parser.Discriminator, Is.EqualTo(0xF));
+            Assert.That(parser.VendorID, Is.EqualTo(65535), "Invalid Vendor ID");
+            Assert.That(parser.ProductID, Is.EqualTo(65535), "Invalid Product ID");
+            Assert.That(parser.Passcode, Is.EqualTo(0x7FFFFFF), "Invalid Passcode");
+            Assert.That(parser.DiscriminatorLength, Is.EqualTo(4), "Invalid Discriminator Length");
+        }
+
+        [Test]
+        public void PIN_TestValues()
+        {
+            string PIN = "641295075300001000018";
+            PayloadParser parser = PayloadParser.FromPIN(PIN);
+            Assert.That(parser.Discriminator, Is.EqualTo(0xA));
+            Assert.That(parser.VendorID, Is.EqualTo(1), "Invalid Vendor ID");
+            Assert.That(parser.ProductID, Is.EqualTo(1), "Invalid Product ID");
+            Assert.That(parser.Passcode, Is.EqualTo(12345679), "Invalid Passcode");
+            Assert.That(parser.DiscriminatorLength, Is.EqualTo(4), "Invalid Discriminator Length");
+        }
+
+        [Test]
+        public void QR_Test()
+        {
+            string QR = "MT:Y.K9042C00KA0648G00";
+            PayloadParser parser = PayloadParser.FromQR(QR);
+            Assert.That(parser.Discriminator, Is.EqualTo(3840));
+            Assert.That(parser.VendorID, Is.EqualTo(0xfff1), "Invalid Vendor ID");
+            Assert.That(parser.ProductID, Is.EqualTo(0x8000), "Invalid Product ID");
+            Assert.That(parser.Passcode, Is.EqualTo(20202021), "Invalid Passcode");
+            Assert.That(parser.Capabiilities, Is.EqualTo(PayloadParser.DiscoveryCapabilities.BLE), "Invalid Capabilities");
+            Assert.That(parser.Flow, Is.EqualTo(PayloadParser.FlowType.STANDARD), "Invalid Capabilities");
+            Assert.That(parser.DiscriminatorLength, Is.EqualTo(12), "Invalid Discriminator Length");
+        }
+    }
+}
diff --git a/Test/Test.csproj b/Test/Test.csproj
@@ -0,0 +1,28 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net8.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+
+    <IsPackable>false</IsPackable>
+    <IsTestProject>true</IsTestProject>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="coverlet.collector" Version="6.0.0" />
+    <PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.8.0" />
+    <PackageReference Include="NUnit" Version="3.14.0" />
+    <PackageReference Include="NUnit.Analyzers" Version="3.9.0" />
+    <PackageReference Include="NUnit3TestAdapter" Version="4.5.0" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\MatterDotNet\MatterDotNet.csproj" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Using Include="NUnit.Framework" />
+  </ItemGroup>
+
+</Project>