Add GATT server registeration

3 files changed, 201 insertions, 58 deletions

diff --git a/README.md b/README.md
index eee1ab9..8c4e7f2 100644
--- a/README.md
+++ b/README.md
@@ -31,13 +31,13 @@ The following softdevices are supported.
 - S113 (peripheral only)
 - S122 (central only)
 - S132 (central and peripheral)
-- S140 (central and peripheral)
+- S140 v7.x.x (central and peripheral)
 
 The following nRF chips are supported
 
 - nRF52810
 - nRF52832
-- nRF52832
+- nRF52833
 - nRF52840
 
 Some softdevices support only some chips, check Nordic's documentation for details.
diff --git a/examples/src/bin/ble_bas_peripheral.rs b/examples/src/bin/ble_bas_peripheral.rs
index b330b91..6c16940 100644
--- a/examples/src/bin/ble_bas_peripheral.rs
+++ b/examples/src/bin/ble_bas_peripheral.rs
@@ -10,7 +10,8 @@ use core::mem;
 use cortex_m_rt::entry;
 use defmt::info;
 
-use nrf_softdevice::ble::{peripheral, Uuid};
+use nrf_softdevice::ble::gatt_server::{Characteristic, CharacteristicHandles, RegisterError};
+use nrf_softdevice::ble::{gatt_server, peripheral, Uuid};
 use nrf_softdevice::{raw, RawError, Softdevice};
 
 #[static_executor::task]
