commit f12e94092f3f3faa048414e1350f3d9f4497676e Author: STCisGood Date: Sat Jan 10 20:17:45 2026 -0500 Add files via upload diff --git a/Cargo.lock b/Cargo.lock new file mode 100644 index 0000000..ca7bed6 --- /dev/null +++ b/Cargo.lock @@ -0,0 +1,1747 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "aho-corasick" +version = "1.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301" +dependencies = [ + "memchr", +] + +[[package]] +name = "aligned" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee4508988c62edf04abd8d92897fca0c2995d907ce1dfeaf369dac3716a40685" +dependencies = [ + "as-slice", +] + +[[package]] +name = "android_system_properties" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" +dependencies = [ + "libc", +] + +[[package]] +name = "anyhow" +version = "1.0.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a23eb6b1614318a8071c9b2521f36b424b2c83db5eb3a0fead4a6c0809af6e61" + +[[package]] +name = "as-slice" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "516b6b4f0e40d50dcda9365d53964ec74560ad4284da2e7fc97122cd83174516" +dependencies = [ + "stable_deref_trait", +] + +[[package]] +name = "atomic-waker" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1505bd5d3d116872e7271a6d4e16d81d0c8570876c8de68093a09ac269d8aac0" + +[[package]] +name = "autocfg" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" + +[[package]] +name = "bindgen" +version = "0.71.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f58bf3d7db68cfbac37cfc485a8d711e87e064c3d0fe0435b92f7a407f9d6b3" +dependencies = [ + "bitflags 2.10.0", + "cexpr", + "clang-sys", + "itertools", + "log", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "rustc-hash", + "shlex", + "syn 2.0.113", +] + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3" + +[[package]] +name = "bstr" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63044e1ae8e69f3b5a92c736ca6269b8d12fa7efe39bf34ddb06d102cf0e2cab" +dependencies = [ + "memchr", + "serde", +] + +[[package]] +name = "build-time" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1219c19fc29b7bfd74b7968b420aff5bc951cf517800176e795d6b2300dd382" +dependencies = [ + "chrono", + "once_cell", + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "bumpalo" +version = "3.19.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5dd9dc738b7a8311c7ade152424974d8115f2cdad61e8dab8dac9f2362298510" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "camino" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e629a66d692cb9ff1a1c664e41771b3dcaf961985a9774c0eb0bd1b51cf60a48" +dependencies = [ + "serde_core", +] + +[[package]] +name = "cargo-platform" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e35af189006b9c0f00a064685c727031e3ed2d8020f7ba284d78cc2671bd36ea" +dependencies = [ + "serde", +] + +[[package]] +name = "cargo_metadata" +version = "0.18.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2d886547e41f740c616ae73108f6eb70afe6d940c7bc697cb30f13daec073037" +dependencies = [ + "camino", + "cargo-platform", + "semver", + "serde", + "serde_json", + "thiserror 1.0.69", +] + +[[package]] +name = "cc" +version = "1.2.51" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a0aeaff4ff1a90589618835a598e545176939b97874f7abc7851caa0618f203" +dependencies = [ + "find-msvc-tools", + "shlex", +] + +[[package]] +name = "cexpr" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" +dependencies = [ + "nom", +] + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "cfg_aliases" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724" + +[[package]] +name = "chrono" +version = "0.4.42" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "145052bdd345b87320e369255277e3fb5152762ad123a901ef5c262dd38fe8d2" +dependencies = [ + "iana-time-zone", + "num-traits", + "windows-link", +] + +[[package]] +name = "clang-sys" +version = "1.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4" +dependencies = [ + "glob", + "libc", + "libloading", +] + +[[package]] +name = "cmake" +version = "0.1.57" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "75443c44cd6b379beb8c5b45d85d0773baf31cce901fe7bb252f4eff3008ef7d" +dependencies = [ + "cc", +] + +[[package]] +name = "const_format" +version = "0.2.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7faa7469a93a566e9ccc1c73fe783b4a65c274c5ace346038dca9c39fe0030ad" +dependencies = [ + "const_format_proc_macros", +] + +[[package]] +name = "const_format_proc_macros" +version = "0.2.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d57c2eccfb16dbac1f4e61e206105db5820c9d26c3c472bc17c774259ef7744" +dependencies = [ + "proc-macro2", + "quote", + "unicode-xid", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" + +[[package]] +name = "critical-section" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" + +[[package]] +name = "crossbeam-deque" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28" + +[[package]] +name = "cvt" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2ae9bf77fbf2d39ef573205d554d87e86c12f1994e9ea335b0651b9b278bcf1" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "darling" +version = "0.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9cdf337090841a411e2a7f3deb9187445851f91b309c0c0a29e05f74a00a48c0" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1247195ecd7e3c85f83c8d2a366e4210d588e802133e1e355180a9870b517ea4" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "darling_macro" +version = "0.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d38308df82d1080de0afee5d069fa14b0326a88c14f15c5ccda35b4a6c414c81" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "defmt" +version = "0.3.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0963443817029b2024136fc4dd07a5107eb8f977eaf18fcd1fdeb11306b64ad" +dependencies = [ + "defmt 1.0.1", +] + +[[package]] +name = "defmt" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" +dependencies = [ + "bitflags 1.3.2", + "defmt-macros", +] + +[[package]] +name = "defmt-macros" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" +dependencies = [ + "defmt-parser", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "defmt-parser" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" +dependencies = [ + "thiserror 2.0.17", +] + +[[package]] +name = "either" +version = "1.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" + +[[package]] +name = "embassy-futures" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc2d050bdc5c21e0862a89256ed8029ae6c290a93aecefc73084b3002cdebb01" + +[[package]] +name = "embassy-sync" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049" +dependencies = [ + "cfg-if", + "critical-section", + "embedded-io-async", + "futures-sink", + "futures-util", + "heapless", +] + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "embedded-hal" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" +dependencies = [ + "nb 0.1.3", + "void", +] + +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-nb" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" +dependencies = [ + "embedded-hal 1.0.0", + "nb 1.1.0", +] + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "embedded-io", +] + +[[package]] +name = "embedded-svc" +version = "0.28.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7770e30ab55cfbf954c00019522490d6ce26a3334bede05a732ba61010e98e0" +dependencies = [ + "defmt 0.3.100", + "embedded-io", + "embedded-io-async", + "enumset", + "heapless", + "num_enum", + "serde", + "strum 0.25.0", +] + +[[package]] +name = "embuild" +version = "0.33.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e188ad2bbe82afa841ea4a29880651e53ab86815db036b2cb9f8de3ac32dad75" +dependencies = [ + "anyhow", + "bindgen", + "bitflags 1.3.2", + "cmake", + "filetime", + "globwalk", + "home", + "log", + "regex", + "remove_dir_all", + "serde", + "serde_json", + "shlex", + "strum 0.24.1", + "tempfile", + "thiserror 1.0.69", + "which", +] + +[[package]] +name = "enumset" +version = "1.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "25b07a8dfbbbfc0064c0a6bdf9edcf966de6b1c33ce344bdeca3b41615452634" +dependencies = [ + "enumset_derive", +] + +[[package]] +name = "enumset_derive" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f43e744e4ea338060faee68ed933e46e722fb7f3617e722a5772d7e856d8b3ce" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "envy" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f47e0157f2cb54f5ae1bd371b30a2ae4311e1c028f575cd4e81de7353215965" +dependencies = [ + "serde", +] + +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "esp-idf-hal" +version = "0.45.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "775ce25171dc4f615146a4a27ed3a64c6fd99ced77d7112062f2b19bf933f5db" +dependencies = [ + "atomic-waker", + "critical-section", + "embassy-sync", + "embedded-can", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-nb", + "embedded-io", + "embedded-io-async", + "embuild", + "enumset", + "esp-idf-sys", + "heapless", + "log", + "nb 1.1.0", + "num_enum", +] + +[[package]] +name = "esp-idf-svc" +version = "0.51.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2bc07aaba257d28d54a96af005ca67d0b38876d8837f5d54a3e0547e100b219c" +dependencies = [ + "embassy-futures", + "embedded-hal-async", + "embedded-svc", + "embuild", + "enumset", + "esp-idf-hal", + "futures-io", + "heapless", + "log", + "num_enum", + "uncased", +] + +[[package]] +name = "esp-idf-sys" +version = "0.36.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb77a3d02b579a60a811ed9be22b78c5e794bc492d833ee7fc44d3a0155885e1" +dependencies = [ + "anyhow", + "build-time", + "cargo_metadata", + "cmake", + "const_format", + "embuild", + "envy", + "libc", + "regex", + "serde", + "strum 0.24.1", + "which", +] + +[[package]] +name = "fastrand" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "37909eebbb50d72f9059c3b6d82c0463f2ff062c9e95845c43a6c9c0355411be" + +[[package]] +name = "filetime" +version = "0.2.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bc0505cd1b6fa6580283f6bdf70a73fcf4aba1184038c90902b92b3dd0df63ed" +dependencies = [ + "cfg-if", + "libc", + "libredox", + "windows-sys 0.60.2", +] + +[[package]] +name = "find-msvc-tools" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "645cbb3a84e60b7531617d5ae4e57f7e27308f6445f5abf653209ea76dec8dff" + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "fs_at" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "14af6c9694ea25db25baa2a1788703b9e7c6648dcaeeebeb98f7561b5384c036" +dependencies = [ + "aligned", + "cfg-if", + "cvt", + "libc", + "nix", + "windows-sys 0.52.0", +] + +[[package]] +name = "futures-core" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e" + +[[package]] +name = "futures-io" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6" + +[[package]] +name = "futures-sink" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7" + +[[package]] +name = "futures-task" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988" + +[[package]] +name = "futures-util" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + +[[package]] +name = "getrandom" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd" +dependencies = [ + "cfg-if", + "libc", + "r-efi", + "wasip2", +] + +[[package]] +name = "glob" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0cc23270f6e1808e30a928bdc84dea0b9b4136a8bc82338574f23baf47bbd280" + +[[package]] +name = "globset" +version = "0.4.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "52dfc19153a48bde0cbd630453615c8151bce3a5adfac7a0aebfbf0a1e1f57e3" +dependencies = [ + "aho-corasick", + "bstr", + "log", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "globwalk" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93e3af942408868f6934a7b85134a3230832b9977cf66125df2f9edcfce4ddcc" +dependencies = [ + "bitflags 1.3.2", + "ignore", + "walkdir", +] + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hashbrown" +version = "0.16.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "841d1cc9bed7f9236f321df977030373f4a4163ae1a7dbfe1a51a2c1a51d9100" + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" + +[[package]] +name = "home" +version = "0.5.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" +dependencies = [ + "windows-sys 0.52.0", +] + +[[package]] +name = "iana-time-zone" +version = "0.1.64" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "33e57f83510bb73707521ebaffa789ec8caf86f9657cad665b092b581d40e9fb" +dependencies = [ + "android_system_properties", + "core-foundation-sys", + "iana-time-zone-haiku", + "js-sys", + "log", + "wasm-bindgen", + "windows-core", +] + +[[package]] +name = "iana-time-zone-haiku" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" +dependencies = [ + "cc", +] + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "ignore" +version = "0.4.25" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d3d782a365a015e0f5c04902246139249abf769125006fbe7649e2ee88169b4a" +dependencies = [ + "crossbeam-deque", + "globset", + "log", + "memchr", + "regex-automata", + "same-file", + "walkdir", + "winapi-util", +] + +[[package]] +name = "indexmap" +version = "2.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ad4bb2b565bca0645f4d68c5c9af97fba094e9791da685bf83cb5f3ce74acf2" +dependencies = [ + "equivalent", + "hashbrown", +] + +[[package]] +name = "itertools" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba291022dbbd398a455acf126c1e341954079855bc60dfdda641363bd6922569" +dependencies = [ + "either", +] + +[[package]] +name = "itoa" +version = "1.0.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92ecc6618181def0457392ccd0ee51198e065e016d1d527a7ac1b6dc7c1f09d2" + +[[package]] +name = "js-sys" +version = "0.3.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "464a3709c7f55f1f721e5389aa6ea4e3bc6aba669353300af094b29ffbdde1d8" +dependencies = [ + "once_cell", + "wasm-bindgen", +] + +[[package]] +name = "libc" +version = "0.2.179" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c5a2d376baa530d1238d133232d15e239abad80d05838b4b59354e5268af431f" + +[[package]] +name = "libloading" +version = "0.8.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7c4b02199fee7c5d21a5ae7d8cfa79a6ef5bb2fc834d6e9058e89c825efdc55" +dependencies = [ + "cfg-if", + "windows-link", +] + +[[package]] +name = "libredox" +version = "0.1.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d0b95e02c851351f877147b7deea7b1afb1df71b63aa5f8270716e0c5720616" +dependencies = [ + "bitflags 2.10.0", + "libc", + "redox_syscall", +] + +[[package]] +name = "linux-raw-sys" +version = "0.4.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" + +[[package]] +name = "linux-raw-sys" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df1d3c3b53da64cf5760482273a98e575c651a67eec7f77df96b5b642de8f039" + +[[package]] +name = "log" +version = "0.4.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897" + +[[package]] +name = "lunarcore" +version = "1.0.0" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-io", + "embuild", + "esp-idf-hal", + "esp-idf-svc", + "esp-idf-sys", + "heapless", + "log", + "nb 1.1.0", +] + +[[package]] +name = "memchr" +version = "2.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f52b00d39961fc5b2736ea853c9cc86238e165017a493d1d5c8eac6bdc4cc273" + +[[package]] +name = "minimal-lexical" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" + +[[package]] +name = "nb" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "nix" +version = "0.29.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "71e2746dc3a24dd78b3cfcb7be93368c6de9963d30f43a6a73998a9cf4b17b46" +dependencies = [ + "bitflags 2.10.0", + "cfg-if", + "cfg_aliases", + "libc", +] + +[[package]] +name = "nom" +version = "7.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" +dependencies = [ + "memchr", + "minimal-lexical", +] + +[[package]] +name = "normpath" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf23ab2b905654b4cb177e30b629937b3868311d4e1cba859f899c041046e69b" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + +[[package]] +name = "num_enum" +version = "0.7.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1207a7e20ad57b847bbddc6776b968420d38292bbfe2089accff5e19e82454c" +dependencies = [ + "num_enum_derive", + "rustversion", +] + +[[package]] +name = "num_enum_derive" +version = "0.7.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff32365de1b6743cb203b710788263c44a03de03802daf96092f2da4fe6ba4d7" +dependencies = [ + "proc-macro-crate", + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "once_cell" +version = "1.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" + +[[package]] +name = "pin-project-lite" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "prettyplease" +version = "0.2.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b" +dependencies = [ + "proc-macro2", + "syn 2.0.113", +] + +[[package]] +name = "proc-macro-crate" +version = "3.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "219cb19e96be00ab2e37d6e299658a0cfa83e52429179969b0f0121b4ac46983" +dependencies = [ + "toml_edit", +] + +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +dependencies = [ + "proc-macro-error-attr2", + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "proc-macro2" +version = "1.0.104" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9695f8df41bb4f3d222c95a67532365f569318332d03d5f3f67f37b20e6ebdf0" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.42" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a338cc41d27e6cc6dce6cefc13a0729dfbb81c262b1f519331575dd80ef3067f" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r-efi" +version = "5.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f" + +[[package]] +name = "redox_syscall" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49f3fe0889e69e2ae9e41f4d6c4c0181701d00e4697b356fb1f74173a5e0ee27" +dependencies = [ + "bitflags 2.10.0", +] + +[[package]] +name = "regex" +version = "1.12.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "843bc0191f75f3e22651ae5f1e72939ab2f72a4bc30fa80a066bd66edefc24d4" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5276caf25ac86c8d810222b3dbb938e512c55c6831a10f3e6ed1c93b84041f1c" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a2d987857b319362043e95f5353c0535c1f58eec5336fdfcf626430af7def58" + +[[package]] +name = "remove_dir_all" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a694f9e0eb3104451127f6cc1e5de55f59d3b1fc8c5ddfaeb6f1e716479ceb4a" +dependencies = [ + "cfg-if", + "cvt", + "fs_at", + "libc", + "normpath", + "windows-sys 0.59.0", +] + +[[package]] +name = "rustc-hash" +version = "2.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "357703d41365b4b27c590e3ed91eabb1b663f07c4c084095e60cbed4362dff0d" + +[[package]] +name = "rustix" +version = "0.38.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fdb5bc1ae2baa591800df16c9ca78619bf65c0488b41b96ccec5d11220d8c154" +dependencies = [ + "bitflags 2.10.0", + "errno", + "libc", + "linux-raw-sys 0.4.15", + "windows-sys 0.59.0", +] + +[[package]] +name = "rustix" +version = "1.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "146c9e247ccc180c1f61615433868c99f3de3ae256a30a43b49f67c2d9171f34" +dependencies = [ + "bitflags 2.10.0", + "errno", + "libc", + "linux-raw-sys 0.11.0", + "windows-sys 0.61.2", +] + +[[package]] +name = "rustversion" +version = "1.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" + +[[package]] +name = "same-file" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "semver" +version = "1.0.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d767eb0aabc880b29956c35734170f26ed551a859dbd361d140cdbeca61ab1e2" +dependencies = [ + "serde", + "serde_core", +] + +[[package]] +name = "serde" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "serde_json" +version = "1.0.148" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3084b546a1dd6289475996f182a22aba973866ea8e8b02c51d9f46b1336a22da" +dependencies = [ + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + +[[package]] +name = "shlex" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + +[[package]] +name = "stable_deref_trait" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" + +[[package]] +name = "strum" +version = "0.24.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "063e6045c0e62079840579a7e47a355ae92f60eb74daaf156fb1e84ba164e63f" +dependencies = [ + "strum_macros 0.24.3", +] + +[[package]] +name = "strum" +version = "0.25.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "290d54ea6f91c969195bdbcd7442c8c2a2ba87da8bf60a7ee86a235d4bc1e125" +dependencies = [ + "strum_macros 0.25.3", +] + +[[package]] +name = "strum_macros" +version = "0.24.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e385be0d24f186b4ce2f9982191e7101bb737312ad61c1f2f984f34bcf85d59" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "rustversion", + "syn 1.0.109", +] + +[[package]] +name = "strum_macros" +version = "0.25.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "23dc1fa9ac9c169a78ba62f0b841814b7abae11bdd047b9c58f893439e309ea0" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "rustversion", + "syn 2.0.113", +] + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.113" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "678faa00651c9eb72dd2020cbdf275d92eccb2400d568e419efdd64838145cb4" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "tempfile" +version = "3.24.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "655da9c7eb6305c55742045d5a8d2037996d61d8de95806335c7c86ce0f82e9c" +dependencies = [ + "fastrand", + "getrandom", + "once_cell", + "rustix 1.1.3", + "windows-sys 0.61.2", +] + +[[package]] +name = "thiserror" +version = "1.0.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6aaf5339b578ea85b50e080feb250a3e8ae8cfcdff9a461c9ec2904bc923f52" +dependencies = [ + "thiserror-impl 1.0.69", +] + +[[package]] +name = "thiserror" +version = "2.0.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f63587ca0f12b72a0600bcba1d40081f830876000bb46dd2337a3051618f4fc8" +dependencies = [ + "thiserror-impl 2.0.17", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff15c8ecd7de3849db632e14d18d2571fa09dfc5ed93479bc4485c7a517c913" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "toml_datetime" +version = "0.7.5+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92e1cfed4a3038bc5a127e35a2d360f145e1f4b971b551a2ba5fd7aedf7e1347" +dependencies = [ + "serde_core", +] + +[[package]] +name = "toml_edit" +version = "0.23.10+spec-1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "84c8b9f757e028cee9fa244aea147aab2a9ec09d5325a9b01e0a49730c2b5269" +dependencies = [ + "indexmap", + "toml_datetime", + "toml_parser", + "winnow", +] + +[[package]] +name = "toml_parser" +version = "1.0.6+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a3198b4b0a8e11f09dd03e133c0280504d0801269e9afa46362ffde1cbeebf44" +dependencies = [ + "winnow", +] + +[[package]] +name = "uncased" +version = "0.9.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e1b88fcfe09e89d3866a5c11019378088af2d24c3fbd4f0543f96b479ec90697" +dependencies = [ + "version_check", +] + +[[package]] +name = "unicode-ident" +version = "1.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5" + +[[package]] +name = "unicode-xid" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853" + +[[package]] +name = "version_check" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "walkdir" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b" +dependencies = [ + "same-file", + "winapi-util", +] + +[[package]] +name = "wasip2" +version = "1.0.1+wasi-0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0562428422c63773dad2c345a1882263bbf4d65cf3f42e90921f787ef5ad58e7" +dependencies = [ + "wit-bindgen", +] + +[[package]] +name = "wasm-bindgen" +version = "0.2.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d759f433fa64a2d763d1340820e46e111a7a5ab75f993d1852d70b03dbb80fd" +dependencies = [ + "cfg-if", + "once_cell", + "rustversion", + "wasm-bindgen-macro", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "48cb0d2638f8baedbc542ed444afc0644a29166f1595371af4fecf8ce1e7eeb3" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cefb59d5cd5f92d9dcf80e4683949f15ca4b511f4ac0a6e14d4e1ac60c6ecd40" +dependencies = [ + "bumpalo", + "proc-macro2", + "quote", + "syn 2.0.113", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cbc538057e648b67f72a982e708d485b2efa771e1ac05fec311f9f63e5800db4" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "which" +version = "4.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" +dependencies = [ + "either", + "home", + "once_cell", + "rustix 0.38.44", +] + +[[package]] +name = "winapi-util" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c2a7b1c03c876122aa43f3020e6c3c3ee5c05081c9a00739faf7503aeba10d22" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "windows-core" +version = "0.62.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8e83a14d34d0623b51dce9581199302a221863196a1dde71a7663a4c2be9deb" +dependencies = [ + "windows-implement", + "windows-interface", + "windows-link", + "windows-result", + "windows-strings", +] + +[[package]] +name = "windows-implement" +version = "0.60.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "053e2e040ab57b9dc951b72c264860db7eb3b0200ba345b4e4c3b14f67855ddf" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "windows-interface" +version = "0.59.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f316c4a2570ba26bbec722032c4099d8c8bc095efccdc15688708623367e358" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.113", +] + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-result" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7781fa89eaf60850ac3d2da7af8e5242a5ea78d1a11c49bf2910bb5a73853eb5" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-strings" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7837d08f69c77cf6b07689544538e017c1bfcf57e34b4c0ff58e6c2cd3b37091" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-sys" +version = "0.60.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2f500e4d28234f72040990ec9d39e3a6b950f9f22d3dba18416c35882612bcb" +dependencies = [ + "windows-targets 0.53.5", +] + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm 0.52.6", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", + "windows_i686_gnullvm 0.52.6", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm 0.52.6", + "windows_x86_64_msvc 0.52.6", +] + +[[package]] +name = "windows-targets" +version = "0.53.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4945f9f551b88e0d65f3db0bc25c33b8acea4d9e41163edf90dcd0b19f9069f3" +dependencies = [ + "windows-link", + "windows_aarch64_gnullvm 0.53.1", + "windows_aarch64_msvc 0.53.1", + "windows_i686_gnu 0.53.1", + "windows_i686_gnullvm 0.53.1", + "windows_i686_msvc 0.53.1", + "windows_x86_64_gnu 0.53.1", + "windows_x86_64_gnullvm 0.53.1", + "windows_x86_64_msvc 0.53.1", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a9d8416fa8b42f5c947f8482c43e7d89e73a173cead56d044f6a56104a6d1b53" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9d782e804c2f632e395708e99a94275910eb9100b2114651e04744e9b125006" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnu" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "960e6da069d81e09becb0ca57a65220ddff016ff2d6af6a223cf372a506593a3" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa7359d10048f68ab8b09fa71c3daccfb0e9b559aed648a8f95469c27057180c" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_i686_msvc" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e7ac75179f18232fe9c285163565a57ef8d3c89254a30685b57d83a38d326c2" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c3842cdd74a865a8066ab39c8a7a473c0778a3f29370b5fd6b4b9aa7df4a499" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ffa179e2d07eee8ad8f57493436566c7cc30ac536a3379fdf008f47f6bb7ae1" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d6bbff5f0aada427a1e5a6da5f1f98158182f26556f345ac9e04d36d0ebed650" + +[[package]] +name = "winnow" +version = "0.7.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a5364e9d77fcdeeaa6062ced926ee3381faa2ee02d3eb83a5c27a8825540829" +dependencies = [ + "memchr", +] + +[[package]] +name = "wit-bindgen" +version = "0.46.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f17a85883d4e6d00e8a97c586de764dabcc06133f7f1d55dce5cdc070ad7fe59" + +[[package]] +name = "zmij" +version = "1.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dcb2c125bd7365735bebeb420ccb880265ed2d2bddcbcd49f597fdfe6bd5e577" diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..6a17a2a --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,37 @@ +[package] +name = "lunarcore" +version = "1.0.0" +edition = "2021" +license = "MIT" + +[dependencies] +esp-idf-hal = { version = "0.45", features = ["critical-section"] } +esp-idf-svc = "0.51" +esp-idf-sys = { version = "0.36", features = ["binstart"] } +embedded-hal = "1.0" +embedded-io = "0.6" +heapless = "0.8" +nb = "1.1" +critical-section = "1.2" +log = "0.4" + +[build-dependencies] +embuild = "0.33" + +[features] +default = ["ble"] +ble = [] +meshtastic = [] +rnode = [] + +[profile.release] +opt-level = "s" +lto = false +codegen-units = 1 + +[profile.dev] +opt-level = "z" + +[[bin]] +name = "lunarcore" +path = "src/main.rs" diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..14fac91 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2026 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..ae16ac3 --- /dev/null +++ b/README.md @@ -0,0 +1,25 @@ +# LunarCore + +Multi-protocol mesh firmware for ESP32-S3 LoRa devices. + +## Protocols + +- MeshCore +- Meshtastic +- RNode/KISS (Reticulum) + +## Hardware + +Heltec WiFi LoRa 32 V3 (ESP32-S3 + SX1262) + +## Build + +```bash +espup install +cargo build --release +espflash flash target/xtensa-esp32s3-espidf/release/lunarcore --monitor +``` + +## License + +MIT diff --git a/build.rs b/build.rs new file mode 100644 index 0000000..112ec3f --- /dev/null +++ b/build.rs @@ -0,0 +1,3 @@ +fn main() { + embuild::espidf::sysenv::output(); +} diff --git a/rust-toolchain.toml b/rust-toolchain.toml new file mode 100644 index 0000000..0a93d54 --- /dev/null +++ b/rust-toolchain.toml @@ -0,0 +1,2 @@ +[toolchain] +channel = "esp" diff --git a/sdkconfig.defaults b/sdkconfig.defaults new file mode 100644 index 0000000..1dfadf9 --- /dev/null +++ b/sdkconfig.defaults @@ -0,0 +1,17 @@ +CONFIG_IDF_TARGET="esp32s3" +CONFIG_ESPTOOLPY_FLASHSIZE_8MB=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y +CONFIG_SPI_MASTER_IN_IRAM=y +CONFIG_GPIO_CTRL_FUNC_IN_IRAM=y +CONFIG_BT_ENABLED=y +CONFIG_BT_NIMBLE_ENABLED=y +CONFIG_BT_NIMBLE_MAX_CONNECTIONS=3 +CONFIG_BT_NIMBLE_ROLE_PERIPHERAL=y +CONFIG_BT_NIMBLE_ROLE_BROADCASTER=y +CONFIG_BT_NIMBLE_GAP_DEVICE_NAME_MAX_LEN=32 +CONFIG_ESP_TASK_WDT_EN=y +CONFIG_ESP_TASK_WDT_TIMEOUT_S=30 +CONFIG_LOG_DEFAULT_LEVEL_INFO=y +CONFIG_ESP_MAIN_TASK_STACK_SIZE=131072 +CONFIG_PARTITION_TABLE_SINGLE_APP_LARGE=y +CONFIG_ADC_DISABLE_DAC=y diff --git a/src/ble.rs b/src/ble.rs new file mode 100644 index 0000000..c64f5ec --- /dev/null +++ b/src/ble.rs @@ -0,0 +1,1267 @@ +use core::cell::RefCell; +use core::sync::atomic::{AtomicBool, AtomicU16, Ordering}; +use critical_section::Mutex; +use heapless::{Vec, Deque}; + +pub const MAX_CONNECTIONS: usize = 3; + +pub const MAX_MTU: usize = 512; + +pub const MAX_ADV_DATA: usize = 31; + +pub const MAX_SCAN_RSP: usize = 31; + +pub const TX_QUEUE_DEPTH: usize = 8; + +pub const RX_BUFFER_SIZE: usize = 512; + +pub mod nus { + + pub const SERVICE: [u8; 16] = [ + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e, + ]; + + pub const RX: [u8; 16] = [ + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x02, 0x00, 0x40, 0x6e, + ]; + + pub const TX: [u8; 16] = [ + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x03, 0x00, 0x40, 0x6e, + ]; +} + +pub mod meshtastic { + + pub const SERVICE: [u8; 16] = [ + 0xfd, 0xea, 0x73, 0xe2, 0xca, 0x5d, 0xa8, 0x9f, + 0x1f, 0x46, 0xa8, 0x15, 0x18, 0xb2, 0xa1, 0x6b, + ]; + + pub const FROM_RADIO: [u8; 16] = [ + 0x02, 0x00, 0x12, 0xac, 0x42, 0x02, 0x78, 0xb8, + 0xed, 0x11, 0x93, 0x49, 0x9e, 0xe6, 0x55, 0x2c, + ]; + + pub const TO_RADIO: [u8; 16] = [ + 0xe7, 0x01, 0x44, 0x12, 0x66, 0x78, 0xdd, 0xa1, + 0xad, 0x4d, 0x9e, 0x12, 0xd2, 0x76, 0x5c, 0xf7, + ]; + + pub const FROM_NUM: [u8; 16] = [ + 0x53, 0x44, 0xe3, 0x47, 0x75, 0xaa, 0x70, 0xa6, + 0x66, 0x4f, 0x00, 0xa8, 0x8c, 0xa1, 0x9d, 0xed, + ]; +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ConnectionState { + Disconnected, + Connected, + Subscribed, + Encrypted, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ServiceType { + Nus, + Meshtastic, + Unknown, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum BleError { + NotInitialized, + AlreadyInitialized, + MaxConnections, + NotConnected, + NotSubscribed, + MtuExceeded, + QueueFull, + InvalidHandle, + StackError(i32), + Timeout, + InvalidParameter, +} + +#[derive(Debug, Clone)] +pub enum BleEvent { + Connected { conn_handle: u16 }, + Disconnected { conn_handle: u16, reason: u8 }, + MtuExchange { conn_handle: u16, mtu: u16 }, + Subscribed { conn_handle: u16, service: ServiceType }, + Unsubscribed { conn_handle: u16, service: ServiceType }, + DataReceived { conn_handle: u16, service: ServiceType, data: Vec }, + TxComplete { conn_handle: u16 }, + EncryptionChanged { conn_handle: u16, encrypted: bool }, +} + +#[derive(Debug, Clone)] +pub struct BleDataPacket { + + pub conn_handle: u16, + + pub service: ServiceType, + + pub data: Vec, +} + +pub struct BleConnection { + + handle: u16, + + state: ConnectionState, + + service: ServiceType, + + mtu: u16, + + tx_queue: Deque, TX_QUEUE_DEPTH>, + + rx_buffer: Vec, + + nus_tx_notify: bool, + + mesh_from_radio_notify: bool, + + mesh_from_num_notify: bool, + + encrypted: bool, + + peer_addr: [u8; 6], + + conn_interval: u16, + + conn_latency: u16, + + supervision_timeout: u16, +} + +impl BleConnection { + pub fn new(handle: u16, peer_addr: [u8; 6]) -> Self { + Self { + handle, + state: ConnectionState::Connected, + service: ServiceType::Unknown, + mtu: 23, + tx_queue: Deque::new(), + rx_buffer: Vec::new(), + nus_tx_notify: false, + mesh_from_radio_notify: false, + mesh_from_num_notify: false, + encrypted: false, + peer_addr, + conn_interval: 0, + conn_latency: 0, + supervision_timeout: 0, + } + } + + pub fn is_subscribed(&self) -> bool { + match self.service { + ServiceType::Nus => self.nus_tx_notify, + ServiceType::Meshtastic => self.mesh_from_radio_notify, + ServiceType::Unknown => false, + } + } + + pub fn max_payload(&self) -> usize { + (self.mtu as usize).saturating_sub(3) + } + + pub fn queue_tx(&mut self, data: &[u8]) -> Result<(), BleError> { + if data.len() > self.max_payload() { + return Err(BleError::MtuExceeded); + } + + let mut vec = Vec::new(); + vec.extend_from_slice(data).map_err(|_| BleError::MtuExceeded)?; + + self.tx_queue.push_back(vec).map_err(|_| BleError::QueueFull)?; + Ok(()) + } +} + +static BLE_STATE: Mutex>> = Mutex::new(RefCell::new(None)); + +static EVENT_QUEUE: Mutex>> = Mutex::new(RefCell::new(Deque::new())); + +static ADVERTISING: AtomicBool = AtomicBool::new(false); + +static CONNECTION_COUNT: AtomicU16 = AtomicU16::new(0); + +struct BleState { + connections: [Option; MAX_CONNECTIONS], + device_name: [u8; 32], + device_name_len: usize, + + nus_rx_handle: u16, + nus_tx_handle: u16, + mesh_to_radio_handle: u16, + mesh_from_radio_handle: u16, + mesh_from_num_handle: u16, + + from_radio_queue: Deque, 16>, +} + +impl BleState { + fn new(name: &str) -> Self { + let mut device_name = [0u8; 32]; + let name_bytes = name.as_bytes(); + let len = core::cmp::min(name_bytes.len(), 32); + device_name[..len].copy_from_slice(&name_bytes[..len]); + + Self { + connections: [None, None, None], + device_name, + device_name_len: len, + nus_rx_handle: 0, + nus_tx_handle: 0, + mesh_to_radio_handle: 0, + mesh_from_radio_handle: 0, + mesh_from_num_handle: 0, + from_radio_queue: Deque::new(), + } + } + + fn find_connection(&mut self, handle: u16) -> Option<&mut BleConnection> { + for slot in &mut self.connections { + if let Some(conn) = slot { + if conn.handle == handle { + return Some(conn); + } + } + } + None + } + + fn add_connection(&mut self, handle: u16, peer_addr: [u8; 6]) -> Result<(), BleError> { + for slot in &mut self.connections { + if slot.is_none() { + *slot = Some(BleConnection::new(handle, peer_addr)); + CONNECTION_COUNT.fetch_add(1, Ordering::SeqCst); + return Ok(()); + } + } + Err(BleError::MaxConnections) + } + + fn remove_connection(&mut self, handle: u16) { + for slot in &mut self.connections { + if let Some(conn) = slot { + if conn.handle == handle { + *slot = None; + CONNECTION_COUNT.fetch_sub(1, Ordering::SeqCst); + return; + } + } + } + } +} + +pub struct BleManager { + initialized: bool, +} + +impl BleManager { + + pub const fn new() -> Self { + Self { initialized: false } + } + + pub fn init(&mut self, device_name: &str) -> Result<(), BleError> { + if self.initialized { + return Err(BleError::AlreadyInitialized); + } + + critical_section::with(|cs| { + BLE_STATE.borrow(cs).replace(Some(BleState::new(device_name))); + }); + + unsafe { + self.init_nimble()?; + self.register_services()?; + } + + self.initialized = true; + Ok(()) + } + + unsafe fn init_nimble(&self) -> Result<(), BleError> { + + #[cfg(target_arch = "xtensa")] + { + extern "C" { + fn nimble_port_init() -> i32; + fn nimble_port_freertos_init(task: extern "C" fn(*mut core::ffi::c_void)) -> i32; + } + + let rc = nimble_port_init(); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + + esp_idf_sys::ble_hs_cfg.sync_cb = Some(on_sync); + esp_idf_sys::ble_hs_cfg.reset_cb = Some(on_reset); + + let rc = nimble_port_freertos_init(nimble_host_task); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + } + + #[cfg(not(target_arch = "xtensa"))] + { + + } + + Ok(()) + } + + unsafe fn register_services(&self) -> Result<(), BleError> { + #[cfg(target_arch = "xtensa")] + { + + static NUS_SERVICE: GattService = GattService { + uuid: &nus::SERVICE, + characteristics: &[ + GattCharacteristic { + uuid: &nus::RX, + flags: CHR_FLAG_WRITE | CHR_FLAG_WRITE_NO_RSP, + callback: Some(nus_rx_callback), + }, + GattCharacteristic { + uuid: &nus::TX, + flags: CHR_FLAG_NOTIFY, + callback: None, + }, + ], + }; + + static MESH_SERVICE: GattService = GattService { + uuid: &meshtastic::SERVICE, + characteristics: &[ + GattCharacteristic { + uuid: &meshtastic::TO_RADIO, + flags: CHR_FLAG_WRITE | CHR_FLAG_WRITE_NO_RSP, + callback: Some(mesh_to_radio_callback), + }, + GattCharacteristic { + uuid: &meshtastic::FROM_RADIO, + flags: CHR_FLAG_READ | CHR_FLAG_NOTIFY, + callback: Some(mesh_from_radio_callback), + }, + GattCharacteristic { + uuid: &meshtastic::FROM_NUM, + flags: CHR_FLAG_NOTIFY, + callback: None, + }, + ], + }; + + extern "C" { + fn ble_gatts_count_cfg(svcs: *const GattService) -> i32; + fn ble_gatts_add_svcs(svcs: *const GattService) -> i32; + } + + let services = [NUS_SERVICE, MESH_SERVICE]; + + let rc = ble_gatts_count_cfg(services.as_ptr()); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + + let rc = ble_gatts_add_svcs(services.as_ptr()); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + } + + Ok(()) + } + + pub fn start_advertising(&mut self) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + #[cfg(target_arch = "xtensa")] + unsafe { + let mut adv_params = ble_gap_adv_params { + conn_mode: BLE_GAP_CONN_MODE_UND, + disc_mode: BLE_GAP_DISC_MODE_GEN, + itvl_min: 160, + itvl_max: 320, + channel_map: 0x07, + filter_policy: 0, + high_duty_cycle: 0, + }; + + extern "C" { + fn ble_gap_adv_start( + own_addr_type: u8, + direct_addr: *const u8, + duration_ms: i32, + params: *const ble_gap_adv_params, + cb: extern "C" fn(*mut ble_gap_event, *mut core::ffi::c_void) -> i32, + arg: *mut core::ffi::c_void, + ) -> i32; + } + + let rc = ble_gap_adv_start( + BLE_OWN_ADDR_PUBLIC, + core::ptr::null(), + BLE_HS_FOREVER, + &adv_params, + gap_event_callback, + core::ptr::null_mut(), + ); + + if rc != 0 { + return Err(BleError::StackError(rc)); + } + } + + ADVERTISING.store(true, Ordering::SeqCst); + Ok(()) + } + + pub fn stop_advertising(&mut self) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + #[cfg(target_arch = "xtensa")] + unsafe { + extern "C" { + fn ble_gap_adv_stop() -> i32; + } + + let rc = ble_gap_adv_stop(); + if rc != 0 && rc != BLE_HS_EALREADY { + return Err(BleError::StackError(rc)); + } + } + + ADVERTISING.store(false, Ordering::SeqCst); + Ok(()) + } + + pub fn is_advertising(&self) -> bool { + ADVERTISING.load(Ordering::SeqCst) + } + + pub fn connection_count(&self) -> u16 { + CONNECTION_COUNT.load(Ordering::SeqCst) + } + + pub fn send(&self, conn_handle: u16, data: &[u8]) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + critical_section::with(|cs| { + let mut state = BLE_STATE.borrow(cs).borrow_mut(); + let state = state.as_mut().ok_or(BleError::NotInitialized)?; + + let conn = state.find_connection(conn_handle).ok_or(BleError::NotConnected)?; + + if !conn.is_subscribed() { + return Err(BleError::NotSubscribed); + } + + if data.len() > conn.max_payload() { + return Err(BleError::MtuExceeded); + } + + conn.queue_tx(data)?; + + let char_handle = match conn.service { + ServiceType::Nus => state.nus_tx_handle, + ServiceType::Meshtastic => state.mesh_from_radio_handle, + ServiceType::Unknown => return Err(BleError::InvalidHandle), + }; + + #[cfg(target_arch = "xtensa")] + unsafe { + self.send_notification(conn_handle, char_handle, data)?; + } + + Ok(()) + }) + } + + #[cfg(target_arch = "xtensa")] + unsafe fn send_notification(&self, conn_handle: u16, char_handle: u16, data: &[u8]) -> Result<(), BleError> { + extern "C" { + fn ble_gatts_notify_custom( + conn_handle: u16, + chr_val_handle: u16, + om: *mut os_mbuf, + ) -> i32; + fn os_mbuf_get_pkthdr(pool: *mut core::ffi::c_void, user_hdr_len: u16) -> *mut os_mbuf; + fn os_mbuf_append(om: *mut os_mbuf, data: *const u8, len: u16) -> i32; + fn os_mbuf_free_chain(om: *mut os_mbuf) -> i32; + fn ble_hs_mbuf_att_pkt() -> *mut os_mbuf; + } + + let om = ble_hs_mbuf_att_pkt(); + if om.is_null() { + return Err(BleError::StackError(-1)); + } + + let rc = os_mbuf_append(om, data.as_ptr(), data.len() as u16); + if rc != 0 { + os_mbuf_free_chain(om); + return Err(BleError::StackError(rc)); + } + + let rc = ble_gatts_notify_custom(conn_handle, char_handle, om); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + + Ok(()) + } + + pub fn broadcast(&self, service: ServiceType, data: &[u8]) -> Result { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + let mut sent = 0; + + critical_section::with(|cs| { + let mut state = BLE_STATE.borrow(cs).borrow_mut(); + let state = state.as_mut().ok_or(BleError::NotInitialized)?; + + for slot in &mut state.connections { + if let Some(conn) = slot { + if conn.service == service && conn.is_subscribed() { + if data.len() <= conn.max_payload() { + if conn.queue_tx(data).is_ok() { + sent += 1; + } + } + } + } + } + Ok(sent) + }) + } + + pub fn notify_from_num(&self, counter: u32) -> Result { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + let counter_bytes = counter.to_le_bytes(); + let mut sent = 0; + + critical_section::with(|cs| { + let mut state = BLE_STATE.borrow(cs).borrow_mut(); + let state = state.as_mut().ok_or(BleError::NotInitialized)?; + + let from_num_handle = state.mesh_from_num_handle; + + for slot in &mut state.connections { + if let Some(conn) = slot { + + if conn.mesh_from_num_notify { + #[cfg(target_arch = "xtensa")] + unsafe { + if self.send_notification(conn.handle, from_num_handle, &counter_bytes).is_ok() { + sent += 1; + } + } + + #[cfg(not(target_arch = "xtensa"))] + { + sent += 1; + } + } + } + } + Ok(sent) + }) + } + + pub fn queue_from_radio(&self, data: &[u8]) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + if data.len() > MAX_MTU { + return Err(BleError::MtuExceeded); + } + + critical_section::with(|cs| { + let mut state = BLE_STATE.borrow(cs).borrow_mut(); + let state = state.as_mut().ok_or(BleError::NotInitialized)?; + + let mut vec: Vec = Vec::new(); + vec.extend_from_slice(data).map_err(|_| BleError::MtuExceeded)?; + + state.from_radio_queue.push_back(vec).map_err(|_| BleError::QueueFull)?; + + Ok(()) + }) + } + + pub fn dequeue_from_radio(&self) -> Option> { + if !self.initialized { + return None; + } + + critical_section::with(|cs| { + let mut state = BLE_STATE.borrow(cs).borrow_mut(); + state.as_mut()?.from_radio_queue.pop_front() + }) + } + + pub fn has_from_radio_data(&self) -> bool { + if !self.initialized { + return false; + } + + critical_section::with(|cs| { + let state = BLE_STATE.borrow(cs).borrow(); + state.as_ref().map_or(false, |s| !s.from_radio_queue.is_empty()) + }) + } + + pub fn poll_event(&self) -> Option { + critical_section::with(|cs| { + EVENT_QUEUE.borrow(cs).borrow_mut().pop_front() + }) + } + + pub fn read(&self) -> Option> { + self.read_with_service().map(|packet| packet.data) + } + + pub fn read_with_service(&self) -> Option { + if !self.initialized { + return None; + } + + critical_section::with(|cs| { + let mut queue = EVENT_QUEUE.borrow(cs).borrow_mut(); + + let mut found_idx = None; + for (idx, event) in queue.iter().enumerate() { + if matches!(event, BleEvent::DataReceived { .. }) { + found_idx = Some(idx); + break; + } + } + + if found_idx.is_some() { + + let mut temp: Deque = Deque::new(); + let mut result = None; + + while let Some(event) = queue.pop_front() { + if result.is_none() { + if let BleEvent::DataReceived { conn_handle, service, data } = event { + result = Some(BleDataPacket { + conn_handle, + service, + data, + }); + continue; + } + } + let _ = temp.push_back(event); + } + + while let Some(event) = temp.pop_front() { + let _ = queue.push_back(event); + } + + result + } else { + None + } + }) + } + + pub fn process_tx(&self) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + critical_section::with(|cs| { + let mut state = BLE_STATE.borrow(cs).borrow_mut(); + let state = state.as_mut().ok_or(BleError::NotInitialized)?; + + for slot in &mut state.connections { + if let Some(conn) = slot { + if conn.is_subscribed() { + while let Some(data) = conn.tx_queue.pop_front() { + let char_handle = match conn.service { + ServiceType::Nus => state.nus_tx_handle, + ServiceType::Meshtastic => state.mesh_from_radio_handle, + ServiceType::Unknown => continue, + }; + + #[cfg(target_arch = "xtensa")] + unsafe { + + let _ = self.send_notification(conn.handle, char_handle, &data); + } + + } + } + } + } + Ok(()) + }) + } + + pub fn disconnect(&self, conn_handle: u16) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + #[cfg(target_arch = "xtensa")] + unsafe { + extern "C" { + fn ble_gap_terminate(conn_handle: u16, reason: u8) -> i32; + } + + let rc = ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + } + + Ok(()) + } + + pub fn update_conn_params( + &self, + conn_handle: u16, + min_interval: u16, + max_interval: u16, + latency: u16, + timeout: u16, + ) -> Result<(), BleError> { + if !self.initialized { + return Err(BleError::NotInitialized); + } + + #[cfg(target_arch = "xtensa")] + unsafe { + extern "C" { + fn ble_gap_update_params( + conn_handle: u16, + params: *const ble_gap_upd_params, + ) -> i32; + } + + let params = ble_gap_upd_params { + itvl_min: min_interval, + itvl_max: max_interval, + latency, + supervision_timeout: timeout, + min_ce_len: 0, + max_ce_len: 0, + }; + + let rc = ble_gap_update_params(conn_handle, ¶ms); + if rc != 0 { + return Err(BleError::StackError(rc)); + } + } + + Ok(()) + } +} + +impl Default for BleManager { + fn default() -> Self { + Self::new() + } +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn gap_event_callback(event: *mut ble_gap_event, _arg: *mut core::ffi::c_void) -> i32 { + unsafe { + let event = &*event; + + match event.event_type { + BLE_GAP_EVENT_CONNECT => { + let connect = &event.event_data.connect; + if connect.status == 0 { + let conn_handle = connect.conn_handle; + let peer_addr = connect.peer_addr; + + critical_section::with(|cs| { + if let Some(state) = BLE_STATE.borrow(cs).borrow_mut().as_mut() { + let _ = state.add_connection(conn_handle, peer_addr); + } + + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::Connected { conn_handle } + ); + }); + } + } + + BLE_GAP_EVENT_DISCONNECT => { + let disconnect = &event.event_data.disconnect; + let conn_handle = disconnect.conn_handle; + let reason = disconnect.reason as u8; + + critical_section::with(|cs| { + if let Some(state) = BLE_STATE.borrow(cs).borrow_mut().as_mut() { + state.remove_connection(conn_handle); + } + + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::Disconnected { conn_handle, reason } + ); + }); + + if CONNECTION_COUNT.load(Ordering::SeqCst) < MAX_CONNECTIONS as u16 { + + } + } + + BLE_GAP_EVENT_MTU => { + let mtu_event = &event.event_data.mtu; + let conn_handle = mtu_event.conn_handle; + let mtu = mtu_event.value; + + critical_section::with(|cs| { + if let Some(state) = BLE_STATE.borrow(cs).borrow_mut().as_mut() { + if let Some(conn) = state.find_connection(conn_handle) { + conn.mtu = mtu; + } + } + + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::MtuExchange { conn_handle, mtu } + ); + }); + } + + BLE_GAP_EVENT_SUBSCRIBE => { + let subscribe = &event.event_data.subscribe; + let conn_handle = subscribe.conn_handle; + let attr_handle = subscribe.attr_handle; + let notify = subscribe.cur_notify != 0; + + critical_section::with(|cs| { + if let Some(state) = BLE_STATE.borrow(cs).borrow_mut().as_mut() { + + let nus_tx_handle = state.nus_tx_handle; + let mesh_from_radio_handle = state.mesh_from_radio_handle; + let mesh_from_num_handle = state.mesh_from_num_handle; + + if let Some(conn) = state.find_connection(conn_handle) { + let service = if attr_handle == nus_tx_handle { + conn.nus_tx_notify = notify; + conn.service = ServiceType::Nus; + ServiceType::Nus + } else if attr_handle == mesh_from_radio_handle { + conn.mesh_from_radio_notify = notify; + conn.service = ServiceType::Meshtastic; + ServiceType::Meshtastic + } else if attr_handle == mesh_from_num_handle { + conn.mesh_from_num_notify = notify; + conn.service = ServiceType::Meshtastic; + ServiceType::Meshtastic + } else { + ServiceType::Unknown + }; + + if notify { + conn.state = ConnectionState::Subscribed; + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::Subscribed { conn_handle, service } + ); + } else { + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::Unsubscribed { conn_handle, service } + ); + } + } + } + }); + } + + BLE_GAP_EVENT_ENC_CHANGE => { + let enc_change = &event.event_data.enc_change; + let conn_handle = enc_change.conn_handle; + let status = enc_change.status; + + critical_section::with(|cs| { + if let Some(state) = BLE_STATE.borrow(cs).borrow_mut().as_mut() { + if let Some(conn) = state.find_connection(conn_handle) { + conn.encrypted = status == 0; + if conn.encrypted { + conn.state = ConnectionState::Encrypted; + } + } + } + + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::EncryptionChanged { + conn_handle, + encrypted: status == 0, + } + ); + }); + } + + _ => {} + } + } + + 0 +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn nus_rx_callback( + conn_handle: u16, + _attr_handle: u16, + ctxt: *mut ble_gatt_access_ctxt, + _arg: *mut core::ffi::c_void, +) -> i32 { + unsafe { + let ctxt = &*ctxt; + if ctxt.op == BLE_GATT_ACCESS_OP_WRITE_CHR { + let om = ctxt.om; + let mut data: Vec = Vec::new(); + + let mut current = om; + while !current.is_null() { + let mbuf = &*current; + let slice = core::slice::from_raw_parts(mbuf.data, mbuf.len as usize); + let _ = data.extend_from_slice(slice); + current = mbuf.next; + } + + critical_section::with(|cs| { + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::DataReceived { + conn_handle, + service: ServiceType::Nus, + data, + } + ); + }); + } + } + + 0 +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn mesh_to_radio_callback( + conn_handle: u16, + _attr_handle: u16, + ctxt: *mut ble_gatt_access_ctxt, + _arg: *mut core::ffi::c_void, +) -> i32 { + unsafe { + let ctxt = &*ctxt; + if ctxt.op == BLE_GATT_ACCESS_OP_WRITE_CHR { + let om = ctxt.om; + let mut data: Vec = Vec::new(); + + let mut current = om; + while !current.is_null() { + let mbuf = &*current; + let slice = core::slice::from_raw_parts(mbuf.data, mbuf.len as usize); + let _ = data.extend_from_slice(slice); + current = mbuf.next; + } + + critical_section::with(|cs| { + let _ = EVENT_QUEUE.borrow(cs).borrow_mut().push_back( + BleEvent::DataReceived { + conn_handle, + service: ServiceType::Meshtastic, + data, + } + ); + }); + } + } + + 0 +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn mesh_from_radio_callback( + _conn_handle: u16, + _attr_handle: u16, + ctxt: *mut ble_gatt_access_ctxt, + _arg: *mut core::ffi::c_void, +) -> i32 { + unsafe { + let ctxt = &*ctxt; + if ctxt.op == BLE_GATT_ACCESS_OP_READ_CHR { + + critical_section::with(|cs| { + if let Some(state) = BLE_STATE.borrow(cs).borrow_mut().as_mut() { + + if let Some(data) = state.from_radio_queue.pop_front() { + extern "C" { + fn os_mbuf_append(om: *mut os_mbuf, data: *const u8, len: u16) -> i32; + } + let _ = os_mbuf_append(ctxt.om, data.as_ptr(), data.len() as u16); + } + } + }); + } + } + + 0 +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn on_sync() { + + ADVERTISING.store(false, Ordering::SeqCst); +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn on_reset(reason: i32) { + + log::warn!("NimBLE reset: {}", reason); + ADVERTISING.store(false, Ordering::SeqCst); + CONNECTION_COUNT.store(0, Ordering::SeqCst); +} + +#[cfg(target_arch = "xtensa")] +extern "C" fn nimble_host_task(_arg: *mut core::ffi::c_void) { + extern "C" { + fn nimble_port_run() -> !; + } + unsafe { + nimble_port_run(); + } +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +struct ble_gap_event { + event_type: u8, + _padding: [u8; 3], + event_data: ble_gap_event_union, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +union ble_gap_event_union { + connect: ble_gap_event_connect, + disconnect: ble_gap_event_disconnect, + mtu: ble_gap_event_mtu, + subscribe: ble_gap_event_subscribe, + enc_change: ble_gap_event_enc_change, + _raw: [u8; 64], +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct ble_gap_event_connect { + status: i32, + conn_handle: u16, + _padding: [u8; 2], + peer_addr: [u8; 6], +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct ble_gap_event_disconnect { + reason: i32, + conn_handle: u16, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct ble_gap_event_mtu { + conn_handle: u16, + channel_id: u16, + value: u16, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct ble_gap_event_subscribe { + conn_handle: u16, + attr_handle: u16, + reason: u8, + prev_notify: u8, + cur_notify: u8, + prev_indicate: u8, + cur_indicate: u8, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct ble_gap_event_enc_change { + status: i32, + conn_handle: u16, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +struct ble_gap_adv_params { + conn_mode: u8, + disc_mode: u8, + itvl_min: u16, + itvl_max: u16, + channel_map: u8, + filter_policy: u8, + high_duty_cycle: u8, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +struct ble_gap_upd_params { + itvl_min: u16, + itvl_max: u16, + latency: u16, + supervision_timeout: u16, + min_ce_len: u16, + max_ce_len: u16, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +struct ble_gatt_access_ctxt { + op: u8, + om: *mut os_mbuf, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +struct os_mbuf { + data: *const u8, + len: u16, + next: *mut os_mbuf, +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct GattService { + uuid: *const [u8; 16], + characteristics: *const [GattCharacteristic], +} + +#[cfg(target_arch = "xtensa")] +#[repr(C)] +#[derive(Clone, Copy)] +struct GattCharacteristic { + uuid: *const [u8; 16], + flags: u16, + callback: Option i32>, +} + +#[cfg(target_arch = "xtensa")] +unsafe impl Sync for GattService {} +#[cfg(target_arch = "xtensa")] +unsafe impl Send for GattService {} +#[cfg(target_arch = "xtensa")] +unsafe impl Sync for GattCharacteristic {} +#[cfg(target_arch = "xtensa")] +unsafe impl Send for GattCharacteristic {} + +#[cfg(target_arch = "xtensa")] +const BLE_GAP_CONN_MODE_UND: u8 = 0; +#[cfg(target_arch = "xtensa")] +const BLE_GAP_DISC_MODE_GEN: u8 = 2; +#[cfg(target_arch = "xtensa")] +const BLE_OWN_ADDR_PUBLIC: u8 = 0; +#[cfg(target_arch = "xtensa")] +const BLE_HS_FOREVER: i32 = i32::MAX; +#[cfg(target_arch = "xtensa")] +const BLE_HS_EALREADY: i32 = 2; +#[cfg(target_arch = "xtensa")] +const BLE_ERR_REM_USER_CONN_TERM: u8 = 0x13; +#[cfg(target_arch = "xtensa")] +const BLE_GAP_EVENT_CONNECT: u8 = 0; +#[cfg(target_arch = "xtensa")] +const BLE_GAP_EVENT_DISCONNECT: u8 = 1; +#[cfg(target_arch = "xtensa")] +const BLE_GAP_EVENT_MTU: u8 = 15; +#[cfg(target_arch = "xtensa")] +const BLE_GAP_EVENT_SUBSCRIBE: u8 = 14; +#[cfg(target_arch = "xtensa")] +const BLE_GAP_EVENT_ENC_CHANGE: u8 = 10; +#[cfg(target_arch = "xtensa")] +const BLE_GATT_ACCESS_OP_READ_CHR: u8 = 0; +#[cfg(target_arch = "xtensa")] +const BLE_GATT_ACCESS_OP_WRITE_CHR: u8 = 1; +#[cfg(target_arch = "xtensa")] +const CHR_FLAG_WRITE: u16 = 0x0008; +#[cfg(target_arch = "xtensa")] +const CHR_FLAG_WRITE_NO_RSP: u16 = 0x0004; +#[cfg(target_arch = "xtensa")] +const CHR_FLAG_NOTIFY: u16 = 0x0010; +#[cfg(target_arch = "xtensa")] +const CHR_FLAG_READ: u16 = 0x0002; + +pub fn build_adv_data(name: &str, include_nus: bool, include_meshtastic: bool) -> Vec { + let mut data: Vec = Vec::new(); + + let _ = data.push(0x02); + let _ = data.push(0x01); + let _ = data.push(0x06); + + let _ = data.push(0x02); + let _ = data.push(0x0A); + let _ = data.push(0x00); + + let name_bytes = name.as_bytes(); + let max_name = MAX_ADV_DATA - data.len() - 2; + let name_len = core::cmp::min(name_bytes.len(), max_name); + if name_len > 0 { + let _ = data.push((name_len + 1) as u8); + if name_len == name_bytes.len() { + let _ = data.push(0x09); + } else { + let _ = data.push(0x08); + } + for i in 0..name_len { + let _ = data.push(name_bytes[i]); + } + } + + data +} + +pub fn build_scan_rsp(include_nus: bool, include_meshtastic: bool) -> Vec { + let mut data: Vec = Vec::new(); + + if include_nus && data.len() + 18 <= MAX_SCAN_RSP { + let _ = data.push(17); + let _ = data.push(0x07); + for &b in &nus::SERVICE { + let _ = data.push(b); + } + } + + if include_meshtastic && data.len() + 18 <= MAX_SCAN_RSP { + let _ = data.push(17); + let _ = data.push(0x06); + for &b in &meshtastic::SERVICE { + let _ = data.push(b); + } + } + + data +} diff --git a/src/contact.rs b/src/contact.rs new file mode 100644 index 0000000..1e1064b --- /dev/null +++ b/src/contact.rs @@ -0,0 +1,598 @@ +use crate::crypto::{ + sha256::Sha256, + ed25519::{Ed25519, Signature}, +}; +use heapless::Vec as HeaplessVec; + +pub const CONTACT_HELLO_VERSION: u8 = 0x03; + +pub const ED25519_PK_SIZE: usize = 32; + +pub const X25519_PK_SIZE: usize = 32; + +pub const ED25519_SIG_SIZE: usize = 64; + +pub const HASH_SIZE: usize = 32; + +pub const MAX_DID_LENGTH: usize = 128; + +pub const MAX_NAME_LENGTH: usize = 64; + +pub const DILITHIUM_PK_SIZE: usize = 1952; + +pub const DILITHIUM_SIG_SIZE: usize = 2420; + +#[derive(Debug, Clone)] +pub struct ContactHello { + + pub version: u8, + + pub timestamp: u64, + + pub did: HeaplessVec, + + pub ed25519_public: [u8; ED25519_PK_SIZE], + + pub x25519_public: [u8; X25519_PK_SIZE], + + pub name: HeaplessVec, + + pub avatar_hash: [u8; HASH_SIZE], + + pub signature: [u8; ED25519_SIG_SIZE], +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ContactHelloError { + + InvalidVersion, + + InvalidFormat, + + SignatureInvalid, + + BufferTooSmall, + + DidTooLong, + + NameTooLong, +} + +impl ContactHello { + + pub fn new( + timestamp: u64, + did: &[u8], + ed25519_public: [u8; 32], + x25519_public: [u8; 32], + name: &[u8], + avatar_hash: Option<[u8; 32]>, + ) -> Result { + if did.len() > MAX_DID_LENGTH { + return Err(ContactHelloError::DidTooLong); + } + if name.len() > MAX_NAME_LENGTH { + return Err(ContactHelloError::NameTooLong); + } + + let mut did_vec = HeaplessVec::new(); + did_vec.extend_from_slice(did).map_err(|_| ContactHelloError::DidTooLong)?; + + let mut name_vec = HeaplessVec::new(); + name_vec.extend_from_slice(name).map_err(|_| ContactHelloError::NameTooLong)?; + + Ok(Self { + version: CONTACT_HELLO_VERSION, + timestamp, + did: did_vec, + ed25519_public, + x25519_public, + name: name_vec, + avatar_hash: avatar_hash.unwrap_or([0u8; 32]), + signature: [0u8; 64], + }) + } + + pub fn sign(&mut self, private_key: &[u8; 32]) { + let data_to_sign = self.encode_for_signing(); + let sig = Ed25519::sign(private_key, &data_to_sign); + self.signature = sig.0; + } + + pub fn verify(&self) -> Result { + let data = self.encode_for_signing(); + let sig = Signature(self.signature); + Ok(Ed25519::verify(&self.ed25519_public, &data, &sig)) + } + + fn encode_for_signing(&self) -> HeaplessVec { + let mut buf = HeaplessVec::new(); + + let _ = buf.push(self.version); + + let _ = buf.extend_from_slice(&self.timestamp.to_le_bytes()); + + let did_len = self.did.len() as u16; + let _ = buf.extend_from_slice(&did_len.to_le_bytes()); + let _ = buf.extend_from_slice(&self.did); + + let _ = buf.extend_from_slice(&self.ed25519_public); + + let _ = buf.extend_from_slice(&self.x25519_public); + + let _ = buf.push(self.name.len() as u8); + let _ = buf.extend_from_slice(&self.name); + + let _ = buf.extend_from_slice(&self.avatar_hash); + + buf + } + + pub fn encode(&self) -> HeaplessVec { + let signed_data = self.encode_for_signing(); + let mut buf = HeaplessVec::new(); + let _ = buf.extend_from_slice(&signed_data); + let _ = buf.extend_from_slice(&self.signature); + buf + } + + pub fn decode(data: &[u8]) -> Result { + if data.len() < 1 + 8 + 2 + 32 + 32 + 1 + 32 + 64 { + return Err(ContactHelloError::InvalidFormat); + } + + let mut pos = 0; + + let version = data[pos]; + if version != CONTACT_HELLO_VERSION { + return Err(ContactHelloError::InvalidVersion); + } + pos += 1; + + let mut ts_bytes = [0u8; 8]; + ts_bytes.copy_from_slice(&data[pos..pos + 8]); + let timestamp = u64::from_le_bytes(ts_bytes); + pos += 8; + + let mut did_len_bytes = [0u8; 2]; + did_len_bytes.copy_from_slice(&data[pos..pos + 2]); + let did_len = u16::from_le_bytes(did_len_bytes) as usize; + pos += 2; + + if did_len > MAX_DID_LENGTH || pos + did_len > data.len() { + return Err(ContactHelloError::DidTooLong); + } + + let mut did = HeaplessVec::new(); + did.extend_from_slice(&data[pos..pos + did_len]) + .map_err(|_| ContactHelloError::DidTooLong)?; + pos += did_len; + + if pos + 32 > data.len() { + return Err(ContactHelloError::InvalidFormat); + } + let mut ed25519_public = [0u8; 32]; + ed25519_public.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + if pos + 32 > data.len() { + return Err(ContactHelloError::InvalidFormat); + } + let mut x25519_public = [0u8; 32]; + x25519_public.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + if pos >= data.len() { + return Err(ContactHelloError::InvalidFormat); + } + let name_len = data[pos] as usize; + pos += 1; + + if name_len > MAX_NAME_LENGTH || pos + name_len > data.len() { + return Err(ContactHelloError::NameTooLong); + } + + let mut name = HeaplessVec::new(); + name.extend_from_slice(&data[pos..pos + name_len]) + .map_err(|_| ContactHelloError::NameTooLong)?; + pos += name_len; + + if pos + 32 > data.len() { + return Err(ContactHelloError::InvalidFormat); + } + let mut avatar_hash = [0u8; 32]; + avatar_hash.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + if pos + 64 > data.len() { + return Err(ContactHelloError::InvalidFormat); + } + let mut signature = [0u8; 64]; + signature.copy_from_slice(&data[pos..pos + 64]); + + Ok(Self { + version, + timestamp, + did, + ed25519_public, + x25519_public, + name, + avatar_hash, + signature, + }) + } + + pub fn to_qr_data(&self) -> HeaplessVec { + + let mut buf = HeaplessVec::new(); + let _ = buf.extend_from_slice(b"YCH:"); + let _ = buf.extend_from_slice(&self.encode()); + buf + } + + pub fn from_qr_data(data: &[u8]) -> Result { + if data.len() < 4 || &data[..4] != b"YCH:" { + return Err(ContactHelloError::InvalidFormat); + } + Self::decode(&data[4..]) + } + + pub fn fingerprint(&self) -> [u8; 8] { + let hash = Sha256::hash(&self.ed25519_public); + let mut fp = [0u8; 8]; + fp.copy_from_slice(&hash[..8]); + fp + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +pub enum TrustLevel { + + Unknown = 0, + + Seen = 1, + + Verified = 2, + + Trusted = 3, +} + +#[derive(Debug, Clone)] +pub struct Contact { + + pub hello: ContactHello, + + pub trust: TrustLevel, + + pub petname: HeaplessVec, + + pub last_seen: u64, + + pub message_count: u32, +} + +impl Contact { + pub fn from_hello(hello: ContactHello) -> Self { + Self { + hello, + trust: TrustLevel::Unknown, + petname: HeaplessVec::new(), + last_seen: 0, + message_count: 0, + } + } + + pub fn set_petname(&mut self, name: &[u8]) { + self.petname.clear(); + let _ = self.petname.extend_from_slice(name); + } + + pub fn display_name(&self) -> &[u8] { + if !self.petname.is_empty() { + &self.petname + } else { + &self.hello.name + } + } + + pub fn serialize(&self) -> HeaplessVec { + let mut buf = HeaplessVec::new(); + + let hello_encoded = self.hello.encode(); + let hello_len = hello_encoded.len() as u16; + let _ = buf.extend_from_slice(&hello_len.to_le_bytes()); + let _ = buf.extend_from_slice(&hello_encoded); + + let _ = buf.push(self.trust as u8); + + let _ = buf.push(self.petname.len() as u8); + let _ = buf.extend_from_slice(&self.petname); + + let _ = buf.extend_from_slice(&self.last_seen.to_le_bytes()); + + let _ = buf.extend_from_slice(&self.message_count.to_le_bytes()); + + buf + } + + pub fn deserialize(data: &[u8]) -> Option { + if data.len() < 2 { + return None; + } + + let mut pos = 0; + + let mut hello_len_bytes = [0u8; 2]; + hello_len_bytes.copy_from_slice(&data[pos..pos + 2]); + let hello_len = u16::from_le_bytes(hello_len_bytes) as usize; + pos += 2; + + if pos + hello_len > data.len() { + return None; + } + + let hello = ContactHello::decode(&data[pos..pos + hello_len]).ok()?; + pos += hello_len; + + if pos >= data.len() { + return None; + } + let trust = match data[pos] { + 0 => TrustLevel::Unknown, + 1 => TrustLevel::Seen, + 2 => TrustLevel::Verified, + 3 => TrustLevel::Trusted, + _ => TrustLevel::Unknown, + }; + pos += 1; + + if pos >= data.len() { + return None; + } + let petname_len = data[pos] as usize; + pos += 1; + + if pos + petname_len > data.len() { + return None; + } + let mut petname = HeaplessVec::new(); + let _ = petname.extend_from_slice(&data[pos..pos + petname_len]); + pos += petname_len; + + if pos + 8 > data.len() { + return None; + } + let mut last_seen_bytes = [0u8; 8]; + last_seen_bytes.copy_from_slice(&data[pos..pos + 8]); + let last_seen = u64::from_le_bytes(last_seen_bytes); + pos += 8; + + if pos + 4 > data.len() { + return None; + } + let mut count_bytes = [0u8; 4]; + count_bytes.copy_from_slice(&data[pos..pos + 4]); + let message_count = u32::from_le_bytes(count_bytes); + + Some(Self { + hello, + trust, + petname, + last_seen, + message_count, + }) + } + + pub fn key(&self) -> [u8; 8] { + self.hello.fingerprint() + } +} + +const NVS_CONTACT_NAMESPACE: &[u8] = b"contacts\0"; + +const MAX_CONTACTS: usize = 64; + +pub struct ContactStore { + + contacts: heapless::FnvIndexMap<[u8; 8], Contact, MAX_CONTACTS>, +} + +impl ContactStore { + pub fn new() -> Self { + Self { + contacts: heapless::FnvIndexMap::new(), + } + } + + pub fn add(&mut self, contact: Contact) -> Result<(), ContactHelloError> { + let key = contact.key(); + let _ = self.contacts.insert(key, contact); + Ok(()) + } + + pub fn get(&self, fingerprint: &[u8; 8]) -> Option<&Contact> { + self.contacts.get(fingerprint) + } + + pub fn get_mut(&mut self, fingerprint: &[u8; 8]) -> Option<&mut Contact> { + self.contacts.get_mut(fingerprint) + } + + pub fn remove(&mut self, fingerprint: &[u8; 8]) -> Option { + self.contacts.remove(fingerprint) + } + + pub fn len(&self) -> usize { + self.contacts.len() + } + + pub fn is_empty(&self) -> bool { + self.contacts.is_empty() + } + + pub fn iter(&self) -> impl Iterator { + self.contacts.iter() + } + + pub fn find_by_pubkey(&self, ed25519_public: &[u8; 32]) -> Option<&Contact> { + self.contacts.values().find(|c| &c.hello.ed25519_public == ed25519_public) + } + + #[cfg(target_arch = "xtensa")] + pub fn save_to_nvs(&self) -> Result<(), ContactHelloError> { + use esp_idf_sys::*; + + unsafe { + + let mut handle: nvs_handle_t = 0; + let namespace = core::ffi::CStr::from_ptr( + NVS_CONTACT_NAMESPACE.as_ptr() as *const core::ffi::c_char + ); + + let mut err = nvs_open( + namespace.as_ptr(), + nvs_open_mode_t_NVS_READWRITE, + &mut handle, + ); + + if err != ESP_OK { + nvs_flash_init(); + err = nvs_open( + namespace.as_ptr(), + nvs_open_mode_t_NVS_READWRITE, + &mut handle, + ); + if err != ESP_OK { + return Err(ContactHelloError::BufferTooSmall); + } + } + + let count_key = core::ffi::CStr::from_ptr( + b"cnt_count\0".as_ptr() as *const core::ffi::c_char + ); + nvs_set_u32(handle, count_key.as_ptr(), self.contacts.len() as u32); + + let mut idx = 0u32; + for (_key, contact) in self.contacts.iter() { + + let mut key_name = [0u8; 16]; + let prefix = b"cnt_"; + key_name[..4].copy_from_slice(prefix); + if idx < 10 { + key_name[4] = b'0' + idx as u8; + key_name[5] = 0; + } else { + key_name[4] = b'0' + (idx / 10) as u8; + key_name[5] = b'0' + (idx % 10) as u8; + key_name[6] = 0; + } + let key_cstr = core::ffi::CStr::from_ptr( + key_name.as_ptr() as *const core::ffi::c_char + ); + + let blob = contact.serialize(); + + nvs_set_blob( + handle, + key_cstr.as_ptr(), + blob.as_ptr() as *const _, + blob.len(), + ); + + idx += 1; + } + + nvs_commit(handle); + nvs_close(handle); + + ::log::info!("Saved {} contacts to NVS", self.contacts.len()); + } + + Ok(()) + } + + #[cfg(target_arch = "xtensa")] + pub fn load_from_nvs(&mut self) -> Result { + use esp_idf_sys::*; + + unsafe { + + let mut handle: nvs_handle_t = 0; + let namespace = core::ffi::CStr::from_ptr( + NVS_CONTACT_NAMESPACE.as_ptr() as *const core::ffi::c_char + ); + + let err = nvs_open( + namespace.as_ptr(), + nvs_open_mode_t_NVS_READONLY, + &mut handle, + ); + + if err != ESP_OK { + return Ok(0); + } + + let count_key = core::ffi::CStr::from_ptr( + b"cnt_count\0".as_ptr() as *const core::ffi::c_char + ); + let mut count: u32 = 0; + if nvs_get_u32(handle, count_key.as_ptr(), &mut count) != ESP_OK { + nvs_close(handle); + return Ok(0); + } + + let count = core::cmp::min(count as usize, MAX_CONTACTS); + + let mut loaded = 0; + for idx in 0..count { + + let mut key_name = [0u8; 16]; + let prefix = b"cnt_"; + key_name[..4].copy_from_slice(prefix); + if idx < 10 { + key_name[4] = b'0' + idx as u8; + key_name[5] = 0; + } else { + key_name[4] = b'0' + (idx / 10) as u8; + key_name[5] = b'0' + (idx % 10) as u8; + key_name[6] = 0; + } + let key_cstr = core::ffi::CStr::from_ptr( + key_name.as_ptr() as *const core::ffi::c_char + ); + + let mut blob = [0u8; 512]; + let mut blob_len = blob.len(); + + if nvs_get_blob( + handle, + key_cstr.as_ptr(), + blob.as_mut_ptr() as *mut _, + &mut blob_len, + ) == ESP_OK && blob_len > 0 + { + + if let Some(contact) = Contact::deserialize(&blob[..blob_len]) { + let key = contact.key(); + let _ = self.contacts.insert(key, contact); + loaded += 1; + } + } + } + + nvs_close(handle); + ::log::info!("Loaded {} contacts from NVS", loaded); + Ok(loaded) + } + } + + #[cfg(not(target_arch = "xtensa"))] + pub fn save_to_nvs(&self) -> Result<(), ContactHelloError> { + Ok(()) + } + + #[cfg(not(target_arch = "xtensa"))] + pub fn load_from_nvs(&mut self) -> Result { + Ok(0) + } +} diff --git a/src/crypto/aes.rs b/src/crypto/aes.rs new file mode 100644 index 0000000..8190551 --- /dev/null +++ b/src/crypto/aes.rs @@ -0,0 +1,913 @@ +#[inline(never)] +fn sbox_ct(input: u8) -> u8 { + + let x = input; + let x2 = gf_square(x); + let x4 = gf_square(x2); + let x8 = gf_square(x4); + let x16 = gf_square(x8); + let x32 = gf_square(x16); + let x64 = gf_square(x32); + let x128 = gf_square(x64); + + let inv = gf_mul_ct(x2, x4); + let inv = gf_mul_ct(inv, x8); + let inv = gf_mul_ct(inv, x16); + let inv = gf_mul_ct(inv, x32); + let inv = gf_mul_ct(inv, x64); + let inv = gf_mul_ct(inv, x128); + + affine_transform(inv) +} + +#[inline(never)] +fn inv_sbox_ct(input: u8) -> u8 { + + let x = inv_affine_transform(input); + + let x2 = gf_square(x); + let x4 = gf_square(x2); + let x8 = gf_square(x4); + let x16 = gf_square(x8); + let x32 = gf_square(x16); + let x64 = gf_square(x32); + let x128 = gf_square(x64); + + let inv = gf_mul_ct(x2, x4); + let inv = gf_mul_ct(inv, x8); + let inv = gf_mul_ct(inv, x16); + let inv = gf_mul_ct(inv, x32); + let inv = gf_mul_ct(inv, x64); + gf_mul_ct(inv, x128) +} + +#[inline] +fn gf_square(x: u8) -> u8 { + gf_mul_ct(x, x) +} + +#[inline] +fn gf_mul_ct(a: u8, b: u8) -> u8 { + let mut result: u8 = 0; + let mut aa = a; + + result ^= aa & (((b & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 1) & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 2) & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 3) & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 4) & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 5) & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 6) & 0x01) as i8).wrapping_neg() as u8); + let mask = ((aa >> 7) as i8).wrapping_neg() as u8; + aa = (aa << 1) ^ (0x1b & mask); + + result ^= aa & ((((b >> 7) & 0x01) as i8).wrapping_neg() as u8); + + result +} + +#[inline] +fn affine_transform(x: u8) -> u8 { + + let mut result = 0u8; + + result |= (parity(x & 0b11110001) ^ 1) << 0; + result |= (parity(x & 0b11100011) ^ 1) << 1; + result |= (parity(x & 0b11000111) ^ 0) << 2; + result |= (parity(x & 0b10001111) ^ 0) << 3; + result |= (parity(x & 0b00011111) ^ 0) << 4; + result |= (parity(x & 0b00111110) ^ 1) << 5; + result |= (parity(x & 0b01111100) ^ 1) << 6; + result |= (parity(x & 0b11111000) ^ 0) << 7; + + result +} + +#[inline] +fn inv_affine_transform(x: u8) -> u8 { + + let y = x ^ 0x63; + + let mut result = 0u8; + + result |= parity(y & 0b10100100) << 0; + result |= parity(y & 0b01001001) << 1; + result |= parity(y & 0b10010010) << 2; + result |= parity(y & 0b00100101) << 3; + result |= parity(y & 0b01001010) << 4; + result |= parity(y & 0b10010100) << 5; + result |= parity(y & 0b00101001) << 6; + result |= parity(y & 0b01010010) << 7; + + result +} + +#[inline] +fn parity(mut x: u8) -> u8 { + x ^= x >> 4; + x ^= x >> 2; + x ^= x >> 1; + x & 1 +} + +const SBOX: [u8; 256] = [ + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16, +]; + +const INV_SBOX: [u8; 256] = [ + 0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, 0x81, 0xf3, 0xd7, 0xfb, + 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, + 0x54, 0x7b, 0x94, 0x32, 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, + 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, 0x6d, 0x8b, 0xd1, 0x25, + 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, + 0x6c, 0x70, 0x48, 0x50, 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84, + 0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, 0xb8, 0xb3, 0x45, 0x06, + 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, + 0x3a, 0x91, 0x11, 0x41, 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, + 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, 0x1c, 0x75, 0xdf, 0x6e, + 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, + 0xfc, 0x56, 0x3e, 0x4b, 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4, + 0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xec, 0x5f, + 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, + 0xa0, 0xe0, 0x3b, 0x4d, 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, + 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0c, 0x7d, +]; + +const RCON: [u8; 11] = [0x00, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36]; + +pub const BLOCK_SIZE: usize = 16; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum AesMode { + + Ecb, + + Ctr, + + Cbc, +} + +pub struct Aes128 { + + round_keys: [u8; 176], +} + +pub struct Aes256 { + + round_keys: [u8; 240], +} + +impl Drop for Aes128 { + fn drop(&mut self) { + + crate::crypto::secure_zero(&mut self.round_keys); + } +} + +impl Aes128 { + + pub fn new(key: &[u8; 16]) -> Self { + let mut cipher = Self { + round_keys: [0u8; 176], + }; + cipher.key_expansion(key); + cipher + } + + fn key_expansion(&mut self, key: &[u8; 16]) { + + self.round_keys[..16].copy_from_slice(key); + + let mut i = 16; + let mut rcon_idx = 1; + + while i < 176 { + + let mut temp = [ + self.round_keys[i - 4], + self.round_keys[i - 3], + self.round_keys[i - 2], + self.round_keys[i - 1], + ]; + + if i % 16 == 0 { + + temp.rotate_left(1); + + for byte in &mut temp { + *byte = SBOX[*byte as usize]; + } + + temp[0] ^= RCON[rcon_idx]; + rcon_idx += 1; + } + + for j in 0..4 { + self.round_keys[i + j] = self.round_keys[i - 16 + j] ^ temp[j]; + } + i += 4; + } + } + + pub fn encrypt_block(&self, block: &mut [u8; 16]) { + let mut state = *block; + + Self::add_round_key(&mut state, &self.round_keys[0..16]); + + for round in 1..10 { + Self::sub_bytes(&mut state); + Self::shift_rows(&mut state); + Self::mix_columns(&mut state); + Self::add_round_key(&mut state, &self.round_keys[round * 16..(round + 1) * 16]); + } + + Self::sub_bytes(&mut state); + Self::shift_rows(&mut state); + Self::add_round_key(&mut state, &self.round_keys[160..176]); + + *block = state; + } + + pub fn decrypt_block(&self, block: &mut [u8; 16]) { + let mut state = *block; + + Self::add_round_key(&mut state, &self.round_keys[160..176]); + + for round in (1..10).rev() { + Self::inv_shift_rows(&mut state); + Self::inv_sub_bytes(&mut state); + Self::add_round_key(&mut state, &self.round_keys[round * 16..(round + 1) * 16]); + Self::inv_mix_columns(&mut state); + } + + Self::inv_shift_rows(&mut state); + Self::inv_sub_bytes(&mut state); + Self::add_round_key(&mut state, &self.round_keys[0..16]); + + *block = state; + } + + pub fn encrypt_ctr(&self, nonce: &[u8; 16], data: &mut [u8]) { + let mut counter = *nonce; + let mut keystream = [0u8; 16]; + + for (block_idx, chunk) in data.chunks_mut(16).enumerate() { + + keystream = counter; + self.encrypt_block(&mut keystream); + + for (i, byte) in chunk.iter_mut().enumerate() { + *byte ^= keystream[i]; + } + + Self::increment_counter(&mut counter); + } + } + + pub fn decrypt_ctr(&self, nonce: &[u8; 16], data: &mut [u8]) { + self.encrypt_ctr(nonce, data); + } + + pub fn encrypt_cbc(&self, iv: &[u8; 16], data: &mut [u8]) { + assert!(data.len() % 16 == 0, "Data must be multiple of block size"); + + let mut prev = *iv; + + for chunk in data.chunks_mut(16) { + + for (i, byte) in chunk.iter_mut().enumerate() { + *byte ^= prev[i]; + } + + let mut block = [0u8; 16]; + block.copy_from_slice(chunk); + self.encrypt_block(&mut block); + chunk.copy_from_slice(&block); + + prev.copy_from_slice(chunk); + } + } + + pub fn decrypt_cbc(&self, iv: &[u8; 16], data: &mut [u8]) { + assert!(data.len() % 16 == 0, "Data must be multiple of block size"); + + let mut prev = *iv; + + for chunk in data.chunks_mut(16) { + + let mut saved = [0u8; 16]; + saved.copy_from_slice(chunk); + + let mut block = [0u8; 16]; + block.copy_from_slice(chunk); + self.decrypt_block(&mut block); + + for (i, byte) in block.iter_mut().enumerate() { + *byte ^= prev[i]; + } + chunk.copy_from_slice(&block); + + prev = saved; + } + } + + #[inline] + fn sub_bytes(state: &mut [u8; 16]) { + for byte in state.iter_mut() { + *byte = SBOX[*byte as usize]; + } + } + + #[inline] + fn inv_sub_bytes(state: &mut [u8; 16]) { + for byte in state.iter_mut() { + *byte = INV_SBOX[*byte as usize]; + } + } + + #[inline] + fn shift_rows(state: &mut [u8; 16]) { + + let temp = state[1]; + state[1] = state[5]; + state[5] = state[9]; + state[9] = state[13]; + state[13] = temp; + + let temp0 = state[2]; + let temp1 = state[6]; + state[2] = state[10]; + state[6] = state[14]; + state[10] = temp0; + state[14] = temp1; + + let temp = state[15]; + state[15] = state[11]; + state[11] = state[7]; + state[7] = state[3]; + state[3] = temp; + } + + #[inline] + fn inv_shift_rows(state: &mut [u8; 16]) { + + let temp = state[13]; + state[13] = state[9]; + state[9] = state[5]; + state[5] = state[1]; + state[1] = temp; + + let temp0 = state[2]; + let temp1 = state[6]; + state[2] = state[10]; + state[6] = state[14]; + state[10] = temp0; + state[14] = temp1; + + let temp = state[3]; + state[3] = state[7]; + state[7] = state[11]; + state[11] = state[15]; + state[15] = temp; + } + + #[inline] + fn mix_columns(state: &mut [u8; 16]) { + for col in 0..4 { + let i = col * 4; + let s0 = state[i]; + let s1 = state[i + 1]; + let s2 = state[i + 2]; + let s3 = state[i + 3]; + + state[i] = gf_mul(s0, 2) ^ gf_mul(s1, 3) ^ s2 ^ s3; + state[i + 1] = s0 ^ gf_mul(s1, 2) ^ gf_mul(s2, 3) ^ s3; + state[i + 2] = s0 ^ s1 ^ gf_mul(s2, 2) ^ gf_mul(s3, 3); + state[i + 3] = gf_mul(s0, 3) ^ s1 ^ s2 ^ gf_mul(s3, 2); + } + } + + #[inline] + fn inv_mix_columns(state: &mut [u8; 16]) { + for col in 0..4 { + let i = col * 4; + let s0 = state[i]; + let s1 = state[i + 1]; + let s2 = state[i + 2]; + let s3 = state[i + 3]; + + state[i] = gf_mul(s0, 0x0e) ^ gf_mul(s1, 0x0b) ^ gf_mul(s2, 0x0d) ^ gf_mul(s3, 0x09); + state[i + 1] = gf_mul(s0, 0x09) ^ gf_mul(s1, 0x0e) ^ gf_mul(s2, 0x0b) ^ gf_mul(s3, 0x0d); + state[i + 2] = gf_mul(s0, 0x0d) ^ gf_mul(s1, 0x09) ^ gf_mul(s2, 0x0e) ^ gf_mul(s3, 0x0b); + state[i + 3] = gf_mul(s0, 0x0b) ^ gf_mul(s1, 0x0d) ^ gf_mul(s2, 0x09) ^ gf_mul(s3, 0x0e); + } + } + + #[inline] + fn add_round_key(state: &mut [u8; 16], round_key: &[u8]) { + for (i, byte) in state.iter_mut().enumerate() { + *byte ^= round_key[i]; + } + } + + #[inline] + fn increment_counter(counter: &mut [u8; 16]) { + for i in (0..16).rev() { + counter[i] = counter[i].wrapping_add(1); + if counter[i] != 0 { + break; + } + } + } +} + +impl Drop for Aes256 { + fn drop(&mut self) { + + crate::crypto::secure_zero(&mut self.round_keys); + } +} + +impl Aes256 { + + pub fn new(key: &[u8; 32]) -> Self { + let mut cipher = Self { + round_keys: [0u8; 240], + }; + cipher.key_expansion(key); + cipher + } + + fn key_expansion(&mut self, key: &[u8; 32]) { + + self.round_keys[..32].copy_from_slice(key); + + let mut i = 32; + let mut rcon_idx = 1; + + while i < 240 { + let mut temp = [ + self.round_keys[i - 4], + self.round_keys[i - 3], + self.round_keys[i - 2], + self.round_keys[i - 1], + ]; + + if i % 32 == 0 { + + temp.rotate_left(1); + + for byte in &mut temp { + *byte = SBOX[*byte as usize]; + } + + temp[0] ^= RCON[rcon_idx]; + rcon_idx += 1; + } else if i % 32 == 16 { + + for byte in &mut temp { + *byte = SBOX[*byte as usize]; + } + } + + for j in 0..4 { + self.round_keys[i + j] = self.round_keys[i - 32 + j] ^ temp[j]; + } + i += 4; + } + } + + pub fn encrypt_block(&self, block: &mut [u8; 16]) { + let mut state = *block; + + add_round_key_256(&mut state, &self.round_keys[0..16]); + + for round in 1..14 { + sub_bytes_256(&mut state); + shift_rows_256(&mut state); + mix_columns_256(&mut state); + add_round_key_256(&mut state, &self.round_keys[round * 16..(round + 1) * 16]); + } + + sub_bytes_256(&mut state); + shift_rows_256(&mut state); + add_round_key_256(&mut state, &self.round_keys[224..240]); + + *block = state; + } + + pub fn decrypt_block(&self, block: &mut [u8; 16]) { + let mut state = *block; + + add_round_key_256(&mut state, &self.round_keys[224..240]); + + for round in (1..14).rev() { + inv_shift_rows_256(&mut state); + inv_sub_bytes_256(&mut state); + add_round_key_256(&mut state, &self.round_keys[round * 16..(round + 1) * 16]); + inv_mix_columns_256(&mut state); + } + + inv_shift_rows_256(&mut state); + inv_sub_bytes_256(&mut state); + add_round_key_256(&mut state, &self.round_keys[0..16]); + + *block = state; + } + + pub fn encrypt_ctr(&self, nonce: &[u8; 16], data: &mut [u8]) { + let mut counter = *nonce; + let mut keystream = [0u8; 16]; + + for chunk in data.chunks_mut(16) { + keystream = counter; + self.encrypt_block(&mut keystream); + + for (i, byte) in chunk.iter_mut().enumerate() { + *byte ^= keystream[i]; + } + + increment_counter_256(&mut counter); + } + } + + pub fn decrypt_ctr(&self, nonce: &[u8; 16], data: &mut [u8]) { + self.encrypt_ctr(nonce, data); + } +} + +#[inline] +fn sub_bytes_256(state: &mut [u8; 16]) { + for byte in state.iter_mut() { + *byte = SBOX[*byte as usize]; + } +} + +#[inline] +fn inv_sub_bytes_256(state: &mut [u8; 16]) { + for byte in state.iter_mut() { + *byte = INV_SBOX[*byte as usize]; + } +} + +#[inline] +fn shift_rows_256(state: &mut [u8; 16]) { + let temp = state[1]; + state[1] = state[5]; + state[5] = state[9]; + state[9] = state[13]; + state[13] = temp; + + let temp0 = state[2]; + let temp1 = state[6]; + state[2] = state[10]; + state[6] = state[14]; + state[10] = temp0; + state[14] = temp1; + + let temp = state[15]; + state[15] = state[11]; + state[11] = state[7]; + state[7] = state[3]; + state[3] = temp; +} + +#[inline] +fn inv_shift_rows_256(state: &mut [u8; 16]) { + let temp = state[13]; + state[13] = state[9]; + state[9] = state[5]; + state[5] = state[1]; + state[1] = temp; + + let temp0 = state[2]; + let temp1 = state[6]; + state[2] = state[10]; + state[6] = state[14]; + state[10] = temp0; + state[14] = temp1; + + let temp = state[3]; + state[3] = state[7]; + state[7] = state[11]; + state[11] = state[15]; + state[15] = temp; +} + +#[inline] +fn mix_columns_256(state: &mut [u8; 16]) { + for col in 0..4 { + let i = col * 4; + let s0 = state[i]; + let s1 = state[i + 1]; + let s2 = state[i + 2]; + let s3 = state[i + 3]; + + state[i] = gf_mul(s0, 2) ^ gf_mul(s1, 3) ^ s2 ^ s3; + state[i + 1] = s0 ^ gf_mul(s1, 2) ^ gf_mul(s2, 3) ^ s3; + state[i + 2] = s0 ^ s1 ^ gf_mul(s2, 2) ^ gf_mul(s3, 3); + state[i + 3] = gf_mul(s0, 3) ^ s1 ^ s2 ^ gf_mul(s3, 2); + } +} + +#[inline] +fn inv_mix_columns_256(state: &mut [u8; 16]) { + for col in 0..4 { + let i = col * 4; + let s0 = state[i]; + let s1 = state[i + 1]; + let s2 = state[i + 2]; + let s3 = state[i + 3]; + + state[i] = gf_mul(s0, 0x0e) ^ gf_mul(s1, 0x0b) ^ gf_mul(s2, 0x0d) ^ gf_mul(s3, 0x09); + state[i + 1] = gf_mul(s0, 0x09) ^ gf_mul(s1, 0x0e) ^ gf_mul(s2, 0x0b) ^ gf_mul(s3, 0x0d); + state[i + 2] = gf_mul(s0, 0x0d) ^ gf_mul(s1, 0x09) ^ gf_mul(s2, 0x0e) ^ gf_mul(s3, 0x0b); + state[i + 3] = gf_mul(s0, 0x0b) ^ gf_mul(s1, 0x0d) ^ gf_mul(s2, 0x09) ^ gf_mul(s3, 0x0e); + } +} + +#[inline] +fn add_round_key_256(state: &mut [u8; 16], round_key: &[u8]) { + for (i, byte) in state.iter_mut().enumerate() { + *byte ^= round_key[i]; + } +} + +#[inline] +fn increment_counter_256(counter: &mut [u8; 16]) { + for i in (0..16).rev() { + counter[i] = counter[i].wrapping_add(1); + if counter[i] != 0 { + break; + } + } +} + +#[inline] +fn gf_mul(mut a: u8, mut b: u8) -> u8 { + let mut result: u8 = 0; + while b != 0 { + if b & 1 != 0 { + result ^= a; + } + let high_bit = a & 0x80; + a <<= 1; + if high_bit != 0 { + a ^= 0x1b; + } + b >>= 1; + } + result +} + +pub struct Aes128Ct { + + round_keys: [u8; 176], +} + +impl Drop for Aes128Ct { + fn drop(&mut self) { + + crate::crypto::secure_zero(&mut self.round_keys); + } +} + +impl Aes128Ct { + + pub fn new(key: &[u8; 16]) -> Self { + let mut cipher = Self { + round_keys: [0u8; 176], + }; + cipher.key_expansion(key); + cipher + } + + fn key_expansion(&mut self, key: &[u8; 16]) { + self.round_keys[..16].copy_from_slice(key); + + let mut i = 16; + let mut rcon_idx = 1; + + while i < 176 { + let mut temp = [ + self.round_keys[i - 4], + self.round_keys[i - 3], + self.round_keys[i - 2], + self.round_keys[i - 1], + ]; + + if i % 16 == 0 { + temp.rotate_left(1); + + for byte in &mut temp { + *byte = sbox_ct(*byte); + } + temp[0] ^= RCON[rcon_idx]; + rcon_idx += 1; + } + + for j in 0..4 { + self.round_keys[i + j] = self.round_keys[i - 16 + j] ^ temp[j]; + } + i += 4; + } + } + + pub fn encrypt_block(&self, block: &mut [u8; 16]) { + let mut state = *block; + + Self::add_round_key(&mut state, &self.round_keys[0..16]); + + for round in 1..10 { + Self::sub_bytes_ct(&mut state); + Self::shift_rows(&mut state); + Self::mix_columns(&mut state); + Self::add_round_key(&mut state, &self.round_keys[round * 16..(round + 1) * 16]); + } + + Self::sub_bytes_ct(&mut state); + Self::shift_rows(&mut state); + Self::add_round_key(&mut state, &self.round_keys[160..176]); + + *block = state; + } + + pub fn decrypt_block(&self, block: &mut [u8; 16]) { + let mut state = *block; + + Self::add_round_key(&mut state, &self.round_keys[160..176]); + + for round in (1..10).rev() { + Self::inv_shift_rows(&mut state); + Self::inv_sub_bytes_ct(&mut state); + Self::add_round_key(&mut state, &self.round_keys[round * 16..(round + 1) * 16]); + Self::inv_mix_columns(&mut state); + } + + Self::inv_shift_rows(&mut state); + Self::inv_sub_bytes_ct(&mut state); + Self::add_round_key(&mut state, &self.round_keys[0..16]); + + *block = state; + } + + pub fn encrypt_ctr(&self, nonce: &[u8; 16], data: &mut [u8]) { + let mut counter = *nonce; + let mut keystream = [0u8; 16]; + + for chunk in data.chunks_mut(16) { + keystream = counter; + self.encrypt_block(&mut keystream); + + for (i, byte) in chunk.iter_mut().enumerate() { + *byte ^= keystream[i]; + } + + Self::increment_counter(&mut counter); + } + } + + pub fn decrypt_ctr(&self, nonce: &[u8; 16], data: &mut [u8]) { + self.encrypt_ctr(nonce, data); + } + + #[inline] + fn sub_bytes_ct(state: &mut [u8; 16]) { + for byte in state.iter_mut() { + *byte = sbox_ct(*byte); + } + } + + #[inline] + fn inv_sub_bytes_ct(state: &mut [u8; 16]) { + for byte in state.iter_mut() { + *byte = inv_sbox_ct(*byte); + } + } + + #[inline] + fn shift_rows(state: &mut [u8; 16]) { + let temp = state[1]; + state[1] = state[5]; + state[5] = state[9]; + state[9] = state[13]; + state[13] = temp; + + let temp0 = state[2]; + let temp1 = state[6]; + state[2] = state[10]; + state[6] = state[14]; + state[10] = temp0; + state[14] = temp1; + + let temp = state[15]; + state[15] = state[11]; + state[11] = state[7]; + state[7] = state[3]; + state[3] = temp; + } + + #[inline] + fn inv_shift_rows(state: &mut [u8; 16]) { + let temp = state[13]; + state[13] = state[9]; + state[9] = state[5]; + state[5] = state[1]; + state[1] = temp; + + let temp0 = state[2]; + let temp1 = state[6]; + state[2] = state[10]; + state[6] = state[14]; + state[10] = temp0; + state[14] = temp1; + + let temp = state[3]; + state[3] = state[7]; + state[7] = state[11]; + state[11] = state[15]; + state[15] = temp; + } + + #[inline] + fn mix_columns(state: &mut [u8; 16]) { + for col in 0..4 { + let i = col * 4; + let s0 = state[i]; + let s1 = state[i + 1]; + let s2 = state[i + 2]; + let s3 = state[i + 3]; + + state[i] = gf_mul_ct(s0, 2) ^ gf_mul_ct(s1, 3) ^ s2 ^ s3; + state[i + 1] = s0 ^ gf_mul_ct(s1, 2) ^ gf_mul_ct(s2, 3) ^ s3; + state[i + 2] = s0 ^ s1 ^ gf_mul_ct(s2, 2) ^ gf_mul_ct(s3, 3); + state[i + 3] = gf_mul_ct(s0, 3) ^ s1 ^ s2 ^ gf_mul_ct(s3, 2); + } + } + + #[inline] + fn inv_mix_columns(state: &mut [u8; 16]) { + for col in 0..4 { + let i = col * 4; + let s0 = state[i]; + let s1 = state[i + 1]; + let s2 = state[i + 2]; + let s3 = state[i + 3]; + + state[i] = gf_mul_ct(s0, 0x0e) ^ gf_mul_ct(s1, 0x0b) ^ gf_mul_ct(s2, 0x0d) ^ gf_mul_ct(s3, 0x09); + state[i + 1] = gf_mul_ct(s0, 0x09) ^ gf_mul_ct(s1, 0x0e) ^ gf_mul_ct(s2, 0x0b) ^ gf_mul_ct(s3, 0x0d); + state[i + 2] = gf_mul_ct(s0, 0x0d) ^ gf_mul_ct(s1, 0x09) ^ gf_mul_ct(s2, 0x0e) ^ gf_mul_ct(s3, 0x0b); + state[i + 3] = gf_mul_ct(s0, 0x0b) ^ gf_mul_ct(s1, 0x0d) ^ gf_mul_ct(s2, 0x09) ^ gf_mul_ct(s3, 0x0e); + } + } + + #[inline] + fn add_round_key(state: &mut [u8; 16], round_key: &[u8]) { + for (i, byte) in state.iter_mut().enumerate() { + *byte ^= round_key[i]; + } + } + + #[inline] + fn increment_counter(counter: &mut [u8; 16]) { + for i in (0..16).rev() { + counter[i] = counter[i].wrapping_add(1); + if counter[i] != 0 { + break; + } + } + } +} diff --git a/src/crypto/chacha20.rs b/src/crypto/chacha20.rs new file mode 100644 index 0000000..eaf74ac --- /dev/null +++ b/src/crypto/chacha20.rs @@ -0,0 +1,229 @@ +const STATE_SIZE: usize = 16; + +pub const BLOCK_SIZE: usize = 64; + +pub const KEY_SIZE: usize = 32; + +pub const NONCE_SIZE: usize = 12; + +pub struct ChaCha20 { + + state: [u32; STATE_SIZE], +} + +impl Drop for ChaCha20 { + fn drop(&mut self) { + + crate::crypto::secure_zero_u32(&mut self.state); + } +} + +impl ChaCha20 { + + const CONSTANTS: [u32; 4] = [0x61707865, 0x3320646e, 0x79622d32, 0x6b206574]; + + pub fn new(key: &[u8; KEY_SIZE], nonce: &[u8; NONCE_SIZE]) -> Self { + let mut state = [0u32; STATE_SIZE]; + + state[0] = Self::CONSTANTS[0]; + state[1] = Self::CONSTANTS[1]; + state[2] = Self::CONSTANTS[2]; + state[3] = Self::CONSTANTS[3]; + + state[4] = u32::from_le_bytes([key[0], key[1], key[2], key[3]]); + state[5] = u32::from_le_bytes([key[4], key[5], key[6], key[7]]); + state[6] = u32::from_le_bytes([key[8], key[9], key[10], key[11]]); + state[7] = u32::from_le_bytes([key[12], key[13], key[14], key[15]]); + state[8] = u32::from_le_bytes([key[16], key[17], key[18], key[19]]); + state[9] = u32::from_le_bytes([key[20], key[21], key[22], key[23]]); + state[10] = u32::from_le_bytes([key[24], key[25], key[26], key[27]]); + state[11] = u32::from_le_bytes([key[28], key[29], key[30], key[31]]); + + state[12] = 0; + + state[13] = u32::from_le_bytes([nonce[0], nonce[1], nonce[2], nonce[3]]); + state[14] = u32::from_le_bytes([nonce[4], nonce[5], nonce[6], nonce[7]]); + state[15] = u32::from_le_bytes([nonce[8], nonce[9], nonce[10], nonce[11]]); + + Self { state } + } + + pub fn new_with_counter(key: &[u8; KEY_SIZE], nonce: &[u8; NONCE_SIZE], counter: u32) -> Self { + let mut cipher = Self::new(key, nonce); + cipher.state[12] = counter; + cipher + } + + fn block(&self, counter: u32) -> [u8; BLOCK_SIZE] { + let mut state = self.state; + state[12] = counter; + + let mut working = state; + + for _ in 0..10 { + + quarter_round(&mut working, 0, 4, 8, 12); + quarter_round(&mut working, 1, 5, 9, 13); + quarter_round(&mut working, 2, 6, 10, 14); + quarter_round(&mut working, 3, 7, 11, 15); + + quarter_round(&mut working, 0, 5, 10, 15); + quarter_round(&mut working, 1, 6, 11, 12); + quarter_round(&mut working, 2, 7, 8, 13); + quarter_round(&mut working, 3, 4, 9, 14); + } + + for i in 0..STATE_SIZE { + working[i] = working[i].wrapping_add(state[i]); + } + + let mut output = [0u8; BLOCK_SIZE]; + for (i, word) in working.iter().enumerate() { + let bytes = word.to_le_bytes(); + output[i * 4] = bytes[0]; + output[i * 4 + 1] = bytes[1]; + output[i * 4 + 2] = bytes[2]; + output[i * 4 + 3] = bytes[3]; + } + + output + } + + pub fn apply_keystream(&self, data: &mut [u8]) { + let mut counter = self.state[12]; + + for chunk in data.chunks_mut(BLOCK_SIZE) { + let keystream = self.block(counter); + for (i, byte) in chunk.iter_mut().enumerate() { + *byte ^= keystream[i]; + } + counter = counter.wrapping_add(1); + } + } + + pub fn encrypt(&self, data: &mut [u8]) { + self.apply_keystream(data); + } + + pub fn decrypt(&self, data: &mut [u8]) { + self.apply_keystream(data); + } + + pub fn keystream(&self, len: usize) -> heapless::Vec { + let mut output = heapless::Vec::new(); + let mut counter = self.state[12]; + + let mut remaining = len; + while remaining > 0 { + let block = self.block(counter); + let to_copy = core::cmp::min(remaining, BLOCK_SIZE); + for i in 0..to_copy { + let _ = output.push(block[i]); + } + remaining -= to_copy; + counter = counter.wrapping_add(1); + } + + output + } +} + +#[inline] +fn quarter_round(state: &mut [u32; STATE_SIZE], a: usize, b: usize, c: usize, d: usize) { + state[a] = state[a].wrapping_add(state[b]); + state[d] ^= state[a]; + state[d] = state[d].rotate_left(16); + + state[c] = state[c].wrapping_add(state[d]); + state[b] ^= state[c]; + state[b] = state[b].rotate_left(12); + + state[a] = state[a].wrapping_add(state[b]); + state[d] ^= state[a]; + state[d] = state[d].rotate_left(8); + + state[c] = state[c].wrapping_add(state[d]); + state[b] ^= state[c]; + state[b] = state[b].rotate_left(7); +} + +pub fn hchacha20(key: &[u8; 32], nonce: &[u8; 16]) -> [u8; 32] { + let mut state = [0u32; 16]; + + state[0] = 0x61707865; + state[1] = 0x3320646e; + state[2] = 0x79622d32; + state[3] = 0x6b206574; + + for i in 0..8 { + state[4 + i] = u32::from_le_bytes([ + key[i * 4], + key[i * 4 + 1], + key[i * 4 + 2], + key[i * 4 + 3], + ]); + } + + for i in 0..4 { + state[12 + i] = u32::from_le_bytes([ + nonce[i * 4], + nonce[i * 4 + 1], + nonce[i * 4 + 2], + nonce[i * 4 + 3], + ]); + } + + for _ in 0..10 { + quarter_round(&mut state, 0, 4, 8, 12); + quarter_round(&mut state, 1, 5, 9, 13); + quarter_round(&mut state, 2, 6, 10, 14); + quarter_round(&mut state, 3, 7, 11, 15); + quarter_round(&mut state, 0, 5, 10, 15); + quarter_round(&mut state, 1, 6, 11, 12); + quarter_round(&mut state, 2, 7, 8, 13); + quarter_round(&mut state, 3, 4, 9, 14); + } + + let mut output = [0u8; 32]; + for i in 0..4 { + let bytes = state[i].to_le_bytes(); + output[i * 4] = bytes[0]; + output[i * 4 + 1] = bytes[1]; + output[i * 4 + 2] = bytes[2]; + output[i * 4 + 3] = bytes[3]; + } + for i in 0..4 { + let bytes = state[12 + i].to_le_bytes(); + output[16 + i * 4] = bytes[0]; + output[16 + i * 4 + 1] = bytes[1]; + output[16 + i * 4 + 2] = bytes[2]; + output[16 + i * 4 + 3] = bytes[3]; + } + + output +} + +pub struct XChaCha20 { + inner: ChaCha20, +} + +impl XChaCha20 { + + pub fn new(key: &[u8; 32], nonce: &[u8; 24]) -> Self { + + let mut hnonce = [0u8; 16]; + hnonce.copy_from_slice(&nonce[..16]); + let subkey = hchacha20(key, &hnonce); + + let mut chacha_nonce = [0u8; 12]; + chacha_nonce[4..].copy_from_slice(&nonce[16..]); + + Self { + inner: ChaCha20::new(&subkey, &chacha_nonce), + } + } + + pub fn apply_keystream(&self, data: &mut [u8]) { + self.inner.apply_keystream(data); + } +} diff --git a/src/crypto/ed25519.rs b/src/crypto/ed25519.rs new file mode 100644 index 0000000..48ce040 --- /dev/null +++ b/src/crypto/ed25519.rs @@ -0,0 +1,1001 @@ +use super::sha256::Sha256; + +pub struct Signature(pub [u8; 64]); + +pub type PublicKey = [u8; 32]; + +pub type PrivateKey = [u8; 32]; + +#[derive(Clone, Copy)] +struct Fe([i64; 10]); + +#[derive(Clone, Copy)] +struct ExtendedPoint { + x: Fe, + y: Fe, + z: Fe, + t: Fe, +} + +#[derive(Clone, Copy)] +struct PrecomputedPoint { + y_plus_x: Fe, + y_minus_x: Fe, + xy2d: Fe, +} + +#[derive(Clone, Copy)] +struct Scalar([u8; 32]); + +const BASE_Y: [u8; 32] = [ + 0x58, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, + 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, + 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, + 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, +]; + +const D: Fe = Fe([ + -10913610, 13857413, -15372611, 6949391, 114729, + -8787816, -6275908, -3247719, -18696448, -12055116, +]); + +const D2: Fe = Fe([ + -21827239, -5839606, -30745221, 13898782, 229458, + 15978800, -12551817, -6495438, 29715968, 9444199, +]); + +const SQRT_M1: Fe = Fe([ + -32595792, -7943725, 9377950, 3500415, 12389472, + -272473, -25146209, -2005654, 326686, 11406482, +]); + +impl Fe { + const fn zero() -> Self { + Fe([0; 10]) + } + + const fn one() -> Self { + Fe([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + } + + fn from_bytes(bytes: &[u8; 32]) -> Self { + let mut h = [0i64; 10]; + + h[0] = load_4(&bytes[0..4]) & 0x3ffffff; + h[1] = (load_4(&bytes[3..7]) >> 2) & 0x1ffffff; + h[2] = (load_4(&bytes[6..10]) >> 3) & 0x3ffffff; + h[3] = (load_4(&bytes[9..13]) >> 5) & 0x1ffffff; + h[4] = (load_4(&bytes[12..16]) >> 6) & 0x3ffffff; + h[5] = load_4(&bytes[16..20]) & 0x1ffffff; + h[6] = (load_4(&bytes[19..23]) >> 1) & 0x3ffffff; + h[7] = (load_4(&bytes[22..26]) >> 3) & 0x1ffffff; + h[8] = (load_4(&bytes[25..29]) >> 4) & 0x3ffffff; + h[9] = (load_4(&bytes[28..32]) >> 6) & 0x1ffffff; + + Fe(h) + } + + fn to_bytes(&self) -> [u8; 32] { + let mut h = self.0; + + let mut q = (19 * h[9] + (1 << 24)) >> 25; + q = (h[0] + q) >> 26; + q = (h[1] + q) >> 25; + q = (h[2] + q) >> 26; + q = (h[3] + q) >> 25; + q = (h[4] + q) >> 26; + q = (h[5] + q) >> 25; + q = (h[6] + q) >> 26; + q = (h[7] + q) >> 25; + q = (h[8] + q) >> 26; + q = (h[9] + q) >> 25; + + h[0] += 19 * q; + + let carry0 = h[0] >> 26; h[1] += carry0; h[0] -= carry0 << 26; + let carry1 = h[1] >> 25; h[2] += carry1; h[1] -= carry1 << 25; + let carry2 = h[2] >> 26; h[3] += carry2; h[2] -= carry2 << 26; + let carry3 = h[3] >> 25; h[4] += carry3; h[3] -= carry3 << 25; + let carry4 = h[4] >> 26; h[5] += carry4; h[4] -= carry4 << 26; + let carry5 = h[5] >> 25; h[6] += carry5; h[5] -= carry5 << 25; + let carry6 = h[6] >> 26; h[7] += carry6; h[6] -= carry6 << 26; + let carry7 = h[7] >> 25; h[8] += carry7; h[7] -= carry7 << 25; + let carry8 = h[8] >> 26; h[9] += carry8; h[8] -= carry8 << 26; + let carry9 = h[9] >> 25; h[9] -= carry9 << 25; + + let mut s = [0u8; 32]; + s[0] = h[0] as u8; + s[1] = (h[0] >> 8) as u8; + s[2] = (h[0] >> 16) as u8; + s[3] = ((h[0] >> 24) | (h[1] << 2)) as u8; + s[4] = (h[1] >> 6) as u8; + s[5] = (h[1] >> 14) as u8; + s[6] = ((h[1] >> 22) | (h[2] << 3)) as u8; + s[7] = (h[2] >> 5) as u8; + s[8] = (h[2] >> 13) as u8; + s[9] = ((h[2] >> 21) | (h[3] << 5)) as u8; + s[10] = (h[3] >> 3) as u8; + s[11] = (h[3] >> 11) as u8; + s[12] = ((h[3] >> 19) | (h[4] << 6)) as u8; + s[13] = (h[4] >> 2) as u8; + s[14] = (h[4] >> 10) as u8; + s[15] = (h[4] >> 18) as u8; + s[16] = h[5] as u8; + s[17] = (h[5] >> 8) as u8; + s[18] = (h[5] >> 16) as u8; + s[19] = ((h[5] >> 24) | (h[6] << 1)) as u8; + s[20] = (h[6] >> 7) as u8; + s[21] = (h[6] >> 15) as u8; + s[22] = ((h[6] >> 23) | (h[7] << 3)) as u8; + s[23] = (h[7] >> 5) as u8; + s[24] = (h[7] >> 13) as u8; + s[25] = ((h[7] >> 21) | (h[8] << 4)) as u8; + s[26] = (h[8] >> 4) as u8; + s[27] = (h[8] >> 12) as u8; + s[28] = ((h[8] >> 20) | (h[9] << 6)) as u8; + s[29] = (h[9] >> 2) as u8; + s[30] = (h[9] >> 10) as u8; + s[31] = (h[9] >> 18) as u8; + + s + } + + fn add(&self, rhs: &Fe) -> Fe { + Fe([ + self.0[0] + rhs.0[0], self.0[1] + rhs.0[1], self.0[2] + rhs.0[2], + self.0[3] + rhs.0[3], self.0[4] + rhs.0[4], self.0[5] + rhs.0[5], + self.0[6] + rhs.0[6], self.0[7] + rhs.0[7], self.0[8] + rhs.0[8], + self.0[9] + rhs.0[9], + ]) + } + + fn sub(&self, rhs: &Fe) -> Fe { + Fe([ + self.0[0] - rhs.0[0], self.0[1] - rhs.0[1], self.0[2] - rhs.0[2], + self.0[3] - rhs.0[3], self.0[4] - rhs.0[4], self.0[5] - rhs.0[5], + self.0[6] - rhs.0[6], self.0[7] - rhs.0[7], self.0[8] - rhs.0[8], + self.0[9] - rhs.0[9], + ]) + } + + fn neg(&self) -> Fe { + Fe([ + -self.0[0], -self.0[1], -self.0[2], -self.0[3], -self.0[4], + -self.0[5], -self.0[6], -self.0[7], -self.0[8], -self.0[9], + ]) + } + + fn mul(&self, rhs: &Fe) -> Fe { + let f = &self.0; + let g = &rhs.0; + + let f0 = f[0] as i128; let f1 = f[1] as i128; let f2 = f[2] as i128; + let f3 = f[3] as i128; let f4 = f[4] as i128; let f5 = f[5] as i128; + let f6 = f[6] as i128; let f7 = f[7] as i128; let f8 = f[8] as i128; + let f9 = f[9] as i128; + + let g0 = g[0] as i128; let g1 = g[1] as i128; let g2 = g[2] as i128; + let g3 = g[3] as i128; let g4 = g[4] as i128; let g5 = g[5] as i128; + let g6 = g[6] as i128; let g7 = g[7] as i128; let g8 = g[8] as i128; + let g9 = g[9] as i128; + + let g1_19 = 19 * g1; let g2_19 = 19 * g2; let g3_19 = 19 * g3; + let g4_19 = 19 * g4; let g5_19 = 19 * g5; let g6_19 = 19 * g6; + let g7_19 = 19 * g7; let g8_19 = 19 * g8; let g9_19 = 19 * g9; + + let f1_2 = 2 * f1; let f3_2 = 2 * f3; let f5_2 = 2 * f5; + let f7_2 = 2 * f7; let f9_2 = 2 * f9; + + let h0 = f0*g0 + f1_2*g9_19 + f2*g8_19 + f3_2*g7_19 + f4*g6_19 + f5_2*g5_19 + f6*g4_19 + f7_2*g3_19 + f8*g2_19 + f9_2*g1_19; + let h1 = f0*g1 + f1*g0 + f2*g9_19 + f3*g8_19 + f4*g7_19 + f5*g6_19 + f6*g5_19 + f7*g4_19 + f8*g3_19 + f9*g2_19; + let h2 = f0*g2 + f1_2*g1 + f2*g0 + f3_2*g9_19 + f4*g8_19 + f5_2*g7_19 + f6*g6_19 + f7_2*g5_19 + f8*g4_19 + f9_2*g3_19; + let h3 = f0*g3 + f1*g2 + f2*g1 + f3*g0 + f4*g9_19 + f5*g8_19 + f6*g7_19 + f7*g6_19 + f8*g5_19 + f9*g4_19; + let h4 = f0*g4 + f1_2*g3 + f2*g2 + f3_2*g1 + f4*g0 + f5_2*g9_19 + f6*g8_19 + f7_2*g7_19 + f8*g6_19 + f9_2*g5_19; + let h5 = f0*g5 + f1*g4 + f2*g3 + f3*g2 + f4*g1 + f5*g0 + f6*g9_19 + f7*g8_19 + f8*g7_19 + f9*g6_19; + let h6 = f0*g6 + f1_2*g5 + f2*g4 + f3_2*g3 + f4*g2 + f5_2*g1 + f6*g0 + f7_2*g9_19 + f8*g8_19 + f9_2*g7_19; + let h7 = f0*g7 + f1*g6 + f2*g5 + f3*g4 + f4*g3 + f5*g2 + f6*g1 + f7*g0 + f8*g9_19 + f9*g8_19; + let h8 = f0*g8 + f1_2*g7 + f2*g6 + f3_2*g5 + f4*g4 + f5_2*g3 + f6*g2 + f7_2*g1 + f8*g0 + f9_2*g9_19; + let h9 = f0*g9 + f1*g8 + f2*g7 + f3*g6 + f4*g5 + f5*g4 + f6*g3 + f7*g2 + f8*g1 + f9*g0; + + carry_mul([h0, h1, h2, h3, h4, h5, h6, h7, h8, h9]) + } + + fn square(&self) -> Fe { + self.mul(self) + } + + fn square2(&self) -> Fe { + let h = self.square(); + h.add(&h) + } + + fn invert(&self) -> Fe { + let mut t0 = self.square(); + let mut t1 = t0.square(); + t1 = t1.square(); + t1 = self.mul(&t1); + t0 = t0.mul(&t1); + let mut t2 = t0.square(); + t1 = t1.mul(&t2); + t2 = t1.square(); + for _ in 1..5 { t2 = t2.square(); } + t1 = t2.mul(&t1); + t2 = t1.square(); + for _ in 1..10 { t2 = t2.square(); } + t2 = t2.mul(&t1); + let mut t3 = t2.square(); + for _ in 1..20 { t3 = t3.square(); } + t2 = t3.mul(&t2); + t2 = t2.square(); + for _ in 1..10 { t2 = t2.square(); } + t1 = t2.mul(&t1); + t2 = t1.square(); + for _ in 1..50 { t2 = t2.square(); } + t2 = t2.mul(&t1); + t3 = t2.square(); + for _ in 1..100 { t3 = t3.square(); } + t2 = t3.mul(&t2); + t2 = t2.square(); + for _ in 1..50 { t2 = t2.square(); } + t1 = t2.mul(&t1); + t1 = t1.square(); + for _ in 1..5 { t1 = t1.square(); } + t0.mul(&t1) + } + + fn pow22523(&self) -> Fe { + let mut t0 = self.square(); + let mut t1 = t0.square(); + t1 = t1.square(); + t1 = self.mul(&t1); + t0 = t0.mul(&t1); + t0 = t0.square(); + t0 = t1.mul(&t0); + t1 = t0.square(); + for _ in 1..5 { t1 = t1.square(); } + t0 = t1.mul(&t0); + t1 = t0.square(); + for _ in 1..10 { t1 = t1.square(); } + t1 = t1.mul(&t0); + let mut t2 = t1.square(); + for _ in 1..20 { t2 = t2.square(); } + t1 = t2.mul(&t1); + t1 = t1.square(); + for _ in 1..10 { t1 = t1.square(); } + t0 = t1.mul(&t0); + t1 = t0.square(); + for _ in 1..50 { t1 = t1.square(); } + t1 = t1.mul(&t0); + t2 = t1.square(); + for _ in 1..100 { t2 = t2.square(); } + t1 = t2.mul(&t1); + t1 = t1.square(); + for _ in 1..50 { t1 = t1.square(); } + t0 = t1.mul(&t0); + t0 = t0.square(); + t0 = t0.square(); + self.mul(&t0) + } + + fn is_negative(&self) -> bool { + (self.to_bytes()[0] & 1) == 1 + } + + fn is_zero(&self) -> bool { + self.to_bytes() == [0u8; 32] + } + + fn abs(&self) -> Fe { + if self.is_negative() { + self.neg() + } else { + *self + } + } +} + +fn load_4(s: &[u8]) -> i64 { + (s[0] as i64) | ((s[1] as i64) << 8) | ((s[2] as i64) << 16) | ((s[3] as i64) << 24) +} + +fn load_3(s: &[u8]) -> i64 { + (s[0] as i64) | ((s[1] as i64) << 8) | ((s[2] as i64) << 16) +} + +fn carry_mul(h: [i128; 10]) -> Fe { + let mut out = [0i64; 10]; + let mut carry = (h[0] + (1 << 25)) >> 26; + out[0] = (h[0] - (carry << 26)) as i64; + let h1 = h[1] + carry; + carry = (h1 + (1 << 24)) >> 25; + out[1] = (h1 - (carry << 25)) as i64; + let h2 = h[2] + carry; + carry = (h2 + (1 << 25)) >> 26; + out[2] = (h2 - (carry << 26)) as i64; + let h3 = h[3] + carry; + carry = (h3 + (1 << 24)) >> 25; + out[3] = (h3 - (carry << 25)) as i64; + let h4 = h[4] + carry; + carry = (h4 + (1 << 25)) >> 26; + out[4] = (h4 - (carry << 26)) as i64; + let h5 = h[5] + carry; + carry = (h5 + (1 << 24)) >> 25; + out[5] = (h5 - (carry << 25)) as i64; + let h6 = h[6] + carry; + carry = (h6 + (1 << 25)) >> 26; + out[6] = (h6 - (carry << 26)) as i64; + let h7 = h[7] + carry; + carry = (h7 + (1 << 24)) >> 25; + out[7] = (h7 - (carry << 25)) as i64; + let h8 = h[8] + carry; + carry = (h8 + (1 << 25)) >> 26; + out[8] = (h8 - (carry << 26)) as i64; + let h9 = h[9] + carry; + carry = (h9 + (1 << 24)) >> 25; + out[9] = (h9 - (carry << 25)) as i64; + out[0] += (carry * 19) as i64; + carry = (out[0] as i128 + (1 << 25)) >> 26; + out[0] -= (carry << 26) as i64; + out[1] += carry as i64; + Fe(out) +} + +impl ExtendedPoint { + fn identity() -> Self { + ExtendedPoint { + x: Fe::zero(), + y: Fe::one(), + z: Fe::one(), + t: Fe::zero(), + } + } + + fn from_bytes(bytes: &[u8; 32]) -> Option { + let y = Fe::from_bytes(bytes); + let z = Fe::one(); + let y2 = y.square(); + let u = y2.sub(&Fe::one()); + let v = y2.mul(&D).add(&Fe::one()); + let v3 = v.square().mul(&v); + let v7 = v3.square().mul(&v); + let uv7 = u.mul(&v7); + let mut x = uv7.pow22523().mul(&u).mul(&v3); + + let vx2 = x.square().mul(&v); + let check = vx2.sub(&u); + if !check.is_zero() { + let check2 = vx2.add(&u); + if !check2.is_zero() { + return None; + } + x = x.mul(&SQRT_M1); + } + + if x.is_negative() != ((bytes[31] >> 7) == 1) { + x = x.neg(); + } + + let t = x.mul(&y); + Some(ExtendedPoint { x, y, z, t }) + } + + fn to_bytes(&self) -> [u8; 32] { + let z_inv = self.z.invert(); + let x = self.x.mul(&z_inv); + let y = self.y.mul(&z_inv); + let mut s = y.to_bytes(); + s[31] ^= (x.is_negative() as u8) << 7; + s + } + + fn double(&self) -> Self { + let a = self.x.square(); + let b = self.y.square(); + let c = self.z.square2(); + let d = a.neg(); + let e = self.x.add(&self.y).square().sub(&a).sub(&b); + let g = d.add(&b); + let f = g.sub(&c); + let h = d.sub(&b); + let x3 = e.mul(&f); + let y3 = g.mul(&h); + let t3 = e.mul(&h); + let z3 = f.mul(&g); + ExtendedPoint { x: x3, y: y3, z: z3, t: t3 } + } + + fn add(&self, other: &ExtendedPoint) -> Self { + let a = self.y.sub(&self.x).mul(&other.y.sub(&other.x)); + let b = self.y.add(&self.x).mul(&other.y.add(&other.x)); + let c = self.t.mul(&D2).mul(&other.t); + let d = self.z.mul(&other.z).add(&self.z.mul(&other.z)); + let e = b.sub(&a); + let f = d.sub(&c); + let g = d.add(&c); + let h = b.add(&a); + let x3 = e.mul(&f); + let y3 = g.mul(&h); + let t3 = e.mul(&h); + let z3 = f.mul(&g); + ExtendedPoint { x: x3, y: y3, z: z3, t: t3 } + } + + fn neg(&self) -> Self { + ExtendedPoint { + x: self.x.neg(), + y: self.y, + z: self.z, + t: self.t.neg(), + } + } + + fn scalar_mul(&self, scalar: &[u8; 32]) -> Self { + + let mut r0 = Self::identity(); + let mut r1 = *self; + + for i in (0..256).rev() { + let byte_idx = i / 8; + let bit_idx = i % 8; + let bit = ((scalar[byte_idx] >> bit_idx) & 1) as i64; + + ct_swap(&mut r0, &mut r1, bit); + + r1 = r0.add(&r1); + r0 = r0.double(); + + ct_swap(&mut r0, &mut r1, bit); + } + + r0 + } + + fn ct_select(a: &Self, b: &Self, choice: i64) -> Self { + + let mask = -(choice as i64); + ExtendedPoint { + x: fe_ct_select(&a.x, &b.x, mask), + y: fe_ct_select(&a.y, &b.y, mask), + z: fe_ct_select(&a.z, &b.z, mask), + t: fe_ct_select(&a.t, &b.t, mask), + } + } +} + +fn ct_swap(a: &mut ExtendedPoint, b: &mut ExtendedPoint, choice: i64) { + let mask = -(choice as i64); + fe_ct_swap(&mut a.x, &mut b.x, mask); + fe_ct_swap(&mut a.y, &mut b.y, mask); + fe_ct_swap(&mut a.z, &mut b.z, mask); + fe_ct_swap(&mut a.t, &mut b.t, mask); +} + +fn fe_ct_swap(a: &mut Fe, b: &mut Fe, mask: i64) { + for i in 0..10 { + let t = mask & (a.0[i] ^ b.0[i]); + a.0[i] ^= t; + b.0[i] ^= t; + } +} + +fn fe_ct_select(a: &Fe, b: &Fe, mask: i64) -> Fe { + let mut result = Fe::zero(); + for i in 0..10 { + result.0[i] = a.0[i] ^ (mask & (a.0[i] ^ b.0[i])); + } + result +} + +fn basepoint() -> ExtendedPoint { + ExtendedPoint::from_bytes(&BASE_Y).unwrap() +} + +pub struct Ed25519; + +impl Ed25519 { + + pub fn public_key(private_key: &PrivateKey) -> PublicKey { + + let h = sha512(private_key); + + let mut s = [0u8; 32]; + s.copy_from_slice(&h[..32]); + s[0] &= 248; + s[31] &= 127; + s[31] |= 64; + + let b = basepoint(); + let a = b.scalar_mul(&s); + a.to_bytes() + } + + pub fn sign(private_key: &PrivateKey, message: &[u8]) -> Signature { + + let h = sha512(private_key); + + let mut s = [0u8; 32]; + s.copy_from_slice(&h[..32]); + s[0] &= 248; + s[31] &= 127; + s[31] |= 64; + + let b = basepoint(); + let a_point = b.scalar_mul(&s); + let a = a_point.to_bytes(); + + let mut hasher_r = Sha512::new(); + hasher_r.update(&h[32..]); + hasher_r.update(message); + let r_hash = hasher_r.finalize(); + let r = sc_reduce(&r_hash); + + let r_point = b.scalar_mul(&r); + let r_bytes = r_point.to_bytes(); + + let mut hasher_k = Sha512::new(); + hasher_k.update(&r_bytes); + hasher_k.update(&a); + hasher_k.update(message); + let k_hash = hasher_k.finalize(); + let k = sc_reduce(&k_hash); + + let s_scalar = sc_muladd(&k, &s, &r); + + let mut sig = [0u8; 64]; + sig[..32].copy_from_slice(&r_bytes); + sig[32..].copy_from_slice(&s_scalar); + Signature(sig) + } + + pub fn verify(public_key: &PublicKey, message: &[u8], signature: &Signature) -> bool { + + let a_point = match ExtendedPoint::from_bytes(public_key) { + Some(p) => p, + None => return false, + }; + + let s = &signature.0[32..]; + if !sc_is_valid(s) { + return false; + } + + let mut hasher = Sha512::new(); + hasher.update(&signature.0[..32]); + hasher.update(public_key); + hasher.update(message); + let k_hash = hasher.finalize(); + let k = sc_reduce(&k_hash); + + let b = basepoint(); + let mut s_bytes = [0u8; 32]; + s_bytes.copy_from_slice(s); + let sb = b.scalar_mul(&s_bytes); + + let r_point = match ExtendedPoint::from_bytes(&signature.0[..32].try_into().unwrap()) { + Some(p) => p, + None => return false, + }; + + let ka = a_point.scalar_mul(&k); + let rhs = r_point.add(&ka); + + let lhs_bytes = sb.to_bytes(); + let rhs_bytes = rhs.to_bytes(); + + super::constant_time_eq(&lhs_bytes, &rhs_bytes) + } +} + +const L: [i64; 12] = [ + 0x1cf5d3ed, 0x009318d2, 0x1de73596, 0x1df3bd45, + 0x0000014d, 0x00000000, 0x00000000, 0x00000000, + 0x00000000, 0x00000000, 0x00000000, 0x00200000, +]; + +fn sc_reduce(s: &[u8; 64]) -> [u8; 32] { + + let mut a = [0i64; 24]; + + a[0] = 0x1fffff & load_3_i64(&s[0..3]); + a[1] = 0x1fffff & (load_4_i64(&s[2..6]) >> 5); + a[2] = 0x1fffff & (load_3_i64(&s[5..8]) >> 2); + a[3] = 0x1fffff & (load_4_i64(&s[7..11]) >> 7); + a[4] = 0x1fffff & (load_4_i64(&s[10..14]) >> 4); + a[5] = 0x1fffff & (load_3_i64(&s[13..16]) >> 1); + a[6] = 0x1fffff & (load_4_i64(&s[15..19]) >> 6); + a[7] = 0x1fffff & (load_3_i64(&s[18..21]) >> 3); + a[8] = 0x1fffff & load_3_i64(&s[21..24]); + a[9] = 0x1fffff & (load_4_i64(&s[23..27]) >> 5); + a[10] = 0x1fffff & (load_3_i64(&s[26..29]) >> 2); + a[11] = 0x1fffff & (load_4_i64(&s[28..32]) >> 7); + a[12] = 0x1fffff & (load_4_i64(&s[31..35]) >> 4); + a[13] = 0x1fffff & (load_3_i64(&s[34..37]) >> 1); + a[14] = 0x1fffff & (load_4_i64(&s[36..40]) >> 6); + a[15] = 0x1fffff & (load_3_i64(&s[39..42]) >> 3); + a[16] = 0x1fffff & load_3_i64(&s[42..45]); + a[17] = 0x1fffff & (load_4_i64(&s[44..48]) >> 5); + a[18] = 0x1fffff & (load_3_i64(&s[47..50]) >> 2); + a[19] = 0x1fffff & (load_4_i64(&s[49..53]) >> 7); + a[20] = 0x1fffff & (load_4_i64(&s[52..56]) >> 4); + a[21] = 0x1fffff & (load_3_i64(&s[55..58]) >> 1); + a[22] = 0x1fffff & (load_4_i64(&s[57..61]) >> 6); + a[23] = load_4_i64(&s[60..64]) >> 3; + + sc_reduce_limbs(&mut a); + + let mut out = [0u8; 32]; + out[0] = a[0] as u8; + out[1] = (a[0] >> 8) as u8; + out[2] = ((a[0] >> 16) | (a[1] << 5)) as u8; + out[3] = (a[1] >> 3) as u8; + out[4] = (a[1] >> 11) as u8; + out[5] = ((a[1] >> 19) | (a[2] << 2)) as u8; + out[6] = (a[2] >> 6) as u8; + out[7] = ((a[2] >> 14) | (a[3] << 7)) as u8; + out[8] = (a[3] >> 1) as u8; + out[9] = (a[3] >> 9) as u8; + out[10] = ((a[3] >> 17) | (a[4] << 4)) as u8; + out[11] = (a[4] >> 4) as u8; + out[12] = (a[4] >> 12) as u8; + out[13] = ((a[4] >> 20) | (a[5] << 1)) as u8; + out[14] = (a[5] >> 7) as u8; + out[15] = ((a[5] >> 15) | (a[6] << 6)) as u8; + out[16] = (a[6] >> 2) as u8; + out[17] = (a[6] >> 10) as u8; + out[18] = ((a[6] >> 18) | (a[7] << 3)) as u8; + out[19] = (a[7] >> 5) as u8; + out[20] = (a[7] >> 13) as u8; + out[21] = a[8] as u8; + out[22] = (a[8] >> 8) as u8; + out[23] = ((a[8] >> 16) | (a[9] << 5)) as u8; + out[24] = (a[9] >> 3) as u8; + out[25] = (a[9] >> 11) as u8; + out[26] = ((a[9] >> 19) | (a[10] << 2)) as u8; + out[27] = (a[10] >> 6) as u8; + out[28] = ((a[10] >> 14) | (a[11] << 7)) as u8; + out[29] = (a[11] >> 1) as u8; + out[30] = (a[11] >> 9) as u8; + out[31] = (a[11] >> 17) as u8; + + out +} + +fn sc_reduce_limbs(a: &mut [i64; 24]) { + + for i in (12..24).rev() { + let q = a[i]; + if q == 0 { continue; } + + let shift = i - 11; + a[shift + 0] -= q * 0x1cf5d3ed; + a[shift + 1] -= q * 0x009318d2; + a[shift + 2] -= q * 0x1de73596; + a[shift + 3] -= q * 0x1df3bd45; + a[shift + 4] -= q * 0x0000014d; + + a[shift + 11] -= q * 0x00200000; + a[i] = 0; + } + + for _ in 0..2 { + for i in 0..11 { + let carry = a[i] >> 21; + a[i] &= 0x1fffff; + a[i + 1] += carry; + } + + for i in 0..11 { + if a[i] < 0 { + a[i] += 0x200000; + a[i + 1] -= 1; + } + } + } + + let mut borrow = 0i64; + let mut tmp = [0i64; 12]; + + tmp[0] = a[0] - 0x1cf5d3ed; + tmp[1] = a[1] - 0x009318d2; + tmp[2] = a[2] - 0x1de73596; + tmp[3] = a[3] - 0x1df3bd45; + tmp[4] = a[4] - 0x0000014d; + tmp[5] = a[5]; + tmp[6] = a[6]; + tmp[7] = a[7]; + tmp[8] = a[8]; + tmp[9] = a[9]; + tmp[10] = a[10]; + tmp[11] = a[11] - 0x00200000; + + for i in 0..11 { + tmp[i] += borrow; + borrow = tmp[i] >> 63; + if tmp[i] < 0 { + tmp[i] += 0x200000; + borrow = -1; + } else { + borrow = 0; + } + } + tmp[11] += borrow; + + let mask = !(tmp[11] >> 63); + for i in 0..12 { + a[i] = (a[i] & !mask) | (tmp[i] & mask); + } +} + +fn load_3_i64(s: &[u8]) -> i64 { + (s[0] as i64) | ((s[1] as i64) << 8) | ((s[2] as i64) << 16) +} + +fn load_4_i64(s: &[u8]) -> i64 { + (s[0] as i64) | ((s[1] as i64) << 8) | ((s[2] as i64) << 16) | ((s[3] as i64) << 24) +} + +fn sc_muladd(a: &[u8; 32], b: &[u8; 32], c: &[u8; 32]) -> [u8; 32] { + + let a_limbs = sc_load(a); + let b_limbs = sc_load(b); + let c_limbs = sc_load(c); + + let mut product = [0i64; 24]; + for i in 0..12 { + for j in 0..12 { + product[i + j] += a_limbs[i] * b_limbs[j]; + } + } + + for i in 0..12 { + product[i] += c_limbs[i]; + } + + for i in 0..23 { + let carry = product[i] >> 21; + product[i] &= 0x1fffff; + product[i + 1] += carry; + } + + sc_reduce_limbs(&mut product); + + let mut out = [0u8; 32]; + out[0] = product[0] as u8; + out[1] = (product[0] >> 8) as u8; + out[2] = ((product[0] >> 16) | (product[1] << 5)) as u8; + out[3] = (product[1] >> 3) as u8; + out[4] = (product[1] >> 11) as u8; + out[5] = ((product[1] >> 19) | (product[2] << 2)) as u8; + out[6] = (product[2] >> 6) as u8; + out[7] = ((product[2] >> 14) | (product[3] << 7)) as u8; + out[8] = (product[3] >> 1) as u8; + out[9] = (product[3] >> 9) as u8; + out[10] = ((product[3] >> 17) | (product[4] << 4)) as u8; + out[11] = (product[4] >> 4) as u8; + out[12] = (product[4] >> 12) as u8; + out[13] = ((product[4] >> 20) | (product[5] << 1)) as u8; + out[14] = (product[5] >> 7) as u8; + out[15] = ((product[5] >> 15) | (product[6] << 6)) as u8; + out[16] = (product[6] >> 2) as u8; + out[17] = (product[6] >> 10) as u8; + out[18] = ((product[6] >> 18) | (product[7] << 3)) as u8; + out[19] = (product[7] >> 5) as u8; + out[20] = (product[7] >> 13) as u8; + out[21] = product[8] as u8; + out[22] = (product[8] >> 8) as u8; + out[23] = ((product[8] >> 16) | (product[9] << 5)) as u8; + out[24] = (product[9] >> 3) as u8; + out[25] = (product[9] >> 11) as u8; + out[26] = ((product[9] >> 19) | (product[10] << 2)) as u8; + out[27] = (product[10] >> 6) as u8; + out[28] = ((product[10] >> 14) | (product[11] << 7)) as u8; + out[29] = (product[11] >> 1) as u8; + out[30] = (product[11] >> 9) as u8; + out[31] = (product[11] >> 17) as u8; + + out +} + +fn sc_load(s: &[u8; 32]) -> [i64; 12] { + let mut a = [0i64; 12]; + a[0] = 0x1fffff & load_3_i64(&s[0..3]); + a[1] = 0x1fffff & (load_4_i64(&s[2..6]) >> 5); + a[2] = 0x1fffff & (load_3_i64(&s[5..8]) >> 2); + a[3] = 0x1fffff & (load_4_i64(&s[7..11]) >> 7); + a[4] = 0x1fffff & (load_4_i64(&s[10..14]) >> 4); + a[5] = 0x1fffff & (load_3_i64(&s[13..16]) >> 1); + a[6] = 0x1fffff & (load_4_i64(&s[15..19]) >> 6); + a[7] = 0x1fffff & (load_3_i64(&s[18..21]) >> 3); + a[8] = 0x1fffff & load_3_i64(&s[21..24]); + a[9] = 0x1fffff & (load_4_i64(&s[23..27]) >> 5); + a[10] = 0x1fffff & (load_3_i64(&s[26..29]) >> 2); + a[11] = load_4_i64(&s[28..32]) >> 7; + a +} + +fn sc_is_valid(s: &[u8]) -> bool { + + if s.len() != 32 { + return false; + } + + const L: [u8; 32] = [ + 0xed, 0xd3, 0xf5, 0x5c, 0x1a, 0x63, 0x12, 0x58, + 0xd6, 0x9c, 0xf7, 0xa2, 0xde, 0xf9, 0xde, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, + ]; + + let mut borrow: i16 = 0; + + for i in 0..32 { + let diff = (s[i] as i16) - (L[i] as i16) - borrow; + + borrow = (diff >> 15) & 1; + } + + borrow == 1 +} + +struct Sha512 { + state: [u64; 8], + total_len: u128, + buffer: [u8; 128], + buffer_len: usize, +} + +const K512: [u64; 80] = [ + 0x428a2f98d728ae22, 0x7137449123ef65cd, 0xb5c0fbcfec4d3b2f, 0xe9b5dba58189dbbc, + 0x3956c25bf348b538, 0x59f111f1b605d019, 0x923f82a4af194f9b, 0xab1c5ed5da6d8118, + 0xd807aa98a3030242, 0x12835b0145706fbe, 0x243185be4ee4b28c, 0x550c7dc3d5ffb4e2, + 0x72be5d74f27b896f, 0x80deb1fe3b1696b1, 0x9bdc06a725c71235, 0xc19bf174cf692694, + 0xe49b69c19ef14ad2, 0xefbe4786384f25e3, 0x0fc19dc68b8cd5b5, 0x240ca1cc77ac9c65, + 0x2de92c6f592b0275, 0x4a7484aa6ea6e483, 0x5cb0a9dcbd41fbd4, 0x76f988da831153b5, + 0x983e5152ee66dfab, 0xa831c66d2db43210, 0xb00327c898fb213f, 0xbf597fc7beef0ee4, + 0xc6e00bf33da88fc2, 0xd5a79147930aa725, 0x06ca6351e003826f, 0x142929670a0e6e70, + 0x27b70a8546d22ffc, 0x2e1b21385c26c926, 0x4d2c6dfc5ac42aed, 0x53380d139d95b3df, + 0x650a73548baf63de, 0x766a0abb3c77b2a8, 0x81c2c92e47edaee6, 0x92722c851482353b, + 0xa2bfe8a14cf10364, 0xa81a664bbc423001, 0xc24b8b70d0f89791, 0xc76c51a30654be30, + 0xd192e819d6ef5218, 0xd69906245565a910, 0xf40e35855771202a, 0x106aa07032bbd1b8, + 0x19a4c116b8d2d0c8, 0x1e376c085141ab53, 0x2748774cdf8eeb99, 0x34b0bcb5e19b48a8, + 0x391c0cb3c5c95a63, 0x4ed8aa4ae3418acb, 0x5b9cca4f7763e373, 0x682e6ff3d6b2b8a3, + 0x748f82ee5defb2fc, 0x78a5636f43172f60, 0x84c87814a1f0ab72, 0x8cc702081a6439ec, + 0x90befffa23631e28, 0xa4506cebde82bde9, 0xbef9a3f7b2c67915, 0xc67178f2e372532b, + 0xca273eceea26619c, 0xd186b8c721c0c207, 0xeada7dd6cde0eb1e, 0xf57d4f7fee6ed178, + 0x06f067aa72176fba, 0x0a637dc5a2c898a6, 0x113f9804bef90dae, 0x1b710b35131c471b, + 0x28db77f523047d84, 0x32caab7b40c72493, 0x3c9ebe0a15c9bebc, 0x431d67c49c100d4c, + 0x4cc5d4becb3e42b6, 0x597f299cfc657e2a, 0x5fcb6fab3ad6faec, 0x6c44198c4a475817, +]; + +impl Sha512 { + fn new() -> Self { + Self { + state: [ + 0x6a09e667f3bcc908, 0xbb67ae8584caa73b, + 0x3c6ef372fe94f82b, 0xa54ff53a5f1d36f1, + 0x510e527fade682d1, 0x9b05688c2b3e6c1f, + 0x1f83d9abfb41bd6b, 0x5be0cd19137e2179, + ], + total_len: 0, + buffer: [0u8; 128], + buffer_len: 0, + } + } + + fn update(&mut self, data: &[u8]) { + let mut offset = 0; + + if self.buffer_len > 0 { + let needed = 128 - self.buffer_len; + if data.len() >= needed { + self.buffer[self.buffer_len..].copy_from_slice(&data[..needed]); + self.process_block(&self.buffer.clone()); + self.buffer_len = 0; + offset = needed; + } else { + self.buffer[self.buffer_len..self.buffer_len + data.len()].copy_from_slice(data); + self.buffer_len += data.len(); + self.total_len += data.len() as u128; + return; + } + } + + while offset + 128 <= data.len() { + let mut block = [0u8; 128]; + block.copy_from_slice(&data[offset..offset + 128]); + self.process_block(&block); + offset += 128; + } + + if offset < data.len() { + let remaining = data.len() - offset; + self.buffer[..remaining].copy_from_slice(&data[offset..]); + self.buffer_len = remaining; + } + + self.total_len += data.len() as u128; + } + + fn finalize(mut self) -> [u8; 64] { + let total_bits = self.total_len * 8; + + self.buffer[self.buffer_len] = 0x80; + self.buffer_len += 1; + + if self.buffer_len > 112 { + for i in self.buffer_len..128 { + self.buffer[i] = 0; + } + self.process_block(&self.buffer.clone()); + self.buffer_len = 0; + } + + for i in self.buffer_len..112 { + self.buffer[i] = 0; + } + + self.buffer[112..128].copy_from_slice(&total_bits.to_be_bytes()); + self.process_block(&self.buffer.clone()); + + let mut digest = [0u8; 64]; + for (i, word) in self.state.iter().enumerate() { + digest[i * 8..(i + 1) * 8].copy_from_slice(&word.to_be_bytes()); + } + digest + } + + fn process_block(&mut self, block: &[u8; 128]) { + let mut w = [0u64; 80]; + + for i in 0..16 { + w[i] = u64::from_be_bytes([ + block[i * 8], block[i * 8 + 1], block[i * 8 + 2], block[i * 8 + 3], + block[i * 8 + 4], block[i * 8 + 5], block[i * 8 + 6], block[i * 8 + 7], + ]); + } + + for i in 16..80 { + let s0 = w[i - 15].rotate_right(1) ^ w[i - 15].rotate_right(8) ^ (w[i - 15] >> 7); + let s1 = w[i - 2].rotate_right(19) ^ w[i - 2].rotate_right(61) ^ (w[i - 2] >> 6); + w[i] = w[i - 16].wrapping_add(s0).wrapping_add(w[i - 7]).wrapping_add(s1); + } + + let mut a = self.state[0]; + let mut b = self.state[1]; + let mut c = self.state[2]; + let mut d = self.state[3]; + let mut e = self.state[4]; + let mut f = self.state[5]; + let mut g = self.state[6]; + let mut h = self.state[7]; + + for i in 0..80 { + let s1 = e.rotate_right(14) ^ e.rotate_right(18) ^ e.rotate_right(41); + let ch = (e & f) ^ ((!e) & g); + let temp1 = h.wrapping_add(s1).wrapping_add(ch).wrapping_add(K512[i]).wrapping_add(w[i]); + let s0 = a.rotate_right(28) ^ a.rotate_right(34) ^ a.rotate_right(39); + let maj = (a & b) ^ (a & c) ^ (b & c); + let temp2 = s0.wrapping_add(maj); + + h = g; g = f; f = e; + e = d.wrapping_add(temp1); + d = c; c = b; b = a; + a = temp1.wrapping_add(temp2); + } + + self.state[0] = self.state[0].wrapping_add(a); + self.state[1] = self.state[1].wrapping_add(b); + self.state[2] = self.state[2].wrapping_add(c); + self.state[3] = self.state[3].wrapping_add(d); + self.state[4] = self.state[4].wrapping_add(e); + self.state[5] = self.state[5].wrapping_add(f); + self.state[6] = self.state[6].wrapping_add(g); + self.state[7] = self.state[7].wrapping_add(h); + } +} + +fn sha512(data: &[u8]) -> [u8; 64] { + let mut hasher = Sha512::new(); + hasher.update(data); + hasher.finalize() +} diff --git a/src/crypto/hkdf.rs b/src/crypto/hkdf.rs new file mode 100644 index 0000000..748d4c5 --- /dev/null +++ b/src/crypto/hkdf.rs @@ -0,0 +1,149 @@ +use super::hmac::HmacSha256; +use super::sha256::DIGEST_SIZE; + +pub struct Hkdf; + +impl Hkdf { + + pub fn extract(salt: &[u8], ikm: &[u8]) -> [u8; DIGEST_SIZE] { + let salt = if salt.is_empty() { + &[0u8; DIGEST_SIZE][..] + } else { + salt + }; + HmacSha256::mac(salt, ikm) + } + + pub fn expand(prk: &[u8; DIGEST_SIZE], info: &[u8], okm: &mut [u8]) { + let n = (okm.len() + DIGEST_SIZE - 1) / DIGEST_SIZE; + assert!(n <= 255, "Output too long"); + + let mut t = [0u8; DIGEST_SIZE]; + let mut offset = 0; + + for i in 1..=n { + let mut hmac = HmacSha256::new(prk); + + if i > 1 { + hmac.update(&t); + } + hmac.update(info); + hmac.update(&[i as u8]); + + t = hmac.finalize(); + + let to_copy = core::cmp::min(DIGEST_SIZE, okm.len() - offset); + okm[offset..offset + to_copy].copy_from_slice(&t[..to_copy]); + offset += to_copy; + } + } + + pub fn derive(salt: &[u8], ikm: &[u8], info: &[u8], okm: &mut [u8]) { + let prk = Self::extract(salt, ikm); + Self::expand(&prk, info, okm); + } + + pub fn derive_key(salt: &[u8], ikm: &[u8], info: &[u8]) -> [u8; N] { + let mut key = [0u8; N]; + Self::derive(salt, ikm, info, &mut key); + key + } +} + +pub mod reticulum { + use super::*; + use crate::crypto::sha256::Sha256; + + pub const IDENTITY_HASH_SIZE: usize = 16; + + pub const FULL_HASH_SIZE: usize = 32; + + pub fn identity_hash(signing_key: &[u8; 32], encryption_key: &[u8; 32]) -> [u8; IDENTITY_HASH_SIZE] { + let mut hasher = Sha256::new(); + hasher.update(signing_key); + hasher.update(encryption_key); + let full = hasher.finalize(); + + let mut hash = [0u8; IDENTITY_HASH_SIZE]; + hash.copy_from_slice(&full[..IDENTITY_HASH_SIZE]); + hash + } + + pub fn full_identity_hash(signing_key: &[u8; 32], encryption_key: &[u8; 32]) -> [u8; FULL_HASH_SIZE] { + let mut hasher = Sha256::new(); + hasher.update(signing_key); + hasher.update(encryption_key); + hasher.finalize() + } + + pub fn derive_link_keys( + shared_secret: &[u8; 32], + initiator_pub: &[u8; 32], + responder_pub: &[u8; 32], + ) -> LinkKeys { + + let mut context = [0u8; 64]; + context[..32].copy_from_slice(initiator_pub); + context[32..].copy_from_slice(responder_pub); + + let mut master = [0u8; 64]; + Hkdf::derive(b"reticulum", shared_secret, &context, &mut master); + + LinkKeys { + tx_key: { + let mut k = [0u8; 32]; + k.copy_from_slice(&master[..32]); + k + }, + rx_key: { + let mut k = [0u8; 32]; + k.copy_from_slice(&master[32..]); + k + }, + } + } + + pub struct LinkKeys { + pub tx_key: [u8; 32], + pub rx_key: [u8; 32], + } +} + +pub mod meshcore { + use super::*; + + pub fn derive_channel_key(psk: &[u8], channel_id: u8) -> [u8; 32] { + let info = [b'C', b'H', channel_id]; + Hkdf::derive_key(b"meshcore", psk, &info) + } + + pub fn derive_node_key(identity: &[u8; 32], purpose: &[u8]) -> [u8; 32] { + Hkdf::derive_key(b"meshcore-node", identity, purpose) + } +} + +pub mod meshtastic { + use super::*; + use crate::crypto::sha256::Sha256; + + pub const DEFAULT_KEY: [u8; 16] = [ + 0xd4, 0xf1, 0xbb, 0x3a, 0x20, 0x29, 0x07, 0x59, + 0xf0, 0xbc, 0xff, 0xab, 0xcf, 0x4e, 0x69, 0x01 + ]; + + pub fn derive_channel_key(channel_name: &str) -> [u8; 32] { + + let hash = Sha256::hash(channel_name.as_bytes()); + hash + } + + pub fn derive_nonce(packet_id: u32, sender: u32) -> [u8; 16] { + let mut nonce = [0u8; 16]; + + nonce[..4].copy_from_slice(&packet_id.to_le_bytes()); + + nonce[8..12].copy_from_slice(&sender.to_le_bytes()); + + nonce + } +} diff --git a/src/crypto/hmac.rs b/src/crypto/hmac.rs new file mode 100644 index 0000000..e1cd21e --- /dev/null +++ b/src/crypto/hmac.rs @@ -0,0 +1,69 @@ +use super::sha256::{Sha256, BLOCK_SIZE, DIGEST_SIZE}; + +pub struct HmacSha256 { + + inner: Sha256, + + outer_key: [u8; BLOCK_SIZE], +} + +impl HmacSha256 { + + pub fn new(key: &[u8]) -> Self { + let mut key_block = [0u8; BLOCK_SIZE]; + + if key.len() > BLOCK_SIZE { + let hashed = Sha256::hash(key); + key_block[..DIGEST_SIZE].copy_from_slice(&hashed); + } else { + key_block[..key.len()].copy_from_slice(key); + } + + let mut inner_key = [0u8; BLOCK_SIZE]; + let mut outer_key = [0u8; BLOCK_SIZE]; + + for i in 0..BLOCK_SIZE { + inner_key[i] = key_block[i] ^ 0x36; + outer_key[i] = key_block[i] ^ 0x5c; + } + + let mut inner = Sha256::new(); + inner.update(&inner_key); + + Self { inner, outer_key } + } + + pub fn update(&mut self, data: &[u8]) { + self.inner.update(data); + } + + pub fn finalize(self) -> [u8; DIGEST_SIZE] { + + let inner_hash = self.inner.finalize(); + + let mut outer = Sha256::new(); + outer.update(&self.outer_key); + outer.update(&inner_hash); + outer.finalize() + } + + pub fn mac(key: &[u8], data: &[u8]) -> [u8; DIGEST_SIZE] { + let mut hmac = Self::new(key); + hmac.update(data); + hmac.finalize() + } + + pub fn verify(key: &[u8], data: &[u8], expected: &[u8; DIGEST_SIZE]) -> bool { + let computed = Self::mac(key, data); + super::constant_time_eq(&computed, expected) + } +} + +impl Clone for HmacSha256 { + fn clone(&self) -> Self { + Self { + inner: self.inner.clone(), + outer_key: self.outer_key, + } + } +} diff --git a/src/crypto/mod.rs b/src/crypto/mod.rs new file mode 100644 index 0000000..7a9b328 --- /dev/null +++ b/src/crypto/mod.rs @@ -0,0 +1,61 @@ +pub mod aes; +pub mod sha256; +pub mod hmac; +pub mod x25519; +pub mod ed25519; +pub mod chacha20; +pub mod poly1305; +pub mod hkdf; + +pub use aes::{Aes128, Aes256, AesMode}; +pub use sha256::Sha256; +pub use hmac::HmacSha256; +pub use x25519::{x25519, x25519_base}; +pub use ed25519::{Ed25519, Signature}; +pub use chacha20::ChaCha20; +pub use poly1305::Poly1305; +pub use hkdf::Hkdf; + +#[inline] +pub fn constant_time_eq(a: &[u8], b: &[u8]) -> bool { + if a.len() != b.len() { + return false; + } + let mut result: u8 = 0; + for (x, y) in a.iter().zip(b.iter()) { + result |= x ^ y; + } + result == 0 +} + +#[inline] +pub fn secure_zero(data: &mut [u8]) { + for byte in data.iter_mut() { + unsafe { + core::ptr::write_volatile(byte, 0); + } + } + core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst); +} + +#[inline] +pub fn secure_zero_u32(data: &mut [u32]) { + for word in data.iter_mut() { + unsafe { + core::ptr::write_volatile(word, 0); + } + } + core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst); +} + +pub fn random_bytes(dest: &mut [u8]) { + crate::rng::fill_random(dest); +} + +pub fn random_bytes_checked(dest: &mut [u8]) -> bool { + crate::rng::fill_random_checked(dest) +} + +pub fn rng_is_healthy() -> bool { + crate::rng::is_healthy() +} diff --git a/src/crypto/poly1305.rs b/src/crypto/poly1305.rs new file mode 100644 index 0000000..fea8814 --- /dev/null +++ b/src/crypto/poly1305.rs @@ -0,0 +1,318 @@ +pub const TAG_SIZE: usize = 16; + +pub const KEY_SIZE: usize = 32; + +pub struct Poly1305 { + + r: [u32; 5], + + s: [u32; 4], + + h: [u32; 5], + + buffer: [u8; 16], + + buffer_len: usize, +} + +impl Poly1305 { + + pub fn new(key: &[u8; KEY_SIZE]) -> Self { + + let r0 = u32::from_le_bytes([key[0], key[1], key[2], key[3]]) & 0x0fff_fffc; + let r1 = u32::from_le_bytes([key[4], key[5], key[6], key[7]]) & 0x0fff_fffc; + let r2 = u32::from_le_bytes([key[8], key[9], key[10], key[11]]) & 0x0fff_fffc; + let r3 = u32::from_le_bytes([key[12], key[13], key[14], key[15]]) & 0x0fff_fffc; + + let r = [ + r0 & 0x03ff_ffff, + ((r0 >> 26) | (r1 << 6)) & 0x03ff_ffff, + ((r1 >> 20) | (r2 << 12)) & 0x03ff_ffff, + ((r2 >> 14) | (r3 << 18)) & 0x03ff_ffff, + r3 >> 8, + ]; + + let s = [ + u32::from_le_bytes([key[16], key[17], key[18], key[19]]), + u32::from_le_bytes([key[20], key[21], key[22], key[23]]), + u32::from_le_bytes([key[24], key[25], key[26], key[27]]), + u32::from_le_bytes([key[28], key[29], key[30], key[31]]), + ]; + + Self { + r, + s, + h: [0; 5], + buffer: [0; 16], + buffer_len: 0, + } + } + + fn process_block(&mut self, block: &[u8], final_block: bool) { + + let t0 = u32::from_le_bytes([block[0], block[1], block[2], block[3]]); + let t1 = u32::from_le_bytes([block[4], block[5], block[6], block[7]]); + let t2 = u32::from_le_bytes([block[8], block[9], block[10], block[11]]); + let t3 = u32::from_le_bytes([block[12], block[13], block[14], block[15]]); + + let hibit = if final_block { 0 } else { 1 << 24 }; + + self.h[0] += t0 & 0x03ff_ffff; + self.h[1] += ((t0 >> 26) | (t1 << 6)) & 0x03ff_ffff; + self.h[2] += ((t1 >> 20) | (t2 << 12)) & 0x03ff_ffff; + self.h[3] += ((t2 >> 14) | (t3 << 18)) & 0x03ff_ffff; + self.h[4] += (t3 >> 8) | hibit; + + let r0 = self.r[0] as u64; + let r1 = self.r[1] as u64; + let r2 = self.r[2] as u64; + let r3 = self.r[3] as u64; + let r4 = self.r[4] as u64; + + let s1 = r1 * 5; + let s2 = r2 * 5; + let s3 = r3 * 5; + let s4 = r4 * 5; + + let h0 = self.h[0] as u64; + let h1 = self.h[1] as u64; + let h2 = self.h[2] as u64; + let h3 = self.h[3] as u64; + let h4 = self.h[4] as u64; + + let d0 = h0 * r0 + h1 * s4 + h2 * s3 + h3 * s2 + h4 * s1; + let d1 = h0 * r1 + h1 * r0 + h2 * s4 + h3 * s3 + h4 * s2; + let d2 = h0 * r2 + h1 * r1 + h2 * r0 + h3 * s4 + h4 * s3; + let d3 = h0 * r3 + h1 * r2 + h2 * r1 + h3 * r0 + h4 * s4; + let d4 = h0 * r4 + h1 * r3 + h2 * r2 + h3 * r1 + h4 * r0; + + let mut c: u64; + c = d0 >> 26; + self.h[0] = (d0 & 0x03ff_ffff) as u32; + let d1 = d1 + c; + c = d1 >> 26; + self.h[1] = (d1 & 0x03ff_ffff) as u32; + let d2 = d2 + c; + c = d2 >> 26; + self.h[2] = (d2 & 0x03ff_ffff) as u32; + let d3 = d3 + c; + c = d3 >> 26; + self.h[3] = (d3 & 0x03ff_ffff) as u32; + let d4 = d4 + c; + c = d4 >> 26; + self.h[4] = (d4 & 0x03ff_ffff) as u32; + self.h[0] += (c * 5) as u32; + c = (self.h[0] >> 26) as u64; + self.h[0] &= 0x03ff_ffff; + self.h[1] += c as u32; + } + + pub fn update(&mut self, data: &[u8]) { + let mut offset = 0; + + if self.buffer_len > 0 { + let needed = 16 - self.buffer_len; + if data.len() >= needed { + self.buffer[self.buffer_len..].copy_from_slice(&data[..needed]); + let block = self.buffer; + self.process_block(&block, false); + self.buffer_len = 0; + offset = needed; + } else { + self.buffer[self.buffer_len..self.buffer_len + data.len()].copy_from_slice(data); + self.buffer_len += data.len(); + return; + } + } + + while offset + 16 <= data.len() { + self.process_block(&data[offset..offset + 16], false); + offset += 16; + } + + if offset < data.len() { + let remaining = data.len() - offset; + self.buffer[..remaining].copy_from_slice(&data[offset..]); + self.buffer_len = remaining; + } + } + + pub fn finalize(mut self) -> [u8; TAG_SIZE] { + + if self.buffer_len > 0 { + + self.buffer[self.buffer_len] = 1; + for i in self.buffer_len + 1..16 { + self.buffer[i] = 0; + } + let block = self.buffer; + self.process_block(&block, true); + } + + let mut c: u32; + c = self.h[1] >> 26; + self.h[1] &= 0x03ff_ffff; + self.h[2] += c; + c = self.h[2] >> 26; + self.h[2] &= 0x03ff_ffff; + self.h[3] += c; + c = self.h[3] >> 26; + self.h[3] &= 0x03ff_ffff; + self.h[4] += c; + c = self.h[4] >> 26; + self.h[4] &= 0x03ff_ffff; + self.h[0] += c * 5; + c = self.h[0] >> 26; + self.h[0] &= 0x03ff_ffff; + self.h[1] += c; + + let mut g0 = self.h[0].wrapping_add(5); + c = g0 >> 26; + g0 &= 0x03ff_ffff; + let mut g1 = self.h[1].wrapping_add(c); + c = g1 >> 26; + g1 &= 0x03ff_ffff; + let mut g2 = self.h[2].wrapping_add(c); + c = g2 >> 26; + g2 &= 0x03ff_ffff; + let mut g3 = self.h[3].wrapping_add(c); + c = g3 >> 26; + g3 &= 0x03ff_ffff; + let g4 = self.h[4].wrapping_add(c).wrapping_sub(1 << 26); + + let mask = (g4 >> 31).wrapping_sub(1); + g0 &= mask; + g1 &= mask; + g2 &= mask; + g3 &= mask; + let mask = !mask; + self.h[0] = (self.h[0] & mask) | g0; + self.h[1] = (self.h[1] & mask) | g1; + self.h[2] = (self.h[2] & mask) | g2; + self.h[3] = (self.h[3] & mask) | g3; + + let h0 = self.h[0] | (self.h[1] << 26); + let h1 = (self.h[1] >> 6) | (self.h[2] << 20); + let h2 = (self.h[2] >> 12) | (self.h[3] << 14); + let h3 = (self.h[3] >> 18) | (self.h[4] << 8); + + let mut f: u64; + f = h0 as u64 + self.s[0] as u64; + let t0 = f as u32; + f = h1 as u64 + self.s[1] as u64 + (f >> 32); + let t1 = f as u32; + f = h2 as u64 + self.s[2] as u64 + (f >> 32); + let t2 = f as u32; + f = h3 as u64 + self.s[3] as u64 + (f >> 32); + let t3 = f as u32; + + let mut tag = [0u8; TAG_SIZE]; + tag[0..4].copy_from_slice(&t0.to_le_bytes()); + tag[4..8].copy_from_slice(&t1.to_le_bytes()); + tag[8..12].copy_from_slice(&t2.to_le_bytes()); + tag[12..16].copy_from_slice(&t3.to_le_bytes()); + + tag + } + + pub fn mac(key: &[u8; KEY_SIZE], data: &[u8]) -> [u8; TAG_SIZE] { + let mut poly = Self::new(key); + poly.update(data); + poly.finalize() + } + + pub fn verify(key: &[u8; KEY_SIZE], data: &[u8], expected: &[u8; TAG_SIZE]) -> bool { + let computed = Self::mac(key, data); + super::constant_time_eq(&computed, expected) + } +} + +pub struct ChaCha20Poly1305; + +impl ChaCha20Poly1305 { + + pub fn seal( + key: &[u8; 32], + nonce: &[u8; 12], + aad: &[u8], + plaintext: &[u8], + ciphertext: &mut [u8], + tag: &mut [u8; 16], + ) { + assert!(ciphertext.len() >= plaintext.len()); + + let mut poly_key = [0u8; 32]; + let chacha = super::chacha20::ChaCha20::new(key, nonce); + let keystream = chacha.keystream(32); + poly_key.copy_from_slice(&keystream[..32]); + + ciphertext[..plaintext.len()].copy_from_slice(plaintext); + let chacha = super::chacha20::ChaCha20::new_with_counter(key, nonce, 1); + chacha.encrypt(&mut ciphertext[..plaintext.len()]); + + let mut poly = Poly1305::new(&poly_key); + + poly.update(aad); + + let aad_pad = (16 - (aad.len() % 16)) % 16; + if aad_pad > 0 { + poly.update(&[0u8; 16][..aad_pad]); + } + + poly.update(&ciphertext[..plaintext.len()]); + + let ct_pad = (16 - (plaintext.len() % 16)) % 16; + if ct_pad > 0 { + poly.update(&[0u8; 16][..ct_pad]); + } + + poly.update(&(aad.len() as u64).to_le_bytes()); + poly.update(&(plaintext.len() as u64).to_le_bytes()); + + *tag = poly.finalize(); + } + + pub fn open( + key: &[u8; 32], + nonce: &[u8; 12], + aad: &[u8], + ciphertext: &[u8], + tag: &[u8; 16], + plaintext: &mut [u8], + ) -> bool { + assert!(plaintext.len() >= ciphertext.len()); + + let mut poly_key = [0u8; 32]; + let chacha = super::chacha20::ChaCha20::new(key, nonce); + let keystream = chacha.keystream(32); + poly_key.copy_from_slice(&keystream[..32]); + + let mut poly = Poly1305::new(&poly_key); + + poly.update(aad); + let aad_pad = (16 - (aad.len() % 16)) % 16; + if aad_pad > 0 { + poly.update(&[0u8; 16][..aad_pad]); + } + + poly.update(ciphertext); + let ct_pad = (16 - (ciphertext.len() % 16)) % 16; + if ct_pad > 0 { + poly.update(&[0u8; 16][..ct_pad]); + } + + poly.update(&(aad.len() as u64).to_le_bytes()); + poly.update(&(ciphertext.len() as u64).to_le_bytes()); + + let computed_tag = poly.finalize(); + if !super::constant_time_eq(&computed_tag, tag) { + return false; + } + + plaintext[..ciphertext.len()].copy_from_slice(ciphertext); + let chacha = super::chacha20::ChaCha20::new_with_counter(key, nonce, 1); + chacha.decrypt(&mut plaintext[..ciphertext.len()]); + + true + } +} diff --git a/src/crypto/sha256.rs b/src/crypto/sha256.rs new file mode 100644 index 0000000..e859a92 --- /dev/null +++ b/src/crypto/sha256.rs @@ -0,0 +1,198 @@ +const K: [u32; 64] = [ + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2, +]; + +const H_INIT: [u32; 8] = [ + 0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a, + 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19, +]; + +pub const DIGEST_SIZE: usize = 32; + +pub const BLOCK_SIZE: usize = 64; + +pub struct Sha256 { + + state: [u32; 8], + + total_len: u64, + + buffer: [u8; BLOCK_SIZE], + + buffer_len: usize, +} + +impl Sha256 { + + pub fn new() -> Self { + Self { + state: H_INIT, + total_len: 0, + buffer: [0u8; BLOCK_SIZE], + buffer_len: 0, + } + } + + pub fn update(&mut self, data: &[u8]) { + let mut offset = 0; + + if self.buffer_len > 0 { + let needed = BLOCK_SIZE - self.buffer_len; + if data.len() >= needed { + self.buffer[self.buffer_len..].copy_from_slice(&data[..needed]); + self.process_block(&self.buffer.clone()); + self.buffer_len = 0; + offset = needed; + } else { + self.buffer[self.buffer_len..self.buffer_len + data.len()].copy_from_slice(data); + self.buffer_len += data.len(); + self.total_len += data.len() as u64; + return; + } + } + + while offset + BLOCK_SIZE <= data.len() { + let mut block = [0u8; BLOCK_SIZE]; + block.copy_from_slice(&data[offset..offset + BLOCK_SIZE]); + self.process_block(&block); + offset += BLOCK_SIZE; + } + + if offset < data.len() { + let remaining = data.len() - offset; + self.buffer[..remaining].copy_from_slice(&data[offset..]); + self.buffer_len = remaining; + } + + self.total_len += data.len() as u64; + } + + pub fn finalize(mut self) -> [u8; DIGEST_SIZE] { + + let total_bits = self.total_len * 8; + + self.buffer[self.buffer_len] = 0x80; + self.buffer_len += 1; + + if self.buffer_len > 56 { + + for i in self.buffer_len..BLOCK_SIZE { + self.buffer[i] = 0; + } + self.process_block(&self.buffer.clone()); + self.buffer_len = 0; + } + + for i in self.buffer_len..56 { + self.buffer[i] = 0; + } + + self.buffer[56..64].copy_from_slice(&total_bits.to_be_bytes()); + self.process_block(&self.buffer.clone()); + + let mut digest = [0u8; DIGEST_SIZE]; + for (i, word) in self.state.iter().enumerate() { + digest[i * 4..(i + 1) * 4].copy_from_slice(&word.to_be_bytes()); + } + + digest + } + + fn process_block(&mut self, block: &[u8; BLOCK_SIZE]) { + + let mut w = [0u32; 64]; + + for i in 0..16 { + w[i] = u32::from_be_bytes([ + block[i * 4], + block[i * 4 + 1], + block[i * 4 + 2], + block[i * 4 + 3], + ]); + } + + for i in 16..64 { + let s0 = w[i - 15].rotate_right(7) ^ w[i - 15].rotate_right(18) ^ (w[i - 15] >> 3); + let s1 = w[i - 2].rotate_right(17) ^ w[i - 2].rotate_right(19) ^ (w[i - 2] >> 10); + w[i] = w[i - 16] + .wrapping_add(s0) + .wrapping_add(w[i - 7]) + .wrapping_add(s1); + } + + let mut a = self.state[0]; + let mut b = self.state[1]; + let mut c = self.state[2]; + let mut d = self.state[3]; + let mut e = self.state[4]; + let mut f = self.state[5]; + let mut g = self.state[6]; + let mut h = self.state[7]; + + for i in 0..64 { + let s1 = e.rotate_right(6) ^ e.rotate_right(11) ^ e.rotate_right(25); + let ch = (e & f) ^ ((!e) & g); + let temp1 = h + .wrapping_add(s1) + .wrapping_add(ch) + .wrapping_add(K[i]) + .wrapping_add(w[i]); + let s0 = a.rotate_right(2) ^ a.rotate_right(13) ^ a.rotate_right(22); + let maj = (a & b) ^ (a & c) ^ (b & c); + let temp2 = s0.wrapping_add(maj); + + h = g; + g = f; + f = e; + e = d.wrapping_add(temp1); + d = c; + c = b; + b = a; + a = temp1.wrapping_add(temp2); + } + + self.state[0] = self.state[0].wrapping_add(a); + self.state[1] = self.state[1].wrapping_add(b); + self.state[2] = self.state[2].wrapping_add(c); + self.state[3] = self.state[3].wrapping_add(d); + self.state[4] = self.state[4].wrapping_add(e); + self.state[5] = self.state[5].wrapping_add(f); + self.state[6] = self.state[6].wrapping_add(g); + self.state[7] = self.state[7].wrapping_add(h); + } + + pub fn hash(data: &[u8]) -> [u8; DIGEST_SIZE] { + let mut hasher = Self::new(); + hasher.update(data); + hasher.finalize() + } + + pub fn hash256(data: &[u8]) -> [u8; DIGEST_SIZE] { + let first = Self::hash(data); + Self::hash(&first) + } +} + +impl Default for Sha256 { + fn default() -> Self { + Self::new() + } +} + +impl Clone for Sha256 { + fn clone(&self) -> Self { + Self { + state: self.state, + total_len: self.total_len, + buffer: self.buffer, + buffer_len: self.buffer_len, + } + } +} diff --git a/src/crypto/x25519.rs b/src/crypto/x25519.rs new file mode 100644 index 0000000..f676194 --- /dev/null +++ b/src/crypto/x25519.rs @@ -0,0 +1,395 @@ +#[derive(Clone, Copy)] +struct Fe([i64; 10]); + +const P: [i64; 10] = [ + 0x3ffffed, 0x1ffffff, 0x3ffffff, 0x1ffffff, 0x3ffffff, + 0x1ffffff, 0x3ffffff, 0x1ffffff, 0x3ffffff, 0x1ffffff, +]; + +impl Fe { + + const fn zero() -> Self { + Fe([0; 10]) + } + + const fn one() -> Self { + Fe([1, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + } + + fn from_bytes(bytes: &[u8; 32]) -> Self { + let mut h = [0i64; 10]; + + h[0] = load_4(&bytes[0..4]) & 0x3ffffff; + h[1] = (load_4(&bytes[3..7]) >> 2) & 0x1ffffff; + h[2] = (load_4(&bytes[6..10]) >> 3) & 0x3ffffff; + h[3] = (load_4(&bytes[9..13]) >> 5) & 0x1ffffff; + h[4] = (load_4(&bytes[12..16]) >> 6) & 0x3ffffff; + h[5] = (load_4(&bytes[16..20])) & 0x1ffffff; + h[6] = (load_4(&bytes[19..23]) >> 1) & 0x3ffffff; + h[7] = (load_4(&bytes[22..26]) >> 3) & 0x1ffffff; + h[8] = (load_4(&bytes[25..29]) >> 4) & 0x3ffffff; + h[9] = (load_3(&bytes[28..31]) >> 6) & 0x1ffffff; + + Fe(h) + } + + fn to_bytes(&self) -> [u8; 32] { + let mut h = self.0; + + let mut q = (19 * h[9] + (1 << 24)) >> 25; + q = (h[0] + q) >> 26; + q = (h[1] + q) >> 25; + q = (h[2] + q) >> 26; + q = (h[3] + q) >> 25; + q = (h[4] + q) >> 26; + q = (h[5] + q) >> 25; + q = (h[6] + q) >> 26; + q = (h[7] + q) >> 25; + q = (h[8] + q) >> 26; + q = (h[9] + q) >> 25; + + h[0] += 19 * q; + + let carry0 = h[0] >> 26; + h[1] += carry0; + h[0] -= carry0 << 26; + let carry1 = h[1] >> 25; + h[2] += carry1; + h[1] -= carry1 << 25; + let carry2 = h[2] >> 26; + h[3] += carry2; + h[2] -= carry2 << 26; + let carry3 = h[3] >> 25; + h[4] += carry3; + h[3] -= carry3 << 25; + let carry4 = h[4] >> 26; + h[5] += carry4; + h[4] -= carry4 << 26; + let carry5 = h[5] >> 25; + h[6] += carry5; + h[5] -= carry5 << 25; + let carry6 = h[6] >> 26; + h[7] += carry6; + h[6] -= carry6 << 26; + let carry7 = h[7] >> 25; + h[8] += carry7; + h[7] -= carry7 << 25; + let carry8 = h[8] >> 26; + h[9] += carry8; + h[8] -= carry8 << 26; + let carry9 = h[9] >> 25; + h[9] -= carry9 << 25; + + let mut s = [0u8; 32]; + s[0] = h[0] as u8; + s[1] = (h[0] >> 8) as u8; + s[2] = (h[0] >> 16) as u8; + s[3] = ((h[0] >> 24) | (h[1] << 2)) as u8; + s[4] = (h[1] >> 6) as u8; + s[5] = (h[1] >> 14) as u8; + s[6] = ((h[1] >> 22) | (h[2] << 3)) as u8; + s[7] = (h[2] >> 5) as u8; + s[8] = (h[2] >> 13) as u8; + s[9] = ((h[2] >> 21) | (h[3] << 5)) as u8; + s[10] = (h[3] >> 3) as u8; + s[11] = (h[3] >> 11) as u8; + s[12] = ((h[3] >> 19) | (h[4] << 6)) as u8; + s[13] = (h[4] >> 2) as u8; + s[14] = (h[4] >> 10) as u8; + s[15] = (h[4] >> 18) as u8; + s[16] = h[5] as u8; + s[17] = (h[5] >> 8) as u8; + s[18] = (h[5] >> 16) as u8; + s[19] = ((h[5] >> 24) | (h[6] << 1)) as u8; + s[20] = (h[6] >> 7) as u8; + s[21] = (h[6] >> 15) as u8; + s[22] = ((h[6] >> 23) | (h[7] << 3)) as u8; + s[23] = (h[7] >> 5) as u8; + s[24] = (h[7] >> 13) as u8; + s[25] = ((h[7] >> 21) | (h[8] << 4)) as u8; + s[26] = (h[8] >> 4) as u8; + s[27] = (h[8] >> 12) as u8; + s[28] = ((h[8] >> 20) | (h[9] << 6)) as u8; + s[29] = (h[9] >> 2) as u8; + s[30] = (h[9] >> 10) as u8; + s[31] = (h[9] >> 18) as u8; + + s + } + + fn add(&self, rhs: &Fe) -> Fe { + Fe([ + self.0[0] + rhs.0[0], + self.0[1] + rhs.0[1], + self.0[2] + rhs.0[2], + self.0[3] + rhs.0[3], + self.0[4] + rhs.0[4], + self.0[5] + rhs.0[5], + self.0[6] + rhs.0[6], + self.0[7] + rhs.0[7], + self.0[8] + rhs.0[8], + self.0[9] + rhs.0[9], + ]) + } + + fn sub(&self, rhs: &Fe) -> Fe { + Fe([ + self.0[0] - rhs.0[0], + self.0[1] - rhs.0[1], + self.0[2] - rhs.0[2], + self.0[3] - rhs.0[3], + self.0[4] - rhs.0[4], + self.0[5] - rhs.0[5], + self.0[6] - rhs.0[6], + self.0[7] - rhs.0[7], + self.0[8] - rhs.0[8], + self.0[9] - rhs.0[9], + ]) + } + + fn mul(&self, rhs: &Fe) -> Fe { + let f = &self.0; + let g = &rhs.0; + + let f0 = f[0] as i128; + let f1 = f[1] as i128; + let f2 = f[2] as i128; + let f3 = f[3] as i128; + let f4 = f[4] as i128; + let f5 = f[5] as i128; + let f6 = f[6] as i128; + let f7 = f[7] as i128; + let f8 = f[8] as i128; + let f9 = f[9] as i128; + + let g0 = g[0] as i128; + let g1 = g[1] as i128; + let g2 = g[2] as i128; + let g3 = g[3] as i128; + let g4 = g[4] as i128; + let g5 = g[5] as i128; + let g6 = g[6] as i128; + let g7 = g[7] as i128; + let g8 = g[8] as i128; + let g9 = g[9] as i128; + + let g1_19 = 19 * g1; + let g2_19 = 19 * g2; + let g3_19 = 19 * g3; + let g4_19 = 19 * g4; + let g5_19 = 19 * g5; + let g6_19 = 19 * g6; + let g7_19 = 19 * g7; + let g8_19 = 19 * g8; + let g9_19 = 19 * g9; + + let f1_2 = 2 * f1; + let f3_2 = 2 * f3; + let f5_2 = 2 * f5; + let f7_2 = 2 * f7; + let f9_2 = 2 * f9; + + let h0 = f0 * g0 + f1_2 * g9_19 + f2 * g8_19 + f3_2 * g7_19 + f4 * g6_19 + f5_2 * g5_19 + f6 * g4_19 + f7_2 * g3_19 + f8 * g2_19 + f9_2 * g1_19; + let h1 = f0 * g1 + f1 * g0 + f2 * g9_19 + f3 * g8_19 + f4 * g7_19 + f5 * g6_19 + f6 * g5_19 + f7 * g4_19 + f8 * g3_19 + f9 * g2_19; + let h2 = f0 * g2 + f1_2 * g1 + f2 * g0 + f3_2 * g9_19 + f4 * g8_19 + f5_2 * g7_19 + f6 * g6_19 + f7_2 * g5_19 + f8 * g4_19 + f9_2 * g3_19; + let h3 = f0 * g3 + f1 * g2 + f2 * g1 + f3 * g0 + f4 * g9_19 + f5 * g8_19 + f6 * g7_19 + f7 * g6_19 + f8 * g5_19 + f9 * g4_19; + let h4 = f0 * g4 + f1_2 * g3 + f2 * g2 + f3_2 * g1 + f4 * g0 + f5_2 * g9_19 + f6 * g8_19 + f7_2 * g7_19 + f8 * g6_19 + f9_2 * g5_19; + let h5 = f0 * g5 + f1 * g4 + f2 * g3 + f3 * g2 + f4 * g1 + f5 * g0 + f6 * g9_19 + f7 * g8_19 + f8 * g7_19 + f9 * g6_19; + let h6 = f0 * g6 + f1_2 * g5 + f2 * g4 + f3_2 * g3 + f4 * g2 + f5_2 * g1 + f6 * g0 + f7_2 * g9_19 + f8 * g8_19 + f9_2 * g7_19; + let h7 = f0 * g7 + f1 * g6 + f2 * g5 + f3 * g4 + f4 * g3 + f5 * g2 + f6 * g1 + f7 * g0 + f8 * g9_19 + f9 * g8_19; + let h8 = f0 * g8 + f1_2 * g7 + f2 * g6 + f3_2 * g5 + f4 * g4 + f5_2 * g3 + f6 * g2 + f7_2 * g1 + f8 * g0 + f9_2 * g9_19; + let h9 = f0 * g9 + f1 * g8 + f2 * g7 + f3 * g6 + f4 * g5 + f5 * g4 + f6 * g3 + f7 * g2 + f8 * g1 + f9 * g0; + + carry_mul([h0, h1, h2, h3, h4, h5, h6, h7, h8, h9]) + } + + fn square(&self) -> Fe { + self.mul(self) + } + + fn mul121666(&self) -> Fe { + let mut h = [0i128; 10]; + for i in 0..10 { + h[i] = (self.0[i] as i128) * 121666; + } + carry_mul(h) + } + + fn invert(&self) -> Fe { + let mut t0 = self.square(); + let mut t1 = t0.square(); + t1 = t1.square(); + t1 = self.mul(&t1); + t0 = t0.mul(&t1); + let mut t2 = t0.square(); + t1 = t1.mul(&t2); + t2 = t1.square(); + for _ in 1..5 { + t2 = t2.square(); + } + t1 = t2.mul(&t1); + t2 = t1.square(); + for _ in 1..10 { + t2 = t2.square(); + } + t2 = t2.mul(&t1); + let mut t3 = t2.square(); + for _ in 1..20 { + t3 = t3.square(); + } + t2 = t3.mul(&t2); + t2 = t2.square(); + for _ in 1..10 { + t2 = t2.square(); + } + t1 = t2.mul(&t1); + t2 = t1.square(); + for _ in 1..50 { + t2 = t2.square(); + } + t2 = t2.mul(&t1); + t3 = t2.square(); + for _ in 1..100 { + t3 = t3.square(); + } + t2 = t3.mul(&t2); + t2 = t2.square(); + for _ in 1..50 { + t2 = t2.square(); + } + t1 = t2.mul(&t1); + t1 = t1.square(); + t1 = t1.square(); + t1 = t1.square(); + t1 = t1.square(); + t1 = t1.square(); + t0.mul(&t1) + } + + fn cswap(a: &mut Fe, b: &mut Fe, swap: i64) { + let swap = -swap; + for i in 0..10 { + let x = swap & (a.0[i] ^ b.0[i]); + a.0[i] ^= x; + b.0[i] ^= x; + } + } +} + +fn load_4(s: &[u8]) -> i64 { + (s[0] as i64) + | ((s[1] as i64) << 8) + | ((s[2] as i64) << 16) + | ((s[3] as i64) << 24) +} + +fn load_3(s: &[u8]) -> i64 { + (s[0] as i64) | ((s[1] as i64) << 8) | ((s[2] as i64) << 16) +} + +fn carry_mul(h: [i128; 10]) -> Fe { + let mut out = [0i64; 10]; + + let mut carry = (h[0] + (1 << 25)) >> 26; + out[0] = (h[0] - (carry << 26)) as i64; + let h1 = h[1] + carry; + + carry = (h1 + (1 << 24)) >> 25; + out[1] = (h1 - (carry << 25)) as i64; + let h2 = h[2] + carry; + + carry = (h2 + (1 << 25)) >> 26; + out[2] = (h2 - (carry << 26)) as i64; + let h3 = h[3] + carry; + + carry = (h3 + (1 << 24)) >> 25; + out[3] = (h3 - (carry << 25)) as i64; + let h4 = h[4] + carry; + + carry = (h4 + (1 << 25)) >> 26; + out[4] = (h4 - (carry << 26)) as i64; + let h5 = h[5] + carry; + + carry = (h5 + (1 << 24)) >> 25; + out[5] = (h5 - (carry << 25)) as i64; + let h6 = h[6] + carry; + + carry = (h6 + (1 << 25)) >> 26; + out[6] = (h6 - (carry << 26)) as i64; + let h7 = h[7] + carry; + + carry = (h7 + (1 << 24)) >> 25; + out[7] = (h7 - (carry << 25)) as i64; + let h8 = h[8] + carry; + + carry = (h8 + (1 << 25)) >> 26; + out[8] = (h8 - (carry << 26)) as i64; + let h9 = h[9] + carry; + + carry = (h9 + (1 << 24)) >> 25; + out[9] = (h9 - (carry << 25)) as i64; + out[0] += (carry * 19) as i64; + + carry = (out[0] as i128 + (1 << 25)) >> 26; + out[0] -= (carry << 26) as i64; + out[1] += carry as i64; + + Fe(out) +} + +pub fn x25519(scalar: &[u8; 32], point: &[u8; 32]) -> [u8; 32] { + + let mut k = *scalar; + k[0] &= 248; + k[31] &= 127; + k[31] |= 64; + + let u = Fe::from_bytes(point); + + let mut x_1 = u; + let mut x_2 = Fe::one(); + let mut z_2 = Fe::zero(); + let mut x_3 = u; + let mut z_3 = Fe::one(); + + let mut swap: i64 = 0; + + for pos in (0..255).rev() { + let bit = ((k[pos / 8] >> (pos & 7)) & 1) as i64; + swap ^= bit; + Fe::cswap(&mut x_2, &mut x_3, swap); + Fe::cswap(&mut z_2, &mut z_3, swap); + swap = bit; + + let a = x_2.add(&z_2); + let aa = a.square(); + let b = x_2.sub(&z_2); + let bb = b.square(); + let e = aa.sub(&bb); + let c = x_3.add(&z_3); + let d = x_3.sub(&z_3); + let da = d.mul(&a); + let cb = c.mul(&b); + let sum = da.add(&cb); + let diff = da.sub(&cb); + x_3 = sum.square(); + z_3 = x_1.mul(&diff.square()); + x_2 = aa.mul(&bb); + z_2 = e.mul(&aa.add(&e.mul121666())); + } + + Fe::cswap(&mut x_2, &mut x_3, swap); + Fe::cswap(&mut z_2, &mut z_3, swap); + + let result = x_2.mul(&z_2.invert()); + result.to_bytes() +} + +pub fn x25519_base(scalar: &[u8; 32]) -> [u8; 32] { + + let basepoint: [u8; 32] = [ + 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ]; + x25519(scalar, &basepoint) +} diff --git a/src/display.rs b/src/display.rs new file mode 100644 index 0000000..e2ca860 --- /dev/null +++ b/src/display.rs @@ -0,0 +1,788 @@ +use heapless::String; + +pub const SSD1306_ADDR: u8 = 0x3C; + +pub const DISPLAY_WIDTH: usize = 128; +pub const DISPLAY_HEIGHT: usize = 64; + +pub const DISPLAY_PAGES: usize = DISPLAY_HEIGHT / 8; + +pub const FRAMEBUFFER_SIZE: usize = DISPLAY_WIDTH * DISPLAY_PAGES; + +const CMD_SET_CONTRAST: u8 = 0x81; +const CMD_DISPLAY_ALL_ON_RESUME: u8 = 0xA4; +const CMD_DISPLAY_ALL_ON: u8 = 0xA5; +const CMD_NORMAL_DISPLAY: u8 = 0xA6; +const CMD_INVERT_DISPLAY: u8 = 0xA7; +const CMD_DISPLAY_OFF: u8 = 0xAE; +const CMD_DISPLAY_ON: u8 = 0xAF; +const CMD_SET_DISPLAY_OFFSET: u8 = 0xD3; +const CMD_SET_COM_PINS: u8 = 0xDA; +const CMD_SET_VCOM_DETECT: u8 = 0xDB; +const CMD_SET_DISPLAY_CLOCK_DIV: u8 = 0xD5; +const CMD_SET_PRECHARGE: u8 = 0xD9; +const CMD_SET_MULTIPLEX: u8 = 0xA8; +const CMD_SET_LOW_COLUMN: u8 = 0x00; +const CMD_SET_HIGH_COLUMN: u8 = 0x10; +const CMD_SET_START_LINE: u8 = 0x40; +const CMD_MEMORY_MODE: u8 = 0x20; +const CMD_COLUMN_ADDR: u8 = 0x21; +const CMD_PAGE_ADDR: u8 = 0x22; +const CMD_COM_SCAN_INC: u8 = 0xC0; +const CMD_COM_SCAN_DEC: u8 = 0xC8; +const CMD_SEG_REMAP: u8 = 0xA0; +const CMD_CHARGE_PUMP: u8 = 0x8D; +const CMD_DEACTIVATE_SCROLL: u8 = 0x2E; + +const CONTROL_CMD_SINGLE: u8 = 0x80; +const CONTROL_CMD_STREAM: u8 = 0x00; +const CONTROL_DATA_STREAM: u8 = 0x40; + +static FONT_5X7: [u8; 320] = [ + + 0x00, 0x00, 0x00, 0x00, 0x00, + + 0x00, 0x00, 0x5F, 0x00, 0x00, + + 0x00, 0x07, 0x00, 0x07, 0x00, + + 0x14, 0x7F, 0x14, 0x7F, 0x14, + + 0x24, 0x2A, 0x7F, 0x2A, 0x12, + + 0x23, 0x13, 0x08, 0x64, 0x62, + + 0x36, 0x49, 0x55, 0x22, 0x50, + + 0x00, 0x05, 0x03, 0x00, 0x00, + + 0x00, 0x1C, 0x22, 0x41, 0x00, + + 0x00, 0x41, 0x22, 0x1C, 0x00, + + 0x08, 0x2A, 0x1C, 0x2A, 0x08, + + 0x08, 0x08, 0x3E, 0x08, 0x08, + + 0x00, 0x50, 0x30, 0x00, 0x00, + + 0x08, 0x08, 0x08, 0x08, 0x08, + + 0x00, 0x60, 0x60, 0x00, 0x00, + + 0x20, 0x10, 0x08, 0x04, 0x02, + + 0x3E, 0x51, 0x49, 0x45, 0x3E, + + 0x00, 0x42, 0x7F, 0x40, 0x00, + + 0x42, 0x61, 0x51, 0x49, 0x46, + + 0x21, 0x41, 0x45, 0x4B, 0x31, + + 0x18, 0x14, 0x12, 0x7F, 0x10, + + 0x27, 0x45, 0x45, 0x45, 0x39, + + 0x3C, 0x4A, 0x49, 0x49, 0x30, + + 0x01, 0x71, 0x09, 0x05, 0x03, + + 0x36, 0x49, 0x49, 0x49, 0x36, + + 0x06, 0x49, 0x49, 0x29, 0x1E, + + 0x00, 0x36, 0x36, 0x00, 0x00, + + 0x00, 0x56, 0x36, 0x00, 0x00, + + 0x00, 0x08, 0x14, 0x22, 0x41, + + 0x14, 0x14, 0x14, 0x14, 0x14, + + 0x41, 0x22, 0x14, 0x08, 0x00, + + 0x02, 0x01, 0x51, 0x09, 0x06, + + 0x32, 0x49, 0x79, 0x41, 0x3E, + + 0x7E, 0x11, 0x11, 0x11, 0x7E, + + 0x7F, 0x49, 0x49, 0x49, 0x36, + + 0x3E, 0x41, 0x41, 0x41, 0x22, + + 0x7F, 0x41, 0x41, 0x22, 0x1C, + + 0x7F, 0x49, 0x49, 0x49, 0x41, + + 0x7F, 0x09, 0x09, 0x01, 0x01, + + 0x3E, 0x41, 0x41, 0x51, 0x32, + + 0x7F, 0x08, 0x08, 0x08, 0x7F, + + 0x00, 0x41, 0x7F, 0x41, 0x00, + + 0x20, 0x40, 0x41, 0x3F, 0x01, + + 0x7F, 0x08, 0x14, 0x22, 0x41, + + 0x7F, 0x40, 0x40, 0x40, 0x40, + + 0x7F, 0x02, 0x04, 0x02, 0x7F, + + 0x7F, 0x04, 0x08, 0x10, 0x7F, + + 0x3E, 0x41, 0x41, 0x41, 0x3E, + + 0x7F, 0x09, 0x09, 0x09, 0x06, + + 0x3E, 0x41, 0x51, 0x21, 0x5E, + + 0x7F, 0x09, 0x19, 0x29, 0x46, + + 0x46, 0x49, 0x49, 0x49, 0x31, + + 0x01, 0x01, 0x7F, 0x01, 0x01, + + 0x3F, 0x40, 0x40, 0x40, 0x3F, + + 0x1F, 0x20, 0x40, 0x20, 0x1F, + + 0x7F, 0x20, 0x18, 0x20, 0x7F, + + 0x63, 0x14, 0x08, 0x14, 0x63, + + 0x03, 0x04, 0x78, 0x04, 0x03, + + 0x61, 0x51, 0x49, 0x45, 0x43, + + 0x00, 0x00, 0x7F, 0x41, 0x41, + + 0x02, 0x04, 0x08, 0x10, 0x20, + + 0x41, 0x41, 0x7F, 0x00, 0x00, + + 0x04, 0x02, 0x01, 0x02, 0x04, + + 0x40, 0x40, 0x40, 0x40, 0x40, +]; + +pub struct Display { + + i2c: I2C, + + framebuffer: [u8; FRAMEBUFFER_SIZE], + + power_on: bool, + + inverted: bool, + + contrast: u8, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum DisplayError { + + I2cError, + + InvalidCoordinates, + + BufferOverflow, +} + +impl Display +where + I2C: embedded_hal::i2c::I2c, +{ + + pub fn new(i2c: I2C) -> Self { + Self { + i2c, + framebuffer: [0u8; FRAMEBUFFER_SIZE], + power_on: false, + inverted: false, + contrast: 0x7F, + } + } + + pub fn init(&mut self) -> Result<(), DisplayError> { + + let init_cmds: [u8; 26] = [ + CMD_DISPLAY_OFF, + CMD_SET_DISPLAY_CLOCK_DIV, 0x80, + CMD_SET_MULTIPLEX, 0x3F, + CMD_SET_DISPLAY_OFFSET, 0x00, + CMD_SET_START_LINE | 0x00, + CMD_CHARGE_PUMP, 0x14, + CMD_MEMORY_MODE, 0x00, + CMD_SEG_REMAP | 0x01, + CMD_COM_SCAN_DEC, + CMD_SET_COM_PINS, 0x12, + CMD_SET_CONTRAST, self.contrast, + CMD_SET_PRECHARGE, 0xF1, + CMD_SET_VCOM_DETECT, 0x40, + CMD_DISPLAY_ALL_ON_RESUME, + CMD_NORMAL_DISPLAY, + CMD_DEACTIVATE_SCROLL, + CMD_DISPLAY_ON, + ]; + + for cmd in init_cmds { + self.write_command(cmd)?; + } + + self.power_on = true; + self.clear(); + self.flush()?; + + Ok(()) + } + + fn write_command(&mut self, cmd: u8) -> Result<(), DisplayError> { + let buf = [CONTROL_CMD_SINGLE, cmd]; + self.i2c.write(SSD1306_ADDR, &buf).map_err(|_| DisplayError::I2cError) + } + + fn write_commands(&mut self, cmds: &[u8]) -> Result<(), DisplayError> { + for &cmd in cmds { + self.write_command(cmd)?; + } + Ok(()) + } + + pub fn flush(&mut self) -> Result<(), DisplayError> { + + self.write_commands(&[CMD_COLUMN_ADDR, 0, (DISPLAY_WIDTH - 1) as u8])?; + + self.write_commands(&[CMD_PAGE_ADDR, 0, (DISPLAY_PAGES - 1) as u8])?; + + const CHUNK_SIZE: usize = 128; + for chunk in self.framebuffer.chunks(CHUNK_SIZE) { + let mut buf = [0u8; CHUNK_SIZE + 1]; + buf[0] = CONTROL_DATA_STREAM; + buf[1..1 + chunk.len()].copy_from_slice(chunk); + self.i2c.write(SSD1306_ADDR, &buf[..1 + chunk.len()]) + .map_err(|_| DisplayError::I2cError)?; + } + + Ok(()) + } + + pub fn clear(&mut self) { + self.framebuffer.fill(0); + } + + pub fn fill(&mut self) { + self.framebuffer.fill(0xFF); + } + + pub fn set_pixel(&mut self, x: usize, y: usize, on: bool) { + if x >= DISPLAY_WIDTH || y >= DISPLAY_HEIGHT { + return; + } + + let page = y / 8; + let bit = y % 8; + let idx = page * DISPLAY_WIDTH + x; + + if on { + self.framebuffer[idx] |= 1 << bit; + } else { + self.framebuffer[idx] &= !(1 << bit); + } + } + + pub fn get_pixel(&self, x: usize, y: usize) -> bool { + if x >= DISPLAY_WIDTH || y >= DISPLAY_HEIGHT { + return false; + } + + let page = y / 8; + let bit = y % 8; + let idx = page * DISPLAY_WIDTH + x; + + (self.framebuffer[idx] >> bit) & 1 != 0 + } + + pub fn draw_hline(&mut self, x: usize, y: usize, width: usize, on: bool) { + for dx in 0..width { + self.set_pixel(x + dx, y, on); + } + } + + pub fn draw_vline(&mut self, x: usize, y: usize, height: usize, on: bool) { + for dy in 0..height { + self.set_pixel(x, y + dy, on); + } + } + + pub fn draw_rect(&mut self, x: usize, y: usize, width: usize, height: usize, on: bool) { + self.draw_hline(x, y, width, on); + self.draw_hline(x, y + height - 1, width, on); + self.draw_vline(x, y, height, on); + self.draw_vline(x + width - 1, y, height, on); + } + + pub fn fill_rect(&mut self, x: usize, y: usize, width: usize, height: usize, on: bool) { + for dy in 0..height { + self.draw_hline(x, y + dy, width, on); + } + } + + pub fn draw_char(&mut self, x: usize, y: usize, c: char) -> usize { + let c = c as u8; + if c < 32 || c > 95 + 32 { + return 0; + } + + let idx = ((c - 32) as usize) * 5; + if idx + 5 > FONT_5X7.len() { + return 0; + } + + for col in 0..5 { + let bits = FONT_5X7[idx + col]; + for row in 0..7 { + let on = (bits >> row) & 1 != 0; + self.set_pixel(x + col, y + row, on); + } + } + + 6 + } + + pub fn draw_text(&mut self, x: usize, y: usize, text: &str) { + let mut cx = x; + for c in text.chars() { + if cx >= DISPLAY_WIDTH { + break; + } + cx += self.draw_char(cx, y, c); + } + } + + pub fn draw_text_centered(&mut self, y: usize, text: &str) { + let width = text.len() * 6; + let x = if width < DISPLAY_WIDTH { + (DISPLAY_WIDTH - width) / 2 + } else { + 0 + }; + self.draw_text(x, y, text); + } + + pub fn set_contrast(&mut self, contrast: u8) -> Result<(), DisplayError> { + self.contrast = contrast; + self.write_commands(&[CMD_SET_CONTRAST, contrast]) + } + + pub fn power_on(&mut self) -> Result<(), DisplayError> { + self.write_command(CMD_DISPLAY_ON)?; + self.power_on = true; + Ok(()) + } + + pub fn power_off(&mut self) -> Result<(), DisplayError> { + self.write_command(CMD_DISPLAY_OFF)?; + self.power_on = false; + Ok(()) + } + + pub fn invert(&mut self, invert: bool) -> Result<(), DisplayError> { + self.inverted = invert; + if invert { + self.write_command(CMD_INVERT_DISPLAY) + } else { + self.write_command(CMD_NORMAL_DISPLAY) + } + } + + pub fn is_on(&self) -> bool { + self.power_on + } + + pub fn framebuffer(&self) -> &[u8; FRAMEBUFFER_SIZE] { + &self.framebuffer + } + + pub fn framebuffer_mut(&mut self) -> &mut [u8; FRAMEBUFFER_SIZE] { + &mut self.framebuffer + } +} + +pub struct StatusDisplay { + display: Display, +} + +pub struct StatusContent { + + pub protocol: &'static str, + + pub node_id: u32, + + pub battery_pct: u8, + + pub rx_count: u32, + + pub tx_count: u32, + + pub rssi: i16, + + pub connected: bool, +} + +impl StatusDisplay +where + I2C: embedded_hal::i2c::I2c, +{ + + pub fn new(i2c: I2C) -> Self { + Self { + display: Display::new(i2c), + } + } + + pub fn init(&mut self) -> Result<(), DisplayError> { + self.display.init() + } + + pub fn show_splash(&mut self) -> Result<(), DisplayError> { + self.display.clear(); + + self.display.draw_rect(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT, true); + + self.display.draw_text_centered(8, "LunarCore"); + self.display.draw_text_centered(18, "v1.0.0"); + + self.display.draw_text_centered(32, "Unified Mesh"); + self.display.draw_text_centered(42, "Bridge Firmware"); + + self.display.draw_text_centered(54, "MC | MT | RN"); + + self.display.flush() + } + + pub fn show_status(&mut self, status: &StatusContent) -> Result<(), DisplayError> { + self.display.clear(); + + self.display.draw_text(0, 0, status.protocol); + + let batt_str = format_battery(status.battery_pct); + self.display.draw_text(DISPLAY_WIDTH - 24, 0, &batt_str); + + self.display.draw_hline(0, 9, DISPLAY_WIDTH, true); + + self.display.draw_text(0, 12, "ID:"); + let id_str = format_hex32(status.node_id); + self.display.draw_text(24, 12, &id_str); + + self.display.draw_text(0, 22, "RX:"); + let rx_str = format_u32(status.rx_count); + self.display.draw_text(24, 22, &rx_str); + + self.display.draw_text(64, 22, "TX:"); + let tx_str = format_u32(status.tx_count); + self.display.draw_text(88, 22, &tx_str); + + self.display.draw_text(0, 32, "RSSI:"); + let rssi_str = format_i16(status.rssi); + self.display.draw_text(36, 32, &rssi_str); + self.display.draw_text(72, 32, "dBm"); + + self.display.draw_text(0, 44, "Status:"); + if status.connected { + self.display.draw_text(48, 44, "CONNECTED"); + } else { + self.display.draw_text(48, 44, "WAITING"); + } + + self.display.draw_hline(0, 54, DISPLAY_WIDTH, true); + + self.display.draw_text_centered(56, "github.com/yours"); + + self.display.flush() + } + + pub fn show_error(&mut self, msg: &str) -> Result<(), DisplayError> { + self.display.clear(); + + self.display.draw_text_centered(20, "ERROR"); + self.display.draw_hline(20, 30, DISPLAY_WIDTH - 40, true); + self.display.draw_text_centered(36, msg); + + self.display.flush() + } + + pub fn show_message(&mut self, line1: &str, line2: &str) -> Result<(), DisplayError> { + self.display.clear(); + + self.display.draw_text_centered(24, line1); + self.display.draw_text_centered(36, line2); + + self.display.flush() + } + + pub fn power_off(&mut self) -> Result<(), DisplayError> { + self.display.power_off() + } + + pub fn power_on(&mut self) -> Result<(), DisplayError> { + self.display.power_on() + } + + pub fn display(&mut self) -> &mut Display { + &mut self.display + } +} + +fn format_battery(pct: u8) -> String<4> { + let mut s = String::new(); + if pct >= 100 { + let _ = s.push_str("100"); + } else if pct >= 10 { + let _ = s.push((b'0' + pct / 10) as char); + let _ = s.push((b'0' + pct % 10) as char); + } else { + let _ = s.push((b'0' + pct) as char); + } + let _ = s.push('%'); + s +} + +fn format_hex32(val: u32) -> String<8> { + const HEX: &[u8] = b"0123456789ABCDEF"; + let mut s = String::new(); + for i in (0..8).rev() { + let nibble = ((val >> (i * 4)) & 0xF) as usize; + let _ = s.push(HEX[nibble] as char); + } + s +} + +fn format_u32(val: u32) -> String<10> { + let mut s = String::new(); + if val == 0 { + let _ = s.push('0'); + return s; + } + + let mut v = val; + let mut digits = [0u8; 10]; + let mut count = 0; + + while v > 0 { + digits[count] = (v % 10) as u8; + v /= 10; + count += 1; + } + + for i in (0..count).rev() { + let _ = s.push((b'0' + digits[i]) as char); + } + s +} + +fn format_i16(val: i16) -> String<6> { + let mut s = String::new(); + if val < 0 { + let _ = s.push('-'); + let abs = (-(val as i32)) as u32; + let formatted = format_u32(abs); + let _ = s.push_str(&formatted); + } else { + let formatted = format_u32(val as u32); + let _ = s.push_str(&formatted); + } + s +} + +static MOON_PHASES: [[u8; 72]; 8] = [ + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0x38,0x1C,0x0C,0x06,0x06, + 0x06,0x06,0x0C,0x1C,0x38,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xE0,0x80,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x80,0xE0,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1C,0x38,0x30,0x60,0x60, + 0x60,0x60,0x30,0x38,0x1C,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0x38,0x1C,0x0C,0x06,0x06, + 0x06,0x06,0x0C,0x1C,0x38,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0xFE,0xFC,0xF8,0xF0,0xE0, + 0xC0,0x80,0x00,0x00,0x00,0x80,0xE0,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1C,0x38,0x31,0x63,0x63, + 0x63,0x63,0x31,0x38,0x1C,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0x38,0x1C,0x0C,0x06,0x06, + 0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0xFE,0xFC,0xF8,0xF0,0xE0, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1C,0x38,0x30,0x60,0x60, + 0x7F,0x7F,0x3F,0x3F,0x1F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE, + 0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1F,0x3F,0x3F,0x7F,0x7F, + 0x7F,0x7F,0x3F,0x3F,0x1F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE, + 0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1F,0x3F,0x3F,0x7F,0x7F, + 0x7F,0x7F,0x3F,0x3F,0x1F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE, + 0xFE,0xFE,0xFC,0xFC,0xF8,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1F,0x3F,0x3F,0x7F,0x7F, + 0x7F,0x7F,0x3F,0x3F,0x1F,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE, + 0x06,0x06,0x0C,0x1C,0x38,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, + 0x00,0x00,0x00,0x00,0x00,0x80,0xE0,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1F,0x3F,0x3F,0x7F,0x7F, + 0x60,0x60,0x30,0x38,0x1C,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], + + [ + 0x00,0x00,0x00,0x00,0x00,0xE0,0xF0,0xF8,0xFC,0xFC,0xFE,0xFE, + 0x06,0x06,0x0C,0x1C,0x38,0xF0,0xE0,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x1F,0x7F,0xFF,0xFF,0x07,0x03,0x01,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x80,0xE0,0x7F,0x1F,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x1F,0x3F,0x3F,0x7F,0x7F, + 0x60,0x60,0x30,0x38,0x1C,0x0F,0x07,0x00,0x00,0x00,0x00,0x00, + ], +]; + +static YOURS_FACE: [u8; 128] = [ + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xE0,0xF0,0xF8,0xF8,0xF8,0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,0xF8,0xF8,0xF0,0xF0,0xE0,0xE0,0xE0,0xC0,0x80,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0xFF,0xFF,0xFF,0xFF,0xFF,0xEF,0xE7,0xE7,0xE7,0xC7,0xC7,0xC7,0xC7,0xC7,0xFF,0xFF,0xFF,0xFF,0x0F,0x07,0x03,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x0F,0x0F,0x7F,0xFF,0xFD,0xF9,0xC1,0xC1,0xDE,0xFE,0xF0,0xFF,0xFF,0xFF,0xFF,0x7F,0x1F,0x07,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x07,0x0F,0x1F,0x1F,0x1D,0x1D,0x0F,0x0F,0x03,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +]; + +impl Display +where + I2C: embedded_hal::i2c::I2c, +{ + + pub fn draw_bitmap_24x24(&mut self, x: usize, y: usize, bitmap: &[u8; 72]) { + for col in 0..24 { + for page in 0..3 { + let byte = bitmap[page * 24 + col]; + for bit in 0..8 { + let px_y = y + page * 8 + bit; + let px_x = x + col; + if px_x < DISPLAY_WIDTH && px_y < DISPLAY_HEIGHT { + self.set_pixel(px_x, px_y, (byte >> bit) & 1 != 0); + } + } + } + } + } + + pub fn draw_bitmap_32x32(&mut self, x: usize, y: usize, bitmap: &[u8; 128]) { + for col in 0..32 { + for page in 0..4 { + let byte = bitmap[page * 32 + col]; + for bit in 0..8 { + let px_y = y + page * 8 + bit; + let px_x = x + col; + if px_x < DISPLAY_WIDTH && px_y < DISPLAY_HEIGHT { + self.set_pixel(px_x, px_y, (byte >> bit) & 1 != 0); + } + } + } + } + } +} + +impl StatusDisplay +where + I2C: embedded_hal::i2c::I2c, +{ + + pub fn boot_animation(&mut self, delay_fn: &mut dyn FnMut(u32)) -> Result<(), DisplayError> { + + for cycle in 0..2 { + for phase in 0..8 { + self.display.clear(); + + let moon_x = (DISPLAY_WIDTH - 24) / 2; + let moon_y = 8; + self.display.draw_bitmap_24x24(moon_x, moon_y, &MOON_PHASES[phase]); + + self.display.draw_text_centered(40, "LUNARCORE"); + + let dots_x = (DISPLAY_WIDTH - 8 * 4) / 2; + for i in 0..8 { + let on = i <= phase; + self.display.fill_rect(dots_x + i * 4, 54, 2, 2, on); + } + + self.display.flush()?; + + let delay = if cycle == 1 { 120 } else { 80 }; + delay_fn(delay); + } + } + + self.show_branding()?; + delay_fn(1500); + + Ok(()) + } + + pub fn show_branding(&mut self) -> Result<(), DisplayError> { + self.display.clear(); + + let logo_x = (DISPLAY_WIDTH - 32) / 2; + let logo_y = 4; + self.display.draw_bitmap_32x32(logo_x, logo_y, &YOURS_FACE); + + self.display.draw_text_centered(42, "[ YOURS ]"); + self.display.draw_text_centered(52, "x [ LUNARCORE ]"); + + self.display.flush() + } + + pub fn show_init_progress(&mut self, step: &str, progress: u8) -> Result<(), DisplayError> { + self.display.clear(); + + self.display.draw_text_centered(8, "INITIALIZING"); + + self.display.draw_text_centered(24, step); + + let bar_width = 100; + let bar_x = (DISPLAY_WIDTH - bar_width) / 2; + let bar_y = 40; + let filled = (bar_width as u32 * progress as u32 / 100) as usize; + + self.display.draw_rect(bar_x, bar_y, bar_width, 8, true); + self.display.fill_rect(bar_x + 1, bar_y + 1, filled.saturating_sub(2), 6, true); + + let pct_str = format_battery(progress); + self.display.draw_text_centered(52, &pct_str); + + self.display.flush() + } +} diff --git a/src/identity.rs b/src/identity.rs new file mode 100644 index 0000000..dfb80e1 --- /dev/null +++ b/src/identity.rs @@ -0,0 +1,451 @@ +use crate::crypto::ed25519::{Ed25519, PublicKey, PrivateKey, Signature}; +use crate::crypto::x25519::{x25519, x25519_base}; +use crate::crypto::sha256::Sha256; +use crate::crypto::chacha20::{ChaCha20, KEY_SIZE, NONCE_SIZE}; + +pub const SEED_SIZE: usize = 32; + +pub const NODE_ID_SIZE: usize = 4; + +pub const SHORT_ID_SIZE: usize = 8; + +const IDENTITY_FLASH_KEY: &str = "lunar_id"; + +const KDF_CONTEXT_IDENTITY: &[u8] = b"LunarCore Identity v1"; +const KDF_CONTEXT_SIGNING: &[u8] = b"LunarCore Signing v1"; +const KDF_CONTEXT_ENCRYPTION: &[u8] = b"LunarCore Encryption v1"; + +pub struct Identity { + + signing_key: PrivateKey, + + public_key: PublicKey, + + encryption_key: [u8; 32], + + encryption_public: [u8; 32], + + created_at: u32, +} + +impl Drop for Identity { + fn drop(&mut self) { + crate::crypto::secure_zero(&mut self.signing_key); + crate::crypto::secure_zero(&mut self.encryption_key); + } +} + +impl Identity { + + pub fn generate() -> Self { + + let mut seed = [0u8; SEED_SIZE]; + if !crate::rng::fill_random_checked(&mut seed) { + panic!("RNG health check failed - cannot generate identity with weak entropy"); + } + + Self::from_seed(&seed) + } + + pub fn from_seed(seed: &[u8; SEED_SIZE]) -> Self { + + let signing_key = Self::derive_key(seed, KDF_CONTEXT_SIGNING); + let public_key = Ed25519::public_key(&signing_key); + + let encryption_key = Self::derive_key(seed, KDF_CONTEXT_ENCRYPTION); + let encryption_public = x25519_base(&encryption_key); + + Self { + signing_key, + public_key, + encryption_key, + encryption_public, + created_at: 0, + } + } + + fn derive_key(seed: &[u8; SEED_SIZE], context: &[u8]) -> [u8; 32] { + let mut input = [0u8; 64]; + input[..32].copy_from_slice(seed); + let context_len = context.len().min(32); + input[32..32 + context_len].copy_from_slice(&context[..context_len]); + Sha256::hash(&input) + } + + pub fn public_key(&self) -> &PublicKey { + &self.public_key + } + + pub fn encryption_public_key(&self) -> &[u8; 32] { + &self.encryption_public + } + + pub fn node_id(&self) -> u32 { + let hash = Sha256::hash(&self.public_key); + u32::from_le_bytes([hash[0], hash[1], hash[2], hash[3]]) + } + + pub fn short_id(&self) -> [u8; SHORT_ID_SIZE] { + let hash = Sha256::hash(&self.public_key); + let mut short = [0u8; SHORT_ID_SIZE]; + for i in 0..4 { + let byte = hash[i]; + short[i * 2] = hex_char(byte >> 4); + short[i * 2 + 1] = hex_char(byte & 0x0F); + } + short + } + + pub fn sign(&self, message: &[u8]) -> Signature { + Ed25519::sign(&self.signing_key, message) + } + + pub fn verify(public_key: &PublicKey, message: &[u8], signature: &Signature) -> bool { + Ed25519::verify(public_key, message, signature) + } + + pub fn key_agree(&self, their_public: &[u8; 32]) -> [u8; 32] { + x25519(&self.encryption_key, their_public) + } + + pub fn encrypt_for_storage(&self, storage_key: &[u8; 32]) -> EncryptedIdentity { + + let mut plaintext = [0u8; 128]; + plaintext[0..32].copy_from_slice(&self.signing_key); + plaintext[32..64].copy_from_slice(&self.encryption_key); + plaintext[64..68].copy_from_slice(&self.created_at.to_le_bytes()); + + let mut nonce = [0u8; NONCE_SIZE]; + if !crate::rng::fill_random_checked(&mut nonce) { + panic!("RNG health check failed - cannot encrypt identity with weak nonce"); + } + + let cipher = ChaCha20::new(storage_key, &nonce); + let mut ciphertext = plaintext; + cipher.encrypt(&mut ciphertext); + + let mut tag_input = [0u8; 128 + NONCE_SIZE]; + tag_input[..128].copy_from_slice(&ciphertext); + tag_input[128..].copy_from_slice(&nonce); + let tag = Sha256::hash(&tag_input); + + EncryptedIdentity { + ciphertext, + nonce, + tag, + } + } + + pub fn decrypt_from_storage( + encrypted: &EncryptedIdentity, + storage_key: &[u8; 32], + ) -> Option { + + let mut tag_input = [0u8; 128 + NONCE_SIZE]; + tag_input[..128].copy_from_slice(&encrypted.ciphertext); + tag_input[128..].copy_from_slice(&encrypted.nonce); + let expected_tag = Sha256::hash(&tag_input); + + if !crate::crypto::constant_time_eq(&encrypted.tag, &expected_tag) { + return None; + } + + let cipher = ChaCha20::new(storage_key, &encrypted.nonce); + let mut plaintext = encrypted.ciphertext; + cipher.decrypt(&mut plaintext); + + let mut signing_key = [0u8; 32]; + let mut encryption_key = [0u8; 32]; + signing_key.copy_from_slice(&plaintext[0..32]); + encryption_key.copy_from_slice(&plaintext[32..64]); + let created_at = u32::from_le_bytes([ + plaintext[64], plaintext[65], plaintext[66], plaintext[67] + ]); + + let public_key = Ed25519::public_key(&signing_key); + let encryption_public = x25519_base(&encryption_key); + + crate::crypto::secure_zero(&mut plaintext); + + Some(Self { + signing_key, + public_key, + encryption_key, + encryption_public, + created_at, + }) + } + + pub fn export_seed(&self) -> [u8; SEED_SIZE] { + + self.signing_key + } +} + +pub struct EncryptedIdentity { + + pub ciphertext: [u8; 128], + + pub nonce: [u8; NONCE_SIZE], + + pub tag: [u8; 32], +} + +impl EncryptedIdentity { + + pub fn to_bytes(&self) -> [u8; 128 + NONCE_SIZE + 32] { + let mut bytes = [0u8; 128 + NONCE_SIZE + 32]; + bytes[0..128].copy_from_slice(&self.ciphertext); + bytes[128..128 + NONCE_SIZE].copy_from_slice(&self.nonce); + bytes[128 + NONCE_SIZE..].copy_from_slice(&self.tag); + bytes + } + + pub fn from_bytes(bytes: &[u8; 128 + NONCE_SIZE + 32]) -> Self { + let mut ciphertext = [0u8; 128]; + let mut nonce = [0u8; NONCE_SIZE]; + let mut tag = [0u8; 32]; + + ciphertext.copy_from_slice(&bytes[0..128]); + nonce.copy_from_slice(&bytes[128..128 + NONCE_SIZE]); + tag.copy_from_slice(&bytes[128 + NONCE_SIZE..]); + + Self { ciphertext, nonce, tag } + } +} + +pub struct IdentityManager { + + current: Option, + + storage_key: [u8; 32], +} + +impl Drop for IdentityManager { + fn drop(&mut self) { + crate::crypto::secure_zero(&mut self.storage_key); + } +} + +impl IdentityManager { + + pub fn new() -> Self { + let storage_key = Self::derive_storage_key(); + Self { + current: None, + storage_key, + } + } + + fn derive_storage_key() -> [u8; 32] { + + let mut efuse_data = [0u8; 32]; + + #[cfg(target_arch = "xtensa")] + unsafe { + + let efuse_base = 0x6001_A000 as *const u32; + for i in 0..8 { + let val = core::ptr::read_volatile(efuse_base.add(i)); + efuse_data[i * 4..(i + 1) * 4].copy_from_slice(&val.to_le_bytes()); + } + } + + #[cfg(not(target_arch = "xtensa"))] + { + + efuse_data = [0u8; 32]; + } + + let mut kdf_input = [0u8; 64]; + kdf_input[..32].copy_from_slice(&efuse_data); + kdf_input[32..].copy_from_slice(b"LunarCore Storage Key v1\0\0\0\0\0\0\0\0"); + + Sha256::hash(&kdf_input) + } + + pub fn init(&mut self) -> &Identity { + + if let Some(identity) = self.load_from_flash() { + self.current = Some(identity); + } else { + + let identity = Identity::generate(); + self.save_to_flash(&identity); + self.current = Some(identity); + } + + self.current.as_ref().unwrap() + } + + pub fn current(&self) -> Option<&Identity> { + self.current.as_ref() + } + + pub fn rotate(&mut self) -> &Identity { + let identity = Identity::generate(); + self.save_to_flash(&identity); + self.current = Some(identity); + self.current.as_ref().unwrap() + } + + pub fn import_seed(&mut self, seed: &[u8; SEED_SIZE]) -> &Identity { + let identity = Identity::from_seed(seed); + self.save_to_flash(&identity); + self.current = Some(identity); + self.current.as_ref().unwrap() + } + + fn load_from_flash(&self) -> Option { + + #[cfg(target_arch = "xtensa")] + { + use esp_idf_sys::*; + + unsafe { + let mut handle: nvs_handle_t = 0; + let namespace = b"lunar\0".as_ptr(); + + let ret = nvs_open(namespace, nvs_open_mode_t_NVS_READONLY, &mut handle); + if ret != 0 { + return None; + } + + let key = b"identity\0".as_ptr(); + let mut size: usize = 128 + NONCE_SIZE + 32; + let mut bytes = [0u8; 128 + NONCE_SIZE + 32]; + + let ret = nvs_get_blob(handle, key, bytes.as_mut_ptr() as *mut _, &mut size); + nvs_close(handle); + + if ret != 0 || size != bytes.len() { + return None; + } + + let encrypted = EncryptedIdentity::from_bytes(&bytes); + Identity::decrypt_from_storage(&encrypted, &self.storage_key) + } + } + + #[cfg(not(target_arch = "xtensa"))] + { + None + } + } + + fn save_to_flash(&self, identity: &Identity) { + let encrypted = identity.encrypt_for_storage(&self.storage_key); + let bytes = encrypted.to_bytes(); + + #[cfg(target_arch = "xtensa")] + unsafe { + use esp_idf_sys::*; + + let mut handle: nvs_handle_t = 0; + let namespace = b"lunar\0".as_ptr(); + + let ret = nvs_open(namespace, nvs_open_mode_t_NVS_READWRITE, &mut handle); + if ret != 0 { + return; + } + + let key = b"identity\0".as_ptr(); + nvs_set_blob(handle, key, bytes.as_ptr() as *const _, bytes.len()); + nvs_commit(handle); + nvs_close(handle); + } + + #[cfg(not(target_arch = "xtensa"))] + { + let _ = bytes; + } + } +} + +impl Default for IdentityManager { + fn default() -> Self { + Self::new() + } +} + +fn hex_char(nibble: u8) -> u8 { + match nibble { + 0..=9 => b'0' + nibble, + 10..=15 => b'a' + (nibble - 10), + _ => b'?', + } +} + +use core::sync::atomic::{AtomicBool, Ordering}; + +static mut IDENTITY_MANAGER: Option = None; +static IDENTITY_INIT: AtomicBool = AtomicBool::new(false); + +pub fn init() { + if !IDENTITY_INIT.swap(true, Ordering::SeqCst) { + unsafe { + IDENTITY_MANAGER = Some(IdentityManager::new()); + IDENTITY_MANAGER.as_mut().unwrap().init(); + } + } +} + +pub fn node_id() -> u32 { + init(); + unsafe { + IDENTITY_MANAGER + .as_ref() + .and_then(|m| m.current()) + .map(|i| i.node_id()) + .unwrap_or(0) + } +} + +pub fn public_key() -> Option { + init(); + unsafe { + IDENTITY_MANAGER + .as_ref() + .and_then(|m| m.current()) + .map(|i| *i.public_key()) + } +} + +pub fn encryption_public_key() -> Option<[u8; 32]> { + init(); + unsafe { + IDENTITY_MANAGER + .as_ref() + .and_then(|m| m.current()) + .map(|i| *i.encryption_public_key()) + } +} + +pub fn sign(message: &[u8]) -> Option { + init(); + unsafe { + IDENTITY_MANAGER + .as_ref() + .and_then(|m| m.current()) + .map(|i| i.sign(message)) + } +} + +pub fn key_agree(their_public: &[u8; 32]) -> Option<[u8; 32]> { + init(); + unsafe { + IDENTITY_MANAGER + .as_ref() + .and_then(|m| m.current()) + .map(|i| i.key_agree(their_public)) + } +} + +pub fn rotate() -> Option { + init(); + unsafe { + IDENTITY_MANAGER + .as_mut() + .map(|m| m.rotate().node_id()) + } +} diff --git a/src/main.rs b/src/main.rs new file mode 100644 index 0000000..aaa767f --- /dev/null +++ b/src/main.rs @@ -0,0 +1,1662 @@ +mod crypto; +mod rng; +mod sx1262; +mod protocol; +mod protocol_router; +mod meshtastic; +mod rnode; +mod ble; +mod display; +mod transport; +mod session; +mod onion; + +use esp_idf_hal::delay::FreeRtos; +use esp_idf_sys as _; + +use esp_idf_hal::prelude::*; +use esp_idf_hal::gpio::*; +use esp_idf_hal::spi::{SpiDeviceDriver, SpiDriverConfig}; +use esp_idf_hal::i2c::{I2cConfig, I2cDriver}; +use esp_idf_hal::uart::UartDriver; +use esp_idf_hal::adc::attenuation::DB_11; +use esp_idf_hal::adc::oneshot::config::AdcChannelConfig; +use esp_idf_hal::adc::oneshot::{AdcDriver, AdcChannelDriver}; +use display::StatusDisplay; +use core::sync::atomic::{AtomicBool, AtomicU32, Ordering}; +use crypto::sha256::Sha256; +use heapless::Vec; + +use sx1262::{Sx1262, RadioConfig, RadioState, RadioError}; +use protocol::{Frame, FrameParser, Command, MAX_FRAME_SIZE}; +use protocol_router::{Protocol, ProtocolRouter, ProtocolDetector, TransportType}; +use meshtastic::{MeshtasticParser, MeshtasticFrame, MeshtasticHandler}; +use rnode::{KissParser, KissFrame, RNodeHandler, KissCommand}; +use ble::{BleManager, ServiceType}; +use session::{SessionManager, Session, SessionParams, SessionError, MessageHeader}; +use onion::{OnionRouter, OnionRoute, RouteHop, OnionPacket, OnionError, RouteBuilder}; +use transport::{WirePacket, AddressTranslator, UniversalAddress}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum CryptoError { + + SessionError, + + EncryptionFailed, + + DecryptionFailed, + + NoSession, + + OnionError, + + InvalidFormat, + + BufferOverflow, +} + +pub enum DecryptResult { + + Plaintext(heapless::Vec), + + Forward { + next_hop: u16, + data: heapless::Vec, + }, +} + +const FIRMWARE_VERSION: &str = "LunarCore v1.0.0"; + +const BAUD_RATE: u32 = 115200; + +const RX_BUFFER_SIZE: usize = 512; + +const DEFAULT_FREQUENCY: u32 = 915_000_000; + +const BATTERY_DIVIDER_RATIO: f32 = 4.9; + +const ADC_VREF_MV: u32 = 3300; + +const ADC_MAX_VALUE: u32 = 4095; + +const BATTERY_LOW_MV: u32 = 3400; + +const BATTERY_CRITICAL_MV: u32 = 3200; + +const WATCHDOG_TIMEOUT_SEC: u32 = 30; + +const LED_BLINK_IDLE: u32 = 2000; +const LED_BLINK_ACTIVE: u32 = 500; +const LED_BLINK_ERROR: u32 = 100; + +const PIN_SPI_MOSI: i32 = 10; +const PIN_SPI_MISO: i32 = 11; +const PIN_SPI_SCK: i32 = 9; + +const PIN_LORA_NSS: i32 = 8; +const PIN_LORA_RST: i32 = 12; +const PIN_LORA_BUSY: i32 = 13; +const PIN_LORA_DIO1: i32 = 14; + +const PIN_LED: i32 = 35; +const PIN_VEXT: i32 = 36; + +const PIN_BATTERY_ADC: i32 = 1; + +const PIN_I2C_SDA: i32 = 17; +const PIN_I2C_SCL: i32 = 18; +const PIN_OLED_RST: i32 = 21; + +static DIO1_TRIGGERED: AtomicBool = AtomicBool::new(false); + +static PACKET_PENDING: AtomicBool = AtomicBool::new(false); + +static TX_COMPLETE: AtomicBool = AtomicBool::new(false); + +static SYSTEM_TICKS: AtomicU32 = AtomicU32::new(0); + +static LAST_ACTIVITY: AtomicU32 = AtomicU32::new(0); + +static ERROR_COUNT: AtomicU32 = AtomicU32::new(0); + +#[derive(Clone)] +struct NodeIdentity { + + node_id: u32, + + mac_address: [u8; 6], + + hardware_serial: [u8; 8], + + public_key: [u8; 32], + + private_key: [u8; 32], +} + +const NVS_NAMESPACE: &str = "lunarcore"; +const NVS_KEY_NODE_ID: &str = "node_id"; +const NVS_KEY_PRIVATE_KEY: &str = "priv_key"; + +impl NodeIdentity { + + fn from_hardware() -> Self { + + let mac_address = Self::read_mac_address(); + + let hardware_serial = Self::read_hardware_serial(); + + let (node_id, private_key) = Self::load_or_create_identity(&hardware_serial); + + let public_key = crypto::ed25519::Ed25519::public_key(&private_key); + + Self { + node_id, + mac_address, + hardware_serial, + public_key, + private_key, + } + } + + fn load_or_create_identity(hardware_serial: &[u8; 8]) -> (u32, [u8; 32]) { + + let nvs_result = unsafe { + let mut handle: esp_idf_sys::nvs_handle_t = 0; + let namespace = core::ffi::CStr::from_bytes_with_nul(b"lunarcore\0").unwrap(); + let err = esp_idf_sys::nvs_open( + namespace.as_ptr(), + esp_idf_sys::nvs_open_mode_t_NVS_READWRITE, + &mut handle, + ); + if err == esp_idf_sys::ESP_OK { + Some(handle) + } else { + + esp_idf_sys::nvs_flash_init(); + let err = esp_idf_sys::nvs_open( + namespace.as_ptr(), + esp_idf_sys::nvs_open_mode_t_NVS_READWRITE, + &mut handle, + ); + if err == esp_idf_sys::ESP_OK { + Some(handle) + } else { + None + } + } + }; + + if let Some(handle) = nvs_result { + + let mut node_id: u32 = 0; + let mut private_key = [0u8; 32]; + let mut key_len: usize = 32; + + let node_id_key = core::ffi::CStr::from_bytes_with_nul(b"node_id\0").unwrap(); + let priv_key_key = core::ffi::CStr::from_bytes_with_nul(b"priv_key\0").unwrap(); + + let has_node_id = unsafe { + esp_idf_sys::nvs_get_u32(handle, node_id_key.as_ptr(), &mut node_id) == esp_idf_sys::ESP_OK + }; + + let has_private_key = unsafe { + esp_idf_sys::nvs_get_blob( + handle, + priv_key_key.as_ptr(), + private_key.as_mut_ptr() as *mut _, + &mut key_len, + ) == esp_idf_sys::ESP_OK && key_len == 32 + }; + + if has_node_id && has_private_key { + + log::info!("Loaded existing node identity from NVS"); + unsafe { esp_idf_sys::nvs_close(handle); } + return (node_id, private_key); + } + + log::info!("Creating new random node identity (privacy-first)"); + + node_id = Self::generate_random_node_id(); + + private_key = Self::generate_random_private_key(hardware_serial); + + unsafe { + esp_idf_sys::nvs_set_u32(handle, node_id_key.as_ptr(), node_id); + esp_idf_sys::nvs_set_blob( + handle, + priv_key_key.as_ptr(), + private_key.as_ptr() as *const _, + 32, + ); + esp_idf_sys::nvs_commit(handle); + esp_idf_sys::nvs_close(handle); + } + + log::info!("Stored new identity in NVS"); + (node_id, private_key) + } else { + + log::warn!("NVS not available, using ephemeral identity"); + let node_id = Self::generate_random_node_id(); + let private_key = Self::generate_random_private_key(hardware_serial); + (node_id, private_key) + } + } + + fn generate_random_node_id() -> u32 { + let mut random_bytes = [0u8; 4]; + unsafe { + esp_idf_sys::esp_fill_random(random_bytes.as_mut_ptr() as *mut _, 4); + } + + let id = u32::from_le_bytes(random_bytes); + id | 0x80000000 + } + + fn generate_random_private_key(hardware_serial: &[u8; 8]) -> [u8; 32] { + let mut random_bytes = [0u8; 32]; + unsafe { + esp_idf_sys::esp_fill_random(random_bytes.as_mut_ptr() as *mut _, 32); + } + + let mut seed_input = [0u8; 40]; + seed_input[0..32].copy_from_slice(&random_bytes); + seed_input[32..40].copy_from_slice(hardware_serial); + + let mut private_key = Sha256::hash(&seed_input); + + private_key[0] &= 248; + private_key[31] &= 127; + private_key[31] |= 64; + + private_key + } + + #[allow(dead_code)] + fn factory_reset() -> Option { + + unsafe { + let namespace = core::ffi::CStr::from_bytes_with_nul(b"lunarcore\0").unwrap(); + esp_idf_sys::nvs_flash_erase_partition(namespace.as_ptr()); + } + + log::info!("Factory reset: erased old identity, generating new one"); + + Some(Self::from_hardware()) + } + + fn read_mac_address() -> [u8; 6] { + let mut mac = [0u8; 6]; + + unsafe { + + esp_idf_sys::esp_efuse_mac_get_default(mac.as_mut_ptr()); + } + + mac + } + + fn read_hardware_serial() -> [u8; 8] { + let mut serial = [0u8; 8]; + + unsafe { + + let efuse_base: *const u32 = 0x6001A044 as *const u32; + let word0 = core::ptr::read_volatile(efuse_base); + let word1 = core::ptr::read_volatile(efuse_base.add(1)); + + serial[0..4].copy_from_slice(&word0.to_le_bytes()); + serial[4..8].copy_from_slice(&word1.to_le_bytes()); + } + + serial + } + + fn x25519_pubkey(&self) -> [u8; 32] { + use crypto::x25519; + x25519::x25519_base(&self.private_key) + } +} + +struct BatteryState { + + voltage_mv: u32, + + percentage: u8, + + is_charging: bool, + + is_low: bool, + + is_critical: bool, +} + +impl BatteryState { + fn new() -> Self { + Self { + voltage_mv: 0, + percentage: 0, + is_charging: false, + is_low: false, + is_critical: false, + } + } + + fn update(&mut self, adc_value: u32) { + + let vadc_mv = (adc_value * ADC_VREF_MV) / ADC_MAX_VALUE; + self.voltage_mv = ((vadc_mv as f32) * BATTERY_DIVIDER_RATIO) as u32; + + self.percentage = Self::voltage_to_percentage(self.voltage_mv); + + self.is_low = self.voltage_mv < BATTERY_LOW_MV; + self.is_critical = self.voltage_mv < BATTERY_CRITICAL_MV; + + self.is_charging = self.voltage_mv > 4200; + } + + fn voltage_to_percentage(mv: u32) -> u8 { + + const CURVE: [(u32, u8); 11] = [ + (4200, 100), + (4100, 90), + (4000, 80), + (3900, 70), + (3800, 60), + (3700, 50), + (3600, 40), + (3500, 30), + (3400, 20), + (3300, 10), + (3000, 0), + ]; + + if mv >= CURVE[0].0 { + return 100; + } + if mv <= CURVE[10].0 { + return 0; + } + + for i in 0..10 { + if mv >= CURVE[i + 1].0 && mv <= CURVE[i].0 { + let v_range = CURVE[i].0 - CURVE[i + 1].0; + let p_range = CURVE[i].1 - CURVE[i + 1].1; + let v_offset = mv - CURVE[i + 1].0; + + return CURVE[i + 1].1 + ((v_offset * p_range as u32) / v_range) as u8; + } + } + + 50 + } +} + +struct Stats { + + tx_packets: u32, + + rx_packets: u32, + + tx_errors: u32, + + rx_errors: u32, + + protocol_switches: u32, + + tx_bytes: u64, + + rx_bytes: u64, + + uptime_seconds: u32, + + last_rssi: i16, + + last_snr: i8, + + airtime_ms: u64, +} + +impl Stats { + fn new() -> Self { + Self { + tx_packets: 0, + rx_packets: 0, + tx_errors: 0, + rx_errors: 0, + protocol_switches: 0, + tx_bytes: 0, + rx_bytes: 0, + uptime_seconds: 0, + last_rssi: 0, + last_snr: 0, + airtime_ms: 0, + } + } + + fn record_rx(&mut self, len: usize, rssi: i16, snr: i8) { + self.rx_packets += 1; + self.rx_bytes += len as u64; + self.last_rssi = rssi; + self.last_snr = snr; + } + + fn record_tx(&mut self, len: usize, airtime_ms: u32) { + self.tx_packets += 1; + self.tx_bytes += len as u64; + self.airtime_ms += airtime_ms as u64; + } +} + +struct LedController { + + is_on: bool, + + last_toggle: u32, + + interval_ms: u32, + + blink_count: u8, + + remaining: u8, +} + +impl LedController { + fn new() -> Self { + Self { + is_on: false, + last_toggle: 0, + interval_ms: LED_BLINK_IDLE, + blink_count: 0, + remaining: 0, + } + } + + fn set_idle(&mut self) { + self.interval_ms = LED_BLINK_IDLE; + self.blink_count = 0; + } + + fn set_active(&mut self) { + self.interval_ms = LED_BLINK_ACTIVE; + self.blink_count = 0; + } + + fn set_error(&mut self) { + self.interval_ms = LED_BLINK_ERROR; + self.blink_count = 0; + } + + fn flash(&mut self, count: u8) { + self.blink_count = count; + self.remaining = count * 2; + self.interval_ms = 100; + } + + fn update(&mut self, current_time: u32) -> bool { + if current_time.wrapping_sub(self.last_toggle) >= self.interval_ms { + self.last_toggle = current_time; + + if self.blink_count > 0 { + if self.remaining > 0 { + self.remaining -= 1; + self.is_on = !self.is_on; + return true; + } else { + + self.set_idle(); + } + } else { + self.is_on = !self.is_on; + return true; + } + } + false + } +} + +struct LunarCore { + + radio: Sx1262, + + router: ProtocolRouter, + + meshcore_parser: FrameParser, + + meshtastic: MeshtasticHandler, + + rnode: RNodeHandler, + + ble: BleManager, + + stats: Stats, + + identity: NodeIdentity, + + battery: BatteryState, + + led: LedController, + + rx_active: bool, + + serial_protocol: Protocol, + + vext_enabled: bool, + + last_battery_check: u32, + + at_buffer: Vec, + + session_manager: SessionManager, + + onion_router: OnionRouter, + + route_builder: RouteBuilder, + + our_address: UniversalAddress, +} + +impl LunarCore +where + SPI: embedded_hal::spi::SpiDevice, + NSS: embedded_hal::digital::OutputPin, + RESET: embedded_hal::digital::OutputPin, + BUSY: embedded_hal::digital::InputPin, + DIO1: embedded_hal::digital::InputPin, +{ + fn new(radio: Sx1262, identity: NodeIdentity) -> Self { + let node_id = identity.node_id; + + let x25519_private = { + let mut key = identity.private_key; + + key[0] &= 248; + key[31] &= 127; + key[31] |= 64; + key + }; + + let onion_router = OnionRouter::new(x25519_private); + + let our_address = AddressTranslator::from_public_key(&identity.public_key); + + Self { + radio, + router: ProtocolRouter::new(), + meshcore_parser: FrameParser::new(), + meshtastic: MeshtasticHandler::new(node_id), + rnode: RNodeHandler::new(), + ble: BleManager::new(), + stats: Stats::new(), + identity, + battery: BatteryState::new(), + led: LedController::new(), + rx_active: false, + serial_protocol: Protocol::Unknown, + vext_enabled: true, + last_battery_check: 0, + at_buffer: Vec::new(), + + session_manager: SessionManager::new(), + onion_router, + route_builder: RouteBuilder::new(), + our_address, + } + } + + fn identity(&self) -> &NodeIdentity { + &self.identity + } + + fn update_battery(&mut self, adc_value: u32) { + self.battery.update(adc_value); + + if self.battery.is_critical { + self.led.set_error(); + log::warn!("Battery critical: {}mV", self.battery.voltage_mv); + } else if self.battery.is_low { + log::info!("Battery low: {}mV ({}%)", self.battery.voltage_mv, self.battery.percentage); + } + } + + fn handle_dio1_interrupt(&mut self) { + + DIO1_TRIGGERED.store(false, Ordering::Relaxed); + + if let Ok(irq_status) = self.radio.get_irq_status() { + if irq_status & 0x01 != 0 { + + TX_COMPLETE.store(true, Ordering::Release); + LAST_ACTIVITY.store(SYSTEM_TICKS.load(Ordering::Relaxed), Ordering::Relaxed); + } + if irq_status & 0x02 != 0 { + + PACKET_PENDING.store(true, Ordering::Release); + LAST_ACTIVITY.store(SYSTEM_TICKS.load(Ordering::Relaxed), Ordering::Relaxed); + } + if irq_status & 0x04 != 0 { + + } + if irq_status & 0x40 != 0 { + + self.stats.rx_errors += 1; + ERROR_COUNT.fetch_add(1, Ordering::Relaxed); + } + + let _ = self.radio.clear_irq(irq_status); + } + } + + fn process_radio_events(&mut self) { + + if TX_COMPLETE.swap(false, Ordering::Acquire) { + + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + self.led.flash(1); + } + + if PACKET_PENDING.swap(false, Ordering::Acquire) { + self.led.flash(2); + } + } + + fn process_at_command(&mut self, uart: &UartDriver) { + + let mut cmd_upper = [0u8; 128]; + let len = self.at_buffer.len().min(128); + for i in 0..len { + cmd_upper[i] = self.at_buffer[i].to_ascii_uppercase(); + } + let cmd = &cmd_upper[..len]; + + let _ = uart.write(b"\r\n"); + + if cmd.starts_with(b"AT+VERSION") || cmd.starts_with(b"ATI") { + + let _ = uart.write(b"LunarCore v1.0.0\r\n"); + let _ = uart.write(b"Unified Mesh Bridge Firmware\r\n"); + let _ = uart.write(b"Protocols: MeshCore, Meshtastic, RNode/KISS\r\n"); + let _ = uart.write(b"OK\r\n"); + } else if cmd.starts_with(b"AT+STATUS") { + + let _ = uart.write(b"Status: "); + if self.rx_active { + let _ = uart.write(b"RX Active\r\n"); + } else { + let _ = uart.write(b"Idle\r\n"); + } + + let mut buf = [0u8; 32]; + let s = format_battery(&self.battery, &mut buf); + let _ = uart.write(s.as_bytes()); + let _ = uart.write(b"\r\n"); + + let _ = uart.write(b"TX: "); + write_u32(uart, self.stats.tx_packets); + let _ = uart.write(b" RX: "); + write_u32(uart, self.stats.rx_packets); + let _ = uart.write(b"\r\n"); + + let _ = uart.write(b"OK\r\n"); + } else if cmd.starts_with(b"AT+NODEID") { + + let _ = uart.write(b"Node ID: "); + write_hex32(uart, self.identity.node_id); + let _ = uart.write(b"\r\n"); + let _ = uart.write(b"OK\r\n"); + } else if cmd.starts_with(b"AT+MAC") { + + let _ = uart.write(b"MAC: "); + for (i, &b) in self.identity.mac_address.iter().enumerate() { + write_hex8(uart, b); + if i < 5 { + let _ = uart.write(b":"); + } + } + let _ = uart.write(b"\r\n"); + let _ = uart.write(b"OK\r\n"); + } else if cmd.starts_with(b"AT+FREQ=") { + + if let Some(freq) = parse_u32_from_cmd(&cmd[8..]) { + log::info!("Setting frequency to {} Hz", freq); + let mut config = self.radio.config.clone(); + config.frequency = freq; + if self.radio.configure(&config).is_ok() { + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR\r\n"); + } + } else { + let _ = uart.write(b"ERROR: Invalid frequency\r\n"); + } + } else if cmd.starts_with(b"AT+SF=") { + + if let Some(sf) = parse_u32_from_cmd(&cmd[6..]) { + if sf >= 7 && sf <= 12 { + log::info!("Setting SF to {}", sf); + let mut config = self.radio.config.clone(); + config.spreading_factor = sf as u8; + if self.radio.configure(&config).is_ok() { + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR\r\n"); + } + } else { + let _ = uart.write(b"ERROR: SF must be 7-12\r\n"); + } + } else { + let _ = uart.write(b"ERROR: Invalid SF\r\n"); + } + } else if cmd.starts_with(b"AT+TXPOWER=") { + + if let Some(power) = parse_i8_from_cmd(&cmd[11..]) { + if power >= -9 && power <= 22 { + log::info!("Setting TX power to {} dBm", power); + let mut config = self.radio.config.clone(); + config.tx_power = power; + if self.radio.configure(&config).is_ok() { + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR\r\n"); + } + } else { + let _ = uart.write(b"ERROR: Power must be -9 to +22\r\n"); + } + } else { + let _ = uart.write(b"ERROR: Invalid power\r\n"); + } + } else if cmd.starts_with(b"AT+RESET") { + + if self.radio.init().is_ok() { + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR\r\n"); + } + } else if cmd.starts_with(b"AT+RX") { + + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR\r\n"); + } + } else if cmd.starts_with(b"AT+RSSI") { + + if let Ok(rssi) = self.radio.get_rssi() { + let _ = uart.write(b"RSSI: "); + write_i16(uart, rssi); + let _ = uart.write(b" dBm\r\n"); + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR\r\n"); + } + } else if cmd == b"AT" { + + let _ = uart.write(b"OK\r\n"); + } else if cmd.starts_with(b"AT+HELP") || cmd.starts_with(b"AT?") { + + let _ = uart.write(b"Available commands:\r\n"); + let _ = uart.write(b" AT - Test\r\n"); + let _ = uart.write(b" ATI - Version info\r\n"); + let _ = uart.write(b" AT+STATUS - System status\r\n"); + let _ = uart.write(b" AT+NODEID - Node ID\r\n"); + let _ = uart.write(b" AT+MAC - MAC address\r\n"); + let _ = uart.write(b" AT+FREQ=Hz - Set frequency\r\n"); + let _ = uart.write(b" AT+SF=n - Set spreading factor\r\n"); + let _ = uart.write(b" AT+TXPOWER=n - Set TX power\r\n"); + let _ = uart.write(b" AT+RX - Start RX mode\r\n"); + let _ = uart.write(b" AT+RSSI - Get RSSI\r\n"); + let _ = uart.write(b" AT+RESET - Reset radio\r\n"); + let _ = uart.write(b"OK\r\n"); + } else { + let _ = uart.write(b"ERROR: Unknown command\r\n"); + } + } + + fn process_serial_byte(&mut self, byte: u8, uart: &UartDriver) { + + if self.serial_protocol == Protocol::Unknown { + if let Some(protocol) = self.router.transport(TransportType::UsbSerial) + .detector.feed(byte) + { + self.serial_protocol = protocol; + self.stats.protocol_switches += 1; + log::info!("Protocol detected: {}", protocol.name()); + + self.configure_radio_for_protocol(protocol); + } + } + + match self.serial_protocol { + Protocol::MeshCore => { + if let Some(frame) = self.meshcore_parser.feed(byte) { + self.handle_meshcore_frame(&frame, uart); + self.router.transport(TransportType::UsbSerial) + .detector.confirm_frame(); + } + } + + Protocol::Meshtastic => { + if let Some(frame) = self.meshtastic.feed_serial(byte) { + self.handle_meshtastic_frame(&frame, uart); + self.router.transport(TransportType::UsbSerial) + .detector.confirm_frame(); + } + } + + Protocol::RNode => { + if let Some(frame) = self.rnode.feed_serial(byte) { + self.handle_rnode_frame(&frame, uart); + self.router.transport(TransportType::UsbSerial) + .detector.confirm_frame(); + } + } + + Protocol::AtCommand => { + + if byte == b'\r' || byte == b'\n' { + if !self.at_buffer.is_empty() { + self.process_at_command(uart); + self.at_buffer.clear(); + } + } else if byte >= 0x20 && byte < 0x7F { + + let _ = self.at_buffer.push(byte); + } + } + + Protocol::Unknown => { + + } + } + } + + fn configure_radio_for_protocol(&mut self, protocol: Protocol) { + let config = match protocol { + Protocol::MeshCore => RadioConfig { + frequency: 915_000_000, + spreading_factor: 9, + bandwidth: 0, + coding_rate: 1, + tx_power: 14, + sync_word: 0x12, + preamble_length: 8, + crc_enabled: true, + implicit_header: false, + ldro: false, + }, + Protocol::Meshtastic => RadioConfig { + frequency: 906_875_000, + spreading_factor: 11, + bandwidth: 0, + coding_rate: 1, + tx_power: 17, + sync_word: 0x2B, + preamble_length: 16, + crc_enabled: true, + implicit_header: false, + ldro: true, + }, + Protocol::RNode => { + + let cfg = self.rnode.config(); + RadioConfig { + frequency: cfg.frequency, + spreading_factor: cfg.spreading_factor, + bandwidth: match cfg.bandwidth { + 125_000 => 0, + 250_000 => 1, + 500_000 => 2, + _ => 0, + }, + coding_rate: cfg.coding_rate.saturating_sub(4), + tx_power: cfg.tx_power, + sync_word: 0x12, + preamble_length: 8, + crc_enabled: true, + implicit_header: false, + ldro: cfg.spreading_factor >= 11, + } + } + _ => return, + }; + + if let Err(e) = self.radio.configure(&config) { + log::error!("Failed to configure radio: {:?}", e); + } + + self.router.set_lora_protocol(protocol); + } + + fn handle_meshcore_frame(&mut self, frame: &Frame, uart: &UartDriver) { + match frame.command { + Command::Ping => { + let response = protocol::build_pong(frame.sequence); + self.send_frame(uart, &response); + } + + Command::Configure => { + if let Some(config) = protocol::parse_config(&frame.data) { + match self.radio.configure(&config) { + Ok(()) => { + let response = protocol::build_config_ack(frame.sequence); + self.send_frame(uart, &response); + } + Err(_) => { + if let Some(response) = protocol::build_error(frame.sequence, "Config failed") { + self.send_frame(uart, &response); + } + } + } + } + } + + Command::Transmit => { + self.rx_active = false; + match self.radio.transmit(&frame.data) { + Ok(()) => { + self.stats.tx_packets += 1; + let response = protocol::build_tx_done(frame.sequence); + self.send_frame(uart, &response); + let _ = self.radio.start_rx(0); + self.rx_active = true; + } + Err(e) => { + self.stats.tx_errors += 1; + let code = match e { + RadioError::TxTimeout => 1, + RadioError::BusyTimeout => 2, + _ => 255, + }; + if let Some(response) = protocol::build_tx_error(frame.sequence, code) { + self.send_frame(uart, &response); + } + } + } + } + + Command::Version => { + if let Some(response) = protocol::build_version_response( + frame.sequence, + FIRMWARE_VERSION, + ) { + self.send_frame(uart, &response); + } + } + + Command::GetStats => { + if let Some(response) = protocol::build_stats_response( + frame.sequence, + self.stats.tx_packets, + self.stats.rx_packets, + self.stats.tx_errors, + self.stats.rx_errors, + ) { + self.send_frame(uart, &response); + } + } + + Command::Reset => { + let _ = self.radio.init(); + self.rx_active = false; + let response = protocol::build_pong(frame.sequence); + self.send_frame(uart, &response); + } + + _ => { + if let Some(response) = protocol::build_error(frame.sequence, "Unknown command") { + self.send_frame(uart, &response); + } + } + } + + if !self.rx_active && self.radio.state() == RadioState::Standby { + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + } + } + + fn handle_meshtastic_frame(&mut self, frame: &MeshtasticFrame, uart: &UartDriver) { + + match self.meshtastic.process_toradio(frame) { + Some(meshtastic::ToRadioResponse::LoRaPacket(tx_data)) => { + + self.rx_active = false; + match self.radio.transmit(&tx_data) { + Ok(()) => { + self.stats.tx_packets += 1; + let _ = self.radio.start_rx(0); + self.rx_active = true; + } + Err(_) => { + self.stats.tx_errors += 1; + } + } + } + + Some(meshtastic::ToRadioResponse::FromRadio(response)) => { + + self.send_meshtastic_response(&response, uart); + + self.flush_meshtastic_responses(uart); + } + + None => { + + } + } + + if !self.rx_active { + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + } + } + + fn send_meshtastic_response(&mut self, response: &[u8], uart: &UartDriver) { + + if let Some(serial_frame) = self.meshtastic.build_serial_frame(response) { + let _ = uart.write(&serial_frame); + } + + let _ = self.ble.queue_from_radio(response); + + self.meshtastic.rx_count = self.meshtastic.rx_count.wrapping_add(1); + let _ = self.ble.notify_from_num(self.meshtastic.rx_count); + } + + fn flush_meshtastic_responses(&mut self, uart: &UartDriver) { + + while let Some(meshtastic::ToRadioResponse::FromRadio(response)) = + self.meshtastic.poll_pending_response() + { + self.send_meshtastic_response(&response, uart); + + FreeRtos::delay_ms(10); + } + } + + fn handle_rnode_frame(&mut self, frame: &KissFrame, uart: &UartDriver) { + + if let Some(response) = self.rnode.process_frame(frame) { + + let encoded = response.encode(); + let _ = uart.write(&encoded); + } + + if frame.command == KissCommand::DataFrame as u8 { + if let Some(tx_data) = self.rnode.get_tx_data(frame) { + self.rx_active = false; + match self.radio.transmit(tx_data) { + Ok(()) => { + self.stats.tx_packets += 1; + let _ = self.radio.start_rx(0); + self.rx_active = true; + } + Err(_) => { + self.stats.tx_errors += 1; + } + } + } + } + + if self.rnode.is_online() && !self.rx_active { + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + } + } + + fn check_rx(&mut self, uart: &UartDriver) { + if !self.rx_active { + return; + } + + match self.radio.check_rx() { + Ok(Some((data, rssi, snr))) => { + self.stats.rx_packets += 1; + self.route_rx_packet(&data, rssi, snr, uart); + } + Ok(None) => { + + } + Err(RadioError::RxTimeout) => { + let _ = self.radio.start_rx(0); + } + Err(RadioError::CrcError) => { + self.stats.rx_errors += 1; + let _ = self.radio.start_rx(0); + } + Err(_) => { + self.stats.rx_errors += 1; + } + } + } + + fn route_rx_packet(&mut self, data: &[u8], rssi: i16, snr: i8, uart: &UartDriver) { + match self.serial_protocol { + Protocol::MeshCore => { + if let Some(frame) = protocol::build_receive(rssi, snr, data) { + self.send_frame(uart, &frame); + } + } + + Protocol::Meshtastic => { + if let Some(packet) = self.meshtastic.process_lora_packet(data, rssi as i32, snr as f32) { + if let Some(from_radio) = meshtastic::encode_fromradio_packet(&packet) { + + if let Some(serial_frame) = self.meshtastic.build_serial_frame(&from_radio) { + let _ = uart.write(&serial_frame); + } + + let _ = self.ble.notify_from_num(self.meshtastic.rx_count); + } + } + } + + Protocol::RNode => { + let frame = self.rnode.process_lora_packet(data, rssi, snr); + let encoded = frame.encode(); + let _ = uart.write(&encoded); + } + + _ => { + + } + } + } + + fn send_frame(&self, uart: &UartDriver, frame: &Frame) { + let encoded = frame.encode(); + let _ = uart.write(&encoded); + } + + fn encrypt_for_tx( + &mut self, + plaintext: &[u8], + recipient_public: &[u8; 32], + use_onion: bool, + ) -> Result, CryptoError> { + + let session = match self.session_manager.get_session(recipient_public) { + Some(s) => s, + None => { + + let our_x25519 = crypto::x25519::x25519_base(&self.identity.private_key); + let shared = crypto::x25519::x25519(&self.identity.private_key, recipient_public); + + let params = SessionParams { + shared_secret: shared, + our_private: self.identity.private_key, + their_public: *recipient_public, + is_initiator: true, + }; + + self.session_manager.create_session(params); + + self.session_manager.get_session(recipient_public) + .ok_or(CryptoError::SessionError)? + } + }; + + let (header, ciphertext) = session.encrypt(plaintext) + .map_err(|_| CryptoError::EncryptionFailed)?; + + let mut session_encrypted = Vec::::new(); + session_encrypted.extend_from_slice(&header.encode()) + .map_err(|_| CryptoError::BufferOverflow)?; + session_encrypted.extend_from_slice(&ciphertext) + .map_err(|_| CryptoError::BufferOverflow)?; + + let payload_for_wire = if use_onion && self.route_builder.relay_count() >= 3 { + + let dest_hint = AddressTranslator::from_public_key(recipient_public); + let dest_hop = RouteHop { + hint: dest_hint.meshcore_addr, + public_key: *recipient_public, + }; + + if let Some(route) = self.route_builder.build_route(dest_hop, 3) { + match self.onion_router.wrap(&session_encrypted, &route) { + Ok(onion_packet) => onion_packet.data, + Err(_) => session_encrypted, + } + } else { + session_encrypted + } + } else { + session_encrypted + }; + + let dest_address = AddressTranslator::from_public_key(recipient_public); + let epoch = (millis() / 1000) as u64; + let session_hint_bytes = session.derive_session_hint(epoch); + + let session_hint_u32 = ((session_hint_bytes[0] as u32) << 24) + | ((session_hint_bytes[1] as u32) << 16) + | ((session_hint_bytes[2] as u32) << 8) + | (session_hint_bytes[3] as u32); + + let mut payload_truncated = Vec::::new(); + let copy_len = core::cmp::min(payload_for_wire.len(), 214); + let _ = payload_truncated.extend_from_slice(&payload_for_wire[..copy_len]); + + let wire_packet = WirePacket::new_data( + dest_address.meshcore_addr, + session_hint_u32, + &payload_truncated, + ).ok_or(CryptoError::BufferOverflow)?; + + let mut output = Vec::::new(); + output.extend_from_slice(&wire_packet.encode()) + .map_err(|_| CryptoError::BufferOverflow)?; + + Ok(output) + } + + fn decrypt_from_rx( + &mut self, + wire_data: &[u8], + sender_public: &[u8; 32], + ) -> Result { + + let wire_packet = WirePacket::decode(wire_data) + .ok_or(CryptoError::InvalidFormat)?; + + let is_for_us = wire_packet.next_hop_hint == self.our_address.meshcore_addr; + + if !is_for_us { + + let mut expanded_payload = heapless::Vec::::new(); + let _ = expanded_payload.extend_from_slice(&wire_packet.payload); + + let onion_packet = OnionPacket { + data: expanded_payload, + num_layers: (wire_data.len() / 18).min(7) as u8, + }; + + match self.onion_router.unwrap(&onion_packet, sender_public) { + Ok((next_hint, inner_packet)) => { + + let mut truncated = heapless::Vec::::new(); + let copy_len = core::cmp::min(inner_packet.data.len(), 214); + let _ = truncated.extend_from_slice(&inner_packet.data[..copy_len]); + + if let Some(forward_packet) = WirePacket::new_data( + next_hint, + wire_packet.session_hint, + &truncated, + ) { + let mut encoded = Vec::::new(); + let _ = encoded.extend_from_slice(&forward_packet.encode()); + + return Ok(DecryptResult::Forward { + next_hop: next_hint, + data: encoded, + }); + } + return Err(CryptoError::BufferOverflow); + } + Err(OnionError::NoMoreLayers) => { + + } + Err(_) => { + return Err(CryptoError::OnionError); + } + } + } + + let mut session_encrypted = heapless::Vec::::new(); + let _ = session_encrypted.extend_from_slice(&wire_packet.payload); + + if session_encrypted.len() < 48 { + return Err(CryptoError::InvalidFormat); + } + + let header = MessageHeader::decode(&session_encrypted[..48]) + .ok_or(CryptoError::InvalidFormat)?; + let ciphertext = &session_encrypted[48..]; + + let session = self.session_manager.get_session(sender_public) + .ok_or(CryptoError::NoSession)?; + + let plaintext = session.decrypt(&header, ciphertext) + .map_err(|_| CryptoError::DecryptionFailed)?; + + Ok(DecryptResult::Plaintext(plaintext)) + } + + fn add_relay(&mut self, hint: u16, public_key: [u8; 32]) { + self.route_builder.add_relay(hint, public_key); + } + + fn relay_count(&self) -> usize { + self.route_builder.relay_count() + } +} + +fn dio1_isr() { + DIO1_TRIGGERED.store(true, Ordering::Release); +} + +fn timer_tick_isr() { + SYSTEM_TICKS.fetch_add(1, Ordering::Relaxed); +} + +fn millis() -> u32 { + SYSTEM_TICKS.load(Ordering::Relaxed) +} + +fn init_watchdog() { + unsafe { + + let config = esp_idf_sys::esp_task_wdt_config_t { + timeout_ms: WATCHDOG_TIMEOUT_SEC * 1000, + idle_core_mask: 0, + trigger_panic: true, + }; + esp_idf_sys::esp_task_wdt_init(&config); + esp_idf_sys::esp_task_wdt_add(core::ptr::null_mut()); + } +} + +fn feed_watchdog() { + unsafe { + esp_idf_sys::esp_task_wdt_reset(); + } +} + +fn format_battery<'a>(battery: &BatteryState, buf: &'a mut [u8; 32]) -> &'a str { + let mut idx = 0; + + let prefix = b"Battery: "; + buf[idx..idx + prefix.len()].copy_from_slice(prefix); + idx += prefix.len(); + + idx += write_u32_to_buf(battery.voltage_mv, &mut buf[idx..]); + + let suffix = b"mV ("; + buf[idx..idx + suffix.len()].copy_from_slice(suffix); + idx += suffix.len(); + + idx += write_u32_to_buf(battery.percentage as u32, &mut buf[idx..]); + + let end = b"%)"; + buf[idx..idx + end.len()].copy_from_slice(end); + idx += end.len(); + + core::str::from_utf8(&buf[..idx]).unwrap_or("Battery: ???") +} + +fn write_u32_to_buf(val: u32, buf: &mut [u8]) -> usize { + if val == 0 { + buf[0] = b'0'; + return 1; + } + + let mut v = val; + let mut digits = [0u8; 10]; + let mut count = 0; + + while v > 0 { + digits[count] = b'0' + (v % 10) as u8; + v /= 10; + count += 1; + } + + for i in 0..count { + buf[i] = digits[count - 1 - i]; + } + + count +} + +fn write_u32(uart: &UartDriver, val: u32) { + let mut buf = [0u8; 10]; + let len = write_u32_to_buf(val, &mut buf); + let _ = uart.write(&buf[..len]); +} + +fn write_i16(uart: &UartDriver, val: i16) { + if val < 0 { + let _ = uart.write(b"-"); + write_u32(uart, (-val) as u32); + } else { + write_u32(uart, val as u32); + } +} + +fn write_hex8(uart: &UartDriver, val: u8) { + const HEX: &[u8] = b"0123456789ABCDEF"; + let buf = [HEX[(val >> 4) as usize], HEX[(val & 0xF) as usize]]; + let _ = uart.write(&buf); +} + +fn write_hex32(uart: &UartDriver, val: u32) { + const HEX: &[u8] = b"0123456789ABCDEF"; + let mut buf = [0u8; 8]; + for i in 0..8 { + buf[7 - i] = HEX[((val >> (i * 4)) & 0xF) as usize]; + } + let _ = uart.write(&buf); +} + +fn parse_u32_from_cmd(bytes: &[u8]) -> Option { + let mut result: u32 = 0; + let mut found_digit = false; + + for &b in bytes { + if b >= b'0' && b <= b'9' { + result = result.checked_mul(10)?.checked_add((b - b'0') as u32)?; + found_digit = true; + } else if found_digit { + break; + } + } + + if found_digit { Some(result) } else { None } +} + +fn parse_i8_from_cmd(bytes: &[u8]) -> Option { + let mut result: i32 = 0; + let mut negative = false; + let mut found_digit = false; + let mut started = false; + + for &b in bytes { + if b == b'-' && !started { + negative = true; + started = true; + } else if b >= b'0' && b <= b'9' { + result = result.checked_mul(10)?.checked_add((b - b'0') as i32)?; + found_digit = true; + started = true; + } else if found_digit { + break; + } + } + + if found_digit { + let val = if negative { -result } else { result }; + if val >= -128 && val <= 127 { + Some(val as i8) + } else { + None + } + } else { + None + } +} + +fn main() -> ! { + + esp_idf_sys::link_patches(); + + esp_idf_svc::log::EspLogger::initialize_default(); + + log::info!("========================================"); + log::info!(" LunarCore Mesh Firmware v1.0.0"); + log::info!(" Pure Rust Cryptography Stack"); + log::info!(" Cypherpunk / Lunarpunk Design"); + log::info!("========================================"); + + run_lunarcore(); +} + +fn run_lunarcore() -> ! { + + let identity = NodeIdentity::from_hardware(); + log::info!("[INIT] Node ID: {:08X}", identity.node_id); + + let peripherals = Peripherals::take().unwrap(); + + let mut led_pin = PinDriver::output(peripherals.pins.gpio35).unwrap(); + led_pin.set_low().unwrap(); + let mut vext_pin = PinDriver::output(peripherals.pins.gpio36).unwrap(); + vext_pin.set_low().unwrap(); + drop(vext_pin); + log::info!("[INIT] GPIO OK"); + + let mut oled_rst = PinDriver::output(peripherals.pins.gpio21).unwrap(); + oled_rst.set_low().unwrap(); + FreeRtos::delay_ms(10); + oled_rst.set_high().unwrap(); + FreeRtos::delay_ms(10); + let i2c_config = I2cConfig::new().baudrate(Hertz(400_000)); + let i2c = I2cDriver::new( + peripherals.i2c0, + peripherals.pins.gpio17, + peripherals.pins.gpio18, + &i2c_config, + ).unwrap(); + let mut status_display = StatusDisplay::new(i2c); + let _ = status_display.init(); + log::info!("[INIT] OLED OK"); + + let _ = status_display.boot_animation(&mut |ms| FreeRtos::delay_ms(ms)); + + let spi_config = esp_idf_hal::spi::config::Config::new() + .baudrate(Hertz(8_000_000)) + .data_mode(embedded_hal::spi::MODE_0); + let spi = SpiDeviceDriver::new_single( + peripherals.spi2, + peripherals.pins.gpio9, + peripherals.pins.gpio10, + Some(peripherals.pins.gpio11), + Option::::None, + &SpiDriverConfig::default(), + &spi_config, + ).unwrap(); + let nss = PinDriver::output(peripherals.pins.gpio8).unwrap(); + let reset = PinDriver::output(peripherals.pins.gpio12).unwrap(); + let busy = PinDriver::input(peripherals.pins.gpio13).unwrap(); + let mut dio1 = PinDriver::input(peripherals.pins.gpio14).unwrap(); + dio1.set_pull(Pull::Down).unwrap(); + log::info!("[INIT] SPI OK"); + + unsafe { + dio1.subscribe(dio1_isr).unwrap(); + } + dio1.enable_interrupt().unwrap(); + + let mut radio = Sx1262::new(spi, nss, reset, busy, dio1); + match radio.init() { + Ok(()) => log::info!("[INIT] SX1262 OK"), + Err(e) => log::error!("[INIT] SX1262 FAILED: {:?}", e), + } + + let mut lunarcore = LunarCore::new(radio, identity); + log::info!("[INIT] LunarCore OK"); + + match lunarcore.ble.init("LunarCore") { + Ok(()) => { + log::info!("[INIT] BLE OK"); + let _ = lunarcore.ble.start_advertising(); + } + Err(e) => log::error!("[INIT] BLE FAILED: {:?}", e), + } + + let uart_config = esp_idf_hal::uart::config::Config::default() + .baudrate(Hertz(BAUD_RATE)); + let uart = UartDriver::new( + peripherals.uart0, + peripherals.pins.gpio43, + peripherals.pins.gpio44, + Option::::None, + Option::::None, + &uart_config, + ).unwrap(); + log::info!("[INIT] UART OK"); + + init_watchdog(); + + let adc1 = AdcDriver::new(peripherals.adc1).unwrap(); + let adc_config = AdcChannelConfig { + attenuation: DB_11, + ..Default::default() + }; + let mut battery_channel = AdcChannelDriver::new(&adc1, peripherals.pins.gpio1, &adc_config).unwrap(); + log::info!("[INIT] ADC OK"); + + log::info!("========================================"); + log::info!(" Protocols: MeshCore, Meshtastic, KISS"); + log::info!(" Waiting for protocol detection..."); + log::info!("========================================"); + + let mut last_second = 0u32; + let mut battery_check_interval = 0u32; + + loop { + let now = millis(); + + feed_watchdog(); + + if DIO1_TRIGGERED.load(Ordering::Acquire) { + lunarcore.handle_dio1_interrupt(); + } + + lunarcore.process_radio_events(); + + let mut byte = [0u8; 1]; + while uart.read(&mut byte, 0).unwrap_or(0) > 0 { + lunarcore.process_serial_byte(byte[0], &uart); + LAST_ACTIVITY.store(now, Ordering::Relaxed); + } + + lunarcore.check_rx(&uart); + + if lunarcore.led.update(now) { + if lunarcore.led.is_on { + let _ = led_pin.set_high(); + } else { + let _ = led_pin.set_low(); + } + } + + if now.wrapping_sub(battery_check_interval) >= 10_000 { + battery_check_interval = now; + if let Ok(adc_value) = adc1.read(&mut battery_channel) { + lunarcore.update_battery(adc_value as u32); + } + } + + let current_second = now / 1000; + if current_second > last_second { + last_second = current_second; + lunarcore.stats.uptime_seconds = current_second; + + if current_second % 60 == 0 { + log::info!("Uptime: {}s, RX: {}, TX: {}", + current_second, + lunarcore.stats.rx_packets, + lunarcore.stats.tx_packets); + } + } + + FreeRtos::delay_ms(1); + } +} diff --git a/src/meshtastic/channel.rs b/src/meshtastic/channel.rs new file mode 100644 index 0000000..34dfafc --- /dev/null +++ b/src/meshtastic/channel.rs @@ -0,0 +1,553 @@ +use heapless::Vec; +use crate::crypto::aes::{Aes128, Aes256}; +use crate::crypto::sha256::Sha256; +use crate::crypto::hkdf::meshtastic as mesh_kdf; + +pub const MAX_CHANNEL_NAME: usize = 12; + +pub const KEY_SIZE_128: usize = 16; + +pub const KEY_SIZE_256: usize = 32; + +pub const NONCE_SIZE: usize = 16; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum ModemPreset { + + LongSlow = 0, + + LongFast = 1, + + LongModerate = 2, + + VeryLongSlow = 3, + + MediumSlow = 4, + + MediumFast = 5, + + ShortSlow = 6, + + ShortFast = 7, + + ShortTurbo = 8, +} + +impl Default for ModemPreset { + fn default() -> Self { + ModemPreset::LongFast + } +} + +#[derive(Debug, Clone, Copy)] +pub struct LoraParams { + + pub spreading_factor: u8, + + pub bandwidth: u32, + + pub coding_rate: u8, +} + +impl ModemPreset { + + pub const fn lora_params(&self) -> LoraParams { + match self { + ModemPreset::LongSlow => LoraParams { + spreading_factor: 12, + bandwidth: 125_000, + coding_rate: 8, + }, + ModemPreset::LongFast => LoraParams { + spreading_factor: 11, + bandwidth: 125_000, + coding_rate: 8, + }, + ModemPreset::LongModerate => LoraParams { + spreading_factor: 11, + bandwidth: 125_000, + coding_rate: 5, + }, + ModemPreset::VeryLongSlow => LoraParams { + spreading_factor: 12, + bandwidth: 125_000, + coding_rate: 8, + }, + ModemPreset::MediumSlow => LoraParams { + spreading_factor: 10, + bandwidth: 250_000, + coding_rate: 5, + }, + ModemPreset::MediumFast => LoraParams { + spreading_factor: 9, + bandwidth: 250_000, + coding_rate: 5, + }, + ModemPreset::ShortSlow => LoraParams { + spreading_factor: 8, + bandwidth: 250_000, + coding_rate: 5, + }, + ModemPreset::ShortFast => LoraParams { + spreading_factor: 7, + bandwidth: 250_000, + coding_rate: 5, + }, + ModemPreset::ShortTurbo => LoraParams { + spreading_factor: 7, + bandwidth: 500_000, + coding_rate: 5, + }, + } + } + + pub fn airtime_ms(&self, payload_bytes: usize) -> u32 { + let params = self.lora_params(); + let sf = params.spreading_factor as f32; + let bw = params.bandwidth as f32; + let cr = params.coding_rate as f32; + + let t_sym = (2.0_f32.powf(sf)) / bw * 1000.0; + let t_preamble = (8.0 + 4.25) * t_sym; + + let pl = payload_bytes as f32; + let de = if sf >= 11.0 { 1.0 } else { 0.0 }; + let h = 0.0; + let crc = 1.0; + + let numerator = 8.0 * pl - 4.0 * sf + 28.0 + 16.0 * crc - 20.0 * h; + let denominator = 4.0 * (sf - 2.0 * de); + let n_payload = 8.0 + (numerator / denominator).ceil().max(0.0) * (cr + 4.0); + + let t_payload = n_payload * t_sym; + + (t_preamble + t_payload) as u32 + } +} + +#[derive(Clone)] +pub enum ChannelKey { + + None, + + Aes128([u8; KEY_SIZE_128]), + + Aes256([u8; KEY_SIZE_256]), +} + +impl Drop for ChannelKey { + fn drop(&mut self) { + + match self { + ChannelKey::None => {} + ChannelKey::Aes128(key) => { + crate::crypto::secure_zero(key); + } + ChannelKey::Aes256(key) => { + crate::crypto::secure_zero(key); + } + } + } +} + +impl ChannelKey { + + pub fn from_bytes(key: &[u8]) -> Self { + match key.len() { + 0 => ChannelKey::None, + 1..=16 => { + let mut k = [0u8; KEY_SIZE_128]; + k[..key.len()].copy_from_slice(key); + ChannelKey::Aes128(k) + } + _ => { + let mut k = [0u8; KEY_SIZE_256]; + let len = core::cmp::min(key.len(), KEY_SIZE_256); + k[..len].copy_from_slice(&key[..len]); + ChannelKey::Aes256(k) + } + } + } + + pub fn default_key() -> Self { + ChannelKey::Aes128(mesh_kdf::DEFAULT_KEY) + } + + pub fn from_channel_name(name: &str) -> Self { + if name.is_empty() { + return Self::default_key(); + } + let hash = mesh_kdf::derive_channel_key(name); + ChannelKey::Aes256(hash) + } + + pub fn is_encrypted(&self) -> bool { + !matches!(self, ChannelKey::None) + } + + pub fn as_bytes(&self) -> &[u8] { + match self { + ChannelKey::None => &[], + ChannelKey::Aes128(k) => k, + ChannelKey::Aes256(k) => k, + } + } +} + +impl Default for ChannelKey { + fn default() -> Self { + ChannelKey::default_key() + } +} + +impl core::fmt::Debug for ChannelKey { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + ChannelKey::None => write!(f, "ChannelKey::None"), + ChannelKey::Aes128(_) => write!(f, "ChannelKey::Aes128([REDACTED])"), + ChannelKey::Aes256(_) => write!(f, "ChannelKey::Aes256([REDACTED])"), + } + } +} + +#[derive(Clone)] +pub struct Channel { + + pub index: u8, + + pub name: Vec, + + pub key: ChannelKey, + + pub modem_preset: ModemPreset, + + pub uplink_enabled: bool, + + pub downlink_enabled: bool, + + pub position_precision: u8, +} + +impl Channel { + + pub fn new(index: u8) -> Self { + Self { + index, + name: Vec::new(), + key: ChannelKey::default_key(), + modem_preset: ModemPreset::default(), + uplink_enabled: false, + downlink_enabled: false, + position_precision: 0, + } + } + + pub fn primary() -> Self { + let mut ch = Self::new(0); + ch.name.extend_from_slice(b"Primary").ok(); + ch + } + + pub fn set_name(&mut self, name: &str) { + self.name.clear(); + let len = core::cmp::min(name.len(), MAX_CHANNEL_NAME); + self.name.extend_from_slice(&name.as_bytes()[..len]).ok(); + + self.key = ChannelKey::from_channel_name(name); + } + + pub fn set_key(&mut self, key: &[u8]) { + self.key = ChannelKey::from_bytes(key); + } + + pub fn encrypt(&self, packet_id: u32, sender: u32, plaintext: &[u8]) -> Option> { + if !self.key.is_encrypted() { + + return Vec::from_slice(plaintext).ok(); + } + + let nonce = mesh_kdf::derive_nonce(packet_id, sender); + + let mut ciphertext = Vec::new(); + ciphertext.extend_from_slice(plaintext).ok()?; + + match &self.key { + ChannelKey::Aes128(key) => { + let cipher = Aes128::new(key); + + let mut nonce_block = [0u8; 16]; + nonce_block.copy_from_slice(&nonce); + cipher.encrypt_ctr(&nonce_block, &mut ciphertext); + } + ChannelKey::Aes256(key) => { + let cipher = Aes256::new(key); + let mut nonce_block = [0u8; 16]; + nonce_block.copy_from_slice(&nonce); + cipher.encrypt_ctr(&nonce_block, &mut ciphertext); + } + ChannelKey::None => {} + } + + Some(ciphertext) + } + + pub fn decrypt(&self, packet_id: u32, sender: u32, ciphertext: &[u8]) -> Option> { + if !self.key.is_encrypted() { + return Vec::from_slice(ciphertext).ok(); + } + + let nonce = mesh_kdf::derive_nonce(packet_id, sender); + + let mut plaintext = Vec::new(); + plaintext.extend_from_slice(ciphertext).ok()?; + + match &self.key { + ChannelKey::Aes128(key) => { + let cipher = Aes128::new(key); + let mut nonce_block = [0u8; 16]; + nonce_block.copy_from_slice(&nonce); + cipher.decrypt_ctr(&nonce_block, &mut plaintext); + } + ChannelKey::Aes256(key) => { + let cipher = Aes256::new(key); + let mut nonce_block = [0u8; 16]; + nonce_block.copy_from_slice(&nonce); + cipher.decrypt_ctr(&nonce_block, &mut plaintext); + } + ChannelKey::None => {} + } + + Some(plaintext) + } + + pub fn hash(&self) -> u8 { + + let key_bytes = self.key.as_bytes(); + if key_bytes.is_empty() { + return 0; + } + + let mut h: u8 = 0; + for &b in key_bytes { + h ^= b; + } + h + } + + pub fn name_str(&self) -> &str { + core::str::from_utf8(&self.name).unwrap_or("") + } +} + +impl Default for Channel { + fn default() -> Self { + Self::primary() + } +} + +impl core::fmt::Debug for Channel { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + f.debug_struct("Channel") + .field("index", &self.index) + .field("name", &self.name_str()) + .field("key", &self.key) + .field("modem_preset", &self.modem_preset) + .finish() + } +} + +pub const MAX_CHANNELS: usize = 8; + +pub struct ChannelSet { + + channels: [Option; MAX_CHANNELS], +} + +impl ChannelSet { + + pub fn new() -> Self { + let mut channels = [None, None, None, None, None, None, None, None]; + channels[0] = Some(Channel::primary()); + Self { channels } + } + + pub fn get(&self, index: u8) -> Option<&Channel> { + self.channels.get(index as usize)?.as_ref() + } + + pub fn get_mut(&mut self, index: u8) -> Option<&mut Channel> { + self.channels.get_mut(index as usize)?.as_mut() + } + + pub fn set(&mut self, index: u8, channel: Channel) { + if (index as usize) < MAX_CHANNELS { + self.channels[index as usize] = Some(channel); + } + } + + #[inline] + pub fn primary(&self) -> Option<&Channel> { + self.channels[0].as_ref() + } + + #[inline] + pub fn primary_mut(&mut self) -> Option<&mut Channel> { + self.channels[0].as_mut() + } + + pub fn primary_or_init(&mut self) -> &mut Channel { + if self.channels[0].is_none() { + self.channels[0] = Some(Channel::primary()); + } + + self.channels[0].as_mut().unwrap() + } + + pub fn iter(&self) -> impl Iterator { + self.channels + .iter() + .enumerate() + .filter_map(|(i, ch)| ch.as_ref().map(|c| (i as u8, c))) + } + + pub fn find_by_hash(&self, hash: u8) -> Option<&Channel> { + for ch in self.channels.iter().flatten() { + if ch.hash() == hash { + return Some(ch); + } + } + None + } + + pub fn count(&self) -> usize { + self.channels.iter().filter(|c| c.is_some()).count() + } +} + +impl Default for ChannelSet { + fn default() -> Self { + Self::new() + } +} + +const BASE64_CHARS: &[u8; 64] = b"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-_"; + +const BASE64_DECODE: [i8; 128] = { + let mut table = [-1i8; 128]; + let chars = b"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-_"; + let mut i = 0; + while i < 64 { + table[chars[i] as usize] = i as i8; + i += 1; + } + + table[b'+' as usize] = 62; + table[b'/' as usize] = 63; + table +}; + +pub fn base64_encode(data: &[u8], output: &mut [u8]) -> usize { + let mut o = 0; + let mut i = 0; + + while i + 2 < data.len() { + if o + 4 > output.len() { + break; + } + let n = ((data[i] as u32) << 16) | ((data[i + 1] as u32) << 8) | (data[i + 2] as u32); + output[o] = BASE64_CHARS[((n >> 18) & 0x3F) as usize]; + output[o + 1] = BASE64_CHARS[((n >> 12) & 0x3F) as usize]; + output[o + 2] = BASE64_CHARS[((n >> 6) & 0x3F) as usize]; + output[o + 3] = BASE64_CHARS[(n & 0x3F) as usize]; + i += 3; + o += 4; + } + + if i < data.len() && o + 4 <= output.len() { + let remaining = data.len() - i; + if remaining == 1 { + let n = (data[i] as u32) << 16; + output[o] = BASE64_CHARS[((n >> 18) & 0x3F) as usize]; + output[o + 1] = BASE64_CHARS[((n >> 12) & 0x3F) as usize]; + o += 2; + } else if remaining == 2 { + let n = ((data[i] as u32) << 16) | ((data[i + 1] as u32) << 8); + output[o] = BASE64_CHARS[((n >> 18) & 0x3F) as usize]; + output[o + 1] = BASE64_CHARS[((n >> 12) & 0x3F) as usize]; + output[o + 2] = BASE64_CHARS[((n >> 6) & 0x3F) as usize]; + o += 3; + } + } + + o +} + +pub fn base64_decode(data: &[u8], output: &mut [u8]) -> usize { + let mut o = 0; + let mut i = 0; + + let data = if data.starts_with(b"https://meshtastic.org/e/#") { + &data[26..] + } else { + data + }; + + while i + 3 < data.len() { + if o + 3 > output.len() { + break; + } + + let b0 = BASE64_DECODE.get(data[i] as usize).copied().unwrap_or(-1); + let b1 = BASE64_DECODE.get(data[i + 1] as usize).copied().unwrap_or(-1); + let b2 = BASE64_DECODE.get(data[i + 2] as usize).copied().unwrap_or(-1); + let b3 = BASE64_DECODE.get(data[i + 3] as usize).copied().unwrap_or(-1); + + if b0 < 0 || b1 < 0 { + break; + } + + let n = ((b0 as u32) << 18) + | ((b1 as u32) << 12) + | (if b2 >= 0 { (b2 as u32) << 6 } else { 0 }) + | (if b3 >= 0 { b3 as u32 } else { 0 }); + + output[o] = (n >> 16) as u8; + o += 1; + + if b2 >= 0 { + output[o] = (n >> 8) as u8; + o += 1; + } + + if b3 >= 0 { + output[o] = n as u8; + o += 1; + } + + i += 4; + } + + if i + 1 < data.len() && o < output.len() { + let b0 = BASE64_DECODE.get(data[i] as usize).copied().unwrap_or(-1); + let b1 = BASE64_DECODE.get(data[i + 1] as usize).copied().unwrap_or(-1); + + if b0 >= 0 && b1 >= 0 { + let n = ((b0 as u32) << 18) | ((b1 as u32) << 12); + output[o] = (n >> 16) as u8; + o += 1; + + if i + 2 < data.len() && o < output.len() { + let b2 = BASE64_DECODE.get(data[i + 2] as usize).copied().unwrap_or(-1); + if b2 >= 0 { + let n = ((b0 as u32) << 18) | ((b1 as u32) << 12) | ((b2 as u32) << 6); + output[o - 1] = (n >> 16) as u8; + output[o] = (n >> 8) as u8; + o += 1; + } + } + } + } + + o +} diff --git a/src/meshtastic/encryption.rs b/src/meshtastic/encryption.rs new file mode 100644 index 0000000..a13cc0b --- /dev/null +++ b/src/meshtastic/encryption.rs @@ -0,0 +1,352 @@ +use heapless::Vec; +use crate::crypto::aes::{Aes128, Aes256}; +use crate::crypto::sha256::Sha256; +use crate::crypto::hmac::HmacSha256; +use crate::crypto::hkdf::meshtastic as mesh_kdf; +use super::channel::ChannelKey; + +pub const MAX_PAYLOAD_SIZE: usize = 237; + +pub const NONCE_SIZE: usize = 16; + +pub const MIC_SIZE: usize = 4; + +pub const MIC_SIZE_ENHANCED: usize = 8; + +pub struct EncryptionContext { + + key: ChannelKey, +} + +impl EncryptionContext { + + pub fn new(key: ChannelKey) -> Self { + Self { key } + } + + pub fn with_default_key() -> Self { + Self::new(ChannelKey::default_key()) + } + + pub fn from_channel_name(name: &str) -> Self { + Self::new(ChannelKey::from_channel_name(name)) + } + + pub fn from_key_bytes(key: &[u8]) -> Self { + Self::new(ChannelKey::from_bytes(key)) + } + + pub fn is_encrypted(&self) -> bool { + self.key.is_encrypted() + } + + pub fn encrypt(&self, packet_id: u32, sender: u32, plaintext: &[u8]) -> Option> { + if !self.key.is_encrypted() { + + return Vec::from_slice(plaintext).ok(); + } + + if plaintext.len() > MAX_PAYLOAD_SIZE { + return None; + } + + let nonce = mesh_kdf::derive_nonce(packet_id, sender); + + let mut ciphertext = Vec::new(); + ciphertext.extend_from_slice(plaintext).ok()?; + + match &self.key { + ChannelKey::Aes128(key) => { + let cipher = Aes128::new(key); + cipher.encrypt_ctr(&nonce, &mut ciphertext); + } + ChannelKey::Aes256(key) => { + let cipher = Aes256::new(key); + cipher.encrypt_ctr(&nonce, &mut ciphertext); + } + ChannelKey::None => {} + } + + Some(ciphertext) + } + + pub fn decrypt(&self, packet_id: u32, sender: u32, ciphertext: &[u8]) -> Option> { + if !self.key.is_encrypted() { + return Vec::from_slice(ciphertext).ok(); + } + + if ciphertext.is_empty() || ciphertext.len() > MAX_PAYLOAD_SIZE { + return None; + } + + let nonce = mesh_kdf::derive_nonce(packet_id, sender); + + let mut plaintext = Vec::new(); + plaintext.extend_from_slice(ciphertext).ok()?; + + match &self.key { + ChannelKey::Aes128(key) => { + let cipher = Aes128::new(key); + cipher.decrypt_ctr(&nonce, &mut plaintext); + } + ChannelKey::Aes256(key) => { + let cipher = Aes256::new(key); + cipher.decrypt_ctr(&nonce, &mut plaintext); + } + ChannelKey::None => {} + } + + Some(plaintext) + } + + pub fn key_hash(&self) -> u8 { + let key_bytes = self.key.as_bytes(); + if key_bytes.is_empty() { + return 0; + } + + let mut h: u8 = 0; + for &b in key_bytes { + h ^= b; + } + h + } +} + +impl Default for EncryptionContext { + fn default() -> Self { + Self::with_default_key() + } +} + +pub fn compute_mic(data: &[u8]) -> [u8; MIC_SIZE] { + let hash = Sha256::hash(data); + let mut mic = [0u8; MIC_SIZE]; + mic.copy_from_slice(&hash[..MIC_SIZE]); + mic +} + +pub fn verify_mic(data: &[u8], expected_mic: &[u8]) -> bool { + if expected_mic.len() != MIC_SIZE { + return false; + } + + let computed = compute_mic(data); + constant_time_eq(&computed, expected_mic) +} + +#[inline(never)] +fn constant_time_eq(a: &[u8], b: &[u8]) -> bool { + if a.len() != b.len() { + return false; + } + let mut result: u8 = 0; + for (x, y) in a.iter().zip(b.iter()) { + result |= x ^ y; + } + result == 0 +} + +pub fn compute_mic_enhanced(key: &[u8], data: &[u8]) -> [u8; MIC_SIZE_ENHANCED] { + + let mut hmac_key = [0u8; 32]; + if key.len() >= 32 { + hmac_key.copy_from_slice(&key[..32]); + } else { + hmac_key[..key.len()].copy_from_slice(key); + } + + let mac = HmacSha256::mac(&hmac_key, data); + let mut mic = [0u8; MIC_SIZE_ENHANCED]; + mic.copy_from_slice(&mac[..MIC_SIZE_ENHANCED]); + mic +} + +pub fn verify_mic_enhanced(key: &[u8], data: &[u8], expected_mic: &[u8]) -> bool { + if expected_mic.len() != MIC_SIZE_ENHANCED { + return false; + } + + let computed = compute_mic_enhanced(key, data); + constant_time_eq(&computed, expected_mic) +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum MicMode { + + Standard, + + Enhanced, +} + +impl Default for MicMode { + fn default() -> Self { + MicMode::Standard + } +} + +use crate::crypto::x25519; +use crate::crypto::chacha20::ChaCha20; +use crate::crypto::poly1305::ChaCha20Poly1305; +use crate::crypto::hkdf::Hkdf; + +pub const PKI_OVERHEAD: usize = 32 + 16; + +pub fn pki_encrypt( + recipient_pubkey: &[u8; 32], + sender_privkey: &[u8; 32], + plaintext: &[u8], + nonce: &[u8; 12], +) -> Option> { + if plaintext.len() + PKI_OVERHEAD > 256 { + return None; + } + + let mut ephemeral_seed = [0u8; 32]; + Hkdf::derive(nonce, sender_privkey, b"ephemeral", &mut ephemeral_seed); + + let ephemeral_pubkey = x25519::x25519_base(&ephemeral_seed); + + let shared_secret = x25519::x25519(&ephemeral_seed, recipient_pubkey); + + let mut key = [0u8; 32]; + Hkdf::derive(b"meshtastic-pki", &shared_secret, &ephemeral_pubkey, &mut key); + + let mut ciphertext = [0u8; 240]; + let mut tag = [0u8; 16]; + + if plaintext.len() > ciphertext.len() { + return None; + } + + ChaCha20Poly1305::seal(&key, nonce, &[], plaintext, &mut ciphertext[..plaintext.len()], &mut tag); + + let mut output = Vec::new(); + output.extend_from_slice(&ephemeral_pubkey).ok()?; + output.extend_from_slice(&ciphertext[..plaintext.len()]).ok()?; + output.extend_from_slice(&tag).ok()?; + + Some(output) +} + +pub fn pki_decrypt( + recipient_privkey: &[u8; 32], + encrypted: &[u8], + nonce: &[u8; 12], +) -> Option> { + if encrypted.len() < PKI_OVERHEAD { + return None; + } + + let mut ephemeral_pubkey = [0u8; 32]; + ephemeral_pubkey.copy_from_slice(&encrypted[..32]); + + let ciphertext = &encrypted[32..encrypted.len() - 16]; + let mut tag = [0u8; 16]; + tag.copy_from_slice(&encrypted[encrypted.len() - 16..]); + + let shared_secret = x25519::x25519(recipient_privkey, &ephemeral_pubkey); + + let mut key = [0u8; 32]; + Hkdf::derive(b"meshtastic-pki", &shared_secret, &ephemeral_pubkey, &mut key); + + let mut plaintext = [0u8; 240]; + if ciphertext.len() > plaintext.len() { + return None; + } + + if !ChaCha20Poly1305::open(&key, nonce, &[], ciphertext, &tag, &mut plaintext[..ciphertext.len()]) { + return None; + } + + let mut output = Vec::new(); + output.extend_from_slice(&plaintext[..ciphertext.len()]).ok()?; + Some(output) +} + +pub struct KeyStore { + + channel_keys: [Option; 8], + + node_privkey: Option<[u8; 32]>, + + node_pubkey: Option<[u8; 32]>, +} + +impl Drop for KeyStore { + fn drop(&mut self) { + + if let Some(ref mut privkey) = self.node_privkey { + crate::crypto::secure_zero(privkey); + } + + } +} + +impl KeyStore { + + pub const fn new() -> Self { + Self { + channel_keys: [None, None, None, None, None, None, None, None], + node_privkey: None, + node_pubkey: None, + } + } + + pub fn set_channel_key(&mut self, index: u8, key: &[u8]) { + if (index as usize) < self.channel_keys.len() { + self.channel_keys[index as usize] = Some(EncryptionContext::from_key_bytes(key)); + } + } + + pub fn set_channel_name(&mut self, index: u8, name: &str) { + if (index as usize) < self.channel_keys.len() { + self.channel_keys[index as usize] = Some(EncryptionContext::from_channel_name(name)); + } + } + + pub fn get_channel(&self, index: u8) -> Option<&EncryptionContext> { + self.channel_keys.get(index as usize)?.as_ref() + } + + pub fn set_node_keypair(&mut self, privkey: &[u8; 32]) { + self.node_privkey = Some(*privkey); + self.node_pubkey = Some(x25519::x25519_base(privkey)); + } + + pub fn generate_node_keypair(&mut self, entropy: &[u8; 32]) { + + let mut privkey = [0u8; 32]; + Hkdf::derive(b"meshtastic", entropy, b"node-key", &mut privkey); + self.set_node_keypair(&privkey); + } + + pub fn node_pubkey(&self) -> Option<&[u8; 32]> { + self.node_pubkey.as_ref() + } + + pub fn encrypt_for_node( + &self, + recipient_pubkey: &[u8; 32], + plaintext: &[u8], + nonce: &[u8; 12], + ) -> Option> { + let privkey = self.node_privkey.as_ref()?; + pki_encrypt(recipient_pubkey, privkey, plaintext, nonce) + } + + pub fn decrypt_from_node( + &self, + encrypted: &[u8], + nonce: &[u8; 12], + ) -> Option> { + let privkey = self.node_privkey.as_ref()?; + pki_decrypt(privkey, encrypted, nonce) + } +} + +impl Default for KeyStore { + fn default() -> Self { + Self::new() + } +} diff --git a/src/meshtastic/mod.rs b/src/meshtastic/mod.rs new file mode 100644 index 0000000..8bc66f4 --- /dev/null +++ b/src/meshtastic/mod.rs @@ -0,0 +1,1263 @@ +pub mod protobuf; +pub mod channel; +pub mod packet; +pub mod encryption; + +pub use protobuf::*; +pub use channel::*; +pub use packet::*; +pub use encryption::*; + +use heapless::Vec; + +pub const SERIAL_SYNC: [u8; 2] = [0x94, 0xC3]; + +pub const MAX_MESSAGE_SIZE: usize = 512; + +pub const LORA_HEADER_SIZE: usize = 16; + +pub const MAX_LORA_PAYLOAD: usize = 237; + +pub const MIC_SIZE: usize = 4; + +pub const DEFAULT_HOP_LIMIT: u8 = 3; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u16)] +pub enum PortNum { + Unknown = 0, + TextMessage = 1, + RemoteHardware = 2, + Position = 3, + NodeInfo = 4, + Routing = 5, + Admin = 6, + TextMessageCompressed = 7, + Waypoint = 8, + Audio = 9, + DetectionSensor = 10, + Reply = 32, + IpTunnelApp = 33, + Paxcounter = 34, + Serial = 64, + StoreForward = 65, + RangeTest = 66, + Telemetry = 67, + Zps = 68, + Simulator = 69, + Traceroute = 70, + Neighborinfo = 71, + Atak = 72, + Map = 73, + Private = 256, + AtakForwarder = 257, + Max = 511, +} + +impl From for PortNum { + fn from(v: u32) -> Self { + match v { + 1 => PortNum::TextMessage, + 2 => PortNum::RemoteHardware, + 3 => PortNum::Position, + 4 => PortNum::NodeInfo, + 5 => PortNum::Routing, + 6 => PortNum::Admin, + 7 => PortNum::TextMessageCompressed, + 8 => PortNum::Waypoint, + 32 => PortNum::Reply, + 33 => PortNum::IpTunnelApp, + 64 => PortNum::Serial, + 65 => PortNum::StoreForward, + 66 => PortNum::RangeTest, + 67 => PortNum::Telemetry, + 70 => PortNum::Traceroute, + 71 => PortNum::Neighborinfo, + _ => PortNum::Unknown, + } + } +} + +#[derive(Debug, Clone)] +pub struct MeshPacket { + + pub from: u32, + + pub to: u32, + + pub channel: u8, + + pub id: u32, + + pub hop_limit: u8, + + pub want_ack: bool, + + pub priority: Priority, + + pub rx_time: u32, + + pub rx_snr: f32, + + pub rx_rssi: i32, + + pub payload: PacketPayload, +} + +#[derive(Debug, Clone)] +pub enum PacketPayload { + + Encrypted(Vec), + + Decoded(DataPayload), +} + +#[derive(Debug, Clone)] +pub struct DataPayload { + + pub port: PortNum, + + pub payload: Vec, + + pub want_response: bool, + + pub dest: u32, + + pub source: u32, + + pub request_id: u32, + + pub reply_id: u32, + + pub emoji: u32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum Priority { + Unset = 0, + Min = 1, + Background = 10, + Default = 64, + Reliable = 70, + Ack = 120, + Max = 127, +} + +impl From for Priority { + fn from(v: u8) -> Self { + match v { + 1 => Priority::Min, + 10 => Priority::Background, + 70 => Priority::Reliable, + 120 => Priority::Ack, + 127 => Priority::Max, + _ => Priority::Default, + } + } +} + +impl Default for MeshPacket { + fn default() -> Self { + Self { + from: 0, + to: 0xFFFFFFFF, + channel: 0, + id: 0, + hop_limit: DEFAULT_HOP_LIMIT, + want_ack: false, + priority: Priority::Default, + rx_time: 0, + rx_snr: 0.0, + rx_rssi: 0, + payload: PacketPayload::Encrypted(Vec::new()), + } + } +} + +impl Default for DataPayload { + fn default() -> Self { + Self { + port: PortNum::Unknown, + payload: Vec::new(), + want_response: false, + dest: 0, + source: 0, + request_id: 0, + reply_id: 0, + emoji: 0, + } + } +} + +#[derive(Debug, Clone)] +pub struct NodeInfo { + + pub num: u32, + + pub user: Option, + + pub position: Option, + + pub last_heard: u32, + + pub snr: f32, +} + +#[derive(Debug, Clone)] +pub struct User { + + pub id: [u8; 8], + + pub long_name: Vec, + + pub short_name: Vec, + + pub hw_model: HardwareModel, + + pub is_licensed: bool, + + pub role: Role, +} + +#[derive(Debug, Clone, Default)] +pub struct Position { + + pub latitude_i: i32, + + pub longitude_i: i32, + + pub altitude: i32, + + pub time: u32, + + pub timestamp: u32, + + pub location_source: LocationSource, + + pub altitude_source: LocationSource, + + pub pdop: u32, + + pub hdop: u32, + + pub sats_in_view: u32, + + pub ground_speed: u32, + + pub ground_track: u32, + + pub fix_quality: u32, + + pub fix_type: u32, + + pub seq_number: u32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] +#[repr(u8)] +pub enum LocationSource { + #[default] + Unset = 0, + Manual = 1, + InternalGps = 2, + ExternalGps = 3, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u16)] +pub enum HardwareModel { + Unset = 0, + TloraV2 = 1, + TloraV1 = 2, + TloraV21_1p6 = 3, + Tbeam = 4, + HeltecV2_0 = 5, + TbeamV0p7 = 6, + Techo = 7, + TloraV1_1p3 = 8, + Rak4631 = 9, + HeltecV2_1 = 10, + HeltecV1 = 11, + LilygoTbeamS3Core = 12, + Rak11200 = 13, + NanoG1 = 14, + TloraV2_1_1p8 = 15, + TloraT3S3 = 16, + NanoG1Explorer = 17, + NanoG2Ultra = 18, + LoraType = 19, + WiPhone = 20, + WioWm1110 = 21, + Rak2560 = 22, + HeltecHt62 = 23, + Ebyte900 = 24, + EbyteEsp32S3 = 25, + Esp32S3Pico = 26, + Chatter2 = 27, + HeltecWirelessPaperV1_0 = 28, + HeltecWirelessTrackerV1_0 = 29, + Unphone = 30, + Tdeck = 31, + TWatchS3 = 32, + PicomputerS3 = 33, + HeltecWifiLoraV3 = 34, + PrivateHw = 255, +} + +impl Default for HardwareModel { + fn default() -> Self { + HardwareModel::Unset + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum Role { + Client = 0, + ClientMute = 1, + Router = 2, + RouterClient = 3, + Repeater = 4, + Tracker = 5, + Sensor = 6, + Tak = 7, + ClientHidden = 8, + LostAndFound = 9, + TakTracker = 10, +} + +impl Default for Role { + fn default() -> Self { + Role::Client + } +} + +pub struct MeshtasticHandler { + + pub node_id: u32, + + pub primary_channel: Channel, + + pub secondary_channels: [Option; 7], + + pub last_packet_id: u32, + + pub rx_count: u32, + + pub tx_count: u32, + + pub position: Option, + + pub user: Option, + + parser: MeshtasticParser, + + pending_responses: heapless::Deque, 16>, + + config_request_id: u32, + + config_channel_index: u8, +} + +impl MeshtasticHandler { + + pub fn new(node_id: u32) -> Self { + Self { + node_id, + primary_channel: Channel::default(), + secondary_channels: [None, None, None, None, None, None, None], + last_packet_id: 0, + rx_count: 0, + tx_count: 0, + position: None, + user: None, + parser: MeshtasticParser::new(), + pending_responses: heapless::Deque::new(), + config_request_id: 0, + config_channel_index: 0, + } + } + + pub fn set_channel_key(&mut self, psk: &[u8]) { + self.primary_channel.set_key(psk); + } + + pub fn next_packet_id(&mut self) -> u32 { + self.last_packet_id = self.last_packet_id.wrapping_add(1); + if self.last_packet_id == 0 { + self.last_packet_id = 1; + } + self.last_packet_id + } + + pub fn process_lora_packet(&mut self, data: &[u8], rssi: i32, snr: f32) -> Option { + if data.len() < LORA_HEADER_SIZE + MIC_SIZE { + return None; + } + + let mut packet = packet::parse_lora_packet(data)?; + + packet.rx_rssi = rssi; + packet.rx_snr = snr; + + let decrypted = self.decrypt_packet(&packet)?; + + self.rx_count += 1; + + Some(decrypted) + } + + fn decrypt_packet(&self, packet: &MeshPacket) -> Option { + let mut result = packet.clone(); + + if let PacketPayload::Encrypted(ref encrypted) = packet.payload { + + let channel = if packet.channel == 0 { + &self.primary_channel + } else { + self.secondary_channels + .get((packet.channel - 1) as usize)? + .as_ref()? + }; + + let decrypted = channel.decrypt(packet.id, packet.from, encrypted)?; + + if let Some(data) = protobuf::decode_data(&decrypted) { + result.payload = PacketPayload::Decoded(data); + } + } + + Some(result) + } + + pub fn create_packet( + &mut self, + to: u32, + port: PortNum, + payload: &[u8], + want_ack: bool, + ) -> Option> { + let packet_id = self.next_packet_id(); + + let data = DataPayload { + port, + payload: Vec::from_slice(payload).ok()?, + want_response: want_ack, + source: self.node_id, + dest: to, + ..Default::default() + }; + + let encoded = protobuf::encode_data(&data)?; + + let encrypted = self.primary_channel.encrypt(packet_id, self.node_id, &encoded)?; + + let lora_packet = packet::build_lora_packet( + self.node_id, + to, + packet_id, + 0, + DEFAULT_HOP_LIMIT, + want_ack, + &encrypted, + )?; + + self.tx_count += 1; + + Some(lora_packet) + } + + pub fn create_text_message(&mut self, to: u32, text: &str) -> Option> { + self.create_packet(to, PortNum::TextMessage, text.as_bytes(), true) + } + + pub fn create_position_packet(&mut self) -> Option> { + let position = self.position.as_ref()?; + let encoded = protobuf::encode_position(position)?; + self.create_packet(0xFFFFFFFF, PortNum::Position, &encoded, false) + } + + pub fn create_node_info_packet(&mut self) -> Option> { + let user = self.user.as_ref()?; + let encoded = protobuf::encode_user(user)?; + self.create_packet(0xFFFFFFFF, PortNum::NodeInfo, &encoded, false) + } + + pub fn parse_serial_frame(&self, data: &[u8]) -> Option> { + if data.len() < 4 { + return None; + } + + if data[0] != SERIAL_SYNC[0] || data[1] != SERIAL_SYNC[1] { + return None; + } + + let len = u16::from_be_bytes([data[2], data[3]]) as usize; + if data.len() < 4 + len { + return None; + } + + let mut payload = Vec::new(); + payload.extend_from_slice(&data[4..4 + len]).ok()?; + Some(payload) + } + + pub fn build_serial_frame(&self, payload: &[u8]) -> Option> { + let mut frame = Vec::new(); + frame.push(SERIAL_SYNC[0]).ok()?; + frame.push(SERIAL_SYNC[1]).ok()?; + let len_bytes = (payload.len() as u16).to_be_bytes(); + frame.push(len_bytes[0]).ok()?; + frame.push(len_bytes[1]).ok()?; + frame.extend_from_slice(payload).ok()?; + Some(frame) + } + + pub fn feed_serial(&mut self, byte: u8) -> Option { + self.parser.feed(byte) + } + + pub fn build_lora_packet(&mut self, frame: &MeshtasticFrame) -> Option> { + + if frame.payload.is_empty() { + return None; + } + + let payload = &frame.payload; + + if payload.len() < 2 { + return None; + } + + if payload[0] != 0x0A { + + return None; + } + + let mesh_packet_len = payload[1] as usize; + if payload.len() < 2 + mesh_packet_len { + return None; + } + + let mesh_packet_data = &payload[2..2 + mesh_packet_len]; + + let mut to: u32 = 0xFFFFFFFF; + let mut channel: u8 = 0; + let mut want_ack = false; + let mut hop_limit = DEFAULT_HOP_LIMIT; + let mut inner_payload: Option> = None; + let mut port = PortNum::Unknown; + + let mut idx = 0; + while idx < mesh_packet_data.len() { + let tag = mesh_packet_data[idx]; + idx += 1; + if idx >= mesh_packet_data.len() { + break; + } + + let field_num = tag >> 3; + let wire_type = tag & 0x07; + + match (field_num, wire_type) { + + (2, 0) => { + let (val, consumed) = decode_varint(&mesh_packet_data[idx..]); + to = val as u32; + idx += consumed; + } + + (4, 0) => { + let (val, consumed) = decode_varint(&mesh_packet_data[idx..]); + channel = val as u8; + idx += consumed; + } + + (6, 0) => { + let (val, consumed) = decode_varint(&mesh_packet_data[idx..]); + hop_limit = val as u8; + idx += consumed; + } + + (7, 0) => { + let (val, consumed) = decode_varint(&mesh_packet_data[idx..]); + want_ack = val != 0; + idx += consumed; + } + + (8, 2) => { + if idx >= mesh_packet_data.len() { + break; + } + let len = mesh_packet_data[idx] as usize; + idx += 1; + if idx + len > mesh_packet_data.len() { + break; + } + let data_msg = &mesh_packet_data[idx..idx + len]; + + let mut data_idx = 0; + while data_idx < data_msg.len() { + let dtag = data_msg[data_idx]; + data_idx += 1; + if data_idx >= data_msg.len() { + break; + } + let dfield = dtag >> 3; + let dwire = dtag & 0x07; + match (dfield, dwire) { + + (1, 0) => { + let (val, consumed) = decode_varint(&data_msg[data_idx..]); + port = PortNum::from(val as u32); + data_idx += consumed; + } + + (2, 2) => { + if data_idx >= data_msg.len() { + break; + } + let plen = data_msg[data_idx] as usize; + data_idx += 1; + if data_idx + plen > data_msg.len() { + break; + } + let mut p = Vec::new(); + let _ = p.extend_from_slice(&data_msg[data_idx..data_idx + plen]); + inner_payload = Some(p); + data_idx += plen; + } + + (_, 0) => { + let (_, consumed) = decode_varint(&data_msg[data_idx..]); + data_idx += consumed; + } + (_, 2) => { + if data_idx >= data_msg.len() { + break; + } + let skip_len = data_msg[data_idx] as usize; + data_idx += 1 + skip_len; + } + _ => break, + } + } + idx += len; + } + + (_, 0) => { + let (_, consumed) = decode_varint(&mesh_packet_data[idx..]); + idx += consumed; + } + (_, 2) => { + if idx >= mesh_packet_data.len() { + break; + } + let skip_len = mesh_packet_data[idx] as usize; + idx += 1 + skip_len; + } + _ => break, + } + } + + let payload_data = inner_payload?; + + self.create_packet(to, port, &payload_data, want_ack) + } + + pub fn reset_parser(&mut self) { + self.parser.reset(); + } + + pub fn process_toradio(&mut self, frame: &MeshtasticFrame) -> Option { + if frame.payload.is_empty() { + return None; + } + + let payload = &frame.payload; + if payload.len() < 2 { + return None; + } + + let field_tag = payload[0]; + let field_num = field_tag >> 3; + let wire_type = field_tag & 0x07; + + match (field_num, wire_type) { + + (1, 2) => { + + self.build_lora_packet(frame).map(ToRadioResponse::LoRaPacket) + } + + (3, 0) => { + let (config_id, _) = decode_varint(&payload[1..]); + self.build_config_response(config_id as u32) + } + + (4, 0) => { + + self.reset_parser(); + None + } + + _ => None, + } + } + + fn build_config_response(&mut self, config_id: u32) -> Option { + + self.pending_responses.clear(); + self.config_request_id = config_id; + self.config_channel_index = 0; + + if let Some(my_info) = self.encode_privacy_myinfo() { + let _ = self.pending_responses.push_back(my_info); + } + + if let Some(node_info) = self.encode_privacy_nodeinfo() { + let _ = self.pending_responses.push_back(node_info); + } + + if let Some(channel_config) = self.encode_channel_config(0) { + let _ = self.pending_responses.push_back(channel_config); + } + + for i in 0..7 { + if self.secondary_channels[i].is_some() { + if let Some(channel_config) = self.encode_channel_config((i + 1) as u8) { + let _ = self.pending_responses.push_back(channel_config); + } + } + } + + if let Some(complete) = self.encode_config_complete(config_id) { + let _ = self.pending_responses.push_back(complete); + } + + self.pending_responses.pop_front().map(ToRadioResponse::FromRadio) + } + + pub fn poll_pending_response(&mut self) -> Option { + self.pending_responses.pop_front().map(ToRadioResponse::FromRadio) + } + + pub fn has_pending_responses(&self) -> bool { + !self.pending_responses.is_empty() + } + + pub fn pending_response_count(&self) -> usize { + self.pending_responses.len() + } + + fn encode_privacy_myinfo(&self) -> Option> { + let mut buf: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut buf); + let _ = encode_varint(0, &mut buf); + + let mut my_info: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut my_info); + let _ = encode_varint(self.node_id as u64, &mut my_info); + + let _ = write_tag(3, WIRE_VARINT, &mut my_info); + let _ = encode_varint(30000, &mut my_info); + + let _ = write_tag(4, WIRE_VARINT, &mut my_info); + let _ = encode_varint(8, &mut my_info); + + let _ = write_tag(6, WIRE_VARINT, &mut my_info); + let _ = encode_varint(0, &mut my_info); + + let _ = write_tag(7, WIRE_VARINT, &mut my_info); + let _ = encode_varint(1, &mut my_info); + + let _ = write_tag(4, WIRE_LEN, &mut buf); + let _ = encode_varint(my_info.len() as u64, &mut buf); + let _ = buf.extend_from_slice(&my_info); + + Some(buf) + } + + fn encode_privacy_nodeinfo(&self) -> Option> { + let mut buf: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut buf); + let _ = encode_varint(0, &mut buf); + + let mut node_info: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut node_info); + let _ = encode_varint(self.node_id as u64, &mut node_info); + + let mut user: Vec = Vec::new(); + + let id_str = format_node_id(self.node_id); + let _ = write_tag(1, WIRE_LEN, &mut user); + let _ = encode_varint(id_str.len() as u64, &mut user); + let _ = user.extend_from_slice(id_str.as_bytes()); + + if let Some(ref u) = self.user { + let _ = write_tag(2, WIRE_LEN, &mut user); + let _ = encode_varint(u.long_name.len() as u64, &mut user); + let _ = user.extend_from_slice(&u.long_name); + } else { + let name = b"LunarNode"; + let _ = write_tag(2, WIRE_LEN, &mut user); + let _ = encode_varint(name.len() as u64, &mut user); + let _ = user.extend_from_slice(name); + } + + if let Some(ref u) = self.user { + let _ = write_tag(3, WIRE_LEN, &mut user); + let _ = encode_varint(u.short_name.len() as u64, &mut user); + let _ = user.extend_from_slice(&u.short_name); + } else { + let short = b"LNOD"; + let _ = write_tag(3, WIRE_LEN, &mut user); + let _ = encode_varint(short.len() as u64, &mut user); + let _ = user.extend_from_slice(short); + } + + let _ = write_tag(5, WIRE_VARINT, &mut user); + let _ = encode_varint(43, &mut user); + + let _ = write_tag(7, WIRE_VARINT, &mut user); + let _ = encode_varint(4, &mut user); + + let _ = write_tag(2, WIRE_LEN, &mut node_info); + let _ = encode_varint(user.len() as u64, &mut node_info); + let _ = node_info.extend_from_slice(&user); + + let _ = write_tag(6, WIRE_LEN, &mut buf); + let _ = encode_varint(node_info.len() as u64, &mut buf); + let _ = buf.extend_from_slice(&node_info); + + Some(buf) + } + + fn encode_channel_config(&self, index: u8) -> Option> { + let mut buf: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut buf); + let _ = encode_varint(0, &mut buf); + + let mut channel: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut channel); + let _ = encode_varint(index as u64, &mut channel); + + let mut settings: Vec = Vec::new(); + + let name = self.primary_channel.name_str(); + if !name.is_empty() { + let _ = write_tag(2, WIRE_LEN, &mut settings); + let _ = encode_varint(name.len() as u64, &mut settings); + let _ = settings.extend_from_slice(name.as_bytes()); + } + + let _ = write_tag(2, WIRE_LEN, &mut channel); + let _ = encode_varint(settings.len() as u64, &mut channel); + let _ = channel.extend_from_slice(&settings); + + let _ = write_tag(3, WIRE_VARINT, &mut channel); + let _ = encode_varint(1, &mut channel); + + let _ = write_tag(8, WIRE_LEN, &mut buf); + let _ = encode_varint(channel.len() as u64, &mut buf); + let _ = buf.extend_from_slice(&channel); + + Some(buf) + } + + fn encode_config_complete(&self, config_id: u32) -> Option> { + let mut buf: Vec = Vec::new(); + + let _ = write_tag(1, WIRE_VARINT, &mut buf); + let _ = encode_varint(0, &mut buf); + + let _ = write_tag(9, WIRE_VARINT, &mut buf); + let _ = encode_varint(config_id as u64, &mut buf); + + Some(buf) + } + + pub fn handle_admin_message(&mut self, payload: &[u8]) -> Option> { + if payload.is_empty() { + return None; + } + + let tag = payload[0]; + let field_num = tag >> 3; + + match field_num { + + 1 => self.encode_privacy_myinfo(), + + 7 => self.encode_privacy_nodeinfo(), + + 5 => { + + None + } + + 6 => { + + None + } + + _ => None, + } + } +} + +fn format_node_id(node_id: u32) -> heapless::String<16> { + let mut s = heapless::String::new(); + let _ = s.push('!'); + + for i in (0..8).rev() { + let nibble = (node_id >> (i * 4)) & 0xF; + let c = if nibble < 10 { b'0' + nibble as u8 } else { b'a' + (nibble - 10) as u8 }; + let _ = s.push(c as char); + } + s +} + +pub enum ToRadioResponse { + + LoRaPacket(Vec), + + FromRadio(Vec), +} + +fn decode_varint(data: &[u8]) -> (u64, usize) { + let mut result: u64 = 0; + let mut shift = 0; + let mut consumed = 0; + + for &byte in data { + consumed += 1; + result |= ((byte & 0x7F) as u64) << shift; + if byte & 0x80 == 0 { + break; + } + shift += 7; + if shift >= 64 { + break; + } + } + + (result, consumed) +} + +fn encode_varint(value: u64, buf: &mut Vec) -> bool { + let mut v = value; + loop { + let byte = (v & 0x7F) as u8; + v >>= 7; + if v == 0 { + return buf.push(byte).is_ok(); + } else { + if buf.push(byte | 0x80).is_err() { + return false; + } + } + } +} + +fn encode_sint32(value: i32, buf: &mut Vec) -> bool { + let encoded = ((value << 1) ^ (value >> 31)) as u32; + encode_varint(encoded as u64, buf) +} + +fn write_tag(field: u32, wire_type: u8, buf: &mut Vec) -> bool { + let tag = (field << 3) | (wire_type as u32); + encode_varint(tag as u64, buf) +} + +const WIRE_VARINT: u8 = 0; +const WIRE_32BIT: u8 = 5; +const WIRE_LEN: u8 = 2; + +pub fn encode_fromradio_packet(packet: &MeshPacket) -> Option> { + + let mut mesh_buf: Vec = Vec::new(); + + if packet.from != 0 { + if !write_tag(1, WIRE_32BIT, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(&packet.from.to_le_bytes()).is_err() { return None; } + } + + if packet.to != 0 { + if !write_tag(2, WIRE_32BIT, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(&packet.to.to_le_bytes()).is_err() { return None; } + } + + if packet.channel != 0 { + if !write_tag(3, WIRE_VARINT, &mut mesh_buf) { return None; } + if !encode_varint(packet.channel as u64, &mut mesh_buf) { return None; } + } + + match &packet.payload { + PacketPayload::Decoded(data) => { + + let mut data_buf: Vec = Vec::new(); + + if !write_tag(1, WIRE_VARINT, &mut data_buf) { return None; } + if !encode_varint(data.port as u64, &mut data_buf) { return None; } + + if !data.payload.is_empty() { + if !write_tag(2, WIRE_LEN, &mut data_buf) { return None; } + if !encode_varint(data.payload.len() as u64, &mut data_buf) { return None; } + if data_buf.extend_from_slice(&data.payload).is_err() { return None; } + } + + if data.want_response { + if !write_tag(3, WIRE_VARINT, &mut data_buf) { return None; } + if data_buf.push(1).is_err() { return None; } + } + + if data.dest != 0 { + if !write_tag(4, WIRE_32BIT, &mut data_buf) { return None; } + if data_buf.extend_from_slice(&data.dest.to_le_bytes()).is_err() { return None; } + } + + if data.source != 0 { + if !write_tag(5, WIRE_32BIT, &mut data_buf) { return None; } + if data_buf.extend_from_slice(&data.source.to_le_bytes()).is_err() { return None; } + } + + if data.request_id != 0 { + if !write_tag(6, WIRE_32BIT, &mut data_buf) { return None; } + if data_buf.extend_from_slice(&data.request_id.to_le_bytes()).is_err() { return None; } + } + + if data.reply_id != 0 { + if !write_tag(7, WIRE_32BIT, &mut data_buf) { return None; } + if data_buf.extend_from_slice(&data.reply_id.to_le_bytes()).is_err() { return None; } + } + + if data.emoji != 0 { + if !write_tag(8, WIRE_32BIT, &mut data_buf) { return None; } + if data_buf.extend_from_slice(&data.emoji.to_le_bytes()).is_err() { return None; } + } + + if !data_buf.is_empty() { + if !write_tag(4, WIRE_LEN, &mut mesh_buf) { return None; } + if !encode_varint(data_buf.len() as u64, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(&data_buf).is_err() { return None; } + } + } + PacketPayload::Encrypted(encrypted) => { + + if !encrypted.is_empty() { + if !write_tag(5, WIRE_LEN, &mut mesh_buf) { return None; } + if !encode_varint(encrypted.len() as u64, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(encrypted).is_err() { return None; } + } + } + } + + if packet.id != 0 { + if !write_tag(6, WIRE_32BIT, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(&packet.id.to_le_bytes()).is_err() { return None; } + } + + if packet.rx_time != 0 { + if !write_tag(7, WIRE_32BIT, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(&packet.rx_time.to_le_bytes()).is_err() { return None; } + } + + if packet.rx_snr != 0.0 { + if !write_tag(8, WIRE_32BIT, &mut mesh_buf) { return None; } + if mesh_buf.extend_from_slice(&packet.rx_snr.to_bits().to_le_bytes()).is_err() { return None; } + } + + if packet.hop_limit != 0 { + if !write_tag(9, WIRE_VARINT, &mut mesh_buf) { return None; } + if !encode_varint(packet.hop_limit as u64, &mut mesh_buf) { return None; } + } + + if packet.want_ack { + if !write_tag(10, WIRE_VARINT, &mut mesh_buf) { return None; } + if mesh_buf.push(1).is_err() { return None; } + } + + let priority_val = packet.priority as u8; + if priority_val != 0 { + if !write_tag(11, WIRE_VARINT, &mut mesh_buf) { return None; } + if !encode_varint(priority_val as u64, &mut mesh_buf) { return None; } + } + + if packet.rx_rssi != 0 { + if !write_tag(12, WIRE_VARINT, &mut mesh_buf) { return None; } + if !encode_sint32(packet.rx_rssi, &mut mesh_buf) { return None; } + } + + let mut from_radio_buf: Vec = Vec::new(); + + if !mesh_buf.is_empty() { + if !write_tag(2, WIRE_LEN, &mut from_radio_buf) { return None; } + if !encode_varint(mesh_buf.len() as u64, &mut from_radio_buf) { return None; } + if from_radio_buf.extend_from_slice(&mesh_buf).is_err() { return None; } + } + + Some(from_radio_buf) +} + +impl Default for MeshtasticHandler { + fn default() -> Self { + Self::new(0) + } +} + +pub const MAX_SERIAL_PAYLOAD: usize = 512; + +pub const MAX_FRAME_SIZE: usize = 4 + MAX_SERIAL_PAYLOAD; + +#[derive(Debug, Clone)] +pub struct MeshtasticFrame { + + pub payload: Vec, +} + +impl MeshtasticFrame { + + pub fn new() -> Self { + Self { + payload: Vec::new(), + } + } + + pub fn with_payload(data: &[u8]) -> Option { + if data.len() > MAX_SERIAL_PAYLOAD { + return None; + } + let mut frame = Self::new(); + for &b in data { + let _ = frame.payload.push(b); + } + Some(frame) + } + + pub fn encode(&self) -> Vec { + let mut buf = Vec::new(); + + let _ = buf.push(SERIAL_SYNC[0]); + let _ = buf.push(SERIAL_SYNC[1]); + + let len = self.payload.len() as u16; + let _ = buf.push((len >> 8) as u8); + let _ = buf.push(len as u8); + + for &b in &self.payload { + let _ = buf.push(b); + } + + buf + } +} + +impl Default for MeshtasticFrame { + fn default() -> Self { + Self::new() + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum ParserState { + WaitSync1, + WaitSync2, + WaitLenHigh, + WaitLenLow, + WaitPayload, +} + +pub struct MeshtasticParser { + state: ParserState, + payload_len: u16, + payload_idx: u16, + payload: Vec, +} + +impl Default for MeshtasticParser { + fn default() -> Self { + Self::new() + } +} + +impl MeshtasticParser { + pub fn new() -> Self { + Self { + state: ParserState::WaitSync1, + payload_len: 0, + payload_idx: 0, + payload: Vec::new(), + } + } + + pub fn reset(&mut self) { + self.state = ParserState::WaitSync1; + self.payload.clear(); + self.payload_len = 0; + self.payload_idx = 0; + } + + pub fn feed(&mut self, byte: u8) -> Option { + match self.state { + ParserState::WaitSync1 => { + if byte == SERIAL_SYNC[0] { + self.state = ParserState::WaitSync2; + } + } + + ParserState::WaitSync2 => { + if byte == SERIAL_SYNC[1] { + self.state = ParserState::WaitLenHigh; + } else if byte == SERIAL_SYNC[0] { + + } else { + self.reset(); + } + } + + ParserState::WaitLenHigh => { + self.payload_len = (byte as u16) << 8; + self.state = ParserState::WaitLenLow; + } + + ParserState::WaitLenLow => { + self.payload_len |= byte as u16; + if self.payload_len > MAX_SERIAL_PAYLOAD as u16 { + self.reset(); + return None; + } + self.payload.clear(); + self.payload_idx = 0; + if self.payload_len == 0 { + + let frame = MeshtasticFrame::new(); + self.reset(); + return Some(frame); + } + self.state = ParserState::WaitPayload; + } + + ParserState::WaitPayload => { + let _ = self.payload.push(byte); + self.payload_idx += 1; + if self.payload_idx >= self.payload_len { + + let frame = MeshtasticFrame { + payload: self.payload.clone(), + }; + self.reset(); + return Some(frame); + } + } + } + None + } +} diff --git a/src/meshtastic/packet.rs b/src/meshtastic/packet.rs new file mode 100644 index 0000000..3e831fd --- /dev/null +++ b/src/meshtastic/packet.rs @@ -0,0 +1,388 @@ +use heapless::Vec; +use super::{MeshPacket, PacketPayload, Priority, LORA_HEADER_SIZE, MAX_LORA_PAYLOAD, MIC_SIZE, DEFAULT_HOP_LIMIT}; +use crate::crypto::sha256::Sha256; + +const OFFSET_TO: usize = 0; +const OFFSET_FROM: usize = 4; +const OFFSET_ID: usize = 8; +const OFFSET_FLAGS: usize = 12; +const OFFSET_CHANNEL_HASH: usize = 13; + +const FLAG_WANT_ACK: u8 = 0x01; +const FLAG_HOP_LIMIT_MASK: u8 = 0x0E; +const FLAG_HOP_LIMIT_SHIFT: u8 = 1; +const FLAG_CHANNEL_MASK: u8 = 0xF0; +const FLAG_CHANNEL_SHIFT: u8 = 4; + +pub fn parse_lora_packet(data: &[u8]) -> Option { + + if data.len() < LORA_HEADER_SIZE + MIC_SIZE { + return None; + } + + let to = u32::from_le_bytes([data[0], data[1], data[2], data[3]]); + let from = u32::from_le_bytes([data[4], data[5], data[6], data[7]]); + let id = u32::from_le_bytes([data[8], data[9], data[10], data[11]]); + let flags = data[OFFSET_FLAGS]; + let channel_hash = data[OFFSET_CHANNEL_HASH]; + + let want_ack = (flags & FLAG_WANT_ACK) != 0; + let hop_limit = (flags & FLAG_HOP_LIMIT_MASK) >> FLAG_HOP_LIMIT_SHIFT; + let channel = (flags & FLAG_CHANNEL_MASK) >> FLAG_CHANNEL_SHIFT; + + let payload_end = data.len() - MIC_SIZE; + let payload_start = LORA_HEADER_SIZE; + + if payload_end <= payload_start { + return None; + } + + let mut payload_data = Vec::new(); + payload_data.extend_from_slice(&data[payload_start..payload_end]).ok()?; + + let received_mic = &data[payload_end..]; + let computed_mic = compute_mic(&data[..payload_end]); + + if !constant_time_eq(received_mic, &computed_mic) { + + } + + Some(MeshPacket { + from, + to, + channel, + id, + hop_limit, + want_ack, + priority: Priority::Default, + rx_time: 0, + rx_snr: 0.0, + rx_rssi: 0, + payload: PacketPayload::Encrypted(payload_data), + }) +} + +pub fn build_lora_packet( + from: u32, + to: u32, + id: u32, + channel: u8, + hop_limit: u8, + want_ack: bool, + payload: &[u8], +) -> Option> { + let total_len = LORA_HEADER_SIZE + payload.len() + MIC_SIZE; + if total_len > 256 || payload.len() > MAX_LORA_PAYLOAD { + return None; + } + + let mut packet = Vec::new(); + + packet.extend_from_slice(&to.to_le_bytes()).ok()?; + packet.extend_from_slice(&from.to_le_bytes()).ok()?; + packet.extend_from_slice(&id.to_le_bytes()).ok()?; + + let flags = (if want_ack { FLAG_WANT_ACK } else { 0 }) + | ((hop_limit & 0x07) << FLAG_HOP_LIMIT_SHIFT) + | ((channel & 0x0F) << FLAG_CHANNEL_SHIFT); + packet.push(flags).ok()?; + + packet.push(compute_channel_hash(channel)).ok()?; + + packet.push(0).ok()?; + packet.push(0).ok()?; + + packet.extend_from_slice(payload).ok()?; + + let mic = compute_mic(&packet); + packet.extend_from_slice(&mic).ok()?; + + Some(packet) +} + +fn compute_mic(data: &[u8]) -> [u8; MIC_SIZE] { + let hash = Sha256::hash(data); + let mut mic = [0u8; MIC_SIZE]; + mic.copy_from_slice(&hash[..MIC_SIZE]); + mic +} + +fn compute_channel_hash(channel: u8) -> u8 { + + let mut h = channel.wrapping_mul(0x9E).wrapping_add(0x37); + h ^= h >> 4; + h.wrapping_mul(0xB5) +} + +fn constant_time_eq(a: &[u8], b: &[u8]) -> bool { + if a.len() != b.len() { + return false; + } + let mut result: u8 = 0; + for (x, y) in a.iter().zip(b.iter()) { + result |= x ^ y; + } + result == 0 +} + +pub struct PacketCache { + + entries: [(u32, u32, u32); 32], + + index: usize, +} + +impl PacketCache { + + pub const fn new() -> Self { + Self { + entries: [(0, 0, 0); 32], + index: 0, + } + } + + pub fn is_duplicate(&self, from: u32, packet_id: u32) -> bool { + for &(f, id, _) in &self.entries { + if f == from && id == packet_id && f != 0 { + return true; + } + } + false + } + + pub fn add(&mut self, from: u32, packet_id: u32, timestamp: u32) { + self.entries[self.index] = (from, packet_id, timestamp); + self.index = (self.index + 1) % self.entries.len(); + } + + pub fn check_and_add(&mut self, from: u32, packet_id: u32, timestamp: u32) -> bool { + if self.is_duplicate(from, packet_id) { + return true; + } + self.add(from, packet_id, timestamp); + false + } + + pub fn clear_old(&mut self, current_time: u32, max_age: u32) { + for entry in &mut self.entries { + if entry.0 != 0 && current_time.saturating_sub(entry.2) > max_age { + *entry = (0, 0, 0); + } + } + } +} + +impl Default for PacketCache { + fn default() -> Self { + Self::new() + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum RoutingDecision { + + Local, + + Forward, + + Drop, +} + +pub fn route_packet(packet: &MeshPacket, our_node_id: u32, cache: &mut PacketCache, timestamp: u32) -> RoutingDecision { + + if cache.check_and_add(packet.from, packet.id, timestamp) { + return RoutingDecision::Drop; + } + + let is_broadcast = packet.to == 0xFFFFFFFF; + let is_for_us = packet.to == our_node_id; + + if is_for_us || is_broadcast { + + if is_broadcast && packet.hop_limit > 0 { + return RoutingDecision::Forward; + } + return RoutingDecision::Local; + } + + if packet.hop_limit > 0 { + RoutingDecision::Forward + } else { + RoutingDecision::Drop + } +} + +pub fn create_forward_packet(original: &MeshPacket, payload: &[u8]) -> Option> { + if original.hop_limit == 0 { + return None; + } + + build_lora_packet( + original.from, + original.to, + original.id, + original.channel, + original.hop_limit - 1, + original.want_ack, + payload, + ) +} + +#[derive(Debug, Default)] +pub struct PacketStats { + + pub rx_total: u32, + + pub rx_local: u32, + + pub rx_forwarded: u32, + + pub rx_dropped_dup: u32, + + pub rx_dropped_expired: u32, + + pub rx_bad: u32, + + pub tx_total: u32, + + pub tx_retransmit: u32, + + pub tx_fail: u32, +} + +impl PacketStats { + + pub const fn new() -> Self { + Self { + rx_total: 0, + rx_local: 0, + rx_forwarded: 0, + rx_dropped_dup: 0, + rx_dropped_expired: 0, + rx_bad: 0, + tx_total: 0, + tx_retransmit: 0, + tx_fail: 0, + } + } + + pub fn record_rx(&mut self, decision: RoutingDecision) { + self.rx_total += 1; + match decision { + RoutingDecision::Local => self.rx_local += 1, + RoutingDecision::Forward => self.rx_forwarded += 1, + RoutingDecision::Drop => self.rx_dropped_dup += 1, + } + } + + pub fn record_rx_bad(&mut self) { + self.rx_total += 1; + self.rx_bad += 1; + } + + pub fn record_tx(&mut self, success: bool) { + self.tx_total += 1; + if !success { + self.tx_fail += 1; + } + } + + pub fn record_retransmit(&mut self) { + self.tx_retransmit += 1; + } +} + +#[derive(Clone)] +pub struct PendingAck { + + pub packet_id: u32, + + pub to: u32, + + pub packet_data: Vec, + + pub tx_count: u8, + + pub max_retransmit: u8, + + pub last_tx_time: u32, + + pub retransmit_timeout: u32, +} + +pub struct AckTracker { + + pending: [Option; 8], +} + +impl AckTracker { + + pub const fn new() -> Self { + Self { + pending: [None, None, None, None, None, None, None, None], + } + } + + pub fn add(&mut self, ack: PendingAck) -> bool { + for slot in &mut self.pending { + if slot.is_none() { + *slot = Some(ack); + return true; + } + } + false + } + + pub fn is_pending(&self, to: u32, packet_id: u32) -> bool { + self.pending.iter().any(|p| { + p.as_ref().map_or(false, |a| a.to == to && a.packet_id == packet_id) + }) + } + + pub fn handle_ack(&mut self, from: u32, request_id: u32) -> bool { + for slot in &mut self.pending { + if let Some(ref ack) = slot { + if ack.to == from && ack.packet_id == request_id { + *slot = None; + return true; + } + } + } + false + } + + pub fn get_retransmit(&mut self, current_time: u32) -> Option> { + for slot in &mut self.pending { + if let Some(ref mut ack) = slot { + let elapsed = current_time.saturating_sub(ack.last_tx_time); + if elapsed >= ack.retransmit_timeout { + if ack.tx_count < ack.max_retransmit { + ack.tx_count += 1; + ack.last_tx_time = current_time; + return Some(ack.packet_data.clone()); + } else { + + *slot = None; + } + } + } + } + None + } + + pub fn clear(&mut self) { + for slot in &mut self.pending { + *slot = None; + } + } + + pub fn pending_count(&self) -> usize { + self.pending.iter().filter(|p| p.is_some()).count() + } +} + +impl Default for AckTracker { + fn default() -> Self { + Self::new() + } +} diff --git a/src/meshtastic/protobuf.rs b/src/meshtastic/protobuf.rs new file mode 100644 index 0000000..5d37ea3 --- /dev/null +++ b/src/meshtastic/protobuf.rs @@ -0,0 +1,1281 @@ +use heapless::Vec; +use super::{DataPayload, Position, User, PortNum, HardwareModel, Role, LocationSource, MAX_LORA_PAYLOAD}; + +const WIRE_TYPE_VARINT: u8 = 0; + +const WIRE_TYPE_64BIT: u8 = 1; + +const WIRE_TYPE_LENGTH_DELIMITED: u8 = 2; + +const WIRE_TYPE_32BIT: u8 = 5; + +mod data_fields { + pub const PORTNUM: u32 = 1; + pub const PAYLOAD: u32 = 2; + pub const WANT_RESPONSE: u32 = 3; + pub const DEST: u32 = 4; + pub const SOURCE: u32 = 5; + pub const REQUEST_ID: u32 = 6; + pub const REPLY_ID: u32 = 7; + pub const EMOJI: u32 = 8; +} + +mod position_fields { + pub const LATITUDE_I: u32 = 1; + pub const LONGITUDE_I: u32 = 2; + pub const ALTITUDE: u32 = 3; + pub const TIME: u32 = 4; + pub const LOCATION_SOURCE: u32 = 5; + pub const ALTITUDE_SOURCE: u32 = 6; + pub const TIMESTAMP: u32 = 7; + pub const TIMESTAMP_MILLIS_ADJUST: u32 = 8; + pub const ALTITUDE_HAE: u32 = 9; + pub const ALTITUDE_GEOIDAL_SEPARATION: u32 = 10; + pub const PDOP: u32 = 11; + pub const HDOP: u32 = 12; + pub const VDOP: u32 = 13; + pub const GPS_ACCURACY: u32 = 14; + pub const GROUND_SPEED: u32 = 15; + pub const GROUND_TRACK: u32 = 16; + pub const FIX_QUALITY: u32 = 17; + pub const FIX_TYPE: u32 = 18; + pub const SATS_IN_VIEW: u32 = 19; + pub const SENSOR_ID: u32 = 20; + pub const NEXT_UPDATE: u32 = 21; + pub const SEQ_NUMBER: u32 = 22; +} + +mod user_fields { + pub const ID: u32 = 1; + pub const LONG_NAME: u32 = 2; + pub const SHORT_NAME: u32 = 3; + pub const MACADDR: u32 = 4; + pub const HW_MODEL: u32 = 5; + pub const IS_LICENSED: u32 = 6; + pub const ROLE: u32 = 7; +} + +pub struct ProtobufEncoder { + buffer: Vec, +} + +impl ProtobufEncoder { + + pub fn new() -> Self { + Self { + buffer: Vec::new(), + } + } + + pub fn finish(self) -> Vec { + self.buffer + } + + pub fn write_varint(&mut self, value: u64) -> bool { + let mut v = value; + loop { + let byte = (v & 0x7F) as u8; + v >>= 7; + if v == 0 { + return self.buffer.push(byte).is_ok(); + } else { + if self.buffer.push(byte | 0x80).is_err() { + return false; + } + } + } + } + + pub fn write_sint32(&mut self, value: i32) -> bool { + let encoded = ((value << 1) ^ (value >> 31)) as u32; + self.write_varint(encoded as u64) + } + + pub fn write_sint64(&mut self, value: i64) -> bool { + let encoded = ((value << 1) ^ (value >> 63)) as u64; + self.write_varint(encoded) + } + + pub fn write_tag(&mut self, field_number: u32, wire_type: u8) -> bool { + let tag = (field_number << 3) | (wire_type as u32); + self.write_varint(tag as u64) + } + + pub fn write_varint_field(&mut self, field_number: u32, value: u64) -> bool { + if value == 0 { + return true; + } + self.write_tag(field_number, WIRE_TYPE_VARINT) && self.write_varint(value) + } + + pub fn write_sint32_field(&mut self, field_number: u32, value: i32) -> bool { + if value == 0 { + return true; + } + self.write_tag(field_number, WIRE_TYPE_VARINT) && self.write_sint32(value) + } + + pub fn write_bool_field(&mut self, field_number: u32, value: bool) -> bool { + if !value { + return true; + } + self.write_tag(field_number, WIRE_TYPE_VARINT) && self.buffer.push(1).is_ok() + } + + pub fn write_bytes_field(&mut self, field_number: u32, data: &[u8]) -> bool { + if data.is_empty() { + return true; + } + self.write_tag(field_number, WIRE_TYPE_LENGTH_DELIMITED) + && self.write_varint(data.len() as u64) + && self.buffer.extend_from_slice(data).is_ok() + } + + pub fn write_string_field(&mut self, field_number: u32, s: &str) -> bool { + self.write_bytes_field(field_number, s.as_bytes()) + } + + pub fn write_fixed32_field(&mut self, field_number: u32, value: u32) -> bool { + if value == 0 { + return true; + } + if !self.write_tag(field_number, WIRE_TYPE_32BIT) { + return false; + } + let bytes = value.to_le_bytes(); + self.buffer.extend_from_slice(&bytes).is_ok() + } + + pub fn write_fixed64_field(&mut self, field_number: u32, value: u64) -> bool { + if value == 0 { + return true; + } + if !self.write_tag(field_number, WIRE_TYPE_64BIT) { + return false; + } + let bytes = value.to_le_bytes(); + self.buffer.extend_from_slice(&bytes).is_ok() + } + + pub fn write_message_field(&mut self, field_number: u32, encode_fn: F) -> bool + where + F: FnOnce(&mut ProtobufEncoder<256>) -> bool, + { + let mut nested = ProtobufEncoder::<256>::new(); + if !encode_fn(&mut nested) { + return false; + } + let nested_data = nested.finish(); + if nested_data.is_empty() { + return true; + } + self.write_bytes_field(field_number, &nested_data) + } +} + +impl Default for ProtobufEncoder { + fn default() -> Self { + Self::new() + } +} + +impl ProtobufEncoder { + + pub fn encode_varint_to_slice(value: u64, output: &mut Vec) { + let mut v = value; + loop { + let byte = (v & 0x7F) as u8; + v >>= 7; + if v == 0 { + let _ = output.push(byte); + break; + } else { + let _ = output.push(byte | 0x80); + } + } + } +} + +pub struct ProtobufDecoder<'a> { + data: &'a [u8], + pos: usize, +} + +impl<'a> ProtobufDecoder<'a> { + + pub fn new(data: &'a [u8]) -> Self { + Self { data, pos: 0 } + } + + pub fn has_more(&self) -> bool { + self.pos < self.data.len() + } + + pub fn read_varint(&mut self) -> Option { + let mut result: u64 = 0; + let mut shift = 0; + + loop { + if self.pos >= self.data.len() { + return None; + } + + let byte = self.data[self.pos]; + self.pos += 1; + + result |= ((byte & 0x7F) as u64) << shift; + + if byte & 0x80 == 0 { + return Some(result); + } + + shift += 7; + if shift >= 64 { + return None; + } + } + } + + pub fn read_sint32(&mut self) -> Option { + let encoded = self.read_varint()? as u32; + Some(((encoded >> 1) as i32) ^ -((encoded & 1) as i32)) + } + + pub fn read_sint64(&mut self) -> Option { + let encoded = self.read_varint()?; + Some(((encoded >> 1) as i64) ^ -((encoded & 1) as i64)) + } + + pub fn read_tag(&mut self) -> Option<(u32, u8)> { + let tag = self.read_varint()? as u32; + let field_number = tag >> 3; + let wire_type = (tag & 0x07) as u8; + Some((field_number, wire_type)) + } + + pub fn next_field(&mut self) -> Option<(u32, u8, &'a [u8])> { + if !self.has_more() { + return None; + } + + let (field_number, wire_type) = self.read_tag()?; + + let field_data = match wire_type { + WIRE_TYPE_VARINT => { + + let start = self.pos; + while self.pos < self.data.len() { + let byte = self.data[self.pos]; + self.pos += 1; + if byte & 0x80 == 0 { + break; + } + } + &self.data[start..self.pos] + } + WIRE_TYPE_64BIT => { + if self.pos + 8 > self.data.len() { + return None; + } + let result = &self.data[self.pos..self.pos + 8]; + self.pos += 8; + result + } + WIRE_TYPE_LENGTH_DELIMITED => { + let len = self.read_varint()? as usize; + if self.pos + len > self.data.len() { + return None; + } + let result = &self.data[self.pos..self.pos + len]; + self.pos += len; + result + } + WIRE_TYPE_32BIT => { + if self.pos + 4 > self.data.len() { + return None; + } + let result = &self.data[self.pos..self.pos + 4]; + self.pos += 4; + result + } + _ => return None, + }; + + Some((field_number, wire_type, field_data)) + } + + pub fn read_bytes(&mut self) -> Option<&'a [u8]> { + let len = self.read_varint()? as usize; + if self.pos + len > self.data.len() { + return None; + } + let result = &self.data[self.pos..self.pos + len]; + self.pos += len; + Some(result) + } + + pub fn read_fixed32(&mut self) -> Option { + if self.pos + 4 > self.data.len() { + return None; + } + let result = u32::from_le_bytes([ + self.data[self.pos], + self.data[self.pos + 1], + self.data[self.pos + 2], + self.data[self.pos + 3], + ]); + self.pos += 4; + Some(result) + } + + pub fn read_fixed64(&mut self) -> Option { + if self.pos + 8 > self.data.len() { + return None; + } + let result = u64::from_le_bytes([ + self.data[self.pos], + self.data[self.pos + 1], + self.data[self.pos + 2], + self.data[self.pos + 3], + self.data[self.pos + 4], + self.data[self.pos + 5], + self.data[self.pos + 6], + self.data[self.pos + 7], + ]); + self.pos += 8; + Some(result) + } + + pub fn skip_field(&mut self, wire_type: u8) -> bool { + match wire_type { + WIRE_TYPE_VARINT => self.read_varint().is_some(), + WIRE_TYPE_64BIT => { + if self.pos + 8 <= self.data.len() { + self.pos += 8; + true + } else { + false + } + } + WIRE_TYPE_LENGTH_DELIMITED => self.read_bytes().is_some(), + WIRE_TYPE_32BIT => { + if self.pos + 4 <= self.data.len() { + self.pos += 4; + true + } else { + false + } + } + _ => false, + } + } + + pub fn read_varint_from_slice(data: &[u8]) -> Option { + let mut result: u64 = 0; + let mut shift = 0; + + for &byte in data.iter() { + result |= ((byte & 0x7F) as u64) << shift; + + if byte & 0x80 == 0 { + return Some(result); + } + + shift += 7; + if shift >= 64 { + return None; + } + } + + None + } + + pub fn read_varint_advancing(data: &mut &[u8]) -> Option { + let mut result: u64 = 0; + let mut shift = 0; + let mut consumed = 0; + + for (i, &byte) in data.iter().enumerate() { + result |= ((byte & 0x7F) as u64) << shift; + consumed = i + 1; + + if byte & 0x80 == 0 { + *data = &data[consumed..]; + return Some(result); + } + + shift += 7; + if shift >= 64 { + return None; + } + } + + None + } + + pub fn read_bytes_from_slice<'b>(data: &mut &'b [u8]) -> Option<&'b [u8]> { + let len = Self::read_varint_advancing(data)? as usize; + if data.len() < len { + return None; + } + let result = &data[..len]; + *data = &data[len..]; + Some(result) + } + + pub fn read_tag_from_slice(data: &mut &[u8]) -> Option<(u32, u8)> { + let tag = Self::read_varint_advancing(data)? as u32; + let field_number = tag >> 3; + let wire_type = (tag & 0x07) as u8; + Some((field_number, wire_type)) + } + + pub fn skip_field_from_slice(data: &mut &[u8], wire_type: u8) -> bool { + match wire_type { + WIRE_TYPE_VARINT => Self::read_varint_advancing(data).is_some(), + WIRE_TYPE_64BIT => { + if data.len() >= 8 { + *data = &data[8..]; + true + } else { + false + } + } + WIRE_TYPE_LENGTH_DELIMITED => Self::read_bytes_from_slice(data).is_some(), + WIRE_TYPE_32BIT => { + if data.len() >= 4 { + *data = &data[4..]; + true + } else { + false + } + } + _ => false, + } + } +} + +pub fn encode_data(data: &DataPayload) -> Option> { + let mut encoder = ProtobufEncoder::::new(); + + encoder.write_varint_field(data_fields::PORTNUM, data.port as u64); + encoder.write_bytes_field(data_fields::PAYLOAD, &data.payload); + encoder.write_bool_field(data_fields::WANT_RESPONSE, data.want_response); + encoder.write_varint_field(data_fields::DEST, data.dest as u64); + encoder.write_varint_field(data_fields::SOURCE, data.source as u64); + encoder.write_varint_field(data_fields::REQUEST_ID, data.request_id as u64); + encoder.write_varint_field(data_fields::REPLY_ID, data.reply_id as u64); + encoder.write_varint_field(data_fields::EMOJI, data.emoji as u64); + + Some(encoder.finish()) +} + +pub fn encode_position(pos: &Position) -> Option> { + let mut encoder = ProtobufEncoder::::new(); + + encoder.write_sint32_field(position_fields::LATITUDE_I, pos.latitude_i); + encoder.write_sint32_field(position_fields::LONGITUDE_I, pos.longitude_i); + encoder.write_sint32_field(position_fields::ALTITUDE, pos.altitude); + encoder.write_varint_field(position_fields::TIME, pos.time as u64); + encoder.write_varint_field(position_fields::LOCATION_SOURCE, pos.location_source as u64); + encoder.write_varint_field(position_fields::ALTITUDE_SOURCE, pos.altitude_source as u64); + encoder.write_varint_field(position_fields::TIMESTAMP, pos.timestamp as u64); + encoder.write_varint_field(position_fields::PDOP, pos.pdop as u64); + encoder.write_varint_field(position_fields::HDOP, pos.hdop as u64); + encoder.write_varint_field(position_fields::SATS_IN_VIEW, pos.sats_in_view as u64); + encoder.write_varint_field(position_fields::GROUND_SPEED, pos.ground_speed as u64); + encoder.write_varint_field(position_fields::GROUND_TRACK, pos.ground_track as u64); + encoder.write_varint_field(position_fields::FIX_QUALITY, pos.fix_quality as u64); + encoder.write_varint_field(position_fields::FIX_TYPE, pos.fix_type as u64); + encoder.write_varint_field(position_fields::SEQ_NUMBER, pos.seq_number as u64); + + Some(encoder.finish()) +} + +pub fn encode_user(user: &User) -> Option> { + let mut encoder = ProtobufEncoder::::new(); + + let mut id_str = [0u8; 17]; + id_str[0] = b'!'; + hex_encode(&user.id, &mut id_str[1..]); + encoder.write_bytes_field(user_fields::ID, &id_str[..17]); + + encoder.write_bytes_field(user_fields::LONG_NAME, &user.long_name); + encoder.write_bytes_field(user_fields::SHORT_NAME, &user.short_name); + encoder.write_bytes_field(user_fields::MACADDR, &user.id[2..]); + encoder.write_varint_field(user_fields::HW_MODEL, user.hw_model as u64); + encoder.write_bool_field(user_fields::IS_LICENSED, user.is_licensed); + encoder.write_varint_field(user_fields::ROLE, user.role as u64); + + Some(encoder.finish()) +} + +pub fn decode_data(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + let mut result = DataPayload::default(); + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + match field_number { + data_fields::PORTNUM if wire_type == WIRE_TYPE_VARINT => { + result.port = PortNum::from(decoder.read_varint()? as u32); + } + data_fields::PAYLOAD if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let bytes = decoder.read_bytes()?; + result.payload = Vec::from_slice(bytes).ok()?; + } + data_fields::WANT_RESPONSE if wire_type == WIRE_TYPE_VARINT => { + result.want_response = decoder.read_varint()? != 0; + } + data_fields::DEST if wire_type == WIRE_TYPE_VARINT => { + result.dest = decoder.read_varint()? as u32; + } + data_fields::SOURCE if wire_type == WIRE_TYPE_VARINT => { + result.source = decoder.read_varint()? as u32; + } + data_fields::REQUEST_ID if wire_type == WIRE_TYPE_VARINT => { + result.request_id = decoder.read_varint()? as u32; + } + data_fields::REPLY_ID if wire_type == WIRE_TYPE_VARINT => { + result.reply_id = decoder.read_varint()? as u32; + } + data_fields::EMOJI if wire_type == WIRE_TYPE_VARINT => { + result.emoji = decoder.read_varint()? as u32; + } + _ => { + if !decoder.skip_field(wire_type) { + return None; + } + } + } + } + + Some(result) +} + +pub fn decode_position(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + let mut result = Position::default(); + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + match field_number { + position_fields::LATITUDE_I if wire_type == WIRE_TYPE_VARINT => { + result.latitude_i = decoder.read_sint32()?; + } + position_fields::LONGITUDE_I if wire_type == WIRE_TYPE_VARINT => { + result.longitude_i = decoder.read_sint32()?; + } + position_fields::ALTITUDE if wire_type == WIRE_TYPE_VARINT => { + result.altitude = decoder.read_sint32()?; + } + position_fields::TIME if wire_type == WIRE_TYPE_VARINT => { + result.time = decoder.read_varint()? as u32; + } + position_fields::LOCATION_SOURCE if wire_type == WIRE_TYPE_VARINT => { + result.location_source = match decoder.read_varint()? as u8 { + 1 => LocationSource::Manual, + 2 => LocationSource::InternalGps, + 3 => LocationSource::ExternalGps, + _ => LocationSource::Unset, + }; + } + position_fields::ALTITUDE_SOURCE if wire_type == WIRE_TYPE_VARINT => { + result.altitude_source = match decoder.read_varint()? as u8 { + 1 => LocationSource::Manual, + 2 => LocationSource::InternalGps, + 3 => LocationSource::ExternalGps, + _ => LocationSource::Unset, + }; + } + position_fields::TIMESTAMP if wire_type == WIRE_TYPE_VARINT => { + result.timestamp = decoder.read_varint()? as u32; + } + position_fields::PDOP if wire_type == WIRE_TYPE_VARINT => { + result.pdop = decoder.read_varint()? as u32; + } + position_fields::HDOP if wire_type == WIRE_TYPE_VARINT => { + result.hdop = decoder.read_varint()? as u32; + } + position_fields::SATS_IN_VIEW if wire_type == WIRE_TYPE_VARINT => { + result.sats_in_view = decoder.read_varint()? as u32; + } + position_fields::GROUND_SPEED if wire_type == WIRE_TYPE_VARINT => { + result.ground_speed = decoder.read_varint()? as u32; + } + position_fields::GROUND_TRACK if wire_type == WIRE_TYPE_VARINT => { + result.ground_track = decoder.read_varint()? as u32; + } + position_fields::FIX_QUALITY if wire_type == WIRE_TYPE_VARINT => { + result.fix_quality = decoder.read_varint()? as u32; + } + position_fields::FIX_TYPE if wire_type == WIRE_TYPE_VARINT => { + result.fix_type = decoder.read_varint()? as u32; + } + position_fields::SEQ_NUMBER if wire_type == WIRE_TYPE_VARINT => { + result.seq_number = decoder.read_varint()? as u32; + } + _ => { + if !decoder.skip_field(wire_type) { + return None; + } + } + } + } + + Some(result) +} + +pub fn decode_user(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + let mut result = User { + id: [0u8; 8], + long_name: Vec::new(), + short_name: Vec::new(), + hw_model: HardwareModel::Unset, + is_licensed: false, + role: Role::Client, + }; + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + match field_number { + user_fields::ID if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let bytes = decoder.read_bytes()?; + + if bytes.len() >= 17 && bytes[0] == b'!' { + hex_decode(&bytes[1..17], &mut result.id); + } else if bytes.len() == 8 { + result.id.copy_from_slice(bytes); + } + } + user_fields::LONG_NAME if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let bytes = decoder.read_bytes()?; + result.long_name = Vec::from_slice(bytes).ok()?; + } + user_fields::SHORT_NAME if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let bytes = decoder.read_bytes()?; + result.short_name = Vec::from_slice(bytes).ok()?; + } + user_fields::MACADDR if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let bytes = decoder.read_bytes()?; + if bytes.len() == 6 { + result.id[2..].copy_from_slice(bytes); + } + } + user_fields::HW_MODEL if wire_type == WIRE_TYPE_VARINT => { + result.hw_model = match decoder.read_varint()? as u16 { + 1 => HardwareModel::TloraV2, + 2 => HardwareModel::TloraV1, + 4 => HardwareModel::Tbeam, + 5 => HardwareModel::HeltecV2_0, + 9 => HardwareModel::Rak4631, + 10 => HardwareModel::HeltecV2_1, + 34 => HardwareModel::HeltecWifiLoraV3, + 255 => HardwareModel::PrivateHw, + _ => HardwareModel::Unset, + }; + } + user_fields::IS_LICENSED if wire_type == WIRE_TYPE_VARINT => { + result.is_licensed = decoder.read_varint()? != 0; + } + user_fields::ROLE if wire_type == WIRE_TYPE_VARINT => { + result.role = match decoder.read_varint()? as u8 { + 0 => Role::Client, + 1 => Role::ClientMute, + 2 => Role::Router, + 3 => Role::RouterClient, + 4 => Role::Repeater, + 5 => Role::Tracker, + 6 => Role::Sensor, + 7 => Role::Tak, + 8 => Role::ClientHidden, + 9 => Role::LostAndFound, + 10 => Role::TakTracker, + _ => Role::Client, + }; + } + _ => { + if !decoder.skip_field(wire_type) { + return None; + } + } + } + } + + Some(result) +} + +fn hex_encode(data: &[u8], output: &mut [u8]) { + const HEX_CHARS: &[u8; 16] = b"0123456789abcdef"; + for (i, &byte) in data.iter().enumerate() { + if i * 2 + 1 < output.len() { + output[i * 2] = HEX_CHARS[(byte >> 4) as usize]; + output[i * 2 + 1] = HEX_CHARS[(byte & 0x0F) as usize]; + } + } +} + +fn hex_decode(data: &[u8], output: &mut [u8]) { + fn hex_val(c: u8) -> u8 { + match c { + b'0'..=b'9' => c - b'0', + b'a'..=b'f' => c - b'a' + 10, + b'A'..=b'F' => c - b'A' + 10, + _ => 0, + } + } + + for i in 0..output.len() { + if i * 2 + 1 < data.len() { + output[i] = (hex_val(data[i * 2]) << 4) | hex_val(data[i * 2 + 1]); + } + } +} + +mod nodeinfo_fields { + pub const NUM: u32 = 1; + pub const USER: u32 = 2; + pub const POSITION: u32 = 3; + pub const SNR: u32 = 4; + pub const LAST_HEARD: u32 = 5; + pub const DEVICE_METRICS: u32 = 6; +} + +pub fn encode_nodeinfo( + num: u32, + user: Option<&User>, + position: Option<&Position>, + snr: f32, + last_heard: u32, +) -> Option> { + let mut encoder = ProtobufEncoder::::new(); + + encoder.write_varint_field(nodeinfo_fields::NUM, num as u64); + + if let Some(u) = user { + let user_data = encode_user(u)?; + encoder.write_bytes_field(nodeinfo_fields::USER, &user_data); + } + + if let Some(p) = position { + let pos_data = encode_position(p)?; + encoder.write_bytes_field(nodeinfo_fields::POSITION, &pos_data); + } + + if snr != 0.0 { + encoder.write_fixed32_field(nodeinfo_fields::SNR, snr.to_bits()); + } + + encoder.write_varint_field(nodeinfo_fields::LAST_HEARD, last_heard as u64); + + Some(encoder.finish()) +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum RoutingError { + None = 0, + NoRoute = 1, + GotNak = 2, + Timeout = 3, + NoInterface = 4, + MaxRetransmit = 5, + NoChannel = 6, + TooLarge = 7, + NoResponse = 8, + DutyCycleLimit = 9, + BadRequest = 32, + NotAuthorized = 33, +} + +mod routing_fields { + pub const ROUTE_REQUEST: u32 = 1; + pub const ROUTE_REPLY: u32 = 2; + pub const ERROR_REASON: u32 = 3; +} + +pub fn encode_routing_error(error: RoutingError) -> Option> { + let mut encoder = ProtobufEncoder::<16>::new(); + encoder.write_varint_field(routing_fields::ERROR_REASON, error as u64); + Some(encoder.finish()) +} + +pub fn decode_routing_error(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + if field_number == routing_fields::ERROR_REASON && wire_type == WIRE_TYPE_VARINT { + return Some(match decoder.read_varint()? as u8 { + 0 => RoutingError::None, + 1 => RoutingError::NoRoute, + 2 => RoutingError::GotNak, + 3 => RoutingError::Timeout, + 4 => RoutingError::NoInterface, + 5 => RoutingError::MaxRetransmit, + 6 => RoutingError::NoChannel, + 7 => RoutingError::TooLarge, + 8 => RoutingError::NoResponse, + 9 => RoutingError::DutyCycleLimit, + 32 => RoutingError::BadRequest, + 33 => RoutingError::NotAuthorized, + _ => RoutingError::None, + }); + } else { + decoder.skip_field(wire_type); + } + } + + None +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum AdminOp { + GetChannelRequest = 1, + GetChannelResponse = 2, + GetOwnerRequest = 3, + GetOwnerResponse = 4, + GetConfigRequest = 5, + GetConfigResponse = 6, + GetModuleConfigRequest = 7, + GetModuleConfigResponse = 8, + GetCannedMessageRequest = 10, + GetCannedMessageResponse = 11, + GetDeviceMetadataRequest = 12, + GetDeviceMetadataResponse = 13, + GetRingtoneRequest = 14, + GetRingtoneResponse = 15, + GetDeviceConnectionStatusRequest = 16, + GetDeviceConnectionStatusResponse = 17, + SetOwner = 32, + SetChannel = 33, + SetConfig = 34, + SetModuleConfig = 35, + SetCannedMessageModule = 36, + SetRingtoneMessage = 37, + RemoveByNodenum = 38, + SetFavoriteNode = 39, + RemoveFavoriteNode = 40, + BeginEditSettings = 64, + CommitEditSettings = 65, + RebootOtaSeconds = 95, + ExitSimulator = 96, + RebootSeconds = 97, + ShutdownSeconds = 98, + FactoryResetDevice = 99, + NodedbReset = 100, +} + +mod telemetry_fields { + pub const TIME: u32 = 1; + pub const DEVICE_METRICS: u32 = 2; + pub const ENVIRONMENT_METRICS: u32 = 3; + pub const AIR_QUALITY_METRICS: u32 = 4; + pub const POWER_METRICS: u32 = 5; +} + +mod device_metrics_fields { + pub const BATTERY_LEVEL: u32 = 1; + pub const VOLTAGE: u32 = 2; + pub const CHANNEL_UTILIZATION: u32 = 3; + pub const AIR_UTIL_TX: u32 = 4; + pub const UPTIME_SECONDS: u32 = 5; +} + +#[derive(Debug, Clone, Default)] +pub struct DeviceMetrics { + + pub battery_level: u32, + + pub voltage: u32, + + pub channel_utilization: u32, + + pub air_util_tx: u32, + + pub uptime_seconds: u32, +} + +pub fn encode_device_metrics(metrics: &DeviceMetrics) -> Option> { + let mut encoder = ProtobufEncoder::<64>::new(); + + encoder.write_varint_field(device_metrics_fields::BATTERY_LEVEL, metrics.battery_level as u64); + encoder.write_varint_field(device_metrics_fields::VOLTAGE, metrics.voltage as u64); + encoder.write_varint_field(device_metrics_fields::CHANNEL_UTILIZATION, metrics.channel_utilization as u64); + encoder.write_varint_field(device_metrics_fields::AIR_UTIL_TX, metrics.air_util_tx as u64); + encoder.write_varint_field(device_metrics_fields::UPTIME_SECONDS, metrics.uptime_seconds as u64); + + Some(encoder.finish()) +} + +pub fn encode_telemetry(time: u32, metrics: &DeviceMetrics) -> Option> { + let mut encoder = ProtobufEncoder::::new(); + + encoder.write_varint_field(telemetry_fields::TIME, time as u64); + + let metrics_data = encode_device_metrics(metrics)?; + encoder.write_bytes_field(telemetry_fields::DEVICE_METRICS, &metrics_data); + + Some(encoder.finish()) +} + +pub fn decode_device_metrics(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + let mut result = DeviceMetrics::default(); + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + match field_number { + device_metrics_fields::BATTERY_LEVEL if wire_type == WIRE_TYPE_VARINT => { + result.battery_level = decoder.read_varint()? as u32; + } + device_metrics_fields::VOLTAGE if wire_type == WIRE_TYPE_VARINT => { + result.voltage = decoder.read_varint()? as u32; + } + device_metrics_fields::CHANNEL_UTILIZATION if wire_type == WIRE_TYPE_VARINT => { + result.channel_utilization = decoder.read_varint()? as u32; + } + device_metrics_fields::AIR_UTIL_TX if wire_type == WIRE_TYPE_VARINT => { + result.air_util_tx = decoder.read_varint()? as u32; + } + device_metrics_fields::UPTIME_SECONDS if wire_type == WIRE_TYPE_VARINT => { + result.uptime_seconds = decoder.read_varint()? as u32; + } + _ => { + decoder.skip_field(wire_type); + } + } + } + + Some(result) +} + +mod to_radio_fields { + pub const PACKET: u32 = 1; + pub const WANT_CONFIG_ID: u32 = 3; + pub const DISCONNECT: u32 = 4; +} + +mod from_radio_fields { + pub const ID: u32 = 1; + pub const PACKET: u32 = 2; + pub const MY_INFO: u32 = 3; + pub const NODE_INFO: u32 = 4; + pub const CONFIG: u32 = 5; + pub const LOG_RECORD: u32 = 6; + pub const CONFIG_COMPLETE_ID: u32 = 7; + pub const REBOOTED: u32 = 8; + pub const MODULE_CONFIG: u32 = 9; + pub const CHANNEL: u32 = 10; + pub const QUEUED_TEXT_MESSAGE_ACK: u32 = 11; + pub const XM0DEM: u32 = 12; + pub const METADATA: u32 = 13; + pub const MQTTCLIENT_PROXY_MESSAGE: u32 = 14; +} + +mod mesh_packet_fields { + pub const FROM: u32 = 1; + pub const TO: u32 = 2; + pub const CHANNEL: u32 = 3; + pub const ENCRYPTED: u32 = 4; + pub const DECODED: u32 = 5; + pub const ID: u32 = 6; + pub const RX_TIME: u32 = 7; + pub const RX_SNR: u32 = 8; + pub const HOP_LIMIT: u32 = 9; + pub const WANT_ACK: u32 = 10; + pub const PRIORITY: u32 = 11; + pub const RX_RSSI: u32 = 12; + pub const DELAYED: u32 = 13; + pub const VIA_MQTT: u32 = 14; + pub const HOP_START: u32 = 15; +} + +pub enum ToRadio { + + Packet(super::MeshPacket), + + WantConfigId(u32), + + Disconnect, +} + +pub fn decode_to_radio(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + match field_number { + to_radio_fields::PACKET if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let packet_data = decoder.read_bytes()?; + let packet = decode_mesh_packet(packet_data)?; + return Some(ToRadio::Packet(packet)); + } + to_radio_fields::WANT_CONFIG_ID if wire_type == WIRE_TYPE_VARINT => { + let id = decoder.read_varint()? as u32; + return Some(ToRadio::WantConfigId(id)); + } + to_radio_fields::DISCONNECT if wire_type == WIRE_TYPE_VARINT => { + decoder.read_varint()?; + return Some(ToRadio::Disconnect); + } + _ => { + if !decoder.skip_field(wire_type) { + return None; + } + } + } + } + + None +} + +pub fn decode_mesh_packet(data: &[u8]) -> Option { + let mut decoder = ProtobufDecoder::new(data); + let mut packet = super::MeshPacket::default(); + + while decoder.has_more() { + let (field_number, wire_type) = decoder.read_tag()?; + + match field_number { + mesh_packet_fields::FROM if wire_type == WIRE_TYPE_VARINT => { + packet.from = decoder.read_varint()? as u32; + } + mesh_packet_fields::TO if wire_type == WIRE_TYPE_VARINT => { + packet.to = decoder.read_varint()? as u32; + } + mesh_packet_fields::CHANNEL if wire_type == WIRE_TYPE_VARINT => { + packet.channel = decoder.read_varint()? as u8; + } + mesh_packet_fields::ENCRYPTED if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let bytes = decoder.read_bytes()?; + packet.payload = super::PacketPayload::Encrypted( + heapless::Vec::from_slice(bytes).ok()? + ); + } + mesh_packet_fields::DECODED if wire_type == WIRE_TYPE_LENGTH_DELIMITED => { + let decoded_data = decoder.read_bytes()?; + if let Some(data_payload) = decode_data(decoded_data) { + packet.payload = super::PacketPayload::Decoded(data_payload); + } + } + mesh_packet_fields::ID if wire_type == WIRE_TYPE_VARINT => { + packet.id = decoder.read_varint()? as u32; + } + mesh_packet_fields::RX_TIME if wire_type == WIRE_TYPE_VARINT => { + packet.rx_time = decoder.read_varint()? as u32; + } + mesh_packet_fields::RX_SNR if wire_type == WIRE_TYPE_32BIT => { + let bits = decoder.read_fixed32()?; + packet.rx_snr = f32::from_bits(bits); + } + mesh_packet_fields::HOP_LIMIT if wire_type == WIRE_TYPE_VARINT => { + packet.hop_limit = decoder.read_varint()? as u8; + } + mesh_packet_fields::WANT_ACK if wire_type == WIRE_TYPE_VARINT => { + packet.want_ack = decoder.read_varint()? != 0; + } + mesh_packet_fields::PRIORITY if wire_type == WIRE_TYPE_VARINT => { + packet.priority = super::Priority::from(decoder.read_varint()? as u8); + } + mesh_packet_fields::RX_RSSI if wire_type == WIRE_TYPE_VARINT => { + packet.rx_rssi = decoder.read_varint()? as i32; + } + _ => { + if !decoder.skip_field(wire_type) { + return None; + } + } + } + } + + Some(packet) +} + +pub fn encode_mesh_packet(packet: &super::MeshPacket) -> Option> { + let mut encoder = ProtobufEncoder::<256>::new(); + + encoder.write_varint_field(mesh_packet_fields::FROM, packet.from as u64); + encoder.write_varint_field(mesh_packet_fields::TO, packet.to as u64); + encoder.write_varint_field(mesh_packet_fields::CHANNEL, packet.channel as u64); + + match &packet.payload { + super::PacketPayload::Encrypted(data) => { + encoder.write_bytes_field(mesh_packet_fields::ENCRYPTED, data); + } + super::PacketPayload::Decoded(data) => { + let encoded = encode_data(data)?; + encoder.write_bytes_field(mesh_packet_fields::DECODED, &encoded); + } + } + + encoder.write_varint_field(mesh_packet_fields::ID, packet.id as u64); + encoder.write_varint_field(mesh_packet_fields::RX_TIME, packet.rx_time as u64); + + if packet.rx_snr != 0.0 { + encoder.write_fixed32_field(mesh_packet_fields::RX_SNR, packet.rx_snr.to_bits()); + } + + encoder.write_varint_field(mesh_packet_fields::HOP_LIMIT, packet.hop_limit as u64); + encoder.write_bool_field(mesh_packet_fields::WANT_ACK, packet.want_ack); + encoder.write_varint_field(mesh_packet_fields::PRIORITY, packet.priority as u64); + + if packet.rx_rssi != 0 { + + let encoded_rssi = ((packet.rx_rssi << 1) ^ (packet.rx_rssi >> 31)) as u32; + encoder.write_varint_field(mesh_packet_fields::RX_RSSI, encoded_rssi as u64); + } + + Some(encoder.finish()) +} + +fn next_from_radio_id() -> u32 { + use core::sync::atomic::{AtomicU32, Ordering}; + static FROM_RADIO_ID: AtomicU32 = AtomicU32::new(0); + FROM_RADIO_ID.fetch_add(1, Ordering::SeqCst).wrapping_add(1) +} + +pub fn encode_from_radio_packet(packet: &super::MeshPacket) -> Option> { + let mut encoder = ProtobufEncoder::<512>::new(); + + encoder.write_varint_field(from_radio_fields::ID, next_from_radio_id() as u64); + + let packet_data = encode_mesh_packet(packet)?; + encoder.write_bytes_field(from_radio_fields::PACKET, &packet_data); + + Some(encoder.finish()) +} + +mod my_info_fields { + pub const MY_NODE_NUM: u32 = 1; + pub const REBOOT_COUNT: u32 = 8; + pub const MIN_APP_VERSION: u32 = 11; + pub const DEVICE_ID: u32 = 12; +} + +pub fn encode_from_radio_my_info( + node_num: u32, + reboot_count: u32, +) -> Option> { + let mut encoder = ProtobufEncoder::<512>::new(); + + encoder.write_varint_field(from_radio_fields::ID, next_from_radio_id() as u64); + + encoder.write_message_field(from_radio_fields::MY_INFO, |inner| { + inner.write_varint_field(my_info_fields::MY_NODE_NUM, node_num as u64); + inner.write_varint_field(my_info_fields::REBOOT_COUNT, reboot_count as u64); + inner.write_varint_field(my_info_fields::MIN_APP_VERSION, 20300); + true + }); + + Some(encoder.finish()) +} + +mod node_info_fields { + pub const NUM: u32 = 1; + pub const USER: u32 = 2; + pub const POSITION: u32 = 3; + pub const SNR: u32 = 4; + pub const LAST_HEARD: u32 = 5; + pub const DEVICE_METRICS: u32 = 6; + pub const CHANNEL: u32 = 7; + pub const VIA_MQTT: u32 = 8; + pub const HOPS_AWAY: u32 = 9; + pub const IS_FAVORITE: u32 = 10; +} + +pub fn encode_from_radio_node_info( + node_info: &super::NodeInfo, +) -> Option> { + let mut encoder = ProtobufEncoder::<512>::new(); + + encoder.write_varint_field(from_radio_fields::ID, next_from_radio_id() as u64); + + encoder.write_message_field(from_radio_fields::NODE_INFO, |inner| { + inner.write_varint_field(node_info_fields::NUM, node_info.num as u64); + + if let Some(ref user) = node_info.user { + let user_data = encode_user(user); + if let Some(data) = user_data { + inner.write_bytes_field(node_info_fields::USER, &data); + } + } + + if let Some(ref position) = node_info.position { + let pos_data = encode_position(position); + if let Some(data) = pos_data { + inner.write_bytes_field(node_info_fields::POSITION, &data); + } + } + + if node_info.snr != 0.0 { + inner.write_fixed32_field(node_info_fields::SNR, node_info.snr.to_bits()); + } + inner.write_varint_field(node_info_fields::LAST_HEARD, node_info.last_heard as u64); + + true + }); + + Some(encoder.finish()) +} + +mod channel_fields { + pub const INDEX: u32 = 1; + pub const SETTINGS: u32 = 2; + pub const ROLE: u32 = 3; +} + +mod channel_settings_fields { + pub const CHANNEL_NUM: u32 = 1; + pub const PSK: u32 = 2; + pub const NAME: u32 = 3; + pub const ID: u32 = 4; + pub const UPLINK_ENABLED: u32 = 5; + pub const DOWNLINK_ENABLED: u32 = 6; + pub const MODULE_SETTINGS: u32 = 7; +} + +pub fn encode_from_radio_channel( + index: u8, + channel: &super::Channel, + is_primary: bool, +) -> Option> { + let mut encoder = ProtobufEncoder::<512>::new(); + + encoder.write_varint_field(from_radio_fields::ID, next_from_radio_id() as u64); + + encoder.write_message_field(from_radio_fields::CHANNEL, |inner| { + inner.write_varint_field(channel_fields::INDEX, index as u64); + + inner.write_message_field(channel_fields::SETTINGS, |settings| { + settings.write_varint_field(channel_settings_fields::CHANNEL_NUM, index as u64); + + settings.write_string_field(channel_settings_fields::NAME, channel.name_str()); + true + }); + + let role = if is_primary { 1u64 } else { 2u64 }; + inner.write_varint_field(channel_fields::ROLE, role); + + true + }); + + Some(encoder.finish()) +} + +pub fn encode_from_radio_config_complete(config_id: u32) -> Option> { + let mut encoder = ProtobufEncoder::<512>::new(); + + encoder.write_varint_field(from_radio_fields::ID, next_from_radio_id() as u64); + encoder.write_varint_field(from_radio_fields::CONFIG_COMPLETE_ID, config_id as u64); + + Some(encoder.finish()) +} diff --git a/src/onion.rs b/src/onion.rs new file mode 100644 index 0000000..46ae463 --- /dev/null +++ b/src/onion.rs @@ -0,0 +1,368 @@ +use crate::crypto::{ + x25519::x25519, + hkdf::Hkdf, + aes::Aes256, + sha256::Sha256, +}; +use crate::transport::{NODE_HINT_SIZE, AUTH_TAG_SIZE}; +use heapless::Vec as HeaplessVec; + +#[inline(never)] +fn constant_time_eq(a: &[u8], b: &[u8]) -> bool { + if a.len() != b.len() { + return false; + } + + let mut result: u8 = 0; + for (x, y) in a.iter().zip(b.iter()) { + result |= x ^ y; + } + + unsafe { + core::ptr::read_volatile(&result) == 0 + } +} + +pub const MAX_HOPS: usize = 7; + +pub const MIN_HOPS: usize = 3; + +pub const HOP_OVERHEAD: usize = NODE_HINT_SIZE + AUTH_TAG_SIZE; + +const ONION_KEY_INFO: &[u8] = b"lunarpunk-onion-key-v1"; + +const ONION_BLIND_INFO: &[u8] = b"lunarpunk-onion-blind-v1"; + +#[derive(Debug, Clone)] +pub struct RouteHop { + + pub hint: u16, + + pub public_key: [u8; 32], +} + +#[derive(Debug, Clone)] +pub struct OnionRoute { + + pub hops: HeaplessVec, +} + +impl OnionRoute { + + pub fn new(hops: &[RouteHop]) -> Option { + if hops.len() < MIN_HOPS || hops.len() > MAX_HOPS { + return None; + } + + let mut route_hops = HeaplessVec::new(); + for hop in hops { + route_hops.push(hop.clone()).ok()?; + } + + Some(Self { hops: route_hops }) + } + + pub fn len(&self) -> usize { + self.hops.len() + } + + pub fn is_empty(&self) -> bool { + self.hops.is_empty() + } + + pub fn entry_hint(&self) -> u16 { + self.hops.first().map(|h| h.hint).unwrap_or(0) + } + + pub fn exit_hint(&self) -> u16 { + self.hops.last().map(|h| h.hint).unwrap_or(0) + } + + pub fn overhead(&self) -> usize { + self.hops.len() * HOP_OVERHEAD + } +} + +#[derive(Debug, Clone)] +pub struct OnionPacket { + + pub data: HeaplessVec, + + pub num_layers: u8, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum OnionError { + + InvalidRoute, + + PacketTooLarge, + + AuthenticationFailed, + + DecryptionFailed, + + NoMoreLayers, +} + +pub struct OnionRouter { + + our_private: [u8; 32], + + our_public: [u8; 32], + + our_hint: u16, +} + +impl OnionRouter { + + pub fn new(private_key: [u8; 32]) -> Self { + use crate::crypto::x25519::x25519_base; + + let our_public = x25519_base(&private_key); + + let hint_hash = Sha256::hash(&our_public); + let our_hint = ((hint_hash[0] as u16) << 8) | (hint_hash[1] as u16); + + Self { + our_private: private_key, + our_public, + our_hint, + } + } + + pub fn wrap(&self, payload: &[u8], route: &OnionRoute) -> Result { + if route.len() < MIN_HOPS || route.len() > MAX_HOPS { + return Err(OnionError::InvalidRoute); + } + + let total_overhead = route.overhead(); + if payload.len() + total_overhead > 256 { + return Err(OnionError::PacketTooLarge); + } + + let mut current = HeaplessVec::::new(); + current.extend_from_slice(payload).map_err(|_| OnionError::PacketTooLarge)?; + + for (i, hop) in route.hops.iter().enumerate().rev() { + + let shared = x25519(&self.our_private, &hop.public_key); + + let layer_index = (route.len() - 1 - i) as u8; + let mut key_input = [0u8; 33]; + key_input[..32].copy_from_slice(&shared); + key_input[32] = layer_index; + + let mut layer_key = [0u8; 32]; + Hkdf::derive(&key_input, &[layer_index], ONION_KEY_INFO, &mut layer_key); + + let encrypted = self.encrypt_layer(&layer_key, ¤t)?; + + let tag = self.compute_tag(&layer_key, &encrypted); + + let mut new_layer = HeaplessVec::::new(); + + let next_hint = if i == route.len() - 1 { + + 0u16 + } else { + route.hops[i + 1].hint + }; + + new_layer.push((next_hint >> 8) as u8).map_err(|_| OnionError::PacketTooLarge)?; + new_layer.push(next_hint as u8).map_err(|_| OnionError::PacketTooLarge)?; + new_layer.extend_from_slice(&tag).map_err(|_| OnionError::PacketTooLarge)?; + new_layer.extend_from_slice(&encrypted).map_err(|_| OnionError::PacketTooLarge)?; + + current = new_layer; + } + + Ok(OnionPacket { + data: current, + num_layers: route.len() as u8, + }) + } + + pub fn unwrap(&self, packet: &OnionPacket, sender_public: &[u8; 32]) -> Result<(u16, OnionPacket), OnionError> { + if packet.data.len() < HOP_OVERHEAD { + return Err(OnionError::DecryptionFailed); + } + + let next_hint = ((packet.data[0] as u16) << 8) | (packet.data[1] as u16); + let tag = &packet.data[2..18]; + let encrypted = &packet.data[18..]; + + let shared = x25519(&self.our_private, sender_public); + + let layer_index = packet.num_layers.saturating_sub(1); + let mut key_input = [0u8; 33]; + key_input[..32].copy_from_slice(&shared); + key_input[32] = layer_index; + + let mut layer_key = [0u8; 32]; + Hkdf::derive(&key_input, &[layer_index], ONION_KEY_INFO, &mut layer_key); + + let expected_tag = self.compute_tag(&layer_key, encrypted); + if !constant_time_eq(tag, &expected_tag) { + return Err(OnionError::AuthenticationFailed); + } + + let inner = self.decrypt_layer(&layer_key, encrypted)?; + + if next_hint == 0 { + return Err(OnionError::NoMoreLayers); + } + + let mut inner_packet = OnionPacket { + data: inner, + num_layers: packet.num_layers.saturating_sub(1), + }; + + Ok((next_hint, inner_packet)) + } + + pub fn unwrap_final(&self, packet: &OnionPacket, sender_public: &[u8; 32]) -> Result, OnionError> { + if packet.data.len() < HOP_OVERHEAD { + return Err(OnionError::DecryptionFailed); + } + + let tag = &packet.data[2..18]; + let encrypted = &packet.data[18..]; + + let shared = x25519(&self.our_private, sender_public); + + let layer_index = packet.num_layers.saturating_sub(1); + let mut key_input = [0u8; 33]; + key_input[..32].copy_from_slice(&shared); + key_input[32] = layer_index; + + let mut layer_key = [0u8; 32]; + Hkdf::derive(&key_input, &[layer_index], ONION_KEY_INFO, &mut layer_key); + + let expected_tag = self.compute_tag(&layer_key, encrypted); + if !constant_time_eq(tag, &expected_tag) { + return Err(OnionError::AuthenticationFailed); + } + + self.decrypt_layer(&layer_key, encrypted) + } + + fn encrypt_layer(&self, key: &[u8; 32], data: &[u8]) -> Result, OnionError> { + let mut output = HeaplessVec::new(); + output.extend_from_slice(data).map_err(|_| OnionError::PacketTooLarge)?; + + let aes = Aes256::new(key); + let mut counter = [0u8; 16]; + let mut keystream = [0u8; 16]; + let mut block_num = 0u64; + + for chunk in output.chunks_mut(16) { + keystream.copy_from_slice(&counter); + keystream[8..].copy_from_slice(&block_num.to_le_bytes()); + aes.encrypt_block(&mut keystream); + for (c, k) in chunk.iter_mut().zip(keystream.iter()) { + *c ^= k; + } + block_num += 1; + } + + Ok(output) + } + + fn decrypt_layer(&self, key: &[u8; 32], data: &[u8]) -> Result, OnionError> { + + self.encrypt_layer(key, data) + } + + fn compute_tag(&self, key: &[u8; 32], data: &[u8]) -> [u8; 16] { + + let mut inner = [0x36u8; 64]; + let mut outer = [0x5cu8; 64]; + + for (i, k) in key.iter().enumerate() { + inner[i] ^= k; + outer[i] ^= k; + } + + let mut inner_input = HeaplessVec::::new(); + let _ = inner_input.extend_from_slice(&inner); + let _ = inner_input.extend_from_slice(data); + let inner_hash = Sha256::hash(&inner_input); + + let mut outer_input = [0u8; 96]; + outer_input[..64].copy_from_slice(&outer); + outer_input[64..].copy_from_slice(&inner_hash); + let full = Sha256::hash(&outer_input); + + let mut tag = [0u8; 16]; + tag.copy_from_slice(&full[..16]); + tag + } + + pub fn our_hint(&self) -> u16 { + self.our_hint + } + + pub fn our_public(&self) -> &[u8; 32] { + &self.our_public + } + + pub fn derive_blinded_hint(&self, epoch: u64) -> u16 { + let mut input = [0u8; 40]; + input[..32].copy_from_slice(&self.our_public); + input[32..].copy_from_slice(&epoch.to_le_bytes()); + + let mut blinded = [0u8; 2]; + Hkdf::derive(&input, &epoch.to_le_bytes(), ONION_BLIND_INFO, &mut blinded); + + ((blinded[0] as u16) << 8) | (blinded[1] as u16) + } +} + +pub struct RouteBuilder { + + relays: HeaplessVec, +} + +impl RouteBuilder { + pub fn new() -> Self { + Self { + relays: HeaplessVec::new(), + } + } + + pub fn add_relay(&mut self, hint: u16, public_key: [u8; 32]) -> bool { + self.relays.push(RouteHop { hint, public_key }).is_ok() + } + + pub fn build_route(&self, destination: RouteHop, num_hops: usize) -> Option { + if num_hops < MIN_HOPS || num_hops > MAX_HOPS { + return None; + } + + let relay_count = num_hops - 1; + if self.relays.len() < relay_count { + return None; + } + + let mut hops = HeaplessVec::::new(); + + for i in 0..relay_count { + hops.push(self.relays[i % self.relays.len()].clone()).ok()?; + } + + hops.push(destination).ok()?; + + Some(OnionRoute { hops }) + } + + pub fn relay_count(&self) -> usize { + self.relays.len() + } +} + +impl Default for RouteBuilder { + fn default() -> Self { + Self::new() + } +} diff --git a/src/ota.rs b/src/ota.rs new file mode 100644 index 0000000..a1ec739 --- /dev/null +++ b/src/ota.rs @@ -0,0 +1,572 @@ +use crate::crypto::sha256::Sha256; +use crate::crypto::ed25519; + +pub const OTA_CHUNK_SIZE: usize = 4096; + +pub const MAX_FIRMWARE_SIZE: usize = 3_584_000; + +pub const OTA_HEADER_MAGIC: [u8; 4] = [0x4C, 0x55, 0x4E, 0x41]; + +pub const OTA_HEADER_VERSION: u8 = 1; + +pub const SIGNATURE_SIZE: usize = 64; + +pub const PUBLIC_KEY_SIZE: usize = 32; + +#[inline(never)] +fn ct_key_eq(a: &[u8; 32], b: &[u8; 32]) -> bool { + let mut diff: u8 = 0; + for i in 0..32 { + diff |= a[i] ^ b[i]; + } + diff == 0 +} + +#[inline] +fn pack_version(major: u8, minor: u8, patch: u8) -> u32 { + ((major as u32) << 16) | ((minor as u32) << 8) | (patch as u32) +} + +#[repr(C, packed)] +#[derive(Debug, Clone, Copy)] +pub struct OtaHeader { + + pub magic: [u8; 4], + + pub version: u8, + + pub header_size: u8, + + pub fw_major: u8, + + pub fw_minor: u8, + + pub fw_patch: u8, + + pub reserved: [u8; 3], + + pub firmware_size: u32, + + pub firmware_hash: [u8; 32], + + pub signature: [u8; SIGNATURE_SIZE], + + pub public_key: [u8; PUBLIC_KEY_SIZE], +} + +impl OtaHeader { + + pub const SIZE: usize = 4 + 1 + 1 + 3 + 3 + 4 + 32 + 64 + 32; + + pub fn from_bytes(data: &[u8]) -> Option { + if data.len() < Self::SIZE { + return None; + } + + let mut magic = [0u8; 4]; + magic.copy_from_slice(&data[0..4]); + + if magic != OTA_HEADER_MAGIC { + return None; + } + + let version = data[4]; + if version != OTA_HEADER_VERSION { + return None; + } + + let header_size = data[5]; + let fw_major = data[6]; + let fw_minor = data[7]; + let fw_patch = data[8]; + + let mut reserved = [0u8; 3]; + reserved.copy_from_slice(&data[9..12]); + + let firmware_size = u32::from_le_bytes([data[12], data[13], data[14], data[15]]); + + let mut firmware_hash = [0u8; 32]; + firmware_hash.copy_from_slice(&data[16..48]); + + let mut signature = [0u8; SIGNATURE_SIZE]; + signature.copy_from_slice(&data[48..112]); + + let mut public_key = [0u8; PUBLIC_KEY_SIZE]; + public_key.copy_from_slice(&data[112..144]); + + Some(Self { + magic, + version, + header_size, + fw_major, + fw_minor, + fw_patch, + reserved, + firmware_size, + firmware_hash, + signature, + public_key, + }) + } + + pub fn version_string(&self) -> [u8; 12] { + let mut buf = [0u8; 12]; + let mut idx = 0; + + idx += write_u8_to_buf(self.fw_major, &mut buf[idx..]); + buf[idx] = b'.'; + idx += 1; + + idx += write_u8_to_buf(self.fw_minor, &mut buf[idx..]); + buf[idx] = b'.'; + idx += 1; + + write_u8_to_buf(self.fw_patch, &mut buf[idx..]); + + buf + } +} + +fn write_u8_to_buf(val: u8, buf: &mut [u8]) -> usize { + if val >= 100 { + buf[0] = b'0' + val / 100; + buf[1] = b'0' + (val / 10) % 10; + buf[2] = b'0' + val % 10; + 3 + } else if val >= 10 { + buf[0] = b'0' + val / 10; + buf[1] = b'0' + val % 10; + 2 + } else { + buf[0] = b'0' + val; + 1 + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum OtaState { + + Idle, + + ReceivingHeader, + + Receiving, + + Verifying, + + Writing, + + Complete, + + Error(OtaError), +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum OtaError { + + InvalidHeader, + + SignatureInvalid, + + HashMismatch, + + TooLarge, + + FlashError, + + PartitionError, + + Busy, + + Aborted, + + Timeout, + + RolledBack, + + DowngradeRejected, + + NoTrustedKeys, +} + +pub struct OtaManager { + + state: OtaState, + + header_buf: [u8; OtaHeader::SIZE], + + header_received: usize, + + header: Option, + + bytes_received: u32, + + hasher: Sha256, + + partition_handle: i32, + + ota_handle: u32, + + trusted_keys: [[u8; PUBLIC_KEY_SIZE]; 2], + + trusted_key_count: usize, + + progress_percent: u8, + + min_version: u32, +} + +impl OtaManager { + + pub fn new() -> Self { + Self { + state: OtaState::Idle, + header_buf: [0u8; OtaHeader::SIZE], + header_received: 0, + header: None, + bytes_received: 0, + hasher: Sha256::new(), + partition_handle: -1, + ota_handle: 0, + trusted_keys: [[0u8; PUBLIC_KEY_SIZE]; 2], + trusted_key_count: 0, + progress_percent: 0, + min_version: 0, + } + } + + pub fn add_trusted_key(&mut self, key: &[u8; PUBLIC_KEY_SIZE]) -> bool { + if self.trusted_key_count >= 2 { + return false; + } + self.trusted_keys[self.trusted_key_count].copy_from_slice(key); + self.trusted_key_count += 1; + true + } + + pub fn set_min_version(&mut self, major: u8, minor: u8, patch: u8) { + self.min_version = pack_version(major, minor, patch); + } + + fn is_version_allowed(&self, header: &OtaHeader) -> bool { + let new_version = pack_version(header.fw_major, header.fw_minor, header.fw_patch); + new_version >= self.min_version + } + + pub fn state(&self) -> OtaState { + self.state + } + + pub fn progress(&self) -> u8 { + self.progress_percent + } + + pub fn begin(&mut self) -> Result<(), OtaError> { + if self.state != OtaState::Idle { + return Err(OtaError::Busy); + } + + self.header_buf.fill(0); + self.header_received = 0; + self.header = None; + self.bytes_received = 0; + self.hasher = Sha256::new(); + self.progress_percent = 0; + + unsafe { + let next_partition = esp_idf_sys::esp_ota_get_next_update_partition(core::ptr::null()); + if next_partition.is_null() { + return Err(OtaError::PartitionError); + } + + let mut handle: esp_idf_sys::esp_ota_handle_t = 0; + let ret = esp_idf_sys::esp_ota_begin(next_partition, 0, &mut handle); + if ret != 0 { + return Err(OtaError::FlashError); + } + + self.ota_handle = handle; + } + + self.state = OtaState::ReceivingHeader; + Ok(()) + } + + pub fn write(&mut self, data: &[u8]) -> Result { + match self.state { + OtaState::Idle => Err(OtaError::Aborted), + OtaState::Error(e) => Err(e), + OtaState::ReceivingHeader => self.write_header(data), + OtaState::Receiving | OtaState::Writing => self.write_firmware(data), + _ => Ok(0), + } + } + + fn write_header(&mut self, data: &[u8]) -> Result { + let needed = OtaHeader::SIZE - self.header_received; + let to_copy = data.len().min(needed); + + self.header_buf[self.header_received..self.header_received + to_copy] + .copy_from_slice(&data[..to_copy]); + self.header_received += to_copy; + + if self.header_received >= OtaHeader::SIZE { + + let header = OtaHeader::from_bytes(&self.header_buf) + .ok_or(OtaError::InvalidHeader)?; + + if header.firmware_size as usize > MAX_FIRMWARE_SIZE { + self.state = OtaState::Error(OtaError::TooLarge); + return Err(OtaError::TooLarge); + } + + if self.trusted_key_count == 0 { + self.state = OtaState::Error(OtaError::NoTrustedKeys); + return Err(OtaError::NoTrustedKeys); + } + + if !self.is_version_allowed(&header) { + self.state = OtaState::Error(OtaError::DowngradeRejected); + return Err(OtaError::DowngradeRejected); + } + + if !self.verify_signature(&header) { + self.state = OtaState::Error(OtaError::SignatureInvalid); + return Err(OtaError::SignatureInvalid); + } + + self.header = Some(header); + self.state = OtaState::Receiving; + + if to_copy < data.len() { + let remaining = &data[to_copy..]; + return self.write_firmware(remaining).map(|n| to_copy + n); + } + } + + Ok(to_copy) + } + + fn write_firmware(&mut self, data: &[u8]) -> Result { + let header = self.header.as_ref().ok_or(OtaError::InvalidHeader)?; + + let remaining = header.firmware_size - self.bytes_received; + let to_write = (data.len() as u32).min(remaining) as usize; + + if to_write == 0 { + return Ok(0); + } + + self.hasher.update(&data[..to_write]); + + unsafe { + let ret = esp_idf_sys::esp_ota_write( + self.ota_handle, + data.as_ptr() as *const core::ffi::c_void, + to_write, + ); + + if ret != 0 { + self.state = OtaState::Error(OtaError::FlashError); + return Err(OtaError::FlashError); + } + } + + self.bytes_received += to_write as u32; + + self.progress_percent = ((self.bytes_received as u64 * 100) / header.firmware_size as u64) as u8; + + if self.bytes_received >= header.firmware_size { + self.state = OtaState::Verifying; + self.verify_and_finish()?; + } + + Ok(to_write) + } + + fn verify_and_finish(&mut self) -> Result<(), OtaError> { + let header = self.header.as_ref().ok_or(OtaError::InvalidHeader)?; + + let computed_hash = self.hasher.clone().finalize(); + + if computed_hash != header.firmware_hash { + self.state = OtaState::Error(OtaError::HashMismatch); + self.abort(); + return Err(OtaError::HashMismatch); + } + + unsafe { + let ret = esp_idf_sys::esp_ota_end(self.ota_handle); + if ret != 0 { + self.state = OtaState::Error(OtaError::FlashError); + return Err(OtaError::FlashError); + } + + let next_partition = esp_idf_sys::esp_ota_get_next_update_partition(core::ptr::null()); + let ret = esp_idf_sys::esp_ota_set_boot_partition(next_partition); + if ret != 0 { + self.state = OtaState::Error(OtaError::PartitionError); + return Err(OtaError::PartitionError); + } + } + + self.state = OtaState::Complete; + self.progress_percent = 100; + Ok(()) + } + + fn verify_signature(&self, header: &OtaHeader) -> bool { + + if self.trusted_key_count == 0 { + + return false; + } + + for i in 0..self.trusted_key_count { + + if ct_key_eq(&header.public_key, &self.trusted_keys[i]) { + + let signature = ed25519::Signature(header.signature); + if ed25519::Ed25519::verify(&header.public_key, &header.firmware_hash, &signature) { + return true; + } + } + } + + false + } + + pub fn is_enabled(&self) -> bool { + self.trusted_key_count > 0 + } + + pub fn trusted_key_count(&self) -> usize { + self.trusted_key_count + } + + pub fn abort(&mut self) { + if self.ota_handle != 0 { + unsafe { + esp_idf_sys::esp_ota_abort(self.ota_handle); + } + self.ota_handle = 0; + } + self.state = OtaState::Idle; + } + + pub fn confirm() -> Result<(), OtaError> { + unsafe { + let ret = esp_idf_sys::esp_ota_mark_app_valid_cancel_rollback(); + if ret != 0 { + return Err(OtaError::PartitionError); + } + } + Ok(()) + } + + pub fn rollback() -> Result<(), OtaError> { + unsafe { + let ret = esp_idf_sys::esp_ota_mark_app_invalid_rollback_and_reboot(); + if ret != 0 { + return Err(OtaError::RolledBack); + } + } + Ok(()) + } + + pub fn get_running_partition() -> Option { + unsafe { + let partition = esp_idf_sys::esp_ota_get_running_partition(); + if partition.is_null() { + return None; + } + + let part = &*partition; + let mut label = [0u8; 16]; + label[..part.label.len()].copy_from_slice( + core::slice::from_raw_parts(part.label.as_ptr() as *const u8, part.label.len()) + ); + + Some(PartitionInfo { + address: part.address, + size: part.size, + label, + }) + } + } + + pub fn reboot() -> ! { + unsafe { + esp_idf_sys::esp_restart(); + } + loop {} + } +} + +impl Default for OtaManager { + fn default() -> Self { + Self::new() + } +} + +#[derive(Debug, Clone)] +pub struct PartitionInfo { + + pub address: u32, + + pub size: u32, + + pub label: [u8; 16], +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum OtaCommand { + + Begin = 0x01, + + Data = 0x02, + + End = 0x03, + + Abort = 0x04, + + Status = 0x05, + + Confirm = 0x06, + + Rollback = 0x07, + + Reboot = 0x08, +} + +impl From for OtaCommand { + fn from(v: u8) -> Self { + match v { + 0x01 => OtaCommand::Begin, + 0x02 => OtaCommand::Data, + 0x03 => OtaCommand::End, + 0x04 => OtaCommand::Abort, + 0x05 => OtaCommand::Status, + 0x06 => OtaCommand::Confirm, + 0x07 => OtaCommand::Rollback, + 0x08 => OtaCommand::Reboot, + _ => OtaCommand::Status, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum OtaResponse { + + Ok = 0x00, + + Error = 0x01, + + Busy = 0x02, + + Progress = 0x03, + + Complete = 0x04, +} diff --git a/src/packet_id.rs b/src/packet_id.rs new file mode 100644 index 0000000..f34b17e --- /dev/null +++ b/src/packet_id.rs @@ -0,0 +1,256 @@ +const RTC_SLOW_MEM_BASE: u32 = 0x5000_0000; + +const RTC_SLOT_SESSION_HI: u32 = 8; +const RTC_SLOT_SESSION_LO: u32 = 9; +const RTC_SLOT_SEQUENCE: u32 = 10; +const RTC_SLOT_BOOT_COUNT: u32 = 11; +const RTC_SLOT_MAGIC: u32 = 12; + +const RTC_MAGIC: u32 = 0x4C554E42; + +const MAX_SEQUENCE: u32 = 0xFFFF_FFFE; + +const WAKE_SEQUENCE_SKIP: u32 = 256; + +const RNG_DATA_REG: u32 = 0x6003_5110; + +struct PacketIdState { + + session_id: u32, + + sequence: u32, + + boot_count: u32, +} + +impl PacketIdState { + + fn new_session(&mut self) { + + let hw_random1 = hw_rng_u32(); + let hw_random2 = hw_rng_u32(); + let boot_entropy = self.boot_count.wrapping_mul(0x9E3779B9); + let timestamp = read_rtc_time(); + + let mixed = hw_random1 + ^ boot_entropy + ^ timestamp + ^ self.session_id.wrapping_mul(0x85EBCA6B); + + let mixed = mix32(mixed); + let mixed = mix32(mixed ^ hw_random2); + + self.session_id = mixed; + + if self.session_id == 0 { + self.session_id = mix32(hw_rng_u32()) | 1; + } + + self.sequence = 0; + + self.persist(); + } + + fn persist(&self) { + rtc_write(RTC_SLOT_SESSION_HI, self.session_id); + rtc_write(RTC_SLOT_SEQUENCE, self.sequence); + } + + fn to_u64(&self) -> u64 { + ((self.session_id as u64) << 32) | (self.sequence as u64) + } +} + +#[inline] +fn rtc_read(slot: u32) -> u32 { + unsafe { + let addr = (RTC_SLOW_MEM_BASE + slot * 4) as *const u32; + core::ptr::read_volatile(addr) + } +} + +#[inline] +fn rtc_write(slot: u32, value: u32) { + unsafe { + let addr = (RTC_SLOW_MEM_BASE + slot * 4) as *mut u32; + core::ptr::write_volatile(addr, value); + } +} + +#[inline] +fn hw_rng_u32() -> u32 { + + crate::rng::random_u32() +} + +#[inline] +#[allow(dead_code)] +fn hw_rng_raw() -> u32 { + unsafe { + let rng_reg = RNG_DATA_REG as *const u32; + + let r1 = core::ptr::read_volatile(rng_reg); + + for _ in 0..10 { + core::hint::spin_loop(); + } + let r2 = core::ptr::read_volatile(rng_reg); + r1 ^ r2.rotate_left(13) + } +} + +fn read_rtc_time() -> u32 { + + unsafe { + let rtc_time_low = 0x6000_8048 as *const u32; + core::ptr::read_volatile(rtc_time_low) + } +} + +#[inline] +const fn mix32(mut x: u32) -> u32 { + x ^= x >> 16; + x = x.wrapping_mul(0x85EBCA6B); + x ^= x >> 13; + x = x.wrapping_mul(0xC2B2AE35); + x ^= x >> 16; + x +} + +use core::sync::atomic::{AtomicBool, AtomicU32, Ordering}; + +static PACKET_ID_SESSION: AtomicU32 = AtomicU32::new(0); +static PACKET_ID_SEQUENCE: AtomicU32 = AtomicU32::new(0); + +static BOOT_COUNT: AtomicU32 = AtomicU32::new(0); + +static INITIALIZED: AtomicBool = AtomicBool::new(false); + +pub fn init() { + + if INITIALIZED.load(Ordering::Acquire) { + return; + } + + if INITIALIZED.compare_exchange(false, true, Ordering::SeqCst, Ordering::SeqCst).is_err() { + + while !INITIALIZED.load(Ordering::Acquire) { + core::hint::spin_loop(); + } + return; + } + + let magic = rtc_read(RTC_SLOT_MAGIC); + + let (session_id, sequence, boot_count) = if magic == RTC_MAGIC { + + let stored_session = rtc_read(RTC_SLOT_SESSION_HI); + let stored_sequence = rtc_read(RTC_SLOT_SEQUENCE); + let stored_boot_count = rtc_read(RTC_SLOT_BOOT_COUNT); + + let boot_count = stored_boot_count.wrapping_add(1); + + if stored_sequence >= MAX_SEQUENCE { + + let new_session = generate_session_id(stored_session, boot_count); + (new_session, 0u32, boot_count) + } else { + + let new_sequence = stored_sequence.saturating_add(WAKE_SEQUENCE_SKIP); + (stored_session, new_sequence, boot_count) + } + } else { + + let new_session = generate_session_id(0, 0); + rtc_write(RTC_SLOT_MAGIC, RTC_MAGIC); + (new_session, 0u32, 0u32) + }; + + BOOT_COUNT.store(boot_count, Ordering::SeqCst); + rtc_write(RTC_SLOT_BOOT_COUNT, boot_count); + + PACKET_ID_SESSION.store(session_id, Ordering::SeqCst); + PACKET_ID_SEQUENCE.store(sequence, Ordering::SeqCst); + + rtc_write(RTC_SLOT_SESSION_HI, session_id); + rtc_write(RTC_SLOT_SEQUENCE, sequence); +} + +fn generate_session_id(previous: u32, boot_count: u32) -> u32 { + let hw_random1 = hw_rng_u32(); + let hw_random2 = hw_rng_u32(); + let boot_entropy = boot_count.wrapping_mul(0x9E3779B9); + let timestamp = read_rtc_time(); + + let mixed = hw_random1 + ^ boot_entropy + ^ timestamp + ^ previous.wrapping_mul(0x85EBCA6B); + + let mixed = mix32(mixed); + let mixed = mix32(mixed ^ hw_random2); + + if mixed == 0 { mix32(hw_rng_u32()) | 1 } else { mixed } +} + +pub fn next_packet_id() -> u32 { + init(); + + let sequence = PACKET_ID_SEQUENCE.fetch_add(1, Ordering::SeqCst); + let session_id = PACKET_ID_SESSION.load(Ordering::SeqCst); + + if sequence >= MAX_SEQUENCE { + + rotate_session_internal(); + } + + if sequence & 0xFF == 0 { + rtc_write(RTC_SLOT_SESSION_HI, session_id); + rtc_write(RTC_SLOT_SEQUENCE, sequence); + } + + mix32(session_id ^ sequence.wrapping_mul(0x85EBCA6B)) +} + +fn rotate_session_internal() { + + let boot_count = BOOT_COUNT.load(Ordering::SeqCst); + let old_session = PACKET_ID_SESSION.load(Ordering::SeqCst); + + let new_session = generate_session_id(old_session, boot_count); + + PACKET_ID_SESSION.store(new_session, Ordering::SeqCst); + PACKET_ID_SEQUENCE.store(0, Ordering::SeqCst); + + rtc_write(RTC_SLOT_SESSION_HI, new_session); + rtc_write(RTC_SLOT_SEQUENCE, 0); +} + +pub fn rotate_session() { + init(); + rotate_session_internal(); +} + +pub fn session_info() -> (u32, u32, u32) { + init(); + let session_id = PACKET_ID_SESSION.load(Ordering::SeqCst); + let sequence = PACKET_ID_SEQUENCE.load(Ordering::SeqCst); + let boot_count = BOOT_COUNT.load(Ordering::SeqCst); + (session_id, sequence, boot_count) +} + +pub fn next_packet_id_64() -> u64 { + init(); + let sequence = PACKET_ID_SEQUENCE.fetch_add(1, Ordering::SeqCst); + let session_id = PACKET_ID_SESSION.load(Ordering::SeqCst); + + if sequence >= MAX_SEQUENCE { + rotate_session_internal(); + } + + ((session_id as u64) << 32) | (sequence as u64) +} + +pub fn invalidate_rtc() { + rtc_write(RTC_SLOT_MAGIC, 0); +} diff --git a/src/power.rs b/src/power.rs new file mode 100644 index 0000000..e450326 --- /dev/null +++ b/src/power.rs @@ -0,0 +1,402 @@ +pub const MIN_DEEP_SLEEP_US: u64 = 1_000; + +pub const MAX_DEEP_SLEEP_US: u64 = 86_400_000_000; + +pub const DEFAULT_LIGHT_SLEEP_MS: u32 = 100; + +pub const LOW_BATTERY_THRESHOLD_MV: u32 = 3400; + +pub const CRITICAL_BATTERY_THRESHOLD_MV: u32 = 3200; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum PowerMode { + + Performance, + + Balanced, + + LowPower, + + UltraLow, +} + +impl Default for PowerMode { + fn default() -> Self { + PowerMode::Balanced + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct WakeSources(pub u32); + +impl WakeSources { + + pub const NONE: Self = Self(0); + + pub const TIMER: Self = Self(1 << 0); + + pub const GPIO: Self = Self(1 << 1); + + pub const UART: Self = Self(1 << 2); + + pub const TOUCH: Self = Self(1 << 3); + + pub const ULP: Self = Self(1 << 4); + + pub const BLE: Self = Self(1 << 5); + + pub const WIFI: Self = Self(1 << 6); + + pub const EXT0: Self = Self(1 << 7); + + pub const EXT1: Self = Self(1 << 8); + + pub const ALL: Self = Self(0x1FF); + + pub const fn or(self, other: Self) -> Self { + Self(self.0 | other.0) + } + + pub const fn has(self, source: Self) -> bool { + (self.0 & source.0) != 0 + } +} + +impl core::ops::BitOr for WakeSources { + type Output = Self; + fn bitor(self, rhs: Self) -> Self { + Self(self.0 | rhs.0) + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum WakeCause { + + PowerOn, + + Timer, + + Gpio(u8), + + Uart, + + Touch, + + Ulp, + + Ble, + + Wifi, + + Ext0, + + Ext1, + + Unknown, +} + +#[derive(Debug, Clone, Copy)] +pub struct GpioWakeConfig { + + pub pin: u8, + + pub level_high: bool, +} + +pub struct PowerManager { + + mode: PowerMode, + + light_sleep_wake: WakeSources, + + deep_sleep_wake: WakeSources, + + gpio_wake_pins: [Option; 8], + + cpu_freq_mhz: u32, + + last_wake_cause: WakeCause, + + total_sleep_us: u64, + + sleep_count: u32, +} + +impl PowerManager { + + pub const fn new() -> Self { + Self { + mode: PowerMode::Balanced, + light_sleep_wake: WakeSources::TIMER.or(WakeSources::GPIO).or(WakeSources::UART), + deep_sleep_wake: WakeSources::TIMER.or(WakeSources::GPIO), + gpio_wake_pins: [None; 8], + cpu_freq_mhz: 240, + last_wake_cause: WakeCause::PowerOn, + total_sleep_us: 0, + sleep_count: 0, + } + } + + pub fn init(&mut self) -> Result<(), PowerError> { + + self.last_wake_cause = self.read_wake_cause(); + + self.apply_mode()?; + + Ok(()) + } + + pub fn set_mode(&mut self, mode: PowerMode) -> Result<(), PowerError> { + self.mode = mode; + self.apply_mode() + } + + fn apply_mode(&mut self) -> Result<(), PowerError> { + let (cpu_freq, wifi_ps, bt_power) = match self.mode { + PowerMode::Performance => (240, false, true), + PowerMode::Balanced => (160, true, true), + PowerMode::LowPower => (80, true, false), + PowerMode::UltraLow => (40, true, false), + }; + + self.set_cpu_frequency(cpu_freq)?; + + if wifi_ps { + self.enable_wifi_power_save(); + } else { + self.disable_wifi_power_save(); + } + + Ok(()) + } + + pub fn set_cpu_frequency(&mut self, mhz: u32) -> Result<(), PowerError> { + + match mhz { + 240 | 160 | 80 | 40 | 20 | 10 => {}, + _ => return Err(PowerError::InvalidFrequency), + }; + + unsafe { + let pm_config = esp_idf_sys::esp_pm_config_esp32s3_t { + max_freq_mhz: mhz as i32, + min_freq_mhz: 10, + light_sleep_enable: false, + }; + + let ret = esp_idf_sys::esp_pm_configure(&pm_config as *const _ as *const core::ffi::c_void); + if ret != 0 { + return Err(PowerError::ConfigFailed); + } + } + + self.cpu_freq_mhz = mhz; + Ok(()) + } + + pub fn configure_gpio_wake(&mut self, config: GpioWakeConfig) -> Result<(), PowerError> { + + for slot in &mut self.gpio_wake_pins { + if slot.is_none() || slot.as_ref().map(|c| c.pin) == Some(config.pin) { + *slot = Some(config); + return Ok(()); + } + } + Err(PowerError::TooManyWakePins) + } + + pub fn light_sleep(&mut self, duration_ms: u32) -> Result { + if duration_ms == 0 { + return Err(PowerError::InvalidDuration); + } + + let duration_us = duration_ms as u64 * 1000; + + unsafe { + + if self.light_sleep_wake.has(WakeSources::TIMER) { + esp_idf_sys::esp_sleep_enable_timer_wakeup(duration_us); + } + + if self.light_sleep_wake.has(WakeSources::UART) { + + esp_idf_sys::esp_sleep_enable_uart_wakeup(0); + } + + if self.light_sleep_wake.has(WakeSources::GPIO) { + self.configure_gpio_wake_internal()?; + } + + let ret = esp_idf_sys::esp_light_sleep_start(); + if ret != 0 { + return Err(PowerError::SleepFailed); + } + } + + self.sleep_count += 1; + self.total_sleep_us += duration_us; + + let cause = self.read_wake_cause(); + self.last_wake_cause = cause; + Ok(cause) + } + + pub fn deep_sleep(&mut self, duration_us: u64) -> ! { + if duration_us < MIN_DEEP_SLEEP_US { + + unsafe { + esp_idf_sys::esp_sleep_enable_timer_wakeup(MIN_DEEP_SLEEP_US); + } + } else if duration_us > MAX_DEEP_SLEEP_US { + unsafe { + esp_idf_sys::esp_sleep_enable_timer_wakeup(MAX_DEEP_SLEEP_US); + } + } else { + unsafe { + esp_idf_sys::esp_sleep_enable_timer_wakeup(duration_us); + } + } + + if self.deep_sleep_wake.has(WakeSources::GPIO) { + let _ = self.configure_gpio_wake_internal(); + } + + unsafe { + esp_idf_sys::esp_deep_sleep_start(); + } + + loop { + core::hint::spin_loop(); + } + } + + fn configure_gpio_wake_internal(&self) -> Result<(), PowerError> { + unsafe { + let mut mask: u64 = 0; + let mut mode = esp_idf_sys::esp_sleep_ext1_wakeup_mode_t_ESP_EXT1_WAKEUP_ANY_HIGH; + + for config in &self.gpio_wake_pins { + if let Some(cfg) = config { + if cfg.pin < 64 { + mask |= 1u64 << cfg.pin; + if !cfg.level_high { + mode = esp_idf_sys::esp_sleep_ext1_wakeup_mode_t_ESP_EXT1_WAKEUP_ALL_LOW; + } + } + } + } + + if mask != 0 { + esp_idf_sys::esp_sleep_enable_ext1_wakeup(mask, mode); + } + } + Ok(()) + } + + fn read_wake_cause(&self) -> WakeCause { + unsafe { + let cause = esp_idf_sys::esp_sleep_get_wakeup_cause(); + match cause { + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_UNDEFINED => WakeCause::PowerOn, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_TIMER => WakeCause::Timer, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_EXT0 => WakeCause::Ext0, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_EXT1 => { + + let gpio_mask = esp_idf_sys::esp_sleep_get_ext1_wakeup_status(); + let gpio = gpio_mask.trailing_zeros() as u8; + WakeCause::Gpio(gpio) + } + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_TOUCHPAD => WakeCause::Touch, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_ULP => WakeCause::Ulp, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_GPIO => { + WakeCause::Gpio(0) + } + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_UART => WakeCause::Uart, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_WIFI => WakeCause::Wifi, + esp_idf_sys::esp_sleep_source_t_ESP_SLEEP_WAKEUP_BT => WakeCause::Ble, + _ => WakeCause::Unknown, + } + } + } + + fn enable_wifi_power_save(&self) { + unsafe { + esp_idf_sys::esp_wifi_set_ps(esp_idf_sys::wifi_ps_type_t_WIFI_PS_MIN_MODEM); + } + } + + fn disable_wifi_power_save(&self) { + unsafe { + esp_idf_sys::esp_wifi_set_ps(esp_idf_sys::wifi_ps_type_t_WIFI_PS_NONE); + } + } + + pub fn mode(&self) -> PowerMode { + self.mode + } + + pub fn last_wake_cause(&self) -> WakeCause { + self.last_wake_cause + } + + pub fn total_sleep_us(&self) -> u64 { + self.total_sleep_us + } + + pub fn sleep_count(&self) -> u32 { + self.sleep_count + } + + pub fn cpu_freq_mhz(&self) -> u32 { + self.cpu_freq_mhz + } + + pub fn is_battery_low(voltage_mv: u32) -> bool { + voltage_mv < LOW_BATTERY_THRESHOLD_MV + } + + pub fn is_battery_critical(voltage_mv: u32) -> bool { + voltage_mv < CRITICAL_BATTERY_THRESHOLD_MV + } + + pub fn set_rtc_data(&self, slot: u8, value: u32) { + if slot >= 8 { + return; + } + unsafe { + + let rtc_mem = (0x50000000 + slot as u32 * 4) as *mut u32; + core::ptr::write_volatile(rtc_mem, value); + } + } + + pub fn get_rtc_data(&self, slot: u8) -> u32 { + if slot >= 8 { + return 0; + } + unsafe { + let rtc_mem = (0x50000000 + slot as u32 * 4) as *const u32; + core::ptr::read_volatile(rtc_mem) + } + } +} + +impl Default for PowerManager { + fn default() -> Self { + Self::new() + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum PowerError { + + InvalidFrequency, + + ConfigFailed, + + InvalidDuration, + + SleepFailed, + + TooManyWakePins, +} diff --git a/src/protocol.rs b/src/protocol.rs new file mode 100644 index 0000000..6aacf4a --- /dev/null +++ b/src/protocol.rs @@ -0,0 +1,403 @@ +use heapless::Vec; + +pub const SYNC: [u8; 2] = [0xAA, 0x55]; + +pub const END: u8 = 0x0D; + +pub const MAX_DATA_SIZE: usize = 255; + +pub const MAX_FRAME_SIZE: usize = 2 + 2 + 1 + 1 + MAX_DATA_SIZE + 2 + 1; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum Command { + + Ping = 0x01, + + Pong = 0x02, + + Configure = 0x10, + + ConfigAck = 0x11, + + Transmit = 0x20, + + TxDone = 0x21, + + TxError = 0x22, + + Receive = 0x30, + + GetStats = 0x40, + + StatsResponse = 0x41, + + Cad = 0x50, + + CadResult = 0x51, + + Reset = 0xF0, + + Version = 0xF1, + + VersionResponse = 0xF2, + + Error = 0xFF, +} + +impl Command { + pub fn from_byte(b: u8) -> Option { + match b { + 0x01 => Some(Command::Ping), + 0x02 => Some(Command::Pong), + 0x10 => Some(Command::Configure), + 0x11 => Some(Command::ConfigAck), + 0x20 => Some(Command::Transmit), + 0x21 => Some(Command::TxDone), + 0x22 => Some(Command::TxError), + 0x30 => Some(Command::Receive), + 0x40 => Some(Command::GetStats), + 0x41 => Some(Command::StatsResponse), + 0x50 => Some(Command::Cad), + 0x51 => Some(Command::CadResult), + 0xF0 => Some(Command::Reset), + 0xF1 => Some(Command::Version), + 0xF2 => Some(Command::VersionResponse), + 0xFF => Some(Command::Error), + _ => None, + } + } +} + +#[derive(Debug, Clone)] +pub struct Frame { + pub command: Command, + pub sequence: u8, + pub data: Vec, +} + +impl Frame { + + pub fn new(command: Command, sequence: u8) -> Self { + Self { + command, + sequence, + data: Vec::new(), + } + } + + pub fn with_data(command: Command, sequence: u8, data: &[u8]) -> Option { + let mut frame = Self::new(command, sequence); + if data.len() > MAX_DATA_SIZE { + return None; + } + for &b in data { + let _ = frame.data.push(b); + } + Some(frame) + } + + pub fn encode(&self) -> Vec { + let mut buf = Vec::new(); + + let _ = buf.push(SYNC[0]); + let _ = buf.push(SYNC[1]); + + let len = self.data.len() as u16; + let _ = buf.push(len as u8); + let _ = buf.push((len >> 8) as u8); + + let _ = buf.push(self.command as u8); + let _ = buf.push(self.sequence); + + for &b in &self.data { + let _ = buf.push(b); + } + + let crc = crc16(&buf[4..]); + let _ = buf.push(crc as u8); + let _ = buf.push((crc >> 8) as u8); + + let _ = buf.push(END); + + buf + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum ParserState { + WaitSync1, + WaitSync2, + WaitLenLow, + WaitLenHigh, + WaitCommand, + WaitSequence, + WaitData, + WaitCrcLow, + WaitCrcHigh, + WaitEnd, +} + +pub struct FrameParser { + state: ParserState, + data_len: u16, + data_idx: u16, + command: u8, + sequence: u8, + data: Vec, + crc_low: u8, +} + +impl Default for FrameParser { + fn default() -> Self { + Self::new() + } +} + +impl FrameParser { + pub fn new() -> Self { + Self { + state: ParserState::WaitSync1, + data_len: 0, + data_idx: 0, + command: 0, + sequence: 0, + data: Vec::new(), + crc_low: 0, + } + } + + pub fn reset(&mut self) { + self.state = ParserState::WaitSync1; + self.data.clear(); + self.data_len = 0; + self.data_idx = 0; + } + + pub fn feed(&mut self, byte: u8) -> Option { + match self.state { + ParserState::WaitSync1 => { + if byte == SYNC[0] { + self.state = ParserState::WaitSync2; + } + } + ParserState::WaitSync2 => { + if byte == SYNC[1] { + self.state = ParserState::WaitLenLow; + } else if byte == SYNC[0] { + + } else { + self.reset(); + } + } + ParserState::WaitLenLow => { + self.data_len = byte as u16; + self.state = ParserState::WaitLenHigh; + } + ParserState::WaitLenHigh => { + self.data_len |= (byte as u16) << 8; + if self.data_len > MAX_DATA_SIZE as u16 { + self.reset(); + return None; + } + self.state = ParserState::WaitCommand; + } + ParserState::WaitCommand => { + self.command = byte; + self.state = ParserState::WaitSequence; + } + ParserState::WaitSequence => { + self.sequence = byte; + self.data.clear(); + self.data_idx = 0; + if self.data_len == 0 { + self.state = ParserState::WaitCrcLow; + } else { + self.state = ParserState::WaitData; + } + } + ParserState::WaitData => { + let _ = self.data.push(byte); + self.data_idx += 1; + if self.data_idx >= self.data_len { + self.state = ParserState::WaitCrcLow; + } + } + ParserState::WaitCrcLow => { + self.crc_low = byte; + self.state = ParserState::WaitCrcHigh; + } + ParserState::WaitCrcHigh => { + let received_crc = (self.crc_low as u16) | ((byte as u16) << 8); + + let mut crc_data: Vec = Vec::new(); + let _ = crc_data.push(self.command); + let _ = crc_data.push(self.sequence); + for &b in &self.data { + let _ = crc_data.push(b); + } + let calculated_crc = crc16(&crc_data); + + if received_crc == calculated_crc { + self.state = ParserState::WaitEnd; + } else { + self.reset(); + } + } + ParserState::WaitEnd => { + if byte == END { + + if let Some(cmd) = Command::from_byte(self.command) { + let frame = Frame { + command: cmd, + sequence: self.sequence, + data: self.data.clone(), + }; + self.reset(); + return Some(frame); + } + } + self.reset(); + } + } + None + } +} + +#[rustfmt::skip] +const CRC16_TABLE: [u16; 256] = [ + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0, +]; + +#[inline] +pub fn crc16(data: &[u8]) -> u16 { + let mut crc: u16 = 0xFFFF; + for &byte in data { + let index = ((crc >> 8) ^ (byte as u16)) as usize; + crc = (crc << 8) ^ CRC16_TABLE[index]; + } + crc +} + +pub fn build_pong(sequence: u8) -> Frame { + Frame::new(Command::Pong, sequence) +} + +pub fn build_config_ack(sequence: u8) -> Frame { + Frame::new(Command::ConfigAck, sequence) +} + +pub fn build_tx_done(sequence: u8) -> Frame { + Frame::new(Command::TxDone, sequence) +} + +pub fn build_tx_error(sequence: u8, error_code: u8) -> Option { + Frame::with_data(Command::TxError, sequence, &[error_code]) +} + +pub fn build_receive(rssi: i16, snr: i8, data: &[u8]) -> Option { + if data.len() > MAX_DATA_SIZE - 4 { + return None; + } + + let mut frame = Frame::new(Command::Receive, 0); + + let _ = frame.data.push(rssi as u8); + let _ = frame.data.push((rssi >> 8) as u8); + + let _ = frame.data.push(snr as u8); + + let _ = frame.data.push(0); + + for &b in data { + let _ = frame.data.push(b); + } + + Some(frame) +} + +pub fn build_cad_result(sequence: u8, detected: bool) -> Option { + Frame::with_data(Command::CadResult, sequence, &[if detected { 1 } else { 0 }]) +} + +pub fn build_version_response(sequence: u8, version: &str) -> Option { + Frame::with_data(Command::VersionResponse, sequence, version.as_bytes()) +} + +pub fn build_error(sequence: u8, message: &str) -> Option { + Frame::with_data(Command::Error, sequence, message.as_bytes()) +} + +pub fn build_stats_response( + sequence: u8, + tx_packets: u32, + rx_packets: u32, + tx_errors: u32, + rx_errors: u32, +) -> Option { + let mut data = [0u8; 16]; + data[0..4].copy_from_slice(&tx_packets.to_le_bytes()); + data[4..8].copy_from_slice(&rx_packets.to_le_bytes()); + data[8..12].copy_from_slice(&tx_errors.to_le_bytes()); + data[12..16].copy_from_slice(&rx_errors.to_le_bytes()); + Frame::with_data(Command::StatsResponse, sequence, &data) +} + +pub fn parse_config(data: &[u8]) -> Option { + if data.len() < 14 { + return None; + } + + let frequency = u32::from_le_bytes([data[0], data[1], data[2], data[3]]); + let spreading_factor = data[4]; + let bandwidth = (u16::from_le_bytes([data[5], data[6]]) / 125) as u8; + let coding_rate = data[7]; + let tx_power = data[8] as i8; + let sync_word = data[9]; + let preamble_length = u16::from_le_bytes([data[10], data[11]]); + let flags = data[12]; + + Some(crate::sx1262::RadioConfig { + frequency, + spreading_factor, + bandwidth: bandwidth.min(2), + coding_rate, + tx_power, + sync_word, + preamble_length, + crc_enabled: flags & 0x01 != 0, + implicit_header: flags & 0x02 != 0, + ldro: flags & 0x04 != 0, + }) +} diff --git a/src/protocol_router.rs b/src/protocol_router.rs new file mode 100644 index 0000000..411a7e1 --- /dev/null +++ b/src/protocol_router.rs @@ -0,0 +1,494 @@ +use heapless::Vec; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum Protocol { + + Unknown, + + MeshCore, + + Meshtastic, + + RNode, + + AtCommand, +} + +impl Protocol { + + pub fn name(&self) -> &'static str { + match self { + Protocol::Unknown => "Unknown", + Protocol::MeshCore => "MeshCore", + Protocol::Meshtastic => "Meshtastic", + Protocol::RNode => "RNode/KISS", + Protocol::AtCommand => "AT Command", + } + } +} + +pub mod magic { + + pub const MESHCORE_SYNC1: u8 = 0xAA; + pub const MESHCORE_SYNC2: u8 = 0x55; + + pub const MESHTASTIC_SYNC1: u8 = 0x94; + pub const MESHTASTIC_SYNC2: u8 = 0xC3; + + pub const KISS_FEND: u8 = 0xC0; + + pub const AT_PREFIX: [u8; 2] = [b'A', b'T']; +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum DetectState { + + Idle, + + MeshCore1, + + Meshtastic1, + + At1, +} + +const SYNC_TIMEOUT_BYTES: u16 = 256; + +pub struct ProtocolDetector { + state: DetectState, + detected: Protocol, + + bytes_seen: u16, + + lock_threshold: u8, + + lock_count: u8, + + state_bytes: u16, + + last_detect_ms: u32, +} + +impl Default for ProtocolDetector { + fn default() -> Self { + Self::new() + } +} + +impl ProtocolDetector { + pub fn new() -> Self { + Self { + state: DetectState::Idle, + detected: Protocol::Unknown, + bytes_seen: 0, + lock_threshold: 3, + lock_count: 0, + state_bytes: 0, + last_detect_ms: 0, + } + } + + pub fn reset(&mut self) { + self.state = DetectState::Idle; + self.detected = Protocol::Unknown; + self.bytes_seen = 0; + self.lock_count = 0; + self.state_bytes = 0; + } + + pub fn soft_reset(&mut self) { + self.state = DetectState::Idle; + self.state_bytes = 0; + } + + pub fn force_protocol(&mut self, protocol: Protocol) { + self.detected = protocol; + self.state = DetectState::Idle; + + } + + pub fn protocol(&self) -> Protocol { + self.detected + } + + pub fn is_locked(&self) -> bool { + self.lock_count >= self.lock_threshold + } + + pub fn confirm_frame(&mut self) { + if self.lock_count < 255 { + self.lock_count += 1; + } + } + + pub fn error_frame(&mut self) { + if self.lock_count > 0 { + self.lock_count = self.lock_count.saturating_sub(2); + } + + if self.lock_count == 0 && self.detected != Protocol::Unknown { + self.detected = Protocol::Unknown; + } + } + + fn check_timeout(&mut self) { + if self.state != DetectState::Idle && self.state_bytes > SYNC_TIMEOUT_BYTES { + + self.state = DetectState::Idle; + self.state_bytes = 0; + } + } + + pub fn feed(&mut self, byte: u8) -> Option { + self.bytes_seen += 1; + self.state_bytes += 1; + + self.check_timeout(); + + if self.is_locked() { + return Some(self.detected); + } + + let prev_state = self.state; + + match self.state { + DetectState::Idle => { + match byte { + magic::MESHCORE_SYNC1 => { + self.state = DetectState::MeshCore1; + } + magic::MESHTASTIC_SYNC1 => { + self.state = DetectState::Meshtastic1; + } + magic::KISS_FEND => { + + if self.detected == Protocol::Unknown || self.detected == Protocol::RNode { + self.detected = Protocol::RNode; + return Some(Protocol::RNode); + } + } + b'A' => { + self.state = DetectState::At1; + } + _ => { + + } + } + } + + DetectState::MeshCore1 => { + if byte == magic::MESHCORE_SYNC2 { + self.detected = Protocol::MeshCore; + self.state = DetectState::Idle; + return Some(Protocol::MeshCore); + } else if byte == magic::MESHCORE_SYNC1 { + + } else { + self.state = DetectState::Idle; + } + } + + DetectState::Meshtastic1 => { + if byte == magic::MESHTASTIC_SYNC2 { + self.detected = Protocol::Meshtastic; + self.state = DetectState::Idle; + return Some(Protocol::Meshtastic); + } else { + self.state = DetectState::Idle; + } + } + + DetectState::At1 => { + if byte == b'T' { + self.detected = Protocol::AtCommand; + self.state = DetectState::Idle; + return Some(Protocol::AtCommand); + } else { + self.state = DetectState::Idle; + } + } + } + + if self.state != prev_state { + self.state_bytes = 0; + } + + None + } +} + +pub const MAX_TRANSPORTS: usize = 3; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum TransportType { + UsbSerial, + Ble, + WiFi, +} + +pub struct TransportState { + pub transport: TransportType, + pub detector: ProtocolDetector, + pub active: bool, +} + +impl TransportState { + pub fn new(transport: TransportType) -> Self { + Self { + transport, + detector: ProtocolDetector::new(), + active: false, + } + } +} + +pub struct ProtocolRouter { + + transports: [TransportState; MAX_TRANSPORTS], + + lora_protocol: Protocol, + + lora_shared: bool, + + priority_transport: Option, +} + +impl ProtocolRouter { + pub fn new() -> Self { + Self { + transports: [ + TransportState::new(TransportType::UsbSerial), + TransportState::new(TransportType::Ble), + TransportState::new(TransportType::WiFi), + ], + lora_protocol: Protocol::Unknown, + lora_shared: true, + priority_transport: None, + } + } + + pub fn transport(&mut self, t: TransportType) -> &mut TransportState { + match t { + TransportType::UsbSerial => &mut self.transports[0], + TransportType::Ble => &mut self.transports[1], + TransportType::WiFi => &mut self.transports[2], + } + } + + pub fn transport_ref(&self, t: TransportType) -> &TransportState { + match t { + TransportType::UsbSerial => &self.transports[0], + TransportType::Ble => &self.transports[1], + TransportType::WiFi => &self.transports[2], + } + } + + pub fn lora_protocol(&self) -> Protocol { + self.lora_protocol + } + + pub fn set_lora_protocol(&mut self, protocol: Protocol) { + self.lora_protocol = protocol; + } + + pub fn is_lora_shared(&self) -> bool { + self.lora_shared + } + + pub fn priority_transport(&self) -> Option { + self.priority_transport + } + + pub fn release_lora_control(&mut self, transport: TransportType) { + if self.priority_transport == Some(transport) { + self.priority_transport = None; + + } + + let state = self.transport(transport); + state.detector.reset(); + state.active = false; + } + + fn can_claim_lora(&self, transport: TransportType, protocol: Protocol) -> bool { + + if self.priority_transport.is_none() { + return true; + } + + if self.priority_transport == Some(transport) { + return true; + } + + if self.lora_protocol == protocol { + return true; + } + + false + } + + pub fn route_incoming(&mut self, transport: TransportType, byte: u8) -> Protocol { + + let idx = match transport { + TransportType::UsbSerial => 0, + TransportType::Ble => 1, + TransportType::WiFi => 2, + }; + + self.transports[idx].active = true; + + if let Some(protocol) = self.transports[idx].detector.feed(byte) { + + let can_claim = self.priority_transport.is_none() + || self.priority_transport == Some(transport) + || self.lora_protocol == protocol; + + if can_claim { + + if self.lora_protocol == Protocol::Unknown || self.lora_protocol == protocol { + self.lora_protocol = protocol; + } + + if self.priority_transport.is_none() && self.transports[idx].detector.is_locked() { + self.priority_transport = Some(transport); + } + } + protocol + } else { + self.transports[idx].detector.protocol() + } + } + + pub fn resolve_conflict(&mut self, transport: TransportType, new_protocol: Protocol) -> bool { + + if new_protocol == Protocol::AtCommand { + return true; + } + + if self.priority_transport.is_none() { + return true; + } + + if self.priority_transport == Some(transport) { + return true; + } + + if self.lora_protocol != new_protocol && self.lora_protocol != Protocol::Unknown { + return false; + } + + true + } + + pub fn status(&self) -> [(TransportType, Protocol, bool); MAX_TRANSPORTS] { + [ + (TransportType::UsbSerial, self.transports[0].detector.protocol(), self.transports[0].active), + (TransportType::Ble, self.transports[1].detector.protocol(), self.transports[1].active), + (TransportType::WiFi, self.transports[2].detector.protocol(), self.transports[2].active), + ] + } +} + +impl Default for ProtocolRouter { + fn default() -> Self { + Self::new() + } +} + +pub struct LoRaPacket { + pub protocol: Protocol, + pub data: Vec, + pub rssi: i16, + pub snr: i8, +} + +impl LoRaPacket { + + pub fn detect_protocol(data: &[u8]) -> Protocol { + if data.len() < 4 { + return Protocol::Unknown; + } + + if data.len() >= 20 { + + let channel_hash = data[3]; + + if data.len() >= 12 { + let flags = data[11]; + let hop_limit = flags & 0x07; + if hop_limit >= 1 && hop_limit <= 7 && channel_hash != 0 { + return Protocol::Meshtastic; + } + } + } + + if data.len() >= 9 { + let flags = data[8]; + + let msg_type = flags & 0x0F; + + let hop = (flags >> 4) & 0x0F; + if msg_type <= 15 && hop <= 7 { + return Protocol::MeshCore; + } + } + + if data.len() >= 18 { + + let context = data[16]; + let header_type = (context >> 6) & 0x03; + let propagation_type = (context >> 4) & 0x03; + + if header_type <= 3 && propagation_type <= 3 { + + let zeros = data[..16].iter().filter(|&&b| b == 0).count(); + if zeros < 8 { + return Protocol::RNode; + } + } + } + + Protocol::Unknown + } +} + +#[derive(Clone)] +pub struct UnifiedPacket { + + pub source_protocol: Protocol, + + pub dest_protocol: Protocol, + + pub payload: Vec, + + pub source_addr: [u8; 32], + + pub dest_addr: [u8; 32], + + pub hops: u8, + + pub rssi: i16, + + pub snr: i8, +} + +impl UnifiedPacket { + pub fn new() -> Self { + Self { + source_protocol: Protocol::Unknown, + dest_protocol: Protocol::Unknown, + payload: Vec::new(), + source_addr: [0; 32], + dest_addr: [0; 32], + hops: 0, + rssi: 0, + snr: 0, + } + } +} + +impl Default for UnifiedPacket { + fn default() -> Self { + Self::new() + } +} diff --git a/src/rng.rs b/src/rng.rs new file mode 100644 index 0000000..b87d408 --- /dev/null +++ b/src/rng.rs @@ -0,0 +1,292 @@ +use core::sync::atomic::{AtomicBool, AtomicU32, Ordering}; + +const RNG_DATA_REG: u32 = 0x6003_5110; + +const WIFI_MAC_TIME_REG: u32 = 0x6003_3010; + +const MAX_REPETITION_COUNT: u32 = 8; + +const APT_WINDOW_SIZE: u32 = 512; + +const APT_CUTOFF: u32 = 20; + +const MIN_SAMPLES_FOR_HEALTH: u32 = 64; + +const MIN_ENTROPY_SCALED: u32 = 24; + +static RNG_HEALTHY: AtomicBool = AtomicBool::new(false); + +static SAMPLE_COUNT: AtomicU32 = AtomicU32::new(0); + +static REPETITION_COUNT: AtomicU32 = AtomicU32::new(0); + +static LAST_VALUE: AtomicU32 = AtomicU32::new(0); + +static FAILURE_COUNT: AtomicU32 = AtomicU32::new(0); + +static BIT_COUNTS: [AtomicU32; 32] = [ + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), + AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), AtomicU32::new(0), +]; + +static INITIALIZED: AtomicBool = AtomicBool::new(false); + +pub fn init() { + if INITIALIZED.swap(true, Ordering::SeqCst) { + return; + } + + SAMPLE_COUNT.store(0, Ordering::SeqCst); + REPETITION_COUNT.store(0, Ordering::SeqCst); + FAILURE_COUNT.store(0, Ordering::SeqCst); + + for _ in 0..MIN_SAMPLES_FOR_HEALTH { + let _ = raw_random_u32_with_health(); + } + + let failures = FAILURE_COUNT.load(Ordering::SeqCst); + let healthy = failures == 0 && estimate_entropy() >= MIN_ENTROPY_SCALED; + RNG_HEALTHY.store(healthy, Ordering::SeqCst); + + if !healthy { + log_rng_warning("RNG health check failed during initialization"); + } +} + +static RNG_WARNING_COUNT: AtomicU32 = AtomicU32::new(0); + +#[inline] +fn log_rng_warning(_msg: &str) { + RNG_WARNING_COUNT.fetch_add(1, Ordering::SeqCst); + +} + +pub fn warning_count() -> u32 { + RNG_WARNING_COUNT.load(Ordering::SeqCst) +} + +#[inline] +fn hw_rng_raw() -> u32 { + unsafe { + let reg = RNG_DATA_REG as *const u32; + core::ptr::read_volatile(reg) + } +} + +fn raw_random_u32_with_health() -> u32 { + let value = hw_rng_raw(); + update_health_state(value); + value +} + +fn update_health_state(value: u32) { + let count = SAMPLE_COUNT.fetch_add(1, Ordering::SeqCst); + + let last = LAST_VALUE.swap(value, Ordering::SeqCst); + if value == last { + let rep = REPETITION_COUNT.fetch_add(1, Ordering::SeqCst) + 1; + if rep >= MAX_REPETITION_COUNT { + FAILURE_COUNT.fetch_add(1, Ordering::SeqCst); + RNG_HEALTHY.store(false, Ordering::SeqCst); + log_rng_warning("RNG repetition count exceeded"); + } + } else { + REPETITION_COUNT.store(0, Ordering::SeqCst); + } + + for i in 0..32 { + if (value >> i) & 1 == 1 { + BIT_COUNTS[i].fetch_add(1, Ordering::Relaxed); + } + } + + if count > 0 && count % APT_WINDOW_SIZE == 0 { + reassess_health(); + } +} + +fn reassess_health() { + let failures = FAILURE_COUNT.load(Ordering::SeqCst); + let entropy = estimate_entropy(); + + if failures == 0 && entropy >= MIN_ENTROPY_SCALED { + RNG_HEALTHY.store(true, Ordering::SeqCst); + } else if entropy < MIN_ENTROPY_SCALED { + RNG_HEALTHY.store(false, Ordering::SeqCst); + log_rng_warning("RNG entropy below threshold"); + } + + if failures > 0 { + + let _ = FAILURE_COUNT.compare_exchange( + failures, + failures - 1, + Ordering::SeqCst, + Ordering::Relaxed + ); + + } + + if SAMPLE_COUNT.load(Ordering::SeqCst) % (APT_WINDOW_SIZE * 4) == 0 { + for bc in &BIT_COUNTS { + bc.store(0, Ordering::Relaxed); + } + } +} + +fn estimate_entropy() -> u32 { + let samples = SAMPLE_COUNT.load(Ordering::SeqCst); + if samples < MIN_SAMPLES_FOR_HEALTH { + return 0; + } + + let mut total_entropy: u32 = 0; + + for bc in &BIT_COUNTS { + let ones = bc.load(Ordering::Relaxed); + + let expected = samples / 2; + let diff = if ones > expected { ones - expected } else { expected - ones }; + + let scaled_entropy = if diff >= expected { + 0 + } else { + + 8u32.saturating_sub((8 * diff) / expected.max(1)) + }; + + total_entropy += scaled_entropy; + } + + total_entropy / 32 +} + +#[inline] +pub fn is_healthy() -> bool { + RNG_HEALTHY.load(Ordering::SeqCst) +} + +pub fn health_stats() -> RngHealthStats { + RngHealthStats { + healthy: RNG_HEALTHY.load(Ordering::SeqCst), + sample_count: SAMPLE_COUNT.load(Ordering::SeqCst), + failure_count: FAILURE_COUNT.load(Ordering::SeqCst), + estimated_entropy: estimate_entropy(), + } +} + +#[derive(Debug, Clone, Copy)] +pub struct RngHealthStats { + + pub healthy: bool, + + pub sample_count: u32, + + pub failure_count: u32, + + pub estimated_entropy: u32, +} + +pub fn random_u32() -> u32 { + if !INITIALIZED.load(Ordering::SeqCst) { + init(); + } + + let healthy = RNG_HEALTHY.load(Ordering::SeqCst); + + let r1 = raw_random_u32_with_health(); + + for _ in 0..5 { + core::hint::spin_loop(); + } + + let r2 = raw_random_u32_with_health(); + + let time_entropy = read_timer_entropy(); + + let mut result = mix_entropy(r1, r2, time_entropy); + + if !healthy { + log_rng_warning("RNG unhealthy - applying compensating entropy mixing"); + + for i in 0..4 { + for _ in 0..10 { + core::hint::spin_loop(); + } + let extra = raw_random_u32_with_health(); + let time = read_timer_entropy(); + result = mix_entropy(result, extra, time.wrapping_add(i)); + } + } + + result +} + +pub fn random_u32_checked() -> Option { + if !is_healthy() { + return None; + } + Some(random_u32()) +} + +pub fn fill_random(dest: &mut [u8]) { + if !INITIALIZED.load(Ordering::SeqCst) { + init(); + } + + let mut offset = 0; + while offset < dest.len() { + let random = random_u32(); + let bytes = random.to_le_bytes(); + + let remaining = dest.len() - offset; + let to_copy = remaining.min(4); + + dest[offset..offset + to_copy].copy_from_slice(&bytes[..to_copy]); + offset += to_copy; + } +} + +pub fn fill_random_checked(dest: &mut [u8]) -> bool { + if !is_healthy() { + return false; + } + fill_random(dest); + true +} + +pub fn recheck_health() { + + FAILURE_COUNT.store(0, Ordering::SeqCst); + + for _ in 0..MIN_SAMPLES_FOR_HEALTH { + let _ = raw_random_u32_with_health(); + } + + reassess_health(); +} + +fn read_timer_entropy() -> u32 { + unsafe { + + let timer = core::ptr::read_volatile(WIFI_MAC_TIME_REG as *const u32); + timer + } +} + +#[inline] +fn mix_entropy(a: u32, b: u32, c: u32) -> u32 { + let mut h = a; + h ^= b.rotate_left(13); + h = h.wrapping_mul(0x85EBCA6B); + h ^= c.rotate_right(7); + h = h.wrapping_mul(0xC2B2AE35); + h ^= h >> 16; + h +} diff --git a/src/rnode.rs b/src/rnode.rs new file mode 100644 index 0000000..613fd53 --- /dev/null +++ b/src/rnode.rs @@ -0,0 +1,1014 @@ +use heapless::Vec; +use crate::crypto::sha256::Sha256; + +pub const FEND: u8 = 0xC0; + +pub const FESC: u8 = 0xDB; + +pub const TFEND: u8 = 0xDC; + +pub const TFESC: u8 = 0xDD; + +pub const MAX_DATA_SIZE: usize = 512; + +pub const MAX_FRAME_SIZE: usize = MAX_DATA_SIZE * 2 + 4; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum KissCommand { + + DataFrame = 0x00, + + TxDelay = 0x01, + + Persistence = 0x02, + + SlotTime = 0x03, + + TxTail = 0x04, + + FullDuplex = 0x05, + + SetHardware = 0x06, + + Return = 0xFF, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum RNodeCommand { + + Frequency = 0x01, + + Bandwidth = 0x02, + + TxPower = 0x03, + + SpreadingFactor = 0x04, + + CodingRate = 0x05, + + RadioState = 0x06, + + RadioLock = 0x07, + + Detect = 0x08, + + Promisc = 0x0E, + + Ready = 0x0F, + + PreambleLength = 0x10, + + SymbolTimeout = 0x11, + + SyncWord = 0x12, + + CrcMode = 0x13, + + ImplicitHeader = 0x14, + + Ldro = 0x15, + + Leave = 0x0A, + + SaveConfig = 0x0B, + + ResetConfig = 0x0C, + + Bootloader = 0x0D, + + StatRx = 0x21, + + StatTx = 0x22, + + StatRssi = 0x23, + + StatSnr = 0x24, + + StatBattery = 0x25, + + StatChannel = 0x26, + + AirtimeLimit = 0x27, + + AirtimeUsage = 0x28, + + Blink = 0x30, + + LedIntensity = 0x31, + + Random = 0x40, + + FwVersion = 0x50, + + ProtocolVersion = 0x51, + + Platform = 0x48, + + Mcu = 0x49, + + Board = 0x4A, + + RomInfo = 0x4B, + + HardwareSerial = 0x55, + + Signature = 0x56, + + TcxoVoltage = 0x60, + + Error = 0x90, + + RomData = 0xA0, + + Info = 0xB0, + + DataRssi = 0xFE, +} + +impl RNodeCommand { + pub fn from_byte(b: u8) -> Option { + match b { + + 0x01 => Some(RNodeCommand::Frequency), + 0x02 => Some(RNodeCommand::Bandwidth), + 0x03 => Some(RNodeCommand::TxPower), + 0x04 => Some(RNodeCommand::SpreadingFactor), + 0x05 => Some(RNodeCommand::CodingRate), + 0x06 => Some(RNodeCommand::RadioState), + 0x07 => Some(RNodeCommand::RadioLock), + 0x08 => Some(RNodeCommand::Detect), + 0x0A => Some(RNodeCommand::Leave), + 0x0B => Some(RNodeCommand::SaveConfig), + 0x0C => Some(RNodeCommand::ResetConfig), + 0x0D => Some(RNodeCommand::Bootloader), + 0x0E => Some(RNodeCommand::Promisc), + 0x0F => Some(RNodeCommand::Ready), + + 0x10 => Some(RNodeCommand::PreambleLength), + 0x11 => Some(RNodeCommand::SymbolTimeout), + 0x12 => Some(RNodeCommand::SyncWord), + 0x13 => Some(RNodeCommand::CrcMode), + 0x14 => Some(RNodeCommand::ImplicitHeader), + 0x15 => Some(RNodeCommand::Ldro), + + 0x21 => Some(RNodeCommand::StatRx), + 0x22 => Some(RNodeCommand::StatTx), + 0x23 => Some(RNodeCommand::StatRssi), + 0x24 => Some(RNodeCommand::StatSnr), + 0x25 => Some(RNodeCommand::StatBattery), + 0x26 => Some(RNodeCommand::StatChannel), + 0x27 => Some(RNodeCommand::AirtimeLimit), + 0x28 => Some(RNodeCommand::AirtimeUsage), + + 0x30 => Some(RNodeCommand::Blink), + 0x31 => Some(RNodeCommand::LedIntensity), + 0x40 => Some(RNodeCommand::Random), + 0x48 => Some(RNodeCommand::Platform), + 0x49 => Some(RNodeCommand::Mcu), + 0x4A => Some(RNodeCommand::Board), + 0x4B => Some(RNodeCommand::RomInfo), + 0x50 => Some(RNodeCommand::FwVersion), + 0x51 => Some(RNodeCommand::ProtocolVersion), + 0x55 => Some(RNodeCommand::HardwareSerial), + 0x56 => Some(RNodeCommand::Signature), + 0x60 => Some(RNodeCommand::TcxoVoltage), + + 0x90 => Some(RNodeCommand::Error), + 0xA0 => Some(RNodeCommand::RomData), + 0xB0 => Some(RNodeCommand::Info), + 0xFE => Some(RNodeCommand::DataRssi), + _ => None, + } + } +} + +#[derive(Debug, Clone)] +pub struct KissFrame { + + pub command: u8, + + pub data: Vec, +} + +impl KissFrame { + + pub fn new(command: u8) -> Self { + Self { + command, + data: Vec::new(), + } + } + + pub fn data_frame(data: &[u8]) -> Option { + if data.len() > MAX_DATA_SIZE { + return None; + } + let mut frame = Self::new(KissCommand::DataFrame as u8); + for &b in data { + let _ = frame.data.push(b); + } + Some(frame) + } + + pub fn command_frame(cmd: RNodeCommand, data: &[u8]) -> Option { + if data.len() > MAX_DATA_SIZE { + return None; + } + let mut frame = Self::new(cmd as u8); + for &b in data { + let _ = frame.data.push(b); + } + Some(frame) + } + + pub fn encode(&self) -> Vec { + let mut buf = Vec::new(); + + let _ = buf.push(FEND); + + escape_byte(self.command, &mut buf); + + for &b in &self.data { + escape_byte(b, &mut buf); + } + + let _ = buf.push(FEND); + + buf + } + + pub fn port(&self) -> u8 { + (self.command >> 4) & 0x0F + } + + pub fn cmd_type(&self) -> u8 { + self.command & 0x0F + } +} + +fn escape_byte(byte: u8, buf: &mut Vec) { + match byte { + FEND => { + let _ = buf.push(FESC); + let _ = buf.push(TFEND); + } + FESC => { + let _ = buf.push(FESC); + let _ = buf.push(TFESC); + } + _ => { + let _ = buf.push(byte); + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum ParserState { + + WaitStart, + + WaitCommand, + + ReadData, + + Escape, +} + +pub struct KissParser { + state: ParserState, + command: u8, + data: Vec, +} + +impl Default for KissParser { + fn default() -> Self { + Self::new() + } +} + +impl KissParser { + pub fn new() -> Self { + Self { + state: ParserState::WaitStart, + command: 0, + data: Vec::new(), + } + } + + pub fn reset(&mut self) { + self.state = ParserState::WaitStart; + self.command = 0; + self.data.clear(); + } + + pub fn feed(&mut self, byte: u8) -> Option { + match self.state { + ParserState::WaitStart => { + if byte == FEND { + self.state = ParserState::WaitCommand; + self.data.clear(); + } + } + + ParserState::WaitCommand => { + match byte { + FEND => { + + } + FESC => { + + self.state = ParserState::Escape; + } + _ => { + self.command = byte; + self.state = ParserState::ReadData; + } + } + } + + ParserState::ReadData => { + match byte { + FEND => { + + let frame = KissFrame { + command: self.command, + data: self.data.clone(), + }; + self.reset(); + return Some(frame); + } + FESC => { + self.state = ParserState::Escape; + } + _ => { + let _ = self.data.push(byte); + } + } + } + + ParserState::Escape => { + let unescaped = match byte { + TFEND => FEND, + TFESC => FESC, + _ => byte, + }; + let _ = self.data.push(unescaped); + self.state = ParserState::ReadData; + } + } + None + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum RNodeState { + + Offline, + + Online, + + Transmitting, + + Receiving, +} + +#[derive(Debug, Clone)] +pub struct RNodeConfig { + + pub frequency: u32, + + pub bandwidth: u32, + + pub tx_power: i8, + + pub spreading_factor: u8, + + pub coding_rate: u8, + + pub preamble_length: u16, + + pub sync_word: u8, + + pub crc_enabled: bool, + + pub implicit_header: bool, + + pub ldro: bool, +} + +impl Default for RNodeConfig { + fn default() -> Self { + Self { + frequency: 868_100_000, + bandwidth: 125_000, + tx_power: 14, + spreading_factor: 9, + coding_rate: 5, + preamble_length: 8, + sync_word: 0x12, + crc_enabled: true, + implicit_header: false, + ldro: false, + } + } +} + +impl RNodeConfig { + + pub fn eu868() -> Self { + Self { + frequency: 868_100_000, + ..Default::default() + } + } + + pub fn us915() -> Self { + Self { + frequency: 915_000_000, + ..Default::default() + } + } + + pub fn bandwidth_to_hz(bw_value: u8) -> u32 { + match bw_value { + 0 => 125_000, + 1 => 250_000, + 2 => 500_000, + _ => 125_000, + } + } + + pub fn hz_to_bandwidth(hz: u32) -> u8 { + match hz { + 0..=187_500 => 0, + 187_501..=375_000 => 1, + _ => 2, + } + } + + pub fn coding_rate_to_ratio(cr: u8) -> (u8, u8) { + (4, cr) + } + + pub fn should_enable_ldro(&self) -> bool { + + if self.bandwidth <= 125_000 { + self.spreading_factor >= 11 + } else if self.bandwidth <= 250_000 { + self.spreading_factor >= 12 + } else { + false + } + } + + pub fn packet_airtime_ms(&self, payload_len: usize) -> u32 { + let sf = self.spreading_factor as f32; + let bw = self.bandwidth as f32; + let cr = self.coding_rate as f32; + let pl = payload_len as f32; + let preamble = self.preamble_length as f32; + + let t_sym = (2.0_f32.powf(sf)) / bw * 1000.0; + + let t_preamble = (preamble + 4.25) * t_sym; + + let de = if self.should_enable_ldro() { 1.0 } else { 0.0 }; + let h = if self.implicit_header { 1.0 } else { 0.0 }; + let crc = if self.crc_enabled { 1.0 } else { 0.0 }; + + let numerator = 8.0 * pl - 4.0 * sf + 28.0 + 16.0 * crc - 20.0 * h; + let denominator = 4.0 * (sf - 2.0 * de); + let n_payload = 8.0 + (numerator / denominator).ceil().max(0.0) * (cr); + + let t_payload = n_payload * t_sym; + + (t_preamble + t_payload) as u32 + } + + #[cfg(feature = "sx1262")] + pub fn to_radio_config(&self) -> crate::sx1262::RadioConfig { + crate::sx1262::RadioConfig { + frequency: self.frequency, + spreading_factor: self.spreading_factor, + bandwidth: Self::hz_to_bandwidth(self.bandwidth), + coding_rate: self.coding_rate.saturating_sub(4), + tx_power: self.tx_power, + sync_word: self.sync_word, + preamble_length: self.preamble_length, + crc_enabled: self.crc_enabled, + implicit_header: self.implicit_header, + ldro: self.should_enable_ldro(), + } + } +} + +#[derive(Debug, Clone, Default)] +pub struct RNodeStats { + + pub rx_count: u32, + + pub rx_bytes: u64, + + pub tx_count: u32, + + pub tx_bytes: u64, + + pub last_rssi: i16, + + pub last_snr: i8, + + pub airtime_used: u64, + + pub channel_busy: u64, +} + +#[derive(Debug, Clone)] +pub struct RNodeIdentity { + + pub serial: [u8; 16], + + pub platform: &'static str, + + pub mcu: &'static str, + + pub board: &'static str, + + pub fw_version: &'static str, + + pub protocol_version: u8, + + pub hw_revision: u8, +} + +impl Default for RNodeIdentity { + fn default() -> Self { + Self { + serial: [0u8; 16], + platform: "ESP32-S3", + mcu: "ESP32-S3", + board: "LunarNode", + fw_version: "1.0.0-lunarcore", + protocol_version: 1, + hw_revision: 1, + } + } +} + +impl RNodeIdentity { + + pub fn identity_hash(&self) -> [u8; 32] { + let mut data = [0u8; 64]; + data[..16].copy_from_slice(&self.serial); + data[16..24].copy_from_slice(self.platform.as_bytes().get(..8).unwrap_or(b"ESP32-S3")); + data[24] = self.protocol_version; + data[25] = self.hw_revision; + Sha256::hash(&data) + } +} + +pub struct RNodeHandler { + + parser: KissParser, + + config: RNodeConfig, + + state: RNodeState, + + locked: bool, + + promiscuous: bool, + + stats: RNodeStats, + + identity: RNodeIdentity, + + random_seed: u32, + + battery_mv: u16, + + led_intensity: u8, + + airtime_limit: u32, +} + +impl RNodeHandler { + + pub fn new() -> Self { + Self { + parser: KissParser::new(), + config: RNodeConfig::default(), + state: RNodeState::Offline, + locked: false, + promiscuous: false, + stats: RNodeStats::default(), + identity: RNodeIdentity::default(), + random_seed: 0, + battery_mv: 0, + led_intensity: 64, + airtime_limit: 0, + } + } + + pub fn with_identity(identity: RNodeIdentity) -> Self { + Self { + identity, + ..Self::new() + } + } + + pub fn set_serial(&mut self, serial: &[u8; 16]) { + self.identity.serial = *serial; + } + + pub fn set_battery_voltage(&mut self, mv: u16) { + self.battery_mv = mv; + } + + pub fn set_random_seed(&mut self, seed: u32) { + self.random_seed = seed; + } + + pub fn next_random(&mut self) -> u32 { + + self.random_seed = self.random_seed.wrapping_mul(1103515245).wrapping_add(12345); + self.random_seed + } + + pub fn feed_serial(&mut self, byte: u8) -> Option { + self.parser.feed(byte) + } + + pub fn process_frame(&mut self, frame: &KissFrame) -> Option { + + if frame.command == KissCommand::DataFrame as u8 { + return None; + } + + match RNodeCommand::from_byte(frame.command) { + + Some(RNodeCommand::Frequency) => { + if frame.data.len() >= 4 && !self.locked { + self.config.frequency = u32::from_be_bytes([ + frame.data[0], frame.data[1], frame.data[2], frame.data[3] + ]); + } + + KissFrame::command_frame(RNodeCommand::Frequency, &self.config.frequency.to_be_bytes()) + } + + Some(RNodeCommand::Bandwidth) => { + if frame.data.len() >= 4 && !self.locked { + self.config.bandwidth = u32::from_be_bytes([ + frame.data[0], frame.data[1], frame.data[2], frame.data[3] + ]); + } + KissFrame::command_frame(RNodeCommand::Bandwidth, &self.config.bandwidth.to_be_bytes()) + } + + Some(RNodeCommand::TxPower) => { + if frame.data.len() >= 1 && !self.locked { + + let power = (frame.data[0] as i8).max(-9).min(22); + self.config.tx_power = power; + } + KissFrame::command_frame(RNodeCommand::TxPower, &[self.config.tx_power as u8]) + } + + Some(RNodeCommand::SpreadingFactor) => { + if frame.data.len() >= 1 && !self.locked { + let sf = frame.data[0].max(7).min(12); + self.config.spreading_factor = sf; + + self.config.ldro = self.config.should_enable_ldro(); + } + KissFrame::command_frame(RNodeCommand::SpreadingFactor, &[self.config.spreading_factor]) + } + + Some(RNodeCommand::CodingRate) => { + if frame.data.len() >= 1 && !self.locked { + let cr = frame.data[0].max(5).min(8); + self.config.coding_rate = cr; + } + KissFrame::command_frame(RNodeCommand::CodingRate, &[self.config.coding_rate]) + } + + Some(RNodeCommand::PreambleLength) => { + if frame.data.len() >= 2 && !self.locked { + self.config.preamble_length = u16::from_be_bytes([frame.data[0], frame.data[1]]); + } + KissFrame::command_frame(RNodeCommand::PreambleLength, &self.config.preamble_length.to_be_bytes()) + } + + Some(RNodeCommand::SyncWord) => { + if frame.data.len() >= 1 && !self.locked { + self.config.sync_word = frame.data[0]; + } + KissFrame::command_frame(RNodeCommand::SyncWord, &[self.config.sync_word]) + } + + Some(RNodeCommand::CrcMode) => { + if frame.data.len() >= 1 && !self.locked { + self.config.crc_enabled = frame.data[0] != 0; + } + KissFrame::command_frame(RNodeCommand::CrcMode, &[if self.config.crc_enabled { 1 } else { 0 }]) + } + + Some(RNodeCommand::ImplicitHeader) => { + if frame.data.len() >= 1 && !self.locked { + self.config.implicit_header = frame.data[0] != 0; + } + KissFrame::command_frame(RNodeCommand::ImplicitHeader, &[if self.config.implicit_header { 1 } else { 0 }]) + } + + Some(RNodeCommand::Ldro) => { + if frame.data.len() >= 1 && !self.locked { + self.config.ldro = frame.data[0] != 0; + } + KissFrame::command_frame(RNodeCommand::Ldro, &[if self.config.ldro { 1 } else { 0 }]) + } + + Some(RNodeCommand::RadioState) => { + if frame.data.len() >= 1 { + self.state = if frame.data[0] != 0 { + RNodeState::Online + } else { + RNodeState::Offline + }; + } + KissFrame::command_frame( + RNodeCommand::RadioState, + &[if self.state != RNodeState::Offline { 1 } else { 0 }], + ) + } + + Some(RNodeCommand::RadioLock) => { + if frame.data.len() >= 1 { + self.locked = frame.data[0] != 0; + } + KissFrame::command_frame(RNodeCommand::RadioLock, &[if self.locked { 1 } else { 0 }]) + } + + Some(RNodeCommand::Promisc) => { + if frame.data.len() >= 1 { + self.promiscuous = frame.data[0] != 0; + } + KissFrame::command_frame(RNodeCommand::Promisc, &[if self.promiscuous { 1 } else { 0 }]) + } + + Some(RNodeCommand::Detect) => { + + KissFrame::command_frame(RNodeCommand::Detect, &[0x01, self.identity.hw_revision]) + } + + Some(RNodeCommand::Ready) => { + + KissFrame::command_frame(RNodeCommand::Ready, &[0x01]) + } + + Some(RNodeCommand::FwVersion) => { + KissFrame::command_frame( + RNodeCommand::FwVersion, + self.identity.fw_version.as_bytes(), + ) + } + + Some(RNodeCommand::ProtocolVersion) => { + KissFrame::command_frame(RNodeCommand::ProtocolVersion, &[self.identity.protocol_version]) + } + + Some(RNodeCommand::Platform) => { + KissFrame::command_frame( + RNodeCommand::Platform, + self.identity.platform.as_bytes(), + ) + } + + Some(RNodeCommand::Mcu) => { + KissFrame::command_frame( + RNodeCommand::Mcu, + self.identity.mcu.as_bytes(), + ) + } + + Some(RNodeCommand::Board) => { + KissFrame::command_frame( + RNodeCommand::Board, + self.identity.board.as_bytes(), + ) + } + + Some(RNodeCommand::HardwareSerial) => { + KissFrame::command_frame(RNodeCommand::HardwareSerial, &self.identity.serial) + } + + Some(RNodeCommand::Signature) => { + + let hash = self.identity.identity_hash(); + KissFrame::command_frame(RNodeCommand::Signature, &hash) + } + + Some(RNodeCommand::StatRx) => { + KissFrame::command_frame(RNodeCommand::StatRx, &self.stats.rx_count.to_be_bytes()) + } + + Some(RNodeCommand::StatTx) => { + KissFrame::command_frame(RNodeCommand::StatTx, &self.stats.tx_count.to_be_bytes()) + } + + Some(RNodeCommand::StatRssi) => { + KissFrame::command_frame(RNodeCommand::StatRssi, &self.stats.last_rssi.to_be_bytes()) + } + + Some(RNodeCommand::StatSnr) => { + KissFrame::command_frame(RNodeCommand::StatSnr, &[self.stats.last_snr as u8]) + } + + Some(RNodeCommand::StatBattery) => { + KissFrame::command_frame(RNodeCommand::StatBattery, &self.battery_mv.to_be_bytes()) + } + + Some(RNodeCommand::StatChannel) => { + + let util = if self.stats.channel_busy > 0 { + ((self.stats.airtime_used * 100) / self.stats.channel_busy).min(100) as u8 + } else { + 0u8 + }; + KissFrame::command_frame(RNodeCommand::StatChannel, &[util]) + } + + Some(RNodeCommand::AirtimeLimit) => { + if frame.data.len() >= 4 { + self.airtime_limit = u32::from_be_bytes([ + frame.data[0], frame.data[1], frame.data[2], frame.data[3] + ]); + } + KissFrame::command_frame(RNodeCommand::AirtimeLimit, &self.airtime_limit.to_be_bytes()) + } + + Some(RNodeCommand::AirtimeUsage) => { + let usage = (self.stats.airtime_used % (u32::MAX as u64)) as u32; + KissFrame::command_frame(RNodeCommand::AirtimeUsage, &usage.to_be_bytes()) + } + + Some(RNodeCommand::Blink) => { + + KissFrame::command_frame(RNodeCommand::Blink, &[0x01]) + } + + Some(RNodeCommand::LedIntensity) => { + if frame.data.len() >= 1 { + self.led_intensity = frame.data[0]; + } + KissFrame::command_frame(RNodeCommand::LedIntensity, &[self.led_intensity]) + } + + Some(RNodeCommand::Random) => { + + let r = self.next_random(); + KissFrame::command_frame(RNodeCommand::Random, &r.to_be_bytes()) + } + + Some(RNodeCommand::Leave) => { + + self.state = RNodeState::Offline; + None + } + + Some(RNodeCommand::SaveConfig) => { + + KissFrame::command_frame(RNodeCommand::SaveConfig, &[0x01]) + } + + Some(RNodeCommand::ResetConfig) => { + if !self.locked { + self.config = RNodeConfig::default(); + } + KissFrame::command_frame(RNodeCommand::ResetConfig, &[0x01]) + } + + Some(RNodeCommand::Bootloader) => { + + KissFrame::command_frame(RNodeCommand::Bootloader, &[0x01]) + } + + Some(RNodeCommand::Error) => { + + if !frame.data.is_empty() { + KissFrame::command_frame(RNodeCommand::Error, &frame.data) + } else { + None + } + } + + _ => { + + KissFrame::command_frame(RNodeCommand::Error, &[0xFF]) + } + } + } + + pub fn process_lora_packet(&mut self, data: &[u8], rssi: i16, snr: i8) -> KissFrame { + self.stats.rx_count += 1; + self.stats.rx_bytes += data.len() as u64; + self.stats.last_rssi = rssi; + self.stats.last_snr = snr; + + let mut frame = KissFrame::new(RNodeCommand::DataRssi as u8); + for &b in data { + let _ = frame.data.push(b); + } + + let rssi_bytes = rssi.to_be_bytes(); + let _ = frame.data.push(rssi_bytes[0]); + let _ = frame.data.push(rssi_bytes[1]); + let _ = frame.data.push(snr as u8); + + frame + } + + pub fn process_lora_packet_raw(&mut self, data: &[u8]) -> KissFrame { + self.stats.rx_count += 1; + self.stats.rx_bytes += data.len() as u64; + + let mut frame = KissFrame::new(KissCommand::DataFrame as u8); + for &b in data { + let _ = frame.data.push(b); + } + frame + } + + pub fn get_tx_data<'a>(&mut self, frame: &'a KissFrame) -> Option<&'a [u8]> { + if frame.command == KissCommand::DataFrame as u8 { + self.stats.tx_count += 1; + self.stats.tx_bytes += frame.data.len() as u64; + + let airtime = self.config.packet_airtime_ms(frame.data.len()) as u64; + self.stats.airtime_used += airtime; + Some(&frame.data) + } else { + None + } + } + + pub fn record_channel_busy(&mut self, ms: u64) { + self.stats.channel_busy += ms; + } + + pub fn config(&self) -> &RNodeConfig { + &self.config + } + + pub fn config_mut(&mut self) -> &mut RNodeConfig { + &mut self.config + } + + pub fn state(&self) -> RNodeState { + self.state + } + + pub fn set_state(&mut self, state: RNodeState) { + self.state = state; + } + + pub fn stats(&self) -> &RNodeStats { + &self.stats + } + + pub fn reset_stats(&mut self) { + self.stats = RNodeStats::default(); + } + + pub fn identity(&self) -> &RNodeIdentity { + &self.identity + } + + pub fn is_online(&self) -> bool { + self.state != RNodeState::Offline + } + + pub fn is_locked(&self) -> bool { + self.locked + } + + pub fn is_promiscuous(&self) -> bool { + self.promiscuous + } + + pub fn check_airtime_limit(&self, packet_len: usize) -> bool { + if self.airtime_limit == 0 { + return true; + } + let airtime = self.config.packet_airtime_ms(packet_len) as u64; + + self.stats.airtime_used + airtime <= self.airtime_limit as u64 + } +} + +impl Default for RNodeHandler { + fn default() -> Self { + Self::new() + } +} diff --git a/src/session.rs b/src/session.rs new file mode 100644 index 0000000..9b7ca58 --- /dev/null +++ b/src/session.rs @@ -0,0 +1,781 @@ +use crate::crypto::{ + x25519::{x25519, x25519_base}, + hkdf::Hkdf, + aes::{Aes256, AesMode}, +}; +use heapless::Vec as HeaplessVec; +use std::collections::HashMap; + +#[cfg(target_arch = "xtensa")] +fn fill_random(dest: &mut [u8]) { + + const RNG_DATA_REG: u32 = 0x3FF7_5144; + + for chunk in dest.chunks_mut(4) { + + let random_word: u32 = unsafe { + core::ptr::read_volatile(RNG_DATA_REG as *const u32) + }; + let bytes = random_word.to_le_bytes(); + for (i, byte) in chunk.iter_mut().enumerate() { + *byte = bytes[i]; + } + } +} + +#[cfg(not(target_arch = "xtensa"))] +fn fill_random(dest: &mut [u8]) { + + use crate::crypto::sha256::Sha256; + static mut COUNTER: u64 = 0; + + let mut seed = [0u8; 40]; + unsafe { + seed[..8].copy_from_slice(&COUNTER.to_le_bytes()); + COUNTER = COUNTER.wrapping_add(1); + } + + let stack_addr = &seed as *const _ as usize; + seed[8..16].copy_from_slice(&stack_addr.to_le_bytes()); + + let hash = Sha256::hash(&seed); + let copy_len = core::cmp::min(dest.len(), 32); + dest[..copy_len].copy_from_slice(&hash[..copy_len]); + + if dest.len() > 32 { + fill_random(&mut dest[32..]); + } +} + +#[inline(never)] +fn constant_time_eq(a: &[u8], b: &[u8]) -> bool { + if a.len() != b.len() { + return false; + } + + let mut result: u8 = 0; + for (x, y) in a.iter().zip(b.iter()) { + result |= x ^ y; + } + + unsafe { + core::ptr::read_volatile(&result) == 0 + } +} + +const MAX_MESSAGES_BEFORE_RATCHET: u64 = 100; + +const MAX_TIME_BEFORE_RATCHET_SECS: u64 = 600; + +const MAX_SKIPPED_KEYS: usize = 100; + +const SESSION_HINT_INFO: &[u8] = b"session-hint-v1"; + +const ROOT_KEY_INFO: &[u8] = b"lunarpunk-root-key-v2"; + +const CHAIN_KEY_INFO: &[u8] = b"lunarpunk-chain-key-v2"; + +const MESSAGE_KEY_INFO: &[u8] = b"lunarpunk-message-key-v2"; + +#[derive(Clone)] +pub struct Session { + + root_key: [u8; 32], + + send_chain_key: [u8; 32], + + recv_chain_key: [u8; 32], + + send_ratchet_private: [u8; 32], + + send_ratchet_public: [u8; 32], + + recv_ratchet_public: [u8; 32], + + send_count: u64, + + recv_count: u64, + + prev_recv_chain: u64, + + last_ratchet_time: u64, + + skipped_keys: HashMap<([u8; 8], u64), [u8; 32]>, + + established: bool, +} + +pub struct SessionParams { + + pub shared_secret: [u8; 32], + + pub our_private: [u8; 32], + + pub their_public: [u8; 32], + + pub is_initiator: bool, +} + +#[derive(Debug, Clone)] +pub struct MessageHeader { + + pub dh_public: [u8; 32], + + pub prev_chain_len: u64, + + pub message_num: u64, +} + +impl MessageHeader { + + pub fn encode(&self) -> [u8; 48] { + let mut buf = [0u8; 48]; + buf[..32].copy_from_slice(&self.dh_public); + buf[32..40].copy_from_slice(&self.prev_chain_len.to_le_bytes()); + buf[40..48].copy_from_slice(&self.message_num.to_le_bytes()); + buf + } + + pub fn decode(data: &[u8]) -> Option { + if data.len() < 48 { + return None; + } + let mut dh_public = [0u8; 32]; + dh_public.copy_from_slice(&data[..32]); + + let mut prev_bytes = [0u8; 8]; + prev_bytes.copy_from_slice(&data[32..40]); + let prev_chain_len = u64::from_le_bytes(prev_bytes); + + let mut num_bytes = [0u8; 8]; + num_bytes.copy_from_slice(&data[40..48]); + let message_num = u64::from_le_bytes(num_bytes); + + Some(Self { + dh_public, + prev_chain_len, + message_num, + }) + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SessionError { + + NotEstablished, + + InvalidFormat, + + DecryptionFailed, + + OldChain, + + TooManySkipped, + + KeyDerivationFailed, +} + +impl Session { + + pub fn new(params: SessionParams) -> Self { + + let mut root_key = [0u8; 32]; + let mut send_chain_key = [0u8; 32]; + let mut recv_chain_key = [0u8; 32]; + + let salt = if params.is_initiator { + b"initiator-salt-v1" + } else { + b"responder-salt-v1" + }; + + Hkdf::derive(¶ms.shared_secret, salt, ROOT_KEY_INFO, &mut root_key); + + let dh_output = x25519(¶ms.our_private, ¶ms.their_public); + + let mut kdf_input = [0u8; 64]; + kdf_input[..32].copy_from_slice(&root_key); + kdf_input[32..].copy_from_slice(&dh_output); + + if params.is_initiator { + Hkdf::derive(&kdf_input, b"send", CHAIN_KEY_INFO, &mut send_chain_key); + Hkdf::derive(&kdf_input, b"recv", CHAIN_KEY_INFO, &mut recv_chain_key); + } else { + Hkdf::derive(&kdf_input, b"recv", CHAIN_KEY_INFO, &mut send_chain_key); + Hkdf::derive(&kdf_input, b"send", CHAIN_KEY_INFO, &mut recv_chain_key); + } + + let send_ratchet_public = x25519_base(¶ms.our_private); + + Self { + root_key, + send_chain_key, + recv_chain_key, + send_ratchet_private: params.our_private, + send_ratchet_public, + recv_ratchet_public: params.their_public, + send_count: 0, + recv_count: 0, + prev_recv_chain: 0, + last_ratchet_time: 0, + skipped_keys: HashMap::new(), + established: true, + } + } + + pub fn uninitialized() -> Self { + Self { + root_key: [0u8; 32], + send_chain_key: [0u8; 32], + recv_chain_key: [0u8; 32], + send_ratchet_private: [0u8; 32], + send_ratchet_public: [0u8; 32], + recv_ratchet_public: [0u8; 32], + send_count: 0, + recv_count: 0, + prev_recv_chain: 0, + last_ratchet_time: 0, + skipped_keys: HashMap::new(), + established: false, + } + } + + pub fn is_established(&self) -> bool { + self.established + } + + pub fn encrypt(&mut self, plaintext: &[u8]) -> Result<(MessageHeader, HeaplessVec), SessionError> { + if !self.established { + return Err(SessionError::NotEstablished); + } + + let mut message_key = [0u8; 32]; + Hkdf::derive(&self.send_chain_key, &self.send_count.to_le_bytes(), MESSAGE_KEY_INFO, &mut message_key); + + let mut new_chain_key = [0u8; 32]; + Hkdf::derive(&self.send_chain_key, b"chain-advance", CHAIN_KEY_INFO, &mut new_chain_key); + self.send_chain_key = new_chain_key; + + let header = MessageHeader { + dh_public: self.send_ratchet_public, + prev_chain_len: self.prev_recv_chain, + message_num: self.send_count, + }; + + let mut nonce = [0u8; 16]; + nonce[..8].copy_from_slice(&self.send_count.to_le_bytes()); + + let mut ciphertext = HeaplessVec::new(); + let _ = ciphertext.extend_from_slice(plaintext); + + let aes = Aes256::new(&message_key); + let mut keystream = [0u8; 16]; + let mut block_counter = 0u64; + + for chunk in ciphertext.chunks_mut(16) { + keystream = nonce; + keystream[8..].copy_from_slice(&block_counter.to_le_bytes()); + aes.encrypt_block(&mut keystream); + for (c, k) in chunk.iter_mut().zip(keystream.iter()) { + *c ^= k; + } + block_counter += 1; + } + + let tag = Self::compute_tag(&message_key, &header.encode(), &ciphertext); + let _ = ciphertext.extend_from_slice(&tag); + + self.send_count += 1; + + if self.should_ratchet() { + self.advance_send_ratchet(); + } + + Ok((header, ciphertext)) + } + + pub fn decrypt(&mut self, header: &MessageHeader, ciphertext: &[u8]) -> Result, SessionError> { + if !self.established { + return Err(SessionError::NotEstablished); + } + + if ciphertext.len() < 16 { + return Err(SessionError::InvalidFormat); + } + + if header.dh_public != self.recv_ratchet_public { + + self.skip_message_keys(header.prev_chain_len)?; + + self.advance_recv_ratchet(&header.dh_public)?; + } + + let message_key = self.get_message_key(header)?; + + let tag_start = ciphertext.len() - 16; + let received_tag = &ciphertext[tag_start..]; + let ct_without_tag = &ciphertext[..tag_start]; + + let expected_tag = Self::compute_tag(&message_key, &header.encode(), ct_without_tag); + if !constant_time_eq(received_tag, &expected_tag) { + return Err(SessionError::DecryptionFailed); + } + + let mut plaintext = HeaplessVec::new(); + let _ = plaintext.extend_from_slice(ct_without_tag); + + let mut nonce = [0u8; 16]; + nonce[..8].copy_from_slice(&header.message_num.to_le_bytes()); + + let aes = Aes256::new(&message_key); + let mut keystream = [0u8; 16]; + let mut block_counter = 0u64; + + for chunk in plaintext.chunks_mut(16) { + keystream = nonce; + keystream[8..].copy_from_slice(&block_counter.to_le_bytes()); + aes.encrypt_block(&mut keystream); + for (c, k) in chunk.iter_mut().zip(keystream.iter()) { + *c ^= k; + } + block_counter += 1; + } + + Ok(plaintext) + } + + pub fn derive_session_hint(&self, epoch: u64) -> [u8; 4] { + let mut input = [0u8; 40]; + input[..32].copy_from_slice(&self.root_key); + input[32..].copy_from_slice(&epoch.to_le_bytes()); + + let mut hint = [0u8; 4]; + Hkdf::derive(&input, b"hint", SESSION_HINT_INFO, &mut hint); + hint + } + + fn should_ratchet(&self) -> bool { + self.send_count >= MAX_MESSAGES_BEFORE_RATCHET + + } + + fn advance_send_ratchet(&mut self) { + + let mut new_private = [0u8; 32]; + fill_random(&mut new_private); + + new_private[0] &= 248; + new_private[31] &= 127; + new_private[31] |= 64; + + let new_public = x25519_base(&new_private); + + let dh_output = x25519(&new_private, &self.recv_ratchet_public); + + let mut kdf_input = [0u8; 64]; + kdf_input[..32].copy_from_slice(&self.root_key); + kdf_input[32..].copy_from_slice(&dh_output); + + Hkdf::derive(&kdf_input, b"root", ROOT_KEY_INFO, &mut self.root_key); + Hkdf::derive(&kdf_input, b"chain", CHAIN_KEY_INFO, &mut self.send_chain_key); + + self.send_ratchet_private = new_private; + self.send_ratchet_public = new_public; + self.prev_recv_chain = self.recv_count; + self.send_count = 0; + } + + fn advance_recv_ratchet(&mut self, their_new_public: &[u8; 32]) -> Result<(), SessionError> { + + let dh_output = x25519(&self.send_ratchet_private, their_new_public); + + let mut kdf_input = [0u8; 64]; + kdf_input[..32].copy_from_slice(&self.root_key); + kdf_input[32..].copy_from_slice(&dh_output); + + Hkdf::derive(&kdf_input, b"root", ROOT_KEY_INFO, &mut self.root_key); + Hkdf::derive(&kdf_input, b"chain", CHAIN_KEY_INFO, &mut self.recv_chain_key); + + self.recv_ratchet_public = *their_new_public; + self.recv_count = 0; + + Ok(()) + } + + fn skip_message_keys(&mut self, until: u64) -> Result<(), SessionError> { + let to_skip = until.saturating_sub(self.recv_count); + if to_skip as usize > MAX_SKIPPED_KEYS { + return Err(SessionError::TooManySkipped); + } + + while self.recv_count < until { + let mut message_key = [0u8; 32]; + Hkdf::derive(&self.recv_chain_key, &self.recv_count.to_le_bytes(), MESSAGE_KEY_INFO, &mut message_key); + + let mut key_prefix = [0u8; 8]; + key_prefix.copy_from_slice(&self.recv_ratchet_public[..8]); + + let _ = self.skipped_keys.insert((key_prefix, self.recv_count), message_key); + + let mut new_chain = [0u8; 32]; + Hkdf::derive(&self.recv_chain_key, b"chain-advance", CHAIN_KEY_INFO, &mut new_chain); + self.recv_chain_key = new_chain; + self.recv_count += 1; + } + + Ok(()) + } + + fn get_message_key(&mut self, header: &MessageHeader) -> Result<[u8; 32], SessionError> { + + let mut key_prefix = [0u8; 8]; + key_prefix.copy_from_slice(&header.dh_public[..8]); + + if let Some(key) = self.skipped_keys.remove(&(key_prefix, header.message_num)) { + return Ok(key); + } + + if header.message_num > self.recv_count { + self.skip_message_keys(header.message_num)?; + } + + let mut message_key = [0u8; 32]; + Hkdf::derive(&self.recv_chain_key, &header.message_num.to_le_bytes(), MESSAGE_KEY_INFO, &mut message_key); + + let mut new_chain = [0u8; 32]; + Hkdf::derive(&self.recv_chain_key, b"chain-advance", CHAIN_KEY_INFO, &mut new_chain); + self.recv_chain_key = new_chain; + self.recv_count = header.message_num + 1; + + Ok(message_key) + } + + fn compute_tag(key: &[u8; 32], header: &[u8], ciphertext: &[u8]) -> [u8; 16] { + use crate::crypto::sha256::Sha256; + + let mut inner = [0x36u8; 64]; + let mut outer = [0x5cu8; 64]; + + for (i, k) in key.iter().enumerate() { + inner[i] ^= k; + outer[i] ^= k; + } + + let mut hasher_data = HeaplessVec::::new(); + let _ = hasher_data.extend_from_slice(&inner); + let _ = hasher_data.extend_from_slice(header); + let _ = hasher_data.extend_from_slice(ciphertext); + let inner_hash = Sha256::hash(&hasher_data); + + let mut outer_data = [0u8; 96]; + outer_data[..64].copy_from_slice(&outer); + outer_data[64..].copy_from_slice(&inner_hash); + let full_tag = Sha256::hash(&outer_data); + + let mut tag = [0u8; 16]; + tag.copy_from_slice(&full_tag[..16]); + tag + } +} + +pub struct SessionManager { + + sessions: HashMap<[u8; 8], Session>, +} + +impl SessionManager { + pub fn new() -> Self { + Self { + sessions: HashMap::new(), + } + } + + pub fn get_session(&mut self, peer_public: &[u8; 32]) -> Option<&mut Session> { + let mut key = [0u8; 8]; + key.copy_from_slice(&peer_public[..8]); + self.sessions.get_mut(&key) + } + + pub fn create_session(&mut self, params: SessionParams) { + let mut key = [0u8; 8]; + key.copy_from_slice(¶ms.their_public[..8]); + + let session = Session::new(params); + self.sessions.insert(key, session); + } + + pub fn remove_session(&mut self, peer_public: &[u8; 32]) { + let mut key = [0u8; 8]; + key.copy_from_slice(&peer_public[..8]); + self.sessions.remove(&key); + } +} + +impl Default for SessionManager { + fn default() -> Self { + Self::new() + } +} + +const SESSION_SERIALIZED_SIZE: usize = 225; + +impl Session { + + pub fn serialize(&self) -> [u8; SESSION_SERIALIZED_SIZE] { + let mut buf = [0u8; SESSION_SERIALIZED_SIZE]; + let mut pos = 0; + + buf[pos..pos + 32].copy_from_slice(&self.root_key); + pos += 32; + buf[pos..pos + 32].copy_from_slice(&self.send_chain_key); + pos += 32; + buf[pos..pos + 32].copy_from_slice(&self.recv_chain_key); + pos += 32; + buf[pos..pos + 32].copy_from_slice(&self.send_ratchet_private); + pos += 32; + buf[pos..pos + 32].copy_from_slice(&self.send_ratchet_public); + pos += 32; + buf[pos..pos + 32].copy_from_slice(&self.recv_ratchet_public); + pos += 32; + buf[pos..pos + 8].copy_from_slice(&self.send_count.to_le_bytes()); + pos += 8; + buf[pos..pos + 8].copy_from_slice(&self.recv_count.to_le_bytes()); + pos += 8; + buf[pos..pos + 8].copy_from_slice(&self.prev_recv_chain.to_le_bytes()); + pos += 8; + buf[pos..pos + 8].copy_from_slice(&self.last_ratchet_time.to_le_bytes()); + pos += 8; + buf[pos] = if self.established { 1 } else { 0 }; + + buf + } + + pub fn deserialize(data: &[u8]) -> Option { + if data.len() < SESSION_SERIALIZED_SIZE { + return None; + } + + let mut pos = 0; + + let mut root_key = [0u8; 32]; + root_key.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + let mut send_chain_key = [0u8; 32]; + send_chain_key.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + let mut recv_chain_key = [0u8; 32]; + recv_chain_key.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + let mut send_ratchet_private = [0u8; 32]; + send_ratchet_private.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + let mut send_ratchet_public = [0u8; 32]; + send_ratchet_public.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + let mut recv_ratchet_public = [0u8; 32]; + recv_ratchet_public.copy_from_slice(&data[pos..pos + 32]); + pos += 32; + + let mut u64_bytes = [0u8; 8]; + + u64_bytes.copy_from_slice(&data[pos..pos + 8]); + let send_count = u64::from_le_bytes(u64_bytes); + pos += 8; + + u64_bytes.copy_from_slice(&data[pos..pos + 8]); + let recv_count = u64::from_le_bytes(u64_bytes); + pos += 8; + + u64_bytes.copy_from_slice(&data[pos..pos + 8]); + let prev_recv_chain = u64::from_le_bytes(u64_bytes); + pos += 8; + + u64_bytes.copy_from_slice(&data[pos..pos + 8]); + let last_ratchet_time = u64::from_le_bytes(u64_bytes); + pos += 8; + + let established = data[pos] != 0; + + Some(Self { + root_key, + send_chain_key, + recv_chain_key, + send_ratchet_private, + send_ratchet_public, + recv_ratchet_public, + send_count, + recv_count, + prev_recv_chain, + last_ratchet_time, + skipped_keys: HashMap::new(), + established, + }) + } +} + +const NVS_SESSION_NAMESPACE: &[u8] = b"sessions\0"; + +const MAX_PERSISTED_SESSIONS: usize = 32; + +impl SessionManager { + + #[cfg(target_arch = "xtensa")] + pub fn save_to_nvs(&self) -> Result<(), SessionError> { + use esp_idf_sys::*; + + unsafe { + + let mut handle: nvs_handle_t = 0; + let namespace = core::ffi::CStr::from_ptr(NVS_SESSION_NAMESPACE.as_ptr() as *const core::ffi::c_char); + + let mut err = nvs_open( + namespace.as_ptr(), + nvs_open_mode_t_NVS_READWRITE, + &mut handle, + ); + + if err != ESP_OK { + nvs_flash_init(); + err = nvs_open( + namespace.as_ptr(), + nvs_open_mode_t_NVS_READWRITE, + &mut handle, + ); + if err != ESP_OK { + return Err(SessionError::KeyDerivationFailed); + } + } + + let count_key = core::ffi::CStr::from_ptr(b"sess_count\0".as_ptr() as *const core::ffi::c_char); + nvs_set_u32(handle, count_key.as_ptr(), self.sessions.len() as u32); + + let mut idx = 0u32; + for (key, session) in self.sessions.iter() { + + let mut key_name = [0u8; 16]; + let prefix = b"sess_"; + key_name[..5].copy_from_slice(prefix); + + if idx < 10 { + key_name[5] = b'0' + idx as u8; + key_name[6] = 0; + } else { + key_name[5] = b'0' + (idx / 10) as u8; + key_name[6] = b'0' + (idx % 10) as u8; + key_name[7] = 0; + } + let key_cstr = core::ffi::CStr::from_ptr(key_name.as_ptr() as *const core::ffi::c_char); + + let mut blob = [0u8; 8 + SESSION_SERIALIZED_SIZE]; + blob[..8].copy_from_slice(key); + blob[8..].copy_from_slice(&session.serialize()); + + nvs_set_blob( + handle, + key_cstr.as_ptr(), + blob.as_ptr() as *const _, + blob.len(), + ); + + idx += 1; + } + + nvs_commit(handle); + nvs_close(handle); + + ::log::info!("Saved {} sessions to NVS", self.sessions.len()); + } + + Ok(()) + } + + #[cfg(target_arch = "xtensa")] + pub fn load_from_nvs(&mut self) -> Result { + use esp_idf_sys::*; + + unsafe { + + let mut handle: nvs_handle_t = 0; + let namespace = core::ffi::CStr::from_ptr(NVS_SESSION_NAMESPACE.as_ptr() as *const core::ffi::c_char); + + let err = nvs_open( + namespace.as_ptr(), + nvs_open_mode_t_NVS_READONLY, + &mut handle, + ); + + if err != ESP_OK { + return Ok(0); + } + + let count_key = core::ffi::CStr::from_ptr(b"sess_count\0".as_ptr() as *const core::ffi::c_char); + let mut count: u32 = 0; + if nvs_get_u32(handle, count_key.as_ptr(), &mut count) != ESP_OK { + nvs_close(handle); + return Ok(0); + } + + let count = core::cmp::min(count as usize, MAX_PERSISTED_SESSIONS); + + let mut loaded = 0; + for idx in 0..count { + + let mut key_name = [0u8; 16]; + let prefix = b"sess_"; + key_name[..5].copy_from_slice(prefix); + if idx < 10 { + key_name[5] = b'0' + idx as u8; + key_name[6] = 0; + } else { + key_name[5] = b'0' + (idx / 10) as u8; + key_name[6] = b'0' + (idx % 10) as u8; + key_name[7] = 0; + } + let key_cstr = core::ffi::CStr::from_ptr(key_name.as_ptr() as *const core::ffi::c_char); + + let mut blob = [0u8; 8 + SESSION_SERIALIZED_SIZE]; + let mut blob_len = blob.len(); + + if nvs_get_blob( + handle, + key_cstr.as_ptr(), + blob.as_mut_ptr() as *mut _, + &mut blob_len, + ) == ESP_OK && blob_len == blob.len() + { + + let mut peer_key = [0u8; 8]; + peer_key.copy_from_slice(&blob[..8]); + + if let Some(session) = Session::deserialize(&blob[8..]) { + let _ = self.sessions.insert(peer_key, session); + loaded += 1; + } + } + } + + nvs_close(handle); + ::log::info!("Loaded {} sessions from NVS", loaded); + Ok(loaded) + } + } + + #[cfg(not(target_arch = "xtensa"))] + pub fn save_to_nvs(&self) -> Result<(), SessionError> { + Ok(()) + } + + #[cfg(not(target_arch = "xtensa"))] + pub fn load_from_nvs(&mut self) -> Result { + Ok(0) + } + + pub fn session_count(&self) -> usize { + self.sessions.len() + } +} diff --git a/src/sx1262.rs b/src/sx1262.rs new file mode 100644 index 0000000..497a046 --- /dev/null +++ b/src/sx1262.rs @@ -0,0 +1,610 @@ +use embedded_hal::spi::SpiDevice; +use embedded_hal::digital::{InputPin, OutputPin}; +use esp_idf_hal::delay::FreeRtos; + +#[allow(dead_code)] +mod opcode { + + pub const SET_SLEEP: u8 = 0x84; + pub const SET_STANDBY: u8 = 0x80; + pub const SET_FS: u8 = 0xC1; + pub const SET_TX: u8 = 0x83; + pub const SET_RX: u8 = 0x82; + pub const STOP_TIMER_ON_PREAMBLE: u8 = 0x9F; + pub const SET_RX_DUTY_CYCLE: u8 = 0x94; + pub const SET_CAD: u8 = 0xC5; + pub const SET_TX_CONTINUOUS_WAVE: u8 = 0xD1; + pub const SET_TX_INFINITE_PREAMBLE: u8 = 0xD2; + pub const SET_REGULATOR_MODE: u8 = 0x96; + pub const CALIBRATE: u8 = 0x89; + pub const CALIBRATE_IMAGE: u8 = 0x98; + pub const SET_PA_CONFIG: u8 = 0x95; + pub const SET_RX_TX_FALLBACK_MODE: u8 = 0x93; + + pub const WRITE_REGISTER: u8 = 0x0D; + pub const READ_REGISTER: u8 = 0x1D; + pub const WRITE_BUFFER: u8 = 0x0E; + pub const READ_BUFFER: u8 = 0x1E; + + pub const SET_DIO_IRQ_PARAMS: u8 = 0x08; + pub const GET_IRQ_STATUS: u8 = 0x12; + pub const CLEAR_IRQ_STATUS: u8 = 0x02; + pub const SET_DIO2_AS_RF_SWITCH_CTRL: u8 = 0x9D; + pub const SET_DIO3_AS_TCXO_CTRL: u8 = 0x97; + + pub const SET_RF_FREQUENCY: u8 = 0x86; + pub const SET_PACKET_TYPE: u8 = 0x8A; + pub const GET_PACKET_TYPE: u8 = 0x11; + pub const SET_TX_PARAMS: u8 = 0x8E; + pub const SET_MODULATION_PARAMS: u8 = 0x8B; + pub const SET_PACKET_PARAMS: u8 = 0x8C; + pub const SET_CAD_PARAMS: u8 = 0x88; + pub const SET_BUFFER_BASE_ADDRESS: u8 = 0x8F; + pub const SET_LORA_SYMB_NUM_TIMEOUT: u8 = 0xA0; + + pub const GET_STATUS: u8 = 0xC0; + pub const GET_RX_BUFFER_STATUS: u8 = 0x13; + pub const GET_PACKET_STATUS: u8 = 0x14; + pub const GET_RSSI_INST: u8 = 0x15; + pub const GET_STATS: u8 = 0x10; + pub const RESET_STATS: u8 = 0x00; + pub const GET_DEVICE_ERRORS: u8 = 0x17; + pub const CLEAR_DEVICE_ERRORS: u8 = 0x07; +} + +#[allow(dead_code)] +mod register { + pub const WHITENING_INITIAL_MSB: u16 = 0x06B8; + pub const WHITENING_INITIAL_LSB: u16 = 0x06B9; + pub const CRC_INITIAL_MSB: u16 = 0x06BC; + pub const CRC_INITIAL_LSB: u16 = 0x06BD; + pub const CRC_POLYNOMIAL_MSB: u16 = 0x06BE; + pub const CRC_POLYNOMIAL_LSB: u16 = 0x06BF; + pub const SYNC_WORD_0: u16 = 0x06C0; + pub const SYNC_WORD_1: u16 = 0x06C1; + pub const NODE_ADDRESS: u16 = 0x06CD; + pub const BROADCAST_ADDRESS: u16 = 0x06CE; + pub const LORA_SYNC_WORD_MSB: u16 = 0x0740; + pub const LORA_SYNC_WORD_LSB: u16 = 0x0741; + pub const RANDOM_NUMBER_0: u16 = 0x0819; + pub const RANDOM_NUMBER_1: u16 = 0x081A; + pub const RANDOM_NUMBER_2: u16 = 0x081B; + pub const RANDOM_NUMBER_3: u16 = 0x081C; + pub const RX_GAIN: u16 = 0x08AC; + pub const OCP_CONFIGURATION: u16 = 0x08E7; + pub const XTA_TRIM: u16 = 0x0911; + pub const XTB_TRIM: u16 = 0x0912; +} + +#[allow(dead_code)] +pub mod irq { + pub const TX_DONE: u16 = 1 << 0; + pub const RX_DONE: u16 = 1 << 1; + pub const PREAMBLE_DETECTED: u16 = 1 << 2; + pub const SYNC_WORD_VALID: u16 = 1 << 3; + pub const HEADER_VALID: u16 = 1 << 4; + pub const HEADER_ERR: u16 = 1 << 5; + pub const CRC_ERR: u16 = 1 << 6; + pub const CAD_DONE: u16 = 1 << 7; + pub const CAD_DETECTED: u16 = 1 << 8; + pub const TIMEOUT: u16 = 1 << 9; + pub const ALL: u16 = 0x03FF; +} + +#[derive(Debug, Clone)] +pub struct RadioConfig { + + pub frequency: u32, + + pub spreading_factor: u8, + + pub bandwidth: u8, + + pub coding_rate: u8, + + pub tx_power: i8, + + pub sync_word: u8, + + pub preamble_length: u16, + + pub crc_enabled: bool, + + pub implicit_header: bool, + + pub ldro: bool, +} + +impl Default for RadioConfig { + fn default() -> Self { + Self { + frequency: 868_100_000, + spreading_factor: 9, + bandwidth: 0, + coding_rate: 1, + tx_power: 14, + sync_word: 0x12, + preamble_length: 8, + crc_enabled: true, + implicit_header: false, + ldro: false, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum RadioState { + Sleep, + Standby, + Tx, + Rx, + Cad, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum RadioError { + + Spi, + + BusyTimeout, + + InvalidConfig, + + TxTimeout, + + RxTimeout, + + CrcError, + + BufferOverflow, +} + +pub struct Sx1262 { + spi: SPI, + nss: NSS, + reset: RESET, + busy: BUSY, + dio1: DIO1, + + pub config: RadioConfig, + state: RadioState, +} + +impl Sx1262 +where + SPI: SpiDevice, + NSS: OutputPin, + RESET: OutputPin, + BUSY: InputPin, + DIO1: InputPin, +{ + + pub fn new(spi: SPI, nss: NSS, reset: RESET, busy: BUSY, dio1: DIO1) -> Self { + Self { + spi, + nss, + reset, + busy, + dio1, + config: RadioConfig::default(), + state: RadioState::Sleep, + } + } + + pub fn init(&mut self) -> Result<(), RadioError> { + + self.reset()?; + + self.wait_busy_extended()?; + + self.write_command(&[ + opcode::SET_DIO3_AS_TCXO_CTRL, + 0x02, + 0x00, + 0x01, + 0x40, + ])?; + + self.delay_ms(10); + self.wait_busy_extended()?; + + self.write_command(&[opcode::SET_STANDBY, 0x01])?; + self.state = RadioState::Standby; + self.wait_busy()?; + + self.write_command(&[opcode::SET_REGULATOR_MODE, 0x01])?; + self.wait_busy()?; + + self.write_command(&[opcode::CALIBRATE, 0x7F])?; + self.wait_busy_extended()?; + + self.write_command(&[ + opcode::CALIBRATE_IMAGE, + 0xE1, + 0xE9, + ])?; + self.wait_busy()?; + + self.write_command(&[opcode::SET_DIO2_AS_RF_SWITCH_CTRL, 0x01])?; + + self.write_command(&[opcode::SET_PACKET_TYPE, 0x01])?; + + self.configure(&self.config.clone())?; + + Ok(()) + } + + pub fn reset(&mut self) -> Result<(), RadioError> { + + let _ = self.reset.set_low(); + + FreeRtos::delay_ms(1); + + let _ = self.reset.set_high(); + + FreeRtos::delay_ms(10); + Ok(()) + } + + fn wait_busy(&mut self) -> Result<(), RadioError> { + + for _ in 0..100 { + + match self.busy.is_high() { + Ok(false) => return Ok(()), + Ok(true) => {}, + Err(_) => {}, + } + + FreeRtos::delay_ms(1); + } + Err(RadioError::BusyTimeout) + } + + fn wait_busy_extended(&mut self) -> Result<(), RadioError> { + + for _ in 0..500 { + match self.busy.is_high() { + Ok(false) => return Ok(()), + Ok(true) => {}, + Err(_) => {}, + } + + FreeRtos::delay_ms(1); + } + Err(RadioError::BusyTimeout) + } + + fn delay_ms(&self, ms: u32) { + FreeRtos::delay_ms(ms); + } + + fn write_command(&mut self, data: &[u8]) -> Result<(), RadioError> { + self.wait_busy()?; + let _ = self.nss.set_low(); + let result = self.spi.write(data); + let _ = self.nss.set_high(); + result.map_err(|_| RadioError::Spi) + } + + fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), RadioError> { + self.wait_busy()?; + let _ = self.nss.set_low(); + let result = self.spi.transfer(rx, tx); + let _ = self.nss.set_high(); + result.map_err(|_| RadioError::Spi) + } + + pub fn set_standby(&mut self) -> Result<(), RadioError> { + self.write_command(&[opcode::SET_STANDBY, 0x01])?; + self.state = RadioState::Standby; + Ok(()) + } + + pub fn configure(&mut self, config: &RadioConfig) -> Result<(), RadioError> { + + if config.spreading_factor < 7 || config.spreading_factor > 12 { + return Err(RadioError::InvalidConfig); + } + if config.bandwidth > 2 { + return Err(RadioError::InvalidConfig); + } + + let freq_reg = ((config.frequency as u64 * (1 << 25)) / 32_000_000) as u32; + self.write_command(&[ + opcode::SET_RF_FREQUENCY, + (freq_reg >> 24) as u8, + (freq_reg >> 16) as u8, + (freq_reg >> 8) as u8, + freq_reg as u8, + ])?; + + self.write_command(&[ + opcode::SET_PA_CONFIG, + 0x04, + 0x07, + 0x00, + 0x01, + ])?; + + let power = config.tx_power.max(-9).min(22) as u8; + self.write_command(&[ + opcode::SET_TX_PARAMS, + power.wrapping_add(9), + 0x04, + ])?; + + let bw_hz: u32 = match config.bandwidth { + 0 => 125_000, + 1 => 250_000, + 2 => 500_000, + _ => 125_000, + }; + let symbol_time_us = ((1u32 << config.spreading_factor) * 1_000_000) / bw_hz; + let ldro_required = symbol_time_us > 16380; + let ldro = if config.ldro || ldro_required { 0x01 } else { 0x00 }; + self.write_command(&[ + opcode::SET_MODULATION_PARAMS, + config.spreading_factor, + config.bandwidth, + config.coding_rate, + ldro, + ])?; + + let header_type = if config.implicit_header { 0x01 } else { 0x00 }; + let crc_type = if config.crc_enabled { 0x01 } else { 0x00 }; + self.write_command(&[ + opcode::SET_PACKET_PARAMS, + (config.preamble_length >> 8) as u8, + config.preamble_length as u8, + header_type, + 255, + crc_type, + 0x00, + ])?; + + let sync_msb = (config.sync_word >> 4) | 0x40; + let sync_lsb = (config.sync_word << 4) | 0x04; + self.write_register(register::LORA_SYNC_WORD_MSB, sync_msb)?; + self.write_register(register::LORA_SYNC_WORD_LSB, sync_lsb)?; + + self.write_command(&[opcode::SET_BUFFER_BASE_ADDRESS, 0x00, 0x00])?; + + self.write_command(&[ + opcode::SET_DIO_IRQ_PARAMS, + (irq::ALL >> 8) as u8, + irq::ALL as u8, + (irq::ALL >> 8) as u8, + irq::ALL as u8, + 0x00, 0x00, + 0x00, 0x00, + ])?; + + self.config = config.clone(); + Ok(()) + } + + fn write_register(&mut self, addr: u16, value: u8) -> Result<(), RadioError> { + self.write_command(&[ + opcode::WRITE_REGISTER, + (addr >> 8) as u8, + addr as u8, + value, + ]) + } + + pub fn transmit(&mut self, data: &[u8]) -> Result<(), RadioError> { + if data.len() > 255 { + return Err(RadioError::BufferOverflow); + } + + self.set_standby()?; + + let mut cmd = heapless::Vec::::new(); + let _ = cmd.push(opcode::WRITE_BUFFER); + let _ = cmd.push(0x00); + for &b in data { + let _ = cmd.push(b); + } + self.write_command(&cmd)?; + + let header_type = if self.config.implicit_header { 0x01 } else { 0x00 }; + let crc_type = if self.config.crc_enabled { 0x01 } else { 0x00 }; + self.write_command(&[ + opcode::SET_PACKET_PARAMS, + (self.config.preamble_length >> 8) as u8, + self.config.preamble_length as u8, + header_type, + data.len() as u8, + crc_type, + 0x00, + ])?; + + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + + self.write_command(&[opcode::SET_TX, 0x00, 0x00, 0x00])?; + self.state = RadioState::Tx; + + self.wait_tx_done()?; + + self.state = RadioState::Standby; + Ok(()) + } + + fn wait_tx_done(&mut self) -> Result<(), RadioError> { + + for _ in 0..10000 { + + if self.dio1.is_high().unwrap_or(false) { + let irq = self.get_irq_status()?; + if irq & irq::TX_DONE != 0 { + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + return Ok(()); + } + } + + FreeRtos::delay_ms(1); + } + self.set_standby()?; + Err(RadioError::TxTimeout) + } + + pub fn get_irq_status(&mut self) -> Result { + let mut rx = [0u8; 4]; + self.transfer(&[opcode::GET_IRQ_STATUS, 0, 0, 0], &mut rx)?; + Ok(((rx[2] as u16) << 8) | (rx[3] as u16)) + } + + pub fn clear_irq(&mut self, flags: u16) -> Result<(), RadioError> { + self.write_command(&[ + opcode::CLEAR_IRQ_STATUS, + (flags >> 8) as u8, + flags as u8, + ]) + } + + pub fn start_rx(&mut self, timeout_ms: u32) -> Result<(), RadioError> { + self.set_standby()?; + + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + + let timeout_ticks = if timeout_ms == 0 { + 0xFFFFFF + } else { + ((timeout_ms as u64 * 64) / 1000).min(0xFFFFFF) as u32 + }; + + self.write_command(&[ + opcode::SET_RX, + (timeout_ticks >> 16) as u8, + (timeout_ticks >> 8) as u8, + timeout_ticks as u8, + ])?; + + self.state = RadioState::Rx; + Ok(()) + } + + pub fn check_rx(&mut self) -> Result, i16, i8)>, RadioError> { + + if !self.dio1.is_high().unwrap_or(false) { + return Ok(None); + } + + let irq = self.get_irq_status()?; + + if irq & irq::CRC_ERR != 0 { + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + return Err(RadioError::CrcError); + } + + if irq & irq::TIMEOUT != 0 { + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + return Err(RadioError::RxTimeout); + } + + if irq & irq::RX_DONE != 0 { + + let mut buf_status = [0u8; 4]; + self.transfer(&[opcode::GET_RX_BUFFER_STATUS, 0, 0, 0], &mut buf_status)?; + let payload_len = buf_status[2]; + let start_offset = buf_status[3]; + + let mut pkt_status = [0u8; 5]; + self.transfer(&[opcode::GET_PACKET_STATUS, 0, 0, 0, 0], &mut pkt_status)?; + let rssi = -(pkt_status[2] as i16 / 2); + let snr = pkt_status[3] as i8 / 4; + + let mut data = heapless::Vec::::new(); + let mut read_cmd = [0u8; 258]; + read_cmd[0] = opcode::READ_BUFFER; + read_cmd[1] = start_offset; + read_cmd[2] = 0; + + let len = payload_len as usize + 3; + let mut rx_buf = [0u8; 258]; + self.transfer(&read_cmd[..len], &mut rx_buf[..len])?; + + for i in 0..payload_len as usize { + let _ = data.push(rx_buf[3 + i]); + } + + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + + return Ok(Some((data, rssi, snr))); + } + + Ok(None) + } + + pub fn cad(&mut self) -> Result { + self.set_standby()?; + + self.write_command(&[ + opcode::SET_CAD_PARAMS, + 0x04, + 24, + 10, + 0x00, + 0x00, 0x00, 0x00, + ])?; + + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + + self.write_command(&[opcode::SET_CAD])?; + self.state = RadioState::Cad; + + for _ in 0..1000 { + if self.dio1.is_high().unwrap_or(false) { + let irq = self.get_irq_status()?; + if irq & irq::CAD_DONE != 0 { + let detected = irq & irq::CAD_DETECTED != 0; + self.write_command(&[opcode::CLEAR_IRQ_STATUS, 0x03, 0xFF])?; + self.state = RadioState::Standby; + return Ok(detected); + } + } + + FreeRtos::delay_ms(1); + } + self.set_standby()?; + Err(RadioError::BusyTimeout) + } + + pub fn state(&self) -> RadioState { + self.state + } + + pub fn get_rssi(&mut self) -> Result { + let mut rx = [0u8; 3]; + self.transfer(&[opcode::GET_RSSI_INST, 0, 0], &mut rx)?; + Ok(-(rx[2] as i16 / 2)) + } + + pub fn random(&mut self) -> Result { + + self.start_rx(0)?; + + FreeRtos::delay_ms(10); + self.set_standby()?; + + let mut random = 0u32; + let mut rx = [0u8; 3]; + + for (i, addr) in [ + register::RANDOM_NUMBER_0, + register::RANDOM_NUMBER_1, + register::RANDOM_NUMBER_2, + register::RANDOM_NUMBER_3, + ].iter().enumerate() { + self.transfer(&[ + opcode::READ_REGISTER, + (*addr >> 8) as u8, + *addr as u8, + ], &mut rx)?; + random |= (rx[2] as u32) << (i * 8); + } + + Ok(random) + } +} diff --git a/src/transport.rs b/src/transport.rs new file mode 100644 index 0000000..5a341b5 --- /dev/null +++ b/src/transport.rs @@ -0,0 +1,450 @@ +use heapless::Vec as HeaplessVec; + +pub const MAX_PACKET_SIZE: usize = 237; + +pub const NODE_HINT_SIZE: usize = 2; + +pub const SESSION_HINT_SIZE: usize = 4; + +pub const AUTH_TAG_SIZE: usize = 16; + +pub const FLAGS_SIZE: usize = 1; + +pub const DATA_OVERHEAD: usize = FLAGS_SIZE + NODE_HINT_SIZE + SESSION_HINT_SIZE + AUTH_TAG_SIZE; + +pub const DATA_MAX_PAYLOAD: usize = MAX_PACKET_SIZE - DATA_OVERHEAD; + +pub const PADDED_MESSAGE_SIZE: usize = 200; + +#[derive(Debug, Clone)] +pub struct UniversalAddress { + + pub did: HeaplessVec, + + pub public_key: [u8; 32], + + pub meshcore_addr: u16, + + pub meshtastic_id: u32, + + pub reticulum_hash: [u8; 16], +} + +pub struct AddressTranslator; + +impl AddressTranslator { + + pub fn from_public_key(public_key: &[u8; 32]) -> UniversalAddress { + use crate::crypto::sha256::Sha256; + + let pubkey_hash = Sha256::hash(public_key); + + let meshcore_addr = ((pubkey_hash[0] as u16) << 8) | (pubkey_hash[1] as u16); + + let meshtastic_id = ((pubkey_hash[0] as u32) << 24) + | ((pubkey_hash[1] as u32) << 16) + | ((pubkey_hash[2] as u32) << 8) + | (pubkey_hash[3] as u32); + + let app_hash = Sha256::hash(b"yours.messaging"); + let mut combined = [0u8; 64]; + combined[..32].copy_from_slice(&app_hash); + combined[32..].copy_from_slice(public_key); + let reticulum_full = Sha256::hash(&combined); + let mut reticulum_hash = [0u8; 16]; + reticulum_hash.copy_from_slice(&reticulum_full[..16]); + + let mut did = HeaplessVec::new(); + let _ = did.extend_from_slice(b"did:offgrid:z"); + + UniversalAddress { + did, + public_key: *public_key, + meshcore_addr, + meshtastic_id, + reticulum_hash, + } + } + + pub fn derive_meshcore_address(public_key: &[u8; 32]) -> u16 { + use crate::crypto::sha256::Sha256; + let hash = Sha256::hash(public_key); + ((hash[0] as u16) << 8) | (hash[1] as u16) + } + + pub fn derive_meshtastic_id(public_key: &[u8; 32]) -> u32 { + use crate::crypto::sha256::Sha256; + let hash = Sha256::hash(public_key); + ((hash[0] as u32) << 24) + | ((hash[1] as u32) << 16) + | ((hash[2] as u32) << 8) + | (hash[3] as u32) + } + + pub fn derive_reticulum_hash(public_key: &[u8; 32], app_name: &[u8]) -> [u8; 16] { + use crate::crypto::sha256::Sha256; + let app_hash = Sha256::hash(app_name); + let mut combined = [0u8; 64]; + combined[..32].copy_from_slice(&app_hash); + combined[32..].copy_from_slice(public_key); + let full_hash = Sha256::hash(&combined); + let mut result = [0u8; 16]; + result.copy_from_slice(&full_hash[..16]); + result + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[repr(u8)] +pub enum PacketType { + + Data = 0b00, + + Handshake = 0b01, + + Control = 0b10, + + Cover = 0b11, +} + +impl PacketType { + pub fn from_flags(flags: u8) -> Self { + match (flags >> 6) & 0b11 { + 0b00 => PacketType::Data, + 0b01 => PacketType::Handshake, + 0b10 => PacketType::Control, + 0b11 => PacketType::Cover, + _ => unreachable!(), + } + } +} + +#[derive(Debug, Clone)] +pub struct WirePacket { + + pub packet_type: PacketType, + + pub hop_count: u8, + + pub next_hop_hint: u16, + + pub session_hint: u32, + + pub payload: HeaplessVec, +} + +impl WirePacket { + + pub fn new_data(next_hop: u16, session: u32, payload: &[u8]) -> Option { + if payload.len() > DATA_MAX_PAYLOAD { + return None; + } + let mut p = HeaplessVec::new(); + p.extend_from_slice(payload).ok()?; + Some(Self { + packet_type: PacketType::Data, + hop_count: 0, + next_hop_hint: next_hop, + session_hint: session, + payload: p, + }) + } + + pub fn encode(&self) -> HeaplessVec { + let mut buf = HeaplessVec::new(); + + let flags = ((self.packet_type as u8) << 6) | (self.hop_count & 0x0F); + let _ = buf.push(flags); + + let _ = buf.push((self.next_hop_hint >> 8) as u8); + let _ = buf.push(self.next_hop_hint as u8); + + let _ = buf.push((self.session_hint >> 24) as u8); + let _ = buf.push((self.session_hint >> 16) as u8); + let _ = buf.push((self.session_hint >> 8) as u8); + let _ = buf.push(self.session_hint as u8); + + let _ = buf.extend_from_slice(&self.payload); + + buf + } + + pub fn decode(data: &[u8]) -> Option { + if data.len() < 7 { + return None; + } + + let flags = data[0]; + let packet_type = PacketType::from_flags(flags); + let hop_count = flags & 0x0F; + + let next_hop_hint = ((data[1] as u16) << 8) | (data[2] as u16); + let session_hint = ((data[3] as u32) << 24) + | ((data[4] as u32) << 16) + | ((data[5] as u32) << 8) + | (data[6] as u32); + + let mut payload = HeaplessVec::new(); + if data.len() > 7 { + payload.extend_from_slice(&data[7..]).ok()?; + } + + Some(Self { + packet_type, + hop_count, + next_hop_hint, + session_hint, + payload, + }) + } + + pub fn increment_hop(&mut self) -> bool { + if self.hop_count < 15 { + self.hop_count += 1; + true + } else { + false + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +pub enum MessagePriority { + + Low = 0, + + Normal = 1, + + High = 2, + + Critical = 3, +} + +#[derive(Debug, Clone)] +pub struct UniversalMessage { + + pub id: [u8; 8], + + pub recipient: UniversalAddress, + + pub payload: HeaplessVec, + + pub priority: MessagePriority, + + pub timestamp: u64, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ConnectionState { + + Disconnected, + + Connecting, + + Connected, + + Error, +} + +#[derive(Debug, Clone, Copy)] +pub struct SignalQuality { + + pub rssi: i16, + + pub snr: i8, + + pub quality: u8, +} + +impl SignalQuality { + pub fn new(rssi: i16, snr: i8) -> Self { + + let rssi_norm = ((rssi.max(-120).min(-50) + 120) as u16 * 100 / 70) as u8; + + let snr_norm = ((snr.max(-20).min(10) + 20) as u16 * 100 / 30) as u8; + + let quality = (rssi_norm + snr_norm) / 2; + + Self { rssi, snr, quality } + } +} + +#[derive(Debug, Clone)] +pub struct MeshDeviceInfo { + + pub name: HeaplessVec, + + pub firmware_version: HeaplessVec, + + pub hardware_model: HeaplessVec, + + pub protocols: ProtocolSupport, + + pub battery_level: u8, +} + +#[derive(Debug, Clone, Copy, Default)] +pub struct ProtocolSupport { + pub meshcore: bool, + pub meshtastic: bool, + pub reticulum: bool, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum TransportError { + + NotConnected, + + ConnectionFailed, + + SendFailed, + + ReceiveTimeout, + + InvalidMessage, + + BufferOverflow, + + DeviceBusy, + + ProtocolError, + + Unknown, +} + +pub trait UniversalMeshTransport { + + fn connect(&mut self) -> Result<(), TransportError>; + + fn disconnect(&mut self); + + fn send_message(&mut self, message: &UniversalMessage) -> Result<[u8; 8], TransportError>; + + fn poll_message(&mut self) -> Option; + + fn get_device_info(&self) -> Result; + + fn connection_state(&self) -> ConnectionState; + + fn signal_quality(&self) -> SignalQuality; + + fn discover_peers(&mut self, timeout_ms: u32) -> HeaplessVec; + + fn ping_peer(&mut self, address: &UniversalAddress) -> Result; +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum Protocol { + MeshCore, + Meshtastic, + Reticulum, +} + +impl Protocol { + + pub fn magic_bytes(&self) -> [u8; 2] { + match self { + Protocol::MeshCore => [0xAA, 0x55], + Protocol::Meshtastic => [0x94, 0xC3], + Protocol::Reticulum => [0xC0, 0x00], + } + } + + pub fn detect(data: &[u8]) -> Option { + if data.len() < 2 { + return None; + } + match (data[0], data[1]) { + (0xAA, 0x55) => Some(Protocol::MeshCore), + (0x94, 0xC3) => Some(Protocol::Meshtastic), + (0xC0, _) => Some(Protocol::Reticulum), + (b'A', b'T') => None, + _ => None, + } + } +} + +const MAX_KNOWN_ADDRESSES: usize = 64; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ProtocolAddress { + MeshCore(u16), + Meshtastic(u32), + Reticulum([u8; 16]), +} + +pub struct AddressLookupTable { + + meshcore_index: heapless::FnvIndexMap, + + meshtastic_index: heapless::FnvIndexMap, + + reticulum_index: heapless::FnvIndexMap<[u8; 8], [u8; 32], MAX_KNOWN_ADDRESSES>, +} + +impl AddressLookupTable { + pub fn new() -> Self { + Self { + meshcore_index: heapless::FnvIndexMap::new(), + meshtastic_index: heapless::FnvIndexMap::new(), + reticulum_index: heapless::FnvIndexMap::new(), + } + } + + pub fn register(&mut self, public_key: &[u8; 32]) { + let addr = AddressTranslator::from_public_key(public_key); + + let _ = self.meshcore_index.insert(addr.meshcore_addr, *public_key); + + let _ = self.meshtastic_index.insert(addr.meshtastic_id, *public_key); + + let mut ret_key = [0u8; 8]; + ret_key.copy_from_slice(&addr.reticulum_hash[..8]); + let _ = self.reticulum_index.insert(ret_key, *public_key); + } + + pub fn unregister(&mut self, public_key: &[u8; 32]) { + let addr = AddressTranslator::from_public_key(public_key); + self.meshcore_index.remove(&addr.meshcore_addr); + self.meshtastic_index.remove(&addr.meshtastic_id); + let mut ret_key = [0u8; 8]; + ret_key.copy_from_slice(&addr.reticulum_hash[..8]); + self.reticulum_index.remove(&ret_key); + } + + pub fn lookup_meshcore(&self, addr: u16) -> Option<&[u8; 32]> { + self.meshcore_index.get(&addr) + } + + pub fn lookup_meshtastic(&self, id: u32) -> Option<&[u8; 32]> { + self.meshtastic_index.get(&id) + } + + pub fn lookup_reticulum(&self, hash: &[u8; 16]) -> Option<&[u8; 32]> { + let mut key = [0u8; 8]; + key.copy_from_slice(&hash[..8]); + self.reticulum_index.get(&key) + } + + pub fn lookup(&self, addr: ProtocolAddress) -> Option<&[u8; 32]> { + match addr { + ProtocolAddress::MeshCore(a) => self.lookup_meshcore(a), + ProtocolAddress::Meshtastic(id) => self.lookup_meshtastic(id), + ProtocolAddress::Reticulum(hash) => self.lookup_reticulum(&hash), + } + } + + pub fn len(&self) -> usize { + self.meshcore_index.len() + } + + pub fn is_empty(&self) -> bool { + self.meshcore_index.is_empty() + } +} + +impl Default for AddressLookupTable { + fn default() -> Self { + Self::new() + } +} diff --git a/src/wifi.rs b/src/wifi.rs new file mode 100644 index 0000000..97f687b --- /dev/null +++ b/src/wifi.rs @@ -0,0 +1,964 @@ +use heapless::{String, Vec}; +use crate::crypto::chacha20::ChaCha20; +use crate::crypto::sha256::Sha256; +use core::net::{Ipv4Addr, SocketAddr, SocketAddrV4}; + +pub const DEFAULT_TCP_PORT: u16 = 4000; + +pub const MAX_TCP_CLIENTS: usize = 4; + +pub const TCP_RX_BUFFER_SIZE: usize = 512; + +pub const TCP_TX_BUFFER_SIZE: usize = 512; + +pub const WIFI_CONNECT_TIMEOUT_SEC: u32 = 30; + +pub const DEFAULT_AP_SSID: &str = "LunarCore"; + +pub const DEFAULT_AP_PASSWORD: &str = "lunarpunk"; + +pub const MAX_SSID_LEN: usize = 32; + +pub const MAX_PASSWORD_LEN: usize = 64; + +pub const AUTH_CHALLENGE_SIZE: usize = 32; + +pub const AUTH_RESPONSE_SIZE: usize = 32; + +pub const SESSION_KEY_SIZE: usize = 32; + +pub const SESSION_NONCE_SIZE: usize = 12; + +pub const AUTH_TIMEOUT_SEC: u32 = 10; + +pub const MAX_AUTH_FAILURES: u8 = 3; + +pub const AUTH_LOCKOUT_SEC: u32 = 300; + +pub const CONN_RATE_LIMIT: u8 = 10; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum WifiMode { + + Off, + + Ap, + + Sta, + + ApSta, +} + +impl Default for WifiMode { + fn default() -> Self { + WifiMode::Off + } +} + +#[derive(Debug, Clone)] +pub struct SecurityConfig { + + pub require_auth: bool, + + pub psk: [u8; 32], + + pub encrypt_traffic: bool, + + pub rate_limiting: bool, + + pub allow_external: bool, +} + +impl Drop for SecurityConfig { + fn drop(&mut self) { + + crate::crypto::secure_zero(&mut self.psk); + } +} + +impl Default for SecurityConfig { + fn default() -> Self { + Self { + require_auth: true, + psk: [0u8; 32], + encrypt_traffic: true, + rate_limiting: true, + allow_external: false, + } + } +} + +impl SecurityConfig { + + pub fn with_psk(psk: &[u8; 32]) -> Self { + Self { + require_auth: true, + psk: *psk, + encrypt_traffic: true, + rate_limiting: true, + allow_external: false, + } + } + + pub fn insecure() -> Self { + Self { + require_auth: false, + psk: [0u8; 32], + encrypt_traffic: false, + rate_limiting: false, + allow_external: true, + } + } + + pub fn is_psk_set(&self) -> bool { + self.psk.iter().any(|&b| b != 0) + } +} + +#[derive(Debug, Clone)] +pub struct WifiConfig { + + pub mode: WifiMode, + + pub ap_ssid: String, + + pub ap_password: String, + + pub ap_channel: u8, + + pub sta_ssid: String, + + pub sta_password: String, + + pub tcp_port: u16, + + pub mdns_enabled: bool, + + pub mdns_hostname: String<32>, + + pub security: SecurityConfig, +} + +impl Default for WifiConfig { + fn default() -> Self { + let mut ap_ssid = String::new(); + let _ = ap_ssid.push_str(DEFAULT_AP_SSID); + + let mut ap_password = String::new(); + let _ = ap_password.push_str(DEFAULT_AP_PASSWORD); + + let mut mdns_hostname = String::new(); + let _ = mdns_hostname.push_str("lunarcore"); + + Self { + mode: WifiMode::Off, + ap_ssid, + ap_password, + ap_channel: 1, + sta_ssid: String::new(), + sta_password: String::new(), + tcp_port: DEFAULT_TCP_PORT, + mdns_enabled: true, + mdns_hostname, + security: SecurityConfig::default(), + } + } +} + +#[derive(Debug, Clone)] +pub struct NetworkStatus { + + pub mode: WifiMode, + + pub sta_connected: bool, + + pub sta_ip: Option, + + pub ap_active: bool, + + pub ap_ip: Option, + + pub ap_client_count: u8, + + pub tcp_client_count: u8, + + pub rssi: i8, +} + +impl Default for NetworkStatus { + fn default() -> Self { + Self { + mode: WifiMode::Off, + sta_connected: false, + sta_ip: None, + ap_active: false, + ap_ip: None, + ap_client_count: 0, + tcp_client_count: 0, + rssi: 0, + } + } +} + +fn hmac_sha256(key: &[u8; 32], message: &[u8]) -> [u8; 32] { + + const IPAD: u8 = 0x36; + const OPAD: u8 = 0x5c; + const BLOCK_SIZE: usize = 64; + + let mut k_pad = [0u8; BLOCK_SIZE]; + k_pad[..32].copy_from_slice(key); + + let mut inner_hasher = Sha256::new(); + let mut inner_key = [0u8; BLOCK_SIZE]; + for i in 0..BLOCK_SIZE { + inner_key[i] = k_pad[i] ^ IPAD; + } + inner_hasher.update(&inner_key); + inner_hasher.update(message); + let inner_hash = inner_hasher.finalize(); + + let mut outer_hasher = Sha256::new(); + let mut outer_key = [0u8; BLOCK_SIZE]; + for i in 0..BLOCK_SIZE { + outer_key[i] = k_pad[i] ^ OPAD; + } + outer_hasher.update(&outer_key); + outer_hasher.update(&inner_hash); + outer_hasher.finalize() +} + +#[inline(never)] +fn ct_eq_32(a: &[u8; 32], b: &[u8; 32]) -> bool { + let mut diff: u8 = 0; + for i in 0..32 { + diff |= a[i] ^ b[i]; + } + diff == 0 +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum AuthState { + + None, + + ChallengeSent, + + Authenticated, + + Failed, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum TcpClientState { + + Disconnected, + + Authenticating, + + Detecting, + + Connected, +} + +pub struct SessionCrypto { + + pub key: [u8; SESSION_KEY_SIZE], + + pub nonce: [u8; SESSION_NONCE_SIZE], + + pub counter: u32, + + pub enabled: bool, +} + +impl Drop for SessionCrypto { + fn drop(&mut self) { + + self.clear(); + } +} + +impl SessionCrypto { + + pub const fn new() -> Self { + Self { + key: [0u8; SESSION_KEY_SIZE], + nonce: [0u8; SESSION_NONCE_SIZE], + counter: 0, + enabled: false, + } + } + + pub fn init(&mut self, key: &[u8; SESSION_KEY_SIZE], nonce: &[u8; SESSION_NONCE_SIZE]) { + self.key.copy_from_slice(key); + self.nonce.copy_from_slice(nonce); + self.counter = 0; + self.enabled = true; + } + + pub fn encrypt(&mut self, data: &mut [u8]) { + if !self.enabled { + return; + } + + let counter_bytes = self.counter.to_le_bytes(); + self.nonce[0..4].copy_from_slice(&counter_bytes); + + let cipher = ChaCha20::new(&self.key, &self.nonce); + cipher.encrypt(data); + + self.counter = self.counter.wrapping_add(1); + } + + pub fn decrypt(&mut self, data: &mut [u8]) { + + self.encrypt(data); + } + + pub fn clear(&mut self) { + + for b in &mut self.key { + unsafe { core::ptr::write_volatile(b, 0) }; + } + for b in &mut self.nonce { + unsafe { core::ptr::write_volatile(b, 0) }; + } + self.counter = 0; + self.enabled = false; + } +} + +pub struct TcpClient { + + pub fd: i32, + + pub addr: SocketAddrV4, + + pub state: TcpClientState, + + pub auth_state: AuthState, + + pub auth_challenge: [u8; AUTH_CHALLENGE_SIZE], + + pub auth_failures: u8, + + pub auth_started: u32, + + pub session: SessionCrypto, + + pub protocol: u8, + + pub rx_buffer: Vec, + + pub tx_buffer: Vec, + + pub last_activity: u32, +} + +impl TcpClient { + + pub const fn empty() -> Self { + Self { + fd: -1, + addr: SocketAddrV4::new(Ipv4Addr::new(0, 0, 0, 0), 0), + state: TcpClientState::Disconnected, + auth_state: AuthState::None, + auth_challenge: [0u8; AUTH_CHALLENGE_SIZE], + auth_failures: 0, + auth_started: 0, + session: SessionCrypto::new(), + protocol: 0, + rx_buffer: Vec::new(), + tx_buffer: Vec::new(), + last_activity: 0, + } + } + + pub fn is_available(&self) -> bool { + self.state == TcpClientState::Disconnected + } + + pub fn is_authenticated(&self) -> bool { + self.auth_state == AuthState::Authenticated || self.auth_state == AuthState::None + } + + pub fn reset(&mut self) { + self.fd = -1; + self.state = TcpClientState::Disconnected; + self.auth_state = AuthState::None; + self.auth_challenge.fill(0); + self.auth_failures = 0; + self.auth_started = 0; + self.session.clear(); + self.protocol = 0; + self.rx_buffer.clear(); + self.tx_buffer.clear(); + } +} + +pub struct WifiManager { + + pub config: WifiConfig, + + pub status: NetworkStatus, + + server_fd: i32, + + clients: [TcpClient; MAX_TCP_CLIENTS], + + initialized: bool, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum WifiError { + + NotInitialized, + + AlreadyInitialized, + + ConfigError, + + ConnectionFailed, + + Timeout, + + SocketError, + + NoClientSlots, + + BufferFull, + + AuthenticationFailed, + + NoPskConfigured, + + NotAuthenticated, + + RateLimited, +} + +impl WifiManager { + + pub const fn new() -> Self { + Self { + config: WifiConfig { + mode: WifiMode::Off, + ap_ssid: String::new(), + ap_password: String::new(), + ap_channel: 1, + sta_ssid: String::new(), + sta_password: String::new(), + tcp_port: DEFAULT_TCP_PORT, + mdns_enabled: true, + mdns_hostname: String::new(), + security: SecurityConfig { + require_auth: true, + psk: [0u8; 32], + encrypt_traffic: true, + rate_limiting: true, + allow_external: false, + }, + }, + status: NetworkStatus { + mode: WifiMode::Off, + sta_connected: false, + sta_ip: None, + ap_active: false, + ap_ip: None, + ap_client_count: 0, + tcp_client_count: 0, + rssi: 0, + }, + server_fd: -1, + clients: [ + TcpClient::empty(), + TcpClient::empty(), + TcpClient::empty(), + TcpClient::empty(), + ], + initialized: false, + } + } + + pub fn set_psk(&mut self, psk: &[u8; 32]) { + self.config.security.psk.copy_from_slice(psk); + } + + pub fn is_auth_configured(&self) -> bool { + !self.config.security.require_auth || self.config.security.is_psk_set() + } + + pub fn init(&mut self, config: WifiConfig) -> Result<(), WifiError> { + if self.initialized { + return Err(WifiError::AlreadyInitialized); + } + + self.config = config; + + unsafe { + + let ret = esp_idf_sys::nvs_flash_init(); + if ret != 0 && ret != esp_idf_sys::ESP_ERR_NVS_NO_FREE_PAGES as i32 { + + esp_idf_sys::nvs_flash_erase(); + esp_idf_sys::nvs_flash_init(); + } + + esp_idf_sys::esp_netif_init(); + + esp_idf_sys::esp_event_loop_create_default(); + + match self.config.mode { + WifiMode::Off => {} + WifiMode::Ap => { + esp_idf_sys::esp_netif_create_default_wifi_ap(); + } + WifiMode::Sta => { + esp_idf_sys::esp_netif_create_default_wifi_sta(); + } + WifiMode::ApSta => { + esp_idf_sys::esp_netif_create_default_wifi_ap(); + esp_idf_sys::esp_netif_create_default_wifi_sta(); + } + } + + let mut wifi_init_config = esp_idf_sys::wifi_init_config_t::default(); + esp_idf_sys::esp_wifi_init(&wifi_init_config); + + esp_idf_sys::esp_wifi_set_storage(esp_idf_sys::wifi_storage_t_WIFI_STORAGE_RAM); + } + + self.initialized = true; + Ok(()) + } + + pub fn start(&mut self) -> Result<(), WifiError> { + if !self.initialized { + return Err(WifiError::NotInitialized); + } + + unsafe { + match self.config.mode { + WifiMode::Off => { + esp_idf_sys::esp_wifi_stop(); + } + WifiMode::Ap => { + self.configure_ap()?; + esp_idf_sys::esp_wifi_set_mode(esp_idf_sys::wifi_mode_t_WIFI_MODE_AP); + esp_idf_sys::esp_wifi_start(); + self.status.ap_active = true; + self.status.ap_ip = Some(Ipv4Addr::new(192, 168, 4, 1)); + } + WifiMode::Sta => { + self.configure_sta()?; + esp_idf_sys::esp_wifi_set_mode(esp_idf_sys::wifi_mode_t_WIFI_MODE_STA); + esp_idf_sys::esp_wifi_start(); + esp_idf_sys::esp_wifi_connect(); + } + WifiMode::ApSta => { + self.configure_ap()?; + self.configure_sta()?; + esp_idf_sys::esp_wifi_set_mode(esp_idf_sys::wifi_mode_t_WIFI_MODE_APSTA); + esp_idf_sys::esp_wifi_start(); + esp_idf_sys::esp_wifi_connect(); + self.status.ap_active = true; + self.status.ap_ip = Some(Ipv4Addr::new(192, 168, 4, 1)); + } + } + } + + self.status.mode = self.config.mode; + Ok(()) + } + + fn configure_ap(&self) -> Result<(), WifiError> { + unsafe { + let mut ap_config: esp_idf_sys::wifi_config_t = core::mem::zeroed(); + + let ssid_bytes = self.config.ap_ssid.as_bytes(); + let ssid_len = ssid_bytes.len().min(32); + ap_config.ap.ssid[..ssid_len].copy_from_slice(&ssid_bytes[..ssid_len]); + ap_config.ap.ssid_len = ssid_len as u8; + + let pass_bytes = self.config.ap_password.as_bytes(); + let pass_len = pass_bytes.len().min(64); + ap_config.ap.password[..pass_len].copy_from_slice(&pass_bytes[..pass_len]); + + ap_config.ap.channel = self.config.ap_channel; + ap_config.ap.max_connection = MAX_TCP_CLIENTS as u8; + + if pass_len > 0 { + ap_config.ap.authmode = esp_idf_sys::wifi_auth_mode_t_WIFI_AUTH_WPA2_PSK; + } else { + ap_config.ap.authmode = esp_idf_sys::wifi_auth_mode_t_WIFI_AUTH_OPEN; + } + + let ret = esp_idf_sys::esp_wifi_set_config( + esp_idf_sys::wifi_interface_t_WIFI_IF_AP, + &mut ap_config, + ); + + if ret != 0 { + return Err(WifiError::ConfigError); + } + } + Ok(()) + } + + fn configure_sta(&self) -> Result<(), WifiError> { + unsafe { + let mut sta_config: esp_idf_sys::wifi_config_t = core::mem::zeroed(); + + let ssid_bytes = self.config.sta_ssid.as_bytes(); + let ssid_len = ssid_bytes.len().min(32); + sta_config.sta.ssid[..ssid_len].copy_from_slice(&ssid_bytes[..ssid_len]); + + let pass_bytes = self.config.sta_password.as_bytes(); + let pass_len = pass_bytes.len().min(64); + sta_config.sta.password[..pass_len].copy_from_slice(&pass_bytes[..pass_len]); + + let ret = esp_idf_sys::esp_wifi_set_config( + esp_idf_sys::wifi_interface_t_WIFI_IF_STA, + &mut sta_config, + ); + + if ret != 0 { + return Err(WifiError::ConfigError); + } + } + Ok(()) + } + + pub fn start_tcp_server(&mut self) -> Result<(), WifiError> { + if !self.initialized { + return Err(WifiError::NotInitialized); + } + + unsafe { + + let fd = esp_idf_sys::lwip_socket( + esp_idf_sys::AF_INET as i32, + esp_idf_sys::SOCK_STREAM as i32, + esp_idf_sys::IPPROTO_TCP as i32, + ); + + if fd < 0 { + return Err(WifiError::SocketError); + } + + let opt: i32 = 1; + esp_idf_sys::lwip_setsockopt( + fd, + esp_idf_sys::SOL_SOCKET as i32, + esp_idf_sys::SO_REUSEADDR as i32, + &opt as *const _ as *const core::ffi::c_void, + core::mem::size_of::() as u32, + ); + + let mut addr: esp_idf_sys::sockaddr_in = core::mem::zeroed(); + addr.sin_family = esp_idf_sys::AF_INET as u8; + addr.sin_port = self.config.tcp_port.to_be(); + addr.sin_addr.s_addr = 0; + + let ret = esp_idf_sys::lwip_bind( + fd, + &addr as *const _ as *const esp_idf_sys::sockaddr, + core::mem::size_of::() as u32, + ); + + if ret < 0 { + esp_idf_sys::lwip_close(fd); + return Err(WifiError::SocketError); + } + + let ret = esp_idf_sys::lwip_listen(fd, MAX_TCP_CLIENTS as i32); + if ret < 0 { + esp_idf_sys::lwip_close(fd); + return Err(WifiError::SocketError); + } + + let flags = esp_idf_sys::lwip_fcntl(fd, esp_idf_sys::F_GETFL as i32, 0); + esp_idf_sys::lwip_fcntl(fd, esp_idf_sys::F_SETFL as i32, flags | esp_idf_sys::O_NONBLOCK as i32); + + self.server_fd = fd; + } + + Ok(()) + } + + pub fn stop_tcp_server(&mut self) { + if self.server_fd >= 0 { + unsafe { + esp_idf_sys::lwip_close(self.server_fd); + } + self.server_fd = -1; + } + + for client in &mut self.clients { + if client.fd >= 0 { + unsafe { + esp_idf_sys::lwip_close(client.fd); + } + client.reset(); + } + } + } + + pub fn poll(&mut self) -> Option<(usize, Vec)> { + if self.server_fd < 0 { + return None; + } + + self.accept_connections(); + + for i in 0..MAX_TCP_CLIENTS { + if self.clients[i].fd >= 0 { + if let Some(data) = self.read_client(i) { + return Some((i, data)); + } + } + } + + None + } + + fn accept_connections(&mut self) { + unsafe { + let mut client_addr: esp_idf_sys::sockaddr_in = core::mem::zeroed(); + let mut addr_len: u32 = core::mem::size_of::() as u32; + + let client_fd = esp_idf_sys::lwip_accept( + self.server_fd, + &mut client_addr as *mut _ as *mut esp_idf_sys::sockaddr, + &mut addr_len, + ); + + if client_fd >= 0 { + + for client in &mut self.clients { + if client.is_available() { + + let flags = esp_idf_sys::lwip_fcntl(client_fd, esp_idf_sys::F_GETFL as i32, 0); + esp_idf_sys::lwip_fcntl(client_fd, esp_idf_sys::F_SETFL as i32, flags | esp_idf_sys::O_NONBLOCK as i32); + + client.fd = client_fd; + client.state = TcpClientState::Detecting; + + let ip_bytes = client_addr.sin_addr.s_addr.to_le_bytes(); + let ip = Ipv4Addr::new(ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); + let port = u16::from_be(client_addr.sin_port); + client.addr = SocketAddrV4::new(ip, port); + + self.status.tcp_client_count += 1; + return; + } + } + + esp_idf_sys::lwip_close(client_fd); + } + } + } + + fn read_client(&mut self, idx: usize) -> Option> { + let client = &mut self.clients[idx]; + if client.fd < 0 { + return None; + } + + let mut buf = [0u8; TCP_RX_BUFFER_SIZE]; + + unsafe { + let n = esp_idf_sys::lwip_recv( + client.fd, + buf.as_mut_ptr() as *mut core::ffi::c_void, + buf.len(), + 0, + ); + + if n > 0 { + let mut data = Vec::new(); + for &b in &buf[..n as usize] { + let _ = data.push(b); + } + return Some(data); + } else if n == 0 { + + self.disconnect_client(idx); + } + + } + + None + } + + pub fn send_to_client(&mut self, idx: usize, data: &[u8]) -> Result { + if idx >= MAX_TCP_CLIENTS { + return Err(WifiError::SocketError); + } + + let client = &mut self.clients[idx]; + if client.fd < 0 { + return Err(WifiError::SocketError); + } + + unsafe { + let n = esp_idf_sys::lwip_send( + client.fd, + data.as_ptr() as *const core::ffi::c_void, + data.len(), + 0, + ); + + if n < 0 { + self.disconnect_client(idx); + return Err(WifiError::SocketError); + } + + Ok(n as usize) + } + } + + pub fn broadcast(&mut self, data: &[u8]) { + for i in 0..MAX_TCP_CLIENTS { + if self.clients[i].fd >= 0 { + let _ = self.send_to_client(i, data); + } + } + } + + pub fn disconnect_client(&mut self, idx: usize) { + if idx >= MAX_TCP_CLIENTS { + return; + } + + let client = &mut self.clients[idx]; + if client.fd >= 0 { + unsafe { + esp_idf_sys::lwip_close(client.fd); + } + client.reset(); + if self.status.tcp_client_count > 0 { + self.status.tcp_client_count -= 1; + } + } + } + + pub fn get_client(&self, idx: usize) -> Option<&TcpClient> { + if idx < MAX_TCP_CLIENTS && self.clients[idx].fd >= 0 { + Some(&self.clients[idx]) + } else { + None + } + } + + pub fn get_client_mut(&mut self, idx: usize) -> Option<&mut TcpClient> { + if idx < MAX_TCP_CLIENTS && self.clients[idx].fd >= 0 { + Some(&mut self.clients[idx]) + } else { + None + } + } + + pub fn stop(&mut self) { + self.stop_tcp_server(); + + unsafe { + esp_idf_sys::esp_wifi_stop(); + } + + self.status.mode = WifiMode::Off; + self.status.sta_connected = false; + self.status.ap_active = false; + } + + pub fn status(&self) -> &NetworkStatus { + &self.status + } + + pub fn update_status(&mut self) { + unsafe { + + let mut ap_info: esp_idf_sys::wifi_ap_record_t = core::mem::zeroed(); + if esp_idf_sys::esp_wifi_sta_get_ap_info(&mut ap_info) == 0 { + self.status.sta_connected = true; + self.status.rssi = ap_info.rssi; + } else { + self.status.sta_connected = false; + } + + } + } + + pub fn scan(&mut self) -> Result, WifiError> { + if !self.initialized { + return Err(WifiError::NotInitialized); + } + + let mut networks = Vec::new(); + + unsafe { + + let scan_config: esp_idf_sys::wifi_scan_config_t = core::mem::zeroed(); + let ret = esp_idf_sys::esp_wifi_scan_start(&scan_config, true); + if ret != 0 { + return Err(WifiError::ConfigError); + } + + let mut ap_count: u16 = 0; + esp_idf_sys::esp_wifi_scan_get_ap_num(&mut ap_count); + + let count = ap_count.min(16) as usize; + let mut ap_records: [esp_idf_sys::wifi_ap_record_t; 16] = core::mem::zeroed(); + let mut actual_count = count as u16; + + esp_idf_sys::esp_wifi_scan_get_ap_records(&mut actual_count, ap_records.as_mut_ptr()); + + for i in 0..actual_count as usize { + let ap = &ap_records[i]; + let mut ssid = String::new(); + for &b in &ap.ssid { + if b == 0 { + break; + } + let _ = ssid.push(b as char); + } + + let info = NetworkInfo { + ssid, + rssi: ap.rssi, + channel: ap.primary, + auth: ap.authmode as u8, + }; + let _ = networks.push(info); + } + } + + Ok(networks) + } +} + +impl Default for WifiManager { + fn default() -> Self { + Self::new() + } +} + +#[derive(Debug, Clone)] +pub struct NetworkInfo { + + pub ssid: String, + + pub rssi: i8, + + pub channel: u8, + + pub auth: u8, +} + +impl NetworkInfo { + + pub fn is_open(&self) -> bool { + self.auth == 0 + } +}