From 218d49513d915a105523e29f98be940b0690171a Mon Sep 17 00:00:00 2001 From: STCisGOOD Date: Fri, 6 Feb 2026 16:44:28 -0500 Subject: [PATCH] v1.1.0: standalone repeater mode, prebuilt binary, protocol hot-switching Repeater mode: device auto-relays MeshCore packets when no app is connected. Enabled by default, toggle with AT+REPEATER=0/1. Includes deduplication ring and random jitter to prevent loops. Prebuilt binary: added CI workflow that builds a merged .bin on tag push so users can flash without installing the Xtensa toolchain. Protocol hot-switching: auto-resets to detection mode on serial inactivity (30s) or BLE disconnect. AT+SWITCH for manual reset. Also: gated test beacons when app connected, updated README with flash instructions and AT command reference. --- .cargo/config.toml | 18 + .github/workflows/build-release.yml | 84 + .gitignore | 13 + Cargo.toml | 8 +- README.md | 66 +- build.rs | 3 + partitions.csv | 6 + rust-toolchain.toml | 4 +- src/ble.rs | 2656 +++++++++--------- src/contact.rs | 1310 +++++---- src/crypto/aes.rs | 2109 ++++++++------ src/crypto/chacha20.rs | 569 ++-- src/crypto/ed25519.rs | 2126 +++++++------- src/crypto/hkdf.rs | 389 ++- src/crypto/hmac.rs | 249 +- src/crypto/mod.rs | 205 +- src/crypto/poly1305.rs | 735 ++--- src/crypto/sha256.rs | 491 ++-- src/crypto/x25519.rs | 912 +++--- src/display.rs | 1743 ++++++------ src/identity.rs | 1038 ++++--- src/main.rs | 3961 ++++++++++++++++----------- src/main_minimal.rs | 47 + src/meshtastic/channel.rs | 1259 +++++---- src/meshtastic/encryption.rs | 921 ++++--- src/meshtastic/mod.rs | 2703 +++++++++--------- src/meshtastic/packet.rs | 958 ++++--- src/meshtastic/protobuf.rs | 2761 ++++++++++--------- src/onion.rs | 825 +++--- src/ota.rs | 1314 +++++---- src/packet_id.rs | 666 +++-- src/power.rs | 867 +++--- src/protocol.rs | 845 +++--- src/protocol_router.rs | 1084 ++++---- src/rng.rs | 694 +++-- src/rnode.rs | 2292 +++++++++------- src/session.rs | 1681 ++++++------ src/sx1262.rs | 1391 +++++----- src/transport.rs | 1025 ++++--- src/wifi.rs | 2167 ++++++++------- 40 files changed, 23625 insertions(+), 18570 deletions(-) create mode 100644 .cargo/config.toml create mode 100644 .github/workflows/build-release.yml create mode 100644 .gitignore create mode 100644 partitions.csv create mode 100644 src/main_minimal.rs diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..33dc512 --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,18 @@ +[build] +target = "xtensa-esp32s3-espidf" + +[target.xtensa-esp32s3-espidf] +linker = "ldproxy" +runner = "espflash flash --monitor" + +[unstable] +build-std = ["std", "panic_abort"] + +[env] +MCU = "esp32s3" +LIBCLANG_PATH = "C:/Users/anon/.rustup/toolchains/esp/xtensa-esp32-elf-clang/esp-clang/bin" +ESP_IDF_VERSION = "v5.2.1" +ESP_IDF_SDKCONFIG_DEFAULTS = "sdkconfig.defaults" +ESP_IDF_CMAKE_GENERATOR = "Ninja" +ESP_IDF_PARTITION_TABLE_PATH = { value = "partitions.csv", relative = true } +ESP_IDF_CMAKE_ARGS = "-DPARTITION_TABLE_CSV_PATH=partitions.csv" diff --git a/.github/workflows/build-release.yml b/.github/workflows/build-release.yml new file mode 100644 index 0000000..2d0173c --- /dev/null +++ b/.github/workflows/build-release.yml @@ -0,0 +1,84 @@ +name: Build & Release + +on: + push: + tags: + - 'v*' + workflow_dispatch: + +env: + CARGO_TERM_COLOR: always + +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Install system dependencies + run: | + sudo apt-get update + sudo apt-get install -y git wget flex bison gperf python3 python3-pip \ + python3-venv cmake ninja-build ccache libffi-dev libssl-dev dfu-util \ + libusb-1.0-0 pkg-config + + - name: Install espup + run: | + cargo install espup + espup install + . $HOME/export-esp.sh + echo "$HOME/.rustup/toolchains/esp/bin" >> $GITHUB_PATH + + - name: Set ESP-IDF environment + run: | + . $HOME/export-esp.sh + echo "LIBCLANG_PATH=$HOME/.rustup/toolchains/esp/xtensa-esp32-elf-clang/esp-clang/bin" >> $GITHUB_ENV + echo "ESP_IDF_VERSION=v5.2.1" >> $GITHUB_ENV + echo "ESP_IDF_SDKCONFIG_DEFAULTS=sdkconfig.defaults" >> $GITHUB_ENV + echo "ESP_IDF_CMAKE_GENERATOR=Ninja" >> $GITHUB_ENV + + - name: Install espflash + run: cargo install espflash + + - name: Build release + run: | + . $HOME/export-esp.sh + cargo build --target xtensa-esp32s3-espidf --release + + - name: Create merged firmware binary + run: | + espflash save-image --chip esp32s3 --merge \ + target/xtensa-esp32s3-espidf/release/lunarcore \ + lunarcore-esp32s3.bin + + - name: Upload artifact + uses: actions/upload-artifact@v4 + with: + name: lunarcore-esp32s3 + path: lunarcore-esp32s3.bin + + - name: Create GitHub Release + if: startsWith(github.ref, 'refs/tags/v') + uses: softprops/action-gh-release@v2 + with: + files: lunarcore-esp32s3.bin + generate_release_notes: true + body: | + ## Flash Instructions + + **Prerequisites:** Install esptool (`pip install esptool`) + + **Flash the firmware:** + ```bash + esptool.py --chip esp32s3 -p PORT write_flash 0x0 lunarcore-esp32s3.bin + ``` + + Replace `PORT` with your serial port: + - **Linux:** `/dev/ttyACM0` or `/dev/ttyUSB0` + - **macOS:** `/dev/cu.usbmodem*` + - **Windows:** `COM3` (check Device Manager) + + **Supported hardware:** Heltec WiFi LoRa 32 V3, LilyGo T3-S3, or any ESP32-S3 + SX1262 board. + + The binary includes bootloader + partition table + application merged at offset 0x0. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..972fc3c --- /dev/null +++ b/.gitignore @@ -0,0 +1,13 @@ +/target/ +/.embuild/ +/build/ +sdkconfig +sdkconfig.old +.idea/ +.vscode/ +*.swp +*.swo +*~ +.DS_Store +Thumbs.db +*.log diff --git a/Cargo.toml b/Cargo.toml index 6a17a2a..1dcc085 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,8 +1,12 @@ [package] name = "lunarcore" -version = "1.0.0" +version = "1.1.0" edition = "2021" +authors = ["Yours App Team"] +description = "LunarCore - Unified mesh firmware supporting MeshCore, Meshtastic, and RNode/Reticulum protocols" license = "MIT" +repository = "https://github.com/anthropics/lunarcore" +readme = "README.md" [dependencies] esp-idf-hal = { version = "0.45", features = ["critical-section"] } @@ -28,8 +32,10 @@ rnode = [] opt-level = "s" lto = false codegen-units = 1 +debug = true [profile.dev] +debug = true opt-level = "z" [[bin]] diff --git a/README.md b/README.md index ae16ac3..4d87a44 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,41 @@ -# 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 +# LunarCore + +Multi-protocol mesh firmware for ESP32-S3 LoRa devices. + +## Protocols + +- MeshCore +- Meshtastic +- RNode/KISS (Reticulum) + +Protocol is auto-detected from the first bytes over serial or BLE. + +## Hardware + +Heltec WiFi LoRa 32 V3 (ESP32-S3 + SX1262) + +## Flash (prebuilt) + +Download `lunarcore-esp32s3.bin` from [Releases](../../releases). + +```bash +pip install esptool +esptool.py --chip esp32s3 -p PORT write_flash 0x0 lunarcore-esp32s3.bin +``` + +## Build from source + +```bash +espup install +. ~/export-esp.sh +cargo build --release +espflash flash target/xtensa-esp32s3-espidf/release/lunarcore --monitor +``` + +## Repeater + +Enabled by default. Relays MeshCore packets when no app is connected. Disable with `AT+REPEATER=0`. + +## License + +MIT diff --git a/build.rs b/build.rs index 112ec3f..2d13580 100644 --- a/build.rs +++ b/build.rs @@ -1,3 +1,6 @@ fn main() { + // ESP-IDF embuild setup + // The partition table is handled by the lunarcore_config component + // via components/lunarcore_config/CMakeLists.txt embuild::espidf::sysenv::output(); } diff --git a/partitions.csv b/partitions.csv new file mode 100644 index 0000000..da92bb8 --- /dev/null +++ b/partitions.csv @@ -0,0 +1,6 @@ +# LunarCore Partition Table +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x6000, +phy_init, data, phy, 0xf000, 0x1000, +factory, app, factory, 0x10000, 0x3F0000, +storage, data, spiffs, 0x400000,0x400000, diff --git a/rust-toolchain.toml b/rust-toolchain.toml index 0a93d54..a2f5ab5 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -1,2 +1,2 @@ -[toolchain] -channel = "esp" +[toolchain] +channel = "esp" diff --git a/src/ble.rs b/src/ble.rs index c64f5ec..69a56fd 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -1,1267 +1,1389 @@ -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 -} +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 +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_nus_uuids() { + + assert_eq!(nus::SERVICE[12], 0x01); + assert_eq!(nus::RX[12], 0x02); + assert_eq!(nus::TX[12], 0x03); + } + + #[test] + fn test_adv_data_builder() { + let data = build_adv_data("LunarCore", true, false); + assert!(data.len() <= MAX_ADV_DATA); + + + assert_eq!(data[0], 0x02); + assert_eq!(data[1], 0x01); + assert_eq!(data[2], 0x06); + } + + #[test] + fn test_connection() { + let conn = BleConnection::new(1, [0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF]); + assert_eq!(conn.handle, 1); + assert_eq!(conn.state, ConnectionState::Connected); + assert_eq!(conn.mtu, 23); + assert_eq!(conn.max_payload(), 20); + } + + #[test] + fn test_ble_manager_init() { + + let manager = BleManager::new(); + assert!(!manager.initialized); + } +} diff --git a/src/contact.rs b/src/contact.rs index 1e1064b..b1145ab 100644 --- a/src/contact.rs +++ b/src/contact.rs @@ -1,598 +1,712 @@ -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) - } -} +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) + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_contact_hello_roundtrip() { + let mut hello = ContactHello::new( + 1234567890000, + b"did:offgrid:zTestDid", + [1u8; 32], + [2u8; 32], + b"Alice", + None, + ).unwrap(); + + + let private_key = [0u8; 32]; + hello.sign(&private_key); + + + let encoded = hello.encode(); + let decoded = ContactHello::decode(&encoded).unwrap(); + + assert_eq!(decoded.version, CONTACT_HELLO_VERSION); + assert_eq!(decoded.timestamp, 1234567890000); + assert_eq!(&decoded.name[..], b"Alice"); + } + + #[test] + fn test_contact_hello_qr() { + let hello = ContactHello::new( + 1234567890000, + b"did:offgrid:zTestDid", + [1u8; 32], + [2u8; 32], + b"Bob", + None, + ).unwrap(); + + let qr_data = hello.to_qr_data(); + assert!(qr_data.starts_with(b"YCH:")); + + let parsed = ContactHello::from_qr_data(&qr_data).unwrap(); + assert_eq!(&parsed.name[..], b"Bob"); + } +} diff --git a/src/crypto/aes.rs b/src/crypto/aes.rs index 8190551..870f683 100644 --- a/src/crypto/aes.rs +++ b/src/crypto/aes.rs @@ -1,913 +1,1196 @@ -#[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; - } - } - } -} +#[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; + } + } + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_sbox_ct_correctness() { + + for i in 0..=255u8 { + let expected = SBOX[i as usize]; + let computed = sbox_ct(i); + assert_eq!(computed, expected, "S-box mismatch at input {:#04x}: expected {:#04x}, got {:#04x}", i, expected, computed); + } + } + + #[test] + fn test_inv_sbox_ct_correctness() { + + for i in 0..=255u8 { + let expected = INV_SBOX[i as usize]; + let computed = inv_sbox_ct(i); + assert_eq!(computed, expected, "Inv S-box mismatch at input {:#04x}: expected {:#04x}, got {:#04x}", i, expected, computed); + } + } + + #[test] + fn test_sbox_inv_sbox_roundtrip() { + + for i in 0..=255u8 { + assert_eq!(inv_sbox_ct(sbox_ct(i)), i); + assert_eq!(sbox_ct(inv_sbox_ct(i)), i); + } + } + + #[test] + fn test_gf_mul_ct_correctness() { + + assert_eq!(gf_mul_ct(0x57, 0x83), 0xc1); + assert_eq!(gf_mul_ct(0x02, 0x87), 0x15); + assert_eq!(gf_mul_ct(0, 0x42), 0); + assert_eq!(gf_mul_ct(0x42, 0), 0); + assert_eq!(gf_mul_ct(1, 0x42), 0x42); + assert_eq!(gf_mul_ct(0x42, 1), 0x42); + } + + #[test] + fn test_aes128_known_vector() { + + let key: [u8; 16] = [ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, + ]; + let plaintext: [u8; 16] = [ + 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, + 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff, + ]; + let expected: [u8; 16] = [ + 0x69, 0xc4, 0xe0, 0xd8, 0x6a, 0x7b, 0x04, 0x30, + 0xd8, 0xcd, 0xb7, 0x80, 0x70, 0xb4, 0xc5, 0x5a, + ]; + + let cipher = Aes128::new(&key); + let mut block = plaintext; + cipher.encrypt_block(&mut block); + + assert_eq!(block, expected); + + + cipher.decrypt_block(&mut block); + assert_eq!(block, plaintext); + } + + #[test] + fn test_aes128ct_known_vector() { + + let key: [u8; 16] = [ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, + ]; + let plaintext: [u8; 16] = [ + 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, + 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff, + ]; + let expected: [u8; 16] = [ + 0x69, 0xc4, 0xe0, 0xd8, 0x6a, 0x7b, 0x04, 0x30, + 0xd8, 0xcd, 0xb7, 0x80, 0x70, 0xb4, 0xc5, 0x5a, + ]; + + let cipher = Aes128Ct::new(&key); + let mut block = plaintext; + cipher.encrypt_block(&mut block); + + assert_eq!(block, expected); + + cipher.decrypt_block(&mut block); + assert_eq!(block, plaintext); + } + + #[test] + fn test_aes128_vs_aes128ct() { + + let key: [u8; 16] = [0x42; 16]; + let plaintext: [u8; 16] = [0x24; 16]; + + let cipher = Aes128::new(&key); + let cipher_ct = Aes128Ct::new(&key); + + let mut block1 = plaintext; + let mut block2 = plaintext; + + cipher.encrypt_block(&mut block1); + cipher_ct.encrypt_block(&mut block2); + + assert_eq!(block1, block2); + + cipher.decrypt_block(&mut block1); + cipher_ct.decrypt_block(&mut block2); + + assert_eq!(block1, block2); + assert_eq!(block1, plaintext); + } + + #[test] + fn test_aes256_known_vector() { + + let key: [u8; 32] = [ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, + 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, + 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, + ]; + let plaintext: [u8; 16] = [ + 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, + 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff, + ]; + let expected: [u8; 16] = [ + 0x8e, 0xa2, 0xb7, 0xca, 0x51, 0x67, 0x45, 0xbf, + 0xea, 0xfc, 0x49, 0x90, 0x4b, 0x49, 0x60, 0x89, + ]; + + let cipher = Aes256::new(&key); + let mut block = plaintext; + cipher.encrypt_block(&mut block); + + assert_eq!(block, expected); + + cipher.decrypt_block(&mut block); + assert_eq!(block, plaintext); + } + + #[test] + fn test_ctr_mode() { + let key: [u8; 16] = [0x2b; 16]; + let nonce: [u8; 16] = [0x00; 16]; + let plaintext = b"Hello, World! This is a test message."; + + let cipher = Aes128::new(&key); + + let mut data = [0u8; 64]; + data[..plaintext.len()].copy_from_slice(plaintext); + let len = plaintext.len(); + + + cipher.encrypt_ctr(&nonce, &mut data[..len]); + + + assert_ne!(&data[..len], plaintext); + + + cipher.decrypt_ctr(&nonce, &mut data[..len]); + + + assert_eq!(&data[..len], plaintext); + } + + #[test] + fn test_ctr_mode_ct() { + + let key: [u8; 16] = [0x2b; 16]; + let nonce: [u8; 16] = [0x00; 16]; + let plaintext = b"Hello, World! This is a test message."; + + let cipher = Aes128Ct::new(&key); + + let mut data = [0u8; 64]; + data[..plaintext.len()].copy_from_slice(plaintext); + let len = plaintext.len(); + + cipher.encrypt_ctr(&nonce, &mut data[..len]); + assert_ne!(&data[..len], plaintext); + + cipher.decrypt_ctr(&nonce, &mut data[..len]); + assert_eq!(&data[..len], plaintext); + } + + #[test] + fn test_gf_mul() { + + assert_eq!(gf_mul(0x57, 0x83), 0xc1); + assert_eq!(gf_mul(0x02, 0x87), 0x15); + } +} diff --git a/src/crypto/chacha20.rs b/src/crypto/chacha20.rs index eaf74ac..b134dee 100644 --- a/src/crypto/chacha20.rs +++ b/src/crypto/chacha20.rs @@ -1,229 +1,340 @@ -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); - } -} +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); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_rfc8439_block() { + + let key: [u8; 32] = [ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, + 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, + 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, + ]; + let nonce: [u8; 12] = [ + 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x4a, + 0x00, 0x00, 0x00, 0x00, + ]; + + let cipher = ChaCha20::new_with_counter(&key, &nonce, 1); + let block = cipher.block(1); + + + let expected_start: [u8; 16] = [ + 0x10, 0xf1, 0xe7, 0xe4, 0xd1, 0x3b, 0x59, 0x15, + 0x50, 0x0f, 0xdd, 0x1f, 0xa3, 0x20, 0x71, 0xc4, + ]; + + assert_eq!(&block[..16], &expected_start); + } + + #[test] + fn test_encryption_decryption() { + let key: [u8; 32] = [0x42; 32]; + let nonce: [u8; 12] = [0x24; 12]; + let plaintext = b"Hello, ChaCha20!"; + + let cipher = ChaCha20::new(&key, &nonce); + + let mut data = [0u8; 16]; + data.copy_from_slice(plaintext); + + + cipher.encrypt(&mut data); + assert_ne!(&data, plaintext); + + + cipher.decrypt(&mut data); + assert_eq!(&data, plaintext); + } + + #[test] + fn test_rfc8439_encryption() { + + let key: [u8; 32] = [ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, + 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, + 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, + ]; + let nonce: [u8; 12] = [ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4a, + 0x00, 0x00, 0x00, 0x00, + ]; + + let plaintext = b"Ladies and Gentlemen of the class of '99: If I could offer you only one tip for the future, sunscreen would be it."; + + let cipher = ChaCha20::new_with_counter(&key, &nonce, 1); + + let mut data = [0u8; 114]; + data.copy_from_slice(plaintext); + cipher.encrypt(&mut data); + + + let expected_start: [u8; 16] = [ + 0x6e, 0x2e, 0x35, 0x9a, 0x25, 0x68, 0xf9, 0x80, + 0x41, 0xba, 0x07, 0x28, 0xdd, 0x0d, 0x69, 0x81, + ]; + + assert_eq!(&data[..16], &expected_start); + } +} diff --git a/src/crypto/ed25519.rs b/src/crypto/ed25519.rs index 48ce040..f1937e1 100644 --- a/src/crypto/ed25519.rs +++ b/src/crypto/ed25519.rs @@ -1,1001 +1,1125 @@ -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() -} +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() { + + + if i % 4 == 0 { + unsafe { esp_idf_sys::vTaskDelay(1); } + } + + 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() +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_key_generation() { + let private_key: [u8; 32] = [ + 0x9d, 0x61, 0xb1, 0x9d, 0xef, 0xfd, 0x5a, 0x60, + 0xba, 0x84, 0x4a, 0xf4, 0x92, 0xec, 0x2c, 0xc4, + 0x44, 0x49, 0xc5, 0x69, 0x7b, 0x32, 0x69, 0x19, + 0x70, 0x3b, 0xac, 0x03, 0x1c, 0xae, 0x7f, 0x60, + ]; + let expected_public: [u8; 32] = [ + 0xd7, 0x5a, 0x98, 0x01, 0x82, 0xb1, 0x0a, 0xb7, + 0xd5, 0x4b, 0xfe, 0xd3, 0xc9, 0x64, 0x07, 0x3a, + 0x0e, 0xe1, 0x72, 0xf3, 0xda, 0xa6, 0x23, 0x25, + 0xaf, 0x02, 0x1a, 0x68, 0xf7, 0x07, 0x51, 0x1a, + ]; + + let public_key = Ed25519::public_key(&private_key); + assert_eq!(public_key, expected_public); + } + + #[test] + fn test_sign_verify_empty() { + let private_key: [u8; 32] = [ + 0x9d, 0x61, 0xb1, 0x9d, 0xef, 0xfd, 0x5a, 0x60, + 0xba, 0x84, 0x4a, 0xf4, 0x92, 0xec, 0x2c, 0xc4, + 0x44, 0x49, 0xc5, 0x69, 0x7b, 0x32, 0x69, 0x19, + 0x70, 0x3b, 0xac, 0x03, 0x1c, 0xae, 0x7f, 0x60, + ]; + let public_key = Ed25519::public_key(&private_key); + let message = b""; + + let signature = Ed25519::sign(&private_key, message); + assert!(Ed25519::verify(&public_key, message, &signature)); + } + + #[test] + fn test_sign_verify_message() { + let private_key: [u8; 32] = [ + 0xc5, 0xaa, 0x8d, 0xf4, 0x3f, 0x9f, 0x83, 0x7b, + 0xed, 0xb7, 0x44, 0x2f, 0x31, 0xdc, 0xb7, 0xb1, + 0x66, 0xd3, 0x85, 0x35, 0x07, 0x6f, 0x09, 0x4b, + 0x85, 0xce, 0x3a, 0x2e, 0x0b, 0x44, 0x58, 0xf7, + ]; + let public_key = Ed25519::public_key(&private_key); + let message = b"test message"; + + let signature = Ed25519::sign(&private_key, message); + assert!(Ed25519::verify(&public_key, message, &signature)); + + + assert!(!Ed25519::verify(&public_key, b"wrong message", &signature)); + } +} diff --git a/src/crypto/hkdf.rs b/src/crypto/hkdf.rs index 748d4c5..9b100a6 100644 --- a/src/crypto/hkdf.rs +++ b/src/crypto/hkdf.rs @@ -1,149 +1,240 @@ -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 - } -} +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 + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_rfc5869_vector1() { + + let ikm: [u8; 22] = [0x0b; 22]; + let salt: [u8; 13] = [ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0a, 0x0b, 0x0c, + ]; + let info: [u8; 10] = [ + 0xf0, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, + 0xf8, 0xf9, + ]; + + let prk = Hkdf::extract(&salt, &ikm); + + let expected_prk: [u8; 32] = [ + 0x07, 0x77, 0x09, 0x36, 0x2c, 0x2e, 0x32, 0xdf, + 0x0d, 0xdc, 0x3f, 0x0d, 0xc4, 0x7b, 0xba, 0x63, + 0x90, 0xb6, 0xc7, 0x3b, 0xb5, 0x0f, 0x9c, 0x31, + 0x22, 0xec, 0x84, 0x4a, 0xd7, 0xc2, 0xb3, 0xe5, + ]; + + assert_eq!(prk, expected_prk); + + let mut okm = [0u8; 42]; + Hkdf::expand(&prk, &info, &mut okm); + + let expected_okm: [u8; 42] = [ + 0x3c, 0xb2, 0x5f, 0x25, 0xfa, 0xac, 0xd5, 0x7a, + 0x90, 0x43, 0x4f, 0x64, 0xd0, 0x36, 0x2f, 0x2a, + 0x2d, 0x2d, 0x0a, 0x90, 0xcf, 0x1a, 0x5a, 0x4c, + 0x5d, 0xb0, 0x2d, 0x56, 0xec, 0xc4, 0xc5, 0xbf, + 0x34, 0x00, 0x72, 0x08, 0xd5, 0xb8, 0x87, 0x18, + 0x58, 0x65, + ]; + + assert_eq!(okm, expected_okm); + } + + #[test] + fn test_derive_key() { + let key: [u8; 16] = Hkdf::derive_key(b"salt", b"secret", b"info"); + assert_eq!(key.len(), 16); + + + let key2: [u8; 16] = Hkdf::derive_key(b"salt", b"secret", b"info"); + assert_eq!(key, key2); + + + let key3: [u8; 16] = Hkdf::derive_key(b"salt", b"secret", b"other"); + assert_ne!(key, key3); + } + + #[test] + fn test_meshtastic_nonce() { + let nonce = meshtastic::derive_nonce(0x12345678, 0xDEADBEEF); + + assert_eq!(&nonce[..4], &[0x78, 0x56, 0x34, 0x12]); + + assert_eq!(&nonce[4..8], &[0u8; 4]); + + assert_eq!(&nonce[8..12], &[0xEF, 0xBE, 0xAD, 0xDE]); + + assert_eq!(&nonce[12..], &[0u8; 4]); + } +} diff --git a/src/crypto/hmac.rs b/src/crypto/hmac.rs index e1cd21e..62e1406 100644 --- a/src/crypto/hmac.rs +++ b/src/crypto/hmac.rs @@ -1,69 +1,180 @@ -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, - } - } -} +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, + } + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_rfc4231_vector1() { + + let key = [0x0b; 20]; + let data = b"Hi There"; + + let mac = HmacSha256::mac(&key, data); + let expected: [u8; 32] = [ + 0xb0, 0x34, 0x4c, 0x61, 0xd8, 0xdb, 0x38, 0x53, + 0x5c, 0xa8, 0xaf, 0xce, 0xaf, 0x0b, 0xf1, 0x2b, + 0x88, 0x1d, 0xc2, 0x00, 0xc9, 0x83, 0x3d, 0xa7, + 0x26, 0xe9, 0x37, 0x6c, 0x2e, 0x32, 0xcf, 0xf7, + ]; + + assert_eq!(mac, expected); + } + + #[test] + fn test_rfc4231_vector2() { + + let key = b"Jefe"; + let data = b"what do ya want for nothing?"; + + let mac = HmacSha256::mac(key, data); + let expected: [u8; 32] = [ + 0x5b, 0xdc, 0xc1, 0x46, 0xbf, 0x60, 0x75, 0x4e, + 0x6a, 0x04, 0x24, 0x26, 0x08, 0x95, 0x75, 0xc7, + 0x5a, 0x00, 0x3f, 0x08, 0x9d, 0x27, 0x39, 0x83, + 0x9d, 0xec, 0x58, 0xb9, 0x64, 0xec, 0x38, 0x43, + ]; + + assert_eq!(mac, expected); + } + + #[test] + fn test_rfc4231_vector3() { + + let key = [0xaa; 20]; + let data = [0xdd; 50]; + + let mac = HmacSha256::mac(&key, &data); + let expected: [u8; 32] = [ + 0x77, 0x3e, 0xa9, 0x1e, 0x36, 0x80, 0x0e, 0x46, + 0x85, 0x4d, 0xb8, 0xeb, 0xd0, 0x91, 0x81, 0xa7, + 0x29, 0x59, 0x09, 0x8b, 0x3e, 0xf8, 0xc1, 0x22, + 0xd9, 0x63, 0x55, 0x14, 0xce, 0xd5, 0x65, 0xfe, + ]; + + assert_eq!(mac, expected); + } + + #[test] + fn test_long_key() { + + let key = [0xaa; 131]; + let data = b"Test Using Larger Than Block-Size Key - Hash Key First"; + + let mac = HmacSha256::mac(&key, data); + let expected: [u8; 32] = [ + 0x60, 0xe4, 0x31, 0x59, 0x1e, 0xe0, 0xb6, 0x7f, + 0x0d, 0x8a, 0x26, 0xaa, 0xcb, 0xf5, 0xb7, 0x7f, + 0x8e, 0x0b, 0xc6, 0x21, 0x37, 0x28, 0xc5, 0x14, + 0x05, 0x46, 0x04, 0x0f, 0x0e, 0xe3, 0x7f, 0x54, + ]; + + assert_eq!(mac, expected); + } + + #[test] + fn test_verify() { + let key = b"secret"; + let data = b"message"; + let mac = HmacSha256::mac(key, data); + + assert!(HmacSha256::verify(key, data, &mac)); + + + let mut bad_mac = mac; + bad_mac[0] ^= 1; + assert!(!HmacSha256::verify(key, data, &bad_mac)); + } + + #[test] + fn test_incremental() { + let key = b"key"; + + let mac1 = HmacSha256::mac(key, b"hello world"); + + let mut hmac = HmacSha256::new(key); + hmac.update(b"hello "); + hmac.update(b"world"); + let mac2 = hmac.finalize(); + + assert_eq!(mac1, mac2); + } +} diff --git a/src/crypto/mod.rs b/src/crypto/mod.rs index 7a9b328..06ebf4e 100644 --- a/src/crypto/mod.rs +++ b/src/crypto/mod.rs @@ -1,61 +1,144 @@ -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() -} +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() +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_secure_zero() { + let mut data = [0x42u8; 32]; + assert!(data.iter().all(|&b| b == 0x42)); + + secure_zero(&mut data); + assert!(data.iter().all(|&b| b == 0), "Data should be zeroed"); + } + + #[test] + fn test_secure_zero_u32() { + let mut data = [0xDEADBEEFu32; 16]; + assert!(data.iter().all(|&w| w == 0xDEADBEEF)); + + secure_zero_u32(&mut data); + assert!(data.iter().all(|&w| w == 0), "Data should be zeroed"); + } + + #[test] + fn test_secure_zero_empty() { + + let mut empty: [u8; 0] = []; + secure_zero(&mut empty); + + let mut empty_u32: [u32; 0] = []; + secure_zero_u32(&mut empty_u32); + } + + #[test] + fn test_constant_time_eq() { + let a = [0x42u8; 32]; + let b = [0x42u8; 32]; + let c = [0x43u8; 32]; + + assert!(constant_time_eq(&a, &b)); + assert!(!constant_time_eq(&a, &c)); + + + assert!(!constant_time_eq(&a[..16], &b)); + } + + #[test] + fn test_aes128_drop_zeros_keys() { + + let key = [0x42u8; 16]; + let cipher = Aes128::new(&key); + + + let round_keys_ptr = &cipher.round_keys as *const [u8; 176]; + + + drop(cipher); + + + } + + #[test] + fn test_chacha20_drop_zeros_state() { + let key = [0x42u8; 32]; + let nonce = [0x24u8; 12]; + let cipher = ChaCha20::new(&key, &nonce); + + + let mut data = [0u8; 16]; + cipher.encrypt(&mut data); + + + drop(cipher); + } +} diff --git a/src/crypto/poly1305.rs b/src/crypto/poly1305.rs index fea8814..2e68c60 100644 --- a/src/crypto/poly1305.rs +++ b/src/crypto/poly1305.rs @@ -1,318 +1,417 @@ -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 - } -} +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 + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_rfc8439_poly1305() { + + let key: [u8; 32] = [ + 0x85, 0xd6, 0xbe, 0x78, 0x57, 0x55, 0x6d, 0x33, + 0x7f, 0x44, 0x52, 0xfe, 0x42, 0xd5, 0x06, 0xa8, + 0x01, 0x03, 0x80, 0x8a, 0xfb, 0x0d, 0xb2, 0xfd, + 0x4a, 0xbf, 0xf6, 0xaf, 0x41, 0x49, 0xf5, 0x1b, + ]; + + let msg = b"Cryptographic Forum Research Group"; + + let tag = Poly1305::mac(&key, msg); + + let expected: [u8; 16] = [ + 0xa8, 0x06, 0x1d, 0xc1, 0x30, 0x51, 0x36, 0xc6, + 0xc2, 0x2b, 0x8b, 0xaf, 0x0c, 0x01, 0x27, 0xa9, + ]; + + assert_eq!(tag, expected); + } + + #[test] + fn test_chacha20_poly1305_aead() { + + let key: [u8; 32] = [ + 0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, + 0x88, 0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f, + 0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, + 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, + ]; + + let nonce: [u8; 12] = [ + 0x07, 0x00, 0x00, 0x00, 0x40, 0x41, 0x42, 0x43, + 0x44, 0x45, 0x46, 0x47, + ]; + + let aad: [u8; 12] = [ + 0x50, 0x51, 0x52, 0x53, 0xc0, 0xc1, 0xc2, 0xc3, + 0xc4, 0xc5, 0xc6, 0xc7, + ]; + + let plaintext = b"Ladies and Gentlemen of the class of '99: If I could offer you only one tip for the future, sunscreen would be it."; + + let mut ciphertext = [0u8; 114]; + let mut tag = [0u8; 16]; + + ChaCha20Poly1305::seal(&key, &nonce, &aad, plaintext, &mut ciphertext, &mut tag); + + + let mut decrypted = [0u8; 114]; + assert!(ChaCha20Poly1305::open(&key, &nonce, &aad, &ciphertext, &tag, &mut decrypted)); + assert_eq!(&decrypted[..], &plaintext[..]); + + + ciphertext[0] ^= 1; + assert!(!ChaCha20Poly1305::open(&key, &nonce, &aad, &ciphertext, &tag, &mut decrypted)); + } +} diff --git a/src/crypto/sha256.rs b/src/crypto/sha256.rs index e859a92..40f5f0f 100644 --- a/src/crypto/sha256.rs +++ b/src/crypto/sha256.rs @@ -1,198 +1,293 @@ -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, - } - } -} +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, + } + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_empty() { + let hash = Sha256::hash(b""); + let expected: [u8; 32] = [ + 0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14, + 0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24, + 0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c, + 0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55, + ]; + assert_eq!(hash, expected); + } + + #[test] + fn test_abc() { + let hash = Sha256::hash(b"abc"); + let expected: [u8; 32] = [ + 0xba, 0x78, 0x16, 0xbf, 0x8f, 0x01, 0xcf, 0xea, + 0x41, 0x41, 0x40, 0xde, 0x5d, 0xae, 0x22, 0x23, + 0xb0, 0x03, 0x61, 0xa3, 0x96, 0x17, 0x7a, 0x9c, + 0xb4, 0x10, 0xff, 0x61, 0xf2, 0x00, 0x15, 0xad, + ]; + assert_eq!(hash, expected); + } + + #[test] + fn test_long() { + + let input = b"abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"; + let hash = Sha256::hash(input); + let expected: [u8; 32] = [ + 0x24, 0x8d, 0x6a, 0x61, 0xd2, 0x06, 0x38, 0xb8, + 0xe5, 0xc0, 0x26, 0x93, 0x0c, 0x3e, 0x60, 0x39, + 0xa3, 0x3c, 0xe4, 0x59, 0x64, 0xff, 0x21, 0x67, + 0xf6, 0xec, 0xed, 0xd4, 0x19, 0xdb, 0x06, 0xc1, + ]; + assert_eq!(hash, expected); + } + + #[test] + fn test_incremental() { + let mut hasher = Sha256::new(); + hasher.update(b"abc"); + hasher.update(b"def"); + let hash1 = hasher.finalize(); + + let hash2 = Sha256::hash(b"abcdef"); + + assert_eq!(hash1, hash2); + } + + #[test] + fn test_one_million_a() { + + let mut hasher = Sha256::new(); + for _ in 0..10000 { + hasher.update(&[b'a'; 100]); + } + let hash = hasher.finalize(); + let expected: [u8; 32] = [ + 0xcd, 0xc7, 0x6e, 0x5c, 0x99, 0x14, 0xfb, 0x92, + 0x81, 0xa1, 0xc7, 0xe2, 0x84, 0xd7, 0x3e, 0x67, + 0xf1, 0x80, 0x9a, 0x48, 0xa4, 0x97, 0x20, 0x0e, + 0x04, 0x6d, 0x39, 0xcc, 0xc7, 0x11, 0x2c, 0xd0, + ]; + assert_eq!(hash, expected); + } +} diff --git a/src/crypto/x25519.rs b/src/crypto/x25519.rs index f676194..c6cd02d 100644 --- a/src/crypto/x25519.rs +++ b/src/crypto/x25519.rs @@ -1,395 +1,517 @@ -#[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) -} +#[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) +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_rfc7748_vector1() { + + let scalar: [u8; 32] = [ + 0xa5, 0x46, 0xe3, 0x6b, 0xf0, 0x52, 0x7c, 0x9d, + 0x3b, 0x16, 0x15, 0x4b, 0x82, 0x46, 0x5e, 0xdd, + 0x62, 0x14, 0x4c, 0x0a, 0xc1, 0xfc, 0x5a, 0x18, + 0x50, 0x6a, 0x22, 0x44, 0xba, 0x44, 0x9a, 0xc4, + ]; + let u: [u8; 32] = [ + 0xe6, 0xdb, 0x68, 0x67, 0x58, 0x30, 0x30, 0xdb, + 0x35, 0x94, 0xc1, 0xa4, 0x24, 0xb1, 0x5f, 0x7c, + 0x72, 0x66, 0x24, 0xec, 0x26, 0xb3, 0x35, 0x3b, + 0x10, 0xa9, 0x03, 0xa6, 0xd0, 0xab, 0x1c, 0x4c, + ]; + let expected: [u8; 32] = [ + 0xc3, 0xda, 0x55, 0x37, 0x9d, 0xe9, 0xc6, 0x90, + 0x8e, 0x94, 0xea, 0x4d, 0xf2, 0x8d, 0x08, 0x4f, + 0x32, 0xec, 0xcf, 0x03, 0x49, 0x1c, 0x71, 0xf7, + 0x54, 0xb4, 0x07, 0x55, 0x77, 0xa2, 0x85, 0x52, + ]; + + let result = x25519(&scalar, &u); + assert_eq!(result, expected); + } + + #[test] + fn test_rfc7748_vector2() { + let scalar: [u8; 32] = [ + 0x4b, 0x66, 0xe9, 0xd4, 0xd1, 0xb4, 0x67, 0x3c, + 0x5a, 0xd2, 0x26, 0x91, 0x95, 0x7d, 0x6a, 0xf5, + 0xc1, 0x1b, 0x64, 0x21, 0xe0, 0xea, 0x01, 0xd4, + 0x2c, 0xa4, 0x16, 0x9e, 0x79, 0x18, 0xba, 0x0d, + ]; + let u: [u8; 32] = [ + 0xe5, 0x21, 0x0f, 0x12, 0x78, 0x68, 0x11, 0xd3, + 0xf4, 0xb7, 0x95, 0x9d, 0x05, 0x38, 0xae, 0x2c, + 0x31, 0xdb, 0xe7, 0x10, 0x6f, 0xc0, 0x3c, 0x3e, + 0xfc, 0x4c, 0xd5, 0x49, 0xc7, 0x15, 0xa4, 0x93, + ]; + let expected: [u8; 32] = [ + 0x95, 0xcb, 0xde, 0x94, 0x76, 0xe8, 0x90, 0x7d, + 0x7a, 0xad, 0xe4, 0x5c, 0xb4, 0xb8, 0x73, 0xf8, + 0x8b, 0x59, 0x5a, 0x68, 0x79, 0x9f, 0xa1, 0x52, + 0xe6, 0xf8, 0xf7, 0x64, 0x7a, 0xac, 0x79, 0x57, + ]; + + let result = x25519(&scalar, &u); + assert_eq!(result, expected); + } + + #[test] + fn test_basepoint() { + + let scalar: [u8; 32] = [ + 0x77, 0x07, 0x6d, 0x0a, 0x73, 0x18, 0xa5, 0x7d, + 0x3c, 0x16, 0xc1, 0x72, 0x51, 0xb2, 0x66, 0x45, + 0xdf, 0x4c, 0x2f, 0x87, 0xeb, 0xc0, 0x99, 0x2a, + 0xb1, 0x77, 0xfb, 0xa5, 0x1d, 0xb9, 0x2c, 0x2a, + ]; + let expected: [u8; 32] = [ + 0x85, 0x20, 0xf0, 0x09, 0x89, 0x30, 0xa7, 0x54, + 0x74, 0x8b, 0x7d, 0xdc, 0xb4, 0x3e, 0xf7, 0x5a, + 0x0d, 0xbf, 0x3a, 0x0d, 0x26, 0x38, 0x1a, 0xf4, + 0xeb, 0xa4, 0xa9, 0x8e, 0xaa, 0x9b, 0x4e, 0x6a, + ]; + + let result = x25519_base(&scalar); + assert_eq!(result, expected); + } + + #[test] + fn test_key_exchange() { + + let alice_private: [u8; 32] = [ + 0x77, 0x07, 0x6d, 0x0a, 0x73, 0x18, 0xa5, 0x7d, + 0x3c, 0x16, 0xc1, 0x72, 0x51, 0xb2, 0x66, 0x45, + 0xdf, 0x4c, 0x2f, 0x87, 0xeb, 0xc0, 0x99, 0x2a, + 0xb1, 0x77, 0xfb, 0xa5, 0x1d, 0xb9, 0x2c, 0x2a, + ]; + let bob_private: [u8; 32] = [ + 0x5d, 0xab, 0x08, 0x7e, 0x62, 0x4a, 0x8a, 0x4b, + 0x79, 0xe1, 0x7f, 0x8b, 0x83, 0x80, 0x0e, 0xe6, + 0x6f, 0x3b, 0xb1, 0x29, 0x26, 0x18, 0xb6, 0xfd, + 0x1c, 0x2f, 0x8b, 0x27, 0xff, 0x88, 0xe0, 0xeb, + ]; + + let alice_public = x25519_base(&alice_private); + let bob_public = x25519_base(&bob_private); + + let alice_shared = x25519(&alice_private, &bob_public); + let bob_shared = x25519(&bob_private, &alice_public); + + assert_eq!(alice_shared, bob_shared); + } +} diff --git a/src/display.rs b/src/display.rs index e2ca860..81c9a94 100644 --- a/src/display.rs +++ b/src/display.rs @@ -1,788 +1,955 @@ -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() - } -} +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, + + pub irq_status: u16, + + pub last_irq: u16, + + pub dio1_count: u32, + + pub chip_mode: u8, + + pub device_errors: u16, + + pub repeater_active: bool, + + pub relay_count: u32, +} + +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 if status.repeater_active { + + self.display.draw_text(48, 44, "RELAY:"); + let count_str = format_u32(status.relay_count); + self.display.draw_text(84, 44, &count_str); + } else { + self.display.draw_text(48, 44, "WAITING"); + } + + + self.display.draw_hline(0, 46, DISPLAY_WIDTH, true); + + + self.display.draw_text(0, 48, "M:"); + let mode_str = chip_mode_str(status.chip_mode); + self.display.draw_text(12, 48, mode_str); + self.display.draw_text(48, 48, "E:"); + let err_str = format_hex16(status.device_errors); + self.display.draw_text(60, 48, &err_str); + self.display.draw_text(96, 48, "D1:"); + let d1_str = format_u32(status.dio1_count); + self.display.draw_text(114, 48, &d1_str); + + + self.display.draw_text(0, 56, "IRQ:"); + let irq_str = format_hex16(status.irq_status); + self.display.draw_text(24, 56, &irq_str); + self.display.draw_text(60, 56, "LAST:"); + let last_str = format_hex16(status.last_irq); + self.display.draw_text(90, 56, &last_str); + + 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_hex16(val: u16) -> String<4> { + const HEX: &[u8] = b"0123456789ABCDEF"; + let mut s = String::new(); + for i in (0..4).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 +} + + +fn chip_mode_str(mode: u8) -> &'static str { + match mode { + 2 => "STBY_RC", + 3 => "STBY_X", + 4 => "FS", + 5 => "RX", + 6 => "TX", + _ => "???", + } +} + + +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_format_battery() { + assert_eq!(&format_battery(0)[..], "0%"); + assert_eq!(&format_battery(5)[..], "5%"); + assert_eq!(&format_battery(50)[..], "50%"); + assert_eq!(&format_battery(100)[..], "100%"); + } + + #[test] + fn test_format_hex32() { + assert_eq!(&format_hex32(0)[..], "00000000"); + assert_eq!(&format_hex32(0xDEADBEEF)[..], "DEADBEEF"); + assert_eq!(&format_hex32(0x12345678)[..], "12345678"); + } + + #[test] + fn test_format_u32() { + assert_eq!(&format_u32(0)[..], "0"); + assert_eq!(&format_u32(123)[..], "123"); + assert_eq!(&format_u32(999999)[..], "999999"); + } + + #[test] + fn test_format_i16() { + assert_eq!(&format_i16(0)[..], "0"); + assert_eq!(&format_i16(100)[..], "100"); + assert_eq!(&format_i16(-50)[..], "-50"); + assert_eq!(&format_i16(-128)[..], "-128"); + } +} diff --git a/src/identity.rs b/src/identity.rs index dfb80e1..d85ef03 100644 --- a/src/identity.rs +++ b/src/identity.rs @@ -1,451 +1,587 @@ -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()) - } -} +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()) + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_identity_generation() { + let seed = [0x42u8; 32]; + let identity = Identity::from_seed(&seed); + + + let node_id = identity.node_id(); + assert_ne!(node_id, 0); + + + let short = identity.short_id(); + for &c in &short { + assert!(c.is_ascii_hexdigit()); + } + } + + #[test] + fn test_identity_deterministic() { + let seed = [0x42u8; 32]; + let id1 = Identity::from_seed(&seed); + let id2 = Identity::from_seed(&seed); + + assert_eq!(id1.public_key(), id2.public_key()); + assert_eq!(id1.node_id(), id2.node_id()); + } + + #[test] + fn test_sign_verify() { + let seed = [0x42u8; 32]; + let identity = Identity::from_seed(&seed); + + let message = b"Hello, cypherpunk!"; + let signature = identity.sign(message); + + assert!(Identity::verify(identity.public_key(), message, &signature)); + assert!(!Identity::verify(identity.public_key(), b"wrong", &signature)); + } + + #[test] + fn test_key_agreement() { + let seed_alice = [0x41u8; 32]; + let seed_bob = [0x42u8; 32]; + + let alice = Identity::from_seed(&seed_alice); + let bob = Identity::from_seed(&seed_bob); + + let alice_shared = alice.key_agree(bob.encryption_public_key()); + let bob_shared = bob.key_agree(alice.encryption_public_key()); + + assert_eq!(alice_shared, bob_shared); + } + + #[test] + fn test_storage_encryption() { + let seed = [0x42u8; 32]; + let storage_key = [0x55u8; 32]; + + let identity = Identity::from_seed(&seed); + let encrypted = identity.encrypt_for_storage(&storage_key); + + + let recovered = Identity::decrypt_from_storage(&encrypted, &storage_key); + assert!(recovered.is_some()); + assert_eq!(recovered.unwrap().public_key(), identity.public_key()); + + + let wrong_key = [0xAAu8; 32]; + let failed = Identity::decrypt_from_storage(&encrypted, &wrong_key); + assert!(failed.is_none()); + } + + #[test] + fn test_different_seeds_different_ids() { + let id1 = Identity::from_seed(&[0x01u8; 32]); + let id2 = Identity::from_seed(&[0x02u8; 32]); + + assert_ne!(id1.public_key(), id2.public_key()); + assert_ne!(id1.node_id(), id2.node_id()); + } +} diff --git a/src/main.rs b/src/main.rs index aaa767f..c492914 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,1662 +1,2299 @@ -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); - } -} +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, AtomicU16, 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 DIO1_COUNT: AtomicU32 = AtomicU32::new(0); + + +static DIO1_IRQ_SEEN: AtomicU16 = AtomicU16::new(0); + + +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 + } +} + + +fn load_repeater_setting() -> bool { + 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_READONLY, + &mut handle, + ); + if err != esp_idf_sys::ESP_OK { + return true; + } + + let key = core::ffi::CStr::from_bytes_with_nul(b"repeater\0").unwrap(); + let mut val: u8 = 1; + let result = esp_idf_sys::nvs_get_u8(handle, key.as_ptr(), &mut val); + esp_idf_sys::nvs_close(handle); + + if result == esp_idf_sys::ESP_OK { + val != 0 + } else { + true + } + } +} + + +fn save_repeater_setting(enabled: bool) { + 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 { + return; + } + + let key = core::ffi::CStr::from_bytes_with_nul(b"repeater\0").unwrap(); + esp_idf_sys::nvs_set_u8(handle, key.as_ptr(), if enabled { 1 } else { 0 }); + esp_idf_sys::nvs_commit(handle); + esp_idf_sys::nvs_close(handle); + } +} + + +const DEDUP_RING_SIZE: usize = 32; + +struct DeduplicatorRing { + hashes: [u32; DEDUP_RING_SIZE], + head: usize, + count: usize, +} + +impl DeduplicatorRing { + fn new() -> Self { + Self { + hashes: [0u32; DEDUP_RING_SIZE], + head: 0, + count: 0, + } + } + + + fn check_and_insert(&mut self, data: &[u8]) -> bool { + let hash = Self::fingerprint(data); + + + let entries = self.count.min(DEDUP_RING_SIZE); + for i in 0..entries { + if self.hashes[i] == hash { + return false; + } + } + + + self.hashes[self.head] = hash; + self.head = (self.head + 1) % DEDUP_RING_SIZE; + if self.count < DEDUP_RING_SIZE { + self.count += 1; + } + + true + } + + + fn clear(&mut self) { + self.hashes = [0u32; DEDUP_RING_SIZE]; + self.head = 0; + self.count = 0; + } + + + fn fingerprint(data: &[u8]) -> u32 { + let mid = data.len() / 2; + let hi = protocol::crc16(&data[..mid]) as u32; + let lo = protocol::crc16(&data[mid..]) as u32; + (hi << 16) | lo + } +} + + +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, + + last_serial_rx: u32, + + prev_ble_connections: u16, + + + repeater_enabled: bool, + + dedup: DeduplicatorRing, + + relay_count: u32, + + + 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(), + last_serial_rx: 0, + prev_ble_connections: 0, + + repeater_enabled: true, + dedup: DeduplicatorRing::new(), + relay_count: 0, + + 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 app_connected(&self) -> bool { + self.serial_protocol != Protocol::Unknown || self.ble.connection_count() > 0 + } + + + fn repeater_active(&self) -> bool { + self.repeater_enabled && !self.app_connected() + } + + + fn reset_protocol(&mut self) { + let prev = self.serial_protocol; + if prev == Protocol::Unknown { + return; + } + + self.serial_protocol = Protocol::Unknown; + + + self.router.transport(TransportType::UsbSerial).detector.reset(); + + + self.meshcore_parser.reset(); + self.meshtastic.reset_parser(); + self.rnode.reset_parser(); + + + self.at_buffer.clear(); + self.dedup.clear(); + + self.stats.protocol_switches += 1; + log::info!("Protocol reset: {} -> Detecting", prev.name()); + } + + + fn maybe_relay_packet(&mut self, data: &[u8], now: u32) -> bool { + if !self.repeater_active() { + return false; + } + + + if data.len() >= 4 && &data[..4] == b"TEST" { + return false; + } + + + if data.len() < 4 { + return false; + } + + + if !self.dedup.check_and_insert(data) { + return false; + } + + + let jitter = 20 + ((self.identity.node_id ^ now) % 81); + let mut waited = 0u32; + while waited < jitter { + FreeRtos::delay_ms(1); + waited += 1; + } + + + self.rx_active = false; + if self.radio.transmit(data).is_ok() { + self.relay_count += 1; + self.stats.tx_packets += 1; + self.rx_active = true; + log::info!("Relayed packet ({} bytes), total relays: {}", data.len(), self.relay_count); + true + } else { + + let _ = self.radio.start_rx(0); + self.rx_active = true; + false + } + } + + + fn handle_dio1_interrupt(&mut self) { + + DIO1_TRIGGERED.store(false, Ordering::Relaxed); + + + DIO1_COUNT.fetch_add(1, Ordering::Relaxed); + + + if let Ok(irq_status) = self.radio.get_irq_status() { + + DIO1_IRQ_SEEN.store(irq_status, Ordering::Relaxed); + 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, uart: &UartDriver) { + + 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) { + + match self.radio.read_packet() { + Ok((data, rssi, snr)) => { + self.stats.rx_packets += 1; + self.stats.last_rssi = rssi; + self.route_rx_packet(&data, rssi, snr, uart); + let now = millis(); + if self.maybe_relay_packet(&data, now) { + self.led.flash(3); + } else { + self.led.flash(2); + } + } + Err(_) => { + self.stats.rx_errors += 1; + } + } + + let _ = self.radio.start_rx(0); + } + } + + + 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.starts_with(b"AT+SWITCH") { + + self.reset_protocol(); + let _ = uart.write(b"Protocol reset: Detecting...\r\n"); + let _ = uart.write(b"OK\r\n"); + } else if cmd.starts_with(b"AT+REPEATER=") { + + if cmd.len() > 12 { + match cmd[12] { + b'1' => { + self.repeater_enabled = true; + save_repeater_setting(true); + let _ = uart.write(b"Repeater: ON\r\n"); + let _ = uart.write(b"OK\r\n"); + } + b'0' => { + self.repeater_enabled = false; + save_repeater_setting(false); + self.dedup.clear(); + let _ = uart.write(b"Repeater: OFF\r\n"); + let _ = uart.write(b"OK\r\n"); + } + _ => { + let _ = uart.write(b"ERROR: Use 0 or 1\r\n"); + } + } + } else { + let _ = uart.write(b"ERROR: Use AT+REPEATER=0 or AT+REPEATER=1\r\n"); + } + } else if cmd.starts_with(b"AT+REPEATER") { + + let _ = uart.write(b"Repeater: "); + if self.repeater_enabled { + let _ = uart.write(b"ON"); + } else { + let _ = uart.write(b"OFF"); + } + let _ = uart.write(b"\r\nRelayed: "); + write_u32(uart, self.relay_count); + let _ = uart.write(b"\r\nOK\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" AT+SWITCH - Reset protocol detection\r\n"); + let _ = uart.write(b" AT+REPEATER - Repeater status\r\n"); + let _ = uart.write(b" AT+REPEATER=n - Enable(1)/disable(0)\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) { + self.last_serial_rx = millis(); + + + 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 protocol { + Protocol::MeshCore => { + + + self.meshcore_parser.feed(0xAA); + } + Protocol::Meshtastic => { + + + self.meshtastic.feed_serial(0x94); + } + Protocol::RNode => { + + + } + Protocol::AtCommand => { + + + let _ = self.at_buffer.push(b'A'); + } + Protocol::Unknown => {} + } + } + } + + + 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: 910_525_000, + spreading_factor: 7, + bandwidth: 0x03, + coding_rate: 1, + tx_power: 14, + sync_word: 0x12, + preamble_length: 16, + crc_enabled: true, + implicit_header: false, + ldro: false, + }, + Protocol::Meshtastic => RadioConfig { + frequency: 906_875_000, + spreading_factor: 11, + bandwidth: 0x04, + 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 => 0x04, + 250_000 => 0x05, + 500_000 => 0x06, + 62_500 => 0x03, + _ => 0x04, + }, + 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); + } + + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + } + } + } + + 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; + + if self.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + } + } + } + + 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.radio.start_rx(0).is_ok() { + self.rx_active = true; + } + } + } + } + } + + + 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); + + let _ = self.radio.start_rx(0); + } + 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; + + let _ = self.radio.start_rx(0); + } + } + } + + + 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); + } + + _ => { + + + if let Some(frame) = protocol::build_receive(rssi, snr, data) { + self.send_frame(uart, &frame); + } + } + } + } + + + 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_COUNT.fetch_add(1, Ordering::Relaxed); + DIO1_TRIGGERED.store(true, Ordering::Release); +} + + +fn timer_tick_isr() { + SYSTEM_TICKS.fetch_add(1, Ordering::Relaxed); +} + + +fn millis() -> u32 { + + + unsafe { esp_idf_sys::xTaskGetTickCount() as u32 } +} + + +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 + } +} + + +unsafe extern "C" fn null_vprintf( + _fmt: *const core::ffi::c_char, + _args: esp_idf_sys::va_list, +) -> core::ffi::c_int { + 0 +} + +fn main() -> ! { + + esp_idf_sys::link_patches(); + + + unsafe { + esp_idf_sys::esp_log_level_set(b"*\0".as_ptr() as *const _, esp_idf_sys::esp_log_level_t_ESP_LOG_NONE); + } + + + esp_idf_svc::log::EspLogger::initialize_default(); + + + 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(); + dio1.set_interrupt_type(esp_idf_hal::gpio::InterruptType::PosEdge).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); + lunarcore.repeater_enabled = load_repeater_setting(); + log::info!("[INIT] LunarCore OK (repeater: {})", if lunarcore.repeater_enabled { "ON" } else { "OFF" }); + + + 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 meshcore_config = RadioConfig { + frequency: 910_525_000, + spreading_factor: 7, + bandwidth: 0x03, + coding_rate: 1, + tx_power: 14, + sync_word: 0x12, + preamble_length: 16, + crc_enabled: true, + implicit_header: false, + ldro: false, + }; + if lunarcore.radio.configure(&meshcore_config).is_ok() { + log::info!("[INIT] Radio configured for MeshCore (910.525MHz, SF7, 62.5kHz, private sync)"); + } + + + if lunarcore.radio.start_rx(0).is_ok() { + lunarcore.rx_active = true; + log::info!("[INIT] RX mode started - listening for LoRa packets"); + } + + + let mut last_second = 0u32; + let mut battery_check_interval = 0u32; + let mut last_beacon_time = 0u32; + + let beacon_interval = 3000 + (lunarcore.identity.public_key[0] as u32 % 2000); + + let mut beacon_data = [0u8; 8]; + beacon_data[0..4].copy_from_slice(b"TEST"); + beacon_data[4..8].copy_from_slice(&lunarcore.identity.public_key[0..4]); + + + let mut current_irq: u16 = 0; + let mut last_nonzero_irq: u16 = 0; + + loop { + let now = millis(); + + + if !lunarcore.app_connected() && now.wrapping_sub(last_beacon_time) >= beacon_interval { + last_beacon_time = now; + + lunarcore.rx_active = false; + if lunarcore.radio.transmit(&beacon_data).is_ok() { + lunarcore.stats.tx_packets += 1; + + lunarcore.rx_active = true; + } + } + + + feed_watchdog(); + + + lunarcore.process_radio_events(&uart); + + + 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); + } + + + if let Ok(irq) = lunarcore.radio.get_irq_status() { + current_irq = irq; + if irq != 0 { + last_nonzero_irq = irq; + } + + + if irq != 0 { + + if irq & 0x40 != 0 { + lunarcore.stats.rx_errors += 1; + } + + + if (irq & 0x02 != 0) && (irq & 0x40 == 0) { + match lunarcore.radio.read_packet() { + Ok((data, rssi, snr)) => { + lunarcore.stats.rx_packets += 1; + lunarcore.stats.last_rssi = rssi; + lunarcore.route_rx_packet(&data, rssi, snr, &uart); + + if lunarcore.maybe_relay_packet(&data, now) { + lunarcore.led.flash(3); + } else { + lunarcore.led.flash(2); + } + } + Err(_) => { + lunarcore.stats.rx_errors += 1; + } + } + } + + + let _ = lunarcore.radio.clear_irq(irq); + + + if (irq & 0x01 != 0) || (irq & 0x02 != 0) { + let _ = lunarcore.radio.start_rx(0); + lunarcore.rx_active = true; + } + } + } + + + 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 ble_now = lunarcore.ble.connection_count(); + + + if lunarcore.serial_protocol != Protocol::Unknown + && ble_now == 0 + && lunarcore.last_serial_rx > 0 + && now.wrapping_sub(lunarcore.last_serial_rx) > 30_000 + { + log::info!("Serial idle 30s, resetting protocol"); + lunarcore.reset_protocol(); + } + + + if lunarcore.prev_ble_connections > 0 && ble_now == 0 + && (lunarcore.last_serial_rx == 0 + || now.wrapping_sub(lunarcore.last_serial_rx) > 5_000) + { + log::info!("BLE disconnected, resetting protocol"); + lunarcore.reset_protocol(); + } + + lunarcore.prev_ble_connections = ble_now; + } + + + let current_second = now / 1000; + if current_second > last_second { + last_second = current_second; + lunarcore.stats.uptime_seconds = current_second; + + + if current_second % 2 == 0 { + let protocol_name = match lunarcore.serial_protocol { + Protocol::MeshCore => "MeshCore", + Protocol::Meshtastic => "Meshtastic", + Protocol::RNode => "RNode/KISS", + Protocol::AtCommand => "AT Command", + Protocol::Unknown => "Detecting...", + }; + + + let (chip_mode, _cmd_status) = lunarcore.radio.get_status().unwrap_or((0, 0)); + let device_errors = lunarcore.radio.get_errors().unwrap_or(0); + + let status = display::StatusContent { + protocol: protocol_name, + node_id: lunarcore.identity.node_id, + battery_pct: lunarcore.battery.percentage, + rx_count: lunarcore.stats.rx_packets, + tx_count: lunarcore.stats.tx_packets, + rssi: lunarcore.stats.last_rssi, + connected: lunarcore.serial_protocol != Protocol::Unknown, + irq_status: current_irq, + last_irq: last_nonzero_irq, + dio1_count: DIO1_COUNT.load(Ordering::Relaxed), + chip_mode, + device_errors, + repeater_active: lunarcore.repeater_active(), + relay_count: lunarcore.relay_count, + }; + let _ = status_display.show_status(&status); + } + + + 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/main_minimal.rs b/src/main_minimal.rs new file mode 100644 index 0000000..b6f12a7 --- /dev/null +++ b/src/main_minimal.rs @@ -0,0 +1,47 @@ +mod rng; +mod crypto; +mod protocol; +mod protocol_router; +mod sx1262; +mod display; +mod meshtastic; +mod rnode; +mod session; +mod onion; +mod transport; +mod ble; + +use esp_idf_hal::delay::FreeRtos; +use esp_idf_sys as _; + +fn main() -> ! { + + esp_idf_sys::link_patches(); + + + esp_idf_svc::log::EspLogger::initialize_default(); + + log::info!("=== Incremental Test: crypto + protocol + sx1262 ==="); + + + let test_data = b"hello"; + let hash = crypto::sha256::Sha256::hash(test_data); + log::info!("SHA256 test: {:02x}{:02x}{:02x}{:02x}...", hash[0], hash[1], hash[2], hash[3]); + + + let mut parser = protocol::FrameParser::new(); + log::info!("Protocol parser created"); + + + let router = protocol_router::ProtocolRouter::new(); + log::info!("Protocol router created"); + + log::info!("All tested modules loaded successfully"); + + let mut counter = 0u32; + loop { + log::info!("Loop iteration: {}", counter); + counter = counter.wrapping_add(1); + FreeRtos::delay_ms(1000); + } +} diff --git a/src/meshtastic/channel.rs b/src/meshtastic/channel.rs index 34dfafc..d3d40a6 100644 --- a/src/meshtastic/channel.rs +++ b/src/meshtastic/channel.rs @@ -1,553 +1,706 @@ -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 -} +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 +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_channel_creation() { + let ch = Channel::new(0); + assert_eq!(ch.index, 0); + assert!(ch.key.is_encrypted()); + } + + #[test] + fn test_channel_name_key_derivation() { + let mut ch = Channel::new(0); + ch.set_name("MyChannel"); + + + match &ch.key { + ChannelKey::Aes256(k) => { + + let expected = Sha256::hash(b"MyChannel"); + assert_eq!(k, &expected); + } + _ => panic!("Expected AES-256 key"), + } + } + + #[test] + fn test_encrypt_decrypt_roundtrip() { + let ch = Channel::primary(); + let plaintext = b"Hello, Meshtastic!"; + let packet_id = 0x12345678; + let sender = 0xDEADBEEF; + + let ciphertext = ch.encrypt(packet_id, sender, plaintext).unwrap(); + assert_ne!(&ciphertext[..], plaintext); + + let decrypted = ch.decrypt(packet_id, sender, &ciphertext).unwrap(); + assert_eq!(&decrypted[..], plaintext); + } + + #[test] + fn test_channel_hash() { + let mut ch = Channel::new(0); + ch.set_key(&[0x01, 0x02, 0x03, 0x04]); + + + assert_eq!(ch.hash(), 0x04); + } + + #[test] + fn test_channel_set() { + let mut set = ChannelSet::new(); + assert_eq!(set.count(), 1); + + let mut secondary = Channel::new(1); + secondary.set_name("Secondary"); + set.set(1, secondary); + + assert_eq!(set.count(), 2); + assert_eq!(set.get(1).unwrap().name_str(), "Secondary"); + } + + #[test] + fn test_modem_preset_params() { + let params = ModemPreset::LongFast.lora_params(); + assert_eq!(params.spreading_factor, 11); + assert_eq!(params.bandwidth, 125_000); + assert_eq!(params.coding_rate, 8); + } + + #[test] + fn test_base64_encode() { + let data = b"Hello"; + let mut output = [0u8; 16]; + let len = base64_encode(data, &mut output); + assert_eq!(&output[..len], b"SGVsbG8"); + } + + #[test] + fn test_base64_decode() { + let data = b"SGVsbG8"; + let mut output = [0u8; 16]; + let len = base64_decode(data, &mut output); + assert_eq!(&output[..len], b"Hello"); + } + + #[test] + fn test_base64_roundtrip() { + let original = [0xDE, 0xAD, 0xBE, 0xEF, 0x12, 0x34]; + let mut encoded = [0u8; 16]; + let enc_len = base64_encode(&original, &mut encoded); + + let mut decoded = [0u8; 16]; + let dec_len = base64_decode(&encoded[..enc_len], &mut decoded); + + assert_eq!(&decoded[..dec_len], &original); + } + + #[test] + fn test_airtime_calculation() { + + let airtime = ModemPreset::LongFast.airtime_ms(50); + assert!(airtime > 100); + assert!(airtime < 2000); + } +} diff --git a/src/meshtastic/encryption.rs b/src/meshtastic/encryption.rs index a13cc0b..a0dc9ab 100644 --- a/src/meshtastic/encryption.rs +++ b/src/meshtastic/encryption.rs @@ -1,352 +1,569 @@ -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() - } -} +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_encryption_roundtrip() { + let ctx = EncryptionContext::with_default_key(); + let plaintext = b"Hello, Meshtastic!"; + let packet_id = 0x12345678; + let sender = 0xDEADBEEF; + + let ciphertext = ctx.encrypt(packet_id, sender, plaintext).unwrap(); + assert_ne!(&ciphertext[..], plaintext); + + let decrypted = ctx.decrypt(packet_id, sender, &ciphertext).unwrap(); + assert_eq!(&decrypted[..], plaintext); + } + + #[test] + fn test_encryption_with_channel_name() { + let ctx = EncryptionContext::from_channel_name("TestChannel"); + let plaintext = b"Encrypted with channel name key"; + let packet_id = 0x11111111; + let sender = 0x22222222; + + let ciphertext = ctx.encrypt(packet_id, sender, plaintext).unwrap(); + let decrypted = ctx.decrypt(packet_id, sender, &ciphertext).unwrap(); + assert_eq!(&decrypted[..], plaintext); + } + + #[test] + fn test_different_nonces_different_ciphertext() { + let ctx = EncryptionContext::with_default_key(); + let plaintext = b"Same plaintext"; + + let ct1 = ctx.encrypt(1, 1, plaintext).unwrap(); + let ct2 = ctx.encrypt(2, 1, plaintext).unwrap(); + let ct3 = ctx.encrypt(1, 2, plaintext).unwrap(); + + + assert_ne!(&ct1[..], &ct2[..]); + assert_ne!(&ct1[..], &ct3[..]); + assert_ne!(&ct2[..], &ct3[..]); + } + + #[test] + fn test_mic_computation() { + let data = b"Test data for MIC"; + let mic = compute_mic(data); + assert_eq!(mic.len(), MIC_SIZE); + + + assert!(verify_mic(data, &mic)); + + + assert!(!verify_mic(data, &[0, 0, 0, 0])); + } + + #[test] + fn test_enhanced_mic_computation() { + let key = b"test channel key for HMAC"; + let data = b"Test data for enhanced MIC"; + + let mic = compute_mic_enhanced(key, data); + assert_eq!(mic.len(), MIC_SIZE_ENHANCED); + + + assert!(verify_mic_enhanced(key, data, &mic)); + + + let other_key = b"different key for testing"; + let other_mic = compute_mic_enhanced(other_key, data); + assert_ne!(mic, other_mic); + assert!(!verify_mic_enhanced(other_key, data, &mic)); + + + let other_data = b"Different test data"; + let other_mic2 = compute_mic_enhanced(key, other_data); + assert_ne!(mic, other_mic2); + + + assert!(!verify_mic_enhanced(key, data, &[0, 0, 0, 0])); + } + + #[test] + fn test_enhanced_mic_is_keyed() { + + + let data = b"Same data for both"; + + let key1 = [0x01u8; 32]; + let key2 = [0x02u8; 32]; + + let mic1 = compute_mic_enhanced(&key1, data); + let mic2 = compute_mic_enhanced(&key2, data); + + + assert_ne!(mic1, mic2); + + + assert!(verify_mic_enhanced(&key1, data, &mic1)); + assert!(verify_mic_enhanced(&key2, data, &mic2)); + assert!(!verify_mic_enhanced(&key1, data, &mic2)); + assert!(!verify_mic_enhanced(&key2, data, &mic1)); + } + + #[test] + fn test_no_encryption() { + let ctx = EncryptionContext::new(ChannelKey::None); + let plaintext = b"Unencrypted data"; + let packet_id = 1; + let sender = 2; + + let output = ctx.encrypt(packet_id, sender, plaintext).unwrap(); + assert_eq!(&output[..], plaintext); + } + + #[test] + fn test_key_hash() { + let ctx = EncryptionContext::from_key_bytes(&[0x01, 0x02, 0x03, 0x04]); + + assert_eq!(ctx.key_hash(), 0x04); + } + + #[test] + fn test_pki_encryption_roundtrip() { + + let alice_priv = [0x42u8; 32]; + let alice_pub = x25519::x25519_base(&alice_priv); + + let bob_priv = [0x24u8; 32]; + let _bob_pub = x25519::x25519_base(&bob_priv); + + let plaintext = b"Secret message for Alice"; + let nonce = [0x11u8; 12]; + + + let encrypted = pki_encrypt(&alice_pub, &bob_priv, plaintext, &nonce).unwrap(); + assert!(encrypted.len() > plaintext.len()); + + + let decrypted = pki_decrypt(&alice_priv, &encrypted, &nonce).unwrap(); + assert_eq!(&decrypted[..], plaintext); + } + + #[test] + fn test_key_store() { + let mut store = KeyStore::new(); + + + store.set_channel_key(0, &[0x01, 0x02, 0x03, 0x04]); + assert!(store.get_channel(0).is_some()); + assert!(store.get_channel(1).is_none()); + + + store.set_channel_name(1, "SecondChannel"); + assert!(store.get_channel(1).is_some()); + } + + #[test] + fn test_constant_time_eq() { + assert!(constant_time_eq(&[1, 2, 3], &[1, 2, 3])); + assert!(!constant_time_eq(&[1, 2, 3], &[1, 2, 4])); + assert!(!constant_time_eq(&[1, 2, 3], &[1, 2, 3, 4])); + } +} diff --git a/src/meshtastic/mod.rs b/src/meshtastic/mod.rs index 8bc66f4..557004a 100644 --- a/src/meshtastic/mod.rs +++ b/src/meshtastic/mod.rs @@ -1,1263 +1,1440 @@ -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 - } -} +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 + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_handler_creation() { + let handler = MeshtasticHandler::new(0x12345678); + assert_eq!(handler.node_id, 0x12345678); + assert_eq!(handler.rx_count, 0); + assert_eq!(handler.tx_count, 0); + } + + #[test] + fn test_packet_id_generation() { + let mut handler = MeshtasticHandler::new(1); + assert_eq!(handler.next_packet_id(), 1); + assert_eq!(handler.next_packet_id(), 2); + assert_eq!(handler.next_packet_id(), 3); + } + + #[test] + fn test_serial_frame() { + let handler = MeshtasticHandler::new(1); + let payload = b"test"; + + let frame = handler.build_serial_frame(payload).unwrap(); + assert_eq!(frame[0], 0x94); + assert_eq!(frame[1], 0xC3); + assert_eq!(frame[2], 0x00); + assert_eq!(frame[3], 0x04); + + let parsed = handler.parse_serial_frame(&frame).unwrap(); + assert_eq!(&parsed[..], payload); + } +} diff --git a/src/meshtastic/packet.rs b/src/meshtastic/packet.rs index 3e831fd..a4753a8 100644 --- a/src/meshtastic/packet.rs +++ b/src/meshtastic/packet.rs @@ -1,388 +1,570 @@ -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() - } -} +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_parse_build_roundtrip() { + let from = 0x12345678; + let to = 0xFFFFFFFF; + let id = 0xABCDEF01; + let channel = 0; + let hop_limit = 3; + let want_ack = true; + let payload = [0x01, 0x02, 0x03, 0x04, 0x05]; + + let packet = build_lora_packet(from, to, id, channel, hop_limit, want_ack, &payload).unwrap(); + + let parsed = parse_lora_packet(&packet).unwrap(); + assert_eq!(parsed.from, from); + assert_eq!(parsed.to, to); + assert_eq!(parsed.id, id); + assert_eq!(parsed.channel, channel); + assert_eq!(parsed.hop_limit, hop_limit); + assert_eq!(parsed.want_ack, want_ack); + + if let PacketPayload::Encrypted(ref data) = parsed.payload { + assert_eq!(&data[..], &payload); + } else { + panic!("Expected encrypted payload"); + } + } + + #[test] + fn test_packet_cache() { + let mut cache = PacketCache::new(); + + + assert!(!cache.check_and_add(0x1234, 0x01, 1000)); + + + assert!(cache.check_and_add(0x1234, 0x01, 1001)); + + + assert!(!cache.check_and_add(0x5678, 0x01, 1002)); + + + assert!(!cache.check_and_add(0x1234, 0x02, 1003)); + } + + #[test] + fn test_routing_decision() { + let our_id = 0x12345678; + let mut cache = PacketCache::new(); + + + let packet = MeshPacket { + to: our_id, + from: 0x87654321, + id: 1, + hop_limit: 3, + ..Default::default() + }; + assert_eq!(route_packet(&packet, our_id, &mut cache, 1000), RoutingDecision::Local); + + + let packet = MeshPacket { + to: 0xFFFFFFFF, + from: 0x87654321, + id: 2, + hop_limit: 3, + ..Default::default() + }; + assert_eq!(route_packet(&packet, our_id, &mut cache, 1001), RoutingDecision::Forward); + + + let packet = MeshPacket { + to: 0x11111111, + from: 0x87654321, + id: 3, + hop_limit: 2, + ..Default::default() + }; + assert_eq!(route_packet(&packet, our_id, &mut cache, 1002), RoutingDecision::Forward); + + + let packet = MeshPacket { + to: 0x11111111, + from: 0x87654321, + id: 4, + hop_limit: 0, + ..Default::default() + }; + assert_eq!(route_packet(&packet, our_id, &mut cache, 1003), RoutingDecision::Drop); + } + + #[test] + fn test_ack_tracker() { + let mut tracker = AckTracker::new(); + + let ack = PendingAck { + packet_id: 0x1234, + to: 0xABCD, + packet_data: Vec::new(), + tx_count: 1, + max_retransmit: 3, + last_tx_time: 1000, + retransmit_timeout: 500, + }; + + assert!(tracker.add(ack)); + assert!(tracker.is_pending(0xABCD, 0x1234)); + assert!(!tracker.is_pending(0xABCD, 0x5678)); + + assert!(tracker.handle_ack(0xABCD, 0x1234)); + assert!(!tracker.is_pending(0xABCD, 0x1234)); + } + + #[test] + fn test_constant_time_eq() { + assert!(constant_time_eq(&[1, 2, 3], &[1, 2, 3])); + assert!(!constant_time_eq(&[1, 2, 3], &[1, 2, 4])); + assert!(!constant_time_eq(&[1, 2, 3], &[1, 2])); + } + + #[test] + fn test_mic_computation() { + let data = [0x01, 0x02, 0x03, 0x04]; + let mic = compute_mic(&data); + assert_eq!(mic.len(), MIC_SIZE); + + + let mic2 = compute_mic(&data); + assert_eq!(mic, mic2); + + + let mic3 = compute_mic(&[0x05, 0x06, 0x07, 0x08]); + assert_ne!(mic, mic3); + } +} diff --git a/src/meshtastic/protobuf.rs b/src/meshtastic/protobuf.rs index 5d37ea3..576d106 100644 --- a/src/meshtastic/protobuf.rs +++ b/src/meshtastic/protobuf.rs @@ -1,1281 +1,1480 @@ -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()) -} +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()) +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_varint_encoding() { + let mut encoder = ProtobufEncoder::<16>::new(); + assert!(encoder.write_varint(300)); + let result = encoder.finish(); + + assert_eq!(&result[..], &[0xAC, 0x02]); + } + + #[test] + fn test_varint_decoding() { + let data = [0xAC, 0x02]; + let mut decoder = ProtobufDecoder::new(&data); + assert_eq!(decoder.read_varint(), Some(300)); + } + + #[test] + fn test_data_roundtrip() { + let original = DataPayload { + port: PortNum::TextMessage, + payload: Vec::from_slice(b"Hello").unwrap(), + want_response: true, + dest: 0xFFFFFFFF, + source: 0x12345678, + request_id: 0, + reply_id: 0, + emoji: 0, + }; + + let encoded = encode_data(&original).unwrap(); + let decoded = decode_data(&encoded).unwrap(); + + assert_eq!(decoded.port, original.port); + assert_eq!(&decoded.payload[..], &original.payload[..]); + assert_eq!(decoded.want_response, original.want_response); + assert_eq!(decoded.source, original.source); + } + + #[test] + fn test_position_roundtrip() { + let original = Position { + latitude_i: 374220000, + longitude_i: -1220840000, + altitude: 100, + time: 1700000000, + location_source: LocationSource::InternalGps, + ..Default::default() + }; + + let encoded = encode_position(&original).unwrap(); + let decoded = decode_position(&encoded).unwrap(); + + assert_eq!(decoded.latitude_i, original.latitude_i); + assert_eq!(decoded.longitude_i, original.longitude_i); + assert_eq!(decoded.altitude, original.altitude); + assert_eq!(decoded.location_source, original.location_source); + } + + #[test] + fn test_hex_encode() { + let data = [0xDE, 0xAD, 0xBE, 0xEF]; + let mut output = [0u8; 8]; + hex_encode(&data, &mut output); + assert_eq!(&output, b"deadbeef"); + } + + #[test] + fn test_hex_decode() { + let data = b"deadbeef"; + let mut output = [0u8; 4]; + hex_decode(data, &mut output); + assert_eq!(&output, &[0xDE, 0xAD, 0xBE, 0xEF]); + } + + #[test] + fn test_sint32_encoding() { + + let mut encoder = ProtobufEncoder::<16>::new(); + encoder.write_sint32(-1); + let result = encoder.finish(); + + assert_eq!(&result[..], &[0x01]); + + let mut encoder = ProtobufEncoder::<16>::new(); + encoder.write_sint32(1); + let result = encoder.finish(); + + assert_eq!(&result[..], &[0x02]); + + let mut encoder = ProtobufEncoder::<16>::new(); + encoder.write_sint32(-2); + let result = encoder.finish(); + + assert_eq!(&result[..], &[0x03]); + } + + #[test] + fn test_sint32_decoding() { + let mut decoder = ProtobufDecoder::new(&[0x01]); + assert_eq!(decoder.read_sint32(), Some(-1)); + + let mut decoder = ProtobufDecoder::new(&[0x02]); + assert_eq!(decoder.read_sint32(), Some(1)); + + let mut decoder = ProtobufDecoder::new(&[0x03]); + assert_eq!(decoder.read_sint32(), Some(-2)); + } +} diff --git a/src/onion.rs b/src/onion.rs index 46ae463..567726e 100644 --- a/src/onion.rs +++ b/src/onion.rs @@ -1,368 +1,457 @@ -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() - } -} +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + use crate::crypto::x25519::x25519_base; + + #[test] + fn test_onion_wrap_unwrap() { + + let relay1_priv = [1u8; 32]; + let relay2_priv = [2u8; 32]; + let relay3_priv = [3u8; 32]; + + let relay1_pub = x25519_base(&relay1_priv); + let relay2_pub = x25519_base(&relay2_priv); + let relay3_pub = x25519_base(&relay3_priv); + + + let sender_priv = [4u8; 32]; + let sender = OnionRouter::new(sender_priv); + + + let route = OnionRoute::new(&[ + RouteHop { hint: 0x0001, public_key: relay1_pub }, + RouteHop { hint: 0x0002, public_key: relay2_pub }, + RouteHop { hint: 0x0003, public_key: relay3_pub }, + ]).unwrap(); + + + let payload = b"Secret message!"; + let packet = sender.wrap(payload, &route).unwrap(); + + + assert!(packet.data.len() > payload.len()); + } +} diff --git a/src/ota.rs b/src/ota.rs index a1ec739..57a9785 100644 --- a/src/ota.rs +++ b/src/ota.rs @@ -1,572 +1,742 @@ -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, -} +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, +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_ota_header_size() { + assert_eq!(OtaHeader::SIZE, 144); + } + + #[test] + fn test_version_string() { + let header = OtaHeader { + magic: OTA_HEADER_MAGIC, + version: 1, + header_size: OtaHeader::SIZE as u8, + fw_major: 1, + fw_minor: 2, + fw_patch: 3, + reserved: [0; 3], + firmware_size: 0, + firmware_hash: [0; 32], + signature: [0; 64], + public_key: [0; 32], + }; + + let version = header.version_string(); + assert_eq!(&version[..5], b"1.2.3"); + } + + #[test] + fn test_pack_version() { + assert_eq!(pack_version(1, 2, 3), 0x00010203); + assert_eq!(pack_version(0, 0, 0), 0); + assert_eq!(pack_version(255, 255, 255), 0x00FFFFFF); + + assert!(pack_version(2, 0, 0) > pack_version(1, 255, 255)); + assert!(pack_version(1, 1, 0) > pack_version(1, 0, 255)); + assert!(pack_version(1, 0, 1) > pack_version(1, 0, 0)); + } + + #[test] + fn test_ct_key_eq() { + let a = [0x42u8; 32]; + let b = [0x42u8; 32]; + let c = [0x43u8; 32]; + + assert!(ct_key_eq(&a, &b)); + assert!(!ct_key_eq(&a, &c)); + + + let mut d = a; + d[31] ^= 1; + assert!(!ct_key_eq(&a, &d)); + } + + #[test] + fn test_version_allowed() { + let mut manager = OtaManager::new(); + + + let header_100 = OtaHeader { + magic: OTA_HEADER_MAGIC, + version: 1, + header_size: OtaHeader::SIZE as u8, + fw_major: 1, + fw_minor: 0, + fw_patch: 0, + reserved: [0; 3], + firmware_size: 1000, + firmware_hash: [0; 32], + signature: [0; 64], + public_key: [0; 32], + }; + assert!(manager.is_version_allowed(&header_100)); + + + manager.set_min_version(1, 5, 0); + + + assert!(!manager.is_version_allowed(&header_100)); + + + let mut header_150 = header_100; + header_150.fw_minor = 5; + assert!(manager.is_version_allowed(&header_150)); + + + let mut header_200 = header_100; + header_200.fw_major = 2; + assert!(manager.is_version_allowed(&header_200)); + } + + #[test] + fn test_no_trusted_keys_disables_ota() { + let manager = OtaManager::new(); + assert!(!manager.is_enabled()); + assert_eq!(manager.trusted_key_count(), 0); + } + + #[test] + fn test_trusted_key_limit() { + let mut manager = OtaManager::new(); + let key = [0x42u8; 32]; + + assert!(manager.add_trusted_key(&key)); + assert!(manager.add_trusted_key(&key)); + + assert!(!manager.add_trusted_key(&key)); + assert_eq!(manager.trusted_key_count(), 2); + } +} diff --git a/src/packet_id.rs b/src/packet_id.rs index f34b17e..9008c0d 100644 --- a/src/packet_id.rs +++ b/src/packet_id.rs @@ -1,256 +1,410 @@ -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); -} +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); +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_mix32() { + + let a = mix32(0); + let b = mix32(1); + let c = mix32(2); + assert_ne!(a, b); + assert_ne!(b, c); + assert_ne!(a, c); + } + + #[test] + fn test_mix32_bijective() { + + let mut outputs = [0u32; 1000]; + for i in 0..1000 { + outputs[i] = mix32(i as u32); + } + + + for i in 0..outputs.len() { + for j in (i + 1)..outputs.len() { + assert_ne!(outputs[i], outputs[j], "Collision at {} and {}", i, j); + } + } + } + + #[test] + fn test_packet_id_state() { + let mut state = PacketIdState { + session_id: 0xABCD_1234, + sequence: 0x5678_9ABC, + boot_count: 0, + }; + + let nonce = state.to_u64(); + assert_eq!(nonce >> 32, 0xABCD_1234); + assert_eq!(nonce as u32, 0x5678_9ABC); + } + + #[test] + fn test_generate_session_id_nonzero() { + + for _ in 0..100 { + let session = generate_session_id(0, 0); + assert_ne!(session, 0, "Session ID must not be zero"); + } + } + + #[test] + fn test_uniqueness_atomic() { + + PACKET_ID_SESSION.store(0x1234_0000, Ordering::SeqCst); + PACKET_ID_SEQUENCE.store(0, Ordering::SeqCst); + INITIALIZED.store(true, Ordering::SeqCst); + + let mut ids = [0u32; 100]; + for id in &mut ids { + + let sequence = PACKET_ID_SEQUENCE.fetch_add(1, Ordering::SeqCst); + let session = PACKET_ID_SESSION.load(Ordering::SeqCst); + *id = mix32(session ^ sequence.wrapping_mul(0x85EBCA6B)); + } + + + for i in 0..ids.len() { + for j in (i + 1)..ids.len() { + assert_ne!(ids[i], ids[j], "Duplicate ID at {} and {}", i, j); + } + } + } + + #[test] + fn test_64bit_nonce_uniqueness() { + + PACKET_ID_SESSION.store(0x1234, Ordering::SeqCst); + PACKET_ID_SEQUENCE.store(0, Ordering::SeqCst); + INITIALIZED.store(true, Ordering::SeqCst); + + let mut prev = 0u64; + for _ in 0..1000 { + let sequence = PACKET_ID_SEQUENCE.fetch_add(1, Ordering::SeqCst); + let session = PACKET_ID_SESSION.load(Ordering::SeqCst); + let nonce = ((session as u64) << 32) | (sequence as u64); + assert!(nonce > prev || prev == 0, "Nonces must be strictly monotonic"); + prev = nonce; + } + } + + #[test] + fn test_session_info() { + PACKET_ID_SESSION.store(0xDEAD_BEEF, Ordering::SeqCst); + PACKET_ID_SEQUENCE.store(0xCAFE_BABE, Ordering::SeqCst); + BOOT_COUNT.store(42, Ordering::SeqCst); + INITIALIZED.store(true, Ordering::SeqCst); + + let (session, sequence, boot) = session_info(); + assert_eq!(session, 0xDEAD_BEEF); + assert_eq!(sequence, 0xCAFE_BABE); + assert_eq!(boot, 42); + } +} diff --git a/src/power.rs b/src/power.rs index e450326..27a70bf 100644 --- a/src/power.rs +++ b/src/power.rs @@ -1,402 +1,465 @@ -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, -} +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, +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_wake_sources() { + let sources = WakeSources::TIMER | WakeSources::GPIO; + assert!(sources.has(WakeSources::TIMER)); + assert!(sources.has(WakeSources::GPIO)); + assert!(!sources.has(WakeSources::UART)); + } + + #[test] + fn test_battery_thresholds() { + assert!(!PowerManager::is_battery_low(3600)); + assert!(PowerManager::is_battery_low(3300)); + assert!(PowerManager::is_battery_critical(3100)); + } +} diff --git a/src/protocol.rs b/src/protocol.rs index 6aacf4a..6f402d2 100644 --- a/src/protocol.rs +++ b/src/protocol.rs @@ -1,403 +1,442 @@ -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, - }) -} +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_khz = u16::from_le_bytes([data[5], data[6]]); + + let bandwidth = match bandwidth_khz { + 0..=80 => 0x03, + 81..=187 => 0x04, + 188..=375 => 0x05, + _ => 0x06, + }; + 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, + 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 index 411a7e1..746ec0d 100644 --- a/src/protocol_router.rs +++ b/src/protocol_router.rs @@ -1,494 +1,590 @@ -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() - } -} +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_meshcore_detection() { + let mut detector = ProtocolDetector::new(); + assert_eq!(detector.protocol(), Protocol::Unknown); + + detector.feed(0xAA); + assert_eq!(detector.protocol(), Protocol::Unknown); + + let result = detector.feed(0x55); + assert_eq!(result, Some(Protocol::MeshCore)); + assert_eq!(detector.protocol(), Protocol::MeshCore); + } + + #[test] + fn test_meshtastic_detection() { + let mut detector = ProtocolDetector::new(); + + detector.feed(0x94); + let result = detector.feed(0xC3); + assert_eq!(result, Some(Protocol::Meshtastic)); + } + + #[test] + fn test_kiss_detection() { + let mut detector = ProtocolDetector::new(); + + let result = detector.feed(0xC0); + assert_eq!(result, Some(Protocol::RNode)); + } + + #[test] + fn test_at_detection() { + let mut detector = ProtocolDetector::new(); + + detector.feed(b'A'); + let result = detector.feed(b'T'); + assert_eq!(result, Some(Protocol::AtCommand)); + } +} diff --git a/src/rng.rs b/src/rng.rs index b87d408..6b24199 100644 --- a/src/rng.rs +++ b/src/rng.rs @@ -1,292 +1,402 @@ -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 -} +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 +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_mix_entropy() { + + let a = mix_entropy(1, 2, 3); + let b = mix_entropy(1, 2, 4); + let c = mix_entropy(2, 2, 3); + + assert_ne!(a, b); + assert_ne!(a, c); + assert_ne!(b, c); + } + + #[test] + fn test_entropy_estimation() { + + SAMPLE_COUNT.store(100, Ordering::SeqCst); + + + for bc in &BIT_COUNTS { + bc.store(50, Ordering::Relaxed); + } + + let entropy = estimate_entropy(); + assert!(entropy >= 7, "Perfect distribution should have high entropy"); + } + + #[test] + fn test_entropy_estimation_biased() { + + SAMPLE_COUNT.store(100, Ordering::SeqCst); + + + for bc in &BIT_COUNTS { + bc.store(95, Ordering::Relaxed); + } + + let entropy = estimate_entropy(); + assert!(entropy < 4, "Biased distribution should have low entropy"); + } + + #[test] + fn test_health_stats() { + let stats = health_stats(); + + let _ = stats.healthy; + let _ = stats.sample_count; + let _ = stats.failure_count; + let _ = stats.estimated_entropy; + } +} diff --git a/src/rnode.rs b/src/rnode.rs index 613fd53..af51cce 100644 --- a/src/rnode.rs +++ b/src/rnode.rs @@ -1,1014 +1,1278 @@ -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() - } -} +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 reset_parser(&mut self) { + self.parser.reset(); + } + + + 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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_escape_encode() { + let frame = KissFrame::data_frame(&[0x00, 0xC0, 0xDB, 0xFF]).unwrap(); + let encoded = frame.encode(); + + + assert_eq!(encoded[0], FEND); + assert_eq!(encoded[1], 0x00); + assert_eq!(encoded[2], 0x00); + assert_eq!(encoded[3], FESC); + assert_eq!(encoded[4], TFEND); + assert_eq!(encoded[5], FESC); + assert_eq!(encoded[6], TFESC); + assert_eq!(encoded[7], 0xFF); + assert_eq!(encoded[8], FEND); + } + + #[test] + fn test_parser() { + let mut parser = KissParser::new(); + + + let bytes = [FEND, 0x00, 0x01, 0x02, 0x03, FEND]; + + for (i, &byte) in bytes.iter().enumerate() { + let result = parser.feed(byte); + if i == bytes.len() - 1 { + assert!(result.is_some()); + let frame = result.unwrap(); + assert_eq!(frame.command, 0x00); + assert_eq!(frame.data.len(), 3); + } else { + assert!(result.is_none()); + } + } + } + + #[test] + fn test_parser_escape() { + let mut parser = KissParser::new(); + + + let bytes = [FEND, 0x00, FESC, TFEND, FEND]; + + for &byte in &bytes { + if let Some(frame) = parser.feed(byte) { + assert_eq!(frame.data.len(), 1); + assert_eq!(frame.data[0], FEND); + } + } + } + + #[test] + fn test_rnode_config() { + let mut handler = RNodeHandler::new(); + + + let frame = KissFrame::command_frame( + RNodeCommand::Frequency, + &915_000_000u32.to_be_bytes(), + ).unwrap(); + handler.process_frame(&frame); + assert_eq!(handler.config().frequency, 915_000_000); + + + let frame = KissFrame::command_frame( + RNodeCommand::SpreadingFactor, + &[10], + ).unwrap(); + handler.process_frame(&frame); + assert_eq!(handler.config().spreading_factor, 10); + } + + #[test] + fn test_rnode_statistics() { + let mut handler = RNodeHandler::new(); + + + let _ = handler.process_lora_packet(&[0x01, 0x02, 0x03], -80, 5); + assert_eq!(handler.stats().rx_count, 1); + assert_eq!(handler.stats().rx_bytes, 3); + assert_eq!(handler.stats().last_rssi, -80); + assert_eq!(handler.stats().last_snr, 5); + + + let _ = handler.process_lora_packet(&[0x04, 0x05], -90, 3); + assert_eq!(handler.stats().rx_count, 2); + assert_eq!(handler.stats().rx_bytes, 5); + } + + #[test] + fn test_rnode_airtime() { + let config = RNodeConfig::default(); + + + let airtime = config.packet_airtime_ms(50); + assert!(airtime > 50); + assert!(airtime < 500); + } + + #[test] + fn test_rnode_ldro_calculation() { + let mut config = RNodeConfig::default(); + + + config.spreading_factor = 9; + config.bandwidth = 125_000; + assert!(!config.should_enable_ldro()); + + + config.spreading_factor = 11; + assert!(config.should_enable_ldro()); + + + config.spreading_factor = 12; + assert!(config.should_enable_ldro()); + + + config.bandwidth = 500_000; + assert!(!config.should_enable_ldro()); + } + + #[test] + fn test_rnode_identity_hash() { + let mut identity = RNodeIdentity::default(); + identity.serial = [0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10]; + + let hash1 = identity.identity_hash(); + let hash2 = identity.identity_hash(); + + + assert_eq!(hash1, hash2); + + + identity.serial[0] = 0xFF; + let hash3 = identity.identity_hash(); + assert_ne!(hash1, hash3); + } + + #[test] + fn test_rnode_airtime_limit() { + let mut handler = RNodeHandler::new(); + handler.airtime_limit = 1000; + + + assert!(handler.check_airtime_limit(10)); + + + } + + #[test] + fn test_rnode_all_commands() { + let mut handler = RNodeHandler::new(); + + + let detect_frame = KissFrame::command_frame(RNodeCommand::Detect, &[]).unwrap(); + assert!(handler.process_frame(&detect_frame).is_some()); + + let ready_frame = KissFrame::command_frame(RNodeCommand::Ready, &[]).unwrap(); + assert!(handler.process_frame(&ready_frame).is_some()); + + let fw_frame = KissFrame::command_frame(RNodeCommand::FwVersion, &[]).unwrap(); + assert!(handler.process_frame(&fw_frame).is_some()); + + let platform_frame = KissFrame::command_frame(RNodeCommand::Platform, &[]).unwrap(); + assert!(handler.process_frame(&platform_frame).is_some()); + } +} diff --git a/src/session.rs b/src/session.rs index 9b7ca58..f874739 100644 --- a/src/session.rs +++ b/src/session.rs @@ -1,781 +1,900 @@ -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() - } -} +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_session_encrypt_decrypt() { + + let alice_private = [1u8; 32]; + let bob_private = [2u8; 32]; + + let alice_public = x25519_base(&alice_private); + let bob_public = x25519_base(&bob_private); + + + let shared = x25519(&alice_private, &bob_public); + + let alice_params = SessionParams { + shared_secret: shared, + our_private: alice_private, + their_public: bob_public, + is_initiator: true, + }; + + let bob_params = SessionParams { + shared_secret: shared, + our_private: bob_private, + their_public: alice_public, + is_initiator: false, + }; + + let mut alice = Session::new(alice_params); + let mut bob = Session::new(bob_params); + + + let message = b"Hello, Bob!"; + let (header, ciphertext) = alice.encrypt(message).unwrap(); + + + let plaintext = bob.decrypt(&header, &ciphertext).unwrap(); + assert_eq!(&plaintext[..message.len()], message); + } +} diff --git a/src/sx1262.rs b/src/sx1262.rs index 497a046..25a226d 100644 --- a/src/sx1262.rs +++ b/src/sx1262.rs @@ -1,610 +1,781 @@ -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) - } -} +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: 915_000_000, + spreading_factor: 9, + bandwidth: 0x04, + 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.clear_errors()?; + + + 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> { + + log::info!("[SX1262] ========== RADIO CONFIG =========="); + log::info!("[SX1262] Freq: {} Hz, SF: {}, BW: {}, CR: {}", + config.frequency, config.spreading_factor, config.bandwidth, config.coding_rate); + log::info!("[SX1262] TX Power: {}, Sync: 0x{:02X}, Preamble: {}", + config.tx_power, config.sync_word, config.preamble_length); + log::info!("[SX1262] CRC: {}, ImplicitHdr: {}, LDRO: {}", + config.crc_enabled, config.implicit_header, config.ldro); + log::info!("[SX1262] ==================================="); + + + if config.spreading_factor < 7 || config.spreading_factor > 12 { + return Err(RadioError::InvalidConfig); + } + if config.bandwidth > 6 { + 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 { + 0x00 => 7_810, + 0x01 => 15_630, + 0x02 => 31_250, + 0x03 => 62_500, + 0x04 => 125_000, + 0x05 => 250_000, + 0x06 => 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 mut iq_config = self.read_register(0x0736)?; + iq_config |= 0x04; + self.write_register(0x0736, iq_config)?; + + + let sync_msb = (config.sync_word & 0xF0) | 0x04; + let sync_lsb = ((config.sync_word & 0x0F) << 4) | 0x04; + log::info!("[SX1262] Sync word: input=0x{:02X} -> MSB=0x{:02X}, LSB=0x{:02X}", + config.sync_word, sync_msb, sync_lsb); + 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.write_register(register::RX_GAIN, 0x96)?; + + 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, + ]) + } + + + fn read_register(&mut self, addr: u16) -> Result { + let mut rx = [0u8; 5]; + self.transfer( + &[ + opcode::READ_REGISTER, + (addr >> 8) as u8, + addr as u8, + 0x00, + ], + &mut rx, + )?; + Ok(rx[4]) + } + + + 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])?; + + + self.start_rx(0)?; + 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 get_status(&mut self) -> Result<(u8, u8), RadioError> { + let mut rx = [0u8; 2]; + self.transfer(&[opcode::GET_STATUS, 0], &mut rx)?; + + let status = rx[1]; + let chip_mode = (status >> 4) & 0x07; + let cmd_status = (status >> 1) & 0x07; + Ok((chip_mode, cmd_status)) + } + + + pub fn get_errors(&mut self) -> Result { + let mut rx = [0u8; 4]; + self.transfer(&[opcode::GET_DEVICE_ERRORS, 0, 0, 0], &mut rx)?; + Ok(((rx[2] as u16) << 8) | (rx[3] as u16)) + } + + + pub fn clear_errors(&mut self) -> Result<(), RadioError> { + self.write_command(&[opcode::CLEAR_DEVICE_ERRORS, 0x00, 0x00]) + } + + + 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 read_packet(&mut self) -> Result<(heapless::Vec, i16, i8), RadioError> { + + 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]; + + if payload_len == 0 { + return Err(RadioError::RxTimeout); + } + + + 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]); + } + + Ok((data, rssi, snr)) + } + + + 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 index 5a341b5..c2ff0f3 100644 --- a/src/transport.rs +++ b/src/transport.rs @@ -1,450 +1,575 @@ -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() - } -} +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() + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_address_derivation() { + let pubkey = [0u8; 32]; + let addr = AddressTranslator::from_public_key(&pubkey); + + + let addr2 = AddressTranslator::from_public_key(&pubkey); + assert_eq!(addr.meshcore_addr, addr2.meshcore_addr); + assert_eq!(addr.meshtastic_id, addr2.meshtastic_id); + assert_eq!(addr.reticulum_hash, addr2.reticulum_hash); + } + + #[test] + fn test_wire_packet_roundtrip() { + let packet = WirePacket::new_data(0x1234, 0xDEADBEEF, &[1, 2, 3, 4, 5]).unwrap(); + let encoded = packet.encode(); + let decoded = WirePacket::decode(&encoded).unwrap(); + + assert_eq!(decoded.packet_type, PacketType::Data); + assert_eq!(decoded.next_hop_hint, 0x1234); + assert_eq!(decoded.session_hint, 0xDEADBEEF); + assert_eq!(&decoded.payload[..], &[1, 2, 3, 4, 5]); + } + + #[test] + fn test_protocol_detection() { + assert_eq!(Protocol::detect(&[0xAA, 0x55]), Some(Protocol::MeshCore)); + assert_eq!(Protocol::detect(&[0x94, 0xC3]), Some(Protocol::Meshtastic)); + assert_eq!(Protocol::detect(&[0xC0, 0x00]), Some(Protocol::Reticulum)); + assert_eq!(Protocol::detect(&[0x00, 0x00]), None); + } + + #[test] + fn test_address_lookup_table() { + let mut table = AddressLookupTable::new(); + let pubkey1 = [1u8; 32]; + let pubkey2 = [2u8; 32]; + + + table.register(&pubkey1); + table.register(&pubkey2); + assert_eq!(table.len(), 2); + + + let addr1 = AddressTranslator::from_public_key(&pubkey1); + let addr2 = AddressTranslator::from_public_key(&pubkey2); + + + assert_eq!(table.lookup_meshcore(addr1.meshcore_addr), Some(&pubkey1)); + assert_eq!(table.lookup_meshcore(addr2.meshcore_addr), Some(&pubkey2)); + assert_eq!(table.lookup_meshcore(0xFFFF), None); + + + assert_eq!(table.lookup_meshtastic(addr1.meshtastic_id), Some(&pubkey1)); + assert_eq!(table.lookup_meshtastic(addr2.meshtastic_id), Some(&pubkey2)); + + + assert_eq!(table.lookup_reticulum(&addr1.reticulum_hash), Some(&pubkey1)); + + + table.unregister(&pubkey1); + assert_eq!(table.len(), 1); + assert_eq!(table.lookup_meshcore(addr1.meshcore_addr), None); + } +} diff --git a/src/wifi.rs b/src/wifi.rs index 97f687b..36f5315 100644 --- a/src/wifi.rs +++ b/src/wifi.rs @@ -1,964 +1,1203 @@ -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 - } -} +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 + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_wifi_config_default() { + let config = WifiConfig::default(); + assert_eq!(config.mode, WifiMode::Off); + assert_eq!(config.tcp_port, DEFAULT_TCP_PORT); + assert_eq!(&config.ap_ssid[..], DEFAULT_AP_SSID); + + assert!(config.security.require_auth); + assert!(config.security.encrypt_traffic); + } + + #[test] + fn test_tcp_client_empty() { + let client = TcpClient::empty(); + assert!(client.is_available()); + assert_eq!(client.fd, -1); + assert_eq!(client.auth_state, AuthState::None); + } + + #[test] + fn test_security_config_default() { + let sec = SecurityConfig::default(); + assert!(sec.require_auth); + assert!(sec.encrypt_traffic); + assert!(sec.rate_limiting); + assert!(!sec.allow_external); + + assert!(!sec.is_psk_set()); + } + + #[test] + fn test_security_config_with_psk() { + let psk = [0x42u8; 32]; + let sec = SecurityConfig::with_psk(&psk); + assert!(sec.is_psk_set()); + assert_eq!(sec.psk, psk); + } + + #[test] + fn test_security_config_insecure() { + let sec = SecurityConfig::insecure(); + assert!(!sec.require_auth); + assert!(!sec.encrypt_traffic); + assert!(!sec.rate_limiting); + assert!(sec.allow_external); + } + + #[test] + fn test_hmac_sha256() { + + let key = [0x0bu8; 32]; + let msg = b"Hi There"; + let mac = hmac_sha256(&key, msg); + + let mac2 = hmac_sha256(&key, msg); + assert_eq!(mac, mac2); + + let mac3 = hmac_sha256(&key, b"Hi There!"); + assert_ne!(mac, mac3); + } + + #[test] + fn test_ct_eq_32() { + let a = [0x42u8; 32]; + let b = [0x42u8; 32]; + let c = [0x43u8; 32]; + assert!(ct_eq_32(&a, &b)); + assert!(!ct_eq_32(&a, &c)); + + let mut d = a; + d[31] ^= 1; + assert!(!ct_eq_32(&a, &d)); + } + + #[test] + fn test_session_crypto() { + let mut session = SessionCrypto::new(); + assert!(!session.enabled); + + let key = [0x42u8; 32]; + let nonce = [0x24u8; 12]; + session.init(&key, &nonce); + assert!(session.enabled); + assert_eq!(session.counter, 0); + + + let original = b"Hello, secure world!"; + let mut data = [0u8; 32]; + data[..original.len()].copy_from_slice(original); + + + let counter_before = session.counter; + session.encrypt(&mut data[..original.len()]); + + + assert_eq!(session.counter, counter_before + 1); + + + session.init(&key, &nonce); + session.decrypt(&mut data[..original.len()]); + + assert_eq!(&data[..original.len()], original); + + + session.clear(); + assert!(!session.enabled); + assert_eq!(session.key, [0u8; 32]); + } + + #[test] + fn test_client_authentication_state() { + let mut client = TcpClient::empty(); + + + assert!(client.is_authenticated()); + + + client.auth_state = AuthState::ChallengeSent; + assert!(!client.is_authenticated()); + + + client.auth_state = AuthState::Authenticated; + assert!(client.is_authenticated()); + + + client.auth_state = AuthState::Failed; + assert!(!client.is_authenticated()); + } + + #[test] + fn test_wifi_manager_auth_configured() { + let mut manager = WifiManager::new(); + + + assert!(!manager.is_auth_configured()); + + + let psk = [0x42u8; 32]; + manager.set_psk(&psk); + assert!(manager.is_auth_configured()); + + + manager.config.security.require_auth = false; + manager.config.security.psk = [0u8; 32]; + assert!(manager.is_auth_configured()); + } +}