Scan feature loosing some bytes in LeAdvReport

[iamtio]

Description

Hello guys. Thanks for your awesome job done here in implementing BLE satck in Rust.

Looks like I found an issue with the scan feature.
Some advertisment packets (LeAdvReport) are losing the last byte and previous one sometimes is corrupted in the .data on read. I haven't found any pattern when this happens, except if you build the code with cargo build ... --release and without it. And even with --release flag sometimes it doesn't work well, but for other Adv packet payloads

I am not quite sure that this bug is in embassy-rs/trouble or in the embassy-rs/bt-hci project.

Example log without --release flag:

...
> AdvInd        BdAddr([EB, 33, 8C, 69, E2, 7C]): [2, 1, 6, 11, 6, BA, 5C, F7, 93, 3B, 12, D3, 89, E4, 11, E7, AD, 8, 1C, 2E, B4, 9, FF, 34, 3, E7, D5, AA, AA, 4B]
Flags(6)
Unknown { ty: 6, data: [BA, 5C, F7, 93, 3B, 12, D3, 89, E4, 11, E7, AD, 8, 1C, 2E, B4] }
Structure decode error: InsufficientSpace
...

Example log with --release flag:

...
> AdvInd        BdAddr([EB, 33, 8C, 69, E2, 7C]): [2, 1, 6, 11, 6, BA, 5C, F7, 93, 3B, 12, D3, 89, E4, 11, E7, AD, 8, 1C, 2E, B4, 9, FF, 34, 3, E7, D5, AA, AA, 49, 0]
Flags(6)
Unknown { ty: 6, data: [BA, 5C, F7, 93, 3B, 12, D3, 89, E4, 11, E7, AD, 8, 1C, 2E, B4] }
Unknown { ty: FF, data: [34, 3, E7, D5, AA, AA, 49, 0] }
...

As you can see we lost 0 at the end and the 49 became 4B

Platofrm details

• Chip: RP2040W
• PCB: Pimoroni Badger 2040W
• rustc 1.81.0 (eeb90cda1 2024-09-04)

How to reproduce

Cargo.toml

[package]
name = "app-examples"
version = "0.1.0"
edition = "2021"


[dependencies]
embassy-embedded-hal = { version = "0.2.0"}
embassy-sync = { version = "0.6.0"}
embassy-executor = { version = "0.6.0", features = ["task-arena-size-98304", "arch-cortex-m", "executor-thread", "executor-interrupt", "integrated-timers"] }
embassy-time = { version = "0.3.2" }
embassy-rp = { version = "0.2.0", features = ["unstable-pac", "time-driver", "critical-section-impl", "rp2040"] }
embassy-futures = { version = "0.1.0"}
embassy-usb-logger = { version = "0.2.0"}
static_cell = "2.1.0"
portable-atomic = { version = "1.5", features = ["critical-section"] }
panic-probe = { version = "0.3" }

cyw43 = { version = "0.2.0", features = ["firmware-logs", "bluetooth"] }
cyw43-pio = { version = "0.2.0"}

cortex-m = { version = "0.7.7", features = ["inline-asm"] }
cortex-m-rt = "0.7.0"
critical-section = "1.1"

log = "0.4"
pio-proc = "0.2.2"
pio = "0.2.1"

bt-hci = { version = "0.1.1", default-features = false }
trouble-host = { version = "0.1.0", features = ["gatt", "peripheral", "central", "scan"] }

[profile.release]
debug = 2
lto = true
opt-level = 'z'

[profile.dev]
debug = 2
lto = true
opt-level = "z"

[patch.crates-io]
trouble-host = { git = "https://github.com/embassy-rs/trouble.git", rev = "f13539641b1fedb4cc458c8f912b88665c805cba" }
embassy-embedded-hal = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
embassy-time = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
embassy-rp = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
embassy-usb-logger = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
cyw43 = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
cyw43-pio = { git = "https://github.com/embassy-rs/embassy.git", rev = "6e0b08291b63a0da8eba9284869d1d046bc5dabb" }
bt-hci = { git = "https://github.com/embassy-rs/bt-hci.git", rev = "be26c47a6a311602f8a40659ae6031c0e551111e" }

src/bin/rpi_ble_scan_bug.rs

#![no_std]
#![no_main]

