use super::*;
use byteorder::{BigEndian, ByteOrder};
use crate::sp::services::service_1::RequestId;
use alloc::vec::Vec;
pub trait SpacePacketDataField {}
#[derive(Debug)]
pub struct SpacePacket<T: SpacePacketDataField> {
primary_header: PrimaryHeader,
data: T,
}
impl SpacePacketDataField for Vec<u8> {
}
impl<T: SpacePacketDataField> SpacePacket<T> {
pub fn set_ver_no(&mut self, ver_no: u8) -> Result<(), Error> {
if ver_no > (1 << PrimaryHeader::VER_NO_BITS) {
return Err(Error::InvalidVersionNo);
}
self.primary_header.ver_no = ver_no;
Ok(())
}
pub fn get_ver_no(&self) -> u8 {
self.primary_header.ver_no
}
pub fn set_type_flag(&mut self, type_flag: bool) {
self.primary_header.type_flag = type_flag;
}
pub fn get_type_flag(&self) -> bool {
self.primary_header.type_flag
}
pub fn set_sec_header_flag(&mut self, sec_header_flag: bool) {
self.primary_header.sec_header_flag = sec_header_flag;
}
pub fn get_sec_header_flag(&self) -> bool {
self.primary_header.sec_header_flag
}
pub fn set_apid(&mut self, apid: u16) -> Result<(), Error> {
if apid > (1 << PrimaryHeader::APID_BITS) {
return Err(Error::InvalidApid);
}
self.primary_header.apid = apid;
Ok(())
}
pub fn get_apid(&self) -> u16 {
self.primary_header.apid
}
pub fn set_seq_flags(&mut self, seq_1: bool, seq_2: bool) {
self.primary_header.seq_flags.0 = seq_1;
self.primary_header.seq_flags.1 = seq_2;
}
pub fn get_seq_flags(&self) -> (bool, bool) {
self.primary_header.seq_flags
}
pub fn set_packet_name(&mut self, packet_name: u16) -> Result<(), Error> {
if packet_name > (1 << PrimaryHeader::PACKET_NAME_BITS) {
return Err(Error::InvalidPacketName);
}
self.primary_header.packet_name = packet_name;
Ok(())
}
pub fn get_packet_name(&self) -> u16 {
self.primary_header.packet_name
}
pub fn get_data_len(&self) -> u16 {
self.primary_header.data_len
}
}
impl SpacePacket<Vec<u8>> {
pub fn from_bytes(packet: &[u8]) -> Result<Self, Error> {
if !(packet.len() > 6) {
return Err(Error::InvalidPacket);
};
let primary_header = PrimaryHeader::from_bytes(&packet[0..6])?;
if packet.len() - 6 != primary_header.data_len as usize + 1 {
return Err(Error::InvalidPacket);
}
let data: Vec<u8> = Vec::from(&packet[6..]);
Ok(SpacePacket {
primary_header,
data,
})
}
pub fn new(
ver_no: u8,
type_flag: bool,
sec_header_flag: bool,
apid: u16,
seq_flags: (bool, bool),
packet_name: u16,
data: Vec<u8>,
) -> Result<Self, Error> {
if ver_no > (1 << PrimaryHeader::VER_NO_BITS)
|| apid > (1 << PrimaryHeader::APID_BITS)
|| packet_name > (1 << PrimaryHeader::PACKET_NAME_BITS)
|| data.len() == 0
{
return Err(Error::InvalidPacket);
}
Ok(SpacePacket {
primary_header: PrimaryHeader::new(
ver_no,
type_flag,
sec_header_flag,
apid,
seq_flags,
packet_name,
data.len() as u16 - 1,
)?,
data,
})
}
}
impl core::fmt::Display for SpacePacket<Vec<u8>> {
fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result {
write!(f, "SpacePacket {{ \n")?;
write!(f, " {:?},\n", self.primary_header)?;
write!(f, " Data {:X?}\n", self.data)?;
write!(f, " PH_HEX {:X?}\n", self.primary_header.to_bytes())?;
write!(f, "}}}}")
}
}
#[derive(Debug)]
pub struct PrimaryHeader {
ver_no: u8,
type_flag: bool,
sec_header_flag: bool,
apid: u16,
seq_flags: (bool, bool),
packet_name: u16,
data_len: u16,
}
fn get_bits_u32(num: u32, start: u8, end: u8) -> u32 {
if start >= end {
panic!("get_bits_u32: invalid arguments");
}
let x = 32 - end;
let mut res = num >> x;
res = res & ((1 << (end - start)) - 1);
res
}
impl PrimaryHeader {
const VER_NO_POS: u8 = 0;
const VER_NO_BITS: u8 = 3;
const TYPE_FLAG_POS: u8 = 3;
const _TYPE_FLAG_BITS: u8 = 1;
const SEC_HEADER_FLAG_POS: u8 = 4;
const _SEC_HEADER_FLAG_BITS: u8 = 1;
const APID_POS: u8 = 5;
const APID_BITS: u8 = 11;
const SEQ_FLAGS_POS: u8 = 16;
const _SEQ_FLAGS_BITS: u8 = 2;
const PACKET_NAME_POS: u8 = 18;
const PACKET_NAME_BITS: u8 = 14;
const PACKET_DATA_LEN_POS: u8 = 32;
const _PACKET_DATA_LEN_BITS: u8 = 16;
pub const PH_LEN: usize = 6;
const VER_NO: u8 = 0;
pub fn new(
ver_no: u8,
type_flag: bool,
sec_header_flag: bool,
apid: u16,
seq_flags: (bool, bool),
packet_name: u16,
data_len: u16,
) -> Result<Self, Error> {
if ver_no > (1 << PrimaryHeader::VER_NO_BITS)
|| apid > (1 << PrimaryHeader::APID_BITS)
|| packet_name > (1 << PrimaryHeader::PACKET_NAME_BITS)
{
return Err(Error::InvalidPacket);
}
Ok(PrimaryHeader {
ver_no,
type_flag,
sec_header_flag,
apid,
seq_flags,
packet_name,
data_len,
})
}
pub fn from_bytes(packet: &[u8]) -> Result<Self, Error> {
if packet.len() != PrimaryHeader::PH_LEN {
return Err(Error::InvalidPacket);
}
let packet_int = BigEndian::read_u32(&packet[0..4]);
let ver_no_: u8 = get_bits_u32(
packet_int,
PrimaryHeader::VER_NO_POS,
PrimaryHeader::TYPE_FLAG_POS,
) as u8;
let type_flag_: bool = 1
== get_bits_u32(
packet_int,
PrimaryHeader::TYPE_FLAG_POS,
PrimaryHeader::SEC_HEADER_FLAG_POS,
);
let sec_header_flag_: bool = 1
== get_bits_u32(
packet_int,
PrimaryHeader::SEC_HEADER_FLAG_POS,
PrimaryHeader::APID_POS,
);
let apid_: u16 = get_bits_u32(
packet_int,
PrimaryHeader::APID_POS,
PrimaryHeader::SEQ_FLAGS_POS,
) as u16;
let seq_flags_: (bool, bool) = (
get_bits_u32(
packet_int,
PrimaryHeader::SEQ_FLAGS_POS,
PrimaryHeader::SEQ_FLAGS_POS + 1,
) == 1,
get_bits_u32(
packet_int,
PrimaryHeader::SEQ_FLAGS_POS + 1,
PrimaryHeader::PACKET_NAME_POS,
) == 1,
);
let packet_name_: u16 = get_bits_u32(
packet_int,
PrimaryHeader::PACKET_NAME_POS,
PrimaryHeader::PACKET_DATA_LEN_POS,
) as u16;
let data_len_: u16 = BigEndian::read_u16(&packet[4..6]);
if ver_no_ != 0 {
return Err(Error::InvalidVersionNo);
}
Ok(PrimaryHeader {
ver_no: ver_no_,
type_flag: type_flag_,
sec_header_flag: sec_header_flag_,
apid: apid_,
seq_flags: seq_flags_,
packet_name: packet_name_,
data_len: data_len_,
})
}
pub fn to_bytes(&self) -> [u8; PrimaryHeader::PH_LEN] {
let mut res: [u8; PrimaryHeader::PH_LEN] = [0; PrimaryHeader::PH_LEN];
let mut packet_part: u16 = self.ver_no as u16;
packet_part = packet_part << (16 - PrimaryHeader::TYPE_FLAG_POS);
if self.type_flag {
packet_part += 1 << (16 - PrimaryHeader::SEC_HEADER_FLAG_POS);
}
if self.sec_header_flag {
packet_part += 1 << (16 - PrimaryHeader::APID_POS);
}
packet_part += self.apid;
BigEndian::write_u16(&mut res[0..2], packet_part);
packet_part = self.packet_name;
if self.seq_flags.0 {
packet_part += 1 << (32 - (PrimaryHeader::SEQ_FLAGS_POS + 1))
}
if self.seq_flags.1 {
packet_part += 1 << (32 - (PrimaryHeader::PACKET_NAME_POS))
}
BigEndian::write_u16(&mut res[2..4], packet_part);
BigEndian::write_u16(&mut res[4..6], self.data_len);
res
}
pub fn get_data_len(&self) -> usize {
self.data_len as usize
}
}
#[cfg(test)]
mod tests;
#[derive(Debug)]
pub struct TxUserData<T> {
packet_error_control: u16,
data: T,
}
pub trait Request {
fn to_request(&self) -> RequestId;
}
impl<T: SpacePacketDataField> Request for SpacePacket<T> {
fn to_request(&self) -> RequestId {
RequestId {
ver_no: self.primary_header.ver_no,
packet_type: self.primary_header.type_flag,
sec_header_flag: self.primary_header.sec_header_flag,
apid: self.primary_header.apid,
seq_flags: self.primary_header.seq_flags,
packet_seq_count: self.primary_header.packet_name,
}
}
}
pub fn get_service_type(buf: &[u8]) -> Result<(u8, u8), ()> {
if buf.len() <= PrimaryHeader::PH_LEN + sp::tc::TcPacketHeader::TC_HEADER_LEN {
Err(())
} else {
Ok((
buf[PrimaryHeader::PH_LEN + 1],
buf[PrimaryHeader::PH_LEN + 2],
))
}
}
const PEC_LEN: usize = 2;
pub mod services;
pub mod tc;
pub mod tm;