@@ -21,64 +22,42 @@ async fn softdevice_task(sd: &'static Softdevice) {
 const GATT_BAS_SVC_UUID: Uuid = Uuid::new_16(0x180F);
 const GATT_BAS_BATTERY_LEVEL_CHAR_UUID: Uuid = Uuid::new_16(0x2A19);
 
-#[static_executor::task]
-async fn bluetooth_task(sd: &'static Softdevice) {
-    // There'll eventually be a safe API for creating GATT servers.
-    // but for now this allows us to test ble_bas_central.
-
-    let mut service_handle: u16 = 0;
-    let ret = unsafe {
-        raw::sd_ble_gatts_service_add(
-            raw::BLE_GATTS_SRVC_TYPE_PRIMARY as u8,
-            GATT_BAS_SVC_UUID.as_raw_ptr(),
-            &mut service_handle as _,
-        )
-    };
-    RawError::convert(ret).dewrap();
+struct BatteryServiceServer {
+    battery_level_value_handle: u16,
+    battery_level_cccd_handle: u16,
+}
 
-    let mut val: u8 = 123;
+impl gatt_server::Server for BatteryServiceServer {
+    fn uuid() -> Uuid {
+        GATT_BAS_SVC_UUID
+    }
 
-    let mut cccd_attr_md: raw::ble_gatts_attr_md_t = unsafe { mem::zeroed() };
-    cccd_attr_md.read_perm = raw::ble_gap_conn_sec_mode_t {
-        _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
-    };
-    cccd_attr_md.write_perm = raw::ble_gap_conn_sec_mode_t {
-        _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
-    };
-    cccd_attr_md.set_vloc(raw::BLE_GATTS_VLOC_STACK as u8);
+    fn register<F>(service_handle: u16, mut register_char: F) -> Result<Self, RegisterError>
+    where
+        F: FnMut(Characteristic, &[u8]) -> Result<CharacteristicHandles, RegisterError>,
+    {
+        let battery_level = register_char(
+            Characteristic {
+                uuid: GATT_BAS_BATTERY_LEVEL_CHAR_UUID,
+                can_indicate: false,
+                can_notify: true,
+                can_read: true,
+                can_write: true,
+                max_len: 1,
+            },
+            &[123],
+        )?;
 
-    let mut attr_md: raw::ble_gatts_attr_md_t = unsafe { mem::zeroed() };
-    attr_md.read_perm = raw::ble_gap_conn_sec_mode_t {
-        _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
-    };
-    attr_md.write_perm = raw::ble_gap_conn_sec_mode_t {
-        _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
-    };
-    attr_md.set_vloc(raw::BLE_GATTS_VLOC_STACK as u8);
-
-    let mut attr: raw::ble_gatts_attr_t = unsafe { mem::zeroed() };
-    attr.p_uuid = unsafe { GATT_BAS_BATTERY_LEVEL_CHAR_UUID.as_raw_ptr() };
-    attr.p_attr_md = &attr_md as _;
-    attr.init_len = 1;
-    attr.max_len = 1;
-    attr.p_value = &mut val;
-
-    let mut char_md: raw::ble_gatts_char_md_t = unsafe { mem::zeroed() };
-    char_md.char_props.set_read(1);
-    char_md.char_props.set_notify(1);
-    char_md.p_cccd_md = &mut cccd_attr_md;
-
-    let mut char_handles: raw::ble_gatts_char_handles_t = unsafe { mem::zeroed() };
-
-    let ret = unsafe {
-        raw::sd_ble_gatts_characteristic_add(
-            service_handle,
-            &mut char_md as _,
-            &mut attr as _,
-            &mut char_handles as _,
-        )
-    };
-    RawError::convert(ret).dewrap();
+        Ok(Self {
+            battery_level_cccd_handle: battery_level.cccd_handle,
+            battery_level_value_handle: battery_level.value_handle,
+        })
+    }
+}
+
+#[static_executor::task]
+async fn bluetooth_task(sd: &'static Softdevice) {
+    let server: BatteryServiceServer = gatt_server::register(sd).dewrap();
 
     #[rustfmt::skip]
     let adv_data = &[
diff --git a/nrf-softdevice/src/ble/gatt_server.rs b/nrf-softdevice/src/ble/gatt_server.rs
index 628fe48..739c830 100644
--- a/nrf-softdevice/src/ble/gatt_server.rs
+++ b/nrf-softdevice/src/ble/gatt_server.rs
@@ -3,9 +3,173 @@
 //! Typically the peripheral device is the GATT server, but it is not necessary.
 //! In a connection any device can be server and client, and even both can be both at the same time.
 
+use core::convert::TryInto;
+use core::mem;
 use core::ptr;
 
+use crate::ble::types::*;
 use crate::raw;
+use crate::util::*;
+use crate::RawError;
+use crate::Softdevice;
+
+pub struct Characteristic {
+    pub uuid: Uuid,
+    pub can_read: bool,
+    pub can_write: bool,
+    pub can_notify: bool,
+    pub can_indicate: bool,
+    pub max_len: usize,
+}
+
+pub struct CharacteristicHandles {
+    pub value_handle: u16,
+    pub user_desc_handle: u16,
+    pub cccd_handle: u16,
+    pub sccd_handle: u16,
+}
+
+pub trait Server: Sized {
+    fn uuid() -> Uuid;
+    fn register<F>(service_handle: u16, register_char: F) -> Result<Self, RegisterError>
+    where
+        F: FnMut(Characteristic, &[u8]) -> Result<CharacteristicHandles, RegisterError>;
+}
+
+#[derive(defmt::Format)]
+pub enum RegisterError {
+    Raw(RawError),
+}
+
+impl From<RawError> for RegisterError {
+    fn from(err: RawError) -> Self {
+        RegisterError::Raw(err)
+    }
+}
+
+pub fn register<S: Server>(_sd: &Softdevice) -> Result<S, RegisterError> {
+    info!("hi");
+
+    let mut service_handle: u16 = 0;
+    let ret = unsafe {
+        raw::sd_ble_gatts_service_add(
+            raw::BLE_GATTS_SRVC_TYPE_PRIMARY as u8,
+            S::uuid().as_raw_ptr(),
+            &mut service_handle as _,
+        )
+    };
+    RawError::convert(ret)?;
+
+    S::register(service_handle, |char, initial_value| {
+        let mut cccd_attr_md: raw::ble_gatts_attr_md_t = unsafe { mem::zeroed() };
+        cccd_attr_md.read_perm = raw::ble_gap_conn_sec_mode_t {
+            _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
+        };
+        cccd_attr_md.write_perm = raw::ble_gap_conn_sec_mode_t {
+            _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
+        };
+        cccd_attr_md.set_vloc(raw::BLE_GATTS_VLOC_STACK as u8);
+
+        let mut attr_md: raw::ble_gatts_attr_md_t = unsafe { mem::zeroed() };
+        attr_md.read_perm = raw::ble_gap_conn_sec_mode_t {
+            _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
+        };
+        attr_md.write_perm = raw::ble_gap_conn_sec_mode_t {
+            _bitfield_1: raw::ble_gap_conn_sec_mode_t::new_bitfield_1(1, 1),
+        };
+        attr_md.set_vloc(raw::BLE_GATTS_VLOC_STACK as u8);
+
+        let mut attr: raw::ble_gatts_attr_t = unsafe { mem::zeroed() };
+        attr.p_uuid = unsafe { char.uuid.as_raw_ptr() };
+        attr.p_attr_md = &attr_md as _;
+        attr.max_len = char.max_len as _;
+
+        attr.p_value = initial_value.as_ptr() as *mut u8;
+        attr.init_len = initial_value.len() as _;
+
+        let mut char_md: raw::ble_gatts_char_md_t = unsafe { mem::zeroed() };
+        char_md.char_props.set_read(char.can_read as u8);
+        char_md.char_props.set_write(char.can_write as u8);
+        char_md.char_props.set_notify(char.can_notify as u8);
+        char_md.char_props.set_indicate(char.can_indicate as u8);
+        char_md.p_cccd_md = &mut cccd_attr_md;
+
+        let mut handles: raw::ble_gatts_char_handles_t = unsafe { mem::zeroed() };
+
+        let ret = unsafe {
+            raw::sd_ble_gatts_characteristic_add(
+                service_handle,
+                &mut char_md as _,
+                &mut attr as _,
+                &mut handles as _,
+            )
+        };
+        RawError::convert(ret)?;
+
+        Ok(CharacteristicHandles {
+            value_handle: handles.value_handle,
+            user_desc_handle: handles.user_desc_handle,
+            cccd_handle: handles.cccd_handle,
+            sccd_handle: handles.sccd_handle,
+        })
+    })
+}
+
+#[derive(defmt::Format)]
+pub enum GetValueError {
+    Truncated,
+    Raw(RawError),
+}
+
+impl From<RawError> for GetValueError {
+    fn from(err: RawError) -> Self {
+        Self::Raw(err)
+    }
+}
+
+pub fn get_value(_sd: &Softdevice, handle: u16, buf: &mut [u8]) -> Result<usize, GetValueError> {
+    let mut value = raw::ble_gatts_value_t {
+        p_value: buf.as_mut_ptr(),
+        len: buf.len() as _,
+        offset: 0,
+    };
+    let ret = unsafe {
+        raw::sd_ble_gatts_value_get(raw::BLE_CONN_HANDLE_INVALID as u16, handle, &mut value)
+    };
+    RawError::convert(ret)?;
+
+    if value.len as usize > buf.len() {
+        return Err(GetValueError::Truncated);
+    }
+
+    Ok(value.len as _)
+}
+
+#[derive(defmt::Format)]
+pub enum SetValueError {
+    Truncated,
+    Raw(RawError),
+}
+
+impl From<RawError> for SetValueError {
+    fn from(err: RawError) -> Self {
+        Self::Raw(err)
+    }
+}
+
+pub fn set_value(_sd: &Softdevice, handle: u16, val: &[u8]) -> Result<(), SetValueError> {
+    let mut value = raw::ble_gatts_value_t {
+        p_value: val.as_ptr() as _,
+        len: val.len() as _,
+        offset: 0,
+    };
+    let ret = unsafe {
+        raw::sd_ble_gatts_value_set(raw::BLE_CONN_HANDLE_INVALID as u16, handle, &mut value)
+    };
+    RawError::convert(ret)?;
+
+    Ok(())
+}
 
 pub(crate) unsafe fn on_write(_ble_evt: *const raw::ble_evt_t, _gattc_evt: &raw::ble_gatts_evt_t) {}