sbf_tools/
header.rs

1//! SBF block header parsing
2//!
3//! Every SBF block starts with an 8-byte header:
4//! ```text
5//! [Sync: 2] [CRC: 2] [ID: 2] [Length: 2]
6//! ```
7//! Followed by optional TOW and WNc fields (6 bytes) in most blocks.
8
9use crate::crc::crc16_ccitt;
10use crate::error::{SbfError, SbfResult};
11
12/// SBF sync bytes
13pub const SBF_SYNC: [u8; 2] = [0x24, 0x40]; // "$@"
14
15/// Minimum block length (header only)
16pub const MIN_BLOCK_LENGTH: u16 = 8;
17
18/// SBF block header
19#[derive(Debug, Clone, Copy, PartialEq, Eq)]
20pub struct SbfHeader {
21    /// CRC-16 checksum
22    pub crc: u16,
23    /// Block ID (13 bits)
24    pub block_id: u16,
25    /// Block revision (3 bits)
26    pub block_rev: u8,
27    /// Total block length (including header)
28    pub length: u16,
29    /// Time of week in milliseconds (0xFFFFFFFF if not available)
30    pub tow_ms: u32,
31    /// GPS week number (0xFFFF if not available)
32    pub wnc: u16,
33}
34
35impl SbfHeader {
36    /// Parse header from bytes (starting after sync bytes)
37    ///
38    /// # Arguments
39    /// * `data` - Data starting from CRC field (after 0x24 0x40 sync)
40    ///
41    /// # Returns
42    /// Parsed header or error
43    pub fn parse(data: &[u8]) -> SbfResult<Self> {
44        if data.len() < 6 {
45            return Err(SbfError::IncompleteBlock {
46                needed: 6,
47                have: data.len(),
48            });
49        }
50
51        let crc = u16::from_le_bytes([data[0], data[1]]);
52        let id_rev = u16::from_le_bytes([data[2], data[3]]);
53        let length = u16::from_le_bytes([data[4], data[5]]);
54
55        let block_id = id_rev & 0x1FFF;
56        let block_rev = ((id_rev >> 13) & 0x07) as u8;
57
58        // Validate length
59        if length < MIN_BLOCK_LENGTH || (length & 0x03) != 0 {
60            return Err(SbfError::InvalidLength(length));
61        }
62
63        // Parse TOW and WNc if available (most blocks have these at offset 6-11)
64        let (tow_ms, wnc) = if data.len() >= 12 {
65            (
66                u32::from_le_bytes([data[6], data[7], data[8], data[9]]),
67                u16::from_le_bytes([data[10], data[11]]),
68            )
69        } else {
70            (0xFFFFFFFF, 0xFFFF)
71        };
72
73        Ok(Self {
74            crc,
75            block_id,
76            block_rev,
77            length,
78            tow_ms,
79            wnc,
80        })
81    }
82
83    /// Parse header from complete block data (including sync bytes)
84    ///
85    /// # Arguments
86    /// * `block_data` - Complete block data starting with sync bytes
87    ///
88    /// # Returns
89    /// Parsed header or error
90    pub fn parse_from_block(block_data: &[u8]) -> SbfResult<Self> {
91        if block_data.len() < 2 {
92            return Err(SbfError::IncompleteBlock {
93                needed: 2,
94                have: block_data.len(),
95            });
96        }
97
98        // Verify sync bytes
99        if block_data[0] != SBF_SYNC[0] || block_data[1] != SBF_SYNC[1] {
100            return Err(SbfError::InvalidSync);
101        }
102
103        Self::parse(&block_data[2..])
104    }
105
106    /// Validate CRC against block data
107    ///
108    /// # Arguments
109    /// * `block_data` - Complete block data starting with sync bytes
110    ///
111    /// # Returns
112    /// `Ok(())` if CRC is valid, `Err(SbfError::CrcMismatch)` otherwise
113    pub fn validate_crc(&self, block_data: &[u8]) -> SbfResult<()> {
114        let length = self.length as usize;
115        if block_data.len() < length {
116            return Err(SbfError::IncompleteBlock {
117                needed: length,
118                have: block_data.len(),
119            });
120        }
121
122        // CRC is calculated over ID + Length + Body (offset 4 to length)
123        let calculated_crc = crc16_ccitt(&block_data[4..length]);
124
125        if calculated_crc != self.crc {
126            return Err(SbfError::CrcMismatch {
127                expected: self.crc,
128                actual: calculated_crc,
129            });
130        }
131
132        Ok(())
133    }
134
135    /// Get TOW in seconds (scaled from milliseconds)
136    ///
137    /// Returns `None` if TOW is not available (0xFFFFFFFF)
138    pub fn tow_seconds(&self) -> Option<f64> {
139        if self.tow_ms == 0xFFFFFFFF {
140            None
141        } else {
142            Some(self.tow_ms as f64 * 0.001)
143        }
144    }
145
146    /// Get raw TOW in milliseconds
147    ///
148    /// Returns `None` if TOW is not available (0xFFFFFFFF)
149    pub fn tow_ms_raw(&self) -> Option<u32> {
150        if self.tow_ms == 0xFFFFFFFF {
151            None
152        } else {
153            Some(self.tow_ms)
154        }
155    }
156
157    /// Get week number
158    ///
159    /// Returns `None` if WNc is not available (0xFFFF)
160    pub fn week_number(&self) -> Option<u16> {
161        if self.wnc == 0xFFFF {
162            None
163        } else {
164            Some(self.wnc)
165        }
166    }
167
168    /// Check if time fields are valid
169    pub fn has_valid_time(&self) -> bool {
170        self.tow_ms != 0xFFFFFFFF && self.wnc != 0xFFFF
171    }
172
173    /// Get body offset (where block-specific data starts)
174    ///
175    /// Most blocks have TOW (4 bytes) + WNc (2 bytes) after the 8-byte header,
176    /// so body starts at offset 14 from block start, or offset 12 from CRC.
177    pub const fn body_offset() -> usize {
178        12 // CRC(2) + ID(2) + Length(2) + TOW(4) + WNc(2) = 12 bytes from start of CRC
179    }
180}
181
182impl std::fmt::Display for SbfHeader {
183    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
184        write!(
185            f,
186            "SbfHeader {{ id: {}, rev: {}, len: {}, tow: {}ms, wnc: {} }}",
187            self.block_id, self.block_rev, self.length, self.tow_ms, self.wnc
188        )
189    }
190}
191
192#[cfg(test)]
193mod tests {
194    use super::*;
195
196    #[test]
197    fn test_header_parse() {
198        // Minimal header data: CRC(2) + ID/Rev(2) + Length(2) + TOW(4) + WNc(2)
199        let data = [
200            0x00, 0x00, // CRC (placeholder)
201            0xAB, 0x0F, // ID=0x0FAB (4011), Rev=0
202            0x10, 0x00, // Length=16
203            0xE8, 0x03, 0x00, 0x00, // TOW=1000ms
204            0x64, 0x00, // WNc=100
205        ];
206
207        let header = SbfHeader::parse(&data).unwrap();
208        assert_eq!(header.block_id, 4011);
209        assert_eq!(header.block_rev, 0);
210        assert_eq!(header.length, 16);
211        assert_eq!(header.tow_ms, 1000);
212        assert_eq!(header.wnc, 100);
213    }
214
215    #[test]
216    fn test_header_id_rev_extraction() {
217        // ID=4007 (PVTGeodetic), Rev=2
218        // id_rev = (2 << 13) | 4007 = 0x4FA7
219        let data = [
220            0x00, 0x00, // CRC
221            0xA7, 0x4F, // ID/Rev: 0x4FA7 -> ID=4007, Rev=2
222            0x10, 0x00, // Length=16
223            0x00, 0x00, 0x00, 0x00, // TOW
224            0x00, 0x00, // WNc
225        ];
226
227        let header = SbfHeader::parse(&data).unwrap();
228        assert_eq!(header.block_id, 4007);
229        assert_eq!(header.block_rev, 2);
230    }
231
232    #[test]
233    fn test_header_invalid_length() {
234        // Length not divisible by 4
235        let data = [
236            0x00, 0x00, // CRC
237            0xAB, 0x0F, // ID/Rev
238            0x09, 0x00, // Length=9 (invalid)
239            0x00, 0x00, 0x00, 0x00, // TOW
240            0x00, 0x00, // WNc
241        ];
242
243        let result = SbfHeader::parse(&data);
244        assert!(matches!(result, Err(SbfError::InvalidLength(9))));
245    }
246
247    #[test]
248    fn test_header_too_short() {
249        let data = [0x00, 0x00, 0x00];
250
251        let result = SbfHeader::parse(&data);
252        assert!(matches!(result, Err(SbfError::IncompleteBlock { .. })));
253    }
254
255    #[test]
256    fn test_tow_seconds() {
257        let header = SbfHeader {
258            crc: 0,
259            block_id: 4007,
260            block_rev: 0,
261            length: 16,
262            tow_ms: 1500,
263            wnc: 100,
264        };
265
266        assert_eq!(header.tow_seconds(), Some(1.5));
267
268        let header_no_tow = SbfHeader {
269            tow_ms: 0xFFFFFFFF,
270            ..header
271        };
272        assert_eq!(header_no_tow.tow_seconds(), None);
273    }
274
275    #[test]
276    fn test_parse_from_block_with_sync() {
277        let block = [
278            0x24, 0x40, // Sync
279            0x00, 0x00, // CRC
280            0xAB, 0x0F, // ID/Rev
281            0x10, 0x00, // Length=16
282            0x00, 0x00, 0x00, 0x00, // TOW
283            0x00, 0x00, // WNc
284        ];
285
286        let header = SbfHeader::parse_from_block(&block).unwrap();
287        assert_eq!(header.block_id, 4011);
288    }
289
290    #[test]
291    fn test_parse_from_block_invalid_sync() {
292        let block = [
293            0x00, 0x00, // Bad sync
294            0x00, 0x00, // CRC
295            0xAB, 0x0F, // ID/Rev
296            0x10, 0x00, // Length
297            0x00, 0x00, 0x00, 0x00, // TOW
298            0x00, 0x00, // WNc
299        ];
300
301        let result = SbfHeader::parse_from_block(&block);
302        assert!(matches!(result, Err(SbfError::InvalidSync)));
303    }
304}
sbf_tools/header.rs

sbf_tools/
header.rs
