pfzetto/canome
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

double linked list can driver

Browse source
This commit is contained in:
Paul Zinselmeyer 2024-08-08 23:52:46 +02:00
parent d5126a6c34
commit 491de0564f
Signed by: pfzetto
GPG key ID: 142847B253911DB0
14 changed files with 1789 additions and 456 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

26
canome/Cargo.toml
View file

@ -11,16 +11,24 @@ keywords = [ "CAN", "embedded", "smarthome" ]
[dependencies]
defmt = "0.3"
heapless = { version = "0.8.0", default-features = false }
critical-section = "1.0"
heapless = { version = "0.8", default-features = false }
critical-section = "1.1"
embedded-hal = { version = "0.2.7", optional = true }
stm32f1xx-hal = { version = "0.10.0", optional = true }
rtic-monotonics = { version = "1.0.0", optional = true }
bxcan = { version = "0.7.0", optional = true }
nb = "1.1.0"
embedded-hal = { version = "0.2", optional = true }
stm32f1xx-hal = { version = "0.10", optional = true }
rtic-monotonics = { version = "1.0", optional = true }
bxcan = { version = "0.7", optional = true }
socketcan = { version = "3.3", default-features = false, optional = true }
nb = "1.1"
once_cell = { version = "1.19.0", default-features = false, features = ["critical-section"] }
embedded-can = "0.4"
[features]
default = [ "can", "stm32" ]
can = [ "dep:bxcan" ]
default = [ "bxcan" ]
bxcan = [ "dep:bxcan" ]
socketcan = [ "dep:socketcan" ]
stm32 = [ "dep:embedded-hal", "dep:stm32f1xx-hal", "dep:rtic-monotonics", "dep:bxcan" ]
[dev-dependencies]
critical-section = { version = "1.1", features = ["std"]}

395
canome/src/bus.rs
View file

