commit 79fd6c5af44aa03c9ff1232cc3e352ff71b28bde Author: pfzetto Date: Fri Aug 29 21:44:27 2025 +0200 init diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..58f4d26 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +**/target +Cargo.lock diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..15af5a6 --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,3 @@ +[workspace] +resolver = "2" +members = ["canopen"] diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/canopen/Cargo.toml b/canopen/Cargo.toml new file mode 100644 index 0000000..8eda846 --- /dev/null +++ b/canopen/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "canopen" +version = "0.1.0" +edition = "2024" + +[dependencies] +embedded-can = "0.4" +ux = "0.1" +heapless = "0.8" +derive_more = { version = "2", default-features = false, features = ["from"]} +num_enum = { version = "0.7", default-features = false } + +[dev-dependencies] +async-channel = "2.5" +async-io = "2.5" +smol = "2.0" +futures = { version = "0.3" } diff --git a/canopen/src/can.rs b/canopen/src/can.rs new file mode 100644 index 0000000..b6bff0a --- /dev/null +++ b/canopen/src/can.rs @@ -0,0 +1,88 @@ +use core::pin::Pin; +use core::task::{Context, Poll}; +use embedded_can::{Frame, Id}; +use heapless::Vec; + +pub trait CanTx { + type Error; + type Frame: Frame; + + fn send(&mut self, frame: Self::Frame) -> impl Future>; +} + +pub trait CanRx { + type Error; + type Frame: Frame; + + fn poll_recv( + self: Pin<&mut Self>, + cx: &mut Context<'_>, + ) -> Poll>; +} + +pub enum CanBody { + RemoteTransmissionRequest(CanBodyRtr), + Data(CanBodyData), +} + +impl CanBody { + pub fn new_remote(dlc: usize) -> Option { + if dlc <= 8 { + Some(Self::RemoteTransmissionRequest(CanBodyRtr { + size: dlc as _, + })) + } else { + None + } + } + + pub fn to_frame(self, id: Id) -> F { + match self { + Self::Data(x) => F::new(id, x.data.as_slice()).unwrap(), + Self::RemoteTransmissionRequest(x) => F::new_remote(id, x.size as _).unwrap(), + } + } + + pub fn from_frame(frame: &F) -> Self { + if frame.is_data_frame() { + Self::Data(CanBodyData { + data: Vec::from_slice(frame.data()).unwrap(), + }) + } else { + Self::RemoteTransmissionRequest(CanBodyRtr { + size: frame.dlc() as _, + }) + } + } + pub fn from_slice(array: &[u8]) -> Option { + Some(Self::Data(CanBodyData { + data: Vec::from_slice(&array).ok()?, + })) + } + pub fn data_from_array(array: [u8; N]) -> Self { + assert!(N <= 8); + Self::Data(CanBodyData { + data: Vec::from_slice(&array).unwrap(), + }) + } + pub fn data(&self) -> Option<&[u8]> { + match self { + Self::Data(data) => Some(data.data.as_slice()), + _ => None, + } + } + pub const fn is_data_frame(&self) -> bool { + matches!(self, Self::Data(_)) + } + pub const fn is_remote_frame(&self) -> bool { + matches!(self, Self::RemoteTransmissionRequest(_)) + } +} + +pub struct CanBodyRtr { + pub size: u8, +} + +pub struct CanBodyData { + pub data: Vec, +} diff --git a/canopen/src/communication.rs b/canopen/src/communication.rs new file mode 100644 index 0000000..c5a9d1d --- /dev/null +++ b/canopen/src/communication.rs @@ -0,0 +1,22 @@ +use crate::can::CanBody; + +pub mod emcy; +pub mod nmt; +pub mod pdo; +pub mod sdo; + +pub trait DecodeCommunicationObject { + fn decode(body: CanBody) -> Result + where + Self: Sized; +} + +pub trait EncodeCommunicationObject { + fn encode(self) -> CanBody; +} + +pub enum DecodeError { + InvalidFrameType, + InvalidFormat, + InvalidValue, +} diff --git a/canopen/src/communication/emcy.rs b/canopen/src/communication/emcy.rs new file mode 100644 index 0000000..4d97b61 --- /dev/null +++ b/canopen/src/communication/emcy.rs @@ -0,0 +1,59 @@ +use ux::u40; + +use crate::{ + can::CanBody, + communication::{DecodeCommunicationObject, DecodeError, EncodeCommunicationObject}, +}; + +pub enum EmcyCommunicationObject { + Write(EmcyWrite), +} + +/// CiA 301 7.2.7.3.1 Protocol EMCY write +pub struct EmcyWrite { + pub emergency_error_code: u16, + pub error_register: u8, + pub manufacturer_error_code: u40, +} + +impl EncodeCommunicationObject for EmcyWrite { + fn encode(self) -> CanBody { + let emergency_error_code = self.emergency_error_code.to_be_bytes(); + let manufacturer_error_code = &u64::from(self.manufacturer_error_code).to_be_bytes()[3..]; + CanBody::data_from_array([ + emergency_error_code[0], + emergency_error_code[1], + self.error_register, + manufacturer_error_code[0], + manufacturer_error_code[1], + manufacturer_error_code[2], + manufacturer_error_code[3], + manufacturer_error_code[4], + manufacturer_error_code[5], + ]) + } +} +impl DecodeCommunicationObject for EmcyWrite { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let payload = body.data().ok_or(DecodeError::InvalidFrameType)?; + if payload.len() != 8 { + return Err(DecodeError::InvalidFormat); + } + let emergency_error_code = u16::from_be_bytes([payload[0], payload[1]]); + let error_register = payload[2]; + let Ok(manufacturer_error_code) = u40::try_from(u64::from_be_bytes([ + 0, 0, 0, payload[3], payload[4], payload[5], payload[6], payload[7], + ])) else { + unreachable!("u64 with 24 leading zeros to fit into u40"); + }; + + Ok(Self { + emergency_error_code, + error_register, + manufacturer_error_code, + }) + } +} diff --git a/canopen/src/communication/nmt.rs b/canopen/src/communication/nmt.rs new file mode 100644 index 0000000..f200104 --- /dev/null +++ b/canopen/src/communication/nmt.rs @@ -0,0 +1,231 @@ +use crate::{ + NodeId, + can::{CanBody, CanBodyRtr}, + communication::{DecodeCommunicationObject, DecodeError, EncodeCommunicationObject}, +}; + +pub enum NmtCommunicationObjectMS { + StartRemoteNode(NmtStartRemoteNode), + StopRemoteNode(NmtStopRemoteNode), + EnterPreoperational(NmtEnterPreoperational), + ResetNode(NmtResetNode), + ResetCommunication(NmtResetCommunication), + NodeGuardingReq(NmtNodeGuardingReq), +} + +pub enum NmtCommunicationObjectSM { + NodeGuardingRes(NmtNodeGuardingRes), + Heartbeat(NmtHeartbeat), + Bootup(NmtBootup), +} + +impl NmtCommunicationObjectSM { + pub fn serialize(self) -> CanBody { + /* + match self { + Self::StartRemoteNode(x) => x.serialize(), + Self::StopRemoteNode(x) => x.serialize(), + Self::EnterPreoperational(x) => x.serialize(), + Self::ResetNode(x) => x.serialize(), + Self::ResetCommunication(x) => x.serialize(), + Self::NodeGuardingReq(x) => x.serialize(), + Self::NodeGuardingRes(x) => x.serialize(), + Self::Heartbeat(x) => x.serialize(), + //Self::Bootup(x) => x.serialize(), + _ => todo!(), + } + */ + todo!() + } +} + +impl DecodeCommunicationObject for NmtCommunicationObjectMS { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let payload = body.data().ok_or(DecodeError::InvalidFrameType)?; + if payload.len() != 2 { + return Err(DecodeError::InvalidFormat); + } + let node = NodeId::new(payload[1]).ok_or(DecodeError::InvalidValue)?; + match payload[0] { + 1 => Ok(Self::StartRemoteNode(NmtStartRemoteNode { node })), + 2 => Ok(Self::StopRemoteNode(NmtStopRemoteNode { node })), + 128 => Ok(Self::EnterPreoperational(NmtEnterPreoperational { node })), + 129 => Ok(Self::ResetNode(NmtResetNode { node })), + 130 => Ok(Self::ResetCommunication(NmtResetCommunication { node })), + _ => Err(DecodeError::InvalidFormat), + } + } +} + +/// CiA 301 7.2.8.3.1.1 Protocol start remote node +pub struct NmtStartRemoteNode { + pub node: NodeId, +} +impl EncodeCommunicationObject for NmtStartRemoteNode { + fn encode(self) -> CanBody { + CanBody::data_from_array([1, self.node.0]) + } +} + +/// CiA 301 7.2.8.3.1.2 Protocol stop remote node +pub struct NmtStopRemoteNode { + pub node: NodeId, +} +impl EncodeCommunicationObject for NmtStopRemoteNode { + fn encode(self) -> CanBody { + CanBody::data_from_array([2, self.node.0]) + } +} + +/// CiA 301 7.2.8.3.1.3 Protocol enter pre-operational +pub struct NmtEnterPreoperational { + pub node: NodeId, +} + +impl EncodeCommunicationObject for NmtEnterPreoperational { + fn encode(self) -> CanBody { + CanBody::data_from_array([128, self.node.0]) + } +} + +/// CiA 301 7.2.8.3.1.4 Protocol reset node +pub struct NmtResetNode { + pub node: NodeId, +} + +impl EncodeCommunicationObject for NmtResetNode { + fn encode(self) -> CanBody { + CanBody::data_from_array([129, self.node.0]) + } +} + +/// CiA 301 7.2.8.3.1.5 Protocol reset communication +pub struct NmtResetCommunication { + pub node: NodeId, +} + +impl EncodeCommunicationObject for NmtResetCommunication { + fn encode(self) -> CanBody { + CanBody::data_from_array([130, self.node.0]) + } +} + +/// CiA 301 7.2.8.3.2.1 Protocol node guarding +pub struct NmtNodeGuardingReq; + +impl EncodeCommunicationObject for NmtNodeGuardingReq { + fn encode(self) -> CanBody { + //TODO is size necessary? + CanBody::RemoteTransmissionRequest(CanBodyRtr { size: 0 }) + } +} + +/// CiA 301 7.2.8.3.2.1 Protocol node guarding +pub struct NmtNodeGuardingRes { + pub toggle_bit: bool, + pub state: NmtNodeGuardingResState, +} + +#[derive(Clone, Copy)] +pub enum NmtNodeGuardingResState { + Stopped, + Operational, + PreOperational, +} + +impl EncodeCommunicationObject for NmtNodeGuardingRes { + fn encode(self) -> CanBody { + let state = match self.state { + NmtNodeGuardingResState::Stopped => 4, + NmtNodeGuardingResState::Operational => 5, + NmtNodeGuardingResState::PreOperational => 127, + }; + CanBody::data_from_array([state]) + } +} +impl DecodeCommunicationObject for NmtNodeGuardingRes { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let payload = body.data().ok_or(DecodeError::InvalidFormat)?; + if payload.len() != 1 { + return Err(DecodeError::InvalidFormat); + } + let toggle_bit = (payload[0] >> 7) & 1 == 1; + let state = match payload[0] & 0b01111111 { + 4 => NmtNodeGuardingResState::Stopped, + 5 => NmtNodeGuardingResState::Operational, + 127 => NmtNodeGuardingResState::PreOperational, + _ => return Err(DecodeError::InvalidFormat), + }; + Ok(Self { toggle_bit, state }) + } +} + +/// CiA 301 7.2.8.3.2.2 Protocol heartbeat +pub struct NmtHeartbeat { + pub state: NmtHeartbeatState, +} + +pub enum NmtHeartbeatState { + BootUp, + Stopped, + Operational, + PreOperational, +} + +impl EncodeCommunicationObject for NmtHeartbeat { + fn encode(self) -> CanBody { + let state = match self.state { + NmtHeartbeatState::BootUp => 0, + NmtHeartbeatState::Stopped => 4, + NmtHeartbeatState::Operational => 5, + NmtHeartbeatState::PreOperational => 127, + }; + CanBody::data_from_array([state]) + } +} +impl DecodeCommunicationObject for NmtHeartbeat { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let payload = body.data().ok_or(DecodeError::InvalidFrameType)?; + if payload.len() != 1 { + return Err(DecodeError::InvalidFormat); + } + let state = match payload[0] { + 0 => NmtHeartbeatState::BootUp, + 4 => NmtHeartbeatState::Stopped, + 5 => NmtHeartbeatState::Operational, + 127 => NmtHeartbeatState::PreOperational, + _ => return Err(DecodeError::InvalidFormat), + }; + Ok(Self { state }) + } +} + +/// CiA 301 7.2.8.3.3 Protocol boot-up +pub struct NmtBootup; + +impl EncodeCommunicationObject for NmtBootup { + fn encode(self) -> CanBody { + CanBody::data_from_array([0]) + } +} +impl DecodeCommunicationObject for NmtBootup { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let payload = body.data().ok_or(DecodeError::InvalidFrameType)?; + if payload.len() != 1 || payload[0] != 0 { + return Err(DecodeError::InvalidFormat); + } + Ok(Self) + } +} diff --git a/canopen/src/communication/pdo.rs b/canopen/src/communication/pdo.rs new file mode 100644 index 0000000..76422ec --- /dev/null +++ b/canopen/src/communication/pdo.rs @@ -0,0 +1,239 @@ +use heapless::Vec; +use ux::u9; + +pub mod consumer; +pub mod producer; + +use crate::{ + can::{CanBody, CanBodyData, CanBodyRtr}, + communication::{DecodeCommunicationObject, DecodeError, EncodeCommunicationObject}, +}; +pub enum PdoCommunicationObject { + Write(PdoWrite), + ReadReq(PdoReadReq), + ReadRes(PdoReadRes), + // MuxedWriteSource(MpdoWriteSource), + // MuxedWriteDestination(MpdoWriteDestination), +} + +pub struct PdoId(pub u9); + +/// CiA 301 7.2.2.5.1 Protocol PDO write +pub struct PdoWrite { + /// application data + /// `|data| > 0` + // TODO maybe introduce BoundedVec that has min and max const parameterss + pub data: Vec, +} + +impl EncodeCommunicationObject for PdoWrite { + fn encode(self) -> CanBody { + CanBody::Data(CanBodyData { data: self.data }) + } +} +impl DecodeCommunicationObject for PdoWrite { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let CanBody::Data(data) = body else { + return Err(DecodeError::InvalidFormat); + }; + Ok(Self { data: data.data }) + } +} + +/// CiA 301 7.2.2.5.2 Protocol PDO read +pub struct PdoReadReq; + +impl EncodeCommunicationObject for PdoReadReq { + fn encode(self) -> CanBody { + //TODO is the size needed? + CanBody::RemoteTransmissionRequest(CanBodyRtr { size: 0 }) + } +} +impl DecodeCommunicationObject for PdoReadReq { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let CanBody::RemoteTransmissionRequest(_) = body else { + return Err(DecodeError::InvalidFormat); + }; + Ok(Self) + } +} + +/// CiA 301 7.2.2.5.2 Protocol PDO read +pub struct PdoReadRes { + /// application data + /// `|data| > 0` + // TODO maybe introduce BoundedVec that has min and max const parameterss + pub data: Vec, +} + +impl EncodeCommunicationObject for PdoReadRes { + fn encode(self) -> CanBody { + CanBody::Data(CanBodyData { data: self.data }) + } +} +impl DecodeCommunicationObject for PdoReadRes { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let CanBody::Data(data) = body else { + return Err(DecodeError::InvalidFormat); + }; + Ok(Self { data: data.data }) + } +} +/* + +/// CiA 301 7.2.3.3.2 Protocol MPDO read +pub struct MpdoWriteSource { + /// producer of the MPDO transmission + pub source: NodeId, + /// index/sub-index that is used to identify the data on the transmitting CANopen device + pub multiplexer: u24, + /// process data filled to fit 32-Bit + pub data: [u8; 4], +} + +impl EncodeCommunicationObject for MpdoWriteSource { + fn encode(self) -> Result<(crate::CobId, Option), Error> { + let cob_id = CobId::Pdo(self.pdo_channel, self.source); + //TODO is big endian correct encoding? + let multiplexer = &u32::from(self.multiplexer).to_be_bytes()[1..3]; + Ok(( + cob_id, + Some(Payload::from_array([ + u8::from(self.destination.get_raw()), + multiplexer[0], + multiplexer[1], + multiplexer[3], + self.data[0], + self.data[1], + self.data[2], + self.data[3], + ])), + )) + } +} +impl DecodeCommunicationObject for MpdoWriteSource { + fn decode(cob_id: crate::CobId, payload: Option) -> Result + where + Self: Sized, + { + let CobId::Pdo(pdo_channel, destination) = cob_id else { + return Err(Error::InvalidCobId); + }; + + let data = payload.ok_or(Error::InvalidFormat)?.as_array::<8>()?; + let address = data[0]; + let address_type = (address >> 7) & 1; + + if address_type == 0 { + let source = address & 0b1111111; + let source = NodeId::from_raw( + u7::try_from(source).expect("7-Bit masked integer to fit into u7"), + ); + + //TODO is big endian correct encoding? + let multiplexer = u32::from_be_bytes([0, data[1], data[2], data[3]]); + let multiplexer = + u24::try_from(multiplexer).expect("u32 with 8 leading zeros to fit into u24"); + let data = [data[4], data[5], data[6], data[7]]; + Ok(Self { + pdo_channel, + destination, + source, + multiplexer, + data, + }) + } else { + Err(Error::InvalidFormat) + } + } +} + +/// CiA 301 7.2.3.3.2 Protocol MPDO read +pub struct MpdoWriteDestination { + pub pdo_channel: PdoChannel, + pub source: NodeId, + /// This communicaton object targets all CANopen devices in the network that are configured for MPDO reception in destination + /// addressing mode if `addr=None`. Otherwise it targets the CANopen device with the `node_id` if + /// `addr=Some(node_id)`. + pub destination: Option, + /// index/sub-index that is used to identify the data on the receiving CANopen device + pub multiplexer: u24, + /// process data filled to fit 32-Bit + pub data: [u8; 4], +} + +impl EncodeCommunicationObject for MpdoWriteDestination { + fn encode(self) -> Result<(crate::CobId, Option), Error> { + //TODO is big endian correct encoding? + let multiplexer = &u32::from(self.multiplexer).to_be_bytes()[1..3]; + Ok(( + CobId::Pdo(self.pdo_channel, self.source), + Some(Payload::from_array([ + 0b10000000 + | u8::from( + self.destination + .as_ref() + .map(NodeId::get_raw) + .unwrap_or_default(), + ), + multiplexer[0], + multiplexer[1], + multiplexer[3], + self.data[0], + self.data[1], + self.data[2], + self.data[3], + ])), + )) + } +} +impl DecodeCommunicationObject for MpdoWriteDestination { + fn decode(cob_id: crate::CobId, payload: Option) -> Result + where + Self: Sized, + { + let CobId::Pdo(pdo_channel, source) = cob_id else { + return Err(Error::InvalidCobId); + }; + + let data = payload.ok_or(Error::InvalidFormat)?.as_array::<8>()?; + let address = data[0]; + let address_type = (address >> 7) & 1; + + if address_type == 1 { + let destination = address & 0b1111111; + let destination = match destination { + 0 => None, + x => Some(NodeId::from_raw( + u7::try_from(x).expect("7-Bit masked integer to fit into u7"), + )), + }; + + //TODO is big endian correct encoding? + let multiplexer = u32::from_be_bytes([0, data[1], data[2], data[3]]); + let multiplexer = + u24::try_from(multiplexer).expect("u32 with 8 leading zeros to fit into u24"); + let data = [data[4], data[5], data[6], data[7]]; + Ok(Self { + pdo_channel, + source, + destination, + multiplexer, + data, + }) + } else { + Err(Error::InvalidFormat) + } + } +} + +*/ diff --git a/canopen/src/communication/pdo/consumer.rs b/canopen/src/communication/pdo/consumer.rs new file mode 100644 index 0000000..766acd7 --- /dev/null +++ b/canopen/src/communication/pdo/consumer.rs @@ -0,0 +1,31 @@ +use core::iter::Iterator; + +use crate::object::{cia301::PdoMapping, dictionary::ObjectDictionary}; +pub enum Error { + ObjectDictionary(OD::Error), + OutOfBounds, +} + +pub fn pdo_consume<'a, OD: ObjectDictionary>( + data: [u8; 8], + mappings: impl Iterator, + od: &mut OD, +) -> Result<(), Error> { + let mut bit_idx = 0; + let value: u64 = u64::from_le_bytes(data); + for mapping in mappings { + if bit_idx + mapping.length > 64 { + return Err(Error::OutOfBounds); + } + let mask = (1 << mapping.length) - 1; + let obj_val = (value >> bit_idx) & mask; + + let data = obj_val.to_le_bytes(); + + od.write(mapping.index, mapping.sub_index, &data) + .map_err(Error::ObjectDictionary)?; + + bit_idx += mapping.length; + } + Ok(()) +} diff --git a/canopen/src/communication/pdo/producer.rs b/canopen/src/communication/pdo/producer.rs new file mode 100644 index 0000000..46f5617 --- /dev/null +++ b/canopen/src/communication/pdo/producer.rs @@ -0,0 +1,40 @@ +use core::iter::Iterator; + +use crate::object::{cia301::PdoMapping, dictionary::ObjectDictionary}; + +#[derive(Debug)] +pub enum Error { + Can(CAN::Error), + ObjectDictionary(OD::Error), + OutOfBounds, +} + +pub trait CanDriver { + type Error; + fn send(&mut self, data: [u8; 8]) -> impl Future>; +} + +pub async fn pdo_transmit<'a, CAN: CanDriver, OD: ObjectDictionary>( + mappings: impl Iterator, + can: &mut CAN, + od: &mut OD, +) -> Result<(), Error> { + let mut bit_idx = 0; + let mut value: u64 = 0; + for mapping in mappings { + if bit_idx + mapping.length > 64 { + return Err(Error::OutOfBounds); + } + let mut buf = [0u8; 8]; + od.read(mapping.index, mapping.sub_index, &mut buf) + .map_err(Error::ObjectDictionary)?; + + let mask = (1 << mapping.length) - 1; + let obj_val = u64::from_le_bytes(buf) & mask; + value |= obj_val << bit_idx; + bit_idx += mapping.length + } + let data = value.to_le_bytes(); + can.send(data).await.map_err(Error::Can)?; + Ok(()) +} diff --git a/canopen/src/communication/sdo.rs b/canopen/src/communication/sdo.rs new file mode 100644 index 0000000..ae23aa7 --- /dev/null +++ b/canopen/src/communication/sdo.rs @@ -0,0 +1,513 @@ +use crate::{ + can::CanBody, + communication::{DecodeCommunicationObject, DecodeError, EncodeCommunicationObject}, +}; +use derive_more::From; +use heapless::Vec; +use num_enum::{FromPrimitive, IntoPrimitive}; + +pub mod client; +pub mod server; + +#[derive(From)] +pub enum SdoCommunicationObjectReq { + DownloadInitiateReq(SdoDownloadInitiateReq), + DownloadSegmentReq(SdoDownloadSegmentReq), + UploadInitiateReq(SdoUploadInitiateReq), + UploadSegmentReq(SdoUploadSegmentReq), + Abort(SdoAbort), +} + +impl DecodeCommunicationObject for SdoCommunicationObjectReq { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let body_data = body.data().ok_or(DecodeError::InvalidFrameType)?; + if body_data.len() != 8 { + return Err(DecodeError::InvalidFormat); + } + let mut data = [0u8; 8]; + data.copy_from_slice(body_data); + let ccs = (data[0] >> 5) & 0b111; + match ccs { + 0 => Ok(Self::DownloadSegmentReq(SdoDownloadSegmentReq::decode( + &data, + )?)), + 1 => Ok(Self::DownloadInitiateReq(SdoDownloadInitiateReq::decode( + &data, + )?)), + 2 => Ok(Self::UploadInitiateReq(SdoUploadInitiateReq::decode( + &data, + )?)), + 3 => Ok(Self::UploadSegmentReq(SdoUploadSegmentReq::decode(&data)?)), + 4 => Ok(Self::Abort(SdoAbort::decode(&data)?)), + _ => Err(DecodeError::InvalidFormat), + } + } +} + +impl EncodeCommunicationObject for SdoCommunicationObjectReq { + fn encode(self) -> CanBody { + match self { + Self::DownloadInitiateReq(x) => x.encode(), + Self::DownloadSegmentReq(x) => x.encode(), + Self::UploadInitiateReq(x) => x.encode(), + Self::UploadSegmentReq(x) => x.encode(), + Self::Abort(x) => x.encode(), + } + } +} + +#[derive(From)] +pub enum SdoCommunicationObjectRes { + DownloadInitiateRes(SdoDownloadInitiateRes), + DownloadSegmentRes(SdoDownloadSegmentRes), + UploadInitiateRes(SdoUploadInitiateRes), + UploadSegmentRes(SdoUploadSegmentRes), + Abort(SdoAbort), +} + +impl EncodeCommunicationObject for SdoCommunicationObjectRes { + fn encode(self) -> CanBody { + match self { + Self::DownloadInitiateRes(x) => x.encode(), + Self::DownloadSegmentRes(x) => x.encode(), + Self::UploadInitiateRes(x) => x.encode(), + Self::UploadSegmentRes(x) => x.encode(), + Self::Abort(x) => x.encode(), + } + } +} +impl DecodeCommunicationObject for SdoCommunicationObjectRes { + fn decode(body: CanBody) -> Result + where + Self: Sized, + { + let body_data = body.data().ok_or(DecodeError::InvalidFormat)?; + if body_data.len() != 8 { + return Err(DecodeError::InvalidFormat); + } + let mut data = [0u8; 8]; + data.copy_from_slice(body_data); + let scs = (data[0] >> 5) & 0b111; + match scs { + 2 => Ok(Self::UploadInitiateRes(SdoUploadInitiateRes::decode( + &data, + )?)), + 1 => Ok(Self::DownloadSegmentRes(SdoDownloadSegmentRes::decode( + &data, + )?)), + 0 => Ok(Self::UploadSegmentRes(SdoUploadSegmentRes::decode(&data)?)), + 3 => Ok(Self::DownloadInitiateRes(SdoDownloadInitiateRes::decode( + &data, + )?)), + 4 => Ok(Self::Abort(SdoAbort::decode(&data)?)), + _ => Err(DecodeError::InvalidFormat), + } + } +} + +/// CiA 301 7.2.4.3.3 Protocol SDO download initiate +pub struct SdoDownloadInitiateReq { + pub transfer_type: SdoDownloadInitiateType, + pub index: u16, + pub subindex: u8, +} + +pub enum SdoDownloadInitiateType { + NormalIndicated { bytes_to_download: u32 }, + NormalNotIndicated, + ExpeditedIndicated { data: Vec }, + ExpeditedNotIndicated { data: Vec }, +} + +impl EncodeCommunicationObject for SdoDownloadInitiateReq { + fn encode(self) -> CanBody { + let (n, e, s, data): (u8, u8, u8, [u8; 4]) = match self.transfer_type { + SdoDownloadInitiateType::NormalNotIndicated => (0, 0, 0, [0, 0, 0, 0]), + SdoDownloadInitiateType::NormalIndicated { bytes_to_download } => { + (0, 0, 1, bytes_to_download.to_le_bytes()) + } + SdoDownloadInitiateType::ExpeditedNotIndicated { data } => todo!(), + SdoDownloadInitiateType::ExpeditedIndicated { data } => { + let mut data_arr = [0; 4]; + data_arr[0..data.len()].copy_from_slice(&data); + (4 - data.len() as u8, 1, 1, data_arr) + } + }; + let index = self.index.to_le_bytes(); + let body = [ + 0b00100000 | (n << 2) | (e << 1) | s, + index[0], + index[1], + self.subindex, + data[0], + data[1], + data[2], + data[3], + ]; + + CanBody::data_from_array(body) + } +} +impl SdoDownloadInitiateReq { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let n = (data[0] >> 2) & 0b11; + let e = data[0] & 0b10 == 0b10; + let s = data[0] & 0b1 == 1; + let index = u16::from_le_bytes([data[1], data[2]]); + let subindex = data[3]; + let data = [data[4], data[5], data[6], data[7]]; + + let transfer_type = match (e, s) { + (false, false) => SdoDownloadInitiateType::NormalNotIndicated, + (false, true) => SdoDownloadInitiateType::NormalIndicated { + bytes_to_download: u32::from_le_bytes(data), + }, + (true, false) => todo!(), + (true, true) => SdoDownloadInitiateType::ExpeditedIndicated { + data: Vec::from_slice(&data[0..((4 - n) as usize)]) + .expect("slice not longer than 4"), + }, + }; + + Ok(Self { + transfer_type, + index, + subindex, + }) + } +} + +/// CiA 301 7.2.4.3.3 Protocol SDO download initiate +pub struct SdoDownloadInitiateRes { + pub index: u16, + pub subindex: u8, +} + +impl EncodeCommunicationObject for SdoDownloadInitiateRes { + fn encode(self) -> CanBody { + let index = self.index.to_le_bytes(); + CanBody::data_from_array([0b01100000, index[0], index[1], self.subindex, 0, 0, 0, 0]) + } +} +impl SdoDownloadInitiateRes { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let index = u16::from_le_bytes([data[1], data[2]]); + let subindex = data[3]; + Ok(Self { index, subindex }) + } +} + +/// CiA 301 7.2.4.3.4 Protocol SDO download segment +pub struct SdoDownloadSegmentReq { + pub toggle_bit: bool, + pub no_more_segments: bool, + pub data: Vec, +} +impl EncodeCommunicationObject for SdoDownloadSegmentReq { + fn encode(self) -> CanBody { + let n: u8 = 7 - self.data.len() as u8; + let c = match self.no_more_segments { + true => 1, + false => 0, + }; + let t = match self.toggle_bit { + true => 1, + false => 0, + }; + + let mut seg_data = [0; 8]; + seg_data[0] = (t << 4) | (n << 1) | c; + seg_data[1..self.data.len() + 1].copy_from_slice(&self.data); + + CanBody::data_from_array(seg_data) + } +} +impl SdoDownloadSegmentReq { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let t = (data[0] >> 4) & 0b1; + let n = (data[0] >> 1) & 0b111; + let c = data[0] & 0b1; + let seg_data = &data[1..]; + + Ok(Self { + toggle_bit: t == 1, + no_more_segments: c == 1, + data: Vec::from_slice(&seg_data[0..((7 - n) as usize)]) + .expect("seg_data to fit into vec"), + }) + } +} + +/// CiA 301 7.2.4.3.4 Protocol SDO download segment +pub struct SdoDownloadSegmentRes { + pub toggle_bit: bool, +} + +impl EncodeCommunicationObject for SdoDownloadSegmentRes { + fn encode(self) -> CanBody { + let t = match self.toggle_bit { + true => 1, + false => 0, + }; + CanBody::data_from_array([0b00100000 | (t << 4), 0, 0, 0, 0, 0, 0, 0]) + } +} +impl SdoDownloadSegmentRes { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let toggle_bit = (data[0] >> 4) & 1 == 1; + Ok(Self { toggle_bit }) + } +} + +/// CiA 301 7.2.4.3.6 Protocol SDO upload initiate +pub struct SdoUploadInitiateReq { + pub index: u16, + pub subindex: u8, +} + +impl EncodeCommunicationObject for SdoUploadInitiateReq { + fn encode(self) -> CanBody { + let index = self.index.to_le_bytes(); + CanBody::data_from_array([0b01000000, index[0], index[1], self.subindex, 0, 0, 0, 0]) + } +} +impl SdoUploadInitiateReq { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let index = u16::from_le_bytes([data[1], data[2]]); + let subindex = data[3]; + Ok(Self { index, subindex }) + } +} + +/// CiA 301 7.2.4.3.6 Protocol SDO upload initiate +pub struct SdoUploadInitiateRes { + pub index: u16, + pub subindex: u8, + pub transfer_type: SdoUploadInitiateType, +} + +pub enum SdoUploadInitiateType { + NormalIndicated { bytes_to_upload: u32 }, + NormalNotIndicated, + ExpeditedIndicated { data: Vec }, + ExpeditedNotIndicated { data: [u8; 4] }, +} + +impl EncodeCommunicationObject for SdoUploadInitiateRes { + fn encode(self) -> CanBody { + let (n, e, s, data): (u8, u8, u8, [u8; 4]) = match self.transfer_type { + SdoUploadInitiateType::NormalNotIndicated => (0, 0, 0, [0, 0, 0, 0]), + SdoUploadInitiateType::NormalIndicated { bytes_to_upload } => { + (0, 0, 1, bytes_to_upload.to_le_bytes()) + } + SdoUploadInitiateType::ExpeditedNotIndicated { data } => (0, 1, 0, data), + SdoUploadInitiateType::ExpeditedIndicated { data } => { + let mut data_arr = [0; 4]; + data_arr[0..data.len()].copy_from_slice(&data); + (4 - data.len() as u8, 1, 1, data_arr) + } + }; + let index = self.index.to_le_bytes(); + let payload = [ + 0b01000000 | (n << 2) | (e << 1) | s, + index[0], + index[1], + self.subindex, + data[0], + data[1], + data[2], + data[3], + ]; + + CanBody::data_from_array(payload) + } +} +impl SdoUploadInitiateRes { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let n = (data[0] >> 2) & 0b11; + let e = (data[0] >> 1) & 0b1 == 1; + let s = data[0] & 0b1 == 1; + let index = u16::from_le_bytes([data[1], data[2]]); + let subindex = data[3]; + + let data = [data[4], data[5], data[6], data[7]]; + + let transfer_type = match (e, s) { + (false, false) => SdoUploadInitiateType::NormalNotIndicated, + (false, true) => SdoUploadInitiateType::NormalIndicated { + bytes_to_upload: u32::from_le_bytes(data), + }, + (true, false) => SdoUploadInitiateType::ExpeditedNotIndicated { data }, + (true, true) => SdoUploadInitiateType::ExpeditedIndicated { + data: Vec::from_slice(&data[0..((4 - n) as usize)]) + .expect("slice not longer than 4"), + }, + }; + + Ok(Self { + index, + subindex, + transfer_type, + }) + } +} + +/// CiA 301 7.2.4.3.7 Protocol SDO upload segment +pub struct SdoUploadSegmentReq { + pub toggle_bit: bool, +} + +impl EncodeCommunicationObject for SdoUploadSegmentReq { + fn encode(self) -> CanBody { + let t = match self.toggle_bit { + true => 1, + false => 0, + }; + CanBody::data_from_array([0b01100000 | (t << 4), 0, 0, 0, 0, 0, 0, 0]) + } +} +impl SdoUploadSegmentReq { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let toggle_bit = (data[0] >> 4) & 1 == 1; + Ok(Self { toggle_bit }) + } +} + +/// CiA 301 7.2.4.3.7 Protocol SDO upload segment +pub struct SdoUploadSegmentRes { + pub toggle_bit: bool, + pub no_more_segments: bool, + pub data: Vec, +} + +impl EncodeCommunicationObject for SdoUploadSegmentRes { + fn encode(self) -> CanBody { + let n: u8 = 7 - self.data.len() as u8; + let c = match self.no_more_segments { + true => 1, + false => 0, + }; + let t = match self.toggle_bit { + true => 1, + false => 0, + }; + + let mut seg_data = [0; 7]; + seg_data[0..self.data.len()].copy_from_slice(&self.data); + + CanBody::data_from_array([ + (t << 4) | (n << 1) | c, + seg_data[0], + seg_data[1], + seg_data[2], + seg_data[3], + seg_data[4], + seg_data[5], + seg_data[6], + ]) + } +} +impl SdoUploadSegmentRes { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let t = (data[0] >> 4) & 0b1; + let n = (data[0] >> 1) & 0b111; + let c = data[0] & 0b1; + let seg_data = &data[1..]; + + Ok(Self { + toggle_bit: t == 1, + no_more_segments: c == 1, + data: Vec::from_slice(&seg_data[0..((7 - n) as usize)]) + .expect("seg_data to fit into vec"), + }) + } +} + +/// CiA 301 7.2.4.3.17 SDO abort +pub struct SdoAbort { + pub index: u16, + pub subindex: u8, + pub abort_code: SdoAbortCode, +} + +#[derive(Debug, FromPrimitive, IntoPrimitive, Clone, Copy)] +#[repr(u32)] +pub enum SdoAbortCode { + ToggleBitNotAlternated = 0x0503_0000, + SdoProtocolTimedOut = 0x0504_0000, + CommandSpecifierInvalid = 0x0504_0001, + InvalidBlockSize = 0x0504_0002, + InvalidSequenceNumber = 0x0504_0003, + CrcError = 0x0504_0004, + OutOfMemory = 0x0504_0005, + UnsupportedAccess = 0x0601_0000, + ReadToWriteOnly = 0x0601_0001, + WriteToReadOnly = 0x0601_0002, + ObjectDoesNotExist = 0x0602_0000, + ObjectCannotBeMapped = 0x0604_0041, + WouldExceedPdoLength = 0x0604_0042, + ParameterIncompatible = 0x0604_0043, + InternalIncompatible = 0x0604_0047, + AccessFailedDueToHardwareError = 0x0606_0000, + DataTypeMismatch = 0x0607_0010, + DataTypeMismatchTooLong = 0x0607_0012, + DataTypeMismatchTooShort = 0x607_0013, + SubIndexDoesNotExist = 0x0609_0011, + InvalidValue = 0x0609_0030, + InvalidValueTooHigh = 0x0609_0031, + InvalidValueTooShort = 0x0609_0032, + MaxLessThanMin = 0x0609_0036, + ResourceNotAvailableSdoConn = 0x060A_0023, + General = 0x0800_0000, + CannotTransfer = 0x0800_0020, + CannotTransferBecauseLocalControl = 0x0800_0021, + CannotTransferBecauseState = 0x0800_0022, + DynamicGenerationFailed = 0x0800_0023, + NoData = 0x0800_0024, + #[num_enum(catch_all)] + Other(u32), +} + +impl From for SdoAbortCode { + fn from(_value: core::convert::Infallible) -> Self { + unreachable!("Infallible never happens") + } +} + +impl EncodeCommunicationObject for SdoAbort { + fn encode(self) -> CanBody { + let index = self.index.to_le_bytes(); + let abort_code = u32::from(self.abort_code).to_le_bytes(); + + CanBody::data_from_array([ + 0b10000000, + index[0], + index[1], + self.subindex, + abort_code[0], + abort_code[1], + abort_code[2], + abort_code[3], + ]) + } +} +impl SdoAbort { + pub(crate) fn decode(data: &[u8; 8]) -> Result { + let cs = (data[0] >> 5) & 0b111; + + let index = u16::from_le_bytes([data[1], data[2]]); + let subindex = data[3]; + + let abort_code = u32::from_le_bytes([data[4], data[5], data[6], data[7]]); + let abort_code = SdoAbortCode::from_primitive(abort_code); + + Ok(Self { + index, + subindex, + abort_code, + }) + } +} diff --git a/canopen/src/communication/sdo/client.rs b/canopen/src/communication/sdo/client.rs new file mode 100644 index 0000000..3f85efd --- /dev/null +++ b/canopen/src/communication/sdo/client.rs @@ -0,0 +1,291 @@ +use core::future::Future; + +use core::unreachable; +use heapless::Vec; + +use crate::{ + communication::sdo::{ + SdoAbort, SdoAbortCode, SdoCommunicationObjectReq, SdoCommunicationObjectRes, + SdoDownloadInitiateReq, SdoDownloadInitiateType, SdoDownloadSegmentReq, + SdoUploadInitiateReq, SdoUploadInitiateType, SdoUploadSegmentReq, + }, + timer::Timer, +}; + +#[derive(Debug)] +pub enum Error { + /// An error occured in the CAN driver + Can(CAN::Error), + + Timer(TIM::Error), + + /// The server aborted the transfer + ServerAbort(SdoAbortCode), + + /// The transferred object is larger than the provided buffer + OutOfSpace, + + /// The server failed to respond in time + Timeout, + + /// The toggle bits didn't match + ToggleBitMismatch, +} + +pub trait CanDriver { + type Error; + fn send( + &mut self, + co: impl Into, + ) -> impl Future>; + fn recv(&self) -> impl Future>; +} + +pub async fn upload( + index: u16, + subindex: u8, + buf: &mut [u8], + can: &mut CAN, + timer: &mut TIM, +) -> Result> { + can.send(SdoUploadInitiateReq { index, subindex }) + .await + .map_err(Error::Can)?; + + loop { + let Some(frame) = timer + .timeout_after_ms(1000, can.recv()) + .await + .map_err(Error::Timer)? + else { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::SdoProtocolTimedOut, + }) + .await + .map_err(Error::Can)?; + return Err(Error::Timeout); + }; + match frame.map_err(Error::Can)? { + SdoCommunicationObjectRes::UploadInitiateRes(co) => { + match co.transfer_type { + SdoUploadInitiateType::ExpeditedIndicated { data } => { + if buf.len() < data.len() { + return Err(Error::OutOfSpace); + } + buf[..data.len()].copy_from_slice(&data); + return Ok(data.len()); + } + SdoUploadInitiateType::ExpeditedNotIndicated { data } => { + if buf.len() < data.len() { + return Err(Error::OutOfSpace); + } + buf[..data.len()].copy_from_slice(&data); + return Ok(data.len()); + } + SdoUploadInitiateType::NormalIndicated { bytes_to_upload } + if bytes_to_upload > buf.len() as u32 => + { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::OutOfMemory, + }) + .await + .map_err(Error::Can)?; + return Err(Error::OutOfSpace); + } + _ => (), + } + + let mut toggle_bit = false; + let mut buf_end = 0; + + loop { + can.send(SdoUploadSegmentReq { toggle_bit }) + .await + .map_err(Error::Can)?; + + loop { + let Some(frame) = timer + .timeout_after_ms(1000, can.recv()) + .await + .map_err(Error::Timer)? + else { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::SdoProtocolTimedOut, + }) + .await + .map_err(Error::Can)?; + return Err(Error::Timeout); + }; + let frame = frame.map_err(Error::Can)?; + + match frame { + SdoCommunicationObjectRes::UploadSegmentRes(co) => { + if toggle_bit != toggle_bit { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::ToggleBitNotAlternated, + }) + .await + .map_err(Error::Can)?; + return Err(Error::ToggleBitMismatch); + } + + let new_buf_end = buf_end + co.data.len(); + if new_buf_end > buf.len() { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::OutOfMemory, + }) + .await + .map_err(Error::Can)?; + return Err(Error::OutOfSpace); + } + + buf[buf_end..new_buf_end].copy_from_slice(&co.data); + buf_end = new_buf_end; + + if co.no_more_segments { + return Ok(buf_end); + } + + break; + } + SdoCommunicationObjectRes::Abort(co) => { + return Err(Error::ServerAbort(co.abort_code)); + } + _ => (), + } + } + + toggle_bit = !toggle_bit; + } + } + SdoCommunicationObjectRes::Abort(co) => return Err(Error::ServerAbort(co.abort_code)), + _ => (), + } + } +} + +pub async fn download( + index: u16, + subindex: u8, + buf: &[u8], + can: &mut CAN, + timer: &mut TIM, +) -> Result<(), Error> { + let (expedited, transfer_type) = if buf.len() <= 4 { + let Ok(data) = Vec::from_slice(buf) else { + unreachable!(); + }; + (true, SdoDownloadInitiateType::ExpeditedIndicated { data }) + } else { + ( + false, + SdoDownloadInitiateType::NormalIndicated { + bytes_to_download: buf.len() as u32, + }, + ) + }; + + can.send(SdoDownloadInitiateReq { + index, + subindex, + transfer_type, + }) + .await + .map_err(Error::Can)?; + + loop { + let Some(frame) = timer + .timeout_after_ms(1000, can.recv()) + .await + .map_err(Error::Timer)? + else { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::SdoProtocolTimedOut, + }) + .await + .map_err(Error::Can)?; + return Err(Error::Timeout); + }; + let frame = frame.map_err(Error::Can)?; + match frame { + SdoCommunicationObjectRes::DownloadInitiateRes(_) => { + break; + } + SdoCommunicationObjectRes::Abort(co) => { + return Err(Error::ServerAbort(co.abort_code)); + } + _ => (), + } + } + + if expedited { + return Ok(()); + } + + let mut toggle_bit = false; + let total_segments = buf.len().div_ceil(7); + for (segment_idx, segment) in buf.chunks(7).enumerate() { + let Ok(data) = Vec::from_slice(segment) else { + unreachable!() + }; + can.send(SdoDownloadSegmentReq { + toggle_bit, + no_more_segments: segment_idx == total_segments - 1, + data, + }) + .await + .map_err(Error::Can)?; + + loop { + let Some(frame) = timer + .timeout_after_ms(1000, can.recv()) + .await + .map_err(Error::Timer)? + else { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::SdoProtocolTimedOut, + }) + .await + .map_err(Error::Can)?; + return Err(Error::Timeout); + }; + let frame = frame.map_err(Error::Can)?; + match frame { + SdoCommunicationObjectRes::DownloadSegmentRes(co) => { + if co.toggle_bit != toggle_bit { + can.send(SdoAbort { + index, + subindex, + abort_code: SdoAbortCode::ToggleBitNotAlternated, + }) + .await + .map_err(Error::Can)?; + return Err(Error::ToggleBitMismatch); + } + break; + } + SdoCommunicationObjectRes::Abort(co) => { + return Err(Error::ServerAbort(co.abort_code)); + } + _ => (), + } + } + + toggle_bit = !toggle_bit; + } + Ok(()) +} diff --git a/canopen/src/communication/sdo/server.rs b/canopen/src/communication/sdo/server.rs new file mode 100644 index 0000000..047808d --- /dev/null +++ b/canopen/src/communication/sdo/server.rs @@ -0,0 +1,313 @@ +use core::unreachable; +use heapless::Vec; + +use crate::{ + communication::{ + DecodeCommunicationObject, DecodeError, + sdo::{ + SdoAbort, SdoAbortCode, SdoCommunicationObjectRes, SdoDownloadInitiateReq, + SdoDownloadInitiateRes, SdoDownloadInitiateType, SdoDownloadSegmentReq, + SdoDownloadSegmentRes, SdoUploadInitiateReq, SdoUploadInitiateRes, + SdoUploadInitiateType, SdoUploadSegmentReq, SdoUploadSegmentRes, + }, + }, + object::dictionary::ObjectDictionary, + timer::Timer, +}; + +pub enum Error { + /// An error occured in the CAN Driver + Can(CAN::Error), + + /// An error occured in the Object Dictionary + ObjectDictionary(OD::Error), + + Timer(T::Error), + + /// The SDO client aborted the transfer + ClientAbort(SdoAbortCode), + + /// The transferred object is larger than the provided buffer + OutOfSpace, + + /// The toggle bit from the client didn't meet expectations + ToggleBitMismatch, + + /// The client failed to answer in time + Timeout, +} + +pub enum InitReq { + Download(SdoDownloadInitiateReq), + Upload(SdoUploadInitiateReq), +} + +impl DecodeCommunicationObject for InitReq { + fn decode(body: crate::can::CanBody) -> Result + where + Self: Sized, + { + let body_data = body.data().ok_or(DecodeError::InvalidFrameType)?; + if body_data.len() != 8 { + return Err(DecodeError::InvalidFormat); + } + let mut data = [0u8; 8]; + data.copy_from_slice(body_data); + let ccs = (data[0] >> 5) & 0b111; + match ccs { + 1 => Ok(Self::Download(SdoDownloadInitiateReq::decode(&data)?)), + 2 => Ok(Self::Upload(SdoUploadInitiateReq::decode(&data)?)), + _ => Err(DecodeError::InvalidFormat), + } + } +} + +pub enum Req { + DownloadSegment(SdoDownloadSegmentReq), + UploadSegment(SdoUploadSegmentReq), + Abort(SdoAbort), +} + +impl DecodeCommunicationObject for Req { + fn decode(body: crate::can::CanBody) -> Result + where + Self: Sized, + { + let body_data = body.data().ok_or(DecodeError::InvalidFrameType)?; + if body_data.len() != 8 { + return Err(DecodeError::InvalidFormat); + } + let mut data = [0u8; 8]; + data.copy_from_slice(body_data); + let ccs = (data[0] >> 5) & 0b111; + match ccs { + 0 => Ok(Self::DownloadSegment(SdoDownloadSegmentReq::decode(&data)?)), + 3 => Ok(Self::UploadSegment(SdoUploadSegmentReq::decode(&data)?)), + 4 => Ok(Self::Abort(SdoAbort::decode(&data)?)), + _ => Err(DecodeError::InvalidFormat), + } + } +} + +pub trait CanDriver { + type Error; + fn send( + &mut self, + co: impl Into, + ) -> impl Future>; + fn recv(&mut self) -> impl Future>; +} + +pub async fn sdo_server( + init: &InitReq, + buf: &mut [u8], + can: &mut CAN, + od: &mut OD, + tim: &mut TIM, +) -> Result<(), Error> { + match init { + InitReq::Download(co) => { + let expedited_data = match &co.transfer_type { + SdoDownloadInitiateType::ExpeditedIndicated { data } => Some(data.as_slice()), + SdoDownloadInitiateType::ExpeditedNotIndicated { data } => Some(data.as_slice()), + _ => None, + }; + if let Some(data) = expedited_data { + if let Err(err) = od.write(co.index, co.subindex, data) { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: todo!(), + }) + .await + .map_err(Error::Can)?; + return Err(Error::ObjectDictionary(err)); + } + + can.send(SdoDownloadInitiateRes { + index: co.index, + subindex: co.subindex, + }) + .await + .map_err(Error::Can)?; + + Ok(()) + } else { + can.send(SdoDownloadInitiateRes { + index: co.index, + subindex: co.subindex, + }) + .await + .map_err(Error::Can)?; + + let mut expected_toggle_bit = false; + let mut buf_end = 0; + + loop { + let Some(frame) = tim + .timeout_after_ms(1000, can.recv()) + .await + .map_err(Error::Timer)? + else { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: SdoAbortCode::SdoProtocolTimedOut, + }) + .await + .map_err(Error::Can)?; + return Err(Error::Timeout); + }; + let frame = frame.map_err(Error::Can)?; + + match frame { + Req::DownloadSegment(seg_co) => { + if seg_co.toggle_bit != expected_toggle_bit { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: SdoAbortCode::ToggleBitNotAlternated, + }) + .await + .map_err(Error::Can)?; + return Err(Error::ToggleBitMismatch); + } + + let new_buf_end = buf_end + seg_co.data.len(); + if new_buf_end > buf.len() { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: SdoAbortCode::OutOfMemory, + }) + .await + .map_err(Error::Can)?; + return Err(Error::OutOfSpace); + } + + buf[buf_end..new_buf_end].copy_from_slice(seg_co.data.as_slice()); + + buf_end = new_buf_end; + + if seg_co.no_more_segments { + if let Err(err) = od.write(co.index, co.subindex, &buf[..buf_end]) { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: todo!(), + }) + .await + .map_err(Error::Can)?; + return Err(Error::ObjectDictionary(err)); + } + } + can.send(SdoDownloadSegmentRes { + toggle_bit: seg_co.toggle_bit, + }) + .await + .map_err(Error::Can)?; + + if seg_co.no_more_segments { + return Ok(()); + } + + expected_toggle_bit = !expected_toggle_bit; + } + Req::Abort(co) => return Err(Error::ClientAbort(co.abort_code)), + _ => (), + } + } + } + } + InitReq::Upload(co) => { + let buf_end = match od.read(co.index, co.subindex, buf) { + Ok(x) => x, + Err(err) => { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: todo!(), + }) + .await + .map_err(Error::Can)?; + return Err(Error::ObjectDictionary(err)); + } + }; + + if buf_end <= 4 { + let Ok(data) = Vec::from_slice(&buf[..buf_end]) else { + unreachable!() + }; + can.send(SdoUploadInitiateRes { + index: co.index, + subindex: co.subindex, + transfer_type: SdoUploadInitiateType::ExpeditedIndicated { data }, + }) + .await + .map_err(Error::Can)?; + return Ok(()); + } + + can.send(SdoUploadInitiateRes { + index: co.index, + subindex: co.subindex, + transfer_type: SdoUploadInitiateType::NormalIndicated { + bytes_to_upload: buf_end as _, + }, + }) + .await + .map_err(Error::Can)?; + + let mut expected_toggle_bit = false; + + let total_segments = buf_end.div_ceil(7); + for (segment_idx, segment) in buf[..buf_end].chunks(7).enumerate() { + let Some(frame) = tim + .timeout_after_ms(1000, can.recv()) + .await + .map_err(Error::Timer)? + else { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: SdoAbortCode::SdoProtocolTimedOut, + }) + .await + .map_err(Error::Can)?; + return Err(Error::Timeout); + }; + let frame = frame.map_err(Error::Can)?; + match frame { + Req::UploadSegment(seg_co) => { + if seg_co.toggle_bit != expected_toggle_bit { + can.send(SdoAbort { + index: co.index, + subindex: co.subindex, + abort_code: SdoAbortCode::ToggleBitNotAlternated, + }) + .await + .map_err(Error::Can)?; + return Err(Error::ToggleBitMismatch); + } + + let Ok(data) = Vec::from_slice(segment) else { + unreachable!(); + }; + + can.send(SdoUploadSegmentRes { + toggle_bit: seg_co.toggle_bit, + data, + no_more_segments: segment_idx == total_segments - 1, + }) + .await + .map_err(Error::Can)?; + } + Req::Abort(co) => return Err(Error::ClientAbort(co.abort_code)), + _ => (), + } + expected_toggle_bit = !expected_toggle_bit; + } + return Ok(()); + } + } +} diff --git a/canopen/src/lib.rs b/canopen/src/lib.rs new file mode 100644 index 0000000..2384bdc --- /dev/null +++ b/canopen/src/lib.rs @@ -0,0 +1,75 @@ +#![cfg_attr(not(test), no_std)] + +use embedded_can::{Id, StandardId}; + +extern crate alloc; + +pub mod can; +pub mod communication; +pub mod node; +pub mod object; +pub mod timer; + +#[cfg(test)] +mod test; + +#[derive(PartialEq, Eq, PartialOrd, Ord, Debug, Copy, Clone)] +pub struct NodeId(u8); +impl NodeId { + pub const unsafe fn new_unchecked(value: u8) -> Self { + Self(value) + } + pub const fn new(value: u8) -> Option { + if value == 0 || value > 0b1111111 { + None + } else { + Some(Self(value)) + } + } +} + +pub struct CobId; +impl CobId { + pub const fn nmt() -> Id { + Id::Standard(StandardId::new(0).unwrap()) + } + pub const fn sync() -> Id { + Id::Standard(StandardId::new(0x80).unwrap()) + } + pub const fn nmt_error_control(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x700 + node.0 as u16).unwrap()) + } + pub const fn emcy(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x80 + node.0 as u16).unwrap()) + } + pub const fn default_sdo_rx(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x600 + node.0 as u16).unwrap()) + } + pub const fn default_sdo_tx(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x580 + node.0 as u16).unwrap()) + } + pub const fn tpdo1(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x180 + node.0 as u16).unwrap()) + } + pub const fn rpdo1(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x200 + node.0 as u16).unwrap()) + } + pub const fn tpdo2(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x280 + node.0 as u16).unwrap()) + } + pub const fn rpdo2(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x300 + node.0 as u16).unwrap()) + } + pub const fn tpdo3(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x380 + node.0 as u16).unwrap()) + } + pub const fn rpdo3(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x400 + node.0 as u16).unwrap()) + } + pub const fn tpdo4(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x480 + node.0 as u16).unwrap()) + } + pub const fn rpdo4(node: NodeId) -> Id { + Id::Standard(StandardId::new(0x500 + node.0 as u16).unwrap()) + } +} diff --git a/canopen/src/node.rs b/canopen/src/node.rs new file mode 100644 index 0000000..ff4bef0 --- /dev/null +++ b/canopen/src/node.rs @@ -0,0 +1,828 @@ +use crate::can::CanBody; +use crate::communication::nmt::{ + NmtHeartbeat, NmtHeartbeatState, NmtNodeGuardingRes, NmtNodeGuardingResState, +}; +use crate::communication::pdo::{self, consumer::pdo_consume, producer::pdo_transmit}; +use crate::communication::sdo::{ + self, + server::{InitReq, Req, sdo_server}, +}; +use crate::communication::{DecodeCommunicationObject, EncodeCommunicationObject}; +use crate::object::cia301::{ + CobIdEmcy, CobIdSync, CommunicationCyclePeriod, ConsumerHeartbeatTime, EmergencyConsumer, + GuardTime, InhibitTimeEmcy, LifeTimeFactor, PdoMapping, ProducerHeartbeatTime, + SynchronousCounterOverflowValue, SynchronousWindowLength, +}; +use crate::object::{self, Object}; +use alloc::boxed::Box; +use alloc::collections::binary_heap::BinaryHeap; +use alloc::vec; +use alloc::vec::Vec; +use core::cmp::Ordering; +use core::convert::Infallible; +use core::marker::PhantomPinned; +use core::pin::Pin; +use core::ptr::NonNull; +use core::task::{Context, Poll}; +use core::unreachable; +use embedded_can::{Frame, Id}; + +use crate::{CobId, NodeId}; +use crate::{ + can::{CanRx, CanTx}, + object::dictionary::ObjectDictionary, + timer::Timer, +}; + +pub struct HeapFrame(pub F); +impl PartialEq for HeapFrame { + fn eq(&self, other: &Self) -> bool { + self.0.id().eq(&other.0.id()) + } +} +impl Eq for HeapFrame {} +impl PartialOrd for HeapFrame { + fn partial_cmp(&self, other: &Self) -> Option { + self.0.id().partial_cmp(&other.0.id()) + } +} +impl Ord for HeapFrame { + fn cmp(&self, other: &Self) -> Ordering { + self.0.id().cmp(&other.0.id()) + } +} + +pub enum Error {} + +pub enum CanIdMapping { + Sync, + Time, + Emcy(u8), + NmtHeartbeatConsumer(u8), + NmtNodeGuardClient(u8), + SdoServer(u8), + Rpdo(u16), + Tpdo(u16), +} + +#[derive(PartialEq, Eq, PartialOrd, Ord)] +pub enum RunningTask { + EmcyConsumer(u8), + HeartbeatProducer, + HeartbeatConsumer(u8), + NodeGuardingServer, + NodeGuardingClient(u8), + SdoServer(u8), + PdoProducer(u16), +} + +pub struct CanOpenNode { + can_tx: CT, + can_rx: CR, + od: DefaultDictionary, + timer: T, + node_id: NodeId, + running: BinarySearchMap>>>, + can_id_map: BinarySearchMap, + node_guarding_bit: bool, + state: NmtNodeGuardingResState, +} + +impl CanOpenNode +where + CT: CanTx + Clone + 'static, + CT::Frame: 'static, + CR: CanRx, + CR::Frame: 'static, + D: ObjectDictionary, + T: Timer + Clone + 'static, +{ + pub fn new( + node_id: NodeId, + can_tx: CT, + can_rx: CR, + od: DefaultDictionary, + timer: T, + ) -> Self { + let mut can_id_map = BinarySearchMap::default(); + + for (idx, sdo_cfg) in od.sdo_server.iter() { + if sdo_cfg.server_to_client.valid && sdo_cfg.client_to_server.valid { + can_id_map.insert(sdo_cfg.client_to_server.id, CanIdMapping::SdoServer(*idx)); + } + } + for (idx, tpdo_cfg) in od.tpdos.iter() { + if tpdo_cfg.valid { + can_id_map.insert(tpdo_cfg.id, CanIdMapping::Tpdo(*idx)); + } + } + + for (idx, rpdo_cfg) in od.rpdos.iter() { + if rpdo_cfg.valid { + can_id_map.insert(rpdo_cfg.id, CanIdMapping::Rpdo(*idx)); + } + } + + let mut running = BinarySearchMap::default(); + + running.insert( + RunningTask::HeartbeatProducer, + Running::new(|run| { + let mut can_tx = can_tx.clone(); + let mut timer = timer.clone(); + Box::new(async move { + loop { + let id = CobId::nmt_error_control(node_id); + can_tx + .send( + NmtHeartbeat { + state: match run.node_state() { + NmtNodeGuardingResState::PreOperational => { + NmtHeartbeatState::PreOperational + } + NmtNodeGuardingResState::Operational => { + NmtHeartbeatState::Operational + } + NmtNodeGuardingResState::Stopped => { + NmtHeartbeatState::Stopped + } + }, + } + .encode() + .to_frame(id), + ) + .await; + let time = run.object_dictionary().heartbeat_producer.period; + timer.delay_ms(time as _).await; + } + }) + }), + ); + + Self { + can_tx, + can_rx, + od, + timer, + node_id, + can_id_map, + node_guarding_bit: false, + state: NmtNodeGuardingResState::PreOperational, + running, + } + } + pub fn obj_dict(&mut self) -> &mut D { + &mut self.od.extension + } + pub fn configure_sdo_server(&mut self, index: u8) -> Result<(), Error> { + todo!() + } + pub fn configure_rpdo(&mut self, index: usize) -> Result<(), Error> { + todo!() + } + pub fn configure_tpdo(&mut self, index: usize) -> Result<(), Error> { + todo!() + } + + pub fn enter_preoperational(&mut self) { + self.state = NmtNodeGuardingResState::PreOperational; + } + pub fn enter_operational(&mut self) { + self.state = NmtNodeGuardingResState::Operational; + } + pub fn enter_stopped(&mut self) { + self.state = NmtNodeGuardingResState::Stopped; + } +} + +unsafe impl Send for CanOpenNode +where + CT: CanTx + Clone + 'static, + CR::Frame: 'static, + CR: CanRx, + D: ObjectDictionary + 'static, + T: Timer + Clone + 'static, +{ +} + +impl Future for CanOpenNode +where + CT: CanTx + Clone + 'static, + CR::Frame: 'static, + CR: CanRx, + D: ObjectDictionary + 'static, + T: Timer + Clone + 'static, +{ + type Output = Result<(), Error>; + + fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { + let this = unsafe { self.get_unchecked_mut() }; + + let mut one_ready = false; + + if let Poll::Ready(poll_frame_res) = + unsafe { Pin::new_unchecked(&mut this.can_rx) }.poll_recv(cx) + { + one_ready = true; + if let Ok(frame) = poll_frame_res { + if frame.is_data_frame() && frame.id() == CobId::nmt() { + todo!("signal application") + } + if frame.is_remote_frame() && frame.id() == CobId::nmt_error_control(this.node_id) { + let mut can_tx = this.can_tx.clone(); + let node_guarding_bit = this.node_guarding_bit; + this.node_guarding_bit = !this.node_guarding_bit; + let state = this.state; + let id = frame.id(); + this.running.insert( + RunningTask::NodeGuardingServer, + Running::new(|_| { + Box::new(async move { + can_tx + .send( + NmtNodeGuardingRes { + toggle_bit: node_guarding_bit, + state: state, + } + .encode() + .to_frame(id), + ) + .await; + Ok(()) + }) + }), + ); + } + match this.can_id_map.get(&frame.id()) { + Some(CanIdMapping::SdoServer(idx)) => { + let Some(sdo_config) = this.od.sdo_server.get(idx) else { + unreachable!("can_id_map is synced with object dictionary"); + }; + let body = CanBody::from_frame(&frame); + if let Some(running_sdo_server) = + this.running.get_mut(&RunningTask::SdoServer(*idx)) + { + running_sdo_server.as_mut().enqueue_frame(frame); + } else if let Ok(init_co) = InitReq::decode(body) { + let server_to_client: Id = sdo_config.server_to_client.id; + let can_tx = this.can_tx.clone(); + let mut timer = this.timer.clone(); + + this.running.insert(RunningTask::SdoServer(*idx), { + Running::new(|mut run| { + Box::new(async move { + let mut can = SdoServerCan { + can_tx, + run: run.clone(), + server_to_client, + }; + let mut buf = Box::new([0u8; 1024]); + sdo_server( + &init_co, + buf.as_mut(), + &mut can, + &mut run, + &mut timer, + ) + .await; + Ok(()) + }) + }) + }); + } + } + Some(CanIdMapping::Rpdo(idx)) if frame.is_data_frame() => { + let Some(rpdo) = this.od.rpdos.get(idx) else { + unreachable!("can_id_map is synced with object dictionary"); + }; + let mut data = [0u8; 8]; + data[..frame.data().len()].copy_from_slice(frame.data()); + + pdo_consume(data, rpdo.mappings.clone().values(), &mut this.od); + } + Some(CanIdMapping::Tpdo(idx)) if frame.is_remote_frame() => { + let Some(tpdo) = this.od.tpdos.get(idx) else { + unreachable!("can_id_map is synced with object dictionary"); + }; + //TODO implement correct PDO type handling + this.running.insert(RunningTask::PdoProducer(*idx), { + Running::new(|mut run| { + let mappings = tpdo.mappings.clone(); + let server_to_client: Id = tpdo.id; + let can_tx = this.can_tx.clone(); + Box::new(async move { + let mut can = PdoCan { + server_to_client, + can_tx, + }; + pdo_transmit(mappings.values(), &mut can, &mut run).await; + Ok(()) + }) + }) + }); + } + None => { + // The frame is not interesting for this node + } + _ => unimplemented!(), + } + } else { + todo!("error handling") + } + } + + this.running.retain(|_idx, running_sdo| { + match running_sdo.as_mut().poll(&mut this.od, this.state, cx) { + Poll::Pending => true, + Poll::Ready(Ok(_)) => { + one_ready = true; + false + } + _ => todo!("error handling"), + } + }); + + if one_ready { + cx.waker().clone().wake(); + } + + Poll::Pending + } +} + +pub struct DefaultDictionary { + cob_id_sync: CobIdSync, + communication_cycle_period: CommunicationCyclePeriod, + synchronous_window_length: SynchronousWindowLength, + synchronous_counter_overflow_value: SynchronousCounterOverflowValue, + + /// CiA 301 object `0x100C`, `0x100D` + guard_time: GuardTime, + life_time_factor: LifeTimeFactor, + + emcy_producer: CobIdEmcy, + inhibit_time_emcy: InhibitTimeEmcy, + + /// CiA 301 object `0x1016` + heartbeat_consumer: BinarySearchMap, + + /// CiA 301 object `0x1017` + heartbeat_producer: ProducerHeartbeatTime, + + /// CiA 301 object `0x1028` + emcy_consumer: BinarySearchMap, + + /// CiA 301 objects `0x1200` - `0x127F` + sdo_server: BinarySearchMap, + + /// CiA 301 objects `0x1400`-`0x15FF` and `0x1600`-`0x17FF` + rpdos: BinarySearchMap, + + /// CiA 301 objects `0x1800`-`0x19FF` and `0x1A00`-`0x1BFF` + tpdos: BinarySearchMap, + + pub extension: D, +} +impl DefaultDictionary { + pub fn new(node_id: NodeId, extension: D) -> Self { + let mut sdo_server = BinarySearchMap::default(); + sdo_server.insert( + 0, + SdoConfiguration { + client_to_server: SdoCobId { + valid: true, + dynamic: false, + id: CobId::default_sdo_rx(node_id), + }, + server_to_client: SdoCobId { + valid: true, + dynamic: false, + id: CobId::default_sdo_tx(node_id), + }, + remote_node_id: None, + }, + ); + Self { + cob_id_sync: CobIdSync { + generate: false, + id: CobId::sync(), + }, + communication_cycle_period: CommunicationCyclePeriod { period: 1000 }, + synchronous_window_length: SynchronousWindowLength, + synchronous_counter_overflow_value: SynchronousCounterOverflowValue, + guard_time: GuardTime { guard_time: 1000 }, + life_time_factor: LifeTimeFactor { + life_time_factor: None, + }, + emcy_producer: CobIdEmcy { + valid: true, + id: CobId::emcy(node_id), + }, + inhibit_time_emcy: InhibitTimeEmcy { inhibit_time: None }, + heartbeat_consumer: BinarySearchMap::default(), + heartbeat_producer: ProducerHeartbeatTime { period: 1000 }, + emcy_consumer: BinarySearchMap::default(), + sdo_server, + rpdos: BinarySearchMap::default(), + tpdos: BinarySearchMap::default(), + extension, + } + } +} + +impl ObjectDictionary for DefaultDictionary { + type Error = object::Error; + + fn read(&self, index: u16, subindex: u8, buf: &mut [u8]) -> Result { + match index { + 0x1005 => self.cob_id_sync.read(subindex, buf).map_err(Into::into), + 0x1006 => self + .communication_cycle_period + .read(subindex, buf) + .map_err(Into::into), + 0x1007 => todo!("synchronous_window_length"), + 0x100C => self.guard_time.read(subindex, buf).map_err(Into::into), + 0x100D => self + .life_time_factor + .read(subindex, buf) + .map_err(Into::into), + 0x1014 => self.emcy_producer.read(subindex, buf).map_err(Into::into), + 0x1015 => self + .inhibit_time_emcy + .read(subindex, buf) + .map_err(Into::into), + 0x1016 => todo!("heartbeat_consumer"), + 0x1017 => self + .heartbeat_producer + .read(subindex, buf) + .map_err(Into::into), + 0x1019 => todo!("synchronous_counter_overflow_value"), + 0x1028 => todo!("emcy_consumer"), + x if (0x1200..=0x127F).contains(&x) => unimplemented!("sdo_server"), + x if (0x1400..=0x15FF).contains(&x) || (0x1600..=0x17FF).contains(&x) => { + todo!("rpdo_server") + } + x if (0x1800..=0x19FF).contains(&x) || (0x1A00..=0x1BFF).contains(&x) => { + todo!("tpdo_server") + } + + _ => self + .extension + .read(index, subindex, buf) + .map_err(Into::into), + } + } + + fn write(&mut self, index: u16, subindex: u8, buf: &[u8]) -> Result<(), Self::Error> { + match index { + 0x1005 => self.cob_id_sync.write(subindex, buf).map_err(Into::into), + 0x1006 => self + .communication_cycle_period + .write(subindex, buf) + .map_err(Into::into), + 0x1007 => todo!("synchronous_window_length"), + 0x100C => self.guard_time.write(subindex, buf).map_err(Into::into), + 0x100D => self + .life_time_factor + .write(subindex, buf) + .map_err(Into::into), + 0x1014 => self.emcy_producer.write(subindex, buf).map_err(Into::into), + 0x1015 => self + .inhibit_time_emcy + .write(subindex, buf) + .map_err(Into::into), + 0x1016 => todo!("heartbeat_consumer"), + 0x1017 => self + .heartbeat_producer + .write(subindex, buf) + .map_err(Into::into), + 0x1019 => todo!("synchronous_counter_overflow_value"), + 0x1028 => todo!("emcy_consumer"), + x if (0x1200..=0x127F).contains(&x) => unimplemented!("sdo_server"), + x if (0x1400..=0x15FF).contains(&x) || (0x1600..=0x17FF).contains(&x) => { + todo!("rpdo_server") + } + x if (0x1800..=0x19FF).contains(&x) || (0x1A00..=0x1BFF).contains(&x) => { + todo!("tpdo_server") + } + _ => self + .extension + .write(index, subindex, buf) + .map_err(Into::into), + } + } +} + +#[derive(Clone)] +pub struct PdoConfiguration { + valid: bool, + id: Id, + mappings: BinarySearchMap, +} + +#[derive(Clone)] +pub struct SdoConfiguration { + pub client_to_server: SdoCobId, + pub server_to_client: SdoCobId, + pub remote_node_id: Option, +} + +#[derive(Clone, PartialEq, Eq)] +pub struct SdoCobId { + pub valid: bool, + pub dynamic: bool, + pub id: Id, +} + +struct Running { + frames: BinaryHeap>, + od: *mut DefaultDictionary, + state: NmtNodeGuardingResState, + task: Box>>, + _pp: PhantomPinned, +} + +impl Running { + pub fn new) -> Box>>>( + task_creation_fn: FN, + ) -> Pin> { + //SAEFTY is initialized below + let mut this = Box::::new_uninit(); + let ptr = this.as_mut_ptr(); + + unsafe { + (&raw mut (*ptr).frames).write(BinaryHeap::new()); + } + + unsafe { + (&raw mut (*ptr).od).write(core::ptr::null_mut()); + } + unsafe { + (&raw mut (*ptr).state).write(NmtNodeGuardingResState::PreOperational); + } + + let ctx = RunningRef(unsafe { NonNull::new_unchecked(ptr) }); + unsafe { + (&raw mut (*ptr).task).write(task_creation_fn(ctx)); + } + unsafe { + (&raw mut (*ptr)._pp).write(PhantomPinned); + } + + Box::into_pin(unsafe { this.assume_init() }) + } + fn enqueue_frame(self: Pin<&mut Self>, frame: F) { + //SAEFTY nothing is moved out + let this = unsafe { self.get_unchecked_mut() }; + this.frames.push(HeapFrame(frame)); + } + + fn poll( + self: Pin<&mut Self>, + od: &mut DefaultDictionary, + state: NmtNodeGuardingResState, + cx: &mut Context<'_>, + ) -> Poll> { + let this = unsafe { self.get_unchecked_mut() }; + this.od = od; + this.state = state; + let task = unsafe { Pin::new_unchecked(this.task.as_mut()) }; + let res = task.poll(cx); + this.od = core::ptr::null_mut(); + res + } +} + +struct RunningRef(NonNull>); + +impl RunningRef { + fn node_state(&self) -> NmtNodeGuardingResState { + let running = unsafe { self.0.as_ref() }; + running.state + } + fn object_dictionary(&self) -> &DefaultDictionary { + let running = unsafe { self.0.as_ref() }; + let od = unsafe { &*running.od }; + od + } +} + +impl Clone for RunningRef { + fn clone(&self) -> Self { + Self(self.0) + } +} +impl Copy for RunningRef {} + +impl CanRx for RunningRef { + type Error = Infallible; + type Frame = F; + + fn poll_recv( + self: Pin<&mut Self>, + _cx: &mut Context<'_>, + ) -> Poll> { + let this = self.get_mut(); + // SAEFTY pointer is valid because the corresponding `Running` is pinned and not dropped. + let running = unsafe { this.0.as_mut() }; + + match running.frames.pop() { + Some(x) => Poll::Ready(Ok(x.0)), + None => Poll::Pending, + } + } +} + +impl ObjectDictionary for RunningRef { + type Error = object::Error; + + fn read(&self, index: u16, subindex: u8, buf: &mut [u8]) -> Result { + let running = unsafe { self.0.as_ref() }; + let od: &DefaultDictionary = unsafe { running.od.as_ref().unwrap() }; + od.read(index, subindex, buf) + } + fn write(&mut self, index: u16, subindex: u8, buf: &[u8]) -> Result<(), Self::Error> { + let running = unsafe { self.0.as_mut() }; + let od: &mut DefaultDictionary = unsafe { running.od.as_mut().unwrap() }; + od.write(index, subindex, buf) + } +} + +struct SdoServerCan { + run: RunningRef, + can_tx: CT, + server_to_client: Id, +} + +impl sdo::server::CanDriver + for SdoServerCan +{ + type Error = CT::Error; + + fn send( + &mut self, + co: impl Into, + ) -> impl Future> { + let body = co.into().encode(); + let frame: CT::Frame = body.to_frame::(self.server_to_client); + self.can_tx.send(frame) + } + + fn recv(&mut self) -> impl Future> { + core::future::poll_fn(|cx| { + let run = Pin::new(&mut self.run); + match run.poll_recv(cx) { + Poll::Ready(Ok(frame)) => { + let body = CanBody::from_frame(&frame); + if let Ok(co) = Req::decode(body) { + Poll::Ready(Ok(co)) + } else { + Poll::Pending + } + } + Poll::Pending => Poll::Pending, + } + }) + } +} + +struct PdoCan { + can_tx: CT, + server_to_client: Id, +} + +impl pdo::producer::CanDriver for PdoCan { + type Error = CT::Error; + + fn send(&mut self, data: [u8; 8]) -> impl Future> { + let frame = CT::Frame::new(self.server_to_client, &data).unwrap(); + self.can_tx.send(frame) + } +} + +#[cfg(test)] +mod test { + + use embedded_can::Frame; + + use super::CanOpenNode; + use crate::{ + CobId, NodeId, + can::CanTx, + communication::sdo::client::upload, + node::DefaultDictionary, + object::dictionary::ObjectDictionary, + test::{AsyncIoTimer, CanFrame, SdoClientCan}, + }; + + #[derive(Default)] + struct TestOd; + + impl ObjectDictionary for TestOd { + type Error = core::convert::Infallible; + + fn read(&self, index: u16, subindex: u8, buf: &mut [u8]) -> Result { + match (index, subindex) { + (0x1000, 0x00) => { + buf[0] = 0xFF; + Ok(1) + } + _ => todo!(), + } + } + + fn write(&mut self, index: u16, subindex: u8, buf: &[u8]) -> Result<(), Self::Error> { + todo!() + } + } + + #[test] + fn nmt() { + let (mut can0, can1) = crate::test::CanChannel::new(); + let od: DefaultDictionary = DefaultDictionary::new(NodeId(1), TestOd::default()); + let timer = AsyncIoTimer; + + let node = CanOpenNode::new(NodeId(1), can1.clone(), can1, od, timer); + + let mut client_can = SdoClientCan { + channel: can0.clone(), + server_to_client: CobId::default_sdo_tx(NodeId(1)), + client_to_server: CobId::default_sdo_rx(NodeId(1)), + }; + + let task = smol::spawn(async move { + if node.await.is_err() { + panic!() + } + }); + + smol::block_on(async move { + let mut timer = AsyncIoTimer; + let mut buf = [0u8; 1024]; + let Ok(buf_size) = upload(0x1000, 0x00, &mut buf, &mut client_can, &mut timer).await + else { + panic!(); + }; + dbg!(&buf[..buf_size]); + + can0.send(CanFrame::new(CobId::nmt(), &[127, 1]).unwrap()) + .await; + + task.await; + }) + } +} + +#[derive(Clone)] +struct BinarySearchMap(Vec<(K, V)>); + +impl Default for BinarySearchMap { + fn default() -> Self { + Self(vec![]) + } +} + +impl BinarySearchMap { + fn get(&self, key: &K) -> Option<&V> { + match self.0.binary_search_by(|x| x.0.cmp(&key)) { + Ok(x) => Some(&self.0[x].1), + Err(_) => None, + } + } + fn get_mut(&mut self, key: &K) -> Option<&mut V> { + match self.0.binary_search_by(|x| x.0.cmp(&key)) { + Ok(x) => Some(&mut self.0[x].1), + Err(_) => None, + } + } + fn insert(&mut self, key: K, value: V) -> Option { + match self.0.binary_search_by(|x| x.0.cmp(&key)) { + Ok(x) => { + let mut item = (key, value); + core::mem::swap(&mut item, &mut self.0[x]); + Some(item.1) + } + Err(x) => { + self.0.insert(x, (key, value)); + None + } + } + } + fn remove(&mut self, key: &K) -> Option { + match self.0.binary_search_by(|x| x.0.cmp(&key)) { + Ok(x) => Some(self.0.remove(x).1), + Err(_) => None, + } + } + + fn retain bool>(&mut self, mut f: F) { + self.0.retain_mut(|x| (f)(&x.0, &mut x.1)) + } + + fn iter(&self) -> impl Iterator { + self.0.iter().map(|x| (&x.0, &x.1)) + } + fn values(&self) -> impl Iterator { + self.0.iter().map(|x| &x.1) + } +} diff --git a/canopen/src/object.rs b/canopen/src/object.rs new file mode 100644 index 0000000..b161c23 --- /dev/null +++ b/canopen/src/object.rs @@ -0,0 +1,77 @@ +use crate::communication::sdo::SdoAbortCode; + +pub mod cia301; +pub mod dictionary; + +#[derive(Debug, Clone)] +pub enum Error { + UnsupportedAccess, + ReadToWriteOnly, + WriteToReadOnly, + ObjectNotFound, + ObjectCannotBeMapped, + WouldExceedPdoLength, + ParameterIncompatibility, + InternalIncompatibility, + DataTypeMismatch, + SubobjectNotFound, + InvalidValue, + General, + OutOfMemory, +} + +impl From for Error { + fn from(_value: core::convert::Infallible) -> Self { + unreachable!() + } +} + +impl From for SdoAbortCode { + fn from(val: Error) -> Self { + match val { + Error::UnsupportedAccess => Self::UnsupportedAccess, + Error::ReadToWriteOnly => Self::ReadToWriteOnly, + Error::WriteToReadOnly => Self::WriteToReadOnly, + Error::ObjectNotFound => Self::ObjectDoesNotExist, + Error::ObjectCannotBeMapped => Self::ObjectCannotBeMapped, + Error::WouldExceedPdoLength => Self::WouldExceedPdoLength, + Error::ParameterIncompatibility => Self::ParameterIncompatible, + Error::DataTypeMismatch => Self::DataTypeMismatch, + Error::SubobjectNotFound => Self::SubIndexDoesNotExist, + Error::InvalidValue => Self::InvalidValue, + Error::General => Self::General, + Error::OutOfMemory => Self::OutOfMemory, + Error::InternalIncompatibility => Self::InternalIncompatible, + } + } +} + +pub trait Object { + const INDEX: u16; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error>; + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result; + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind; +} + +#[derive(PartialEq, Eq, Copy, Clone)] +pub enum Access { + ReadOnly, + ReadWrite, + WriteOnly, + None, +} + +#[derive(PartialEq, Eq, Copy, Clone)] +pub enum PdoMappingKind { + /// object may be mapped into a PDO + Optional, + /// object is part of the default mapping + Default, + /// object may be mapped into a TPDO and shall not be mapped into a RPDO + Tpdo, + /// object may be mapped into a RPDO and shall not be mapped into a TPDO + Rpdo, + /// object shall not be mapped into a PDO + No, +} diff --git a/canopen/src/object/cia301.rs b/canopen/src/object/cia301.rs new file mode 100644 index 0000000..911f403 --- /dev/null +++ b/canopen/src/object/cia301.rs @@ -0,0 +1,1097 @@ +use crate::{ + NodeId, + object::{Error, Object, PdoMappingKind}, +}; +use embedded_can::{ExtendedId, Id, StandardId}; + +/// The Device type object specified in CiA 301 0x1000 +/// This object is mandatory. +#[derive(Clone, PartialEq, Eq)] +pub struct DeviceType { + pub device_profile_number: u16, + pub additional: u16, +} +impl Object for DeviceType { + const INDEX: u16 = 0x1000; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 4]; + value.copy_from_slice(buf); + let value = u32::from_le_bytes(value); + + let device_profile_number = value as u16; + let additional = (value >> 16) as u16; + self.device_profile_number = device_profile_number; + self.additional = additional; + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let value: u32 = + ((self.additional as u32) << 16) | ((self.device_profile_number as u32) << 16); + buf[0..4].copy_from_slice(&value.to_le_bytes()); + Ok(4) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +/// The Error register object specified in CiA 301 0x1001 +/// This object is mandatory. +#[derive(Clone, PartialEq, Eq, Default)] +pub struct ErrorRegister { + pub manufacturer_specific: bool, + pub device_profile_specific: bool, + pub communication: bool, + pub temperature: bool, + pub voltage: bool, + pub current: bool, + pub generic: bool, +} +impl ErrorRegister { + pub const fn new() -> Self { + Self { + manufacturer_specific: false, + device_profile_specific: false, + communication: false, + temperature: false, + voltage: false, + current: false, + generic: false, + } + } +} +impl Object for ErrorRegister { + const INDEX: u16 = 0x1001; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => { + let b = |index: usize| (buf[0] >> index) & 0b1 == 1; + self.manufacturer_specific = b(7); + self.device_profile_specific = b(5); + self.communication = b(4); + self.temperature = b(3); + self.voltage = b(2); + self.current = b(1); + self.generic = b(0); + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => { + let b = |x: bool, index: usize| match x { + true => 1 << index, + false => 0, + }; + let value: u8 = b(self.manufacturer_specific, 7) + | b(self.device_profile_specific, 5) + | b(self.communication, 4) + | b(self.temperature, 3) + | b(self.voltage, 2) + | b(self.current, 1) + | b(self.generic, 0); + + buf[0] = value; + Ok(1) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + match subindex { + 0x00 => PdoMappingKind::Optional, + _ => PdoMappingKind::No, + } + } +} + +// TODO implement +/// The Manufacturer status register specified in CiA 301 0x1002 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct ManufacturerStatusRegister; + +// TODO implement +/// The Pre-defined error field specified in CiA 301 0x1003 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct PredefinedErrorField; + +/// The COB-ID SYNC object specified in CiA 301 0x1005 +/// This object is mandatory if PDO communication on a synchronous base is supported. +#[derive(Clone, PartialEq, Eq)] +pub struct CobIdSync { + pub generate: bool, + pub id: Id, +} +impl Object for CobIdSync { + const INDEX: u16 = 0x1005; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + let value = u32::from_le_bytes(value); + self.generate = (value >> 30) & 0b1 == 1; + self.id = match (value >> 29) & 0b1 { + 0 => StandardId::new((value & ((1 << 12) - 1)) as _) + .unwrap() + .into(), + 1 => ExtendedId::new((value & ((1 << 29) - 1)) as _) + .unwrap() + .into(), + _ => unreachable!(), + }; + + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let b = |x: bool, index: usize| match x { + true => 1 << index, + false => 0, + }; + let value: u32 = b(self.generate, 30) + | match &self.id { + Id::Standard(id) => u32::from(id.as_raw()), + Id::Extended(id) => (1 << 29) | u32::from(id.as_raw()), + }; + buf[0..4].copy_from_slice(&value.to_le_bytes()); + Ok(4) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +/// The Communication Cylce Period object specified in CiA 301 0x1006 +/// This object is mandatory for SYNC producers. +#[derive(Clone, PartialEq, Eq)] +pub struct CommunicationCyclePeriod { + pub period: u32, +} +impl Object for CommunicationCyclePeriod { + const INDEX: u16 = 0x1006; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + self.period = u32::from_le_bytes(value); + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0..4].copy_from_slice(&self.period.to_le_bytes()); + Ok(4) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +// TODO implement +/// The Synchronous window length specified in CiA 301 0x1007 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct SynchronousWindowLength; + +// TODO implement +// The Manufacturer device name object specified in CiA 301 0x1008 +// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct ManufacturerDeviceName; + +// TODO implement +// The Manufacturer hardware version object specified in CiA 301 0x1009 +// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct ManufacturerHardwareVersion; + +// TODO implement +// The Manufacturer hardware version object specified in CiA 301 0x100A +// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct ManufacturerSoftwareVersion; + +/// The Guard Time object specified in CiA 301 0x100C +/// This object is mandatory if node guarding is supported. +#[derive(Clone, PartialEq, Eq)] +pub struct GuardTime { + pub guard_time: u16, +} +impl Object for GuardTime { + const INDEX: u16 = 0x100C; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 2]; + value.copy_from_slice(&buf); + let value = u16::from_le_bytes(value); + if value == 0 { + return Err(Error::InvalidValue); + } + self.guard_time = value; + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0..2].copy_from_slice(&self.guard_time.to_le_bytes()); + Ok(2) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +/// The Life Time Factor object specified in CiA 301 0x100D +/// This object is mandatory if node guarding is supported. +#[derive(Clone, PartialEq, Eq)] +pub struct LifeTimeFactor { + pub life_time_factor: Option, +} +impl Object for LifeTimeFactor { + const INDEX: u16 = 0x100D; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => { + self.life_time_factor = match buf[0] { + 0 => None, + x => Some(x), + }; + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0] = self.life_time_factor.unwrap_or_default(); + Ok(1) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +// TODO implement +/// The Store parameters object specified in CiA 301 0x1010 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct StoreParameters; + +// TODO implement +/// The Restore default parameters object specified in CiA 301 0x1011 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct RestoreDefaultParameters; + +/// The COB-ID TIME object specified in CiA 301 0x1012 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct CobIdTime { + pub consume: bool, + pub produce: bool, + pub id: Id, +} +impl Object for CobIdTime { + const INDEX: u16 = 0x1012; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + let value = u32::from_le_bytes(value); + self.consume = (value >> 31) & 0b1 == 1; + self.produce = (value >> 30) & 0b1 == 1; + self.id = match (value >> 29) & 0b1 { + 0 => StandardId::new((value & ((1 << 12) - 1)) as _) + .unwrap() + .into(), + 1 => ExtendedId::new((value & ((1 << 29) - 1)) as _) + .unwrap() + .into(), + _ => unreachable!(), + }; + + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let b = |x: bool, index: usize| match x { + true => 1 << index, + false => 0, + }; + let value = b(self.consume, 31) + | b(self.produce, 30) + | match &self.id { + Id::Standard(id) => u32::from(id.as_raw()), + Id::Extended(id) => (1 << 29) | u32::from(id.as_raw()), + }; + buf[0..4].copy_from_slice(&value.to_le_bytes()); + Ok(4) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +// TODO implement +/// The High resolution time stamp object specified in CiA 301 0x1013 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct HighResTimestamp; + +/// The COB-ID EMCY object specified in CiA 301 0x1014 +/// This object is mandatory if emergency is supported. +#[derive(Clone, PartialEq, Eq)] +pub struct CobIdEmcy { + pub valid: bool, + pub id: Id, +} +impl Object for CobIdEmcy { + const INDEX: u16 = 0x1014; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + let value = u32::from_le_bytes(value); + self.valid = (value >> 31) & 0b1 == 1; + self.id = match (value >> 29) & 0b1 { + 0 => StandardId::new((value & ((1 << 12) - 1)) as _) + .unwrap() + .into(), + 1 => ExtendedId::new((value & ((1 << 29) - 1)) as _) + .unwrap() + .into(), + _ => unreachable!(), + }; + + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => { + let b = |x: bool, index: usize| match x { + true => 1 << index, + false => 0, + }; + let value = b(self.valid, 31) + | match &self.id { + Id::Standard(id) => u32::from(id.as_raw()), + Id::Extended(id) => (1 << 29) | u32::from(id.as_raw()), + }; + buf[0..4].copy_from_slice(&value.to_le_bytes()); + Ok(4) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +/// The Guard Time object specified in CiA 301 0x1015 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct InhibitTimeEmcy { + pub inhibit_time: Option, +} +impl Object for InhibitTimeEmcy { + const INDEX: u16 = 0x1015; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 2]; + value.copy_from_slice(&buf); + self.inhibit_time = match u16::from_le_bytes(value) { + 0 => None, + x => Some(x), + }; + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0..2].copy_from_slice(&self.inhibit_time.unwrap_or_default().to_le_bytes()); + Ok(2) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +// TODO implement +/// The Consumer heartbeat time object specified in CiA 301 0x1016 +/// This object is optional +#[derive(Clone, PartialEq, Eq)] +pub struct ConsumerHeartbeatTime; + +/// The Producer Heartbeat Time object specified in CiA 301 0x1017 +/// This object is mandatory if node guarding is NOT supported. +#[derive(Clone, PartialEq, Eq)] +pub struct ProducerHeartbeatTime { + pub period: u16, +} +impl Object for ProducerHeartbeatTime { + const INDEX: u16 = 0x1017; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + let mut value = [0; 2]; + value.copy_from_slice(&buf[..2]); + self.period = u16::from_le_bytes(value); + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 2 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0..2].copy_from_slice(&self.period.to_le_bytes()); + Ok(2) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +/// The Identity object specified in CiA 301 0x1018 +/// This object is mandatory. +#[derive(Clone, PartialEq, Eq)] +pub struct Identity { + pub vendor_id: u32, + pub product_code: u32, + pub revision_major: u16, + pub revision_minor: u16, + pub serial_number: u32, +} +impl Object for Identity { + const INDEX: u16 = 0x1018; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x01 | 0x02 | 0x03 | 0x04 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x00 => Err(Error::WriteToReadOnly), + 0x01 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + self.vendor_id = u32::from_le_bytes(value); + Ok(()) + } + 0x02 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + self.product_code = u32::from_le_bytes(value); + Ok(()) + } + 0x03 => { + let mut minor = [0; 2]; + let mut major = [0; 2]; + minor.copy_from_slice(&buf[0..2]); + major.copy_from_slice(&buf[2..4]); + + self.revision_major = u16::from_le_bytes(major); + self.revision_minor = u16::from_le_bytes(minor); + Ok(()) + } + 0x04 => { + let mut value = [0; 4]; + value.copy_from_slice(&buf); + self.serial_number = u32::from_le_bytes(value); + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0] = 4; + Ok(1) + } + 0x01 | 0x02 | 0x03 | 0x04 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x01 => { + buf[..4].copy_from_slice(&self.vendor_id.to_le_bytes()); + Ok(4) + } + 0x02 => { + buf[..4].copy_from_slice(&self.product_code.to_le_bytes()); + Ok(4) + } + 0x03 => { + buf[0..2].copy_from_slice(&self.revision_minor.to_le_bytes()); + buf[2..4].copy_from_slice(&self.revision_major.to_le_bytes()); + Ok(4) + } + 0x04 => { + buf[..4].copy_from_slice(&self.serial_number.to_le_bytes()); + Ok(4) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + + fn pdo_mapping(&self, _subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +// TODO implement +/// The Synchronous counter overflow value object specified in CiA 301 0x1019 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct SynchronousCounterOverflowValue; + +// TODO implement +/// The Verify configuration object specified in CiA 301 0x1020 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct VerifyConfiguration; + +// TODO implement +/// The Store eds object specified in CiA 301 0x1021 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct StoreEds; + +// TODO implement +/// The Store format object specified in CiA 301 0x1022 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct StoreFormat; + +// TODO implement +/// The OS command object specified in CiA 301 0x1023 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct OsCommand; + +// TODO implement +/// The OS command mode object specified in CiA 301 0x1024 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct OsCommandMode; + +// TODO implement +/// The OS debugger interface object specified in CiA 301 0x1025 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct OsDebuggerInterface; + +// TODO implement +/// The OS prompt interface object specified in CiA 301 0x1026 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct OsPrompt; + +//TODO implement +/// The Module list object specified in CiA 301 0x1027 +/// This object is mandatory if modular devices are supported. +#[derive(Clone, PartialEq, Eq)] +pub struct ModuleList; + +//TODO implement +/// The Emergency consumer object specified in CiA 301 0x1028 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct EmergencyConsumer; + +//TODO implement +/// The Error behavior object specified in CiA 301 0x1029 +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct ErrorBehavior; + +#[derive(Clone, PartialEq, Eq)] +pub struct SdoParameter { + pub client_to_server: SdoCobId, + pub server_to_client: SdoCobId, + pub remote_node_id: Option, +} + +#[derive(Clone, PartialEq, Eq)] +pub struct SdoCobId { + pub valid: bool, + pub dynamic: bool, + pub id: Id, +} + +/// The SDO (client/server) parameter object specified in CiA 301 0x1200..=0x127F and 0x1280..=0x12FF +/// This object is optional for the default sdo client/server (0x1200) and mandatory for each additionally +/// supported SDO client/server. +#[derive(Clone, PartialEq, Eq)] +pub struct SdoServerParameter(pub SdoParameter); + +impl Object for SdoServerParameter { + const INDEX: u16 = match I { + x if x > 0x7F => panic!("I out of bounds"), + x => 0x1200 + x, + }; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => Err(Error::WriteToReadOnly), + 0x01 | 0x02 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x01 => todo!(), + 0x02 => todo!(), + 0x03 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x03 => { + self.0.remote_node_id = Some(unsafe { NodeId::new_unchecked(buf[0] & 0b01111111) }); + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0] = 3; + Ok(1) + } + 0x01 | 0x02 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x01 => todo!(), + 0x02 => todo!(), + 0x03 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x03 => { + if let Some(remote_node_id) = self.0.remote_node_id { + buf[0] = remote_node_id.0; + Ok(1) + } else { + todo!() + } + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + match subindex { + 0x00 => PdoMappingKind::No, + 0x01 | 0x02 => PdoMappingKind::Optional, + 0x03 => PdoMappingKind::No, + 0xFF => PdoMappingKind::No, + _ => PdoMappingKind::No, + } + } +} + +/// The SDO (client/server) parameter object specified in CiA 301 0x1200..=0x127F and 0x1280..=0x12FF +/// This object is optional for the default sdo client/server (0x1200) and mandatory for each additionally +/// supported SDO client/server. +#[derive(Clone, PartialEq, Eq)] +pub struct SdoClientParameter(pub SdoParameter); + +impl Object for SdoClientParameter { + const INDEX: u16 = match I { + x if x > 0x7F => panic!("I out of bounds"), + x => 0x1280 + x, + }; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => Err(Error::WriteToReadOnly), + 0x01 | 0x02 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x01 => todo!(), + 0x02 => todo!(), + 0x03 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x03 => { + self.0.remote_node_id = Some(unsafe { NodeId::new_unchecked(buf[0] & 0b01111111) }); + Ok(()) + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x00 => { + buf[0] = 3; + Ok(1) + } + 0x01 | 0x02 if buf.len() < 4 => Err(Error::DataTypeMismatch), + 0x01 => todo!(), + 0x02 => todo!(), + 0x03 if buf.len() < 1 => Err(Error::DataTypeMismatch), + 0x03 => { + if let Some(remote_node_id) = self.0.remote_node_id { + buf[0] = remote_node_id.0; + Ok(1) + } else { + todo!() + } + } + 0xFF => todo!(), + _ => Err(Error::SubobjectNotFound), + } + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + match subindex { + 0x00 => PdoMappingKind::No, + 0x01 | 0x02 => PdoMappingKind::Optional, + 0x03 => PdoMappingKind::No, + 0xFF => PdoMappingKind::No, + _ => PdoMappingKind::No, + } + } +} + +#[derive(Clone, PartialEq, Eq)] +pub struct PdoCommunicationParameter { + pub cob_id: PdoCobId, + pub transmission_type: u8, + pub inhibit_time: u16, + // reserved + pub event_timer: u16, + pub sync_start_value: u8, +} +#[derive(Clone, PartialEq, Eq)] +pub struct PdoCobId { + pub valid: bool, + pub id: Id, +} + +/// The RPDO communication parameter object specified in CiA 301 0x1400..=0x15FF +/// This object is mandatory for each supported RPDO. +#[derive(Clone, PartialEq, Eq)] +pub struct RpdoCommunicationParameter(pub PdoCommunicationParameter); +impl Object for RpdoCommunicationParameter { + const INDEX: u16 = match I { + x if x > 0x1FF => panic!("I out of bounds"), + x => 0x1400 + x, + }; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + todo!() + } + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + todo!() + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + todo!() + } +} + +/// The TPDO communication parameter object specified in CiA 301 0x1800..=0x19FF +/// This object is mandatory for each supported TPDO. +#[derive(Clone, PartialEq, Eq)] +pub struct TpdoCommunicationParameter(pub PdoCommunicationParameter); +impl Object for TpdoCommunicationParameter { + const INDEX: u16 = match I { + x if x > 0x1FF => panic!("I out of bounds"), + x => 0x1800 + x, + }; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + todo!() + } + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + todo!() + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + todo!() + } +} + +/// The PDO mapping parameter object specified in CiA 301 0x1600..=17FF and 0x1A00..=0x1BFF +/// This object is mandatory for each supported PDO +#[derive(Clone, PartialEq, Eq)] +pub struct PdoMappingParameter { + pub mappings: [PdoMapping; 64], + pub used_mappings: usize, +} +#[derive(Clone, Copy, PartialEq, Eq, Default)] +pub struct PdoMapping { + pub index: u16, + pub sub_index: u8, + pub length: u8, +} + +/// The PDO mapping parameter object specified in CiA 301 0x1600..=17FF +/// This object is mandatory for each supported PDO +#[derive(Clone, PartialEq, Eq)] +pub struct RpdoMappingParameter(pub PdoMappingParameter); +impl RpdoMappingParameter { + pub const fn new() -> Self { + Self(PdoMappingParameter { + mappings: [PdoMapping { + index: 0, + sub_index: 0, + length: 0, + }; 64], + used_mappings: 0, + }) + } +} +impl Object for RpdoMappingParameter { + const INDEX: u16 = match I { + x if x > 0x1FF => panic!("I out of reach"), + x => 0x1600 + x, + }; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 => { + self.0.used_mappings = buf[0] as usize; + Ok(()) + } + 0xFF => todo!(), + x if x <= 0x40 => { + if buf.len() != 4 { + return Err(Error::DataTypeMismatch); + } + let mapping = &mut self.0.mappings[x as usize - 1]; + mapping.length = buf[0]; + mapping.sub_index = buf[1]; + mapping.index = u16::from_le_bytes([buf[2], buf[3]]); + Ok(()) + } + _ => Err(Error::SubobjectNotFound), + } + } + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 => { + buf[0] = self.0.used_mappings as u8; + Ok(1) + } + 0xFF => todo!(), + x if x <= 0x40 => { + let mapping = &self.0.mappings[x as usize - 1]; + buf[0] = mapping.length; + buf[1] = mapping.sub_index; + buf[2..=4].copy_from_slice(&mapping.index.to_le_bytes()); + Ok(4) + } + _ => Err(Error::SubobjectNotFound), + } + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} + +/// The TPDO mapping parameter object specified in CiA 301 0x1A00..=0x1BFF +/// This object is mandatory for each supported PDO +#[derive(Clone, PartialEq, Eq)] +pub struct TpdoMappingParameter(pub PdoMappingParameter); +impl TpdoMappingParameter { + pub const fn new() -> Self { + Self(PdoMappingParameter { + mappings: [PdoMapping { + index: 0, + sub_index: 0, + length: 0, + }; 64], + used_mappings: 0, + }) + } + pub const fn with(mut self, index: u16, sub_index: u8, length: u8) -> Self { + self.0.mappings[self.0.used_mappings] = PdoMapping { + index, + sub_index, + length, + }; + self.0.used_mappings += 1; + self + } + pub fn mappings(&self) -> &[PdoMapping] { + &self.0.mappings[..self.0.used_mappings] + } +} +impl Object for TpdoMappingParameter { + const INDEX: u16 = match I { + x if x > 0x1FF => panic!("I out of reach"), + x => 0x1A00 + x, + }; + + fn write(&mut self, subindex: u8, buf: &[u8]) -> Result<(), Error> { + match subindex { + 0x00 => { + self.0.used_mappings = buf[0] as usize; + Ok(()) + } + 0xFF => todo!(), + x if x <= 0x40 => { + if buf.len() != 4 { + return Err(Error::DataTypeMismatch); + } + let mapping = &mut self.0.mappings[x as usize - 1]; + mapping.length = buf[0]; + mapping.sub_index = buf[1]; + mapping.index = u16::from_le_bytes([buf[2], buf[3]]); + Ok(()) + } + _ => Err(Error::SubobjectNotFound), + } + } + fn read(&self, subindex: u8, buf: &mut [u8]) -> Result { + match subindex { + 0x00 => { + buf[0] = self.0.used_mappings as u8; + Ok(1) + } + 0xFF => todo!(), + x if x <= 0x40 => { + let mapping = &self.0.mappings[x as usize - 1]; + buf[0] = mapping.length; + buf[1] = mapping.sub_index; + buf[2..=4].copy_from_slice(&mapping.index.to_le_bytes()); + Ok(4) + } + _ => Err(Error::SubobjectNotFound), + } + } + fn pdo_mapping(&self, subindex: u8) -> PdoMappingKind { + PdoMappingKind::No + } +} +// TODO implement +/// The SimpleObject scanner list object specified in CiA 301 0x1FA0..=0x1FCF +/// This object is optional. +#[derive(Clone, PartialEq, Eq)] +pub struct ObjectScanerList; + +// TODO implement +/// The SimpleObject dispatching list object specified in CiA 301 0x1FD0..=0x1FFF +/// This object is optional +#[derive(Clone, PartialEq, Eq)] +pub struct ObjectDispatchingList; diff --git a/canopen/src/object/dictionary.rs b/canopen/src/object/dictionary.rs new file mode 100644 index 0000000..1d02cd0 --- /dev/null +++ b/canopen/src/object/dictionary.rs @@ -0,0 +1,8 @@ +use crate::communication::sdo::SdoAbortCode; + +pub trait ObjectDictionary { + type Error: Into + Into; + fn read(&self, index: u16, subindex: u8, buf: &mut [u8]) -> Result; + + fn write(&mut self, index: u16, subindex: u8, buf: &[u8]) -> Result<(), Self::Error>; +} diff --git a/canopen/src/test.rs b/canopen/src/test.rs new file mode 100644 index 0000000..5307b90 --- /dev/null +++ b/canopen/src/test.rs @@ -0,0 +1,145 @@ +use async_channel::{Receiver, Sender}; +use core::pin::Pin; +use core::task::{Context, Poll}; +use embedded_can::{Frame, Id}; +use futures::Stream; + +use crate::can::{CanBody, CanRx, CanTx}; +use crate::communication::sdo::{self, SdoCommunicationObjectRes}; +use crate::communication::{DecodeCommunicationObject, EncodeCommunicationObject}; +use crate::timer::Timer; + +pub struct CanFrame { + id: Id, + body: CanBody, +} + +impl Frame for CanFrame { + fn new(id: impl Into, data: &[u8]) -> Option { + Some(Self { + id: id.into(), + body: CanBody::from_slice(data)?, + }) + } + + fn new_remote(id: impl Into, dlc: usize) -> Option { + Some(Self { + id: id.into(), + body: CanBody::new_remote(dlc)?, + }) + } + + fn is_extended(&self) -> bool { + matches!(self.id, Id::Extended(_)) + } + + fn is_remote_frame(&self) -> bool { + self.body.is_remote_frame() + } + + fn id(&self) -> embedded_can::Id { + self.id + } + + fn dlc(&self) -> usize { + match &self.body { + CanBody::Data(data) => data.data.len(), + CanBody::RemoteTransmissionRequest(rtr) => rtr.size as _, + } + } + + fn data(&self) -> &[u8] { + match &self.body { + CanBody::Data(data) => &data.data, + CanBody::RemoteTransmissionRequest(_) => &[], + } + } +} + +#[derive(Clone)] +pub struct CanChannel { + tx: Sender, + rx: Receiver, +} + +impl CanChannel { + pub fn new() -> (Self, Self) { + let (tx1, rx1) = async_channel::unbounded(); + let (tx2, rx2) = async_channel::unbounded(); + + (Self { tx: tx1, rx: rx2 }, Self { tx: tx2, rx: rx1 }) + } +} + +impl CanTx for CanChannel { + type Error = core::convert::Infallible; + + type Frame = CanFrame; + + async fn send(&mut self, frame: Self::Frame) -> Result<(), Self::Error> { + self.tx.send(frame).await.unwrap(); + Ok(()) + } +} + +impl CanRx for CanChannel { + type Error = core::convert::Infallible; + + type Frame = CanFrame; + + fn poll_recv( + self: Pin<&mut Self>, + cx: &mut Context<'_>, + ) -> Poll> { + let rx = unsafe { self.map_unchecked_mut(|x| &mut x.rx) }; + + match rx.poll_next(cx) { + Poll::Ready(x) => Poll::Ready(Ok(x.unwrap())), + Poll::Pending => Poll::Pending, + } + } +} + +#[derive(Clone)] +pub struct AsyncIoTimer; + +impl Timer for AsyncIoTimer { + type Error = core::convert::Infallible; + + async fn delay_ms(&mut self, millis: usize) -> Result<(), Self::Error> { + async_io::Timer::after(core::time::Duration::from_millis(millis as _)).await; + Ok(()) + } +} + +pub(crate) struct SdoClientCan { + pub(crate) channel: CanChannel, + pub(crate) client_to_server: Id, + pub(crate) server_to_client: Id, +} + +impl sdo::client::CanDriver for SdoClientCan { + type Error = core::convert::Infallible; + + fn send( + &mut self, + co: impl Into, + ) -> impl Future> { + self.channel + .send(co.into().encode().to_frame(self.client_to_server)) + } + + async fn recv( + &self, + ) -> Result { + loop { + let frame = self.channel.rx.recv().await.unwrap(); + let body = CanBody::from_frame(&frame); + if frame.id() == self.server_to_client { + if let Ok(co) = SdoCommunicationObjectRes::decode(body) { + return Ok(co); + } + } + } + } +} diff --git a/canopen/src/timer.rs b/canopen/src/timer.rs new file mode 100644 index 0000000..41ce570 --- /dev/null +++ b/canopen/src/timer.rs @@ -0,0 +1,28 @@ +use core::future::{Future, poll_fn}; +use core::task::Poll; + +pub trait Timer { + type Error; + + fn delay_ms(&mut self, millis: usize) -> impl Future>; + + fn timeout_after_ms( + &mut self, + millis: usize, + future: F, + ) -> impl Future, Self::Error>> { + async move { + let mut delay = core::pin::pin!(self.delay_ms(millis)); + let mut future = core::pin::pin!(future); + poll_fn(|cx| match delay.as_mut().poll(cx) { + Poll::Ready(Ok(())) => Poll::Ready(Ok(None)), + Poll::Ready(Err(err)) => Poll::Ready(Err(err)), + Poll::Pending => match future.as_mut().poll(cx) { + Poll::Ready(x) => Poll::Ready(Ok(Some(x))), + Poll::Pending => Poll::Pending, + }, + }) + .await + } + } +}