use alloc::vec::Vec;
use byte::BytesExt;
use embassy_futures::select::Either;
use embassy_futures::select::select;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_time::Timer;
use esp_radio::ieee802154::Config;
use esp_radio::ieee802154::Frame;
use esp_radio::ieee802154::Ieee802154;
use esp_radio::ieee802154::ReceivedFrame;
use ieee802154::mac::Address;
use ieee802154::mac::FrameContent;
use ieee802154::mac::FrameType;
use ieee802154::mac::FrameVersion;
use ieee802154::mac::Header;
use ieee802154::mac::PanId;
use ieee802154::mac::command::CapabilityInformation;
use ieee802154::mac::command::Command;
use ieee802154::mac::security::SecurityContext;
use crate::esp::driver::Ieee802154Driver;
use crate::esp::fair_mutex::FairMutex;
use crate::esp::fair_mutex::FairMutexGuard;
use crate::mlme::A_BASE_SUPER_FRAME_DURATION;
use crate::mlme::A_MAX_FRAME_RETRIES;
use crate::mlme::A_RESPONSE_WAIT_TIME;
use crate::mlme::AssociationResponse;
use crate::mlme::MAX_IEEE802154_CHANNELS;
use crate::mlme::MacConfig;
use crate::mlme::MacError;
use crate::mlme::Mlme;
use crate::mlme::PanDescriptor;
use crate::mlme::PanDescriptorList;
use crate::mlme::ScanResult;
use crate::mlme::ScanType;
mod driver;
mod fair_mutex;
const ASSOCIATE_REQUEST_RETRIES: u8 = 3;
const ASSOCIATE_POLL_RETRIES: u8 = 5;
const POLL_DATA_RETRIES: u8 = 5;
const RADIO_LOCK_TIMEOUT_MS: u64 = 5_000;
pub struct EspMlme<'a> {
inner: FairMutex<CriticalSectionRawMutex, EspMlmeInner<'a>>,
ieee_address: u64,
}
impl<'a> EspMlme<'a> {
pub fn new(ieee802154: Ieee802154<'a>, config: Config) -> Self {
let inner = EspMlmeInner {
driver: Ieee802154Driver::new(ieee802154, config),
seq_number: (esp_hal::rng::Rng::new().random() & 0xff) as u8,
};
let ieee_address = inner.driver.ieee_address().0;
Self {
inner: FairMutex::new(inner),
ieee_address,
}
}
pub fn ieee_address(&self) -> u64 {
self.ieee_address
}
async fn lock(
&self,
) -> Result<FairMutexGuard<'_, CriticalSectionRawMutex, EspMlmeInner<'a>>, MacError> {
match select(
Timer::after_millis(RADIO_LOCK_TIMEOUT_MS),
self.inner.lock(),
)
.await
{
Either::First(_) => Err(MacError::RadioLockTimeout),
Either::Second(Ok(guard)) => Ok(guard),
Either::Second(Err(_)) => {
log::warn!("[MAC] radio lock queue full");
Err(MacError::RadioLockQueueFull)
}
}
}
async fn receive_data(&self, buf: &mut [u8]) -> Result<(usize, u8), MacError> {
loop {
{
let mut inner = self.lock().await?;
if let Some(received) = inner.try_drain(buf)? {
return Ok(received);
}
}
driver::wait_rx_signal().await;
}
}
}
struct EspMlmeInner<'a> {
driver: Ieee802154Driver<'a>,
seq_number: u8,
}
impl EspMlmeInner<'_> {
fn sequence_number(&mut self) -> u8 {
self.seq_number = self.seq_number.wrapping_add(1);
self.seq_number
}
async fn transmit_acked(&mut self, frame: &[u8]) -> Result<(), MacError> {
for _ in 0..=A_MAX_FRAME_RETRIES {
match self.driver.transmit(frame).await {
Ok(()) if self.driver.last_tx_acked() => return Ok(()),
Ok(()) => {}
Err(MacError::TxFailed) => {}
Err(MacError::TxTimeout) => log::warn!("[MLME] tx-done timeout, retrying"),
Err(e) => return Err(e),
}
}
Err(MacError::NoAck)
}
fn flush(&mut self) {
while self.driver.poll_received().is_some() {}
}
fn poll_frame(&mut self) -> Option<Result<ReceivedFrame, MacError>> {
self.driver
.poll_received()
.map(|r| r.map_err(MacError::RadioError))
}
async fn next_frame(&mut self) -> Result<ReceivedFrame, MacError> {
loop {
self.driver.wait_rx_available().await;
if let Some(result) = self.poll_frame() {
return result;
}
self.driver.reset_rx_signal();
}
}
fn try_drain(&mut self, buf: &mut [u8]) -> Result<Option<(usize, u8)>, MacError> {
while let Some(result) = self.poll_frame() {
if let Some(received) = copy_data_payload(result?, buf) {
return Ok(Some(received));
}
}
Ok(None)
}
fn beacon_request_frame(&mut self) -> [u8; 10] {
let seq_number = self.sequence_number();
[0x3, 0x8, seq_number, 0xff, 0xff, 0xff, 0xff, 0x7, 0x0, 0x0]
}
async fn scan_channel_active(
&mut self,
channel: u8,
duration: u8,
) -> Result<Option<PanDescriptorList>, MacError> {
self.flush();
self.driver.update_driver_config(|config| {
config.promiscuous = false;
config.channel = channel;
});
self.driver.start_receive();
let frame = self.beacon_request_frame();
if let Err(e) = self.driver.transmit(&frame).await {
log::error!("[MLME-SCAN]: error transmitting beacon: {e}");
}
log::debug!("[MLME-SCAN] sent beacon frame to channel {channel}, waiting for messages...");
let delay_us: u64 = calculate_scan_duration_max_us(duration).into();
log::debug!("[MLME-SCAN] waiting for response for {delay_us}us");
let mut pds = Vec::new();
let deadline = Timer::after_micros(delay_us);
let mut deadline = core::pin::pin!(deadline);
loop {
match embassy_futures::select::select(&mut deadline, self.next_frame()).await {
Either::First(_) => break,
Either::Second(Ok(frame)) => {
if let Some(pd) = self.parse_beacon(frame) {
pds.push(pd);
}
}
Either::Second(Err(_)) => continue,
}
}
Ok(Some(pds))
}
fn parse_beacon(&self, received: ReceivedFrame) -> Option<PanDescriptor> {
match received {
ReceivedFrame {
frame:
Frame {
header:
hdr @ Header {
source: Some(source),
..
},
content: FrameContent::Beacon(beacon_content),
payload,
..
},
channel,
lqi,
..
} => {
log::debug!("[MLME-SCAN] received beacon frame on channel {channel}");
let zigbee_beacon = match payload.read_with(&mut 0, ()) {
Ok(zb) => zb,
Err(e) => {
log::warn!("[MLME-SCAN] failed to parse zigbee beacon: {e:?}");
return None;
}
};
Some(PanDescriptor {
channel,
coord_addr_mode: match source {
Address::Short(_, _) => 0x2,
Address::Extended(_, _) => 0x3,
},
coord_pan_id: source.pan_id().0.into(),
coord_address: source,
superframe_spec: beacon_content.superframe_spec,
link_quality: lqi,
security_use: hdr.has_security(),
zigbee_beacon,
})
}
other => {
log::debug!("[MLME-SCAN] received non-beacon frame: {other:?}");
None
}
}
}
fn data_request_frame(&mut self, dest: Address) -> Result<([u8; 20], usize), MacError> {
let seq = self.sequence_number();
let source = match self.driver.short_address() {
Some(short) => Address::Short(dest.pan_id(), ieee802154::mac::ShortAddress(short)),
None => Address::Extended(dest.pan_id(), self.driver.ieee_address()),
};
let frame_header = Header {
frame_type: FrameType::MacCommand,
frame_pending: false,
ack_request: true,
pan_id_compress: true,
seq_no_suppress: false,
ie_present: false,
version: FrameVersion::Ieee802154_2003,
seq,
destination: Some(dest),
source: Some(source),
auxiliary_security_header: None,
};
let frame_content = FrameContent::Command(Command::DataRequest);
let mut buf = [0u8; 20];
let offset = &mut 0;
buf.write_with(
offset,
frame_header,
&Some(&mut SecurityContext::no_security()),
)?;
buf.write_with(offset, frame_content, ())?;
Ok((buf, *offset + 2))
}
async fn recv_association_response(
&mut self,
timeout_us: u64,
) -> Result<Option<AssociationResponse>, MacError> {
let timeout = Timer::after_micros(timeout_us);
let receive = async {
loop {
Timer::after_micros(4000).await;
while let Some(result) = self.poll_frame() {
if let ReceivedFrame {
frame:
Frame {
content:
FrameContent::Command(Command::AssociationResponse(
short_addr,
status,
)),
..
},
..
} = result?
{
return Ok::<_, MacError>(AssociationResponse {
device_address: zigbee_types::IeeeAddress(self.driver.ieee_address().0),
association_address: zigbee_types::ShortAddress(short_addr.0),
status,
});
}
}
}
};
match embassy_futures::select::select(timeout, receive).await {
Either::First(_) => Ok(None),
Either::Second(Ok(r)) => Ok(Some(r)),
Either::Second(Err(e)) => Err(e),
}
}
fn take_unicast_data(&mut self, buf: &mut [u8]) -> Result<Option<(usize, u8)>, MacError> {
while let Some(result) = self.poll_frame() {
let received = result?;
if matches!(
received.frame.header.destination,
Some(Address::Short(_, ieee802154::mac::ShortAddress(d))) if d >= 0xfff8
) {
continue;
}
if let Some(data) = copy_data_payload(received, buf) {
return Ok(Some(data));
}
}
Ok(None)
}
async fn recv_data_response(
&mut self,
timeout_us: u64,
buf: &mut [u8],
) -> Result<Option<(usize, u8)>, MacError> {
let timeout = Timer::after_micros(timeout_us);
let receive = async {
loop {
Timer::after_micros(4000).await;
if let Some(data) = self.take_unicast_data(buf)? {
return Ok::<_, MacError>(Some(data));
}
}
};
match embassy_futures::select::select(timeout, receive).await {
Either::First(_) => Ok(None),
Either::Second(result) => result,
}
}
fn association_request_frame(
&mut self,
dest: Address,
src: Option<Address>,
capabilities: CapabilityInformation,
) -> Result<[u8; 21], MacError> {
let seq = self.sequence_number();
let frame_header = Header {
frame_type: FrameType::MacCommand,
frame_pending: false,
ack_request: true,
pan_id_compress: false,
seq_no_suppress: false,
ie_present: false,
version: FrameVersion::Ieee802154_2003,
seq,
destination: Some(dest),
source: src,
auxiliary_security_header: None,
};
let frame_content = FrameContent::Command(Command::AssociationRequest(capabilities));
let mut buf = [0u8; 21];
let offset = &mut 0;
buf.write_with(
offset,
frame_header,
&Some(&mut SecurityContext::no_security()),
)?;
buf.write_with(offset, frame_content, ())?;
Ok(buf)
}
}
fn copy_data_payload(received: ReceivedFrame, buf: &mut [u8]) -> Option<(usize, u8)> {
let ReceivedFrame {
frame:
Frame {
content: FrameContent::Data,
payload,
..
},
lqi,
..
} = received
else {
return None;
};
let len = payload.len().min(buf.len());
buf[..len].copy_from_slice(&payload[..len]);
Some((len, lqi))
}
fn calculate_scan_duration_max_us(duration: u8) -> u32 {
16 * A_BASE_SUPER_FRAME_DURATION * (2 * (duration as u32) + 1)
}
impl EspMlmeInner<'_> {
async fn scan_network(
&mut self,
scan_type: ScanType,
channels: core::ops::Range<u8>,
duration: u8,
) -> Result<ScanResult, MacError> {
if !matches!(scan_type, ScanType::Active) {
return Err(MacError::InvalidScanParams);
}
log::debug!("[MLME-SCAN] start scan");
let mut pan_descriptor = Vec::new();
for c in channels {
if (c as usize) >= MAX_IEEE802154_CHANNELS {
continue;
}
match self.scan_channel_active(c, duration).await {
Ok(Some(mut pd)) => {
pan_descriptor.append(&mut pd);
}
Err(e) => {
log::error!("[MLME-SCAN] error on channel {c}: {e}");
}
_ => (),
}
}
log::debug!("[MLME-SCAN] success");
Ok(ScanResult {
scan_type,
pan_descriptor,
})
}
async fn associate(
&mut self,
channel: u8,
dest: Address,
capabilities: CapabilityInformation,
) -> Result<AssociationResponse, MacError> {
self.driver.update_driver_config(|config| {
*config = Default::default();
config.channel = channel;
config.pan_id = Some(dest.pan_id().0);
config.auto_ack_tx = true;
config.auto_ack_rx = true;
config.promiscuous = false;
});
self.driver.start_receive();
let ext_addr = self.driver.ieee_address();
let src = Address::Extended(PanId::broadcast(), ext_addr);
let timeout_us = (A_RESPONSE_WAIT_TIME as u64) * 16;
let mut response = None;
'association: for _ in 0..ASSOCIATE_REQUEST_RETRIES {
let frame = self.association_request_frame(dest, Some(src), capabilities)?;
match self.transmit_acked(&frame).await {
Ok(()) => {}
Err(MacError::NoAck) => {
log::debug!("[MLME-ASSOCIATE] request not acked, retrying");
continue;
}
Err(e) => return Err(e),
}
log::debug!(
"[MLME-ASSOCIATE] request acked, ack_pending={:?}",
self.driver.last_ack_frame_pending()
);
if let Some(r) = self.recv_association_response(timeout_us).await? {
response = Some(r);
break 'association;
}
for _ in 0..ASSOCIATE_POLL_RETRIES {
let (data_req, len) = self.data_request_frame(dest)?;
match self.transmit_acked(&data_req[..len]).await {
Ok(()) | Err(MacError::NoAck) => {}
Err(e) => return Err(e),
}
self.driver.start_receive();
if let Some(r) = self.recv_association_response(timeout_us).await? {
response = Some(r);
break 'association;
}
}
}
let response = response.ok_or(MacError::NoData)?;
log::debug!(
"[MLME-ASSOCIATE] success, short_addr={:?}",
response.association_address
);
let short = response.association_address.0;
self.driver.update_driver_config(|config| {
config.promiscuous = false;
config.short_addr = Some(short);
});
Ok(response)
}
async fn poll_data(
&mut self,
coord_address: Address,
buf: &mut [u8],
) -> Result<(usize, u8), MacError> {
if let Some((len, lqi)) = self.take_unicast_data(buf)? {
log::trace!("[MLME-POLL] rx buffered data len={len}");
return Ok((len, lqi));
}
let timeout_us = (A_RESPONSE_WAIT_TIME as u64) * 16;
let mut acked = false;
for _ in 0..POLL_DATA_RETRIES {
let (data_req, len) = self.data_request_frame(coord_address)?;
match self.transmit_acked(&data_req[..len]).await {
Ok(()) => acked = true,
Err(MacError::NoAck) => {}
Err(e) => return Err(e),
}
self.driver.start_receive();
if let Some((len, lqi)) = self.recv_data_response(timeout_us, buf).await? {
log::trace!("[MLME-POLL] rx data len={len}");
return Ok((len, lqi));
}
}
if acked {
log::trace!("[MLME-POLL] no data");
Err(MacError::NoData)
} else {
log::trace!("[MLME-POLL] no ack");
Err(MacError::NoAck)
}
}
async fn transmit_data(&mut self, dest: Address, payload: &[u8]) -> Result<(), MacError> {
let seq = self.sequence_number();
let is_broadcast = matches!(dest, Address::Short(_, sa) if sa.0 >= 0xfff8);
let dest = if is_broadcast {
Address::Short(dest.pan_id(), ieee802154::mac::ShortAddress(0xffff))
} else {
dest
};
let source = Some(match self.driver.short_address() {
Some(short) => Address::Short(dest.pan_id(), ieee802154::mac::ShortAddress(short)),
None => Address::Extended(dest.pan_id(), self.driver.ieee_address()),
});
let frame_header = Header {
frame_type: FrameType::Data,
frame_pending: false,
ack_request: !is_broadcast,
pan_id_compress: source.is_some(),
seq_no_suppress: false,
ie_present: false,
version: FrameVersion::Ieee802154_2003,
seq,
destination: Some(dest),
source,
auxiliary_security_header: None,
};
let mut frame_buf = [0u8; 127];
let offset = &mut 0;
frame_buf.write_with(
offset,
frame_header,
&Some(&mut SecurityContext::no_security()),
)?;
let hdr_len = *offset;
let payload_len = payload.len().min(frame_buf.len() - hdr_len - 2);
frame_buf[hdr_len..hdr_len + payload_len].copy_from_slice(&payload[..payload_len]);
let total_len = hdr_len + payload_len + 2;
if is_broadcast {
self.driver.transmit(&frame_buf[..total_len]).await?;
} else {
self.transmit_acked(&frame_buf[..total_len]).await?;
}
log::trace!("[MLME] tx data, len={total_len}");
Ok(())
}
}
impl Mlme for EspMlme<'_> {
fn ieee_address(&self) -> zigbee_types::IeeeAddress {
zigbee_types::IeeeAddress(self.ieee_address)
}
async fn configure(&self, config: MacConfig) {
let Ok(mut inner) = self.inner.lock().await else {
log::error!("[MAC] radio lock queue full, skipping configure");
return;
};
inner.driver.update_driver_config(|driver| {
if let Some(channel) = config.channel {
driver.channel = channel;
}
if let Some(pan_id) = config.pan_id {
driver.pan_id = Some(pan_id.0);
}
if let Some(short_address) = config.short_address {
driver.short_addr = Some(short_address.0);
}
if let Some(promiscuous) = config.promiscuous {
driver.promiscuous = promiscuous;
}
if let Some(auto_ack_rx) = config.auto_ack_rx {
driver.auto_ack_rx = auto_ack_rx;
}
if let Some(auto_ack_tx) = config.auto_ack_tx {
driver.auto_ack_tx = auto_ack_tx;
}
});
inner.driver.start_receive();
}
async fn scan_network(
&self,
ty: ScanType,
channels: core::ops::Range<u8>,
duration: u8,
) -> Result<ScanResult, MacError> {
self.lock()
.await?
.scan_network(ty, channels, duration)
.await
}
async fn associate(
&self,
channel: u8,
dest: Address,
capabilities: CapabilityInformation,
) -> Result<AssociationResponse, MacError> {
self.lock()
.await?
.associate(channel, dest, capabilities)
.await
}
async fn poll_data(
&self,
coord_address: Address,
buf: &mut [u8],
) -> Result<(usize, u8), MacError> {
self.lock().await?.poll_data(coord_address, buf).await
}
async fn receive(&self, buf: &mut [u8]) -> Result<(usize, u8), MacError> {
match self.receive_data(buf).await {
Ok((len, lqi)) => {
log::trace!("[MLME] rx data, len={len}");
Ok((len, lqi))
}
Err(e) => Err(e),
}
}
async fn transmit_data(&self, dest: Address, payload: &[u8]) -> Result<(), MacError> {
self.lock().await?.transmit_data(dest, payload).await
}
fn random_u32(&self) -> u32 {
esp_hal::rng::Rng::new().random()
}
}