use crate::error::LibraryError;
use crate::util::is_bit_set;
const DATA_MASK: u16 = 0x07FF;
const PARITY_BITS: usize = 5;
const PARITY_MASKS: [u16; PARITY_BITS] = [
0b0000_0101_0101_1011,
0b0000_0110_0110_1101,
0b0000_0111_1000_1110,
0b0000_0111_1111_0000,
0b0111_1111_1111_1111,
];
pub fn add_checksum(word: u16) -> Result<u16, LibraryError> {
if word & !DATA_MASK != 0 {
Err(LibraryError::Other(
"Value given to add_checksum already has a checksum",
))
} else {
let mut calculated_word = word;
for (mask_index, parity_mask) in PARITY_MASKS.iter().enumerate() {
let parity_index = DATA_MASK.count_ones() as usize + mask_index;
let parity = ((calculated_word & parity_mask).count_ones() % 2) as u16;
calculated_word |= parity << parity_index;
}
Ok(calculated_word)
}
}
pub fn validate_checksum(word: u16) -> Result<u16, LibraryError> {
let calculated_word = add_checksum(word & DATA_MASK)?;
if calculated_word != word {
let given_parity = word & (!DATA_MASK);
let calculated_parity = calculated_word & (!DATA_MASK);
const PARITY_OFFSET: u16 = u16::BITS as u16 - PARITY_BITS as u16;
let affected_parity = (given_parity ^ calculated_parity) >> PARITY_OFFSET;
let mut error_bits = 0xFFFF;
for (mask_index, mask) in PARITY_MASKS[..(PARITY_MASKS.len() - 1)].iter().enumerate() {
let mask = mask | (1 << (mask_index as u16 + PARITY_OFFSET));
if is_bit_set(affected_parity, mask_index) {
error_bits &= mask;
} else {
error_bits &= !mask;
}
}
let corrected_word = word ^ error_bits;
if add_checksum(corrected_word & DATA_MASK) == Ok(corrected_word) {
Ok(corrected_word & DATA_MASK)
} else {
Err(LibraryError::Checksum(word))
}
} else {
Ok(word & DATA_MASK)
}
}
#[cfg(test)]
mod test {
use core::convert::TryInto;
use crate::error::LibraryError;
use crate::test::{mlx90641_datasheet_eeprom, EEPROM_LENGTH};
fn eeprom_values() -> [u16; EEPROM_LENGTH] {
let mut dest = [0u16; EEPROM_LENGTH];
let raw = mlx90641_datasheet_eeprom();
raw.chunks_exact(2)
.map(|chunk| {
let bytes: [u8; 2] = chunk.try_into().unwrap();
u16::from_be_bytes(bytes)
})
.zip(dest.iter_mut())
.for_each(|(source, dest)| *dest = source);
dest
}
#[test]
fn checksum_no_errors() {
for value in eeprom_values().iter() {
let checked = super::validate_checksum(*value);
assert_eq!(
checked,
Ok(*value & super::DATA_MASK),
"{:#06X} did not pass checksum validation",
*value
);
}
}
#[test]
fn checksum_one_error() {
for value in eeprom_values().iter() {
for bit_index in 0..16 {
let bad_value = *value ^ (1 << bit_index);
let checked = super::validate_checksum(bad_value);
assert_eq!(
checked,
Ok(*value & super::DATA_MASK),
"Unable to correct {:#06X} to {:#06X}",
bad_value,
*value
);
}
}
}
#[test]
fn checksum_two_errors() {
for value in eeprom_values().iter() {
for bit_index1 in 0..16 {
for bit_index2 in 0..16 {
if bit_index1 == bit_index2 {
continue;
}
let bad_value = *value ^ (1 << bit_index1) ^ (1 << bit_index2);
let checked = super::validate_checksum(bad_value);
assert_eq!(
checked,
Err(LibraryError::Checksum(bad_value)),
"{:#06X} (originally {:#06X}) passed checksum validation unexpectedly",
bad_value,
*value,
);
}
}
}
}
}