bond

src/main.rs

@@ -3,9 +3,11 @@
 
 mod fmt;
 
+use core::cell::{Cell, RefCell};
 use core::mem;
 use core::ops::{AddAssign, SubAssign};
 use core::pin::Pin;
+use core::sync::atomic::{AtomicU32, AtomicU8, Ordering};
 use defmt::{error, Format};
 #[cfg(not(feature = "defmt"))]
 use panic_halt as _;
@@ -16,10 +18,11 @@ use defmt::info;
 
 use embassy_executor::Spawner;
 use embassy_futures::join::{join3, join4};
-use embassy_nrf::gpio::{AnyPin, Input, OutputDrive, Pull};
+use embassy_nrf::gpio::{AnyPin, Input, Level, Output, OutputDrive, Pull};
 use embassy_nrf::interrupt::{InterruptExt, Priority};
 use embassy_nrf::pwm::{DutyCycle, Prescaler, SimplePwm};
 use embassy_nrf::{bind_interrupts, interrupt, saadc, Peri};
+use embassy_nrf::gpio::OutputDrive::Standard;
 use embassy_nrf::peripherals::SAADC;
 use embassy_nrf::saadc::{ChannelConfig, Gain, Reference, Saadc};
 use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
@@ -30,8 +33,11 @@ use futures::future::{select, Either};
 use futures::pin_mut;
 use nrf_softdevice::{raw, Flash, Softdevice};
 use nrf_softdevice::ble::advertisement_builder::{Flag, LegacyAdvertisementBuilder, LegacyAdvertisementPayload, ServiceList, ServiceUuid16};
-use nrf_softdevice::ble::{peripheral, gatt_server, Connection};
+use nrf_softdevice::ble::{peripheral, gatt_server, Connection, MasterId, EncryptionInfo, IdentityKey, PasskeyReply};
 use embedded_storage_async::nor_flash::{NorFlash, ReadNorFlash};
