continuing obsw example

fsrc-core/src/hal/host/udp_server.rs

@@ -1,6 +1,6 @@
-use crate::hal::host::udp_server::ReceiveResult::{IoError, ReceiverError};
 use crate::tmtc::ReceivesTc;
 use std::boxed::Box;
+use std::io::ErrorKind;
 use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
 use std::vec;
 use std::vec::Vec;
@@ -14,7 +14,8 @@ pub struct UdpTcServer<E> {
 
 #[derive(Debug)]
 pub enum ReceiveResult<E> {
-    IoError(std::io::Error),
+    WouldBlock,
+    OtherIoError(std::io::Error),
     ReceiverError(E),
 }
 
@@ -24,23 +25,36 @@ impl<E> UdpTcServer<E> {
         max_recv_size: usize,
         tc_receiver: Box<dyn ReceivesTc<Error = E>>,
     ) -> Result<Self, std::io::Error> {
-        Ok(Self {
+        let server = Self {
             socket: UdpSocket::bind(addr)?,
             recv_buf: vec![0; max_recv_size],
             sender_addr: None,
             tc_receiver,
-        })
+        };
+        server
+            .socket
+            .set_nonblocking(true)
+            .expect("Setting server non blocking failed");
+        Ok(server)
     }
 
-    pub fn recv_tc(&mut self) -> Result<(usize, SocketAddr), ReceiveResult<E>> {
+    pub fn try_recv_tc(&mut self) -> Result<(usize, SocketAddr), ReceiveResult<E>> {
-        let res = self
+        // .map_err(|e| IoError(e))?;
-            .socket
+        let res = match self.socket.recv_from(&mut self.recv_buf) {
-            .recv_from(&mut self.recv_buf)
+            Ok(res) => res,
-            .map_err(|e| IoError(e))?;
+            Err(e) => {
-        self.sender_addr = Some(res.1);
+                if e.kind() != ErrorKind::WouldBlock {
+                    return Err(ReceiveResult::WouldBlock);
+                } else {
+                    return Err(ReceiveResult::OtherIoError(e));
+                }
+            }
+        };
+        let (num_bytes, from) = res;
+        self.sender_addr = Some(from);
         self.tc_receiver
-            .pass_tc(&self.recv_buf[0..res.0])
+            .pass_tc(&self.recv_buf[0..num_bytes])
-            .map_err(|e| ReceiverError(e))?;
+            .map_err(|e| ReceiveResult::ReceiverError(e))?;
         Ok(res)
     }
 }

fsrc-example/src/bin/obsw.rs

@@ -1,17 +1,20 @@
 use fsrc_core::hal::host::udp_server::{ReceiveResult, UdpTcServer};
+use fsrc_core::pool::{LocalPool, PoolCfg, StoreAddr};
 use fsrc_core::tmtc::{
     CcsdsDistributor, CcsdsError, CcsdsPacketHandler, PusDistributor, PusServiceProvider,
     ReceivesCcsdsTc,
 };
 use fsrc_example::{OBSW_SERVER_ADDR, SERVER_PORT};
 use spacepackets::tc::PusTc;
+use spacepackets::time::{CdsShortTimeProvider, TimeWriter};
+use spacepackets::tm::{PusTm, PusTmSecondaryHeader};
 use spacepackets::{CcsdsPacket, SpHeader};
 use std::net::{IpAddr, SocketAddr};
+use std::sync::{mpsc, Arc, Mutex};
+use std::thread;
 
 const PUS_APID: u16 = 0x02;
 
-struct PusReceiver {}
-
 struct CcsdsReceiver {
     pus_handler: PusDistributor<()>,
 }
@@ -46,6 +49,14 @@ impl CcsdsPacketHandler for CcsdsReceiver {
     }
 }
 
+unsafe impl Send for CcsdsReceiver {}
+
+struct PusReceiver {
+    tm_apid: u16,
+    tm_tx: mpsc::Sender<StoreAddr>,
+    tm_store: Arc<Mutex<TmStore>>,
+}
+
 impl PusServiceProvider for PusReceiver {
     type Error = ();
 
@@ -59,38 +70,120 @@ impl PusServiceProvider for PusReceiver {
             println!("Received PUS ping command");
             let raw_data = pus_tc.raw().expect("Could not retrieve raw data");
             println!("Raw data: 0x{raw_data:x?}");
+            let mut reply_header = SpHeader::tm(self.tm_apid, 0, 0).unwrap();
+            let time_stamp = CdsShortTimeProvider::from_now().unwrap();
+            let mut timestamp_buf = [0; 10];
+            time_stamp.write_to_bytes(&mut timestamp_buf).unwrap();
+            let tc_header = PusTmSecondaryHeader::new_simple(17, 2, &timestamp_buf);
+            let ping_reply = PusTm::new(&mut reply_header, tc_header, None, true);
+            let addr = self
+                .tm_store
+                .lock()
+                .expect("Locking TM store failed")
+                .add_pus_tm(&ping_reply);
+            self.tm_tx
+                .send(addr)
+                .expect("Sending TM to TM funnel failed");
         }
         Ok(())
     }
 }
 
