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
127
128
129
130

mod packets;
mod types;

use crate::error::MemWriterError;
pub use packets::*;
pub use types::*;

/// Information about concrete UBX protocol's packet
pub trait UbxPacketMeta {
    const CLASS: u8;
    const ID: u8;
    const FIXED_PAYLOAD_LEN: Option<u16>;
    const MAX_PAYLOAD_LEN: u16;
}

pub(crate) const SYNC_CHAR_1: u8 = 0xb5;
pub(crate) const SYNC_CHAR_2: u8 = 0x62;

/// The checksum is calculated over the packet, starting and including
/// the CLASS field, up until, but excluding, the checksum field.
/// So slice should starts with class id.
/// Return ck_a and ck_b
pub(crate) fn ubx_checksum(data: &[u8]) -> (u8, u8) {
    let mut ck_a = 0_u8;
    let mut ck_b = 0_u8;
    for byte in data {
        ck_a = ck_a.overflowing_add(*byte).0;
        ck_b = ck_b.overflowing_add(ck_a).0;
    }
    (ck_a, ck_b)
}

/// For ubx checksum on the fly
#[derive(Default)]
struct UbxChecksumCalc {
    ck_a: u8,
    ck_b: u8,
}

impl UbxChecksumCalc {
    fn update(&mut self, chunk: &[u8]) {
        for byte in chunk {
            self.ck_a = self.ck_a.overflowing_add(*byte).0;
            self.ck_b = self.ck_b.overflowing_add(self.ck_a).0;
        }
    }
    fn result(self) -> (u8, u8) {
        (self.ck_a, self.ck_b)
    }
}

/// Abstraction for buffer creation/reallocation
/// to storing packet
pub trait MemWriter {
    type Error;
    /// make sure that we have at least `len` bytes for writing
    fn reserve_allocate(&mut self, len: usize) -> Result<(), MemWriterError<Self::Error>>;
    fn write(&mut self, buf: &[u8]) -> Result<(), MemWriterError<Self::Error>>;
}

#[cfg(feature = "std")]
impl MemWriter for Vec<u8> {
    type Error = std::io::Error;

    fn reserve_allocate(&mut self, len: usize) -> Result<(), MemWriterError<Self::Error>> {
        self.reserve(len);
        Ok(())
    }
    fn write(&mut self, buf: &[u8]) -> Result<(), MemWriterError<Self::Error>> {
        let ret = <dyn std::io::Write>::write(self, buf).map_err(MemWriterError::Custom)?;
        if ret == buf.len() {
            Ok(())
        } else {
            Err(MemWriterError::NotEnoughMem)
        }
    }
}

pub trait UbxPacketCreator {
    /// Create packet and store bytes sequence to somewhere using `out`
    fn create_packet<T: MemWriter>(self, out: &mut T) -> Result<(), MemWriterError<T::Error>>;
}

/// Packet not supported yet by this crate
#[derive(Debug)]
pub struct UbxUnknownPacketRef<'a> {
    pub payload: &'a [u8],
    pub class: u8,
    pub msg_id: u8,
}

/// Request specific packet
pub struct UbxPacketRequest {
    req_class: u8,
    req_id: u8,
}

impl UbxPacketRequest {
    pub const PACKET_LEN: usize = 8;

    #[inline]
    pub fn request_for<T: UbxPacketMeta>() -> Self {
        Self {
            req_class: T::CLASS,
            req_id: T::ID,
        }
    }
    #[inline]
    pub fn request_for_unknown(req_class: u8, req_id: u8) -> Self {
        Self { req_class, req_id }
    }

    #[inline]
    pub fn into_packet_bytes(self) -> [u8; Self::PACKET_LEN] {
        let mut ret = [
            SYNC_CHAR_1,
            SYNC_CHAR_2,
            self.req_class,
            self.req_id,
            0,
            0,
            0,
            0,
        ];
        let (ck_a, ck_b) = ubx_checksum(&ret[2..6]);
        ret[6] = ck_a;
        ret[7] = ck_b;
        ret
    }
}