1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
use crate::cpu::{Body, GlobalHeader};
#[derive(Clone)]
pub struct TxDatagram {
data: Vec<u8>,
size: usize,
pub num_bodies: usize,
}
impl TxDatagram {
pub fn new(size: usize) -> Self {
Self {
data: vec![
0x00;
std::mem::size_of::<GlobalHeader>() + std::mem::size_of::<Body>() * size
],
size,
num_bodies: size,
}
}
pub fn size(&self) -> usize {
std::mem::size_of::<GlobalHeader>() + std::mem::size_of::<Body>() * self.num_bodies
}
pub fn data(&self) -> &[u8] {
&self.data
}
pub fn header(&self) -> &GlobalHeader {
unsafe {
(self.data.as_ptr() as *const GlobalHeader)
.as_ref()
.unwrap()
}
}
pub fn header_mut(&mut self) -> &mut GlobalHeader {
unsafe {
(self.data.as_mut_ptr() as *mut GlobalHeader)
.as_mut()
.unwrap()
}
}
pub fn body(&self) -> &[Body] {
unsafe {
let ptr = self.data.as_ptr().add(std::mem::size_of::<GlobalHeader>()) as *const Body;
std::slice::from_raw_parts(ptr, self.size)
}
}
pub fn body_mut(&mut self) -> &mut [Body] {
unsafe {
let ptr = self
.data
.as_mut_ptr()
.add(std::mem::size_of::<GlobalHeader>()) as *mut Body;
std::slice::from_raw_parts_mut(ptr, self.size)
}
}
pub fn copy_from(&mut self, src: &TxDatagram) {
self.data.copy_from_slice(&src.data);
}
}
#[derive(Clone, Copy)]
#[repr(C)]
pub struct RxMessage {
pub ack: u8,
pub msg_id: u8,
}
impl RxMessage {
pub fn new() -> Self {
Self { ack: 0, msg_id: 0 }
}
}
impl Default for RxMessage {
fn default() -> Self {
Self::new()
}
}
pub struct RxDatagram {
data: Vec<RxMessage>,
}
impl RxDatagram {
pub fn new(size: usize) -> Self {
Self {
data: vec![RxMessage::default(); size],
}
}
pub fn copy_from(&mut self, src: &RxDatagram) {
self.data.copy_from_slice(&src.data);
}
pub fn messages(&self) -> &[RxMessage] {
&self.data
}
pub fn messages_mut(&mut self) -> &mut [RxMessage] {
&mut self.data
}
}
pub fn is_msg_processed(msg_id: u8, rx: &RxDatagram) -> bool {
rx.data.iter().all(|msg| msg.msg_id == msg_id)
}
