ssp/encrypted/
command.rs

1#[cfg(not(test))]
2use rand_chacha::rand_core::RngCore;
3#[cfg(all(not(test), not(feature = "std")))]
4use rand_chacha::{rand_core::SeedableRng, ChaCha20Rng};
5
6#[cfg(all(not(test), not(feature = "std")))]
7use crate::seed;
8
9use crate::{
10    impl_command_display, impl_command_ops, impl_default, impl_encrypted_message_ops,
11    impl_message_from_buf, len, AesKey, CommandOps, Error, MessageOps, Result, SequenceCount,
12};
13
14use super::{encrypted_index as index, WrappedEncryptedMessage};
15
16/// Encrypted - Command (0x7E)
17///
18/// The encrypted packet is wrapped inside the data field of a standard SSP packet. The
19/// encrypted section is constructed from the following fields.
20///
21/// | STEX | LENGTH | COUNT 0 | COUNT 1 | COUNT 2 | COUNT 3 | DATA 0 | ... | DATA N | PACKING 0 | ... | PACKING N | CRC_L | CRC_H |
22/// |:----:|:------:|:-------:|:-------:|:-------:|:-------:|:------:|:---:|:------:|:---------:|:---:|:---------:|:-----:|:-----:|
23/// | 0x7E | 0xnn   | 0xnn    | 0xnn    | 0xnn    | 0xnn    | 0xnn   | ... | 0xnn   | 0xnn      | ... | 0xnn      | 0xnn  | 0xnn  |
24#[repr(C)]
25#[derive(Clone, Debug, PartialEq, zeroize::Zeroize, zeroize::ZeroizeOnDrop)]
26pub struct EncryptedCommand {
27    buf: [u8; len::ENCRYPTED_COMMAND],
28}
29
30impl EncryptedCommand {
31    /// Creates a new [EncryptedCommand].
32    pub fn new() -> Self {
33        let mut msg = Self {
34            buf: [0u8; len::ENCRYPTED_COMMAND],
35        };
36
37        msg.init();
38
39        msg
40    }
41
42    /// Gets the sequence count.
43    pub fn count(&self) -> SequenceCount {
44        self.count_buf().into()
45    }
46
47    fn count_buf(&self) -> &[u8] {
48        self.buf[index::COUNT..index::COUNT_END].as_ref()
49    }
50
51    fn set_count(&mut self, count: SequenceCount) {
52        self.buf[index::COUNT..index::COUNT_END]
53            .copy_from_slice(count.as_inner().to_le_bytes().as_ref());
54    }
55
56    /// Builder function that sets the [SequenceCount].
57    pub fn with_count(mut self, count: SequenceCount) -> Self {
58        self.set_count(count);
59        self
60    }
61
62    /// Gets the message data.
63    pub fn message_data(&self) -> &[u8] {
64        let start = self.data_start();
65        let end = self.data_end();
66
67        self.buf[start..end].as_ref()
68    }
69
70    fn data_start(&self) -> usize {
71        index::DATA
72    }
73
74    fn data_end(&self) -> usize {
75        self.data_start() + self.data_len()
76    }
77
78    /// Sets the message data.
79    ///
80    /// Returns `Err(_)` if the message data exceeds the maximum length.
81    ///
82    /// Maximum length: [MAX_ENCRYPTED_DATA](crate::len::MAX_ENCRYPTED_DATA)
83    ///
84    /// This maximum refers to the data field of the wrapped inner message.
85    ///
86    /// Encrypted messages wrap the data field of a standard SSP message, and then the entire
87    /// encrypted message is wrapped in an outer standard SSP message.
88    ///
89    /// Matryoshka dolls all the way down...
90    pub fn set_message_data(&mut self, message: &dyn CommandOps) -> Result<()> {
91        let len = message.data_len();
92
93        if message.data().len() != len {
94            return Err(Error::InvalidDataLength((len, message.data().len())));
95        }
96
97        if (0..=len::MAX_ENCRYPTED_DATA).contains(&len) {
98            self.set_data_len(len as u8);
99
100            let start = self.data_start();
101            let end = self.data_end();
102
103            let data = message.data();
104
105            log::trace!("Encrypted data: {data:x?}, length: {len}");
106
107            self.buf[start..end].copy_from_slice(data);
108
109            Ok(())
110        } else {
111            Err(Error::InvalidDataLength((len, len::MAX_ENCRYPTED_DATA)))
112        }
113    }
114
115    /// Builder function that sets the message data.
116    pub fn with_message_data(mut self, message: &dyn CommandOps) -> Result<Self> {
117        self.set_message_data(message)?;
118        Ok(self)
119    }
120
121    /// Gets random packing data used to make the encrypted packet a mutliple of the [AES block
122    /// length](crate::len::AES).
123    ///
124    /// The encrypted fields are:
125    ///
126    /// ```no_build,no_run
127    /// LEN + COUNT + DATA + PACKING + CRC_L + CRC_H
128    /// ```
129    pub fn packing(&self) -> &[u8] {
130        let start = self.packing_start();
131        let end = self.packing_end();
132
133        self.buf[start..end].as_ref()
134    }
135
136    /// Adds random packing data to make the encrypted packet a mutliple of the [AES block
137    /// length](crate::len::AES).
138    ///
139    /// The encrypted fields are:
140    ///
141    /// ```no_build,no_run
142    /// LEN + COUNT + DATA + PACKING + CRC_L + CRC_H
143    /// ```
144    #[cfg(not(feature = "std"))]
145    pub fn set_packing(&mut self) {
146        if self.packing_len() == 0 {
147            return;
148        }
149
150        // Less robust than using rand::thread_rng, but still better than PKCS#7 padding...
151        #[cfg(not(test))]
152        let mut rng = ChaCha20Rng::from_seed(seed(self.buf(), self.count_buf()));
153
154        let start = self.packing_start();
155        let end = self.packing_end();
156
157        #[cfg(not(test))]
158        rng.fill_bytes(&mut self.buf[start..end]);
159        #[cfg(test)]
160        self.buf[start..end].copy_from_slice([0; 255][..end - start].as_ref());
161    }
162
163    /// Adds random packing data to make the encrypted packet a mutliple of the [AES block
164    /// length](crate::len::AES).
165    ///
166    /// The encrypted fields are:
167    ///
168    /// ```no_build,no_run
169    /// LEN + COUNT + DATA + PACKING + CRC_L + CRC_H
170    /// ```
171    #[cfg(feature = "std")]
172    pub fn set_packing(&mut self) {
173        if self.packing_len() == 0 {
174            return;
175        }
176
177        #[cfg(not(test))]
178        let mut rng = rand::thread_rng();
179
180        let start = self.packing_start();
181        let end = self.packing_end();
182
183        #[cfg(not(test))]
184        rng.fill_bytes(&mut self.buf[start..end]);
185        #[cfg(test)]
186        self.buf[start..end].copy_from_slice([0; 255][..end - start].as_ref());
187    }
188
189    fn packing_start(&self) -> usize {
190        self.data_end()
191    }
192
193    fn packing_end(&self) -> usize {
194        self.packing_start() + self.packing_len()
195    }
196
197    /// Gets the length of packing bytes needed to make the packet's encrypted data a multiple of
198    /// the [AES block length](crate::len::AES).
199    pub fn packing_len(&self) -> usize {
200        // count all metadata bytes except STEX
201        let meta = len::METADATA - 1;
202        let raw_len = meta + self.data_len();
203
204        len::aes_packing_len(raw_len)
205    }
206
207    fn encrypt_data(&mut self) -> &mut [u8] {
208        let len = self.len();
209        self.buf[index::LEN..len].as_mut()
210    }
211
212    /// Encrypts and consumes the [EncryptedCommand] message.
213    ///
214    /// Converts the [EncryptedCommand] message into a standard [WrappedEncryptedMessage].
215    pub fn encrypt(mut self, key: &AesKey) -> WrappedEncryptedMessage {
216        //use crate::aes;
217        use aes::cipher::{BlockEncrypt, KeyInit};
218
219        self.set_packing();
220
221        if super::sequence_count().as_inner() != 0 {
222            self.set_count(super::sequence_count());
223        }
224
225        self.calculate_checksum();
226
227        let mut enc_msg = WrappedEncryptedMessage::new();
228
229        let enc_len = self.len();
230        enc_msg.set_data_len(enc_len as u8);
231
232        log::trace!("Encrypted message: {:x?}", self.buf());
233
234        let plain_data = self.encrypt_data();
235        let cipher_data = enc_msg.data_mut()[1..].as_mut();
236
237        let ciph = aes::Aes128::new(key);
238
239        for (pchunk, cchunk) in plain_data
240            .chunks_exact(16)
241            .zip(cipher_data.chunks_exact_mut(16))
242        {
243            ciph.encrypt_block_b2b(pchunk.into(), cchunk.into());
244        }
245
246        enc_msg.calculate_checksum();
247        if let Err(err) = enc_msg.verify_checksum() {
248            log::error!("error validating wrapped encrypted checksum: {err}");
249        }
250
251        if let Err(err) = enc_msg.stuff_encrypted_data() {
252            log::error!("error stuffing encrypted command message: {err}");
253        }
254
255        let count = super::sequence_count();
256        let next_count = super::increment_sequence_count();
257
258        log::trace!("encryption sequence count: {count}");
259        log::trace!("next encryption sequence count: {next_count}");
260
261        enc_msg
262    }
263
264    /// Decrypts and consumes the [WrappedEncryptedMessage].
265    ///
266    /// Converts the [WrappedEncryptedMessage] into an [EncryptedCommand].
267    ///
268    /// **Note**: only useful if implementing a device-side binary, and/or testing host-side
269    /// functionality.
270    pub fn decrypt(key: &AesKey, mut message: WrappedEncryptedMessage) -> Self {
271        use crate::aes;
272
273        if let Err(err) = message.unstuff_encrypted_data() {
274            log::error!("error unstuffing encrypted command message: {err}");
275        }
276
277        let mut dec_msg = Self::new();
278        dec_msg.set_data_len(message.data_len().saturating_sub(len::ENCRYPTED_METADATA) as u8);
279
280        // Skip the STEX (0x7E) byte, it's not encrypted/decrypted
281        let cipher_data = message.data()[1..].as_ref();
282        let plain_data = dec_msg.encrypt_data();
283
284        if let Err(err) = aes::aes_decrypt_inplace(key.as_ref(), cipher_data, plain_data) {
285            log::error!("error decrypting message: {err}");
286        }
287
288        super::increment_sequence_count();
289
290        dec_msg
291    }
292}
293
294impl_default!(EncryptedCommand);
295impl_command_display!(EncryptedCommand);
296impl_message_from_buf!(EncryptedCommand);
297impl_encrypted_message_ops!(EncryptedCommand);
298impl_command_ops!(EncryptedCommand);
ssp/encrypted/command.rs

ssp/encrypted/
command.rs
