Add basic BLE I/O

src/ble.rs

@@ -0,0 +1,263 @@
+#![allow(
+    clippy::needless_borrows_for_generic_args,
+    reason = "gatt_service creates false positives"
+)]
+
+use core::cell::RefCell;
+
+use critical_section::Mutex;
+use esp_backtrace as _;
+use log::{info, warn};
+use trouble_host::prelude::*;
+
+use crate::io;
+
+pub const CONTROLLER_SLOTS: usize = 20;
+const CONNECTIONS_MAX: usize = 1;
+const L2CAP_CHANNELS_MAX: usize = 1;
+
+pub static TX_DATA: Mutex<RefCell<Option<BleSendable>>> = Mutex::new(RefCell::new(None));
+pub static RX_DATA: Mutex<RefCell<Option<io::msg::PositionState>>> = Mutex::new(RefCell::new(None));
+
+pub enum BleSendable {
+    #[cfg(feature = "cam")]
+    Cam(io::msg::CamEvent),
+    #[cfg(feature = "denm")]
+    Denm(io::msg::DenmEvent),
+}
+
+#[gatt_server]
+struct Server {
+    c_its_events: CITSEvents,
+}
+
+#[cfg(feature = "cam")]
+const CAM_EVENT_SIZE: usize = 38;
+#[cfg(feature = "denm")]
+const DENM_EVENT_SIZE: usize = 51;
+
+#[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")]
+struct CITSEvents {
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Position", read, value = "PositionState proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4001-ada8-f99a02ee315c", write)]
+    position: [u8; 27],
+
+    #[cfg(feature = "cam")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])]
+    cam_event: [u8; CAM_EVENT_SIZE],
+
+    #[cfg(feature = "denm")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])]
+    denm_event: [u8; DENM_EVENT_SIZE],
+}
+
+/// Creates the BLE controller
+pub fn make_controller(
+    bt_peripheral: esp_hal::peripherals::BT<'static>,
+) -> bt_hci::controller::ExternalController<
+    esp_radio::ble::controller::BleConnector<'static>,
+    CONTROLLER_SLOTS,
+> {
+    let transport = esp_radio::ble::controller::BleConnector::new(
+        bt_peripheral,
+        esp_radio::ble::Config::default(),
+    )
+    .unwrap();
+    bt_hci::controller::ExternalController::<_, { CONTROLLER_SLOTS }>::new(transport)
+}
+
+/// Run the BLE stack.
+#[embassy_executor::task]
+pub async fn run(
+    controller: bt_hci::controller::ExternalController<
+        esp_radio::ble::controller::BleConnector<'static>,
+        CONTROLLER_SLOTS,
+    >,
+) {
+    let mut resources: HostResources<DefaultPacketPool, CONNECTIONS_MAX, L2CAP_CHANNELS_MAX> =
+        HostResources::new();
+    let stack = trouble_host::new(controller, &mut resources);
+    let Host {
+        mut peripheral,
+        runner,
+        ..
+    } = stack.build();
+
+    info!("Starting advertising and GATT service");
+    let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig {
+        name: "ESP32 C-ITS",
+        appearance: &appearance::UNKNOWN,
+    }))
+    .unwrap();
+
+    let _ = embassy_futures::join::join(ble_task(runner), async {
+        loop {
+            match advertise("ESP32 C-ITS", &mut peripheral, &server).await {
+                Ok(conn) => {
+                    // set up tasks when the connection is established to a central, so they don't run when no one is connected.
+                    let a = gatt_events_task(&server, &conn);
+                    let b = custom_task(&server, &conn);
+                    // run until any task ends (usually because the connection has been closed),
+                    // then return to advertising state.
+                    embassy_futures::select::select(a, b).await;
+                }
+                Err(e) => {
+                    panic!("BLE ADV: fatal error {e:?}");
+                }
+            }
+        }
+    })
+    .await;
+}
+
+/// Create an advertiser to use to connect to a BLE Central, and wait for it to connect.
+async fn advertise<'values, 'server, C: Controller>(
+    name: &'values str,
+    peripheral: &mut Peripheral<'values, C, DefaultPacketPool>,
+    server: &'server Server<'values>,
+) -> Result<GattConnection<'values, 'server, DefaultPacketPool>, BleHostError<C::Error>> {
+    let mut advertiser_data = [0; 35];
+    let len = AdStructure::encode_slice(
+        &[
+            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
+            AdStructure::CompleteLocalName(name.as_bytes()),
+        ],
+        &mut advertiser_data[..],
+    )?;
+
+    let advertiser = peripheral
+        .advertise(
+            &trouble_host::advertise::AdvertisementParameters {
+                timeout: Some(embassy_time::Duration::from_secs(2)),
+                ..Default::default()
+            },
+            Advertisement::ConnectableScannableUndirected {
+                adv_data: &advertiser_data[..len],
+                scan_data: &[],
+            },
+        )
+        .await?;
+    // info!("[adv] advertising {len} bytes");
+    let conn = advertiser.accept().await?.with_attribute_server(server)?;
+    // info!("[adv] connection established");
+    Ok(conn)
+}
+
+async fn ble_task<C: Controller, P: PacketPool>(mut runner: Runner<'_, C, P>) {
+    loop {
+        if let Err(e) = runner.run().await {
+            panic!("BLE Task: fatal error {e:?}");
+        }
+    }
+}
+
+/// Stream Events until the connection closes.
+///
+/// This function will handle the GATT events and process them.
+/// This is how we interact with read and write requests.
+async fn gatt_events_task<P: PacketPool>(
+    server: &Server<'_>,
+    conn: &GattConnection<'_, '_, P>,
+) -> Result<(), Error> {
+    let reason = loop {
+        match conn.next().await {
+            GattConnectionEvent::Disconnected { reason } => break reason,
+            GattConnectionEvent::Gatt { event } => {
+                match &event {
+                    GattEvent::Write(event)
+                        if event.handle() == server.c_its_events.position.handle =>
+                    {
+                        use prost::Message as _;
+
+                        // parse and send to main thread
+                        match io::msg::PositionState::decode(event.data()) {
+                            Ok(data) => {
+                                critical_section::with(|cs| {
+                                    let rx_data_ref = RX_DATA.borrow(cs);
+                                    let _ = rx_data_ref.replace(Some(data));
+                                });
+                            }
+                            Err(err) => log::error!("BLE: Failed to parse input: {err:?}"),
+                        }
+                    }
+
+                    _ => {}
+                }
+
+                // This step is also performed at drop(), but writing it explicitly is necessary
+                // in order to ensure reply is sent.
+                match event.accept() {
+                    Ok(reply) => reply.send().await,
+                    Err(e) => warn!("BLE: error sending response: {e:?}"),
+                }
+            }
+            _ => {} // ignore other Gatt Connection Events
+        }
+    };
+
+    info!("BLE: disconnected b/c {reason:?}");
+    Ok(())
+}
+
+/// Example task to use the BLE notifier interface.
+/// This task will notify the connected central of a counter value every 2 seconds.
+/// It will also read the RSSI value every 2 seconds.
+/// and will stop when the connection is closed by the central or an error occurs.
+async fn custom_task<P: PacketPool>(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) {
+    loop {
+        embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await;
+
+        let mut new_data = None;
+        critical_section::with(|cs| {
+            let tx_data_ref = TX_DATA.borrow(cs);
+
+            if let Some(tx_data) = tx_data_ref.replace(None) {
+                new_data = Some(tx_data);
+            }
+        });
+
+        match new_data {
+            #[cfg(feature = "cam")]
+            Some(BleSendable::Cam(cam_event)) => {
+                let mut buf = cam_event.serialize_ble_proto();
+
+                if buf.len() <= CAM_EVENT_SIZE {
+                    // pad with zeros
+                    let pad = alloc::vec![0u8; CAM_EVENT_SIZE - buf.len()];
+                    buf.extend_from_slice(&pad);
+
+                    // unwrap should be fine since we ensured the vector size before
+                    let value = buf.try_into().unwrap();
+
+                    if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await {
+                        warn!("BLE: failed notifying connection of new CAM event: {err:?}");
+                    }
+                } else {
+                    warn!("BLE: CAM event too big with {} bytes", buf.len());
+                }
+            }
+            #[cfg(feature = "denm")]
+            Some(BleSendable::Denm(denm_event)) => {
+                let mut buf = denm_event.serialize_ble_proto();
+
+                if buf.len() <= DENM_EVENT_SIZE {
+                    // pad with zeros
+                    let pad = alloc::vec![0u8; DENM_EVENT_SIZE - buf.len()];
+                    buf.extend_from_slice(&pad);
+
+                    // unwrap should be fine since we ensured the vector size before
+                    let value = buf.try_into().unwrap();
+
+                    if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await {
+                        warn!("BLE: failed notifying connection of new DENM event: {err:?}");
+                    }
+                } else {
+                    warn!("BLE: DENM event too big with {} bytes", buf.len());
+                }
+            }
+            None => {}
+        }
+    }
+}