Add indicate functionality

host/src/att.rs

@@ -32,7 +32,7 @@ pub(crate) const ATT_READ_BLOB_REQ: u8 = 0x0c;
 pub(crate) const ATT_READ_BLOB_RSP: u8 = 0x0d;
 pub(crate) const ATT_HANDLE_VALUE_NTF: u8 = 0x1b;
 pub(crate) const ATT_HANDLE_VALUE_IND: u8 = 0x1d;
-pub(crate) const ATT_HANDLE_VALUE_CMF: u8 = 0x1e;
+pub(crate) const ATT_HANDLE_VALUE_CFM: u8 = 0x1e;
 
 /// Attribute Error Code
 ///
@@ -666,7 +666,7 @@ impl<'d> AttClient<'d> {
     fn decode_with_opcode(opcode: u8, r: ReadCursor<'d>) -> Result<Self, codec::Error> {
         let decoded = match opcode {
             ATT_WRITE_CMD => Self::Command(AttCmd::decode_with_opcode(opcode, r)?),
-            ATT_HANDLE_VALUE_CMF => Self::Confirmation(AttCfm::decode_with_opcode(opcode, r)?),
+            ATT_HANDLE_VALUE_CFM => Self::Confirmation(AttCfm::decode_with_opcode(opcode, r)?),
             _ => Self::Request(AttReq::decode_with_opcode(opcode, r)?),
         };
         Ok(decoded)
@@ -904,7 +904,7 @@ impl AttCfm {
         let mut w = WriteCursor::new(dest);
         match self {
             Self::ConfirmIndication => {
-                w.write(ATT_HANDLE_VALUE_CMF)?;
+                w.write(ATT_HANDLE_VALUE_CFM)?;
             }
         }
         Ok(())
@@ -913,7 +913,7 @@ impl AttCfm {
     fn decode_with_opcode(opcode: u8, r: ReadCursor<'_>) -> Result<Self, codec::Error> {
         let payload = r.remaining();
         match opcode {
-            ATT_HANDLE_VALUE_CMF => Ok(Self::ConfirmIndication),
+            ATT_HANDLE_VALUE_CFM => Ok(Self::ConfirmIndication),
             code => {
                 warn!("[att] unknown opcode {:x}", code);
                 Err(codec::Error::InvalidValue)

host/src/attribute.rs

@@ -9,7 +9,9 @@ use embassy_sync::blocking_mutex::raw::RawMutex;
 use embassy_sync::blocking_mutex::Mutex;
 use heapless::Vec;
 
-use crate::att::AttErrorCode;
+use crate::att::{AttErrorCode, AttUns};
+use crate::gatt;
+
 use crate::attribute_server::AttributeServer;
 use crate::cursor::{ReadCursor, WriteCursor};
 use crate::prelude::{AsGatt, FixedGattValue, FromGatt, GattConnection};
@@ -654,18 +656,44 @@ impl<T: FromGatt> Characteristic<T> {
             return Ok(());
         }
 
-        let mut tx = P::allocate().ok_or(Error::OutOfMemory)?;
-        let mut w = WriteCursor::new(tx.as_mut());
-        let (mut header, mut data) = w.split(4)?;
-        data.write(crate::att::ATT_HANDLE_VALUE_NTF)?;
-        data.write(self.handle)?;
-        data.append(value)?;
+        let uns = AttUns::Notify {
+            handle: self.handle,
+            data: value,
+        };
+        let pdu = gatt::assemble(connection, crate::att::AttServer::Unsolicited(uns))?;
+        connection.send(pdu).await;
+        Ok(())
+    }
+
+    /// Write a value to a characteristic, and indicate a connection with the new value of the characteristic.
+    ///
+    /// If the provided connection has not subscribed for this characteristic, it will not be sent an indication.
+    ///
+    /// If the characteristic does not support indications, an error is returned.
+    ///
+    /// This function does not block for the confirmation to the indication message, if the client sends a confirmation
+    /// this will be seen on the [GattConnection] as a [crate::att::AttClient::Confirmation] event.
+    pub async fn indicate<P: PacketPool>(
+        &self,
+        connection: &GattConnection<'_, '_, P>,
+        value: &T,
+    ) -> Result<(), Error> {
+        let value = value.as_gatt();
+        let server = connection.server;
+        server.set(self.handle, value)?;
 
-        header.write(data.len() as u16)?;
-        header.write(4_u16)?;
-        let total = header.len() + data.len();
+        let cccd_handle = self.cccd_handle.ok_or(Error::NotFound)?;
+        let connection = connection.raw();
+        if !server.should_indicate(connection, cccd_handle) {
+            // No reason to fail?
+            return Ok(());
+        }
 
-        let pdu = crate::pdu::Pdu::new(tx, total);
+        let uns = AttUns::Indicate {
+            handle: self.handle,
+            data: value,
+        };
+        let pdu = gatt::assemble(connection, crate::att::AttServer::Unsolicited(uns))?;
         connection.send(pdu).await;
         Ok(())
     }
@@ -937,4 +965,15 @@ impl CCCD {
     pub fn should_notify(&self) -> bool {
         (self.0 & (CCCDFlag::Notify as u16)) != 0
     }
+
+    /// Enable or disable indication
+    pub fn set_indicate(&mut self, is_enabled: bool) {
+        let mask: u16 = CCCDFlag::Indicate as u16;
+        self.0 = if is_enabled { self.0 | mask } else { self.0 & !mask };
+    }
+
+    /// Check if indications are enabled
+    pub fn should_indicate(&self) -> bool {
+        (self.0 & (CCCDFlag::Indicate as u16)) != 0
+    }
 }

host/src/attribute_server.rs

@@ -91,6 +91,24 @@ impl<const ENTRIES: usize> CccdTable<ENTRIES> {
         }
         false
     }
+
+    fn set_indicate(&mut self, cccd_handle: u16, is_enabled: bool) {
+        for (handle, value) in self.inner.iter_mut() {
+            if *handle == cccd_handle {
+                trace!("\n\n\n[cccd] set_indicate({}) = {}", cccd_handle, is_enabled);
+                value.set_indicate(is_enabled);
+                break;
+            }
+        }
+    }
+    fn should_indicate(&self, cccd_handle: u16) -> bool {
+        for (handle, value) in self.inner.iter() {
+            if *handle == cccd_handle {
+                return value.should_indicate();
+            }
+        }
+        false
+    }
 }
 
 /// A table of CCCD values for each connected client.
@@ -203,6 +221,30 @@ impl<M: RawMutex, const CCCD_MAX: usize, const CONN_MAX: usize> CccdTables<M, CC
         })
     }
 
+    fn set_indicate(&self, peer_identity: &Identity, cccd_handle: u16, is_enabled: bool) {
+        self.state.lock(|n| {
+            let mut n = n.borrow_mut();
+            for (client, table) in n.iter_mut() {
+                if client.identity.match_identity(peer_identity) {
+                    table.set_indicate(cccd_handle, is_enabled);
+                    break;
+                }
+            }
+        })
+    }
+
+    fn should_indicate(&self, peer_identity: &Identity, cccd_handle: u16) -> bool {
+        self.state.lock(|n| {
+            let n = n.borrow();
+            for (client, table) in n.iter() {
+                if client.identity.match_identity(peer_identity) {
+                    return table.should_indicate(cccd_handle);
+                }
+            }
+            false
+        })
+    }
+
     fn get_cccd_table(&self, peer_identity: &Identity) -> Option<CccdTable<CCCD_MAX>> {
         self.state.lock(|n| {
             let n = n.borrow();
@@ -269,6 +311,7 @@ pub(crate) mod sealed {
             rx: &mut [u8],
         ) -> Result<Option<usize>, Error>;
         fn should_notify(&self, connection: &Connection<'_, P>, cccd_handle: u16) -> bool;
+        fn should_indicate(&self, connection: &Connection<'_, P>, cccd_handle: u16) -> bool;
         fn set(&self, characteristic: u16, input: &[u8]) -> Result<(), Error>;
         fn update_identity(&self, identity: Identity) -> Result<(), Error>;
     }
@@ -305,6 +348,9 @@ impl<M: RawMutex, P: PacketPool, const ATT_MAX: usize, const CCCD_MAX: usize, co
     fn should_notify(&self, connection: &Connection<'_, P>, cccd_handle: u16) -> bool {
         AttributeServer::should_notify(self, connection, cccd_handle)
     }
+    fn should_indicate(&self, connection: &Connection<'_, P>, cccd_handle: u16) -> bool {
+        AttributeServer::should_indicate(self, connection, cccd_handle)
+    }
 
     fn set(&self, characteristic: u16, input: &[u8]) -> Result<(), Error> {
         self.att_table.set_raw(characteristic, input)
@@ -338,6 +384,11 @@ impl<'values, M: RawMutex, P: PacketPool, const ATT_MAX: usize, const CCCD_MAX:
         self.cccd_tables.should_notify(&connection.peer_identity(), cccd_handle)
     }
 
+    pub(crate) fn should_indicate(&self, connection: &Connection<'_, P>, cccd_handle: u16) -> bool {
+        self.cccd_tables
+            .should_indicate(&connection.peer_identity(), cccd_handle)
+    }
+
     fn read_attribute_data(
         &self,
         connection: &Connection<'_, P>,
@@ -372,6 +423,8 @@ impl<'values, M: RawMutex, P: PacketPool, const ATT_MAX: usize, const CCCD_MAX:
             {
                 self.cccd_tables
                     .set_notify(&connection.peer_identity(), att.handle, notifications);
+                self.cccd_tables
+                    .set_indicate(&connection.peer_identity(), att.handle, indications);
             }
         }
         err

host/src/gatt.rs

@@ -255,14 +255,14 @@ impl<'stack, P: PacketPool> GattData<'stack, P> {
 
     /// Respond directly to request.
     pub async fn reply(self, rsp: AttRsp<'_>) -> Result<(), Error> {
-        let pdu = send(&self.connection, AttServer::Response(rsp))?;
+        let pdu = assemble(&self.connection, AttServer::Response(rsp))?;
         self.connection.send(pdu).await;
         Ok(())
     }
 
     /// Send an unsolicited ATT PDU without having a request (e.g. notification or indication)
     pub async fn send_unsolicited(connection: &Connection<'_, P>, uns: AttUns<'_>) -> Result<(), Error> {
-        let pdu = send(connection, AttServer::Unsolicited(uns))?;
+        let pdu = assemble(connection, AttServer::Unsolicited(uns))?;
         connection.send(pdu).await;
         Ok(())
     }
@@ -563,11 +563,14 @@ fn process_reject<'stack, P: PacketPool>(
     // We know it has been checked, therefore this cannot fail
     let request = pdu.as_ref()[0];
     let rsp = AttRsp::Error { request, handle, code };
-    let pdu = send(connection, AttServer::Response(rsp))?;
+    let pdu = assemble(connection, AttServer::Response(rsp))?;
     Ok(Reply::new(connection.clone(), Some(pdu)))
 }
 
-fn send<'stack, P: PacketPool>(conn: &Connection<'stack, P>, att: AttServer<'_>) -> Result<Pdu<P::Packet>, Error> {
+pub(crate) fn assemble<'stack, P: PacketPool>(
+    conn: &Connection<'stack, P>,
+    att: AttServer<'_>,
+) -> Result<Pdu<P::Packet>, Error> {
     let mut tx = P::allocate().ok_or(Error::OutOfMemory)?;
     let mut w = WriteCursor::new(tx.as_mut());
     let (mut header, mut data) = w.split(4)?;