From b2237f43dc30099f9b6675180e5d131b45ccd7a7 Mon Sep 17 00:00:00 2001
From: Haobo Gu <haobogu@outlook.com>
Date: Thu, 7 Mar 2024 22:22:01 +0800
Subject: [PATCH] fix(ble): do not save bond info of same device

Signed-off-by: Haobo Gu <haobogu@outlook.com>
---
 rmk/src/ble.rs        | 26 ++++++++++++++++++++++----
 rmk/src/ble/bonder.rs |  4 ++--
 rmk/src/lib.rs        | 30 ++++++++++++++++++++++++++----
 3 files changed, 50 insertions(+), 10 deletions(-)

diff --git a/rmk/src/ble.rs b/rmk/src/ble.rs
index 9134f944..7ba8a804 100644
--- a/rmk/src/ble.rs
+++ b/rmk/src/ble.rs
@@ -21,14 +21,13 @@ use embedded_storage::nor_flash::NorFlash;
 use nrf_softdevice::{raw, Config, Flash};
 use sequential_storage::{
     cache::NoCache,
-    map::{remove_item, store_item},
+    map::{fetch_item, remove_item, store_item},
 };
 
-
 /// Flash range which used to save bonding info
 pub(crate) const CONFIG_FLASH_RANGE: Range<u32> = 0x80000..0x82000;
 /// Maximum number of bonded devices
-pub const BONDED_DEVICE_NUM: usize = 2;
+pub const BONDED_DEVICE_NUM: usize = 8;
 
 /// Create default nrf ble config
 pub fn nrf_ble_config(keyboard_name: &str) -> Config {
@@ -91,8 +90,27 @@ pub(crate) async fn flash_task(f: &'static mut Flash) -> ! {
                 .await
                 .unwrap();
             }
-            FlashOperationMessage::BondInfo(b) => {
+            FlashOperationMessage::BondInfo(mut b) => {
                 info!("Saving item: {}", info);
+                for key in 0..BONDED_DEVICE_NUM {
+                    if let Ok(Some(info)) = fetch_item::<BondInfo, _>(
+                        f,
+                        CONFIG_FLASH_RANGE,
+                        NoCache::new(),
+                        &mut storage_data_buffer,
+                        key as u8,
+                    )
+                    .await
+                    {
+                        // The device has been stored in flash, update
+                        if b.peer.peer_id.addr == info.peer.peer_id.addr {
+                            info!("Peer exists in flash, replace it");
+                            b.slot_num = info.slot_num;
+                            break;
+                        }
+                    }
+                }
+
                 store_item::<BondInfo, _>(
                     f,
                     CONFIG_FLASH_RANGE,
diff --git a/rmk/src/ble/bonder.rs b/rmk/src/ble/bonder.rs
index 10c8fb5d..915be9ca 100644
--- a/rmk/src/ble/bonder.rs
+++ b/rmk/src/ble/bonder.rs
@@ -19,8 +19,8 @@ pub(crate) static FLASH_CHANNEL: Channel<ThreadModeRawMutex, FlashOperationMessa
 // Bond info which will be stored in flash
 #[derive(Clone, Copy, Debug, Format)]
 pub(crate) struct BondInfo {
-    slot_num: u8,
-    peer: Peer,
+    pub(crate) slot_num: u8,
+    pub(crate) peer: Peer,
     sys_attr: SystemAttribute,
 }
 
diff --git a/rmk/src/lib.rs b/rmk/src/lib.rs
index e96fa86e..8bf75a48 100644
--- a/rmk/src/lib.rs
+++ b/rmk/src/lib.rs
@@ -240,7 +240,10 @@ pub async fn initialize_ble_keyboard_with_config_and_run<
     use defmt::*;
     use embassy_futures::select::{select, Either};
     use nrf_softdevice::Softdevice;
-    use sequential_storage::{cache::NoCache, map::fetch_item};
+    use sequential_storage::{
+        cache::NoCache,
+        map::{fetch_item, remove_item},
+    };
     use static_cell::StaticCell;
     // FIXME: add auto recognition of ble/usb
     use crate::ble::{
@@ -280,9 +283,28 @@ pub async fn initialize_ble_keyboard_with_config_and_run<
             fetch_item::<BondInfo, _>(f, CONFIG_FLASH_RANGE, NoCache::new(), &mut buf, key as u8)
                 .await
         {
-            match bond_info.push(info) {
-                Ok(_) => (),
-                Err(_) => error!("Add bond info error"),
+            // Iterate through bond_info, remove same devices by comparing peer address
+            if bond_info
+                .iter()
+                .filter(|i| i.peer.peer_id.addr == info.peer.peer_id.addr)
+                .count()
+                > 0
+            {
+                info!("Peer exists, removing current");
+                remove_item::<BondInfo, _>(
+                    f,
+                    CONFIG_FLASH_RANGE,
+                    NoCache::new(),
+                    &mut buf,
+                    key as u8,
+                )
+                .await
+                .unwrap();
+            } else {
+                match bond_info.push(info) {
+                    Ok(_) => (),
+                    Err(_) => error!("Add bond info error"),
+                }
             }
         }
     }