Initial commit

merged from https://github.com/kedars/matter-rs

.github/workflows/build-tlv-tool.yml

@@ -0,0 +1,26 @@
+name: Build-TLV-Tool
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  CARGO_TERM_COLOR: always
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+    - name: Build
+      run: cd tools/tlv_tool; cargo build --verbose
+    - name: Archive artifacts
+      uses: actions/upload-artifact@v2
+      with:
+        name: tlv_tool
+        path: tools/tlv_tool/target/debug/tlv_tool
+

.github/workflows/test-linux-mbedtls.yml

@@ -0,0 +1,22 @@
+name: Test-Linux-mbedTLS
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  CARGO_TERM_COLOR: always
+
+jobs:
+  build_and_test:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+    - name: Build
+      run: cd matter; cargo build --verbose --no-default-features --features crypto_mbedtls
+    - name: Run tests
+      run: cd matter; cargo test --verbose --no-default-features --features crypto_mbedtls -- --test-threads=1

.github/workflows/test-linux-openssl.yml

@@ -0,0 +1,22 @@
+name: Test-Linux-OpenSSL
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  CARGO_TERM_COLOR: always
+
+jobs:
+  build_and_test:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+    - name: Build
+      run: cd matter; cargo build --verbose --no-default-features --features crypto_openssl
+    - name: Run tests
+      run: cd matter; cargo test --verbose --no-default-features --features crypto_openssl -- --test-threads=1

.gitignore

@@ -0,0 +1,3 @@
+target
+Cargo.lock
+.vscode

README.md

@@ -1,4 +1,58 @@
-# matter-rs: The Rust Implementaion of Matter
+
+# matter-rs: The Rust Implementation of Matter
 
 ![experimental](https://img.shields.io/badge/status-Experimental-red) [![license](https://img.shields.io/badge/license-Apache2-green.svg)](https://raw.githubusercontent.com/project-chip/matter-rs/main/LICENSE)
 
+
+
+[![Test Linux (OpenSSL)](https://github.com/project-chip/matter-rs/actions/workflows/test-linux-openssl.yml/badge.svg)](https://github.com/project-chip/matter-rs/actions/workflows/test-linux-openssl.yml)
+[![Test Linux (mbedTLS)](https://github.com/project-chip/matter-rs/actions/workflows/test-linux-mbedtls.yml/badge.svg)](https://github.com/project-chip/matter-rs/actions/workflows/test-linux-mbedtls.yml)
+
+## Build
+
+Building the library:
+```
+$ cd matter
+$ cargo build
+```
+
+Building the example:
+```
+$ cd matter
+$ RUST_LOG="matter" cargo run --example onoff_light
+```
+
+With the chip-tool (the current tool for testing Matter) use the Ethernet commissioning mechanism:
+```
+$ chip-tool pairing ethernet 12344321 123456 0 <IP-Address> 5540
+```
+
+Interact with the device
+```
+# Read server-list
+$ chip-tool descriptor read server-list 12344321 0
+
+# Read On/Off status
+$ chip-tool onoff read on-off 12344321 1
+
+# Toggle On/Off by invoking the command
+$ chip-tool onoff on 12344321 1
+```
+
+## Functionality
+- Secure Channel:
+  - PASE
+  - CASE
+- Interactions:
+  - Invoke Command(s), Read Attribute(s), Write Attribute(s)
+- Commissioning:
+  - over Ethernet
+  - Network Commissioning Cluster
+  - General Commissioning Cluster
+  - Operational Certificates Cluster
+- Some [TODO](TODO.md) are captured here
+
+## Notes
+
+The matter-rs project is a work-in-progress and does NOT yet fully implement Matter.
+

TODO.md

@@ -0,0 +1,49 @@
+* udp.c:
+  * Could get rid of 'smol' in here, no other processing is performed in this thread
+* TLVList:
+  * The 'Pointer' could be directly used in the TLVListIterator, makes it common
+  * Not too happy with the way iterator_consumer is done for ContainerIterator, we could just zip the internal ListIterator instead?
+  * Implement the IntoIterator Trait as well for the TLVElement. This was done earlier, but I backtracker after I ran into same lifetime issues
+* Some configurable values like number of exchanges per session, number of sessions supported etc, can be bubbled up to some configurator for this crate. I wonder how that is done.
+* About outgoing counter, is it incremented if we send mutliple acknowledgements to the same retransmitted packet? So let's say peer retransmits a packet with ctr 4, for 3 times. Our response ctr, is, say 20. Then should we respond with 20, 21, 22, or 20, 20, 20?
+* I had to use Box::new() to pin ownership for certain objects. Not yet able to use try_new() in the stable releases, and I am not a fan of APIs that panic. We should mostly look at things like heapless:pool or stuff. These objects should really be in the bss, with a single ownership.
+* It might be more efficient to avoid using .find_element() on TLVs. Earlier it was created this way because the spec mentions that the order may change, but it appears that this is unlikely, looking at the C++ implementation. If so, we could be faster, by just specifying looking for tag followed by value.
+* PASE:
+  - Pick some sensible and strong values for PBKDF2{iterCnt and Salt-length} based on SoC capability
+  - Verifier should only store w0 and L, w1 shouldn't even be stored 
+  - Allow some way to open the PASE window
+  - Allow some way to pass in the 'passcode' and 'salt'
+  - In case of error in any of the legs, return StatusReport
+  - Provide a way to delete the exchange
+  - SPAKE2+: the check with I (abort if `h*X == I`), as indicated by the RFC is pending
+
+* Implement the ARM Fail Safe and Regulatory Config properly. Currently we just ack them to proceed further
+* Currently AEAD, sha256 etc are directly used from rust crates. Instead use implementations from openssl/mbedtls - Done. Upstream MRs pending
+* rust-mbedTLS: We have to do some gymnastics because current APIs only support signature encoded in ASN1 format. Fix this upstream
+* CASE:
+  - Handle initial MRP Parameters struct from Sigma1
+* FailSafe:
+  - Enable timer and expiration handling for fail-safe context
+* Cert Verification:
+  - Time validation (Not Before/Not After)
+  - KeyUsage flags and others are pending
+* Transport Mgr:
+  - Add plain_encode and proto_encode in Packet
+  - A new proto_tx should be created in the acks_to_send loop also, otherwise, there is a potential chance of reuse
+  - 'transport' object's ownership needs to be inside session, or in the least 'exchange'
+  - Sending 'close session' is pending on session reclamation because 'transport' object isn't owned
+  - Convert the SessionHandle to &Session? Why maintain a separate object for this?
+* Exchange:
+  - What should happen when an exchange is closed by the higher layer, our tx-retrans is pending, and we got a retrans for that exchange?
+* ACL:
+  - Device-Type based ACLs
+  - NOC CAT
+  - Applying ACLs to commands (requires some restructuring of the commands)
+  - I think we can the encoder to AccessReq Object making it a complete object for access within the DM
+  - List processing of attribute write is missing in IM. List behaviour is add/edit/delete. Currently we only do 'add'
+* Interaction Model
+  - List processing of write attributes is different (delete, modify, edit), needs to be handled
+* DataModel:
+  - Shall we use a CmdEncoder as a parameter for all the handle_commands()?
+  - Need to define common data types for cluster_id_t, endpoint_id_t so their sizes are constantly defined somewhere
+ 

boxslab/Cargo.toml

@@ -0,0 +1,9 @@
+[package]
+name = "boxslab"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+bitmaps={version="3.2.0", features=[]}

boxslab/src/lib.rs

@@ -0,0 +1,241 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+
+use std::{
+    mem::MaybeUninit,
+    ops::{Deref, DerefMut},
+    sync::Mutex,
+};
+
+// TODO: why is max bitmap size 64 a correct max size? Could we match
+// boxslabs instead or store used/not used inside the box slabs themselves?
+const MAX_BITMAP_SIZE: usize = 64;
+pub struct Bitmap {
+    inner: bitmaps::Bitmap<MAX_BITMAP_SIZE>,
+    max_size: usize,
+}
+
+impl Bitmap {
+    pub fn new(max_size: usize) -> Self {
+        assert!(max_size <= MAX_BITMAP_SIZE);
+        Bitmap {
+            inner: bitmaps::Bitmap::new(),
+            max_size,
+        }
+    }
+
+    pub fn set(&mut self, index: usize) -> bool {
+        assert!(index < self.max_size);
+        self.inner.set(index, true)
+    }
+
+    pub fn reset(&mut self, index: usize) -> bool {
+        assert!(index < self.max_size);
+        self.inner.set(index, false)
+    }
+
+    pub fn first_false_index(&self) -> Option<usize> {
+        match self.inner.first_false_index() {
+            Some(idx) if idx < self.max_size => Some(idx),
+            _ => None,
+        }
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.inner.is_empty()
+    }
+
+    pub fn is_full(&self) -> bool {
+        self.first_false_index().is_none()
+    }
+}
+
+#[macro_export]
+macro_rules! box_slab {
+    ($name:ident,$t:ty,$v:expr) => {
+        use std::mem::MaybeUninit;
+        use std::sync::Once;
+        use $crate::{BoxSlab, Slab, SlabPool};
+
+        pub struct $name;
+
+        impl SlabPool for $name {
+            type SlabType = $t;
+            fn get_slab() -> &'static Slab<Self> {
+                const MAYBE_INIT: MaybeUninit<$t> = MaybeUninit::uninit();
+                static mut SLAB_POOL: [MaybeUninit<$t>; $v] = [MAYBE_INIT; $v];
+                static mut SLAB_SPACE: Option<Slab<$name>> = None;
+                static mut INIT: Once = Once::new();
+                unsafe {
+                    INIT.call_once(|| {
+                        SLAB_SPACE = Some(Slab::<$name>::init(&mut SLAB_POOL, $v));
+                    });
+                    SLAB_SPACE.as_ref().unwrap()
+                }
+            }
+        }
+    };
+}
+
+pub trait SlabPool {
+    type SlabType: 'static;
+    fn get_slab() -> &'static Slab<Self>
+    where
+        Self: Sized;
+}
+
+pub struct Inner<T: 'static + SlabPool> {
+    pool: &'static mut [MaybeUninit<T::SlabType>],
+    map: Bitmap,
+}
+
+// TODO: Instead of a mutex, we should replace this with a CAS loop
+pub struct Slab<T: 'static + SlabPool>(Mutex<Inner<T>>);
+
+impl<T: SlabPool> Slab<T> {
+    pub fn init(pool: &'static mut [MaybeUninit<T::SlabType>], size: usize) -> Self {
+        Self(Mutex::new(Inner {
+            pool,
+            map: Bitmap::new(size),
+        }))
+    }
+
+    pub fn new(new_object: T::SlabType) -> Option<BoxSlab<T>> {
+        let slab = T::get_slab();
+        let mut inner = slab.0.lock().unwrap();
+        if let Some(index) = inner.map.first_false_index() {
+            inner.map.set(index);
+            inner.pool[index].write(new_object);
+            let cell_ptr = unsafe { &mut *inner.pool[index].as_mut_ptr() };
+            Some(BoxSlab {
+                data: cell_ptr,
+                index,
+            })
+        } else {
+            None
+        }
+    }
+
+    pub fn free(&self, index: usize) {
+        let mut inner = self.0.lock().unwrap();
+        inner.map.reset(index);
+        let old_value = std::mem::replace(&mut inner.pool[index], MaybeUninit::uninit());
+        let _old_value = unsafe { old_value.assume_init() };
+        // This will drop the old_value
+    }
+}
+
+pub struct BoxSlab<T: 'static + SlabPool> {
+    // Because the data is a reference within the MaybeUninit, we don't have a mechanism
+    // to go out to the MaybeUninit from this reference. Hence this index
+    index: usize,
+    // TODO: We should figure out a way to get rid of the index too
+    data: &'static mut T::SlabType,
+}
+
+impl<T: 'static + SlabPool> Drop for BoxSlab<T> {
+    fn drop(&mut self) {
+        T::get_slab().free(self.index);
+    }
+}
+
+impl<T: SlabPool> Deref for BoxSlab<T> {
+    type Target = T::SlabType;
+    fn deref(&self) -> &Self::Target {
+        self.data
+    }
+}
+
+impl<T: SlabPool> DerefMut for BoxSlab<T> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        self.data
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use std::{ops::Deref, sync::Arc};
+
+    pub struct Test {
+        val: Arc<u32>,
+    }
+
+    box_slab!(TestSlab, Test, 3);
+
+    #[test]
+    fn simple_alloc_free() {
+        {
+            let a = Slab::<TestSlab>::new(Test { val: Arc::new(10) }).unwrap();
+            assert_eq!(*a.val.deref(), 10);
+            let inner = TestSlab::get_slab().0.lock().unwrap();
+            assert_eq!(inner.map.is_empty(), false);
+        }
+        // Validates that the 'Drop' got executed
+        let inner = TestSlab::get_slab().0.lock().unwrap();
+        assert_eq!(inner.map.is_empty(), true);
+        println!("Box Size {}", std::mem::size_of::<Box<Test>>());
+        println!("BoxSlab Size {}", std::mem::size_of::<BoxSlab<TestSlab>>());
+    }
+
+    #[test]
+    fn alloc_full_block() {
+        {
+            let a = Slab::<TestSlab>::new(Test { val: Arc::new(10) }).unwrap();
+            let b = Slab::<TestSlab>::new(Test { val: Arc::new(11) }).unwrap();
+            let c = Slab::<TestSlab>::new(Test { val: Arc::new(12) }).unwrap();
+            // Test that at overflow, we return None
+            assert_eq!(
+                Slab::<TestSlab>::new(Test { val: Arc::new(13) }).is_none(),
+                true
+            );
+            assert_eq!(*b.val.deref(), 11);
+
+            {
+                let inner = TestSlab::get_slab().0.lock().unwrap();
+                // Test that the bitmap is marked as full
+                assert_eq!(inner.map.is_full(), true);
+            }
+
+            // Purposefully drop, to test that new allocation is possible
+            std::mem::drop(b);
+            let d = Slab::<TestSlab>::new(Test { val: Arc::new(21) }).unwrap();
+            assert_eq!(*d.val.deref(), 21);
+
+            // Ensure older allocations are still valid
+            assert_eq!(*a.val.deref(), 10);
+            assert_eq!(*c.val.deref(), 12);
+        }
+
+        // Validates that the 'Drop' got executed - test that the bitmap is empty
+        let inner = TestSlab::get_slab().0.lock().unwrap();
+        assert_eq!(inner.map.is_empty(), true);
+    }
+
+    #[test]
+    fn test_drop_logic() {
+        let root = Arc::new(10);
+        {
+            let _a = Slab::<TestSlab>::new(Test { val: root.clone() }).unwrap();
+            let _b = Slab::<TestSlab>::new(Test { val: root.clone() }).unwrap();
+            let _c = Slab::<TestSlab>::new(Test { val: root.clone() }).unwrap();
+            assert_eq!(Arc::strong_count(&root), 4);
+        }
+        // Test that Drop was correctly called on all the members of the pool
+        assert_eq!(Arc::strong_count(&root), 1);
+    }
+}

examples/onoff_light/src/dev_att.rs

@@ -0,0 +1,165 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use matter::data_model::sdm::dev_att::{DataType, DevAttDataFetcher};
+use matter::error::Error;
+
+pub struct HardCodedDevAtt {}
+
+impl HardCodedDevAtt {
+    pub fn new() -> Self {
+        Self {}
+    }
+}
+
+// credentials/examples/ExamplePAI.cpp FFF1
+const PAI_CERT: [u8; 463] = [
+    0x30, 0x82, 0x01, 0xcb, 0x30, 0x82, 0x01, 0x71, 0xa0, 0x03, 0x02, 0x01, 0x02, 0x02, 0x08, 0x56,
+    0xad, 0x82, 0x22, 0xad, 0x94, 0x5b, 0x64, 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d,
+    0x04, 0x03, 0x02, 0x30, 0x30, 0x31, 0x18, 0x30, 0x16, 0x06, 0x03, 0x55, 0x04, 0x03, 0x0c, 0x0f,
+    0x4d, 0x61, 0x74, 0x74, 0x65, 0x72, 0x20, 0x54, 0x65, 0x73, 0x74, 0x20, 0x50, 0x41, 0x41, 0x31,
+    0x14, 0x30, 0x12, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x02, 0x01, 0x0c,
+    0x04, 0x46, 0x46, 0x46, 0x31, 0x30, 0x20, 0x17, 0x0d, 0x32, 0x32, 0x30, 0x32, 0x30, 0x35, 0x30,
+    0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x18, 0x0f, 0x39, 0x39, 0x39, 0x39, 0x31, 0x32, 0x33, 0x31,
+    0x32, 0x33, 0x35, 0x39, 0x35, 0x39, 0x5a, 0x30, 0x3d, 0x31, 0x25, 0x30, 0x23, 0x06, 0x03, 0x55,
+    0x04, 0x03, 0x0c, 0x1c, 0x4d, 0x61, 0x74, 0x74, 0x65, 0x72, 0x20, 0x44, 0x65, 0x76, 0x20, 0x50,
+    0x41, 0x49, 0x20, 0x30, 0x78, 0x46, 0x46, 0x46, 0x31, 0x20, 0x6e, 0x6f, 0x20, 0x50, 0x49, 0x44,
+    0x31, 0x14, 0x30, 0x12, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x02, 0x01,
+    0x0c, 0x04, 0x46, 0x46, 0x46, 0x31, 0x30, 0x59, 0x30, 0x13, 0x06, 0x07, 0x2a, 0x86, 0x48, 0xce,
+    0x3d, 0x02, 0x01, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07, 0x03, 0x42, 0x00,
+    0x04, 0x41, 0x9a, 0x93, 0x15, 0xc2, 0x17, 0x3e, 0x0c, 0x8c, 0x87, 0x6d, 0x03, 0xcc, 0xfc, 0x94,
+    0x48, 0x52, 0x64, 0x7f, 0x7f, 0xec, 0x5e, 0x50, 0x82, 0xf4, 0x05, 0x99, 0x28, 0xec, 0xa8, 0x94,
+    0xc5, 0x94, 0x15, 0x13, 0x09, 0xac, 0x63, 0x1e, 0x4c, 0xb0, 0x33, 0x92, 0xaf, 0x68, 0x4b, 0x0b,
+    0xaf, 0xb7, 0xe6, 0x5b, 0x3b, 0x81, 0x62, 0xc2, 0xf5, 0x2b, 0xf9, 0x31, 0xb8, 0xe7, 0x7a, 0xaa,
+    0x82, 0xa3, 0x66, 0x30, 0x64, 0x30, 0x12, 0x06, 0x03, 0x55, 0x1d, 0x13, 0x01, 0x01, 0xff, 0x04,
+    0x08, 0x30, 0x06, 0x01, 0x01, 0xff, 0x02, 0x01, 0x00, 0x30, 0x0e, 0x06, 0x03, 0x55, 0x1d, 0x0f,
+    0x01, 0x01, 0xff, 0x04, 0x04, 0x03, 0x02, 0x01, 0x06, 0x30, 0x1d, 0x06, 0x03, 0x55, 0x1d, 0x0e,
+    0x04, 0x16, 0x04, 0x14, 0x63, 0x54, 0x0e, 0x47, 0xf6, 0x4b, 0x1c, 0x38, 0xd1, 0x38, 0x84, 0xa4,
+    0x62, 0xd1, 0x6c, 0x19, 0x5d, 0x8f, 0xfb, 0x3c, 0x30, 0x1f, 0x06, 0x03, 0x55, 0x1d, 0x23, 0x04,
+    0x18, 0x30, 0x16, 0x80, 0x14, 0x6a, 0xfd, 0x22, 0x77, 0x1f, 0x51, 0x1f, 0xec, 0xbf, 0x16, 0x41,
+    0x97, 0x67, 0x10, 0xdc, 0xdc, 0x31, 0xa1, 0x71, 0x7e, 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48,
+    0xce, 0x3d, 0x04, 0x03, 0x02, 0x03, 0x48, 0x00, 0x30, 0x45, 0x02, 0x21, 0x00, 0xb2, 0xef, 0x27,
+    0xf4, 0x9a, 0xe9, 0xb5, 0x0f, 0xb9, 0x1e, 0xea, 0xc9, 0x4c, 0x4d, 0x0b, 0xdb, 0xb8, 0xd7, 0x92,
+    0x9c, 0x6c, 0xb8, 0x8f, 0xac, 0xe5, 0x29, 0x36, 0x8d, 0x12, 0x05, 0x4c, 0x0c, 0x02, 0x20, 0x65,
+    0x5d, 0xc9, 0x2b, 0x86, 0xbd, 0x90, 0x98, 0x82, 0xa6, 0xc6, 0x21, 0x77, 0xb8, 0x25, 0xd7, 0xd0,
+    0x5e, 0xdb, 0xe7, 0xc2, 0x2f, 0x9f, 0xea, 0x71, 0x22, 0x0e, 0x7e, 0xa7, 0x03, 0xf8, 0x91,
+];
+
+// credentials/examples/ExampleDACs.cpp FFF1-8000-0002-Cert
+const DAC_CERT: [u8; 492] = [
+    0x30, 0x82, 0x01, 0xe8, 0x30, 0x82, 0x01, 0x8e, 0xa0, 0x03, 0x02, 0x01, 0x02, 0x02, 0x08, 0x52,
+    0x72, 0x4d, 0x21, 0xe2, 0xc1, 0x74, 0xaf, 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d,
+    0x04, 0x03, 0x02, 0x30, 0x3d, 0x31, 0x25, 0x30, 0x23, 0x06, 0x03, 0x55, 0x04, 0x03, 0x0c, 0x1c,
+    0x4d, 0x61, 0x74, 0x74, 0x65, 0x72, 0x20, 0x44, 0x65, 0x76, 0x20, 0x50, 0x41, 0x49, 0x20, 0x30,
+    0x78, 0x46, 0x46, 0x46, 0x31, 0x20, 0x6e, 0x6f, 0x20, 0x50, 0x49, 0x44, 0x31, 0x14, 0x30, 0x12,
+    0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x02, 0x01, 0x0c, 0x04, 0x46, 0x46,
+    0x46, 0x31, 0x30, 0x20, 0x17, 0x0d, 0x32, 0x32, 0x30, 0x32, 0x30, 0x35, 0x30, 0x30, 0x30, 0x30,
+    0x30, 0x30, 0x5a, 0x18, 0x0f, 0x39, 0x39, 0x39, 0x39, 0x31, 0x32, 0x33, 0x31, 0x32, 0x33, 0x35,
+    0x39, 0x35, 0x39, 0x5a, 0x30, 0x53, 0x31, 0x25, 0x30, 0x23, 0x06, 0x03, 0x55, 0x04, 0x03, 0x0c,
+    0x1c, 0x4d, 0x61, 0x74, 0x74, 0x65, 0x72, 0x20, 0x44, 0x65, 0x76, 0x20, 0x44, 0x41, 0x43, 0x20,
+    0x30, 0x78, 0x46, 0x46, 0x46, 0x31, 0x2f, 0x30, 0x78, 0x38, 0x30, 0x30, 0x32, 0x31, 0x14, 0x30,
+    0x12, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x02, 0x01, 0x0c, 0x04, 0x46,
+    0x46, 0x46, 0x31, 0x31, 0x14, 0x30, 0x12, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2,
+    0x7c, 0x02, 0x02, 0x0c, 0x04, 0x38, 0x30, 0x30, 0x32, 0x30, 0x59, 0x30, 0x13, 0x06, 0x07, 0x2a,
+    0x86, 0x48, 0xce, 0x3d, 0x02, 0x01, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07,
+    0x03, 0x42, 0x00, 0x04, 0xda, 0x93, 0xf1, 0x67, 0x36, 0x25, 0x67, 0x50, 0xd9, 0x03, 0xb0, 0x34,
+    0xba, 0x45, 0x88, 0xab, 0xaf, 0x58, 0x95, 0x4f, 0x77, 0xaa, 0x9f, 0xd9, 0x98, 0x9d, 0xfd, 0x40,
+    0x0d, 0x7a, 0xb3, 0xfd, 0xc9, 0x75, 0x3b, 0x3b, 0x92, 0x1b, 0x29, 0x4c, 0x95, 0x0f, 0xd9, 0xd2,
+    0x80, 0xd1, 0x4c, 0x43, 0x86, 0x2f, 0x16, 0xdc, 0x85, 0x4b, 0x00, 0xed, 0x39, 0xe7, 0x50, 0xba,
+    0xbf, 0x1d, 0xc4, 0xca, 0xa3, 0x60, 0x30, 0x5e, 0x30, 0x0c, 0x06, 0x03, 0x55, 0x1d, 0x13, 0x01,
+    0x01, 0xff, 0x04, 0x02, 0x30, 0x00, 0x30, 0x0e, 0x06, 0x03, 0x55, 0x1d, 0x0f, 0x01, 0x01, 0xff,
+    0x04, 0x04, 0x03, 0x02, 0x07, 0x80, 0x30, 0x1d, 0x06, 0x03, 0x55, 0x1d, 0x0e, 0x04, 0x16, 0x04,
+    0x14, 0xef, 0x06, 0x56, 0x11, 0x9c, 0x1c, 0x91, 0xa7, 0x9a, 0x94, 0xe6, 0xdc, 0xf3, 0x79, 0x79,
+    0xdb, 0xd0, 0x7f, 0xf8, 0xa3, 0x30, 0x1f, 0x06, 0x03, 0x55, 0x1d, 0x23, 0x04, 0x18, 0x30, 0x16,
+    0x80, 0x14, 0x63, 0x54, 0x0e, 0x47, 0xf6, 0x4b, 0x1c, 0x38, 0xd1, 0x38, 0x84, 0xa4, 0x62, 0xd1,
+    0x6c, 0x19, 0x5d, 0x8f, 0xfb, 0x3c, 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x04,
+    0x03, 0x02, 0x03, 0x48, 0x00, 0x30, 0x45, 0x02, 0x20, 0x46, 0x86, 0x81, 0x07, 0x33, 0xbf, 0x0d,
+    0xc8, 0xff, 0x4c, 0xb5, 0x14, 0x5a, 0x6b, 0xfa, 0x1a, 0xec, 0xff, 0xa8, 0xb6, 0xda, 0xb6, 0xc3,
+    0x51, 0xaa, 0xee, 0xcd, 0xaf, 0xb8, 0xbe, 0x95, 0x7d, 0x02, 0x21, 0x00, 0xe8, 0xc2, 0x8d, 0x6b,
+    0xfc, 0xc8, 0x7a, 0x7d, 0x54, 0x2e, 0xad, 0x6e, 0xda, 0xca, 0x14, 0x8d, 0x5f, 0xa5, 0x06, 0x1e,
+    0x51, 0x7c, 0xbe, 0x4f, 0x24, 0xa7, 0x20, 0xe1, 0xc0, 0x59, 0xde, 0x1a,
+];
+
+const DAC_PUBKEY: [u8; 65] = [
+    0x04, 0xda, 0x93, 0xf1, 0x67, 0x36, 0x25, 0x67, 0x50, 0xd9, 0x03, 0xb0, 0x34, 0xba, 0x45, 0x88,
+    0xab, 0xaf, 0x58, 0x95, 0x4f, 0x77, 0xaa, 0x9f, 0xd9, 0x98, 0x9d, 0xfd, 0x40, 0x0d, 0x7a, 0xb3,
+    0xfd, 0xc9, 0x75, 0x3b, 0x3b, 0x92, 0x1b, 0x29, 0x4c, 0x95, 0x0f, 0xd9, 0xd2, 0x80, 0xd1, 0x4c,
+    0x43, 0x86, 0x2f, 0x16, 0xdc, 0x85, 0x4b, 0x00, 0xed, 0x39, 0xe7, 0x50, 0xba, 0xbf, 0x1d, 0xc4,
+    0xca,
+];
+
+const DAC_PRIVKEY: [u8; 32] = [
+    0xda, 0xf2, 0x1a, 0x7e, 0xa4, 0x7a, 0x70, 0x48, 0x02, 0xa7, 0xe6, 0x6c, 0x50, 0xeb, 0x10, 0xba,
+    0xc3, 0xbd, 0xd1, 0x68, 0x80, 0x39, 0x80, 0x66, 0xff, 0xda, 0xd7, 0xf5, 0x20, 0x98, 0xb6, 0x85,
+];
+
+//
+const CERT_DECLARATION: [u8; 541] = [
+    0x30, 0x82, 0x02, 0x19, 0x06, 0x09, 0x2a, 0x86, 0x48, 0x86, 0xf7, 0x0d, 0x01, 0x07, 0x02, 0xa0,
+    0x82, 0x02, 0x0a, 0x30, 0x82, 0x02, 0x06, 0x02, 0x01, 0x03, 0x31, 0x0d, 0x30, 0x0b, 0x06, 0x09,
+    0x60, 0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x30, 0x82, 0x01, 0x71, 0x06, 0x09, 0x2a,
+    0x86, 0x48, 0x86, 0xf7, 0x0d, 0x01, 0x07, 0x01, 0xa0, 0x82, 0x01, 0x62, 0x04, 0x82, 0x01, 0x5e,
+    0x15, 0x24, 0x00, 0x01, 0x25, 0x01, 0xf1, 0xff, 0x36, 0x02, 0x05, 0x00, 0x80, 0x05, 0x01, 0x80,
+    0x05, 0x02, 0x80, 0x05, 0x03, 0x80, 0x05, 0x04, 0x80, 0x05, 0x05, 0x80, 0x05, 0x06, 0x80, 0x05,
+    0x07, 0x80, 0x05, 0x08, 0x80, 0x05, 0x09, 0x80, 0x05, 0x0a, 0x80, 0x05, 0x0b, 0x80, 0x05, 0x0c,
+    0x80, 0x05, 0x0d, 0x80, 0x05, 0x0e, 0x80, 0x05, 0x0f, 0x80, 0x05, 0x10, 0x80, 0x05, 0x11, 0x80,
+    0x05, 0x12, 0x80, 0x05, 0x13, 0x80, 0x05, 0x14, 0x80, 0x05, 0x15, 0x80, 0x05, 0x16, 0x80, 0x05,
+    0x17, 0x80, 0x05, 0x18, 0x80, 0x05, 0x19, 0x80, 0x05, 0x1a, 0x80, 0x05, 0x1b, 0x80, 0x05, 0x1c,
+    0x80, 0x05, 0x1d, 0x80, 0x05, 0x1e, 0x80, 0x05, 0x1f, 0x80, 0x05, 0x20, 0x80, 0x05, 0x21, 0x80,
+    0x05, 0x22, 0x80, 0x05, 0x23, 0x80, 0x05, 0x24, 0x80, 0x05, 0x25, 0x80, 0x05, 0x26, 0x80, 0x05,
+    0x27, 0x80, 0x05, 0x28, 0x80, 0x05, 0x29, 0x80, 0x05, 0x2a, 0x80, 0x05, 0x2b, 0x80, 0x05, 0x2c,
+    0x80, 0x05, 0x2d, 0x80, 0x05, 0x2e, 0x80, 0x05, 0x2f, 0x80, 0x05, 0x30, 0x80, 0x05, 0x31, 0x80,
+    0x05, 0x32, 0x80, 0x05, 0x33, 0x80, 0x05, 0x34, 0x80, 0x05, 0x35, 0x80, 0x05, 0x36, 0x80, 0x05,
+    0x37, 0x80, 0x05, 0x38, 0x80, 0x05, 0x39, 0x80, 0x05, 0x3a, 0x80, 0x05, 0x3b, 0x80, 0x05, 0x3c,
+    0x80, 0x05, 0x3d, 0x80, 0x05, 0x3e, 0x80, 0x05, 0x3f, 0x80, 0x05, 0x40, 0x80, 0x05, 0x41, 0x80,
+    0x05, 0x42, 0x80, 0x05, 0x43, 0x80, 0x05, 0x44, 0x80, 0x05, 0x45, 0x80, 0x05, 0x46, 0x80, 0x05,
+    0x47, 0x80, 0x05, 0x48, 0x80, 0x05, 0x49, 0x80, 0x05, 0x4a, 0x80, 0x05, 0x4b, 0x80, 0x05, 0x4c,
+    0x80, 0x05, 0x4d, 0x80, 0x05, 0x4e, 0x80, 0x05, 0x4f, 0x80, 0x05, 0x50, 0x80, 0x05, 0x51, 0x80,
+    0x05, 0x52, 0x80, 0x05, 0x53, 0x80, 0x05, 0x54, 0x80, 0x05, 0x55, 0x80, 0x05, 0x56, 0x80, 0x05,
+    0x57, 0x80, 0x05, 0x58, 0x80, 0x05, 0x59, 0x80, 0x05, 0x5a, 0x80, 0x05, 0x5b, 0x80, 0x05, 0x5c,
+    0x80, 0x05, 0x5d, 0x80, 0x05, 0x5e, 0x80, 0x05, 0x5f, 0x80, 0x05, 0x60, 0x80, 0x05, 0x61, 0x80,
+    0x05, 0x62, 0x80, 0x05, 0x63, 0x80, 0x18, 0x24, 0x03, 0x16, 0x2c, 0x04, 0x13, 0x5a, 0x49, 0x47,
+    0x32, 0x30, 0x31, 0x34, 0x32, 0x5a, 0x42, 0x33, 0x33, 0x30, 0x30, 0x30, 0x33, 0x2d, 0x32, 0x34,
+    0x24, 0x05, 0x00, 0x24, 0x06, 0x00, 0x25, 0x07, 0x94, 0x26, 0x24, 0x08, 0x00, 0x18, 0x31, 0x7d,
+    0x30, 0x7b, 0x02, 0x01, 0x03, 0x80, 0x14, 0x62, 0xfa, 0x82, 0x33, 0x59, 0xac, 0xfa, 0xa9, 0x96,
+    0x3e, 0x1c, 0xfa, 0x14, 0x0a, 0xdd, 0xf5, 0x04, 0xf3, 0x71, 0x60, 0x30, 0x0b, 0x06, 0x09, 0x60,
+    0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce,
+    0x3d, 0x04, 0x03, 0x02, 0x04, 0x47, 0x30, 0x45, 0x02, 0x20, 0x24, 0xe5, 0xd1, 0xf4, 0x7a, 0x7d,
+    0x7b, 0x0d, 0x20, 0x6a, 0x26, 0xef, 0x69, 0x9b, 0x7c, 0x97, 0x57, 0xb7, 0x2d, 0x46, 0x90, 0x89,
+    0xde, 0x31, 0x92, 0xe6, 0x78, 0xc7, 0x45, 0xe7, 0xf6, 0x0c, 0x02, 0x21, 0x00, 0xf8, 0xaa, 0x2f,
+    0xa7, 0x11, 0xfc, 0xb7, 0x9b, 0x97, 0xe3, 0x97, 0xce, 0xda, 0x66, 0x7b, 0xae, 0x46, 0x4e, 0x2b,
+    0xd3, 0xff, 0xdf, 0xc3, 0xcc, 0xed, 0x7a, 0xa8, 0xca, 0x5f, 0x4c, 0x1a, 0x7c,
+];
+
+impl DevAttDataFetcher for HardCodedDevAtt {
+    fn get_devatt_data(&self, data_type: DataType, data: &mut [u8]) -> Result<usize, Error> {
+        let src = match data_type {
+            DataType::CertDeclaration => &CERT_DECLARATION[..],
+            DataType::PAI => &PAI_CERT[..],
+            DataType::DAC => &DAC_CERT[..],
+            DataType::DACPubKey => &DAC_PUBKEY[..],
+            DataType::DACPrivKey => &DAC_PRIVKEY[..],
+        };
+        if src.len() <= data.len() {
+            let data = &mut data[0..src.len()];
+            data.copy_from_slice(src);
+            Ok(src.len())
+        } else {
+            Err(Error::NoSpace)
+        }
+    }
+}

examples/onoff_light/src/lib.rs

@@ -0,0 +1,18 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod dev_att;

examples/onoff_light/src/main.rs

@@ -0,0 +1,53 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+mod dev_att;
+use matter::core::{self, CommissioningData};
+use matter::data_model::cluster_basic_information::BasicInfoConfig;
+use matter::data_model::device_types::device_type_add_on_off_light;
+use rand::prelude::*;
+
+fn main() {
+    env_logger::init();
+    let mut comm_data = CommissioningData {
+        // TODO: Hard-coded for now
+        passwd: 123456,
+        discriminator: 250,
+        ..Default::default()
+    };
+    rand::thread_rng().fill_bytes(&mut comm_data.salt);
+
+    // vid/pid should match those in the DAC
+    let dev_info = BasicInfoConfig {
+        vid: 0xFFF1,
+        pid: 0x8002,
+        hw_ver: 2,
+        sw_ver: 1,
+    };
+    let dev_att = Box::new(dev_att::HardCodedDevAtt::new());
+
+    let mut matter = core::Matter::new(dev_info, dev_att, comm_data).unwrap();
+    let dm = matter.get_data_model();
+    {
+        let mut node = dm.node.write().unwrap();
+        let endpoint = device_type_add_on_off_light(&mut node).unwrap();
+        println!("Added OnOff Light Device type at endpoint id: {}", endpoint);
+        println!("Data Model now is: {}", node);
+    }
+
+    matter.start_daemon().unwrap();
+}

matter/Cargo.toml

@@ -0,0 +1,55 @@
+[package]
+name = "matter-iot"
+version = "0.1.0"
+edition = "2018"
+authors = ["Kedar Sovani <kedars@gmail.com>"]
+description = "Native RUST implementation of the Matter (Smart-Home) ecosystem"
+repository = "https://github.com/kedars/matter-rs"
+readme = "README.md"
+keywords = ["matter", "smart", "smart-home", "IoT", "ESP32"]
+categories = ["embedded", "network-programming"]
+license = "MIT"
+
+[lib]
+name = "matter"
+path = "src/lib.rs"
+
+[features]
+default = ["crypto_mbedtls"]
+crypto_openssl = ["openssl", "foreign-types", "hmac", "sha2"]
+crypto_mbedtls = ["mbedtls"]
+crypto_esp_mbedtls = ["esp-idf-sys"]
+
+[dependencies]
+boxslab = { path = "../boxslab"}
+matter_macro_derive = { path = "../matter_macro_derive"}
+bitflags = "1.3"
+byteorder = "1.4.3"
+heapless = {version = "0.7.7", features = ["x86-sync-pool"] }
+generic-array = "0.14.5"
+num = "0.3"
+num-derive = "0.3.3"
+num-traits = "0.2.14"
+log = { version = "0.4.14", features = ["max_level_debug", "release_max_level_debug"] }
+env_logger = "0.9.0"
+rand = "0.8.4"
+esp-idf-sys = { version = "0.30", features = ["binstart"], optional = true }
+openssl = { git = "https://github.com/sfackler/rust-openssl", optional = true}
+foreign-types = { version = "0.3.1", optional = true}
+sha2 = { version = "0.9.8", optional = true}
+hmac = { version = "0.11.0", optional = true}
+mbedtls = { git = "https://github.com/fortanix/rust-mbedtls", optional = true}
+subtle = "2.4.1"
+colored = "2.0.0"
+smol = "1.2.5"
+owning_ref = "0.4.1"
+safemem = "0.3.3"
+chrono = { version = "0.4.19", default-features = false, features = ["clock", "std"] }
+async-channel = "1.6"
+
+[target.'cfg(target_os = "macos")'.dependencies]
+astro-dnssd = "0.3"
+
+[[example]]
+name = "onoff_light"
+path = "../examples/onoff_light/src/main.rs"

matter/src/acl.rs

@@ -0,0 +1,649 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::{Arc, Mutex, MutexGuard, RwLock};
+
+use crate::{
+    data_model::objects::{Access, Privilege},
+    error::Error,
+    fabric,
+    interaction_model::messages::GenericPath,
+    sys::Psm,
+    tlv::{FromTLV, TLVElement, TLVList, TLVWriter, TagType, ToTLV},
+    utils::writebuf::WriteBuf,
+};
+use log::error;
+use num_derive::FromPrimitive;
+
+// Matter Minimum Requirements
+pub const SUBJECTS_PER_ENTRY: usize = 4;
+pub const TARGETS_PER_ENTRY: usize = 3;
+pub const ENTRIES_PER_FABRIC: usize = 3;
+
+// TODO: Check if this and the SessionMode can be combined into some generic data structure
+#[derive(FromPrimitive, Copy, Clone, PartialEq, Debug)]
+pub enum AuthMode {
+    Pase = 1,
+    Case = 2,
+    Group = 3,
+    Invalid = 4,
+}
+
+impl FromTLV<'_> for AuthMode {
+    fn from_tlv(t: &TLVElement) -> Result<Self, Error>
+    where
+        Self: Sized,
+    {
+        num::FromPrimitive::from_u32(t.u32()?)
+            .filter(|a| *a != AuthMode::Invalid)
+            .ok_or(Error::Invalid)
+    }
+}
+
+impl ToTLV for AuthMode {
+    fn to_tlv(
+        &self,
+        tw: &mut crate::tlv::TLVWriter,
+        tag: crate::tlv::TagType,
+    ) -> Result<(), Error> {
+        match self {
+            AuthMode::Invalid => Ok(()),
+            _ => tw.u8(tag, *self as u8),
+        }
+    }
+}
+
+/// The Accessor Object
+pub struct Accessor {
+    /// The fabric index of the accessor
+    pub fab_idx: u8,
+    /// Accessor's identified: could be node-id, NoC CAT, group id
+    id: u64,
+    /// The Authmode of this session
+    auth_mode: AuthMode,
+    // TODO: Is this the right place for this though, or should we just use a global-acl-handle-get
+    acl_mgr: Arc<AclMgr>,
+}
+
+impl Accessor {
+    pub fn new(fab_idx: u8, id: u64, auth_mode: AuthMode, acl_mgr: Arc<AclMgr>) -> Self {
+        Self {
+            fab_idx,
+            id,
+            auth_mode,
+            acl_mgr,
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct AccessDesc<'a> {
+    /// The object to be acted upon
+    path: &'a GenericPath,
+    /// The target permissions
+    target_perms: Option<Access>,
+    // The operation being done
+    // TODO: Currently this is Access, but we need a way to represent the 'invoke' somehow too
+    operation: Access,
+}
+
+/// Access Request Object
+pub struct AccessReq<'a> {
+    accessor: &'a Accessor,
+    object: AccessDesc<'a>,
+}
+
+impl<'a> AccessReq<'a> {
+    /// Creates an access request object
+    ///
+    /// An access request specifies the _accessor_ attempting to access _path_
+    /// with _operation_
+    pub fn new(accessor: &'a Accessor, path: &'a GenericPath, operation: Access) -> Self {
+        AccessReq {
+            accessor,
+            object: AccessDesc {
+                path,
+                target_perms: None,
+                operation,
+            },
+        }
+    }
+
+    /// Add target's permissions to the request
+    ///
+    /// The permissions that are associated with the target (identified by the
+    /// path in the AccessReq) are added to the request
+    pub fn set_target_perms(&mut self, perms: Access) {
+        self.object.target_perms = Some(perms);
+    }
+
+    /// Checks if access is allowed
+    ///
+    /// This checks all the ACL list to identify if any of the ACLs provides the
+    /// _accessor_ the necessary privileges to access the target as per its
+    /// permissions
+    pub fn allow(&self) -> bool {
+        self.accessor.acl_mgr.allow(self)
+    }
+}
+
+#[derive(FromTLV, ToTLV, Copy, Clone, Debug, PartialEq)]
+pub struct Target {
+    cluster: Option<u32>,
+    endpoint: Option<u16>,
+    device_type: Option<u32>,
+}
+
+impl Target {
+    pub fn new(endpoint: Option<u16>, cluster: Option<u32>, device_type: Option<u32>) -> Self {
+        Self {
+            cluster,
+            endpoint,
+            device_type,
+        }
+    }
+}
+
+type Subjects = [Option<u64>; SUBJECTS_PER_ENTRY];
+type Targets = [Option<Target>; TARGETS_PER_ENTRY];
+#[derive(ToTLV, FromTLV, Copy, Clone, Debug, PartialEq)]
+#[tlvargs(start = 1)]
+pub struct AclEntry {
+    privilege: Privilege,
+    auth_mode: AuthMode,
+    subjects: Subjects,
+    targets: Targets,
+    // TODO: Instead of the direct value, we should consider GlobalElements::FabricIndex
+    #[tagval(0xFE)]
+    pub fab_idx: Option<u8>,
+}
+
+impl AclEntry {
+    pub fn new(fab_idx: u8, privilege: Privilege, auth_mode: AuthMode) -> Self {
+        const INIT_SUBJECTS: Option<u64> = None;
+        const INIT_TARGETS: Option<Target> = None;
+        let privilege = privilege;
+        Self {
+            fab_idx: Some(fab_idx),
+            privilege,
+            auth_mode,
+            subjects: [INIT_SUBJECTS; SUBJECTS_PER_ENTRY],
+            targets: [INIT_TARGETS; TARGETS_PER_ENTRY],
+        }
+    }
+
+    pub fn add_subject(&mut self, subject: u64) -> Result<(), Error> {
+        let index = self
+            .subjects
+            .iter()
+            .position(|s| s.is_none())
+            .ok_or(Error::NoSpace)?;
+        self.subjects[index] = Some(subject);
+        Ok(())
+    }
+
+    pub fn add_target(&mut self, target: Target) -> Result<(), Error> {
+        let index = self
+            .targets
+            .iter()
+            .position(|s| s.is_none())
+            .ok_or(Error::NoSpace)?;
+        self.targets[index] = Some(target);
+        Ok(())
+    }
+
+    fn match_accessor(&self, accessor: &Accessor) -> bool {
+        if self.auth_mode != accessor.auth_mode {
+            return false;
+        }
+
+        let mut allow = false;
+        let mut entries_exist = false;
+        for i in self.subjects.iter().flatten() {
+            entries_exist = true;
+            if accessor.id == *i {
+                allow = true;
+            }
+        }
+        if !entries_exist {
+            // Subjects array empty implies allow for all subjects
+            allow = true;
+        }
+
+        // true if both are true
+        allow && self.fab_idx == Some(accessor.fab_idx)
+    }
+
+    fn match_access_desc(&self, object: &AccessDesc) -> bool {
+        let mut allow = false;
+        let mut entries_exist = false;
+        for t in self.targets.iter().flatten() {
+            entries_exist = true;
+            if (t.endpoint.is_none() || t.endpoint == object.path.endpoint)
+                && (t.cluster.is_none() || t.cluster == object.path.cluster)
+            {
+                allow = true
+            }
+        }
+        if !entries_exist {
+            // Targets array empty implies allow for all targets
+            allow = true;
+        }
+
+        if allow {
+            // Check that the object's access allows this operation with this privilege
+            if let Some(access) = object.target_perms {
+                access.is_ok(object.operation, self.privilege)
+            } else {
+                false
+            }
+        } else {
+            false
+        }
+    }
+
+    pub fn allow(&self, req: &AccessReq) -> bool {
+        self.match_accessor(req.accessor) && self.match_access_desc(&req.object)
+    }
+}
+
+const MAX_ACL_ENTRIES: usize = ENTRIES_PER_FABRIC * fabric::MAX_SUPPORTED_FABRICS;
+type AclEntries = [Option<AclEntry>; MAX_ACL_ENTRIES];
+
+#[derive(ToTLV, FromTLV, Debug)]
+struct AclMgrInner {
+    entries: AclEntries,
+}
+
+const ACL_KV_ENTRY: &str = "acl";
+const ACL_KV_MAX_SIZE: usize = 300;
+impl AclMgrInner {
+    pub fn store(&self, psm: &MutexGuard<Psm>) -> Result<(), Error> {
+        let mut acl_tlvs = [0u8; ACL_KV_MAX_SIZE];
+        let mut wb = WriteBuf::new(&mut acl_tlvs, ACL_KV_MAX_SIZE);
+        let mut tw = TLVWriter::new(&mut wb);
+        self.entries.to_tlv(&mut tw, TagType::Anonymous)?;
+        psm.set_kv_slice(ACL_KV_ENTRY, wb.as_slice())
+    }
+
+    pub fn load(psm: &MutexGuard<Psm>) -> Result<Self, Error> {
+        let mut acl_tlvs = Vec::new();
+        psm.get_kv_slice(ACL_KV_ENTRY, &mut acl_tlvs)?;
+        let root = TLVList::new(&acl_tlvs)
+            .iter()
+            .next()
+            .ok_or(Error::Invalid)?;
+
+        Ok(Self {
+            entries: AclEntries::from_tlv(&root)?,
+        })
+    }
+
+    /// Traverse fabric specific entries to find the index
+    ///
+    /// If the ACL Mgr has 3 entries with fabric indexes, 1, 2, 1, then the list
+    /// index 1 for Fabric 1 in the ACL Mgr will be the actual index 2 (starting from  0)
+    fn for_index_in_fabric(
+        &mut self,
+        index: u8,
+        fab_idx: u8,
+    ) -> Result<&mut Option<AclEntry>, Error> {
+        // Can't use flatten as we need to borrow the Option<> not the 'AclEntry'
+        for (curr_index, entry) in self
+            .entries
+            .iter_mut()
+            .filter(|e| e.filter(|e1| e1.fab_idx == Some(fab_idx)).is_some())
+            .enumerate()
+        {
+            if curr_index == index as usize {
+                return Ok(entry);
+            }
+        }
+        Err(Error::NotFound)
+    }
+}
+
+pub struct AclMgr {
+    inner: RwLock<AclMgrInner>,
+    // The Option<> is solely because test execution is faster
+    // Doing this here adds the least overhead during ACL verification
+    psm: Option<Arc<Mutex<Psm>>>,
+}
+
+impl AclMgr {
+    pub fn new() -> Result<Self, Error> {
+        AclMgr::new_with(true)
+    }
+
+    pub fn new_with(psm_support: bool) -> Result<Self, Error> {
+        const INIT: Option<AclEntry> = None;
+        let mut psm = None;
+
+        let inner = if !psm_support {
+            AclMgrInner {
+                entries: [INIT; MAX_ACL_ENTRIES],
+            }
+        } else {
+            let psm_handle = Psm::get()?;
+            let inner = {
+                let psm_lock = psm_handle.lock().unwrap();
+                AclMgrInner::load(&psm_lock)
+            };
+
+            psm = Some(psm_handle);
+            inner.unwrap_or({
+                // Error loading from PSM
+                AclMgrInner {
+                    entries: [INIT; MAX_ACL_ENTRIES],
+                }
+            })
+        };
+        Ok(Self {
+            inner: RwLock::new(inner),
+            psm,
+        })
+    }
+
+    pub fn erase_all(&self) {
+        let mut inner = self.inner.write().unwrap();
+        for i in 0..MAX_ACL_ENTRIES {
+            inner.entries[i] = None;
+        }
+        if let Some(psm) = self.psm.as_ref() {
+            let psm = psm.lock().unwrap();
+            let _ = inner.store(&psm).map_err(|e| {
+                error!("Error in storing ACLs {}", e);
+            });
+        }
+    }
+
+    pub fn add(&self, entry: AclEntry) -> Result<(), Error> {
+        let mut inner = self.inner.write().unwrap();
+        let cnt = inner
+            .entries
+            .iter()
+            .flatten()
+            .filter(|a| a.fab_idx == entry.fab_idx)
+            .count();
+        if cnt >= ENTRIES_PER_FABRIC {
+            return Err(Error::NoSpace);
+        }
+        let index = inner
+            .entries
+            .iter()
+            .position(|a| a.is_none())
+            .ok_or(Error::NoSpace)?;
+        inner.entries[index] = Some(entry);
+
+        if let Some(psm) = self.psm.as_ref() {
+            let psm = psm.lock().unwrap();
+            inner.store(&psm)
+        } else {
+            Ok(())
+        }
+    }
+
+    // Since the entries are fabric-scoped, the index is only for entries with the matching fabric index
+    pub fn edit(&self, index: u8, fab_idx: u8, new: AclEntry) -> Result<(), Error> {
+        let mut inner = self.inner.write().unwrap();
+        let old = inner.for_index_in_fabric(index, fab_idx)?;
+        *old = Some(new);
+
+        if let Some(psm) = self.psm.as_ref() {
+            let psm = psm.lock().unwrap();
+            inner.store(&psm)
+        } else {
+            Ok(())
+        }
+    }
+
+    pub fn delete(&self, index: u8, fab_idx: u8) -> Result<(), Error> {
+        let mut inner = self.inner.write().unwrap();
+        let old = inner.for_index_in_fabric(index, fab_idx)?;
+        *old = None;
+
+        if let Some(psm) = self.psm.as_ref() {
+            let psm = psm.lock().unwrap();
+            inner.store(&psm)
+        } else {
+            Ok(())
+        }
+    }
+
+    pub fn delete_for_fabric(&self, fab_idx: u8) -> Result<(), Error> {
+        let mut inner = self.inner.write().unwrap();
+
+        for i in 0..MAX_ACL_ENTRIES {
+            if inner.entries[i]
+                .filter(|e| e.fab_idx == Some(fab_idx))
+                .is_some()
+            {
+                inner.entries[i] = None;
+            }
+        }
+
+        if let Some(psm) = self.psm.as_ref() {
+            let psm = psm.lock().unwrap();
+            inner.store(&psm)
+        } else {
+            Ok(())
+        }
+    }
+
+    pub fn for_each_acl<T>(&self, mut f: T) -> Result<(), Error>
+    where
+        T: FnMut(&AclEntry),
+    {
+        let inner = self.inner.read().unwrap();
+        for entry in inner.entries.iter().flatten() {
+            f(entry)
+        }
+        Ok(())
+    }
+
+    pub fn allow(&self, req: &AccessReq) -> bool {
+        // PASE Sessions have implicit access grant
+        if req.accessor.auth_mode == AuthMode::Pase {
+            return true;
+        }
+        let inner = self.inner.read().unwrap();
+        for e in inner.entries.iter().flatten() {
+            if e.allow(req) {
+                return true;
+            }
+        }
+        error!(
+            "ACL Disallow for src id {} fab idx {}",
+            req.accessor.id, req.accessor.fab_idx
+        );
+        error!("{}", self);
+        false
+    }
+}
+
+impl std::fmt::Display for AclMgr {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        let inner = self.inner.read().unwrap();
+        write!(f, "ACLS: [")?;
+        for i in inner.entries.iter().flatten() {
+            write!(f, "  {{ {:?} }}, ", i)?;
+        }
+        write!(f, "]")
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::{
+        data_model::objects::{Access, Privilege},
+        interaction_model::messages::GenericPath,
+    };
+    use std::sync::Arc;
+
+    use super::{AccessReq, Accessor, AclEntry, AclMgr, AuthMode, Target};
+
+    #[test]
+    fn test_basic_empty_subject_target() {
+        let am = Arc::new(AclMgr::new_with(false).unwrap());
+        am.erase_all();
+        let accessor = Accessor::new(2, 112233, AuthMode::Case, am.clone());
+        let path = GenericPath::new(Some(1), Some(1234), None);
+        let mut req = AccessReq::new(&accessor, &path, Access::READ);
+        req.set_target_perms(Access::RWVA);
+
+        // Default deny
+        assert_eq!(req.allow(), false);
+
+        // Deny for session mode mismatch
+        let new = AclEntry::new(1, Privilege::VIEW, AuthMode::Pase);
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), false);
+
+        // Deny for fab idx mismatch
+        let new = AclEntry::new(1, Privilege::VIEW, AuthMode::Case);
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), false);
+
+        // Allow
+        let new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), true);
+    }
+
+    #[test]
+    fn test_subject() {
+        let am = Arc::new(AclMgr::new_with(false).unwrap());
+        am.erase_all();
+        let accessor = Accessor::new(2, 112233, AuthMode::Case, am.clone());
+        let path = GenericPath::new(Some(1), Some(1234), None);
+        let mut req = AccessReq::new(&accessor, &path, Access::READ);
+        req.set_target_perms(Access::RWVA);
+
+        // Deny for subject mismatch
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_subject(112232).unwrap();
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), false);
+
+        // Allow for subject match - target is wildcard
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_subject(112233).unwrap();
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), true);
+    }
+
+    #[test]
+    fn test_target() {
+        let am = Arc::new(AclMgr::new_with(false).unwrap());
+        am.erase_all();
+        let accessor = Accessor::new(2, 112233, AuthMode::Case, am.clone());
+        let path = GenericPath::new(Some(1), Some(1234), None);
+        let mut req = AccessReq::new(&accessor, &path, Access::READ);
+        req.set_target_perms(Access::RWVA);
+
+        // Deny for target mismatch
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_target(Target {
+            cluster: Some(2),
+            endpoint: Some(4567),
+            device_type: None,
+        })
+        .unwrap();
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), false);
+
+        // Allow for cluster match - subject wildcard
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_target(Target {
+            cluster: Some(1234),
+            endpoint: None,
+            device_type: None,
+        })
+        .unwrap();
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), true);
+
+        // Clean Slate
+        am.erase_all();
+
+        // Allow for endpoint match - subject wildcard
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_target(Target {
+            cluster: None,
+            endpoint: Some(1),
+            device_type: None,
+        })
+        .unwrap();
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), true);
+
+        // Clean Slate
+        am.erase_all();
+
+        // Allow for exact match
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_target(Target {
+            cluster: Some(1234),
+            endpoint: Some(1),
+            device_type: None,
+        })
+        .unwrap();
+        new.add_subject(112233).unwrap();
+        am.add(new).unwrap();
+        assert_eq!(req.allow(), true);
+    }
+
+    #[test]
+    fn test_privilege() {
+        let am = Arc::new(AclMgr::new_with(false).unwrap());
+        am.erase_all();
+        let accessor = Accessor::new(2, 112233, AuthMode::Case, am.clone());
+        let path = GenericPath::new(Some(1), Some(1234), None);
+
+        // Create an Exact Match ACL with View privilege
+        let mut new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.add_target(Target {
+            cluster: Some(1234),
+            endpoint: Some(1),
+            device_type: None,
+        })
+        .unwrap();
+        new.add_subject(112233).unwrap();
+        am.add(new).unwrap();
+
+        // Write on an RWVA without admin access - deny
+        let mut req = AccessReq::new(&accessor, &path, Access::WRITE);
+        req.set_target_perms(Access::RWVA);
+        assert_eq!(req.allow(), false);
+
+        // Create an Exact Match ACL with Admin privilege
+        let mut new = AclEntry::new(2, Privilege::ADMIN, AuthMode::Case);
+        new.add_target(Target {
+            cluster: Some(1234),
+            endpoint: Some(1),
+            device_type: None,
+        })
+        .unwrap();
+        new.add_subject(112233).unwrap();
+        am.add(new).unwrap();
+
+        // Write on an RWVA with admin access - allow
+        let mut req = AccessReq::new(&accessor, &path, Access::WRITE);
+        req.set_target_perms(Access::RWVA);
+        assert_eq!(req.allow(), true);
+    }
+}

matter/src/cert/asn1_writer.rs

@@ -0,0 +1,265 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::{CertConsumer, MAX_DEPTH};
+use crate::error::Error;
+use chrono::{TimeZone, Utc};
+
+#[derive(Debug)]
+pub struct ASN1Writer<'a> {
+    buf: &'a mut [u8],
+    // The current write offset in the buffer
+    offset: usize,
+    // If multiple 'composite' structures are being written, their starts are
+    // captured in this
+    depth: [usize; MAX_DEPTH],
+    // The current depth of operation within the depth stack
+    current_depth: usize,
+}
+
+const RESERVE_LEN_BYTES: usize = 3;
+impl<'a> ASN1Writer<'a> {
+    pub fn new(buf: &'a mut [u8]) -> Self {
+        Self {
+            buf,
+            offset: 0,
+            depth: [0; MAX_DEPTH],
+            current_depth: 0,
+        }
+    }
+
+    pub fn append_with<F>(&mut self, size: usize, f: F) -> Result<(), Error>
+    where
+        F: FnOnce(&mut Self),
+    {
+        if self.offset + size <= self.buf.len() {
+            f(self);
+            self.offset += size;
+            return Ok(());
+        }
+        Err(Error::NoSpace)
+    }
+
+    pub fn append_tlv<F>(&mut self, tag: u8, len: usize, f: F) -> Result<(), Error>
+    where
+        F: FnOnce(&mut Self),
+    {
+        let total_len = 1 + ASN1Writer::bytes_to_encode_len(len)? + len;
+        if self.offset + total_len <= self.buf.len() {
+            self.buf[self.offset] = tag;
+            self.offset += 1;
+            self.offset = self.encode_len(self.offset, len)?;
+            f(self);
+            self.offset += len;
+            return Ok(());
+        }
+        Err(Error::NoSpace)
+    }
+
+    fn add_compound(&mut self, val: u8) -> Result<(), Error> {
+        // We reserve 3 bytes for encoding the length
+        // If a shorter length is actually required, we will move everything back
+        self.append_with(1 + RESERVE_LEN_BYTES, |t| t.buf[t.offset] = val)?;
+        self.depth[self.current_depth] = self.offset;
+        self.current_depth += 1;
+        if self.current_depth >= MAX_DEPTH {
+            Err(Error::NoSpace)
+        } else {
+            Ok(())
+        }
+    }
+
+    fn encode_len(&mut self, mut at_offset: usize, len: usize) -> Result<usize, Error> {
+        let mut bytes_of_len = ASN1Writer::bytes_to_encode_len(len)?;
+        if bytes_of_len > 1 {
+            self.buf[at_offset] = (0x80 | (bytes_of_len - 1)) as u8;
+            at_offset += 1;
+            bytes_of_len -= 1;
+        }
+
+        // At this point bytes_of_len is the actual number of bytes for the length encoding
+        // after the 0x80 (if it was present)
+        let mut octet_number = bytes_of_len - 1;
+        // We start encoding the highest octest first
+        loop {
+            self.buf[at_offset] = ((len >> (octet_number * 8)) & 0xff) as u8;
+
+            at_offset += 1;
+            if octet_number == 0 {
+                break;
+            }
+            octet_number -= 1;
+        }
+
+        Ok(at_offset)
+    }
+
+    fn end_compound(&mut self) -> Result<(), Error> {
+        if self.current_depth == 0 {
+            return Err(Error::Invalid);
+        }
+        let seq_len = self.get_compound_len();
+        let write_offset = self.get_length_encoding_offset();
+
+        let mut write_offset = self.encode_len(write_offset, seq_len)?;
+
+        // Shift everything by as much
+        let shift_len = self.depth[self.current_depth - 1] - write_offset;
+        if shift_len > 0 {
+            for _i in 0..seq_len {
+                self.buf[write_offset] = self.buf[write_offset + shift_len];
+                write_offset += 1;
+            }
+        }
+        self.current_depth -= 1;
+        self.offset -= shift_len;
+        Ok(())
+    }
+
+    fn get_compound_len(&self) -> usize {
+        self.offset - self.depth[self.current_depth - 1]
+    }
+
+    fn bytes_to_encode_len(len: usize) -> Result<usize, Error> {
+        let len = if len < 128 {
+            // This is directly encoded
+            1
+        } else if len < 256 {
+            // This is done with an 0xA1 followed by actual len
+            2
+        } else if len < 65536 {
+            // This is done with an 0xA2 followed by 2 bytes of actual len
+            3
+        } else {
+            return Err(Error::NoSpace);
+        };
+        Ok(len)
+    }
+
+    fn get_length_encoding_offset(&self) -> usize {
+        self.depth[self.current_depth - 1] - RESERVE_LEN_BYTES
+    }
+
+    pub fn as_slice(&self) -> &[u8] {
+        &self.buf[..self.offset]
+    }
+
+    fn write_str(&mut self, vtype: u8, s: &[u8]) -> Result<(), Error> {
+        self.append_tlv(vtype, s.len(), |t| {
+            let end_offset = t.offset + s.len();
+            t.buf[t.offset..end_offset].copy_from_slice(s);
+        })
+    }
+}
+
+impl<'a> CertConsumer for ASN1Writer<'a> {
+    fn start_seq(&mut self, _tag: &str) -> Result<(), Error> {
+        self.add_compound(0x30)
+    }
+
+    fn end_seq(&mut self) -> Result<(), Error> {
+        self.end_compound()
+    }
+
+    fn integer(&mut self, _tag: &str, i: &[u8]) -> Result<(), Error> {
+        self.write_str(0x02, i)
+    }
+
+    fn utf8str(&mut self, _tag: &str, s: &str) -> Result<(), Error> {
+        // Note: ASN1 has 3 string, this is UTF8String
+        self.write_str(0x0c, s.as_bytes())
+    }
+
+    fn bitstr(&mut self, _tag: &str, truncate: bool, s: &[u8]) -> Result<(), Error> {
+        // Note: ASN1 has 3 string, this is BIT String
+
+        // Strip off the end zeroes
+        let mut last_byte = s.len() - 1;
+        let mut num_of_zero = 0;
+        if truncate {
+            while s[last_byte] == 0 {
+                last_byte -= 1;
+            }
+            // For the last valid byte, identifying the number of last bits
+            // that are 0s
+            num_of_zero = s[last_byte].trailing_zeros() as u8;
+        }
+        let s = &s[..(last_byte + 1)];
+        self.append_tlv(0x03, s.len() + 1, |t| {
+            t.buf[t.offset] = num_of_zero;
+            let end_offset = t.offset + 1 + s.len();
+            t.buf[(t.offset + 1)..end_offset].copy_from_slice(s);
+        })
+    }
+
+    fn ostr(&mut self, _tag: &str, s: &[u8]) -> Result<(), Error> {
+        // Note: ASN1 has 3 string, this is Octet String
+        self.write_str(0x04, s)
+    }
+
+    fn start_compound_ostr(&mut self, _tag: &str) -> Result<(), Error> {
+        // Note: ASN1 has 3 string, this is compound Octet String
+        self.add_compound(0x04)
+    }
+
+    fn end_compound_ostr(&mut self) -> Result<(), Error> {
+        self.end_compound()
+    }
+
+    fn bool(&mut self, _tag: &str, b: bool) -> Result<(), Error> {
+        self.append_tlv(0x01, 1, |t| {
+            if b {
+                t.buf[t.offset] = 0xFF;
+            } else {
+                t.buf[t.offset] = 0x00;
+            }
+        })
+    }
+
+    fn start_set(&mut self, _tag: &str) -> Result<(), Error> {
+        self.add_compound(0x31)
+    }
+
+    fn end_set(&mut self) -> Result<(), Error> {
+        self.end_compound()
+    }
+
+    fn ctx(&mut self, _tag: &str, id: u8, val: &[u8]) -> Result<(), Error> {
+        self.write_str(0x80 | id, val)
+    }
+
+    fn start_ctx(&mut self, _tag: &str, val: u8) -> Result<(), Error> {
+        self.add_compound(0xA0 | val)
+    }
+
+    fn end_ctx(&mut self) -> Result<(), Error> {
+        self.end_compound()
+    }
+
+    fn oid(&mut self, _tag: &str, oid: &[u8]) -> Result<(), Error> {
+        self.write_str(0x06, oid)
+    }
+
+    fn utctime(&mut self, _tag: &str, epoch: u32) -> Result<(), Error> {
+        let mut matter_epoch = Utc.ymd(2000, 1, 1).and_hms(0, 0, 0).timestamp();
+        matter_epoch += epoch as i64;
+
+        let dt = Utc.timestamp(matter_epoch, 0);
+        let time_str = format!("{}Z", dt.format("%y%m%d%H%M%S"));
+        self.write_str(0x17, time_str.as_bytes())
+    }
+}

matter/src/cert/mod.rs

@@ -0,0 +1,869 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::fmt;
+
+use crate::{
+    crypto::{CryptoKeyPair, KeyPair},
+    error::Error,
+    tlv::{self, FromTLV, TLVArrayOwned, TLVElement, TLVWriter, TagType, ToTLV},
+    utils::writebuf::WriteBuf,
+};
+use log::error;
+use num_derive::FromPrimitive;
+
+use self::{asn1_writer::ASN1Writer, printer::CertPrinter};
+
+// As per https://datatracker.ietf.org/doc/html/rfc5280
+
+const OID_PUB_KEY_ECPUBKEY: [u8; 7] = [0x2A, 0x86, 0x48, 0xCE, 0x3D, 0x02, 0x01];
+const OID_EC_TYPE_PRIME256V1: [u8; 8] = [0x2A, 0x86, 0x48, 0xCE, 0x3D, 0x03, 0x01, 0x07];
+const OID_ECDSA_WITH_SHA256: [u8; 8] = [0x2A, 0x86, 0x48, 0xCE, 0x3D, 0x04, 0x03, 0x02];
+
+#[derive(FromPrimitive)]
+pub enum CertTags {
+    SerialNum = 1,
+    SignAlgo = 2,
+    Issuer = 3,
+    NotBefore = 4,
+    NotAfter = 5,
+    Subject = 6,
+    PubKeyAlgo = 7,
+    EcCurveId = 8,
+    EcPubKey = 9,
+    Extensions = 10,
+    Signature = 11,
+}
+
+#[derive(FromPrimitive, Debug)]
+pub enum EcCurveIdValue {
+    Prime256V1 = 1,
+}
+
+pub fn get_ec_curve_id(algo: u8) -> Option<EcCurveIdValue> {
+    num::FromPrimitive::from_u8(algo)
+}
+
+#[derive(FromPrimitive, Debug)]
+pub enum PubKeyAlgoValue {
+    EcPubKey = 1,
+}
+
+pub fn get_pubkey_algo(algo: u8) -> Option<PubKeyAlgoValue> {
+    num::FromPrimitive::from_u8(algo)
+}
+
+#[derive(FromPrimitive, Debug)]
+pub enum SignAlgoValue {
+    ECDSAWithSHA256 = 1,
+}
+
+pub fn get_sign_algo(algo: u8) -> Option<SignAlgoValue> {
+    num::FromPrimitive::from_u8(algo)
+}
+
+const KEY_USAGE_DIGITAL_SIGN: u16 = 0x0001;
+const KEY_USAGE_NON_REPUDIATION: u16 = 0x0002;
+const KEY_USAGE_KEY_ENCIPHERMENT: u16 = 0x0004;
+const KEY_USAGE_DATA_ENCIPHERMENT: u16 = 0x0008;
+const KEY_USAGE_KEY_AGREEMENT: u16 = 0x0010;
+const KEY_USAGE_KEY_CERT_SIGN: u16 = 0x0020;
+const KEY_USAGE_CRL_SIGN: u16 = 0x0040;
+const KEY_USAGE_ENCIPHER_ONLY: u16 = 0x0080;
+const KEY_USAGE_DECIPHER_ONLY: u16 = 0x0100;
+
+fn reverse_byte(byte: u8) -> u8 {
+    const LOOKUP: [u8; 16] = [
+        0x00, 0x08, 0x04, 0x0c, 0x02, 0x0a, 0x06, 0x0e, 0x01, 0x09, 0x05, 0x0d, 0x03, 0x0b, 0x07,
+        0x0f,
+    ];
+    (LOOKUP[(byte & 0x0f) as usize] << 4) | LOOKUP[(byte >> 4) as usize]
+}
+
+fn int_to_bitstring(mut a: u16, buf: &mut [u8]) {
+    if buf.len() >= 2 {
+        buf[0] = reverse_byte((a & 0xff) as u8);
+        a >>= 8;
+        buf[1] = reverse_byte((a & 0xff) as u8);
+    }
+}
+
+macro_rules! add_if {
+    ($key:ident, $bit:ident,$str:literal) => {
+        if ($key & $bit) != 0 {
+            $str
+        } else {
+            ""
+        }
+    };
+}
+
+fn get_print_str(key_usage: u16) -> String {
+    format!(
+        "{}{}{}{}{}{}{}{}{}",
+        add_if!(key_usage, KEY_USAGE_DIGITAL_SIGN, "digitalSignature "),
+        add_if!(key_usage, KEY_USAGE_NON_REPUDIATION, "nonRepudiation "),
+        add_if!(key_usage, KEY_USAGE_KEY_ENCIPHERMENT, "keyEncipherment "),
+        add_if!(key_usage, KEY_USAGE_DATA_ENCIPHERMENT, "dataEncipherment "),
+        add_if!(key_usage, KEY_USAGE_KEY_AGREEMENT, "keyAgreement "),
+        add_if!(key_usage, KEY_USAGE_KEY_CERT_SIGN, "keyCertSign "),
+        add_if!(key_usage, KEY_USAGE_CRL_SIGN, "CRLSign "),
+        add_if!(key_usage, KEY_USAGE_ENCIPHER_ONLY, "encipherOnly "),
+        add_if!(key_usage, KEY_USAGE_DECIPHER_ONLY, "decipherOnly "),
+    )
+}
+
+#[allow(unused_assignments)]
+fn encode_key_usage(key_usage: u16, w: &mut dyn CertConsumer) -> Result<(), Error> {
+    let mut key_usage_str = [0u8; 2];
+    int_to_bitstring(key_usage, &mut key_usage_str);
+    w.bitstr(&get_print_str(key_usage), true, &key_usage_str)?;
+    Ok(())
+}
+
+fn encode_extended_key_usage(
+    list: &TLVArrayOwned<u8>,
+    w: &mut dyn CertConsumer,
+) -> Result<(), Error> {
+    const OID_SERVER_AUTH: [u8; 8] = [0x2B, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x01];
+    const OID_CLIENT_AUTH: [u8; 8] = [0x2B, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x02];
+    const OID_CODE_SIGN: [u8; 8] = [0x2B, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x03];
+    const OID_EMAIL_PROT: [u8; 8] = [0x2B, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x04];
+    const OID_TIMESTAMP: [u8; 8] = [0x2B, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x08];
+    const OID_OCSP_SIGN: [u8; 8] = [0x2B, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x09];
+    let encoding = [
+        ("", &[0; 8]),
+        ("ServerAuth", &OID_SERVER_AUTH),
+        ("ClientAuth", &OID_CLIENT_AUTH),
+        ("CodeSign", &OID_CODE_SIGN),
+        ("EmailProtection", &OID_EMAIL_PROT),
+        ("Timestamp", &OID_TIMESTAMP),
+        ("OCSPSign", &OID_OCSP_SIGN),
+    ];
+
+    w.start_seq("")?;
+    for t in list.iter() {
+        let t = *t as usize;
+        if t > 0 && t <= encoding.len() {
+            w.oid(encoding[t].0, encoding[t].1)?;
+        } else {
+            error!("Skipping encoding key usage out of bounds");
+        }
+    }
+    w.end_seq()?;
+    Ok(())
+}
+
+#[derive(FromTLV, ToTLV, Default)]
+#[tlvargs(start = 1)]
+struct BasicConstraints {
+    is_ca: bool,
+    path: Option<u8>,
+}
+
+impl BasicConstraints {
+    pub fn encode(&self, w: &mut dyn CertConsumer) -> Result<(), Error> {
+        w.start_seq("")?;
+        if self.is_ca {
+            // Encode CA only if true
+            w.bool("CA:", true)?
+        }
+        if self.path.is_some() {
+            error!("Path Len is not yet implemented");
+        }
+        w.end_seq()
+    }
+}
+
+fn encode_extension_start(
+    tag: &str,
+    critical: bool,
+    oid: &[u8],
+    w: &mut dyn CertConsumer,
+) -> Result<(), Error> {
+    w.start_seq(tag)?;
+    w.oid("", oid)?;
+    if critical {
+        w.bool("critical:", true)?;
+    }
+    w.start_compound_ostr("value:")
+}
+
+fn encode_extension_end(w: &mut dyn CertConsumer) -> Result<(), Error> {
+    w.end_compound_ostr()?;
+    w.end_seq()
+}
+
+#[derive(FromTLV, ToTLV, Default)]
+#[tlvargs(start = 1, datatype = "list")]
+struct Extensions {
+    basic_const: Option<BasicConstraints>,
+    key_usage: Option<u16>,
+    ext_key_usage: Option<TLVArrayOwned<u8>>,
+    subj_key_id: Option<Vec<u8>>,
+    auth_key_id: Option<Vec<u8>>,
+    future_extensions: Option<Vec<u8>>,
+}
+
+impl Extensions {
+    fn encode(&self, w: &mut dyn CertConsumer) -> Result<(), Error> {
+        const OID_BASIC_CONSTRAINTS: [u8; 3] = [0x55, 0x1D, 0x13];
+        const OID_KEY_USAGE: [u8; 3] = [0x55, 0x1D, 0x0F];
+        const OID_EXT_KEY_USAGE: [u8; 3] = [0x55, 0x1D, 0x25];
+        const OID_SUBJ_KEY_IDENTIFIER: [u8; 3] = [0x55, 0x1D, 0x0E];
+        const OID_AUTH_KEY_ID: [u8; 3] = [0x55, 0x1D, 0x23];
+
+        w.start_ctx("X509v3 extensions:", 3)?;
+        w.start_seq("")?;
+        if let Some(t) = &self.basic_const {
+            encode_extension_start("X509v3 Basic Constraints", true, &OID_BASIC_CONSTRAINTS, w)?;
+            t.encode(w)?;
+            encode_extension_end(w)?;
+        }
+        if let Some(t) = self.key_usage {
+            encode_extension_start("X509v3 Key Usage", true, &OID_KEY_USAGE, w)?;
+            encode_key_usage(t, w)?;
+            encode_extension_end(w)?;
+        }
+        if let Some(t) = &self.ext_key_usage {
+            encode_extension_start("X509v3 Extended Key Usage", true, &OID_EXT_KEY_USAGE, w)?;
+            encode_extended_key_usage(t, w)?;
+            encode_extension_end(w)?;
+        }
+        if let Some(t) = &self.subj_key_id {
+            encode_extension_start("Subject Key ID", false, &OID_SUBJ_KEY_IDENTIFIER, w)?;
+            w.ostr("", t.as_slice())?;
+            encode_extension_end(w)?;
+        }
+        if let Some(t) = &self.auth_key_id {
+            encode_extension_start("Auth Key ID", false, &OID_AUTH_KEY_ID, w)?;
+            w.start_seq("")?;
+            w.ctx("", 0, t.as_slice())?;
+            w.end_seq()?;
+            encode_extension_end(w)?;
+        }
+        if let Some(t) = &self.future_extensions {
+            error!("Future Extensions Not Yet Supported: {:x?}", t.as_slice())
+        }
+        w.end_seq()?;
+        w.end_ctx()?;
+        Ok(())
+    }
+}
+const MAX_DN_ENTRIES: usize = 5;
+
+#[derive(FromPrimitive, Copy, Clone)]
+enum DnTags {
+    NodeId = 17,
+    FirmwareSignId = 18,
+    IcaId = 19,
+    RootCaId = 20,
+    FabricId = 21,
+    NocCat = 22,
+}
+
+#[derive(Default)]
+struct DistNames {
+    // The order in which the DNs arrive is important, as the signing
+    // requires that the ASN1 notation retains the same order
+    dn: Vec<(u8, u64)>,
+}
+
+impl DistNames {
+    fn u64(&self, match_id: DnTags) -> Option<u64> {
+        self.dn
+            .iter()
+            .find(|(id, _)| *id == match_id as u8)
+            .map(|(_, value)| *value)
+    }
+}
+
+impl<'a> FromTLV<'a> for DistNames {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Self, Error> {
+        let mut d = Self {
+            dn: Vec::with_capacity(MAX_DN_ENTRIES),
+        };
+        let iter = t.confirm_list()?.enter().ok_or(Error::Invalid)?;
+        for t in iter {
+            if let TagType::Context(tag) = t.get_tag() {
+                let value = t.u64().map_err(|e| {
+                    // Non-integer DNs not yet supported
+                    error!("This DN is not yet supported{}", tag);
+                    e
+                })?;
+                d.dn.push((tag, value));
+            }
+        }
+        Ok(d)
+    }
+}
+
+impl ToTLV for DistNames {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        tw.start_list(tag)?;
+        for (name, value) in &self.dn {
+            tw.u64(TagType::Context(*name), *value)?;
+        }
+        tw.end_container()
+    }
+}
+
+impl DistNames {
+    fn encode(&self, tag: &str, w: &mut dyn CertConsumer) -> Result<(), Error> {
+        const OID_MATTER_NODE_ID: [u8; 10] =
+            [0x2B, 0x06, 0x01, 0x04, 0x01, 0x82, 0xA2, 0x7C, 0x01, 0x01];
+        const OID_MATTER_FW_SIGN_ID: [u8; 10] =
+            [0x2B, 0x06, 0x01, 0x04, 0x01, 0x82, 0xA2, 0x7C, 0x01, 0x02];
+        const OID_MATTER_ICA_ID: [u8; 10] =
+            [0x2B, 0x06, 0x01, 0x04, 0x01, 0x82, 0xA2, 0x7C, 0x01, 0x03];
+        const OID_MATTER_ROOT_CA_ID: [u8; 10] =
+            [0x2B, 0x06, 0x01, 0x04, 0x01, 0x82, 0xA2, 0x7C, 0x01, 0x04];
+        const OID_MATTER_FABRIC_ID: [u8; 10] =
+            [0x2B, 0x06, 0x01, 0x04, 0x01, 0x82, 0xA2, 0x7C, 0x01, 0x05];
+        const OID_MATTER_NOC_CAT_ID: [u8; 10] =
+            [0x2B, 0x06, 0x01, 0x04, 0x01, 0x82, 0xA2, 0x7C, 0x01, 0x06];
+
+        let dn_encoding = [
+            ("Chip Node Id:", &OID_MATTER_NODE_ID),
+            ("Chip Firmware Signing Id:", &OID_MATTER_FW_SIGN_ID),
+            ("Chip ICA Id:", &OID_MATTER_ICA_ID),
+            ("Chip Root CA Id:", &OID_MATTER_ROOT_CA_ID),
+            ("Chip Fabric Id:", &OID_MATTER_FABRIC_ID),
+        ];
+
+        w.start_seq(tag)?;
+        for (id, value) in &self.dn {
+            if let Ok(tag) = num::FromPrimitive::from_u8(*id).ok_or(Error::InvalidData) {
+                match tag {
+                    DnTags::NocCat => {
+                        w.start_set("")?;
+                        w.start_seq("")?;
+                        w.oid("Chip NOC CAT Id:", &OID_MATTER_NOC_CAT_ID)?;
+                        w.utf8str("", format!("{:08X}", value).as_str())?;
+                        w.end_seq()?;
+                        w.end_set()?;
+                    }
+                    _ => {
+                        let index: usize = (*id as usize) - (DnTags::NodeId as usize);
+                        let this = &dn_encoding[index];
+                        encode_u64_dn(*value, this.0, this.1, w)?;
+                    }
+                }
+            } else {
+                error!("Non Matter DNs are not yet supported {}", id);
+            }
+        }
+        w.end_seq()?;
+        Ok(())
+    }
+}
+
+fn encode_u64_dn(
+    value: u64,
+    name: &str,
+    oid: &[u8],
+    w: &mut dyn CertConsumer,
+) -> Result<(), Error> {
+    w.start_set("")?;
+    w.start_seq("")?;
+    w.oid(name, oid)?;
+    w.utf8str("", format!("{:016X}", value).as_str())?;
+    w.end_seq()?;
+    w.end_set()
+}
+
+#[derive(FromTLV, ToTLV, Default)]
+#[tlvargs(start = 1)]
+pub struct Cert {
+    serial_no: Vec<u8>,
+    sign_algo: u8,
+    issuer: DistNames,
+    not_before: u32,
+    not_after: u32,
+    subject: DistNames,
+    pubkey_algo: u8,
+    ec_curve_id: u8,
+    pubkey: Vec<u8>,
+    extensions: Extensions,
+    signature: Vec<u8>,
+}
+
+// TODO: Instead of parsing the TLVs everytime, we should just cache this, but the encoding
+// rules in terms of sequence may get complicated. Need to look into this
+impl Cert {
+    pub fn new(cert_bin: &[u8]) -> Result<Self, Error> {
+        let root = tlv::get_root_node(cert_bin)?;
+        Cert::from_tlv(&root)
+    }
+
+    pub fn get_node_id(&self) -> Result<u64, Error> {
+        self.subject.u64(DnTags::NodeId).ok_or(Error::NoNodeId)
+    }
+
+    pub fn get_fabric_id(&self) -> Result<u64, Error> {
+        self.subject.u64(DnTags::FabricId).ok_or(Error::NoFabricId)
+    }
+
+    pub fn get_pubkey(&self) -> &[u8] {
+        self.pubkey.as_slice()
+    }
+
+    pub fn get_subject_key_id(&self) -> Result<&[u8], Error> {
+        self.extensions.subj_key_id.as_deref().ok_or(Error::Invalid)
+    }
+
+    pub fn is_authority(&self, their: &Cert) -> Result<bool, Error> {
+        if let Some(our_auth_key) = &self.extensions.auth_key_id {
+            let their_subject = their.get_subject_key_id()?;
+            if our_auth_key == their_subject {
+                Ok(true)
+            } else {
+                Ok(false)
+            }
+        } else {
+            Ok(false)
+        }
+    }
+
+    pub fn get_signature(&self) -> &[u8] {
+        self.signature.as_slice()
+    }
+
+    pub fn as_tlv(&self, buf: &mut [u8]) -> Result<usize, Error> {
+        let mut wb = WriteBuf::new(buf, buf.len());
+        let mut tw = TLVWriter::new(&mut wb);
+        self.to_tlv(&mut tw, TagType::Anonymous)?;
+        Ok(wb.as_slice().len())
+    }
+
+    pub fn as_asn1(&self, buf: &mut [u8]) -> Result<usize, Error> {
+        let mut w = ASN1Writer::new(buf);
+        let _ = self.encode(&mut w)?;
+        Ok(w.as_slice().len())
+    }
+
+    pub fn verify_chain_start(&self) -> CertVerifier {
+        CertVerifier::new(self)
+    }
+
+    fn encode(&self, w: &mut dyn CertConsumer) -> Result<(), Error> {
+        w.start_seq("")?;
+
+        w.start_ctx("Version:", 0)?;
+        w.integer("", &[2])?;
+        w.end_ctx()?;
+
+        w.integer("Serial Num:", self.serial_no.as_slice())?;
+
+        w.start_seq("Signature Algorithm:")?;
+        let (str, oid) = match get_sign_algo(self.sign_algo).ok_or(Error::Invalid)? {
+            SignAlgoValue::ECDSAWithSHA256 => ("ECDSA with SHA256", OID_ECDSA_WITH_SHA256),
+        };
+        w.oid(str, &oid)?;
+        w.end_seq()?;
+
+        self.issuer.encode("Issuer:", w)?;
+
+        w.start_seq("Validity:")?;
+        w.utctime("Not Before:", self.not_before)?;
+        w.utctime("Not After:", self.not_after)?;
+        w.end_seq()?;
+
+        self.subject.encode("Subject:", w)?;
+
+        w.start_seq("")?;
+        w.start_seq("Public Key Algorithm")?;
+        let (str, pub_key) = match get_pubkey_algo(self.pubkey_algo).ok_or(Error::Invalid)? {
+            PubKeyAlgoValue::EcPubKey => ("ECPubKey", OID_PUB_KEY_ECPUBKEY),
+        };
+        w.oid(str, &pub_key)?;
+        let (str, curve_id) = match get_ec_curve_id(self.ec_curve_id).ok_or(Error::Invalid)? {
+            EcCurveIdValue::Prime256V1 => ("Prime256v1", OID_EC_TYPE_PRIME256V1),
+        };
+        w.oid(str, &curve_id)?;
+        w.end_seq()?;
+
+        w.bitstr("Public-Key:", false, self.pubkey.as_slice())?;
+        w.end_seq()?;
+
+        self.extensions.encode(w)?;
+
+        // We do not encode the Signature in the DER certificate
+
+        w.end_seq()
+    }
+}
+
+impl fmt::Display for Cert {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        let mut printer = CertPrinter::new(f);
+        let _ = self
+            .encode(&mut printer)
+            .map_err(|e| error!("Error decoding certificate: {}", e));
+        // Signature is not encoded by the Cert Decoder
+        writeln!(f, "Signature: {:x?}", self.get_signature())
+    }
+}
+
+pub struct CertVerifier<'a> {
+    cert: &'a Cert,
+}
+
+impl<'a> CertVerifier<'a> {
+    pub fn new(cert: &'a Cert) -> Self {
+        Self { cert }
+    }
+
+    pub fn add_cert(self, parent: &'a Cert) -> Result<CertVerifier<'a>, Error> {
+        if !self.cert.is_authority(parent)? {
+            return Err(Error::InvalidAuthKey);
+        }
+        let mut asn1 = [0u8; MAX_ASN1_CERT_SIZE];
+        let len = self.cert.as_asn1(&mut asn1)?;
+        let asn1 = &asn1[..len];
+
+        let k = KeyPair::new_from_public(parent.get_pubkey())?;
+        k.verify_msg(asn1, self.cert.get_signature()).map_err(|e| {
+            error!(
+                "Error in signature verification of certificate: {:#02x?}",
+                self.cert.get_subject_key_id()
+            );
+            e
+        })?;
+
+        // TODO: other validation checks
+        Ok(CertVerifier::new(parent))
+    }
+
+    pub fn finalise(self) -> Result<(), Error> {
+        let cert = self.cert;
+        self.add_cert(cert)?;
+        Ok(())
+    }
+}
+
+pub trait CertConsumer {
+    fn start_seq(&mut self, tag: &str) -> Result<(), Error>;
+    fn end_seq(&mut self) -> Result<(), Error>;
+    fn integer(&mut self, tag: &str, i: &[u8]) -> Result<(), Error>;
+    fn utf8str(&mut self, tag: &str, s: &str) -> Result<(), Error>;
+    fn bitstr(&mut self, tag: &str, truncate: bool, s: &[u8]) -> Result<(), Error>;
+    fn ostr(&mut self, tag: &str, s: &[u8]) -> Result<(), Error>;
+    fn start_compound_ostr(&mut self, tag: &str) -> Result<(), Error>;
+    fn end_compound_ostr(&mut self) -> Result<(), Error>;
+    fn bool(&mut self, tag: &str, b: bool) -> Result<(), Error>;
+    fn start_set(&mut self, tag: &str) -> Result<(), Error>;
+    fn end_set(&mut self) -> Result<(), Error>;
+    fn ctx(&mut self, tag: &str, id: u8, val: &[u8]) -> Result<(), Error>;
+    fn start_ctx(&mut self, tag: &str, id: u8) -> Result<(), Error>;
+    fn end_ctx(&mut self) -> Result<(), Error>;
+    fn oid(&mut self, tag: &str, oid: &[u8]) -> Result<(), Error>;
+    fn utctime(&mut self, tag: &str, epoch: u32) -> Result<(), Error>;
+}
+
+const MAX_DEPTH: usize = 10;
+const MAX_ASN1_CERT_SIZE: usize = 800;
+
+mod asn1_writer;
+mod printer;
+
+#[cfg(test)]
+mod tests {
+    use crate::cert::Cert;
+    use crate::error::Error;
+    use crate::tlv::{self, FromTLV, TLVWriter, TagType, ToTLV};
+    use crate::utils::writebuf::WriteBuf;
+
+    #[test]
+    fn test_asn1_encode_success() {
+        {
+            let mut asn1_buf = [0u8; 1000];
+            let c = Cert::new(&test_vectors::ASN1_INPUT1).unwrap();
+            let len = c.as_asn1(&mut asn1_buf).unwrap();
+            assert_eq!(&test_vectors::ASN1_OUTPUT1, &asn1_buf[..len]);
+        }
+
+        {
+            let mut asn1_buf = [0u8; 1000];
+            let c = Cert::new(&test_vectors::ASN1_INPUT2).unwrap();
+            let len = c.as_asn1(&mut asn1_buf).unwrap();
+            assert_eq!(&test_vectors::ASN1_OUTPUT2, &asn1_buf[..len]);
+        }
+    }
+
+    #[test]
+    fn test_verify_chain_success() {
+        let noc = Cert::new(&test_vectors::NOC1_SUCCESS).unwrap();
+        let icac = Cert::new(&test_vectors::ICAC1_SUCCESS).unwrap();
+        let rca = Cert::new(&test_vectors::RCA1_SUCCESS).unwrap();
+        let a = noc.verify_chain_start();
+        a.add_cert(&icac)
+            .unwrap()
+            .add_cert(&rca)
+            .unwrap()
+            .finalise()
+            .unwrap();
+    }
+
+    #[test]
+    fn test_verify_chain_incomplete() {
+        // The chain doesn't lead up to a self-signed certificate
+        let noc = Cert::new(&test_vectors::NOC1_SUCCESS).unwrap();
+        let icac = Cert::new(&test_vectors::ICAC1_SUCCESS).unwrap();
+        let a = noc.verify_chain_start();
+        assert_eq!(
+            Err(Error::InvalidAuthKey),
+            a.add_cert(&icac).unwrap().finalise()
+        );
+    }
+
+    #[test]
+    fn test_auth_key_chain_incorrect() {
+        let noc = Cert::new(&test_vectors::NOC1_AUTH_KEY_FAIL).unwrap();
+        let icac = Cert::new(&test_vectors::ICAC1_SUCCESS).unwrap();
+        let a = noc.verify_chain_start();
+        assert_eq!(Err(Error::InvalidAuthKey), a.add_cert(&icac).map(|_| ()));
+    }
+
+    #[test]
+    fn test_cert_corrupted() {
+        let noc = Cert::new(&test_vectors::NOC1_CORRUPT_CERT).unwrap();
+        let icac = Cert::new(&test_vectors::ICAC1_SUCCESS).unwrap();
+        let a = noc.verify_chain_start();
+        assert_eq!(Err(Error::InvalidSignature), a.add_cert(&icac).map(|_| ()));
+    }
+
+    #[test]
+    fn test_tlv_conversions() {
+        let test_input: [&[u8]; 3] = [
+            &test_vectors::NOC1_SUCCESS,
+            &test_vectors::ICAC1_SUCCESS,
+            &test_vectors::RCA1_SUCCESS,
+        ];
+
+        for input in test_input.iter() {
+            println!("Testing next input...");
+            let root = tlv::get_root_node(*input).unwrap();
+            let cert = Cert::from_tlv(&root).unwrap();
+            let mut buf = [0u8; 1024];
+            let buf_len = buf.len();
+            let mut wb = WriteBuf::new(&mut buf, buf_len);
+            let mut tw = TLVWriter::new(&mut wb);
+            cert.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+            assert_eq!(*input, wb.as_slice());
+        }
+    }
+
+    mod test_vectors {
+        // Group 1
+        pub const NOC1_SUCCESS: [u8; 247] = [
+            0x15, 0x30, 0x1, 0x1, 0x1, 0x24, 0x2, 0x1, 0x37, 0x3, 0x24, 0x13, 0x1, 0x24, 0x15, 0x1,
+            0x18, 0x26, 0x4, 0x80, 0x22, 0x81, 0x27, 0x26, 0x5, 0x80, 0x25, 0x4d, 0x3a, 0x37, 0x6,
+            0x26, 0x11, 0x2, 0x5c, 0xbc, 0x0, 0x24, 0x15, 0x1, 0x18, 0x24, 0x7, 0x1, 0x24, 0x8,
+            0x1, 0x30, 0x9, 0x41, 0x4, 0xba, 0x22, 0x56, 0x43, 0x4f, 0x59, 0x98, 0x32, 0x8d, 0xb8,
+            0xcb, 0x3f, 0x24, 0x90, 0x9a, 0x96, 0x94, 0x43, 0x46, 0x67, 0xc2, 0x11, 0xe3, 0x80,
+            0x26, 0x65, 0xfc, 0x65, 0x37, 0x77, 0x3, 0x25, 0x18, 0xd8, 0xdc, 0x85, 0xfa, 0xe6,
+            0x42, 0xe7, 0x55, 0xc9, 0x37, 0xcc, 0xb, 0x78, 0x84, 0x3d, 0x2f, 0xac, 0x81, 0x88,
+            0x2e, 0x69, 0x0, 0xa5, 0xfc, 0xcd, 0xe0, 0xad, 0xb2, 0x69, 0xca, 0x73, 0x37, 0xa, 0x35,
+            0x1, 0x28, 0x1, 0x18, 0x24, 0x2, 0x1, 0x36, 0x3, 0x4, 0x2, 0x4, 0x1, 0x18, 0x30, 0x4,
+            0x14, 0x39, 0x68, 0x16, 0x1e, 0xb5, 0x56, 0x6d, 0xd3, 0xf8, 0x61, 0xf2, 0x95, 0xf3,
+            0x55, 0xa0, 0xfb, 0xd2, 0x82, 0xc2, 0x29, 0x30, 0x5, 0x14, 0xce, 0x60, 0xb4, 0x28,
+            0x96, 0x72, 0x27, 0x64, 0x81, 0xbc, 0x4f, 0x0, 0x78, 0xa3, 0x30, 0x48, 0xfe, 0x6e,
+            0x65, 0x86, 0x18, 0x30, 0xb, 0x40, 0x2, 0x88, 0x42, 0x0, 0x6f, 0xcc, 0xe0, 0xf0, 0x6c,
+            0xd9, 0xf9, 0x5e, 0xe4, 0xc2, 0xaa, 0x1f, 0x57, 0x71, 0x62, 0xdb, 0x6b, 0x4e, 0xe7,
+            0x55, 0x3f, 0xc6, 0xc7, 0x9f, 0xf8, 0x30, 0xeb, 0x16, 0x6e, 0x6d, 0xc6, 0x9c, 0xb,
+            0xb7, 0xe2, 0xb8, 0xe3, 0xe7, 0x57, 0x88, 0x7b, 0xda, 0xe5, 0x79, 0x39, 0x6d, 0x2c,
+            0x37, 0xb2, 0x7f, 0xc3, 0x63, 0x2f, 0x7e, 0x70, 0xab, 0x5a, 0x2c, 0xf7, 0x5b, 0x18,
+        ];
+        pub const ICAC1_SUCCESS: [u8; 237] = [
+            21, 48, 1, 1, 0, 36, 2, 1, 55, 3, 36, 20, 0, 36, 21, 1, 24, 38, 4, 128, 34, 129, 39,
+            38, 5, 128, 37, 77, 58, 55, 6, 36, 19, 1, 36, 21, 1, 24, 36, 7, 1, 36, 8, 1, 48, 9, 65,
+            4, 86, 25, 119, 24, 63, 212, 255, 43, 88, 61, 233, 121, 52, 102, 223, 233, 0, 251, 109,
+            161, 239, 224, 204, 220, 119, 48, 192, 111, 182, 45, 255, 190, 84, 160, 149, 117, 11,
+            139, 7, 188, 85, 219, 156, 182, 85, 19, 8, 184, 223, 2, 227, 64, 107, 174, 52, 245, 12,
+            186, 201, 242, 191, 241, 231, 80, 55, 10, 53, 1, 41, 1, 24, 36, 2, 96, 48, 4, 20, 206,
+            96, 180, 40, 150, 114, 39, 100, 129, 188, 79, 0, 120, 163, 48, 72, 254, 110, 101, 134,
+            48, 5, 20, 212, 86, 147, 190, 112, 121, 244, 156, 112, 107, 7, 111, 17, 28, 109, 229,
+            100, 164, 68, 116, 24, 48, 11, 64, 243, 8, 190, 128, 155, 254, 245, 21, 205, 241, 217,
+            246, 204, 182, 247, 41, 81, 91, 33, 155, 230, 223, 212, 116, 33, 162, 208, 148, 100,
+            89, 175, 253, 78, 212, 7, 69, 207, 140, 45, 129, 249, 64, 104, 70, 68, 43, 164, 19,
+            126, 114, 138, 79, 104, 238, 20, 226, 88, 118, 105, 56, 12, 92, 31, 171, 24,
+        ];
+        // A single byte in the auth key id is changed in this
+        pub const NOC1_AUTH_KEY_FAIL: [u8; 247] = [
+            0x15, 0x30, 0x1, 0x1, 0x1, 0x24, 0x2, 0x1, 0x37, 0x3, 0x24, 0x13, 0x1, 0x24, 0x15, 0x1,
+            0x18, 0x26, 0x4, 0x80, 0x22, 0x81, 0x27, 0x26, 0x5, 0x80, 0x25, 0x4d, 0x3a, 0x37, 0x6,
+            0x26, 0x11, 0x2, 0x5c, 0xbc, 0x0, 0x24, 0x15, 0x1, 0x18, 0x24, 0x7, 0x1, 0x24, 0x8,
+            0x1, 0x30, 0x9, 0x41, 0x4, 0xba, 0x22, 0x56, 0x43, 0x4f, 0x59, 0x98, 0x32, 0x8d, 0xb8,
+            0xcb, 0x3f, 0x24, 0x90, 0x9a, 0x96, 0x94, 0x43, 0x46, 0x67, 0xc2, 0x11, 0xe3, 0x80,
+            0x26, 0x65, 0xfc, 0x65, 0x37, 0x77, 0x3, 0x25, 0x18, 0xd8, 0xdc, 0x85, 0xfa, 0xe6,
+            0x42, 0xe7, 0x55, 0xc9, 0x37, 0xcc, 0xb, 0x78, 0x84, 0x3d, 0x2f, 0xac, 0x81, 0x88,
+            0x2e, 0x69, 0x0, 0xa5, 0xfc, 0xcd, 0xe0, 0xad, 0xb2, 0x69, 0xca, 0x73, 0x37, 0xa, 0x35,
+            0x1, 0x28, 0x1, 0x18, 0x24, 0x2, 0x1, 0x36, 0x3, 0x4, 0x2, 0x4, 0x1, 0x18, 0x30, 0x4,
+            0x14, 0x39, 0x68, 0x16, 0x1e, 0xb5, 0x56, 0x6d, 0xd3, 0xf8, 0x61, 0xf2, 0x95, 0xf3,
+            0x55, 0xa0, 0xfb, 0xd2, 0x82, 0xc2, 0x29, 0x30, 0x5, 0x14, 0xce, 0x61, 0xb4, 0x28,
+            0x96, 0x72, 0x27, 0x64, 0x81, 0xbc, 0x4f, 0x0, 0x78, 0xa3, 0x30, 0x48, 0xfe, 0x6e,
+            0x65, 0x86, 0x18, 0x30, 0xb, 0x40, 0x2, 0x88, 0x42, 0x0, 0x6f, 0xcc, 0xe0, 0xf0, 0x6c,
+            0xd9, 0xf9, 0x5e, 0xe4, 0xc2, 0xaa, 0x1f, 0x57, 0x71, 0x62, 0xdb, 0x6b, 0x4e, 0xe7,
+            0x55, 0x3f, 0xc6, 0xc7, 0x9f, 0xf8, 0x30, 0xeb, 0x16, 0x6e, 0x6d, 0xc6, 0x9c, 0xb,
+            0xb7, 0xe2, 0xb8, 0xe3, 0xe7, 0x57, 0x88, 0x7b, 0xda, 0xe5, 0x79, 0x39, 0x6d, 0x2c,
+            0x37, 0xb2, 0x7f, 0xc3, 0x63, 0x2f, 0x7e, 0x70, 0xab, 0x5a, 0x2c, 0xf7, 0x5b, 0x18,
+        ];
+        // A single byte in the Certificate contents is changed in this
+        pub const NOC1_CORRUPT_CERT: [u8; 247] = [
+            0x15, 0x30, 0x1, 0x1, 0x1, 0x24, 0x2, 0x1, 0x37, 0x3, 0x24, 0x13, 0x1, 0x24, 0x15, 0x1,
+            0x18, 0x26, 0x4, 0x80, 0x22, 0x81, 0x27, 0x26, 0x5, 0x80, 0x25, 0x4d, 0x3a, 0x37, 0x6,
+            0x26, 0x11, 0x2, 0x5c, 0xbc, 0x0, 0x24, 0x15, 0x1, 0x18, 0x24, 0x7, 0x1, 0x24, 0x8,
+            0x1, 0x30, 0x9, 0x41, 0x4, 0xba, 0x23, 0x56, 0x43, 0x4f, 0x59, 0x98, 0x32, 0x8d, 0xb8,
+            0xcb, 0x3f, 0x24, 0x90, 0x9a, 0x96, 0x94, 0x43, 0x46, 0x67, 0xc2, 0x11, 0xe3, 0x80,
+            0x26, 0x65, 0xfc, 0x65, 0x37, 0x77, 0x3, 0x25, 0x18, 0xd8, 0xdc, 0x85, 0xfa, 0xe6,
+            0x42, 0xe7, 0x55, 0xc9, 0x37, 0xcc, 0xb, 0x78, 0x84, 0x3d, 0x2f, 0xac, 0x81, 0x88,
+            0x2e, 0x69, 0x0, 0xa5, 0xfc, 0xcd, 0xe0, 0xad, 0xb2, 0x69, 0xca, 0x73, 0x37, 0xa, 0x35,
+            0x1, 0x28, 0x1, 0x18, 0x24, 0x2, 0x1, 0x36, 0x3, 0x4, 0x2, 0x4, 0x1, 0x18, 0x30, 0x4,
+            0x14, 0x39, 0x68, 0x16, 0x1e, 0xb5, 0x56, 0x6d, 0xd3, 0xf8, 0x61, 0xf2, 0x95, 0xf3,
+            0x55, 0xa0, 0xfb, 0xd2, 0x82, 0xc2, 0x29, 0x30, 0x5, 0x14, 0xce, 0x60, 0xb4, 0x28,
+            0x96, 0x72, 0x27, 0x64, 0x81, 0xbc, 0x4f, 0x0, 0x78, 0xa3, 0x30, 0x48, 0xfe, 0x6e,
+            0x65, 0x86, 0x18, 0x30, 0xb, 0x40, 0x2, 0x88, 0x42, 0x0, 0x6f, 0xcc, 0xe0, 0xf0, 0x6c,
+            0xd9, 0xf9, 0x5e, 0xe4, 0xc2, 0xaa, 0x1f, 0x57, 0x71, 0x62, 0xdb, 0x6b, 0x4e, 0xe7,
+            0x55, 0x3f, 0xc6, 0xc7, 0x9f, 0xf8, 0x30, 0xeb, 0x16, 0x6e, 0x6d, 0xc6, 0x9c, 0xb,
+            0xb7, 0xe2, 0xb8, 0xe3, 0xe7, 0x57, 0x88, 0x7b, 0xda, 0xe5, 0x79, 0x39, 0x6d, 0x2c,
+            0x37, 0xb2, 0x7f, 0xc3, 0x63, 0x2f, 0x7e, 0x70, 0xab, 0x5a, 0x2c, 0xf7, 0x5b, 0x18,
+        ];
+        pub const RCA1_SUCCESS: [u8; 237] = [
+            0x15, 0x30, 0x1, 0x1, 0x0, 0x24, 0x2, 0x1, 0x37, 0x3, 0x24, 0x14, 0x0, 0x24, 0x15, 0x1,
+            0x18, 0x26, 0x4, 0x80, 0x22, 0x81, 0x27, 0x26, 0x5, 0x80, 0x25, 0x4d, 0x3a, 0x37, 0x6,
+            0x24, 0x14, 0x0, 0x24, 0x15, 0x1, 0x18, 0x24, 0x7, 0x1, 0x24, 0x8, 0x1, 0x30, 0x9,
+            0x41, 0x4, 0x6d, 0x70, 0x7e, 0x4b, 0x98, 0xf6, 0x2b, 0xab, 0x44, 0xd6, 0xfe, 0xa3,
+            0x2e, 0x39, 0xd8, 0xc3, 0x0, 0xa0, 0xe, 0xa8, 0x6c, 0x83, 0xff, 0x69, 0xd, 0xe8, 0x42,
+            0x1, 0xeb, 0xd, 0xaa, 0x68, 0x5d, 0xcb, 0x97, 0x2, 0x80, 0x1d, 0xa8, 0x50, 0x2, 0x2e,
+            0x5a, 0xa2, 0x5a, 0x2e, 0x51, 0x26, 0x4, 0xd2, 0x39, 0x62, 0xcd, 0x82, 0x38, 0x63,
+            0x28, 0xbf, 0x15, 0x1c, 0xa6, 0x27, 0xe0, 0xd7, 0x37, 0xa, 0x35, 0x1, 0x29, 0x1, 0x18,
+            0x24, 0x2, 0x60, 0x30, 0x4, 0x14, 0xd4, 0x56, 0x93, 0xbe, 0x70, 0x79, 0xf4, 0x9c, 0x70,
+            0x6b, 0x7, 0x6f, 0x11, 0x1c, 0x6d, 0xe5, 0x64, 0xa4, 0x44, 0x74, 0x30, 0x5, 0x14, 0xd4,
+            0x56, 0x93, 0xbe, 0x70, 0x79, 0xf4, 0x9c, 0x70, 0x6b, 0x7, 0x6f, 0x11, 0x1c, 0x6d,
+            0xe5, 0x64, 0xa4, 0x44, 0x74, 0x18, 0x30, 0xb, 0x40, 0x3, 0xd, 0x77, 0xe1, 0x9e, 0xea,
+            0x9c, 0x5, 0x5c, 0xcc, 0x47, 0xe8, 0xb3, 0x18, 0x1a, 0xd1, 0x74, 0xee, 0xc6, 0x2e,
+            0xa1, 0x20, 0x16, 0xbd, 0x20, 0xb4, 0x3d, 0xac, 0x24, 0xbe, 0x17, 0xf9, 0xe, 0xb7,
+            0x9a, 0x98, 0xc8, 0xbc, 0x6a, 0xce, 0x99, 0x2a, 0x2e, 0x63, 0x4c, 0x76, 0x6, 0x45,
+            0x93, 0xd3, 0x7c, 0x4, 0x0, 0xe4, 0xc7, 0x78, 0xe9, 0x83, 0x5b, 0xc, 0x33, 0x61, 0x5c,
+            0x2e, 0x18,
+        ];
+        pub const ASN1_INPUT1: [u8; 237] = [
+            0x15, 0x30, 0x01, 0x01, 0x00, 0x24, 0x02, 0x01, 0x37, 0x03, 0x24, 0x14, 0x00, 0x24,
+            0x15, 0x03, 0x18, 0x26, 0x04, 0x80, 0x22, 0x81, 0x27, 0x26, 0x05, 0x80, 0x25, 0x4d,
+            0x3a, 0x37, 0x06, 0x24, 0x13, 0x01, 0x24, 0x15, 0x03, 0x18, 0x24, 0x07, 0x01, 0x24,
+            0x08, 0x01, 0x30, 0x09, 0x41, 0x04, 0x69, 0xda, 0xe9, 0x42, 0x88, 0xcf, 0x64, 0x94,
+            0x2d, 0xd5, 0x0a, 0x74, 0x2d, 0x50, 0xe8, 0x5e, 0xbe, 0x15, 0x53, 0x24, 0xe5, 0xc5,
+            0x6b, 0xe5, 0x7f, 0xc1, 0x41, 0x11, 0x21, 0xdd, 0x46, 0xa3, 0x0d, 0x63, 0xc3, 0xe3,
+            0x90, 0x7a, 0x69, 0x64, 0xdd, 0x66, 0x78, 0x10, 0xa6, 0xc8, 0x0f, 0xfd, 0xb6, 0xf2,
+            0x9b, 0x88, 0x50, 0x93, 0x77, 0x9e, 0xf7, 0xb4, 0xda, 0x94, 0x11, 0x33, 0x1e, 0xfe,
+            0x37, 0x0a, 0x35, 0x01, 0x29, 0x01, 0x18, 0x24, 0x02, 0x60, 0x30, 0x04, 0x14, 0xdf,
+            0xfb, 0x79, 0xf1, 0x2b, 0xbf, 0x68, 0x18, 0x59, 0x7f, 0xf7, 0xe8, 0xaf, 0x88, 0x91,
+            0x1c, 0x72, 0x32, 0xf7, 0x52, 0x30, 0x05, 0x14, 0xed, 0x31, 0x5e, 0x1a, 0xb7, 0xb9,
+            0x7a, 0xca, 0x04, 0x79, 0x5d, 0x82, 0x57, 0x7a, 0xd7, 0x0a, 0x75, 0xd0, 0xdb, 0x7a,
+            0x18, 0x30, 0x0b, 0x40, 0xe5, 0xd4, 0xe6, 0x0e, 0x98, 0x62, 0x2f, 0xaa, 0x59, 0xe0,
+            0x28, 0x59, 0xc2, 0xd4, 0xcd, 0x34, 0x85, 0x7f, 0x93, 0xbe, 0x14, 0x35, 0xa3, 0x76,
+            0x8a, 0xc9, 0x2f, 0x59, 0x39, 0xa0, 0xb0, 0x75, 0xe8, 0x8e, 0x11, 0xa9, 0xc1, 0x9e,
+            0xaa, 0xab, 0xa0, 0xdb, 0xb4, 0x79, 0x63, 0xfc, 0x02, 0x03, 0x27, 0x25, 0xac, 0x21,
+            0x6f, 0xef, 0x27, 0xab, 0x0f, 0x90, 0x09, 0x99, 0x05, 0xa8, 0x60, 0xd8, 0x18,
+        ];
+        pub const ASN1_INPUT2: [u8; 247] = [
+            0x15, 0x30, 0x01, 0x01, 0x01, 0x24, 0x02, 0x01, 0x37, 0x03, 0x24, 0x13, 0x01, 0x24,
+            0x15, 0x03, 0x18, 0x26, 0x04, 0x80, 0x22, 0x81, 0x27, 0x26, 0x05, 0x80, 0x25, 0x4d,
+            0x3a, 0x37, 0x06, 0x26, 0x11, 0x69, 0xb6, 0x01, 0x00, 0x24, 0x15, 0x03, 0x18, 0x24,
+            0x07, 0x01, 0x24, 0x08, 0x01, 0x30, 0x09, 0x41, 0x04, 0x93, 0x04, 0xc6, 0xc4, 0xe1,
+            0xbc, 0x9a, 0xc8, 0xf5, 0xb3, 0x7f, 0x83, 0xd6, 0x7f, 0x79, 0xc5, 0x35, 0xdc, 0x7f,
+            0xac, 0x87, 0xca, 0xcd, 0x08, 0x80, 0x4a, 0x55, 0x60, 0x80, 0x09, 0xd3, 0x9b, 0x4a,
+            0xc8, 0xe7, 0x7b, 0x4d, 0x5c, 0x82, 0x88, 0x24, 0xdf, 0x1c, 0xfd, 0xef, 0xb4, 0xbc,
+            0xb7, 0x2f, 0x36, 0xf7, 0x2b, 0xb2, 0xcc, 0x14, 0x69, 0x63, 0xcc, 0x89, 0xd2, 0x74,
+            0x3f, 0xd1, 0x98, 0x37, 0x0a, 0x35, 0x01, 0x28, 0x01, 0x18, 0x24, 0x02, 0x01, 0x36,
+            0x03, 0x04, 0x02, 0x04, 0x01, 0x18, 0x30, 0x04, 0x14, 0x9c, 0xe7, 0xd9, 0xa8, 0x6b,
+            0xf8, 0x71, 0xfa, 0x08, 0x10, 0xa3, 0xf2, 0x3a, 0x95, 0x30, 0xb1, 0x9e, 0xae, 0xc4,
+            0x2c, 0x30, 0x05, 0x14, 0xdf, 0xfb, 0x79, 0xf1, 0x2b, 0xbf, 0x68, 0x18, 0x59, 0x7f,
+            0xf7, 0xe8, 0xaf, 0x88, 0x91, 0x1c, 0x72, 0x32, 0xf7, 0x52, 0x18, 0x30, 0x0b, 0x40,
+            0xcf, 0x01, 0x37, 0x65, 0xd6, 0x8a, 0xca, 0xd8, 0x33, 0x9f, 0x0f, 0x4f, 0xd5, 0xed,
+            0x48, 0x42, 0x91, 0xca, 0xab, 0xf7, 0xae, 0xe1, 0x3b, 0x2b, 0xef, 0x9f, 0x43, 0x5a,
+            0x96, 0xe0, 0xa5, 0x38, 0x8e, 0x39, 0xd0, 0x20, 0x8a, 0x0c, 0x92, 0x2b, 0x21, 0x7d,
+            0xf5, 0x6c, 0x1d, 0x65, 0x6c, 0x0f, 0xd1, 0xe8, 0x55, 0x14, 0x5e, 0x27, 0xfd, 0xa4,
+            0xac, 0xf9, 0x93, 0xdb, 0x29, 0x49, 0xaa, 0x71, 0x18,
+        ];
+
+        pub const ASN1_OUTPUT1: [u8; 388] = [
+            0x30, 0x82, 0x01, 0x80, 0xa0, 0x03, 0x02, 0x01, 0x02, 0x02, 0x01, 0x00, 0x30, 0x0a,
+            0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x04, 0x03, 0x02, 0x30, 0x44, 0x31, 0x20,
+            0x30, 0x1e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x01, 0x04,
+            0x0c, 0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x31, 0x20, 0x30, 0x1e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04,
+            0x01, 0x82, 0xa2, 0x7c, 0x01, 0x05, 0x0c, 0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x33, 0x30, 0x1e, 0x17, 0x0d,
+            0x32, 0x31, 0x30, 0x31, 0x30, 0x31, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x17,
+            0x0d, 0x33, 0x30, 0x31, 0x32, 0x33, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x5a,
+            0x30, 0x44, 0x31, 0x20, 0x30, 0x1e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82,
+            0xa2, 0x7c, 0x01, 0x03, 0x0c, 0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x31, 0x31, 0x20, 0x30, 0x1e, 0x06, 0x0a,
+            0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x01, 0x05, 0x0c, 0x10, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x33,
+            0x30, 0x59, 0x30, 0x13, 0x06, 0x07, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x02, 0x01, 0x06,
+            0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07, 0x03, 0x42, 0x00, 0x04, 0x69,
+            0xda, 0xe9, 0x42, 0x88, 0xcf, 0x64, 0x94, 0x2d, 0xd5, 0x0a, 0x74, 0x2d, 0x50, 0xe8,
+            0x5e, 0xbe, 0x15, 0x53, 0x24, 0xe5, 0xc5, 0x6b, 0xe5, 0x7f, 0xc1, 0x41, 0x11, 0x21,
+            0xdd, 0x46, 0xa3, 0x0d, 0x63, 0xc3, 0xe3, 0x90, 0x7a, 0x69, 0x64, 0xdd, 0x66, 0x78,
+            0x10, 0xa6, 0xc8, 0x0f, 0xfd, 0xb6, 0xf2, 0x9b, 0x88, 0x50, 0x93, 0x77, 0x9e, 0xf7,
+            0xb4, 0xda, 0x94, 0x11, 0x33, 0x1e, 0xfe, 0xa3, 0x63, 0x30, 0x61, 0x30, 0x0f, 0x06,
+            0x03, 0x55, 0x1d, 0x13, 0x01, 0x01, 0xff, 0x04, 0x05, 0x30, 0x03, 0x01, 0x01, 0xff,
+            0x30, 0x0e, 0x06, 0x03, 0x55, 0x1d, 0x0f, 0x01, 0x01, 0xff, 0x04, 0x04, 0x03, 0x02,
+            0x01, 0x06, 0x30, 0x1d, 0x06, 0x03, 0x55, 0x1d, 0x0e, 0x04, 0x16, 0x04, 0x14, 0xdf,
+            0xfb, 0x79, 0xf1, 0x2b, 0xbf, 0x68, 0x18, 0x59, 0x7f, 0xf7, 0xe8, 0xaf, 0x88, 0x91,
+            0x1c, 0x72, 0x32, 0xf7, 0x52, 0x30, 0x1f, 0x06, 0x03, 0x55, 0x1d, 0x23, 0x04, 0x18,
+            0x30, 0x16, 0x80, 0x14, 0xed, 0x31, 0x5e, 0x1a, 0xb7, 0xb9, 0x7a, 0xca, 0x04, 0x79,
+            0x5d, 0x82, 0x57, 0x7a, 0xd7, 0x0a, 0x75, 0xd0, 0xdb, 0x7a,
+        ];
+        pub const ASN1_OUTPUT2: [u8; 421] = [
+            0x30, 0x82, 0x01, 0xa1, 0xa0, 0x03, 0x02, 0x01, 0x02, 0x02, 0x01, 0x01, 0x30, 0x0a,
+            0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x04, 0x03, 0x02, 0x30, 0x44, 0x31, 0x20,
+            0x30, 0x1e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x01, 0x03,
+            0x0c, 0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x31, 0x31, 0x20, 0x30, 0x1e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04,
+            0x01, 0x82, 0xa2, 0x7c, 0x01, 0x05, 0x0c, 0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x33, 0x30, 0x1e, 0x17, 0x0d,
+            0x32, 0x31, 0x30, 0x31, 0x30, 0x31, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x17,
+            0x0d, 0x33, 0x30, 0x31, 0x32, 0x33, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x5a,
+            0x30, 0x44, 0x31, 0x20, 0x30, 0x1e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x04, 0x01, 0x82,
+            0xa2, 0x7c, 0x01, 0x01, 0x0c, 0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x31, 0x42, 0x36, 0x36, 0x39, 0x31, 0x20, 0x30, 0x1e, 0x06, 0x0a,
+            0x2b, 0x06, 0x01, 0x04, 0x01, 0x82, 0xa2, 0x7c, 0x01, 0x05, 0x0c, 0x10, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x33,
+            0x30, 0x59, 0x30, 0x13, 0x06, 0x07, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x02, 0x01, 0x06,
+            0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07, 0x03, 0x42, 0x00, 0x04, 0x93,
+            0x04, 0xc6, 0xc4, 0xe1, 0xbc, 0x9a, 0xc8, 0xf5, 0xb3, 0x7f, 0x83, 0xd6, 0x7f, 0x79,
+            0xc5, 0x35, 0xdc, 0x7f, 0xac, 0x87, 0xca, 0xcd, 0x08, 0x80, 0x4a, 0x55, 0x60, 0x80,
+            0x09, 0xd3, 0x9b, 0x4a, 0xc8, 0xe7, 0x7b, 0x4d, 0x5c, 0x82, 0x88, 0x24, 0xdf, 0x1c,
+            0xfd, 0xef, 0xb4, 0xbc, 0xb7, 0x2f, 0x36, 0xf7, 0x2b, 0xb2, 0xcc, 0x14, 0x69, 0x63,
+            0xcc, 0x89, 0xd2, 0x74, 0x3f, 0xd1, 0x98, 0xa3, 0x81, 0x83, 0x30, 0x81, 0x80, 0x30,
+            0x0c, 0x06, 0x03, 0x55, 0x1d, 0x13, 0x01, 0x01, 0xff, 0x04, 0x02, 0x30, 0x00, 0x30,
+            0x0e, 0x06, 0x03, 0x55, 0x1d, 0x0f, 0x01, 0x01, 0xff, 0x04, 0x04, 0x03, 0x02, 0x07,
+            0x80, 0x30, 0x20, 0x06, 0x03, 0x55, 0x1d, 0x25, 0x01, 0x01, 0xff, 0x04, 0x16, 0x30,
+            0x14, 0x06, 0x08, 0x2b, 0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x02, 0x06, 0x08, 0x2b,
+            0x06, 0x01, 0x05, 0x05, 0x07, 0x03, 0x01, 0x30, 0x1d, 0x06, 0x03, 0x55, 0x1d, 0x0e,
+            0x04, 0x16, 0x04, 0x14, 0x9c, 0xe7, 0xd9, 0xa8, 0x6b, 0xf8, 0x71, 0xfa, 0x08, 0x10,
+            0xa3, 0xf2, 0x3a, 0x95, 0x30, 0xb1, 0x9e, 0xae, 0xc4, 0x2c, 0x30, 0x1f, 0x06, 0x03,
+            0x55, 0x1d, 0x23, 0x04, 0x18, 0x30, 0x16, 0x80, 0x14, 0xdf, 0xfb, 0x79, 0xf1, 0x2b,
+            0xbf, 0x68, 0x18, 0x59, 0x7f, 0xf7, 0xe8, 0xaf, 0x88, 0x91, 0x1c, 0x72, 0x32, 0xf7,
+            0x52,
+        ];
+    }
+}

matter/src/cert/printer.rs

@@ -0,0 +1,132 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::{CertConsumer, MAX_DEPTH};
+use crate::error::Error;
+use chrono::{TimeZone, Utc};
+use std::fmt;
+
+pub struct CertPrinter<'a, 'b> {
+    level: usize,
+    f: &'b mut fmt::Formatter<'a>,
+}
+
+impl<'a, 'b> CertPrinter<'a, 'b> {
+    pub fn new(f: &'b mut fmt::Formatter<'a>) -> Self {
+        Self { level: 0, f }
+    }
+}
+
+const SPACE: [&str; MAX_DEPTH] = [
+    "",
+    "",
+    "    ",
+    "        ",
+    "            ",
+    "                ",
+    "                    ",
+    "                        ",
+    "                            ",
+    "                                ",
+];
+
+impl<'a, 'b> CertConsumer for CertPrinter<'a, 'b> {
+    fn start_seq(&mut self, tag: &str) -> Result<(), Error> {
+        if !tag.is_empty() {
+            let _ = writeln!(self.f, "{} {}", SPACE[self.level], tag);
+        }
+        self.level += 1;
+        Ok(())
+    }
+    fn end_seq(&mut self) -> Result<(), Error> {
+        self.level -= 1;
+        Ok(())
+    }
+    fn integer(&mut self, tag: &str, i: &[u8]) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {} {:x?}", SPACE[self.level], tag, i);
+        Ok(())
+    }
+    fn utf8str(&mut self, tag: &str, s: &str) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {} {:x?}", SPACE[self.level], tag, s);
+        Ok(())
+    }
+    fn bitstr(&mut self, tag: &str, _truncate: bool, s: &[u8]) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {} {:x?}", SPACE[self.level], tag, s);
+        Ok(())
+    }
+    fn ostr(&mut self, tag: &str, s: &[u8]) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {} {:x?}", SPACE[self.level], tag, s);
+        Ok(())
+    }
+    fn start_compound_ostr(&mut self, tag: &str) -> Result<(), Error> {
+        if !tag.is_empty() {
+            let _ = writeln!(self.f, "{} {}", SPACE[self.level], tag);
+        }
+        self.level += 1;
+        Ok(())
+    }
+    fn end_compound_ostr(&mut self) -> Result<(), Error> {
+        self.level -= 1;
+        Ok(())
+    }
+    fn bool(&mut self, tag: &str, b: bool) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {} {}", SPACE[self.level], tag, b);
+        Ok(())
+    }
+    fn start_set(&mut self, tag: &str) -> Result<(), Error> {
+        if !tag.is_empty() {
+            let _ = writeln!(self.f, "{} {}", SPACE[self.level], tag);
+        }
+        self.level += 1;
+        Ok(())
+    }
+    fn end_set(&mut self) -> Result<(), Error> {
+        self.level -= 1;
+        Ok(())
+    }
+    fn ctx(&mut self, tag: &str, id: u8, val: &[u8]) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {}[{}]{:x?}", SPACE[self.level], tag, id, val);
+        Ok(())
+    }
+    fn start_ctx(&mut self, tag: &str, val: u8) -> Result<(), Error> {
+        let _ = writeln!(self.f, "{} {} [{}]", SPACE[self.level], tag, val);
+        self.level += 1;
+        Ok(())
+    }
+    fn end_ctx(&mut self) -> Result<(), Error> {
+        self.level -= 1;
+        Ok(())
+    }
+    fn oid(&mut self, tag: &str, _oid: &[u8]) -> Result<(), Error> {
+        if !tag.is_empty() {
+            let _ = writeln!(self.f, "{} {}", SPACE[self.level], tag);
+        }
+        Ok(())
+    }
+    fn utctime(&mut self, tag: &str, epoch: u32) -> Result<(), Error> {
+        let mut matter_epoch = Utc.ymd(2000, 1, 1).and_hms(0, 0, 0).timestamp();
+        matter_epoch += epoch as i64;
+        let _ = writeln!(
+            self.f,
+            "{} {} {}",
+            SPACE[self.level],
+            tag,
+            Utc.timestamp(matter_epoch, 0)
+        );
+        Ok(())
+    }
+}

matter/src/core.rs

@@ -0,0 +1,106 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    acl::AclMgr,
+    data_model::{
+        cluster_basic_information::BasicInfoConfig, core::DataModel,
+        sdm::dev_att::DevAttDataFetcher,
+    },
+    error::*,
+    fabric::FabricMgr,
+    interaction_model::InteractionModel,
+    mdns::Mdns,
+    secure_channel::core::SecureChannel,
+    transport,
+};
+use std::sync::Arc;
+
+#[derive(Default)]
+/// Device Commissioning Data
+pub struct CommissioningData {
+    /// The commissioning salt
+    pub salt: [u8; 16],
+    /// The password for commissioning the device
+    // TODO: We should replace this with verifier instead of password
+    pub passwd: u32,
+    /// The 12-bit discriminator used to differentiate between multiple devices
+    pub discriminator: u16,
+}
+
+/// The primary Matter Object
+pub struct Matter {
+    transport_mgr: transport::mgr::Mgr,
+    data_model: DataModel,
+    fabric_mgr: Arc<FabricMgr>,
+}
+
+impl Matter {
+    /// Creates a new Matter object
+    ///
+    /// # Parameters
+    /// * dev_att: An object that implements the trait [DevAttDataFetcher]. Any Matter device
+    /// requires a set of device attestation certificates and keys. It is the responsibility of
+    /// this object to return the device attestation details when queried upon.
+    pub fn new(
+        dev_det: BasicInfoConfig,
+        dev_att: Box<dyn DevAttDataFetcher>,
+        dev_comm: CommissioningData,
+    ) -> Result<Box<Matter>, Error> {
+        let mdns = Mdns::get()?;
+        mdns.set_values(dev_det.vid, dev_det.pid, dev_comm.discriminator);
+
+        let fabric_mgr = Arc::new(FabricMgr::new()?);
+        let acl_mgr = Arc::new(AclMgr::new()?);
+        let open_comm_window = fabric_mgr.is_empty();
+        let data_model = DataModel::new(dev_det, dev_att, fabric_mgr.clone(), acl_mgr)?;
+        let mut matter = Box::new(Matter {
+            transport_mgr: transport::mgr::Mgr::new()?,
+            data_model,
+            fabric_mgr,
+        });
+        let interaction_model =
+            Box::new(InteractionModel::new(Box::new(matter.data_model.clone())));
+        matter.transport_mgr.register_protocol(interaction_model)?;
+        let mut secure_channel = Box::new(SecureChannel::new(matter.fabric_mgr.clone()));
+        if open_comm_window {
+            secure_channel.open_comm_window(&dev_comm.salt, dev_comm.passwd)?;
+        }
+
+        matter.transport_mgr.register_protocol(secure_channel)?;
+        Ok(matter)
+    }
+
+    /// Returns an Arc to [DataModel]
+    ///
+    /// The Data Model is where you express what is the type of your device. Typically
+    /// once you gets this reference, you acquire the write lock and add your device
+    /// types, clusters, attributes, commands to the data model.
+    pub fn get_data_model(&self) -> DataModel {
+        self.data_model.clone()
+    }
+
+    /// Starts the Matter daemon
+    ///
+    /// This call does NOT return
+    ///
+    /// This call starts the Matter daemon that starts communication with other Matter
+    /// devices on the network.
+    pub fn start_daemon(&mut self) -> Result<(), Error> {
+        self.transport_mgr.start()
+    }
+}

matter/src/crypto/crypto_dummy.rs

@@ -0,0 +1,57 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use log::error;
+
+use crate::error::Error;
+
+use super::CryptoKeyPair;
+
+pub struct KeyPairDummy {}
+
+impl KeyPairDummy {
+    pub fn new() -> Result<Self, Error> {
+        Ok(Self {})
+    }
+}
+
+impl CryptoKeyPair for KeyPairDummy {
+    fn get_csr<'a>(&self, _out_csr: &'a mut [u8]) -> Result<&'a [u8], Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn get_public_key(&self, _pub_key: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn get_private_key(&self, _pub_key: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn derive_secret(self, _peer_pub_key: &[u8], _secret: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn sign_msg(&self, _msg: &[u8], _signature: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn verify_msg(&self, _msg: &[u8], _signature: &[u8]) -> Result<(), Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+}

matter/src/crypto/crypto_esp_mbedtls.rs

@@ -0,0 +1,133 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use log::error;
+
+use crate::error::Error;
+
+use super::CryptoKeyPair;
+
+pub fn hkdf_sha256(_salt: &[u8], _ikm: &[u8], _info: &[u8], _key: &mut [u8]) -> Result<(), Error> {
+    error!("This API should never get called");
+    Ok(())
+}
+
+#[derive(Clone)]
+pub struct Sha256 {}
+
+impl Sha256 {
+    pub fn new() -> Result<Self, Error> {
+        Ok(Self {})
+    }
+
+    pub fn update(&mut self, _data: &[u8]) -> Result<(), Error> {
+        Ok(())
+    }
+
+    pub fn finish(self, _digest: &mut [u8]) -> Result<(), Error> {
+        Ok(())
+    }
+}
+
+pub struct HmacSha256 {}
+
+impl HmacSha256 {
+    pub fn new(_key: &[u8]) -> Result<Self, Error> {
+        error!("This API should never get called");
+        Ok(Self {})
+    }
+
+    pub fn update(&mut self, _data: &[u8]) -> Result<(), Error> {
+        error!("This API should never get called");
+        Ok(())
+    }
+
+    pub fn finish(self, _out: &mut [u8]) -> Result<(), Error> {
+        error!("This API should never get called");
+        Ok(())
+    }
+}
+
+pub struct KeyPair {}
+
+impl KeyPair {
+    pub fn new() -> Result<Self, Error> {
+        error!("This API should never get called");
+
+        Ok(Self {})
+    }
+
+    pub fn new_from_components(_pub_key: &[u8], priv_key: &[u8]) -> Result<Self, Error> {
+        error!("This API should never get called");
+
+        Ok(Self {})
+    }
+
+    pub fn new_from_public(pub_key: &[u8]) -> Result<Self, Error> {
+        error!("This API should never get called");
+
+        Ok(Self {})
+    }
+}
+
+impl CryptoKeyPair for KeyPair {
+    fn get_csr<'a>(&self, _out_csr: &'a mut [u8]) -> Result<&'a [u8], Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn get_public_key(&self, _pub_key: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn derive_secret(self, _peer_pub_key: &[u8], _secret: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn sign_msg(&self, _msg: &[u8], _signature: &mut [u8]) -> Result<usize, Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+    fn verify_msg(&self, _msg: &[u8], _signature: &[u8]) -> Result<(), Error> {
+        error!("This API should never get called");
+        Err(Error::Invalid)
+    }
+}
+
+pub fn pbkdf2_hmac(pass: &[u8], iter: usize, salt: &[u8], key: &mut [u8]) -> Result<(), Error> {
+    error!("This API should never get called");
+
+    Ok(())
+}
+
+pub fn encrypt_in_place(
+    _key: &[u8],
+    _nonce: &[u8],
+    _ad: &[u8],
+    _data: &mut [u8],
+    _data_len: usize,
+) -> Result<usize, Error> {
+    Ok(0)
+}
+
+pub fn decrypt_in_place(
+    _key: &[u8],
+    _nonce: &[u8],
+    _ad: &[u8],
+    _data: &mut [u8],
+) -> Result<usize, Error> {
+    Ok(0)
+}

matter/src/crypto/crypto_mbedtls.rs

@@ -0,0 +1,379 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::Arc;
+
+use log::error;
+use mbedtls::{
+    bignum::Mpi,
+    cipher::{Authenticated, Cipher},
+    ecp::EcPoint,
+    hash::{self, Hkdf, Hmac, Md, Type},
+    pk::{EcGroup, EcGroupId, Pk},
+    rng::{CtrDrbg, OsEntropy},
+    x509,
+};
+
+use super::CryptoKeyPair;
+use crate::error::Error;
+
+pub struct HmacSha256 {
+    inner: Hmac,
+}
+
+impl HmacSha256 {
+    pub fn new(key: &[u8]) -> Result<Self, Error> {
+        Ok(Self {
+            inner: Hmac::new(Type::Sha256, key)?,
+        })
+    }
+
+    pub fn update(&mut self, data: &[u8]) -> Result<(), Error> {
+        self.inner.update(data).map_err(|_| Error::TLSStack)
+    }
+
+    pub fn finish(self, out: &mut [u8]) -> Result<(), Error> {
+        self.inner.finish(out).map_err(|_| Error::TLSStack)?;
+        Ok(())
+    }
+}
+
+pub struct KeyPair {
+    key: Pk,
+}
+
+impl KeyPair {
+    pub fn new() -> Result<Self, Error> {
+        let mut ctr_drbg = CtrDrbg::new(Arc::new(OsEntropy::new()), None)?;
+        Ok(Self {
+            key: Pk::generate_ec(&mut ctr_drbg, EcGroupId::SecP256R1)?,
+        })
+    }
+
+    pub fn new_from_components(_pub_key: &[u8], priv_key: &[u8]) -> Result<Self, Error> {
+        // No rust-mbedtls API yet for creating keypair from both public and private key
+        let priv_key = Mpi::from_binary(priv_key)?;
+        Ok(Self {
+            key: Pk::private_from_ec_components(EcGroup::new(EcGroupId::SecP256R1)?, priv_key)?,
+        })
+    }
+
+    pub fn new_from_public(pub_key: &[u8]) -> Result<Self, Error> {
+        let group = EcGroup::new(EcGroupId::SecP256R1)?;
+        let pub_key = EcPoint::from_binary(&group, pub_key)?;
+
+        Ok(Self {
+            key: Pk::public_from_ec_components(group, pub_key)?,
+        })
+    }
+}
+
+impl CryptoKeyPair for KeyPair {
+    fn get_csr<'a>(&self, out_csr: &'a mut [u8]) -> Result<&'a [u8], Error> {
+        let tmp_priv = self.key.ec_private()?;
+        let mut tmp_key =
+            Pk::private_from_ec_components(EcGroup::new(EcGroupId::SecP256R1)?, tmp_priv)?;
+
+        let mut builder = x509::csr::Builder::new();
+        builder.key(&mut tmp_key);
+        builder.signature_hash(mbedtls::hash::Type::Sha256);
+        builder.subject("O=CSR")?;
+
+        let mut ctr_drbg = CtrDrbg::new(Arc::new(OsEntropy::new()), None)?;
+        match builder.write_der(out_csr, &mut ctr_drbg) {
+            Ok(Some(a)) => Ok(a),
+            Ok(None) => {
+                error!("Error in writing CSR: None received");
+                Err(Error::Invalid)
+            }
+            Err(e) => {
+                error!("Error in writing CSR {}", e);
+                Err(Error::TLSStack)
+            }
+        }
+    }
+
+    fn get_public_key(&self, pub_key: &mut [u8]) -> Result<usize, Error> {
+        let public_key = self.key.ec_public()?;
+        let group = EcGroup::new(EcGroupId::SecP256R1)?;
+        let vec = public_key.to_binary(&group, false)?;
+
+        let len = vec.len();
+        pub_key[..len].copy_from_slice(vec.as_slice());
+        Ok(len)
+    }
+
+    fn get_private_key(&self, priv_key: &mut [u8]) -> Result<usize, Error> {
+        let priv_key_mpi = self.key.ec_private()?;
+        let vec = priv_key_mpi.to_binary()?;
+
+        let len = vec.len();
+        priv_key[..len].copy_from_slice(vec.as_slice());
+        Ok(len)
+    }
+
+    fn derive_secret(self, peer_pub_key: &[u8], secret: &mut [u8]) -> Result<usize, Error> {
+        // mbedtls requires a 'mut' key. Instead of making a change in our Trait,
+        // we just clone the key this way
+
+        let tmp_key = self.key.ec_private()?;
+        let mut tmp_key =
+            Pk::private_from_ec_components(EcGroup::new(EcGroupId::SecP256R1)?, tmp_key)?;
+
+        let group = EcGroup::new(EcGroupId::SecP256R1)?;
+        let other = EcPoint::from_binary(&group, peer_pub_key)?;
+        let other = Pk::public_from_ec_components(group, other)?;
+
+        let mut ctr_drbg = CtrDrbg::new(Arc::new(OsEntropy::new()), None)?;
+
+        let len = tmp_key.agree(&other, secret, &mut ctr_drbg)?;
+        Ok(len)
+    }
+
+    fn sign_msg(&self, msg: &[u8], signature: &mut [u8]) -> Result<usize, Error> {
+        // mbedtls requires a 'mut' key. Instead of making a change in our Trait,
+        // we just clone the key this way
+        let tmp_key = self.key.ec_private()?;
+        let mut tmp_key =
+            Pk::private_from_ec_components(EcGroup::new(EcGroupId::SecP256R1)?, tmp_key)?;
+
+        // First get the SHA256 of the message
+        let mut msg_hash = [0_u8; super::SHA256_HASH_LEN_BYTES];
+        Md::hash(hash::Type::Sha256, msg, &mut msg_hash)?;
+        let mut ctr_drbg = CtrDrbg::new(Arc::new(OsEntropy::new()), None)?;
+
+        if signature.len() < super::EC_SIGNATURE_LEN_BYTES {
+            return Err(Error::NoSpace);
+        }
+        safemem::write_bytes(signature, 0);
+
+        // mbedTLS writes the DER signature first
+        // TODO: Update rust-mbedtls to provide raw level APIs to get r and s values
+        let mut tmp_sign = [0u8; super::EC_SIGNATURE_LEN_BYTES * 3];
+        tmp_key.sign(hash::Type::Sha256, &msg_hash, &mut tmp_sign, &mut ctr_drbg)?;
+        let len = convert_asn1_sign_to_r_s(&mut tmp_sign)?;
+        signature[..len].copy_from_slice(&tmp_sign[..len]);
+        Ok(len)
+    }
+
+    fn verify_msg(&self, msg: &[u8], signature: &[u8]) -> Result<(), Error> {
+        // mbedtls requires a 'mut' key. Instead of making a change in our Trait,
+        // we just clone the key this way
+        let tmp_key = self.key.ec_public()?;
+        let mut tmp_key =
+            Pk::public_from_ec_components(EcGroup::new(EcGroupId::SecP256R1)?, tmp_key)?;
+
+        // First get the SHA256 of the message
+        let mut msg_hash = [0_u8; super::SHA256_HASH_LEN_BYTES];
+        Md::hash(hash::Type::Sha256, msg, &mut msg_hash)?;
+
+        // current rust-mbedTLS APIs the signature to be in DER format
+        let mut mbedtls_sign = [0u8; super::EC_SIGNATURE_LEN_BYTES * 3];
+        let len = convert_r_s_to_asn1_sign(signature, &mut mbedtls_sign);
+        let mbedtls_sign = &mbedtls_sign[..len];
+
+        if let Err(e) = tmp_key.verify(hash::Type::Sha256, &msg_hash, mbedtls_sign) {
+            println!("The error is {}", e);
+            Err(Error::InvalidSignature)
+        } else {
+            Ok(())
+        }
+    }
+}
+
+fn convert_r_s_to_asn1_sign(signature: &[u8], mbedtls_sign: &mut [u8]) -> usize {
+    let mut offset = 0;
+    mbedtls_sign[offset] = 0x30;
+    offset += 1;
+    let mut len = 68;
+    if (signature[0] & 0x80) == 0x80 {
+        len += 1;
+    }
+    if (signature[32] & 0x80) == 0x80 {
+        len += 1;
+    }
+    mbedtls_sign[offset] = len;
+    offset += 1;
+    mbedtls_sign[offset] = 0x02;
+    offset += 1;
+    if (signature[0] & 0x80) == 0x80 {
+        // It seems if topmost bit is 1, there is an extra 0
+        mbedtls_sign[offset] = 33;
+        offset += 1;
+        mbedtls_sign[offset] = 0;
+        offset += 1;
+    } else {
+        mbedtls_sign[offset] = 32;
+        offset += 1;
+    }
+    mbedtls_sign[offset..(offset + 32)].copy_from_slice(&signature[..32]);
+    offset += 32;
+
+    mbedtls_sign[offset] = 0x02;
+    offset += 1;
+    if (signature[32] & 0x80) == 0x80 {
+        // It seems if topmost bit is 1, there is an extra 0
+        mbedtls_sign[offset] = 33;
+        offset += 1;
+        mbedtls_sign[offset] = 0;
+        offset += 1;
+    } else {
+        mbedtls_sign[offset] = 32;
+        offset += 1;
+    }
+
+    mbedtls_sign[offset..(offset + 32)].copy_from_slice(&signature[32..64]);
+    offset += 32;
+
+    offset
+}
+
+// mbedTLS sign() function directly encodes the signature in ASN1. The lower level function
+// is not yet exposed to us through the Rust crate. So here, I am crudely extracting the 'r'
+// and 's' values from the ASN1 encoding and writing 'r' and 's' back sequentially as is expected
+// per the Matter spec.
+fn convert_asn1_sign_to_r_s(signature: &mut [u8]) -> Result<usize, Error> {
+    if signature[0] == 0x30 {
+        // Type 0x30 ASN1 Sequence
+        // Length: Skip
+        let mut offset: usize = 2;
+
+        // Type 0x2 is Integer (first integer is r)
+        if signature[offset] != 2 {
+            return Err(Error::Invalid);
+        }
+        offset += 1;
+
+        // Length
+        let len = signature[offset];
+        offset += 1;
+        // XXX Once, I have seen a crash in this conversion, need to dig
+        if len < 32 {
+            error!(
+                "Cannot deal with this: this will crash: the slice is: {:x?}",
+                signature
+            );
+        }
+
+        // Sometimes length is more than 32 with a 0 prefix-padded, skip over that
+        offset += (len - 32) as usize;
+
+        // Extract the 32 bytes of 'r'
+        let mut r = [0_u8; super::BIGNUM_LEN_BYTES];
+        r.copy_from_slice(&signature[offset..(offset + 32)]);
+        offset += 32;
+
+        // Type 0x2 is Integer (this integer is s)
+        if signature[offset] != 2 {
+            return Err(Error::Invalid);
+        }
+        offset += 1;
+
+        // Length
+        let len = signature[offset];
+        offset += 1;
+        // Sometimes length is more than 32 with a 0 prefix-padded, skip over that
+        offset += (len - 32) as usize;
+
+        // Extract the 32 bytes of 's'
+        let mut s = [0_u8; super::BIGNUM_LEN_BYTES];
+        s.copy_from_slice(&signature[offset..(offset + 32)]);
+
+        signature[0..32].copy_from_slice(&r);
+        signature[32..64].copy_from_slice(&s);
+
+        Ok(64)
+    } else {
+        Err(Error::Invalid)
+    }
+}
+
+pub fn pbkdf2_hmac(pass: &[u8], iter: usize, salt: &[u8], key: &mut [u8]) -> Result<(), Error> {
+    mbedtls::hash::pbkdf2_hmac(Type::Sha256, pass, salt, iter as u32, key)
+        .map_err(|_e| Error::TLSStack)
+}
+
+pub fn hkdf_sha256(salt: &[u8], ikm: &[u8], info: &[u8], key: &mut [u8]) -> Result<(), Error> {
+    Hkdf::hkdf(Type::Sha256, salt, ikm, info, key).map_err(|_e| Error::TLSStack)
+}
+
+pub fn encrypt_in_place(
+    key: &[u8],
+    nonce: &[u8],
+    ad: &[u8],
+    data: &mut [u8],
+    data_len: usize,
+) -> Result<usize, Error> {
+    let cipher = Cipher::<_, Authenticated, _>::new(
+        mbedtls::cipher::raw::CipherId::Aes,
+        mbedtls::cipher::raw::CipherMode::CCM,
+        (key.len() * 8) as u32,
+    )?;
+    let cipher = cipher.set_key_iv(key, nonce)?;
+    let (data, tag) = data.split_at_mut(data_len);
+    let tag = &mut tag[..super::AEAD_MIC_LEN_BYTES];
+    cipher
+        .encrypt_auth_inplace(ad, data, tag)
+        .map(|(len, _)| len)
+        .map_err(|_e| Error::TLSStack)
+}
+
+pub fn decrypt_in_place(
+    key: &[u8],
+    nonce: &[u8],
+    ad: &[u8],
+    data: &mut [u8],
+) -> Result<usize, Error> {
+    let cipher = Cipher::<_, Authenticated, _>::new(
+        mbedtls::cipher::raw::CipherId::Aes,
+        mbedtls::cipher::raw::CipherMode::CCM,
+        (key.len() * 8) as u32,
+    )?;
+    let cipher = cipher.set_key_iv(key, nonce)?;
+    let data_len = data.len() - super::AEAD_MIC_LEN_BYTES;
+    let (data, tag) = data.split_at_mut(data_len);
+    cipher
+        .decrypt_auth_inplace(ad, data, tag)
+        .map(|(len, _)| len)
+        .map_err(|e| {
+            error!("Error during decryption: {:?}", e);
+            Error::TLSStack
+        })
+}
+
+#[derive(Clone)]
+pub struct Sha256 {
+    ctx: Md,
+}
+
+impl Sha256 {
+    pub fn new() -> Result<Self, Error> {
+        Ok(Self {
+            ctx: Md::new(Type::Sha256)?,
+        })
+    }
+
+    pub fn update(&mut self, data: &[u8]) -> Result<(), Error> {
+        self.ctx.update(data).map_err(|_| Error::TLSStack)?;
+        Ok(())
+    }
+
+    pub fn finish(self, digest: &mut [u8]) -> Result<(), Error> {
+        self.ctx.finish(digest).map_err(|_| Error::TLSStack)?;
+        Ok(())
+    }
+}

matter/src/crypto/crypto_openssl.rs

@@ -0,0 +1,386 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+
+use super::CryptoKeyPair;
+use foreign_types::ForeignTypeRef;
+use log::error;
+use openssl::asn1::Asn1Type;
+use openssl::bn::{BigNum, BigNumContext};
+use openssl::cipher::CipherRef;
+use openssl::cipher_ctx::{CipherCtx, CipherCtxRef};
+use openssl::derive::Deriver;
+use openssl::ec::{EcGroup, EcKey, EcPoint, EcPointRef, PointConversionForm};
+use openssl::ecdsa::EcdsaSig;
+use openssl::error::ErrorStack;
+use openssl::hash::{Hasher, MessageDigest};
+use openssl::md::Md;
+use openssl::nid::Nid;
+use openssl::pkey::{self, Id, PKey, Private};
+use openssl::pkey_ctx::PkeyCtx;
+use openssl::symm::{self};
+use openssl::x509::{X509NameBuilder, X509ReqBuilder, X509};
+
+// We directly use the hmac crate here, there was a self-referential structure
+// problem while using OpenSSL's Signer
+// TODO: Use proper OpenSSL method for this
+use hmac::{Hmac, Mac, NewMac};
+pub struct HmacSha256 {
+    ctx: Hmac<sha2::Sha256>,
+}
+
+impl HmacSha256 {
+    pub fn new(key: &[u8]) -> Result<Self, Error> {
+        Ok(Self {
+            ctx: Hmac::<sha2::Sha256>::new_from_slice(key).map_err(|_x| Error::InvalidKeyLength)?,
+        })
+    }
+
+    pub fn update(&mut self, data: &[u8]) -> Result<(), Error> {
+        Ok(self.ctx.update(data))
+    }
+
+    pub fn finish(self, out: &mut [u8]) -> Result<(), Error> {
+        let a = self.ctx.finalize().into_bytes();
+        out.copy_from_slice(a.as_slice());
+        Ok(())
+    }
+}
+
+pub enum KeyType {
+    Public(EcKey<pkey::Public>),
+    Private(EcKey<pkey::Private>),
+}
+pub struct KeyPair {
+    key: KeyType,
+}
+
+impl KeyPair {
+    pub fn new() -> Result<Self, Error> {
+        let group = EcGroup::from_curve_name(Nid::X9_62_PRIME256V1)?;
+        let key = EcKey::generate(&group)?;
+        Ok(Self {
+            key: KeyType::Private(key),
+        })
+    }
+
+    pub fn new_from_components(pub_key: &[u8], priv_key: &[u8]) -> Result<Self, Error> {
+        let group = EcGroup::from_curve_name(Nid::X9_62_PRIME256V1)?;
+        let mut ctx = BigNumContext::new()?;
+        let priv_key = BigNum::from_slice(priv_key)?;
+        let pub_key = EcPoint::from_bytes(&group, pub_key, &mut ctx)?;
+        Ok(Self {
+            key: KeyType::Private(EcKey::from_private_components(&group, &priv_key, &pub_key)?),
+        })
+    }
+
+    pub fn new_from_public(pub_key: &[u8]) -> Result<Self, Error> {
+        let group = EcGroup::from_curve_name(Nid::X9_62_PRIME256V1)?;
+        let mut ctx = BigNumContext::new()?;
+        let pub_key = EcPoint::from_bytes(&group, pub_key, &mut ctx)?;
+
+        Ok(Self {
+            key: KeyType::Public(EcKey::from_public_key(&group, &pub_key)?),
+        })
+    }
+
+    fn public_key_point(&self) -> &EcPointRef {
+        match &self.key {
+            KeyType::Public(k) => k.public_key(),
+            KeyType::Private(k) => k.public_key(),
+        }
+    }
+
+    fn private_key(&self) -> Result<&EcKey<Private>, Error> {
+        match &self.key {
+            KeyType::Public(_) => Err(Error::Invalid),
+            KeyType::Private(k) => Ok(&k),
+        }
+    }
+}
+
+impl CryptoKeyPair for KeyPair {
+    fn get_public_key(&self, pub_key: &mut [u8]) -> Result<usize, Error> {
+        let group = EcGroup::from_curve_name(Nid::X9_62_PRIME256V1)?;
+        let mut bn_ctx = BigNumContext::new()?;
+        let s = self.public_key_point().to_bytes(
+            &group,
+            PointConversionForm::UNCOMPRESSED,
+            &mut bn_ctx,
+        )?;
+        let len = s.len();
+        pub_key[..len].copy_from_slice(s.as_slice());
+        Ok(len)
+    }
+
+    fn get_private_key(&self, priv_key: &mut [u8]) -> Result<usize, Error> {
+        let s = self.private_key()?.private_key().to_vec();
+        let len = s.len();
+        priv_key[..len].copy_from_slice(s.as_slice());
+        Ok(len)
+    }
+
+    fn derive_secret(self, peer_pub_key: &[u8], secret: &mut [u8]) -> Result<usize, Error> {
+        let self_pkey = PKey::from_ec_key(self.private_key()?.clone())?;
+
+        let group = EcGroup::from_curve_name(Nid::X9_62_PRIME256V1)?;
+        let mut ctx = BigNumContext::new()?;
+        let point = EcPoint::from_bytes(&group, peer_pub_key, &mut ctx)?;
+        let peer_key = EcKey::from_public_key(&group, &point)?;
+        let peer_pkey = PKey::from_ec_key(peer_key)?;
+
+        let mut deriver = Deriver::new(&self_pkey)?;
+        deriver.set_peer(&peer_pkey)?;
+        Ok(deriver.derive(secret)?)
+    }
+
+    fn get_csr<'a>(&self, out_csr: &'a mut [u8]) -> Result<&'a [u8], Error> {
+        let mut builder = X509ReqBuilder::new()?;
+        builder.set_version(0)?;
+
+        let pkey = PKey::from_ec_key(self.private_key()?.clone())?;
+        builder.set_pubkey(&pkey)?;
+
+        let mut name_builder = X509NameBuilder::new()?;
+        name_builder.append_entry_by_text_with_type("O", "CSR", Asn1Type::IA5STRING)?;
+        let subject_name = name_builder.build();
+        builder.set_subject_name(&subject_name)?;
+
+        builder.sign(&pkey, MessageDigest::sha256())?;
+
+        let csr_vec = builder.build().to_der()?;
+        let csr = csr_vec.as_slice();
+        if csr.len() < out_csr.len() {
+            let a = &mut out_csr[0..csr.len()];
+            a.copy_from_slice(csr);
+            Ok(a)
+        } else {
+            Err(Error::NoSpace)
+        }
+    }
+
+    fn sign_msg(&self, msg: &[u8], signature: &mut [u8]) -> Result<usize, Error> {
+        // First get the SHA256 of the message
+        let mut h = Hasher::new(MessageDigest::sha256())?;
+        h.update(msg)?;
+        let msg = h.finish()?;
+
+        if signature.len() < super::EC_SIGNATURE_LEN_BYTES {
+            return Err(Error::NoSpace);
+        }
+        safemem::write_bytes(signature, 0);
+
+        let sig = EcdsaSig::sign(&msg, self.private_key()?)?;
+        let r = sig.r().to_vec();
+        signature[0..r.len()].copy_from_slice(r.as_slice());
+        let s = sig.s().to_vec();
+        signature[32..(32 + s.len())].copy_from_slice(s.as_slice());
+        Ok(64)
+    }
+
+    fn verify_msg(&self, msg: &[u8], signature: &[u8]) -> Result<(), Error> {
+        // First get the SHA256 of the message
+        let mut h = Hasher::new(MessageDigest::sha256())?;
+        h.update(msg)?;
+        let msg = h.finish()?;
+
+        let r = BigNum::from_slice(&signature[0..super::BIGNUM_LEN_BYTES])?;
+        let s =
+            BigNum::from_slice(&signature[super::BIGNUM_LEN_BYTES..(2 * super::BIGNUM_LEN_BYTES)])?;
+        let sig = EcdsaSig::from_private_components(r, s)?;
+
+        let k = match &self.key {
+            KeyType::Public(key) => key,
+            _ => {
+                error!("Not yet supported");
+                return Err(Error::Invalid);
+            }
+        };
+        if !sig.verify(&msg, k)? {
+            Err(Error::InvalidSignature)
+        } else {
+            Ok(())
+        }
+    }
+}
+
+const P256_KEY_LEN: usize = 256 / 8;
+pub fn pubkey_from_der<'a>(der: &'a [u8], out_key: &mut [u8]) -> Result<(), Error> {
+    if out_key.len() != P256_KEY_LEN {
+        error!("Insufficient length");
+        Err(Error::NoSpace)
+    } else {
+        let key = X509::from_der(der)?.public_key()?.public_key_to_der()?;
+        let len = key.len();
+        let out_key = &mut out_key[..len];
+        out_key.copy_from_slice(key.as_slice());
+        Ok(())
+    }
+}
+
+pub fn pbkdf2_hmac(pass: &[u8], iter: usize, salt: &[u8], key: &mut [u8]) -> Result<(), Error> {
+    openssl::pkcs5::pbkdf2_hmac(pass, salt, iter, MessageDigest::sha256(), key)
+        .map_err(|_e| Error::TLSStack)
+}
+
+pub fn hkdf_sha256(salt: &[u8], ikm: &[u8], info: &[u8], key: &mut [u8]) -> Result<(), Error> {
+    let mut ctx = PkeyCtx::new_id(Id::HKDF)?;
+    ctx.derive_init()?;
+    ctx.set_hkdf_md(Md::sha256())?;
+    ctx.set_hkdf_key(ikm)?;
+    if !salt.is_empty() {
+        ctx.set_hkdf_salt(salt)?;
+    }
+    ctx.add_hkdf_info(info)?;
+    ctx.derive(Some(key))?;
+    Ok(())
+}
+
+pub fn encrypt_in_place(
+    key: &[u8],
+    nonce: &[u8],
+    ad: &[u8],
+    data: &mut [u8],
+    data_len: usize,
+) -> Result<usize, Error> {
+    let (plain_text, tag) = data.split_at_mut(data_len);
+
+    let result = lowlevel_encrypt_aead(
+        key,
+        Some(nonce),
+        ad,
+        plain_text,
+        &mut tag[..super::AEAD_MIC_LEN_BYTES],
+    )?;
+    data[..data_len].copy_from_slice(result.as_slice());
+    Ok(result.len() + super::AEAD_MIC_LEN_BYTES)
+}
+
+pub fn decrypt_in_place(
+    key: &[u8],
+    nonce: &[u8],
+    ad: &[u8],
+    data: &mut [u8],
+) -> Result<usize, Error> {
+    let tag_start = data.len() - super::AEAD_MIC_LEN_BYTES;
+    let (data, tag) = data.split_at_mut(tag_start);
+    let result = lowlevel_decrypt_aead(key, Some(nonce), ad, data, &tag)?;
+    data[..result.len()].copy_from_slice(result.as_slice());
+    Ok(result.len())
+}
+
+// The default encrypt/decrypt routines in rust-mbedtls have a problem in the ordering of
+// set-tag-length. This causes the CCM tag-length to be use incorrectly.
+// Instead we use the low-level CipherCtx APIs here to get the desired behaviour.
+// More details available here: https://github.com/sfackler/rust-openssl/pull/1594/
+//   Need to pursue this PR when I get a chance
+pub fn lowlevel_encrypt_aead(
+    key: &[u8],
+    iv: Option<&[u8]>,
+    aad: &[u8],
+    data: &[u8],
+    tag: &mut [u8],
+) -> Result<Vec<u8>, ErrorStack> {
+    let t = symm::Cipher::aes_128_ccm();
+    let mut ctx = CipherCtx::new()?;
+    CipherCtxRef::encrypt_init(
+        &mut ctx,
+        Some(unsafe { CipherRef::from_ptr(t.as_ptr() as *mut _) }),
+        None,
+        None,
+    )?;
+
+    ctx.set_tag_length(tag.len())?;
+    ctx.set_key_length(key.len())?;
+    if let (Some(iv), Some(iv_len)) = (iv, t.iv_len()) {
+        if iv.len() != iv_len {
+            ctx.set_iv_length(iv.len())?;
+        }
+    }
+    CipherCtxRef::encrypt_init(&mut ctx, None, Some(key), iv)?;
+
+    let mut out = vec![0; data.len() + t.block_size()];
+    ctx.set_data_len(data.len())?;
+
+    ctx.cipher_update(aad, None)?;
+    let count = ctx.cipher_update(data, Some(&mut out))?;
+    let rest = ctx.cipher_final(&mut out[count..])?;
+    ctx.tag(tag)?;
+    out.truncate(count + rest);
+    Ok(out)
+}
+
+pub fn lowlevel_decrypt_aead(
+    key: &[u8],
+    iv: Option<&[u8]>,
+    aad: &[u8],
+    data: &[u8],
+    tag: &[u8],
+) -> Result<Vec<u8>, ErrorStack> {
+    let t = symm::Cipher::aes_128_ccm();
+    let mut ctx = CipherCtx::new()?;
+    CipherCtxRef::decrypt_init(
+        &mut ctx,
+        Some(unsafe { CipherRef::from_ptr(t.as_ptr() as *mut _) }),
+        None,
+        None,
+    )?;
+
+    ctx.set_tag_length(tag.len())?;
+    ctx.set_key_length(key.len())?;
+    if let (Some(iv), Some(iv_len)) = (iv, t.iv_len()) {
+        if iv.len() != iv_len {
+            ctx.set_iv_length(iv.len())?;
+        }
+    }
+    CipherCtxRef::decrypt_init(&mut ctx, None, Some(key), iv)?;
+
+    let mut out = vec![0; data.len() + t.block_size()];
+
+    ctx.set_tag(tag)?;
+    ctx.set_data_len(data.len())?;
+
+    ctx.cipher_update(aad, None)?;
+    let count = ctx.cipher_update(data, Some(&mut out))?;
+
+    out.truncate(count);
+    Ok(out)
+}
+
+#[derive(Clone)]
+pub struct Sha256 {
+    hasher: Hasher,
+}
+
+impl Sha256 {
+    pub fn new() -> Result<Self, Error> {
+        Ok(Self {
+            hasher: Hasher::new(MessageDigest::sha256())?,
+        })
+    }
+
+    pub fn update(&mut self, data: &[u8]) -> Result<(), Error> {
+        self.hasher.update(data).map_err(|_| Error::TLSStack)
+    }
+
+    pub fn finish(mut self, data: &mut [u8]) -> Result<(), Error> {
+        let h = self.hasher.finish()?;
+        data.copy_from_slice(h.as_ref());
+        Ok(())
+    }
+}

matter/src/crypto/mod.rs

@@ -0,0 +1,164 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+
+pub const SYMM_KEY_LEN_BITS: usize = 128;
+pub const SYMM_KEY_LEN_BYTES: usize = SYMM_KEY_LEN_BITS / 8;
+
+pub const AEAD_MIC_LEN_BITS: usize = 128;
+pub const AEAD_MIC_LEN_BYTES: usize = AEAD_MIC_LEN_BITS / 8;
+
+pub const AEAD_NONCE_LEN_BYTES: usize = 13;
+pub const AEAD_AAD_LEN_BYTES: usize = 8;
+
+pub const SHA256_HASH_LEN_BYTES: usize = 256 / 8;
+
+pub const BIGNUM_LEN_BYTES: usize = 32;
+pub const EC_POINT_LEN_BYTES: usize = 65;
+
+pub const ECDH_SHARED_SECRET_LEN_BYTES: usize = 32;
+
+pub const EC_SIGNATURE_LEN_BYTES: usize = 64;
+
+// APIs particular to a KeyPair so a KeyPair object can be defined
+pub trait CryptoKeyPair {
+    fn get_csr<'a>(&self, csr: &'a mut [u8]) -> Result<&'a [u8], Error>;
+    fn get_public_key(&self, pub_key: &mut [u8]) -> Result<usize, Error>;
+    fn get_private_key(&self, priv_key: &mut [u8]) -> Result<usize, Error>;
+    fn derive_secret(self, peer_pub_key: &[u8], secret: &mut [u8]) -> Result<usize, Error>;
+    fn sign_msg(&self, msg: &[u8], signature: &mut [u8]) -> Result<usize, Error>;
+    fn verify_msg(&self, msg: &[u8], signature: &[u8]) -> Result<(), Error>;
+}
+
+#[cfg(feature = "crypto_esp_mbedtls")]
+mod crypto_esp_mbedtls;
+#[cfg(feature = "crypto_esp_mbedtls")]
+pub use self::crypto_esp_mbedtls::*;
+
+#[cfg(feature = "crypto_mbedtls")]
+mod crypto_mbedtls;
+#[cfg(feature = "crypto_mbedtls")]
+pub use self::crypto_mbedtls::*;
+
+#[cfg(feature = "crypto_openssl")]
+mod crypto_openssl;
+#[cfg(feature = "crypto_openssl")]
+pub use self::crypto_openssl::*;
+
+pub mod crypto_dummy;
+
+#[cfg(test)]
+mod tests {
+    use crate::error::Error;
+
+    use super::{CryptoKeyPair, KeyPair};
+
+    #[test]
+    fn test_verify_msg_success() {
+        let key = KeyPair::new_from_public(&test_vectors::PUB_KEY1).unwrap();
+        key.verify_msg(&test_vectors::MSG1_SUCCESS, &test_vectors::SIGNATURE1)
+            .unwrap();
+    }
+    #[test]
+    fn test_verify_msg_fail() {
+        let key = KeyPair::new_from_public(&test_vectors::PUB_KEY1).unwrap();
+        assert_eq!(
+            key.verify_msg(&test_vectors::MSG1_FAIL, &test_vectors::SIGNATURE1),
+            Err(Error::InvalidSignature)
+        );
+    }
+
+    mod test_vectors {
+        pub const PUB_KEY1: [u8; 65] = [
+            0x4, 0x56, 0x19, 0x77, 0x18, 0x3f, 0xd4, 0xff, 0x2b, 0x58, 0x3d, 0xe9, 0x79, 0x34,
+            0x66, 0xdf, 0xe9, 0x0, 0xfb, 0x6d, 0xa1, 0xef, 0xe0, 0xcc, 0xdc, 0x77, 0x30, 0xc0,
+            0x6f, 0xb6, 0x2d, 0xff, 0xbe, 0x54, 0xa0, 0x95, 0x75, 0xb, 0x8b, 0x7, 0xbc, 0x55, 0xdb,
+            0x9c, 0xb6, 0x55, 0x13, 0x8, 0xb8, 0xdf, 0x2, 0xe3, 0x40, 0x6b, 0xae, 0x34, 0xf5, 0xc,
+            0xba, 0xc9, 0xf2, 0xbf, 0xf1, 0xe7, 0x50,
+        ];
+        pub const MSG1_SUCCESS: [u8; 421] = [
+            0x30, 0x82, 0x1, 0xa1, 0xa0, 0x3, 0x2, 0x1, 0x2, 0x2, 0x1, 0x1, 0x30, 0xa, 0x6, 0x8,
+            0x2a, 0x86, 0x48, 0xce, 0x3d, 0x4, 0x3, 0x2, 0x30, 0x44, 0x31, 0x20, 0x30, 0x1e, 0x6,
+            0xa, 0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x3, 0xc, 0x10, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x31, 0x31,
+            0x20, 0x30, 0x1e, 0x6, 0xa, 0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x5, 0xc,
+            0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x31, 0x30, 0x1e, 0x17, 0xd, 0x32, 0x31, 0x30, 0x31, 0x30, 0x31, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x17, 0xd, 0x33, 0x30, 0x31, 0x32, 0x33, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x30, 0x44, 0x31, 0x20, 0x30, 0x1e, 0x6, 0xa,
+            0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x1, 0xc, 0x10, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x42, 0x43, 0x35, 0x43, 0x30, 0x32, 0x31,
+            0x20, 0x30, 0x1e, 0x6, 0xa, 0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x5, 0xc,
+            0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x31, 0x30, 0x59, 0x30, 0x13, 0x6, 0x7, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x2,
+            0x1, 0x6, 0x8, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x3, 0x1, 0x7, 0x3, 0x42, 0x0, 0x4, 0x6,
+            0x47, 0xf2, 0x86, 0x4d, 0x27, 0x25, 0xdc, 0x1, 0xa, 0x87, 0xde, 0x8d, 0xca, 0x88, 0x37,
+            0xcb, 0x3b, 0xd0, 0xea, 0x93, 0xa6, 0x24, 0x65, 0x8, 0x8f, 0xa1, 0x75, 0xc2, 0xd4,
+            0x41, 0xfa, 0xca, 0x96, 0x54, 0xa3, 0xd8, 0x10, 0x85, 0x73, 0xce, 0x15, 0xa5, 0x38,
+            0xc1, 0xe3, 0xb5, 0x6b, 0x61, 0x1, 0xd3, 0xc4, 0xb7, 0x6b, 0x61, 0x16, 0xc3, 0x77,
+            0x8d, 0xe9, 0xb5, 0x44, 0xac, 0x14, 0xa3, 0x81, 0x83, 0x30, 0x81, 0x80, 0x30, 0xc, 0x6,
+            0x3, 0x55, 0x1d, 0x13, 0x1, 0x1, 0xff, 0x4, 0x2, 0x30, 0x0, 0x30, 0xe, 0x6, 0x3, 0x55,
+            0x1d, 0xf, 0x1, 0x1, 0xff, 0x4, 0x4, 0x3, 0x2, 0x7, 0x80, 0x30, 0x20, 0x6, 0x3, 0x55,
+            0x1d, 0x25, 0x1, 0x1, 0xff, 0x4, 0x16, 0x30, 0x14, 0x6, 0x8, 0x2b, 0x6, 0x1, 0x5, 0x5,
+            0x7, 0x3, 0x2, 0x6, 0x8, 0x2b, 0x6, 0x1, 0x5, 0x5, 0x7, 0x3, 0x1, 0x30, 0x1d, 0x6, 0x3,
+            0x55, 0x1d, 0xe, 0x4, 0x16, 0x4, 0x14, 0xbd, 0xfd, 0x11, 0xac, 0x89, 0xb6, 0xe0, 0x90,
+            0x7a, 0xf6, 0x12, 0x61, 0x78, 0x4d, 0x3d, 0x79, 0x56, 0xeb, 0xc2, 0xdc, 0x30, 0x1f,
+            0x6, 0x3, 0x55, 0x1d, 0x23, 0x4, 0x18, 0x30, 0x16, 0x80, 0x14, 0xce, 0x60, 0xb4, 0x28,
+            0x96, 0x72, 0x27, 0x64, 0x81, 0xbc, 0x4f, 0x0, 0x78, 0xa3, 0x30, 0x48, 0xfe, 0x6e,
+            0x65, 0x86,
+        ];
+        pub const MSG1_FAIL: [u8; 421] = [
+            0x30, 0x82, 0x1, 0xa1, 0xa0, 0x3, 0x2, 0x1, 0x2, 0x2, 0x1, 0x1, 0x30, 0xa, 0x6, 0x8,
+            0x2a, 0x86, 0x48, 0xce, 0x3d, 0x4, 0x3, 0x2, 0x30, 0x44, 0x31, 0x20, 0x30, 0x1e, 0x6,
+            0xa, 0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x3, 0xc, 0x10, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x31, 0x31,
+            0x20, 0x30, 0x1e, 0x6, 0xa, 0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x5, 0xc,
+            0x10, 0x30, 0x30, 0x30, 0x31, 0x32, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x31, 0x30, 0x1e, 0x17, 0xd, 0x32, 0x31, 0x30, 0x31, 0x30, 0x31, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x17, 0xd, 0x33, 0x30, 0x31, 0x32, 0x33, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x5a, 0x30, 0x44, 0x31, 0x20, 0x30, 0x1e, 0x6, 0xa,
+            0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x1, 0xc, 0x10, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x42, 0x43, 0x35, 0x43, 0x30, 0x32, 0x31,
+            0x20, 0x30, 0x1e, 0x6, 0xa, 0x2b, 0x6, 0x1, 0x4, 0x1, 0x82, 0xa2, 0x7c, 0x1, 0x5, 0xc,
+            0x10, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
+            0x30, 0x30, 0x31, 0x30, 0x59, 0x30, 0x13, 0x6, 0x7, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x2,
+            0x1, 0x6, 0x8, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x3, 0x1, 0x7, 0x3, 0x42, 0x0, 0x4, 0x6,
+            0x47, 0xf2, 0x86, 0x4d, 0x27, 0x25, 0xdc, 0x1, 0xa, 0x87, 0xde, 0x8d, 0xca, 0x88, 0x37,
+            0xcb, 0x3b, 0xd0, 0xea, 0x93, 0xa6, 0x24, 0x65, 0x8, 0x8f, 0xa1, 0x75, 0xc2, 0xd4,
+            0x41, 0xfa, 0xca, 0x96, 0x54, 0xa3, 0xd8, 0x10, 0x85, 0x73, 0xce, 0x15, 0xa5, 0x38,
+            0xc1, 0xe3, 0xb5, 0x6b, 0x61, 0x1, 0xd3, 0xc4, 0xb7, 0x6b, 0x61, 0x16, 0xc3, 0x77,
+            0x8d, 0xe9, 0xb5, 0x44, 0xac, 0x14, 0xa3, 0x81, 0x83, 0x30, 0x81, 0x80, 0x30, 0xc, 0x6,
+            0x3, 0x55, 0x1d, 0x13, 0x1, 0x1, 0xff, 0x4, 0x2, 0x30, 0x0, 0x30, 0xe, 0x6, 0x3, 0x55,
+            0x1d, 0xf, 0x1, 0x1, 0xff, 0x4, 0x4, 0x3, 0x2, 0x7, 0x80, 0x30, 0x20, 0x6, 0x3, 0x55,
+            0x1d, 0x25, 0x1, 0x1, 0xff, 0x4, 0x16, 0x30, 0x14, 0x6, 0x8, 0x2b, 0x6, 0x1, 0x5, 0x5,
+            0x7, 0x3, 0x2, 0x6, 0x8, 0x2b, 0x6, 0x1, 0x5, 0x5, 0x7, 0x3, 0x1, 0x30, 0x1d, 0x6, 0x3,
+            0x55, 0x1d, 0xe, 0x4, 0x16, 0x4, 0x14, 0xbd, 0xfd, 0x11, 0xac, 0x89, 0xb6, 0xe0, 0x90,
+            0x7a, 0xf6, 0x12, 0x61, 0x78, 0x4d, 0x3d, 0x79, 0x56, 0xeb, 0xc2, 0xdc, 0x30, 0x1f,
+            0x6, 0x3, 0x55, 0x1d, 0x23, 0x4, 0x18, 0x30, 0x16, 0x80, 0x14, 0xce, 0x60, 0xb4, 0x28,
+            0x96, 0x72, 0x27, 0x64, 0x81, 0xbc, 0x4f, 0x0, 0x78, 0xa3, 0x30, 0x48, 0xfe, 0x6e,
+            0x65, 0x86,
+        ];
+        pub const SIGNATURE1: [u8; 64] = [
+            0x20, 0x16, 0xd0, 0x13, 0x1e, 0xd0, 0xb3, 0x9d, 0x44, 0x25, 0x16, 0xea, 0x9c, 0xf2,
+            0x72, 0x44, 0xd7, 0xb0, 0xf4, 0xae, 0x4a, 0xa4, 0x37, 0x32, 0xcd, 0x6a, 0x79, 0x7a,
+            0x4c, 0x48, 0x3, 0x6d, 0xef, 0xe6, 0x26, 0x82, 0x39, 0x28, 0x9, 0x22, 0xc8, 0x9a, 0xde,
+            0xd5, 0x13, 0x9f, 0xc5, 0x40, 0x25, 0x85, 0x2c, 0x69, 0xe0, 0xdb, 0x6a, 0x79, 0x5b,
+            0x21, 0x82, 0x13, 0xb0, 0x20, 0xb9, 0x69,
+        ];
+    }
+}

matter/src/data_model/cluster_basic_information.rs

@@ -0,0 +1,96 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::objects::*;
+use crate::error::*;
+
+pub const ID: u32 = 0x0028;
+enum Attributes {
+    VendorId = 2,
+    ProductId = 4,
+    HwVer = 7,
+    SwVer = 9,
+}
+
+pub struct BasicInfoConfig {
+    pub vid: u16,
+    pub pid: u16,
+    pub hw_ver: u16,
+    pub sw_ver: u32,
+}
+
+fn attr_vid_new(vid: u16) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::VendorId as u16,
+        AttrValue::Uint16(vid),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+fn attr_pid_new(pid: u16) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::ProductId as u16,
+        AttrValue::Uint16(pid),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+fn attr_hw_ver_new(hw_ver: u16) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::HwVer as u16,
+        AttrValue::Uint16(hw_ver),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+fn attr_sw_ver_new(sw_ver: u32) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::SwVer as u16,
+        AttrValue::Uint32(sw_ver),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+pub struct BasicInfoCluster {
+    base: Cluster,
+}
+
+impl BasicInfoCluster {
+    pub fn new(cfg: BasicInfoConfig) -> Result<Box<Self>, Error> {
+        let mut cluster = Box::new(BasicInfoCluster {
+            base: Cluster::new(ID)?,
+        });
+        cluster.base.add_attribute(attr_vid_new(cfg.vid)?)?;
+        cluster.base.add_attribute(attr_pid_new(cfg.pid)?)?;
+        cluster.base.add_attribute(attr_hw_ver_new(cfg.hw_ver)?)?;
+        cluster.base.add_attribute(attr_sw_ver_new(cfg.sw_ver)?)?;
+        Ok(cluster)
+    }
+}
+
+impl ClusterType for BasicInfoCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+}

matter/src/data_model/cluster_on_off.rs

@@ -0,0 +1,128 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::objects::*;
+use crate::{
+    cmd_enter,
+    error::*,
+    interaction_model::{command::CommandReq, core::IMStatusCode},
+};
+use log::info;
+use num_derive::FromPrimitive;
+
+pub const ID: u32 = 0x0006;
+
+pub enum Attributes {
+    OnOff = 0x0,
+}
+
+#[derive(FromPrimitive)]
+pub enum Commands {
+    Off = 0x0,
+    On = 0x01,
+    Toggle = 0x02,
+}
+
+fn attr_on_off_new() -> Result<Attribute, Error> {
+    // OnOff, Value: false
+    Attribute::new(
+        Attributes::OnOff as u16,
+        AttrValue::Bool(false),
+        Access::RV,
+        Quality::PERSISTENT,
+    )
+}
+
+pub struct OnOffCluster {
+    base: Cluster,
+}
+
+impl OnOffCluster {
+    pub fn new() -> Result<Box<Self>, Error> {
+        let mut cluster = Box::new(OnOffCluster {
+            base: Cluster::new(ID)?,
+        });
+        cluster.base.add_attribute(attr_on_off_new()?)?;
+        Ok(cluster)
+    }
+}
+
+impl ClusterType for OnOffCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+
+    fn handle_command(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        let cmd = cmd_req
+            .cmd
+            .path
+            .leaf
+            .map(num::FromPrimitive::from_u32)
+            .ok_or(IMStatusCode::UnsupportedCommand)?
+            .ok_or(IMStatusCode::UnsupportedCommand)?;
+        match cmd {
+            Commands::Off => {
+                cmd_enter!("Off");
+                let value = self
+                    .base
+                    .read_attribute_raw(Attributes::OnOff as u16)
+                    .unwrap();
+                if AttrValue::Bool(true) == *value {
+                    self.base
+                        .write_attribute_raw(Attributes::OnOff as u16, AttrValue::Bool(false))
+                        .map_err(|_| IMStatusCode::Failure)?;
+                }
+                cmd_req.trans.complete();
+                Err(IMStatusCode::Sucess)
+            }
+            Commands::On => {
+                cmd_enter!("On");
+                let value = self
+                    .base
+                    .read_attribute_raw(Attributes::OnOff as u16)
+                    .unwrap();
+                if AttrValue::Bool(false) == *value {
+                    self.base
+                        .write_attribute_raw(Attributes::OnOff as u16, AttrValue::Bool(true))
+                        .map_err(|_| IMStatusCode::Failure)?;
+                }
+
+                cmd_req.trans.complete();
+                Err(IMStatusCode::Sucess)
+            }
+            Commands::Toggle => {
+                cmd_enter!("Toggle");
+                let value = match self
+                    .base
+                    .read_attribute_raw(Attributes::OnOff as u16)
+                    .unwrap()
+                {
+                    &AttrValue::Bool(v) => v,
+                    _ => false,
+                };
+                self.base
+                    .write_attribute_raw(Attributes::OnOff as u16, AttrValue::Bool(!value))
+                    .map_err(|_| IMStatusCode::Failure)?;
+                cmd_req.trans.complete();
+                Err(IMStatusCode::Sucess)
+            }
+        }
+    }
+}

matter/src/data_model/cluster_template.rs

@@ -0,0 +1,44 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    data_model::objects::{Cluster, ClusterType},
+    error::Error,
+};
+
+const CLUSTER_NETWORK_COMMISSIONING_ID: u32 = 0x0031;
+
+pub struct TemplateCluster {
+    base: Cluster,
+}
+
+impl ClusterType for TemplateCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+}
+
+impl TemplateCluster {
+    pub fn new() -> Result<Box<Self>, Error> {
+        Ok(Box::new(Self {
+            base: Cluster::new(CLUSTER_NETWORK_COMMISSIONING_ID)?,
+        }))
+    }
+}

matter/src/data_model/core.rs

@@ -0,0 +1,443 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::{
+    cluster_basic_information::BasicInfoConfig,
+    device_types::device_type_add_root_node,
+    objects::{self, *},
+    sdm::dev_att::DevAttDataFetcher,
+    system_model::descriptor::DescriptorCluster,
+};
+use crate::{
+    acl::{AccessReq, Accessor, AclMgr, AuthMode},
+    error::*,
+    fabric::FabricMgr,
+    interaction_model::{
+        command::CommandReq,
+        core::IMStatusCode,
+        messages::{
+            ib::{self, AttrData, DataVersionFilter},
+            msg::{self, InvReq, ReadReq, WriteReq},
+            GenericPath,
+        },
+        InteractionConsumer, Transaction,
+    },
+    tlv::{TLVArray, TLVWriter, TagType, ToTLV},
+    transport::session::{Session, SessionMode},
+};
+use log::{error, info};
+use std::sync::{Arc, RwLock};
+
+#[derive(Clone)]
+pub struct DataModel {
+    pub node: Arc<RwLock<Box<Node>>>,
+    acl_mgr: Arc<AclMgr>,
+}
+
+impl DataModel {
+    pub fn new(
+        dev_details: BasicInfoConfig,
+        dev_att: Box<dyn DevAttDataFetcher>,
+        fabric_mgr: Arc<FabricMgr>,
+        acl_mgr: Arc<AclMgr>,
+    ) -> Result<Self, Error> {
+        let dm = DataModel {
+            node: Arc::new(RwLock::new(Node::new()?)),
+            acl_mgr: acl_mgr.clone(),
+        };
+        {
+            let mut node = dm.node.write()?;
+            node.set_changes_cb(Box::new(dm.clone()));
+            device_type_add_root_node(&mut node, dev_details, dev_att, fabric_mgr, acl_mgr)?;
+        }
+        Ok(dm)
+    }
+
+    pub fn read_attribute_raw(
+        &self,
+        endpoint: u16,
+        cluster: u32,
+        attr: u16,
+    ) -> Result<AttrValue, IMStatusCode> {
+        let node = self.node.read().unwrap();
+        let cluster = node.get_cluster(endpoint, cluster)?;
+        cluster.base().read_attribute_raw(attr).map(|a| *a)
+    }
+
+    // Encode a write attribute from a path that may or may not be wildcard
+    fn handle_write_attr_path(
+        node: &mut Node,
+        accessor: &Accessor,
+        attr_data: &AttrData,
+        tw: &mut TLVWriter,
+    ) {
+        let gen_path = attr_data.path.to_gp();
+        let mut encoder = AttrWriteEncoder::new(tw, TagType::Anonymous);
+        encoder.set_path(gen_path);
+
+        // The unsupported pieces of the wildcard path
+        if attr_data.path.cluster.is_none() {
+            encoder.encode_status(IMStatusCode::UnsupportedCluster, 0);
+            return;
+        }
+        if attr_data.path.attr.is_none() {
+            encoder.encode_status(IMStatusCode::UnsupportedAttribute, 0);
+            return;
+        }
+
+        // Get the data
+        let write_data = match &attr_data.data {
+            EncodeValue::Closure(_) | EncodeValue::Value(_) => {
+                error!("Not supported");
+                return;
+            }
+            EncodeValue::Tlv(t) => t,
+        };
+
+        if gen_path.is_wildcard() {
+            // This is a wildcard path, skip error
+            //    This is required because there could be access control errors too that need
+            //    to be taken care of.
+            encoder.skip_error();
+        }
+        let mut attr = AttrDetails {
+            // will be udpated in the loop below
+            attr_id: 0,
+            list_index: attr_data.path.list_index,
+            fab_filter: false,
+            fab_idx: accessor.fab_idx,
+        };
+
+        let result = node.for_each_cluster_mut(&gen_path, |path, c| {
+            if attr_data.data_ver.is_some() && Some(c.base().get_dataver()) != attr_data.data_ver {
+                encoder.encode_status(IMStatusCode::DataVersionMismatch, 0);
+                return Ok(());
+            }
+
+            attr.attr_id = path.leaf.unwrap_or_default() as u16;
+            encoder.set_path(*path);
+            let mut access_req = AccessReq::new(accessor, path, Access::WRITE);
+            let r = match Cluster::write_attribute(c, &mut access_req, write_data, &attr) {
+                Ok(_) => IMStatusCode::Sucess,
+                Err(e) => e,
+            };
+            encoder.encode_status(r, 0);
+            Ok(())
+        });
+        if let Err(e) = result {
+            // We hit this only if this is a non-wildcard path and some parts of the path are missing
+            encoder.encode_status(e, 0);
+        }
+    }
+
+    // Encode a read attribute from a path that may or may not be wildcard
+    fn handle_read_attr_path(
+        node: &Node,
+        accessor: &Accessor,
+        attr_encoder: &mut AttrReadEncoder,
+        attr_details: &mut AttrDetails,
+    ) {
+        let path = attr_encoder.path;
+        // Skip error reporting for wildcard paths, don't for concrete paths
+        attr_encoder.skip_error(path.is_wildcard());
+
+        let result = node.for_each_attribute(&path, |path, c| {
+            // Ignore processing if data filter matches.
+            // For a wildcard attribute, this may end happening unnecessarily for all attributes, although
+            // a single skip for the cluster is sufficient. That requires us to replace this for_each with a
+            // for_each_cluster
+            let cluster_data_ver = c.base().get_dataver();
+            if Self::data_filter_matches(&attr_encoder.data_ver_filters, path, cluster_data_ver) {
+                return Ok(());
+            }
+
+            attr_details.attr_id = path.leaf.unwrap_or_default() as u16;
+            // Overwrite the previous path with the concrete path
+            attr_encoder.set_path(*path);
+            // Set the cluster's data version
+            attr_encoder.set_data_ver(cluster_data_ver);
+            let mut access_req = AccessReq::new(accessor, path, Access::READ);
+            Cluster::read_attribute(c, &mut access_req, attr_encoder, &attr_details);
+            Ok(())
+        });
+        if let Err(e) = result {
+            // We hit this only if this is a non-wildcard path
+            attr_encoder.encode_status(e, 0);
+        }
+    }
+
+    // Handle command from a path that may or may not be wildcard
+    fn handle_command_path(node: &mut Node, cmd_req: &mut CommandReq) {
+        let wildcard = cmd_req.cmd.path.is_wildcard();
+        let path = cmd_req.cmd.path;
+
+        let result = node.for_each_cluster_mut(&path, |path, c| {
+            cmd_req.cmd.path = *path;
+            let result = c.handle_command(cmd_req);
+            if let Err(e) = result {
+                // It is likely that we might have to do an 'Access' aware traversal
+                // if there are other conditions in the wildcard scenario that shouldn't be
+                // encoded as CmdStatus
+                if !(wildcard && e == IMStatusCode::UnsupportedCommand) {
+                    let invoke_resp = ib::InvResp::status_new(cmd_req.cmd, e, 0);
+                    let _ = invoke_resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+                }
+            }
+            Ok(())
+        });
+        if !wildcard {
+            if let Err(e) = result {
+                // We hit this only if this is a non-wildcard path
+                let invoke_resp = ib::InvResp::status_new(cmd_req.cmd, e, 0);
+                let _ = invoke_resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+            }
+        }
+    }
+
+    fn sess_to_accessor(&self, sess: &Session) -> Accessor {
+        match sess.get_session_mode() {
+            SessionMode::Case(c) => Accessor::new(
+                c,
+                sess.get_peer_node_id().unwrap_or_default(),
+                AuthMode::Case,
+                self.acl_mgr.clone(),
+            ),
+            SessionMode::Pase => Accessor::new(0, 1, AuthMode::Pase, self.acl_mgr.clone()),
+            SessionMode::PlainText => Accessor::new(0, 1, AuthMode::Invalid, self.acl_mgr.clone()),
+        }
+    }
+
+    /// Returns true if the path matches the cluster path and the data version is a match
+    fn data_filter_matches(
+        filters: &Option<&TLVArray<DataVersionFilter>>,
+        path: &GenericPath,
+        data_ver: u32,
+    ) -> bool {
+        if let Some(filters) = *filters {
+            for filter in filters.iter() {
+                // TODO: No handling of 'node' comparision yet
+                if Some(filter.path.endpoint) == path.endpoint
+                    && Some(filter.path.cluster) == path.cluster
+                    && filter.data_ver == data_ver
+                {
+                    return true;
+                }
+            }
+        }
+        false
+    }
+}
+
+impl objects::ChangeConsumer for DataModel {
+    fn endpoint_added(&self, id: u16, endpoint: &mut Endpoint) -> Result<(), Error> {
+        endpoint.add_cluster(DescriptorCluster::new(id, self.clone())?)?;
+        Ok(())
+    }
+}
+
+impl InteractionConsumer for DataModel {
+    fn consume_write_attr(
+        &self,
+        write_req: &WriteReq,
+        trans: &mut Transaction,
+        tw: &mut TLVWriter,
+    ) -> Result<(), Error> {
+        let accessor = self.sess_to_accessor(trans.session);
+
+        tw.start_array(TagType::Context(msg::WriteRespTag::WriteResponses as u8))?;
+        let mut node = self.node.write().unwrap();
+        for attr_data in write_req.write_requests.iter() {
+            DataModel::handle_write_attr_path(&mut node, &accessor, &attr_data, tw);
+        }
+        tw.end_container()?;
+
+        Ok(())
+    }
+
+    fn consume_read_attr(
+        &self,
+        read_req: &ReadReq,
+        trans: &mut Transaction,
+        tw: &mut TLVWriter,
+    ) -> Result<(), Error> {
+        let mut attr_encoder = AttrReadEncoder::new(tw);
+        if let Some(filters) = &read_req.dataver_filters {
+            attr_encoder.set_data_ver_filters(filters);
+        }
+
+        let mut attr_details = AttrDetails {
+            // This will be updated internally
+            attr_id: 0,
+            // This will be updated internally
+            list_index: None,
+            // This will be updated internally
+            fab_idx: 0,
+            fab_filter: read_req.fabric_filtered,
+        };
+
+        if let Some(attr_requests) = &read_req.attr_requests {
+            let accessor = self.sess_to_accessor(trans.session);
+            let node = self.node.read().unwrap();
+            attr_encoder
+                .tw
+                .start_array(TagType::Context(msg::ReportDataTag::AttributeReports as u8))?;
+
+            for attr_path in attr_requests.iter() {
+                attr_encoder.set_path(attr_path.to_gp());
+                // Extract the attr_path fields into various structures
+                attr_details.list_index = attr_path.list_index;
+                attr_details.fab_idx = accessor.fab_idx;
+                DataModel::handle_read_attr_path(
+                    &node,
+                    &accessor,
+                    &mut attr_encoder,
+                    &mut attr_details,
+                );
+            }
+            tw.end_container()?;
+        }
+        Ok(())
+    }
+
+    fn consume_invoke_cmd(
+        &self,
+        inv_req_msg: &InvReq,
+        trans: &mut Transaction,
+        tw: &mut TLVWriter,
+    ) -> Result<(), Error> {
+        let mut node = self.node.write().unwrap();
+        if let Some(inv_requests) = &inv_req_msg.inv_requests {
+            // Array of InvokeResponse IBs
+            tw.start_array(TagType::Context(msg::InvRespTag::InvokeResponses as u8))?;
+            for i in inv_requests.iter() {
+                let data = if let Some(data) = i.data.unwrap_tlv() {
+                    data
+                } else {
+                    continue;
+                };
+                info!("Invoke Commmand Handler executing: {:?}", i.path);
+                let mut cmd_req = CommandReq {
+                    cmd: i.path,
+                    data,
+                    trans,
+                    resp: tw,
+                };
+                DataModel::handle_command_path(&mut node, &mut cmd_req);
+            }
+            tw.end_container()?;
+        }
+
+        Ok(())
+    }
+}
+
+/// Encoder for generating a response to a read request
+pub struct AttrReadEncoder<'a, 'b, 'c> {
+    tw: &'a mut TLVWriter<'b, 'c>,
+    data_ver: u32,
+    path: GenericPath,
+    skip_error: bool,
+    data_ver_filters: Option<&'a TLVArray<'a, DataVersionFilter>>,
+}
+
+impl<'a, 'b, 'c> AttrReadEncoder<'a, 'b, 'c> {
+    pub fn new(tw: &'a mut TLVWriter<'b, 'c>) -> Self {
+        Self {
+            tw,
+            data_ver: 0,
+            skip_error: false,
+            path: Default::default(),
+            data_ver_filters: None,
+        }
+    }
+
+    pub fn skip_error(&mut self, skip: bool) {
+        self.skip_error = skip;
+    }
+
+    pub fn set_data_ver(&mut self, data_ver: u32) {
+        self.data_ver = data_ver;
+    }
+
+    pub fn set_data_ver_filters(&mut self, filters: &'a TLVArray<'a, DataVersionFilter>) {
+        self.data_ver_filters = Some(filters);
+    }
+
+    pub fn set_path(&mut self, path: GenericPath) {
+        self.path = path;
+    }
+}
+
+impl<'a, 'b, 'c> Encoder for AttrReadEncoder<'a, 'b, 'c> {
+    fn encode(&mut self, value: EncodeValue) {
+        let resp = ib::AttrResp::Data(ib::AttrData::new(
+            Some(self.data_ver),
+            ib::AttrPath::new(&self.path),
+            value,
+        ));
+        let _ = resp.to_tlv(self.tw, TagType::Anonymous);
+    }
+
+    fn encode_status(&mut self, status: IMStatusCode, cluster_status: u16) {
+        if !self.skip_error {
+            let resp =
+                ib::AttrResp::Status(ib::AttrStatus::new(&self.path, status, cluster_status));
+            let _ = resp.to_tlv(self.tw, TagType::Anonymous);
+        }
+    }
+}
+
+/// Encoder for generating a response to a write request
+pub struct AttrWriteEncoder<'a, 'b, 'c> {
+    tw: &'a mut TLVWriter<'b, 'c>,
+    tag: TagType,
+    path: GenericPath,
+    skip_error: bool,
+}
+impl<'a, 'b, 'c> AttrWriteEncoder<'a, 'b, 'c> {
+    pub fn new(tw: &'a mut TLVWriter<'b, 'c>, tag: TagType) -> Self {
+        Self {
+            tw,
+            tag,
+            path: Default::default(),
+            skip_error: false,
+        }
+    }
+
+    pub fn skip_error(&mut self) {
+        self.skip_error = true;
+    }
+
+    pub fn set_path(&mut self, path: GenericPath) {
+        self.path = path;
+    }
+}
+
+impl<'a, 'b, 'c> Encoder for AttrWriteEncoder<'a, 'b, 'c> {
+    fn encode(&mut self, _value: EncodeValue) {
+        // Only status encodes for AttrWriteResponse
+    }
+
+    fn encode_status(&mut self, status: IMStatusCode, cluster_status: u16) {
+        if self.skip_error && status != IMStatusCode::Sucess {
+            // Don't encode errors
+            return;
+        }
+        let resp = ib::AttrStatus::new(&self.path, status, cluster_status);
+        let _ = resp.to_tlv(self.tw, self.tag);
+    }
+}

matter/src/data_model/device_types.rs

@@ -0,0 +1,66 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::cluster_basic_information::BasicInfoCluster;
+use super::cluster_basic_information::BasicInfoConfig;
+use super::cluster_on_off::OnOffCluster;
+use super::objects::*;
+use super::sdm::dev_att::DevAttDataFetcher;
+use super::sdm::general_commissioning::GenCommCluster;
+use super::sdm::noc::NocCluster;
+use super::sdm::nw_commissioning::NwCommCluster;
+use super::system_model::access_control::AccessControlCluster;
+use crate::acl::AclMgr;
+use crate::error::*;
+use crate::fabric::FabricMgr;
+use std::sync::Arc;
+use std::sync::RwLockWriteGuard;
+
+type WriteNode<'a> = RwLockWriteGuard<'a, Box<Node>>;
+
+pub fn device_type_add_root_node(
+    node: &mut WriteNode,
+    dev_info: BasicInfoConfig,
+    dev_att: Box<dyn DevAttDataFetcher>,
+    fabric_mgr: Arc<FabricMgr>,
+    acl_mgr: Arc<AclMgr>,
+) -> Result<u32, Error> {
+    // Add the root endpoint
+    let endpoint = node.add_endpoint()?;
+    if endpoint != 0 {
+        // Somehow endpoint 0 was already added, this shouldn't be the case
+        return Err(Error::Invalid);
+    };
+    // Add the mandatory clusters
+    node.add_cluster(0, BasicInfoCluster::new(dev_info)?)?;
+    let general_commissioning = GenCommCluster::new()?;
+    let failsafe = general_commissioning.failsafe();
+    node.add_cluster(0, general_commissioning)?;
+    node.add_cluster(0, NwCommCluster::new()?)?;
+    node.add_cluster(
+        0,
+        NocCluster::new(dev_att, fabric_mgr, acl_mgr.clone(), failsafe)?,
+    )?;
+    node.add_cluster(0, AccessControlCluster::new(acl_mgr)?)?;
+    Ok(endpoint)
+}
+
+pub fn device_type_add_on_off_light(node: &mut WriteNode) -> Result<u32, Error> {
+    let endpoint = node.add_endpoint()?;
+    node.add_cluster(endpoint, OnOffCluster::new()?)?;
+    Ok(endpoint)
+}

matter/src/data_model/mod.rs

@@ -0,0 +1,26 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod core;
+pub mod device_types;
+pub mod objects;
+
+pub mod cluster_basic_information;
+pub mod cluster_on_off;
+pub mod cluster_template;
+pub mod sdm;
+pub mod system_model;

matter/src/data_model/objects/attribute.rs

@@ -0,0 +1,274 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::{GlobalElements, Privilege};
+use crate::{
+    error::*,
+    // TODO: This layer shouldn't really depend on the TLV layer, should create an abstraction layer
+    tlv::{TLVElement, TLVWriter, TagType, ToTLV},
+};
+use bitflags::bitflags;
+use log::error;
+use std::fmt::{self, Debug, Formatter};
+
+bitflags! {
+    #[derive(Default)]
+    pub struct Access: u16 {
+        // These must match the bits in the Privilege object :-|
+        const NEED_VIEW = 0x00001;
+        const NEED_OPERATE = 0x0002;
+        const NEED_MANAGE = 0x0004;
+        const NEED_ADMIN = 0x0008;
+
+        const READ = 0x0010;
+        const WRITE = 0x0020;
+        const FAB_SCOPED = 0x0040;
+        const FAB_SENSITIVE = 0x0080;
+        const TIMED_ONLY = 0x0100;
+
+        const READ_PRIVILEGE_MASK = Self::NEED_VIEW.bits | Self::NEED_MANAGE.bits | Self::NEED_OPERATE.bits | Self::NEED_ADMIN.bits;
+        const WRITE_PRIVILEGE_MASK = Self::NEED_MANAGE.bits | Self::NEED_OPERATE.bits | Self::NEED_ADMIN.bits;
+        const RV = Self::READ.bits | Self::NEED_VIEW.bits;
+        const RWVA = Self::READ.bits | Self::WRITE.bits | Self::NEED_VIEW.bits | Self::NEED_ADMIN.bits;
+        const RWFA = Self::READ.bits | Self::WRITE.bits | Self::FAB_SCOPED.bits | Self::NEED_ADMIN.bits;
+        const RWVM = Self::READ.bits | Self::WRITE.bits | Self::NEED_VIEW.bits | Self::NEED_MANAGE.bits;
+    }
+}
+
+impl Access {
+    pub fn is_ok(&self, operation: Access, privilege: Privilege) -> bool {
+        let required = if operation.contains(Access::READ) {
+            *self & Access::READ_PRIVILEGE_MASK
+        } else if operation.contains(Access::WRITE) {
+            *self & Access::WRITE_PRIVILEGE_MASK
+        } else {
+            return false;
+        };
+
+        if required.is_empty() {
+            // There must be some required privilege for any object
+            return false;
+        }
+
+        if ((privilege.bits() as u16) & required.bits()) == 0 {
+            return false;
+        }
+
+        self.contains(operation)
+    }
+}
+
+bitflags! {
+    #[derive(Default)]
+    pub struct Quality: u8 {
+        const NONE = 0x00;
+        const SCENE = 0x01;
+        const PERSISTENT = 0x02;
+        const FIXED = 0x03;
+        const NULLABLE = 0x04;
+    }
+}
+
+/* This file needs some major revamp.
+ * - instead of allocating all over the heap, we should use some kind of slab/block allocator
+ * - instead of arrays, can use linked-lists to conserve space and avoid the internal fragmentation
+ */
+
+#[derive(PartialEq, Copy, Clone)]
+pub enum AttrValue {
+    Int64(i64),
+    Uint8(u8),
+    Uint16(u16),
+    Uint32(u32),
+    Uint64(u64),
+    Bool(bool),
+    Custom,
+}
+
+impl Debug for AttrValue {
+    fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), std::fmt::Error> {
+        match &self {
+            AttrValue::Int64(v) => write!(f, "{:?}", *v),
+            AttrValue::Uint8(v) => write!(f, "{:?}", *v),
+            AttrValue::Uint16(v) => write!(f, "{:?}", *v),
+            AttrValue::Uint32(v) => write!(f, "{:?}", *v),
+            AttrValue::Uint64(v) => write!(f, "{:?}", *v),
+            AttrValue::Bool(v) => write!(f, "{:?}", *v),
+            AttrValue::Custom => write!(f, "custom-attribute"),
+        }?;
+        Ok(())
+    }
+}
+
+impl ToTLV for AttrValue {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag_type: TagType) -> Result<(), Error> {
+        // What is the time complexity of such long match statements?
+        match self {
+            AttrValue::Bool(v) => tw.bool(tag_type, *v),
+            AttrValue::Uint8(v) => tw.u8(tag_type, *v),
+            AttrValue::Uint16(v) => tw.u16(tag_type, *v),
+            AttrValue::Uint32(v) => tw.u32(tag_type, *v),
+            AttrValue::Uint64(v) => tw.u64(tag_type, *v),
+            _ => {
+                error!("Attribute type not yet supported");
+                Err(Error::AttributeNotFound)
+            }
+        }
+    }
+}
+
+impl AttrValue {
+    pub fn update_from_tlv(&mut self, tr: &TLVElement) -> Result<(), Error> {
+        match self {
+            AttrValue::Bool(v) => *v = tr.bool()?,
+            AttrValue::Uint8(v) => *v = tr.u8()?,
+            AttrValue::Uint16(v) => *v = tr.u16()?,
+            AttrValue::Uint32(v) => *v = tr.u32()?,
+            AttrValue::Uint64(v) => *v = tr.u64()?,
+            _ => {
+                error!("Attribute type not yet supported");
+                return Err(Error::AttributeNotFound);
+            }
+        }
+        Ok(())
+    }
+}
+
+#[derive(Debug)]
+pub struct Attribute {
+    pub(super) id: u16,
+    pub(super) value: AttrValue,
+    pub(super) quality: Quality,
+    pub(super) access: Access,
+}
+
+impl Default for Attribute {
+    fn default() -> Attribute {
+        Attribute {
+            id: 0,
+            value: AttrValue::Bool(true),
+            quality: Default::default(),
+            access: Default::default(),
+        }
+    }
+}
+
+impl Attribute {
+    pub fn new(
+        id: u16,
+        value: AttrValue,
+        access: Access,
+        quality: Quality,
+    ) -> Result<Attribute, Error> {
+        Ok(Attribute {
+            id,
+            value,
+            access,
+            quality,
+        })
+    }
+
+    pub fn set_value(&mut self, value: AttrValue) -> Result<(), Error> {
+        if !self.quality.contains(Quality::FIXED) {
+            self.value = value;
+            Ok(())
+        } else {
+            Err(Error::Invalid)
+        }
+    }
+
+    pub fn is_system_attr(attr_id: u16) -> bool {
+        attr_id >= (GlobalElements::ServerGenCmd as u16)
+    }
+}
+
+impl std::fmt::Display for Attribute {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "{}: {:?}", self.id, self.value)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::Access;
+    use crate::data_model::objects::Privilege;
+
+    #[test]
+    fn test_read() {
+        let c = Access::READ;
+        // Read without NEED_VIEW, implies No Read is possible
+        assert_eq!(c.is_ok(Access::READ, Privilege::VIEW), false);
+
+        let c = Access::WRITE | Access::NEED_VIEW;
+        // Read without Read, implies No Read is possible
+        assert_eq!(c.is_ok(Access::READ, Privilege::VIEW), false);
+
+        let c = Access::RV;
+        // Read with View or Admin privilege
+        assert_eq!(c.is_ok(Access::READ, Privilege::VIEW), true);
+        assert_eq!(c.is_ok(Access::READ, Privilege::ADMIN), true);
+
+        let c = Access::READ | Access::NEED_ADMIN;
+        // Read without Admin privilege
+        assert_eq!(c.is_ok(Access::READ, Privilege::VIEW), false);
+        assert_eq!(c.is_ok(Access::READ, Privilege::OPERATE), false);
+        assert_eq!(c.is_ok(Access::READ, Privilege::MANAGE), false);
+        assert_eq!(c.is_ok(Access::READ, Privilege::ADMIN), true);
+
+        let c = Access::READ | Access::NEED_OPERATE;
+        // Read without Operate privilege
+        assert_eq!(c.is_ok(Access::READ, Privilege::VIEW), false);
+        assert_eq!(c.is_ok(Access::READ, Privilege::OPERATE), true);
+        assert_eq!(c.is_ok(Access::READ, Privilege::MANAGE), true);
+        assert_eq!(c.is_ok(Access::READ, Privilege::ADMIN), true);
+    }
+
+    #[test]
+    fn test_write() {
+        let c = Access::WRITE;
+        // Write NEED_*, implies No Write is possible
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::VIEW), false);
+
+        let c = Access::READ | Access::NEED_MANAGE;
+        // Write without Write, implies No Write is possible
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::MANAGE), false);
+
+        let c = Access::RWVA;
+        // Write with View and Admin privilege
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::VIEW), false);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::ADMIN), true);
+
+        let c = Access::RWVA;
+        // WRITE without Admin privilege
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::VIEW), false);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::OPERATE), false);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::MANAGE), false);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::ADMIN), true);
+        // Read with View Privilege
+        assert_eq!(c.is_ok(Access::READ, Privilege::VIEW), true);
+        assert_eq!(c.is_ok(Access::READ, Privilege::OPERATE), true);
+        assert_eq!(c.is_ok(Access::READ, Privilege::MANAGE), true);
+        assert_eq!(c.is_ok(Access::READ, Privilege::ADMIN), true);
+
+        let c = Access::WRITE | Access::NEED_OPERATE;
+        // WRITE without Operate privilege
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::VIEW), false);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::OPERATE), true);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::MANAGE), true);
+        assert_eq!(c.is_ok(Access::WRITE, Privilege::ADMIN), true);
+    }
+}

matter/src/data_model/objects/cluster.rs

@@ -0,0 +1,329 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    acl::AccessReq,
+    data_model::objects::{Access, AttrValue, Attribute, EncodeValue, Quality},
+    error::*,
+    interaction_model::{command::CommandReq, core::IMStatusCode},
+    // TODO: This layer shouldn't really depend on the TLV layer, should create an abstraction layer
+    tlv::{Nullable, TLVElement, TLVWriter, TagType},
+};
+use log::error;
+use num_derive::FromPrimitive;
+use rand::Rng;
+use std::fmt::{self, Debug};
+
+use super::Encoder;
+
+pub const ATTRS_PER_CLUSTER: usize = 8;
+pub const CMDS_PER_CLUSTER: usize = 8;
+
+#[derive(FromPrimitive, Debug)]
+pub enum GlobalElements {
+    _ClusterRevision = 0xFFFD,
+    FeatureMap = 0xFFFC,
+    AttributeList = 0xFFFB,
+    _EventList = 0xFFFA,
+    _ClientGenCmd = 0xFFF9,
+    ServerGenCmd = 0xFFF8,
+    FabricIndex = 0xFE,
+}
+
+// TODO: What if we instead of creating this, we just pass the AttrData/AttrPath to the read/write
+// methods?
+pub struct AttrDetails {
+    pub attr_id: u16,
+    pub list_index: Option<Nullable<u16>>,
+    pub fab_idx: u8,
+    pub fab_filter: bool,
+}
+
+pub trait ClusterType {
+    // TODO: 5 methods is going to be quite expensive for vtables of all the clusters
+    fn base(&self) -> &Cluster;
+    fn base_mut(&mut self) -> &mut Cluster;
+    fn read_custom_attribute(&self, _encoder: &mut dyn Encoder, _attr: &AttrDetails) {}
+
+    fn handle_command(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        let cmd = cmd_req.cmd.path.leaf.map(|a| a as u16);
+        println!("Received command: {:?}", cmd);
+
+        Err(IMStatusCode::UnsupportedCommand)
+    }
+
+    /// Write an attribute
+    ///
+    /// Note that if this method is defined, you must handle the write for all the attributes. Even those
+    /// that are not 'custom'. This is different from how you handle the read_custom_attribute() method.
+    /// The reason for this being, you may want to handle an attribute write request even though it is a
+    /// standard attribute like u16, u32 etc.
+    ///
+    /// If you wish to update the standard attribute in the data model database, you must call the
+    /// write_attribute_from_tlv() method from the base cluster, as is shown here in the default case
+    fn write_attribute(
+        &mut self,
+        attr: &AttrDetails,
+        data: &TLVElement,
+    ) -> Result<(), IMStatusCode> {
+        self.base_mut().write_attribute_from_tlv(attr.attr_id, data)
+    }
+}
+
+pub struct Cluster {
+    pub(super) id: u32,
+    attributes: Vec<Attribute>,
+    feature_map: Option<u32>,
+    data_ver: u32,
+}
+
+impl Cluster {
+    pub fn new(id: u32) -> Result<Cluster, Error> {
+        let mut c = Cluster {
+            id,
+            attributes: Vec::with_capacity(ATTRS_PER_CLUSTER),
+            feature_map: None,
+            data_ver: rand::thread_rng().gen_range(0..0xFFFFFFFF),
+        };
+        c.add_default_attributes()?;
+        Ok(c)
+    }
+
+    pub fn id(&self) -> u32 {
+        self.id
+    }
+
+    pub fn get_dataver(&self) -> u32 {
+        self.data_ver
+    }
+
+    pub fn set_feature_map(&mut self, map: u32) -> Result<(), Error> {
+        if self.feature_map.is_none() {
+            self.add_attribute(Attribute::new(
+                GlobalElements::FeatureMap as u16,
+                AttrValue::Uint32(map),
+                Access::RV,
+                Quality::NONE,
+            )?)?;
+        } else {
+            self.write_attribute_raw(GlobalElements::FeatureMap as u16, AttrValue::Uint32(map))
+                .map_err(|_| Error::Invalid)?;
+        }
+        self.feature_map = Some(map);
+        Ok(())
+    }
+
+    fn add_default_attributes(&mut self) -> Result<(), Error> {
+        self.add_attribute(Attribute::new(
+            GlobalElements::AttributeList as u16,
+            AttrValue::Custom,
+            Access::RV,
+            Quality::NONE,
+        )?)
+    }
+
+    pub fn add_attribute(&mut self, attr: Attribute) -> Result<(), Error> {
+        if self.attributes.len() < self.attributes.capacity() {
+            self.attributes.push(attr);
+            Ok(())
+        } else {
+            Err(Error::NoSpace)
+        }
+    }
+
+    fn get_attribute_index(&self, attr_id: u16) -> Option<usize> {
+        self.attributes.iter().position(|c| c.id == attr_id)
+    }
+
+    fn get_attribute(&self, attr_id: u16) -> Result<&Attribute, Error> {
+        let index = self
+            .get_attribute_index(attr_id)
+            .ok_or(Error::AttributeNotFound)?;
+        Ok(&self.attributes[index])
+    }
+
+    fn get_attribute_mut(&mut self, attr_id: u16) -> Result<&mut Attribute, Error> {
+        let index = self
+            .get_attribute_index(attr_id)
+            .ok_or(Error::AttributeNotFound)?;
+        Ok(&mut self.attributes[index])
+    }
+
+    // Returns a slice of attribute, with either a single attribute or all (wildcard)
+    pub fn get_wildcard_attribute(
+        &self,
+        attribute: Option<u16>,
+    ) -> Result<(&[Attribute], bool), IMStatusCode> {
+        if let Some(a) = attribute {
+            if let Some(i) = self.get_attribute_index(a) {
+                Ok((&self.attributes[i..i + 1], false))
+            } else {
+                Err(IMStatusCode::UnsupportedAttribute)
+            }
+        } else {
+            Ok((&self.attributes[..], true))
+        }
+    }
+
+    pub fn read_attribute(
+        c: &dyn ClusterType,
+        access_req: &mut AccessReq,
+        encoder: &mut dyn Encoder,
+        attr: &AttrDetails,
+    ) {
+        let mut error = IMStatusCode::Sucess;
+        let base = c.base();
+        let a = if let Ok(a) = base.get_attribute(attr.attr_id) {
+            a
+        } else {
+            encoder.encode_status(IMStatusCode::UnsupportedAttribute, 0);
+            return;
+        };
+
+        if !a.access.contains(Access::READ) {
+            error = IMStatusCode::UnsupportedRead;
+        }
+
+        access_req.set_target_perms(a.access);
+        if !access_req.allow() {
+            error = IMStatusCode::UnsupportedAccess;
+        }
+
+        if error != IMStatusCode::Sucess {
+            encoder.encode_status(error, 0);
+        } else if Attribute::is_system_attr(attr.attr_id) {
+            c.base().read_system_attribute(encoder, a)
+        } else if a.value != AttrValue::Custom {
+            encoder.encode(EncodeValue::Value(&a.value))
+        } else {
+            c.read_custom_attribute(encoder, attr)
+        }
+    }
+
+    fn encode_attribute_ids(&self, tag: TagType, tw: &mut TLVWriter) {
+        let _ = tw.start_array(tag);
+        for a in &self.attributes {
+            let _ = tw.u16(TagType::Anonymous, a.id);
+        }
+        let _ = tw.end_container();
+    }
+
+    fn read_system_attribute(&self, encoder: &mut dyn Encoder, attr: &Attribute) {
+        let global_attr: Option<GlobalElements> = num::FromPrimitive::from_u16(attr.id);
+        if let Some(global_attr) = global_attr {
+            match global_attr {
+                GlobalElements::AttributeList => {
+                    encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                        self.encode_attribute_ids(tag, tw)
+                    }));
+                    return;
+                }
+                GlobalElements::FeatureMap => {
+                    let val = if let Some(m) = self.feature_map { m } else { 0 };
+                    encoder.encode(EncodeValue::Value(&val));
+                    return;
+                }
+                _ => {
+                    error!("This attribute not yet handled {:?}", global_attr);
+                }
+            }
+        }
+        encoder.encode_status(IMStatusCode::UnsupportedAttribute, 0)
+    }
+
+    pub fn read_attribute_raw(&self, attr_id: u16) -> Result<&AttrValue, IMStatusCode> {
+        let a = self
+            .get_attribute(attr_id)
+            .map_err(|_| IMStatusCode::UnsupportedAttribute)?;
+        Ok(&a.value)
+    }
+
+    pub fn write_attribute(
+        c: &mut dyn ClusterType,
+        access_req: &mut AccessReq,
+        data: &TLVElement,
+        attr: &AttrDetails,
+    ) -> Result<(), IMStatusCode> {
+        let base = c.base_mut();
+        let a = if let Ok(a) = base.get_attribute_mut(attr.attr_id) {
+            a
+        } else {
+            return Err(IMStatusCode::UnsupportedAttribute);
+        };
+
+        if !a.access.contains(Access::WRITE) {
+            return Err(IMStatusCode::UnsupportedWrite);
+        }
+
+        access_req.set_target_perms(a.access);
+        if !access_req.allow() {
+            return Err(IMStatusCode::UnsupportedAccess);
+        }
+
+        c.write_attribute(attr, data)
+    }
+
+    pub fn write_attribute_from_tlv(
+        &mut self,
+        attr_id: u16,
+        data: &TLVElement,
+    ) -> Result<(), IMStatusCode> {
+        let a = self.get_attribute_mut(attr_id)?;
+        if a.value != AttrValue::Custom {
+            let mut value = a.value;
+            value
+                .update_from_tlv(data)
+                .map_err(|_| IMStatusCode::Failure)?;
+            a.set_value(value)
+                .map(|_| {
+                    self.cluster_changed();
+                    ()
+                })
+                .map_err(|_| IMStatusCode::UnsupportedWrite)
+        } else {
+            Err(IMStatusCode::UnsupportedAttribute)
+        }
+    }
+
+    pub fn write_attribute_raw(&mut self, attr_id: u16, value: AttrValue) -> Result<(), Error> {
+        let a = self.get_attribute_mut(attr_id)?;
+        a.set_value(value).map(|_| {
+            self.cluster_changed();
+            ()
+        })
+    }
+
+    /// This method must be called for any changes to the data model
+    ///     Currently this only increments the data version, but we can reuse the same
+    ///     for raising events too
+    pub fn cluster_changed(&mut self) {
+        self.data_ver = self.data_ver.wrapping_add(1);
+    }
+}
+
+impl std::fmt::Display for Cluster {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "id:{}, ", self.id)?;
+        write!(f, "attrs[")?;
+        let mut comma = "";
+        for element in self.attributes.iter() {
+            write!(f, "{} {}", comma, element)?;
+            comma = ",";
+        }
+        write!(f, " ], ")
+    }
+}

matter/src/data_model/objects/encoder.rs

@@ -0,0 +1,117 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::fmt::{Debug, Formatter};
+
+use crate::{
+    error::Error,
+    interaction_model::core::IMStatusCode,
+    tlv::{FromTLV, TLVElement, TLVWriter, TagType, ToTLV},
+};
+use log::error;
+
+// TODO: Should this return an IMStatusCode Error? But if yes, the higher layer
+// may have already started encoding the 'success' headers, we might not to manage
+// the tw.rewind() in that case, if we add this support
+pub type EncodeValueGen<'a> = &'a dyn Fn(TagType, &mut TLVWriter);
+
+#[derive(Copy, Clone)]
+/// A structure for encoding various types of values
+pub enum EncodeValue<'a> {
+    /// This indicates a value that is dynamically generated. This variant
+    /// is typically used in the transmit/to-tlv path where we want to encode a value at
+    /// run time
+    Closure(EncodeValueGen<'a>),
+    /// This indicates a value that is in the TLVElement form. this variant is
+    /// typically used in the receive/from-tlv path where we don't want to decode the
+    /// full value but it can be done at the time of its usage
+    Tlv(TLVElement<'a>),
+    /// This indicates a static value. This variant is typically used in the transmit/
+    /// to-tlv path
+    Value(&'a (dyn ToTLV)),
+}
+
+impl<'a> EncodeValue<'a> {
+    pub fn unwrap_tlv(self) -> Option<TLVElement<'a>> {
+        match self {
+            EncodeValue::Tlv(t) => Some(t),
+            _ => None,
+        }
+    }
+}
+
+impl<'a> PartialEq for EncodeValue<'a> {
+    fn eq(&self, other: &Self) -> bool {
+        match *self {
+            EncodeValue::Closure(_) => {
+                error!("PartialEq not yet supported");
+                false
+            }
+            EncodeValue::Tlv(a) => {
+                if let EncodeValue::Tlv(b) = *other {
+                    a == b
+                } else {
+                    false
+                }
+            }
+            // Just claim false for now
+            EncodeValue::Value(_) => {
+                error!("PartialEq not yet supported");
+                false
+            }
+        }
+    }
+}
+
+impl<'a> Debug for EncodeValue<'a> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), std::fmt::Error> {
+        match *self {
+            EncodeValue::Closure(_) => write!(f, "Contains closure"),
+            EncodeValue::Tlv(t) => write!(f, "{:?}", t),
+            EncodeValue::Value(_) => write!(f, "Contains EncodeValue"),
+        }?;
+        Ok(())
+    }
+}
+
+impl<'a> ToTLV for EncodeValue<'a> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag_type: TagType) -> Result<(), Error> {
+        match self {
+            EncodeValue::Closure(f) => {
+                (f)(tag_type, tw);
+                Ok(())
+            }
+            EncodeValue::Tlv(_) => (panic!("This looks invalid")),
+            EncodeValue::Value(v) => v.to_tlv(tw, tag_type),
+        }
+    }
+}
+
+impl<'a> FromTLV<'a> for EncodeValue<'a> {
+    fn from_tlv(data: &TLVElement<'a>) -> Result<Self, Error> {
+        Ok(EncodeValue::Tlv(*data))
+    }
+}
+
+/// An object that can encode EncodeValue into the necessary hierarchical structure
+/// as expected by the Interaction Model
+pub trait Encoder {
+    /// Encode a given value
+    fn encode(&mut self, value: EncodeValue);
+    /// Encode a status report
+    fn encode_status(&mut self, status: IMStatusCode, cluster_status: u16);
+}

matter/src/data_model/objects/endpoint.rs

@@ -0,0 +1,105 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{data_model::objects::ClusterType, error::*, interaction_model::core::IMStatusCode};
+
+use std::fmt;
+
+pub const CLUSTERS_PER_ENDPT: usize = 7;
+
+pub struct Endpoint {
+    clusters: Vec<Box<dyn ClusterType>>,
+}
+
+impl Endpoint {
+    pub fn new() -> Result<Box<Endpoint>, Error> {
+        Ok(Box::new(Endpoint {
+            clusters: Vec::with_capacity(CLUSTERS_PER_ENDPT),
+        }))
+    }
+
+    pub fn add_cluster(&mut self, cluster: Box<dyn ClusterType>) -> Result<(), Error> {
+        if self.clusters.len() < self.clusters.capacity() {
+            self.clusters.push(cluster);
+            Ok(())
+        } else {
+            Err(Error::NoSpace)
+        }
+    }
+
+    fn get_cluster_index(&self, cluster_id: u32) -> Option<usize> {
+        self.clusters.iter().position(|c| c.base().id == cluster_id)
+    }
+
+    pub fn get_cluster(&self, cluster_id: u32) -> Result<&dyn ClusterType, Error> {
+        let index = self
+            .get_cluster_index(cluster_id)
+            .ok_or(Error::ClusterNotFound)?;
+        Ok(self.clusters[index].as_ref())
+    }
+
+    pub fn get_cluster_mut(&mut self, cluster_id: u32) -> Result<&mut dyn ClusterType, Error> {
+        let index = self
+            .get_cluster_index(cluster_id)
+            .ok_or(Error::ClusterNotFound)?;
+        Ok(self.clusters[index].as_mut())
+    }
+
+    // Returns a slice of clusters, with either a single cluster or all (wildcard)
+    pub fn get_wildcard_clusters(
+        &self,
+        cluster: Option<u32>,
+    ) -> Result<(&[Box<dyn ClusterType>], bool), IMStatusCode> {
+        if let Some(c) = cluster {
+            if let Some(i) = self.get_cluster_index(c) {
+                Ok((&self.clusters[i..i + 1], false))
+            } else {
+                Err(IMStatusCode::UnsupportedCluster)
+            }
+        } else {
+            Ok((self.clusters.as_slice(), true))
+        }
+    }
+
+    // Returns a slice of clusters, with either a single cluster or all (wildcard)
+    pub fn get_wildcard_clusters_mut(
+        &mut self,
+        cluster: Option<u32>,
+    ) -> Result<(&mut [Box<dyn ClusterType>], bool), IMStatusCode> {
+        if let Some(c) = cluster {
+            if let Some(i) = self.get_cluster_index(c) {
+                Ok((&mut self.clusters[i..i + 1], false))
+            } else {
+                Err(IMStatusCode::UnsupportedCluster)
+            }
+        } else {
+            Ok((&mut self.clusters[..], true))
+        }
+    }
+}
+
+impl std::fmt::Display for Endpoint {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "clusters:[")?;
+        let mut comma = "";
+        for element in self.clusters.iter() {
+            write!(f, "{} {{ {} }}", comma, element.base())?;
+            comma = ", ";
+        }
+        write!(f, "]")
+    }
+}

matter/src/data_model/objects/mod.rs

@@ -0,0 +1,34 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+mod attribute;
+pub use attribute::*;
+
+mod cluster;
+pub use cluster::*;
+
+mod endpoint;
+pub use endpoint::*;
+
+mod node;
+pub use node::*;
+
+mod privilege;
+pub use privilege::*;
+
+mod encoder;
+pub use encoder::*;

matter/src/data_model/objects/node.rs

@@ -0,0 +1,290 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    data_model::objects::{ClusterType, Endpoint},
+    error::*,
+    interaction_model::{core::IMStatusCode, messages::GenericPath},
+    // TODO: This layer shouldn't really depend on the TLV layer, should create an abstraction layer
+};
+use std::fmt;
+
+pub trait ChangeConsumer {
+    fn endpoint_added(&self, id: u16, endpoint: &mut Endpoint) -> Result<(), Error>;
+}
+
+pub const ENDPTS_PER_ACC: usize = 3;
+
+#[derive(Default)]
+pub struct Node {
+    endpoints: [Option<Box<Endpoint>>; ENDPTS_PER_ACC],
+    changes_cb: Option<Box<dyn ChangeConsumer>>,
+}
+
+impl std::fmt::Display for Node {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        writeln!(f, "node:")?;
+        for (i, element) in self.endpoints.iter().enumerate() {
+            if let Some(e) = element {
+                writeln!(f, "endpoint {}: {}", i, e)?;
+            }
+        }
+        write!(f, "")
+    }
+}
+
+impl Node {
+    pub fn new() -> Result<Box<Node>, Error> {
+        let node = Box::new(Node::default());
+        Ok(node)
+    }
+
+    pub fn set_changes_cb(&mut self, consumer: Box<dyn ChangeConsumer>) {
+        self.changes_cb = Some(consumer);
+    }
+
+    pub fn add_endpoint(&mut self) -> Result<u32, Error> {
+        let index = self
+            .endpoints
+            .iter()
+            .position(|x| x.is_none())
+            .ok_or(Error::NoSpace)?;
+        let mut endpoint = Endpoint::new()?;
+        if let Some(cb) = &self.changes_cb {
+            cb.endpoint_added(index as u16, &mut endpoint)?;
+        }
+        self.endpoints[index] = Some(endpoint);
+        Ok(index as u32)
+    }
+
+    pub fn get_endpoint(&self, endpoint_id: u16) -> Result<&Endpoint, Error> {
+        if (endpoint_id as usize) < ENDPTS_PER_ACC {
+            let endpoint = self.endpoints[endpoint_id as usize]
+                .as_ref()
+                .ok_or(Error::EndpointNotFound)?;
+            Ok(endpoint)
+        } else {
+            Err(Error::EndpointNotFound)
+        }
+    }
+
+    pub fn get_endpoint_mut(&mut self, endpoint_id: u16) -> Result<&mut Endpoint, Error> {
+        if (endpoint_id as usize) < ENDPTS_PER_ACC {
+            let endpoint = self.endpoints[endpoint_id as usize]
+                .as_mut()
+                .ok_or(Error::EndpointNotFound)?;
+            Ok(endpoint)
+        } else {
+            Err(Error::EndpointNotFound)
+        }
+    }
+
+    pub fn get_cluster_mut(&mut self, e: u16, c: u32) -> Result<&mut dyn ClusterType, Error> {
+        self.get_endpoint_mut(e)?.get_cluster_mut(c)
+    }
+
+    pub fn get_cluster(&self, e: u16, c: u32) -> Result<&dyn ClusterType, Error> {
+        self.get_endpoint(e)?.get_cluster(c)
+    }
+
+    pub fn add_cluster(
+        &mut self,
+        endpoint_id: u32,
+        cluster: Box<dyn ClusterType>,
+    ) -> Result<(), Error> {
+        let endpoint_id = endpoint_id as usize;
+        if endpoint_id < ENDPTS_PER_ACC {
+            self.endpoints[endpoint_id]
+                .as_mut()
+                .ok_or(Error::NoEndpoint)?
+                .add_cluster(cluster)
+        } else {
+            Err(Error::Invalid)
+        }
+    }
+
+    // Returns a slice of endpoints, with either a single endpoint or all (wildcard)
+    pub fn get_wildcard_endpoints(
+        &self,
+        endpoint: Option<u16>,
+    ) -> Result<(&[Option<Box<Endpoint>>], usize, bool), IMStatusCode> {
+        if let Some(e) = endpoint {
+            let e = e as usize;
+            if self.endpoints.len() <= e || self.endpoints[e].is_none() {
+                Err(IMStatusCode::UnsupportedEndpoint)
+            } else {
+                Ok((&self.endpoints[e..e + 1], e, false))
+            }
+        } else {
+            Ok((&self.endpoints[..], 0, true))
+        }
+    }
+
+    pub fn get_wildcard_endpoints_mut(
+        &mut self,
+        endpoint: Option<u16>,
+    ) -> Result<(&mut [Option<Box<Endpoint>>], usize, bool), IMStatusCode> {
+        if let Some(e) = endpoint {
+            let e = e as usize;
+            if self.endpoints.len() <= e || self.endpoints[e].is_none() {
+                Err(IMStatusCode::UnsupportedEndpoint)
+            } else {
+                Ok((&mut self.endpoints[e..e + 1], e, false))
+            }
+        } else {
+            Ok((&mut self.endpoints[..], 0, true))
+        }
+    }
+
+    /// Run a closure for all endpoints as specified in the path
+    ///
+    /// Note that the path is a GenericPath and hence can be a wildcard path. The behaviour
+    /// of this function is to only capture the successful invocations and ignore the erroneous
+    /// ones. This is inline with the expected behaviour for wildcard, where it implies that
+    /// 'please run this operation on this wildcard path "wherever possible"'
+    ///
+    /// It is expected that if the closure that you pass here returns an error it may not reach
+    /// out to the caller, in case there was a wildcard path specified
+    pub fn for_each_endpoint<T>(&self, path: &GenericPath, mut f: T) -> Result<(), IMStatusCode>
+    where
+        T: FnMut(&GenericPath, &Endpoint) -> Result<(), IMStatusCode>,
+    {
+        let mut current_path = *path;
+        let (endpoints, mut endpoint_id, wildcard) = self.get_wildcard_endpoints(path.endpoint)?;
+        for e in endpoints.iter() {
+            if let Some(e) = e {
+                current_path.endpoint = Some(endpoint_id as u16);
+                f(&current_path, e.as_ref())
+                    .or_else(|e| if !wildcard { Err(e) } else { Ok(()) })?;
+            }
+            endpoint_id += 1;
+        }
+        Ok(())
+    }
+
+    /// Run a closure for all endpoints  (mutable) as specified in the path
+    ///
+    /// Note that the path is a GenericPath and hence can be a wildcard path. The behaviour
+    /// of this function is to only capture the successful invocations and ignore the erroneous
+    /// ones. This is inline with the expected behaviour for wildcard, where it implies that
+    /// 'please run this operation on this wildcard path "wherever possible"'
+    ///
+    /// It is expected that if the closure that you pass here returns an error it may not reach
+    /// out to the caller, in case there was a wildcard path specified
+    pub fn for_each_endpoint_mut<T>(
+        &mut self,
+        path: &GenericPath,
+        mut f: T,
+    ) -> Result<(), IMStatusCode>
+    where
+        T: FnMut(&GenericPath, &mut Endpoint) -> Result<(), IMStatusCode>,
+    {
+        let mut current_path = *path;
+        let (endpoints, mut endpoint_id, wildcard) =
+            self.get_wildcard_endpoints_mut(path.endpoint)?;
+        for e in endpoints.iter_mut() {
+            if let Some(e) = e {
+                current_path.endpoint = Some(endpoint_id as u16);
+                f(&current_path, e.as_mut())
+                    .or_else(|e| if !wildcard { Err(e) } else { Ok(()) })?;
+            }
+            endpoint_id += 1;
+        }
+        Ok(())
+    }
+
+    /// Run a closure for all clusters as specified in the path
+    ///
+    /// Note that the path is a GenericPath and hence can be a wildcard path. The behaviour
+    /// of this function is to only capture the successful invocations and ignore the erroneous
+    /// ones. This is inline with the expected behaviour for wildcard, where it implies that
+    /// 'please run this operation on this wildcard path "wherever possible"'
+    ///
+    /// It is expected that if the closure that you pass here returns an error it may not reach
+    /// out to the caller, in case there was a wildcard path specified
+    pub fn for_each_cluster<T>(&self, path: &GenericPath, mut f: T) -> Result<(), IMStatusCode>
+    where
+        T: FnMut(&GenericPath, &dyn ClusterType) -> Result<(), IMStatusCode>,
+    {
+        self.for_each_endpoint(path, |p, e| {
+            let mut current_path = *p;
+            let (clusters, wildcard) = e.get_wildcard_clusters(p.cluster)?;
+            for c in clusters.iter() {
+                current_path.cluster = Some(c.base().id);
+                f(&current_path, c.as_ref())
+                    .or_else(|e| if !wildcard { Err(e) } else { Ok(()) })?;
+            }
+            Ok(())
+        })
+    }
+
+    /// Run a closure for all clusters (mutable) as specified in the path
+    ///
+    /// Note that the path is a GenericPath and hence can be a wildcard path. The behaviour
+    /// of this function is to only capture the successful invocations and ignore the erroneous
+    /// ones. This is inline with the expected behaviour for wildcard, where it implies that
+    /// 'please run this operation on this wildcard path "wherever possible"'
+    ///
+    /// It is expected that if the closure that you pass here returns an error it may not reach
+    /// out to the caller, in case there was a wildcard path specified
+    pub fn for_each_cluster_mut<T>(
+        &mut self,
+        path: &GenericPath,
+        mut f: T,
+    ) -> Result<(), IMStatusCode>
+    where
+        T: FnMut(&GenericPath, &mut dyn ClusterType) -> Result<(), IMStatusCode>,
+    {
+        self.for_each_endpoint_mut(path, |p, e| {
+            let mut current_path = *p;
+            let (clusters, wildcard) = e.get_wildcard_clusters_mut(p.cluster)?;
+
+            for c in clusters.iter_mut() {
+                current_path.cluster = Some(c.base().id);
+                f(&current_path, c.as_mut())
+                    .or_else(|e| if !wildcard { Err(e) } else { Ok(()) })?;
+            }
+            Ok(())
+        })
+    }
+
+    /// Run a closure for all attributes as specified in the path
+    ///
+    /// Note that the path is a GenericPath and hence can be a wildcard path. The behaviour
+    /// of this function is to only capture the successful invocations and ignore the erroneous
+    /// ones. This is inline with the expected behaviour for wildcard, where it implies that
+    /// 'please run this operation on this wildcard path "wherever possible"'
+    ///
+    /// It is expected that if the closure that you pass here returns an error it may not reach
+    /// out to the caller, in case there was a wildcard path specified
+    pub fn for_each_attribute<T>(&self, path: &GenericPath, mut f: T) -> Result<(), IMStatusCode>
+    where
+        T: FnMut(&GenericPath, &dyn ClusterType) -> Result<(), IMStatusCode>,
+    {
+        self.for_each_cluster(path, |current_path, c| {
+            let mut current_path = *current_path;
+            let (attributes, wildcard) = c
+                .base()
+                .get_wildcard_attribute(path.leaf.map(|at| at as u16))?;
+            for a in attributes.iter() {
+                current_path.leaf = Some(a.id as u32);
+                f(&current_path, c).or_else(|e| if !wildcard { Err(e) } else { Ok(()) })?;
+            }
+            Ok(())
+        })
+    }
+}

matter/src/data_model/objects/privilege.rs

@@ -0,0 +1,79 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    error::Error,
+    tlv::{FromTLV, TLVElement, ToTLV},
+};
+use log::error;
+
+use bitflags::bitflags;
+
+bitflags! {
+    #[derive(Default)]
+    pub struct Privilege: u8 {
+        const V = 0x01;
+        const O = 0x02;
+        const M = 0x04;
+        const A = 0x08;
+
+        const VIEW = Self::V.bits;
+        const OPERATE = Self::V.bits | Self::O.bits;
+        const MANAGE = Self::V.bits | Self::O.bits | Self::M.bits;
+        const ADMIN = Self::V.bits | Self::O.bits| Self::M.bits| Self::A.bits;
+    }
+}
+
+impl FromTLV<'_> for Privilege {
+    fn from_tlv(t: &TLVElement) -> Result<Self, Error>
+    where
+        Self: Sized,
+    {
+        match t.u32()? {
+            1 => Ok(Privilege::VIEW),
+            2 => {
+                error!("ProxyView privilege not yet supporteds");
+                Err(Error::Invalid)
+            }
+            3 => Ok(Privilege::OPERATE),
+            4 => Ok(Privilege::MANAGE),
+            5 => Ok(Privilege::ADMIN),
+            _ => Err(Error::Invalid),
+        }
+    }
+}
+
+impl ToTLV for Privilege {
+    fn to_tlv(
+        &self,
+        tw: &mut crate::tlv::TLVWriter,
+        tag: crate::tlv::TagType,
+    ) -> Result<(), Error> {
+        let val = if self.contains(Privilege::ADMIN) {
+            5
+        } else if self.contains(Privilege::OPERATE) {
+            4
+        } else if self.contains(Privilege::MANAGE) {
+            3
+        } else if self.contains(Privilege::VIEW) {
+            1
+        } else {
+            0
+        };
+        tw.u8(tag, val)
+    }
+}

matter/src/data_model/sdm/dev_att.rs

@@ -0,0 +1,45 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+
+/// Device Attestation Data Type
+pub enum DataType {
+    /// Certificate Declaration
+    CertDeclaration,
+    /// Product Attestation Intermediary Certificate
+    PAI,
+    /// Device Attestation Certificate
+    DAC,
+    /// Device Attestation Certificate - Public Key
+    DACPubKey,
+    /// Device Attestation Certificate - Private Key
+    DACPrivKey,
+}
+
+/// The Device Attestation Data Fetcher Trait
+///
+/// Objects that implement this trait allow the Matter subsystem to query the object
+/// for the Device Attestation data that is programmed in the Matter device.
+pub trait DevAttDataFetcher {
+    /// Get Device Attestation Data
+    ///
+    /// This API is expected to return the particular Device Attestation data as is
+    /// requested by the Matter subsystem.
+    /// The type of data that can be queried is defined in the [DataType] enum.
+    fn get_devatt_data(&self, data_type: DataType, data: &mut [u8]) -> Result<usize, Error>;
+}

matter/src/data_model/sdm/failsafe.rs

@@ -0,0 +1,138 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{error::Error, transport::session::SessionMode};
+use log::error;
+use std::sync::RwLock;
+
+#[derive(PartialEq)]
+#[allow(dead_code)]
+enum NocState {
+    NocNotRecvd,
+    // This is the local fabric index
+    AddNocRecvd(u8),
+    UpdateNocRecvd(u8),
+}
+
+#[derive(PartialEq)]
+pub struct ArmedCtx {
+    session_mode: SessionMode,
+    timeout: u8,
+    noc_state: NocState,
+}
+
+#[derive(PartialEq)]
+pub enum State {
+    Idle,
+    Armed(ArmedCtx),
+}
+
+pub struct FailSafeInner {
+    state: State,
+}
+
+pub struct FailSafe {
+    state: RwLock<FailSafeInner>,
+}
+
+impl FailSafe {
+    pub fn new() -> Self {
+        Self {
+            state: RwLock::new(FailSafeInner { state: State::Idle }),
+        }
+    }
+
+    pub fn arm(&self, timeout: u8, session_mode: SessionMode) -> Result<(), Error> {
+        let mut inner = self.state.write()?;
+        match &mut inner.state {
+            State::Idle => {
+                inner.state = State::Armed(ArmedCtx {
+                    session_mode,
+                    timeout,
+                    noc_state: NocState::NocNotRecvd,
+                })
+            }
+            State::Armed(c) => {
+                if c.session_mode != session_mode {
+                    return Err(Error::Invalid);
+                }
+                // re-arm
+                c.timeout = timeout;
+            }
+        }
+        Ok(())
+    }
+
+    pub fn disarm(&self, session_mode: SessionMode) -> Result<(), Error> {
+        let mut inner = self.state.write()?;
+        match &mut inner.state {
+            State::Idle => {
+                error!("Received Fail-Safe Disarm without it being armed");
+                return Err(Error::Invalid);
+            }
+            State::Armed(c) => {
+                match c.noc_state {
+                    NocState::NocNotRecvd => return Err(Error::Invalid),
+                    NocState::AddNocRecvd(idx) | NocState::UpdateNocRecvd(idx) => {
+                        if SessionMode::Case(idx) != session_mode {
+                            error!(
+                                "Received disarm in separate session from previous Add/Update NOC"
+                            );
+                            return Err(Error::Invalid);
+                        }
+                    }
+                }
+                inner.state = State::Idle;
+            }
+        }
+        Ok(())
+    }
+
+    pub fn is_armed(&self) -> bool {
+        self.state.read().unwrap().state != State::Idle
+    }
+
+    pub fn record_add_noc(&self, fabric_index: u8) -> Result<(), Error> {
+        let mut inner = self.state.write()?;
+        match &mut inner.state {
+            State::Idle => Err(Error::Invalid),
+            State::Armed(c) => {
+                if c.noc_state == NocState::NocNotRecvd {
+                    c.noc_state = NocState::AddNocRecvd(fabric_index);
+                    Ok(())
+                } else {
+                    Err(Error::Invalid)
+                }
+            }
+        }
+    }
+
+    pub fn allow_noc_change(&self) -> Result<bool, Error> {
+        let mut inner = self.state.write()?;
+        let allow = match &mut inner.state {
+            State::Idle => false,
+            State::Armed(c) => c.noc_state == NocState::NocNotRecvd,
+        };
+        Ok(allow)
+    }
+}
+
+impl Default for FailSafe {
+    fn default() -> Self {
+        Self::new()
+    }
+}

matter/src/data_model/sdm/general_commissioning.rs

@@ -0,0 +1,277 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::cmd_enter;
+use crate::data_model::objects::*;
+use crate::data_model::sdm::failsafe::FailSafe;
+use crate::interaction_model::core::IMStatusCode;
+use crate::interaction_model::messages::ib;
+use crate::tlv::{FromTLV, TLVElement, TLVWriter, TagType, ToTLV};
+use crate::{error::*, interaction_model::command::CommandReq};
+use log::{error, info};
+use num_derive::FromPrimitive;
+use std::sync::Arc;
+
+#[derive(Clone, Copy)]
+#[allow(dead_code)]
+enum CommissioningError {
+    Ok = 0,
+    ErrValueOutsideRange = 1,
+    ErrInvalidAuth = 2,
+    ErrNotCommissioning = 3,
+}
+
+pub const ID: u32 = 0x0030;
+
+#[derive(FromPrimitive)]
+pub enum Attributes {
+    BreadCrumb = 0,
+    BasicCommissioningInfo = 1,
+    RegConfig = 2,
+    LocationCapability = 3,
+}
+
+#[derive(FromPrimitive)]
+pub enum Commands {
+    ArmFailsafe = 0x00,
+    ArmFailsafeResp = 0x01,
+    SetRegulatoryConfig = 0x02,
+    SetRegulatoryConfigResp = 0x03,
+    CommissioningComplete = 0x04,
+    CommissioningCompleteResp = 0x05,
+}
+
+pub enum RegLocationType {
+    Indoor = 0,
+    Outdoor = 1,
+    IndoorOutdoor = 2,
+}
+
+fn attr_bread_crumb_new(bread_crumb: u64) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::BreadCrumb as u16,
+        AttrValue::Uint64(bread_crumb),
+        Access::READ | Access::WRITE | Access::NEED_ADMIN,
+        Quality::NONE,
+    )
+}
+
+fn attr_reg_config_new(reg_config: RegLocationType) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::RegConfig as u16,
+        AttrValue::Uint8(reg_config as u8),
+        Access::RV,
+        Quality::NONE,
+    )
+}
+
+fn attr_location_capability_new(reg_config: RegLocationType) -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::LocationCapability as u16,
+        AttrValue::Uint8(reg_config as u8),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+fn attr_comm_info_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::BasicCommissioningInfo as u16,
+        AttrValue::Custom,
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+#[derive(FromTLV, ToTLV)]
+struct FailSafeParams {
+    expiry_len: u8,
+    bread_crumb: u8,
+}
+
+pub struct GenCommCluster {
+    expiry_len: u16,
+    failsafe: Arc<FailSafe>,
+    base: Cluster,
+}
+
+impl ClusterType for GenCommCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+
+    fn read_custom_attribute(&self, encoder: &mut dyn Encoder, attr: &AttrDetails) {
+        match num::FromPrimitive::from_u16(attr.attr_id) {
+            Some(Attributes::BasicCommissioningInfo) => {
+                encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                    let _ = tw.start_struct(tag);
+                    let _ = tw.u16(TagType::Context(0), self.expiry_len);
+                    let _ = tw.end_container();
+                }))
+            }
+            _ => {
+                error!("Unsupported Attribute: this shouldn't happen");
+            }
+        }
+    }
+
+    fn handle_command(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        let cmd = cmd_req
+            .cmd
+            .path
+            .leaf
+            .map(num::FromPrimitive::from_u32)
+            .ok_or(IMStatusCode::UnsupportedCommand)?
+            .ok_or(IMStatusCode::UnsupportedCommand)?;
+        match cmd {
+            Commands::ArmFailsafe => self.handle_command_armfailsafe(cmd_req),
+            Commands::SetRegulatoryConfig => self.handle_command_setregulatoryconfig(cmd_req),
+            Commands::CommissioningComplete => self.handle_command_commissioningcomplete(cmd_req),
+            _ => Err(IMStatusCode::UnsupportedCommand),
+        }
+    }
+}
+
+impl GenCommCluster {
+    pub fn new() -> Result<Box<Self>, Error> {
+        let failsafe = Arc::new(FailSafe::new());
+
+        let mut c = Box::new(GenCommCluster {
+            // TODO: Arch-Specific
+            expiry_len: 120,
+            failsafe,
+            base: Cluster::new(ID)?,
+        });
+        c.base.add_attribute(attr_bread_crumb_new(0)?)?;
+        // TODO: Arch-Specific
+        c.base
+            .add_attribute(attr_reg_config_new(RegLocationType::IndoorOutdoor)?)?;
+        // TODO: Arch-Specific
+        c.base.add_attribute(attr_location_capability_new(
+            RegLocationType::IndoorOutdoor,
+        )?)?;
+        c.base.add_attribute(attr_comm_info_new()?)?;
+
+        Ok(c)
+    }
+
+    pub fn failsafe(&self) -> Arc<FailSafe> {
+        self.failsafe.clone()
+    }
+
+    fn handle_command_armfailsafe(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        cmd_enter!("ARM Fail Safe");
+
+        let p = FailSafeParams::from_tlv(&cmd_req.data)?;
+
+        if self
+            .failsafe
+            .arm(p.expiry_len, cmd_req.trans.session.get_session_mode())
+            .is_err()
+        {
+            return Err(IMStatusCode::Busy);
+        }
+
+        let cmd_data = CommonResponse {
+            error_code: CommissioningError::Ok as u8,
+            debug_txt: "".to_owned(),
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::ArmFailsafeResp as u16,
+            EncodeValue::Value(&cmd_data),
+        );
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+
+    fn handle_command_setregulatoryconfig(
+        &mut self,
+        cmd_req: &mut CommandReq,
+    ) -> Result<(), IMStatusCode> {
+        cmd_enter!("Set Regulatory Config");
+        let country_code = cmd_req
+            .data
+            .find_tag(1)
+            .map_err(|_| IMStatusCode::InvalidCommand)?
+            .slice()
+            .map_err(|_| IMStatusCode::InvalidCommand)?;
+        info!("Received country code: {:?}", country_code);
+
+        let cmd_data = CommonResponse {
+            error_code: 0,
+            debug_txt: "".to_owned(),
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::SetRegulatoryConfigResp as u16,
+            EncodeValue::Value(&cmd_data),
+        );
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+
+    fn handle_command_commissioningcomplete(
+        &mut self,
+        cmd_req: &mut CommandReq,
+    ) -> Result<(), IMStatusCode> {
+        cmd_enter!("Commissioning Complete");
+        let mut status: u8 = CommissioningError::Ok as u8;
+
+        // Has to be a Case Session
+        if cmd_req.trans.session.get_local_fabric_idx().is_none() {
+            status = CommissioningError::ErrInvalidAuth as u8;
+        }
+
+        // AddNOC or UpdateNOC must have happened, and that too for the same fabric
+        // scope that is for this session
+        if self
+            .failsafe
+            .disarm(cmd_req.trans.session.get_session_mode())
+            .is_err()
+        {
+            status = CommissioningError::ErrInvalidAuth as u8;
+        }
+
+        let cmd_data = CommonResponse {
+            error_code: status,
+            debug_txt: "".to_owned(),
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::CommissioningCompleteResp as u16,
+            EncodeValue::Value(&cmd_data),
+        );
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+}
+
+#[derive(FromTLV, ToTLV)]
+struct CommonResponse {
+    error_code: u8,
+    debug_txt: String,
+}

matter/src/data_model/sdm/mod.rs

@@ -0,0 +1,22 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod dev_att;
+pub mod failsafe;
+pub mod general_commissioning;
+pub mod noc;
+pub mod nw_commissioning;

matter/src/data_model/sdm/noc.rs

@@ -0,0 +1,475 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::Arc;
+use std::time::{SystemTime, UNIX_EPOCH};
+
+use crate::acl::{AclEntry, AclMgr, AuthMode};
+use crate::cert::Cert;
+use crate::crypto::{self, CryptoKeyPair, KeyPair};
+use crate::data_model::objects::*;
+use crate::data_model::sdm::dev_att;
+use crate::fabric::{Fabric, FabricMgr};
+use crate::interaction_model::command::CommandReq;
+use crate::interaction_model::core::IMStatusCode;
+use crate::interaction_model::messages::ib;
+use crate::tlv::{FromTLV, OctetStr, TLVElement, TLVWriter, TagType, ToTLV};
+use crate::transport::session::SessionMode;
+use crate::utils::writebuf::WriteBuf;
+use crate::{cmd_enter, error::*};
+use log::{error, info};
+use num_derive::FromPrimitive;
+
+use super::dev_att::{DataType, DevAttDataFetcher};
+use super::failsafe::FailSafe;
+
+// Node Operational Credentials Cluster
+
+#[derive(Clone, Copy)]
+#[allow(dead_code)]
+enum NocStatus {
+    Ok = 0,
+    InvalidPublicKey = 1,
+    InvalidNodeOpId = 2,
+    InvalidNOC = 3,
+    MissingCsr = 4,
+    TableFull = 5,
+    MissingAcl = 6,
+    MissingIpk = 7,
+    InsufficientPrivlege = 8,
+    FabricConflict = 9,
+    LabelConflict = 10,
+    InvalidFabricIndex = 11,
+}
+
+// Some placeholder value for now
+const MAX_CERT_DECLARATION_LEN: usize = 600;
+// Some placeholder value for now
+const MAX_CSR_LEN: usize = 300;
+// As defined in the Matter Spec
+const RESP_MAX: usize = 900;
+
+pub const ID: u32 = 0x003E;
+
+#[derive(FromPrimitive)]
+pub enum Commands {
+    AttReq = 0x00,
+    AttReqResp = 0x01,
+    CertChainReq = 0x02,
+    CertChainResp = 0x03,
+    CSRReq = 0x04,
+    CSRResp = 0x05,
+    AddNOC = 0x06,
+    NOCResp = 0x08,
+    AddTrustedRootCert = 0x0b,
+}
+
+pub struct NocCluster {
+    base: Cluster,
+    dev_att: Box<dyn DevAttDataFetcher>,
+    fabric_mgr: Arc<FabricMgr>,
+    acl_mgr: Arc<AclMgr>,
+    failsafe: Arc<FailSafe>,
+}
+struct NocData {
+    pub key_pair: KeyPair,
+    pub root_ca: Cert,
+}
+
+impl NocData {
+    pub fn new(key_pair: KeyPair) -> Self {
+        Self {
+            key_pair,
+            root_ca: Cert::default(),
+        }
+    }
+}
+
+impl NocCluster {
+    pub fn new(
+        dev_att: Box<dyn DevAttDataFetcher>,
+        fabric_mgr: Arc<FabricMgr>,
+        acl_mgr: Arc<AclMgr>,
+        failsafe: Arc<FailSafe>,
+    ) -> Result<Box<Self>, Error> {
+        Ok(Box::new(Self {
+            dev_att,
+            fabric_mgr,
+            acl_mgr,
+            failsafe,
+            base: Cluster::new(ID)?,
+        }))
+    }
+
+    fn add_acl(&self, fab_idx: u8, admin_subject: u64) -> Result<(), Error> {
+        let mut acl = AclEntry::new(fab_idx as u8, Privilege::ADMIN, AuthMode::Case);
+        acl.add_subject(admin_subject)?;
+        self.acl_mgr.add(acl)
+    }
+
+    fn _handle_command_addnoc(&mut self, cmd_req: &mut CommandReq) -> Result<(), NocStatus> {
+        let noc_data = cmd_req
+            .trans
+            .session
+            .take_data::<NocData>()
+            .ok_or(NocStatus::MissingCsr)?;
+
+        if !self
+            .failsafe
+            .allow_noc_change()
+            .map_err(|_| NocStatus::InsufficientPrivlege)?
+        {
+            error!("AddNOC not allowed by Fail Safe");
+            return Err(NocStatus::InsufficientPrivlege);
+        }
+
+        let r = AddNocReq::from_tlv(&cmd_req.data).map_err(|_| NocStatus::InvalidNOC)?;
+
+        let noc_value = Cert::new(r.noc_value.0).map_err(|_| NocStatus::InvalidNOC)?;
+        info!("Received NOC as: {}", noc_value);
+        let icac_value = Cert::new(r.icac_value.0).map_err(|_| NocStatus::InvalidNOC)?;
+        info!("Received ICAC as: {}", icac_value);
+
+        let fabric = Fabric::new(
+            noc_data.key_pair,
+            noc_data.root_ca,
+            icac_value,
+            noc_value,
+            r.ipk_value.0,
+        )
+        .map_err(|_| NocStatus::TableFull)?;
+        let fab_idx = self
+            .fabric_mgr
+            .add(fabric)
+            .map_err(|_| NocStatus::TableFull)?;
+
+        if self.add_acl(fab_idx as u8, r.case_admin_subject).is_err() {
+            error!("Failed to add ACL, what to do?");
+        }
+
+        if self.failsafe.record_add_noc(fab_idx).is_err() {
+            error!("Failed to record NoC in the FailSafe, what to do?");
+        }
+
+        let cmd_data = NocResp {
+            status_code: NocStatus::Ok as u8,
+            fab_idx,
+            debug_txt: "".to_owned(),
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::NOCResp as u16,
+            EncodeValue::Value(&cmd_data),
+        );
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+
+    fn handle_command_addnoc(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        cmd_enter!("AddNOC");
+        if let Err(e) = self._handle_command_addnoc(cmd_req) {
+            let cmd_data = NocResp {
+                status_code: e as u8,
+                fab_idx: 0,
+                debug_txt: "".to_owned(),
+            };
+            let invoke_resp = ib::InvResp::cmd_new(
+                0,
+                ID,
+                Commands::NOCResp as u16,
+                EncodeValue::Value(&cmd_data),
+            );
+            let _ = invoke_resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+            cmd_req.trans.complete();
+        }
+        Ok(())
+    }
+
+    fn handle_command_attrequest(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        cmd_enter!("AttestationRequest");
+
+        let req = CommonReq::from_tlv(&cmd_req.data).map_err(|_| IMStatusCode::InvalidCommand)?;
+        info!("Received Attestation Nonce:{:?}", req.str);
+
+        let mut attest_challenge = [0u8; crypto::SYMM_KEY_LEN_BYTES];
+        attest_challenge.copy_from_slice(cmd_req.trans.session.get_att_challenge());
+
+        let cmd_data = |tag: TagType, t: &mut TLVWriter| {
+            let mut buf: [u8; RESP_MAX] = [0; RESP_MAX];
+            let mut attest_element = WriteBuf::new(&mut buf, RESP_MAX);
+            let _ = t.start_struct(tag);
+            let _ =
+                add_attestation_element(self.dev_att.as_ref(), req.str.0, &mut attest_element, t);
+            let _ = add_attestation_signature(
+                self.dev_att.as_ref(),
+                &mut attest_element,
+                &attest_challenge,
+                t,
+            );
+            let _ = t.end_container();
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::AttReqResp as u16,
+            EncodeValue::Closure(&cmd_data),
+        );
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+
+    fn handle_command_certchainrequest(
+        &mut self,
+        cmd_req: &mut CommandReq,
+    ) -> Result<(), IMStatusCode> {
+        cmd_enter!("CertChainRequest");
+
+        info!("Received data: {}", cmd_req.data);
+        let cert_type =
+            get_certchainrequest_params(&cmd_req.data).map_err(|_| IMStatusCode::InvalidCommand)?;
+
+        let mut buf: [u8; RESP_MAX] = [0; RESP_MAX];
+        let len = self
+            .dev_att
+            .get_devatt_data(cert_type, &mut buf)
+            .map_err(|_| IMStatusCode::Failure)?;
+        let buf = &buf[0..len];
+
+        let cmd_data = CertChainResp {
+            cert: OctetStr::new(buf),
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::CertChainResp as u16,
+            EncodeValue::Value(&cmd_data),
+        );
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+
+    fn handle_command_csrrequest(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        cmd_enter!("CSRRequest");
+
+        let req = CommonReq::from_tlv(&cmd_req.data).map_err(|_| IMStatusCode::InvalidCommand)?;
+        info!("Received CSR Nonce:{:?}", req.str);
+
+        if !self.failsafe.is_armed() {
+            return Err(IMStatusCode::UnsupportedAccess);
+        }
+
+        let noc_keypair = KeyPair::new().map_err(|_| IMStatusCode::Failure)?;
+        let mut attest_challenge = [0u8; crypto::SYMM_KEY_LEN_BYTES];
+        attest_challenge.copy_from_slice(cmd_req.trans.session.get_att_challenge());
+
+        let cmd_data = |tag: TagType, t: &mut TLVWriter| {
+            let mut buf: [u8; RESP_MAX] = [0; RESP_MAX];
+            let mut nocsr_element = WriteBuf::new(&mut buf, RESP_MAX);
+            let _ = t.start_struct(tag);
+            let _ = add_nocsrelement(&noc_keypair, req.str.0, &mut nocsr_element, t);
+            let _ = add_attestation_signature(
+                self.dev_att.as_ref(),
+                &mut nocsr_element,
+                &attest_challenge,
+                t,
+            );
+            let _ = t.end_container();
+        };
+        let resp = ib::InvResp::cmd_new(
+            0,
+            ID,
+            Commands::CSRResp as u16,
+            EncodeValue::Closure(&cmd_data),
+        );
+
+        let _ = resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+        let noc_data = Box::new(NocData::new(noc_keypair));
+        // Store this in the session data instead of cluster data, so it gets cleared
+        // if the session goes away for some reason
+        cmd_req.trans.session.set_data(noc_data);
+        cmd_req.trans.complete();
+        Ok(())
+    }
+
+    fn handle_command_addtrustedrootcert(
+        &mut self,
+        cmd_req: &mut CommandReq,
+    ) -> Result<(), IMStatusCode> {
+        cmd_enter!("AddTrustedRootCert");
+        if !self.failsafe.is_armed() {
+            return Err(IMStatusCode::UnsupportedAccess);
+        }
+
+        // This may happen on CASE or PASE. For PASE, the existence of NOC Data is necessary
+        match cmd_req.trans.session.get_session_mode() {
+            SessionMode::Case(_) => error!("CASE: AddTrustedRootCert handling pending"), // For a CASE Session, we just return success for now,
+            SessionMode::Pase => {
+                let noc_data = cmd_req
+                    .trans
+                    .session
+                    .get_data::<NocData>()
+                    .ok_or(IMStatusCode::Failure)?;
+
+                let req =
+                    CommonReq::from_tlv(&cmd_req.data).map_err(|_| IMStatusCode::InvalidCommand)?;
+                info!("Received Trusted Cert:{:x?}", req.str);
+
+                noc_data.root_ca = Cert::new(req.str.0).map_err(|_| IMStatusCode::Failure)?;
+            }
+            _ => (),
+        }
+        cmd_req.trans.complete();
+
+        Err(IMStatusCode::Sucess)
+    }
+}
+
+impl ClusterType for NocCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+
+    fn handle_command(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        let cmd = cmd_req
+            .cmd
+            .path
+            .leaf
+            .map(num::FromPrimitive::from_u32)
+            .ok_or(IMStatusCode::UnsupportedCommand)?
+            .ok_or(IMStatusCode::UnsupportedCommand)?;
+        match cmd {
+            Commands::AddNOC => self.handle_command_addnoc(cmd_req),
+            Commands::CSRReq => self.handle_command_csrrequest(cmd_req),
+            Commands::AddTrustedRootCert => self.handle_command_addtrustedrootcert(cmd_req),
+            Commands::AttReq => self.handle_command_attrequest(cmd_req),
+            Commands::CertChainReq => self.handle_command_certchainrequest(cmd_req),
+            _ => Err(IMStatusCode::UnsupportedCommand),
+        }
+    }
+}
+
+fn add_attestation_element(
+    dev_att: &dyn DevAttDataFetcher,
+    att_nonce: &[u8],
+    write_buf: &mut WriteBuf,
+    t: &mut TLVWriter,
+) -> Result<(), Error> {
+    let mut cert_dec: [u8; MAX_CERT_DECLARATION_LEN] = [0; MAX_CERT_DECLARATION_LEN];
+    let len = dev_att.get_devatt_data(dev_att::DataType::CertDeclaration, &mut cert_dec)?;
+    let cert_dec = &cert_dec[0..len];
+
+    let epoch = SystemTime::now().duration_since(UNIX_EPOCH)?.as_secs() as u32;
+    let mut writer = TLVWriter::new(write_buf);
+    writer.start_struct(TagType::Anonymous)?;
+    writer.str16(TagType::Context(1), cert_dec)?;
+    writer.str8(TagType::Context(2), att_nonce)?;
+    writer.u32(TagType::Context(3), epoch)?;
+    writer.end_container()?;
+
+    t.str16(TagType::Context(0), write_buf.as_borrow_slice())?;
+    Ok(())
+}
+
+fn add_attestation_signature(
+    dev_att: &dyn DevAttDataFetcher,
+    attest_element: &mut WriteBuf,
+    attest_challenge: &[u8],
+    resp: &mut TLVWriter,
+) -> Result<(), Error> {
+    let dac_key = {
+        let mut pubkey = [0_u8; crypto::EC_POINT_LEN_BYTES];
+        let mut privkey = [0_u8; crypto::BIGNUM_LEN_BYTES];
+        dev_att.get_devatt_data(dev_att::DataType::DACPubKey, &mut pubkey)?;
+        dev_att.get_devatt_data(dev_att::DataType::DACPrivKey, &mut privkey)?;
+        KeyPair::new_from_components(&pubkey, &privkey)
+    }?;
+    attest_element.copy_from_slice(attest_challenge)?;
+    let mut signature = [0u8; crypto::EC_SIGNATURE_LEN_BYTES];
+    dac_key.sign_msg(attest_element.as_borrow_slice(), &mut signature)?;
+    resp.str8(TagType::Context(1), &signature)
+}
+
+fn add_nocsrelement(
+    noc_keypair: &KeyPair,
+    csr_nonce: &[u8],
+    write_buf: &mut WriteBuf,
+    resp: &mut TLVWriter,
+) -> Result<(), Error> {
+    let mut csr: [u8; MAX_CSR_LEN] = [0; MAX_CSR_LEN];
+    let csr = noc_keypair.get_csr(&mut csr)?;
+    let mut writer = TLVWriter::new(write_buf);
+    writer.start_struct(TagType::Anonymous)?;
+    writer.str8(TagType::Context(1), csr)?;
+    writer.str8(TagType::Context(2), csr_nonce)?;
+    writer.end_container()?;
+
+    resp.str8(TagType::Context(0), write_buf.as_borrow_slice())?;
+    Ok(())
+}
+
+#[derive(ToTLV)]
+struct CertChainResp<'a> {
+    cert: OctetStr<'a>,
+}
+
+#[derive(ToTLV)]
+struct NocResp {
+    status_code: u8,
+    fab_idx: u8,
+    debug_txt: String,
+}
+
+#[derive(FromTLV)]
+#[tlvargs(lifetime = "'a")]
+struct AddNocReq<'a> {
+    noc_value: OctetStr<'a>,
+    icac_value: OctetStr<'a>,
+    ipk_value: OctetStr<'a>,
+    case_admin_subject: u64,
+    _vendor_id: u16,
+}
+
+#[derive(FromTLV)]
+#[tlvargs(lifetime = "'a")]
+struct CommonReq<'a> {
+    str: OctetStr<'a>,
+}
+
+#[derive(FromTLV)]
+struct CertChainReq {
+    cert_type: u8,
+}
+
+fn get_certchainrequest_params(data: &TLVElement) -> Result<DataType, Error> {
+    let cert_type = CertChainReq::from_tlv(data)?.cert_type;
+
+    const CERT_TYPE_DAC: u8 = 1;
+    const CERT_TYPE_PAI: u8 = 2;
+    info!("Received Cert Type:{:?}", cert_type);
+    match cert_type {
+        CERT_TYPE_DAC => Ok(dev_att::DataType::DAC),
+        CERT_TYPE_PAI => Ok(dev_att::DataType::PAI),
+        _ => Err(Error::Invalid),
+    }
+}

matter/src/data_model/sdm/nw_commissioning.rs

@@ -0,0 +1,53 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    data_model::objects::{Cluster, ClusterType},
+    error::Error,
+};
+
+pub const ID: u32 = 0x0031;
+
+pub struct NwCommCluster {
+    base: Cluster,
+}
+
+impl ClusterType for NwCommCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+}
+
+enum FeatureMap {
+    _Wifi = 0,
+    _Thread = 1,
+    Ethernet = 2,
+}
+
+impl NwCommCluster {
+    pub fn new() -> Result<Box<Self>, Error> {
+        let mut c = Box::new(Self {
+            base: Cluster::new(ID)?,
+        });
+        // TODO: Arch-Specific
+        c.base.set_feature_map(FeatureMap::Ethernet as u32)?;
+        Ok(c)
+    }
+}

matter/src/data_model/system_model/access_control.rs

@@ -0,0 +1,394 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::Arc;
+
+use num_derive::FromPrimitive;
+
+use crate::acl::{self, AclEntry, AclMgr};
+use crate::data_model::objects::*;
+use crate::error::*;
+use crate::interaction_model::core::IMStatusCode;
+use crate::interaction_model::messages::ib::{attr_list_write, ListOperation};
+use crate::tlv::{FromTLV, TLVElement, TagType, ToTLV};
+use log::{error, info};
+
+pub const ID: u32 = 0x001F;
+
+#[derive(FromPrimitive)]
+pub enum Attributes {
+    Acl = 0,
+    Extension = 1,
+    SubjectsPerEntry = 2,
+    TargetsPerEntry = 3,
+    EntriesPerFabric = 4,
+}
+
+pub struct AccessControlCluster {
+    base: Cluster,
+    acl_mgr: Arc<AclMgr>,
+}
+
+impl AccessControlCluster {
+    pub fn new(acl_mgr: Arc<AclMgr>) -> Result<Box<Self>, Error> {
+        let mut c = Box::new(AccessControlCluster {
+            base: Cluster::new(ID)?,
+            acl_mgr,
+        });
+        c.base.add_attribute(attr_acl_new()?)?;
+        c.base.add_attribute(attr_extension_new()?)?;
+        c.base.add_attribute(attr_subjects_per_entry_new()?)?;
+        c.base.add_attribute(attr_targets_per_entry_new()?)?;
+        c.base.add_attribute(attr_entries_per_fabric_new()?)?;
+        Ok(c)
+    }
+
+    /// Write the ACL Attribute
+    ///
+    /// This takes care of 4 things, add item, edit item, delete item, delete list.
+    /// Care about fabric-scoped behaviour is taken
+    fn write_acl_attr(
+        &mut self,
+        op: ListOperation,
+        data: &TLVElement,
+        fab_idx: u8,
+    ) -> Result<(), IMStatusCode> {
+        info!("Performing ACL operation {:?}", op);
+        let result = match op {
+            ListOperation::AddItem | ListOperation::EditItem(_) => {
+                let mut acl_entry =
+                    AclEntry::from_tlv(data).map_err(|_| IMStatusCode::ConstraintError)?;
+                info!("ACL  {:?}", acl_entry);
+                // Overwrite the fabric index with our accessing fabric index
+                acl_entry.fab_idx = Some(fab_idx);
+
+                if let ListOperation::EditItem(index) = op {
+                    self.acl_mgr.edit(index as u8, fab_idx, acl_entry)
+                } else {
+                    self.acl_mgr.add(acl_entry)
+                }
+            }
+            ListOperation::DeleteItem(index) => self.acl_mgr.delete(index as u8, fab_idx),
+            ListOperation::DeleteList => self.acl_mgr.delete_for_fabric(fab_idx),
+        };
+        match result {
+            Ok(_) => Ok(()),
+            Err(Error::NoSpace) => Err(IMStatusCode::ResourceExhausted),
+            _ => Err(IMStatusCode::ConstraintError),
+        }
+    }
+}
+
+impl ClusterType for AccessControlCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+
+    fn read_custom_attribute(&self, encoder: &mut dyn Encoder, attr: &AttrDetails) {
+        match num::FromPrimitive::from_u16(attr.attr_id) {
+            Some(Attributes::Acl) => encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                let _ = tw.start_array(tag);
+                let _ = self.acl_mgr.for_each_acl(|entry| {
+                    if !attr.fab_filter || Some(attr.fab_idx) == entry.fab_idx {
+                        let _ = entry.to_tlv(tw, TagType::Anonymous);
+                    }
+                });
+                let _ = tw.end_container();
+            })),
+            Some(Attributes::Extension) => encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                // Empty for now
+                let _ = tw.start_array(tag);
+                let _ = tw.end_container();
+            })),
+            _ => {
+                error!("Attribute not yet supported: this shouldn't happen");
+            }
+        }
+    }
+
+    fn write_attribute(
+        &mut self,
+        attr: &AttrDetails,
+        data: &TLVElement,
+    ) -> Result<(), IMStatusCode> {
+        let result = match num::FromPrimitive::from_u16(attr.attr_id) {
+            Some(Attributes::Acl) => attr_list_write(attr, data, |op, data| {
+                self.write_acl_attr(op, data, attr.fab_idx)
+            }),
+            _ => {
+                error!("Attribute not yet supported: this shouldn't happen");
+                Err(IMStatusCode::NotFound)
+            }
+        };
+        if result.is_ok() {
+            self.base.cluster_changed();
+        }
+        result
+    }
+}
+
+fn attr_acl_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::Acl as u16,
+        AttrValue::Custom,
+        Access::RWFA,
+        Quality::NONE,
+    )
+}
+
+fn attr_extension_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::Extension as u16,
+        AttrValue::Custom,
+        Access::RWFA,
+        Quality::NONE,
+    )
+}
+
+fn attr_subjects_per_entry_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::SubjectsPerEntry as u16,
+        AttrValue::Uint16(acl::SUBJECTS_PER_ENTRY as u16),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+fn attr_targets_per_entry_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::TargetsPerEntry as u16,
+        AttrValue::Uint16(acl::TARGETS_PER_ENTRY as u16),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+fn attr_entries_per_fabric_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::EntriesPerFabric as u16,
+        AttrValue::Uint16(acl::ENTRIES_PER_FABRIC as u16),
+        Access::RV,
+        Quality::FIXED,
+    )
+}
+
+#[cfg(test)]
+mod tests {
+    use std::sync::Arc;
+
+    use crate::{
+        acl::{AclEntry, AclMgr, AuthMode},
+        data_model::{
+            core::AttrReadEncoder,
+            objects::{AttrDetails, ClusterType, Privilege},
+        },
+        interaction_model::messages::ib::ListOperation,
+        tlv::{get_root_node_struct, ElementType, TLVElement, TLVWriter, TagType, ToTLV},
+        utils::writebuf::WriteBuf,
+    };
+
+    use super::AccessControlCluster;
+
+    #[test]
+    /// Add an ACL entry
+    fn acl_cluster_add() {
+        let mut buf: [u8; 100] = [0; 100];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        let acl_mgr = Arc::new(AclMgr::new_with(false).unwrap());
+        let mut acl = AccessControlCluster::new(acl_mgr.clone()).unwrap();
+
+        let new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+        let data = get_root_node_struct(writebuf.as_borrow_slice()).unwrap();
+
+        // Test, ACL has fabric index 2, but the accessing fabric is 1
+        //    the fabric index in the TLV should be ignored and the ACL should be created with entry 1
+        let result = acl.write_acl_attr(ListOperation::AddItem, &data, 1);
+        assert_eq!(result, Ok(()));
+
+        let verifier = AclEntry::new(1, Privilege::VIEW, AuthMode::Case);
+        acl_mgr
+            .for_each_acl(|a| {
+                assert_eq!(*a, verifier);
+            })
+            .unwrap();
+    }
+
+    #[test]
+    /// - The listindex used for edit should be relative to the current fabric
+    fn acl_cluster_edit() {
+        let mut buf: [u8; 100] = [0; 100];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        // Add 3 ACLs, belonging to fabric index 2, 1 and 2, in that order
+        let acl_mgr = Arc::new(AclMgr::new_with(false).unwrap());
+        let mut verifier = [
+            AclEntry::new(2, Privilege::VIEW, AuthMode::Case),
+            AclEntry::new(1, Privilege::VIEW, AuthMode::Case),
+            AclEntry::new(2, Privilege::ADMIN, AuthMode::Case),
+        ];
+        for i in verifier {
+            acl_mgr.add(i).unwrap();
+        }
+        let mut acl = AccessControlCluster::new(acl_mgr.clone()).unwrap();
+
+        let new = AclEntry::new(2, Privilege::VIEW, AuthMode::Case);
+        new.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+        let data = get_root_node_struct(writebuf.as_borrow_slice()).unwrap();
+
+        // Test, Edit Fabric 2's index 1 - with accessing fabring as 2 - allow
+        let result = acl.write_acl_attr(ListOperation::EditItem(1), &data, 2);
+        // Fabric 2's index 1, is actually our index 2, update the verifier
+        verifier[2] = new;
+        assert_eq!(result, Ok(()));
+
+        // Also validate in the acl_mgr that the entries are in the right order
+        let mut index = 0;
+        acl_mgr
+            .for_each_acl(|a| {
+                assert_eq!(*a, verifier[index]);
+                index += 1;
+            })
+            .unwrap();
+    }
+
+    #[test]
+    /// - The listindex used for delete should be relative to the current fabric
+    fn acl_cluster_delete() {
+        // Add 3 ACLs, belonging to fabric index 2, 1 and 2, in that order
+        let acl_mgr = Arc::new(AclMgr::new_with(false).unwrap());
+        let input = [
+            AclEntry::new(2, Privilege::VIEW, AuthMode::Case),
+            AclEntry::new(1, Privilege::VIEW, AuthMode::Case),
+            AclEntry::new(2, Privilege::ADMIN, AuthMode::Case),
+        ];
+        for i in input {
+            acl_mgr.add(i).unwrap();
+        }
+        let mut acl = AccessControlCluster::new(acl_mgr.clone()).unwrap();
+        // data is don't-care actually
+        let data = TLVElement::new(TagType::Anonymous, ElementType::True);
+
+        // Test , Delete Fabric 1's index 0
+        let result = acl.write_acl_attr(ListOperation::DeleteItem(0), &data, 1);
+        assert_eq!(result, Ok(()));
+
+        let verifier = [input[0], input[2]];
+        // Also validate in the acl_mgr that the entries are in the right order
+        let mut index = 0;
+        acl_mgr
+            .for_each_acl(|a| {
+                assert_eq!(*a, verifier[index]);
+                index += 1;
+            })
+            .unwrap();
+    }
+
+    #[test]
+    /// - acl read with and without fabric filtering
+    fn acl_cluster_read() {
+        let mut buf: [u8; 100] = [0; 100];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+
+        // Add 3 ACLs, belonging to fabric index 2, 1 and 2, in that order
+        let acl_mgr = Arc::new(AclMgr::new_with(false).unwrap());
+        let input = [
+            AclEntry::new(2, Privilege::VIEW, AuthMode::Case),
+            AclEntry::new(1, Privilege::VIEW, AuthMode::Case),
+            AclEntry::new(2, Privilege::ADMIN, AuthMode::Case),
+        ];
+        for i in input {
+            acl_mgr.add(i).unwrap();
+        }
+        let acl = AccessControlCluster::new(acl_mgr.clone()).unwrap();
+        // Test 1, all 3 entries are read in the response without fabric filtering
+        {
+            let mut tw = TLVWriter::new(&mut writebuf);
+            let mut encoder = AttrReadEncoder::new(&mut tw);
+            let attr_details = AttrDetails {
+                attr_id: 0,
+                list_index: None,
+                fab_idx: 1,
+                fab_filter: false,
+            };
+            acl.read_custom_attribute(&mut encoder, &attr_details);
+            assert_eq!(
+                &[
+                    21, 53, 1, 36, 0, 0, 55, 1, 24, 54, 2, 21, 36, 1, 1, 36, 2, 2, 54, 3, 24, 54,
+                    4, 24, 36, 254, 2, 24, 21, 36, 1, 1, 36, 2, 2, 54, 3, 24, 54, 4, 24, 36, 254,
+                    1, 24, 21, 36, 1, 5, 36, 2, 2, 54, 3, 24, 54, 4, 24, 36, 254, 2, 24, 24, 24,
+                    24
+                ],
+                writebuf.as_borrow_slice()
+            );
+        }
+        writebuf.reset(0);
+
+        // Test 2, only single entry is read in the response with fabric filtering and fabric idx 1
+        {
+            let mut tw = TLVWriter::new(&mut writebuf);
+            let mut encoder = AttrReadEncoder::new(&mut tw);
+
+            let attr_details = AttrDetails {
+                attr_id: 0,
+                list_index: None,
+                fab_idx: 1,
+                fab_filter: true,
+            };
+            acl.read_custom_attribute(&mut encoder, &attr_details);
+            assert_eq!(
+                &[
+                    21, 53, 1, 36, 0, 0, 55, 1, 24, 54, 2, 21, 36, 1, 1, 36, 2, 2, 54, 3, 24, 54,
+                    4, 24, 36, 254, 1, 24, 24, 24, 24
+                ],
+                writebuf.as_borrow_slice()
+            );
+        }
+        writebuf.reset(0);
+
+        // Test 3, only single entry is read in the response with fabric filtering and fabric idx 2
+        {
+            let mut tw = TLVWriter::new(&mut writebuf);
+            let mut encoder = AttrReadEncoder::new(&mut tw);
+
+            let attr_details = AttrDetails {
+                attr_id: 0,
+                list_index: None,
+                fab_idx: 2,
+                fab_filter: true,
+            };
+            acl.read_custom_attribute(&mut encoder, &attr_details);
+            assert_eq!(
+                &[
+                    21, 53, 1, 36, 0, 0, 55, 1, 24, 54, 2, 21, 36, 1, 1, 36, 2, 2, 54, 3, 24, 54,
+                    4, 24, 36, 254, 2, 24, 21, 36, 1, 5, 36, 2, 2, 54, 3, 24, 54, 4, 24, 36, 254,
+                    2, 24, 24, 24, 24
+                ],
+                writebuf.as_borrow_slice()
+            );
+        }
+    }
+}

matter/src/data_model/system_model/descriptor.rs

@@ -0,0 +1,98 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use num_derive::FromPrimitive;
+
+use crate::data_model::core::DataModel;
+use crate::data_model::objects::*;
+use crate::error::*;
+use crate::interaction_model::messages::GenericPath;
+use crate::tlv::{TLVWriter, TagType};
+use log::error;
+
+pub const ID: u32 = 0x001D;
+
+#[derive(FromPrimitive)]
+enum Attributes {
+    DeviceTypeList = 0,
+    ServerList = 1,
+    ClientList = 2,
+    PartsList = 3,
+}
+
+pub struct DescriptorCluster {
+    base: Cluster,
+    endpoint_id: u16,
+    data_model: DataModel,
+}
+
+impl DescriptorCluster {
+    pub fn new(endpoint_id: u16, data_model: DataModel) -> Result<Box<Self>, Error> {
+        let mut c = Box::new(DescriptorCluster {
+            endpoint_id,
+            data_model,
+            base: Cluster::new(ID)?,
+        });
+        c.base.add_attribute(attr_serverlist_new()?)?;
+        Ok(c)
+    }
+
+    fn encode_server_list(&self, tag: TagType, tw: &mut TLVWriter) {
+        let path = GenericPath {
+            endpoint: Some(self.endpoint_id),
+            cluster: None,
+            leaf: None,
+        };
+        let _ = tw.start_array(tag);
+        let dm = self.data_model.node.read().unwrap();
+        let _ = dm.for_each_cluster(&path, |_current_path, c| {
+            let _ = tw.u32(TagType::Anonymous, c.base().id());
+            Ok(())
+        });
+        let _ = tw.end_container();
+    }
+}
+
+impl ClusterType for DescriptorCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+
+    fn read_custom_attribute(&self, encoder: &mut dyn Encoder, attr: &AttrDetails) {
+        match num::FromPrimitive::from_u16(attr.attr_id) {
+            Some(Attributes::ServerList) => encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                self.encode_server_list(tag, tw)
+            })),
+
+            _ => {
+                error!("Attribute not supported: this shouldn't happen");
+            }
+        }
+    }
+}
+
+fn attr_serverlist_new() -> Result<Attribute, Error> {
+    Attribute::new(
+        Attributes::ServerList as u16,
+        AttrValue::Custom,
+        Access::RV,
+        Quality::NONE,
+    )
+}

matter/src/data_model/system_model/mod.rs

@@ -0,0 +1,19 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod access_control;
+pub mod descriptor;

matter/src/error.rs

@@ -0,0 +1,125 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::{array::TryFromSliceError, fmt, sync::PoisonError, time::SystemTimeError};
+
+use async_channel::{SendError, TryRecvError};
+use log::error;
+
+#[derive(Debug, PartialEq, Clone, Copy)]
+pub enum Error {
+    AttributeNotFound,
+    AttributeIsCustom,
+    ClusterNotFound,
+    CommandNotFound,
+    EndpointNotFound,
+    Crypto,
+    TLSStack,
+    MdnsError,
+    Network,
+    NoCommand,
+    NoEndpoint,
+    NoExchange,
+    NoFabricId,
+    NoHandler,
+    NoNetworkInterface,
+    NoNodeId,
+    NoSession,
+    NoSpace,
+    NoSpaceAckTable,
+    NoSpaceRetransTable,
+    NoTagFound,
+    NotFound,
+    PacketPoolExhaust,
+    StdIoError,
+    SysTimeFail,
+    Invalid,
+    InvalidAAD,
+    InvalidData,
+    InvalidKeyLength,
+    InvalidOpcode,
+    InvalidPeerAddr,
+    // Invalid Auth Key in the Matter Certificate
+    InvalidAuthKey,
+    InvalidSignature,
+    InvalidState,
+    RwLock,
+    TLVNotFound,
+    TLVTypeMismatch,
+    TruncatedPacket,
+}
+
+impl From<std::io::Error> for Error {
+    fn from(_e: std::io::Error) -> Self {
+        // Keep things simple for now
+        Self::StdIoError
+    }
+}
+
+impl<T> From<PoisonError<T>> for Error {
+    fn from(_e: PoisonError<T>) -> Self {
+        Self::RwLock
+    }
+}
+
+#[cfg(feature = "crypto_openssl")]
+impl From<openssl::error::ErrorStack> for Error {
+    fn from(e: openssl::error::ErrorStack) -> Self {
+        error!("Error in TLS: {}", e);
+        Self::TLSStack
+    }
+}
+
+#[cfg(feature = "crypto_mbedtls")]
+impl From<mbedtls::Error> for Error {
+    fn from(e: mbedtls::Error) -> Self {
+        error!("Error in TLS: {}", e);
+        Self::TLSStack
+    }
+}
+
+impl From<SystemTimeError> for Error {
+    fn from(_e: SystemTimeError) -> Self {
+        Self::SysTimeFail
+    }
+}
+
+impl From<TryFromSliceError> for Error {
+    fn from(_e: TryFromSliceError) -> Self {
+        Self::Invalid
+    }
+}
+
+impl<T> From<SendError<T>> for Error {
+    fn from(e: SendError<T>) -> Self {
+        error!("Error in channel send {}", e);
+        Self::Invalid
+    }
+}
+
+impl From<TryRecvError> for Error {
+    fn from(e: TryRecvError) -> Self {
+        error!("Error in channel try_recv {}", e);
+        Self::Invalid
+    }
+}
+
+impl<'a> fmt::Display for Error {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(f, "{:?}", self)
+    }
+}

matter/src/fabric.rs

@@ -0,0 +1,299 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::{Arc, Mutex, MutexGuard, RwLock};
+
+use byteorder::{BigEndian, ByteOrder, LittleEndian};
+use log::info;
+use owning_ref::RwLockReadGuardRef;
+
+use crate::{
+    cert::Cert,
+    crypto::{self, crypto_dummy::KeyPairDummy, hkdf_sha256, CryptoKeyPair, HmacSha256, KeyPair},
+    error::Error,
+    group_keys::KeySet,
+    mdns::{self, Mdns},
+    sys::{Psm, SysMdnsService},
+};
+
+const MAX_CERT_TLV_LEN: usize = 300;
+const COMPRESSED_FABRIC_ID_LEN: usize = 8;
+
+macro_rules! fb_key {
+    ($index:ident, $key:ident) => {
+        &format!("fb{}{}", $index, $key)
+    };
+}
+
+const ST_RCA: &str = "rca";
+const ST_ICA: &str = "ica";
+const ST_NOC: &str = "noc";
+const ST_IPK: &str = "ipk";
+const ST_PBKEY: &str = "pubkey";
+const ST_PRKEY: &str = "privkey";
+
+#[allow(dead_code)]
+pub struct Fabric {
+    node_id: u64,
+    fabric_id: u64,
+    key_pair: Box<dyn CryptoKeyPair>,
+    pub root_ca: Cert,
+    pub icac: Cert,
+    pub noc: Cert,
+    pub ipk: KeySet,
+    compressed_id: [u8; COMPRESSED_FABRIC_ID_LEN],
+    mdns_service: Option<SysMdnsService>,
+}
+
+impl Fabric {
+    pub fn new(
+        key_pair: KeyPair,
+        root_ca: Cert,
+        icac: Cert,
+        noc: Cert,
+        ipk: &[u8],
+    ) -> Result<Self, Error> {
+        let node_id = noc.get_node_id()?;
+        let fabric_id = noc.get_fabric_id()?;
+
+        let mut f = Self {
+            node_id,
+            fabric_id,
+            key_pair: Box::new(key_pair),
+            root_ca,
+            icac,
+            noc,
+            ipk: KeySet::default(),
+            compressed_id: [0; COMPRESSED_FABRIC_ID_LEN],
+            mdns_service: None,
+        };
+        Fabric::get_compressed_id(f.root_ca.get_pubkey(), fabric_id, &mut f.compressed_id)?;
+        f.ipk = KeySet::new(ipk, &f.compressed_id)?;
+
+        let mut mdns_service_name = String::with_capacity(33);
+        for c in f.compressed_id {
+            mdns_service_name.push_str(&format!("{:02X}", c));
+        }
+        mdns_service_name.push('-');
+        let mut node_id_be: [u8; 8] = [0; 8];
+        BigEndian::write_u64(&mut node_id_be, node_id);
+        for c in node_id_be {
+            mdns_service_name.push_str(&format!("{:02X}", c));
+        }
+        info!("MDNS Service Name: {}", mdns_service_name);
+        f.mdns_service = Some(
+            Mdns::get()?.publish_service(&mdns_service_name, mdns::ServiceMode::Commissioned)?,
+        );
+        Ok(f)
+    }
+
+    pub fn dummy() -> Result<Self, Error> {
+        Ok(Self {
+            node_id: 0,
+            fabric_id: 0,
+            key_pair: Box::new(KeyPairDummy::new()?),
+            root_ca: Cert::default(),
+            icac: Cert::default(),
+            noc: Cert::default(),
+            ipk: KeySet::default(),
+            compressed_id: [0; COMPRESSED_FABRIC_ID_LEN],
+            mdns_service: None,
+        })
+    }
+
+    fn get_compressed_id(root_pubkey: &[u8], fabric_id: u64, out: &mut [u8]) -> Result<(), Error> {
+        let root_pubkey = &root_pubkey[1..];
+        let mut fabric_id_be: [u8; 8] = [0; 8];
+        BigEndian::write_u64(&mut fabric_id_be, fabric_id);
+        const COMPRESSED_FABRIC_ID_INFO: [u8; 16] = [
+            0x43, 0x6f, 0x6d, 0x70, 0x72, 0x65, 0x73, 0x73, 0x65, 0x64, 0x46, 0x61, 0x62, 0x72,
+            0x69, 0x63,
+        ];
+        hkdf_sha256(&fabric_id_be, root_pubkey, &COMPRESSED_FABRIC_ID_INFO, out)
+            .map_err(|_| Error::NoSpace)
+    }
+
+    pub fn match_dest_id(&self, random: &[u8], target: &[u8]) -> Result<(), Error> {
+        let mut mac = HmacSha256::new(self.ipk.op_key())?;
+
+        mac.update(random)?;
+        mac.update(self.root_ca.get_pubkey())?;
+
+        let mut buf: [u8; 8] = [0; 8];
+        LittleEndian::write_u64(&mut buf, self.fabric_id);
+        mac.update(&buf)?;
+
+        LittleEndian::write_u64(&mut buf, self.node_id);
+        mac.update(&buf)?;
+
+        let mut id = [0_u8; crypto::SHA256_HASH_LEN_BYTES];
+        mac.finish(&mut id)?;
+        if id.as_slice() == target {
+            Ok(())
+        } else {
+            Err(Error::NotFound)
+        }
+    }
+
+    pub fn sign_msg(&self, msg: &[u8], signature: &mut [u8]) -> Result<usize, Error> {
+        self.key_pair.sign_msg(msg, signature)
+    }
+
+    pub fn get_node_id(&self) -> u64 {
+        self.node_id
+    }
+
+    pub fn get_fabric_id(&self) -> u64 {
+        self.fabric_id
+    }
+
+    fn store(&self, index: usize, psm: &MutexGuard<Psm>) -> Result<(), Error> {
+        let mut key = [0u8; MAX_CERT_TLV_LEN];
+        let len = self.root_ca.as_tlv(&mut key)?;
+        psm.set_kv_slice(fb_key!(index, ST_RCA), &key[..len])?;
+        let len = self.icac.as_tlv(&mut key)?;
+        psm.set_kv_slice(fb_key!(index, ST_ICA), &key[..len])?;
+        let len = self.noc.as_tlv(&mut key)?;
+        psm.set_kv_slice(fb_key!(index, ST_NOC), &key[..len])?;
+        psm.set_kv_slice(fb_key!(index, ST_IPK), self.ipk.epoch_key())?;
+
+        let mut key = [0_u8; crypto::EC_POINT_LEN_BYTES];
+        let len = self.key_pair.get_public_key(&mut key)?;
+        let key = &key[..len];
+        psm.set_kv_slice(fb_key!(index, ST_PBKEY), key)?;
+
+        let mut key = [0_u8; crypto::BIGNUM_LEN_BYTES];
+        let len = self.key_pair.get_private_key(&mut key)?;
+        let key = &key[..len];
+        psm.set_kv_slice(fb_key!(index, ST_PRKEY), key)?;
+
+        Ok(())
+    }
+
+    fn load(index: usize, psm: &MutexGuard<Psm>) -> Result<Self, Error> {
+        let mut root_ca = Vec::new();
+        psm.get_kv_slice(fb_key!(index, ST_RCA), &mut root_ca)?;
+        let root_ca = Cert::new(root_ca.as_slice())?;
+
+        let mut icac = Vec::new();
+        psm.get_kv_slice(fb_key!(index, ST_ICA), &mut icac)?;
+        let icac = Cert::new(icac.as_slice())?;
+
+        let mut noc = Vec::new();
+        psm.get_kv_slice(fb_key!(index, ST_NOC), &mut noc)?;
+        let noc = Cert::new(noc.as_slice())?;
+
+        let mut ipk = Vec::new();
+        psm.get_kv_slice(fb_key!(index, ST_IPK), &mut ipk)?;
+
+        let mut pub_key = Vec::new();
+        psm.get_kv_slice(fb_key!(index, ST_PBKEY), &mut pub_key)?;
+        let mut priv_key = Vec::new();
+        psm.get_kv_slice(fb_key!(index, ST_PRKEY), &mut priv_key)?;
+        let keypair = KeyPair::new_from_components(pub_key.as_slice(), priv_key.as_slice())?;
+
+        Fabric::new(keypair, root_ca, icac, noc, ipk.as_slice())
+    }
+}
+
+pub const MAX_SUPPORTED_FABRICS: usize = 3;
+#[derive(Default)]
+pub struct FabricMgrInner {
+    // The outside world expects Fabric Index to be one more than the actual one
+    // since 0 is not allowed. Need to handle this cleanly somehow
+    pub fabrics: [Option<Fabric>; MAX_SUPPORTED_FABRICS],
+}
+
+pub struct FabricMgr {
+    inner: RwLock<FabricMgrInner>,
+    psm: Arc<Mutex<Psm>>,
+}
+
+impl FabricMgr {
+    pub fn new() -> Result<Self, Error> {
+        let dummy_fabric = Fabric::dummy()?;
+        let mut mgr = FabricMgrInner::default();
+        mgr.fabrics[0] = Some(dummy_fabric);
+        let mut fm = Self {
+            inner: RwLock::new(mgr),
+            psm: Psm::get()?,
+        };
+        fm.load()?;
+        Ok(fm)
+    }
+
+    fn store(&self, index: usize, fabric: &Fabric) -> Result<(), Error> {
+        let psm = self.psm.lock().unwrap();
+        fabric.store(index, &psm)
+    }
+
+    fn load(&mut self) -> Result<(), Error> {
+        let mut mgr = self.inner.write()?;
+        let psm = self.psm.lock().unwrap();
+        for i in 0..MAX_SUPPORTED_FABRICS {
+            let result = Fabric::load(i, &psm);
+            if let Ok(fabric) = result {
+                info!("Adding new fabric at index {}", i);
+                mgr.fabrics[i] = Some(fabric);
+            }
+        }
+        Ok(())
+    }
+
+    pub fn add(&self, f: Fabric) -> Result<u8, Error> {
+        let mut mgr = self.inner.write()?;
+        let index = mgr
+            .fabrics
+            .iter()
+            .position(|f| f.is_none())
+            .ok_or(Error::NoSpace)?;
+
+        self.store(index, &f)?;
+
+        mgr.fabrics[index] = Some(f);
+        Ok(index as u8)
+    }
+
+    pub fn match_dest_id(&self, random: &[u8], target: &[u8]) -> Result<usize, Error> {
+        let mgr = self.inner.read()?;
+        for i in 0..MAX_SUPPORTED_FABRICS {
+            if let Some(fabric) = &mgr.fabrics[i] {
+                if fabric.match_dest_id(random, target).is_ok() {
+                    return Ok(i);
+                }
+            }
+        }
+        Err(Error::NotFound)
+    }
+
+    pub fn get_fabric<'ret, 'me: 'ret>(
+        &'me self,
+        idx: usize,
+    ) -> Result<RwLockReadGuardRef<'ret, FabricMgrInner, Option<Fabric>>, Error> {
+        Ok(RwLockReadGuardRef::new(self.inner.read()?).map(|fm| &fm.fabrics[idx]))
+    }
+
+    pub fn is_empty(&self) -> bool {
+        let mgr = self.inner.read().unwrap();
+        for i in 1..MAX_SUPPORTED_FABRICS {
+            if mgr.fabrics[i].is_some() {
+                return false;
+            }
+        }
+        true
+    }
+}

matter/src/group_keys.rs

@@ -0,0 +1,76 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::{Arc, Mutex, Once};
+
+use crate::{crypto, error::Error};
+
+// This is just makeshift implementation for now, not used anywhere
+pub struct GroupKeys {}
+
+static mut G_GRP_KEYS: Option<Arc<Mutex<GroupKeys>>> = None;
+static INIT: Once = Once::new();
+
+impl GroupKeys {
+    fn new() -> Self {
+        Self {}
+    }
+
+    pub fn get() -> Result<Arc<Mutex<Self>>, Error> {
+        unsafe {
+            INIT.call_once(|| {
+                G_GRP_KEYS = Some(Arc::new(Mutex::new(GroupKeys::new())));
+            });
+            Ok(G_GRP_KEYS.as_ref().ok_or(Error::Invalid)?.clone())
+        }
+    }
+
+    pub fn insert_key() -> Result<(), Error> {
+        Ok(())
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct KeySet {
+    pub epoch_key: [u8; crypto::SYMM_KEY_LEN_BYTES],
+    pub op_key: [u8; crypto::SYMM_KEY_LEN_BYTES],
+}
+
+impl KeySet {
+    pub fn new(epoch_key: &[u8], compressed_id: &[u8]) -> Result<Self, Error> {
+        let mut ks = KeySet::default();
+        KeySet::op_key_from_ipk(epoch_key, compressed_id, &mut ks.op_key)?;
+        ks.epoch_key.copy_from_slice(epoch_key);
+        Ok(ks)
+    }
+
+    fn op_key_from_ipk(ipk: &[u8], compressed_id: &[u8], opkey: &mut [u8]) -> Result<(), Error> {
+        const GRP_KEY_INFO: [u8; 13] = [
+            0x47, 0x72, 0x6f, 0x75, 0x70, 0x4b, 0x65, 0x79, 0x20, 0x76, 0x31, 0x2e, 0x30,
+        ];
+
+        crypto::hkdf_sha256(compressed_id, ipk, &GRP_KEY_INFO, opkey).map_err(|_| Error::NoSpace)
+    }
+
+    pub fn op_key(&self) -> &[u8] {
+        &self.op_key
+    }
+
+    pub fn epoch_key(&self) -> &[u8] {
+        &self.epoch_key
+    }
+}

matter/src/interaction_model/command.rs

@@ -0,0 +1,76 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::core::OpCode;
+use super::messages::ib;
+use super::messages::msg;
+use super::messages::msg::InvReq;
+use super::InteractionModel;
+use super::Transaction;
+use crate::{
+    error::*,
+    tlv::{get_root_node_struct, print_tlv_list, FromTLV, TLVElement, TLVWriter, TagType},
+    transport::{packet::Packet, proto_demux::ResponseRequired},
+};
+use log::error;
+
+#[macro_export]
+macro_rules! cmd_enter {
+    ($e:expr) => {{
+        use colored::Colorize;
+        info! {"{} {}", "Handling Command".cyan(), $e.cyan()}
+    }};
+}
+
+pub struct CommandReq<'a, 'b, 'c, 'd> {
+    pub cmd: ib::CmdPath,
+    pub data: TLVElement<'a>,
+    pub resp: &'a mut TLVWriter<'b, 'c>,
+    pub trans: &'a mut Transaction<'d>,
+}
+
+impl InteractionModel {
+    pub fn handle_invoke_req(
+        &mut self,
+        trans: &mut Transaction,
+        rx_buf: &[u8],
+        proto_tx: &mut Packet,
+    ) -> Result<ResponseRequired, Error> {
+        proto_tx.set_proto_opcode(OpCode::InvokeResponse as u8);
+
+        let mut tw = TLVWriter::new(proto_tx.get_writebuf()?);
+        let root = get_root_node_struct(rx_buf)?;
+        let inv_req = InvReq::from_tlv(&root)?;
+
+        tw.start_struct(TagType::Anonymous)?;
+        // Suppress Response -> TODO: Need to revisit this for cases where we send a command back
+        tw.bool(
+            TagType::Context(msg::InvRespTag::SupressResponse as u8),
+            false,
+        )?;
+
+        self.consumer
+            .consume_invoke_cmd(&inv_req, trans, &mut tw)
+            .map_err(|e| {
+                error!("Error in handling command: {:?}", e);
+                print_tlv_list(rx_buf);
+                e
+            })?;
+        tw.end_container()?;
+        Ok(ResponseRequired::Yes)
+    }
+}

matter/src/interaction_model/core.rs

@@ -0,0 +1,164 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    error::*,
+    tlv::{self, FromTLV, TLVElement, TLVWriter, TagType, ToTLV},
+    transport::{
+        proto_demux::{self, ProtoCtx, ResponseRequired},
+        session::Session,
+    },
+};
+use colored::Colorize;
+use log::{error, info};
+use num;
+use num_derive::FromPrimitive;
+
+use super::InteractionConsumer;
+use super::InteractionModel;
+use super::Transaction;
+use super::TransactionState;
+
+/* Handle messages related to the Interation Model
+ */
+
+/* Interaction Model ID as per the Matter Spec */
+const PROTO_ID_INTERACTION_MODEL: usize = 0x01;
+
+#[derive(FromPrimitive, Debug, Copy, Clone)]
+pub enum OpCode {
+    Reserved = 0,
+    StatusResponse = 1,
+    ReadRequest = 2,
+    SubscribeRequest = 3,
+    SubscriptResponse = 4,
+    ReportData = 5,
+    WriteRequest = 6,
+    WriteResponse = 7,
+    InvokeRequest = 8,
+    InvokeResponse = 9,
+    TimedRequest = 10,
+}
+
+impl<'a> Transaction<'a> {
+    pub fn new(session: &'a mut Session) -> Self {
+        Self {
+            state: TransactionState::Ongoing,
+            data: None,
+            session,
+        }
+    }
+
+    pub fn complete(&mut self) {
+        self.state = TransactionState::Complete
+    }
+
+    pub fn is_complete(&self) -> bool {
+        self.state == TransactionState::Complete
+    }
+}
+
+impl InteractionModel {
+    pub fn new(consumer: Box<dyn InteractionConsumer>) -> InteractionModel {
+        InteractionModel { consumer }
+    }
+}
+
+impl proto_demux::HandleProto for InteractionModel {
+    fn handle_proto_id(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        let mut trans = Transaction::new(&mut ctx.exch_ctx.sess);
+        let proto_opcode: OpCode =
+            num::FromPrimitive::from_u8(ctx.rx.get_proto_opcode()).ok_or(Error::Invalid)?;
+        ctx.tx.set_proto_id(PROTO_ID_INTERACTION_MODEL as u16);
+
+        let buf = ctx.rx.as_borrow_slice();
+        info!("{} {:?}", "Received command".cyan(), proto_opcode);
+        tlv::print_tlv_list(buf);
+        let result = match proto_opcode {
+            OpCode::InvokeRequest => self.handle_invoke_req(&mut trans, buf, &mut ctx.tx)?,
+            OpCode::ReadRequest => self.handle_read_req(&mut trans, buf, &mut ctx.tx)?,
+            OpCode::WriteRequest => self.handle_write_req(&mut trans, buf, &mut ctx.tx)?,
+            _ => {
+                error!("Opcode Not Handled: {:?}", proto_opcode);
+                return Err(Error::InvalidOpcode);
+            }
+        };
+
+        if result == ResponseRequired::Yes {
+            info!("Sending response");
+            tlv::print_tlv_list(ctx.tx.as_borrow_slice());
+        }
+        if trans.is_complete() {
+            ctx.exch_ctx.exch.close();
+        }
+        Ok(result)
+    }
+
+    fn get_proto_id(&self) -> usize {
+        PROTO_ID_INTERACTION_MODEL as usize
+    }
+}
+
+#[derive(FromPrimitive, Debug, Clone, Copy, PartialEq)]
+pub enum IMStatusCode {
+    Sucess = 0,
+    Failure = 1,
+    InvalidSubscription = 0x7D,
+    UnsupportedAccess = 0x7E,
+    UnsupportedEndpoint = 0x7F,
+    InvalidAction = 0x80,
+    UnsupportedCommand = 0x81,
+    InvalidCommand = 0x85,
+    UnsupportedAttribute = 0x86,
+    ConstraintError = 0x87,
+    UnsupportedWrite = 0x88,
+    ResourceExhausted = 0x89,
+    NotFound = 0x8b,
+    UnreportableAttribute = 0x8c,
+    InvalidDataType = 0x8d,
+    UnsupportedRead = 0x8f,
+    DataVersionMismatch = 0x92,
+    Timeout = 0x94,
+    Busy = 0x9c,
+    UnsupportedCluster = 0xc3,
+    NoUpstreamSubscription = 0xc5,
+    NeedsTimedInteraction = 0xc6,
+}
+
+impl From<Error> for IMStatusCode {
+    fn from(e: Error) -> Self {
+        match e {
+            Error::EndpointNotFound => IMStatusCode::UnsupportedEndpoint,
+            Error::ClusterNotFound => IMStatusCode::UnsupportedCluster,
+            Error::AttributeNotFound => IMStatusCode::UnsupportedAttribute,
+            Error::CommandNotFound => IMStatusCode::UnsupportedCommand,
+            _ => IMStatusCode::Failure,
+        }
+    }
+}
+
+impl FromTLV<'_> for IMStatusCode {
+    fn from_tlv(t: &TLVElement) -> Result<Self, Error> {
+        num::FromPrimitive::from_u16(t.u16()?).ok_or(Error::Invalid)
+    }
+}
+
+impl ToTLV for IMStatusCode {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag_type: TagType) -> Result<(), Error> {
+        tw.u16(tag_type, *self as u16)
+    }
+}

matter/src/interaction_model/messages.rs

@@ -0,0 +1,451 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    error::Error,
+    tlv::{FromTLV, TLVElement, TLVWriter, TagType, ToTLV},
+};
+
+// A generic path with endpoint, clusters, and a leaf
+// The leaf could be command, attribute, event
+#[derive(Default, Clone, Copy, Debug, PartialEq, FromTLV, ToTLV)]
+#[tlvargs(datatype = "list")]
+pub struct GenericPath {
+    pub endpoint: Option<u16>,
+    pub cluster: Option<u32>,
+    pub leaf: Option<u32>,
+}
+
+impl GenericPath {
+    pub fn new(endpoint: Option<u16>, cluster: Option<u32>, leaf: Option<u32>) -> Self {
+        Self {
+            endpoint,
+            cluster,
+            leaf,
+        }
+    }
+
+    /// Returns Ok, if the path is non wildcard, otherwise returns an error
+    pub fn not_wildcard(&self) -> Result<(u16, u32, u32), Error> {
+        match *self {
+            GenericPath {
+                endpoint: Some(e),
+                cluster: Some(c),
+                leaf: Some(l),
+            } => Ok((e, c, l)),
+            _ => Err(Error::Invalid),
+        }
+    }
+    /// Returns true, if the path is wildcard
+    pub fn is_wildcard(&self) -> bool {
+        !matches!(
+            *self,
+            GenericPath {
+                endpoint: Some(_),
+                cluster: Some(_),
+                leaf: Some(_),
+            }
+        )
+    }
+}
+
+pub mod msg {
+
+    use crate::{
+        error::Error,
+        tlv::{FromTLV, TLVArray, TLVElement, TLVWriter, TagType, ToTLV},
+    };
+
+    use super::ib::{AttrData, AttrPath, AttrResp, CmdData, DataVersionFilter};
+
+    #[derive(FromTLV)]
+    #[tlvargs(lifetime = "'a")]
+    pub struct InvReq<'a> {
+        pub suppress_response: Option<bool>,
+        pub timed_request: Option<bool>,
+        pub inv_requests: Option<TLVArray<'a, CmdData<'a>>>,
+    }
+
+    pub enum InvRespTag {
+        SupressResponse = 0,
+        InvokeResponses = 1,
+    }
+
+    pub enum InvReqTag {
+        SupressResponse = 0,
+        TimedReq = 1,
+        InvokeRequests = 2,
+    }
+
+    #[derive(Default, ToTLV, FromTLV)]
+    #[tlvargs(lifetime = "'a")]
+    pub struct ReadReq<'a> {
+        pub attr_requests: Option<TLVArray<'a, AttrPath>>,
+        event_requests: Option<bool>,
+        event_filters: Option<bool>,
+        pub fabric_filtered: bool,
+        pub dataver_filters: Option<TLVArray<'a, DataVersionFilter>>,
+    }
+
+    impl<'a> ReadReq<'a> {
+        pub fn new(fabric_filtered: bool) -> Self {
+            Self {
+                fabric_filtered,
+                ..Default::default()
+            }
+        }
+
+        pub fn set_attr_requests(mut self, requests: &'a [AttrPath]) -> Self {
+            self.attr_requests = Some(TLVArray::new(requests));
+            self
+        }
+    }
+
+    #[derive(ToTLV, FromTLV)]
+    #[tlvargs(lifetime = "'b")]
+    pub struct WriteReq<'a, 'b> {
+        pub supress_response: Option<bool>,
+        timed_request: Option<bool>,
+        pub write_requests: TLVArray<'a, AttrData<'b>>,
+        more_chunked: Option<bool>,
+    }
+
+    impl<'a, 'b> WriteReq<'a, 'b> {
+        pub fn new(supress_response: bool, write_requests: &'a [AttrData<'b>]) -> Self {
+            let mut w = Self {
+                supress_response: None,
+                write_requests: TLVArray::new(write_requests),
+                timed_request: None,
+                more_chunked: None,
+            };
+            if supress_response {
+                w.supress_response = Some(true);
+            }
+            w
+        }
+    }
+
+    // Report Data
+    #[derive(FromTLV, ToTLV)]
+    #[tlvargs(lifetime = "'a")]
+    pub struct ReportDataMsg<'a> {
+        pub subscription_id: Option<u32>,
+        pub attr_reports: Option<TLVArray<'a, AttrResp<'a>>>,
+        // TODO
+        pub event_reports: Option<bool>,
+        pub more_chunks: Option<bool>,
+        pub suppress_response: Option<bool>,
+    }
+
+    pub enum ReportDataTag {
+        _SubscriptionId = 0,
+        AttributeReports = 1,
+        _EventReport = 2,
+        _MoreChunkedMsgs = 3,
+        SupressResponse = 4,
+    }
+
+    // Write Response
+    pub enum WriteRespTag {
+        WriteResponses = 0,
+    }
+}
+
+pub mod ib {
+    use std::fmt::Debug;
+
+    use crate::{
+        data_model::objects::{AttrDetails, EncodeValue},
+        error::Error,
+        interaction_model::core::IMStatusCode,
+        tlv::{FromTLV, Nullable, TLVElement, TLVWriter, TagType, ToTLV},
+    };
+    use log::error;
+
+    use super::GenericPath;
+
+    // Command Response
+    #[derive(Clone, Copy, FromTLV, ToTLV)]
+    #[tlvargs(lifetime = "'a")]
+    pub enum InvResp<'a> {
+        Cmd(CmdData<'a>),
+        Status(CmdStatus),
+    }
+
+    impl<'a> InvResp<'a> {
+        pub fn cmd_new(endpoint: u16, cluster: u32, cmd: u16, data: EncodeValue<'a>) -> Self {
+            Self::Cmd(CmdData::new(
+                CmdPath::new(Some(endpoint), Some(cluster), Some(cmd)),
+                data,
+            ))
+        }
+
+        pub fn status_new(cmd_path: CmdPath, status: IMStatusCode, cluster_status: u16) -> Self {
+            Self::Status(CmdStatus {
+                path: cmd_path,
+                status: Status::new(status, cluster_status),
+            })
+        }
+    }
+
+    #[derive(FromTLV, ToTLV, Copy, Clone, PartialEq, Debug)]
+    pub struct CmdStatus {
+        path: CmdPath,
+        status: Status,
+    }
+
+    impl CmdStatus {
+        pub fn new(path: CmdPath, status: IMStatusCode, cluster_status: u16) -> Self {
+            Self {
+                path,
+                status: Status {
+                    status,
+                    cluster_status,
+                },
+            }
+        }
+    }
+
+    #[derive(Debug, Clone, Copy, FromTLV, ToTLV)]
+    #[tlvargs(lifetime = "'a")]
+    pub struct CmdData<'a> {
+        pub path: CmdPath,
+        pub data: EncodeValue<'a>,
+    }
+
+    impl<'a> CmdData<'a> {
+        pub fn new(path: CmdPath, data: EncodeValue<'a>) -> Self {
+            Self { path, data }
+        }
+    }
+
+    // Status
+    #[derive(Debug, Clone, Copy, PartialEq, FromTLV, ToTLV)]
+    pub struct Status {
+        pub status: IMStatusCode,
+        pub cluster_status: u16,
+    }
+
+    impl Status {
+        pub fn new(status: IMStatusCode, cluster_status: u16) -> Status {
+            Status {
+                status,
+                cluster_status,
+            }
+        }
+    }
+
+    // Attribute Response
+    #[derive(Clone, Copy, FromTLV, ToTLV, PartialEq, Debug)]
+    #[tlvargs(lifetime = "'a")]
+    pub enum AttrResp<'a> {
+        Status(AttrStatus),
+        Data(AttrData<'a>),
+    }
+
+    impl<'a> AttrResp<'a> {
+        pub fn new(data_ver: u32, path: &AttrPath, data: EncodeValue<'a>) -> Self {
+            AttrResp::Data(AttrData::new(Some(data_ver), *path, data))
+        }
+
+        pub fn unwrap_data(self) -> AttrData<'a> {
+            match self {
+                AttrResp::Data(d) => d,
+                _ => {
+                    panic!("No data exists");
+                }
+            }
+        }
+    }
+
+    // Attribute Data
+    #[derive(Clone, Copy, PartialEq, FromTLV, ToTLV, Debug)]
+    #[tlvargs(lifetime = "'a")]
+    pub struct AttrData<'a> {
+        pub data_ver: Option<u32>,
+        pub path: AttrPath,
+        pub data: EncodeValue<'a>,
+    }
+
+    impl<'a> AttrData<'a> {
+        pub fn new(data_ver: Option<u32>, path: AttrPath, data: EncodeValue<'a>) -> Self {
+            Self {
+                data_ver,
+                path,
+                data,
+            }
+        }
+    }
+
+    #[derive(Debug)]
+    /// Operations on an Interaction Model List
+    pub enum ListOperation {
+        /// Add (append) an item to the list
+        AddItem,
+        /// Edit an item from the list
+        EditItem(u16),
+        /// Delete item from the list
+        DeleteItem(u16),
+        /// Delete the whole list
+        DeleteList,
+    }
+
+    /// Attribute Lists in Attribute Data are special. Infer the correct meaning using this function
+    pub fn attr_list_write<F>(
+        attr: &AttrDetails,
+        data: &TLVElement,
+        mut f: F,
+    ) -> Result<(), IMStatusCode>
+    where
+        F: FnMut(ListOperation, &TLVElement) -> Result<(), IMStatusCode>,
+    {
+        if let Some(Nullable::NotNull(index)) = attr.list_index {
+            // If list index is valid,
+            //    - this is a modify item or delete item operation
+            if data.null().is_ok() {
+                // If data is NULL, delete item
+                f(ListOperation::DeleteItem(index), data)
+            } else {
+                f(ListOperation::EditItem(index), data)
+            }
+        } else {
+            if data.confirm_array().is_ok() {
+                // If data is list, this is either Delete List or OverWrite List operation
+                // in either case, we have to first delete the whole list
+                f(ListOperation::DeleteList, data)?;
+                // Now the data must be a list, that should be added item by item
+
+                let container = data.enter().ok_or(Error::Invalid)?;
+                for d in container {
+                    f(ListOperation::AddItem, &d)?;
+                }
+                Ok(())
+            } else {
+                // If data is not a list, this must be an add operation
+                f(ListOperation::AddItem, data)
+            }
+        }
+    }
+
+    #[derive(Debug, Clone, Copy, PartialEq, FromTLV, ToTLV)]
+    pub struct AttrStatus {
+        path: AttrPath,
+        status: Status,
+    }
+
+    impl AttrStatus {
+        pub fn new(path: &GenericPath, status: IMStatusCode, cluster_status: u16) -> Self {
+            Self {
+                path: AttrPath::new(path),
+                status: super::ib::Status::new(status, cluster_status),
+            }
+        }
+    }
+
+    // Attribute Path
+    #[derive(Default, Clone, Copy, Debug, PartialEq, FromTLV, ToTLV)]
+    #[tlvargs(datatype = "list")]
+    pub struct AttrPath {
+        pub tag_compression: Option<bool>,
+        pub node: Option<u64>,
+        pub endpoint: Option<u16>,
+        pub cluster: Option<u32>,
+        pub attr: Option<u16>,
+        pub list_index: Option<Nullable<u16>>,
+    }
+
+    impl AttrPath {
+        pub fn new(path: &GenericPath) -> Self {
+            Self {
+                endpoint: path.endpoint,
+                cluster: path.cluster,
+                attr: path.leaf.map(|x| x as u16),
+                ..Default::default()
+            }
+        }
+
+        pub fn to_gp(&self) -> GenericPath {
+            GenericPath::new(self.endpoint, self.cluster, self.attr.map(|x| x as u32))
+        }
+    }
+
+    // Command Path
+    #[derive(Default, Debug, Copy, Clone, PartialEq)]
+    pub struct CmdPath {
+        pub path: GenericPath,
+    }
+
+    #[macro_export]
+    macro_rules! cmd_path_ib {
+        ($endpoint:literal,$cluster:ident,$command:expr) => {{
+            use $crate::interaction_model::messages::{ib::CmdPath, GenericPath};
+            CmdPath {
+                path: GenericPath {
+                    endpoint: Some($endpoint),
+                    cluster: Some($cluster),
+                    leaf: Some($command as u32),
+                },
+            }
+        }};
+    }
+
+    impl CmdPath {
+        pub fn new(endpoint: Option<u16>, cluster: Option<u32>, command: Option<u16>) -> Self {
+            Self {
+                path: GenericPath {
+                    endpoint,
+                    cluster,
+                    leaf: command.map(|a| a as u32),
+                },
+            }
+        }
+    }
+
+    impl FromTLV<'_> for CmdPath {
+        fn from_tlv(cmd_path: &TLVElement) -> Result<Self, Error> {
+            let c = CmdPath {
+                path: GenericPath::from_tlv(cmd_path)?,
+            };
+
+            if c.path.leaf.is_none() {
+                error!("Wildcard command parameter not supported");
+                Err(Error::CommandNotFound)
+            } else {
+                Ok(c)
+            }
+        }
+    }
+
+    impl ToTLV for CmdPath {
+        fn to_tlv(&self, tw: &mut TLVWriter, tag_type: TagType) -> Result<(), Error> {
+            self.path.to_tlv(tw, tag_type)
+        }
+    }
+
+    #[derive(FromTLV, ToTLV, Copy, Clone)]
+    pub struct ClusterPath {
+        pub node: Option<u64>,
+        pub endpoint: u16,
+        pub cluster: u32,
+    }
+
+    #[derive(FromTLV, ToTLV, Copy, Clone)]
+    pub struct DataVersionFilter {
+        pub path: ClusterPath,
+        pub data_ver: u32,
+    }
+}

matter/src/interaction_model/mod.rs

@@ -0,0 +1,65 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::any::Any;
+
+use crate::{error::Error, tlv::TLVWriter, transport::session::Session};
+
+use self::messages::msg::{InvReq, ReadReq, WriteReq};
+
+#[derive(PartialEq)]
+pub enum TransactionState {
+    Ongoing,
+    Complete,
+}
+pub struct Transaction<'a> {
+    pub state: TransactionState,
+    pub data: Option<Box<dyn Any>>,
+    pub session: &'a mut Session,
+}
+
+pub trait InteractionConsumer {
+    fn consume_invoke_cmd(
+        &self,
+        req: &InvReq,
+        trans: &mut Transaction,
+        tw: &mut TLVWriter,
+    ) -> Result<(), Error>;
+
+    fn consume_read_attr(
+        &self,
+        req: &ReadReq,
+        trans: &mut Transaction,
+        tw: &mut TLVWriter,
+    ) -> Result<(), Error>;
+
+    fn consume_write_attr(
+        &self,
+        req: &WriteReq,
+        trans: &mut Transaction,
+        tw: &mut TLVWriter,
+    ) -> Result<(), Error>;
+}
+
+pub struct InteractionModel {
+    consumer: Box<dyn InteractionConsumer>,
+}
+pub mod command;
+pub mod core;
+pub mod messages;
+pub mod read;
+pub mod write;

matter/src/interaction_model/read.rs

@@ -0,0 +1,55 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::{
+    error::Error,
+    interaction_model::core::OpCode,
+    tlv::{get_root_node_struct, FromTLV, TLVWriter, TagType},
+    transport::{packet::Packet, proto_demux::ResponseRequired},
+};
+
+use super::{
+    messages::msg::{self, ReadReq},
+    InteractionModel, Transaction,
+};
+
+impl InteractionModel {
+    pub fn handle_read_req(
+        &mut self,
+        trans: &mut Transaction,
+        rx_buf: &[u8],
+        proto_tx: &mut Packet,
+    ) -> Result<ResponseRequired, Error> {
+        proto_tx.set_proto_opcode(OpCode::ReportData as u8);
+
+        let mut tw = TLVWriter::new(proto_tx.get_writebuf()?);
+        let root = get_root_node_struct(rx_buf)?;
+        let read_req = ReadReq::from_tlv(&root)?;
+
+        tw.start_struct(TagType::Anonymous)?;
+        self.consumer.consume_read_attr(&read_req, trans, &mut tw)?;
+        // Supress response always true for read interaction
+        tw.bool(
+            TagType::Context(msg::ReportDataTag::SupressResponse as u8),
+            true,
+        )?;
+        tw.end_container()?;
+
+        trans.complete();
+        Ok(ResponseRequired::Yes)
+    }
+}

matter/src/interaction_model/write.rs

@@ -0,0 +1,55 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use log::error;
+
+use crate::{
+    error::Error,
+    tlv::{get_root_node_struct, FromTLV, TLVWriter, TagType},
+    transport::{packet::Packet, proto_demux::ResponseRequired},
+};
+
+use super::{core::OpCode, messages::msg::WriteReq, InteractionModel, Transaction};
+
+impl InteractionModel {
+    pub fn handle_write_req(
+        &mut self,
+        trans: &mut Transaction,
+        rx_buf: &[u8],
+        proto_tx: &mut Packet,
+    ) -> Result<ResponseRequired, Error> {
+        proto_tx.set_proto_opcode(OpCode::WriteResponse as u8);
+
+        let mut tw = TLVWriter::new(proto_tx.get_writebuf()?);
+        let root = get_root_node_struct(rx_buf)?;
+        let write_req = WriteReq::from_tlv(&root)?;
+        let supress_response = write_req.supress_response.unwrap_or_default();
+
+        tw.start_struct(TagType::Anonymous)?;
+        self.consumer
+            .consume_write_attr(&write_req, trans, &mut tw)?;
+        tw.end_container()?;
+
+        trans.complete();
+        if supress_response {
+            error!("Supress response is set, is this the expected handling?");
+            Ok(ResponseRequired::No)
+        } else {
+            Ok(ResponseRequired::Yes)
+        }
+    }
+}

matter/src/lib.rs

@@ -0,0 +1,87 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+
+//! Native Rust Implementation of Matter (Smart-Home)
+//!
+//! This crate implements the Matter specification that can be run on embedded devices
+//! to build Matter-compatible smart-home/IoT devices.
+//!
+//! Currently Ethernet based transport is supported.
+//!
+//! # Examples
+//! ```
+//! use matter::{Matter, CommissioningData};
+//! use matter::data_model::device_types::device_type_add_on_off_light;
+//! use matter::data_model::cluster_basic_information::BasicInfoConfig;
+//! use rand::prelude::*;
+//!
+//! # use matter::data_model::sdm::dev_att::{DataType, DevAttDataFetcher};
+//! # use matter::error::Error;
+//! # pub struct DevAtt{}
+//! # impl DevAttDataFetcher for DevAtt{
+//! # fn get_devatt_data(&self, data_type: DataType, data: &mut [u8]) -> Result<usize, Error> { Ok(0) }
+//! # }
+//! # let dev_att = Box::new(DevAtt{});
+//!
+//! /// The commissioning data for this device
+//! let mut comm_data = CommissioningData {
+//!     passwd: 123456,
+//!     discriminator: 250,
+//!     ..Default::default()
+//! };
+//! rand::thread_rng().fill_bytes(&mut comm_data.salt);
+//!
+//! /// The basic information about this device
+//! let dev_info = BasicInfoConfig {
+//!     vid: 0x8002,
+//!     pid: 0xFFF1,
+//!     hw_ver: 2,
+//!     sw_ver: 1,
+//! };
+//!
+//! /// Get the Matter Object
+//! /// The dev_att is an object that implements the DevAttDataFetcher trait.
+//! let mut matter = Matter::new(dev_info, dev_att, comm_data).unwrap();
+//! let dm = matter.get_data_model();
+//! {
+//!     let mut node = dm.node.write().unwrap();
+//!     /// Add our device-types
+//!     let endpoint = device_type_add_on_off_light(&mut node).unwrap();
+//! }
+//! // Start the Matter Daemon
+//! // matter.start_daemon().unwrap();
+//! ```
+//! Start off exploring by going to the [Matter] object.
+
+pub mod acl;
+pub mod cert;
+pub mod core;
+pub mod crypto;
+pub mod data_model;
+pub mod error;
+pub mod fabric;
+pub mod group_keys;
+pub mod interaction_model;
+pub mod mdns;
+pub mod secure_channel;
+pub mod sys;
+pub mod tlv;
+pub mod transport;
+pub mod utils;
+
+pub use crate::core::*;

matter/src/mdns.rs

@@ -0,0 +1,101 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::{Arc, Mutex, Once};
+
+use crate::{
+    error::Error,
+    sys::{sys_publish_service, SysMdnsService},
+    transport::udp::MATTER_PORT,
+};
+
+#[derive(Default)]
+/// The mDNS service handler
+pub struct MdnsInner {
+    /// Vendor ID
+    vid: u16,
+    /// Product ID
+    pid: u16,
+    /// Discriminator
+    discriminator: u16,
+}
+
+pub struct Mdns {
+    inner: Mutex<MdnsInner>,
+}
+
+const SHORT_DISCRIMINATOR_MASK: u16 = 0x700;
+const SHORT_DISCRIMINATOR_SHIFT: u16 = 8;
+
+static mut G_MDNS: Option<Arc<Mdns>> = None;
+static INIT: Once = Once::new();
+
+pub enum ServiceMode {
+    Commissioned,
+    Commissionable,
+}
+
+impl Mdns {
+    fn new() -> Self {
+        Self {
+            inner: Mutex::new(MdnsInner {
+                ..Default::default()
+            }),
+        }
+    }
+
+    /// Get a handle to the globally unique mDNS instance
+    pub fn get() -> Result<Arc<Self>, Error> {
+        unsafe {
+            INIT.call_once(|| {
+                G_MDNS = Some(Arc::new(Mdns::new()));
+            });
+            Ok(G_MDNS.as_ref().ok_or(Error::Invalid)?.clone())
+        }
+    }
+
+    /// Set mDNS service specific values
+    /// Values like vid, pid, discriminator etc
+    // TODO: More things like device-type etc can be added here
+    pub fn set_values(&self, vid: u16, pid: u16, discriminator: u16) {
+        let mut inner = self.inner.lock().unwrap();
+        inner.vid = vid;
+        inner.pid = pid;
+        inner.discriminator = discriminator;
+    }
+
+    /// Publish a mDNS service
+    /// name - is the service name (comma separated subtypes may follow)
+    /// mode - the current service mode
+    pub fn publish_service(&self, name: &str, mode: ServiceMode) -> Result<SysMdnsService, Error> {
+        match mode {
+            ServiceMode::Commissioned => {
+                sys_publish_service(name, "_matter._tcp", MATTER_PORT, &[])
+            }
+            ServiceMode::Commissionable => {
+                let inner = self.inner.lock().unwrap();
+                let short =
+                    (inner.discriminator & SHORT_DISCRIMINATOR_MASK) >> SHORT_DISCRIMINATOR_SHIFT;
+                let serv_type = format!("_matterc._udp,_S{},_L{}", short, inner.discriminator);
+
+                let str_discriminator = format!("{}", inner.discriminator);
+                let txt_kvs = [["D", &str_discriminator], ["CM", "1"]];
+                sys_publish_service(name, &serv_type, MATTER_PORT, &txt_kvs)
+            }
+        }
+    }
+}

matter/src/secure_channel/case.rs

@@ -0,0 +1,542 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::Arc;
+
+use log::{error, trace};
+use owning_ref::RwLockReadGuardRef;
+use rand::prelude::*;
+
+use crate::{
+    cert::Cert,
+    crypto::{self, CryptoKeyPair, KeyPair, Sha256},
+    error::Error,
+    fabric::{Fabric, FabricMgr, FabricMgrInner},
+    secure_channel::common,
+    secure_channel::common::SCStatusCodes,
+    tlv::{get_root_node_struct, FromTLV, OctetStr, TLVElement, TLVWriter, TagType},
+    transport::{
+        network::Address,
+        proto_demux::ProtoCtx,
+        queue::{Msg, WorkQ},
+        session::{CloneData, SessionMode},
+    },
+    utils::writebuf::WriteBuf,
+};
+
+#[derive(PartialEq)]
+enum State {
+    Sigma1Rx,
+    Sigma3Rx,
+}
+
+pub struct CaseSession {
+    state: State,
+    peer_sessid: u16,
+    local_sessid: u16,
+    tt_hash: Sha256,
+    shared_secret: [u8; crypto::ECDH_SHARED_SECRET_LEN_BYTES],
+    our_pub_key: [u8; crypto::EC_POINT_LEN_BYTES],
+    peer_pub_key: [u8; crypto::EC_POINT_LEN_BYTES],
+    local_fabric_idx: usize,
+}
+impl CaseSession {
+    pub fn new(peer_sessid: u16, local_sessid: u16) -> Result<Self, Error> {
+        Ok(Self {
+            state: State::Sigma1Rx,
+            peer_sessid,
+            local_sessid,
+            tt_hash: Sha256::new()?,
+            shared_secret: [0; crypto::ECDH_SHARED_SECRET_LEN_BYTES],
+            our_pub_key: [0; crypto::EC_POINT_LEN_BYTES],
+            peer_pub_key: [0; crypto::EC_POINT_LEN_BYTES],
+            local_fabric_idx: 0,
+        })
+    }
+}
+
+pub struct Case {
+    fabric_mgr: Arc<FabricMgr>,
+}
+
+impl Case {
+    pub fn new(fabric_mgr: Arc<FabricMgr>) -> Self {
+        Self { fabric_mgr }
+    }
+
+    pub fn handle_casesigma3(&mut self, ctx: &mut ProtoCtx) -> Result<(), Error> {
+        let mut case_session = ctx
+            .exch_ctx
+            .exch
+            .take_exchange_data::<CaseSession>()
+            .ok_or(Error::InvalidState)?;
+        if case_session.state != State::Sigma1Rx {
+            return Err(Error::Invalid);
+        }
+        case_session.state = State::Sigma3Rx;
+
+        let fabric = self.fabric_mgr.get_fabric(case_session.local_fabric_idx)?;
+        if fabric.is_none() {
+            common::create_sc_status_report(
+                &mut ctx.tx,
+                common::SCStatusCodes::NoSharedTrustRoots,
+                None,
+            )?;
+            ctx.exch_ctx.exch.close();
+            return Ok(());
+        }
+        // Safe to unwrap here
+        let fabric = fabric.as_ref().as_ref().unwrap();
+
+        let root = get_root_node_struct(ctx.rx.as_borrow_slice())?;
+        let encrypted = root.find_tag(1)?.slice()?;
+
+        let mut decrypted: [u8; 800] = [0; 800];
+        if encrypted.len() > decrypted.len() {
+            error!("Data too large");
+            return Err(Error::NoSpace);
+        }
+        let decrypted = &mut decrypted[..encrypted.len()];
+        decrypted.copy_from_slice(encrypted);
+
+        let len = Case::get_sigma3_decryption(fabric.ipk.op_key(), &case_session, decrypted)?;
+        let decrypted = &decrypted[..len];
+
+        let root = get_root_node_struct(decrypted)?;
+        let d = Sigma3Decrypt::from_tlv(&root)?;
+
+        let initiator_noc = Cert::new(d.initiator_noc.0)?;
+        let initiator_icac = Cert::new(d.initiator_icac.0)?;
+        if let Err(e) = Case::validate_certs(fabric, &initiator_noc, &initiator_icac) {
+            error!("Certificate Chain doesn't match: {}", e);
+            common::create_sc_status_report(
+                &mut ctx.tx,
+                common::SCStatusCodes::InvalidParameter,
+                None,
+            )?;
+            ctx.exch_ctx.exch.close();
+            return Ok(());
+        }
+
+        if Case::validate_sigma3_sign(
+            d.initiator_noc.0,
+            d.initiator_icac.0,
+            &initiator_noc,
+            d.signature.0,
+            &case_session,
+        )
+        .is_err()
+        {
+            error!("Sigma3 Signature doesn't match");
+            common::create_sc_status_report(
+                &mut ctx.tx,
+                common::SCStatusCodes::InvalidParameter,
+                None,
+            )?;
+            ctx.exch_ctx.exch.close();
+            return Ok(());
+        }
+
+        // Only now do we add this message to the TT Hash
+        case_session.tt_hash.update(ctx.rx.as_borrow_slice())?;
+        let clone_data = Case::get_session_clone_data(
+            fabric.ipk.op_key(),
+            fabric.get_node_id(),
+            initiator_noc.get_node_id()?,
+            ctx.exch_ctx.sess.get_peer_addr(),
+            &case_session,
+        )?;
+        // Queue a transport mgr request to add a new session
+        WorkQ::get()?.sync_send(Msg::NewSession(clone_data))?;
+
+        common::create_sc_status_report(
+            &mut ctx.tx,
+            SCStatusCodes::SessionEstablishmentSuccess,
+            None,
+        )?;
+        ctx.exch_ctx.exch.clear_exchange_data();
+        ctx.exch_ctx.exch.close();
+
+        Ok(())
+    }
+
+    pub fn handle_casesigma1(&mut self, ctx: &mut ProtoCtx) -> Result<(), Error> {
+        let rx_buf = ctx.rx.as_borrow_slice();
+        let root = get_root_node_struct(rx_buf)?;
+        let r = Sigma1Req::from_tlv(&root)?;
+
+        let local_fabric_idx = self
+            .fabric_mgr
+            .match_dest_id(r.initiator_random.0, r.dest_id.0);
+        if local_fabric_idx.is_err() {
+            error!("Fabric Index mismatch");
+            common::create_sc_status_report(
+                &mut ctx.tx,
+                common::SCStatusCodes::NoSharedTrustRoots,
+                None,
+            )?;
+            ctx.exch_ctx.exch.close();
+            return Ok(());
+        }
+
+        let local_sessid = ctx.exch_ctx.sess.reserve_new_sess_id();
+        let mut case_session = Box::new(CaseSession::new(r.initiator_sessid, local_sessid)?);
+        case_session.tt_hash.update(rx_buf)?;
+        case_session.local_fabric_idx = local_fabric_idx?;
+        if r.peer_pub_key.0.len() != crypto::EC_POINT_LEN_BYTES {
+            error!("Invalid public key length");
+            return Err(Error::Invalid);
+        }
+        case_session.peer_pub_key.copy_from_slice(r.peer_pub_key.0);
+        trace!(
+            "Destination ID matched to fabric index {}",
+            case_session.local_fabric_idx
+        );
+
+        // Create an ephemeral Key Pair
+        let key_pair = KeyPair::new()?;
+        let _ = key_pair.get_public_key(&mut case_session.our_pub_key)?;
+
+        // Derive the Shared Secret
+        let len = key_pair.derive_secret(r.peer_pub_key.0, &mut case_session.shared_secret)?;
+        if len != 32 {
+            error!("Derived secret length incorrect");
+            return Err(Error::Invalid);
+        }
+        //        println!("Derived secret: {:x?} len: {}", secret, len);
+
+        let mut our_random: [u8; 32] = [0; 32];
+        rand::thread_rng().fill_bytes(&mut our_random);
+
+        // Derive the Encrypted Part
+        const MAX_ENCRYPTED_SIZE: usize = 800;
+
+        let mut encrypted: [u8; MAX_ENCRYPTED_SIZE] = [0; MAX_ENCRYPTED_SIZE];
+        let encrypted_len = {
+            let mut signature = [0u8; crypto::EC_SIGNATURE_LEN_BYTES];
+            let fabric = self.fabric_mgr.get_fabric(case_session.local_fabric_idx)?;
+            if fabric.is_none() {
+                common::create_sc_status_report(
+                    &mut ctx.tx,
+                    common::SCStatusCodes::NoSharedTrustRoots,
+                    None,
+                )?;
+                ctx.exch_ctx.exch.close();
+                return Ok(());
+            }
+
+            let sign_len = Case::get_sigma2_sign(
+                &fabric,
+                &case_session.our_pub_key,
+                &case_session.peer_pub_key,
+                &mut signature,
+            )?;
+            let signature = &signature[..sign_len];
+
+            Case::get_sigma2_encryption(
+                &fabric,
+                &our_random,
+                &mut case_session,
+                signature,
+                &mut encrypted,
+            )?
+        };
+        let encrypted = &encrypted[0..encrypted_len];
+
+        // Generate our Response Body
+        let mut tw = TLVWriter::new(ctx.tx.get_writebuf()?);
+        tw.start_struct(TagType::Anonymous)?;
+        tw.str8(TagType::Context(1), &our_random)?;
+        tw.u16(TagType::Context(2), local_sessid)?;
+        tw.str8(TagType::Context(3), &case_session.our_pub_key)?;
+        tw.str16(TagType::Context(4), encrypted)?;
+        tw.end_container()?;
+        case_session.tt_hash.update(ctx.tx.as_borrow_slice())?;
+        ctx.exch_ctx.exch.set_exchange_data(case_session);
+        Ok(())
+    }
+
+    fn get_session_clone_data(
+        ipk: &[u8],
+        local_nodeid: u64,
+        peer_nodeid: u64,
+        peer_addr: Address,
+        case_session: &CaseSession,
+    ) -> Result<CloneData, Error> {
+        let mut session_keys = [0_u8; 3 * crypto::SYMM_KEY_LEN_BYTES];
+        Case::get_session_keys(
+            ipk,
+            &case_session.tt_hash,
+            &case_session.shared_secret,
+            &mut session_keys,
+        )?;
+
+        let mut clone_data = CloneData::new(
+            local_nodeid,
+            peer_nodeid,
+            case_session.peer_sessid,
+            case_session.local_sessid,
+            peer_addr,
+            SessionMode::Case(case_session.local_fabric_idx as u8),
+        );
+
+        clone_data.dec_key.copy_from_slice(&session_keys[0..16]);
+        clone_data.enc_key.copy_from_slice(&session_keys[16..32]);
+        clone_data
+            .att_challenge
+            .copy_from_slice(&session_keys[32..48]);
+        Ok(clone_data)
+    }
+
+    fn validate_sigma3_sign(
+        initiator_noc: &[u8],
+        initiator_icac: &[u8],
+        initiator_noc_cert: &Cert,
+        sign: &[u8],
+        case_session: &CaseSession,
+    ) -> Result<(), Error> {
+        const MAX_TBS_SIZE: usize = 800;
+        let mut buf: [u8; MAX_TBS_SIZE] = [0; MAX_TBS_SIZE];
+        let mut write_buf = WriteBuf::new(&mut buf, MAX_TBS_SIZE);
+        let mut tw = TLVWriter::new(&mut write_buf);
+        tw.start_struct(TagType::Anonymous)?;
+        tw.str8(TagType::Context(1), initiator_noc)?;
+        tw.str8(TagType::Context(2), initiator_icac)?;
+        tw.str8(TagType::Context(3), &case_session.peer_pub_key)?;
+        tw.str8(TagType::Context(4), &case_session.our_pub_key)?;
+        tw.end_container()?;
+
+        let key = KeyPair::new_from_public(initiator_noc_cert.get_pubkey())?;
+        key.verify_msg(write_buf.as_slice(), sign)?;
+        Ok(())
+    }
+
+    fn validate_certs(fabric: &Fabric, noc: &Cert, icac: &Cert) -> Result<(), Error> {
+        if let Ok(fid) = icac.get_fabric_id() {
+            if fid != fabric.get_fabric_id() {
+                return Err(Error::Invalid);
+            }
+        }
+
+        if fabric.get_fabric_id() != noc.get_fabric_id()? {
+            return Err(Error::Invalid);
+        }
+
+        noc.verify_chain_start()
+            .add_cert(icac)?
+            .add_cert(&fabric.root_ca)?
+            .finalise()?;
+
+        Ok(())
+    }
+
+    fn get_session_keys(
+        ipk: &[u8],
+        tt: &Sha256,
+        shared_secret: &[u8],
+        key: &mut [u8],
+    ) -> Result<(), Error> {
+        const SEKEYS_INFO: [u8; 11] = [
+            0x53, 0x65, 0x73, 0x73, 0x69, 0x6f, 0x6e, 0x4b, 0x65, 0x79, 0x73,
+        ];
+        if key.len() < 48 {
+            return Err(Error::NoSpace);
+        }
+        let mut salt = Vec::<u8>::with_capacity(256);
+        salt.extend_from_slice(ipk);
+        let tt = tt.clone();
+        let mut tt_hash = [0u8; crypto::SHA256_HASH_LEN_BYTES];
+        tt.finish(&mut tt_hash)?;
+        salt.extend_from_slice(&tt_hash);
+        //        println!("Session Key: salt: {:x?}, len: {}", salt, salt.len());
+
+        crypto::hkdf_sha256(salt.as_slice(), shared_secret, &SEKEYS_INFO, key)
+            .map_err(|_x| Error::NoSpace)?;
+        //        println!("Session Key: key: {:x?}", key);
+
+        Ok(())
+    }
+
+    fn get_sigma3_decryption(
+        ipk: &[u8],
+        case_session: &CaseSession,
+        encrypted: &mut [u8],
+    ) -> Result<usize, Error> {
+        let mut sigma3_key = [0_u8; crypto::SYMM_KEY_LEN_BYTES];
+        Case::get_sigma3_key(
+            ipk,
+            &case_session.tt_hash,
+            &case_session.shared_secret,
+            &mut sigma3_key,
+        )?;
+        // println!("Sigma3 Key: {:x?}", sigma3_key);
+
+        let nonce: [u8; 13] = [
+            0x4e, 0x43, 0x41, 0x53, 0x45, 0x5f, 0x53, 0x69, 0x67, 0x6d, 0x61, 0x33, 0x4e,
+        ];
+
+        let encrypted_len = encrypted.len();
+        crypto::decrypt_in_place(&sigma3_key, &nonce, &[], encrypted)?;
+        Ok(encrypted_len - crypto::AEAD_MIC_LEN_BYTES)
+    }
+
+    fn get_sigma3_key(
+        ipk: &[u8],
+        tt: &Sha256,
+        shared_secret: &[u8],
+        key: &mut [u8],
+    ) -> Result<(), Error> {
+        const S3K_INFO: [u8; 6] = [0x53, 0x69, 0x67, 0x6d, 0x61, 0x33];
+        if key.len() < 16 {
+            return Err(Error::NoSpace);
+        }
+        let mut salt = Vec::<u8>::with_capacity(256);
+        salt.extend_from_slice(ipk);
+
+        let tt = tt.clone();
+
+        let mut tt_hash = [0u8; crypto::SHA256_HASH_LEN_BYTES];
+        tt.finish(&mut tt_hash)?;
+        salt.extend_from_slice(&tt_hash);
+        //        println!("Sigma3Key: salt: {:x?}, len: {}", salt, salt.len());
+
+        crypto::hkdf_sha256(salt.as_slice(), shared_secret, &S3K_INFO, key)
+            .map_err(|_x| Error::NoSpace)?;
+        //        println!("Sigma3Key: key: {:x?}", key);
+
+        Ok(())
+    }
+
+    fn get_sigma2_key(
+        ipk: &[u8],
+        our_random: &[u8],
+        case_session: &mut CaseSession,
+        key: &mut [u8],
+    ) -> Result<(), Error> {
+        const S2K_INFO: [u8; 6] = [0x53, 0x69, 0x67, 0x6d, 0x61, 0x32];
+        if key.len() < 16 {
+            return Err(Error::NoSpace);
+        }
+        let mut salt = Vec::<u8>::with_capacity(256);
+        salt.extend_from_slice(ipk);
+        salt.extend_from_slice(our_random);
+        salt.extend_from_slice(&case_session.our_pub_key);
+
+        let tt = case_session.tt_hash.clone();
+
+        let mut tt_hash = [0u8; crypto::SHA256_HASH_LEN_BYTES];
+        tt.finish(&mut tt_hash)?;
+        salt.extend_from_slice(&tt_hash);
+        //        println!("Sigma2Key: salt: {:x?}, len: {}", salt, salt.len());
+
+        crypto::hkdf_sha256(salt.as_slice(), &case_session.shared_secret, &S2K_INFO, key)
+            .map_err(|_x| Error::NoSpace)?;
+        //        println!("Sigma2Key: key: {:x?}", key);
+
+        Ok(())
+    }
+
+    fn get_sigma2_encryption(
+        fabric: &RwLockReadGuardRef<FabricMgrInner, Option<Fabric>>,
+        our_random: &[u8],
+        case_session: &mut CaseSession,
+        signature: &[u8],
+        out: &mut [u8],
+    ) -> Result<usize, Error> {
+        let mut resumption_id: [u8; 16] = [0; 16];
+        rand::thread_rng().fill_bytes(&mut resumption_id);
+
+        // We are guaranteed this unwrap will work
+        let fabric = fabric.as_ref().as_ref().unwrap();
+
+        let mut sigma2_key = [0_u8; crypto::SYMM_KEY_LEN_BYTES];
+        Case::get_sigma2_key(
+            fabric.ipk.op_key(),
+            our_random,
+            case_session,
+            &mut sigma2_key,
+        )?;
+
+        let mut write_buf = WriteBuf::new(out, out.len());
+        let mut tw = TLVWriter::new(&mut write_buf);
+        tw.start_struct(TagType::Anonymous)?;
+        tw.str16_as(TagType::Context(1), |buf| fabric.noc.as_tlv(buf))?;
+        tw.str16_as(TagType::Context(2), |buf| fabric.icac.as_tlv(buf))?;
+        tw.str8(TagType::Context(3), signature)?;
+        tw.str8(TagType::Context(4), &resumption_id)?;
+        tw.end_container()?;
+        //println!("TBE is {:x?}", write_buf.as_borrow_slice());
+        let nonce: [u8; crypto::AEAD_NONCE_LEN_BYTES] = [
+            0x4e, 0x43, 0x41, 0x53, 0x45, 0x5f, 0x53, 0x69, 0x67, 0x6d, 0x61, 0x32, 0x4e,
+        ];
+        //        let nonce = GenericArray::from_slice(&nonce);
+        //        type AesCcm = Ccm<Aes128, U16, U13>;
+        //        let cipher = AesCcm::new(GenericArray::from_slice(key));
+        const TAG_LEN: usize = 16;
+        let tag = [0u8; TAG_LEN];
+        write_buf.append(&tag)?;
+        let cipher_text = write_buf.as_mut_slice();
+
+        crypto::encrypt_in_place(
+            &sigma2_key,
+            &nonce,
+            &[],
+            cipher_text,
+            cipher_text.len() - TAG_LEN,
+        )?;
+        Ok(write_buf.as_slice().len())
+    }
+
+    fn get_sigma2_sign(
+        fabric: &RwLockReadGuardRef<FabricMgrInner, Option<Fabric>>,
+        our_pub_key: &[u8],
+        peer_pub_key: &[u8],
+        signature: &mut [u8],
+    ) -> Result<usize, Error> {
+        // We are guaranteed this unwrap will work
+        let fabric = fabric.as_ref().as_ref().unwrap();
+        const MAX_TBS_SIZE: usize = 800;
+        let mut buf: [u8; MAX_TBS_SIZE] = [0; MAX_TBS_SIZE];
+        let mut write_buf = WriteBuf::new(&mut buf, MAX_TBS_SIZE);
+        let mut tw = TLVWriter::new(&mut write_buf);
+        tw.start_struct(TagType::Anonymous)?;
+        tw.str16_as(TagType::Context(1), |buf| fabric.noc.as_tlv(buf))?;
+        tw.str16_as(TagType::Context(2), |buf| fabric.icac.as_tlv(buf))?;
+        tw.str8(TagType::Context(3), our_pub_key)?;
+        tw.str8(TagType::Context(4), peer_pub_key)?;
+        tw.end_container()?;
+        //println!("TBS is {:x?}", write_buf.as_borrow_slice());
+        fabric.sign_msg(write_buf.as_slice(), signature)
+    }
+}
+
+#[derive(FromTLV)]
+#[tlvargs(start = 1, lifetime = "'a")]
+struct Sigma1Req<'a> {
+    initiator_random: OctetStr<'a>,
+    initiator_sessid: u16,
+    dest_id: OctetStr<'a>,
+    peer_pub_key: OctetStr<'a>,
+}
+
+#[derive(FromTLV)]
+#[tlvargs(start = 1, lifetime = "'a")]
+struct Sigma3Decrypt<'a> {
+    initiator_noc: OctetStr<'a>,
+    initiator_icac: OctetStr<'a>,
+    signature: OctetStr<'a>,
+}

matter/src/secure_channel/common.rs

@@ -0,0 +1,85 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use num_derive::FromPrimitive;
+
+use crate::{error::Error, transport::packet::Packet};
+
+use super::status_report::{create_status_report, GeneralCode};
+
+/* Interaction Model ID as per the Matter Spec */
+pub const PROTO_ID_SECURE_CHANNEL: usize = 0x00;
+
+#[derive(FromPrimitive, Debug)]
+pub enum OpCode {
+    MsgCounterSyncReq = 0x00,
+    MsgCounterSyncResp = 0x01,
+    MRPStandAloneAck = 0x10,
+    PBKDFParamRequest = 0x20,
+    PBKDFParamResponse = 0x21,
+    PASEPake1 = 0x22,
+    PASEPake2 = 0x23,
+    PASEPake3 = 0x24,
+    CASESigma1 = 0x30,
+    CASESigma2 = 0x31,
+    CASESigma3 = 0x32,
+    CASESigma2Resume = 0x33,
+    StatusReport = 0x40,
+}
+
+#[derive(PartialEq)]
+pub enum SCStatusCodes {
+    SessionEstablishmentSuccess = 0,
+    NoSharedTrustRoots = 1,
+    InvalidParameter = 2,
+    CloseSession = 3,
+    Busy = 4,
+    SessionNotFound = 5,
+}
+
+pub fn create_sc_status_report(
+    proto_tx: &mut Packet,
+    status_code: SCStatusCodes,
+    proto_data: Option<&[u8]>,
+) -> Result<(), Error> {
+    let general_code = match status_code {
+        SCStatusCodes::SessionEstablishmentSuccess => GeneralCode::Success,
+        SCStatusCodes::CloseSession => {
+            proto_tx.unset_reliable();
+            // No time to manage reliable delivery for close session
+            // the session will be closed soon
+            GeneralCode::Success
+        }
+        SCStatusCodes::Busy
+        | SCStatusCodes::InvalidParameter
+        | SCStatusCodes::NoSharedTrustRoots
+        | SCStatusCodes::SessionNotFound => GeneralCode::Failure,
+    };
+    create_status_report(
+        proto_tx,
+        general_code,
+        PROTO_ID_SECURE_CHANNEL as u32,
+        status_code as u16,
+        proto_data,
+    )
+}
+
+pub fn create_mrp_standalone_ack(proto_tx: &mut Packet) {
+    proto_tx.set_proto_id(PROTO_ID_SECURE_CHANNEL as u16);
+    proto_tx.set_proto_opcode(OpCode::MRPStandAloneAck as u8);
+    proto_tx.unset_reliable();
+}

matter/src/secure_channel/core.rs

@@ -0,0 +1,140 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::Arc;
+
+use crate::{
+    error::*,
+    fabric::FabricMgr,
+    mdns::{self, Mdns},
+    secure_channel::{common::*, pake::PAKE},
+    sys::SysMdnsService,
+    transport::proto_demux::{self, ProtoCtx, ResponseRequired},
+};
+use log::{error, info};
+use num;
+use rand::prelude::*;
+
+use super::case::Case;
+
+/* Handle messages related to the Secure Channel
+ */
+
+pub struct SecureChannel {
+    case: Case,
+    pake: Option<(PAKE, SysMdnsService)>,
+}
+
+impl SecureChannel {
+    pub fn new(fabric_mgr: Arc<FabricMgr>) -> SecureChannel {
+        SecureChannel {
+            pake: None,
+            case: Case::new(fabric_mgr),
+        }
+    }
+
+    pub fn open_comm_window(&mut self, salt: &[u8; 16], passwd: u32) -> Result<(), Error> {
+        let name: u64 = rand::thread_rng().gen_range(0..0xFFFFFFFFFFFFFFFF);
+        let name = format!("{:016X}", name);
+        let mdns = Mdns::get()?.publish_service(&name, mdns::ServiceMode::Commissionable)?;
+        self.pake = Some((PAKE::new(salt, passwd), mdns));
+        Ok(())
+    }
+
+    pub fn close_comm_window(&mut self) {
+        self.pake = None;
+    }
+
+    fn mrpstandaloneack_handler(&mut self, _ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        info!("In MRP StandAlone ACK Handler");
+        Ok(ResponseRequired::No)
+    }
+
+    fn pbkdfparamreq_handler(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        info!("In PBKDF Param Request Handler");
+        ctx.tx.set_proto_opcode(OpCode::PBKDFParamResponse as u8);
+        if let Some((pake, _)) = &mut self.pake {
+            pake.handle_pbkdfparamrequest(ctx)?;
+        } else {
+            error!("PASE Not enabled");
+            create_sc_status_report(&mut ctx.tx, SCStatusCodes::InvalidParameter, None)?;
+        }
+        Ok(ResponseRequired::Yes)
+    }
+
+    fn pasepake1_handler(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        info!("In PASE Pake1 Handler");
+        ctx.tx.set_proto_opcode(OpCode::PASEPake2 as u8);
+        if let Some((pake, _)) = &mut self.pake {
+            pake.handle_pasepake1(ctx)?;
+        } else {
+            error!("PASE Not enabled");
+            create_sc_status_report(&mut ctx.tx, SCStatusCodes::InvalidParameter, None)?;
+        }
+        Ok(ResponseRequired::Yes)
+    }
+
+    fn pasepake3_handler(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        info!("In PASE Pake3 Handler");
+        if let Some((pake, _)) = &mut self.pake {
+            pake.handle_pasepake3(ctx)?;
+            // TODO: Currently we assume that PAKE is not successful and reset the PAKE object
+            self.pake = None;
+        } else {
+            error!("PASE Not enabled");
+            create_sc_status_report(&mut ctx.tx, SCStatusCodes::InvalidParameter, None)?;
+        }
+        Ok(ResponseRequired::Yes)
+    }
+
+    fn casesigma1_handler(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        info!("In CASE Sigma1 Handler");
+        ctx.tx.set_proto_opcode(OpCode::CASESigma2 as u8);
+        self.case.handle_casesigma1(ctx)?;
+        Ok(ResponseRequired::Yes)
+    }
+
+    fn casesigma3_handler(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        info!("In CASE Sigma3 Handler");
+        self.case.handle_casesigma3(ctx)?;
+        Ok(ResponseRequired::Yes)
+    }
+}
+
+impl proto_demux::HandleProto for SecureChannel {
+    fn handle_proto_id(&mut self, ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        let proto_opcode: OpCode =
+            num::FromPrimitive::from_u8(ctx.rx.get_proto_opcode()).ok_or(Error::Invalid)?;
+        ctx.tx.set_proto_id(PROTO_ID_SECURE_CHANNEL as u16);
+        match proto_opcode {
+            OpCode::MRPStandAloneAck => self.mrpstandaloneack_handler(ctx),
+            OpCode::PBKDFParamRequest => self.pbkdfparamreq_handler(ctx),
+            OpCode::PASEPake1 => self.pasepake1_handler(ctx),
+            OpCode::PASEPake3 => self.pasepake3_handler(ctx),
+            OpCode::CASESigma1 => self.casesigma1_handler(ctx),
+            OpCode::CASESigma3 => self.casesigma3_handler(ctx),
+            _ => {
+                error!("OpCode Not Handled: {:?}", proto_opcode);
+                Err(Error::InvalidOpcode)
+            }
+        }
+    }
+
+    fn get_proto_id(&self) -> usize {
+        PROTO_ID_SECURE_CHANNEL as usize
+    }
+}

matter/src/secure_channel/crypto.rs

@@ -0,0 +1,52 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+
+// This trait allows us to switch between crypto providers like OpenSSL and mbedTLS for Spake2
+// Currently this is only validate for a verifier(responder)
+
+// A verifier will typically do:
+// Step 1: w0 and L
+//      set_w0_from_w0s
+//      set_L
+// Step 2: get_pB
+// Step 3: get_TT_as_verifier(pA)
+// Step 4: Computation of cA and cB happens outside since it doesn't use either BigNum or EcPoint
+pub trait CryptoSpake2 {
+    fn new() -> Result<Self, Error>
+    where
+        Self: Sized;
+
+    fn set_w0_from_w0s(&mut self, w0s: &[u8]) -> Result<(), Error>;
+    fn set_w1_from_w1s(&mut self, w1s: &[u8]) -> Result<(), Error>;
+    fn set_w0(&mut self, w0: &[u8]) -> Result<(), Error>;
+    fn set_w1(&mut self, w1: &[u8]) -> Result<(), Error>;
+
+    #[allow(non_snake_case)]
+    fn set_L(&mut self, w1s: &[u8]) -> Result<(), Error>;
+    #[allow(non_snake_case)]
+    fn get_pB(&mut self, pB: &mut [u8]) -> Result<(), Error>;
+    #[allow(non_snake_case)]
+    fn get_TT_as_verifier(
+        &mut self,
+        context: &[u8],
+        pA: &[u8],
+        pB: &[u8],
+        out: &mut [u8],
+    ) -> Result<(), Error>;
+}

matter/src/secure_channel/crypto_esp_mbedtls.rs

@@ -0,0 +1,225 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+
+use super::crypto::CryptoSpake2;
+
+const MATTER_M_BIN: [u8; 65] = [
+    0x04, 0x88, 0x6e, 0x2f, 0x97, 0xac, 0xe4, 0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24, 0x25, 0x79, 0xf2,
+    0x99, 0x3b, 0x64, 0xe1, 0x6e, 0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4, 0x97, 0x33, 0x3d, 0x8f, 0xa1,
+    0x2f, 0x5f, 0xf3, 0x55, 0x16, 0x3e, 0x43, 0xce, 0x22, 0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02, 0xac,
+    0x8e, 0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7, 0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55, 0xa1, 0x2e, 0x2d,
+    0x20,
+];
+const MATTER_N_BIN: [u8; 65] = [
+    0x04, 0xd8, 0xbb, 0xd6, 0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0, 0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77,
+    0x07, 0x19, 0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49, 0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1, 0x29, 0x2b,
+    0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf, 0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36, 0x33, 0x7f, 0x51, 0x68,
+    0xc6, 0x4d, 0x9b, 0xd3, 0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64, 0x49, 0x0b, 0x1e, 0x65, 0x6e, 0xdb,
+    0xe7,
+];
+
+#[allow(non_snake_case)]
+
+pub struct CryptoEspMbedTls {}
+
+impl CryptoSpake2 for CryptoEspMbedTls {
+    #[allow(non_snake_case)]
+    fn new() -> Result<Self, Error> {
+        Ok(CryptoEspMbedTls {})
+    }
+
+    // Computes w0 from w0s respectively
+    fn set_w0_from_w0s(&mut self, w0s: &[u8]) -> Result<(), Error> {
+        // From the Matter Spec,
+        //         w0 = w0s mod p
+        //   where p is the order of the curve
+
+        Ok(())
+    }
+
+    fn set_w1_from_w1s(&mut self, w1s: &[u8]) -> Result<(), Error> {
+        // From the Matter Spec,
+        //         w1 = w1s mod p
+        //   where p is the order of the curve
+
+        Ok(())
+    }
+
+    fn set_w0(&mut self, w0: &[u8]) -> Result<(), Error> {
+        Ok(())
+    }
+
+    fn set_w1(&mut self, w1: &[u8]) -> Result<(), Error> {
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn set_L(&mut self, w1s: &[u8]) -> Result<(), Error> {
+        // From the Matter spec,
+        //        L = w1 * P
+        //    where P is the generator of the underlying elliptic curve
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    fn get_pB(&mut self, pB: &mut [u8]) -> Result<(), Error> {
+        // From the SPAKE2+ spec (https://datatracker.ietf.org/doc/draft-bar-cfrg-spake2plus/)
+        //   for y
+        //   - select random y between 0 to p
+        //   - Y = y*P + w0*N
+        //   - pB = Y
+
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    fn get_TT_as_verifier(
+        &mut self,
+        context: &[u8],
+        pA: &[u8],
+        pB: &[u8],
+        out: &mut [u8],
+    ) -> Result<(), Error> {
+        Ok(())
+    }
+}
+
+impl CryptoEspMbedTls {}
+
+#[cfg(test)]
+mod tests {
+
+    use super::CryptoEspMbedTls;
+    use crate::secure_channel::crypto::CryptoSpake2;
+    use crate::secure_channel::spake2p_test_vectors::test_vectors::*;
+    use openssl::bn::BigNum;
+    use openssl::ec::{EcPoint, PointConversionForm};
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_X() {
+        for t in RFC_T {
+            let mut c = CryptoEspMbedTls::new().unwrap();
+            let x = BigNum::from_slice(&t.x).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let P = c.group.generator();
+
+            let r =
+                CryptoEspMbedTls::do_add_mul(P, &x, &c.M, &c.w0, &c.group, &mut c.bn_ctx).unwrap();
+            assert_eq!(
+                t.X,
+                r.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_Y() {
+        for t in RFC_T {
+            let mut c = CryptoEspMbedTls::new().unwrap();
+            let y = BigNum::from_slice(&t.y).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let P = c.group.generator();
+            let r =
+                CryptoEspMbedTls::do_add_mul(P, &y, &c.N, &c.w0, &c.group, &mut c.bn_ctx).unwrap();
+            assert_eq!(
+                t.Y,
+                r.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_ZV_as_prover() {
+        for t in RFC_T {
+            let mut c = CryptoEspMbedTls::new().unwrap();
+            let x = BigNum::from_slice(&t.x).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            c.set_w1(&t.w1).unwrap();
+            let Y = EcPoint::from_bytes(&c.group, &t.Y, &mut c.bn_ctx).unwrap();
+            let (Z, V) = CryptoEspMbedTls::get_ZV_as_prover(
+                &c.w0,
+                &c.w1,
+                &mut c.N,
+                &Y,
+                &x,
+                &c.order,
+                &c.group,
+                &mut c.bn_ctx,
+            )
+            .unwrap();
+
+            assert_eq!(
+                t.Z,
+                Z.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+            assert_eq!(
+                t.V,
+                V.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_ZV_as_verifier() {
+        for t in RFC_T {
+            let mut c = CryptoEspMbedTls::new().unwrap();
+            let y = BigNum::from_slice(&t.y).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let X = EcPoint::from_bytes(&c.group, &t.X, &mut c.bn_ctx).unwrap();
+            let L = EcPoint::from_bytes(&c.group, &t.L, &mut c.bn_ctx).unwrap();
+            let (Z, V) = CryptoEspMbedTls::get_ZV_as_verifier(
+                &c.w0,
+                &L,
+                &mut c.M,
+                &X,
+                &y,
+                &c.order,
+                &c.group,
+                &mut c.bn_ctx,
+            )
+            .unwrap();
+
+            assert_eq!(
+                t.Z,
+                Z.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+            assert_eq!(
+                t.V,
+                V.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+}

matter/src/secure_channel/crypto_mbedtls.rs

@@ -0,0 +1,382 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::{
+    ops::{Mul, Sub},
+    sync::Arc,
+};
+
+use crate::error::Error;
+
+use super::crypto::CryptoSpake2;
+use byteorder::{ByteOrder, LittleEndian};
+use log::error;
+use mbedtls::{
+    bignum::Mpi,
+    ecp::EcPoint,
+    hash::Md,
+    pk::{EcGroup, EcGroupId, Pk},
+    rng::{CtrDrbg, OsEntropy},
+};
+
+const MATTER_M_BIN: [u8; 65] = [
+    0x04, 0x88, 0x6e, 0x2f, 0x97, 0xac, 0xe4, 0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24, 0x25, 0x79, 0xf2,
+    0x99, 0x3b, 0x64, 0xe1, 0x6e, 0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4, 0x97, 0x33, 0x3d, 0x8f, 0xa1,
+    0x2f, 0x5f, 0xf3, 0x55, 0x16, 0x3e, 0x43, 0xce, 0x22, 0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02, 0xac,
+    0x8e, 0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7, 0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55, 0xa1, 0x2e, 0x2d,
+    0x20,
+];
+const MATTER_N_BIN: [u8; 65] = [
+    0x04, 0xd8, 0xbb, 0xd6, 0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0, 0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77,
+    0x07, 0x19, 0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49, 0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1, 0x29, 0x2b,
+    0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf, 0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36, 0x33, 0x7f, 0x51, 0x68,
+    0xc6, 0x4d, 0x9b, 0xd3, 0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64, 0x49, 0x0b, 0x1e, 0x65, 0x6e, 0xdb,
+    0xe7,
+];
+
+#[allow(non_snake_case)]
+
+pub struct CryptoMbedTLS {
+    group: EcGroup,
+    order: Mpi,
+    xy: Mpi,
+    w0: Mpi,
+    w1: Mpi,
+    M: EcPoint,
+    N: EcPoint,
+    L: EcPoint,
+    pB: EcPoint,
+}
+
+impl CryptoSpake2 for CryptoMbedTLS {
+    #[allow(non_snake_case)]
+    fn new() -> Result<Self, Error> {
+        let group = EcGroup::new(mbedtls::pk::EcGroupId::SecP256R1)?;
+        let order = group.order()?;
+        let M = EcPoint::from_binary(&group, &MATTER_M_BIN)?;
+        let N = EcPoint::from_binary(&group, &MATTER_N_BIN)?;
+
+        Ok(CryptoMbedTLS {
+            group,
+            order,
+            xy: Mpi::new(0)?,
+            M,
+            N,
+            w0: Mpi::new(0)?,
+            w1: Mpi::new(0)?,
+            L: EcPoint::new()?,
+            pB: EcPoint::new()?,
+        })
+    }
+
+    // Computes w0 from w0s respectively
+    fn set_w0_from_w0s(&mut self, w0s: &[u8]) -> Result<(), Error> {
+        // From the Matter Spec,
+        //         w0 = w0s mod p
+        //   where p is the order of the curve
+
+        self.w0 = Mpi::from_binary(w0s)?;
+        self.w0 = self.w0.modulo(&self.order)?;
+        Ok(())
+    }
+
+    fn set_w1_from_w1s(&mut self, w1s: &[u8]) -> Result<(), Error> {
+        // From the Matter Spec,
+        //         w1 = w1s mod p
+        //   where p is the order of the curve
+
+        self.w1 = Mpi::from_binary(w1s)?;
+        self.w1 = self.w1.modulo(&self.order)?;
+        Ok(())
+    }
+
+    fn set_w0(&mut self, w0: &[u8]) -> Result<(), Error> {
+        self.w0 = Mpi::from_binary(w0)?;
+        Ok(())
+    }
+
+    fn set_w1(&mut self, w1: &[u8]) -> Result<(), Error> {
+        self.w1 = Mpi::from_binary(w1)?;
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn set_L(&mut self, w1s: &[u8]) -> Result<(), Error> {
+        // From the Matter spec,
+        //        L = w1 * P
+        //    where P is the generator of the underlying elliptic curve
+        self.set_w1_from_w1s(w1s)?;
+        // TODO: rust-mbedtls doesn't yet accept the DRBG parameter
+        self.L = self.group.generator()?.mul(&mut self.group, &self.w1)?;
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    fn get_pB(&mut self, pB: &mut [u8]) -> Result<(), Error> {
+        // From the SPAKE2+ spec (https://datatracker.ietf.org/doc/draft-bar-cfrg-spake2plus/)
+        //   for y
+        //   - select random y between 0 to p
+        //   - Y = y*P + w0*N
+        //   - pB = Y
+
+        // A private key on this curve is a random number between 0 to p
+        let mut ctr_drbg = CtrDrbg::new(Arc::new(OsEntropy::new()), None)?;
+        self.xy = Pk::generate_ec(&mut ctr_drbg, EcGroupId::SecP256R1)?.ec_private()?;
+
+        let P = self.group.generator()?;
+        self.pB = EcPoint::muladd(&mut self.group, &P, &self.xy, &self.N, &self.w0)?;
+
+        let pB_internal = self.pB.to_binary(&self.group, false)?;
+        let pB_internal = pB_internal.as_slice();
+        if pB_internal.len() != pB.len() {
+            error!("pB length mismatch");
+            return Err(Error::Invalid);
+        }
+        pB.copy_from_slice(pB_internal);
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    fn get_TT_as_verifier(
+        &mut self,
+        context: &[u8],
+        pA: &[u8],
+        pB: &[u8],
+        out: &mut [u8],
+    ) -> Result<(), Error> {
+        let mut TT = Md::new(mbedtls::hash::Type::Sha256)?;
+        // context
+        CryptoMbedTLS::add_to_tt(&mut TT, context)?;
+        // 2 empty identifiers
+        CryptoMbedTLS::add_to_tt(&mut TT, &[])?;
+        CryptoMbedTLS::add_to_tt(&mut TT, &[])?;
+        // M
+        CryptoMbedTLS::add_to_tt(&mut TT, &MATTER_M_BIN)?;
+        // N
+        CryptoMbedTLS::add_to_tt(&mut TT, &MATTER_N_BIN)?;
+        // X = pA
+        CryptoMbedTLS::add_to_tt(&mut TT, pA)?;
+        // Y = pB
+        CryptoMbedTLS::add_to_tt(&mut TT, pB)?;
+
+        let X = EcPoint::from_binary(&self.group, pA)?;
+        let (Z, V) = CryptoMbedTLS::get_ZV_as_verifier(
+            &self.w0,
+            &self.L,
+            &mut self.M,
+            &X,
+            &self.xy,
+            &self.order,
+            &mut self.group,
+        )?;
+
+        // Z
+        let tmp = Z.to_binary(&self.group, false)?;
+        let tmp = tmp.as_slice();
+        CryptoMbedTLS::add_to_tt(&mut TT, tmp)?;
+
+        // V
+        let tmp = V.to_binary(&self.group, false)?;
+        let tmp = tmp.as_slice();
+        CryptoMbedTLS::add_to_tt(&mut TT, tmp)?;
+
+        // w0
+        let tmp = self.w0.to_binary()?;
+        let tmp = tmp.as_slice();
+        CryptoMbedTLS::add_to_tt(&mut TT, tmp)?;
+
+        TT.finish(out)?;
+        Ok(())
+    }
+}
+
+impl CryptoMbedTLS {
+    fn add_to_tt(tt: &mut Md, buf: &[u8]) -> Result<(), Error> {
+        let mut len_buf: [u8; 8] = [0; 8];
+        LittleEndian::write_u64(&mut len_buf, buf.len() as u64);
+        tt.update(&len_buf)?;
+        if !buf.is_empty() {
+            tt.update(buf)?;
+        }
+        Ok(())
+    }
+
+    #[inline(always)]
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn get_ZV_as_prover(
+        w0: &Mpi,
+        w1: &Mpi,
+        N: &mut EcPoint,
+        Y: &EcPoint,
+        x: &Mpi,
+        order: &Mpi,
+        group: &mut EcGroup,
+    ) -> Result<(EcPoint, EcPoint), Error> {
+        // As per the RFC, the operation here is:
+        //   Z = h*x*(Y - w0*N)
+        //   V = h*w1*(Y - w0*N)
+
+        // We will follow the same sequence as in C++ SDK, under the assumption
+        // that the same sequence works for all embedded platforms. So the step
+        // of operations is:
+        //    tmp = x*w0
+        //    Z = x*Y + tmp*N (N is inverted to get the 'negative' effect)
+        //    Z = h*Z (cofactor Mul)
+
+        let mut tmp = x.mul(w0)?;
+        tmp = tmp.modulo(order)?;
+
+        let inverted_N = CryptoMbedTLS::invert(group, N)?;
+        let Z = EcPoint::muladd(group, Y, x, &inverted_N, &tmp)?;
+        // Cofactor for P256 is 1, so that is a No-Op
+
+        let mut tmp = w0.mul(w1)?;
+        tmp = tmp.modulo(order)?;
+        let V = EcPoint::muladd(group, Y, w1, &inverted_N, &tmp)?;
+        Ok((Z, V))
+    }
+
+    #[inline(always)]
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn get_ZV_as_verifier(
+        w0: &Mpi,
+        L: &EcPoint,
+        M: &mut EcPoint,
+        X: &EcPoint,
+        y: &Mpi,
+        order: &Mpi,
+        group: &mut EcGroup,
+    ) -> Result<(EcPoint, EcPoint), Error> {
+        // As per the RFC, the operation here is:
+        //   Z = h*y*(X - w0*M)
+        //   V = h*y*L
+
+        // We will follow the same sequence as in C++ SDK, under the assumption
+        // that the same sequence works for all embedded platforms. So the step
+        // of operations is:
+        //    tmp = y*w0
+        //    Z = y*X + tmp*M (M is inverted to get the 'negative' effect)
+        //    Z = h*Z (cofactor Mul)
+
+        let mut tmp = y.mul(w0)?;
+        tmp = tmp.modulo(order)?;
+
+        let inverted_M = CryptoMbedTLS::invert(group, M)?;
+        let Z = EcPoint::muladd(group, X, y, &inverted_M, &tmp)?;
+        // Cofactor for P256 is 1, so that is a No-Op
+
+        let V = L.mul(group, y)?;
+        Ok((Z, V))
+    }
+
+    fn invert(group: &mut EcGroup, num: &EcPoint) -> Result<EcPoint, mbedtls::Error> {
+        let p = group.p()?;
+        let num_y = num.y()?;
+        let inverted_num_y = p.sub(&num_y)?;
+        EcPoint::from_components(num.x()?, inverted_num_y)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use super::CryptoMbedTLS;
+    use crate::secure_channel::crypto::CryptoSpake2;
+    use crate::secure_channel::spake2p_test_vectors::test_vectors::*;
+    use mbedtls::bignum::Mpi;
+    use mbedtls::ecp::EcPoint;
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_X() {
+        for t in RFC_T {
+            let mut c = CryptoMbedTLS::new().unwrap();
+            let x = Mpi::from_binary(&t.x).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let P = c.group.generator().unwrap();
+
+            let r = EcPoint::muladd(&mut c.group, &P, &x, &c.M, &c.w0).unwrap();
+            assert_eq!(t.X, r.to_binary(&c.group, false).unwrap().as_slice());
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_Y() {
+        for t in RFC_T {
+            let mut c = CryptoMbedTLS::new().unwrap();
+            let y = Mpi::from_binary(&t.y).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let P = c.group.generator().unwrap();
+            let r = EcPoint::muladd(&mut c.group, &P, &y, &c.N, &c.w0).unwrap();
+            assert_eq!(t.Y, r.to_binary(&c.group, false).unwrap().as_slice());
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_ZV_as_prover() {
+        for t in RFC_T {
+            let mut c = CryptoMbedTLS::new().unwrap();
+            let x = Mpi::from_binary(&t.x).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            c.set_w1(&t.w1).unwrap();
+            let Y = EcPoint::from_binary(&c.group, &t.Y).unwrap();
+            let (Z, V) = CryptoMbedTLS::get_ZV_as_prover(
+                &c.w0,
+                &c.w1,
+                &mut c.N,
+                &Y,
+                &x,
+                &c.order,
+                &mut c.group,
+            )
+            .unwrap();
+
+            assert_eq!(t.Z, Z.to_binary(&c.group, false).unwrap().as_slice());
+            assert_eq!(t.V, V.to_binary(&c.group, false).unwrap().as_slice());
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_ZV_as_verifier() {
+        for t in RFC_T {
+            let mut c = CryptoMbedTLS::new().unwrap();
+            let y = Mpi::from_binary(&t.y).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let X = EcPoint::from_binary(&c.group, &t.X).unwrap();
+            let L = EcPoint::from_binary(&c.group, &t.L).unwrap();
+            let (Z, V) = CryptoMbedTLS::get_ZV_as_verifier(
+                &c.w0,
+                &L,
+                &mut c.M,
+                &X,
+                &y,
+                &c.order,
+                &mut c.group,
+            )
+            .unwrap();
+
+            assert_eq!(t.Z, Z.to_binary(&c.group, false).unwrap().as_slice());
+            assert_eq!(t.V, V.to_binary(&c.group, false).unwrap().as_slice());
+        }
+    }
+}

matter/src/secure_channel/crypto_openssl.rs

@@ -0,0 +1,445 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+
+use super::crypto::CryptoSpake2;
+use byteorder::{ByteOrder, LittleEndian};
+use log::error;
+use openssl::{
+    bn::{BigNum, BigNumContext},
+    ec::{EcGroup, EcPoint, EcPointRef, PointConversionForm},
+    hash::{Hasher, MessageDigest},
+    nid::Nid,
+};
+
+const MATTER_M_BIN: [u8; 65] = [
+    0x04, 0x88, 0x6e, 0x2f, 0x97, 0xac, 0xe4, 0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24, 0x25, 0x79, 0xf2,
+    0x99, 0x3b, 0x64, 0xe1, 0x6e, 0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4, 0x97, 0x33, 0x3d, 0x8f, 0xa1,
+    0x2f, 0x5f, 0xf3, 0x55, 0x16, 0x3e, 0x43, 0xce, 0x22, 0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02, 0xac,
+    0x8e, 0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7, 0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55, 0xa1, 0x2e, 0x2d,
+    0x20,
+];
+const MATTER_N_BIN: [u8; 65] = [
+    0x04, 0xd8, 0xbb, 0xd6, 0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0, 0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77,
+    0x07, 0x19, 0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49, 0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1, 0x29, 0x2b,
+    0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf, 0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36, 0x33, 0x7f, 0x51, 0x68,
+    0xc6, 0x4d, 0x9b, 0xd3, 0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64, 0x49, 0x0b, 0x1e, 0x65, 0x6e, 0xdb,
+    0xe7,
+];
+
+#[allow(non_snake_case)]
+
+pub struct CryptoOpenSSL {
+    group: EcGroup,
+    bn_ctx: BigNumContext,
+    // Stores the randomly generated x or y depending upon who we are
+    xy: BigNum,
+    w0: BigNum,
+    w1: BigNum,
+    M: EcPoint,
+    N: EcPoint,
+    L: EcPoint,
+    pB: EcPoint,
+    order: BigNum,
+}
+
+impl CryptoSpake2 for CryptoOpenSSL {
+    #[allow(non_snake_case)]
+    fn new() -> Result<Self, Error> {
+        let group = EcGroup::from_curve_name(Nid::X9_62_PRIME256V1)?;
+        let mut bn_ctx = BigNumContext::new()?;
+        let M = EcPoint::from_bytes(&group, &MATTER_M_BIN, &mut bn_ctx)?;
+        let N = EcPoint::from_bytes(&group, &MATTER_N_BIN, &mut bn_ctx)?;
+        let L = EcPoint::new(&group)?;
+        let pB = EcPoint::from_bytes(&group, &MATTER_N_BIN, &mut bn_ctx)?;
+        let mut order = BigNum::new()?;
+        group.as_ref().order(&mut order, &mut bn_ctx)?;
+
+        Ok(CryptoOpenSSL {
+            group,
+            bn_ctx,
+            xy: BigNum::new()?,
+            w0: BigNum::new()?,
+            w1: BigNum::new()?,
+            order,
+            M,
+            N,
+            pB,
+            L,
+        })
+    }
+
+    // Computes w0 from w0s respectively
+    fn set_w0_from_w0s(&mut self, w0s: &[u8]) -> Result<(), Error> {
+        // From the Matter Spec,
+        //         w0 = w0s mod p
+        //   where p is the order of the curve
+
+        let w0s = BigNum::from_slice(w0s)?;
+        self.w0.checked_rem(&w0s, &self.order, &mut self.bn_ctx)?;
+
+        Ok(())
+    }
+
+    fn set_w1_from_w1s(&mut self, w1s: &[u8]) -> Result<(), Error> {
+        // From the Matter Spec,
+        //         w1 = w1s mod p
+        //   where p is the order of the curve
+
+        let w1s = BigNum::from_slice(w1s)?;
+        self.w1.checked_rem(&w1s, &self.order, &mut self.bn_ctx)?;
+
+        Ok(())
+    }
+
+    fn set_w0(&mut self, w0: &[u8]) -> Result<(), Error> {
+        self.w0 = BigNum::from_slice(w0)?;
+        Ok(())
+    }
+
+    fn set_w1(&mut self, w1: &[u8]) -> Result<(), Error> {
+        self.w1 = BigNum::from_slice(w1)?;
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn set_L(&mut self, w1s: &[u8]) -> Result<(), Error> {
+        // From the Matter spec,
+        //        L = w1 * P
+        //    where P is the generator of the underlying elliptic curve
+        self.set_w1_from_w1s(w1s)?;
+        self.L = EcPoint::new(&self.group)?;
+        self.L.mul_generator(&self.group, &self.w1, &self.bn_ctx)?;
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    fn get_pB(&mut self, pB: &mut [u8]) -> Result<(), Error> {
+        // From the SPAKE2+ spec (https://datatracker.ietf.org/doc/draft-bar-cfrg-spake2plus/)
+        //   for y
+        //   - select random y between 0 to p
+        //   - Y = y*P + w0*N
+        //   - pB = Y
+        self.order.rand_range(&mut self.xy)?;
+        let P = self.group.generator();
+        self.pB = CryptoOpenSSL::do_add_mul(
+            P,
+            &self.xy,
+            &self.N,
+            &self.w0,
+            &self.group,
+            &mut self.bn_ctx,
+        )?;
+        let pB_internal = self.pB.to_bytes(
+            &self.group,
+            PointConversionForm::UNCOMPRESSED,
+            &mut self.bn_ctx,
+        )?;
+        let pB_internal = pB_internal.as_slice();
+        if pB_internal.len() != pB.len() {
+            error!("pB length mismatch");
+            return Err(Error::Invalid);
+        }
+        pB.copy_from_slice(pB_internal);
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    fn get_TT_as_verifier(
+        &mut self,
+        context: &[u8],
+        pA: &[u8],
+        pB: &[u8],
+        TT_hash: &mut [u8],
+    ) -> Result<(), Error> {
+        let mut TT = Hasher::new(MessageDigest::sha256())?;
+        // context
+        CryptoOpenSSL::add_to_tt(&mut TT, context)?;
+        // 2 empty identifiers
+        CryptoOpenSSL::add_to_tt(&mut TT, &[])?;
+        CryptoOpenSSL::add_to_tt(&mut TT, &[])?;
+        // M
+        CryptoOpenSSL::add_to_tt(&mut TT, &MATTER_M_BIN)?;
+        // N
+        CryptoOpenSSL::add_to_tt(&mut TT, &MATTER_N_BIN)?;
+        // X = pA
+        CryptoOpenSSL::add_to_tt(&mut TT, pA)?;
+        // Y = pB
+        CryptoOpenSSL::add_to_tt(&mut TT, pB)?;
+
+        let X = EcPoint::from_bytes(&self.group, pA, &mut self.bn_ctx)?;
+        let (Z, V) = CryptoOpenSSL::get_ZV_as_verifier(
+            &self.w0,
+            &self.L,
+            &mut self.M,
+            &X,
+            &self.xy,
+            &self.order,
+            &self.group,
+            &mut self.bn_ctx,
+        )?;
+
+        // Z
+        let tmp = Z.to_bytes(
+            &self.group,
+            PointConversionForm::UNCOMPRESSED,
+            &mut self.bn_ctx,
+        )?;
+        let tmp = tmp.as_slice();
+        CryptoOpenSSL::add_to_tt(&mut TT, tmp)?;
+
+        // V
+        let tmp = V.to_bytes(
+            &self.group,
+            PointConversionForm::UNCOMPRESSED,
+            &mut self.bn_ctx,
+        )?;
+        let tmp = tmp.as_slice();
+        CryptoOpenSSL::add_to_tt(&mut TT, tmp)?;
+
+        // w0
+        let tmp = self.w0.to_vec();
+        let tmp = tmp.as_slice();
+        CryptoOpenSSL::add_to_tt(&mut TT, tmp)?;
+
+        let h = TT.finish()?;
+        TT_hash.copy_from_slice(h.as_ref());
+        Ok(())
+    }
+}
+
+impl CryptoOpenSSL {
+    fn add_to_tt(tt: &mut Hasher, buf: &[u8]) -> Result<(), Error> {
+        let mut len_buf: [u8; 8] = [0; 8];
+        LittleEndian::write_u64(&mut len_buf, buf.len() as u64);
+        tt.update(&len_buf)?;
+        if buf.len() > 0 {
+            tt.update(buf)?;
+        }
+        Ok(())
+    }
+
+    // Do a*b + c*d
+    #[inline(always)]
+    fn do_add_mul(
+        a: &EcPointRef,
+        b: &BigNum,
+        c: &EcPoint,
+        d: &BigNum,
+        group: &EcGroup,
+        bn_ctx: &mut BigNumContext,
+    ) -> Result<EcPoint, Error> {
+        let mut mul1 = EcPoint::new(group)?;
+        let mut mul2 = EcPoint::new(group)?;
+        mul1.mul(group, a, b, bn_ctx)?;
+        mul2.mul(group, c, d, bn_ctx)?;
+        let mut result = EcPoint::new(group)?;
+        result.add(group, &mul1, &mul2, bn_ctx)?;
+        Ok(result)
+    }
+
+    #[inline(always)]
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn get_ZV_as_prover(
+        w0: &BigNum,
+        w1: &BigNum,
+        N: &mut EcPoint,
+        Y: &EcPoint,
+        x: &BigNum,
+        order: &BigNum,
+        group: &EcGroup,
+        bn_ctx: &mut BigNumContext,
+    ) -> Result<(EcPoint, EcPoint), Error> {
+        // As per the RFC, the operation here is:
+        //   Z = h*x*(Y - w0*N)
+        //   V = h*w1*(Y - w0*N)
+
+        // We will follow the same sequence as in C++ SDK, under the assumption
+        // that the same sequence works for all embedded platforms. So the step
+        // of operations is:
+        //    tmp = x*w0
+        //    Z = x*Y + tmp*N (N is inverted to get the 'negative' effect)
+        //    Z = h*Z (cofactor Mul)
+
+        let mut tmp = BigNum::new()?;
+        tmp.mod_mul(x, w0, order, bn_ctx)?;
+        N.invert(group, bn_ctx)?;
+        let Z = CryptoOpenSSL::do_add_mul(Y, x, N, &tmp, group, bn_ctx)?;
+        // Cofactor for P256 is 1, so that is a No-Op
+
+        tmp.mod_mul(w1, w0, order, bn_ctx)?;
+        let V = CryptoOpenSSL::do_add_mul(Y, w1, N, &tmp, group, bn_ctx)?;
+        Ok((Z, V))
+    }
+
+    #[inline(always)]
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn get_ZV_as_verifier(
+        w0: &BigNum,
+        L: &EcPoint,
+        M: &mut EcPoint,
+        X: &EcPoint,
+        y: &BigNum,
+        order: &BigNum,
+        group: &EcGroup,
+        bn_ctx: &mut BigNumContext,
+    ) -> Result<(EcPoint, EcPoint), Error> {
+        // As per the RFC, the operation here is:
+        //   Z = h*y*(X - w0*M)
+        //   V = h*y*L
+
+        // We will follow the same sequence as in C++ SDK, under the assumption
+        // that the same sequence works for all embedded platforms. So the step
+        // of operations is:
+        //    tmp = y*w0
+        //    Z = y*X + tmp*M (M is inverted to get the 'negative' effect)
+        //    Z = h*Z (cofactor Mul)
+
+        let mut tmp = BigNum::new()?;
+        tmp.mod_mul(y, w0, order, bn_ctx)?;
+        M.invert(group, bn_ctx)?;
+        let Z = CryptoOpenSSL::do_add_mul(X, y, M, &tmp, group, bn_ctx)?;
+        // Cofactor for P256 is 1, so that is a No-Op
+
+        let mut V = EcPoint::new(group)?;
+        V.mul(group, L, y, bn_ctx)?;
+        Ok((Z, V))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use super::CryptoOpenSSL;
+    use crate::secure_channel::crypto::CryptoSpake2;
+    use crate::secure_channel::spake2p_test_vectors::test_vectors::*;
+    use openssl::bn::BigNum;
+    use openssl::ec::{EcPoint, PointConversionForm};
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_X() {
+        for t in RFC_T {
+            let mut c = CryptoOpenSSL::new().unwrap();
+            let x = BigNum::from_slice(&t.x).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let P = c.group.generator();
+
+            let r = CryptoOpenSSL::do_add_mul(P, &x, &c.M, &c.w0, &c.group, &mut c.bn_ctx).unwrap();
+            assert_eq!(
+                t.X,
+                r.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_Y() {
+        for t in RFC_T {
+            let mut c = CryptoOpenSSL::new().unwrap();
+            let y = BigNum::from_slice(&t.y).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let P = c.group.generator();
+            let r = CryptoOpenSSL::do_add_mul(P, &y, &c.N, &c.w0, &c.group, &mut c.bn_ctx).unwrap();
+            assert_eq!(
+                t.Y,
+                r.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_ZV_as_prover() {
+        for t in RFC_T {
+            let mut c = CryptoOpenSSL::new().unwrap();
+            let x = BigNum::from_slice(&t.x).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            c.set_w1(&t.w1).unwrap();
+            let Y = EcPoint::from_bytes(&c.group, &t.Y, &mut c.bn_ctx).unwrap();
+            let (Z, V) = CryptoOpenSSL::get_ZV_as_prover(
+                &c.w0,
+                &c.w1,
+                &mut c.N,
+                &Y,
+                &x,
+                &c.order,
+                &c.group,
+                &mut c.bn_ctx,
+            )
+            .unwrap();
+
+            assert_eq!(
+                t.Z,
+                Z.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+            assert_eq!(
+                t.V,
+                V.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_ZV_as_verifier() {
+        for t in RFC_T {
+            let mut c = CryptoOpenSSL::new().unwrap();
+            let y = BigNum::from_slice(&t.y).unwrap();
+            c.set_w0(&t.w0).unwrap();
+            let X = EcPoint::from_bytes(&c.group, &t.X, &mut c.bn_ctx).unwrap();
+            let L = EcPoint::from_bytes(&c.group, &t.L, &mut c.bn_ctx).unwrap();
+            let (Z, V) = CryptoOpenSSL::get_ZV_as_verifier(
+                &c.w0,
+                &L,
+                &mut c.M,
+                &X,
+                &y,
+                &c.order,
+                &c.group,
+                &mut c.bn_ctx,
+            )
+            .unwrap();
+
+            assert_eq!(
+                t.Z,
+                Z.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+            assert_eq!(
+                t.V,
+                V.to_bytes(&c.group, PointConversionForm::UNCOMPRESSED, &mut c.bn_ctx)
+                    .unwrap()
+                    .as_slice()
+            );
+        }
+    }
+}

matter/src/secure_channel/mod.rs

@@ -0,0 +1,32 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod case;
+pub mod common;
+#[cfg(feature = "crypto_esp_mbedtls")]
+pub mod crypto_esp_mbedtls;
+#[cfg(feature = "crypto_mbedtls")]
+pub mod crypto_mbedtls;
+#[cfg(feature = "crypto_openssl")]
+pub mod crypto_openssl;
+
+pub mod core;
+pub mod crypto;
+pub mod pake;
+pub mod spake2p;
+pub mod spake2p_test_vectors;
+pub mod status_report;

matter/src/secure_channel/pake.rs

@@ -0,0 +1,285 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::time::{Duration, SystemTime};
+
+use super::{
+    common::{create_sc_status_report, SCStatusCodes},
+    spake2p::Spake2P,
+};
+use crate::{
+    crypto,
+    error::Error,
+    sys::SPAKE2_ITERATION_COUNT,
+    tlv::{self, get_root_node_struct, FromTLV, OctetStr, TLVElement, TLVWriter, TagType, ToTLV},
+    transport::{
+        exchange::ExchangeCtx,
+        network::Address,
+        proto_demux::ProtoCtx,
+        queue::{Msg, WorkQ},
+        session::{CloneData, SessionMode},
+    },
+};
+use log::{error, info};
+use rand::prelude::*;
+
+// This file basically deals with the handlers for the PASE secure channel protocol
+// TLV extraction and encoding is done in this file.
+// We create a Spake2p object and set it up in the exchange-data. This object then
+// handles Spake2+ specific stuff.
+
+const PASE_DISCARD_TIMEOUT_SECS: Duration = Duration::from_secs(60);
+
+const SPAKE2_SESSION_KEYS_INFO: [u8; 11] = *b"SessionKeys";
+
+struct SessionData {
+    start_time: SystemTime,
+    exch_id: u16,
+    peer_addr: Address,
+    spake2p: Box<Spake2P>,
+}
+
+impl SessionData {
+    fn is_sess_expired(&self) -> Result<bool, Error> {
+        if SystemTime::now().duration_since(self.start_time)? > PASE_DISCARD_TIMEOUT_SECS {
+            Ok(true)
+        } else {
+            Ok(false)
+        }
+    }
+}
+
+enum PakeState {
+    Idle,
+    InProgress(SessionData),
+}
+
+impl PakeState {
+    fn take(&mut self) -> Result<SessionData, Error> {
+        let new = std::mem::replace(self, PakeState::Idle);
+        if let PakeState::InProgress(s) = new {
+            Ok(s)
+        } else {
+            Err(Error::InvalidSignature)
+        }
+    }
+
+    fn is_idle(&self) -> bool {
+        std::mem::discriminant(self) == std::mem::discriminant(&PakeState::Idle)
+    }
+
+    fn take_sess_data(&mut self, exch_ctx: &ExchangeCtx) -> Result<SessionData, Error> {
+        let sd = self.take()?;
+        if sd.exch_id != exch_ctx.exch.get_id() || sd.peer_addr != exch_ctx.sess.get_peer_addr() {
+            Err(Error::InvalidState)
+        } else {
+            Ok(sd)
+        }
+    }
+
+    fn make_in_progress(&mut self, spake2p: Box<Spake2P>, exch_ctx: &ExchangeCtx) {
+        *self = PakeState::InProgress(SessionData {
+            start_time: SystemTime::now(),
+            spake2p,
+            exch_id: exch_ctx.exch.get_id(),
+            peer_addr: exch_ctx.sess.get_peer_addr(),
+        });
+    }
+
+    fn set_sess_data(&mut self, sd: SessionData) {
+        *self = PakeState::InProgress(sd);
+    }
+}
+
+impl Default for PakeState {
+    fn default() -> Self {
+        Self::Idle
+    }
+}
+
+#[derive(Default)]
+pub struct PAKE {
+    salt: [u8; 16],
+    passwd: u32,
+    state: PakeState,
+}
+
+impl PAKE {
+    pub fn new(salt: &[u8; 16], passwd: u32) -> Self {
+        // TODO: Can any PBKDF2 calculation be pre-computed here
+        PAKE {
+            passwd,
+            salt: *salt,
+            ..Default::default()
+        }
+    }
+
+    #[allow(non_snake_case)]
+    pub fn handle_pasepake3(&mut self, ctx: &mut ProtoCtx) -> Result<(), Error> {
+        let mut sd = self.state.take_sess_data(&ctx.exch_ctx)?;
+
+        let cA = extract_pasepake_1_or_3_params(ctx.rx.as_borrow_slice())?;
+        let (status_code, Ke) = sd.spake2p.handle_cA(cA);
+
+        if status_code == SCStatusCodes::SessionEstablishmentSuccess {
+            // Get the keys
+            let Ke = Ke.ok_or(Error::Invalid)?;
+            let mut session_keys: [u8; 48] = [0; 48];
+            crypto::hkdf_sha256(&[], Ke, &SPAKE2_SESSION_KEYS_INFO, &mut session_keys)
+                .map_err(|_x| Error::NoSpace)?;
+
+            // Create a session
+            let data = sd.spake2p.get_app_data();
+            let peer_sessid: u16 = (data & 0xffff) as u16;
+            let local_sessid: u16 = ((data >> 16) & 0xffff) as u16;
+            let mut clone_data = CloneData::new(
+                0,
+                0,
+                peer_sessid,
+                local_sessid,
+                ctx.exch_ctx.sess.get_peer_addr(),
+                SessionMode::Pase,
+            );
+            clone_data.dec_key.copy_from_slice(&session_keys[0..16]);
+            clone_data.enc_key.copy_from_slice(&session_keys[16..32]);
+            clone_data
+                .att_challenge
+                .copy_from_slice(&session_keys[32..48]);
+
+            // Queue a transport mgr request to add a new session
+            WorkQ::get()?.sync_send(Msg::NewSession(clone_data))?;
+        }
+
+        create_sc_status_report(&mut ctx.tx, status_code, None)?;
+        ctx.exch_ctx.exch.close();
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    pub fn handle_pasepake1(&mut self, ctx: &mut ProtoCtx) -> Result<(), Error> {
+        let mut sd = self.state.take_sess_data(&ctx.exch_ctx)?;
+
+        let pA = extract_pasepake_1_or_3_params(ctx.rx.as_borrow_slice())?;
+        let mut pB: [u8; 65] = [0; 65];
+        let mut cB: [u8; 32] = [0; 32];
+        sd.spake2p
+            .start_verifier(self.passwd, SPAKE2_ITERATION_COUNT, &self.salt)?;
+        sd.spake2p.handle_pA(pA, &mut pB, &mut cB)?;
+
+        let mut tw = TLVWriter::new(ctx.tx.get_writebuf()?);
+        let resp = Pake1Resp {
+            pb: OctetStr(&pB),
+            cb: OctetStr(&cB),
+        };
+        resp.to_tlv(&mut tw, TagType::Anonymous)?;
+
+        self.state.set_sess_data(sd);
+
+        Ok(())
+    }
+
+    pub fn handle_pbkdfparamrequest(&mut self, ctx: &mut ProtoCtx) -> Result<(), Error> {
+        if !self.state.is_idle() {
+            let sd = self.state.take()?;
+            if sd.is_sess_expired()? {
+                info!("Previous session expired, clearing it");
+                self.state = PakeState::Idle;
+            } else {
+                info!("Previous session in-progress, denying new request");
+                // little-endian timeout (here we've hardcoded 500ms)
+                create_sc_status_report(&mut ctx.tx, SCStatusCodes::Busy, Some(&[0xf4, 0x01]))?;
+                return Ok(());
+            }
+        }
+
+        let root = tlv::get_root_node(ctx.rx.as_borrow_slice())?;
+        let a = PBKDFParamReq::from_tlv(&root)?;
+        if a.passcode_id != 0 {
+            error!("Can't yet handle passcode_id != 0");
+            return Err(Error::Invalid);
+        }
+
+        let mut our_random: [u8; 32] = [0; 32];
+        rand::thread_rng().fill_bytes(&mut our_random);
+
+        let local_sessid = ctx.exch_ctx.sess.reserve_new_sess_id();
+        let spake2p_data: u32 = ((local_sessid as u32) << 16) | a.initiator_ssid as u32;
+        let mut spake2p = Box::new(Spake2P::new());
+        spake2p.set_app_data(spake2p_data as u32);
+
+        // Generate response
+        let mut tw = TLVWriter::new(ctx.tx.get_writebuf()?);
+        let mut resp = PBKDFParamResp {
+            init_random: a.initiator_random,
+            our_random: OctetStr(&our_random),
+            local_sessid,
+            params: None,
+        };
+        if !a.has_params {
+            let params_resp = PBKDFParamRespParams {
+                count: SPAKE2_ITERATION_COUNT,
+                salt: OctetStr(&self.salt),
+            };
+            resp.params = Some(params_resp);
+        }
+        resp.to_tlv(&mut tw, TagType::Anonymous)?;
+
+        spake2p.set_context(ctx.rx.as_borrow_slice(), ctx.tx.as_borrow_slice())?;
+        self.state.make_in_progress(spake2p, &ctx.exch_ctx);
+
+        Ok(())
+    }
+}
+
+#[derive(ToTLV)]
+#[tlvargs(start = 1)]
+struct Pake1Resp<'a> {
+    pb: OctetStr<'a>,
+    cb: OctetStr<'a>,
+}
+
+#[derive(ToTLV)]
+#[tlvargs(start = 1)]
+struct PBKDFParamRespParams<'a> {
+    count: u32,
+    salt: OctetStr<'a>,
+}
+
+#[derive(ToTLV)]
+#[tlvargs(start = 1)]
+struct PBKDFParamResp<'a> {
+    init_random: OctetStr<'a>,
+    our_random: OctetStr<'a>,
+    local_sessid: u16,
+    params: Option<PBKDFParamRespParams<'a>>,
+}
+
+#[allow(non_snake_case)]
+fn extract_pasepake_1_or_3_params(buf: &[u8]) -> Result<&[u8], Error> {
+    let root = get_root_node_struct(buf)?;
+    let pA = root.find_tag(1)?.slice()?;
+    Ok(pA)
+}
+
+#[derive(FromTLV)]
+#[tlvargs(lifetime = "'a", start = 1)]
+struct PBKDFParamReq<'a> {
+    initiator_random: OctetStr<'a>,
+    initiator_ssid: u16,
+    passcode_id: u16,
+    has_params: bool,
+}

matter/src/secure_channel/spake2p.rs

@@ -0,0 +1,276 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::crypto::{self, HmacSha256};
+use byteorder::{ByteOrder, LittleEndian};
+use subtle::ConstantTimeEq;
+
+use crate::{
+    crypto::{pbkdf2_hmac, Sha256},
+    error::Error,
+};
+
+#[cfg(feature = "crypto_openssl")]
+use super::crypto_openssl::CryptoOpenSSL;
+
+#[cfg(feature = "crypto_mbedtls")]
+use super::crypto_mbedtls::CryptoMbedTLS;
+
+#[cfg(feature = "crypto_esp_mbedtls")]
+use super::crypto_esp_mbedtls::CryptoEspMbedTls;
+
+use super::{common::SCStatusCodes, crypto::CryptoSpake2};
+
+// This file handle Spake2+ specific instructions. In itself, this file is
+// independent from the BigNum and EC operations that are typically required
+// Spake2+. We use the CryptoSpake2 trait object that allows us to abstract
+// out the specific implementations.
+//
+// In the case of the verifier, we don't actually release the Ke until we
+// validate that the cA is confirmed.
+
+#[derive(PartialEq, Copy, Clone, Debug)]
+pub enum Spake2VerifierState {
+    // Initialised - w0, L are set
+    Init,
+    // Pending Confirmation - Keys are derived but pending confirmation
+    PendingConfirmation,
+    // Confirmed
+    Confirmed,
+}
+
+#[derive(PartialEq, Debug)]
+pub enum Spake2Mode {
+    Unknown,
+    Prover,
+    Verifier(Spake2VerifierState),
+}
+
+#[allow(non_snake_case)]
+pub struct Spake2P {
+    mode: Spake2Mode,
+    context: Option<Sha256>,
+    Ke: [u8; 16],
+    cA: [u8; 32],
+    crypto_spake2: Option<Box<dyn CryptoSpake2>>,
+    app_data: u32,
+}
+
+const SPAKE2P_KEY_CONFIRM_INFO: [u8; 16] = *b"ConfirmationKeys";
+const SPAKE2P_CONTEXT_PREFIX: [u8; 26] = *b"CHIP PAKE V1 Commissioning";
+const CRYPTO_GROUP_SIZE_BYTES: usize = 32;
+const CRYPTO_W_SIZE_BYTES: usize = CRYPTO_GROUP_SIZE_BYTES + 8;
+
+#[cfg(feature = "crypto_openssl")]
+fn crypto_spake2_new() -> Result<Box<dyn CryptoSpake2>, Error> {
+    Ok(Box::new(CryptoOpenSSL::new()?))
+}
+
+#[cfg(feature = "crypto_mbedtls")]
+fn crypto_spake2_new() -> Result<Box<dyn CryptoSpake2>, Error> {
+    Ok(Box::new(CryptoMbedTLS::new()?))
+}
+
+#[cfg(feature = "crypto_esp_mbedtls")]
+fn crypto_spake2_new() -> Result<Box<dyn CryptoSpake2>, Error> {
+    Ok(Box::new(CryptoEspMbedTls::new()?))
+}
+
+impl Default for Spake2P {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl Spake2P {
+    pub fn new() -> Self {
+        Spake2P {
+            mode: Spake2Mode::Unknown,
+            context: None,
+            crypto_spake2: None,
+            Ke: [0; 16],
+            cA: [0; 32],
+            app_data: 0,
+        }
+    }
+
+    pub fn set_app_data(&mut self, data: u32) {
+        self.app_data = data;
+    }
+
+    pub fn get_app_data(&mut self) -> u32 {
+        self.app_data
+    }
+
+    pub fn set_context(&mut self, buf1: &[u8], buf2: &[u8]) -> Result<(), Error> {
+        let mut context = Sha256::new()?;
+        context.update(&SPAKE2P_CONTEXT_PREFIX)?;
+        context.update(buf1)?;
+        context.update(buf2)?;
+        self.context = Some(context);
+        Ok(())
+    }
+
+    #[inline(always)]
+    fn get_w0w1s(pw: u32, iter: u32, salt: &[u8], w0w1s: &mut [u8]) {
+        let mut pw_str: [u8; 4] = [0; 4];
+        LittleEndian::write_u32(&mut pw_str, pw);
+        let _ = pbkdf2_hmac(&pw_str, iter as usize, salt, w0w1s);
+    }
+
+    pub fn start_verifier(&mut self, pw: u32, iter: u32, salt: &[u8]) -> Result<(), Error> {
+        let mut w0w1s: [u8; (2 * CRYPTO_W_SIZE_BYTES)] = [0; (2 * CRYPTO_W_SIZE_BYTES)];
+        Spake2P::get_w0w1s(pw, iter, salt, &mut w0w1s);
+        self.crypto_spake2 = Some(crypto_spake2_new()?);
+
+        let w0s_len = w0w1s.len() / 2;
+        if let Some(crypto_spake2) = &mut self.crypto_spake2 {
+            crypto_spake2.set_w0_from_w0s(&w0w1s[0..w0s_len])?;
+            crypto_spake2.set_L(&w0w1s[w0s_len..])?;
+        }
+
+        self.mode = Spake2Mode::Verifier(Spake2VerifierState::Init);
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    pub fn handle_pA(&mut self, pA: &[u8], pB: &mut [u8], cB: &mut [u8]) -> Result<(), Error> {
+        if self.mode != Spake2Mode::Verifier(Spake2VerifierState::Init) {
+            return Err(Error::InvalidState);
+        }
+
+        if let Some(crypto_spake2) = &mut self.crypto_spake2 {
+            crypto_spake2.get_pB(pB)?;
+            if let Some(context) = self.context.take() {
+                let mut hash = [0u8; crypto::SHA256_HASH_LEN_BYTES];
+                context.finish(&mut hash)?;
+                let mut TT = [0u8; crypto::SHA256_HASH_LEN_BYTES];
+                crypto_spake2.get_TT_as_verifier(&hash, pA, pB, &mut TT)?;
+
+                Spake2P::get_Ke_and_cAcB(&TT, pA, pB, &mut self.Ke, &mut self.cA, cB)?;
+            }
+        }
+        // We are finished with using the crypto_spake2 now
+        self.crypto_spake2 = None;
+        self.mode = Spake2Mode::Verifier(Spake2VerifierState::PendingConfirmation);
+        Ok(())
+    }
+
+    #[allow(non_snake_case)]
+    pub fn handle_cA(&mut self, cA: &[u8]) -> (SCStatusCodes, Option<&[u8]>) {
+        if self.mode != Spake2Mode::Verifier(Spake2VerifierState::PendingConfirmation) {
+            return (SCStatusCodes::SessionNotFound, None);
+        }
+        self.mode = Spake2Mode::Verifier(Spake2VerifierState::Confirmed);
+        if cA.ct_eq(&self.cA).unwrap_u8() == 1 {
+            (SCStatusCodes::SessionEstablishmentSuccess, Some(&self.Ke))
+        } else {
+            (SCStatusCodes::InvalidParameter, None)
+        }
+    }
+
+    #[inline(always)]
+    #[allow(non_snake_case)]
+    #[allow(dead_code)]
+    fn get_Ke_and_cAcB(
+        TT: &[u8],
+        pA: &[u8],
+        pB: &[u8],
+        Ke: &mut [u8],
+        cA: &mut [u8],
+        cB: &mut [u8],
+    ) -> Result<(), Error> {
+        // Step 1: Ka || Ke = Hash(TT)
+        let KaKe = TT;
+        let KaKe_len = KaKe.len();
+        let Ka = &KaKe[0..KaKe_len / 2];
+        let ke_internal = &KaKe[(KaKe_len / 2)..];
+        if ke_internal.len() == Ke.len() {
+            Ke.copy_from_slice(ke_internal);
+        } else {
+            return Err(Error::NoSpace);
+        }
+
+        // Step 2: KcA || KcB = KDF(nil, Ka, "ConfirmationKeys")
+        let mut KcAKcB: [u8; 32] = [0; 32];
+        crypto::hkdf_sha256(&[], Ka, &SPAKE2P_KEY_CONFIRM_INFO, &mut KcAKcB)
+            .map_err(|_x| Error::NoSpace)?;
+
+        let KcA = &KcAKcB[0..(KcAKcB.len() / 2)];
+        let KcB = &KcAKcB[(KcAKcB.len() / 2)..];
+
+        // Step 3: cA = HMAC(KcA, pB), cB = HMAC(KcB, pA)
+        let mut mac = HmacSha256::new(KcA)?;
+        mac.update(pB)?;
+        mac.finish(cA)?;
+
+        let mut mac = HmacSha256::new(KcB)?;
+        mac.update(pA)?;
+        mac.finish(cB)?;
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use super::Spake2P;
+    use crate::{
+        crypto,
+        secure_channel::{spake2p::CRYPTO_W_SIZE_BYTES, spake2p_test_vectors::test_vectors::*},
+    };
+
+    #[test]
+    fn test_pbkdf2() {
+        // These are the vectors from one sample run of chip-tool along with our PBKDFParamResponse
+        let salt = [
+            0x4, 0xa1, 0xd2, 0xc6, 0x11, 0xf0, 0xbd, 0x36, 0x78, 0x67, 0x79, 0x7b, 0xfe, 0x82,
+            0x36, 0x0,
+        ];
+        let mut w0w1s: [u8; (2 * CRYPTO_W_SIZE_BYTES)] = [0; (2 * CRYPTO_W_SIZE_BYTES)];
+        Spake2P::get_w0w1s(123456, 2000, &salt, &mut w0w1s);
+        assert_eq!(
+            w0w1s,
+            [
+                0xc7, 0x89, 0x33, 0x9c, 0xc5, 0xeb, 0xbc, 0xf6, 0xdf, 0x04, 0xa9, 0x11, 0x11, 0x06,
+                0x4c, 0x15, 0xac, 0x5a, 0xea, 0x67, 0x69, 0x9f, 0x32, 0x62, 0xcf, 0xc6, 0xe9, 0x19,
+                0xe8, 0xa4, 0x0b, 0xb3, 0x42, 0xe8, 0xc6, 0x8e, 0xa9, 0x9a, 0x73, 0xe2, 0x59, 0xd1,
+                0x17, 0xd8, 0xed, 0xcb, 0x72, 0x8c, 0xbf, 0x3b, 0xa9, 0x88, 0x02, 0xd8, 0x45, 0x4b,
+                0xd0, 0x2d, 0xe5, 0xe4, 0x1c, 0xc3, 0xd7, 0x00, 0x03, 0x3c, 0x86, 0x20, 0x9a, 0x42,
+                0x5f, 0x55, 0x96, 0x3b, 0x9f, 0x6f, 0x79, 0xef, 0xcb, 0x37
+            ]
+        )
+    }
+
+    #[test]
+    #[allow(non_snake_case)]
+    fn test_get_Ke_and_cAcB() {
+        for t in RFC_T {
+            let mut Ke: [u8; 16] = [0; 16];
+            let mut cA: [u8; 32] = [0; 32];
+            let mut cB: [u8; 32] = [0; 32];
+            let mut TT_hash = [0u8; crypto::SHA256_HASH_LEN_BYTES];
+            let mut h = crypto::Sha256::new().unwrap();
+            h.update(&t.TT[0..t.TT_len]).unwrap();
+            h.finish(&mut TT_hash).unwrap();
+            Spake2P::get_Ke_and_cAcB(&TT_hash, &t.X, &t.Y, &mut Ke, &mut cA, &mut cB).unwrap();
+            assert_eq!(Ke, t.Ke);
+            assert_eq!(cA, t.cA);
+            assert_eq!(cB, t.cB);
+        }
+    }
+}

matter/src/secure_channel/spake2p_test_vectors.rs

@@ -0,0 +1,497 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+#[cfg(test)]
+pub mod test_vectors {
+    // Based on vectors used in the RFC
+    #[allow(non_snake_case)]
+    pub struct RFCTestVector {
+        pub w0: [u8; 32],
+        pub w1: [u8; 32],
+        pub x: [u8; 32],
+        pub X: [u8; 65],
+        pub y: [u8; 32],
+        pub Y: [u8; 65],
+        pub Z: [u8; 65],
+        pub V: [u8; 65],
+        pub L: [u8; 65],
+        pub cA: [u8; 32],
+        pub cB: [u8; 32],
+        pub Ke: [u8; 16],
+        pub TT: [u8; 547],
+        // The TT size changes, as they change the identifiers, address it through this
+        pub TT_len: usize,
+    }
+    pub const RFC_T: [RFCTestVector; 4] = [
+        RFCTestVector {
+            w0: [
+                0xe6, 0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6, 0x9b, 0xf4, 0x79, 0x28, 0xa8,
+                0x45, 0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63, 0xf7, 0xff, 0xaf, 0x43, 0x90,
+                0xe6, 0x7d, 0x79, 0x8c,
+            ],
+            w1: [
+                0x24, 0xb5, 0xae, 0x4a, 0xbd, 0xa8, 0x68, 0xec, 0x93, 0x36, 0xff, 0xc3, 0xb7, 0x8e,
+                0xe3, 0x1c, 0x57, 0x55, 0xbe, 0xf1, 0x75, 0x92, 0x27, 0xef, 0x53, 0x72, 0xca, 0x13,
+                0x9b, 0x94, 0xe5, 0x12,
+            ],
+            x: [
+                0x8b, 0x0f, 0x3f, 0x38, 0x39, 0x05, 0xcf, 0x3a, 0x3b, 0xb9, 0x55, 0xef, 0x8f, 0xb6,
+                0x2e, 0x24, 0x84, 0x9d, 0xd3, 0x49, 0xa0, 0x5c, 0xa7, 0x9a, 0xaf, 0xb1, 0x80, 0x41,
+                0xd3, 0x0c, 0xbd, 0xb6,
+            ],
+            X: [
+                0x04, 0xaf, 0x09, 0x98, 0x7a, 0x59, 0x3d, 0x3b, 0xac, 0x86, 0x94, 0xb1, 0x23, 0x83,
+                0x94, 0x22, 0xc3, 0xcc, 0x87, 0xe3, 0x7d, 0x6b, 0x41, 0xc1, 0xd6, 0x30, 0xf0, 0x00,
+                0xdd, 0x64, 0x98, 0x0e, 0x53, 0x7a, 0xe7, 0x04, 0xbc, 0xed, 0xe0, 0x4e, 0xa3, 0xbe,
+                0xc9, 0xb7, 0x47, 0x5b, 0x32, 0xfa, 0x2c, 0xa3, 0xb6, 0x84, 0xbe, 0x14, 0xd1, 0x16,
+                0x45, 0xe3, 0x8e, 0xa6, 0x60, 0x9e, 0xb3, 0x9e, 0x7e,
+            ],
+            y: [
+                0x2e, 0x08, 0x95, 0xb0, 0xe7, 0x63, 0xd6, 0xd5, 0xa9, 0x56, 0x44, 0x33, 0xe6, 0x4a,
+                0xc3, 0xca, 0xc7, 0x4f, 0xf8, 0x97, 0xf6, 0xc3, 0x44, 0x52, 0x47, 0xba, 0x1b, 0xab,
+                0x40, 0x08, 0x2a, 0x91,
+            ],
+            Y: [
+                0x04, 0x41, 0x75, 0x92, 0x62, 0x0a, 0xeb, 0xf9, 0xfd, 0x20, 0x36, 0x16, 0xbb, 0xb9,
+                0xf1, 0x21, 0xb7, 0x30, 0xc2, 0x58, 0xb2, 0x86, 0xf8, 0x90, 0xc5, 0xf1, 0x9f, 0xea,
+                0x83, 0x3a, 0x9c, 0x90, 0x0c, 0xbe, 0x90, 0x57, 0xbc, 0x54, 0x9a, 0x3e, 0x19, 0x97,
+                0x5b, 0xe9, 0x92, 0x7f, 0x0e, 0x76, 0x14, 0xf0, 0x8d, 0x1f, 0x0a, 0x10, 0x8e, 0xed,
+                0xe5, 0xfd, 0x7e, 0xb5, 0x62, 0x45, 0x84, 0xa4, 0xf4,
+            ],
+            Z: [
+                0x04, 0x71, 0xa3, 0x52, 0x82, 0xd2, 0x02, 0x6f, 0x36, 0xbf, 0x3c, 0xeb, 0x38, 0xfc,
+                0xf8, 0x7e, 0x31, 0x12, 0xa4, 0x45, 0x2f, 0x46, 0xe9, 0xf7, 0xb4, 0x7f, 0xd7, 0x69,
+                0xcf, 0xb5, 0x70, 0x14, 0x5b, 0x62, 0x58, 0x9c, 0x76, 0xb7, 0xaa, 0x1e, 0xb6, 0x08,
+                0x0a, 0x83, 0x2e, 0x53, 0x32, 0xc3, 0x68, 0x98, 0x42, 0x69, 0x12, 0xe2, 0x9c, 0x40,
+                0xef, 0x9e, 0x9c, 0x74, 0x2e, 0xee, 0x82, 0xbf, 0x30,
+            ],
+            V: [
+                0x04, 0x67, 0x18, 0x98, 0x1b, 0xf1, 0x5b, 0xc4, 0xdb, 0x53, 0x8f, 0xc1, 0xf1, 0xc1,
+                0xd0, 0x58, 0xcb, 0x0e, 0xec, 0xec, 0xf1, 0xdb, 0xe1, 0xb1, 0xea, 0x08, 0xa4, 0xe2,
+                0x52, 0x75, 0xd3, 0x82, 0xe8, 0x2b, 0x34, 0x8c, 0x81, 0x31, 0xd8, 0xed, 0x66, 0x9d,
+                0x16, 0x9c, 0x2e, 0x03, 0xa8, 0x58, 0xdb, 0x7c, 0xf6, 0xca, 0x28, 0x53, 0xa4, 0x07,
+                0x12, 0x51, 0xa3, 0x9f, 0xbe, 0x8c, 0xfc, 0x39, 0xbc,
+            ],
+            L: [
+                0x04, 0x95, 0x64, 0x5c, 0xfb, 0x74, 0xdf, 0x6e, 0x58, 0xf9, 0x74, 0x8b, 0xb8, 0x3a,
+                0x86, 0x62, 0x0b, 0xab, 0x7c, 0x82, 0xe1, 0x07, 0xf5, 0x7d, 0x68, 0x70, 0xda, 0x8c,
+                0xbc, 0xb2, 0xff, 0x9f, 0x70, 0x63, 0xa1, 0x4b, 0x64, 0x02, 0xc6, 0x2f, 0x99, 0xaf,
+                0xcb, 0x97, 0x06, 0xa4, 0xd1, 0xa1, 0x43, 0x27, 0x32, 0x59, 0xfe, 0x76, 0xf1, 0xc6,
+                0x05, 0xa3, 0x63, 0x97, 0x45, 0xa9, 0x21, 0x54, 0xb9,
+            ],
+            cA: [
+                0xd4, 0x37, 0x6f, 0x2d, 0xa9, 0xc7, 0x22, 0x26, 0xdd, 0x15, 0x1b, 0x77, 0xc2, 0x91,
+                0x90, 0x71, 0x15, 0x5f, 0xc2, 0x2a, 0x20, 0x68, 0xd9, 0x0b, 0x5f, 0xaa, 0x6c, 0x78,
+                0xc1, 0x1e, 0x77, 0xdd,
+            ],
+            cB: [
+                0x06, 0x60, 0xa6, 0x80, 0x66, 0x3e, 0x8c, 0x56, 0x95, 0x95, 0x6f, 0xb2, 0x2d, 0xff,
+                0x29, 0x8b, 0x1d, 0x07, 0xa5, 0x26, 0xcf, 0x3c, 0xc5, 0x91, 0xad, 0xfe, 0xcd, 0x1f,
+                0x6e, 0xf6, 0xe0, 0x2e,
+            ],
+            Ke: [
+                0x80, 0x1d, 0xb2, 0x97, 0x65, 0x48, 0x16, 0xeb, 0x4f, 0x02, 0x86, 0x81, 0x29, 0xb9,
+                0xdc, 0x89,
+            ],
+            TT: [
+                0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x50, 0x41, 0x4b, 0x45, 0x32,
+                0x2b, 0x2d, 0x50, 0x32, 0x35, 0x36, 0x2d, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36, 0x2d,
+                0x48, 0x4b, 0x44, 0x46, 0x20, 0x64, 0x72, 0x61, 0x66, 0x74, 0x2d, 0x30, 0x31, 0x06,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x6c, 0x69, 0x65, 0x6e, 0x74, 0x06,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x73, 0x65, 0x72, 0x76, 0x65, 0x72, 0x41,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x88, 0x6e, 0x2f, 0x97, 0xac, 0xe4,
+                0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24, 0x25, 0x79, 0xf2, 0x99, 0x3b, 0x64, 0xe1, 0x6e,
+                0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4, 0x97, 0x33, 0x3d, 0x8f, 0xa1, 0x2f, 0x5f, 0xf3,
+                0x55, 0x16, 0x3e, 0x43, 0xce, 0x22, 0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02, 0xac, 0x8e,
+                0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7, 0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55, 0xa1, 0x2e,
+                0x2d, 0x20, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xd8, 0xbb, 0xd6,
+                0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0, 0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77, 0x07, 0x19,
+                0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49, 0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1, 0x29, 0x2b,
+                0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf, 0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36, 0x33, 0x7f,
+                0x51, 0x68, 0xc6, 0x4d, 0x9b, 0xd3, 0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64, 0x49, 0x0b,
+                0x1e, 0x65, 0x6e, 0xdb, 0xe7, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04,
+                0xaf, 0x09, 0x98, 0x7a, 0x59, 0x3d, 0x3b, 0xac, 0x86, 0x94, 0xb1, 0x23, 0x83, 0x94,
+                0x22, 0xc3, 0xcc, 0x87, 0xe3, 0x7d, 0x6b, 0x41, 0xc1, 0xd6, 0x30, 0xf0, 0x00, 0xdd,
+                0x64, 0x98, 0x0e, 0x53, 0x7a, 0xe7, 0x04, 0xbc, 0xed, 0xe0, 0x4e, 0xa3, 0xbe, 0xc9,
+                0xb7, 0x47, 0x5b, 0x32, 0xfa, 0x2c, 0xa3, 0xb6, 0x84, 0xbe, 0x14, 0xd1, 0x16, 0x45,
+                0xe3, 0x8e, 0xa6, 0x60, 0x9e, 0xb3, 0x9e, 0x7e, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x04, 0x41, 0x75, 0x92, 0x62, 0x0a, 0xeb, 0xf9, 0xfd, 0x20, 0x36, 0x16,
+                0xbb, 0xb9, 0xf1, 0x21, 0xb7, 0x30, 0xc2, 0x58, 0xb2, 0x86, 0xf8, 0x90, 0xc5, 0xf1,
+                0x9f, 0xea, 0x83, 0x3a, 0x9c, 0x90, 0x0c, 0xbe, 0x90, 0x57, 0xbc, 0x54, 0x9a, 0x3e,
+                0x19, 0x97, 0x5b, 0xe9, 0x92, 0x7f, 0x0e, 0x76, 0x14, 0xf0, 0x8d, 0x1f, 0x0a, 0x10,
+                0x8e, 0xed, 0xe5, 0xfd, 0x7e, 0xb5, 0x62, 0x45, 0x84, 0xa4, 0xf4, 0x41, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x71, 0xa3, 0x52, 0x82, 0xd2, 0x02, 0x6f, 0x36,
+                0xbf, 0x3c, 0xeb, 0x38, 0xfc, 0xf8, 0x7e, 0x31, 0x12, 0xa4, 0x45, 0x2f, 0x46, 0xe9,
+                0xf7, 0xb4, 0x7f, 0xd7, 0x69, 0xcf, 0xb5, 0x70, 0x14, 0x5b, 0x62, 0x58, 0x9c, 0x76,
+                0xb7, 0xaa, 0x1e, 0xb6, 0x08, 0x0a, 0x83, 0x2e, 0x53, 0x32, 0xc3, 0x68, 0x98, 0x42,
+                0x69, 0x12, 0xe2, 0x9c, 0x40, 0xef, 0x9e, 0x9c, 0x74, 0x2e, 0xee, 0x82, 0xbf, 0x30,
+                0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x67, 0x18, 0x98, 0x1b, 0xf1,
+                0x5b, 0xc4, 0xdb, 0x53, 0x8f, 0xc1, 0xf1, 0xc1, 0xd0, 0x58, 0xcb, 0x0e, 0xec, 0xec,
+                0xf1, 0xdb, 0xe1, 0xb1, 0xea, 0x08, 0xa4, 0xe2, 0x52, 0x75, 0xd3, 0x82, 0xe8, 0x2b,
+                0x34, 0x8c, 0x81, 0x31, 0xd8, 0xed, 0x66, 0x9d, 0x16, 0x9c, 0x2e, 0x03, 0xa8, 0x58,
+                0xdb, 0x7c, 0xf6, 0xca, 0x28, 0x53, 0xa4, 0x07, 0x12, 0x51, 0xa3, 0x9f, 0xbe, 0x8c,
+                0xfc, 0x39, 0xbc, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe6, 0x88, 0x7c,
+                0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6, 0x9b, 0xf4, 0x79, 0x28, 0xa8, 0x45, 0x14, 0xb5,
+                0xe3, 0x55, 0xac, 0x03, 0x48, 0x63, 0xf7, 0xff, 0xaf, 0x43, 0x90, 0xe6, 0x7d, 0x79,
+                0x8c,
+            ],
+            TT_len: 547,
+        },
+        RFCTestVector {
+            w0: [
+                0xe6, 0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6, 0x9b, 0xf4, 0x79, 0x28, 0xa8,
+                0x45, 0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63, 0xf7, 0xff, 0xaf, 0x43, 0x90,
+                0xe6, 0x7d, 0x79, 0x8c,
+            ],
+            w1: [
+                0x24, 0xb5, 0xae, 0x4a, 0xbd, 0xa8, 0x68, 0xec, 0x93, 0x36, 0xff, 0xc3, 0xb7, 0x8e,
+                0xe3, 0x1c, 0x57, 0x55, 0xbe, 0xf1, 0x75, 0x92, 0x27, 0xef, 0x53, 0x72, 0xca, 0x13,
+                0x9b, 0x94, 0xe5, 0x12,
+            ],
+            x: [
+                0xec, 0x82, 0xd9, 0x25, 0x83, 0x37, 0xf6, 0x12, 0x39, 0xc9, 0xcd, 0x68, 0xe8, 0xe5,
+                0x32, 0xa3, 0xa6, 0xb8, 0x3d, 0x12, 0xd2, 0xb1, 0xca, 0x5d, 0x54, 0x3f, 0x44, 0xde,
+                0xf1, 0x7d, 0xfb, 0x8d,
+            ],
+            X: [
+                0x04, 0x23, 0x07, 0x79, 0x96, 0x08, 0x24, 0x07, 0x6d, 0x36, 0x66, 0xa7, 0x41, 0x8e,
+                0x4d, 0x43, 0x3e, 0x2f, 0xa1, 0x5b, 0x06, 0x17, 0x6e, 0xab, 0xdd, 0x57, 0x2f, 0x43,
+                0xa3, 0x2e, 0xcc, 0x79, 0xa1, 0x92, 0xb2, 0x43, 0xd2, 0x62, 0x43, 0x10, 0xa7, 0x35,
+                0x62, 0x73, 0xb8, 0x6e, 0x5f, 0xd9, 0xbd, 0x62, 0x7d, 0x3a, 0xde, 0x76, 0x2b, 0xae,
+                0xff, 0x1a, 0x32, 0x0d, 0x4a, 0xd7, 0xa4, 0xe4, 0x7f,
+            ],
+            y: [
+                0xea, 0xc3, 0xf7, 0xde, 0x4b, 0x19, 0x8d, 0x5f, 0xe2, 0x5c, 0x44, 0x3c, 0x0c, 0xd4,
+                0x96, 0x38, 0x07, 0xad, 0xd7, 0x67, 0x81, 0x5d, 0xd0, 0x2a, 0x6f, 0x01, 0x33, 0xb4,
+                0xbc, 0x2c, 0x9e, 0xb0,
+            ],
+            Y: [
+                0x04, 0x45, 0x58, 0x64, 0x2e, 0x71, 0xb6, 0x16, 0xb2, 0x48, 0xc9, 0x58, 0x3b, 0xd6,
+                0xd7, 0xaa, 0x1b, 0x39, 0x52, 0xc6, 0xdf, 0x6a, 0x9f, 0x74, 0x92, 0xa0, 0x60, 0x35,
+                0xca, 0x5d, 0x92, 0x52, 0x2d, 0x84, 0x44, 0x3d, 0xe7, 0xaa, 0x20, 0xa5, 0x93, 0x80,
+                0xfa, 0x4d, 0xe6, 0xb7, 0x43, 0x8d, 0x92, 0x5d, 0xbf, 0xb7, 0xf1, 0xcf, 0xe6, 0x0d,
+                0x79, 0xac, 0xf9, 0x61, 0xee, 0x33, 0x98, 0x8c, 0x7d,
+            ],
+            Z: [
+                0x04, 0xb4, 0xe8, 0x77, 0x0f, 0x19, 0xf5, 0x8d, 0xdf, 0x83, 0xf9, 0x22, 0x0c, 0x3a,
+                0x93, 0x05, 0x79, 0x26, 0x65, 0xe0, 0xc6, 0x09, 0x89, 0xe6, 0xee, 0x9d, 0x7f, 0xa4,
+                0x49, 0xc7, 0x75, 0xd6, 0x39, 0x5f, 0x6f, 0x25, 0xf3, 0x07, 0xe3, 0x90, 0x3a, 0xc0,
+                0x45, 0xa0, 0x13, 0xfb, 0xb5, 0xa6, 0x76, 0xe8, 0x72, 0xa6, 0xab, 0xfc, 0xf4, 0xd7,
+                0xbb, 0x5a, 0xac, 0x69, 0xef, 0xd6, 0x14, 0x0e, 0xed,
+            ],
+            V: [
+                0x04, 0x14, 0x1d, 0xb8, 0x3b, 0xc7, 0xd9, 0x6f, 0x41, 0xb6, 0x36, 0x62, 0x2e, 0x7a,
+                0x5c, 0x55, 0x2a, 0xd8, 0x32, 0x11, 0xff, 0x55, 0x31, 0x9a, 0xc2, 0x5e, 0xd0, 0xa0,
+                0x9f, 0x08, 0x18, 0xbd, 0x94, 0x2e, 0x81, 0x50, 0x31, 0x9b, 0xfb, 0xfa, 0x68, 0x61,
+                0x83, 0x80, 0x6d, 0xc6, 0x19, 0x11, 0x18, 0x3f, 0x6a, 0x0f, 0x59, 0x56, 0x15, 0x60,
+                0x23, 0xd9, 0x6e, 0x0f, 0x93, 0xd2, 0x75, 0xbf, 0x50,
+            ],
+            L: [
+                0x04, 0x95, 0x64, 0x5c, 0xfb, 0x74, 0xdf, 0x6e, 0x58, 0xf9, 0x74, 0x8b, 0xb8, 0x3a,
+                0x86, 0x62, 0x0b, 0xab, 0x7c, 0x82, 0xe1, 0x07, 0xf5, 0x7d, 0x68, 0x70, 0xda, 0x8c,
+                0xbc, 0xb2, 0xff, 0x9f, 0x70, 0x63, 0xa1, 0x4b, 0x64, 0x02, 0xc6, 0x2f, 0x99, 0xaf,
+                0xcb, 0x97, 0x06, 0xa4, 0xd1, 0xa1, 0x43, 0x27, 0x32, 0x59, 0xfe, 0x76, 0xf1, 0xc6,
+                0x05, 0xa3, 0x63, 0x97, 0x45, 0xa9, 0x21, 0x54, 0xb9,
+            ],
+            cA: [
+                0xe1, 0xb9, 0x25, 0x88, 0x07, 0xba, 0x47, 0x50, 0xda, 0xe1, 0xd7, 0xf3, 0xc3, 0xc2,
+                0x94, 0xf1, 0x3d, 0xc4, 0xfa, 0x60, 0xcd, 0xe3, 0x46, 0xd5, 0xde, 0x7d, 0x20, 0x0e,
+                0x2f, 0x8f, 0xd3, 0xfc,
+            ],
+            cB: [
+                0xb9, 0xc3, 0x9d, 0xfa, 0x49, 0xc4, 0x77, 0x57, 0xde, 0x77, 0x8d, 0x9b, 0xed, 0xea,
+                0xca, 0x24, 0x48, 0xb9, 0x05, 0xbe, 0x19, 0xa4, 0x3b, 0x94, 0xee, 0x24, 0xb7, 0x70,
+                0x20, 0x81, 0x35, 0xe3,
+            ],
+            Ke: [
+                0x69, 0x89, 0xd8, 0xf9, 0x17, 0x7e, 0xf7, 0xdf, 0x67, 0xda, 0x43, 0x79, 0x87, 0xf0,
+                0x72, 0x55,
+            ],
+            TT: [
+                0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x50, 0x41, 0x4b, 0x45, 0x32,
+                0x2b, 0x2d, 0x50, 0x32, 0x35, 0x36, 0x2d, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36, 0x2d,
+                0x48, 0x4b, 0x44, 0x46, 0x20, 0x64, 0x72, 0x61, 0x66, 0x74, 0x2d, 0x30, 0x31, 0x06,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x6c, 0x69, 0x65, 0x6e, 0x74, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x04, 0x88, 0x6e, 0x2f, 0x97, 0xac, 0xe4, 0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24,
+                0x25, 0x79, 0xf2, 0x99, 0x3b, 0x64, 0xe1, 0x6e, 0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4,
+                0x97, 0x33, 0x3d, 0x8f, 0xa1, 0x2f, 0x5f, 0xf3, 0x55, 0x16, 0x3e, 0x43, 0xce, 0x22,
+                0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02, 0xac, 0x8e, 0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7,
+                0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55, 0xa1, 0x2e, 0x2d, 0x20, 0x41, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x04, 0xd8, 0xbb, 0xd6, 0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0,
+                0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77, 0x07, 0x19, 0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49,
+                0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1, 0x29, 0x2b, 0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf,
+                0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36, 0x33, 0x7f, 0x51, 0x68, 0xc6, 0x4d, 0x9b, 0xd3,
+                0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64, 0x49, 0x0b, 0x1e, 0x65, 0x6e, 0xdb, 0xe7, 0x41,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x23, 0x07, 0x79, 0x96, 0x08, 0x24,
+                0x07, 0x6d, 0x36, 0x66, 0xa7, 0x41, 0x8e, 0x4d, 0x43, 0x3e, 0x2f, 0xa1, 0x5b, 0x06,
+                0x17, 0x6e, 0xab, 0xdd, 0x57, 0x2f, 0x43, 0xa3, 0x2e, 0xcc, 0x79, 0xa1, 0x92, 0xb2,
+                0x43, 0xd2, 0x62, 0x43, 0x10, 0xa7, 0x35, 0x62, 0x73, 0xb8, 0x6e, 0x5f, 0xd9, 0xbd,
+                0x62, 0x7d, 0x3a, 0xde, 0x76, 0x2b, 0xae, 0xff, 0x1a, 0x32, 0x0d, 0x4a, 0xd7, 0xa4,
+                0xe4, 0x7f, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x45, 0x58, 0x64,
+                0x2e, 0x71, 0xb6, 0x16, 0xb2, 0x48, 0xc9, 0x58, 0x3b, 0xd6, 0xd7, 0xaa, 0x1b, 0x39,
+                0x52, 0xc6, 0xdf, 0x6a, 0x9f, 0x74, 0x92, 0xa0, 0x60, 0x35, 0xca, 0x5d, 0x92, 0x52,
+                0x2d, 0x84, 0x44, 0x3d, 0xe7, 0xaa, 0x20, 0xa5, 0x93, 0x80, 0xfa, 0x4d, 0xe6, 0xb7,
+                0x43, 0x8d, 0x92, 0x5d, 0xbf, 0xb7, 0xf1, 0xcf, 0xe6, 0x0d, 0x79, 0xac, 0xf9, 0x61,
+                0xee, 0x33, 0x98, 0x8c, 0x7d, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04,
+                0xb4, 0xe8, 0x77, 0x0f, 0x19, 0xf5, 0x8d, 0xdf, 0x83, 0xf9, 0x22, 0x0c, 0x3a, 0x93,
+                0x05, 0x79, 0x26, 0x65, 0xe0, 0xc6, 0x09, 0x89, 0xe6, 0xee, 0x9d, 0x7f, 0xa4, 0x49,
+                0xc7, 0x75, 0xd6, 0x39, 0x5f, 0x6f, 0x25, 0xf3, 0x07, 0xe3, 0x90, 0x3a, 0xc0, 0x45,
+                0xa0, 0x13, 0xfb, 0xb5, 0xa6, 0x76, 0xe8, 0x72, 0xa6, 0xab, 0xfc, 0xf4, 0xd7, 0xbb,
+                0x5a, 0xac, 0x69, 0xef, 0xd6, 0x14, 0x0e, 0xed, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x04, 0x14, 0x1d, 0xb8, 0x3b, 0xc7, 0xd9, 0x6f, 0x41, 0xb6, 0x36, 0x62,
+                0x2e, 0x7a, 0x5c, 0x55, 0x2a, 0xd8, 0x32, 0x11, 0xff, 0x55, 0x31, 0x9a, 0xc2, 0x5e,
+                0xd0, 0xa0, 0x9f, 0x08, 0x18, 0xbd, 0x94, 0x2e, 0x81, 0x50, 0x31, 0x9b, 0xfb, 0xfa,
+                0x68, 0x61, 0x83, 0x80, 0x6d, 0xc6, 0x19, 0x11, 0x18, 0x3f, 0x6a, 0x0f, 0x59, 0x56,
+                0x15, 0x60, 0x23, 0xd9, 0x6e, 0x0f, 0x93, 0xd2, 0x75, 0xbf, 0x50, 0x20, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0xe6, 0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6,
+                0x9b, 0xf4, 0x79, 0x28, 0xa8, 0x45, 0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63,
+                0xf7, 0xff, 0xaf, 0x43, 0x90, 0xe6, 0x7d, 0x79, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00,
+            ],
+            TT_len: 541,
+        },
+        RFCTestVector {
+            w0: [
+                0xe6, 0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6, 0x9b, 0xf4, 0x79, 0x28, 0xa8,
+                0x45, 0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63, 0xf7, 0xff, 0xaf, 0x43, 0x90,
+                0xe6, 0x7d, 0x79, 0x8c,
+            ],
+            w1: [
+                0x24, 0xb5, 0xae, 0x4a, 0xbd, 0xa8, 0x68, 0xec, 0x93, 0x36, 0xff, 0xc3, 0xb7, 0x8e,
+                0xe3, 0x1c, 0x57, 0x55, 0xbe, 0xf1, 0x75, 0x92, 0x27, 0xef, 0x53, 0x72, 0xca, 0x13,
+                0x9b, 0x94, 0xe5, 0x12,
+            ],
+            x: [
+                0xba, 0x0f, 0x0f, 0x5b, 0x78, 0xef, 0x23, 0xfd, 0x07, 0x86, 0x8e, 0x46, 0xae, 0xca,
+                0x63, 0xb5, 0x1f, 0xda, 0x51, 0x9a, 0x34, 0x20, 0x50, 0x1a, 0xcb, 0xe2, 0x3d, 0x53,
+                0xc2, 0x91, 0x87, 0x48,
+            ],
+            X: [
+                0x04, 0xc1, 0x4d, 0x28, 0xf4, 0x37, 0x0f, 0xea, 0x20, 0x74, 0x51, 0x06, 0xce, 0xa5,
+                0x8b, 0xcf, 0xb6, 0x0f, 0x29, 0x49, 0xfa, 0x4e, 0x13, 0x1b, 0x9a, 0xff, 0x5e, 0xa1,
+                0x3f, 0xd5, 0xaa, 0x79, 0xd5, 0x07, 0xae, 0x1d, 0x22, 0x9e, 0x44, 0x7e, 0x00, 0x0f,
+                0x15, 0xeb, 0x78, 0xa9, 0xa3, 0x2c, 0x2b, 0x88, 0x65, 0x2e, 0x34, 0x11, 0x64, 0x20,
+                0x43, 0xc1, 0xb2, 0xb7, 0x99, 0x2c, 0xf2, 0xd4, 0xde,
+            ],
+            y: [
+                0x39, 0x39, 0x7f, 0xbe, 0x6d, 0xb4, 0x7e, 0x9f, 0xbd, 0x1a, 0x26, 0x3d, 0x79, 0xf5,
+                0xd0, 0xaa, 0xa4, 0x4d, 0xf2, 0x6c, 0xe7, 0x55, 0xf7, 0x8e, 0x09, 0x26, 0x44, 0xb4,
+                0x34, 0x53, 0x3a, 0x42,
+            ],
+            Y: [
+                0x04, 0xd1, 0xbe, 0xe3, 0x12, 0x0f, 0xd8, 0x7e, 0x86, 0xfe, 0x18, 0x9c, 0xb9, 0x52,
+                0xdc, 0x68, 0x88, 0x23, 0x08, 0x0e, 0x62, 0x52, 0x4d, 0xd2, 0xc0, 0x8d, 0xff, 0xe3,
+                0xd2, 0x2a, 0x0a, 0x89, 0x86, 0xaa, 0x64, 0xc9, 0xfe, 0x01, 0x91, 0x03, 0x3c, 0xaf,
+                0xbc, 0x9b, 0xca, 0xef, 0xc8, 0xe2, 0xba, 0x8b, 0xa8, 0x60, 0xcd, 0x12, 0x7a, 0xf9,
+                0xef, 0xdd, 0x7f, 0x1c, 0x3a, 0x41, 0x92, 0x0f, 0xe8,
+            ],
+            Z: [
+                0x04, 0xaa, 0xc7, 0x1c, 0xf4, 0xc8, 0xdf, 0x81, 0x81, 0xb8, 0x67, 0xc9, 0xec, 0xbe,
+                0xe9, 0xd0, 0x96, 0x3c, 0xaf, 0x51, 0xf1, 0x53, 0x4a, 0x82, 0x34, 0x29, 0xc2, 0x6f,
+                0xe5, 0x24, 0x83, 0x13, 0xff, 0xc5, 0xc5, 0xe4, 0x4e, 0xa8, 0x16, 0x21, 0x61, 0xab,
+                0x6b, 0x3d, 0x73, 0xb8, 0x77, 0x04, 0xa4, 0x58, 0x89, 0xbf, 0x63, 0x43, 0xd9, 0x6f,
+                0xa9, 0x6c, 0xd1, 0x64, 0x1e, 0xfa, 0x71, 0x60, 0x7c,
+            ],
+            V: [
+                0x04, 0xc7, 0xc9, 0x50, 0x53, 0x65, 0xf7, 0xce, 0x57, 0x29, 0x3c, 0x92, 0xa3, 0x7f,
+                0x1b, 0xbd, 0xc6, 0x8e, 0x03, 0x22, 0x90, 0x1e, 0x61, 0xed, 0xef, 0x59, 0xfe, 0xe7,
+                0x87, 0x6b, 0x17, 0xb0, 0x63, 0xe0, 0xfa, 0x4a, 0x12, 0x6e, 0xae, 0x0a, 0x67, 0x1b,
+                0x37, 0xf1, 0x46, 0x4c, 0xf1, 0xcc, 0xad, 0x59, 0x1c, 0x33, 0xae, 0x94, 0x4e, 0x3b,
+                0x1f, 0x31, 0x8d, 0x76, 0xe3, 0x6f, 0xea, 0x99, 0x66,
+            ],
+            L: [
+                0x04, 0x95, 0x64, 0x5c, 0xfb, 0x74, 0xdf, 0x6e, 0x58, 0xf9, 0x74, 0x8b, 0xb8, 0x3a,
+                0x86, 0x62, 0x0b, 0xab, 0x7c, 0x82, 0xe1, 0x07, 0xf5, 0x7d, 0x68, 0x70, 0xda, 0x8c,
+                0xbc, 0xb2, 0xff, 0x9f, 0x70, 0x63, 0xa1, 0x4b, 0x64, 0x02, 0xc6, 0x2f, 0x99, 0xaf,
+                0xcb, 0x97, 0x06, 0xa4, 0xd1, 0xa1, 0x43, 0x27, 0x32, 0x59, 0xfe, 0x76, 0xf1, 0xc6,
+                0x05, 0xa3, 0x63, 0x97, 0x45, 0xa9, 0x21, 0x54, 0xb9,
+            ],
+            cA: [
+                0xe5, 0x64, 0xc9, 0x3b, 0x30, 0x15, 0xef, 0xb9, 0x46, 0xdc, 0x16, 0xd6, 0x42, 0xbb,
+                0xe7, 0xd1, 0xc8, 0xda, 0x5b, 0xe1, 0x64, 0xed, 0x9f, 0xc3, 0xba, 0xe4, 0xe0, 0xff,
+                0x86, 0xe1, 0xbd, 0x3c,
+            ],
+            cB: [
+                0x07, 0x2a, 0x94, 0xd9, 0xa5, 0x4e, 0xdc, 0x20, 0x1d, 0x88, 0x91, 0x53, 0x4c, 0x23,
+                0x17, 0xca, 0xdf, 0x3e, 0xa3, 0x79, 0x28, 0x27, 0xf4, 0x79, 0xe8, 0x73, 0xf9, 0x3e,
+                0x90, 0xf2, 0x15, 0x52,
+            ],
+            Ke: [
+                0x2e, 0xa4, 0x0e, 0x4b, 0xad, 0xfa, 0x54, 0x52, 0xb5, 0x74, 0x4d, 0xc5, 0x98, 0x3e,
+                0x99, 0xba,
+            ],
+            TT: [
+                0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x50, 0x41, 0x4b, 0x45, 0x32,
+                0x2b, 0x2d, 0x50, 0x32, 0x35, 0x36, 0x2d, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36, 0x2d,
+                0x48, 0x4b, 0x44, 0x46, 0x20, 0x64, 0x72, 0x61, 0x66, 0x74, 0x2d, 0x30, 0x31, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x73, 0x65, 0x72, 0x76, 0x65, 0x72, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x04, 0x88, 0x6e, 0x2f, 0x97, 0xac, 0xe4, 0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24,
+                0x25, 0x79, 0xf2, 0x99, 0x3b, 0x64, 0xe1, 0x6e, 0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4,
+                0x97, 0x33, 0x3d, 0x8f, 0xa1, 0x2f, 0x5f, 0xf3, 0x55, 0x16, 0x3e, 0x43, 0xce, 0x22,
+                0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02, 0xac, 0x8e, 0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7,
+                0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55, 0xa1, 0x2e, 0x2d, 0x20, 0x41, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x04, 0xd8, 0xbb, 0xd6, 0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0,
+                0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77, 0x07, 0x19, 0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49,
+                0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1, 0x29, 0x2b, 0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf,
+                0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36, 0x33, 0x7f, 0x51, 0x68, 0xc6, 0x4d, 0x9b, 0xd3,
+                0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64, 0x49, 0x0b, 0x1e, 0x65, 0x6e, 0xdb, 0xe7, 0x41,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xc1, 0x4d, 0x28, 0xf4, 0x37, 0x0f,
+                0xea, 0x20, 0x74, 0x51, 0x06, 0xce, 0xa5, 0x8b, 0xcf, 0xb6, 0x0f, 0x29, 0x49, 0xfa,
+                0x4e, 0x13, 0x1b, 0x9a, 0xff, 0x5e, 0xa1, 0x3f, 0xd5, 0xaa, 0x79, 0xd5, 0x07, 0xae,
+                0x1d, 0x22, 0x9e, 0x44, 0x7e, 0x00, 0x0f, 0x15, 0xeb, 0x78, 0xa9, 0xa3, 0x2c, 0x2b,
+                0x88, 0x65, 0x2e, 0x34, 0x11, 0x64, 0x20, 0x43, 0xc1, 0xb2, 0xb7, 0x99, 0x2c, 0xf2,
+                0xd4, 0xde, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xd1, 0xbe, 0xe3,
+                0x12, 0x0f, 0xd8, 0x7e, 0x86, 0xfe, 0x18, 0x9c, 0xb9, 0x52, 0xdc, 0x68, 0x88, 0x23,
+                0x08, 0x0e, 0x62, 0x52, 0x4d, 0xd2, 0xc0, 0x8d, 0xff, 0xe3, 0xd2, 0x2a, 0x0a, 0x89,
+                0x86, 0xaa, 0x64, 0xc9, 0xfe, 0x01, 0x91, 0x03, 0x3c, 0xaf, 0xbc, 0x9b, 0xca, 0xef,
+                0xc8, 0xe2, 0xba, 0x8b, 0xa8, 0x60, 0xcd, 0x12, 0x7a, 0xf9, 0xef, 0xdd, 0x7f, 0x1c,
+                0x3a, 0x41, 0x92, 0x0f, 0xe8, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04,
+                0xaa, 0xc7, 0x1c, 0xf4, 0xc8, 0xdf, 0x81, 0x81, 0xb8, 0x67, 0xc9, 0xec, 0xbe, 0xe9,
+                0xd0, 0x96, 0x3c, 0xaf, 0x51, 0xf1, 0x53, 0x4a, 0x82, 0x34, 0x29, 0xc2, 0x6f, 0xe5,
+                0x24, 0x83, 0x13, 0xff, 0xc5, 0xc5, 0xe4, 0x4e, 0xa8, 0x16, 0x21, 0x61, 0xab, 0x6b,
+                0x3d, 0x73, 0xb8, 0x77, 0x04, 0xa4, 0x58, 0x89, 0xbf, 0x63, 0x43, 0xd9, 0x6f, 0xa9,
+                0x6c, 0xd1, 0x64, 0x1e, 0xfa, 0x71, 0x60, 0x7c, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x04, 0xc7, 0xc9, 0x50, 0x53, 0x65, 0xf7, 0xce, 0x57, 0x29, 0x3c, 0x92,
+                0xa3, 0x7f, 0x1b, 0xbd, 0xc6, 0x8e, 0x03, 0x22, 0x90, 0x1e, 0x61, 0xed, 0xef, 0x59,
+                0xfe, 0xe7, 0x87, 0x6b, 0x17, 0xb0, 0x63, 0xe0, 0xfa, 0x4a, 0x12, 0x6e, 0xae, 0x0a,
+                0x67, 0x1b, 0x37, 0xf1, 0x46, 0x4c, 0xf1, 0xcc, 0xad, 0x59, 0x1c, 0x33, 0xae, 0x94,
+                0x4e, 0x3b, 0x1f, 0x31, 0x8d, 0x76, 0xe3, 0x6f, 0xea, 0x99, 0x66, 0x20, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0xe6, 0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6,
+                0x9b, 0xf4, 0x79, 0x28, 0xa8, 0x45, 0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63,
+                0xf7, 0xff, 0xaf, 0x43, 0x90, 0xe6, 0x7d, 0x79, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00,
+            ],
+            TT_len: 541,
+        },
+        RFCTestVector {
+            w0: [
+                0xe6, 0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6, 0x9b, 0xf4, 0x79, 0x28, 0xa8,
+                0x45, 0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63, 0xf7, 0xff, 0xaf, 0x43, 0x90,
+                0xe6, 0x7d, 0x79, 0x8c,
+            ],
+            w1: [
+                0x24, 0xb5, 0xae, 0x4a, 0xbd, 0xa8, 0x68, 0xec, 0x93, 0x36, 0xff, 0xc3, 0xb7, 0x8e,
+                0xe3, 0x1c, 0x57, 0x55, 0xbe, 0xf1, 0x75, 0x92, 0x27, 0xef, 0x53, 0x72, 0xca, 0x13,
+                0x9b, 0x94, 0xe5, 0x12,
+            ],
+            x: [
+                0x5b, 0x47, 0x86, 0x19, 0x80, 0x4f, 0x49, 0x38, 0xd3, 0x61, 0xfb, 0xba, 0x3a, 0x20,
+                0x64, 0x87, 0x25, 0x22, 0x2f, 0x0a, 0x54, 0xcc, 0x4c, 0x87, 0x61, 0x39, 0xef, 0xe7,
+                0xd9, 0xa2, 0x17, 0x86,
+            ],
+            X: [
+                0x04, 0xa6, 0xdb, 0x23, 0xd0, 0x01, 0x72, 0x3f, 0xb0, 0x1f, 0xcf, 0xc9, 0xd0, 0x87,
+                0x46, 0xc3, 0xc2, 0xa0, 0xa3, 0xfe, 0xff, 0x86, 0x35, 0xd2, 0x9c, 0xad, 0x28, 0x53,
+                0xe7, 0x35, 0x86, 0x23, 0x42, 0x5c, 0xf3, 0x97, 0x12, 0xe9, 0x28, 0x05, 0x45, 0x61,
+                0xba, 0x71, 0xe2, 0xdc, 0x11, 0xf3, 0x00, 0xf1, 0x76, 0x0e, 0x71, 0xeb, 0x17, 0x70,
+                0x21, 0xa8, 0xf8, 0x5e, 0x78, 0x68, 0x90, 0x71, 0xcd,
+            ],
+            y: [
+                0x76, 0x67, 0x70, 0xda, 0xd8, 0xc8, 0xee, 0xcb, 0xa9, 0x36, 0x82, 0x3c, 0x0a, 0xed,
+                0x04, 0x4b, 0x8c, 0x3c, 0x4f, 0x76, 0x55, 0xe8, 0xbe, 0xec, 0x44, 0xa1, 0x5d, 0xcb,
+                0xca, 0xf7, 0x8e, 0x5e,
+            ],
+            Y: [
+                0x04, 0x39, 0x0d, 0x29, 0xbf, 0x18, 0x5c, 0x3a, 0xbf, 0x99, 0xf1, 0x50, 0xae, 0x7c,
+                0x13, 0x38, 0x8c, 0x82, 0xb6, 0xbe, 0x0c, 0x07, 0xb1, 0xb8, 0xd9, 0x0d, 0x26, 0x85,
+                0x3e, 0x84, 0x37, 0x4b, 0xbd, 0xc8, 0x2b, 0xec, 0xdb, 0x97, 0x8c, 0xa3, 0x79, 0x2f,
+                0x47, 0x24, 0x24, 0x10, 0x6a, 0x25, 0x78, 0x01, 0x27, 0x52, 0xc1, 0x19, 0x38, 0xfc,
+                0xf6, 0x0a, 0x41, 0xdf, 0x75, 0xff, 0x7c, 0xf9, 0x47,
+            ],
+            Z: [
+                0x04, 0x0a, 0x15, 0x0d, 0x9a, 0x62, 0xf5, 0x14, 0xc9, 0xa1, 0xfe, 0xdd, 0x78, 0x2a,
+                0x02, 0x40, 0xa3, 0x42, 0x72, 0x10, 0x46, 0xce, 0xfb, 0x11, 0x11, 0xc3, 0xad, 0xb3,
+                0xbe, 0x89, 0x3c, 0xe9, 0xfc, 0xd2, 0xff, 0xa1, 0x37, 0x92, 0x2f, 0xcf, 0x8a, 0x58,
+                0x8d, 0x0f, 0x76, 0xba, 0x9c, 0x55, 0xc8, 0x5d, 0xa2, 0xaf, 0x3f, 0x1c, 0x78, 0x9c,
+                0xa1, 0x79, 0x76, 0x81, 0x03, 0x87, 0xfb, 0x1d, 0x7e,
+            ],
+            V: [
+                0x04, 0xf8, 0xe2, 0x47, 0xcc, 0x26, 0x3a, 0x18, 0x46, 0x27, 0x2f, 0x5a, 0x3b, 0x61,
+                0xb6, 0x8a, 0xa6, 0x0a, 0x5a, 0x26, 0x65, 0xd1, 0x0c, 0xd2, 0x2c, 0x89, 0xcd, 0x6b,
+                0xad, 0x05, 0xdc, 0x0e, 0x5e, 0x65, 0x0f, 0x21, 0xff, 0x01, 0x71, 0x86, 0xcc, 0x92,
+                0x65, 0x1a, 0x4c, 0xd7, 0xe6, 0x6c, 0xe8, 0x8f, 0x52, 0x92, 0x99, 0xf3, 0x40, 0xea,
+                0x80, 0xfb, 0x90, 0xa9, 0xba, 0xd0, 0x94, 0xe1, 0xa6,
+            ],
+            L: [
+                0x04, 0x95, 0x64, 0x5c, 0xfb, 0x74, 0xdf, 0x6e, 0x58, 0xf9, 0x74, 0x8b, 0xb8, 0x3a,
+                0x86, 0x62, 0x0b, 0xab, 0x7c, 0x82, 0xe1, 0x07, 0xf5, 0x7d, 0x68, 0x70, 0xda, 0x8c,
+                0xbc, 0xb2, 0xff, 0x9f, 0x70, 0x63, 0xa1, 0x4b, 0x64, 0x02, 0xc6, 0x2f, 0x99, 0xaf,
+                0xcb, 0x97, 0x06, 0xa4, 0xd1, 0xa1, 0x43, 0x27, 0x32, 0x59, 0xfe, 0x76, 0xf1, 0xc6,
+                0x05, 0xa3, 0x63, 0x97, 0x45, 0xa9, 0x21, 0x54, 0xb9,
+            ],
+            cA: [
+                0x71, 0xd9, 0x41, 0x27, 0x79, 0xb6, 0xc4, 0x5a, 0x2c, 0x61, 0x5c, 0x9d, 0xf3, 0xf1,
+                0xfd, 0x93, 0xdc, 0x0a, 0xaf, 0x63, 0x10, 0x4d, 0xa8, 0xec, 0xe4, 0xaa, 0x1b, 0x5a,
+                0x3a, 0x41, 0x5f, 0xea,
+            ],
+            cB: [
+                0x09, 0x5d, 0xc0, 0x40, 0x03, 0x55, 0xcc, 0x23, 0x3f, 0xde, 0x74, 0x37, 0x81, 0x18,
+                0x15, 0xb3, 0xc1, 0x52, 0x4a, 0xae, 0x80, 0xfd, 0x4e, 0x68, 0x10, 0xcf, 0x53, 0x1c,
+                0xf1, 0x1d, 0x20, 0xe3,
+            ],
+            Ke: [
+                0xea, 0x32, 0x76, 0xd6, 0x83, 0x34, 0x57, 0x60, 0x97, 0xe0, 0x4b, 0x19, 0xee, 0x5a,
+                0x3a, 0x8b,
+            ],
+            TT: [
+                0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x50, 0x41, 0x4b, 0x45, 0x32,
+                0x2b, 0x2d, 0x50, 0x32, 0x35, 0x36, 0x2d, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36, 0x2d,
+                0x48, 0x4b, 0x44, 0x46, 0x20, 0x64, 0x72, 0x61, 0x66, 0x74, 0x2d, 0x30, 0x31, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x88, 0x6e, 0x2f, 0x97,
+                0xac, 0xe4, 0x6e, 0x55, 0xba, 0x9d, 0xd7, 0x24, 0x25, 0x79, 0xf2, 0x99, 0x3b, 0x64,
+                0xe1, 0x6e, 0xf3, 0xdc, 0xab, 0x95, 0xaf, 0xd4, 0x97, 0x33, 0x3d, 0x8f, 0xa1, 0x2f,
+                0x5f, 0xf3, 0x55, 0x16, 0x3e, 0x43, 0xce, 0x22, 0x4e, 0x0b, 0x0e, 0x65, 0xff, 0x02,
+                0xac, 0x8e, 0x5c, 0x7b, 0xe0, 0x94, 0x19, 0xc7, 0x85, 0xe0, 0xca, 0x54, 0x7d, 0x55,
+                0xa1, 0x2e, 0x2d, 0x20, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xd8,
+                0xbb, 0xd6, 0xc6, 0x39, 0xc6, 0x29, 0x37, 0xb0, 0x4d, 0x99, 0x7f, 0x38, 0xc3, 0x77,
+                0x07, 0x19, 0xc6, 0x29, 0xd7, 0x01, 0x4d, 0x49, 0xa2, 0x4b, 0x4f, 0x98, 0xba, 0xa1,
+                0x29, 0x2b, 0x49, 0x07, 0xd6, 0x0a, 0xa6, 0xbf, 0xad, 0xe4, 0x50, 0x08, 0xa6, 0x36,
+                0x33, 0x7f, 0x51, 0x68, 0xc6, 0x4d, 0x9b, 0xd3, 0x60, 0x34, 0x80, 0x8c, 0xd5, 0x64,
+                0x49, 0x0b, 0x1e, 0x65, 0x6e, 0xdb, 0xe7, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x04, 0xa6, 0xdb, 0x23, 0xd0, 0x01, 0x72, 0x3f, 0xb0, 0x1f, 0xcf, 0xc9, 0xd0,
+                0x87, 0x46, 0xc3, 0xc2, 0xa0, 0xa3, 0xfe, 0xff, 0x86, 0x35, 0xd2, 0x9c, 0xad, 0x28,
+                0x53, 0xe7, 0x35, 0x86, 0x23, 0x42, 0x5c, 0xf3, 0x97, 0x12, 0xe9, 0x28, 0x05, 0x45,
+                0x61, 0xba, 0x71, 0xe2, 0xdc, 0x11, 0xf3, 0x00, 0xf1, 0x76, 0x0e, 0x71, 0xeb, 0x17,
+                0x70, 0x21, 0xa8, 0xf8, 0x5e, 0x78, 0x68, 0x90, 0x71, 0xcd, 0x41, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x04, 0x39, 0x0d, 0x29, 0xbf, 0x18, 0x5c, 0x3a, 0xbf, 0x99,
+                0xf1, 0x50, 0xae, 0x7c, 0x13, 0x38, 0x8c, 0x82, 0xb6, 0xbe, 0x0c, 0x07, 0xb1, 0xb8,
+                0xd9, 0x0d, 0x26, 0x85, 0x3e, 0x84, 0x37, 0x4b, 0xbd, 0xc8, 0x2b, 0xec, 0xdb, 0x97,
+                0x8c, 0xa3, 0x79, 0x2f, 0x47, 0x24, 0x24, 0x10, 0x6a, 0x25, 0x78, 0x01, 0x27, 0x52,
+                0xc1, 0x19, 0x38, 0xfc, 0xf6, 0x0a, 0x41, 0xdf, 0x75, 0xff, 0x7c, 0xf9, 0x47, 0x41,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x0a, 0x15, 0x0d, 0x9a, 0x62, 0xf5,
+                0x14, 0xc9, 0xa1, 0xfe, 0xdd, 0x78, 0x2a, 0x02, 0x40, 0xa3, 0x42, 0x72, 0x10, 0x46,
+                0xce, 0xfb, 0x11, 0x11, 0xc3, 0xad, 0xb3, 0xbe, 0x89, 0x3c, 0xe9, 0xfc, 0xd2, 0xff,
+                0xa1, 0x37, 0x92, 0x2f, 0xcf, 0x8a, 0x58, 0x8d, 0x0f, 0x76, 0xba, 0x9c, 0x55, 0xc8,
+                0x5d, 0xa2, 0xaf, 0x3f, 0x1c, 0x78, 0x9c, 0xa1, 0x79, 0x76, 0x81, 0x03, 0x87, 0xfb,
+                0x1d, 0x7e, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xf8, 0xe2, 0x47,
+                0xcc, 0x26, 0x3a, 0x18, 0x46, 0x27, 0x2f, 0x5a, 0x3b, 0x61, 0xb6, 0x8a, 0xa6, 0x0a,
+                0x5a, 0x26, 0x65, 0xd1, 0x0c, 0xd2, 0x2c, 0x89, 0xcd, 0x6b, 0xad, 0x05, 0xdc, 0x0e,
+                0x5e, 0x65, 0x0f, 0x21, 0xff, 0x01, 0x71, 0x86, 0xcc, 0x92, 0x65, 0x1a, 0x4c, 0xd7,
+                0xe6, 0x6c, 0xe8, 0x8f, 0x52, 0x92, 0x99, 0xf3, 0x40, 0xea, 0x80, 0xfb, 0x90, 0xa9,
+                0xba, 0xd0, 0x94, 0xe1, 0xa6, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe6,
+                0x88, 0x7c, 0xf9, 0xbd, 0xfb, 0x75, 0x79, 0xc6, 0x9b, 0xf4, 0x79, 0x28, 0xa8, 0x45,
+                0x14, 0xb5, 0xe3, 0x55, 0xac, 0x03, 0x48, 0x63, 0xf7, 0xff, 0xaf, 0x43, 0x90, 0xe6,
+                0x7d, 0x79, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00,
+            ],
+            TT_len: 535,
+        },
+    ];
+}

matter/src/secure_channel/status_report.rs

@@ -0,0 +1,60 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::common::*;
+use crate::{error::Error, transport::packet::Packet};
+
+#[allow(dead_code)]
+#[derive(Debug, Copy, Clone)]
+pub enum GeneralCode {
+    Success = 0,
+    Failure = 1,
+    BadPrecondition = 2,
+    OutOfRange = 3,
+    BadRequest = 4,
+    Unsupported = 5,
+    Unexpected = 6,
+    ResourceExhausted = 7,
+    Busy = 8,
+    Timeout = 9,
+    Continue = 10,
+    Aborted = 11,
+    InvalidArgument = 12,
+    NotFound = 13,
+    AlreadyExists = 14,
+    PermissionDenied = 15,
+    DataLoss = 16,
+}
+pub fn create_status_report(
+    proto_tx: &mut Packet,
+    general_code: GeneralCode,
+    proto_id: u32,
+    proto_code: u16,
+    proto_data: Option<&[u8]>,
+) -> Result<(), Error> {
+    proto_tx.set_proto_id(PROTO_ID_SECURE_CHANNEL as u16);
+    proto_tx.set_proto_opcode(OpCode::StatusReport as u8);
+    let wb = proto_tx.get_writebuf()?;
+    wb.le_u16(general_code as u16)?;
+    wb.le_u32(proto_id)?;
+    wb.le_u16(proto_code)?;
+    if let Some(s) = proto_data {
+        wb.copy_from_slice(s)?;
+    }
+
+    Ok(())
+}

matter/src/sys/mod.rs

@@ -0,0 +1,31 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+#[cfg(target_os = "macos")]
+mod sys_macos;
+#[cfg(target_os = "macos")]
+pub use self::sys_macos::*;
+
+#[cfg(target_os = "linux")]
+mod sys_linux;
+#[cfg(target_os = "linux")]
+pub use self::sys_linux::*;
+
+#[cfg(any(target_os = "macos", target_os = "linux"))]
+mod posix;
+#[cfg(any(target_os = "macos", target_os = "linux"))]
+pub use self::posix::*;

matter/src/sys/posix.rs

@@ -0,0 +1,92 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::{
+    convert::TryInto,
+    fs::{DirBuilder, File},
+    io::{Read, Write},
+    sync::{Arc, Mutex, Once},
+};
+
+use crate::error::Error;
+
+pub const SPAKE2_ITERATION_COUNT: u32 = 2000;
+
+// The Packet Pool that is allocated from. POSIX systems can use
+// higher values unlike embedded systems
+pub const MAX_PACKET_POOL_SIZE: usize = 25;
+
+pub struct Psm {}
+
+static mut G_PSM: Option<Arc<Mutex<Psm>>> = None;
+static INIT: Once = Once::new();
+
+const PSM_DIR: &str = "/tmp/plonk_psm";
+
+macro_rules! psm_path {
+    ($key:ident) => {
+        format!("{}/{}", PSM_DIR, $key)
+    };
+}
+
+impl Psm {
+    fn new() -> Result<Self, Error> {
+        let result = DirBuilder::new().create(PSM_DIR);
+        if let Err(e) = result {
+            if e.kind() != std::io::ErrorKind::AlreadyExists {
+                return Err(e.into());
+            }
+        }
+
+        Ok(Self {})
+    }
+
+    pub fn get() -> Result<Arc<Mutex<Self>>, Error> {
+        unsafe {
+            INIT.call_once(|| {
+                G_PSM = Some(Arc::new(Mutex::new(Psm::new().unwrap())));
+            });
+            Ok(G_PSM.as_ref().ok_or(Error::Invalid)?.clone())
+        }
+    }
+
+    pub fn set_kv_slice(&self, key: &str, val: &[u8]) -> Result<(), Error> {
+        let mut f = File::create(psm_path!(key))?;
+        f.write_all(val)?;
+        Ok(())
+    }
+
+    pub fn get_kv_slice(&self, key: &str, val: &mut Vec<u8>) -> Result<usize, Error> {
+        let mut f = File::open(psm_path!(key))?;
+        let len = f.read_to_end(val)?;
+        Ok(len)
+    }
+
+    pub fn set_kv_u64(&self, key: &str, val: u64) -> Result<(), Error> {
+        let mut f = File::create(psm_path!(key))?;
+        f.write_all(&val.to_be_bytes())?;
+        Ok(())
+    }
+
+    pub fn get_kv_u64(&self, key: &str, val: &mut u64) -> Result<(), Error> {
+        let mut f = File::open(psm_path!(key))?;
+        let mut vec = Vec::new();
+        let _ = f.read_to_end(&mut vec)?;
+        *val = u64::from_be_bytes(vec.as_slice().try_into()?);
+        Ok(())
+    }
+}

matter/src/sys/sys_linux.rs

@@ -0,0 +1,32 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+use log::error;
+
+#[allow(dead_code)]
+pub struct SysMdnsService {}
+
+pub fn sys_publish_service(
+    _name: &str,
+    _regtype: &str,
+    _port: u16,
+    _txt_kvs: &[[&str; 2]],
+) -> Result<SysMdnsService, Error> {
+    error!("Linux is not yet supported for MDNS Service");
+    Ok(SysMdnsService {})
+}

matter/src/sys/sys_macos.rs

@@ -0,0 +1,46 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::Error;
+use astro_dnssd::{DNSServiceBuilder, RegisteredDnsService};
+use log::info;
+
+#[allow(dead_code)]
+pub struct SysMdnsService {
+    s: RegisteredDnsService,
+}
+
+/// Publish a mDNS service
+/// name - can be a service name (comma separate subtypes may follow)
+/// regtype - registration type (e.g. _matter_.tcp etc)
+/// port - the port
+pub fn sys_publish_service(
+    name: &str,
+    regtype: &str,
+    port: u16,
+    txt_kvs: &[[&str; 2]],
+) -> Result<SysMdnsService, Error> {
+    let mut builder = DNSServiceBuilder::new(regtype, port).with_name(name);
+
+    info!("mDNS Registration Type {}", regtype);
+    for kvs in txt_kvs {
+        info!("mDNS TXT key {} val {}", kvs[0], kvs[1]);
+        builder = builder.with_key_value(kvs[0].to_string(), kvs[1].to_string());
+    }
+    let s = builder.register().map_err(|_| Error::MdnsError)?;
+    Ok(SysMdnsService { s })
+}

matter/src/tlv/mod.rs

@@ -0,0 +1,53 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+/* Tag Types */
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub enum TagType {
+    Anonymous,
+    Context(u8),
+    CommonPrf16(u16),
+    CommonPrf32(u32),
+    ImplPrf16(u16),
+    ImplPrf32(u32),
+    FullQual48(u64),
+    FullQual64(u64),
+}
+pub const TAG_SHIFT_BITS: u8 = 5;
+pub const TAG_MASK: u8 = 0xe0;
+pub const TYPE_MASK: u8 = 0x1f;
+pub const MAX_TAG_INDEX: usize = 8;
+
+pub static TAG_SIZE_MAP: [usize; MAX_TAG_INDEX] = [
+    0, // Anonymous
+    1, // Context
+    2, // CommonPrf16
+    4, // CommonPrf32
+    2, // ImplPrf16
+    4, // ImplPrf32
+    6, // FullQual48
+    8, // FullQual64
+];
+
+mod parser;
+mod traits;
+mod writer;
+
+pub use matter_macro_derive::{FromTLV, ToTLV};
+pub use parser::*;
+pub use traits::*;
+pub use writer::*;

matter/src/tlv/traits.rs

@@ -0,0 +1,506 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::{ElementType, TLVContainerIterator, TLVElement, TLVWriter, TagType};
+use crate::error::Error;
+use core::slice::Iter;
+use log::error;
+
+pub trait FromTLV<'a> {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Self, Error>
+    where
+        Self: Sized;
+
+    // I don't think anybody except Option<T> will define this
+    fn tlv_not_found() -> Result<Self, Error>
+    where
+        Self: Sized,
+    {
+        Err(Error::TLVNotFound)
+    }
+}
+
+impl<'a, T: Default + FromTLV<'a> + Copy, const N: usize> FromTLV<'a> for [T; N] {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Self, Error>
+    where
+        Self: Sized,
+    {
+        t.confirm_array()?;
+        let mut a: [T; N] = [Default::default(); N];
+        let mut index = 0;
+        if let Some(tlv_iter) = t.enter() {
+            for element in tlv_iter {
+                if index < N {
+                    a[index] = T::from_tlv(&element)?;
+                    index += 1;
+                } else {
+                    error!("Received TLV Array with elements larger than current size");
+                    break;
+                }
+            }
+        }
+        Ok(a)
+    }
+}
+
+macro_rules! fromtlv_for {
+    ($($t:ident)*) => {
+        $(
+            impl<'a> FromTLV<'a> for $t {
+                fn from_tlv(t: &TLVElement) -> Result<Self, Error> {
+                    t.$t()
+                }
+            }
+        )*
+    };
+}
+
+fromtlv_for!(u8 u16 u32 u64 bool);
+
+pub trait ToTLV {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error>;
+}
+
+macro_rules! totlv_for {
+    ($($t:ident)*) => {
+        $(
+            impl ToTLV for $t {
+                fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+                    tw.$t(tag, *self)
+                }
+            }
+        )*
+    };
+}
+
+impl<'a, T: ToTLV, const N: usize> ToTLV for [T; N] {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        tw.start_array(tag)?;
+        for i in self {
+            i.to_tlv(tw, TagType::Anonymous)?;
+        }
+        tw.end_container()
+    }
+}
+
+// Generate ToTLV for standard data types
+totlv_for!(i8 u8 u16 u32 u64 bool);
+
+// We define a few common data types that will be required here
+//
+// - UtfStr, OctetStr: These are versions that map to utfstr and ostr in the TLV spec
+//     - These only have references into the original list
+// - String, Vec<u8>: Is the owned version of utfstr and ostr, data is cloned into this
+//     - String is only partially implemented
+//
+// - TLVArray: Is an array of entries, with reference within the original list
+// - TLVArrayOwned: Is the owned version of this, data is cloned into this
+
+/// Implements UTFString from the spec
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub struct UtfStr<'a>(pub &'a [u8]);
+
+impl<'a> UtfStr<'a> {
+    pub fn new(str: &'a [u8]) -> Self {
+        Self(str)
+    }
+}
+
+impl<'a> ToTLV for UtfStr<'a> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        tw.utf16(tag, self.0)
+    }
+}
+
+/// Implements OctetString from the spec
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub struct OctetStr<'a>(pub &'a [u8]);
+
+impl<'a> OctetStr<'a> {
+    pub fn new(str: &'a [u8]) -> Self {
+        Self(str)
+    }
+}
+
+impl<'a> FromTLV<'a> for OctetStr<'a> {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<OctetStr<'a>, Error> {
+        t.slice().map(OctetStr)
+    }
+}
+
+impl<'a> ToTLV for OctetStr<'a> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        tw.str16(tag, self.0)
+    }
+}
+
+/// Implements the Owned version of Octet String
+impl FromTLV<'_> for Vec<u8> {
+    fn from_tlv(t: &TLVElement) -> Result<Vec<u8>, Error> {
+        t.slice().map(|x| x.to_owned())
+    }
+}
+
+impl ToTLV for Vec<u8> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        tw.str16(tag, self.as_slice())
+    }
+}
+
+/// Implements the Owned version of UTF String
+impl FromTLV<'_> for String {
+    fn from_tlv(t: &TLVElement) -> Result<String, Error> {
+        match t.slice() {
+            Ok(x) => {
+                if let Ok(s) = String::from_utf8(x.to_vec()) {
+                    Ok(s)
+                } else {
+                    Err(Error::Invalid)
+                }
+            }
+            Err(e) => Err(e),
+        }
+    }
+}
+
+impl ToTLV for String {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        tw.utf16(tag, self.as_bytes())
+    }
+}
+
+/// Applies to all the Option<> Processing
+impl<'a, T: FromTLV<'a>> FromTLV<'a> for Option<T> {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Option<T>, Error> {
+        Ok(Some(T::from_tlv(t)?))
+    }
+
+    fn tlv_not_found() -> Result<Self, Error>
+    where
+        Self: Sized,
+    {
+        Ok(None)
+    }
+}
+
+impl<T: ToTLV> ToTLV for Option<T> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        match self {
+            Some(s) => (s.to_tlv(tw, tag)),
+            None => Ok(()),
+        }
+    }
+}
+
+/// Represent a nullable value
+///
+/// The value may be null or a valid value
+/// Note: Null is different from Option. If the value is optional, include Option<> too. For
+/// example, Option<Nullable<T>>
+#[derive(Copy, Clone, PartialEq, Debug)]
+pub enum Nullable<T> {
+    Null,
+    NotNull(T),
+}
+
+impl<T> Nullable<T> {
+    pub fn is_null(&self) -> bool {
+        match self {
+            Nullable::Null => true,
+            Nullable::NotNull(_) => false,
+        }
+    }
+
+    pub fn unwrap_notnull(self) -> Option<T> {
+        match self {
+            Nullable::Null => None,
+            Nullable::NotNull(t) => Some(t),
+        }
+    }
+}
+
+impl<'a, T: FromTLV<'a>> FromTLV<'a> for Nullable<T> {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Nullable<T>, Error> {
+        match t.get_element_type() {
+            ElementType::Null => Ok(Nullable::Null),
+            _ => Ok(Nullable::NotNull(T::from_tlv(t)?)),
+        }
+    }
+}
+
+impl<T: ToTLV> ToTLV for Nullable<T> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag: TagType) -> Result<(), Error> {
+        match self {
+            Nullable::Null => tw.null(tag),
+            Nullable::NotNull(s) => s.to_tlv(tw, tag),
+        }
+    }
+}
+
+/// Owned version of a TLVArray
+pub struct TLVArrayOwned<T>(Vec<T>);
+impl<'a, T: FromTLV<'a>> FromTLV<'a> for TLVArrayOwned<T> {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Self, Error> {
+        t.confirm_array()?;
+        let mut vec = Vec::<T>::new();
+        if let Some(tlv_iter) = t.enter() {
+            for element in tlv_iter {
+                vec.push(T::from_tlv(&element)?);
+            }
+        }
+        Ok(Self(vec))
+    }
+}
+
+impl<T: ToTLV> ToTLV for TLVArrayOwned<T> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag_type: TagType) -> Result<(), Error> {
+        tw.start_array(tag_type)?;
+        for t in &self.0 {
+            t.to_tlv(tw, TagType::Anonymous)?;
+        }
+        tw.end_container()
+    }
+}
+
+impl<T> TLVArrayOwned<T> {
+    pub fn iter(&self) -> Iter<T> {
+        self.0.iter()
+    }
+}
+
+pub enum TLVArray<'a, T> {
+    // This is used for the to-tlv path
+    Slice(&'a [T]),
+    // This is used for the from-tlv path
+    Ptr(TLVElement<'a>),
+}
+
+pub enum TLVArrayIter<'a, T> {
+    Slice(Iter<'a, T>),
+    Ptr(Option<TLVContainerIterator<'a>>),
+}
+
+impl<'a, T: ToTLV> TLVArray<'a, T> {
+    pub fn new(slice: &'a [T]) -> Self {
+        Self::Slice(slice)
+    }
+
+    pub fn iter(&self) -> TLVArrayIter<'a, T> {
+        match *self {
+            Self::Slice(s) => TLVArrayIter::Slice(s.iter()),
+            Self::Ptr(p) => TLVArrayIter::Ptr(p.enter()),
+        }
+    }
+}
+
+impl<'a, T: ToTLV + FromTLV<'a> + Copy> TLVArray<'a, T> {
+    pub fn get_index(&self, index: usize) -> T {
+        let mut curr = 0;
+        for element in self.iter() {
+            if curr == index {
+                return element;
+            }
+            curr += 1;
+        }
+        panic!("Out of bounds");
+    }
+}
+
+impl<'a, T: FromTLV<'a> + Copy> Iterator for TLVArrayIter<'a, T> {
+    type Item = T;
+    /* Code for going to the next Element */
+    fn next(&mut self) -> Option<Self::Item> {
+        match self {
+            Self::Slice(s_iter) => s_iter.next().copied(),
+            Self::Ptr(p_iter) => {
+                if let Some(tlv_iter) = p_iter.as_mut() {
+                    let e = tlv_iter.next();
+                    if let Some(element) = e {
+                        T::from_tlv(&element).ok()
+                    } else {
+                        None
+                    }
+                } else {
+                    None
+                }
+            }
+        }
+    }
+}
+
+impl<'a, T: ToTLV> ToTLV for TLVArray<'a, T> {
+    fn to_tlv(&self, tw: &mut TLVWriter, tag_type: TagType) -> Result<(), Error> {
+        match *self {
+            Self::Slice(s) => {
+                tw.start_array(tag_type)?;
+                for a in s {
+                    a.to_tlv(tw, TagType::Anonymous)?;
+                }
+                tw.end_container()
+            }
+            Self::Ptr(_) => Err(Error::Invalid),
+        }
+    }
+}
+
+impl<'a, T> FromTLV<'a> for TLVArray<'a, T> {
+    fn from_tlv(t: &TLVElement<'a>) -> Result<Self, Error> {
+        t.confirm_array()?;
+        Ok(Self::Ptr(*t))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{FromTLV, OctetStr, TLVElement, TLVWriter, TagType, ToTLV};
+    use crate::{error::Error, tlv::TLVList, utils::writebuf::WriteBuf};
+    use matter_macro_derive::{FromTLV, ToTLV};
+
+    #[derive(ToTLV)]
+    struct TestDerive {
+        a: u16,
+        b: u32,
+    }
+    #[test]
+    fn test_derive_totlv() {
+        let mut buf: [u8; 20] = [0; 20];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        let abc = TestDerive {
+            a: 0x1010,
+            b: 0x20202020,
+        };
+        abc.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+        assert_eq!(
+            buf,
+            [21, 37, 0, 0x10, 0x10, 38, 1, 0x20, 0x20, 0x20, 0x20, 24, 0, 0, 0, 0, 0, 0, 0, 0]
+        );
+    }
+
+    #[derive(FromTLV)]
+    struct TestDeriveSimple {
+        a: u16,
+        b: u32,
+    }
+
+    #[test]
+    fn test_derive_fromtlv() {
+        let b = [
+            21, 37, 0, 10, 0, 38, 1, 20, 0, 0, 0, 24, 0, 0, 0, 0, 0, 0, 0, 0,
+        ];
+        let root = TLVList::new(&b).iter().next().unwrap();
+        let test = TestDeriveSimple::from_tlv(&root).unwrap();
+        assert_eq!(test.a, 10);
+        assert_eq!(test.b, 20);
+    }
+
+    #[derive(FromTLV)]
+    #[tlvargs(lifetime = "'a")]
+    struct TestDeriveStr<'a> {
+        a: u16,
+        b: OctetStr<'a>,
+    }
+
+    #[test]
+    fn test_derive_fromtlv_str() {
+        let b = [21, 37, 0, 10, 0, 0x30, 0x01, 0x03, 10, 11, 12, 0];
+        let root = TLVList::new(&b).iter().next().unwrap();
+        let test = TestDeriveStr::from_tlv(&root).unwrap();
+        assert_eq!(test.a, 10);
+        assert_eq!(test.b, OctetStr(&[10, 11, 12]));
+    }
+
+    #[derive(FromTLV, Debug)]
+    struct TestDeriveOption {
+        a: u16,
+        b: Option<u16>,
+        c: Option<u16>,
+    }
+
+    #[test]
+    fn test_derive_fromtlv_option() {
+        let b = [21, 37, 0, 10, 0, 37, 2, 11, 0];
+        let root = TLVList::new(&b).iter().next().unwrap();
+        let test = TestDeriveOption::from_tlv(&root).unwrap();
+        assert_eq!(test.a, 10);
+        assert_eq!(test.b, None);
+        assert_eq!(test.c, Some(11));
+    }
+
+    #[derive(FromTLV, ToTLV, Debug)]
+    struct TestDeriveFabScoped {
+        a: u16,
+        #[tagval(0xFE)]
+        fab_idx: u16,
+    }
+    #[test]
+    fn test_derive_fromtlv_fab_scoped() {
+        let b = [21, 37, 0, 10, 0, 37, 0xFE, 11, 0];
+        let root = TLVList::new(&b).iter().next().unwrap();
+        let test = TestDeriveFabScoped::from_tlv(&root).unwrap();
+        assert_eq!(test.a, 10);
+        assert_eq!(test.fab_idx, 11);
+    }
+
+    #[test]
+    fn test_derive_totlv_fab_scoped() {
+        let mut buf: [u8; 20] = [0; 20];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        let abc = TestDeriveFabScoped { a: 20, fab_idx: 3 };
+
+        abc.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+        assert_eq!(
+            buf,
+            [21, 36, 0, 20, 36, 0xFE, 3, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+        );
+    }
+
+    #[derive(ToTLV, FromTLV, PartialEq, Debug)]
+    enum TestDeriveEnum {
+        ValueA(u32),
+        ValueB(u32),
+    }
+
+    #[test]
+    fn test_derive_from_to_tlv_enum() {
+        // Test FromTLV
+        let b = [21, 36, 0, 100, 24, 0];
+        let root = TLVList::new(&b).iter().next().unwrap();
+        let mut enum_val = TestDeriveEnum::from_tlv(&root).unwrap();
+        assert_eq!(enum_val, TestDeriveEnum::ValueA(100));
+
+        // Modify the value and test ToTLV
+        enum_val = TestDeriveEnum::ValueB(10);
+
+        // Test ToTLV
+        let mut buf: [u8; 20] = [0; 20];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        enum_val.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+        assert_eq!(
+            buf,
+            [21, 36, 1, 10, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+        );
+    }
+}

matter/src/tlv/writer.rs

@@ -0,0 +1,322 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use super::{TagType, TAG_SHIFT_BITS, TAG_SIZE_MAP};
+use crate::{error::*, utils::writebuf::WriteBuf};
+use log::error;
+
+#[allow(dead_code)]
+enum WriteElementType {
+    S8 = 0,
+    S16 = 1,
+    S32 = 2,
+    S64 = 3,
+    U8 = 4,
+    U16 = 5,
+    U32 = 6,
+    U64 = 7,
+    False = 8,
+    True = 9,
+    F32 = 10,
+    F64 = 11,
+    Utf8l = 12,
+    Utf16l = 13,
+    Utf32l = 14,
+    Utf64l = 15,
+    Str8l = 16,
+    Str16l = 17,
+    Str32l = 18,
+    Str64l = 19,
+    Null = 20,
+    Struct = 21,
+    Array = 22,
+    List = 23,
+    EndCnt = 24,
+    Last,
+}
+
+pub struct TLVWriter<'a, 'b> {
+    buf: &'b mut WriteBuf<'a>,
+}
+
+impl<'a, 'b> TLVWriter<'a, 'b> {
+    pub fn new(buf: &'b mut WriteBuf<'a>) -> Self {
+        TLVWriter { buf }
+    }
+
+    // TODO: The current method of using writebuf's put methods force us to do
+    // at max 3 checks while writing a single TLV (once for control, once for tag,
+    // once for value), so do a single check and write the whole thing.
+    #[inline(always)]
+    fn put_control_tag(
+        &mut self,
+        tag_type: TagType,
+        val_type: WriteElementType,
+    ) -> Result<(), Error> {
+        let (tag_id, tag_val) = match tag_type {
+            TagType::Anonymous => (0_u8, 0),
+            TagType::Context(v) => (1, v as u64),
+            TagType::CommonPrf16(v) => (2, v as u64),
+            TagType::CommonPrf32(v) => (3, v as u64),
+            TagType::ImplPrf16(v) => (4, v as u64),
+            TagType::ImplPrf32(v) => (5, v as u64),
+            TagType::FullQual48(v) => (6, v as u64),
+            TagType::FullQual64(v) => (7, v as u64),
+        };
+        self.buf
+            .le_u8(((tag_id) << TAG_SHIFT_BITS) | (val_type as u8))?;
+        if tag_type != TagType::Anonymous {
+            self.buf.le_uint(TAG_SIZE_MAP[tag_id as usize], tag_val)?;
+        }
+        Ok(())
+    }
+
+    pub fn i8(&mut self, tag_type: TagType, data: i8) -> Result<(), Error> {
+        self.put_control_tag(tag_type, WriteElementType::S8)?;
+        self.buf.le_i8(data)
+    }
+
+    pub fn u8(&mut self, tag_type: TagType, data: u8) -> Result<(), Error> {
+        self.put_control_tag(tag_type, WriteElementType::U8)?;
+        self.buf.le_u8(data)
+    }
+
+    pub fn u16(&mut self, tag_type: TagType, data: u16) -> Result<(), Error> {
+        if data <= 0xff {
+            self.u8(tag_type, data as u8)
+        } else {
+            self.put_control_tag(tag_type, WriteElementType::U16)?;
+            self.buf.le_u16(data)
+        }
+    }
+
+    pub fn u32(&mut self, tag_type: TagType, data: u32) -> Result<(), Error> {
+        if data <= 0xff {
+            self.u8(tag_type, data as u8)
+        } else if data <= 0xffff {
+            self.u16(tag_type, data as u16)
+        } else {
+            self.put_control_tag(tag_type, WriteElementType::U32)?;
+            self.buf.le_u32(data)
+        }
+    }
+
+    pub fn u64(&mut self, tag_type: TagType, data: u64) -> Result<(), Error> {
+        if data <= 0xff {
+            self.u8(tag_type, data as u8)
+        } else if data <= 0xffff {
+            self.u16(tag_type, data as u16)
+        } else if data <= 0xffffffff {
+            self.u32(tag_type, data as u32)
+        } else {
+            self.put_control_tag(tag_type, WriteElementType::U64)?;
+            self.buf.le_u64(data)
+        }
+    }
+
+    pub fn str8(&mut self, tag_type: TagType, data: &[u8]) -> Result<(), Error> {
+        if data.len() > 256 {
+            error!("use put_str16() instead");
+            return Err(Error::Invalid);
+        }
+        self.put_control_tag(tag_type, WriteElementType::Str8l)?;
+        self.buf.le_u8(data.len() as u8)?;
+        self.buf.copy_from_slice(data)
+    }
+
+    pub fn str16(&mut self, tag_type: TagType, data: &[u8]) -> Result<(), Error> {
+        if data.len() <= 0xff {
+            self.str8(tag_type, data)
+        } else {
+            self.put_control_tag(tag_type, WriteElementType::Str16l)?;
+            self.buf.le_u16(data.len() as u16)?;
+            self.buf.copy_from_slice(data)
+        }
+    }
+
+    // This is quite hacky
+    pub fn str16_as<F>(&mut self, tag_type: TagType, data_gen: F) -> Result<(), Error>
+    where
+        F: FnOnce(&mut [u8]) -> Result<usize, Error>,
+    {
+        let anchor = self.buf.get_tail();
+        self.put_control_tag(tag_type, WriteElementType::Str16l)?;
+
+        let wb = self.buf.empty_as_mut_slice();
+        // Reserve 2 spaces for the control and length
+        let str = &mut wb[2..];
+        let len = data_gen(str).unwrap_or_default();
+        if len <= 0xff {
+            // Shift everything by 1
+            let str = &mut wb[1..];
+            for i in 0..len {
+                str[i] = str[i + 1];
+            }
+            self.buf.rewind_tail_to(anchor);
+            self.put_control_tag(tag_type, WriteElementType::Str8l)?;
+            self.buf.le_u8(len as u8)?;
+        } else {
+            self.buf.le_u16(len as u16)?;
+        }
+        self.buf.forward_tail_by(len);
+        Ok(())
+    }
+
+    pub fn utf8(&mut self, tag_type: TagType, data: &[u8]) -> Result<(), Error> {
+        self.put_control_tag(tag_type, WriteElementType::Utf8l)?;
+        self.buf.le_u8(data.len() as u8)?;
+        self.buf.copy_from_slice(data)
+    }
+
+    pub fn utf16(&mut self, tag_type: TagType, data: &[u8]) -> Result<(), Error> {
+        if data.len() <= 0xff {
+            self.utf8(tag_type, data)
+        } else {
+            self.put_control_tag(tag_type, WriteElementType::Utf16l)?;
+            self.buf.le_u16(data.len() as u16)?;
+            self.buf.copy_from_slice(data)
+        }
+    }
+
+    fn no_val(&mut self, tag_type: TagType, element: WriteElementType) -> Result<(), Error> {
+        self.put_control_tag(tag_type, element)
+    }
+
+    pub fn start_struct(&mut self, tag_type: TagType) -> Result<(), Error> {
+        self.no_val(tag_type, WriteElementType::Struct)
+    }
+
+    pub fn start_array(&mut self, tag_type: TagType) -> Result<(), Error> {
+        self.no_val(tag_type, WriteElementType::Array)
+    }
+
+    pub fn start_list(&mut self, tag_type: TagType) -> Result<(), Error> {
+        self.no_val(tag_type, WriteElementType::List)
+    }
+
+    pub fn end_container(&mut self) -> Result<(), Error> {
+        self.no_val(TagType::Anonymous, WriteElementType::EndCnt)
+    }
+
+    pub fn null(&mut self, tag_type: TagType) -> Result<(), Error> {
+        self.no_val(tag_type, WriteElementType::Null)
+    }
+
+    pub fn bool(&mut self, tag_type: TagType, val: bool) -> Result<(), Error> {
+        if val {
+            self.no_val(tag_type, WriteElementType::True)
+        } else {
+            self.no_val(tag_type, WriteElementType::False)
+        }
+    }
+
+    pub fn get_tail(&self) -> usize {
+        self.buf.get_tail()
+    }
+
+    pub fn rewind_to(&mut self, anchor: usize) {
+        self.buf.rewind_tail_to(anchor);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{TLVWriter, TagType};
+    use crate::utils::writebuf::WriteBuf;
+
+    #[test]
+    fn test_write_success() {
+        let mut buf: [u8; 20] = [0; 20];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        tw.start_struct(TagType::Anonymous).unwrap();
+        tw.u8(TagType::Anonymous, 12).unwrap();
+        tw.u8(TagType::Context(1), 13).unwrap();
+        tw.u16(TagType::Anonymous, 0x1212).unwrap();
+        tw.u16(TagType::Context(2), 0x1313).unwrap();
+        tw.start_array(TagType::Context(3)).unwrap();
+        tw.bool(TagType::Anonymous, true).unwrap();
+        tw.end_container().unwrap();
+        tw.end_container().unwrap();
+        assert_eq!(
+            buf,
+            [21, 4, 12, 36, 1, 13, 5, 0x12, 0x012, 37, 2, 0x13, 0x13, 54, 3, 9, 24, 24, 0, 0]
+        );
+    }
+
+    #[test]
+    fn test_write_overflow() {
+        let mut buf: [u8; 6] = [0; 6];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        tw.u8(TagType::Anonymous, 12).unwrap();
+        tw.u8(TagType::Context(1), 13).unwrap();
+        match tw.u16(TagType::Anonymous, 12) {
+            Ok(_) => panic!("This should have returned error"),
+            _ => (),
+        }
+        match tw.u16(TagType::Context(2), 13) {
+            Ok(_) => panic!("This should have returned error"),
+            _ => (),
+        }
+        assert_eq!(buf, [4, 12, 36, 1, 13, 4]);
+    }
+
+    #[test]
+    fn test_put_str8() {
+        let mut buf: [u8; 20] = [0; 20];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        tw.u8(TagType::Context(1), 13).unwrap();
+        tw.str8(TagType::Anonymous, &[10, 11, 12, 13, 14]).unwrap();
+        tw.u16(TagType::Context(2), 0x1313).unwrap();
+        tw.str8(TagType::Context(3), &[20, 21, 22]).unwrap();
+        assert_eq!(
+            buf,
+            [36, 1, 13, 16, 5, 10, 11, 12, 13, 14, 37, 2, 0x13, 0x13, 48, 3, 3, 20, 21, 22]
+        );
+    }
+
+    #[test]
+    fn test_put_str16_as() {
+        let mut buf: [u8; 20] = [0; 20];
+        let buf_len = buf.len();
+        let mut writebuf = WriteBuf::new(&mut buf, buf_len);
+        let mut tw = TLVWriter::new(&mut writebuf);
+
+        tw.u8(TagType::Context(1), 13).unwrap();
+        tw.str8(TagType::Context(2), &[10, 11, 12, 13, 14]).unwrap();
+        tw.str16_as(TagType::Context(3), |buf| {
+            buf[0] = 10;
+            buf[1] = 11;
+            Ok(2)
+        })
+        .unwrap();
+        tw.u8(TagType::Context(4), 13).unwrap();
+
+        assert_eq!(
+            buf,
+            [36, 1, 13, 48, 2, 5, 10, 11, 12, 13, 14, 48, 3, 2, 10, 11, 36, 4, 13, 0]
+        );
+    }
+}

matter/src/transport/exchange.rs

@@ -0,0 +1,546 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use boxslab::{BoxSlab, Slab};
+use colored::*;
+use log::{error, info, trace};
+use std::any::Any;
+use std::fmt;
+
+use crate::error::Error;
+use crate::secure_channel;
+
+use heapless::LinearMap;
+
+use super::packet::PacketPool;
+use super::session::CloneData;
+use super::{mrp::ReliableMessage, packet::Packet, session::SessionHandle, session::SessionMgr};
+
+pub struct ExchangeCtx<'a> {
+    pub exch: &'a mut Exchange,
+    pub sess: SessionHandle<'a>,
+}
+
+#[derive(Debug, PartialEq, Eq, Copy, Clone)]
+pub enum Role {
+    Initiator = 0,
+    Responder = 1,
+}
+
+impl Default for Role {
+    fn default() -> Self {
+        Role::Initiator
+    }
+}
+
+#[derive(Debug, PartialEq)]
+enum State {
+    Open,
+    Close,
+}
+
+impl Default for State {
+    fn default() -> Self {
+        State::Open
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct Exchange {
+    id: u16,
+    sess_idx: usize,
+    role: Role,
+    state: State,
+    // Currently I see this primarily used in PASE and CASE. If that is the limited use
+    // of this, we might move this into a separate data structure, so as not to burden
+    // all 'exchanges'.
+    data: Option<Box<dyn Any>>,
+    mrp: ReliableMessage,
+}
+
+impl Exchange {
+    pub fn new(id: u16, sess_idx: usize, role: Role) -> Exchange {
+        Exchange {
+            id,
+            sess_idx,
+            role,
+            state: State::Open,
+            data: None,
+            mrp: ReliableMessage::new(),
+        }
+    }
+
+    pub fn close(&mut self) {
+        self.data = None;
+        self.state = State::Close;
+    }
+
+    pub fn is_state_open(&self) -> bool {
+        self.state == State::Open
+    }
+
+    pub fn is_purgeable(&self) -> bool {
+        // No Users, No pending ACKs/Retrans
+        self.state == State::Close && self.mrp.is_empty()
+    }
+
+    pub fn get_id(&self) -> u16 {
+        self.id
+    }
+
+    pub fn get_role(&self) -> Role {
+        self.role
+    }
+
+    pub fn set_exchange_data(&mut self, data: Box<dyn Any>) {
+        self.data = Some(data);
+    }
+
+    pub fn clear_exchange_data(&mut self) {
+        self.data = None;
+    }
+
+    pub fn get_exchange_data<T: Any>(&mut self) -> Option<&mut T> {
+        self.data.as_mut()?.downcast_mut::<T>()
+    }
+
+    pub fn take_exchange_data<T: Any>(&mut self) -> Option<Box<T>> {
+        self.data.take()?.downcast::<T>().ok()
+    }
+
+    fn send(
+        &mut self,
+        mut proto_tx: BoxSlab<PacketPool>,
+        session: &mut SessionHandle,
+    ) -> Result<(), Error> {
+        trace!("payload: {:x?}", proto_tx.as_borrow_slice());
+        info!(
+            "{} with proto id: {} opcode: {}",
+            "Sending".blue(),
+            proto_tx.get_proto_id(),
+            proto_tx.get_proto_opcode(),
+        );
+
+        proto_tx.proto.exch_id = self.id;
+        if self.role == Role::Initiator {
+            proto_tx.proto.set_initiator();
+        }
+
+        session.pre_send(&mut proto_tx)?;
+        self.mrp.pre_send(&mut proto_tx)?;
+        session.send(proto_tx)
+    }
+}
+
+impl fmt::Display for Exchange {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(
+            f,
+            "exch_id: {:?}, sess_index: {}, role: {:?}, data: {:?}, mrp: {:?}, state: {:?}",
+            self.id, self.sess_idx, self.role, self.data, self.mrp, self.state
+        )
+    }
+}
+
+pub fn get_role(is_initiator: bool) -> Role {
+    if is_initiator {
+        Role::Initiator
+    } else {
+        Role::Responder
+    }
+}
+
+pub fn get_complementary_role(is_initiator: bool) -> Role {
+    if is_initiator {
+        Role::Responder
+    } else {
+        Role::Initiator
+    }
+}
+
+const MAX_EXCHANGES: usize = 8;
+
+#[derive(Default)]
+pub struct ExchangeMgr {
+    // keys: exch-id
+    exchanges: LinearMap<u16, Exchange, MAX_EXCHANGES>,
+    sess_mgr: SessionMgr,
+}
+
+pub const MAX_MRP_ENTRIES: usize = 4;
+
+impl ExchangeMgr {
+    pub fn new(sess_mgr: SessionMgr) -> Self {
+        Self {
+            sess_mgr,
+            exchanges: Default::default(),
+        }
+    }
+
+    pub fn get_sess_mgr(&mut self) -> &mut SessionMgr {
+        &mut self.sess_mgr
+    }
+
+    pub fn _get_with_id(
+        exchanges: &mut LinearMap<u16, Exchange, MAX_EXCHANGES>,
+        exch_id: u16,
+    ) -> Option<&mut Exchange> {
+        exchanges.get_mut(&exch_id)
+    }
+
+    pub fn get_with_id(&mut self, exch_id: u16) -> Option<&mut Exchange> {
+        ExchangeMgr::_get_with_id(&mut self.exchanges, exch_id)
+    }
+
+    fn _get(
+        exchanges: &mut LinearMap<u16, Exchange, MAX_EXCHANGES>,
+        sess_idx: usize,
+        id: u16,
+        role: Role,
+        create_new: bool,
+    ) -> Result<&mut Exchange, Error> {
+        // I don't prefer that we scan the list twice here (once for contains_key and other)
+        if !exchanges.contains_key(&(id)) {
+            if create_new {
+                // If an exchange doesn't exist, create a new one
+                info!("Creating new exchange");
+                let e = Exchange::new(id, sess_idx, role);
+                if exchanges.insert(id, e).is_err() {
+                    return Err(Error::NoSpace);
+                }
+            } else {
+                return Err(Error::NoSpace);
+            }
+        }
+
+        // At this point, we would either have inserted the record if 'create_new' was set
+        // or it existed already
+        if let Some(result) = exchanges.get_mut(&id) {
+            if result.get_role() == role && sess_idx == result.sess_idx {
+                Ok(result)
+            } else {
+                Err(Error::NoExchange)
+            }
+        } else {
+            error!("This should never happen");
+            Err(Error::NoSpace)
+        }
+    }
+
+    /// The Exchange Mgr receive is like a big processing function
+    pub fn recv(&mut self) -> Result<Option<(BoxSlab<PacketPool>, ExchangeCtx)>, Error> {
+        // Get the session
+        let (mut proto_rx, index) = self.sess_mgr.recv()?;
+
+        let index = match index {
+            Some(s) => s,
+            None => {
+                // The sessions were full, evict one session, and re-perform post-recv
+                let evict_index = self.sess_mgr.get_lru();
+                self.evict_session(evict_index)?;
+                info!("Reattempting session creation");
+                self.sess_mgr.post_recv(&proto_rx)?.ok_or(Error::Invalid)?
+            }
+        };
+        let mut session = self.sess_mgr.get_session_handle(index);
+
+        // Decrypt the message
+        session.recv(&mut proto_rx)?;
+
+        // Get the exchange
+        let exch = ExchangeMgr::_get(
+            &mut self.exchanges,
+            index,
+            proto_rx.proto.exch_id,
+            get_complementary_role(proto_rx.proto.is_initiator()),
+            // We create a new exchange, only if the peer is the initiator
+            proto_rx.proto.is_initiator(),
+        )?;
+
+        // Message Reliability Protocol
+        exch.mrp.recv(&proto_rx)?;
+
+        if exch.is_state_open() {
+            Ok(Some((
+                proto_rx,
+                ExchangeCtx {
+                    exch,
+                    sess: session,
+                },
+            )))
+        } else {
+            // Instead of an error, we send None here, because it is likely that
+            // we just processed an acknowledgement that cleared the exchange
+            Ok(None)
+        }
+    }
+
+    pub fn send(&mut self, exch_id: u16, proto_tx: BoxSlab<PacketPool>) -> Result<(), Error> {
+        let exchange =
+            ExchangeMgr::_get_with_id(&mut self.exchanges, exch_id).ok_or(Error::NoExchange)?;
+        let mut session = self.sess_mgr.get_session_handle(exchange.sess_idx);
+        exchange.send(proto_tx, &mut session)
+    }
+
+    pub fn purge(&mut self) {
+        let mut to_purge: LinearMap<u16, (), MAX_EXCHANGES> = LinearMap::new();
+
+        for (exch_id, exchange) in self.exchanges.iter() {
+            if exchange.is_purgeable() {
+                let _ = to_purge.insert(*exch_id, ());
+            }
+        }
+        for (exch_id, _) in to_purge.iter() {
+            self.exchanges.remove(&*exch_id);
+        }
+    }
+
+    pub fn pending_acks(&mut self, expired_entries: &mut LinearMap<u16, (), MAX_MRP_ENTRIES>) {
+        for (exch_id, exchange) in self.exchanges.iter() {
+            if exchange.mrp.is_ack_ready() {
+                expired_entries.insert(*exch_id, ()).unwrap();
+            }
+        }
+    }
+
+    pub fn evict_session(&mut self, index: usize) -> Result<(), Error> {
+        info!("Sessions full, vacating session with index: {}", index);
+        // If we enter here, we have an LRU session that needs to be reclaimed
+        // As per the spec, we need to send a CLOSE here
+
+        let mut session = self.sess_mgr.get_session_handle(index);
+        let mut tx = Slab::<PacketPool>::new(Packet::new_tx()?).ok_or(Error::NoSpace)?;
+        secure_channel::common::create_sc_status_report(
+            &mut tx,
+            secure_channel::common::SCStatusCodes::CloseSession,
+            None,
+        )?;
+
+        if let Some((_, exchange)) = self.exchanges.iter_mut().find(|(_, e)| e.sess_idx == index) {
+            // Send Close_session on this exchange, and then close the session
+            // Should this be done for all exchanges?
+            error!("Sending Close Session");
+            exchange.send(tx, &mut session)?;
+            // TODO: This wouldn't actually send it out, because 'transport' isn't owned yet.
+        }
+
+        let remove_exchanges: Vec<u16> = self
+            .exchanges
+            .iter()
+            .filter_map(|(eid, e)| {
+                if e.sess_idx == index {
+                    Some(*eid)
+                } else {
+                    None
+                }
+            })
+            .collect();
+        info!(
+            "Terminating the following exchanges: {:?}",
+            remove_exchanges
+        );
+        for exch_id in remove_exchanges {
+            // Remove from exchange list
+            self.exchanges.remove(&exch_id);
+        }
+        self.sess_mgr.remove(index);
+        Ok(())
+    }
+
+    pub fn add_session(&mut self, clone_data: CloneData) -> Result<SessionHandle, Error> {
+        let sess_idx = match self.sess_mgr.clone_session(&clone_data) {
+            Ok(idx) => idx,
+            Err(Error::NoSpace) => {
+                let evict_index = self.sess_mgr.get_lru();
+                self.evict_session(evict_index)?;
+                self.sess_mgr.clone_session(&clone_data)?
+            }
+            Err(e) => {
+                return Err(e);
+            }
+        };
+        Ok(self.sess_mgr.get_session_handle(sess_idx))
+    }
+}
+
+impl fmt::Display for ExchangeMgr {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        writeln!(f, "{{  Session Mgr: {},", self.sess_mgr)?;
+        writeln!(f, "  Exchanges: [")?;
+        for s in &self.exchanges {
+            writeln!(f, "{{ {}, }},", s.1)?;
+        }
+        writeln!(f, "  ]")?;
+        write!(f, "}}")
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use crate::{
+        error::Error,
+        transport::{
+            network::{Address, NetworkInterface},
+            session::{CloneData, SessionMgr, SessionMode, MAX_SESSIONS},
+        },
+    };
+
+    use super::{ExchangeMgr, Role};
+
+    #[test]
+    fn test_purge() {
+        let sess_mgr = SessionMgr::new();
+        let mut mgr = ExchangeMgr::new(sess_mgr);
+        let _ = ExchangeMgr::_get(&mut mgr.exchanges, 1, 2, Role::Responder, true).unwrap();
+        let _ = ExchangeMgr::_get(&mut mgr.exchanges, 1, 3, Role::Responder, true).unwrap();
+
+        mgr.purge();
+        assert_eq!(
+            ExchangeMgr::_get(&mut mgr.exchanges, 1, 2, Role::Responder, false).is_ok(),
+            true
+        );
+        assert_eq!(
+            ExchangeMgr::_get(&mut mgr.exchanges, 1, 3, Role::Responder, false).is_ok(),
+            true
+        );
+
+        // Close e1
+        let e1 = ExchangeMgr::_get(&mut mgr.exchanges, 1, 2, Role::Responder, false).unwrap();
+        e1.close();
+        mgr.purge();
+        assert_eq!(
+            ExchangeMgr::_get(&mut mgr.exchanges, 1, 2, Role::Responder, false).is_ok(),
+            false
+        );
+        assert_eq!(
+            ExchangeMgr::_get(&mut mgr.exchanges, 1, 3, Role::Responder, false).is_ok(),
+            true
+        );
+    }
+
+    fn get_clone_data(peer_sess_id: u16, local_sess_id: u16) -> CloneData {
+        CloneData::new(
+            12341234,
+            43211234,
+            peer_sess_id,
+            local_sess_id,
+            Address::default(),
+            SessionMode::Pase,
+        )
+    }
+
+    fn fill_sessions(mgr: &mut ExchangeMgr, count: usize) {
+        let mut local_sess_id = 1;
+        let mut peer_sess_id = 100;
+        for _ in 1..count {
+            let clone_data = get_clone_data(peer_sess_id, local_sess_id);
+            match mgr.add_session(clone_data) {
+                Ok(s) => (assert_eq!(peer_sess_id, s.get_peer_sess_id())),
+                Err(Error::NoSpace) => break,
+                _ => {
+                    panic!("Couldn't, create session");
+                }
+            }
+            local_sess_id += 1;
+            peer_sess_id += 1;
+        }
+    }
+
+    pub struct DummyNetwork;
+    impl DummyNetwork {
+        pub fn new() -> Self {
+            Self {}
+        }
+    }
+
+    impl NetworkInterface for DummyNetwork {
+        fn recv(&self, _in_buf: &mut [u8]) -> Result<(usize, Address), Error> {
+            Ok((0, Address::default()))
+        }
+
+        fn send(&self, _out_buf: &[u8], _addr: Address) -> Result<usize, Error> {
+            Ok(0)
+        }
+    }
+
+    #[test]
+    /// We purposefuly overflow the sessions
+    /// and when the overflow happens, we confirm that
+    /// - The sessions are evicted in LRU
+    /// - The exchanges associated with those sessions are evicted too
+    fn test_sess_evict() {
+        let mut sess_mgr = SessionMgr::new();
+        let transport = Box::new(DummyNetwork::new());
+        sess_mgr.add_network_interface(transport).unwrap();
+        let mut mgr = ExchangeMgr::new(sess_mgr);
+
+        fill_sessions(&mut mgr, MAX_SESSIONS + 1);
+        // Sessions are now full from local session id 1 to 16
+
+        // Create exchanges for sessions 2 (i.e. session index 1) and 3 (session index 2)
+        //   Exchange IDs are 20 and 30 respectively
+        let _ = ExchangeMgr::_get(&mut mgr.exchanges, 1, 20, Role::Responder, true).unwrap();
+        let _ = ExchangeMgr::_get(&mut mgr.exchanges, 2, 30, Role::Responder, true).unwrap();
+
+        // Confirm that session ids 1 to MAX_SESSIONS exists
+        for i in 1..(MAX_SESSIONS + 1) {
+            assert_eq!(mgr.sess_mgr.get_with_id(i as u16).is_none(), false);
+        }
+        // Confirm that the exchanges are around
+        assert_eq!(mgr.get_with_id(20).is_none(), false);
+        assert_eq!(mgr.get_with_id(30).is_none(), false);
+        let mut old_local_sess_id = 1;
+        let mut new_local_sess_id = 100;
+        let mut new_peer_sess_id = 200;
+
+        for i in 1..(MAX_SESSIONS + 1) {
+            // Now purposefully overflow the sessions by adding another session
+            let session = mgr
+                .add_session(get_clone_data(new_peer_sess_id, new_local_sess_id))
+                .unwrap();
+            assert_eq!(session.get_peer_sess_id(), new_peer_sess_id);
+
+            // This should have evicted session with local sess_id
+            assert_eq!(mgr.sess_mgr.get_with_id(old_local_sess_id).is_none(), true);
+
+            new_local_sess_id += 1;
+            new_peer_sess_id += 1;
+            old_local_sess_id += 1;
+
+            match i {
+                1 => {
+                    // Both exchanges should exist
+                    assert_eq!(mgr.get_with_id(20).is_none(), false);
+                    assert_eq!(mgr.get_with_id(30).is_none(), false);
+                }
+                2 => {
+                    // Exchange 20 would have been evicted
+                    assert_eq!(mgr.get_with_id(20).is_none(), true);
+                    assert_eq!(mgr.get_with_id(30).is_none(), false);
+                }
+                3 => {
+                    // Exchange 20 and 30 would have been evicted
+                    assert_eq!(mgr.get_with_id(20).is_none(), true);
+                    assert_eq!(mgr.get_with_id(30).is_none(), true);
+                }
+                _ => {}
+            }
+        }
+        //        println!("Session mgr {}", mgr.sess_mgr);
+    }
+}

matter/src/transport/mgr.rs

@@ -0,0 +1,175 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use async_channel::Receiver;
+use boxslab::{BoxSlab, Slab};
+use heapless::LinearMap;
+use log::{debug, error, info};
+
+use crate::error::*;
+
+use crate::transport::mrp::ReliableMessage;
+use crate::transport::packet::PacketPool;
+use crate::transport::{exchange, packet::Packet, proto_demux, queue, session, udp};
+
+use super::proto_demux::ProtoCtx;
+use super::queue::Msg;
+
+pub struct Mgr {
+    exch_mgr: exchange::ExchangeMgr,
+    proto_demux: proto_demux::ProtoDemux,
+    rx_q: Receiver<Msg>,
+}
+
+impl Mgr {
+    pub fn new() -> Result<Mgr, Error> {
+        let mut sess_mgr = session::SessionMgr::new();
+        let udp_transport = Box::new(udp::UdpListener::new()?);
+        sess_mgr.add_network_interface(udp_transport)?;
+        Ok(Mgr {
+            proto_demux: proto_demux::ProtoDemux::new(),
+            exch_mgr: exchange::ExchangeMgr::new(sess_mgr),
+            rx_q: queue::WorkQ::init()?,
+        })
+    }
+
+    // Allows registration of different protocols with the Transport/Protocol Demux
+    pub fn register_protocol(
+        &mut self,
+        proto_id_handle: Box<dyn proto_demux::HandleProto>,
+    ) -> Result<(), Error> {
+        self.proto_demux.register(proto_id_handle)
+    }
+
+    fn send_to_exchange(
+        &mut self,
+        exch_id: u16,
+        proto_tx: BoxSlab<PacketPool>,
+    ) -> Result<(), Error> {
+        self.exch_mgr.send(exch_id, proto_tx)
+    }
+
+    fn handle_rxtx(&mut self) -> Result<(), Error> {
+        let result = self.exch_mgr.recv().map_err(|e| {
+            error!("Error in recv: {:?}", e);
+            e
+        })?;
+
+        if result.is_none() {
+            // Nothing to process, return quietly
+            return Ok(());
+        }
+        // result contains something worth processing, we can safely unwrap
+        // as we already checked for none above
+        let (rx, exch_ctx) = result.unwrap();
+
+        debug!("Exchange is {:?}", exch_ctx.exch);
+        let tx = Self::new_tx()?;
+
+        let mut proto_ctx = ProtoCtx::new(exch_ctx, rx, tx);
+        // Proto Dispatch
+        match self.proto_demux.handle(&mut proto_ctx) {
+            Ok(r) => {
+                if let proto_demux::ResponseRequired::No = r {
+                    // We need to send the Ack if reliability is enabled, in this case
+                    return Ok(());
+                }
+            }
+            Err(e) => {
+                error!("Error in proto_demux {:?}", e);
+                return Err(e);
+            }
+        }
+
+        let ProtoCtx {
+            exch_ctx,
+            rx: _,
+            tx,
+        } = proto_ctx;
+
+        // tx_ctx now contains the response payload, send the packet
+        let exch_id = exch_ctx.exch.get_id();
+        self.send_to_exchange(exch_id, tx).map_err(|e| {
+            error!("Error in sending msg {:?}", e);
+            e
+        })?;
+
+        Ok(())
+    }
+
+    fn handle_queue_msgs(&mut self) -> Result<(), Error> {
+        if let Ok(msg) = self.rx_q.try_recv() {
+            match msg {
+                Msg::NewSession(clone_data) => {
+                    // If a new session was created, add it
+                    let _ = self
+                        .exch_mgr
+                        .add_session(clone_data)
+                        .map_err(|e| error!("Error adding new session {:?}", e));
+                }
+                _ => {
+                    error!("Queue Message Type not yet handled {:?}", msg);
+                }
+            }
+        }
+        Ok(())
+    }
+
+    pub fn start(&mut self) -> Result<(), Error> {
+        loop {
+            // Handle network operations
+            if self.handle_rxtx().is_err() {
+                error!("Error in handle_rxtx");
+                continue;
+            }
+
+            if self.handle_queue_msgs().is_err() {
+                error!("Error in handle_queue_msg");
+                continue;
+            }
+
+            // Handle any pending acknowledgement send
+            let mut acks_to_send: LinearMap<u16, (), { exchange::MAX_MRP_ENTRIES }> =
+                LinearMap::new();
+            self.exch_mgr.pending_acks(&mut acks_to_send);
+            for exch_id in acks_to_send.keys() {
+                info!("Sending MRP Standalone ACK for  exch {}", exch_id);
+                let mut proto_tx = match Self::new_tx() {
+                    Ok(p) => p,
+                    Err(e) => {
+                        error!("Error creating proto_tx {:?}", e);
+                        break;
+                    }
+                };
+                ReliableMessage::prepare_ack(*exch_id, &mut proto_tx);
+                if let Err(e) = self.send_to_exchange(*exch_id, proto_tx) {
+                    error!("Error in sending Ack {:?}", e);
+                }
+            }
+
+            // Handle exchange purging
+            //    This need not be done in each turn of the loop, maybe once in 5 times or so?
+            self.exch_mgr.purge();
+
+            info!("Exchange Mgr: {}", self.exch_mgr);
+        }
+    }
+
+    fn new_tx() -> Result<BoxSlab<PacketPool>, Error> {
+        Slab::<PacketPool>::new(Packet::new_tx()?).ok_or(Error::PacketPoolExhaust)
+    }
+}

matter/src/transport/mod.rs

@@ -0,0 +1,29 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+
+pub mod exchange;
+pub mod mgr;
+pub mod mrp;
+pub mod network;
+pub mod packet;
+pub mod plain_hdr;
+pub mod proto_demux;
+pub mod proto_hdr;
+pub mod queue;
+pub mod session;
+pub mod udp;

matter/src/transport/mrp.rs

@@ -0,0 +1,160 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::time::Duration;
+use std::time::SystemTime;
+
+use crate::{error::*, secure_channel, transport::packet::Packet};
+use log::error;
+
+// 200 ms
+const MRP_STANDALONE_ACK_TIMEOUT: u64 = 200;
+
+#[derive(Debug)]
+pub struct RetransEntry {
+    // The msg counter that we are waiting to be acknowledged
+    msg_ctr: u32,
+    // This will additionally have retransmission count and periods once we implement it
+}
+
+impl RetransEntry {
+    pub fn new(msg_ctr: u32) -> Self {
+        Self { msg_ctr }
+    }
+
+    pub fn get_msg_ctr(&self) -> u32 {
+        self.msg_ctr
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct AckEntry {
+    // The msg counter that we should acknowledge
+    msg_ctr: u32,
+    // The max time after which this entry must be ACK
+    ack_timeout: SystemTime,
+}
+
+impl AckEntry {
+    pub fn new(msg_ctr: u32) -> Result<Self, Error> {
+        if let Some(ack_timeout) =
+            SystemTime::now().checked_add(Duration::from_millis(MRP_STANDALONE_ACK_TIMEOUT))
+        {
+            Ok(Self {
+                msg_ctr,
+                ack_timeout,
+            })
+        } else {
+            Err(Error::Invalid)
+        }
+    }
+
+    pub fn get_msg_ctr(&self) -> u32 {
+        self.msg_ctr
+    }
+
+    pub fn has_timed_out(&self) -> bool {
+        self.ack_timeout > SystemTime::now()
+    }
+}
+
+#[derive(Default, Debug)]
+pub struct ReliableMessage {
+    retrans: Option<RetransEntry>,
+    ack: Option<AckEntry>,
+}
+
+impl ReliableMessage {
+    pub fn new() -> Self {
+        Self {
+            ..Default::default()
+        }
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.retrans.is_none() && self.ack.is_none()
+    }
+
+    // Check any pending acknowledgements / retransmissions and take action
+    pub fn is_ack_ready(&self) -> bool {
+        // Acknowledgements
+        if let Some(ack_entry) = self.ack {
+            ack_entry.has_timed_out()
+        } else {
+            false
+        }
+    }
+
+    pub fn prepare_ack(_exch_id: u16, proto_tx: &mut Packet) {
+        secure_channel::common::create_mrp_standalone_ack(proto_tx);
+    }
+
+    pub fn pre_send(&mut self, proto_tx: &mut Packet) -> Result<(), Error> {
+        // Check if any acknowledgements are pending for this exchange,
+
+        // if so, piggy back in the encoded header here
+        if let Some(ack_entry) = self.ack {
+            // Ack Entry exists, set ACK bit and remove from table
+            proto_tx.proto.set_ack(ack_entry.get_msg_ctr());
+            self.ack = None;
+        }
+
+        if !proto_tx.is_reliable() {
+            return Ok(());
+        }
+
+        if self.retrans.is_some() {
+            // This indicates there was some existing entry for same sess-id/exch-id, which shouldnt happen
+            error!("Previous retrans entry for this exchange already exists");
+            return Err(Error::Invalid);
+        }
+
+        self.retrans = Some(RetransEntry::new(proto_tx.plain.ctr));
+        Ok(())
+    }
+
+    /* A note about Message ACKs, it is a bit asymmetric in the sense that:
+     * -  there can be only one pending ACK per exchange (so this is per-exchange)
+     * -  there can be only one pending retransmission per exchange (so this is per-exchange)
+     * -  duplicate detection should happen per session (obviously), so that part is per-session
+     */
+    pub fn recv(&mut self, proto_rx: &Packet) -> Result<(), Error> {
+        if proto_rx.proto.is_ack() {
+            // Handle received Acks
+            let ack_msg_ctr = proto_rx.proto.get_ack_msg_ctr().ok_or(Error::Invalid)?;
+            if let Some(entry) = &self.retrans {
+                if entry.get_msg_ctr() != ack_msg_ctr {
+                    // TODO: XXX Fix this
+                    error!("Mismatch in retrans-table's msg counter and received msg counter: received {}, expected {}. This is expected for the timebeing", ack_msg_ctr, entry.get_msg_ctr());
+                }
+                self.retrans = None;
+            }
+        }
+
+        if proto_rx.proto.is_reliable() {
+            if self.ack.is_some() {
+                // This indicates there was some existing entry for same sess-id/exch-id, which shouldnt happen
+                // TODO: As per the spec if this happens, we need to send out the previous ACK and note this new ACK
+                error!("Previous ACK entry for this exchange already exists");
+                return Err(Error::Invalid);
+            }
+
+            self.ack = Some(AckEntry::new(proto_rx.plain.ctr)?);
+        }
+        Ok(())
+    }
+}

matter/src/transport/network.rs

@@ -0,0 +1,55 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::{
+    fmt::{Debug, Display},
+    net::{IpAddr, Ipv4Addr, SocketAddr},
+};
+
+use crate::error::Error;
+
+#[derive(PartialEq, Copy, Clone)]
+pub enum Address {
+    Udp(SocketAddr),
+}
+
+impl Default for Address {
+    fn default() -> Self {
+        Address::Udp(SocketAddr::new(IpAddr::V4(Ipv4Addr::new(0, 0, 0, 0)), 8080))
+    }
+}
+
+impl Display for Address {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        match self {
+            Address::Udp(addr) => writeln!(f, "{}", addr),
+        }
+    }
+}
+
+impl Debug for Address {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        match self {
+            Address::Udp(addr) => writeln!(f, "{}", addr),
+        }
+    }
+}
+
+pub trait NetworkInterface {
+    fn recv(&self, in_buf: &mut [u8]) -> Result<(usize, Address), Error>;
+    fn send(&self, out_buf: &[u8], addr: Address) -> Result<usize, Error>;
+}

matter/src/transport/packet.rs

@@ -0,0 +1,239 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use log::{error, trace};
+use std::sync::Mutex;
+
+use boxslab::box_slab;
+
+use crate::{
+    error::Error,
+    sys::MAX_PACKET_POOL_SIZE,
+    utils::{parsebuf::ParseBuf, writebuf::WriteBuf},
+};
+
+use super::{
+    network::Address,
+    plain_hdr::{self, PlainHdr},
+    proto_hdr::{self, ProtoHdr},
+};
+
+pub const MAX_RX_BUF_SIZE: usize = 1583;
+type Buffer = [u8; MAX_RX_BUF_SIZE];
+
+// TODO: I am not very happy with this construction, need to find another way to do this
+pub struct BufferPool {
+    buffers: [Option<Buffer>; MAX_PACKET_POOL_SIZE],
+}
+
+impl BufferPool {
+    const INIT: Option<Buffer> = None;
+    fn get() -> &'static Mutex<BufferPool> {
+        static mut BUFFER_HOLDER: Option<Mutex<BufferPool>> = None;
+        static ONCE: Once = Once::new();
+        unsafe {
+            ONCE.call_once(|| {
+                BUFFER_HOLDER = Some(Mutex::new(BufferPool {
+                    buffers: [BufferPool::INIT; MAX_PACKET_POOL_SIZE],
+                }));
+            });
+            BUFFER_HOLDER.as_ref().unwrap()
+        }
+    }
+
+    pub fn alloc() -> Option<(usize, &'static mut Buffer)> {
+        trace!("Buffer Alloc called\n");
+
+        let mut pool = BufferPool::get().lock().unwrap();
+        for i in 0..MAX_PACKET_POOL_SIZE {
+            if pool.buffers[i].is_none() {
+                pool.buffers[i] = Some([0; MAX_RX_BUF_SIZE]);
+                // Sigh! to by-pass the borrow-checker telling us we are stealing a mutable reference
+                // from under the lock
+                // In this case the lock only protects against the setting of Some/None,
+                // the objects then are independently accessed in a unique way
+                let buffer = unsafe { &mut *(pool.buffers[i].as_mut().unwrap() as *mut Buffer) };
+                return Some((i, buffer));
+            }
+        }
+        None
+    }
+
+    pub fn free(index: usize) {
+        trace!("Buffer Free called\n");
+        let mut pool = BufferPool::get().lock().unwrap();
+        if pool.buffers[index].is_some() {
+            pool.buffers[index] = None;
+        }
+    }
+}
+
+#[derive(PartialEq)]
+enum RxState {
+    Uninit,
+    PlainDecode,
+    ProtoDecode,
+}
+
+enum Direction<'a> {
+    Tx(WriteBuf<'a>),
+    Rx(ParseBuf<'a>, RxState),
+}
+
+pub struct Packet<'a> {
+    pub plain: PlainHdr,
+    pub proto: ProtoHdr,
+    pub peer: Address,
+    data: Direction<'a>,
+    buffer_index: usize,
+}
+
+impl<'a> Packet<'a> {
+    const HDR_RESERVE: usize = plain_hdr::max_plain_hdr_len() + proto_hdr::max_proto_hdr_len();
+
+    pub fn new_rx() -> Result<Self, Error> {
+        let (buffer_index, buffer) = BufferPool::alloc().ok_or(Error::NoSpace)?;
+        let buf_len = buffer.len();
+        Ok(Self {
+            plain: Default::default(),
+            proto: Default::default(),
+            buffer_index,
+            peer: Address::default(),
+            data: Direction::Rx(ParseBuf::new(buffer, buf_len), RxState::Uninit),
+        })
+    }
+
+    pub fn new_tx() -> Result<Self, Error> {
+        let (buffer_index, buffer) = BufferPool::alloc().ok_or(Error::NoSpace)?;
+        let buf_len = buffer.len();
+
+        let mut wb = WriteBuf::new(buffer, buf_len);
+        wb.reserve(Packet::HDR_RESERVE)?;
+
+        let mut p = Self {
+            plain: Default::default(),
+            proto: Default::default(),
+            buffer_index,
+            peer: Address::default(),
+            data: Direction::Tx(wb),
+        };
+        // Reliability on by default
+        p.proto.set_reliable();
+        Ok(p)
+    }
+
+    pub fn as_borrow_slice(&mut self) -> &mut [u8] {
+        match &mut self.data {
+            Direction::Rx(pb, _) => (pb.as_borrow_slice()),
+            Direction::Tx(wb) => (wb.as_mut_slice()),
+        }
+    }
+
+    pub fn get_parsebuf(&mut self) -> Result<&mut ParseBuf<'a>, Error> {
+        if let Direction::Rx(pbuf, _) = &mut self.data {
+            Ok(pbuf)
+        } else {
+            Err(Error::Invalid)
+        }
+    }
+
+    pub fn get_writebuf(&mut self) -> Result<&mut WriteBuf<'a>, Error> {
+        if let Direction::Tx(wbuf) = &mut self.data {
+            Ok(wbuf)
+        } else {
+            Err(Error::Invalid)
+        }
+    }
+
+    pub fn get_proto_id(&self) -> u16 {
+        self.proto.proto_id
+    }
+
+    pub fn set_proto_id(&mut self, proto_id: u16) {
+        self.proto.proto_id = proto_id;
+    }
+
+    pub fn get_proto_opcode(&self) -> u8 {
+        self.proto.proto_opcode
+    }
+
+    pub fn set_proto_opcode(&mut self, proto_opcode: u8) {
+        self.proto.proto_opcode = proto_opcode;
+    }
+
+    pub fn set_reliable(&mut self) {
+        self.proto.set_reliable()
+    }
+
+    pub fn unset_reliable(&mut self) {
+        self.proto.unset_reliable()
+    }
+
+    pub fn is_reliable(&mut self) -> bool {
+        self.proto.is_reliable()
+    }
+
+    pub fn proto_decode(&mut self, peer_nodeid: u64, dec_key: Option<&[u8]>) -> Result<(), Error> {
+        match &mut self.data {
+            Direction::Rx(pb, state) => {
+                if *state == RxState::PlainDecode {
+                    *state = RxState::ProtoDecode;
+                    self.proto
+                        .decrypt_and_decode(&self.plain, pb, peer_nodeid, dec_key)
+                } else {
+                    error!("Invalid state for proto_decode");
+                    Err(Error::InvalidState)
+                }
+            }
+            _ => Err(Error::InvalidState),
+        }
+    }
+
+    pub fn is_plain_hdr_decoded(&self) -> Result<bool, Error> {
+        match &self.data {
+            Direction::Rx(_, state) => match state {
+                RxState::Uninit => Ok(false),
+                _ => Ok(true),
+            },
+            _ => Err(Error::InvalidState),
+        }
+    }
+
+    pub fn plain_hdr_decode(&mut self) -> Result<(), Error> {
+        match &mut self.data {
+            Direction::Rx(pb, state) => {
+                if *state == RxState::Uninit {
+                    *state = RxState::PlainDecode;
+                    self.plain.decode(pb)
+                } else {
+                    error!("Invalid state for plain_decode");
+                    Err(Error::InvalidState)
+                }
+            }
+            _ => Err(Error::InvalidState),
+        }
+    }
+}
+
+impl<'a> Drop for Packet<'a> {
+    fn drop(&mut self) {
+        BufferPool::free(self.buffer_index);
+        trace!("Dropping Packet......");
+    }
+}
+
+box_slab!(PacketPool, Packet<'static>, MAX_PACKET_POOL_SIZE);

matter/src/transport/plain_hdr.rs

@@ -0,0 +1,125 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::*;
+use crate::utils::parsebuf::ParseBuf;
+use crate::utils::writebuf::WriteBuf;
+use bitflags::bitflags;
+use log::info;
+
+#[derive(Debug, PartialEq)]
+pub enum SessionType {
+    None,
+    Encrypted,
+}
+
+impl Default for SessionType {
+    fn default() -> SessionType {
+        SessionType::None
+    }
+}
+
+bitflags! {
+    #[derive(Default)]
+    pub struct MsgFlags: u8 {
+        const DSIZ_UNICAST_NODEID = 0x01;
+        const DSIZ_GROUPCAST_NODEID = 0x02;
+        const SRC_ADDR_PRESENT = 0x04;
+    }
+}
+
+// This is the unencrypted message
+#[derive(Debug, Default)]
+pub struct PlainHdr {
+    pub flags: MsgFlags,
+    pub sess_type: SessionType,
+    pub sess_id: u16,
+    pub ctr: u32,
+    peer_nodeid: Option<u64>,
+}
+
+impl PlainHdr {
+    pub fn set_dest_u64(&mut self, id: u64) {
+        self.flags |= MsgFlags::DSIZ_UNICAST_NODEID;
+        self.peer_nodeid = Some(id);
+    }
+
+    pub fn get_src_u64(&self) -> Option<u64> {
+        if self.flags.contains(MsgFlags::SRC_ADDR_PRESENT) {
+            self.peer_nodeid
+        } else {
+            None
+        }
+    }
+}
+
+impl PlainHdr {
+    // it will have an additional 'message length' field first
+    pub fn decode(&mut self, msg: &mut ParseBuf) -> Result<(), Error> {
+        self.flags = MsgFlags::from_bits(msg.le_u8()?).ok_or(Error::Invalid)?;
+        self.sess_id = msg.le_u16()?;
+        let _sec_flags = msg.le_u8()?;
+        self.sess_type = if self.sess_id != 0 {
+            SessionType::Encrypted
+        } else {
+            SessionType::None
+        };
+        self.ctr = msg.le_u32()?;
+
+        if self.flags.contains(MsgFlags::SRC_ADDR_PRESENT) {
+            self.peer_nodeid = Some(msg.le_u64()?);
+        }
+
+        info!(
+            "[decode] flags: {:?}, session type: {:#?}, sess_id: {}, ctr: {}",
+            self.flags, self.sess_type, self.sess_id, self.ctr
+        );
+        Ok(())
+    }
+
+    pub fn encode(&mut self, resp_buf: &mut WriteBuf) -> Result<(), Error> {
+        resp_buf.le_u8(self.flags.bits())?;
+        resp_buf.le_u16(self.sess_id)?;
+        resp_buf.le_u8(0)?;
+        resp_buf.le_u32(self.ctr)?;
+        if let Some(d) = self.peer_nodeid {
+            resp_buf.le_u64(d)?;
+        }
+        Ok(())
+    }
+
+    pub fn is_encrypted(&self) -> bool {
+        self.sess_type == SessionType::Encrypted
+    }
+}
+
+pub const fn max_plain_hdr_len() -> usize {
+    // [optional] msg len only for TCP
+    2 +
+    // flags
+        1 +
+    // security flags
+        1 +
+    // session ID
+        2 +
+    // message ctr
+        4 +
+    // [optional] source node ID
+        8 +
+    // [optional] destination node ID
+        8
+}

matter/src/transport/proto_demux.rs

@@ -0,0 +1,95 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use boxslab::BoxSlab;
+
+use crate::error::*;
+
+use super::exchange::ExchangeCtx;
+use super::packet::PacketPool;
+
+const MAX_PROTOCOLS: usize = 4;
+
+#[derive(PartialEq)]
+pub enum ResponseRequired {
+    Yes,
+    No,
+}
+pub struct ProtoDemux {
+    proto_id_handlers: [Option<Box<dyn HandleProto>>; MAX_PROTOCOLS],
+}
+
+/// This is the context in which a receive packet is being processed
+pub struct ProtoCtx<'a> {
+    /// This is the exchange context, that includes the exchange and the session
+    pub exch_ctx: ExchangeCtx<'a>,
+    /// This is the received buffer for this transaction
+    pub rx: BoxSlab<PacketPool>,
+    /// This is the transmit buffer for this transaction
+    pub tx: BoxSlab<PacketPool>,
+}
+
+impl<'a> ProtoCtx<'a> {
+    pub fn new(
+        exch_ctx: ExchangeCtx<'a>,
+        rx: BoxSlab<PacketPool>,
+        tx: BoxSlab<PacketPool>,
+    ) -> Self {
+        Self { exch_ctx, rx, tx }
+    }
+}
+
+pub trait HandleProto {
+    fn handle_proto_id(&mut self, proto_ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error>;
+
+    fn get_proto_id(&self) -> usize;
+
+    fn handle_session_event(&self) -> Result<(), Error> {
+        Ok(())
+    }
+}
+
+impl Default for ProtoDemux {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl ProtoDemux {
+    pub fn new() -> ProtoDemux {
+        ProtoDemux {
+            proto_id_handlers: [None, None, None, None],
+        }
+    }
+
+    pub fn register(&mut self, proto_id_handle: Box<dyn HandleProto>) -> Result<(), Error> {
+        let proto_id = proto_id_handle.get_proto_id();
+        self.proto_id_handlers[proto_id] = Some(proto_id_handle);
+        Ok(())
+    }
+
+    pub fn handle(&mut self, proto_ctx: &mut ProtoCtx) -> Result<ResponseRequired, Error> {
+        let proto_id = proto_ctx.rx.get_proto_id() as usize;
+        if proto_id >= MAX_PROTOCOLS {
+            return Err(Error::Invalid);
+        }
+        return self.proto_id_handlers[proto_id]
+            .as_mut()
+            .ok_or(Error::NoHandler)?
+            .handle_proto_id(proto_ctx);
+    }
+}

matter/src/transport/proto_hdr.rs

@@ -0,0 +1,324 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use bitflags::bitflags;
+use std::fmt;
+
+use crate::transport::plain_hdr;
+use crate::utils::parsebuf::ParseBuf;
+use crate::utils::writebuf::WriteBuf;
+use crate::{crypto, error::*};
+
+use log::{info, trace};
+
+bitflags! {
+    #[derive(Default)]
+    pub struct ExchFlags: u8 {
+        const VENDOR = 0x10;
+        const SECEX = 0x08;
+        const RELIABLE = 0x04;
+        const ACK = 0x02;
+        const INITIATOR = 0x01;
+    }
+}
+
+#[derive(Default)]
+pub struct ProtoHdr {
+    pub exch_id: u16,
+    pub exch_flags: ExchFlags,
+    pub proto_id: u16,
+    pub proto_opcode: u8,
+    pub proto_vendor_id: Option<u16>,
+    pub ack_msg_ctr: Option<u32>,
+}
+
+impl ProtoHdr {
+    pub fn is_vendor(&self) -> bool {
+        self.exch_flags.contains(ExchFlags::VENDOR)
+    }
+
+    pub fn set_vendor(&mut self, proto_vendor_id: u16) {
+        self.exch_flags |= ExchFlags::RELIABLE;
+        self.proto_vendor_id = Some(proto_vendor_id);
+    }
+
+    pub fn is_security_ext(&self) -> bool {
+        self.exch_flags.contains(ExchFlags::SECEX)
+    }
+
+    pub fn is_reliable(&self) -> bool {
+        self.exch_flags.contains(ExchFlags::RELIABLE)
+    }
+
+    pub fn unset_reliable(&mut self) {
+        self.exch_flags.remove(ExchFlags::RELIABLE)
+    }
+
+    pub fn set_reliable(&mut self) {
+        self.exch_flags |= ExchFlags::RELIABLE;
+    }
+
+    pub fn is_ack(&self) -> bool {
+        self.exch_flags.contains(ExchFlags::ACK)
+    }
+
+    pub fn get_ack_msg_ctr(&self) -> Option<u32> {
+        self.ack_msg_ctr
+    }
+
+    pub fn set_ack(&mut self, ack_msg_ctr: u32) {
+        self.exch_flags |= ExchFlags::ACK;
+        self.ack_msg_ctr = Some(ack_msg_ctr);
+    }
+
+    pub fn is_initiator(&self) -> bool {
+        self.exch_flags.contains(ExchFlags::INITIATOR)
+    }
+
+    pub fn set_initiator(&mut self) {
+        self.exch_flags |= ExchFlags::INITIATOR;
+    }
+
+    pub fn decrypt_and_decode(
+        &mut self,
+        plain_hdr: &plain_hdr::PlainHdr,
+        parsebuf: &mut ParseBuf,
+        peer_nodeid: u64,
+        dec_key: Option<&[u8]>,
+    ) -> Result<(), Error> {
+        if let Some(d) = dec_key {
+            // We decrypt only if the decryption key is valid
+            decrypt_in_place(plain_hdr.ctr, peer_nodeid, parsebuf, d)?;
+        }
+
+        self.exch_flags = ExchFlags::from_bits(parsebuf.le_u8()?).ok_or(Error::Invalid)?;
+        self.proto_opcode = parsebuf.le_u8()?;
+        self.exch_id = parsebuf.le_u16()?;
+        self.proto_id = parsebuf.le_u16()?;
+
+        info!("[decode] {} ", self);
+        if self.is_vendor() {
+            self.proto_vendor_id = Some(parsebuf.le_u16()?);
+        }
+        if self.is_ack() {
+            self.ack_msg_ctr = Some(parsebuf.le_u32()?);
+        }
+        trace!("[rx payload]: {:x?}", parsebuf.as_borrow_slice());
+        Ok(())
+    }
+
+    pub fn encode(&mut self, resp_buf: &mut WriteBuf) -> Result<(), Error> {
+        info!("[encode] {}", self);
+        resp_buf.le_u8(self.exch_flags.bits())?;
+        resp_buf.le_u8(self.proto_opcode)?;
+        resp_buf.le_u16(self.exch_id)?;
+        resp_buf.le_u16(self.proto_id)?;
+        if self.is_vendor() {
+            resp_buf.le_u16(self.proto_vendor_id.ok_or(Error::Invalid)?)?;
+        }
+        if self.is_ack() {
+            resp_buf.le_u32(self.ack_msg_ctr.ok_or(Error::Invalid)?)?;
+        }
+        Ok(())
+    }
+}
+
+impl fmt::Display for ProtoHdr {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        let mut flag_str: String = "".to_owned();
+        if self.is_vendor() {
+            flag_str.push_str("V|");
+        }
+        if self.is_security_ext() {
+            flag_str.push_str("SX|");
+        }
+        if self.is_reliable() {
+            flag_str.push_str("R|");
+        }
+        if self.is_ack() {
+            flag_str.push_str("A|");
+        }
+        if self.is_initiator() {
+            flag_str.push_str("I|");
+        }
+        write!(
+            f,
+            "ExId: {}, Proto: {}, Opcode: {}, Flags: {}",
+            self.exch_id, self.proto_id, self.proto_opcode, flag_str
+        )
+    }
+}
+
+fn get_iv(recvd_ctr: u32, peer_nodeid: u64, iv: &mut [u8]) -> Result<(), Error> {
+    // The IV is the source address (64-bit) followed by the message counter (32-bit)
+    let mut write_buf = WriteBuf::new(iv, iv.len());
+    // For some reason, this is 0 in the 'bypass' mode
+    write_buf.le_u8(0)?;
+    write_buf.le_u32(recvd_ctr)?;
+    write_buf.le_u64(peer_nodeid)?;
+    Ok(())
+}
+
+pub fn encrypt_in_place(
+    send_ctr: u32,
+    peer_nodeid: u64,
+    plain_hdr: &[u8],
+    writebuf: &mut WriteBuf,
+    key: &[u8],
+) -> Result<(), Error> {
+    // IV
+    let mut iv = [0_u8; crypto::AEAD_NONCE_LEN_BYTES];
+    get_iv(send_ctr, peer_nodeid, &mut iv)?;
+
+    // Cipher Text
+    let tag_space = [0u8; crypto::AEAD_MIC_LEN_BYTES];
+    writebuf.append(&tag_space)?;
+    let cipher_text = writebuf.as_mut_slice();
+
+    crypto::encrypt_in_place(
+        key,
+        &iv,
+        plain_hdr,
+        cipher_text,
+        cipher_text.len() - crypto::AEAD_MIC_LEN_BYTES,
+    )?;
+    //println!("Cipher Text: {:x?}", cipher_text);
+
+    Ok(())
+}
+
+fn decrypt_in_place(
+    recvd_ctr: u32,
+    peer_nodeid: u64,
+    parsebuf: &mut ParseBuf,
+    key: &[u8],
+) -> Result<(), Error> {
+    // AAD:
+    //    the unencrypted header of this packet
+    let mut aad = [0_u8; crypto::AEAD_AAD_LEN_BYTES];
+    let parsed_slice = parsebuf.parsed_as_slice();
+    if parsed_slice.len() == aad.len() {
+        // The plain_header is variable sized in length, I wonder if the AAD is fixed at 8, or the variable size.
+        // If so, we need to handle it cleanly here.
+        aad.copy_from_slice(parsed_slice);
+    } else {
+        return Err(Error::InvalidAAD);
+    }
+
+    // IV:
+    //   the specific way for creating IV is in get_iv
+    let mut iv = [0_u8; crypto::AEAD_NONCE_LEN_BYTES];
+    get_iv(recvd_ctr, peer_nodeid, &mut iv)?;
+
+    let cipher_text = parsebuf.as_borrow_slice();
+    //println!("AAD: {:x?}", aad);
+    //println!("Cipher Text: {:x?}", cipher_text);
+    //println!("IV: {:x?}", iv);
+    //println!("Key: {:x?}", key);
+
+    crypto::decrypt_in_place(key, &iv, &aad, cipher_text)?;
+    // println!("Plain Text: {:x?}", cipher_text);
+    parsebuf.tail(crypto::AEAD_MIC_LEN_BYTES)?;
+    Ok(())
+}
+
+pub const fn max_proto_hdr_len() -> usize {
+    // exchange flags
+    1 +
+    // protocol opcode
+        1 +
+    // exchange ID
+        2 +
+    // protocol ID
+        2 +
+    // [optional] protocol vendor ID
+        2 +
+    // [optional] acknowledged message counter
+        4
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    pub fn test_decrypt_success() {
+        // These values are captured from an execution run of the chip-tool binary
+        let recvd_ctr = 15287282;
+        let mut input_buf: [u8; 71] = [
+            0x0, 0x2, 0x0, 0x0, 0xf2, 0x43, 0xe9, 0x0, 0x31, 0xb5, 0x66, 0xec, 0x8b, 0x5b, 0xf4,
+            0x17, 0xe4, 0x80, 0xf3, 0xd5, 0x11, 0x59, 0x19, 0xb5, 0x23, 0x91, 0x35, 0x37, 0xb,
+            0xf9, 0xbf, 0x69, 0x55, 0x11, 0x75, 0x87, 0x77, 0x19, 0xfc, 0xf3, 0x5d, 0x4b, 0x47,
+            0x1f, 0xb0, 0x5e, 0xbe, 0xb5, 0x10, 0xad, 0xc6, 0x78, 0x94, 0x50, 0xe5, 0xd2, 0xe0,
+            0x80, 0xef, 0xa8, 0x3a, 0xf0, 0xa6, 0xaf, 0x1b, 0x2, 0x35, 0xa7, 0xd1, 0xc6, 0x32,
+        ];
+        let input_buf_len = input_buf.len();
+        let mut parsebuf = ParseBuf::new(&mut input_buf, input_buf_len);
+        let key = [
+            0x66, 0x63, 0x31, 0x97, 0x43, 0x9c, 0x17, 0xb9, 0x7e, 0x10, 0xee, 0x47, 0xc8, 0x8,
+            0x80, 0x4a,
+        ];
+
+        // decrypt_in_place() requires that the plain_text buffer of 8 bytes must be already parsed as AAD, we'll just fake it here
+        parsebuf.le_u32().unwrap();
+        parsebuf.le_u32().unwrap();
+
+        decrypt_in_place(recvd_ctr, 0, &mut parsebuf, &key).unwrap();
+        assert_eq!(
+            parsebuf.as_slice(),
+            [
+                0x5, 0x8, 0x70, 0x0, 0x1, 0x0, 0x15, 0x28, 0x0, 0x28, 0x1, 0x36, 0x2, 0x15, 0x37,
+                0x0, 0x24, 0x0, 0x0, 0x24, 0x1, 0x30, 0x24, 0x2, 0x2, 0x18, 0x35, 0x1, 0x24, 0x0,
+                0x0, 0x2c, 0x1, 0x2, 0x57, 0x57, 0x24, 0x2, 0x3, 0x25, 0x3, 0xb8, 0xb, 0x18, 0x18,
+                0x18, 0x18
+            ]
+        );
+    }
+
+    #[test]
+    pub fn test_encrypt_success() {
+        // These values are captured from an execution run of the chip-tool binary
+        let send_ctr = 41;
+
+        let mut main_buf: [u8; 52] = [0; 52];
+        let main_buf_len = main_buf.len();
+        let mut writebuf = WriteBuf::new(&mut main_buf, main_buf_len);
+
+        let plain_hdr: [u8; 8] = [0x0, 0x11, 0x0, 0x0, 0x29, 0x0, 0x0, 0x0];
+
+        let plain_text: [u8; 28] = [
+            5, 8, 0x58, 0x28, 0x01, 0x00, 0x15, 0x36, 0x00, 0x15, 0x37, 0x00, 0x24, 0x00, 0x01,
+            0x24, 0x02, 0x06, 0x24, 0x03, 0x01, 0x18, 0x35, 0x01, 0x18, 0x18, 0x18, 0x18,
+        ];
+        writebuf.append(&plain_text).unwrap();
+
+        let key = [
+            0x44, 0xd4, 0x3c, 0x91, 0xd2, 0x27, 0xf3, 0xba, 0x08, 0x24, 0xc5, 0xd8, 0x7c, 0xb8,
+            0x1b, 0x33,
+        ];
+
+        encrypt_in_place(send_ctr, 0, &plain_hdr, &mut writebuf, &key).unwrap();
+        assert_eq!(
+            writebuf.as_slice(),
+            [
+                189, 83, 250, 121, 38, 87, 97, 17, 153, 78, 243, 20, 36, 11, 131, 142, 136, 165,
+                227, 107, 204, 129, 193, 153, 42, 131, 138, 254, 22, 190, 76, 244, 116, 45, 156,
+                215, 229, 130, 215, 147, 73, 21, 88, 216
+            ]
+        );
+    }
+}

matter/src/transport/queue.rs

@@ -0,0 +1,68 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+
+use std::sync::Once;
+
+use async_channel::{bounded, Receiver, Sender};
+
+use crate::error::Error;
+
+use super::session::CloneData;
+
+#[derive(Debug)]
+pub enum Msg {
+    Tx(),
+    Rx(),
+    NewSession(CloneData),
+}
+
+#[derive(Clone)]
+pub struct WorkQ {
+    tx: Sender<Msg>,
+}
+
+static mut G_WQ: Option<WorkQ> = None;
+static INIT: Once = Once::new();
+
+impl WorkQ {
+    pub fn init() -> Result<Receiver<Msg>, Error> {
+        let (tx, rx) = bounded::<Msg>(3);
+        WorkQ::configure(tx);
+        Ok(rx)
+    }
+
+    fn configure(tx: Sender<Msg>) {
+        unsafe {
+            INIT.call_once(|| {
+                G_WQ = Some(WorkQ { tx });
+            });
+        }
+    }
+
+    pub fn get() -> Result<WorkQ, Error> {
+        unsafe { G_WQ.as_ref().cloned().ok_or(Error::Invalid) }
+    }
+
+    pub fn sync_send(&self, msg: Msg) -> Result<(), Error> {
+        smol::block_on(self.send(msg))
+    }
+
+    pub async fn send(&self, msg: Msg) -> Result<(), Error> {
+        self.tx.send(msg).await.map_err(|e| e.into())
+    }
+}

matter/src/transport/session.rs

@@ -0,0 +1,587 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use core::fmt;
+use std::{
+    any::Any,
+    ops::{Deref, DerefMut},
+    time::SystemTime,
+};
+
+use crate::{
+    error::*,
+    transport::{plain_hdr, proto_hdr},
+    utils::writebuf::WriteBuf,
+};
+use boxslab::{BoxSlab, Slab};
+use colored::*;
+use log::{info, trace};
+use rand::Rng;
+
+use super::{
+    network::{Address, NetworkInterface},
+    packet::{Packet, PacketPool},
+};
+
+const MATTER_AES128_KEY_SIZE: usize = 16;
+
+#[derive(Debug, PartialEq, Copy, Clone)]
+pub enum SessionMode {
+    // The Case session will capture the local fabric index
+    Case(u8),
+    Pase,
+    PlainText,
+}
+
+impl Default for SessionMode {
+    fn default() -> Self {
+        SessionMode::PlainText
+    }
+}
+
+#[derive(Debug)]
+pub struct Session {
+    peer_addr: Address,
+    local_nodeid: u64,
+    peer_nodeid: Option<u64>,
+    // I find the session initiator/responder role getting confused with exchange initiator/responder
+    // So, we might keep this as enc_key and dec_key for now
+    dec_key: [u8; MATTER_AES128_KEY_SIZE],
+    enc_key: [u8; MATTER_AES128_KEY_SIZE],
+    att_challenge: [u8; MATTER_AES128_KEY_SIZE],
+    local_sess_id: u16,
+    peer_sess_id: u16,
+    msg_ctr: u32,
+    mode: SessionMode,
+    data: Option<Box<dyn Any>>,
+    last_use: SystemTime,
+}
+
+#[derive(Debug)]
+pub struct CloneData {
+    pub dec_key: [u8; MATTER_AES128_KEY_SIZE],
+    pub enc_key: [u8; MATTER_AES128_KEY_SIZE],
+    pub att_challenge: [u8; MATTER_AES128_KEY_SIZE],
+    local_sess_id: u16,
+    peer_sess_id: u16,
+    local_nodeid: u64,
+    peer_nodeid: u64,
+    peer_addr: Address,
+    mode: SessionMode,
+}
+impl CloneData {
+    pub fn new(
+        local_nodeid: u64,
+        peer_nodeid: u64,
+        peer_sess_id: u16,
+        local_sess_id: u16,
+        peer_addr: Address,
+        mode: SessionMode,
+    ) -> CloneData {
+        CloneData {
+            dec_key: [0; MATTER_AES128_KEY_SIZE],
+            enc_key: [0; MATTER_AES128_KEY_SIZE],
+            att_challenge: [0; MATTER_AES128_KEY_SIZE],
+            local_nodeid,
+            peer_nodeid,
+            peer_addr,
+            peer_sess_id,
+            local_sess_id,
+            mode,
+        }
+    }
+}
+
+const MATTER_MSG_CTR_RANGE: u32 = 0x0fffffff;
+
+impl Session {
+    pub fn new(peer_addr: Address, peer_nodeid: Option<u64>) -> Session {
+        Session {
+            peer_addr,
+            local_nodeid: 0,
+            peer_nodeid,
+            dec_key: [0; MATTER_AES128_KEY_SIZE],
+            enc_key: [0; MATTER_AES128_KEY_SIZE],
+            att_challenge: [0; MATTER_AES128_KEY_SIZE],
+            peer_sess_id: 0,
+            local_sess_id: 0,
+            msg_ctr: rand::thread_rng().gen_range(0..MATTER_MSG_CTR_RANGE),
+            mode: SessionMode::PlainText,
+            data: None,
+            last_use: SystemTime::now(),
+        }
+    }
+
+    // A new encrypted session always clones from a previous 'new' session
+    pub fn clone(clone_from: &CloneData) -> Session {
+        Session {
+            peer_addr: clone_from.peer_addr,
+            local_nodeid: clone_from.local_nodeid,
+            peer_nodeid: Some(clone_from.peer_nodeid),
+            dec_key: clone_from.dec_key,
+            enc_key: clone_from.enc_key,
+            att_challenge: clone_from.att_challenge,
+            local_sess_id: clone_from.local_sess_id,
+            peer_sess_id: clone_from.peer_sess_id,
+            msg_ctr: rand::thread_rng().gen_range(0..MATTER_MSG_CTR_RANGE),
+            mode: clone_from.mode,
+            data: None,
+            last_use: SystemTime::now(),
+        }
+    }
+
+    pub fn set_data(&mut self, data: Box<dyn Any>) {
+        self.data = Some(data);
+    }
+
+    pub fn clear_data(&mut self) {
+        self.data = None;
+    }
+
+    pub fn get_data<T: Any>(&mut self) -> Option<&mut T> {
+        self.data.as_mut()?.downcast_mut::<T>()
+    }
+
+    pub fn take_data<T: Any>(&mut self) -> Option<Box<T>> {
+        self.data.take()?.downcast::<T>().ok()
+    }
+
+    pub fn get_local_sess_id(&self) -> u16 {
+        self.local_sess_id
+    }
+
+    #[cfg(test)]
+    pub fn set_local_sess_id(&mut self, sess_id: u16) {
+        self.local_sess_id = sess_id;
+    }
+
+    pub fn get_peer_sess_id(&self) -> u16 {
+        self.peer_sess_id
+    }
+
+    pub fn get_peer_addr(&self) -> Address {
+        self.peer_addr
+    }
+
+    pub fn is_encrypted(&self) -> bool {
+        match self.mode {
+            SessionMode::Case(_) | SessionMode::Pase => true,
+            SessionMode::PlainText => false,
+        }
+    }
+
+    pub fn get_peer_node_id(&self) -> Option<u64> {
+        self.peer_nodeid
+    }
+
+    pub fn get_local_fabric_idx(&self) -> Option<u8> {
+        match self.mode {
+            SessionMode::Case(a) => Some(a),
+            _ => None,
+        }
+    }
+
+    pub fn get_session_mode(&self) -> SessionMode {
+        self.mode
+    }
+
+    pub fn get_msg_ctr(&mut self) -> u32 {
+        let ctr = self.msg_ctr;
+        self.msg_ctr += 1;
+        ctr
+    }
+
+    pub fn get_dec_key(&self) -> Option<&[u8]> {
+        match self.mode {
+            SessionMode::Case(_) | SessionMode::Pase => Some(&self.dec_key),
+            SessionMode::PlainText => None,
+        }
+    }
+
+    pub fn get_enc_key(&self) -> Option<&[u8]> {
+        match self.mode {
+            SessionMode::Case(_) | SessionMode::Pase => Some(&self.enc_key),
+            SessionMode::PlainText => None,
+        }
+    }
+
+    pub fn get_att_challenge(&self) -> &[u8] {
+        &self.att_challenge
+    }
+
+    pub fn recv(&mut self, proto_rx: &mut Packet) -> Result<(), Error> {
+        self.last_use = SystemTime::now();
+        proto_rx.proto_decode(self.peer_nodeid.unwrap_or_default(), self.get_dec_key())
+    }
+
+    pub fn pre_send(&mut self, proto_tx: &mut Packet) -> Result<(), Error> {
+        proto_tx.plain.sess_id = self.get_peer_sess_id();
+        proto_tx.plain.ctr = self.get_msg_ctr();
+        if self.is_encrypted() {
+            proto_tx.plain.sess_type = plain_hdr::SessionType::Encrypted;
+        }
+        Ok(())
+    }
+
+    // TODO: Most of this can now be moved into the 'Packet' module
+    fn do_send(&mut self, proto_tx: &mut Packet) -> Result<(), Error> {
+        self.last_use = SystemTime::now();
+        proto_tx.peer = self.peer_addr;
+
+        // Generate encrypted header
+        let mut tmp_buf: [u8; proto_hdr::max_proto_hdr_len()] = [0; proto_hdr::max_proto_hdr_len()];
+        let mut write_buf = WriteBuf::new(&mut tmp_buf[..], proto_hdr::max_proto_hdr_len());
+        proto_tx.proto.encode(&mut write_buf)?;
+        proto_tx.get_writebuf()?.prepend(write_buf.as_slice())?;
+
+        // Generate plain-text header
+        if self.mode == SessionMode::PlainText {
+            if let Some(d) = self.peer_nodeid {
+                proto_tx.plain.set_dest_u64(d);
+            }
+        }
+        let mut tmp_buf: [u8; plain_hdr::max_plain_hdr_len()] = [0; plain_hdr::max_plain_hdr_len()];
+        let mut write_buf = WriteBuf::new(&mut tmp_buf[..], plain_hdr::max_plain_hdr_len());
+        proto_tx.plain.encode(&mut write_buf)?;
+        let plain_hdr_bytes = write_buf.as_slice();
+
+        trace!("unencrypted packet: {:x?}", proto_tx.as_borrow_slice());
+        let ctr = proto_tx.plain.ctr;
+        let enc_key = self.get_enc_key();
+        if let Some(e) = enc_key {
+            proto_hdr::encrypt_in_place(
+                ctr,
+                self.local_nodeid,
+                plain_hdr_bytes,
+                proto_tx.get_writebuf()?,
+                e,
+            )?;
+        }
+
+        proto_tx.get_writebuf()?.prepend(plain_hdr_bytes)?;
+        trace!("Full encrypted packet: {:x?}", proto_tx.as_borrow_slice());
+        Ok(())
+    }
+}
+
+impl fmt::Display for Session {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        write!(
+            f,
+            "peer: {:?}, peer_nodeid: {:?}, local: {}, remote: {}, msg_ctr: {}, mode: {:?}, ts: {:?}",
+            self.peer_addr,
+            self.peer_nodeid,
+            self.local_sess_id,
+            self.peer_sess_id,
+            self.msg_ctr,
+            self.mode,
+            self.last_use,
+        )
+    }
+}
+
+pub const MAX_SESSIONS: usize = 16;
+pub struct SessionMgr {
+    next_sess_id: u16,
+    sessions: [Option<Session>; MAX_SESSIONS],
+    network: Option<Box<dyn NetworkInterface>>,
+}
+
+impl Default for SessionMgr {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl SessionMgr {
+    pub fn new() -> SessionMgr {
+        SessionMgr {
+            sessions: Default::default(),
+            next_sess_id: 1,
+            network: None,
+        }
+    }
+
+    pub fn add_network_interface(
+        &mut self,
+        interface: Box<dyn NetworkInterface>,
+    ) -> Result<(), Error> {
+        if self.network.is_none() {
+            self.network = Some(interface);
+            Ok(())
+        } else {
+            Err(Error::Invalid)
+        }
+    }
+
+    pub fn mut_by_index(&mut self, index: usize) -> Option<&mut Session> {
+        self.sessions[index].as_mut()
+    }
+
+    fn get_next_sess_id(&mut self) -> u16 {
+        let mut next_sess_id: u16;
+        loop {
+            next_sess_id = self.next_sess_id;
+
+            // Increment next sess id
+            self.next_sess_id = self.next_sess_id.overflowing_add(1).0;
+            if self.next_sess_id == 0 {
+                self.next_sess_id = 1;
+            }
+
+            // Ensure the currently selected id doesn't match any existing session
+            if self.get_with_id(next_sess_id).is_none() {
+                break;
+            }
+        }
+        next_sess_id
+    }
+
+    fn get_empty_slot(&self) -> Option<usize> {
+        self.sessions.iter().position(|x| x.is_none())
+    }
+
+    pub fn get_lru(&mut self) -> usize {
+        let mut lru_index = 0;
+        let mut lru_ts = SystemTime::now();
+        for i in 0..MAX_SESSIONS {
+            if let Some(s) = &self.sessions[i] {
+                if s.last_use < lru_ts {
+                    lru_ts = s.last_use;
+                    lru_index = i;
+                }
+            }
+        }
+        lru_index
+    }
+
+    pub fn add(&mut self, peer_addr: Address, peer_nodeid: Option<u64>) -> Result<usize, Error> {
+        let session = Session::new(peer_addr, peer_nodeid);
+        self.add_session(session)
+    }
+
+    /// This assumes that the higher layer has taken care of doing anything required
+    /// as per the spec before the session is erased
+    pub fn remove(&mut self, idx: usize) {
+        self.sessions[idx] = None;
+    }
+
+    /// We could have returned a SessionHandle here. But the borrow checker doesn't support
+    /// non-lexical lifetimes. This makes it harder for the caller of this function to take
+    /// action in the error return path
+    pub fn add_session(&mut self, session: Session) -> Result<usize, Error> {
+        if let Some(index) = self.get_empty_slot() {
+            self.sessions[index] = Some(session);
+            Ok(index)
+        } else {
+            Err(Error::NoSpace)
+        }
+    }
+
+    pub fn clone_session(&mut self, clone_data: &CloneData) -> Result<usize, Error> {
+        let session = Session::clone(clone_data);
+        self.add_session(session)
+    }
+
+    fn _get(
+        &self,
+        sess_id: u16,
+        peer_addr: Address,
+        peer_nodeid: Option<u64>,
+        is_encrypted: bool,
+    ) -> Option<usize> {
+        self.sessions.iter().position(|x| {
+            if let Some(x) = x {
+                let mut nodeid_matches = true;
+                if x.peer_nodeid.is_some() && peer_nodeid.is_some() && x.peer_nodeid != peer_nodeid
+                {
+                    nodeid_matches = false;
+                }
+                x.local_sess_id == sess_id
+                    && x.peer_addr == peer_addr
+                    && x.is_encrypted() == is_encrypted
+                    && nodeid_matches
+            } else {
+                false
+            }
+        })
+    }
+
+    pub fn get_with_id(&mut self, sess_id: u16) -> Option<SessionHandle> {
+        let index = self
+            .sessions
+            .iter_mut()
+            .position(|x| x.as_ref().map(|s| s.local_sess_id) == Some(sess_id))?;
+        Some(self.get_session_handle(index))
+    }
+
+    pub fn get_or_add(
+        &mut self,
+        sess_id: u16,
+        peer_addr: Address,
+        peer_nodeid: Option<u64>,
+        is_encrypted: bool,
+    ) -> Result<usize, Error> {
+        if let Some(index) = self._get(sess_id, peer_addr, peer_nodeid, is_encrypted) {
+            Ok(index)
+        } else if sess_id == 0 && !is_encrypted {
+            // We must create a new session for this case
+            info!("Creating new session");
+            self.add(peer_addr, peer_nodeid)
+        } else {
+            Err(Error::NotFound)
+        }
+    }
+
+    // We will try to get a session for this Packet. If no session exists, we will try to add one
+    // If the session list is full we will return a None
+    pub fn post_recv(&mut self, rx: &Packet) -> Result<Option<usize>, Error> {
+        let sess_index = match self.get_or_add(
+            rx.plain.sess_id,
+            rx.peer,
+            rx.plain.get_src_u64(),
+            rx.plain.is_encrypted(),
+        ) {
+            Ok(s) => Some(s),
+            Err(Error::NoSpace) => None,
+            Err(e) => {
+                return Err(e);
+            }
+        };
+        Ok(sess_index)
+    }
+
+    pub fn recv(&mut self) -> Result<(BoxSlab<PacketPool>, Option<usize>), Error> {
+        let mut rx = Slab::<PacketPool>::new(Packet::new_rx()?).ok_or(Error::PacketPoolExhaust)?;
+
+        let network = self.network.as_ref().ok_or(Error::NoNetworkInterface)?;
+
+        let (len, src) = network.recv(rx.as_borrow_slice())?;
+        rx.get_parsebuf()?.set_len(len);
+        rx.peer = src;
+
+        info!("{} from src: {}", "Received".blue(), src);
+        trace!("payload: {:x?}", rx.as_borrow_slice());
+
+        // Read unencrypted packet header
+        rx.plain_hdr_decode()?;
+
+        // Get session
+        let sess_handle = self.post_recv(&rx)?;
+        Ok((rx, sess_handle))
+    }
+
+    pub fn send(
+        &mut self,
+        sess_idx: usize,
+        mut proto_tx: BoxSlab<PacketPool>,
+    ) -> Result<(), Error> {
+        self.sessions[sess_idx]
+            .as_mut()
+            .ok_or(Error::NoSession)?
+            .do_send(&mut proto_tx)?;
+
+        let network = self.network.as_ref().ok_or(Error::NoNetworkInterface)?;
+        let peer = proto_tx.peer;
+        network.send(proto_tx.as_borrow_slice(), peer)?;
+        println!("Message Sent to {}", peer);
+        Ok(())
+    }
+
+    pub fn get_session_handle(&mut self, sess_idx: usize) -> SessionHandle {
+        SessionHandle {
+            sess_mgr: self,
+            sess_idx,
+        }
+    }
+}
+
+impl fmt::Display for SessionMgr {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        writeln!(f, "{{[")?;
+        for s in self.sessions.iter().flatten() {
+            writeln!(f, "{{ {}, }},", s)?;
+        }
+        write!(f, "], next_sess_id: {}", self.next_sess_id)?;
+        write!(f, "}}")
+    }
+}
+
+pub struct SessionHandle<'a> {
+    sess_mgr: &'a mut SessionMgr,
+    sess_idx: usize,
+}
+
+impl<'a> SessionHandle<'a> {
+    pub fn reserve_new_sess_id(&mut self) -> u16 {
+        self.sess_mgr.get_next_sess_id()
+    }
+
+    pub fn send(&mut self, proto_tx: BoxSlab<PacketPool>) -> Result<(), Error> {
+        self.sess_mgr.send(self.sess_idx, proto_tx)
+    }
+}
+
+impl<'a> Deref for SessionHandle<'a> {
+    type Target = Session;
+    fn deref(&self) -> &Self::Target {
+        // There is no other option but to panic if this is None
+        self.sess_mgr.sessions[self.sess_idx].as_ref().unwrap()
+    }
+}
+
+impl<'a> DerefMut for SessionHandle<'a> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        // There is no other option but to panic if this is None
+        self.sess_mgr.sessions[self.sess_idx].as_mut().unwrap()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use crate::transport::network::Address;
+
+    use super::SessionMgr;
+
+    #[test]
+    fn test_next_sess_id_doesnt_reuse() {
+        let mut sm = SessionMgr::new();
+        let sess_idx = sm.add(Address::default(), None).unwrap();
+        let mut sess = sm.get_session_handle(sess_idx);
+        sess.set_local_sess_id(1);
+        assert_eq!(sm.get_next_sess_id(), 2);
+        assert_eq!(sm.get_next_sess_id(), 3);
+        let sess_idx = sm.add(Address::default(), None).unwrap();
+        let mut sess = sm.get_session_handle(sess_idx);
+        sess.set_local_sess_id(4);
+        assert_eq!(sm.get_next_sess_id(), 5);
+    }
+
+    #[test]
+    fn test_next_sess_id_overflows() {
+        let mut sm = SessionMgr::new();
+        let sess_idx = sm.add(Address::default(), None).unwrap();
+        let mut sess = sm.get_session_handle(sess_idx);
+        sess.set_local_sess_id(1);
+        assert_eq!(sm.get_next_sess_id(), 2);
+        sm.next_sess_id = 65534;
+        assert_eq!(sm.get_next_sess_id(), 65534);
+        assert_eq!(sm.get_next_sess_id(), 65535);
+        assert_eq!(sm.get_next_sess_id(), 2);
+    }
+}

matter/src/transport/udp.rs

@@ -0,0 +1,57 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::*;
+use smol::net::{Ipv6Addr, UdpSocket};
+
+use super::network::{Address, NetworkInterface};
+
+// We could get rid of the smol here, but keeping it around in case we have to process
+// any other events in this thread's context
+pub struct UdpListener {
+    socket: UdpSocket,
+}
+
+// Currently matches with the one in connectedhomeip repo
+pub const MAX_RX_BUF_SIZE: usize = 1583;
+
+/* The Matter Port */
+pub const MATTER_PORT: u16 = 5540;
+
+impl UdpListener {
+    pub fn new() -> Result<UdpListener, Error> {
+        Ok(UdpListener {
+            socket: smol::block_on(UdpSocket::bind((Ipv6Addr::UNSPECIFIED, MATTER_PORT)))?,
+        })
+    }
+}
+
+impl NetworkInterface for UdpListener {
+    fn recv(&self, in_buf: &mut [u8]) -> Result<(usize, Address), Error> {
+        let (size, addr) = smol::block_on(self.socket.recv_from(in_buf)).map_err(|e| {
+            println!("Error on the network: {:?}", e);
+            Error::Network
+        })?;
+        Ok((size, Address::Udp(addr)))
+    }
+
+    fn send(&self, out_buf: &[u8], addr: Address) -> Result<usize, Error> {
+        match addr {
+            Address::Udp(addr) => Ok(smol::block_on(self.socket.send_to(out_buf, addr))?),
+        }
+    }
+}

matter/src/utils/mod.rs

@@ -0,0 +1,19 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod parsebuf;
+pub mod writebuf;

matter/src/utils/parsebuf.rs

@@ -0,0 +1,182 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::*;
+use byteorder::{ByteOrder, LittleEndian};
+
+pub struct ParseBuf<'a> {
+    buf: &'a mut [u8],
+    read_off: usize,
+    left: usize,
+}
+
+impl<'a> ParseBuf<'a> {
+    pub fn new(buf: &'a mut [u8], len: usize) -> ParseBuf<'a> {
+        ParseBuf {
+            buf: &mut buf[..len],
+            read_off: 0,
+            left: len,
+        }
+    }
+
+    pub fn set_len(&mut self, left: usize) {
+        self.left = left;
+    }
+
+    // Return the data that is valid as a slice, consume self
+    pub fn as_slice(self) -> &'a mut [u8] {
+        &mut self.buf[self.read_off..(self.read_off + self.left)]
+    }
+
+    // Return the data that is valid as a slice
+    pub fn as_borrow_slice(&mut self) -> &mut [u8] {
+        &mut self.buf[self.read_off..(self.read_off + self.left)]
+    }
+
+    pub fn parsed_as_slice(&self) -> &[u8] {
+        &self.buf[0..self.read_off]
+    }
+
+    pub fn tail(&mut self, size: usize) -> Result<&[u8], Error> {
+        if size <= self.left {
+            let end_offset = self.read_off + self.left;
+            let tail = &self.buf[(end_offset - size)..end_offset];
+            self.left -= size;
+            return Ok(tail);
+        }
+        Err(Error::TruncatedPacket)
+    }
+
+    fn advance(&mut self, len: usize) {
+        self.read_off += len;
+        self.left -= len;
+    }
+
+    pub fn parse_head_with<F, T>(&mut self, size: usize, f: F) -> Result<T, Error>
+    where
+        F: FnOnce(&mut Self) -> T,
+    {
+        if self.left >= size {
+            let data: T = f(self);
+            self.advance(size);
+            return Ok(data);
+        }
+        Err(Error::TruncatedPacket)
+    }
+
+    pub fn le_u8(&mut self) -> Result<u8, Error> {
+        self.parse_head_with(1, |x| x.buf[x.read_off])
+    }
+
+    pub fn le_u16(&mut self) -> Result<u16, Error> {
+        self.parse_head_with(2, |x| LittleEndian::read_u16(&x.buf[x.read_off..]))
+    }
+
+    pub fn le_u32(&mut self) -> Result<u32, Error> {
+        self.parse_head_with(4, |x| LittleEndian::read_u32(&x.buf[x.read_off..]))
+    }
+
+    pub fn le_u64(&mut self) -> Result<u64, Error> {
+        self.parse_head_with(8, |x| LittleEndian::read_u64(&x.buf[x.read_off..]))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::utils::parsebuf::*;
+
+    #[test]
+    fn test_parse_with_success() {
+        let mut test_slice: [u8; 11] = [0x01, 65, 0, 0xbe, 0xba, 0xfe, 0xca, 0xa, 0xb, 0xc, 0xd];
+        let mut buf = ParseBuf::new(&mut test_slice, 11);
+
+        assert_eq!(buf.le_u8().unwrap(), 0x01);
+        assert_eq!(buf.le_u16().unwrap(), 65);
+        assert_eq!(buf.le_u32().unwrap(), 0xcafebabe);
+        assert_eq!(buf.as_slice(), [0xa, 0xb, 0xc, 0xd]);
+    }
+
+    #[test]
+    fn test_parse_with_overrun() {
+        let mut test_slice: [u8; 2] = [0x01, 65];
+        let mut buf = ParseBuf::new(&mut test_slice, 2);
+
+        assert_eq!(buf.le_u8().unwrap(), 0x01);
+
+        match buf.le_u16() {
+            Ok(_) => panic!("This should have returned error"),
+            Err(_) => (),
+        }
+
+        match buf.le_u32() {
+            Ok(_) => panic!("This should have returned error"),
+            Err(_) => (),
+        }
+
+        // Now consume the leftover byte
+        assert_eq!(buf.le_u8().unwrap(), 65);
+
+        match buf.le_u8() {
+            Ok(_) => panic!("This should have returned error"),
+            Err(_) => (),
+        }
+        assert_eq!(buf.as_slice(), []);
+    }
+
+    #[test]
+    fn test_tail_with_success() {
+        let mut test_slice: [u8; 11] = [0x01, 65, 0, 0xbe, 0xba, 0xfe, 0xca, 0xa, 0xb, 0xc, 0xd];
+        let mut buf = ParseBuf::new(&mut test_slice, 11);
+
+        assert_eq!(buf.le_u8().unwrap(), 0x01);
+        assert_eq!(buf.le_u16().unwrap(), 65);
+        assert_eq!(buf.le_u32().unwrap(), 0xcafebabe);
+
+        assert_eq!(buf.tail(2).unwrap(), [0xc, 0xd]);
+        assert_eq!(buf.as_borrow_slice(), [0xa, 0xb]);
+
+        assert_eq!(buf.tail(2).unwrap(), [0xa, 0xb]);
+        assert_eq!(buf.as_slice(), []);
+    }
+
+    #[test]
+    fn test_tail_with_overrun() {
+        let mut test_slice: [u8; 11] = [0x01, 65, 0, 0xbe, 0xba, 0xfe, 0xca, 0xa, 0xb, 0xc, 0xd];
+        let mut buf = ParseBuf::new(&mut test_slice, 11);
+
+        assert_eq!(buf.le_u8().unwrap(), 0x01);
+        assert_eq!(buf.le_u16().unwrap(), 65);
+        assert_eq!(buf.le_u32().unwrap(), 0xcafebabe);
+        match buf.tail(5) {
+            Ok(_) => panic!("This should have returned error"),
+            Err(_) => (),
+        }
+        assert_eq!(buf.tail(2).unwrap(), [0xc, 0xd]);
+    }
+
+    #[test]
+    fn test_parsed_as_slice() {
+        let mut test_slice: [u8; 11] = [0x01, 65, 0, 0xbe, 0xba, 0xfe, 0xca, 0xa, 0xb, 0xc, 0xd];
+        let mut buf = ParseBuf::new(&mut test_slice, 11);
+
+        assert_eq!(buf.parsed_as_slice(), []);
+        assert_eq!(buf.le_u8().unwrap(), 0x1);
+        assert_eq!(buf.le_u16().unwrap(), 65);
+        assert_eq!(buf.le_u32().unwrap(), 0xcafebabe);
+        assert_eq!(buf.parsed_as_slice(), [0x01, 65, 0, 0xbe, 0xba, 0xfe, 0xca]);
+    }
+}

matter/src/utils/writebuf.rs

@@ -0,0 +1,326 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::error::*;
+use byteorder::{ByteOrder, LittleEndian};
+
+#[derive(Debug)]
+pub struct WriteBuf<'a> {
+    buf: &'a mut [u8],
+    start: usize,
+    end: usize,
+}
+
+impl<'a> WriteBuf<'a> {
+    pub fn new(buf: &'a mut [u8], len: usize) -> WriteBuf<'a> {
+        WriteBuf {
+            buf: &mut buf[..len],
+            start: 0,
+            end: 0,
+        }
+    }
+
+    pub fn get_tail(&self) -> usize {
+        self.end
+    }
+
+    pub fn rewind_tail_to(&mut self, new_end: usize) {
+        self.end = new_end;
+    }
+
+    pub fn forward_tail_by(&mut self, new_offset: usize) {
+        self.end += new_offset
+    }
+
+    pub fn as_borrow_slice(&self) -> &[u8] {
+        &self.buf[self.start..self.end]
+    }
+
+    pub fn as_slice(self) -> &'a [u8] {
+        &self.buf[self.start..self.end]
+    }
+
+    pub fn as_mut_slice(&mut self) -> &mut [u8] {
+        &mut self.buf[self.start..self.end]
+    }
+
+    pub fn empty_as_mut_slice(&mut self) -> &mut [u8] {
+        &mut self.buf[self.end..]
+    }
+
+    pub fn reset(&mut self, reserve: usize) {
+        self.start = reserve;
+        self.end = reserve;
+    }
+
+    pub fn reserve(&mut self, reserve: usize) -> Result<(), Error> {
+        if self.end != 0 || self.start != 0 {
+            return Err(Error::Invalid);
+        }
+        self.reset(reserve);
+        Ok(())
+    }
+
+    pub fn prepend_with<F>(&mut self, size: usize, f: F) -> Result<(), Error>
+    where
+        F: FnOnce(&mut Self),
+    {
+        if size <= self.start {
+            f(self);
+            self.start -= size;
+            return Ok(());
+        }
+        Err(Error::NoSpace)
+    }
+
+    pub fn prepend(&mut self, src: &[u8]) -> Result<(), Error> {
+        self.prepend_with(src.len(), |x| {
+            let dst_slice = &mut x.buf[(x.start - src.len())..x.start];
+            dst_slice.copy_from_slice(src);
+        })
+    }
+
+    pub fn append_with<F>(&mut self, size: usize, f: F) -> Result<(), Error>
+    where
+        F: FnOnce(&mut Self),
+    {
+        if self.end + size <= self.buf.len() {
+            f(self);
+            self.end += size;
+            return Ok(());
+        }
+        Err(Error::NoSpace)
+    }
+
+    pub fn append(&mut self, src: &[u8]) -> Result<(), Error> {
+        self.copy_from_slice(src)
+    }
+
+    pub fn copy_from_slice(&mut self, src: &[u8]) -> Result<(), Error> {
+        self.append_with(src.len(), |x| {
+            x.buf[x.end..(x.end + src.len())].copy_from_slice(src);
+        })
+    }
+
+    pub fn le_i8(&mut self, data: i8) -> Result<(), Error> {
+        self.le_u8(data as u8)
+    }
+
+    pub fn le_u8(&mut self, data: u8) -> Result<(), Error> {
+        self.append_with(1, |x| {
+            x.buf[x.end] = data;
+        })
+    }
+
+    pub fn le_u16(&mut self, data: u16) -> Result<(), Error> {
+        self.append_with(2, |x| {
+            LittleEndian::write_u16(&mut x.buf[x.end..], data);
+        })
+    }
+
+    pub fn le_u32(&mut self, data: u32) -> Result<(), Error> {
+        self.append_with(4, |x| {
+            LittleEndian::write_u32(&mut x.buf[x.end..], data);
+        })
+    }
+
+    pub fn le_u64(&mut self, data: u64) -> Result<(), Error> {
+        self.append_with(8, |x| {
+            LittleEndian::write_u64(&mut x.buf[x.end..], data);
+        })
+    }
+
+    pub fn le_uint(&mut self, nbytes: usize, data: u64) -> Result<(), Error> {
+        self.append_with(nbytes, |x| {
+            LittleEndian::write_uint(&mut x.buf[x.end..], data, nbytes);
+        })
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::utils::writebuf::*;
+
+    #[test]
+    fn test_append_le_with_success() {
+        let mut test_slice: [u8; 22] = [0; 22];
+        let test_slice_len = test_slice.len();
+        let mut buf = WriteBuf::new(&mut test_slice, test_slice_len);
+        buf.reserve(5).unwrap();
+
+        buf.le_u8(1).unwrap();
+        buf.le_u16(65).unwrap();
+        buf.le_u32(0xcafebabe).unwrap();
+        buf.le_u64(0xcafebabecafebabe).unwrap();
+        buf.le_uint(2, 64).unwrap();
+        assert_eq!(
+            test_slice,
+            [
+                0, 0, 0, 0, 0, 1, 65, 0, 0xbe, 0xba, 0xfe, 0xca, 0xbe, 0xba, 0xfe, 0xca, 0xbe,
+                0xba, 0xfe, 0xca, 64, 0
+            ]
+        );
+    }
+
+    #[test]
+    fn test_len_param() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 5);
+        buf.reserve(5).unwrap();
+
+        let _ = buf.le_u8(1);
+        let _ = buf.le_u16(65);
+        let _ = buf.le_u32(0xcafebabe);
+        let _ = buf.le_u64(0xcafebabecafebabe);
+        // All of the above must return error, and hence the slice shouldn't change
+        assert_eq!(test_slice, [0; 20]);
+    }
+
+    #[test]
+    fn test_overrun() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 20);
+        buf.reserve(4).unwrap();
+        buf.le_u64(0xcafebabecafebabe).unwrap();
+        buf.le_u64(0xcafebabecafebabe).unwrap();
+        // Now the buffer is fully filled up, so no further puts will happen
+
+        match buf.le_u8(1) {
+            Ok(_) => panic!("Should return error"),
+            _ => (),
+        }
+
+        match buf.le_u16(65) {
+            Ok(_) => panic!("Should return error"),
+            _ => (),
+        }
+
+        match buf.le_u32(0xcafebabe) {
+            Ok(_) => panic!("Should return error"),
+            _ => (),
+        }
+
+        match buf.le_u64(0xcafebabecafebabe) {
+            Ok(_) => panic!("Should return error"),
+            _ => (),
+        }
+    }
+
+    #[test]
+    fn test_as_slice() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 20);
+        buf.reserve(5).unwrap();
+
+        buf.le_u8(1).unwrap();
+        buf.le_u16(65).unwrap();
+        buf.le_u32(0xcafebabe).unwrap();
+        buf.le_u64(0xcafebabecafebabe).unwrap();
+
+        let new_slice: [u8; 3] = [0xa, 0xb, 0xc];
+        buf.prepend(&new_slice).unwrap();
+
+        assert_eq!(
+            buf.as_slice(),
+            [
+                0xa, 0xb, 0xc, 1, 65, 0, 0xbe, 0xba, 0xfe, 0xca, 0xbe, 0xba, 0xfe, 0xca, 0xbe,
+                0xba, 0xfe, 0xca
+            ]
+        );
+    }
+
+    #[test]
+    fn test_copy_as_slice() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 20);
+        buf.reserve(5).unwrap();
+
+        buf.le_u16(65).unwrap();
+        let new_slice: [u8; 5] = [0xaa, 0xbb, 0xcc, 0xdd, 0xee];
+        buf.copy_from_slice(&new_slice).unwrap();
+        buf.le_u32(65).unwrap();
+        assert_eq!(
+            test_slice,
+            [0, 0, 0, 0, 0, 65, 0, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 65, 0, 0, 0, 0, 0, 0, 0]
+        );
+    }
+
+    #[test]
+    fn test_copy_as_slice_overrun() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 7);
+        buf.reserve(5).unwrap();
+
+        buf.le_u16(65).unwrap();
+        let new_slice: [u8; 5] = [0xaa, 0xbb, 0xcc, 0xdd, 0xee];
+        match buf.copy_from_slice(&new_slice) {
+            Ok(_) => panic!("This should have returned error"),
+            Err(_) => (),
+        }
+    }
+
+    #[test]
+    fn test_prepend() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 20);
+        buf.reserve(5).unwrap();
+
+        buf.le_u16(65).unwrap();
+        let new_slice: [u8; 5] = [0xaa, 0xbb, 0xcc, 0xdd, 0xee];
+        buf.prepend(&new_slice).unwrap();
+        assert_eq!(
+            test_slice,
+            [0xaa, 0xbb, 0xcc, 0xdd, 0xee, 65, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+        );
+    }
+
+    #[test]
+    fn test_prepend_overrun() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 20);
+        buf.reserve(5).unwrap();
+
+        buf.le_u16(65).unwrap();
+        let new_slice: [u8; 6] = [0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff];
+        match buf.prepend(&new_slice) {
+            Ok(_) => panic!("Prepend should return error"),
+            Err(_) => (),
+        }
+    }
+
+    #[test]
+    fn test_rewind_tail() {
+        let mut test_slice: [u8; 20] = [0; 20];
+        let mut buf = WriteBuf::new(&mut test_slice, 20);
+        buf.reserve(5).unwrap();
+
+        buf.le_u16(65).unwrap();
+
+        let anchor = buf.get_tail();
+
+        let new_slice: [u8; 5] = [0xaa, 0xbb, 0xcc, 0xdd, 0xee];
+        buf.copy_from_slice(&new_slice).unwrap();
+        assert_eq!(
+            buf.as_borrow_slice(),
+            [65, 0, 0xaa, 0xbb, 0xcc, 0xdd, 0xee,]
+        );
+
+        buf.rewind_tail_to(anchor);
+        buf.le_u16(66).unwrap();
+        assert_eq!(buf.as_borrow_slice(), [65, 0, 66, 0,]);
+    }
+}

matter/tests/common/attributes.rs

@@ -0,0 +1,77 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use matter::interaction_model::{messages::ib::AttrResp, messages::msg::ReportDataMsg};
+
+/// Assert that the data received in the outbuf matches our expectations
+pub fn assert_attr_report(received: &ReportDataMsg, expected: &[AttrResp]) {
+    let mut index = 0;
+
+    for inv_response in received.attr_reports.as_ref().unwrap().iter() {
+        println!("Validating index {}", index);
+        match expected[index] {
+            AttrResp::Data(e_d) => match inv_response {
+                AttrResp::Data(d) => {
+                    // We don't match the data-version
+                    assert_eq!(e_d.path, d.path);
+                    assert_eq!(e_d.data, d.data);
+                }
+                _ => {
+                    panic!("Invalid response, expected AttrRespIn::Data");
+                }
+            },
+            AttrResp::Status(e_s) => match inv_response {
+                AttrResp::Status(s) => {
+                    assert_eq!(e_s, s);
+                }
+                _ => {
+                    panic!("Invalid response, expected AttrRespIn::Status");
+                }
+            },
+        }
+        println!("Index {} success", index);
+        index += 1;
+    }
+    assert_eq!(index, expected.len());
+}
+
+// We have to hard-code this here, and it should match the tag
+// of the 'data' part in AttrData
+pub const ATTR_DATA_TAG_DATA: u8 = 2;
+
+#[macro_export]
+macro_rules! attr_data {
+    ($path:expr, $data:expr) => {
+        AttrResp::Data(AttrData {
+            data_ver: None,
+            path: AttrPath {
+                endpoint: $path.endpoint,
+                cluster: $path.cluster,
+                attr: $path.leaf.map(|x| x as u16),
+                ..Default::default()
+            },
+            data: EncodeValue::Tlv(TLVElement::new(TagType::Context(ATTR_DATA_TAG_DATA), $data)),
+        })
+    };
+}
+
+#[macro_export]
+macro_rules! attr_status {
+    ($path:expr, $status:expr) => {
+        AttrResp::Status(AttrStatus::new($path, $status, 0))
+    };
+}

matter/tests/common/echo_cluster.rs

@@ -0,0 +1,250 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use std::sync::{Arc, Mutex, Once};
+
+use matter::{
+    data_model::objects::{
+        Access, AttrDetails, AttrValue, Attribute, Cluster, ClusterType, EncodeValue, Encoder,
+        Quality,
+    },
+    error::Error,
+    interaction_model::{
+        command::CommandReq,
+        core::IMStatusCode,
+        messages::ib::{self, attr_list_write, ListOperation},
+    },
+    tlv::{TLVElement, TLVWriter, TagType, ToTLV},
+};
+use num_derive::FromPrimitive;
+
+pub const ID: u32 = 0xABCD;
+
+#[derive(FromPrimitive)]
+pub enum Commands {
+    EchoReq = 0x00,
+    EchoResp = 0x01,
+}
+
+/// This is used in the tests to validate any settings that may have happened
+/// to the custom data parts of the cluster
+pub struct TestChecker {
+    pub write_list: [Option<u16>; WRITE_LIST_MAX],
+}
+
+static mut G_TEST_CHECKER: Option<Arc<Mutex<TestChecker>>> = None;
+static INIT: Once = Once::new();
+
+impl TestChecker {
+    fn new() -> Self {
+        Self {
+            write_list: [None; WRITE_LIST_MAX],
+        }
+    }
+
+    /// Get a handle to the globally unique mDNS instance
+    pub fn get() -> Result<Arc<Mutex<Self>>, Error> {
+        unsafe {
+            INIT.call_once(|| {
+                G_TEST_CHECKER = Some(Arc::new(Mutex::new(Self::new())));
+            });
+            Ok(G_TEST_CHECKER.as_ref().ok_or(Error::Invalid)?.clone())
+        }
+    }
+}
+
+pub const WRITE_LIST_MAX: usize = 5;
+pub struct EchoCluster {
+    pub base: Cluster,
+    pub multiplier: u8,
+}
+
+#[derive(FromPrimitive)]
+pub enum Attributes {
+    Att1 = 0,
+    Att2 = 1,
+    AttWrite = 2,
+    AttCustom = 3,
+    AttWriteList = 4,
+}
+
+pub const ATTR_CUSTOM_VALUE: u32 = 0xcafebeef;
+pub const ATTR_WRITE_DEFAULT_VALUE: u16 = 0xcafe;
+
+impl ClusterType for EchoCluster {
+    fn base(&self) -> &Cluster {
+        &self.base
+    }
+
+    fn base_mut(&mut self) -> &mut Cluster {
+        &mut self.base
+    }
+
+    fn read_custom_attribute(&self, encoder: &mut dyn Encoder, attr: &AttrDetails) {
+        match num::FromPrimitive::from_u16(attr.attr_id) {
+            Some(Attributes::AttCustom) => encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                let _ = tw.u32(tag, ATTR_CUSTOM_VALUE);
+            })),
+            Some(Attributes::AttWriteList) => {
+                let tc_handle = TestChecker::get().unwrap();
+                let tc = tc_handle.lock().unwrap();
+                encoder.encode(EncodeValue::Closure(&|tag, tw| {
+                    let _ = tw.start_array(tag);
+                    for i in tc.write_list.iter().flatten() {
+                        let _ = tw.u16(TagType::Anonymous, *i);
+                    }
+                    let _ = tw.end_container();
+                }))
+            }
+            _ => (),
+        }
+    }
+
+    fn write_attribute(
+        &mut self,
+        attr: &AttrDetails,
+        data: &TLVElement,
+    ) -> Result<(), IMStatusCode> {
+        match num::FromPrimitive::from_u16(attr.attr_id) {
+            Some(Attributes::AttWriteList) => {
+                attr_list_write(attr, data, |op, data| self.write_attr_list(op, data))
+            }
+            _ => self.base.write_attribute_from_tlv(attr.attr_id, data),
+        }
+    }
+
+    fn handle_command(&mut self, cmd_req: &mut CommandReq) -> Result<(), IMStatusCode> {
+        let cmd = cmd_req
+            .cmd
+            .path
+            .leaf
+            .map(|c| num::FromPrimitive::from_u32(c))
+            .ok_or(IMStatusCode::UnsupportedCommand)?
+            .ok_or(IMStatusCode::UnsupportedCommand)?;
+        match cmd {
+            // This will generate an echo response on the same endpoint
+            // with data multiplied by the multiplier
+            Commands::EchoReq => {
+                let a = cmd_req.data.u8().unwrap();
+                let mut echo_response = cmd_req.cmd;
+                echo_response.path.leaf = Some(Commands::EchoResp as u32);
+
+                let cmd_data = |tag: TagType, t: &mut TLVWriter| {
+                    let _ = t.start_struct(tag);
+                    // Echo = input * self.multiplier
+                    let _ = t.u8(TagType::Context(0), a * self.multiplier);
+                    let _ = t.end_container();
+                };
+
+                let invoke_resp = ib::InvResp::Cmd(ib::CmdData::new(
+                    echo_response,
+                    EncodeValue::Closure(&cmd_data),
+                ));
+                let _ = invoke_resp.to_tlv(cmd_req.resp, TagType::Anonymous);
+                cmd_req.trans.complete();
+            }
+            _ => {
+                return Err(IMStatusCode::UnsupportedCommand);
+            }
+        }
+        Ok(())
+    }
+}
+
+impl EchoCluster {
+    pub fn new(multiplier: u8) -> Result<Box<Self>, Error> {
+        let mut c = Box::new(Self {
+            base: Cluster::new(ID)?,
+            multiplier,
+        });
+        c.base.add_attribute(Attribute::new(
+            Attributes::Att1 as u16,
+            AttrValue::Uint16(0x1234),
+            Access::RV,
+            Quality::NONE,
+        )?)?;
+        c.base.add_attribute(Attribute::new(
+            Attributes::Att2 as u16,
+            AttrValue::Uint16(0x5678),
+            Access::RV,
+            Quality::NONE,
+        )?)?;
+        c.base.add_attribute(Attribute::new(
+            Attributes::AttWrite as u16,
+            AttrValue::Uint16(ATTR_WRITE_DEFAULT_VALUE),
+            Access::WRITE | Access::NEED_ADMIN,
+            Quality::NONE,
+        )?)?;
+        c.base.add_attribute(Attribute::new(
+            Attributes::AttCustom as u16,
+            AttrValue::Custom,
+            Access::READ | Access::NEED_VIEW,
+            Quality::NONE,
+        )?)?;
+        c.base.add_attribute(Attribute::new(
+            Attributes::AttWriteList as u16,
+            AttrValue::Custom,
+            Access::WRITE | Access::NEED_ADMIN,
+            Quality::NONE,
+        )?)?;
+        Ok(c)
+    }
+
+    fn write_attr_list(
+        &mut self,
+        op: ListOperation,
+        data: &TLVElement,
+    ) -> Result<(), IMStatusCode> {
+        let tc_handle = TestChecker::get().unwrap();
+        let mut tc = tc_handle.lock().unwrap();
+        match op {
+            ListOperation::AddItem => {
+                let data = data.u16().map_err(|_| IMStatusCode::Failure)?;
+                for i in 0..WRITE_LIST_MAX {
+                    if tc.write_list[i].is_none() {
+                        tc.write_list[i] = Some(data);
+                        return Ok(());
+                    }
+                }
+                Err(IMStatusCode::ResourceExhausted)
+            }
+            ListOperation::EditItem(index) => {
+                let data = data.u16().map_err(|_| IMStatusCode::Failure)?;
+                if tc.write_list[index as usize].is_some() {
+                    tc.write_list[index as usize] = Some(data);
+                    Ok(())
+                } else {
+                    Err(IMStatusCode::InvalidAction)
+                }
+            }
+            ListOperation::DeleteItem(index) => {
+                if tc.write_list[index as usize].is_some() {
+                    tc.write_list[index as usize] = None;
+                    Ok(())
+                } else {
+                    Err(IMStatusCode::InvalidAction)
+                }
+            }
+            ListOperation::DeleteList => {
+                for i in 0..WRITE_LIST_MAX {
+                    tc.write_list[i] = None;
+                }
+                Ok(())
+            }
+        }
+    }
+}

matter/tests/common/im_engine.rs

@@ -0,0 +1,201 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use crate::common::echo_cluster;
+use boxslab::Slab;
+use matter::{
+    acl::{AclEntry, AclMgr, AuthMode},
+    data_model::{
+        cluster_basic_information::BasicInfoConfig,
+        core::DataModel,
+        device_types::device_type_add_on_off_light,
+        objects::Privilege,
+        sdm::dev_att::{DataType, DevAttDataFetcher},
+    },
+    error::Error,
+    fabric::FabricMgr,
+    interaction_model::{core::OpCode, messages::ib::CmdPath, messages::msg, InteractionModel},
+    tlv::{TLVWriter, TagType, ToTLV},
+    transport::packet::Packet,
+    transport::proto_demux::HandleProto,
+    transport::{
+        exchange::{self, Exchange, ExchangeCtx},
+        network::Address,
+        packet::PacketPool,
+        proto_demux::ProtoCtx,
+        session::{CloneData, SessionMgr, SessionMode},
+    },
+    utils::writebuf::WriteBuf,
+};
+use std::{
+    net::{Ipv4Addr, SocketAddr},
+    sync::Arc,
+};
+
+pub struct DummyDevAtt {}
+impl DevAttDataFetcher for DummyDevAtt {
+    fn get_devatt_data(&self, _data_type: DataType, _data: &mut [u8]) -> Result<usize, Error> {
+        Ok(2)
+    }
+}
+
+/// An Interaction Model Engine to facilitate easy testing
+pub struct ImEngine {
+    pub dm: DataModel,
+    pub acl_mgr: Arc<AclMgr>,
+    pub im: Box<InteractionModel>,
+}
+
+pub struct ImInput<'a> {
+    action: OpCode,
+    data_in: &'a [u8],
+    peer_id: u64,
+}
+
+pub const IM_ENGINE_PEER_ID: u64 = 445566;
+impl<'a> ImInput<'a> {
+    pub fn new(action: OpCode, data_in: &'a [u8]) -> Self {
+        Self {
+            action,
+            data_in,
+            peer_id: IM_ENGINE_PEER_ID,
+        }
+    }
+
+    pub fn set_peer_node_id(&mut self, peer: u64) {
+        self.peer_id = peer;
+    }
+}
+
+impl ImEngine {
+    /// Create the interaction model engine
+    pub fn new() -> Self {
+        let dev_det = BasicInfoConfig {
+            vid: 10,
+            pid: 11,
+            hw_ver: 12,
+            sw_ver: 13,
+        };
+        let dev_att = Box::new(DummyDevAtt {});
+        let fabric_mgr = Arc::new(FabricMgr::new().unwrap());
+        let acl_mgr = Arc::new(AclMgr::new_with(false).unwrap());
+        acl_mgr.erase_all();
+        let mut default_acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+        // Only allow the standard peer node id of the IM Engine
+        default_acl.add_subject(IM_ENGINE_PEER_ID).unwrap();
+        acl_mgr.add(default_acl).unwrap();
+        let dm = DataModel::new(dev_det, dev_att, fabric_mgr.clone(), acl_mgr.clone()).unwrap();
+
+        {
+            let mut d = dm.node.write().unwrap();
+            let light_endpoint = device_type_add_on_off_light(&mut d).unwrap();
+            d.add_cluster(0, echo_cluster::EchoCluster::new(2).unwrap())
+                .unwrap();
+            d.add_cluster(light_endpoint, echo_cluster::EchoCluster::new(3).unwrap())
+                .unwrap();
+        }
+
+        let im = Box::new(InteractionModel::new(Box::new(dm.clone())));
+
+        Self { dm, acl_mgr, im }
+    }
+
+    /// Run a transaction through the interaction model engine
+    pub fn process(&mut self, input: &ImInput, data_out: &mut [u8]) -> usize {
+        let mut exch = Exchange::new(1, 0, exchange::Role::Responder);
+
+        let mut sess_mgr: SessionMgr = Default::default();
+
+        let clone_data = CloneData::new(
+            123456,
+            input.peer_id,
+            10,
+            30,
+            Address::Udp(SocketAddr::new(
+                std::net::IpAddr::V4(Ipv4Addr::new(127, 0, 0, 1)),
+                5542,
+            )),
+            SessionMode::Case(1),
+        );
+        let sess_idx = sess_mgr.clone_session(&clone_data).unwrap();
+        let sess = sess_mgr.get_session_handle(sess_idx);
+        let exch_ctx = ExchangeCtx {
+            exch: &mut exch,
+            sess,
+        };
+        let mut rx = Slab::<PacketPool>::new(Packet::new_rx().unwrap()).unwrap();
+        let tx = Slab::<PacketPool>::new(Packet::new_tx().unwrap()).unwrap();
+        // Create fake rx packet
+        rx.set_proto_id(0x01);
+        rx.set_proto_opcode(input.action as u8);
+        rx.peer = Address::default();
+        let in_data_len = input.data_in.len();
+        let rx_buf = rx.as_borrow_slice();
+        rx_buf[..in_data_len].copy_from_slice(input.data_in);
+        rx.get_parsebuf().unwrap().set_len(in_data_len);
+
+        let mut ctx = ProtoCtx::new(exch_ctx, rx, tx);
+        self.im.handle_proto_id(&mut ctx).unwrap();
+        let out_data_len = ctx.tx.as_borrow_slice().len();
+        data_out[..out_data_len].copy_from_slice(ctx.tx.as_borrow_slice());
+        out_data_len
+    }
+}
+
+// Create an Interaction Model, Data Model and run a rx/tx transaction through it
+pub fn im_engine(action: OpCode, data_in: &[u8], data_out: &mut [u8]) -> (DataModel, usize) {
+    let mut engine = ImEngine::new();
+    let input = ImInput::new(action, data_in);
+    let output_len = engine.process(&input, data_out);
+    (engine.dm, output_len)
+}
+
+pub struct TestData<'a, 'b> {
+    tw: TLVWriter<'a, 'b>,
+}
+
+impl<'a, 'b> TestData<'a, 'b> {
+    pub fn new(buf: &'b mut WriteBuf<'a>) -> Self {
+        Self {
+            tw: TLVWriter::new(buf),
+        }
+    }
+
+    pub fn commands(&mut self, cmds: &[(CmdPath, Option<u8>)]) -> Result<(), Error> {
+        self.tw.start_struct(TagType::Anonymous)?;
+        self.tw.bool(
+            TagType::Context(msg::InvReqTag::SupressResponse as u8),
+            false,
+        )?;
+        self.tw
+            .bool(TagType::Context(msg::InvReqTag::TimedReq as u8), false)?;
+        self.tw
+            .start_array(TagType::Context(msg::InvReqTag::InvokeRequests as u8))?;
+
+        for (cmd, data) in cmds {
+            self.tw.start_struct(TagType::Anonymous)?;
+            cmd.to_tlv(&mut self.tw, TagType::Context(0))?;
+            if let Some(d) = *data {
+                self.tw.u8(TagType::Context(1), d)?;
+            }
+            self.tw.end_container()?;
+        }
+
+        self.tw.end_container()?;
+        self.tw.end_container()
+    }
+}

matter/tests/common/mod.rs

@@ -0,0 +1,20 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+pub mod attributes;
+pub mod echo_cluster;
+pub mod im_engine;

matter/tests/data_model/acl_and_dataver.rs

@@ -0,0 +1,685 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use matter::{
+    acl::{AclEntry, AuthMode, Target},
+    data_model::{
+        objects::{AttrValue, EncodeValue, Privilege},
+        system_model::access_control,
+    },
+    interaction_model::{
+        core::{IMStatusCode, OpCode},
+        messages::{
+            ib::{AttrData, AttrPath, AttrResp, AttrStatus, ClusterPath, DataVersionFilter},
+            msg::{ReadReq, ReportDataMsg, WriteReq},
+        },
+        messages::{msg, GenericPath},
+    },
+    tlv::{self, ElementType, FromTLV, TLVArray, TLVElement, TLVWriter, TagType, ToTLV},
+    utils::writebuf::WriteBuf,
+};
+
+use crate::{
+    attr_data, attr_status,
+    common::{
+        attributes::*,
+        echo_cluster::{self, ATTR_WRITE_DEFAULT_VALUE},
+        im_engine::{ImEngine, ImInput},
+    },
+};
+
+// Helper for handling Read Req sequences for this file
+fn handle_read_reqs(
+    im: &mut ImEngine,
+    peer_node_id: u64,
+    input: &[AttrPath],
+    expected: &[AttrResp],
+) {
+    let mut out_buf = [0u8; 400];
+    let received = gen_read_reqs_output(im, peer_node_id, input, None, &mut out_buf);
+    assert_attr_report(&received, expected)
+}
+
+fn gen_read_reqs_output<'a>(
+    im: &mut ImEngine,
+    peer_node_id: u64,
+    input: &[AttrPath],
+    dataver_filters: Option<TLVArray<'a, DataVersionFilter>>,
+    out_buf: &'a mut [u8],
+) -> ReportDataMsg<'a> {
+    let mut buf = [0u8; 400];
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(&mut buf, buf_len);
+    let mut tw = TLVWriter::new(&mut wb);
+
+    let mut read_req = ReadReq::new(true).set_attr_requests(input);
+    read_req.dataver_filters = dataver_filters;
+    read_req.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+
+    let mut input = ImInput::new(OpCode::ReadRequest, wb.as_borrow_slice());
+    input.set_peer_node_id(peer_node_id);
+
+    let out_buf_len = im.process(&input, out_buf);
+    let out_buf = &out_buf[..out_buf_len];
+
+    tlv::print_tlv_list(out_buf);
+    let root = tlv::get_root_node_struct(out_buf).unwrap();
+    ReportDataMsg::from_tlv(&root).unwrap()
+}
+
+// Helper for handling Write Attribute sequences
+fn handle_write_reqs(
+    im: &mut ImEngine,
+    peer_node_id: u64,
+    input: &[AttrData],
+    expected: &[AttrStatus],
+) {
+    let mut buf = [0u8; 400];
+    let mut out_buf = [0u8; 400];
+
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(&mut buf, buf_len);
+    let mut tw = TLVWriter::new(&mut wb);
+
+    let write_req = WriteReq::new(false, input);
+    write_req.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+
+    let mut input = ImInput::new(OpCode::WriteRequest, wb.as_borrow_slice());
+    input.set_peer_node_id(peer_node_id);
+    let out_buf_len = im.process(&input, &mut out_buf);
+
+    let out_buf = &out_buf[..out_buf_len];
+    tlv::print_tlv_list(out_buf);
+    let root = tlv::get_root_node_struct(out_buf).unwrap();
+
+    let mut index = 0;
+    let response_iter = root
+        .find_tag(msg::WriteRespTag::WriteResponses as u32)
+        .unwrap()
+        .confirm_array()
+        .unwrap()
+        .enter()
+        .unwrap();
+    for response in response_iter {
+        println!("Validating index {}", index);
+        let status = AttrStatus::from_tlv(&response).unwrap();
+        assert_eq!(expected[index], status);
+        println!("Index {} success", index);
+        index += 1;
+    }
+    assert_eq!(index, expected.len());
+}
+
+#[test]
+/// Ensure that wildcard read attributes don't include error response
+/// and silently drop the data when access is not granted
+fn wc_read_attribute() {
+    let _ = env_logger::try_init();
+
+    let wc_att1 = GenericPath::new(
+        None,
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let ep0_att1 = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let ep1_att1 = GenericPath::new(
+        Some(1),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Test1: Empty Response as no ACL matches
+    let input = &[AttrPath::new(&wc_att1)];
+    let expected = &[];
+    handle_read_reqs(&mut im, peer, input, expected);
+
+    // Add ACL to allow our peer to only access endpoint 0
+    let mut acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    acl.add_target(Target::new(Some(0), None, None)).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test2: Only Single response as only single endpoint is allowed
+    let input = &[AttrPath::new(&wc_att1)];
+    let expected = &[attr_data!(ep0_att1, ElementType::U16(0x1234))];
+    handle_read_reqs(&mut im, peer, input, expected);
+
+    // Add ACL to allow our peer to only access endpoint 1
+    let mut acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    acl.add_target(Target::new(Some(1), None, None)).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test3: Both responses are valid
+    let input = &[AttrPath::new(&wc_att1)];
+    let expected = &[
+        attr_data!(ep0_att1, ElementType::U16(0x1234)),
+        attr_data!(ep1_att1, ElementType::U16(0x1234)),
+    ];
+    handle_read_reqs(&mut im, peer, input, expected);
+}
+
+#[test]
+/// Ensure that exact read attribute includes error response
+/// when access is not granted
+fn exact_read_attribute() {
+    let _ = env_logger::try_init();
+
+    let wc_att1 = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let ep0_att1 = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Test1: Unsupported Access error as no ACL matches
+    let input = &[AttrPath::new(&wc_att1)];
+    let expected = &[attr_status!(&ep0_att1, IMStatusCode::UnsupportedAccess)];
+    handle_read_reqs(&mut im, peer, input, expected);
+
+    // Add ACL to allow our peer to access any endpoint
+    let mut acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test2: Only Single response as only single endpoint is allowed
+    let input = &[AttrPath::new(&wc_att1)];
+    let expected = &[attr_data!(ep0_att1, ElementType::U16(0x1234))];
+    handle_read_reqs(&mut im, peer, input, expected);
+}
+
+fn read_cluster_id_write_attr(im: &ImEngine, endpoint: u16) -> AttrValue {
+    let node = im.dm.node.read().unwrap();
+    let echo = node.get_cluster(endpoint, echo_cluster::ID).unwrap();
+
+    *echo
+        .base()
+        .read_attribute_raw(echo_cluster::Attributes::AttWrite as u16)
+        .unwrap()
+}
+
+fn read_cluster_id_data_ver(im: &ImEngine, endpoint: u16) -> u32 {
+    let node = im.dm.node.read().unwrap();
+    let echo = node.get_cluster(endpoint, echo_cluster::ID).unwrap();
+
+    echo.base().get_dataver()
+}
+
+#[test]
+/// Ensure that an write attribute with a wildcard either performs the operation,
+/// if allowed, or silently drops the request
+fn wc_write_attribute() {
+    let _ = env_logger::try_init();
+    let val0 = 10;
+    let val1 = 20;
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+    let attr_data1 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val1);
+    };
+
+    let wc_att = GenericPath::new(
+        None,
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let ep0_att = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let ep1_att = GenericPath::new(
+        Some(1),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+
+    let input0 = &[AttrData::new(
+        None,
+        AttrPath::new(&wc_att),
+        EncodeValue::Closure(&attr_data0),
+    )];
+    let input1 = &[AttrData::new(
+        None,
+        AttrPath::new(&wc_att),
+        EncodeValue::Closure(&attr_data1),
+    )];
+
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Test 1: Wildcard write to an attribute without permission should return
+    // no error
+    handle_write_reqs(&mut im, peer, input0, &[]);
+    {
+        let node = im.dm.node.read().unwrap();
+        let echo = node.get_cluster(0, echo_cluster::ID).unwrap();
+        assert_eq!(
+            AttrValue::Uint16(ATTR_WRITE_DEFAULT_VALUE),
+            *echo
+                .base()
+                .read_attribute_raw(echo_cluster::Attributes::AttWrite as u16)
+                .unwrap()
+        );
+    }
+
+    // Add ACL to allow our peer to access one endpoint
+    let mut acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    acl.add_target(Target::new(Some(0), None, None)).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test 2: Wildcard write to attributes will only return attributes
+    // where the writes were successful
+    handle_write_reqs(
+        &mut im,
+        peer,
+        input0,
+        &[AttrStatus::new(&ep0_att, IMStatusCode::Sucess, 0)],
+    );
+    assert_eq!(AttrValue::Uint16(val0), read_cluster_id_write_attr(&im, 0));
+    assert_eq!(
+        AttrValue::Uint16(ATTR_WRITE_DEFAULT_VALUE),
+        read_cluster_id_write_attr(&im, 1)
+    );
+
+    // Add ACL to allow our peer to access another endpoint
+    let mut acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    acl.add_target(Target::new(Some(1), None, None)).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test 3: Wildcard write to attributes will return multiple attributes
+    // where the writes were successful
+    handle_write_reqs(
+        &mut im,
+        peer,
+        input1,
+        &[
+            AttrStatus::new(&ep0_att, IMStatusCode::Sucess, 0),
+            AttrStatus::new(&ep1_att, IMStatusCode::Sucess, 0),
+        ],
+    );
+    assert_eq!(AttrValue::Uint16(val1), read_cluster_id_write_attr(&im, 0));
+    assert_eq!(AttrValue::Uint16(val1), read_cluster_id_write_attr(&im, 1));
+}
+
+#[test]
+/// Ensure that an write attribute without a wildcard returns an error when the
+/// ACL disallows the access, and returns success once access is granted
+fn exact_write_attribute() {
+    let _ = env_logger::try_init();
+    let val0 = 10;
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+
+    let ep0_att = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+
+    let input = &[AttrData::new(
+        None,
+        AttrPath::new(&ep0_att),
+        EncodeValue::Closure(&attr_data0),
+    )];
+    let expected_fail = &[AttrStatus::new(
+        &ep0_att,
+        IMStatusCode::UnsupportedAccess,
+        0,
+    )];
+    let expected_success = &[AttrStatus::new(&ep0_att, IMStatusCode::Sucess, 0)];
+
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Test 1: Exact write to an attribute without permission should return
+    // Unsupported Access Error
+    handle_write_reqs(&mut im, peer, input, expected_fail);
+    assert_eq!(
+        AttrValue::Uint16(ATTR_WRITE_DEFAULT_VALUE),
+        read_cluster_id_write_attr(&im, 0)
+    );
+
+    // Add ACL to allow our peer to access any endpoint
+    let mut acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test 1: Exact write to an attribute with permission should grant
+    // access
+    handle_write_reqs(&mut im, peer, input, expected_success);
+    assert_eq!(AttrValue::Uint16(val0), read_cluster_id_write_attr(&im, 0));
+}
+
+#[test]
+/// Ensure that a write attribute with insufficient permissions is rejected
+fn insufficient_perms_write() {
+    let _ = env_logger::try_init();
+    let val0 = 10;
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+    let ep0_att = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let input0 = &[AttrData::new(
+        None,
+        AttrPath::new(&ep0_att),
+        EncodeValue::Closure(&attr_data0),
+    )];
+
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Add ACL to allow our peer with only OPERATE permission
+    let mut acl = AclEntry::new(1, Privilege::OPERATE, AuthMode::Case);
+    acl.add_subject(peer).unwrap();
+    acl.add_target(Target::new(Some(0), None, None)).unwrap();
+    im.acl_mgr.add(acl).unwrap();
+
+    // Test: Not enough permission should return error
+    handle_write_reqs(
+        &mut im,
+        peer,
+        input0,
+        &[AttrStatus::new(
+            &ep0_att,
+            IMStatusCode::UnsupportedAccess,
+            0,
+        )],
+    );
+    assert_eq!(
+        AttrValue::Uint16(ATTR_WRITE_DEFAULT_VALUE),
+        read_cluster_id_write_attr(&im, 0)
+    );
+}
+
+#[test]
+/// Ensure that a write to the ACL attribute instantaneously grants permission
+/// Here we have 2 ACLs, the first (basic_acl) allows access only to the ACL cluster
+/// Then we execute a write attribute with 3 writes
+///    - Write Attr to Echo Cluster (permission denied)
+///    - Write Attr to ACL Cluster (allowed, this ACL also grants universal access)
+///    - Write Attr to Echo Cluster again (successful this time)
+fn write_with_runtime_acl_add() {
+    let _ = env_logger::try_init();
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    let val0 = 10;
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+    let ep0_att = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let input0 = AttrData::new(
+        None,
+        AttrPath::new(&ep0_att),
+        EncodeValue::Closure(&attr_data0),
+    );
+
+    // Create ACL to allow our peer ADMIN on everything
+    let mut allow_acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    allow_acl.add_subject(peer).unwrap();
+
+    let acl_att = GenericPath::new(
+        Some(0),
+        Some(access_control::ID),
+        Some(access_control::Attributes::Acl as u32),
+    );
+    let acl_input = AttrData::new(
+        None,
+        AttrPath::new(&acl_att),
+        EncodeValue::Value(&allow_acl),
+    );
+
+    // Create ACL that only allows write to the ACL Cluster
+    let mut basic_acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    basic_acl.add_subject(peer).unwrap();
+    basic_acl
+        .add_target(Target::new(Some(0), Some(access_control::ID), None))
+        .unwrap();
+    im.acl_mgr.add(basic_acl).unwrap();
+
+    // Test: deny write (with error), then ACL is added, then allow write
+    handle_write_reqs(
+        &mut im,
+        peer,
+        // write to echo-cluster attribute, write to acl attribute, write to echo-cluster attribute
+        &[input0, acl_input, input0],
+        &[
+            AttrStatus::new(&ep0_att, IMStatusCode::UnsupportedAccess, 0),
+            AttrStatus::new(&acl_att, IMStatusCode::Sucess, 0),
+            AttrStatus::new(&ep0_att, IMStatusCode::Sucess, 0),
+        ],
+    );
+    assert_eq!(AttrValue::Uint16(val0), read_cluster_id_write_attr(&im, 0));
+}
+
+#[test]
+/// Data Version filtering should ignore the attributes that are filtered
+/// - in case of wildcard reads
+/// - in case of exact read attribute
+fn test_read_data_ver() {
+    // 1 Attr Read Requests
+    // - wildcard endpoint, att1
+    // - 2 responses are expected
+    let _ = env_logger::try_init();
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Add ACL to allow our peer with only OPERATE permission
+    let acl = AclEntry::new(1, Privilege::OPERATE, AuthMode::Case);
+    im.acl_mgr.add(acl).unwrap();
+
+    let wc_ep_att1 = GenericPath::new(
+        None,
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let input = &[AttrPath::new(&wc_ep_att1)];
+
+    let expected = &[
+        attr_data!(
+            GenericPath::new(
+                Some(0),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att1 as u32)
+            ),
+            ElementType::U16(0x1234)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(1),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att1 as u32)
+            ),
+            ElementType::U16(0x1234)
+        ),
+    ];
+    let mut out_buf = [0u8; 400];
+
+    // Test 1: Simple read to retrieve the current Data Version of Cluster at Endpoint 0
+    let received = gen_read_reqs_output(&mut im, peer, input, None, &mut out_buf);
+    assert_attr_report(&received, expected);
+
+    let data_ver_cluster_at_0 = received
+        .attr_reports
+        .as_ref()
+        .unwrap()
+        .get_index(0)
+        .unwrap_data()
+        .data_ver
+        .unwrap();
+
+    let dataver_filter = [DataVersionFilter {
+        path: ClusterPath {
+            node: None,
+            endpoint: 0,
+            cluster: echo_cluster::ID,
+        },
+        data_ver: data_ver_cluster_at_0,
+    }];
+
+    // Test 2: Add Dataversion filter for cluster at endpoint 0 only single entry should be retrieved
+    let received = gen_read_reqs_output(
+        &mut im,
+        peer,
+        input,
+        Some(TLVArray::Slice(&dataver_filter)),
+        &mut out_buf,
+    );
+    let expected_only_one = &[attr_data!(
+        GenericPath::new(
+            Some(1),
+            Some(echo_cluster::ID),
+            Some(echo_cluster::Attributes::Att1 as u32)
+        ),
+        ElementType::U16(0x1234)
+    )];
+
+    assert_attr_report(&received, expected_only_one);
+
+    // Test 3: Exact read attribute
+    let ep0_att1 = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let input = &[AttrPath::new(&ep0_att1)];
+    let received = gen_read_reqs_output(
+        &mut im,
+        peer,
+        input,
+        Some(TLVArray::Slice(&dataver_filter)),
+        &mut out_buf,
+    );
+    let expected_error = &[];
+
+    assert_attr_report(&received, expected_error);
+}
+
+#[test]
+/// - Write with the correct data version should go through
+/// - Write with incorrect data version should fail with error
+/// - Wildcard write with incorrect data version should be ignored
+fn test_write_data_ver() {
+    // 1 Attr Read Requests
+    // - wildcard endpoint, att1
+    // - 2 responses are expected
+    let _ = env_logger::try_init();
+    let peer = 98765;
+    let mut im = ImEngine::new();
+
+    // Add ACL to allow our peer with only OPERATE permission
+    let acl = AclEntry::new(1, Privilege::ADMIN, AuthMode::Case);
+    im.acl_mgr.add(acl).unwrap();
+
+    let wc_ep_attwrite = GenericPath::new(
+        None,
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let ep0_attwrite = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+
+    let val0 = 10u16;
+    let val1 = 11u16;
+    let attr_data0 = EncodeValue::Value(&val0);
+    let attr_data1 = EncodeValue::Value(&val1);
+
+    let initial_data_ver = read_cluster_id_data_ver(&im, 0);
+
+    // Test 1: Write with correct dataversion should succeed
+    let input_correct_dataver = &[AttrData::new(
+        Some(initial_data_ver),
+        AttrPath::new(&ep0_attwrite),
+        attr_data0,
+    )];
+    handle_write_reqs(
+        &mut im,
+        peer,
+        input_correct_dataver,
+        &[AttrStatus::new(&ep0_attwrite, IMStatusCode::Sucess, 0)],
+    );
+    assert_eq!(AttrValue::Uint16(val0), read_cluster_id_write_attr(&im, 0));
+
+    // Test 2: Write with incorrect dataversion should fail
+    // Now the data version would have incremented due to the previous write
+    let input_correct_dataver = &[AttrData::new(
+        Some(initial_data_ver),
+        AttrPath::new(&ep0_attwrite),
+        attr_data1,
+    )];
+    handle_write_reqs(
+        &mut im,
+        peer,
+        input_correct_dataver,
+        &[AttrStatus::new(
+            &ep0_attwrite,
+            IMStatusCode::DataVersionMismatch,
+            0,
+        )],
+    );
+    assert_eq!(AttrValue::Uint16(val0), read_cluster_id_write_attr(&im, 0));
+
+    // Test 3: Wildcard write with incorrect dataversion should ignore that cluster
+    //   In this case, while the data version is correct for endpoint 0, the endpoint 1's
+    //   data version would not match
+    let new_data_ver = read_cluster_id_data_ver(&im, 0);
+
+    let input_correct_dataver = &[AttrData::new(
+        Some(new_data_ver),
+        AttrPath::new(&wc_ep_attwrite),
+        attr_data1,
+    )];
+    handle_write_reqs(
+        &mut im,
+        peer,
+        input_correct_dataver,
+        &[AttrStatus::new(&ep0_attwrite, IMStatusCode::Sucess, 0)],
+    );
+    assert_eq!(AttrValue::Uint16(val1), read_cluster_id_write_attr(&im, 0));
+
+    assert_eq!(initial_data_ver + 1, new_data_ver);
+}

matter/tests/data_model/attribute_lists.rs

@@ -0,0 +1,166 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use matter::{
+    data_model::{core::DataModel, objects::EncodeValue},
+    interaction_model::{
+        core::{IMStatusCode, OpCode},
+        messages::{
+            ib::{AttrData, AttrPath, AttrStatus},
+            msg::WriteReq,
+        },
+        messages::{msg, GenericPath},
+    },
+    tlv::{self, FromTLV, Nullable, TLVWriter, TagType, ToTLV},
+    utils::writebuf::WriteBuf,
+};
+
+use crate::common::{
+    echo_cluster::{self, TestChecker},
+    im_engine::im_engine,
+};
+
+// Helper for handling Write Attribute sequences
+fn handle_write_reqs(input: &[AttrData], expected: &[AttrStatus]) -> DataModel {
+    let mut buf = [0u8; 400];
+    let mut out_buf = [0u8; 400];
+
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(&mut buf, buf_len);
+    let mut tw = TLVWriter::new(&mut wb);
+
+    let write_req = WriteReq::new(false, input);
+    write_req.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+
+    let (dm, out_buf_len) = im_engine(OpCode::WriteRequest, wb.as_borrow_slice(), &mut out_buf);
+    let out_buf = &out_buf[..out_buf_len];
+    tlv::print_tlv_list(out_buf);
+    let root = tlv::get_root_node_struct(out_buf).unwrap();
+
+    let mut index = 0;
+    let response_iter = root
+        .find_tag(msg::WriteRespTag::WriteResponses as u32)
+        .unwrap()
+        .confirm_array()
+        .unwrap()
+        .enter()
+        .unwrap();
+    for response in response_iter {
+        println!("Validating index {}", index);
+        let status = AttrStatus::from_tlv(&response).unwrap();
+        assert_eq!(expected[index], status);
+        println!("Index {} success", index);
+        index += 1;
+    }
+    assert_eq!(index, expected.len());
+    dm
+}
+
+#[test]
+/// This tests all the attribute list operations
+/// add item, edit item, delete item, overwrite list, delete list
+fn attr_list_ops() {
+    let val0: u16 = 10;
+    let val1: u16 = 15;
+    let tc_handle = TestChecker::get().unwrap();
+
+    let _ = env_logger::try_init();
+
+    let delete_item = EncodeValue::Closure(&|tag, t| {
+        let _ = t.null(tag);
+    });
+    let delete_all = EncodeValue::Closure(&|tag, t| {
+        let _ = t.start_array(tag);
+        let _ = t.end_container();
+    });
+
+    let att_data = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWriteList as u32),
+    );
+    let mut att_path = AttrPath::new(&att_data);
+
+    // Test 1: Add Operation - add val0
+    let input = &[AttrData::new(None, att_path, EncodeValue::Value(&val0))];
+    let expected = &[AttrStatus::new(&att_data, IMStatusCode::Sucess, 0)];
+    let _ = handle_write_reqs(input, expected);
+
+    {
+        let tc = tc_handle.lock().unwrap();
+        assert_eq!([Some(val0), None, None, None, None], tc.write_list);
+    }
+
+    // Test 2: Another Add Operation - add val1
+    let input = &[AttrData::new(None, att_path, EncodeValue::Value(&val1))];
+    let expected = &[AttrStatus::new(&att_data, IMStatusCode::Sucess, 0)];
+    let _ = handle_write_reqs(input, expected);
+
+    {
+        let tc = tc_handle.lock().unwrap();
+        assert_eq!([Some(val0), Some(val1), None, None, None], tc.write_list);
+    }
+
+    // Test 3: Edit Operation - edit val1 to val0
+    att_path.list_index = Some(Nullable::NotNull(1));
+    let input = &[AttrData::new(None, att_path, EncodeValue::Value(&val0))];
+    let expected = &[AttrStatus::new(&att_data, IMStatusCode::Sucess, 0)];
+    let _ = handle_write_reqs(input, expected);
+
+    {
+        let tc = tc_handle.lock().unwrap();
+        assert_eq!([Some(val0), Some(val0), None, None, None], tc.write_list);
+    }
+
+    // Test 4: Delete Operation - delete index 0
+    att_path.list_index = Some(Nullable::NotNull(0));
+    let input = &[AttrData::new(None, att_path, delete_item)];
+    let expected = &[AttrStatus::new(&att_data, IMStatusCode::Sucess, 0)];
+    let _ = handle_write_reqs(input, expected);
+
+    {
+        let tc = tc_handle.lock().unwrap();
+        assert_eq!([None, Some(val0), None, None, None], tc.write_list);
+    }
+
+    // Test 5: Overwrite Operation - overwrite first 2 entries
+    let overwrite_val: [u32; 2] = [20, 21];
+    att_path.list_index = None;
+    let input = &[AttrData::new(
+        None,
+        att_path,
+        EncodeValue::Value(&overwrite_val),
+    )];
+    let expected = &[AttrStatus::new(&att_data, IMStatusCode::Sucess, 0)];
+    let _ = handle_write_reqs(input, expected);
+
+    {
+        let tc = tc_handle.lock().unwrap();
+        assert_eq!([Some(20), Some(21), None, None, None], tc.write_list);
+    }
+
+    // Test 6: Overwrite Operation - delete whole list
+    att_path.list_index = None;
+    let input = &[AttrData::new(None, att_path, delete_all)];
+    let expected = &[AttrStatus::new(&att_data, IMStatusCode::Sucess, 0)];
+    let _ = handle_write_reqs(input, expected);
+
+    {
+        let tc = tc_handle.lock().unwrap();
+        assert_eq!([None, None, None, None, None], tc.write_list);
+    }
+}

matter/tests/data_model/attributes.rs

@@ -0,0 +1,566 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use matter::{
+    data_model::{
+        cluster_on_off,
+        core::DataModel,
+        objects::{AttrValue, EncodeValue, GlobalElements},
+    },
+    interaction_model::{
+        core::{IMStatusCode, OpCode},
+        messages::{
+            ib::{AttrData, AttrPath, AttrResp, AttrStatus},
+            msg::{ReadReq, ReportDataMsg, WriteReq},
+        },
+        messages::{msg, GenericPath},
+    },
+    tlv::{self, ElementType, FromTLV, TLVElement, TLVList, TLVWriter, TagType, ToTLV},
+    utils::writebuf::WriteBuf,
+};
+
+use crate::{
+    attr_data, attr_status,
+    common::{attributes::*, echo_cluster, im_engine::im_engine},
+};
+
+fn handle_read_reqs(input: &[AttrPath], expected: &[AttrResp]) {
+    let mut out_buf = [0u8; 400];
+    let received = gen_read_reqs_output(input, &mut out_buf);
+    assert_attr_report(&received, expected)
+}
+
+// Helper for handling Read Req sequences
+fn gen_read_reqs_output<'a>(input: &[AttrPath], out_buf: &'a mut [u8]) -> ReportDataMsg<'a> {
+    let mut buf = [0u8; 400];
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(&mut buf, buf_len);
+    let mut tw = TLVWriter::new(&mut wb);
+
+    let read_req = ReadReq::new(true).set_attr_requests(input);
+    read_req.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+
+    let (_, out_buf_len) = im_engine(OpCode::ReadRequest, wb.as_borrow_slice(), out_buf);
+    let out_buf = &out_buf[..out_buf_len];
+    tlv::print_tlv_list(out_buf);
+    let root = tlv::get_root_node_struct(out_buf).unwrap();
+    ReportDataMsg::from_tlv(&root).unwrap()
+}
+
+// Helper for handling Write Attribute sequences
+fn handle_write_reqs(input: &[AttrData], expected: &[AttrStatus]) -> DataModel {
+    let mut buf = [0u8; 400];
+    let mut out_buf = [0u8; 400];
+
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(&mut buf, buf_len);
+    let mut tw = TLVWriter::new(&mut wb);
+
+    let write_req = WriteReq::new(false, input);
+    write_req.to_tlv(&mut tw, TagType::Anonymous).unwrap();
+
+    let (dm, out_buf_len) = im_engine(OpCode::WriteRequest, wb.as_borrow_slice(), &mut out_buf);
+    let out_buf = &out_buf[..out_buf_len];
+    tlv::print_tlv_list(out_buf);
+    let root = tlv::get_root_node_struct(out_buf).unwrap();
+
+    let mut index = 0;
+    let response_iter = root
+        .find_tag(msg::WriteRespTag::WriteResponses as u32)
+        .unwrap()
+        .confirm_array()
+        .unwrap()
+        .enter()
+        .unwrap();
+    for response in response_iter {
+        println!("Validating index {}", index);
+        let status = AttrStatus::from_tlv(&response).unwrap();
+        assert_eq!(expected[index], status);
+        println!("Index {} success", index);
+        index += 1;
+    }
+    assert_eq!(index, expected.len());
+    dm
+}
+
+#[test]
+fn test_read_success() {
+    // 3 Attr Read Requests
+    // - first on endpoint 0, att1
+    // - second on endpoint 1, att2
+    // - third on endpoint 1, attcustom a custom attribute
+    let _ = env_logger::try_init();
+
+    let ep0_att1 = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let ep1_att2 = GenericPath::new(
+        Some(1),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att2 as u32),
+    );
+    let ep1_attcustom = GenericPath::new(
+        Some(1),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttCustom as u32),
+    );
+    let input = &[
+        AttrPath::new(&ep0_att1),
+        AttrPath::new(&ep1_att2),
+        AttrPath::new(&ep1_attcustom),
+    ];
+    let expected = &[
+        attr_data!(ep0_att1, ElementType::U16(0x1234)),
+        attr_data!(ep1_att2, ElementType::U16(0x5678)),
+        attr_data!(
+            ep1_attcustom,
+            ElementType::U32(echo_cluster::ATTR_CUSTOM_VALUE)
+        ),
+    ];
+    handle_read_reqs(input, expected);
+}
+
+#[test]
+fn test_read_unsupported_fields() {
+    // 6 reads
+    // - endpoint doesn't exist - UnsupportedEndpoint
+    // - cluster doesn't exist - UnsupportedCluster
+    // - cluster doesn't exist and endpoint is wildcard - Silently ignore
+    // - attribute doesn't exist - UnsupportedAttribute
+    // - attribute doesn't exist and endpoint is wildcard - Silently ignore
+    // - attribute doesn't exist and cluster is wildcard - Silently ignore
+    let _ = env_logger::try_init();
+
+    let invalid_endpoint = GenericPath::new(
+        Some(2),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let invalid_cluster = GenericPath::new(
+        Some(0),
+        Some(0x1234),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let invalid_cluster_wc_endpoint = GenericPath::new(
+        None,
+        Some(0x1234),
+        Some(echo_cluster::Attributes::AttCustom as u32),
+    );
+    let invalid_attribute = GenericPath::new(Some(0), Some(echo_cluster::ID), Some(0x1234));
+    let invalid_attribute_wc_endpoint =
+        GenericPath::new(None, Some(echo_cluster::ID), Some(0x1234));
+    let invalid_attribute_wc_cluster = GenericPath::new(Some(0), None, Some(0x1234));
+    let input = &[
+        AttrPath::new(&invalid_endpoint),
+        AttrPath::new(&invalid_cluster),
+        AttrPath::new(&invalid_cluster_wc_endpoint),
+        AttrPath::new(&invalid_attribute),
+        AttrPath::new(&invalid_attribute_wc_endpoint),
+        AttrPath::new(&invalid_attribute_wc_cluster),
+    ];
+
+    let expected = &[
+        attr_status!(&invalid_endpoint, IMStatusCode::UnsupportedEndpoint),
+        attr_status!(&invalid_cluster, IMStatusCode::UnsupportedCluster),
+        attr_status!(&invalid_attribute, IMStatusCode::UnsupportedAttribute),
+    ];
+    handle_read_reqs(input, expected);
+}
+
+#[test]
+fn test_read_wc_endpoint_all_have_clusters() {
+    // 1 Attr Read Requests
+    // - wildcard endpoint, att1
+    // - 2 responses are expected
+    let _ = env_logger::try_init();
+
+    let wc_ep_att1 = GenericPath::new(
+        None,
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::Att1 as u32),
+    );
+    let input = &[AttrPath::new(&wc_ep_att1)];
+
+    let expected = &[
+        attr_data!(
+            GenericPath::new(
+                Some(0),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att1 as u32)
+            ),
+            ElementType::U16(0x1234)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(1),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att1 as u32)
+            ),
+            ElementType::U16(0x1234)
+        ),
+    ];
+    handle_read_reqs(input, expected);
+}
+
+#[test]
+fn test_read_wc_endpoint_only_1_has_cluster() {
+    // 1 Attr Read Requests
+    // - wildcard endpoint, on/off Cluster OnOff Attribute
+    // - 1 response are expected
+    let _ = env_logger::try_init();
+
+    let wc_ep_onoff = GenericPath::new(
+        None,
+        Some(cluster_on_off::ID),
+        Some(cluster_on_off::Attributes::OnOff as u32),
+    );
+    let input = &[AttrPath::new(&wc_ep_onoff)];
+
+    let expected = &[attr_data!(
+        GenericPath::new(
+            Some(1),
+            Some(cluster_on_off::ID),
+            Some(cluster_on_off::Attributes::OnOff as u32)
+        ),
+        ElementType::False
+    )];
+    handle_read_reqs(input, expected);
+}
+
+fn get_tlvs<'a>(buf: &'a mut [u8], data: &[u16]) -> TLVElement<'a> {
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(buf, buf_len);
+    let mut tw = TLVWriter::new(&mut wb);
+    let _ = tw.start_array(TagType::Context(2));
+    for e in data {
+        let _ = tw.u16(TagType::Anonymous, *e);
+    }
+    let _ = tw.end_container();
+    let tlv_array = TLVList::new(wb.as_slice()).iter().next().unwrap();
+    tlv_array
+}
+
+#[test]
+fn test_read_wc_endpoint_wc_attribute() {
+    // 1 Attr Read Request
+    // - wildcard endpoint, wildcard attribute
+    // - 8 responses are expected, 1+3 attributes on endpoint 0, 1+3 on endpoint 1
+    let _ = env_logger::try_init();
+    let wc_ep_wc_attr = GenericPath::new(None, Some(echo_cluster::ID), None);
+    let input = &[AttrPath::new(&wc_ep_wc_attr)];
+
+    let mut buf = [0u8; 100];
+    let attr_list_tlvs = get_tlvs(
+        &mut buf,
+        &[
+            GlobalElements::AttributeList as u16,
+            echo_cluster::Attributes::Att1 as u16,
+            echo_cluster::Attributes::Att2 as u16,
+            echo_cluster::Attributes::AttWrite as u16,
+            echo_cluster::Attributes::AttCustom as u16,
+        ],
+    );
+
+    let expected = &[
+        attr_data!(
+            GenericPath::new(
+                Some(0),
+                Some(echo_cluster::ID),
+                Some(GlobalElements::AttributeList as u32),
+            ),
+            attr_list_tlvs.get_element_type()
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(0),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att1 as u32),
+            ),
+            ElementType::U16(0x1234)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(0),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att2 as u32),
+            ),
+            ElementType::U16(0x5678)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(0),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::AttCustom as u32),
+            ),
+            ElementType::U32(echo_cluster::ATTR_CUSTOM_VALUE)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(1),
+                Some(echo_cluster::ID),
+                Some(GlobalElements::AttributeList as u32),
+            ),
+            attr_list_tlvs.get_element_type()
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(1),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att1 as u32),
+            ),
+            ElementType::U16(0x1234)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(1),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::Att2 as u32),
+            ),
+            ElementType::U16(0x5678)
+        ),
+        attr_data!(
+            GenericPath::new(
+                Some(1),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Attributes::AttCustom as u32),
+            ),
+            ElementType::U32(echo_cluster::ATTR_CUSTOM_VALUE)
+        ),
+    ];
+    handle_read_reqs(input, expected);
+}
+
+#[test]
+fn test_write_success() {
+    // 2 Attr Write Request
+    // - first on endpoint 0, AttWrite
+    // - second on endpoint 1, AttWrite
+    let val0 = 10;
+    let val1 = 15;
+    let _ = env_logger::try_init();
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+    let attr_data1 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val1);
+    };
+
+    let ep0_att = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let ep1_att = GenericPath::new(
+        Some(1),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+
+    let input = &[
+        AttrData::new(
+            None,
+            AttrPath::new(&ep0_att),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&ep1_att),
+            EncodeValue::Closure(&attr_data1),
+        ),
+    ];
+    let expected = &[
+        AttrStatus::new(&ep0_att, IMStatusCode::Sucess, 0),
+        AttrStatus::new(&ep1_att, IMStatusCode::Sucess, 0),
+    ];
+
+    let dm = handle_write_reqs(input, expected);
+    let node = dm.node.read().unwrap();
+    let echo = node.get_cluster(0, echo_cluster::ID).unwrap();
+    assert_eq!(
+        AttrValue::Uint16(val0),
+        *echo
+            .base()
+            .read_attribute_raw(echo_cluster::Attributes::AttWrite as u16)
+            .unwrap()
+    );
+    let echo = node.get_cluster(1, echo_cluster::ID).unwrap();
+    assert_eq!(
+        AttrValue::Uint16(val1),
+        *echo
+            .base()
+            .read_attribute_raw(echo_cluster::Attributes::AttWrite as u16)
+            .unwrap()
+    );
+}
+
+#[test]
+fn test_write_wc_endpoint() {
+    // 1 Attr Write Request
+    // - wildcard endpoint, AttWrite
+    let val0 = 10;
+    let _ = env_logger::try_init();
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+
+    let ep_att = GenericPath::new(
+        None,
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let input = &[AttrData::new(
+        None,
+        AttrPath::new(&ep_att),
+        EncodeValue::Closure(&attr_data0),
+    )];
+
+    let ep0_att = GenericPath::new(
+        Some(0),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+
+    let ep1_att = GenericPath::new(
+        Some(1),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let expected = &[
+        AttrStatus::new(&ep0_att, IMStatusCode::Sucess, 0),
+        AttrStatus::new(&ep1_att, IMStatusCode::Sucess, 0),
+    ];
+
+    let dm = handle_write_reqs(input, expected);
+    assert_eq!(
+        AttrValue::Uint16(val0),
+        dm.read_attribute_raw(
+            0,
+            echo_cluster::ID,
+            echo_cluster::Attributes::AttWrite as u16
+        )
+        .unwrap()
+    );
+    assert_eq!(
+        AttrValue::Uint16(val0),
+        dm.read_attribute_raw(
+            0,
+            echo_cluster::ID,
+            echo_cluster::Attributes::AttWrite as u16
+        )
+        .unwrap()
+    );
+}
+
+#[test]
+fn test_write_unsupported_fields() {
+    // 7 writes
+    // - endpoint doesn't exist - UnsupportedEndpoint
+    // - cluster doesn't exist - UnsupportedCluster
+    // - attribute doesn't exist - UnsupportedAttribute
+    // - cluster doesn't exist and endpoint is wildcard - Silently ignore
+    // - attribute doesn't exist and endpoint is wildcard - Silently ignore
+    // - cluster is wildcard - Cluster cannot be wildcard - UnsupportedCluster
+    // - attribute is wildcard - Attribute cannot be wildcard - UnsupportedAttribute
+    let _ = env_logger::try_init();
+
+    let val0 = 50;
+    let attr_data0 = |tag, t: &mut TLVWriter| {
+        let _ = t.u16(tag, val0);
+    };
+
+    let invalid_endpoint = GenericPath::new(
+        Some(4),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let invalid_cluster = GenericPath::new(
+        Some(0),
+        Some(0x1234),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let invalid_attribute = GenericPath::new(Some(0), Some(echo_cluster::ID), Some(0x1234));
+    let wc_endpoint_invalid_cluster = GenericPath::new(
+        None,
+        Some(0x1234),
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let wc_endpoint_invalid_attribute =
+        GenericPath::new(None, Some(echo_cluster::ID), Some(0x1234));
+    let wc_cluster = GenericPath::new(
+        Some(0),
+        None,
+        Some(echo_cluster::Attributes::AttWrite as u32),
+    );
+    let wc_attribute = GenericPath::new(Some(0), Some(echo_cluster::ID), None);
+
+    let input = &[
+        AttrData::new(
+            None,
+            AttrPath::new(&invalid_endpoint),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&invalid_cluster),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&invalid_attribute),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&wc_endpoint_invalid_cluster),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&wc_endpoint_invalid_attribute),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&wc_cluster),
+            EncodeValue::Closure(&attr_data0),
+        ),
+        AttrData::new(
+            None,
+            AttrPath::new(&wc_attribute),
+            EncodeValue::Closure(&attr_data0),
+        ),
+    ];
+    let expected = &[
+        AttrStatus::new(&invalid_endpoint, IMStatusCode::UnsupportedEndpoint, 0),
+        AttrStatus::new(&invalid_cluster, IMStatusCode::UnsupportedCluster, 0),
+        AttrStatus::new(&invalid_attribute, IMStatusCode::UnsupportedAttribute, 0),
+        AttrStatus::new(&wc_cluster, IMStatusCode::UnsupportedCluster, 0),
+        AttrStatus::new(&wc_attribute, IMStatusCode::UnsupportedAttribute, 0),
+    ];
+    let dm = handle_write_reqs(input, expected);
+    assert_eq!(
+        AttrValue::Uint16(echo_cluster::ATTR_WRITE_DEFAULT_VALUE),
+        dm.read_attribute_raw(
+            0,
+            echo_cluster::ID,
+            echo_cluster::Attributes::AttWrite as u16
+        )
+        .unwrap()
+    );
+}

matter/tests/data_model/commands.rs

@@ -0,0 +1,230 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use matter::{
+    data_model::{cluster_on_off, objects::EncodeValue},
+    interaction_model::{
+        core::{IMStatusCode, OpCode},
+        messages::ib::{CmdPath, CmdStatus, InvResp},
+        messages::msg,
+    },
+    tlv::{self, FromTLV},
+    utils::writebuf::WriteBuf,
+};
+
+use crate::common::{
+    echo_cluster,
+    im_engine::{im_engine, TestData},
+};
+
+enum ExpectedInvResp {
+    Cmd(CmdPath, u8),
+    Status(CmdStatus),
+}
+
+// Helper for handling Invoke Command sequences
+fn handle_commands(input: &[(CmdPath, Option<u8>)], expected: &[ExpectedInvResp]) {
+    let mut buf = [0u8; 400];
+    let mut out_buf = [0u8; 400];
+
+    let buf_len = buf.len();
+    let mut wb = WriteBuf::new(&mut buf, buf_len);
+    let mut td = TestData::new(&mut wb);
+
+    td.commands(input).unwrap();
+
+    let (_, out_buf_len) = im_engine(OpCode::InvokeRequest, wb.as_borrow_slice(), &mut out_buf);
+    let out_buf = &out_buf[..out_buf_len];
+    tlv::print_tlv_list(out_buf);
+    let root = tlv::get_root_node_struct(out_buf).unwrap();
+
+    let mut index = 0;
+    let cmd_list_iter = root
+        .find_tag(msg::InvRespTag::InvokeResponses as u32)
+        .unwrap()
+        .confirm_array()
+        .unwrap()
+        .enter()
+        .unwrap();
+    for response in cmd_list_iter {
+        println!("Validating index {}", index);
+        let inv_response = InvResp::from_tlv(&response).unwrap();
+        match expected[index] {
+            ExpectedInvResp::Cmd(e_c, e_d) => match inv_response {
+                InvResp::Cmd(c) => {
+                    assert_eq!(e_c, c.path);
+                    match c.data {
+                        EncodeValue::Tlv(t) => {
+                            assert_eq!(e_d, t.find_tag(0).unwrap().u8().unwrap())
+                        }
+                        _ => panic!("Incorrect CmdDataType"),
+                    }
+                }
+                _ => {
+                    panic!("Invalid response, expected InvResponse::Cmd");
+                }
+            },
+            ExpectedInvResp::Status(e_status) => match inv_response {
+                InvResp::Status(status) => {
+                    assert_eq!(e_status, status);
+                }
+                _ => {
+                    panic!("Invalid response, expected InvResponse::Status");
+                }
+            },
+        }
+        println!("Index {} success", index);
+        index += 1;
+    }
+    assert_eq!(index, expected.len());
+}
+
+macro_rules! echo_req {
+    ($endpoint:literal, $data:literal) => {
+        (
+            CmdPath::new(
+                Some($endpoint),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Commands::EchoReq as u16),
+            ),
+            Some($data),
+        )
+    };
+}
+
+macro_rules! echo_resp {
+    ($endpoint:literal, $data:literal) => {
+        ExpectedInvResp::Cmd(
+            CmdPath::new(
+                Some($endpoint),
+                Some(echo_cluster::ID),
+                Some(echo_cluster::Commands::EchoResp as u16),
+            ),
+            $data,
+        )
+    };
+}
+
+#[test]
+fn test_invoke_cmds_success() {
+    // 2 echo Requests
+    // - one on endpoint 0 with data 5,
+    // - another on endpoint 1 with data 10
+    let _ = env_logger::try_init();
+
+    let input = &[echo_req!(0, 5), echo_req!(1, 10)];
+    let expected = &[echo_resp!(0, 10), echo_resp!(1, 30)];
+    handle_commands(input, expected);
+}
+
+#[test]
+fn test_invoke_cmds_unsupported_fields() {
+    // 5 commands
+    // - endpoint doesn't exist - UnsupportedEndpoint
+    // - cluster doesn't exist - UnsupportedCluster
+    // - cluster doesn't exist and endpoint is wildcard - UnsupportedCluster
+    // - command doesn't exist - UnsupportedCommand
+    // - command doesn't exist and endpoint is wildcard - UnsupportedCommand
+    let _ = env_logger::try_init();
+
+    let invalid_endpoint = CmdPath::new(
+        Some(2),
+        Some(echo_cluster::ID),
+        Some(echo_cluster::Commands::EchoReq as u16),
+    );
+    let invalid_cluster = CmdPath::new(
+        Some(0),
+        Some(0x1234),
+        Some(echo_cluster::Commands::EchoReq as u16),
+    );
+    let invalid_cluster_wc_endpoint = CmdPath::new(
+        None,
+        Some(0x1234),
+        Some(echo_cluster::Commands::EchoReq as u16),
+    );
+    let invalid_command = CmdPath::new(Some(0), Some(echo_cluster::ID), Some(0x1234));
+    let invalid_command_wc_endpoint = CmdPath::new(None, Some(echo_cluster::ID), Some(0x1234));
+    let input = &[
+        (invalid_endpoint, Some(5)),
+        (invalid_cluster, Some(5)),
+        (invalid_cluster_wc_endpoint, Some(5)),
+        (invalid_command, Some(5)),
+        (invalid_command_wc_endpoint, Some(5)),
+    ];
+
+    let expected = &[
+        ExpectedInvResp::Status(CmdStatus::new(
+            invalid_endpoint,
+            IMStatusCode::UnsupportedEndpoint,
+            0,
+        )),
+        ExpectedInvResp::Status(CmdStatus::new(
+            invalid_cluster,
+            IMStatusCode::UnsupportedCluster,
+            0,
+        )),
+        ExpectedInvResp::Status(CmdStatus::new(
+            invalid_command,
+            IMStatusCode::UnsupportedCommand,
+            0,
+        )),
+    ];
+    handle_commands(input, expected);
+}
+
+#[test]
+fn test_invoke_cmd_wc_endpoint_all_have_clusters() {
+    // 1 echo Request with wildcard endpoint
+    // should generate 2 responses from the echo clusters on both
+    let _ = env_logger::try_init();
+
+    let input = &[(
+        CmdPath::new(
+            None,
+            Some(echo_cluster::ID),
+            Some(echo_cluster::Commands::EchoReq as u16),
+        ),
+        Some(5),
+    )];
+    let expected = &[echo_resp!(0, 10), echo_resp!(1, 15)];
+    handle_commands(input, expected);
+}
+
+#[test]
+fn test_invoke_cmd_wc_endpoint_only_1_has_cluster() {
+    // 1 on command for on/off cluster with wildcard endpoint
+    // should generate 1 response from the on-off cluster
+    let _ = env_logger::try_init();
+
+    let target = CmdPath::new(
+        None,
+        Some(cluster_on_off::ID),
+        Some(cluster_on_off::Commands::On as u16),
+    );
+    let expected_path = CmdPath::new(
+        Some(1),
+        Some(cluster_on_off::ID),
+        Some(cluster_on_off::Commands::On as u16),
+    );
+    let input = &[(target, Some(1))];
+    let expected = &[ExpectedInvResp::Status(CmdStatus::new(
+        expected_path,
+        IMStatusCode::Sucess,
+        0,
+    ))];
+    handle_commands(input, expected);
+}

matter/tests/data_model_tests.rs

@@ -0,0 +1,25 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+mod common;
+
+mod data_model {
+    mod acl_and_dataver;
+    mod attribute_lists;
+    mod attributes;
+    mod commands;
+}

matter/tests/interaction_model.rs

@@ -0,0 +1,176 @@
+/*
+ *
+ *    Copyright (c) 2020-2022 Project CHIP Authors
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *
+ *        http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+use boxslab::Slab;
+use matter::error::Error;
+use matter::interaction_model::core::OpCode;
+use matter::interaction_model::messages::msg::InvReq;
+use matter::interaction_model::messages::msg::ReadReq;
+use matter::interaction_model::messages::msg::WriteReq;
+use matter::interaction_model::InteractionConsumer;
+use matter::interaction_model::InteractionModel;
+use matter::interaction_model::Transaction;
+use matter::tlv::TLVWriter;
+use matter::transport::exchange::Exchange;
+use matter::transport::exchange::ExchangeCtx;
+use matter::transport::network::Address;
+use matter::transport::packet::Packet;
+use matter::transport::packet::PacketPool;
+use matter::transport::proto_demux::HandleProto;
+use matter::transport::proto_demux::ProtoCtx;
+use matter::transport::session::SessionMgr;
+use std::net::Ipv4Addr;
+use std::net::SocketAddr;
+use std::sync::{Arc, Mutex};
+
+struct Node {
+    pub endpoint: u16,
+    pub cluster: u32,
+    pub command: u16,
+    pub variable: u8,
+}
+
+struct DataModel {
+    node: Arc<Mutex<Node>>,
+}
+
+impl DataModel {
+    pub fn new(node: Node) -> Self {
+        DataModel {
+            node: Arc::new(Mutex::new(node)),
+        }
+    }
+}
+
+impl Clone for DataModel {
+    fn clone(&self) -> Self {
+        Self {
+            node: self.node.clone(),
+        }
+    }
+}
+
+impl InteractionConsumer for DataModel {
+    fn consume_invoke_cmd(
+        &self,
+        inv_req_msg: &InvReq,
+        _trans: &mut Transaction,
+        _tlvwriter: &mut TLVWriter,
+    ) -> Result<(), Error> {
+        if let Some(inv_requests) = &inv_req_msg.inv_requests {
+            for i in inv_requests.iter() {
+                let data = if let Some(data) = i.data.unwrap_tlv() {
+                    data
+                } else {
+                    continue;
+                };
+                let cmd_path_ib = i.path;
+                let mut common_data = self.node.lock().unwrap();
+                common_data.endpoint = cmd_path_ib.path.endpoint.unwrap_or(1);
+                common_data.cluster = cmd_path_ib.path.cluster.unwrap_or(0);
+                common_data.command = cmd_path_ib.path.leaf.unwrap_or(0) as u16;
+                data.confirm_struct().unwrap();
+                common_data.variable = data.find_tag(0).unwrap().u8().unwrap();
+            }
+        }
+
+        Ok(())
+    }
+
+    fn consume_read_attr(
+        &self,
+        _req: &ReadReq,
+        _trans: &mut Transaction,
+        _tlvwriter: &mut TLVWriter,
+    ) -> Result<(), Error> {
+        Ok(())
+    }
+
+    fn consume_write_attr(
+        &self,
+        _req: &WriteReq,
+        _trans: &mut Transaction,
+        _tlvwriter: &mut TLVWriter,
+    ) -> Result<(), Error> {
+        Ok(())
+    }
+}
+
+fn handle_data(action: OpCode, data_in: &[u8], data_out: &mut [u8]) -> (DataModel, usize) {
+    let data_model = DataModel::new(Node {
+        endpoint: 0,
+        cluster: 0,
+        command: 0,
+        variable: 0,
+    });
+    let mut interaction_model = InteractionModel::new(Box::new(data_model.clone()));
+    let mut exch: Exchange = Default::default();
+    let mut sess_mgr: SessionMgr = Default::default();
+    let sess_idx = sess_mgr
+        .get_or_add(
+            0,
+            Address::Udp(SocketAddr::new(
+                std::net::IpAddr::V4(Ipv4Addr::new(127, 0, 0, 1)),
+                5542,
+            )),
+            None,
+            false,
+        )
+        .unwrap();
+    let sess = sess_mgr.get_session_handle(sess_idx);
+    let exch_ctx = ExchangeCtx {
+        exch: &mut exch,
+        sess,
+    };
+    let mut rx = Slab::<PacketPool>::new(Packet::new_rx().unwrap()).unwrap();
+    let tx = Slab::<PacketPool>::new(Packet::new_tx().unwrap()).unwrap();
+    // Create fake rx packet
+    rx.set_proto_id(0x01);
+    rx.set_proto_opcode(action as u8);
+    rx.peer = Address::default();
+    let in_data_len = data_in.len();
+    let rx_buf = rx.as_borrow_slice();
+    rx_buf[..in_data_len].copy_from_slice(data_in);
+
+    let mut ctx = ProtoCtx::new(exch_ctx, rx, tx);
+
+    interaction_model.handle_proto_id(&mut ctx).unwrap();
+
+    let out_len = ctx.tx.as_borrow_slice().len();
+    data_out[..out_len].copy_from_slice(ctx.tx.as_borrow_slice());
+    (data_model, out_len)
+}
+
+#[test]
+fn test_valid_invoke_cmd() -> Result<(), Error> {
+    // An invoke command for endpoint 0, cluster 49, command 12 and a u8 variable value of 0x05
+
+    let b = [
+        0x15, 0x28, 0x00, 0x28, 0x01, 0x36, 0x02, 0x15, 0x37, 0x00, 0x24, 0x00, 0x00, 0x24, 0x01,
+        0x31, 0x24, 0x02, 0x0c, 0x18, 0x35, 0x01, 0x24, 0x00, 0x05, 0x18, 0x18, 0x18, 0x18,
+    ];
+
+    let mut out_buf: [u8; 20] = [0; 20];
+
+    let (data_model, _) = handle_data(OpCode::InvokeRequest, &b, &mut out_buf);
+    let data = data_model.node.lock().unwrap();
+    assert_eq!(data.endpoint, 0);
+    assert_eq!(data.cluster, 49);
+    assert_eq!(data.command, 12);
+    assert_eq!(data.variable, 5);
+    Ok(())
+}