Merge pull request #8 from embassy-rs/rssi

add support for reading rssi
embassy-rs · Apr 9, 2024 · f63f245 · f63f245
2 parents 369f5d5 + a129219
commit f63f245
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 30 deletions.
diff --git a/host/src/adapter.rs b/host/src/adapter.rs
@@ -18,6 +18,7 @@ use bt_hci::cmd::le::{
     LeSetScanEnable, LeSetScanParams,
 };
 use bt_hci::cmd::link_control::{Disconnect, DisconnectParams};
+use bt_hci::cmd::status::ReadRssi;
 use bt_hci::cmd::{AsyncCmd, SyncCmd};
 use bt_hci::controller::Controller;
 use bt_hci::controller::{ControllerCmdAsync, ControllerCmdSync};
@@ -135,6 +136,14 @@ where
         Ok(())
     }
 
+    pub(crate) async fn read_rssi(&self, conn: ConnHandle) -> Result<i8, AdapterError<T::Error>>
+    where
+        T: ControllerCmdSync<ReadRssi>,
+    {
+        let val = ReadRssi::new(conn).exec(&self.controller).await?;
+        Ok(val.rssi)
+    }
+
     pub(crate) async fn connect(&self, config: &ConnectConfig<'_>) -> Result<Connection<'_>, AdapterError<T::Error>>
     where
         T: ControllerCmdSync<LeClearFilterAcceptList>
@@ -156,9 +165,9 @@ where
                 return Err(Error::Timeout.into());
             };
             if config.scan_config.extended {
-                for entry in report.iter_ext() {
+                if let Some(entry) = report.iter_ext().next() {
                     self.stop_scan(&config.scan_config).await?;
-                    let entry = entry.map_err(|e| Error::HciDecode(e))?;
+                    let entry = entry.map_err(Error::HciDecode)?;
                     let initiating = InitiatingPhy {
                         scan_interval: bt_hci::param::Duration::from_micros(config.scan_config.interval.as_micros()),
                         scan_window: bt_hci::param::Duration::from_micros(config.scan_config.window.as_micros()),
@@ -191,32 +200,30 @@ where
                         control: self.control.sender().into(),
                     });
                 }
-            } else {
-                for entry in report.iter() {
-                    self.stop_scan(&config.scan_config).await?;
-                    let entry = entry.map_err(|e| Error::HciDecode(e))?;
-                    LeCreateConn::new(
-                        bt_hci::param::Duration::from_micros(config.scan_config.interval.as_micros()),
-                        bt_hci::param::Duration::from_micros(config.scan_config.window.as_micros()),
-                        true,
-                        entry.addr_kind,
-                        entry.addr,
-                        self.address.map(|a| a.kind).unwrap_or(AddrKind::RANDOM),
-                        bt_hci::param::Duration::from_micros(config.connect_params.min_connection_interval.as_micros()),
-                        bt_hci::param::Duration::from_micros(config.connect_params.max_connection_interval.as_micros()),
-                        config.connect_params.max_latency,
-                        bt_hci::param::Duration::from_micros(config.connect_params.supervision_timeout.as_micros()),
-                        bt_hci::param::Duration::from_millis(0),
-                        bt_hci::param::Duration::from_millis(0),
-                    )
-                    .exec(&self.controller)
-                    .await?;
-                    let info = self.connections.accept(Some(entry.addr)).await;
-                    return Ok(Connection {
-                        info,
-                        control: self.control.sender().into(),
-                    });
-                }
+            } else if let Some(entry) = report.iter().next() {
+                self.stop_scan(&config.scan_config).await?;
+                let entry = entry.map_err(Error::HciDecode)?;
+                LeCreateConn::new(
+                    bt_hci::param::Duration::from_micros(config.scan_config.interval.as_micros()),
+                    bt_hci::param::Duration::from_micros(config.scan_config.window.as_micros()),
+                    true,
+                    entry.addr_kind,
+                    entry.addr,
+                    self.address.map(|a| a.kind).unwrap_or(AddrKind::RANDOM),
+                    bt_hci::param::Duration::from_micros(config.connect_params.min_connection_interval.as_micros()),
+                    bt_hci::param::Duration::from_micros(config.connect_params.max_connection_interval.as_micros()),
+                    config.connect_params.max_latency,
+                    bt_hci::param::Duration::from_micros(config.connect_params.supervision_timeout.as_micros()),
+                    bt_hci::param::Duration::from_millis(0),
+                    bt_hci::param::Duration::from_millis(0),
+                )
+                .exec(&self.controller)
+                .await?;
+                let info = self.connections.accept(Some(entry.addr)).await;
+                return Ok(Connection {
+                    info,
+                    control: self.control.sender().into(),
+                });
             }
         }
     }
@@ -281,7 +288,7 @@ where
                 if config.active {
                     bt_hci::param::LeScanKind::Active
                 } else {
-                    bt_hci::param::LeScanKind::Active
+                    bt_hci::param::LeScanKind::Passive
                 },
                 bt_hci::param::Duration::from_micros(config.interval.as_micros()),
                 bt_hci::param::Duration::from_micros(config.interval.as_micros()),

diff --git a/host/src/connection.rs b/host/src/connection.rs
@@ -5,6 +5,7 @@ use bt_hci::{
             LeSetExtScanParams, LeSetScanEnable, LeSetScanParams,
         },
         link_control::DisconnectParams,
+        status::ReadRssi,
     },
     controller::{ControllerCmdAsync, ControllerCmdSync},
     param::{BdAddr, ConnHandle, DisconnectReason, LeConnRole},
@@ -86,6 +87,23 @@ impl<'d> Connection<'d> {
         self.info.peer_address
     }
 
+    pub async fn rssi<
+        M: RawMutex,
+        T,
+        const CONNS: usize,
+        const CHANNELS: usize,
+        const L2CAP_TXQ: usize,
+        const L2CAP_RXQ: usize,
+    >(
+        &self,
+        adapter: &'d Adapter<'_, M, T, CONNS, CHANNELS, L2CAP_TXQ, L2CAP_RXQ>,
+    ) -> Result<i8, AdapterError<T::Error>>
+    where
+        T: ControllerCmdSync<ReadRssi>,
+    {
+        adapter.read_rssi(self.info.handle).await
+    }
+
     pub async fn connect<
         M: RawMutex,
         T,

diff --git a/host/src/l2cap.rs b/host/src/l2cap.rs
@@ -86,7 +86,7 @@ impl<'a, 'd, T: Controller, const L2CAP_MTU: usize> Clone for L2capChannel<'a, '
 
 impl<'a, 'd, T: Controller, const L2CAP_MTU: usize> L2capChannel<'a, 'd, T, L2CAP_MTU> {
     fn encode(&self, data: &[u8], packet: &mut [u8], header: Option<u16>) -> Result<usize, Error> {
-        let mut w = WriteCursor::new(packet.as_mut());
+        let mut w = WriteCursor::new(packet);
         if header.is_some() {
             w.write(2 + data.len() as u16)?;
         } else {