+use nrf_softdevice::ble::gatt_server::set_sys_attrs;
+use nrf_softdevice::ble::security::{IoCapabilities, SecurityHandler};
+use static_cell::StaticCell;
 
 #[derive(Copy, Clone, Eq, PartialEq, Format)]
 enum SysAction {
@@ -127,23 +133,24 @@ static TEXT_ANIMATION_SIGNAL: Signal<ThreadModeRawMutex, AnimationSignal<TextLig
 static HEART_ANIMATION_SIGNAL: Signal<ThreadModeRawMutex, AnimationSignal<HeartLightType>> = Signal::new();
 static SYS_ACTION: Mutex<ThreadModeRawMutex, SysAction> = Mutex::new(SysAction::PresentenceOn);
 static SAVE_SIGNAL: Signal<ThreadModeRawMutex, StoreType> = Signal::new();
+static BOND_COUNTER: AtomicU8 = AtomicU8::new(0);
 
 #[nrf_softdevice::gatt_service(uuid = "9e7312e0-2354-11eb-9f10-fbc30a62cf38")]
 struct AnimationService {
-    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a63cf38", read, write, notify, indicate)]
+    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a63cf38", read, write, notify, indicate, security = "JustWorks")]
     text_mode: u16,
-    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a64cf38", read, write, notify, indicate)]
+    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a64cf38", read, write, notify, indicate, security = "JustWorks")]
     text_color: u16,
-    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a65cf38", read, write, notify, indicate)]
+    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a65cf38", read, write, notify, indicate, security = "JustWorks")]
     heart_mode: u16,
-    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a66cf38", read, write, notify, indicate)]
+    #[characteristic(uuid = "9e7312e0-2354-11eb-9f10-fbc30a66cf38", read, write, notify, indicate, security = "JustWorks")]
     heart_color: u16,
 
 }
 
 #[nrf_softdevice::gatt_service(uuid = "180f")]
 struct BatteryService {
-    #[characteristic(uuid = "2a19", read, notify)]
+    #[characteristic(uuid = "2a19", read, notify, security = "JustWorks")]
     battery_level: u8,
 }
 
@@ -159,6 +166,99 @@ bind_interrupts!(struct AdcIrqs {
 
 const DATA_POINT: u32 =  0x000F9800 - 4096;
 
+#[derive(Debug, Clone, Copy)]
+struct Peer {
+    master_id: MasterId,
+    key: EncryptionInfo,
+    peer_id: IdentityKey,
+}
+
+pub struct Bonder {
+    peer: Cell<Option<Peer>>,
+    sys_attrs: RefCell<heapless::Vec<u8, 62>>,
+}
+
+impl Default for Bonder {
+    fn default() -> Self {
+        Bonder {
+            peer: Cell::new(None),
+            sys_attrs: Default::default(),
+        }
+    }
+}
+
+impl SecurityHandler for Bonder {
+    fn io_capabilities(&self) -> IoCapabilities {
+        let val = BOND_COUNTER.load(Ordering::Relaxed);
+        if val > 0 {
+            IoCapabilities::None
+        } else {
+            IoCapabilities::DisplayYesNo
+        }
+    }
+
+    fn can_bond(&self, _conn: &Connection) -> bool {
+        true
+    }
+
+    fn display_passkey(&self, passkey: &[u8; 6]) {
+        info!("The passkey is \"{:a}\"", passkey)
+    }
+
+    fn enter_passkey(&self, _reply: PasskeyReply) {
+    }
+
+    fn on_bonded(&self, _conn: &Connection, master_id: MasterId, key: EncryptionInfo, peer_id: IdentityKey) {
+        info!("storing bond for: id: {}, key: {}", master_id, key);
+
+        // In a real application you would want to signal another task to permanently store the keys in non-volatile memory here.
+        self.sys_attrs.borrow_mut().clear();
+        self.peer.set(Some(Peer {
+            master_id,
+            key,
+            peer_id,
+        }));
+    }
+
+    fn get_key(&self, _conn: &Connection, master_id: MasterId) -> Option<EncryptionInfo> {
+        info!("getting bond for: id: {}", master_id);
+
+        self.peer
+            .get()
+            .and_then(|peer| (master_id == peer.master_id).then_some(peer.key))
+    }
+
+    fn save_sys_attrs(&self, conn: &Connection) {
+        info!("saving system attributes for: {}", conn.peer_address());
+
+        if let Some(peer) = self.peer.get() {
+            if peer.peer_id.is_match(conn.peer_address()) {
+                let mut sys_attrs = self.sys_attrs.borrow_mut();
+                let capacity = sys_attrs.capacity();
+                sys_attrs.resize(capacity, 0).expect("failed to resize sys attributes");
+                let len = gatt_server::get_sys_attrs(conn, &mut sys_attrs).expect("failed to get sys attributes") as u16;
+                sys_attrs.truncate(usize::from(len));
+                // In a real application you would want to signal another task to permanently store sys_attrs for this connection's peer
+            }
+        }
+    }
+
+    fn load_sys_attrs(&self, conn: &Connection) {
+        let addr = conn.peer_address();
+        info!("loading system attributes for: {}", addr);
+
+        let attrs = self.sys_attrs.borrow();
+        // In a real application you would search all stored peers to find a match
+        let attrs = if self.peer.get().map(|peer| peer.peer_id.is_match(addr)).unwrap_or(false) {
+            (!attrs.is_empty()).then_some(attrs.as_slice())
+        } else {
+            None
+        };
+
+        set_sys_attrs(conn, attrs).expect("failed to set sys attributes");
+    }
+}
+
 #[embassy_executor::main]
 async fn main(spawner: Spawner) {
     let mut config = embassy_nrf::config::Config::default();
@@ -225,7 +325,7 @@ async fn main(spawner: Spawner) {
     let pwm_heart = SimplePwm::new_3ch(p.PWM2, p.P0_17, p.P0_20, p.P0_22, &Default::default());
     spawner.spawn(radar_task()).expect("failed to spawn radar task");
     spawner.spawn(touch_button(p.P0_08.into())).expect("failed to spawn touch task");
-    spawner.spawn(actions_task()).expect("failed to spawn actions task");
+    spawner.spawn(actions_task(p.P0_15.into())).expect("failed to spawn actions task");
     spawner.spawn(softdevice_task(sd)).expect("failed to spawn softdevice task");
     spawner.spawn(storage_task(flash, initial_data.0, initial_data.1, initial_data.2, initial_data.3)).expect("failed to spawn softdevice task");
 
@@ -318,13 +418,21 @@ async fn gatt_task(server: Server, mut saadc: Saadc<'static, 1>, sd: &'static So
         )
         .build();
 
+    static BONDER: StaticCell<Bonder> = StaticCell::new();
+    let bonder = BONDER.init(Bonder::default());
+
     loop {
         let config = peripheral::Config::default();
         let adv = peripheral::ConnectableAdvertisement::ScannableUndirected {
             adv_data: &ADV_DATA,
             scan_data: &SCAN_DATA,
         };
-        let conn = peripheral::advertise_connectable(sd, adv, &config).await.expect("failed to advertise");
+        let conn = peripheral::advertise_pairable(sd, adv, &config, bonder).await.expect("failed to advertise");
+
+        if let Err(e) = conn.request_security() {
+            info!("Failed to request security: {}", e);
+            continue;
+        }
 
         info!("advertising done!");
 
@@ -517,38 +625,55 @@ fn battery_mv_to_percent(mv: u16) -> u8 {
 }
 
 #[embassy_executor::task]
-async fn actions_task() {
+async fn actions_task(pin: Peri<'static, AnyPin>) {
+    let mut led_pin = Output::new(pin, Level::High, Standard);
+
     loop {
-        let signal = TOUCH_SIGNAL.wait().await;
-        info!("Action: {:?}", signal);
-        match signal {
-            // constant on vs presentence
-            TouchAction::Tap => {
-                let mut action = SYS_ACTION.lock().await;
-                match *action {
-                    SysAction::PresentenceOn |
-                    SysAction::PresentenceOff => {
-                        *action = SysAction::ConstantOn;
-                        info!("Constant light");
+        if let Ok(signal) = with_timeout(Duration::from_secs(60), TOUCH_SIGNAL.wait()).await {
+            info!("Action: {:?}", signal);
+            match signal {
+                // constant on vs presentence
+                TouchAction::Tap => {
+                    let mut action = SYS_ACTION.lock().await;
+                    match *action {
+                        SysAction::PresentenceOn |
+                        SysAction::PresentenceOff => {
+                            *action = SysAction::ConstantOn;
+                            info!("Constant light");
+                        }
+                        SysAction::ConstantOn => {
+                            *action = SysAction::PresentenceOn;
+                            info!("Light by presentence");
+                        }
                     }
-                    SysAction::ConstantOn => {
-                        *action = SysAction::PresentenceOn;
-                        info!("Light by presentence");
+                    info!("Light mode {:?}", *action);
+                }
+                // change text animation
+                TouchAction::DoubleTap => {
+                    TEXT_ANIMATION_SIGNAL.signal(AnimationSignal::NextAnimation);
+                }
+                // change anchor animation
+                TouchAction::TripleTap => {
+                    HEART_ANIMATION_SIGNAL.signal(AnimationSignal::NextAnimation);
+                }
+                // enter pairing mode
+                TouchAction::Hold => {
+                    let current = BOND_COUNTER.load(Ordering::Relaxed);
+                    let next = if current > 0 { 0 } else { 1 };
+                    BOND_COUNTER.store(next, Ordering::Relaxed);
+                    info!("Set bond: {}", next);
+                    if next > 0 {
+                        led_pin.set_low();
                     }
                 }
-                info!("Light mode {:?}", *action);
             }
-            // change text animation
-            TouchAction::DoubleTap => {
-                TEXT_ANIMATION_SIGNAL.signal(AnimationSignal::NextAnimation);
-            }
-            // change anchor animation
-            TouchAction::TripleTap => {
-                HEART_ANIMATION_SIGNAL.signal(AnimationSignal::NextAnimation);
-            }
-            // enter pairing mode
-            TouchAction::Hold => {
+        } else {
+            let current = BOND_COUNTER.load(Ordering::Relaxed);
+            if current > 0 {
+                BOND_COUNTER.store(0, Ordering::Relaxed);
+                led_pin.set_high();
             }
+            info!("Timeout bond");
         }
     }
 }
@@ -595,10 +720,11 @@ async fn moving_radar(pin: Peri<'static, AnyPin>) {
         info!("Waiting for radar");
         radar.wait_for_high().await;
         RADAR_SIGNAL.signal(0);
+        info!("Moving!!");
         while let Err(_) = with_timeout(Duration::from_secs(1), radar.wait_for_low()).await {
-            info!("Moving!!");
             RADAR_SIGNAL.signal(0);
         }
+        info!("Stopped");
     }
 }