m_bus_core/
decryption.rs

1use crate::{DeviceType, ManufacturerCode, SecurityMode};
2
3#[cfg(feature = "decryption")]
4use aes::Aes128Dec;
5#[cfg(feature = "decryption")]
6use cbc::{
7    Decryptor,
8    cipher::{BlockModeDecrypt, KeyIvInit},
9};
10
11#[derive(Debug, Clone, PartialEq)]
12pub struct KeyContext {
13    pub manufacturer: ManufacturerCode,
14    pub identification_number: u32,
15    pub version: u8,
16    pub device_type: DeviceType,
17    pub security_mode: SecurityMode,
18    pub access_number: u8,
19}
20
21#[derive(Debug, Clone, Copy, PartialEq)]
22pub enum DecryptionError {
23    UnsupportedMode(SecurityMode),
24    KeyNotFound,
25    DecryptionFailed,
26    InvalidKeyLength,
27    InvalidDataLength,
28    NotEncrypted,
29    UnknownEncryptionState,
30}
31
32impl core::fmt::Display for DecryptionError {
33    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
34        match self {
35            Self::UnsupportedMode(mode) => write!(f, "Unsupported security mode: {:?}", mode),
36            Self::KeyNotFound => write!(f, "Decryption key not found"),
37            Self::DecryptionFailed => write!(f, "Decryption operation failed"),
38            Self::InvalidKeyLength => write!(f, "Invalid key length"),
39            Self::InvalidDataLength => write!(f, "Invalid data length"),
40            Self::NotEncrypted => write!(f, "Data is not encrypted"),
41            Self::UnknownEncryptionState => {
42                write!(f, "Unknown encryption state for this data block type")
43            }
44        }
45    }
46}
47
48impl core::error::Error for DecryptionError {}
49
50pub trait KeyProvider {
51    fn get_key(&self, context: &KeyContext) -> Result<&[u8], DecryptionError>;
52}
53
54#[derive(Debug)]
55pub struct EncryptedPayload<'a> {
56    pub data: &'a [u8],
57    pub context: KeyContext,
58}
59
60impl<'a> EncryptedPayload<'a> {
61    pub fn new(data: &'a [u8], context: KeyContext) -> Self {
62        Self { data, context }
63    }
64
65    /// Return plaintext bytes lazily without a payload-sized destination buffer.
66    #[cfg(feature = "decryption")]
67    pub fn decrypted_bytes<K: KeyProvider>(
68        &self,
69        provider: &K,
70    ) -> Result<DecryptedBytes<'a>, DecryptionError> {
71        let key = provider.get_key(&self.context)?;
72        match self.context.security_mode {
73            SecurityMode::NoEncryption => Ok(DecryptedBytes::plaintext(self.data)),
74            SecurityMode::AesCbc128IvZero => DecryptedBytes::cbc(self.data, key, &[0u8; 16]),
75            SecurityMode::AesCbc128IvNonZero => {
76                DecryptedBytes::cbc(self.data, key, &self._derive_iv())
77            }
78            mode => Err(DecryptionError::UnsupportedMode(mode)),
79        }
80    }
81
82    /// Decrypt into caller-provided contiguous storage.
83    #[cfg(feature = "decryption")]
84    pub fn decrypt_into<K: KeyProvider>(
85        &self,
86        provider: &K,
87        output: &mut [u8],
88    ) -> Result<usize, DecryptionError> {
89        let key = provider.get_key(&self.context)?;
90
91        match self.context.security_mode {
92            SecurityMode::NoEncryption => {
93                let len = self.data.len();
94                let dest = output
95                    .get_mut(..len)
96                    .ok_or(DecryptionError::InvalidDataLength)?;
97                dest.copy_from_slice(self.data);
98                Ok(len)
99            }
100            SecurityMode::AesCbc128IvZero => {
101                decrypt_aes_cbc_into(self.data, key, &[0u8; 16], output)
102            }
103            SecurityMode::AesCbc128IvNonZero => {
104                let iv = self._derive_iv();
105                decrypt_aes_cbc_into(self.data, key, &iv, output)
106            }
107            mode => Err(DecryptionError::UnsupportedMode(mode)),
108        }
109    }
110
111    #[cfg(feature = "decryption")]
112    fn _derive_iv(&self) -> [u8; 16] {
113        let mut iv = [0u8; 16];
114        // Bytes 0-1: Manufacturer ID (numeric, little-endian)
115        let mfr_id = self.context.manufacturer.to_id();
116        iv[0..2].copy_from_slice(&mfr_id.to_le_bytes());
117        // Bytes 2-5: Identification number as BCD bytes (how they appear in frame)
118        let bcd_bytes = decimal_to_bcd(self.context.identification_number);
119        iv[2..6].copy_from_slice(&bcd_bytes);
120        // Byte 6: Version
121        iv[6] = self.context.version;
122        // Byte 7: Device type
123        iv[7] = self.context.device_type.into();
124        // Bytes 8-15: Access number repeated 8 times
125        iv[8..16].fill(self.context.access_number);
126        iv
127    }
128}
129
130/// Convert a decimal number to BCD bytes (little-endian, 4 bytes)
131/// e.g., 14639203 -> [0x03, 0x92, 0x63, 0x14]
132#[cfg(feature = "decryption")]
133fn decimal_to_bcd(mut value: u32) -> [u8; 4] {
134    let mut bcd = [0u8; 4];
135    for byte in &mut bcd {
136        let low = (value % 10) as u8;
137        value /= 10;
138        let high = (value % 10) as u8;
139        value /= 10;
140        *byte = (high << 4) | low;
141    }
142    bcd
143}
144
145pub struct StaticKeyProvider<const N: usize> {
146    entries: [(u64, [u8; 16]); N],
147    count: usize,
148}
149
150impl<const N: usize> Default for StaticKeyProvider<N> {
151    fn default() -> Self {
152        Self {
153            entries: [(0, [0u8; 16]); N],
154            count: 0,
155        }
156    }
157}
158
159impl<const N: usize> StaticKeyProvider<N> {
160    pub fn new() -> Self {
161        Self::default()
162    }
163
164    pub fn add_key(
165        &mut self,
166        manufacturer_id: u16,
167        identification_number: u32,
168        key: [u8; 16],
169    ) -> Result<(), DecryptionError> {
170        let key_id = Self::compute_key_id(manufacturer_id, identification_number);
171        let entry = self
172            .entries
173            .get_mut(self.count)
174            .ok_or(DecryptionError::InvalidDataLength)?;
175        *entry = (key_id, key);
176        self.count += 1;
177        Ok(())
178    }
179
180    const fn compute_key_id(manufacturer_id: u16, identification_number: u32) -> u64 {
181        ((manufacturer_id as u64) << 32) | (identification_number as u64)
182    }
183}
184
185impl<const N: usize> KeyProvider for StaticKeyProvider<N> {
186    fn get_key(&self, context: &KeyContext) -> Result<&[u8], DecryptionError> {
187        let manufacturer_id = context.manufacturer.to_id();
188
189        let key_id = Self::compute_key_id(manufacturer_id, context.identification_number);
190
191        self.entries[..self.count]
192            .iter()
193            .find(|(id, _)| *id == key_id)
194            .map(|(_, key)| key.as_slice())
195            .ok_or(DecryptionError::KeyNotFound)
196    }
197}
198
199/// Lazily decrypted payload bytes, borrowing the original ciphertext.
200///
201/// AES-CBC retains its key schedule, chaining state, and one 16-byte working
202/// block, not a payload-sized plaintext buffer. Trailing incomplete blocks are
203/// yielded unchanged, matching [`EncryptedPayload::decrypt_into`].
204///
205/// Contiguous-slice parsers still require a destination buffer; callers that
206/// consume bytes incrementally can use this iterator directly.
207#[cfg(feature = "decryption")]
208pub struct DecryptedBytes<'a> {
209    input: &'a [u8],
210    decryptor: Option<Decryptor<Aes128Dec>>,
211    block: cipher::Block<Aes128Dec>,
212    position: usize,
213    remaining: usize,
214}
215
216#[cfg(feature = "decryption")]
217impl<'a> DecryptedBytes<'a> {
218    fn plaintext(input: &'a [u8]) -> Self {
219        Self {
220            input,
221            decryptor: None,
222            block: Default::default(),
223            position: 16,
224            remaining: input.len(),
225        }
226    }
227
228    fn cbc(input: &'a [u8], key: &[u8], iv: &[u8]) -> Result<Self, DecryptionError> {
229        if key.len() != 16 {
230            return Err(DecryptionError::InvalidKeyLength);
231        }
232        if iv.len() != 16 || input.is_empty() {
233            return Err(DecryptionError::InvalidDataLength);
234        }
235        let decryptor = Decryptor::<Aes128Dec>::new_from_slices(key, iv)
236            .map_err(|_| DecryptionError::InvalidKeyLength)?;
237        Ok(Self {
238            decryptor: Some(decryptor),
239            ..Self::plaintext(input)
240        })
241    }
242}
243
244#[cfg(feature = "decryption")]
245impl Iterator for DecryptedBytes<'_> {
246    type Item = u8;
247
248    fn next(&mut self) -> Option<Self::Item> {
249        if self.remaining == 0 {
250            return None;
251        }
252        if self.position == 16
253            && let Some(decryptor) = &mut self.decryptor
254            && let Some(block) = self.input.get(..16)
255        {
256            self.block.copy_from_slice(block);
257            decryptor.decrypt_block(&mut self.block);
258            self.input = &self.input[16..];
259            self.position = 0;
260        }
261        let byte = if let Some(byte) = self.block.get(self.position) {
262            self.position += 1;
263            *byte
264        } else {
265            let (byte, rest) = self.input.split_first()?;
266            self.input = rest;
267            *byte
268        };
269        self.remaining -= 1;
270        Some(byte)
271    }
272
273    fn size_hint(&self) -> (usize, Option<usize>) {
274        (self.remaining, Some(self.remaining))
275    }
276}
277
278#[cfg(feature = "decryption")]
279impl ExactSizeIterator for DecryptedBytes<'_> {}
280#[cfg(feature = "decryption")]
281impl core::iter::FusedIterator for DecryptedBytes<'_> {}
282
283// Keep the AES key schedule off the unencrypted caller's stack.
284#[cfg(feature = "decryption")]
285#[inline(never)]
286fn decrypt_aes_cbc_into(
287    data: &[u8],
288    key: &[u8],
289    iv: &[u8],
290    output: &mut [u8],
291) -> Result<usize, DecryptionError> {
292    if key.len() != 16 {
293        return Err(DecryptionError::InvalidKeyLength);
294    }
295    if iv.len() != 16 || data.is_empty() {
296        return Err(DecryptionError::InvalidDataLength);
297    }
298    let target = output
299        .get_mut(..data.len())
300        .ok_or(DecryptionError::InvalidDataLength)?;
301    target.copy_from_slice(data);
302    let encrypted_len = data.len() / 16 * 16;
303    if encrypted_len != 0 {
304        let decryptor = Decryptor::<Aes128Dec>::new_from_slices(key, iv)
305            .map_err(|_| DecryptionError::InvalidKeyLength)?;
306        decryptor
307            .decrypt_padded::<cipher::block_padding::NoPadding>(&mut target[..encrypted_len])
308            .map_err(|_| DecryptionError::DecryptionFailed)?;
309    }
310    Ok(data.len())
311}
312
313#[cfg(all(test, feature = "decryption"))]
314mod tests {
315    use super::*;
316
317    #[test]
318    fn test_decrypt_aes_cbc_basic() {
319        // Test vector: simple AES-128-CBC decryption
320        let key = [0u8; 16];
321        let iv = [0u8; 16];
322        let encrypted = [
323            0x66, 0xe9, 0x4b, 0xd4, 0xef, 0x8a, 0x2c, 0x3b, 0x88, 0x4c, 0xfa, 0x59, 0xca, 0x34,
324            0x2b, 0x2e,
325        ];
326        let mut output = [0u8; 16];
327
328        let result = decrypt_aes_cbc_into(&encrypted, &key, &iv, &mut output);
329        assert!(result.is_ok());
330        let len = result.unwrap();
331        assert_eq!(len, 16);
332        assert_eq!(output, [0; 16]);
333        let mut lazy = DecryptedBytes::cbc(&encrypted, &key, &iv).unwrap();
334        assert_eq!(lazy.len(), 16);
335        for remaining in (0..16).rev() {
336            assert_eq!(lazy.next(), Some(0));
337            assert_eq!(lazy.len(), remaining);
338        }
339        assert_eq!(lazy.next(), None);
340        assert_eq!(lazy.next(), None);
341    }
342
343    #[test]
344    fn test_key_provider_basic() {
345        let mut provider = StaticKeyProvider::<10>::new();
346
347        // Add a test key
348        let key = [
349            0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E,
350            0x0F, 0x10,
351        ];
352
353        // Manufacturer code "ABC" -> ID calculation
354        let manufacturer_code = ManufacturerCode::from_id(0x0421).unwrap(); // ABC
355        let identification_number = 12345678u32;
356
357        // Add the key
358        provider
359            .add_key(0x0421, identification_number, key)
360            .unwrap();
361
362        // Create context
363        let context = KeyContext {
364            manufacturer: manufacturer_code,
365            identification_number,
366            version: 0x01,
367            device_type: DeviceType::WaterMeter,
368            security_mode: crate::SecurityMode::AesCbc128IvZero,
369            access_number: 0x00,
370        };
371
372        // Retrieve the key
373        let retrieved_key = provider.get_key(&context).unwrap();
374        assert_eq!(retrieved_key, &key);
375    }
376
377    #[test]
378    fn test_derive_iv() {
379        let manufacturer_code = ManufacturerCode::from_id(0x1ee6).unwrap(); // GWF
380        let context = KeyContext {
381            manufacturer: manufacturer_code,
382            identification_number: 12345678,
383            version: 0x42,
384            device_type: DeviceType::WaterMeter,
385            security_mode: crate::SecurityMode::AesCbc128IvNonZero,
386            access_number: 0x50,
387        };
388
389        let payload = EncryptedPayload { data: &[], context };
390
391        let iv = payload._derive_iv();
392
393        // Check manufacturer ID (first 2 bytes should be 0x1ee6 in little-endian)
394        assert_eq!(iv[0], 0xe6);
395        assert_eq!(iv[1], 0x1e);
396
397        // Check identification number (bytes 2-5, as BCD)
398        // 12345678 decimal -> BCD [0x78, 0x56, 0x34, 0x12]
399        assert_eq!(&iv[2..6], &[0x78, 0x56, 0x34, 0x12]);
400
401        // Check version (byte 6)
402        assert_eq!(iv[6], 0x42);
403
404        // Check device type (byte 7) - WaterMeter should be 0x07
405        assert_eq!(iv[7], 0x07);
406
407        // Check access number repeated (bytes 8-15)
408        for &byte in iv.iter().skip(8) {
409            assert_eq!(byte, 0x50);
410        }
411    }
412
413    #[test]
414    fn test_mode5_decryption_real_frame() {
415        // Test vector from real wMBus Mode 5 encrypted frame
416        // Frame: 6644496A3100015514377203926314496A00075000500598A78E0D...
417        // Key: F8B24F12F9D113F680BEE765FDE67EC0
418        // Expected IV: 496A0392631400075050505050505050
419
420        let key = [
421            0xF8, 0xB2, 0x4F, 0x12, 0xF9, 0xD1, 0x13, 0xF6, 0x80, 0xBE, 0xE7, 0x65, 0xFD, 0xE6,
422            0x7E, 0xC0,
423        ];
424
425        // Encrypted payload (80 bytes after TPL header)
426        let encrypted = [
427            0x98, 0xA7, 0x8E, 0x0D, 0x71, 0xAA, 0x63, 0x58, 0xEE, 0xBD, 0x0B, 0x20, 0xBF, 0xDF,
428            0x99, 0xED, 0xA2, 0xD2, 0x2F, 0xA2, 0x53, 0x14, 0xF3, 0xF1, 0xB8, 0x44, 0x70, 0x89,
429            0x8E, 0x49, 0x53, 0x03, 0x92, 0x37, 0x70, 0xBA, 0x8D, 0xDA, 0x97, 0xC9, 0x64, 0xF0,
430            0xEA, 0x6C, 0xE2, 0x4F, 0x56, 0x50, 0xC0, 0xA6, 0xCD, 0xF3, 0xDE, 0x37, 0xDE, 0x33,
431            0xFB, 0xFB, 0xEB, 0xAC, 0xE4, 0x00, 0x9B, 0xB0, 0xD8, 0xEB, 0xA2, 0xCB, 0xE8, 0x04,
432            0x33, 0xFF, 0x13, 0x13, 0x28, 0x20, 0x60, 0x20, 0xB1, 0xBF,
433        ];
434
435        // Expected decrypted data
436        let expected = [
437            0x2F, 0x2F, 0x0C, 0x13, 0x29, 0x73, 0x06, 0x00, 0x02, 0x6C, 0x94, 0x21, 0x82, 0x04,
438            0x6C, 0x81, 0x21, 0x8C, 0x04, 0x13, 0x75, 0x44, 0x06, 0x00, 0x8D, 0x04, 0x93, 0x13,
439            0x2C, 0xFB, 0xFE, 0x12, 0x44, 0x00, 0x51, 0x41, 0x00, 0x70, 0x35, 0x00, 0x77, 0x33,
440            0x00, 0x75, 0x49, 0x00, 0x16, 0x36, 0x00, 0x73, 0x56, 0x00, 0x91, 0x55, 0x00, 0x95,
441            0x57, 0x00, 0x31, 0x57, 0x00, 0x28, 0x42, 0x00, 0x18, 0x47, 0x00, 0x61, 0x39, 0x00,
442            0x56, 0x42, 0x00, 0x02, 0xFD, 0x17, 0x00, 0x00, 0x2F, 0x2F,
443        ];
444
445        // Build context from TPL header:
446        // Frame bytes [0x49, 0x6A] = manufacturer ID 0x6A49 when parsed as little-endian
447        // ID bytes [0x03, 0x92, 0x63, 0x14] in BCD = 14639203 decimal
448        let manufacturer = ManufacturerCode::from_id(0x6A49).unwrap();
449        let context = KeyContext {
450            manufacturer,
451            identification_number: 14639203, // BCD 03926314 parsed as decimal
452            version: 0x00,
453            device_type: DeviceType::WaterMeter,
454            security_mode: crate::SecurityMode::AesCbc128IvNonZero,
455            access_number: 0x50,
456        };
457
458        // Verify IV derivation
459        let payload = EncryptedPayload {
460            data: &encrypted,
461            context: context.clone(),
462        };
463        let iv = payload._derive_iv();
464        // Expected IV: 496A0392631400075050505050505050
465        let expected_iv = [
466            0x49, 0x6A, 0x03, 0x92, 0x63, 0x14, 0x00, 0x07, 0x50, 0x50, 0x50, 0x50, 0x50, 0x50,
467            0x50, 0x50,
468        ];
469        assert_eq!(iv, expected_iv);
470
471        // Test decryption
472        let mut provider = StaticKeyProvider::<1>::new();
473        provider.add_key(0x6A49, 14639203, key).unwrap();
474
475        let mut output = [0u8; 80];
476        let len = payload.decrypt_into(&provider, &mut output).unwrap();
477
478        assert_eq!(len, 80);
479        assert_eq!(&output[..80], &expected[..]);
480        assert!(payload.decrypted_bytes(&provider).unwrap().eq(expected));
481
482        // Every partial-block boundary preserves the historical plaintext tail.
483        for length in 1..=encrypted.len() {
484            let payload = EncryptedPayload::new(&encrypted[..length], context.clone());
485            let mut buffered = [0xAA; 80];
486            assert_eq!(payload.decrypt_into(&provider, &mut buffered), Ok(length));
487            let mut bytes = payload.decrypted_bytes(&provider).unwrap();
488            for index in 0..length {
489                assert_eq!(bytes.len(), length - index);
490                let want = if index < length / 16 * 16 {
491                    expected[index]
492                } else {
493                    encrypted[index]
494                };
495                assert_eq!(bytes.next(), Some(want));
496                assert_eq!(buffered[index], want);
497            }
498            assert_eq!(bytes.next(), None);
499            assert_eq!(bytes.next(), None);
500        }
501        // The lazy result is independent of the temporary context/provider.
502        let bytes = {
503            let local_payload = EncryptedPayload::new(&encrypted, context.clone());
504            let mut local_provider = StaticKeyProvider::<1>::new();
505            local_provider.add_key(0x6A49, 14639203, key).unwrap();
506            local_payload.decrypted_bytes(&local_provider).unwrap()
507        };
508        assert!(bytes.eq(expected));
509        let mut too_short = [0xAA; 79];
510        assert_eq!(
511            payload.decrypt_into(&provider, &mut too_short),
512            Err(DecryptionError::InvalidDataLength)
513        );
514        assert_eq!(too_short, [0xAA; 79]);
515
516        let mut context = context;
517        context.security_mode = SecurityMode::NoEncryption;
518        let plaintext = EncryptedPayload::new(&encrypted, context);
519        assert!(plaintext.decrypted_bytes(&provider).unwrap().eq(encrypted));
520        let mut output = [0xAA; 81];
521        assert_eq!(plaintext.decrypt_into(&provider, &mut output), Ok(80));
522        assert_eq!(&output[..80], &encrypted);
523        assert_eq!(output[80], 0xAA);
524        assert_eq!(
525            plaintext.decrypt_into(&provider, &mut too_short),
526            Err(DecryptionError::InvalidDataLength)
527        );
528        assert_eq!(too_short, [0xAA; 79]);
529        let missing_key = StaticKeyProvider::<0>::new();
530        assert_eq!(
531            plaintext.decrypt_into(&missing_key, &mut too_short),
532            Err(DecryptionError::KeyNotFound)
533        );
534        assert_eq!(too_short, [0xAA; 79]);
535        let empty = EncryptedPayload::new(&[], plaintext.context);
536        assert_eq!(empty.decrypt_into(&provider, &mut []), Ok(0));
537    }
538    #[test]
539    fn lazy_decryption_rejects_invalid_parameters() {
540        assert_eq!(
541            DecryptedBytes::cbc(&[0; 16], &[0; 15], &[0; 16]).err(),
542            Some(DecryptionError::InvalidKeyLength)
543        );
544        assert_eq!(
545            DecryptedBytes::cbc(&[0; 16], &[0; 16], &[0; 15]).err(),
546            Some(DecryptionError::InvalidDataLength)
547        );
548        assert_eq!(
549            DecryptedBytes::cbc(&[], &[0; 16], &[0; 16]).err(),
550            Some(DecryptionError::InvalidDataLength)
551        );
552        assert_eq!(DecryptedBytes::plaintext(&[]).next(), None);
553    }
554}
m_bus_core/decryption.rs

m_bus_core/
decryption.rs
