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 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 => {}
        }
    }
}