From a34497bf3ccdf34f3a2d09671f95127cce624795 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 3 Sep 2022 17:09:36 +0200
Subject: [PATCH] conitnued verif reporter

---
 fsrc-core/src/pus/mod.rs          |  2 +-
 fsrc-core/src/pus/verification.rs | 94 ++++++++++++++++++++++++++++---
 2 files changed, 86 insertions(+), 10 deletions(-)

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<E> {
     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::<u32>();
+
+    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<E> {
 }
 
 pub struct VerificationReporter {
-    apid: u16,
+    pub apid: u16,
+    pub dest_id: u16,
     msg_count: u16,
-    dest_id: u16,
     time_stamper: Box<dyn TimeWriter>,
     time_stamp_buf: Vec<u8>,
+    source_data_buf: Vec<u8>,
+}
+
+pub struct VerificationReporterCfg {
+    pub apid: u16,
+    pub dest_id: u16,
+    pub time_stamper: Box<dyn TimeWriter>,
+    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::<u8>() as u8,
+            failure_code_field_width: size_of::<u16>() as u8,
+            max_fail_data_len: 2 * size_of::<u32>(),
+            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<StateNone> {
         VerificationToken::<StateNone>::new(req_id)
     }
@@ -48,7 +103,7 @@ impl VerificationReporter {
         sender: &mut impl VerificationSender<E>,
     ) -> Result<VerificationToken<StateAccepted>, SendStoredTmError<E>> {
         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<StateNone>) {
-        unimplemented!();
+    pub fn acceptance_failure<E>(
+        mut self,
+        token: VerificationToken<StateNone>,
+        sender: &mut impl VerificationSender<E>,
+        _failure_notice: &[u8],
+    ) -> Result<(), SendStoredTmError<E>> {
+        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<PusTm, TimestampError> {
+    fn create_pus_verif_tm(
+        &mut self,
+        service: u8,
+        subservice: u8,
+        req_id: &RequestId,
+    ) -> Result<PusTm, TimestampError> {
+        let source_data_len = size_of::<u32>();
+        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<STATE> VerificationToken<STATE> {
 
 #[cfg(test)]
 mod tests {
-    use crate::pus::verification::VerificationToken;
 
     #[test]
     pub fn test_basic_type_state() {