diff --git a/fsrc-core/src/pus/mod.rs b/fsrc-core/src/pus/mod.rs index 20c53ec..c18a6cf 100644 --- a/fsrc-core/src/pus/mod.rs +++ b/fsrc-core/src/pus/mod.rs @@ -3,7 +3,7 @@ use spacepackets::time::TimestampError; pub mod verification; -#[derive(Debug, Copy, Clone)] +#[derive(Debug, Clone)] pub enum SendStoredTmError { SendError(E), TimeStampError(TimestampError), diff --git a/fsrc-core/src/pus/verification.rs b/fsrc-core/src/pus/verification.rs index b750f27..86a6542 100644 --- a/fsrc-core/src/pus/verification.rs +++ b/fsrc-core/src/pus/verification.rs @@ -1,8 +1,10 @@ use crate::pus::SendStoredTmError; use alloc::boxed::Box; +use alloc::vec; use alloc::vec::Vec; +use core::mem::size_of; use spacepackets::tc::PusTc; -use spacepackets::time::{TimeWriter, TimestampError}; +use spacepackets::time::{CcsdsTimeProvider, TimeWriter, TimestampError}; use spacepackets::tm::{PusTm, PusTmSecondaryHeader}; use spacepackets::SpHeader; use spacepackets::{CcsdsPacket, PacketId, PacketSequenceCtrl}; @@ -15,6 +17,16 @@ pub struct RequestId { psc: PacketSequenceCtrl, } +impl RequestId { + const SIZE_AS_BYTES: usize = size_of::(); + + pub fn to_bytes(&self, buf: &mut [u8]) { + let raw = ((self.version_number as u32) << 31) + | ((self.packet_id.raw() as u32) << 16) + | self.psc.raw() as u32; + buf.copy_from_slice(raw.to_be_bytes().as_slice()); + } +} impl RequestId { pub fn new(tc: &PusTc) -> Self { RequestId { @@ -30,14 +42,57 @@ pub trait VerificationSender { } pub struct VerificationReporter { - apid: u16, + pub apid: u16, + pub dest_id: u16, msg_count: u16, - dest_id: u16, time_stamper: Box, time_stamp_buf: Vec, + source_data_buf: Vec, +} + +pub struct VerificationReporterCfg { + pub apid: u16, + pub dest_id: u16, + pub time_stamper: Box, + pub step_field_width: u8, + pub failure_code_field_width: u8, + pub max_fail_data_len: usize, + pub max_stamp_len: usize, +} + +impl VerificationReporterCfg { + pub fn new(time_stamper: impl TimeWriter + CcsdsTimeProvider + 'static, apid: u16) -> Self { + let max_stamp_len = time_stamper.len_as_bytes(); + Self { + apid, + dest_id: 0, + time_stamper: Box::new(time_stamper), + step_field_width: size_of::() as u8, + failure_code_field_width: size_of::() as u8, + max_fail_data_len: 2 * size_of::(), + max_stamp_len, + } + } } impl VerificationReporter { + pub fn new(cfg: VerificationReporterCfg) -> Self { + Self { + apid: cfg.apid, + dest_id: cfg.dest_id, + msg_count: 0, + time_stamper: cfg.time_stamper, + time_stamp_buf: vec![0; cfg.max_stamp_len], + source_data_buf: vec![ + 0; + RequestId::SIZE_AS_BYTES + + cfg.step_field_width as usize + + cfg.failure_code_field_width as usize + + cfg.max_fail_data_len + ], + } + } + pub fn add_tc(&mut self, req_id: RequestId) -> VerificationToken { VerificationToken::::new(req_id) } @@ -48,7 +103,7 @@ impl VerificationReporter { sender: &mut impl VerificationSender, ) -> Result, SendStoredTmError> { let tm = self - .create_tm(1, 1) + .create_pus_verif_tm(1, 1, &token.req_id) .map_err(|e| SendStoredTmError::TimeStampError(e))?; sender.send_verification_tm(tm)?; self.msg_count += 1; @@ -58,8 +113,18 @@ impl VerificationReporter { }) } - pub fn acceptance_failure(self, _token: VerificationToken) { - unimplemented!(); + pub fn acceptance_failure( + mut self, + token: VerificationToken, + sender: &mut impl VerificationSender, + _failure_notice: &[u8], + ) -> Result<(), SendStoredTmError> { + let tm = self + .create_pus_verif_tm(1, 2, &token.req_id) + .map_err(|e| SendStoredTmError::TimeStampError(e))?; + sender.send_verification_tm(tm)?; + self.msg_count += 1; + Ok(()) } pub fn start_success( @@ -91,7 +156,14 @@ impl VerificationReporter { unimplemented!(); } - fn create_tm(&mut self, service: u8, subservice: u8) -> Result { + fn create_pus_verif_tm( + &mut self, + service: u8, + subservice: u8, + req_id: &RequestId, + ) -> Result { + let source_data_len = size_of::(); + req_id.to_bytes(&mut self.source_data_buf[0..source_data_len]); let mut sp_header = SpHeader::tm(self.apid, 0, 0).unwrap(); // I think it is okay to panic here. This error should never happen, I consider // this a configuration error. @@ -104,7 +176,12 @@ impl VerificationReporter { self.dest_id, &self.time_stamp_buf, ); - Ok(PusTm::new(&mut sp_header, tm_sec_header, None, true)) + Ok(PusTm::new( + &mut sp_header, + tm_sec_header, + Some(&self.source_data_buf[0..source_data_len]), + true, + )) } } @@ -128,7 +205,6 @@ impl VerificationToken { #[cfg(test)] mod tests { - use crate::pus::verification::VerificationToken; #[test] pub fn test_basic_type_state() {