autopilot/crates/mavlink_layer/src/internal/codec/crc.rs

//! ITU-T X.25 CRC-16 — MAVLink's checksum function.
//!
//! Polynomial `0x1021` with initial value `0xFFFF`, reflected per the MAVLink
//! reference implementation. Each frame's CRC is computed over the byte range
//! `[len..payload_end]` followed by the message-specific `CRC_EXTRA` byte.

/// Initial CRC value used by MAVLink. (Polynomial is `0x1021`, implicit in the
/// reflected algorithm below.)
pub const INIT: u16 = 0xFFFF;

/// Update an in-progress CRC accumulator with a single byte.
///
/// Mirrors the MAVLink C reference `crc_accumulate` exactly: `tmp` is a
/// `uint8_t` so the intermediate `tmp ^= (tmp << 4)` truncates to a byte.
/// Implementing this in pure `u16` (without the `as u8` cast) produces a
/// **different** CRC and breaks wire compatibility with real peers.
#[inline]
pub fn accumulate_byte(acc: u16, byte: u8) -> u16 {
    let mut tmp: u8 = byte ^ ((acc & 0xFF) as u8);
    tmp ^= tmp.wrapping_shl(4);
    let tmp = tmp as u16;
    (acc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)
}

/// CRC accumulator over a byte slice, starting from `start`.
#[inline]
pub fn accumulate(start: u16, bytes: &[u8]) -> u16 {
    bytes.iter().fold(start, |acc, b| accumulate_byte(acc, *b))
}

/// Compute the full MAVLink CRC for a frame body and its `crc_extra` byte.
///
/// `frame_body` is the range `[len, incompat_flags, compat_flags, seq, sysid,
/// compid, msgid_lo, msgid_mid, msgid_hi, payload...]` — i.e. the frame
/// without `STX` and without the trailing CRC.
#[inline]
pub fn frame_crc(frame_body: &[u8], crc_extra: u8) -> u16 {
    let intermediate = accumulate(INIT, frame_body);
    accumulate_byte(intermediate, crc_extra)
}

// The dummy value below is the CRC of "123456789" with INIT=0xFFFF and POLY=0x1021,
// computed per the MAVLink reflection. Confirmed against the MAVLink reference
// implementation (crc_accumulate).
#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn empty_slice_returns_init() {
        // Act
        let crc = accumulate(INIT, &[]);

        // Assert
        assert_eq!(crc, INIT);
    }

    #[test]
    fn single_byte_known_value() {
        // Act
        let crc = accumulate(INIT, &[0x00]);

        // Assert: derived by hand from the C reference. tmp = 0xFF ^ 0xFF = 0,
        // wait — tmp = 0x00 ^ 0xFF = 0xFF, then tmp ^= tmp<<4 (u8) = 0xFF ^ 0xF0
        // = 0x0F, then accum = 0x00FF ^ 0x0F00 ^ 0x78 ^ 0 = 0x0F87.
        assert_eq!(crc, 0x0F87);
    }

    #[test]
    fn mavlink_check_string_matches_pymavlink() {
        // MAVLink's CRC variant (byte-wise, not bit-reflected) gives 0x6F91
        // for the ASCII string "123456789" — verified by running the same
        // bytes through pymavlink's `mavcrc.x25crc`. This is NOT the same as
        // the textbook CRC-CCITT (XMODEM) value 0x29B1.
        let crc = accumulate(INIT, b"123456789");
        assert_eq!(crc, 0x6F91);
    }
}