esp32-c_its-companion/src/ble.rs

#![allow(
    clippy::needless_borrows_for_generic_args,
    reason = "gatt_service creates false positives"
)]

use core::cell::RefCell;

use critical_section::Mutex;
use crossbeam_queue::ArrayQueue;
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 CLIENT_CONNECTED: Mutex<RefCell<bool>> = Mutex::new(RefCell::new(false));
pub static TX_DATA: Mutex<RefCell<Option<ArrayQueue<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")]
    CamRaw(io::msg::RawMsgRx),
    #[cfg(feature = "cam")]
    Cam(io::msg::CamEvent),

    #[cfg(feature = "denm")]
    DenmRaw(io::msg::RawMsgRx),
    #[cfg(feature = "denm")]
    Denm(io::msg::DenmEvent),

    #[cfg(feature = "spat")]
    MapemRaw(io::msg::RawMsgRx),
    #[cfg(feature = "spat")]
    SpatemRaw(io::msg::RawMsgRx),
}

#[gatt_server]
struct Server {
    c_its_events: CITSEvents,
}

#[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", read, value = "RawMsgRx proto msg", type = &'static str)]
    #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
    cam_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
    #[cfg(feature = "cam")]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)]
    #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_SIZE])]
    cam_event: [u8; io::msg::CamEvent::PROTO_SIZE],

    #[cfg(feature = "denm")]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)]
    #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
    denm_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
    #[cfg(feature = "denm")]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)]
    #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::DenmEvent::PROTO_SIZE])]
    denm_event: [u8; io::msg::DenmEvent::PROTO_SIZE],

    #[cfg(feature = "spat")]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)]
    #[characteristic(uuid = "c0b70001-d4f4-4006-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
    mapem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
    #[cfg(feature = "spat")]
    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)]
    #[characteristic(uuid = "c0b70001-d4f4-4007-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
    spatem_raw: [u8; io::msg::RawMsgRx::PROTO_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,
> {
    critical_section::with(|cs| {
        TX_DATA.borrow(cs).replace(Some(ArrayQueue::new(2)));
    });

    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 = publishing_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;

                    critical_section::with(|cs| {
                        let value_ref = CLIENT_CONNECTED.borrow(cs);
                        let _ = value_ref.replace(false);
                    });
                }
                Err(e) => {
                    panic!("BLE ADV: fatal error {e:?}");
                }
            }
        }
    })
    .await;
}

pub fn update(payload: BleSendable) {
    critical_section::with(|cs| {
        // only add to queue when someone is connected
        if *CLIENT_CONNECTED.borrow(cs).borrow() {
            let queue_rc = TX_DATA.borrow(cs).borrow();
            // unwrap is fine b/c we stored something in it before
            let queue = queue_rc.as_ref().unwrap();

            if queue.push(payload).is_err() {
                log::error!("BLE TX queue is full");
            }
        }
    });
}

/// 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(())
}

async fn publishing_task<P: PacketPool>(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) {
    critical_section::with(|cs| {
        let value_ref = CLIENT_CONNECTED.borrow(cs);
        let _ = value_ref.replace(true);
    });

    loop {
        embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await;

        let mut new_data = None;
        critical_section::with(|cs| {
            let queue_rc = TX_DATA.borrow(cs).borrow();
            // unwrap is fine b/c we stored something in it before
            let queue = queue_rc.as_ref().unwrap();

            if let Some(data) = queue.pop() {
                new_data = Some(data);
            }
        });

        match new_data {
            #[cfg(feature = "cam")]
            Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() {
                Ok(value) => {
                    if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await {
                        warn!("BLE: failed notifying connection of new CAM event: {err:?}");
                    }
                }
                Err(err) => warn!("BLE: CAM event {err}"),
            },

            #[cfg(feature = "denm")]
            Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() {
                Ok(value) => {
                    if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await {
                        warn!("BLE: failed notifying connection of new DENM event: {err:?}");
                    }
                }
                Err(err) => warn!("BLE: DENM event {err}"),
            },

            #[cfg(feature = "cam")]
            Some(BleSendable::CamRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
                Ok(value) => {
                    if let Err(err) = server.c_its_events.cam_raw.notify(conn, &value).await {
                        warn!("BLE: failed notifying connection of new raw CAM: {err:?}");
                    }
                }
                Err(err) => warn!("BLE: Raw CAM too big: {err}"),
            },
            #[cfg(feature = "denm")]
            Some(BleSendable::DenmRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
                Ok(value) => {
                    if let Err(err) = server.c_its_events.denm_raw.notify(conn, &value).await {
                        warn!("BLE: failed notifying connection of new raw DENM: {err:?}");
                    }
                }
                Err(err) => warn!("BLE: Raw DENM {err}"),
            },
            #[cfg(feature = "spat")]
            Some(BleSendable::MapemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
                Ok(value) => {
                    if let Err(err) = server.c_its_events.mapem_raw.notify(conn, &value).await {
                        warn!("BLE: failed notifying connection of new raw MAPEM: {err:?}");
                    }
                }
                Err(err) => warn!("BLE: Raw MAPEM {err}"),
            },
            #[cfg(feature = "spat")]
            Some(BleSendable::SpatemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() {
                Ok(value) => {
                    if let Err(err) = server.c_its_events.spatem_raw.notify(conn, &value).await {
                        warn!("BLE: failed notifying connection of new raw SPATEM: {err:?}");
                    }
                }
                Err(err) => warn!("BLE: Raw SPATEM {err}"),
            },
            None => {}
        }
    }
}