use bt_hci::controller::ExternalController;
use cyw43_pio::PioSpi;
use embassy_executor::Spawner;
use embassy_rp::bind_interrupts;
use embassy_rp::gpio::{Level, Output};
use embassy_rp::peripherals::{DMA_CH0, PIO0};
use embassy_futures::join::join;
use embassy_rp::pio::{InterruptHandler as PioInterruptHandler, Pio};
use embassy_rp::peripherals::USB;
use embassy_rp::usb::{Driver, InterruptHandler as UsbInterruptHandler};
use trouble_host::scan::ScanConfig;
use trouble_host::{HostResources, PacketQos};
use trouble_host::advertise::AdStructure;

use static_cell::StaticCell;
use panic_probe as _;

bind_interrupts!(struct Irqs {
    PIO0_IRQ_0 => PioInterruptHandler<PIO0>;
    USBCTRL_IRQ => UsbInterruptHandler<USB>;
});

type BleResources<C> = HostResources<C, 1, 3, 251>; //CONNECTIONS_MAX, L2CAP_CHANNELS_MAX, L2CAP_MTU

#[embassy_executor::task]
async fn logger_task(driver: Driver<'static, USB>) {
    embassy_usb_logger::run!(1024, log::LevelFilter::Info, driver);
}

#[embassy_executor::task]
async fn cyw43_task(runner: cyw43::Runner<'static, Output<'static>, PioSpi<'static, PIO0, 0, DMA_CH0>>) -> ! {
    runner.run().await
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let p = embassy_rp::init(Default::default());
    let usb_driver = Driver::new(p.USB, Irqs);
    spawner.spawn(logger_task(usb_driver)).unwrap();

    let fw = include_bytes!("../../../cyw43-firmware/43439A0.bin");
    let clm = include_bytes!("../../../cyw43-firmware/43439A0_clm.bin");
    let btfw = include_bytes!("../../../cyw43-firmware/43439A0_btfw.bin");

    let pwr = Output::new(p.PIN_23, Level::Low);
    let cs = Output::new(p.PIN_25, Level::High);
    let mut pio = Pio::new(p.PIO0, Irqs);
    let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);

    static STATE: StaticCell<cyw43::State> = StaticCell::new();
    let state = STATE.init(cyw43::State::new());
    let (_net_device, bt_device, mut control, runner) = cyw43::new_with_bluetooth(state, pwr, spi, fw, btfw).await;
    spawner.spawn(cyw43_task(runner)).unwrap();
    control.init(clm).await;

    let controller: ExternalController<_, 10> = ExternalController::new(bt_device);
    let mut resources = BleResources::new(PacketQos::None);
    let (_, _, mut central, mut trouble_runner) = trouble_host::new(controller, &mut resources).build();

    let _ = join(trouble_runner.run(), async {
        log::info!("Scan start");
        loop{
            let reports = match central.scan(&ScanConfig::default()).await {
                Ok(reports) => reports,
                Err(e) => {
                    log::error!("Scan error: {:?}", e);
                    continue;
                }
            };
            for report in reports.iter() {
                let report = match report {
                    Ok(report) => report,
                    Err(e) => {
                        log::error!("Reports iterate error: {:?}", e);
                        continue;
                    }
                };
                log::info!("> {:?}\t{:X?}: {:X?}", report.event_kind, report.addr, report.data);
                for ad in AdStructure::decode(&report.data){
                    let ad = match ad {
                        Ok(ad) => ad,
                        Err(e) => {
                            log::error!("Structure decode error: {:?}", e);
                            break
                        }
                    };
                    log::info!("{:X?}", ad)
                }
            }
        }
    }).await;
}

[iamtio]

As temporary hack, I just fill the missing bytes with 0. Maybe this will be useful to someone with a similar problem.

...
use heapless::Vec;
...
    ...
    let mut fixed_report_data = Vec::<u8, 256>::new();
    fix_adv_payload(&report.data, &mut fixed_report_data);
    ...

fn fix_adv_payload<const N: usize>(payload: &[u8], result: &mut Vec<u8, N>) {
    if payload.is_empty(){
        return;
    }
    let mut pos: usize = 0;
    loop {
        let chunk_len = payload[pos] as usize;
        result.push(payload[pos]).unwrap();
        pos += 1;
        let bytes_left = payload[pos..].len();
        defmt::debug!("pos: {}, chunk_len: {}, bytes_left: {}", pos, chunk_len, bytes_left);
        
        let offset = if bytes_left < chunk_len {
            bytes_left
        } else {
            chunk_len
        };
        result.extend(payload[pos..pos+offset].iter().cloned());
        if bytes_left < chunk_len { // Probably at the end
            result.extend(iter::repeat(0u8).take(chunk_len-bytes_left));
            return;
        }
        pos += chunk_len;
        if pos >= payload.len(){
            return;
        }
    }
}