-fn main() {
+struct TmStore {
-    let addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
+    pool: LocalPool,
-    let pus_receiver = PusReceiver {};
+}
-    let pus_distributor = PusDistributor::new(Box::new(pus_receiver));
+
-    let ccsds_receiver = CcsdsReceiver {
+impl TmStore {
-        pus_handler: pus_distributor,
+    fn add_pus_tm(&mut self, pus_tm: &PusTm) -> StoreAddr {
-    };
+        let (addr, mut buf) = self
-    let ccsds_distributor = CcsdsDistributor::new(Box::new(ccsds_receiver));
+            .pool
-    let mut udp_tmtc_server = UdpTcServer::new(addr, 2048, Box::new(ccsds_distributor))
+            .free_element(pus_tm.len_packed())
-        .expect("Creating UDP TMTC server failed");
+            .expect("Store error");
-    loop {
+        pus_tm
-        let res = udp_tmtc_server.recv_tc();
+            .write_to(&mut buf)
-        match res {
+            .expect("Writing PUS TM to store failed");
-            Ok(_) => (),
+        addr
-            Err(e) => match e {
-                ReceiveResult::ReceiverError(e) => match e {
-                    CcsdsError::PacketError(e) => {
-                        println!("Got packet error: {e:?}");
-                    }
-                    CcsdsError::CustomError(_) => {
-                        println!("Unknown receiver error")
-                    }
-                },
-                ReceiveResult::IoError(e) => {
-                    println!("IO error {e}");
-                }
-            },
-        }
     }
 }
+
+struct TmFunnel {
+    tm_funnel_rx: mpsc::Receiver<StoreAddr>,
+    tm_server_tx: mpsc::Sender<StoreAddr>,
+}
+
+struct UdpTmtcServer {
+    udp_tc_server: UdpTcServer<CcsdsError<()>>,
+    tm_rx: mpsc::Receiver<StoreAddr>,
+    tm_store: Arc<Mutex<TmStore>>,
+}
+
+unsafe impl Send for UdpTmtcServer {}
+
+fn main() {
+    let pool_cfg = PoolCfg::new(vec![(8, 32), (4, 64), (2, 128)]);
+    let tm_pool = LocalPool::new(pool_cfg);
+    let tm_store = Arc::new(Mutex::new(TmStore { pool: tm_pool }));
+    let addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
+    let (tm_creator_tx, tm_funnel_rx) = mpsc::channel();
+    let (tm_server_tx, tm_server_rx) = mpsc::channel();
+
+    let jh0 = thread::spawn(move || {
+        let pus_receiver = PusReceiver {
+            tm_apid: PUS_APID,
+            tm_tx: tm_creator_tx.clone(),
+            tm_store: tm_store.clone(),
+        };
+        let pus_distributor = PusDistributor::new(Box::new(pus_receiver));
+        let ccsds_receiver = CcsdsReceiver {
+            pus_handler: pus_distributor,
+        };
+        let ccsds_distributor = CcsdsDistributor::new(Box::new(ccsds_receiver));
+        let udp_tc_server = UdpTcServer::new(addr, 2048, Box::new(ccsds_distributor))
+            .expect("Creating UDP TMTC server failed");
+        let mut udp_tmtc_server = UdpTmtcServer {
+            udp_tc_server,
+            tm_rx: tm_server_rx,
+            tm_store: tm_store.clone(),
+        };
+        loop {
+            let res = udp_tmtc_server.udp_tc_server.try_recv_tc();
+            match res {
+                Ok(_) => (),
+                Err(e) => match e {
+                    ReceiveResult::ReceiverError(e) => match e {
+                        CcsdsError::PacketError(e) => {
+                            println!("Got packet error: {e:?}");
+                        }
+                        CcsdsError::CustomError(_) => {
+                            println!("Unknown receiver error")
+                        }
+                    },
+                    ReceiveResult::OtherIoError(e) => {
+                        println!("IO error {e}");
+                    }
+                    ReceiveResult::WouldBlock => {
+                        // TODO: Send TM Here
+                    }
+                },
+            }
+        }
+    });
+    let jh1 = thread::spawn(move || {
+        let tm_funnel = TmFunnel {
+            tm_server_tx,
+            tm_funnel_rx,
+        };
+        loop {
+            let res = tm_funnel.tm_funnel_rx.recv();
+            if res.is_ok() {
+                let addr = res.unwrap();
+                tm_funnel
+                    .tm_server_tx
+                    .send(addr)
+                    .expect("Sending TM to server failed");
+            }
+        }
+    });
+    jh0.join().expect("Joining UDP TMTC server thread failed");
+    jh1.join().expect("Joining TM Funnel thread failed");
+}