Merge pull request #1576 from OueslatiGhaith/hci_v2

wpan: add BLE HCI

embassy-stm32-wpan/Cargo.toml

@@ -23,11 +23,13 @@ cortex-m = "0.7.6"
 heapless = "0.7.16"
 
 bit_field = "0.10.2"
+stm32-device-signature = { version = "0.3.3", features = ["stm32wb5x"] }
+stm32wb-hci = { version = "0.1.2", features = ["version-5-0"], optional = true }
 
 [features]
 defmt = ["dep:defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-common/defmt"]
 
-ble = []
+ble = ["dep:stm32wb-hci"]
 mac = []
 
 stm32wb10cc = [ "embassy-stm32/stm32wb10cc" ]

embassy-stm32-wpan/src/ble.rs

@@ -1,6 +1,7 @@
 use core::marker::PhantomData;
 
 use embassy_stm32::ipcc::Ipcc;
+use hci::Opcode;
 
 use crate::channels;
 use crate::cmd::CmdPacket;
@@ -29,7 +30,7 @@ impl Ble {
         Self { phantom: PhantomData }
     }
     /// `HW_IPCC_BLE_EvtNot`
-    pub async fn read(&self) -> EvtBox {
+    pub async fn tl_read(&self) -> EvtBox {
         Ipcc::receive(channels::cpu2::IPCC_BLE_EVENT_CHANNEL, || unsafe {
             if let Some(node_ptr) = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr()) {
                 Some(EvtBox::new(node_ptr.cast()))
@@ -41,7 +42,7 @@ impl Ble {
     }
 
     /// `TL_BLE_SendCmd`
-    pub async fn write(&self, opcode: u16, payload: &[u8]) {
+    pub async fn tl_write(&self, opcode: u16, payload: &[u8]) {
         Ipcc::send(channels::cpu1::IPCC_BLE_CMD_CHANNEL, || unsafe {
             CmdPacket::write_into(BLE_CMD_BUFFER.as_mut_ptr(), TlPacketType::BleCmd, opcode, payload);
         })
@@ -61,3 +62,18 @@ impl Ble {
         .await;
     }
 }
+
+pub extern crate stm32wb_hci as hci;
+
+impl hci::Controller for Ble {
+    async fn controller_write(&mut self, opcode: Opcode, payload: &[u8]) {
+        self.tl_write(opcode.0, payload).await;
+    }
+
+    async fn controller_read_into(&self, buf: &mut [u8]) {
+        let evt_box = self.tl_read().await;
+        let evt_serial = evt_box.serial();
+
+        buf[..evt_serial.len()].copy_from_slice(evt_serial);
+    }
+}

embassy-stm32-wpan/src/cmd.rs

@@ -52,7 +52,7 @@ impl CmdPacket {
             p_cmd_serial,
             CmdSerialStub {
                 ty: packet_type as u8,
-                cmd_code: cmd_code,
+                cmd_code,
                 payload_len: payload.len() as u8,
             },
         );

embassy-stm32-wpan/src/evt.rs

@@ -1,6 +1,7 @@
 use core::{ptr, slice};
 
 use super::PacketHeader;
+use crate::consts::TL_EVT_HEADER_SIZE;
 
 /**
  * The payload of `Evt` for a command status event
@@ -124,6 +125,20 @@ impl EvtBox {
             slice::from_raw_parts(p_payload, payload_len as usize)
         }
     }
+
+    /// writes an underlying [`EvtPacket`] into the provided buffer.
+    /// Returns the number of bytes that were written.
+    /// Returns an error if event kind is unknown or if provided buffer size is not enough.
+    pub fn serial<'a>(&'a self) -> &'a [u8] {
+        unsafe {
+            let evt_serial: *const EvtSerial = &(*self.ptr).evt_serial;
+            let evt_serial_buf: *const u8 = evt_serial.cast();
+
+            let len = (*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE;
+
+            slice::from_raw_parts(evt_serial_buf, len)
+        }
+    }
 }
 
 impl Drop for EvtBox {

embassy-stm32-wpan/src/lhci.rs

@@ -0,0 +1,111 @@
+use crate::cmd::CmdPacket;
+use crate::consts::{TlPacketType, TL_EVT_HEADER_SIZE};
+use crate::evt::{CcEvt, EvtPacket, EvtSerial};
+use crate::tables::{DeviceInfoTable, RssInfoTable, SafeBootInfoTable, WirelessFwInfoTable};
+use crate::TL_REF_TABLE;
+
+const TL_BLEEVT_CC_OPCODE: u8 = 0x0e;
+const LHCI_OPCODE_C1_DEVICE_INF: u16 = 0xfd62;
+
+const PACKAGE_DATA_PTR: *const u8 = 0x1FFF_7500 as _;
+const UID64_PTR: *const u32 = 0x1FFF_7580 as _;
+
+#[derive(Debug, Copy, Clone)]
+#[repr(C, packed)]
+pub struct LhciC1DeviceInformationCcrp {
+    pub status: u8,
+    pub rev_id: u16,
+    pub dev_code_id: u16,
+    pub package_type: u8,
+    pub device_type_id: u8,
+    pub st_company_id: u32,
+    pub uid64: u32,
+
+    pub uid96_0: u32,
+    pub uid96_1: u32,
+    pub uid96_2: u32,
+
+    pub safe_boot_info_table: SafeBootInfoTable,
+    pub rss_info_table: RssInfoTable,
+    pub wireless_fw_info_table: WirelessFwInfoTable,
+
+    pub app_fw_inf: u32,
+}
+
+impl Default for LhciC1DeviceInformationCcrp {
+    fn default() -> Self {
+        let DeviceInfoTable {
+            safe_boot_info_table,
+            rss_info_table,
+            wireless_fw_info_table,
+        } = unsafe { &*(*TL_REF_TABLE.as_ptr()).device_info_table }.clone();
+
+        let device_id = stm32_device_signature::device_id();
+        let uid96_0 = (device_id[3] as u32) << 24
+            | (device_id[2] as u32) << 16
+            | (device_id[1] as u32) << 8
+            | device_id[0] as u32;
+        let uid96_1 = (device_id[7] as u32) << 24
+            | (device_id[6] as u32) << 16
+            | (device_id[5] as u32) << 8
+            | device_id[4] as u32;
+        let uid96_2 = (device_id[11] as u32) << 24
+            | (device_id[10] as u32) << 16
+            | (device_id[9] as u32) << 8
+            | device_id[8] as u32;
+
+        let package_type = unsafe { *PACKAGE_DATA_PTR };
+        let uid64 = unsafe { *UID64_PTR };
+        let st_company_id = unsafe { *UID64_PTR.offset(1) } >> 8 & 0x00FF_FFFF;
+        let device_type_id = (unsafe { *UID64_PTR.offset(1) } & 0x000000FF) as u8;
+
+        LhciC1DeviceInformationCcrp {
+            status: 0,
+            rev_id: 0,
+            dev_code_id: 0,
+            package_type,
+            device_type_id,
+            st_company_id,
+            uid64,
+            uid96_0,
+            uid96_1,
+            uid96_2,
+            safe_boot_info_table,
+            rss_info_table,
+            wireless_fw_info_table,
+            app_fw_inf: (1 << 8), // 0.0.1
+        }
+    }
+}
+
+impl LhciC1DeviceInformationCcrp {
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    pub fn write(&self, cmd_packet: &mut CmdPacket) {
+        let self_size = core::mem::size_of::<LhciC1DeviceInformationCcrp>();
+
+        unsafe {
+            let cmd_packet_ptr: *mut CmdPacket = cmd_packet;
+            let evet_packet_ptr: *mut EvtPacket = cmd_packet_ptr.cast();
+
+            let evt_serial: *mut EvtSerial = &mut (*evet_packet_ptr).evt_serial;
+            let evt_payload = (*evt_serial).evt.payload.as_mut_ptr();
+            let evt_cc: *mut CcEvt = evt_payload.cast();
+            let evt_cc_payload_buf = (*evt_cc).payload.as_mut_ptr();
+
+            (*evt_serial).kind = TlPacketType::LocRsp as u8;
+            (*evt_serial).evt.evt_code = TL_BLEEVT_CC_OPCODE;
+            (*evt_serial).evt.payload_len = TL_EVT_HEADER_SIZE as u8 + self_size as u8;
+
+            (*evt_cc).cmd_code = LHCI_OPCODE_C1_DEVICE_INF;
+            (*evt_cc).num_cmd = 1;
+
+            let self_ptr: *const LhciC1DeviceInformationCcrp = self;
+            let self_buf = self_ptr.cast();
+
+            core::ptr::copy(self_buf, evt_cc_payload_buf, self_size);
+        }
+    }
+}

embassy-stm32-wpan/src/lib.rs

@@ -1,4 +1,5 @@
 #![no_std]
+#![cfg_attr(feature = "ble", feature(async_fn_in_trait))]
 
 // This must go FIRST so that all the other modules see its macros.
 pub mod fmt;
@@ -21,6 +22,7 @@ pub mod channels;
 pub mod cmd;
 pub mod consts;
 pub mod evt;
+pub mod lhci;
 #[cfg(feature = "mac")]
 pub mod mac;
 pub mod mm;

examples/stm32wb/.cargo/config.toml

@@ -1,6 +1,6 @@
 [target.'cfg(all(target_arch = "arm", target_os = "none"))']
 # replace STM32WB55CCUx with your chip as listed in `probe-rs-cli chip list`
-# runner = "probe-rs-cli run --chip STM32WB55CCUx --speed 1000 --connect-under-reset"
+# runner = "probe-rs-cli run --chip STM32WB55RGVx --speed 1000 --connect-under-reset"
 runner = "teleprobe local run --chip STM32WB55RG --elf"
 
 [build]

examples/stm32wb/Cargo.toml

@@ -33,4 +33,11 @@ required-features = ["ble"]
 
 [[bin]] 
 name = "tl_mbox_mac"
-required-features = ["mac"]
+required-features = ["mac"]
+
+[[bin]] 
+name = "eddystone_beacon"
+required-features = ["ble"]
+
+[patch.crates-io]
+stm32wb-hci = { git = "https://github.com/OueslatiGhaith/stm32wb-hci", rev = "9f663be"}

examples/stm32wb/src/bin/eddystone_beacon.rs

@@ -0,0 +1,249 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use core::time::Duration;
+
+use defmt::*;
+use embassy_executor::Spawner;
+use embassy_stm32::bind_interrupts;
+use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler};
+use embassy_stm32_wpan::ble::hci::host::uart::UartHci;
+use embassy_stm32_wpan::ble::hci::host::{AdvertisingFilterPolicy, EncryptionKey, HostHci, OwnAddressType};
+use embassy_stm32_wpan::ble::hci::types::AdvertisingType;
+use embassy_stm32_wpan::ble::hci::vendor::stm32wb::command::gap::{
+    AdvertisingDataType, DiscoverableParameters, GapCommands, Role,
+};
+use embassy_stm32_wpan::ble::hci::vendor::stm32wb::command::gatt::GattCommands;
+use embassy_stm32_wpan::ble::hci::vendor::stm32wb::command::hal::{ConfigData, HalCommands, PowerLevel};
+use embassy_stm32_wpan::ble::hci::BdAddr;
+use embassy_stm32_wpan::lhci::LhciC1DeviceInformationCcrp;
+use embassy_stm32_wpan::TlMbox;
+use {defmt_rtt as _, panic_probe as _};
+
+bind_interrupts!(struct Irqs{
+    IPCC_C1_RX => ReceiveInterruptHandler;
+    IPCC_C1_TX => TransmitInterruptHandler;
+});
+
+const BLE_GAP_DEVICE_NAME_LENGTH: u8 = 7;
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    /*
+        How to make this work:
+
+        - Obtain a NUCLEO-STM32WB55 from your preferred supplier.
+        - Download and Install STM32CubeProgrammer.
+        - Download stm32wb5x_FUS_fw.bin, stm32wb5x_BLE_Stack_full_fw.bin, and Release_Notes.html from
+          gh:STMicroelectronics/STM32CubeWB@2234d97/Projects/STM32WB_Copro_Wireless_Binaries/STM32WB5x
+        - Open STM32CubeProgrammer
+        - On the right-hand pane, click "firmware upgrade" to upgrade the st-link firmware.
+        - Once complete, click connect to connect to the device.
+        - On the left hand pane, click the RSS signal icon to open "Firmware Upgrade Services".
+        - In the Release_Notes.html, find the memory address that corresponds to your device for the stm32wb5x_FUS_fw.bin file
+        - Select that file, the memory address, "verify download", and then "Firmware Upgrade".
+        - Once complete, in the Release_Notes.html, find the memory address that corresponds to your device for the
+          stm32wb5x_BLE_Stack_full_fw.bin file. It should not be the same memory address.
+        - Select that file, the memory address, "verify download", and then "Firmware Upgrade".
+        - Select "Start Wireless Stack".
+        - Disconnect from the device.
+        - In the examples folder for stm32wb, modify the memory.x file to match your target device.
+        - Run this example.
+
+        Note: extended stack versions are not supported at this time. Do not attempt to install a stack with "extended" in the name.
+    */
+
+    let p = embassy_stm32::init(Default::default());
+    info!("Hello World!");
+
+    let config = Config::default();
+    let mut mbox = TlMbox::init(p.IPCC, Irqs, config);
+
+    let sys_event = mbox.sys_subsystem.read().await;
+    info!("sys event: {}", sys_event.payload());
+
+    mbox.sys_subsystem.shci_c2_ble_init(Default::default()).await;
+
+    info!("resetting BLE...");
+    mbox.ble_subsystem.reset().await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("config public address...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::public_address(get_bd_addr()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("config random address...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::random_address(get_random_addr()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("config identity root...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::identity_root(&get_irk()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("config encryption root...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::encryption_root(&get_erk()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("config tx power level...");
+    mbox.ble_subsystem.set_tx_power_level(PowerLevel::ZerodBm).await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("GATT init...");
+    mbox.ble_subsystem.init_gatt().await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("GAP init...");
+    mbox.ble_subsystem
+        .init_gap(Role::PERIPHERAL, false, BLE_GAP_DEVICE_NAME_LENGTH)
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    // info!("set scan response...");
+    // mbox.ble_subsystem.le_set_scan_response_data(&[]).await.unwrap();
+    // let response = mbox.ble_subsystem.read().await.unwrap();
+    // defmt::info!("{}", response);
+
+    info!("set discoverable...");
+    mbox.ble_subsystem
+        .set_discoverable(&DiscoverableParameters {
+            advertising_type: AdvertisingType::NonConnectableUndirected,
+            advertising_interval: Some((Duration::from_millis(250), Duration::from_millis(250))),
+            address_type: OwnAddressType::Public,
+            filter_policy: AdvertisingFilterPolicy::AllowConnectionAndScan,
+            local_name: None,
+            advertising_data: &[],
+            conn_interval: (None, None),
+        })
+        .await
+        .unwrap();
+
+    let response = mbox.ble_subsystem.read().await;
+    defmt::info!("{}", response);
+
+    // remove some advertisement to decrease the packet size
+    info!("delete tx power ad type...");
+    mbox.ble_subsystem
+        .delete_ad_type(AdvertisingDataType::TxPowerLevel)
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("delete conn interval ad type...");
+    mbox.ble_subsystem
+        .delete_ad_type(AdvertisingDataType::PeripheralConnectionInterval)
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("update advertising data...");
+    mbox.ble_subsystem
+        .update_advertising_data(&eddystone_advertising_data())
+        .await
+        .unwrap();
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("update advertising data type...");
+    mbox.ble_subsystem
+        .update_advertising_data(&[3, AdvertisingDataType::UuidCompleteList16 as u8, 0xaa, 0xfe])
+        .await
+        .unwrap();
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    info!("update advertising data flags...");
+    mbox.ble_subsystem
+        .update_advertising_data(&[
+            2,
+            AdvertisingDataType::Flags as u8,
+            (0x02 | 0x04) as u8, // BLE general discoverable, without BR/EDR support
+        ])
+        .await
+        .unwrap();
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    defmt::info!("{}", response);
+
+    cortex_m::asm::wfi();
+}
+
+fn get_bd_addr() -> BdAddr {
+    let mut bytes = [0u8; 6];
+
+    let lhci_info = LhciC1DeviceInformationCcrp::new();
+    bytes[0] = (lhci_info.uid64 & 0xff) as u8;
+    bytes[1] = ((lhci_info.uid64 >> 8) & 0xff) as u8;
+    bytes[2] = ((lhci_info.uid64 >> 16) & 0xff) as u8;
+    bytes[3] = lhci_info.device_type_id;
+    bytes[4] = (lhci_info.st_company_id & 0xff) as u8;
+    bytes[5] = (lhci_info.st_company_id >> 8 & 0xff) as u8;
+
+    BdAddr(bytes)
+}
+
+fn get_random_addr() -> BdAddr {
+    let mut bytes = [0u8; 6];
+
+    let lhci_info = LhciC1DeviceInformationCcrp::new();
+    bytes[0] = (lhci_info.uid64 & 0xff) as u8;
+    bytes[1] = ((lhci_info.uid64 >> 8) & 0xff) as u8;
+    bytes[2] = ((lhci_info.uid64 >> 16) & 0xff) as u8;
+    bytes[3] = 0;
+    bytes[4] = 0x6E;
+    bytes[5] = 0xED;
+
+    BdAddr(bytes)
+}
+
+const BLE_CFG_IRK: [u8; 16] = [
+    0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde, 0xf0, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde, 0xf0,
+];
+const BLE_CFG_ERK: [u8; 16] = [
+    0xfe, 0xdc, 0xba, 0x09, 0x87, 0x65, 0x43, 0x21, 0xfe, 0xdc, 0xba, 0x09, 0x87, 0x65, 0x43, 0x21,
+];
+
+fn get_irk() -> EncryptionKey {
+    EncryptionKey(BLE_CFG_IRK)
+}
+
+fn get_erk() -> EncryptionKey {
+    EncryptionKey(BLE_CFG_ERK)
+}
+
+fn eddystone_advertising_data() -> [u8; 24] {
+    const EDDYSTONE_URL: &[u8] = b"www.rust-lang.com";
+
+    let mut service_data = [0u8; 24];
+    let url_len = EDDYSTONE_URL.len();
+
+    service_data[0] = 6 + url_len as u8;
+    service_data[1] = AdvertisingDataType::ServiceData as u8;
+
+    // 16-bit eddystone uuid
+    service_data[2] = 0xaa;
+    service_data[3] = 0xFE;
+
+    service_data[4] = 0x10; // URL frame type
+    service_data[5] = 22_i8 as u8; // calibrated TX power at 0m
+    service_data[6] = 0x03; // eddystone url prefix = https
+
+    service_data[7..(7 + url_len)].copy_from_slice(EDDYSTONE_URL);
+
+    service_data
+}

examples/stm32wb/src/bin/tl_mbox_ble.rs

@@ -52,10 +52,10 @@ async fn main(_spawner: Spawner) {
     mbox.sys_subsystem.shci_c2_ble_init(Default::default()).await;
 
     info!("starting ble...");
-    mbox.ble_subsystem.write(0x0c, &[]).await;
+    mbox.ble_subsystem.tl_write(0x0c, &[]).await;
 
     info!("waiting for ble...");
-    let ble_event = mbox.ble_subsystem.read().await;
+    let ble_event = mbox.ble_subsystem.tl_read().await;
 
     info!("ble event: {}", ble_event.payload());
 

examples/stm32wb/src/bin/tl_mbox_mac.rs

@@ -52,10 +52,10 @@ async fn main(_spawner: Spawner) {
     mbox.sys_subsystem.shci_c2_mac_802_15_4_init().await;
     //
     //    info!("starting ble...");
-    //    mbox.ble_subsystem.write(0x0c, &[]).await;
+    //    mbox.ble_subsystem.t_write(0x0c, &[]).await;
     //
     //    info!("waiting for ble...");
-    //    let ble_event = mbox.ble_subsystem.read().await;
+    //    let ble_event = mbox.ble_subsystem.tl_read().await;
     //
     //    info!("ble event: {}", ble_event.payload());
 

tests/stm32/Cargo.toml

@@ -46,6 +46,9 @@ rand_chacha = { version = "0.3", default-features = false }
 
 chrono = { version = "^0.4", default-features = false, optional = true}
 
+[patch.crates-io]
+stm32wb-hci = { git = "https://github.com/OueslatiGhaith/stm32wb-hci", rev = "9f663be"}
+
 # BEGIN TESTS
 # Generated by gen_test.py. DO NOT EDIT.
 [[bin]]

tests/stm32/src/bin/tl_mbox.rs

@@ -6,43 +6,49 @@
 #[path = "../common.rs"]
 mod common;
 
-use core::mem;
+use core::time::Duration;
 
 use common::*;
 use embassy_executor::Spawner;
-use embassy_futures::poll_once;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler};
+use embassy_stm32_wpan::ble::hci::host::uart::UartHci;
+use embassy_stm32_wpan::ble::hci::host::{AdvertisingFilterPolicy, EncryptionKey, HostHci, OwnAddressType};
+use embassy_stm32_wpan::ble::hci::types::AdvertisingType;
+use embassy_stm32_wpan::ble::hci::vendor::stm32wb::command::gap::{
+    AdvertisingDataType, DiscoverableParameters, GapCommands, Role,
+};
+use embassy_stm32_wpan::ble::hci::vendor::stm32wb::command::gatt::GattCommands;
+use embassy_stm32_wpan::ble::hci::vendor::stm32wb::command::hal::{ConfigData, HalCommands, PowerLevel};
+use embassy_stm32_wpan::ble::hci::BdAddr;
+use embassy_stm32_wpan::lhci::LhciC1DeviceInformationCcrp;
 use embassy_stm32_wpan::{mm, TlMbox};
-use embassy_time::{Duration, Timer};
+use {defmt_rtt as _, panic_probe as _};
 
 bind_interrupts!(struct Irqs{
     IPCC_C1_RX => ReceiveInterruptHandler;
     IPCC_C1_TX => TransmitInterruptHandler;
 });
 
+const BLE_GAP_DEVICE_NAME_LENGTH: u8 = 7;
+
 #[embassy_executor::task]
 async fn run_mm_queue(memory_manager: mm::MemoryManager) {
     memory_manager.run_queue().await;
 }
 
 #[embassy_executor::main]
-async fn main(spawner: Spawner) {
+async fn main(_spawner: Spawner) {
     let p = embassy_stm32::init(config());
     info!("Hello World!");
 
     let config = Config::default();
-    let mbox = TlMbox::init(p.IPCC, Irqs, config);
+    let mut mbox = TlMbox::init(p.IPCC, Irqs, config);
 
-    spawner.spawn(run_mm_queue(mbox.mm_subsystem)).unwrap();
+    // spawner.spawn(run_mm_queue(mbox.mm_subsystem)).unwrap();
 
-    let ready_event = mbox.sys_subsystem.read().await;
-    let _ = poll_once(mbox.sys_subsystem.read()); // clear rx not
-
-    info!("sys event {:x} : {:x}", ready_event.stub().kind, ready_event.payload());
-
-    // test memory manager
-    mem::drop(ready_event);
+    let sys_event = mbox.sys_subsystem.read().await;
+    info!("sys event: {}", sys_event.payload());
 
     let fw_info = mbox.sys_subsystem.wireless_fw_info().unwrap();
     let version_major = fw_info.version_major();
@@ -57,19 +63,188 @@ async fn main(spawner: Spawner) {
         version_major, version_minor, subversion, sram2a_size, sram2b_size
     );
 
-    Timer::after(Duration::from_millis(50)).await;
+    mbox.sys_subsystem.shci_c2_ble_init(Default::default()).await;
 
-    let result = mbox.sys_subsystem.shci_c2_ble_init(Default::default()).await;
-    info!("subsystem initialization: {}", result);
+    info!("resetting BLE...");
+    mbox.ble_subsystem.reset().await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
 
-    info!("starting ble...");
-    mbox.ble_subsystem.write(0x0c, &[]).await;
+    info!("config public address...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::public_address(get_bd_addr()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
 
-    info!("waiting for ble...");
-    let ble_event = mbox.ble_subsystem.read().await;
+    info!("config random address...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::random_address(get_random_addr()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
 
-    info!("ble event {:x} : {:x}", ble_event.stub().kind, ble_event.payload());
+    info!("config identity root...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::identity_root(&get_irk()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("config encryption root...");
+    mbox.ble_subsystem
+        .write_config_data(&ConfigData::encryption_root(&get_erk()).build())
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("config tx power level...");
+    mbox.ble_subsystem.set_tx_power_level(PowerLevel::ZerodBm).await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("GATT init...");
+    mbox.ble_subsystem.init_gatt().await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("GAP init...");
+    mbox.ble_subsystem
+        .init_gap(Role::PERIPHERAL, false, BLE_GAP_DEVICE_NAME_LENGTH)
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    // info!("set scan response...");
+    // mbox.ble_subsystem.le_set_scan_response_data(&[]).await.unwrap();
+    // let response = mbox.ble_subsystem.read().await.unwrap();
+    // info!("{}", response);
+
+    info!("set discoverable...");
+    mbox.ble_subsystem
+        .set_discoverable(&DiscoverableParameters {
+            advertising_type: AdvertisingType::NonConnectableUndirected,
+            advertising_interval: Some((Duration::from_millis(250), Duration::from_millis(250))),
+            address_type: OwnAddressType::Public,
+            filter_policy: AdvertisingFilterPolicy::AllowConnectionAndScan,
+            local_name: None,
+            advertising_data: &[],
+            conn_interval: (None, None),
+        })
+        .await
+        .unwrap();
+
+    let response = mbox.ble_subsystem.read().await;
+    info!("{}", response);
+
+    // remove some advertisement to decrease the packet size
+    info!("delete tx power ad type...");
+    mbox.ble_subsystem
+        .delete_ad_type(AdvertisingDataType::TxPowerLevel)
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("delete conn interval ad type...");
+    mbox.ble_subsystem
+        .delete_ad_type(AdvertisingDataType::PeripheralConnectionInterval)
+        .await;
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("update advertising data...");
+    mbox.ble_subsystem
+        .update_advertising_data(&eddystone_advertising_data())
+        .await
+        .unwrap();
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("update advertising data type...");
+    mbox.ble_subsystem
+        .update_advertising_data(&[3, AdvertisingDataType::UuidCompleteList16 as u8, 0xaa, 0xfe])
+        .await
+        .unwrap();
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
+
+    info!("update advertising data flags...");
+    mbox.ble_subsystem
+        .update_advertising_data(&[
+            2,
+            AdvertisingDataType::Flags as u8,
+            (0x02 | 0x04) as u8, // BLE general discoverable, without BR/EDR support
+        ])
+        .await
+        .unwrap();
+    let response = mbox.ble_subsystem.read().await.unwrap();
+    info!("{}", response);
 
     info!("Test OK");
     cortex_m::asm::bkpt();
 }
+
+fn get_bd_addr() -> BdAddr {
+    let mut bytes = [0u8; 6];
+
+    let lhci_info = LhciC1DeviceInformationCcrp::new();
+    bytes[0] = (lhci_info.uid64 & 0xff) as u8;
+    bytes[1] = ((lhci_info.uid64 >> 8) & 0xff) as u8;
+    bytes[2] = ((lhci_info.uid64 >> 16) & 0xff) as u8;
+    bytes[3] = lhci_info.device_type_id;
+    bytes[4] = (lhci_info.st_company_id & 0xff) as u8;
+    bytes[5] = (lhci_info.st_company_id >> 8 & 0xff) as u8;
+
+    BdAddr(bytes)
+}
+
+fn get_random_addr() -> BdAddr {
+    let mut bytes = [0u8; 6];
+
+    let lhci_info = LhciC1DeviceInformationCcrp::new();
+    bytes[0] = (lhci_info.uid64 & 0xff) as u8;
+    bytes[1] = ((lhci_info.uid64 >> 8) & 0xff) as u8;
+    bytes[2] = ((lhci_info.uid64 >> 16) & 0xff) as u8;
+    bytes[3] = 0;
+    bytes[4] = 0x6E;
+    bytes[5] = 0xED;
+
+    BdAddr(bytes)
+}
+
+const BLE_CFG_IRK: [u8; 16] = [
+    0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde, 0xf0, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde, 0xf0,
+];
+const BLE_CFG_ERK: [u8; 16] = [
+    0xfe, 0xdc, 0xba, 0x09, 0x87, 0x65, 0x43, 0x21, 0xfe, 0xdc, 0xba, 0x09, 0x87, 0x65, 0x43, 0x21,
+];
+
+fn get_irk() -> EncryptionKey {
+    EncryptionKey(BLE_CFG_IRK)
+}
+
+fn get_erk() -> EncryptionKey {
+    EncryptionKey(BLE_CFG_ERK)
+}
+
+fn eddystone_advertising_data() -> [u8; 24] {
+    const EDDYSTONE_URL: &[u8] = b"www.rust-lang.com";
+
+    let mut service_data = [0u8; 24];
+    let url_len = EDDYSTONE_URL.len();
+
+    service_data[0] = 6 + url_len as u8;
+    service_data[1] = AdvertisingDataType::ServiceData as u8;
+
+    // 16-bit eddystone uuid
+    service_data[2] = 0xaa;
+    service_data[3] = 0xFE;
+
+    service_data[4] = 0x10; // URL frame type
+    service_data[5] = 22_i8 as u8; // calibrated TX power at 0m
+    service_data[6] = 0x03; // eddystone url prefix = https
+
+    service_data[7..(7 + url_len)].copy_from_slice(EDDYSTONE_URL);
+
+    service_data
+}