111 lines
3.3 KiB
Rust
111 lines
3.3 KiB
Rust
use core::cell::RefCell;
|
|
use std::sync::mpsc;
|
|
|
|
use spacepackets::{ecss::tc::PusTcReader, SpHeader};
|
|
use thiserror::Error;
|
|
|
|
use crate::{
|
|
pool::{PoolProvider, SharedStaticMemoryPool, StoreAddr, StoreError},
|
|
pus::ReceivesEcssPusTc,
|
|
queue::GenericSendError,
|
|
};
|
|
|
|
use super::{PacketSenderRaw, ReceivesCcsdsTc};
|
|
|
|
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
|
pub enum StoreAndSendError {
|
|
#[error("Store error: {0}")]
|
|
Store(#[from] StoreError),
|
|
#[error("Genreric send error: {0}")]
|
|
Send(#[from] GenericSendError),
|
|
}
|
|
|
|
#[derive(Clone)]
|
|
pub struct SharedTcPool(pub SharedStaticMemoryPool);
|
|
|
|
impl SharedTcPool {
|
|
pub fn add_pus_tc(&mut self, pus_tc: &PusTcReader) -> Result<StoreAddr, StoreError> {
|
|
let mut pg = self.0.write().expect("error locking TC store");
|
|
let addr = pg.free_element(pus_tc.len_packed(), |buf| {
|
|
buf[0..pus_tc.len_packed()].copy_from_slice(pus_tc.raw_data());
|
|
})?;
|
|
Ok(addr)
|
|
}
|
|
|
|
pub fn add_ccsds_tc(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<StoreAddr, StoreError> {
|
|
self.add_raw_tc(tc_raw)
|
|
}
|
|
|
|
pub fn add_raw_tc(&mut self, tc_raw: &[u8]) -> Result<StoreAddr, StoreError> {
|
|
let mut pg = self.0.write().expect("error locking TC store");
|
|
let addr = pg.free_element(tc_raw.len(), |buf| {
|
|
buf[0..tc_raw.len()].copy_from_slice(tc_raw);
|
|
})?;
|
|
Ok(addr)
|
|
}
|
|
}
|
|
|
|
#[derive(Clone)]
|
|
pub struct TcSenderSharedPool {
|
|
pub tc_source: mpsc::SyncSender<StoreAddr>,
|
|
pub shared_pool: RefCell<SharedTcPool>,
|
|
}
|
|
|
|
impl TcSenderSharedPool {
|
|
pub fn new(tc_source: mpsc::SyncSender<StoreAddr>, shared_pool: SharedTcPool) -> Self {
|
|
Self {
|
|
tc_source,
|
|
shared_pool: RefCell::new(shared_pool),
|
|
}
|
|
}
|
|
|
|
#[allow(dead_code)]
|
|
pub fn shared_pool(&self) -> SharedStaticMemoryPool {
|
|
let pool = self.shared_pool.borrow();
|
|
pool.0.clone()
|
|
}
|
|
}
|
|
|
|
impl PacketSenderRaw for TcSenderSharedPool {
|
|
type Error = StoreAndSendError;
|
|
|
|
fn send_raw_tc(&self, tc_raw: &[u8]) -> Result<(), Self::Error> {
|
|
let mut shared_pool = self.shared_pool.borrow_mut();
|
|
let addr = shared_pool.add_raw_tc(tc_raw)?;
|
|
drop(shared_pool);
|
|
self.tc_source.try_send(addr).map_err(|e| match e {
|
|
mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None),
|
|
mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected,
|
|
})?;
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
impl ReceivesEcssPusTc for TcSenderSharedPool {
|
|
type Error = StoreAndSendError;
|
|
|
|
fn pass_pus_tc(&mut self, _: &SpHeader, pus_tc: &PusTcReader) -> Result<(), Self::Error> {
|
|
let mut shared_pool = self.shared_pool.borrow_mut();
|
|
let addr = shared_pool.add_raw_tc(pus_tc.raw_data())?;
|
|
drop(shared_pool);
|
|
self.tc_source.try_send(addr).map_err(|e| match e {
|
|
mpsc::TrySendError::Full(_) => GenericSendError::QueueFull(None),
|
|
mpsc::TrySendError::Disconnected(_) => GenericSendError::RxDisconnected,
|
|
})?;
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
impl ReceivesCcsdsTc for TcSenderSharedPool {
|
|
type Error = StoreAndSendError;
|
|
|
|
fn pass_ccsds(&mut self, _sp_header: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
|
|
self.send_raw_tc(tc_raw)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
// TODO: Add tests for shared pool TC sender component.
|
|
}
|