modbus_buffer/
lib.rs

1#![no_std]
2
3#[derive(Debug)]
4pub struct ModbusBuffer<const CAPACITY: usize> {
5    ring_buffer: [Option<u8>;CAPACITY],
6    /// Head of data (Oldest Byte)
7    head: usize,
8    /// Tail of data (Newest Byte)
9    tail: usize,
10    size: usize,
11    /// Min frame length to be detected (CRC not included) (Default: 3)
12    min_frame_len: usize,
13    /// Max frame length to be detected (CRC not included) Default: CAPACITY
14    max_frame_len: usize,
15    /// Whether the buffer should be overwritten if overflowed, or panic (Default: `true`).
16    overwrite: bool,
17}
18impl<const CAPACITY: usize> ModbusBuffer<CAPACITY> {
19
20    /// Creates a new `ModbusBuffer` with specified constants.
21    #[allow(clippy::new_without_default)]
22    pub fn new() -> Self {
23        
24        assert!(CAPACITY > 4);
25        ModbusBuffer {
26            ring_buffer: [None; CAPACITY],
27            head: 0,
28            tail: 0,
29            size: 0,
30            min_frame_len: 3,
31            max_frame_len: CAPACITY,
32            overwrite: true,
33        }
34    }
35
36    /// Sets the minimum frame length required to detect a Modbus frame, excluding CRC.
37    pub fn min_frame_len(mut self, min_frame_len: usize) -> Self {
38        self.min_frame_len = min_frame_len;
39        self
40    }
41
42    /// Sets the maximum frame length that can be detected, excluding CRC.
43    pub fn max_frame_len(mut self, max_frame_len: usize) -> Self {
44        self.max_frame_len = max_frame_len;
45        self
46    }
47
48    /// Configures whether to overwrite old data if the buffer is full or to panic.
49    pub fn overwrite(mut self, overwrite: bool) -> Self {
50            self.overwrite = overwrite;
51            self
52        }
53
54    /// Adds an item to the buffer, handling overflow based on the `overwrite` flag.
55
56    pub fn push(&mut self, item: u8) {
57        if self.size == CAPACITY {
58            // Buffer is full
59            if self.overwrite {
60                self.ring_buffer[self.head] = Some(item);
61                self.head = (self.head + 1) % CAPACITY;
62                self.tail = (self.tail + 1) % CAPACITY;
63            } else {
64                panic!("ModbusBuffer exceed its capacity!");
65            }
66
67        } else {
68            // Buffer has space
69            self.ring_buffer[self.tail] = Some(item);
70            self.tail = (self.tail + 1) % CAPACITY;
71            self.size += 1;
72        }
73    }
74
75    /// Removes and returns the oldest element from the buffer if available.
76    pub fn pop(&mut self) -> Option<u8> {
77        if self.size == 0 {
78            None
79        } else {
80            let item = self.ring_buffer[self.head];
81            self.ring_buffer[self.head] = None;
82            self.head = (self.head + 1) % CAPACITY;
83            self.size -= 1;
84            item
85        }
86    }
87
88    /// Returns the current number of elements in the buffer.
89    pub fn len(&self) -> usize {
90        self.size
91    }
92
93    /// Returns true if the buffer is empty.
94    pub fn is_empty(&self) -> bool {
95        self.size == 0
96    }
97
98    /// Returns true if the buffer is full.
99    pub fn is_full(&self) -> bool {
100        self.size == CAPACITY
101    }
102
103    /// Copies the current data from the buffer into the provided output buffer and returns the size.
104    fn frame(&self, output_buffer: &mut [u8;CAPACITY]) -> Option<usize> {
105        let mut index = 0;
106
107        if self.size > 0 {
108            if self.head < self.tail {
109                // No wrap-around, direct slice
110                self.ring_buffer[self.head..self.tail]
111                    .iter()
112                    .for_each(|d| {
113                        output_buffer[index] = d.unwrap();
114                        index += 1;
115                    });
116            } else {
117                // Wrap-around, handle two parts
118                // First part from head to end of buffer
119                self.ring_buffer[self.head..CAPACITY]
120                    .iter()
121                    .for_each(|d| {
122                        output_buffer[index] = d.unwrap();
123                        index += 1;
124                    });
125
126                // Second part from start of buffer to tail
127                self.ring_buffer[0..self.tail]
128                    .iter()
129                    .for_each(|d| {
130                        output_buffer[index] = d.unwrap();
131                        index += 1;
132                    });
133            }
134        }
135        Some(self.size)
136    }
137
138    /// Computes the CRC16 for the provided data array.
139    fn crc16(data: &[u8]) -> u16 {
140        let mut crc = 0xFFFF;
141        for x in data {
142            crc ^= u16::from(*x);
143            for _ in 0..8 {
144                // if we followed clippy's suggestion to move out the crc >>= 1, the condition may not be met any more
145                // the recommended action therefore makes no sense and it is better to allow this lint
146                #[allow(clippy::branches_sharing_code)]
147                if (crc & 0x0001) != 0 {
148                    crc >>= 1;
149                    crc ^= 0xA001;
150                } else {
151                    crc >>= 1;
152                }
153            }
154        }
155        crc << 8 | crc >> 8
156    }
157
158    /// Verifies the CRC of the provided frame.
159    fn check_crc(frame: &[u8]) -> bool {
160        if frame.len() > 4 {
161            let crc = Self::crc16(&frame[..frame.len()-2]);
162            let expected_crc = [((crc & 0xff00) >> 8) as u8, (crc & 0x00ff) as u8];
163            expected_crc == frame[frame.len()-2..]
164        } else {
165            false
166        }
167
168    }
169
170    /// Tries to find a valid Modbus frame in the buffer.
171    fn try_decode_buffer(&self, buffer: &[u8]) -> Option<(usize, usize)> {
172        let mut window_size = self.min_frame_len + 2;
173        if buffer.len() < window_size {
174            return None
175        }
176
177        while window_size <= buffer.len() {
178            for i in 0..=buffer.len() - window_size {
179                // Forward direction
180                if Self::check_crc(&buffer[i..i + window_size]) {
181                    return Some((i, i + window_size - 2));
182                }
183
184                if buffer.len() == window_size {
185                    return None;
186                }
187                // Reverse direction
188                let j = buffer.len() - i - window_size;
189                if Self::check_crc(&buffer[j..j + window_size]) {
190                    return Some((j, j + window_size - 2));
191                }
192            }
193            window_size += 1;
194        }
195        None
196    }
197
198    /// Attempts to decode a Modbus frame from the internal buffer and copies it into the provided buffer if successful.
199    pub fn try_decode_frame(&mut self, buffer: &mut [u8;CAPACITY]) -> Option<usize> {
200        if self.size == 0 || self.size < self.min_frame_len {
201            return None
202        }
203
204        let mut frame = [0u8;CAPACITY];
205        // copy data into `frame`
206        let len = self.frame(&mut frame).expect("Should have a frame");
207
208        if let Some((head, tail)) = self.try_decode_buffer(&frame[..len]) {
209            // if CRC match
210            
211            ///  println! is std, not available w/ `#![no_std]` flag
212            // println!(" ");
213            // println!("---------------- CRC Match!! ------------------------");
214            // println!("len={}, tail={}, head={},", len, tail, head);
215            // println!("frame={:?}", frame);
216            // println!("self={:?}", self);
217            // println!("---------------- CRC Match end ------------------------");
218
219            // remove decoded data
220            // let len_to_remove = len - tail + 2 ;
221            let len_to_remove = tail + 2 ;
222            for _ in 0..len_to_remove {
223                self.pop();
224            }
225
226            // output frame length
227            let frame_length = tail - head;
228
229            // copy data
230            buffer[..frame_length].copy_from_slice(&frame[head..tail]);
231
232            // return frame length
233            Some(frame_length)
234        } else {
235            None
236        }
237    }
238}
239
240
241#[cfg(test)]
242mod tests {
243    use super::*;
244
245    #[test]
246    #[should_panic]
247    fn test_overflow_should_panic() {
248        let mut buff = ModbusBuffer::<8>::new()
249            .overwrite(false);
250
251        buff.push(0x00);
252        buff.push(0x01);
253        buff.push(0x02);
254        buff.push(0x03);
255        buff.push(0x04);
256        buff.push(0x05);
257        buff.push(0x06);
258        buff.push(0x07);
259
260        // Should panic here
261        buff.push(0x08);
262
263    }
264
265    #[test]
266    fn test_overflow_should_not_panic() {
267        let mut buff = ModbusBuffer::<8>::new();
268
269        buff.push(0x00);
270        buff.push(0x01);
271        buff.push(0x02);
272
273        let mut buffer = [0u8;8];
274        let len = buff.frame(&mut buffer);
275        assert_eq!(len, Some(3usize));
276        assert_eq!(buffer, [0u8, 1, 2, 0, 0, 0, 0, 0]);
277
278        buff.push(0x03);
279        buff.push(0x04);
280        buff.push(0x05);
281        buff.push(0x06);
282        buff.push(0x07);
283
284        let len = buff.frame(&mut buffer);
285        assert_eq!(len, Some(8usize));
286        assert_eq!(buffer, [0u8, 1, 2, 3, 4, 5, 6, 7]);
287
288        // overflow + overwrite here
289        buff.push(0x08);
290
291        let len = buff.frame(&mut buffer);
292        assert_eq!(len, Some(8usize));
293        assert_eq!(buffer, [1u8, 2, 3, 4, 5, 6, 7, 8]);
294
295    }
296
297    #[test]
298    fn test_receive_request() {
299        let mut buff = ModbusBuffer::<10>::new();
300
301        buff.push(0x12);    // slave addr
302        buff.push(0x06);    // function code
303        buff.push(0x22);    // addr
304        buff.push(0x22);    // addr
305        buff.push(0xAB);    // value
306        buff.push(0xCD);    // value
307        buff.push(0x9F);    // crc
308        buff.push(0xBE);    // crc
309
310        let mut output = [0u8;10];
311        let len = buff.try_decode_frame(&mut output);
312        assert_eq!(len, Some(6));
313        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
314
315    }
316
317    #[test]
318    fn test_receive_multiple_request() {
319        let mut buff = ModbusBuffer::<10>::new();
320        let mut output = [0u8;10];
321
322        buff.push(0x12);    // slave addr
323        buff.push(0x06);    // function code
324
325        // request is not yet complete
326        let len = buff.try_decode_frame(&mut output);
327        assert_eq!(len, None);
328
329        buff.push(0x22);    // addr
330        buff.push(0x22);    // addr
331
332        // request is not yet complete
333        let len = buff.try_decode_frame(&mut output);
334        assert_eq!(len, None);
335
336        buff.push(0xAB);    // value
337        buff.push(0xCD);    // value
338
339        // request is not yet complete
340        let len = buff.try_decode_frame(&mut output);
341        assert_eq!(len, None);
342
343        buff.push(0x9F);    // crc
344        buff.push(0xBE);    // crc
345
346        // now request is complete
347        let len = buff.try_decode_frame(&mut output);
348        assert_eq!(len, Some(6));
349
350        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
351
352        // buffer should be empty
353        let len = buff.try_decode_frame(&mut output);
354        assert_eq!(len, None);
355
356        // new request
357        buff.push(0x12);    // slave addr
358        buff.push(0x06);    // function code
359        buff.push(0x22);    // addr
360        buff.push(0x22);    // addr
361        buff.push(0xAB);    // value
362        buff.push(0xCD);    // value
363        buff.push(0x9F);    // crc
364        buff.push(0xBE);    // crc
365
366        let mut output = [0u8;10];
367        let len = buff.try_decode_frame(&mut output);
368        assert_eq!(len, Some(6));
369        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
370
371        // new request
372        buff.push(0x12);    // slave addr
373        buff.push(0x06);    // function code
374        buff.push(0x22);    // addr
375        buff.push(0x22);    // addr
376        buff.push(0xAB);    // value
377        buff.push(0xCD);    // value
378        buff.push(0x9F);    // crc
379        buff.push(0xBE);    // crc
380
381        let mut output = [0u8;10];
382        let len = buff.try_decode_frame(&mut output);
383        assert_eq!(len, Some(6));
384        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
385
386        // new request
387        buff.push(0x12);    // slave addr
388        buff.push(0x06);    // function code
389        buff.push(0x22);    // addr
390        buff.push(0x22);    // addr
391        buff.push(0xAB);    // value
392        buff.push(0xCD);    // value
393        buff.push(0x9F);    // crc
394        buff.push(0xBE);    // crc
395
396        let mut output = [0u8;10];
397        let len = buff.try_decode_frame(&mut output);
398        assert_eq!(len, Some(6));
399        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
400
401        // new request
402        buff.push(0x12);    // slave addr
403        buff.push(0x06);    // function code
404        buff.push(0x22);    // addr
405        buff.push(0x22);    // addr
406        buff.push(0xAB);    // value
407        buff.push(0xCD);    // value
408        buff.push(0x9F);    // crc
409        buff.push(0xBE);    // crc
410
411        let mut output = [0u8;10];
412        let len = buff.try_decode_frame(&mut output);
413        assert_eq!(len, Some(6));
414        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
415
416    }
417
418    #[test]
419    fn test_noise_on_head() {
420        let mut buff = ModbusBuffer::<10>::new();
421
422        buff.push(0x01);    // noise
423        assert_eq!(buff.len(), 1usize);
424        buff.push(0x02);    // noise
425        assert_eq!(buff.len(), 2usize);
426        buff.push(0x03);    // noise
427        assert_eq!(buff.len(), 3usize);
428        buff.push(0x01);    // noise
429        assert_eq!(buff.len(), 4usize);
430        buff.push(0x02);    // noise
431        assert_eq!(buff.len(), 5usize);
432        buff.push(0x03);    // noise
433        assert_eq!(buff.len(), 6usize);
434
435        buff.push(0x12);    // slave addr
436        buff.push(0x06);    // function code
437        buff.push(0x22);    // addr
438        buff.push(0x22);    // addr
439        buff.push(0xAB);    // value
440        buff.push(0xCD);    // value
441        buff.push(0x9F);    // crc
442        buff.push(0xBE);    // crc
443
444        // buff.len() should be clamp to max capacity
445        assert_eq!(buff.len(), 10usize);
446
447        let mut output = [0u8;10];
448        let len = buff.try_decode_frame(&mut output);
449        assert_eq!(len, Some(6));
450        assert_eq!(output, [0x12u8, 0x06, 0x22, 0x22, 0xAB, 0xCD, 0, 0, 0, 0]);
451        
452        assert_eq!(buff.len(), 0usize);
453
454    }
455
456    #[test]
457    fn test_data_remain_on_tail() {
458        let mut buff = ModbusBuffer::<20>::new();
459        
460        // first request
461        buff.push(0x12);    // slave addr
462        buff.push(0x06);    // function code
463        buff.push(0x22);    // addr
464        buff.push(0x22);    // addr
465        buff.push(0xAB);    // value
466        buff.push(0xCD);    // value
467        buff.push(0x9F);    // crc
468        buff.push(0xBE);    // crc
469
470        // second request
471        buff.push(0x12);    // slave addr
472        buff.push(0x06);    // function code
473        buff.push(0x22);    // addr
474        buff.push(0x22);    // addr
475        buff.push(0xAB);    // value
476        buff.push(0xCD);    // value
477        buff.push(0x9F);    // crc
478        buff.push(0xBE);    // crc
479        
480        let mut output = [0u8;20];
481        
482        // decode the first request but do not remove the second request on tail
483        let len = buff.try_decode_frame(&mut output);
484        assert_eq!(len, Some(6));
485        assert_eq!(buff.len(), 8usize);
486
487        // decode second request
488        let len = buff.try_decode_frame(&mut output);
489        assert_eq!(len, Some(6));
490        assert_eq!(buff.len(), 0usize);
491
492    }
493
494    #[test]
495    fn test_data_remain_on_tail_with_overlap() {
496        // same than previous test but buffer cannot contain 2 request
497        
498        let mut buff = ModbusBuffer::<15>::new();
499
500        // first request
501        buff.push(0x12);    // slave addr
502        buff.push(0x06);    // function code
503        buff.push(0x22);    // addr
504        buff.push(0x22);    // addr
505        buff.push(0xAB);    // value
506        buff.push(0xCD);    // value
507        buff.push(0x9F);    // crc
508        buff.push(0xBE);    // crc
509
510        // second request
511        buff.push(0x12);    // slave addr
512        buff.push(0x06);    // function code
513        buff.push(0x22);    // addr
514        buff.push(0x22);    // addr
515        buff.push(0xAB);    // value
516        buff.push(0xCD);    // value
517        buff.push(0x9F);    // crc
518        buff.push(0xBE);    // crc
519
520        let mut output = [0u8;15];
521
522        // decode the second request, first one have been partially overwritten
523        let len = buff.try_decode_frame(&mut output);
524        assert_eq!(len, Some(6));
525        assert_eq!(buff.len(), 0usize);
526    }
527    
528    #[test]
529    fn buffer_empty() {
530        let mut buff = ModbusBuffer::<10>::new();
531        
532        assert_eq!(buff.len(), 0);
533        let p = buff.pop();
534        assert_eq!(p, None);
535        let mut temp = [0u8;10];
536        let q = buff.try_decode_frame(&mut temp);
537        assert_eq!(q, None);
538    }
539    
540}
modbus_buffer/lib.rs

modbus_buffer/
lib.rs