@ -1,291 +1,184 @@
use core::{
cell::RefCell,
cell::{Cell, RefCell},
fmt::Debug,
future::{poll_fn, Future},
future::poll_fn,
marker::PhantomData,
task::{Poll, Waker},
};
use critical_section::Mutex;
use heapless::{LinearMap, Vec};
use defmt::warn;
use embedded_can::Id;
use heapless::Deque;
use once_cell::sync::OnceCell;
use crate::{BusBackend, Channel, Decode, Encode};
use crate::{CanDataFormat, Channel, Decode, Encode};
pub struct Bus<B: BusBackend, const CHANNELS: usize, const WAKER_CAP: usize> {
rx_queues: critical_section::Mutex<RefCell<LinearMap<B::ID, BusInner<B, WAKER_CAP>, CHANNELS>>>,
backend: B,
pub(crate) struct ReceiverInner {
pub(crate) id: Id,
pub(crate) queue: Mutex<RefCell<Deque<CanDataFormat, 8>>>,
pub(crate) waker: Mutex<Cell<Option<Waker>>>,
}
pub(crate) struct TransmitterInner {
pub(crate) id: Id,
pub(crate) remote_received: Mutex<Cell<bool>>,
pub(crate) waker: Mutex<Cell<Option<Waker>>>,
}
pub struct UnboundReceiver<'a> {
pub(crate) bus: &'a dyn Bus,
pub(crate) inner: &'a OnceCell<ReceiverInner>,
}
pub struct BoundReceiver<'a, C: Channel> {
pub(crate) bus: &'a dyn Bus,
pub(crate) _channel: PhantomData<C>,
pub(crate) inner: &'a ReceiverInner,
}
pub struct UnboundTransmitter<'a> {
pub(crate) bus: &'a dyn Bus,
pub(crate) inner: &'a OnceCell<TransmitterInner>,
}
pub struct BoundTransmitter<'a, C: Channel> {
pub(crate) bus: &'a dyn Bus,
pub(crate) _channel: PhantomData<C>,
pub(crate) inner: &'a TransmitterInner,
}
#[derive(Debug)]
pub enum BusError<B: BusBackend> {
Backend(B::Error),
pub enum BusError {
ChannelCapExceeded,
}
#[derive(Debug)]
struct BusInner<B: BusBackend, const WAKER_CAP: usize> {
state: B::Data,
wakers: Vec<Waker, WAKER_CAP>,
sender: bool,
pub trait Bus: Send + Sync {
fn transmit(&self, id: Id, value: CanDataFormat) -> Result<(), BusError>;
fn request(&self, id: Id) -> Result<(), BusError>;
fn distribute_data(&self, id: Id, value: CanDataFormat);
fn distribute_remote(&self, id: Id);
}
#[derive(Clone)]
pub struct BusChannelTransceiver<'a, B: BusBackend, C: Channel<B>> {
bus: &'a dyn BusBehaviour<B>,
_channel: PhantomData<C>,
impl<'a> UnboundTransmitter<'a> {
pub fn bind<C: Channel>(self) -> BoundTransmitter<'a, C> {
self.inner.set(TransmitterInner {
id: C::ID,
remote_received: Mutex::new(Cell::new(false)),
waker: Mutex::new(Cell::new(None)),
});
self.bus.request(C::ID);
BoundTransmitter {
bus: self.bus,
_channel: PhantomData,
inner: &self.inner.get().unwrap(),
}
}
}
#[derive(Clone)]
pub struct BusChannelReceiver<'a, B: BusBackend, C: Channel<B>> {
bus: &'a dyn BusBehaviour<B>,
_channel: PhantomData<C>,
}
pub trait BusBehaviour<B: BusBackend> {
fn get(&self, id: B::ID) -> Option<B::Data>;
fn register_waker(&self, id: B::ID, waker: &Waker);
fn publish(&self, id: B::ID, value: B::Data) -> Result<(), BusError<B>>;
fn publish_local(&self, id: &B::ID, value: B::Data);
fn request(&self, id: B::ID) -> Result<(), BusError<B>>;
}
impl<B: BusBackend, T: BusBehaviour<B>> BusBehaviour<B> for &T {
fn get(&self, id: <B as BusBackend>::ID) -> Option<<B as BusBackend>::Data> {
(*self).get(id)
impl<'a> UnboundReceiver<'a> {
pub fn bind<C: Channel>(self) -> BoundReceiver<'a, C> {
self.inner.set(ReceiverInner {
id: C::ID,
queue: Mutex::new(RefCell::new(Deque::new())),
waker: Mutex::new(Cell::new(None)),
});
self.bus.request(C::ID);
BoundReceiver {
bus: self.bus,
_channel: PhantomData,
inner: &self.inner.get().unwrap(),
}
fn register_waker(&self, id: <B as BusBackend>::ID, waker: &Waker) {
(*self).register_waker(id, waker)
}
fn publish(
&self,
id: <B as BusBackend>::ID,
value: <B as BusBackend>::Data,
) -> Result<(), BusError<B>> {
(*self).publish(id, value)
}
fn publish_local(&self, id: &<B as BusBackend>::ID, value: <B as BusBackend>::Data) {
(*self).publish_local(id, value)
}
fn request(&self, id: <B as BusBackend>::ID) -> Result<(), BusError<B>> {
(*self).request(id)
}
}
pub trait BusRx<C: Channel<B>, B: BusBackend> {
fn get(&self) -> C::State;
fn request(&self) -> Result<(), BusError<B>>;
fn next(&self) -> impl Future<Output = C::State>;
}
pub trait BusTx<C: Channel<B>, B: BusBackend> {
fn publish(&self, value: C::State) -> Result<(), BusError<B>>;
}
impl<B: BusBackend, const CHANNELS: usize, const WAKER_CAP: usize> BusBehaviour<B>
for Bus<B, CHANNELS, WAKER_CAP>
impl<'a, C: Channel> BoundReceiver<'a, C>
where
B::ID: Eq,
C::Message: Decode<CanDataFormat> + Encode<CanDataFormat>,
{
fn get(&self, id: B::ID) -> Option<B::Data> {
pub fn request(&self) -> Result<(), BusError> {
self.bus.request(C::ID)
}
pub fn try_recv(&self) -> Option<C::Message> {
critical_section::with(|cs| {
self.rx_queues
match self
.inner
.queue
.borrow(cs)
.borrow_mut()
.get_mut(&id)
.map(|x| x.state.clone())
})
}
fn register_waker(&self, id: B::ID, waker: &Waker) {
critical_section::with(|cs| {
if let Some(inner) = self.rx_queues.borrow(cs).borrow_mut().get_mut(&id) {
if inner.wakers.iter().any(|ew| waker.will_wake(ew)) {
return;
}
if inner.wakers.is_full() {
inner.wake();
}
if inner.wakers.push(waker.clone()).is_err() {
panic!("tried to push a waker to a zero length waker list");
.pop_back()
.map(C::Message::decode)
{
Some(Ok(frame)) => Some(frame),
Some(Err(_)) => {
warn!("Failed to decode Frame");
None
}
None => None,
}
})
}
pub async fn recv(&self) -> C::Message {
poll_fn(|cx| {
let frame = critical_section::with(|cs| {
let existing_waker = self.inner.waker.borrow(cs);
fn publish(&self, id: B::ID, value: B::Data) -> Result<(), BusError<B>> {
self.publish_local(&id, value.clone());
self.backend
.publish(id, Some(value))
.map_err(BusError::Backend)
let waker = match existing_waker.take() {
Some(waker) if cx.waker().will_wake(&waker) => waker,
Some(waker) => {
waker.wake();
cx.waker().clone()
}
None => cx.waker().clone(),
};
fn publish_local(&self, id: &B::ID, value: B::Data) {
critical_section::with(|cs| {
if let Some(inner) = self.rx_queues.borrow(cs).borrow_mut().get_mut(id) {
inner.state = value.clone();
inner.wake();
existing_waker.set(Some(waker));
self.inner.queue.borrow(cs).borrow_mut().pop_back()
})
.map(C::Message::decode);
match frame {
Some(Ok(frame)) => Poll::Ready(frame),
Some(Err(_)) => {
warn!("Failed to decode Frame");
Poll::Pending
}
None => Poll::Pending,
}
})
.await
}
}
impl<'a, C: Channel> BoundTransmitter<'a, C>
where
C::Message: Decode<CanDataFormat> + Encode<CanDataFormat>,
{
pub fn transmit(&self, value: C::Message) -> Result<(), BusError> {
let encoded = value.encode();
self.bus.transmit(C::ID, encoded)
}
pub async fn wait_remote(&self) {
poll_fn(|cx| {
let remote_received = critical_section::with(|cs| {
let existing_waker = self.inner.waker.borrow(cs);
let waker = match existing_waker.take() {
Some(waker) if cx.waker().will_wake(&waker) => waker,
Some(waker) => {
waker.wake();
cx.waker().clone()
}
None => cx.waker().clone(),
};
existing_waker.set(Some(waker));
self.inner.remote_received.borrow(cs).replace(false)
});
}
fn request(&self, id: B::ID) -> Result<(), BusError<B>> {
self.backend.publish(id, None).map_err(BusError::Backend)
}
}
impl<B: BusBackend, const WAKER_CAP: usize> BusInner<B, WAKER_CAP> {
fn wake(&mut self) {
let mut wakers = Vec::new();
core::mem::swap(&mut wakers, &mut self.wakers);
wakers.into_iter().for_each(|w| w.wake());
}
}
impl<B: BusBackend, const SUBS: usize, const WAKER_CAP: usize> Bus<B, SUBS, WAKER_CAP>
where
B::ID: Eq,
{
pub const fn new(backend: B) -> Self {
Self {
rx_queues: Mutex::new(RefCell::new(LinearMap::new())),
backend,
}
}
pub fn transceiver<C: Channel<B>>(&self) -> Result<BusChannelTransceiver<'_, B, C>, BusError<B>>
where
C::State: Default + Encode<B::Data>,
{
critical_section::with(|cs| {
let mut rx_queues = self.rx_queues.borrow(cs).borrow_mut();
if let Some(existing) = rx_queues.get_mut(&C::ID) {
existing.sender = true;
Ok(BusChannelTransceiver {
bus: self,
_channel: PhantomData,
})
} else if rx_queues.len() < SUBS {
self.request(C::ID).ok();
let _ = rx_queues.insert(
C::ID,
BusInner {
state: C::State::default().encode(),
wakers: Vec::new(),
sender: true,
},
);
Ok(BusChannelTransceiver {
bus: self,
_channel: PhantomData,
})
} else {
Err(BusError::ChannelCapExceeded)
}
})
}
pub fn receiver<C: Channel<B>>(&self) -> Result<BusChannelReceiver<'_, B, C>, BusError<B>>
where
C::State: Default + Encode<B::Data>,
{
critical_section::with(|cs| {
let mut rx_queues = self.rx_queues.borrow(cs).borrow_mut();
if rx_queues.contains_key(&C::ID) {
Ok(BusChannelReceiver {
bus: self,
_channel: PhantomData,
})
} else if rx_queues.len() < SUBS {
self.request(C::ID).ok();
let _ = rx_queues.insert(
C::ID,
BusInner {
state: C::State::default().encode(),
wakers: Vec::new(),
sender: false,
},
);
Ok(BusChannelReceiver {
bus: self,
_channel: PhantomData,
})
} else {
Err(BusError::ChannelCapExceeded)
}
})
}
pub fn backend(&self) -> &B {
&self.backend
}
}
impl<'a, B: BusBackend, C: Channel<B>> BusTx<C, B> for BusChannelTransceiver<'a, B, C>
where
C::State: Encode<B::Data>,
{
fn publish(&self, value: C::State) -> Result<(), BusError<B>> {
self.bus.publish(C::ID, value.encode())
}
}
impl<'a, B: BusBackend, C> BusRx<C, B> for BusChannelTransceiver<'a, B, C>
where
C::State: Decode<B::Data> + PartialEq + Default,
C: Channel<B>,
{
fn get(&self) -> C::State {
self.bus
.get(C::ID)
.and_then(|x| C::State::decode(x).ok())
.expect("valid group")
}
fn request(&self) -> Result<(), BusError<B>> {
self.bus.request(C::ID)
}
async fn next(&self) -> C::State {
let old_state: C::State = self.get();
poll_fn(|cx| {
self.bus.register_waker(C::ID, cx.waker());
let state: C::State = self.get();
match state == old_state {
false => Poll::Ready(state),
true => Poll::Pending,
}
})
.await
}
}
impl<'a, B: BusBackend, C> BusRx<C, B> for BusChannelReceiver<'a, B, C>
where
C::State: Decode<B::Data> + PartialEq + Default,
C: Channel<B>,
{
fn get(&self) -> C::State {
self.bus
.get(C::ID)
.and_then(|x| C::State::decode(x).ok())
.expect("valid group")
}
fn request(&self) -> Result<(), BusError<B>> {
self.bus.request(C::ID)
}
async fn next(&self) -> C::State {
let old_state: C::State = self.get();
poll_fn(|cx| {
self.bus.register_waker(C::ID, cx.waker());
let state: C::State = self.get();
match state == old_state {
false => Poll::Ready(state),
true => Poll::Pending,
match remote_received {
true => Poll::Ready(()),
false => Poll::Pending,
}
})
.await

198
canome/src/bxcan.rs Normal file
View file

@ -0,0 +1,198 @@
use core::{array, fmt::Debug, mem::MaybeUninit};
use crate::{
bus::{Bus, BusError, ReceiverInner, TransmitterInner, UnboundReceiver, UnboundTransmitter},
CanDataFormat,
};
use bxcan::{Instance, Interrupt, Rx0, Tx};
use embedded_can::{ExtendedId, Id, StandardId};
use heapless::mpmc::MpMcQueue;
use once_cell::sync::OnceCell;
pub struct BxCanBus<I: Instance, const TQ: usize, const RQ: usize> {
transmitters: [OnceCell<TransmitterInner>; TQ],
receivers: [OnceCell<ReceiverInner>; RQ],
tx_queue: MpMcQueue<bxcan::Frame, 32>,
tx: Tx<I>,
rx: Rx0<I>,
}
#[derive(Debug)]
pub enum BxCanBusError {
/// An object couldn't be added to the TX queue of [`BxCanBus`] because it is full.
TxQueueFull,
}
impl<I: Instance, const TQ: usize, const RQ: usize> BxCanBus<I, TQ, RQ> {
pub fn initialize(
bus: &mut MaybeUninit<Self>,
) -> (
&Self,
[UnboundTransmitter<'_>; TQ],
[UnboundReceiver<'_>; RQ],
) {
let bus = bus.write(Self {
receivers: array::from_fn(|_| OnceCell::new()),
transmitters: array::from_fn(|_| OnceCell::new()),
tx_queue: MpMcQueue::new(),
tx: todo!(),
rx: todo!(),
});
(
bus,
array::from_fn(|i| UnboundTransmitter {
bus,
inner: &bus.transmitters[i],
}),
array::from_fn(|i| UnboundReceiver {
bus,
inner: &bus.receivers[i],
}),
)
}
}
impl<I: Instance, const TQ: usize, const RQ: usize> Bus for BxCanBus<I, TQ, RQ> {
fn transmit(&self, id: Id, value: CanDataFormat) -> Result<(), BusError> {
self.tx_queue.enqueue(encode_frame(id, Some(value)));
self.distribute_data(id, value);
//TODO: trigger CAN_TX interrupt
self.handle_tx_interrupt();
Ok(())
}
fn request(&self, id: Id) -> Result<(), BusError> {
self.tx_queue.enqueue(encode_frame(id, None));
self.distribute_remote(id);
//TODO: trigger CAN_TX interrupt
self.handle_tx_interrupt();
Ok(())
}
fn distribute_data(&self, id: Id, value: CanDataFormat) {
for inner in self
.receivers
.iter()
.filter_map(|x| x.get())
.filter(|x| x.id == id)
{
critical_section::with(|cs| {
inner.queue.borrow(cs).borrow_mut().push_front(value);
if let Some(waker) = inner.waker.borrow(cs).take() {
waker.wake()
}
});
}
}
fn distribute_remote(&self, id: Id) {
for inner in self
.transmitters
.iter()
.filter_map(|x| x.get())
.filter(|x| x.id == id)
{
critical_section::with(|cs| {
inner.remote_received.borrow(cs).set(true);
if let Some(waker) = inner.waker.borrow(cs).take() {
waker.wake();
}
});
}
}
}
impl<I: Instance, const TQ: usize, const RQ: usize> BxCanBus<I, TQ, RQ> {
pub fn handle_tx_interrupt(&self) -> Result<(), BxCanBusError> {
let tx = &self.tx_queue;
while let Some(frame) = tx.dequeue() {
match self.tx.transmit(&frame) {
Ok(status) => match status.dequeued_frame() {
None => {
defmt::debug!("Transmitting CAN frame to {}.", frame.id());
}
Some(old_frame) => {
defmt::debug!(
"Transmitting CAN frame to {} by displacing other to {}, requeing it.",
frame.id(),
old_frame.id()
);
tx.enqueue(old_frame.clone())
.map_err(|_| BxCanBusError::TxQueueFull)?;
}
},
Err(nb::Error::WouldBlock) => {
defmt::debug!(
"Couldn't trasmit CAN frame to {}, requeing frame.",
frame.id()
);
tx.enqueue(frame).map_err(|_| BxCanBusError::TxQueueFull)?;
}
Err(_) => unreachable!(),
}
}
Ok(())
}
pub fn handle_rx_interrupt<I: Instance>(
&self,
can_rx: &mut Rx0<I>,
) -> Result<(), BxCanBusError> {
loop {
match can_rx.receive() {
Ok(frame) => {
let (channel_id, data) = decode_frame(&frame);
if let Some(data) = data {
self.distribute_data(channel_id, data);
} else {
self.distribute_remote(channel_id);
}
/*
if let Some(data) = data {
bus.publish_local(&channel_id, data);
} else if let Some(state) = bus.get(channel_id) {
defmt::debug!("responding to remote frame");
self.tx_queue
.enqueue(encode_frame(channel_id, Some(state)))
.map_err(|_| BxCanBusError::TxQueueFull)?;
//TODO on publish
}
*/
}
Err(nb::Error::WouldBlock) => break,
Err(nb::Error::Other(_)) => break,
}
}
Ok(())
}
}
fn decode_frame(frame: &bxcan::Frame) -> (Id, Option<CanDataFormat>) {
let id = match frame.id() {
bxcan::Id::Standard(x) => Id::Standard(unsafe { StandardId::new_unchecked(x.as_raw()) }),
bxcan::Id::Extended(x) => Id::Extended(unsafe { ExtendedId::new_unchecked(x.as_raw()) }),
};
if let Some(data) = frame.data() {
(id, Some(CanDataFormat::new(data)))
} else {
(id, None)
}
}
fn encode_frame(id: Id, data: Option<CanDataFormat>) -> bxcan::Frame {
let id = match id {
Id::Standard(x) => {
bxcan::Id::Standard(unsafe { bxcan::StandardId::new_unchecked(x.as_raw()) })
}
Id::Extended(x) => {
bxcan::Id::Extended(unsafe { bxcan::ExtendedId::new_unchecked(x.as_raw()) })
}
};
if let Some(data) = data {
bxcan::Frame::new_data(id, bxcan::Data::new(data.data()).expect("valid can data"))
} else {
bxcan::Frame::new_remote(id, 8)
}
}

548
canome/src/can.rs
View file

@ -1,102 +1,421 @@
use core::{fmt::Debug, marker::ConstParamTy};
use crate::{bus::BusBehaviour, BusBackend, BusDataFormat};
use bxcan::{ExtendedId, Id, Instance, Rx0, StandardId, Tx};
use core::borrow::{Borrow, BorrowMut};
use core::fmt::Debug;
use core::mem::MaybeUninit;
use core::ops::{Deref, DerefMut};
use defmt::Format;
use heapless::mpmc::MpMcQueue;
use heapless::Vec;
pub struct BxCanBus<const TX_CAP: usize> {
tx_queue: MpMcQueue<(<Self as BusBackend>::ID, Option<<Self as BusBackend>::Data>), TX_CAP>,
on_publish: &'static (dyn Fn(&Self) + Send + Sync),
use crate::spmc::{Channel, ChannelClient, Receive, Receiver, ReceiverRef};
#[derive(Clone, Debug)]
pub struct CanData(pub Vec<u8, 8>);
#[derive(Clone, Copy, PartialEq, Eq, Format, Debug)]
pub enum CanId {
Standard(CanStandardId),
Extended(CanExtendedId),
}
#[derive(Clone, Copy, PartialEq, Eq, Debug, Format)]
pub struct CanStandardId(u16);
#[derive(Clone, Copy, PartialEq, Eq, Debug, Format)]
pub struct CanExtendedId(u32);
#[derive(Clone, Debug, Format)]
pub enum CanFrame {
Data(CanDataFrame),
Remote(CanRemoteFrame),
}
impl<const TX_CAP: usize> Debug for BxCanBus<TX_CAP> {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.debug_struct("BxCanBus").finish()
#[derive(Clone, Debug, Format)]
pub struct CanDataFrame(pub CanId, pub CanData);
#[derive(Clone, Debug, Format)]
pub struct CanRemoteFrame(pub CanId);
impl Format for CanData {
fn format(&self, fmt: defmt::Formatter) {
defmt::write!(fmt, "CanData({})", self.0.as_slice())
}
}
#[derive(PartialEq, Eq, ConstParamTy, Debug, Clone, Copy)]
pub enum BxCanId {
Standard(u16),
Extended(u32),
impl CanStandardId {
pub const fn new(value: u16) -> Self {
assert!(value < (1 << 11));
Self(value)
}
}
#[derive(Debug)]
pub enum BxCanBusError {
/// An object couldn't be added to the TX queue of [`BxCanBus`] because it is full.
TxQueueFull,
}
impl<const TX_CAP: usize> BusBackend for BxCanBus<TX_CAP> {
type ID = BxCanId;
type Data = CanDataFormat;
type Error = BxCanBusError;
fn publish(&self, id: Self::ID, data: Option<Self::Data>) -> Result<(), Self::Error> {
self.tx_queue
.enqueue((id, data))
.map_err(|_| Self::Error::TxQueueFull)?;
(self.on_publish)(self);
Ok(())
impl CanExtendedId {
pub const fn new(value: u32) -> Self {
assert!(value < (1 << 29));
Self(value)
}
}
impl<const TX_CAP: usize> BxCanBus<TX_CAP> {
pub const fn new_with_hook(on_publish: &'static (dyn Fn(&Self) + Send + Sync)) -> Self {
impl CanDataFrame {
pub fn id(&self) -> CanId {
self.0
}
}
impl CanRemoteFrame {
pub fn id(&self) -> CanId {
self.0
}
}
impl CanFrame {
pub fn id(&self) -> CanId {
match *self {
Self::Data(CanDataFrame(id, _)) => id,
Self::Remote(CanRemoteFrame(id)) => id,
}
}
}
impl CanData {
pub fn new(data: &[u8]) -> Self {
Self(Vec::from_slice(data).unwrap())
}
pub fn data(&self) -> &[u8] {
self.0.as_slice()
}
}
pub trait CanDriver {
type Error;
fn handle<'driver: 'client, 'client, I: CanClientInner<'client, 'driver>>(
&'driver self,
unbound: &'client mut UnboundCanHandle<'driver, I>,
builder: I::InitData,
) -> CanHandleRef<'client, 'driver, I>;
fn handle_tx_interrupt(&self) -> Result<(), Self::Error>;
fn handle_rx_interrupt(&self) -> Result<(), Self::Error>;
}
trait CanDriverInternal {
fn send(&self, value: CanFrame);
fn channel(&self) -> &Channel<CanFrame>;
}
pub struct CanHandleRef<'client, 'driver, I: CanClientInner<'client, 'driver>>(
ReceiverRef<'driver, 'client, CanFrame, CanClient<'driver, I>>,
);
pub struct UnboundCanHandle<'driver, I>(MaybeUninit<CanClient<'driver, I>>);
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> Default
for UnboundCanHandle<'driver, I>
{
fn default() -> Self {
Self(MaybeUninit::uninit())
}
}
pub struct CanHandle<'client, 'driver, I>(&'client CanClient<'driver, I>);
struct CanClient<'driver, I> {
driver: &'driver dyn CanDriverInternal,
inner: I,
client: Option<ChannelClient<CanFrame>>,
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> Receive<CanFrame>
for CanClient<'driver, I>
{
fn receive(&self, value: &CanFrame) {
self.inner.handle_frame(value.clone())
}
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> Receiver<'driver, CanFrame>
for CanClient<'driver, I>
{
fn channel(&self) -> &crate::spmc::Channel<CanFrame> {
self.driver.channel()
}
fn channel_client(&self) -> &ChannelClient<CanFrame> {
self.client.as_ref().unwrap()
}
}
pub trait CanClientInner<'client, 'driver> {
type InitData;
fn new(init_data: Self::InitData, handle: CanHandle<'client, 'driver, Self>) -> Self
where
Self: Sized;
fn handle_frame(&self, frame: CanFrame);
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> Borrow<I>
for CanHandleRef<'client, 'driver, I>
{
fn borrow(&self) -> &I {
&self.0.as_ref().inner
}
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> BorrowMut<I>
for CanHandleRef<'client, 'driver, I>
{
fn borrow_mut(&mut self) -> &mut I {
let x: &mut CanClient<'driver, I> = self.0.borrow_mut();
&mut x.inner
}
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> Deref
for CanHandleRef<'client, 'driver, I>
{
type Target = I;
fn deref(&self) -> &Self::Target {
&self.0.as_ref().inner
}
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> DerefMut
for CanHandleRef<'client, 'driver, I>
{
fn deref_mut(&mut self) -> &mut Self::Target {
let x: &mut CanClient<'driver, I> = self.0.borrow_mut();
&mut x.inner
}
}
impl<'client, 'driver, I: CanClientInner<'client, 'driver>> CanHandle<'client, 'driver, I> {
pub fn send(&self, frame: CanFrame) {
self.0.driver.send(frame)
}
}
pub mod basic_client {
use super::{CanClientInner, CanFrame, CanHandle};
use core::cell::RefCell;
use core::future::{poll_fn, Future};
use core::task::{Poll, Waker};
use critical_section::Mutex;
use heapless::Deque;
pub enum Error {
Overflowed,
}
pub struct BasicCanClient<'client, 'driver: 'client, const N: usize> {
handle: CanHandle<'client, 'driver, Self>,
inner: Mutex<RefCell<BasicCanClientInner<N>>>,
}
struct BasicCanClientInner<const N: usize> {
queue: Deque<CanFrame, N>,
overflowed: bool,
waker: Option<Waker>,
}
impl<'client, 'driver: 'client, const N: usize> CanClientInner<'client, 'driver>
for BasicCanClient<'client, 'driver, N>
{
type InitData = ();
fn new(_init_data: Self::InitData, handle: CanHandle<'client, 'driver, Self>) -> Self
where
Self: Sized,
{
Self {
tx_queue: MpMcQueue::new(),
on_publish,
handle,
inner: Mutex::new(RefCell::new(BasicCanClientInner {
queue: Deque::new(),
overflowed: false,
waker: None,
})),
}
}
pub fn handle_tx_interrupt<I: Instance>(
&self,
can_tx: &mut Tx<I>,
) -> Result<(), BxCanBusError> {
let rx = &self.tx_queue;
fn handle_frame(&self, frame: CanFrame) {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
if inner.queue.is_full() {
inner.queue.pop_back();
inner.overflowed = true;
}
let Ok(_) = inner.queue.push_front(frame) else {
unreachable!("queue always has at least one free slot")
};
if let Some(waker) = inner.waker.take() {
waker.wake();
}
})
}
}
impl<'client, 'driver: 'client, const N: usize> BasicCanClient<'client, 'driver, N> {
pub fn recv(&mut self) -> impl Future<Output = Result<CanFrame, Error>> + '_ {
poll_fn(|cx| {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
while let Some((channel_id, data)) = rx.dequeue() {
let frame = encode_frame(channel_id, data);
match can_tx.transmit(&frame) {
Ok(status) => match status.dequeued_frame().map(decode_frame) {
None => {}
Some((channel_id, data)) => rx
.enqueue((channel_id, data))
.map_err(|_| BxCanBusError::TxQueueFull)?,
let waker = cx.waker();
if !inner
.waker
.as_ref()
.map(|x| x.will_wake(waker))
.unwrap_or_default()
{
let mut waker = Some(waker.clone());
core::mem::swap(&mut waker, &mut inner.waker);
if let Some(waker) = waker {
waker.wake();
}
}
if inner.overflowed {
inner.overflowed = false;
Poll::Ready(Err(Error::Overflowed))
} else {
match inner.queue.pop_back() {
Some(x) => Poll::Ready(Ok(x)),
None => Poll::Pending,
}
}
})
})
}
pub fn try_recv(&mut self) -> Result<Option<CanFrame>, Error> {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
if inner.overflowed {
inner.overflowed = false;
Err(Error::Overflowed)
} else {
Ok(inner.queue.pop_back())
}
})
}
pub fn send(&mut self, frame: CanFrame) {
self.handle.send(frame)
}
}
}
pub mod bxcan {
use bxcan::{Can, Instance};
use critical_section::Mutex;
use heapless::{Deque, Vec};
use crate::{
can::CanDriver,
spmc::{Channel, ChannelClient},
};
use core::cell::RefCell;
use super::{
CanClient, CanData, CanDataFrame, CanDriverInternal, CanExtendedId, CanFrame, CanHandle,
CanHandleRef, CanId, CanRemoteFrame, CanStandardId,
};
pub enum Error {
TxQueueFull,
}
pub struct BxCanDriver<I: Instance, const TXN: usize> {
clients: Channel<CanFrame>,
inner: Mutex<RefCell<BxCanDriverInner<I, TXN>>>,
}
struct BxCanDriverInner<I: Instance, const TXN: usize> {
tx_queue: Deque<CanFrame, TXN>,
can: Can<I>,
}
impl<IN: Instance, const TXN: usize> BxCanDriver<IN, TXN> {
pub fn new(can: Can<IN>) -> Self {
Self {
clients: Channel::default(),
inner: Mutex::new(RefCell::new(BxCanDriverInner {
can,
tx_queue: Deque::new(),
})),
}
}
}
impl<IN: Instance, const TXN: usize> CanDriverInternal for BxCanDriver<IN, TXN> {
fn channel(&self) -> &Channel<CanFrame> {
&self.clients
}
fn send(&self, value: CanFrame) {
if critical_section::with(|cs| {
//TODO: error handling
let mut inner = self.inner.borrow_ref_mut(cs);
let was_empty = inner.tx_queue.is_empty();
self.clients.distribute(&value);
inner.tx_queue.push_front(value);
was_empty
}) {
self.handle_tx_interrupt();
}
}
}
impl<IN: Instance, const TXN: usize> CanDriver for BxCanDriver<IN, TXN> {
type Error = Error;
fn handle<'driver: 'client, 'client, I: super::CanClientInner<'client, 'driver>>(
&'driver self,
unbound: &'client mut super::UnboundCanHandle<'driver, I>,
builder: I::InitData,
) -> super::CanHandleRef<'client, 'driver, I> {
let handle = unbound.0.write(CanClient {
driver: self,
inner: I::new(builder, CanHandle(unsafe { &*(unbound.0.as_ptr()) })),
client: None,
});
handle.client = Some(ChannelClient::new(&*handle));
CanHandleRef(self.clients.register(handle))
}
fn handle_tx_interrupt(&self) -> Result<(), Error> {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
inner.can.clear_tx_interrupt();
while let Some(frame) = inner.tx_queue.pop_back() {
match inner.can.transmit(&frame.clone().into()) {
Ok(status) => match status.dequeued_frame() {
None => {
defmt::debug!("Transmitting CAN frame to {}.", frame.id());
}
Some(old_frame) => {
defmt::debug!(
"Transmitting CAN frame to {} by displacing other to {}, requeing it.",
frame.id(),
CanId::from(old_frame.id())
);
inner
.tx_queue
.push_front(old_frame.clone().into())
.map_err(|_| Error::TxQueueFull)?;
}
},
Err(nb::Error::WouldBlock) => {
rx.enqueue((channel_id, data))
.map_err(|_| BxCanBusError::TxQueueFull)?;
break;
defmt::debug!(
"Couldn't trasmit CAN frame to {}, requeing frame.",
frame.id()
);
inner
.tx_queue
.push_front(frame)
.map_err(|_| Error::TxQueueFull)?;
}
Err(_) => unreachable!(),
}
}
Ok(())
})
}
pub fn handle_rx_interrupt<I: Instance>(
&self,
can_rx: &mut Rx0<I>,
bus: impl BusBehaviour<Self>,
) -> Result<(), BxCanBusError> {
fn handle_rx_interrupt(&self) -> Result<(), Error> {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
loop {
match can_rx.receive() {
match inner.can.receive() {
Ok(frame) => {
let (channel_id, data) = decode_frame(&frame);
if let Some(data) = data {
bus.publish_local(&channel_id, data);
} else if let Some(state) = bus.get(channel_id) {
self.tx_queue
.enqueue((channel_id, Some(state)))
.map_err(|_| BxCanBusError::TxQueueFull)?;
(self.on_publish)(self);
}
let frame = frame.into();
self.clients.distribute(&frame)
}
Err(nb::Error::WouldBlock) => break,
Err(nb::Error::Other(_)) => break,
@ -104,55 +423,52 @@ impl<const TX_CAP: usize> BxCanBus<TX_CAP> {
}
Ok(())
}
}
#[derive(Clone, Copy, Format, Debug)]
pub struct CanDataFormat {
length: u8,
data: [u8; 8],
}
impl CanDataFormat {
pub fn new(data: &[u8]) -> Self {
assert!(data.len() <= 8);
let mut padded = [0_u8; 8];
padded[0..data.len()].copy_from_slice(data);
Self {
length: data.len() as u8,
data: padded,
})
}
}
pub fn data(&self) -> &[u8] {
&self.data[..self.length as usize]
impl From<CanFrame> for bxcan::Frame {
fn from(value: CanFrame) -> bxcan::Frame {
match value {
CanFrame::Data(CanDataFrame(id, data)) => {
bxcan::Frame::new_data(id, bxcan::Data::new(&data.0).unwrap())
}
}
impl BusDataFormat for CanDataFormat {}
fn decode_frame(frame: &bxcan::Frame) -> (BxCanId, Option<CanDataFormat>) {
let id = match frame.id() {
Id::Standard(id) => BxCanId::Standard(id.as_raw()),
Id::Extended(id) => BxCanId::Extended(id.as_raw()),
};
if let Some(data) = frame.data() {
(id, Some(CanDataFormat::new(data)))
CanFrame::Remote(CanRemoteFrame(id)) => bxcan::Frame::new_remote(id, 8),
}
}
}
impl From<bxcan::Frame> for CanFrame {
fn from(value: bxcan::Frame) -> CanFrame {
if let Some(data) = value.data() {
CanFrame::Data(CanDataFrame(
value.id().into(),
CanData(Vec::from_slice(data).unwrap()),
))
} else {
(id, None)
CanFrame::Remote(CanRemoteFrame(value.id().into()))
}
}
}
}
fn encode_frame(id: BxCanId, data: Option<CanDataFormat>) -> bxcan::Frame {
let id = match id {
BxCanId::Standard(id) => Id::Standard(StandardId::new(id).unwrap()),
BxCanId::Extended(id) => Id::Extended(ExtendedId::new(id).unwrap()),
};
if let Some(data) = data {
bxcan::Frame::new_data(id, bxcan::Data::new(data.data()).expect("valid can data"))
} else {
bxcan::Frame::new_remote(id, 8)
impl From<CanId> for bxcan::Id {
fn from(value: CanId) -> bxcan::Id {
match value {
CanId::Standard(CanStandardId(id)) => {
bxcan::Id::Standard(unsafe { bxcan::StandardId::new_unchecked(id) })
}
CanId::Extended(CanExtendedId(id)) => {
bxcan::Id::Extended(unsafe { bxcan::ExtendedId::new_unchecked(id) })
}
}
}
}
impl From<bxcan::Id> for CanId {
fn from(value: bxcan::Id) -> CanId {
match value {
bxcan::Id::Standard(id) => CanId::Standard(CanStandardId(id.as_raw())),
bxcan::Id::Extended(id) => CanId::Extended(CanExtendedId(id.as_raw())),
}
}
}
}

242
canome/src/canome.rs Normal file
View file

@ -0,0 +1,242 @@
use critical_section::Mutex;
use defmt::info;
use heapless::Deque;
use crate::{
can::{CanClientInner, CanData, CanFrame, CanHandle, CanId, CanRemoteFrame},
Decode, Encode,
};
use core::cell::RefCell;
use core::future::{poll_fn, Future};
use core::task::{Poll, Waker};
pub enum Error {
Overflowed,
}
pub trait CanomeChannel {
const ID: CanId;
type Data: Encode<CanData> + Decode<CanData>;
}
pub struct CanomeEventClient<'client, 'driver: 'client, C: CanomeChannel, const N: usize> {
handle: CanHandle<'client, 'driver, Self>,
inner: Mutex<RefCell<CanomeEventClientInner<C, N>>>,
}
struct CanomeEventClientInner<C: CanomeChannel, const N: usize> {
queue: Deque<C::Data, N>,
overflowed: bool,
waker: Option<Waker>,
}
impl<'client, 'driver: 'client, C: CanomeChannel, const N: usize> CanClientInner<'client, 'driver>
for CanomeEventClient<'client, 'driver, C, N>
{
type InitData = ();
fn new(_init_data: Self::InitData, handle: CanHandle<'client, 'driver, Self>) -> Self
where
Self: Sized,
{
Self {
handle,
inner: Mutex::new(RefCell::new(CanomeEventClientInner {
queue: Deque::new(),
overflowed: false,
waker: None,
})),
}
}
fn handle_frame(&self, frame: CanFrame) {
if let CanFrame::Data(frame) = frame {
if frame.id() == C::ID {
if let Ok(data) = C::Data::decode(frame.1) {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
if inner.queue.is_full() {
inner.queue.pop_back();
inner.overflowed = true;
}
inner.queue.push_front(data);
if let Some(waker) = inner.waker.take() {
waker.wake();
}
})
} else {
todo!()
}
}
}
}
}
impl<'client, 'driver: 'client, C: CanomeChannel, const N: usize>
CanomeEventClient<'client, 'driver, C, N>
{
pub fn recv(&mut self) -> impl Future<Output = Result<C::Data, Error>> + '_ {
poll_fn(|cx| {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
let waker = cx.waker();
if !inner
.waker
.as_ref()
.map(|x| x.will_wake(waker))
.unwrap_or_default()
{
let mut waker = Some(waker.clone());
core::mem::swap(&mut waker, &mut inner.waker);
if let Some(waker) = waker {
waker.wake();
}
}
if inner.overflowed {
inner.overflowed = false;
Poll::Ready(Err(Error::Overflowed))
} else {
match inner.queue.pop_back() {
Some(x) => Poll::Ready(Ok(x)),
None => Poll::Pending,
}
}
})
})
}
pub fn try_recv(&mut self) -> Result<Option<C::Data>, Error> {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
if inner.overflowed {
inner.overflowed = false;
Err(Error::Overflowed)
} else {
Ok(inner.queue.pop_back())
}
})
}
pub fn send(&mut self, data: C::Data) {
self.handle.send(CanFrame::Data(crate::can::CanDataFrame(
C::ID,
data.encode(),
)))
}
}
pub struct CanomeStateClient<'client, 'driver: 'client, C: CanomeChannel> {
handle: CanHandle<'client, 'driver, Self>,
inner: Mutex<RefCell<CanomeStateClientInner<C>>>,
}
struct CanomeStateClientInner<C: CanomeChannel> {
last: C::Data,
counter: u64,
waker: Option<Waker>,
}
impl<'client, 'driver: 'client, C: CanomeChannel> CanClientInner<'client, 'driver>
for CanomeStateClient<'client, 'driver, C>
where
C::Data: Default,
{
type InitData = ();
fn new(_init_data: Self::InitData, handle: CanHandle<'client, 'driver, Self>) -> Self
where
Self: Sized,
{
Self {
handle,
inner: Mutex::new(RefCell::new(CanomeStateClientInner {
last: C::Data::default(),
counter: 0,
waker: None,
})),
}
}
fn handle_frame(&self, frame: CanFrame) {
if let CanFrame::Data(frame) = frame {
if frame.id() == C::ID {
if let Ok(data) = C::Data::decode(frame.1) {
critical_section::with(|cs| {
let mut inner = self.inner.borrow_ref_mut(cs);
inner.last = data;
inner.counter += 1;
if let Some(waker) = inner.waker.take() {
waker.wake();
}
})
} else {
todo!()
}
}
}
}
}
impl<'client, 'driver: 'client, C: CanomeChannel> CanomeStateClient<'client, 'driver, C>
where
C::Data: Clone + Default,
{
pub fn next(&mut self) -> impl Future<Output = C::Data> + '_ {
let this: &'_ Self = self;
let counter_start: u64 = critical_section::with(|cs| self.inner.borrow_ref(cs).counter);
poll_fn(move |cx| {
critical_section::with(|cs| {
let mut inner = this.inner.borrow_ref_mut(cs);
let waker = cx.waker();
if !inner
.waker
.as_ref()
.map(|x| x.will_wake(waker))
.unwrap_or_default()
{
let mut waker = Some(waker.clone());
core::mem::swap(&mut waker, &mut inner.waker);
if let Some(waker) = waker {
waker.wake();
}
}
if inner.counter > counter_start {
Poll::Ready(inner.last.clone())
} else {
Poll::Pending
}
})
})
}
pub fn get(&mut self) -> C::Data {
critical_section::with(|cs| {
let inner = self.inner.borrow_ref(cs);
inner.last.clone()
})
}
pub fn counter(&self) -> u64 {
critical_section::with(|cs| {
let inner = self.inner.borrow_ref(cs);
inner.counter
})
}
pub fn send(&mut self, data: C::Data) {
self.handle.send(CanFrame::Data(crate::can::CanDataFrame(
C::ID,
data.encode(),
)))
}
pub fn request(&mut self) {
self.handle.send(CanFrame::Remote(CanRemoteFrame(C::ID)));
}
pub fn request_and_wait(&mut self) -> impl Future<Output = C::Data> + '_ {
self.handle.send(CanFrame::Remote(CanRemoteFrame(C::ID)));
self.next()
}
}

13
canome/src/contact.rs
View file

@ -16,24 +16,23 @@ impl ContactState {
}
}
#[cfg(feature = "can")]
mod can {
use crate::{can::CanDataFormat, Decode, Encode};
use crate::{can::CanData, Decode, Encode};
use super::ContactState;
impl Encode<CanDataFormat> for ContactState {
fn encode(self) -> CanDataFormat {
impl Encode<CanData> for ContactState {
fn encode(self) -> CanData {
let data = match self {
Self::Open => 0,
Self::Closed => 1,
};
CanDataFormat::new(&[data])
CanData::new(&[data])
}
}
impl Decode<CanDataFormat> for ContactState {
fn decode(value: CanDataFormat) -> Result<Self, CanDataFormat>
impl Decode<CanData> for ContactState {
fn decode(value: CanData) -> Result<Self, CanData>
where
Self: Sized,
{

88
canome/src/cover.rs
View file

@ -15,22 +15,40 @@ impl CoverState {
}
}
#[cfg(feature = "can")]
#[derive(Default, Debug, Format, Clone, Copy, PartialEq, Eq)]
pub struct CoverControl {
pub sequence_number: u8,
pub command: CoverCommand,
}
#[derive(Default, Debug, Format, Clone, Copy, PartialEq, Eq)]
pub enum CoverCommand {
#[default]
None,
Stop,
Open,
Close,
TiltMin,
TiltMax,
Position(u16),
Tilt(u16),
}
mod can {
use crate::{can::CanDataFormat, Decode, Encode};
use crate::{can::CanData, Decode, Encode};
use super::CoverState;
use super::{CoverCommand, CoverControl, CoverState};
impl Encode<CanDataFormat> for CoverState {
fn encode(self) -> CanDataFormat {
impl Encode<CanData> for CoverState {
fn encode(self) -> CanData {
let hue = self.tilt.to_be_bytes();
let saturation = self.position.to_be_bytes();
CanDataFormat::new(&[hue[0], hue[1], saturation[0], saturation[1]])
CanData::new(&[hue[0], hue[1], saturation[0], saturation[1]])
}
}
impl Decode<CanDataFormat> for CoverState {
fn decode(value: CanDataFormat) -> Result<Self, CanDataFormat>
impl Decode<CanData> for CoverState {
fn decode(value: CanData) -> Result<Self, CanData>
where
Self: Sized,
{
@ -48,4 +66,58 @@ mod can {
})
}
}
impl Encode<CanData> for CoverControl {
fn encode(self) -> CanData {
let seq = self.sequence_number;
let (kind, data): (u8, &[u8]) = match self.command {
CoverCommand::None => (0, &[]),
CoverCommand::Stop => (1, &[]),
CoverCommand::Open => (2, &[]),
CoverCommand::Close => (3, &[]),
CoverCommand::TiltMin => (4, &[]),
CoverCommand::TiltMax => (5, &[]),
CoverCommand::Position(val) => (6, &val.to_be_bytes()),
CoverCommand::Tilt(val) => (7, &val.to_be_bytes()),
};
let mut payload = [seq, kind, 0, 0, 0, 0, 0, 0];
payload[2..].copy_from_slice(&data);
CanData::new(&payload[0..(2 + data.len())])
}
}
impl Decode<CanData> for CoverControl {
fn decode(value: CanData) -> Result<Self, CanData>
where
Self: Sized,
{
use CoverCommand as C;
let data = value.data();
if data.len() < 2 {
return Err(value);
}
let sequence_number = data[0];
let command = data[1];
let data = &data[2..];
let command = match command {
0 => C::None,
1 => C::Stop,
2 => C::Open,
3 => C::Close,
4 => C::TiltMin,
5 => C::TiltMax,
6 if data.len() == 2 => C::Position(u16::from_be_bytes([data[0], data[1]])),
7 if data.len() == 2 => C::Tilt(u16::from_be_bytes([data[0], data[1]])),
_ => return Err(value),
};
Ok(Self {
sequence_number,
command,
})
}
}
}

62
canome/src/lib.rs
View file

@ -1,37 +1,73 @@
#![no_std]
#![feature(adt_const_params)]
#![feature(associated_const_equality)]
use core::fmt::Debug;
pub mod bus;
#[cfg(feature = "can")]
use defmt::Format;
//pub mod bus;
//#[cfg(feature = "bxcan")]
//pub mod bxcan;
pub mod can;
pub mod canome;
pub mod contact;
pub mod cover;
pub mod light;
pub mod power;
#[cfg(feature = "socketcan")]
pub mod socketcan;
pub mod spmc;
pub mod state;
#[cfg(feature = "stm32")]
pub mod stm32;
pub mod time;
pub trait Channel<B: BusBackend> {
const ID: B::ID;
type State: Encode<B::Data> + Decode<B::Data>;
}
pub use embedded_can::{ExtendedId, Id, StandardId};
pub trait Encode<F: BusDataFormat> {
/*
pub trait Channel {
const ID: embedded_can::Id;
type Message: Encode<CanDataFormat> + Decode<CanDataFormat>;
}
*/
pub trait Encode<F> {
fn encode(self) -> F;
}
pub trait Decode<F: BusDataFormat> {
pub trait Decode<F> {
fn decode(value: F) -> Result<Self, F>
where
Self: Sized;
}
pub trait BusDataFormat: Debug + Clone {}
pub trait BusBackend {
type ID: Debug + Clone;
type Data: BusDataFormat;
/*
pub trait BusBackend: Send + Sync {
type Error;
fn publish(&self, id: Self::ID, data: Option<Self::Data>) -> Result<(), Self::Error>;
fn publish(&self, id: embedded_can::Id, data: Option<CanDataFormat>)
-> Result<(), Self::Error>;
}
#[derive(Clone, Copy, Format, Debug)]
pub struct CanDataFormat {
length: usize,
data: [u8; 8],
}
impl CanDataFormat {
pub fn new(data: &[u8]) -> Self {
assert!(data.len() <= 8);
let mut padded = [0_u8; 8];
padded[0..data.len()].copy_from_slice(data);
Self {
length: data.len(),
data: padded,
}
}
pub fn data(&self) -> &[u8] {
&self.data[..self.length]
}
}
*/

13
canome/src/power.rs
View file

@ -5,25 +5,24 @@ pub struct PowermeterState {
pub total: u32,
pub current: u32,
}
#[cfg(feature = "can")]
mod can {
use crate::{can::CanDataFormat, Decode, Encode};
use crate::{can::CanData, Decode, Encode};
use super::PowermeterState;
impl Encode<CanDataFormat> for PowermeterState {
fn encode(self) -> CanDataFormat {
impl Encode<CanData> for PowermeterState {
fn encode(self) -> CanData {
let total = self.total.to_be_bytes();
let current = self.current.to_be_bytes();
CanDataFormat::new(&[
CanData::new(&[
total[0], total[1], total[2], total[3], current[0], current[1], current[2],
current[3],
])
}
}
impl Decode<CanDataFormat> for PowermeterState {
fn decode(value: CanDataFormat) -> Result<Self, CanDataFormat>
impl Decode<CanData> for PowermeterState {
fn decode(value: CanData) -> Result<Self, CanData>
where
Self: Sized,
{

147
canome/src/socketcan.rs Normal file
View file

@ -0,0 +1,147 @@
use core::fmt::Debug;
use crate::{BusBackend, CanDataFormat};
use defmt::Format;
use embedded_can::{ExtendedId, Frame, Id, StandardId};
use heapless::mpmc::MpMcQueue;
use socketcan::{CanDataFrame, CanFrame, CanRemoteFrame};
pub struct SocketCanBus<const TX_CAP: usize> {
pub tx_queue: MpMcQueue<socketcan::CanFrame, TX_CAP>,
on_publish: &'static (dyn Fn(&Self) + Send + Sync),
}
impl<const TX_CAP: usize> Debug for SocketCanBus<TX_CAP> {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.debug_struct("SocketCanBus").finish()
}
}
#[derive(Debug)]
pub enum SocketCanBusError {
/// An object couldn't be added to the TX queue of [`BxCanBus`] because it is full.
TxQueueFull,
}
impl<const TX_CAP: usize> BusBackend for SocketCanBus<TX_CAP> {
type Error = SocketCanBusError;
fn publish(&self, id: Id, data: Option<CanDataFormat>) -> Result<(), Self::Error> {
self.tx_queue
.enqueue(encode_frame(id, data))
.map_err(|_| Self::Error::TxQueueFull)?;
(self.on_publish)(self);
Ok(())
}
}
impl<const TX_CAP: usize> SocketCanBus<TX_CAP> {
pub const fn new_with_hook(on_publish: &'static (dyn Fn(&Self) + Send + Sync)) -> Self {
Self {
tx_queue: MpMcQueue::new(),
on_publish,
}
}
}
impl<const TX_CAP: usize> SocketCanBus<TX_CAP> {}
/*
impl<const TX_CAP: usize> SocketCanBus<TX_CAP> {
pub fn handle_tx_interrupt<I: Instance>(
&self,
can_tx: &mut Tx<I>,
) -> Result<(), SocketCanBusError> {
let rx = &self.tx_queue;
while let Some(frame) = rx.dequeue() {
match can_tx.transmit(&frame) {
Ok(status) => match status.dequeued_frame() {
None => {
defmt::debug!("Transmitting CAN frame to {}.", frame.id());
}
Some(old_frame) => {
defmt::debug!(
"Transmitting CAN frame to {} by displacing other to {}, requeing it.",
frame.id(),
old_frame.id()
);
rx.enqueue(old_frame.clone())
.map_err(|_| SocketCanBusError::TxQueueFull)?;
}
},
Err(nb::Error::WouldBlock) => {
defmt::debug!(
"Couldn't trasmit CAN frame to {}, requeing frame.",
frame.id()
);
rx.enqueue(frame)
.map_err(|_| SocketCanBusError::TxQueueFull)?;
}
Err(_) => unreachable!(),
}
}
Ok(())
}
pub fn handle_rx_interrupt<I: Instance>(
&self,
can_rx: &mut Rx0<I>,
bus: impl BusBehaviour<Self>,
) -> Result<(), SocketCanBusError> {
loop {
match can_rx.receive() {
Ok(frame) => {
let (channel_id, data) = decode_frame(&frame);
if let Some(data) = data {
bus.publish_local(&channel_id, data);
} else if let Some(state) = bus.get(channel_id) {
defmt::debug!("responding to remote frame");
self.tx_queue
.enqueue(encode_frame(channel_id, Some(state)))
.map_err(|_| SocketCanBusError::TxQueueFull)?;
(self.on_publish)(self);
}
}
Err(nb::Error::WouldBlock) => break,
Err(nb::Error::Other(_)) => break,
}
}
Ok(())
}
}
*/
pub fn decode_frame(frame: &socketcan::CanFrame) -> (Id, Option<CanDataFormat>) {
let id = match frame.id() {
socketcan::Id::Standard(x) => {
Id::Standard(unsafe { StandardId::new_unchecked(x.as_raw()) })
}
socketcan::Id::Extended(x) => {
Id::Extended(unsafe { ExtendedId::new_unchecked(x.as_raw()) })
}
};
match frame {
CanFrame::Data(data) => (id, Some(CanDataFormat::new(data.data()))),
CanFrame::Remote(_) => (id, None),
CanFrame::Error(_) => unimplemented!(),
}
}
pub fn encode_frame(id: Id, data: Option<CanDataFormat>) -> CanFrame {
let id = match id {
Id::Standard(x) => {
socketcan::Id::Standard(unsafe { socketcan::StandardId::new_unchecked(x.as_raw()) })
}
Id::Extended(x) => {
socketcan::Id::Extended(unsafe { socketcan::ExtendedId::new_unchecked(x.as_raw()) })
}
};
if let Some(data) = data {
CanFrame::Data(CanDataFrame::new(id, data.data()).unwrap())
} else {
CanFrame::Remote(CanRemoteFrame::new_remote(id, 8).unwrap())
}
}

418
canome/src/spmc.rs Normal file
View file

@ -0,0 +1,418 @@
use core::borrow::{Borrow, BorrowMut};
use core::cell::RefCell;
use core::marker::PhantomData;
use core::ops::{Deref, DerefMut};
use core::ptr::NonNull;
use critical_section::Mutex;
#[derive(PartialEq)]
pub enum Error {
Overflowed,
}
pub type CanData = [u8; 8];
pub type CanId = u32;
pub struct Channel<T>(Mutex<RefCell<Option<ClientList<T>>>>);
unsafe impl<T> Send for Channel<T> {}
struct ClientList<T> {
head: NonNull<ChannelClient<T>>,
tail: NonNull<ChannelClient<T>>,
}
pub struct ChannelClient<T> {
receiver: NonNull<dyn Receive<T>>,
nav: Mutex<RefCell<ChannelClientNav<T>>>,
}
struct ChannelClientNav<T> {
next: Option<NonNull<ChannelClient<T>>>,
prev: Option<NonNull<ChannelClient<T>>>,
}
pub trait Receiver<'channel, T>: Receive<T> {
fn channel_client(&self) -> &ChannelClient<T>;
fn channel(&self) -> &Channel<T>;
}
pub trait Receive<T> {
fn receive(&self, value: &T);
}
pub struct ReceiverRef<'channel, 'receiver, T, R: Receiver<'channel, T>>(
&'receiver mut R,
PhantomData<&'channel Channel<T>>,
);
impl<T> ChannelClient<T> {
pub fn new<'channel, R: Receiver<'channel, T>>(receiver: &R) -> Self {
Self {
receiver: NonNull::new(receiver as *const dyn Receive<T> as *mut _).unwrap(),
nav: Mutex::new(RefCell::new(ChannelClientNav {
prev: None,
next: None,
})),
}
}
}
impl<T> Channel<T> {
pub fn register<'channel, 'receiver, R: Receiver<'channel, T>>(
&'channel self,
receiver: &'receiver mut R,
) -> ReceiverRef<'channel, 'receiver, T, R>
where
'channel: 'receiver,
{
let client_ref = receiver.channel_client();
let client_ptr = NonNull::new(client_ref as *const _ as *mut _).unwrap();
critical_section::with(|cs| {
let mut client_list = self.0.borrow(cs).borrow_mut();
if let Some(client_list) = &mut *client_list {
let old_head = client_list.head;
client_ref.nav.borrow(cs).borrow_mut().next = Some(old_head);
let old_head = unsafe { old_head.as_ref() };
old_head.nav.borrow(cs).borrow_mut().prev = Some(client_ptr);
client_list.head = client_ptr;
} else {
*client_list = Some(ClientList {
head: client_ptr,
tail: client_ptr,
});
}
});
ReceiverRef(receiver, PhantomData)
}
pub fn distribute(&self, value: &T) {
critical_section::with(|cs| {
let list = self.0.borrow(cs).borrow();
if let Some(list) = &*list {
let mut next = Some(list.head);
while let Some(current) = next {
let current = unsafe { current.as_ref() };
let receiver = unsafe { current.receiver.as_ref() };
receiver.receive(value);
next = current.nav.borrow(cs).borrow().next;
}
}
})
}
}
impl<'channel, 'receiver, T, R: Receiver<'channel, T>> Deref
for ReceiverRef<'channel, 'receiver, T, R>
{
type Target = R;
fn deref(&self) -> &Self::Target {
self.0
}
}
impl<'channel, 'receiver, T, R: Receiver<'channel, T>> DerefMut
for ReceiverRef<'channel, 'receiver, T, R>
{
fn deref_mut(&mut self) -> &mut Self::Target {
self.0
}
}
impl<'channel, 'receiver, T, R: Receiver<'channel, T>> AsRef<R>
for ReceiverRef<'channel, 'receiver, T, R>
{
fn as_ref(&self) -> &R {
self.0
}
}
impl<'channel, 'receiver, T, R: Receiver<'channel, T>> Borrow<R>
for ReceiverRef<'channel, 'receiver, T, R>
{
fn borrow(&self) -> &R {
self.0
}
}
impl<'channel, 'receiver, T, R: Receiver<'channel, T>> BorrowMut<R>
for ReceiverRef<'channel, 'receiver, T, R>
{
fn borrow_mut(&mut self) -> &mut R {
self.0
}
}
impl<T> Default for Channel<T> {
fn default() -> Self {
Self(Mutex::new(RefCell::new(None)))
}
}
impl<'channel, 'receiver, T, R: Receiver<'channel, T>> Drop
for ReceiverRef<'channel, 'receiver, T, R>
{
fn drop(&mut self) {
critical_section::with(|cs| {
let receiver = &mut self.0;
let channel = receiver.channel();
let own_nav = receiver.channel_client().nav.borrow(cs).borrow_mut();
match (own_nav.prev, own_nav.next) {
(None, None) => {
// this item is the last one in the list, we need to tear down the entire list
let mut can_driver_list = channel.0.borrow(cs).borrow_mut();
*can_driver_list = None;
}
(None, Some(next)) => {
// this item is the first in the list, we need to update the head ptr and the
// prev ptr of next
let mut can_driver_list = channel.0.borrow(cs).borrow_mut();
let can_driver_list = can_driver_list.as_mut().unwrap();
can_driver_list.head = next;
let next = unsafe { next.as_ref() };
next.nav.borrow(cs).borrow_mut().prev = None;
}
(Some(prev), None) => {
// this item is the last one in the list, we need to update the tail ptr and
// the next ptr of prev
let mut can_driver_list = channel.0.borrow(cs).borrow_mut();
let can_driver_list = can_driver_list.as_mut().unwrap();
can_driver_list.tail = prev;
let prev = unsafe { prev.as_ref() };
prev.nav.borrow(cs).borrow_mut().next = None;
}
(Some(prev), Some(next)) => {
// this item is in the middle of the list, we need to update the next ptr of
// prev and the prev ptr of next
{
let next = unsafe { next.as_ref() };
next.nav.borrow(cs).borrow_mut().prev = Some(prev);
}
{
let prev = unsafe { prev.as_ref() };
prev.nav.borrow(cs).borrow_mut().next = Some(next);
}
}
}
});
}
}
#[cfg(test)]
mod test {
use super::{Channel, ChannelClient, ChannelClientNav, Error, Receive, Receiver};
use core::cell::RefCell;
use core::future::{poll_fn, Future};
use core::mem::MaybeUninit;
use core::ptr::NonNull;
use core::task::{Poll, Waker};
use critical_section::Mutex;
use heapless::Deque;
pub struct UnboundReceiver<'channel, T, const N: usize>(
MaybeUninit<ChannelReceiver<'channel, T, N>>,
);
impl<'channel, T, const N: usize> AsMut<MaybeUninit<ChannelReceiver<'channel, T, N>>>
for UnboundReceiver<'channel, T, N>
{
fn as_mut(&mut self) -> &mut MaybeUninit<ChannelReceiver<'channel, T, N>> {
&mut self.0
}
}
impl<'channel, T, const N: usize> Default for UnboundReceiver<'channel, T, N> {
fn default() -> Self {
Self(MaybeUninit::uninit())
}
}
impl<'candriver, T, const N: usize> ChannelReceiver<'candriver, T, N> {
pub fn recv(&mut self) -> impl Future<Output = Result<T, Error>> + '_ {
poll_fn(|cx| {
match critical_section::with(|cs| {
let mut inner = self.inner.borrow(cs).borrow_mut();
// register waker
let waker = cx.waker();
if !inner
.waker
.as_ref()
.map(|x| x.will_wake(waker))
.unwrap_or_default()
{
let mut waker = Some(waker.clone());
core::mem::swap(&mut inner.waker, &mut waker);
if let Some(waker) = waker {
waker.wake();
}
}
if inner.overflowed {
inner.overflowed = false;
Err(Error::Overflowed)
} else {
Ok(inner.queue.pop_back())
}
}) {
Err(x) => Poll::Ready(Err(x)),
Ok(Some(x)) => Poll::Ready(Ok(x)),
Ok(None) => Poll::Pending,
}
})
}
pub fn try_recv(&mut self) -> Result<Option<T>, Error> {
critical_section::with(|cs| {
let mut inner = self.inner.borrow(cs).borrow_mut();
if inner.overflowed {
inner.overflowed = false;
Err(Error::Overflowed)
} else {
Ok(inner.queue.pop_back())
}
})
}
}
pub struct ChannelReceiver<'channel, T, const N: usize> {
channel: &'channel Channel<T>,
client: Option<ChannelClient<T>>,
inner: Mutex<RefCell<ChannelReceiverInner<T, N>>>,
}
impl<'channel, T: Clone, const N: usize> ChannelReceiver<'channel, T, N> {
fn init<'a>(
channel: &'channel Channel<T>,
memory: &'a mut UnboundReceiver<'channel, T, N>,
) -> &'a mut Self
where
Self: Sized,
{
let receiver = memory.0.write(ChannelReceiver {
channel,
client: None,
inner: Mutex::new(RefCell::new(ChannelReceiverInner {
queue: Deque::new(),
overflowed: false,
waker: None,
})),
});
receiver.client = Some(ChannelClient {
receiver: NonNull::new(receiver as *mut _ as *const dyn Receive<T> as *mut _)
.unwrap(),
nav: Mutex::new(RefCell::new(ChannelClientNav {
next: None,
prev: None,
})),
});
receiver
}
}
impl<'channel, T: Clone, const N: usize> Receiver<'channel, T> for ChannelReceiver<'channel, T, N> {
fn channel(&self) -> &Channel<T> {
self.channel
}
fn channel_client(&self) -> &ChannelClient<T> {
self.client.as_ref().unwrap()
}
}
impl<'candriver, T: Clone, const N: usize> Receive<T> for ChannelReceiver<'candriver, T, N> {
fn receive(&self, value: &T) {
critical_section::with(|cs| {
let mut inner = self.inner.borrow(cs).borrow_mut();
if inner.queue.is_full() {
inner.queue.pop_back();
inner.overflowed = true;
}
let Ok(_) = inner.queue.push_front(value.clone()) else {
unreachable!()
};
})
}
}
struct ChannelReceiverInner<T, const N: usize> {
queue: Deque<T, N>,
overflowed: bool,
waker: Option<Waker>,
}
#[test]
pub fn basic() {
let can_driver = Channel::default();
let mut recv1 = UnboundReceiver::default();
let recv1 = ChannelReceiver::init(&can_driver, &mut recv1);
let mut recv1 = can_driver.register::<ChannelReceiver<'_, u32, 2>>(recv1);
//test overflowing behaviour
assert!(recv1.try_recv() == Ok(None));
can_driver.distribute(&0);
assert!(recv1.try_recv() == Ok(Some(0)));
assert!(recv1.try_recv() == Ok(None));
}
#[test]
pub fn overflow() {
let can_driver = Channel::default();
let mut recv1 = UnboundReceiver::default();
let recv1 = ChannelReceiver::init(&can_driver, &mut recv1);
let mut recv1 = can_driver.register::<ChannelReceiver<'_, u32, 10>>(recv1);
//test overflowing behaviour
assert!(recv1.try_recv() == Ok(None));
for i in 0..11 {
can_driver.distribute(&i);
}
assert!(recv1.try_recv() == Err(Error::Overflowed));
for i in 1..11 {
assert!(recv1.try_recv() == Ok(Some(i)));
}
assert!(recv1.try_recv() == Ok(None));
}
#[test]
pub fn multi_receiver() {
let can_driver = Channel::default();
let mut recv1 = UnboundReceiver::default();
let recv1 = ChannelReceiver::init(&can_driver, &mut recv1);
let mut recv1 = can_driver.register::<ChannelReceiver<'_, u32, 2>>(recv1);
let mut recv2 = UnboundReceiver::default();
let recv2 = ChannelReceiver::init(&can_driver, &mut recv2);
let mut recv2 = can_driver.register::<ChannelReceiver<'_, u32, 2>>(recv2);
{
can_driver.distribute(&0);
assert!(recv1.try_recv() == Ok(Some(0)));
assert!(recv2.try_recv() == Ok(Some(0)));
}
{
can_driver.distribute(&1);
can_driver.distribute(&2);
assert!(recv1.try_recv() == Ok(Some(1)));
assert!(recv1.try_recv() == Ok(Some(2)));
assert!(recv2.try_recv() == Ok(Some(1)));
assert!(recv2.try_recv() == Ok(Some(2)));
}
drop(recv1);
{
can_driver.distribute(&3);
assert!(recv2.try_recv() == Ok(Some(3)));
}
}
}

13
canome/src/state.rs
View file

@ -6,24 +6,23 @@ pub enum ThreeWaySwitchState {
B,
}
#[cfg(feature = "can")]
mod can {
use crate::{can::CanDataFormat, Decode, Encode};
use crate::{can::CanData, Decode, Encode};
use super::ThreeWaySwitchState;
impl Encode<CanDataFormat> for ThreeWaySwitchState {
fn encode(self) -> CanDataFormat {
impl Encode<CanData> for ThreeWaySwitchState {
fn encode(self) -> CanData {
let desc = match self {
Self::Off => 0,
Self::A => 1,
Self::B => 2,
};
CanDataFormat::new(&[desc])
CanData::new(&[desc])
}
}
impl Decode<CanDataFormat> for ThreeWaySwitchState {
fn decode(value: CanDataFormat) -> Result<Self, CanDataFormat>
impl Decode<CanData> for ThreeWaySwitchState {
fn decode(value: CanData) -> Result<Self, CanData>
where
Self: Sized,
{

37
canome/src/stm32.rs
View file

@ -1,6 +1,10 @@
use core::ops::{Deref, DerefMut};
use crate::{bus::BusRx, state::ThreeWaySwitchState, BusBackend, Channel};
use crate::{
can::{CanHandle, CanHandleRef},
canome::{CanomeChannel, CanomeEventClient, CanomeStateClient},
state::ThreeWaySwitchState,
};
use rtic_monotonics::{
systick::{
fugit::{Duration, Instant},
@ -37,26 +41,26 @@ impl<const AP: char, const AN: u8, const BP: char, const BN: u8> ThreeWaySwitch<
}
}
pub struct ChannelDerivative<'a, C: Channel<B>, R: BusRx<C, B>, B: BusBackend> {
channel: &'a R,
last: C::State,
pub struct ChannelDerivative<'handle, 'driver, C: CanomeChannel, const N: usize> {
channel: CanHandleRef<'handle, 'driver, CanomeEventClient<'handle, 'driver, C, N>>,
last: C::Data,
updated: Instant<u32, 1, 1000>,
}
impl<'a, C, R, B> ChannelDerivative<'a, C, R, B>
impl<'handle, 'driver, C, const N: usize> ChannelDerivative<'handle, 'driver, C, N>
where
C: Channel<B>,
C::State: Default + PartialEq + Clone,
R: BusRx<C, B>,
B: BusBackend,
C: CanomeChannel,
C::Data: Default + PartialEq + Clone,
{
pub fn new(channel: &'a R) -> Self {
pub fn new(
channel: CanHandleRef<'handle, 'driver, CanomeEventClient<'handle, 'driver, C, N>>,
) -> Self {
Self {
channel,
last: C::State::default(),
last: C::Data::default(),
updated: Systick::now(),
}
}
pub fn update(&mut self, new: C::State) -> Option<(C::State, Duration<u32, 1, 1000>)> {
pub fn update(&mut self, new: C::Data) -> Option<(C::Data, Duration<u32, 1, 1000>)> {
if new != self.last {
self.last = new.clone();
let now = Systick::now();
@ -67,10 +71,13 @@ where
None
}
}
pub async fn next(&mut self) -> (C::State, Duration<u32, 1, 1000>) {
pub async fn recv(
&mut self,
) -> Result<(C::Data, Duration<u32, 1, 1000>), crate::canome::Error> {
loop {
if let Some(value) = self.update(self.channel.next().await) {
return value;
let new = self.channel.recv().await?;
if let Some(value) = self.update(new) {
return Ok(value);
}
}
}

13
canome/src/time.rs
View file

@ -5,21 +5,20 @@ pub struct Timestamp {
millis_since_epoch: i64,
}
#[cfg(feature = "can")]
mod can {
use crate::{can::CanDataFormat, Decode, Encode};
use crate::{can::CanData, Decode, Encode};
use super::Timestamp;
impl Encode<CanDataFormat> for Timestamp {
fn encode(self) -> CanDataFormat {
impl Encode<CanData> for Timestamp {
fn encode(self) -> CanData {
let timestamp = self.millis_since_epoch.to_be_bytes();
CanDataFormat::new(&timestamp)
CanData::new(&timestamp)
}
}
impl Decode<CanDataFormat> for Timestamp {
fn decode(value: CanDataFormat) -> Result<Self, CanDataFormat>
impl Decode<CanData> for Timestamp {
fn decode(value: CanData) -> Result<Self, CanData>
where
Self: Sized,
{