[Private GATT] Add GATT server + module

Bug: 255880936
Test: unit
Change-Id: I94fafe8afc68dc31501f274099ef6f3a684009a1

12 files changed, 789 insertions, 3 deletions

diff --git a/system/rust/Android.bp b/system/rust/Android.bp
index 3e0f2cb8bb..1e861f36d3 100644
--- a/system/rust/Android.bp
+++ b/system/rust/Android.bp
@@ -23,6 +23,7 @@ rust_defaults {
     ],
     rustlibs: [
         "liblog_rust",
+        "libanyhow",
         "libcxx",
         "libtokio",
         "libbt_common",
diff --git a/system/rust/Cargo.toml b/system/rust/Cargo.toml
index 972b230941..a283d7ac5f 100644
--- a/system/rust/Cargo.toml
+++ b/system/rust/Cargo.toml
@@ -22,7 +22,8 @@ edition = "2021"
 bt_common = { path = "../gd/rust/common", default-features = false }
 
 # External dependencies
-# Note: source-of-truth is Android.bp, these are mirrored solely for IDE convenient
+# Note: source-of-truth is Android.bp, these are mirrored solely for IDE convenience
+anyhow = "1.0"
 log = "*"
 cxx = "*"
 android_logger = "*"
diff --git a/system/rust/src/gatt.rs b/system/rust/src/gatt.rs
index c80114dc86..4b40b1e905 100644
--- a/system/rust/src/gatt.rs
+++ b/system/rust/src/gatt.rs
@@ -1,5 +1,7 @@
 //! This module is a simple GATT server that shares the ATT channel with the
 //! existing C++ GATT client. See go/private-gatt-in-platform for the design.
 
+pub mod channel;
 pub mod ids;
+pub mod mocks;
 pub mod server;
diff --git a/system/rust/src/gatt/channel.rs b/system/rust/src/gatt/channel.rs
new file mode 100644
index 0000000000..6625eb1911
--- /dev/null
+++ b/system/rust/src/gatt/channel.rs
@@ -0,0 +1,22 @@
+//! This represents the TX end of an ATT Transport, to be either mocked (in
+//! test) or linked to FFI (in production).
+
+use crate::packets::{AttBuilder, SerializeError};
+
+use super::ids::TransportIndex;
+
+/// An instance of this trait will be provided to the GattModule on
+/// initialization.
+pub trait AttTransport {
+    /// Serializes and sends a packet to the device associated with the
+    /// specified transport. Note that the packet may be dropped if the link
+    /// is disconnected, but the result will still be Ok(()).
+    ///
+    /// The tcb_idx is an identifier for this transport supplied from the
+    /// native stack, and represents an underlying ACL-LE connection.
+    fn send_packet(
+        &self,
+        tcb_idx: TransportIndex,
+        packet: AttBuilder,
+    ) -> Result<(), SerializeError>;
+}
diff --git a/system/rust/src/gatt/ids.rs b/system/rust/src/gatt/ids.rs
index 712183d0b5..5db940fb05 100644
--- a/system/rust/src/gatt/ids.rs
+++ b/system/rust/src/gatt/ids.rs
@@ -1,6 +1,45 @@
 //! These are strongly-typed identifiers representing the various objects
 //! interacted with, mostly over FFI
 
+/// The ID of a connection at the GATT layer.
+/// A ConnectionId is logically a (TransportIndex, ServerId) tuple,
+/// where each contribute 8 bits to the 16-bit value.
+#[derive(Debug, Copy, Clone, PartialEq, Hash, Eq, PartialOrd, Ord)]
+pub struct ConnectionId(pub u16);
+
+impl ConnectionId {
+    /// Create a ConnectionId from a TransportIndex and ServerId
+    pub const fn new(tcb_idx: TransportIndex, server_id: ServerId) -> ConnectionId {
+        ConnectionId(((tcb_idx.0 as u16) << 8) + (server_id.0 as u16))
+    }
+
+    /// Extract the TransportIndex from a ConnectionId (upper 8 bits)
+    pub fn get_tcb_idx(&self) -> TransportIndex {
+        TransportIndex((self.0 >> 8) as u8)
+    }
+
+    /// Extract the ServerId from a ConnectionId (lower 8 bits)
+    pub fn get_server_id(&self) -> ServerId {
+        ServerId((self.0 & (u8::MAX as u16)) as u8)
+    }
+}
+
+/// The server_if of a GATT server registered in legacy
+#[derive(Debug, Copy, Clone, PartialEq, Hash, Eq)]
+pub struct ServerId(pub u8);
+
+/// An arbitrary id representing a GATT transaction (request/response)
+#[derive(Debug, Copy, Clone, PartialEq, Hash, Eq)]
+pub struct TransactionId(pub u32);
+
+/// The TCB index in legacy GATT
+#[derive(Debug, Copy, Clone, PartialEq, Hash, Eq)]
+pub struct TransportIndex(pub u8);
+
+/// An advertising set ID (zero-based)
+#[derive(Debug, Copy, Clone, PartialEq, Hash, Eq)]
+pub struct AdvertiserId(pub u8);
+
 /// The handle of a given ATT attribute
 #[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, PartialOrd, Ord)]
 pub struct AttHandle(pub u16);
diff --git a/system/rust/src/gatt/mocks.rs b/system/rust/src/gatt/mocks.rs
new file mode 100644
index 0000000000..c9152c1226
--- /dev/null
+++ b/system/rust/src/gatt/mocks.rs
@@ -0,0 +1,2 @@
+//! Mocks for the GattDatastore + AttTransport traits, for use in test
+pub mod mock_transport;
diff --git a/system/rust/src/gatt/mocks/mock_transport.rs b/system/rust/src/gatt/mocks/mock_transport.rs
new file mode 100644
index 0000000000..9227eeb127
--- /dev/null
+++ b/system/rust/src/gatt/mocks/mock_transport.rs
@@ -0,0 +1,31 @@
+//! Mocked implementation of AttTransport for use in test
+
+use crate::{
+    gatt::{channel::AttTransport, ids::TransportIndex},
+    packets::{AttBuilder, Serializable, SerializeError},
+};
+use tokio::sync::mpsc::{self, unbounded_channel, UnboundedReceiver};
+
+/// Routes calls to AttTransport into a channel containing AttBuilders
+pub struct MockAttTransport(mpsc::UnboundedSender<(TransportIndex, AttBuilder)>);
+
+impl MockAttTransport {
+    /// Constructor. Returns Self and the RX side of a channel containing
+    /// AttBuilders sent on TransportIndices
+    pub fn new() -> (Self, UnboundedReceiver<(TransportIndex, AttBuilder)>) {
+        let (tx, rx) = unbounded_channel();
+        (Self(tx), rx)
+    }
+}
+
+impl AttTransport for MockAttTransport {
+    fn send_packet(
+        &self,
+        tcb_idx: TransportIndex,
+        packet: AttBuilder,
+    ) -> Result<(), SerializeError> {
+        packet.to_vec()?; // trigger SerializeError if needed
+        self.0.send((tcb_idx, packet)).unwrap();
+        Ok(())
+    }
+}
diff --git a/system/rust/src/gatt/server.rs b/system/rust/src/gatt/server.rs
index 67fd1d7aa6..44a6650383 100644
--- a/system/rust/src/gatt/server.rs
+++ b/system/rust/src/gatt/server.rs
@@ -3,9 +3,118 @@
 
 mod att_database;
 pub mod att_server_bearer;
+pub mod gatt_database;
 mod transaction_handler;
 mod transactions;
 
 #[cfg(test)]
 mod test;
 mod utils;
+
+use std::{collections::HashMap, rc::Rc};
+
+use crate::{
+    gatt::{ids::ConnectionId, server::gatt_database::GattDatabase},
+    packets::AttView,
+};
+
+use self::{
+    super::ids::ServerId,
+    att_server_bearer::AttServerBearer,
+    gatt_database::{AttDatabaseImpl, GattServiceWithHandle},
+};
+
+use super::{channel::AttTransport, ids::AttHandle};
+use anyhow::{anyhow, bail, Result};
+use log::info;
+
+#[allow(missing_docs)]
+pub struct GattModule {
+    connection_bearers: HashMap<ConnectionId, Rc<AttServerBearer<AttDatabaseImpl>>>,
+    databases: HashMap<ServerId, Rc<GattDatabase>>,
+    transport: Rc<dyn AttTransport>,
+}
+
+impl GattModule {
+    /// Constructor.
+    pub fn new(transport: Rc<dyn AttTransport>) -> Self {
+        Self { connection_bearers: HashMap::new(), databases: HashMap::new(), transport }
+    }
+
+    /// Handle LE link connect
+    pub fn on_le_connect(&mut self, conn_id: ConnectionId) -> Result<()> {
+        info!("connected on conn_id {conn_id:?}");
+        let database = self.databases.get(&conn_id.get_server_id());
+        let Some(database) = database else {
+            bail!(
+                "got connection to conn_id {conn_id:?} (server_id {:?}) but this server does not exist!",
+                conn_id.get_server_id(),
+            );
+        };
+        let transport = self.transport.clone();
+        self.connection_bearers.insert(
+            conn_id,
+            AttServerBearer::new(database.get_att_database(), move |packet| {
+                transport.send_packet(conn_id.get_tcb_idx(), packet)
+            }),
+        );
+        Ok(())
+    }
+
+    /// Handle an LE link disconnect
+    pub fn on_le_disconnect(&mut self, conn_id: ConnectionId) {
+        info!("disconnected conn_id {conn_id:?}");
+        self.connection_bearers.remove(&conn_id);
+    }
+
+    /// Handle an incoming ATT packet
+    pub fn handle_packet(&mut self, conn_id: ConnectionId, packet: AttView<'_>) -> Result<()> {
+        self.connection_bearers
+            .get(&conn_id)
+            .ok_or_else(|| anyhow!("dropping ATT packet for unregistered connection"))?
+            .handle_packet(packet);
+        Ok(())
+    }
+
+    /// Register a new GATT service on a given server
+    pub fn register_gatt_service(
+        &mut self,
+        server_id: ServerId,
+        service: GattServiceWithHandle,
+    ) -> Result<()> {
+        self.databases
+            .get(&server_id)
+            .ok_or_else(|| anyhow!("server {server_id:?} not opened"))?
+            .add_service_with_handles(service)
+    }
+
+    /// Unregister an existing GATT service on a given server
+    pub fn unregister_gatt_service(
+        &mut self,
+        server_id: ServerId,
+        service_handle: AttHandle,
+    ) -> Result<()> {
+        self.databases
+            .get(&server_id)
+            .ok_or_else(|| anyhow!("server {server_id:?} not opened"))?
+            .remove_service_at_handle(service_handle)
+    }
+
+    /// Open a GATT server
+    pub fn open_gatt_server(&mut self, server_id: ServerId) -> Result<()> {
+        let old = self.databases.insert(server_id, GattDatabase::new().into());
+        if old.is_some() {
+            bail!("GATT server {server_id:?} already exists but was re-opened, clobbering old value...")
+        }
+        Ok(())
+    }
+
+    /// Close a GATT server
+    pub fn close_gatt_server(&mut self, server_id: ServerId) -> Result<()> {
+        let old = self.databases.remove(&server_id);
+        if old.is_none() {
+            bail!("GATT server {server_id:?} did not exist")
+        }
+        Ok(())
+    }
+}
diff --git a/system/rust/src/gatt/server/att_database.rs b/system/rust/src/gatt/server/att_database.rs
index c47a262533..227c439a84 100644
--- a/system/rust/src/gatt/server/att_database.rs
+++ b/system/rust/src/gatt/server/att_database.rs
@@ -6,6 +6,10 @@ use crate::{
     packets::{AttAttributeDataChild, AttErrorCode, AttHandleBuilder, AttHandleView},
 };
 
+// UUIDs from Bluetooth Assigned Numbers Sec 3.6
+pub const PRIMARY_SERVICE_DECLARATION_UUID: Uuid = Uuid::new(0x2800);
+pub const CHARACTERISTIC_UUID: Uuid = Uuid::new(0x2803);
+
 impl From<AttHandleView<'_>> for AttHandle {
     fn from(value: AttHandleView) -> Self {
         AttHandle(value.get_handle())
@@ -18,7 +22,7 @@ impl From<AttHandle> for AttHandleBuilder {
     }
 }
 
-#[derive(Debug, Clone)]
+#[derive(Debug, Clone, PartialEq, Eq)]
 pub struct AttAttribute {
     pub handle: AttHandle,
     pub type_: Uuid,
@@ -27,7 +31,7 @@ pub struct AttAttribute {
 
 /// The attribute properties supported by the current GATT server implementation
 /// Unimplemented properties will default to false.
-#[derive(Debug, Clone)]
+#[derive(Debug, Clone, PartialEq, Eq)]
 pub struct AttPermissions {
     /// Whether an attribute is readable
     pub readable: bool,
diff --git a/system/rust/src/gatt/server/gatt_database.rs b/system/rust/src/gatt/server/gatt_database.rs
new file mode 100644
index 0000000000..fad9ea24d6
--- /dev/null
+++ b/system/rust/src/gatt/server/gatt_database.rs
@@ -0,0 +1,473 @@
+//! This module converts a GattDatastore to an AttDatabase,
+//! by converting a registry of services into a list of attributes, and proxying
+//! ATT read/write requests into characteristic reads/writes
+
+use std::{cell::RefCell, collections::BTreeMap, rc::Rc};
+
+use anyhow::{anyhow, bail, Result};
+use async_trait::async_trait;
+
+use crate::{
+    core::uuid::Uuid,
+    gatt::ids::AttHandle,
+    packets::{
+        AttAttributeDataChild, AttCharacteristicPropertiesBuilder, AttErrorCode,
+        GattCharacteristicDeclarationValueBuilder, GattServiceDeclarationValueBuilder, UuidBuilder,
+    },
+};
+
+use super::att_database::{
+    AttAttribute, AttDatabase, CHARACTERISTIC_UUID, PRIMARY_SERVICE_DECLARATION_UUID,
+};
+
+pub use super::att_database::AttPermissions;
+
+/// A GattService (currently, only primary services are supported) has an
+/// identifying UUID and a list of contained characteristics, as well as a
+/// handle (indicating the attribute where the service declaration will live)
+#[derive(Debug, Clone)]
+pub struct GattServiceWithHandle {
+    /// The handle of the service declaration
+    pub handle: AttHandle,
+    /// The type of the service
+    pub type_: Uuid,
+    /// A list of contained characteristics (that must have handles between the
+    /// service declaration handle, and that of the next service)
+    pub characteristics: Vec<GattCharacteristicWithHandle>,
+}
+
+/// A GattCharacteristic consists of a handle (where the value attribute lives),
+/// a UUID identifying its type, and permissions indicating what operations can
+/// be performed
+#[derive(Debug, Clone)]
+pub struct GattCharacteristicWithHandle {
+    /// The handle of the characteristic value attribute. The characteristic
+    /// declaration is one before this handle.
+    pub handle: AttHandle,
+    /// The UUID representing the type of the characteristic value.
+    pub type_: Uuid,
+    /// The permissions (read/write) indicate what operations can be performed.
+    pub permissions: AttPermissions,
+}
+
+/// The GattDatabase implements AttDatabase, and converts attribute reads/writes
+/// into GATT operations to be sent to the upper layers
+#[derive(Default)]
+pub struct GattDatabase {
+    schema: RefCell<GattDatabaseSchema>,
+}
+
+#[derive(Default)]
+struct GattDatabaseSchema {
+    services: Vec<GattServiceWithHandle>,
+    attributes: BTreeMap<AttHandle, AttAttributeWithBackingValue>,
+}
+
+enum AttAttributeBackingValue {
+    Static(AttAttributeDataChild),
+    Dynamic,
+}
+
+struct AttAttributeWithBackingValue {
+    attribute: AttAttribute,
+    value: AttAttributeBackingValue,
+}
+
+impl GattDatabase {
+    /// Constructor
+    pub fn new() -> Self {
+        Default::default()
+    }
+
+    /// Add a service with pre-allocated handles (for co-existence with C++)
+    /// Assumes that the characteristic DECLARATION handles are one less than
+    /// the characteristic handles.
+    /// Returns failure if handles overlap with ones already allocated
+    pub fn add_service_with_handles(&self, service: GattServiceWithHandle) -> Result<()> {
+        let mut attributes = BTreeMap::new();
+        let mut attribute_cnt = 0;
+
+        let mut add_attribute = |attribute: AttAttribute, value: AttAttributeBackingValue| {
+            attribute_cnt += 1;
+            attributes.insert(attribute.handle, AttAttributeWithBackingValue { attribute, value })
+        };
+
+        let mut characteristics = vec![];
+
+        // service definition
+        add_attribute(
+            AttAttribute {
+                handle: service.handle,
+                type_: PRIMARY_SERVICE_DECLARATION_UUID,
+                permissions: AttPermissions { readable: true, writable: false },
+            },
+            AttAttributeBackingValue::Static(
+                GattServiceDeclarationValueBuilder { uuid: UuidBuilder::from(service.type_) }
+                    .into(),
+            ),
+        );
+
+        // characteristics
+        for characteristic in service.characteristics {
+            characteristics.push(GattCharacteristicWithHandle {
+                handle: characteristic.handle,
+                type_: characteristic.type_,
+                permissions: characteristic.permissions.clone(),
+            });
+
+            // declaration
+            // Recall that we assume the declaration handle is one less than the value
+            // handle
+            let declaration_handle = AttHandle(characteristic.handle.0 - 1);
+
+            add_attribute(
+                AttAttribute {
+                    handle: declaration_handle,
+                    type_: CHARACTERISTIC_UUID,
+                    permissions: AttPermissions { readable: true, writable: false },
+                },
+                AttAttributeBackingValue::Static(
+                    GattCharacteristicDeclarationValueBuilder {
+                        properties: AttCharacteristicPropertiesBuilder {
+                            broadcast: 0,
+                            read: characteristic.permissions.readable.into(),
+                            write_without_response: 0,
+                            write: characteristic.permissions.writable.into(),
+                            notify: 0,
+                            indicate: 0,
+                            authenticated_signed_writes: 0,
+                            extended_properties: 0,
+                        },
+                        handle: characteristic.handle.into(),
+                        uuid: characteristic.type_.into(),
+                    }
+                    .into(),
+                ),
+            );
+
+            // value
+            add_attribute(
+                AttAttribute {
+                    handle: characteristic.handle,
+                    type_: characteristic.type_,
+                    permissions: characteristic.permissions,
+                },
+                AttAttributeBackingValue::Dynamic,
+            );
+        }
+
+        // validate attributes for overlap
+        let mut static_data = self.schema.borrow_mut();
+
+        for handle in attributes.keys() {
+            if static_data.attributes.contains_key(handle) {
+                bail!("duplicate handle detected");
+            }
+        }
+        if attributes.len() != attribute_cnt {
+            bail!("duplicate handle detected");
+        }
+
+        // if we made it here, we successfully loaded the new service
+        let service =
+            GattServiceWithHandle { handle: service.handle, type_: service.type_, characteristics };
+        static_data.services.push(service);
+        static_data.attributes.extend(attributes.into_iter());
+        Ok(())
+    }
+
+    /// Remove a previously-added service by service handle
+    pub fn remove_service_at_handle(&self, service_handle: AttHandle) -> Result<()> {
+        let mut static_data = self.schema.borrow_mut();
+
+        // remove old service
+        static_data
+            .services
+            .iter()
+            .position(|service| service.handle == service_handle)
+            .map(|index| static_data.services.remove(index))
+            .ok_or_else(|| {
+                anyhow!("service at handle {service_handle:?} not found, cannot remove")
+            })?;
+
+        // find next service
+        let next_service_handle = static_data
+            .attributes
+            .values()
+            .find(|attribute| {
+                attribute.attribute.handle > service_handle
+                    && attribute.attribute.type_ == PRIMARY_SERVICE_DECLARATION_UUID
+            })
+            .map(|service| service.attribute.handle);
+
+        // clear out attributes
+        static_data.attributes.retain(|curr_handle, _| {
+            !(service_handle <= *curr_handle
+                && next_service_handle.map(|x| *curr_handle < x).unwrap_or(true))
+        });
+
+        Ok(())
+    }
+
+    /// Generate an impl AttDatabase from a backing GattDatabase
+    pub fn get_att_database(self: &Rc<Self>) -> AttDatabaseImpl {
+        AttDatabaseImpl { gatt_db: self.clone() }
+    }
+}
+
+/// An implementation of AttDatabase wrapping an underlying GattDatabase
+pub struct AttDatabaseImpl {
+    gatt_db: Rc<GattDatabase>,
+}
+
+#[async_trait(?Send)]
+impl AttDatabase for AttDatabaseImpl {
+    async fn read_attribute(
+        &self,
+        handle: AttHandle,
+    ) -> Result<AttAttributeDataChild, AttErrorCode> {
+        {
+            let services = self.gatt_db.schema.borrow();
+            match services.attributes.get(&handle).map(|attr| &attr.value) {
+                Some(AttAttributeBackingValue::Static(val)) => return Ok(val.clone()),
+                None => return Err(AttErrorCode::INVALID_HANDLE),
+                Some(AttAttributeBackingValue::Dynamic) => { /* fallthrough */ }
+            };
+        }
+
+        // TODO(aryarahul): read value from upper layers
+        Err(AttErrorCode::INVALID_HANDLE)
+    }
+
+    fn list_attributes(&self) -> Vec<AttAttribute> {
+        self.gatt_db
+            .schema
+            .borrow()
+            .attributes
+            .values()
+            .map(|attr| attr.attribute.clone())
+            .collect()
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    const SERVICE_HANDLE: AttHandle = AttHandle(1);
+    const SERVICE_TYPE: Uuid = Uuid::new(0x1234);
+
+    const CHARACTERISTIC_DECLARATION_HANDLE: AttHandle = AttHandle(2);
+    const CHARACTERISTIC_VALUE_HANDLE: AttHandle = AttHandle(3);
+    const CHARACTERISTIC_TYPE: Uuid = Uuid::new(0x5678);
+
+    #[test]
+    fn test_read_empty_db() {
+        let gatt_db = Rc::new(GattDatabase::new());
+        let att_db = gatt_db.get_att_database();
+
+        let resp = tokio_test::block_on(att_db.read_attribute(AttHandle(1)));
+
+        assert_eq!(resp, Err(AttErrorCode::INVALID_HANDLE))
+    }
+
+    #[test]
+    fn test_single_service() {
+        let gatt_db = Rc::new(GattDatabase::new());
+        gatt_db
+            .add_service_with_handles(GattServiceWithHandle {
+                handle: SERVICE_HANDLE,
+                type_: SERVICE_TYPE,
+                characteristics: vec![],
+            })
+            .unwrap();
+        let att_db = gatt_db.get_att_database();
+
+        let attrs = att_db.list_attributes();
+        let service_value = tokio_test::block_on(att_db.read_attribute(SERVICE_HANDLE));
+
+        assert_eq!(
+            attrs,
+            vec![AttAttribute {
+                handle: SERVICE_HANDLE,
+                type_: PRIMARY_SERVICE_DECLARATION_UUID,
+                permissions: AttPermissions { readable: true, writable: false }
+            }]
+        );
+        assert_eq!(
+            service_value,
+            Ok(AttAttributeDataChild::GattServiceDeclarationValue(
+                GattServiceDeclarationValueBuilder { uuid: SERVICE_TYPE.into() }
+            ))
+        );
+    }
+
+    #[test]
+    fn test_service_removal() {
+        // arrange three services, each with a single characteristic
+        let gatt_db = Rc::new(GattDatabase::new());
+
+        gatt_db
+            .add_service_with_handles(GattServiceWithHandle {
+                handle: AttHandle(1),
+                type_: SERVICE_TYPE,
+                characteristics: vec![GattCharacteristicWithHandle {
+                    handle: AttHandle(3),
+                    type_: CHARACTERISTIC_TYPE,
+                    permissions: AttPermissions { readable: true, writable: false },
+                }],
+            })
+            .unwrap();
+        gatt_db
+            .add_service_with_handles(GattServiceWithHandle {
+                handle: AttHandle(4),
+                type_: SERVICE_TYPE,
+                characteristics: vec![GattCharacteristicWithHandle {
+                    handle: AttHandle(6),
+                    type_: CHARACTERISTIC_TYPE,
+                    permissions: AttPermissions { readable: true, writable: false },
+                }],
+            })
+            .unwrap();
+        gatt_db
+            .add_service_with_handles(GattServiceWithHandle {
+                handle: AttHandle(7),
+                type_: SERVICE_TYPE,
+                characteristics: vec![GattCharacteristicWithHandle {
+                    handle: AttHandle(9),
+                    type_: CHARACTERISTIC_TYPE,
+                    permissions: AttPermissions { readable: true, writable: false },
+                }],
+            })
+            .unwrap();
+        let att_db = gatt_db.get_att_database();
+        assert_eq!(att_db.list_attributes().len(), 9);
+
+        // act: remove the middle service
+        gatt_db.remove_service_at_handle(AttHandle(4)).unwrap();
+        let attrs = att_db.list_attributes();
+
+        // assert that the middle service is gone
+        assert_eq!(attrs.len(), 6, "{attrs:?}");
+
+        // assert the other two old services are still there
+        assert_eq!(
+            attrs[0],
+            AttAttribute {
+                handle: AttHandle(1),
+                type_: PRIMARY_SERVICE_DECLARATION_UUID,
+                permissions: AttPermissions { readable: true, writable: false }
+            }
+        );
+        assert_eq!(
+            attrs[3],
+            AttAttribute {
+                handle: AttHandle(7),
+                type_: PRIMARY_SERVICE_DECLARATION_UUID,
+                permissions: AttPermissions { readable: true, writable: false }
+            }
+        );
+    }
+
+    #[test]
+    fn test_single_characteristic() {
+        let gatt_db = Rc::new(GattDatabase::new());
+        gatt_db
+            .add_service_with_handles(GattServiceWithHandle {
+                handle: SERVICE_HANDLE,
+                type_: SERVICE_TYPE,
+                characteristics: vec![GattCharacteristicWithHandle {
+                    handle: CHARACTERISTIC_VALUE_HANDLE,
+                    type_: CHARACTERISTIC_TYPE,
+                    permissions: AttPermissions { readable: false, writable: true },
+                }],
+            })
+            .unwrap();
+        let att_db = gatt_db.get_att_database();
+
+        let attrs = att_db.list_attributes();
+        let characteristic_decl =
+            tokio_test::block_on(att_db.read_attribute(CHARACTERISTIC_DECLARATION_HANDLE));
+        let characteristic_value =
+            tokio_test::block_on(att_db.read_attribute(CHARACTERISTIC_VALUE_HANDLE));
+
+        assert_eq!(attrs.len(), 3, "{attrs:?}");
+        assert_eq!(attrs[0].type_, PRIMARY_SERVICE_DECLARATION_UUID);
+        assert_eq!(
+            attrs[1],
+            AttAttribute {
+                handle: CHARACTERISTIC_DECLARATION_HANDLE,
+                type_: CHARACTERISTIC_UUID,
+                permissions: AttPermissions { readable: true, writable: false }
+            }
+        );
+        assert_eq!(
+            attrs[2],
+            AttAttribute {
+                handle: CHARACTERISTIC_VALUE_HANDLE,
+                type_: CHARACTERISTIC_TYPE,
+                permissions: AttPermissions { readable: false, writable: true }
+            }
+        );
+
+        assert_eq!(
+            characteristic_decl,
+            Ok(AttAttributeDataChild::GattCharacteristicDeclarationValue(
+                GattCharacteristicDeclarationValueBuilder {
+                    properties: AttCharacteristicPropertiesBuilder {
+                        read: 0,
+                        broadcast: 0,
+                        write_without_response: 0,
+                        write: 1,
+                        notify: 0,
+                        indicate: 0,
+                        authenticated_signed_writes: 0,
+                        extended_properties: 0,
+                    },
+                    handle: CHARACTERISTIC_VALUE_HANDLE.into(),
+                    uuid: CHARACTERISTIC_TYPE.into()
+                }
+            ))
+        );
+        // TODO(aryarahul): fix this once attribute value reading works
+        assert_eq!(characteristic_value, Err(AttErrorCode::INVALID_HANDLE));
+    }
+
+    #[test]
+    fn test_handle_clash() {
+        let gatt_db = Rc::new(GattDatabase::new());
+
+        let result = gatt_db.add_service_with_handles(GattServiceWithHandle {
+            handle: SERVICE_HANDLE,
+            type_: SERVICE_TYPE,
+            characteristics: vec![GattCharacteristicWithHandle {
+                handle: SERVICE_HANDLE,
+                type_: CHARACTERISTIC_TYPE,
+                permissions: AttPermissions { readable: false, writable: true },
+            }],
+        });
+
+        assert!(result.is_err());
+    }
+
+    #[test]
+    fn test_handle_clash_with_existing() {
+        let gatt_db = Rc::new(GattDatabase::new());
+
+        gatt_db
+            .add_service_with_handles(GattServiceWithHandle {
+                handle: SERVICE_HANDLE,
+                type_: SERVICE_TYPE,
+                characteristics: vec![],
+            })
+            .unwrap();
+
+        let result = gatt_db.add_service_with_handles(GattServiceWithHandle {
+            handle: SERVICE_HANDLE,
+            type_: SERVICE_TYPE,
+            characteristics: vec![],
+        });
+
+        assert!(result.is_err());
+    }
+}
diff --git a/system/rust/tests/gatt_server_test.rs b/system/rust/tests/gatt_server_test.rs
new file mode 100644
index 0000000000..b208422ae6
--- /dev/null
+++ b/system/rust/tests/gatt_server_test.rs
@@ -0,0 +1,92 @@
+use std::rc::Rc;
+
+use bluetooth_core::{
+    core::uuid::Uuid,
+    gatt::{
+        self,
+        ids::{AttHandle, ConnectionId, ServerId, TransportIndex},
+        mocks::mock_transport::MockAttTransport,
+        server::{
+            gatt_database::{AttPermissions, GattCharacteristicWithHandle, GattServiceWithHandle},
+            GattModule,
+        },
+    },
+    packets::{
+        AttBuilder, AttOpcode, AttReadRequestBuilder, AttReadResponseBuilder,
+        GattServiceDeclarationValueBuilder,
+    },
+    utils::packet::{build_att_data, build_att_view_or_crash},
+};
+
+use tokio::sync::mpsc::UnboundedReceiver;
+use utils::start_test;
+
+mod utils;
+
+const TCB_IDX: TransportIndex = TransportIndex(1);
+const SERVER_ID: ServerId = ServerId(2);
+const CONN_ID: ConnectionId = ConnectionId::new(TCB_IDX, SERVER_ID);
+const HANDLE_1: AttHandle = AttHandle(3);
+const HANDLE_2: AttHandle = AttHandle(5);
+const UUID_1: Uuid = Uuid::new(0x0102);
+const UUID_2: Uuid = Uuid::new(0x0103);
+
+fn start_gatt_module() -> (gatt::server::GattModule, UnboundedReceiver<(TransportIndex, AttBuilder)>)
+{
+    let (transport, transport_rx) = MockAttTransport::new();
+    let gatt = GattModule::new(Rc::new(transport));
+
+    (gatt, transport_rx)
+}
+
+fn create_server_and_open_connection(gatt: &mut GattModule) {
+    gatt.open_gatt_server(SERVER_ID).unwrap();
+    gatt.register_gatt_service(
+        SERVER_ID,
+        GattServiceWithHandle {
+            handle: HANDLE_1,
+            type_: UUID_1,
+            characteristics: vec![GattCharacteristicWithHandle {
+                handle: HANDLE_2,
+                type_: UUID_2,
+                permissions: AttPermissions { readable: true, writable: false },
+            }],
+        },
+    )
+    .unwrap();
+    gatt.on_le_connect(CONN_ID).unwrap();
+}
+
+#[test]
+fn test_service_read() {
+    start_test(async move {
+        // arrange
+        let (mut gatt, mut transport_rx) = start_gatt_module();
+
+        create_server_and_open_connection(&mut gatt);
+
+        // act
+        gatt.handle_packet(
+            CONN_ID,
+            build_att_view_or_crash(AttReadRequestBuilder { attribute_handle: HANDLE_1.into() })
+                .view(),
+        )
+        .unwrap();
+        let (tcb_idx, resp) = transport_rx.recv().await.unwrap();
+
+        // assert
+        assert_eq!(tcb_idx, TCB_IDX);
+        assert_eq!(
+            resp,
+            AttBuilder {
+                opcode: AttOpcode::READ_RESPONSE,
+                _child_: AttReadResponseBuilder {
+                    value: build_att_data(GattServiceDeclarationValueBuilder {
+                        uuid: UUID_1.into()
+                    })
+                }
+                .into()
+            }
+        );
+    })
+}
diff --git a/system/rust/tests/utils/mod.rs b/system/rust/tests/utils/mod.rs
new file mode 100644
index 0000000000..458583ad1e
--- /dev/null
+++ b/system/rust/tests/utils/mod.rs
@@ -0,0 +1,10 @@
+use std::future::Future;
+
+use tokio::task::LocalSet;
+
+pub fn start_test(f: impl Future<Output = ()>) {
+    tokio_test::block_on(async move {
+        bt_common::init_logging();
+        LocalSet::new().run_until(f).await;
+    });
+}