use embassy_stm32::can::frame::{FdFrame, Frame, Header};
use embedded_can::StandardId;
use crate::protocol::{EncodedFrame, FramePayload, FrameRef};
impl From<&EncodedFrame> for Frame {
fn from(frame: &EncodedFrame) -> Self {
let data = if frame.is_remote() {
&[0; 8]
} else {
frame.data()
};
Self::new(encoded_header(frame), data).expect("valid encoded Classic CAN frame")
}
}
impl From<&EncodedFrame> for FdFrame {
fn from(frame: &EncodedFrame) -> Self {
let data = if frame.is_remote() {
&[0; 8]
} else {
frame.data()
};
Self::new(encoded_header(frame), data).expect("valid encoded Classic CAN frame")
}
}
fn encoded_header(frame: &EncodedFrame) -> Header {
let id = StandardId::new(frame.id()).expect("encoded standard CAN ID");
Header::new(id.into(), frame.dlc(), frame.is_remote())
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum FromEmbassyError {
InvalidPayloadLength,
}
impl core::fmt::Display for FromEmbassyError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.write_str("Embassy frame length exceeds its buffer")
}
}
impl core::error::Error for FromEmbassyError {}
impl<'a> TryFrom<&'a Frame> for FrameRef<'a> {
type Error = FromEmbassyError;
fn try_from(frame: &'a Frame) -> Result<Self, Self::Error> {
from_parts(frame.header(), frame.raw_data())
}
}
impl<'a> TryFrom<&'a FdFrame> for FrameRef<'a> {
type Error = FromEmbassyError;
fn try_from(frame: &'a FdFrame) -> Result<Self, Self::Error> {
let header = frame.header();
if !header.fdcan() && header.rtr() {
return from_parts(header, &[]);
}
if header.len() > 64 {
return Err(FromEmbassyError::InvalidPayloadLength);
}
from_parts(header, frame.data())
}
}
fn from_parts<'a>(header: &Header, data: &'a [u8]) -> Result<FrameRef<'a>, FromEmbassyError> {
let payload = if header.fdcan() {
FramePayload::Fd(data)
} else if header.rtr() {
FramePayload::Remote { dlc: header.len() }
} else {
FramePayload::Data(
data.get(..usize::from(header.len()))
.ok_or(FromEmbassyError::InvalidPayloadLength)?,
)
};
Ok(FrameRef {
id: (*header.id()).into(),
payload,
})
}