|
| 1 | +#![no_std] |
| 2 | +#![no_main] |
| 3 | + |
| 4 | +use defmt::{info, unwrap}; |
| 5 | +use embassy_executor::Spawner; |
| 6 | +use embassy_futures::join::join; |
| 7 | +use embassy_nrf::peripherals::RNG; |
| 8 | +use embassy_nrf::{bind_interrupts, rng}; |
| 9 | +use embassy_time::{Duration, Instant, Timer}; |
| 10 | +use nrf_sdc::mpsl::MultiprotocolServiceLayer; |
| 11 | +use nrf_sdc::{self as sdc, mpsl}; |
| 12 | +use static_cell::StaticCell; |
| 13 | +use trouble_host::prelude::*; |
| 14 | + |
| 15 | +use {defmt_rtt as _, panic_probe as _}; |
| 16 | + |
| 17 | +bind_interrupts!(struct Irqs { |
| 18 | + RNG => rng::InterruptHandler<RNG>; |
| 19 | + EGU0_SWI0 => nrf_sdc::mpsl::LowPrioInterruptHandler; |
| 20 | + CLOCK_POWER => nrf_sdc::mpsl::ClockInterruptHandler; |
| 21 | + RADIO => nrf_sdc::mpsl::HighPrioInterruptHandler; |
| 22 | + TIMER0 => nrf_sdc::mpsl::HighPrioInterruptHandler; |
| 23 | + RTC0 => nrf_sdc::mpsl::HighPrioInterruptHandler; |
| 24 | +}); |
| 25 | + |
| 26 | +#[embassy_executor::task] |
| 27 | +async fn mpsl_task(mpsl: &'static MultiprotocolServiceLayer<'static>) -> ! { |
| 28 | + mpsl.run().await |
| 29 | +} |
| 30 | + |
| 31 | +const L2CAP_TXQ: u8 = 4; |
| 32 | +const L2CAP_RXQ: u8 = 4; |
| 33 | +const L2CAP_MTU: usize = 251; |
| 34 | +const CONNECTIONS_MAX: usize = 1; |
| 35 | +const L2CAP_CHANNELS_MAX: usize = 1; |
| 36 | + |
| 37 | +fn build_sdc<'d, const N: usize>( |
| 38 | + p: nrf_sdc::Peripherals<'d>, |
| 39 | + rng: &'d mut rng::Rng<RNG>, |
| 40 | + mpsl: &'d MultiprotocolServiceLayer, |
| 41 | + mem: &'d mut sdc::Mem<N>, |
| 42 | +) -> Result<nrf_sdc::SoftdeviceController<'d>, nrf_sdc::Error> { |
| 43 | + sdc::Builder::new()? |
| 44 | + .support_scan()? |
| 45 | + .support_central()? |
| 46 | + .central_count(1)? |
| 47 | + .buffer_cfg(L2CAP_MTU as u8, L2CAP_MTU as u8, L2CAP_TXQ, L2CAP_RXQ)? |
| 48 | + .build(p, rng, mpsl, mem) |
| 49 | +} |
| 50 | + |
| 51 | +#[embassy_executor::main] |
| 52 | +async fn main(spawner: Spawner) { |
| 53 | + let p = embassy_nrf::init(Default::default()); |
| 54 | + let mpsl_p = mpsl::Peripherals::new(p.RTC0, p.TIMER0, p.TEMP, p.PPI_CH19, p.PPI_CH30, p.PPI_CH31); |
| 55 | + let lfclk_cfg = mpsl::raw::mpsl_clock_lfclk_cfg_t { |
| 56 | + source: mpsl::raw::MPSL_CLOCK_LF_SRC_RC as u8, |
| 57 | + rc_ctiv: mpsl::raw::MPSL_RECOMMENDED_RC_CTIV as u8, |
| 58 | + rc_temp_ctiv: mpsl::raw::MPSL_RECOMMENDED_RC_TEMP_CTIV as u8, |
| 59 | + accuracy_ppm: mpsl::raw::MPSL_DEFAULT_CLOCK_ACCURACY_PPM as u16, |
| 60 | + skip_wait_lfclk_started: mpsl::raw::MPSL_DEFAULT_SKIP_WAIT_LFCLK_STARTED != 0, |
| 61 | + }; |
| 62 | + static MPSL: StaticCell<MultiprotocolServiceLayer> = StaticCell::new(); |
| 63 | + let mpsl = MPSL.init(unwrap!(mpsl::MultiprotocolServiceLayer::new(mpsl_p, Irqs, lfclk_cfg))); |
| 64 | + spawner.must_spawn(mpsl_task(&*mpsl)); |
| 65 | + |
| 66 | + let sdc_p = sdc::Peripherals::new( |
| 67 | + p.PPI_CH17, p.PPI_CH18, p.PPI_CH20, p.PPI_CH21, p.PPI_CH22, p.PPI_CH23, p.PPI_CH24, p.PPI_CH25, p.PPI_CH26, |
| 68 | + p.PPI_CH27, p.PPI_CH28, p.PPI_CH29, |
| 69 | + ); |
| 70 | + |
| 71 | + let mut rng = rng::Rng::new(p.RNG, Irqs); |
| 72 | + |
| 73 | + let mut sdc_mem = sdc::Mem::<16384>::new(); |
| 74 | + let sdc = unwrap!(build_sdc(sdc_p, &mut rng, mpsl, &mut sdc_mem)); |
| 75 | + |
| 76 | + Timer::after(Duration::from_millis(200)).await; |
| 77 | + |
| 78 | + let address: Address = Address::random([0xff, 0x8f, 0x1b, 0x05, 0xe4, 0xff]); |
| 79 | + let mut resources: HostResources<CONNECTIONS_MAX, L2CAP_CHANNELS_MAX, L2CAP_MTU> = HostResources::new(); |
| 80 | + let stack = trouble_host::new(sdc, &mut resources).set_random_address(address); |
| 81 | + let Host { |
| 82 | + mut central, |
| 83 | + mut runner, |
| 84 | + .. |
| 85 | + } = stack.build(); |
| 86 | + |
| 87 | + let target: Address = Address::random([0xff, 0x8f, 0x1a, 0x05, 0xe4, 0xff]); |
| 88 | + let config = ConnectConfig { |
| 89 | + connect_params: Default::default(), |
| 90 | + scan_config: ScanConfig { |
| 91 | + filter_accept_list: &[(target.kind, &target.addr)], |
| 92 | + ..Default::default() |
| 93 | + }, |
| 94 | + }; |
| 95 | + |
| 96 | + let _ = join(runner.run(), async { |
| 97 | + loop { |
| 98 | + let conn = unwrap!(central.connect(&config).await); |
| 99 | + const PAYLOAD_LEN: usize = 492; |
| 100 | + let config = L2capChannelConfig { |
| 101 | + flow_policy: CreditFlowPolicy::MinThreshold(4), |
| 102 | + initial_credits: Some(8), |
| 103 | + mtu: PAYLOAD_LEN as u16, |
| 104 | + }; |
| 105 | + let mut ch1 = unwrap!(L2capChannel::create(&stack, &conn, 0x2349, &config).await); |
| 106 | + info!("sending l2cap data"); |
| 107 | + let mut last = Instant::now(); |
| 108 | + let mut bytes: u64 = 0; |
| 109 | + for i in 0..500 { |
| 110 | + let tx = [(i % 255) as u8; PAYLOAD_LEN]; |
| 111 | + unwrap!(ch1.send::<_, L2CAP_MTU>(&stack, &tx).await); |
| 112 | + bytes += PAYLOAD_LEN as u64; |
| 113 | + let duration = Instant::now() - last; |
| 114 | + if duration.as_secs() > 10 { |
| 115 | + info!("throughput: {} bytes/sec", bytes / duration.as_secs()); |
| 116 | + bytes = 0; |
| 117 | + last = Instant::now(); |
| 118 | + } |
| 119 | + } |
| 120 | + info!("waiting for echo data"); |
| 121 | + let mut rx = [0; PAYLOAD_LEN]; |
| 122 | + for i in 0..500 { |
| 123 | + let len = unwrap!(ch1.receive(&stack, &mut rx).await); |
| 124 | + assert_eq!(len, rx.len()); |
| 125 | + assert_eq!(rx, [(i % 255) as u8; PAYLOAD_LEN]); |
| 126 | + } |
| 127 | + |
| 128 | + info!("done"); |
| 129 | + Timer::after(Duration::from_secs(2)).await; |
| 130 | + break; |
| 131 | + } |
| 132 | + }) |
| 133 | + .await; |
| 134 | +} |
0 commit comments