netlink_bindings/
utils.rs

1use std::fmt;
2
3pub use crate::primitives::*;
4pub use std::{ffi::CStr, fmt::Debug, iter::Iterator};
5
6pub fn dump_hex(buf: &[u8]) {
7    let mut len = 0;
8    for chunk in buf.chunks(16) {
9        print!("{len:04x?}: ");
10        print!("{chunk:02x?} ");
11        for b in chunk {
12            if b.is_ascii() && !b.is_ascii_control() {
13                print!("{}", char::from_u32(*b as u32).unwrap());
14            } else {
15                print!(".");
16            }
17        }
18        println!();
19        len += chunk.len();
20    }
21}
22
23pub fn dump_assert_eq(left: &[u8], right: &[u8]) {
24    if left.len() != right.len() {
25        dump_hex(left);
26        dump_hex(right);
27        panic!("Length mismatched");
28    }
29    if let Some(pos) = left.iter().zip(right.iter()).position(|(l, r)| *l != *r) {
30        println!();
31        println!("Left:");
32        dump_hex(left);
33        println!();
34        println!("Right:");
35        dump_hex(right);
36        panic!("Differ at byte {pos} (0x{pos:x?})");
37    }
38}
39
40pub struct FormatBinStr<T: AsRef<[u8]>>(pub T);
41impl<T: AsRef<[u8]>> Debug for FormatBinStr<T> {
42    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
43        // core::bstr is currently unstable
44        let bstr = self.0.as_ref();
45        if let Ok(cstr) = CStr::from_bytes_with_nul(bstr) {
46            write!(fmt, "{cstr:?}")?;
47        } else {
48            for c in bstr.chunks(127) {
49                let mut buf = [0u8; 128];
50                buf[..c.len()].clone_from_slice(c);
51                write!(fmt, "{:?}", CStr::from_bytes_until_nul(&buf).unwrap())?;
52            }
53        };
54        Ok(())
55    }
56}
57
58pub struct FormatIter<I: Iterator<Item = T> + Clone, T: Debug>(pub I);
59
60impl<I: Iterator<Item = T> + Clone, T: Debug> Debug for FormatIter<I, T> {
61    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
62        let mut f = fmt.debug_list();
63        for item in self.0.clone() {
64            f.entry(&item);
65        }
66        f.finish()
67    }
68}
69
70pub struct FormatHex<T: AsRef<[u8]>>(pub T);
71
72impl<T: AsRef<[u8]>> Debug for FormatHex<T> {
73    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
74        write!(fmt, "\"")?;
75        for i in self.0.as_ref() {
76            write!(fmt, "{i:02x}")?
77        }
78        write!(fmt, "\"")?;
79        Ok(())
80    }
81}
82
83pub struct FormatEnum<T: Debug>(pub u64, pub fn(u64) -> Option<T>);
84
85impl<T: Debug> Debug for FormatEnum<T> {
86    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
87        write!(fmt, "{} ", self.0)?;
88
89        if let Some(var) = (self.1)(self.0) {
90            write!(fmt, "[{var:?}]")?;
91        } else {
92            write!(fmt, "(unknown variant)")?;
93        }
94
95        Ok(())
96    }
97}
98
99pub struct FormatFlags<T: Debug>(pub u64, pub fn(u64) -> Option<T>);
100
101impl<T: Debug> Debug for FormatFlags<T> {
102    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
103        write!(fmt, "{} ", self.0)?;
104
105        if self.0 == 0 {
106            write!(fmt, "(empty)")?;
107            return Ok(());
108        }
109
110        let mut seen_variant = false;
111        for i in 0..u64::BITS {
112            let bit = self.0 & (1 << i);
113            if bit == 0 {
114                continue;
115            }
116
117            if !seen_variant {
118                seen_variant = true;
119                write!(fmt, "[")?;
120            } else {
121                write!(fmt, ",")?;
122            }
123
124            if let Some(var) = (self.1)(bit) {
125                write!(fmt, "{var:?}")?;
126            } else {
127                write!(fmt, "(unknown bit {i})")?;
128            }
129        }
130
131        if seen_variant {
132            write!(fmt, "]")?;
133        }
134
135        Ok(())
136    }
137}
138
139pub struct DisplayAsDebug<T>(T);
140
141impl<T: fmt::Display> fmt::Debug for DisplayAsDebug<T> {
142    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
143        write!(f, "{}", self.0)
144    }
145}
146
147pub struct FlattenErrorContext<T: fmt::Debug>(pub Result<T, ErrorContext>);
148
149impl<T: Debug> fmt::Debug for FlattenErrorContext<T> {
150    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
151        match &self.0 {
152            Ok(ok) => ok.fmt(f),
153            Err(err) => {
154                f.write_str("Err(")?;
155                err.fmt(f)?;
156                f.write_str(")")
157            }
158        }
159    }
160}
161
162pub struct MapFormatArray<I, T, M, D>(pub T, pub M)
163where
164    T: Clone + Iterator<Item = Result<I, ErrorContext>>,
165    M: Clone + FnMut(I) -> D,
166    D: fmt::Debug;
167
168impl<I, T, M, D> fmt::Debug for MapFormatArray<I, T, M, D>
169where
170    T: Clone + Iterator<Item = Result<I, ErrorContext>>,
171    M: Clone + FnMut(I) -> D,
172    D: fmt::Debug,
173{
174    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
175        let mut f = f.debug_list();
176        for item in self.0.clone() {
177            f.entry(&FlattenErrorContext(item.map(self.1.clone())));
178        }
179        f.finish()
180    }
181}
182
183pub const NLA_F_NESTED: u16 = 1 << 15;
184pub const NLA_F_NET_BYTEORDER: u16 = 1 << 14;
185
186pub const fn nla_type(r#type: u16) -> u16 {
187    r#type & (!(NLA_F_NESTED | NLA_F_NET_BYTEORDER))
188}
189
190pub const NLA_ALIGNTO: usize = 4;
191
192pub const fn align_up(len: usize, alignment: usize) -> usize {
193    ((len) + alignment - 1) & !(alignment - 1)
194}
195
196pub const fn nla_align_up(len: usize) -> usize {
197    ((len) + NLA_ALIGNTO - 1) & !(NLA_ALIGNTO - 1)
198}
199
200pub fn align(buf: &mut Vec<u8>) {
201    let len = buf.len();
202    buf.extend(std::iter::repeat_n(0u8, nla_align_up(len) - len));
203}
204
205/// Returns header offset
206pub fn push_nested_header(buf: &mut Vec<u8>, r#type: u16) -> usize {
207    push_header_type(buf, r#type, 0, true)
208}
209
210/// Returns header offset
211pub fn push_header(buf: &mut Vec<u8>, r#type: u16, len: u16) -> usize {
212    push_header_type(buf, r#type, len, false)
213}
214
215/// Returns header offset
216pub fn write_header(buf: &mut Vec<u8>, r#type: u16) -> usize {
217    push_header_type(buf, r#type, 0, false)
218}
219
220/// Returns header offset
221/// The kernel doesn't really check byteorder bit nor set it correctly
222fn push_header_type(buf: &mut Vec<u8>, mut r#type: u16, len: u16, is_nested: bool) -> usize {
223    align(buf);
224
225    let header_offset = buf.len();
226
227    if is_nested {
228        r#type |= NLA_F_NESTED;
229    }
230
231    // TODO: alignment for 8 byte types?
232    buf.extend((len + 4).to_ne_bytes());
233    buf.extend(r#type.to_ne_bytes());
234
235    align(buf);
236
237    header_offset
238}
239
240pub fn push_pad(buf: &mut Vec<u8>, pad_type: u16, alignment: usize) {
241    assert!(alignment.is_power_of_two());
242    if alignment <= 4 {
243        return;
244    }
245
246    align(buf);
247
248    let needed = align_up(buf.len(), alignment) - buf.len();
249
250    if needed == 0 {
251        return;
252    }
253
254    push_header(buf, pad_type, (needed - 4) as u16);
255    buf.extend(std::iter::repeat_n(0, needed - 4));
256}
257
258pub fn finalize_nested_header(buf: &mut Vec<u8>, offset: usize) {
259    align(buf);
260
261    let len = (buf.len() - offset) as u16;
262    buf[offset..(offset + 2)].copy_from_slice(&len.to_ne_bytes());
263}
264
265#[derive(Debug, Clone, Copy)]
266pub struct Header {
267    pub r#type: u16,
268    pub is_nested: bool,
269}
270
271pub fn chop_header<'a>(buf: &'a [u8], pos: &mut usize) -> Option<(Header, &'a [u8])> {
272    let buf = &buf[*pos..];
273
274    if buf.len() < 4 {
275        return None;
276    }
277
278    let len = parse_u16(&buf[0..2]).unwrap();
279    let r#type = parse_u16(&buf[2..4]).unwrap();
280
281    let next_len = nla_align_up(len as usize);
282
283    if len < 4 || buf.len() < len as usize {
284        return None;
285    }
286
287    let next = &buf[4..len as usize];
288    *pos += next_len.min(buf.len());
289
290    Some((
291        Header {
292            r#type: nla_type(r#type),
293            is_nested: r#type & NLA_F_NESTED != 0,
294        },
295        next,
296    ))
297}
298
299pub trait Rec {
300    fn as_rec_mut(&mut self) -> &mut Vec<u8>;
301    fn as_rec(&self) -> &Vec<u8>;
302}
303
304impl Rec for &mut Vec<u8> {
305    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
306        self
307    }
308    fn as_rec(&self) -> &Vec<u8> {
309        self
310    }
311}
312
313#[derive(Debug, Clone, PartialEq, Eq)]
314pub enum ErrorReason {
315    /// Only used in `.get_<attr>()` methods
316    AttrMissing,
317    /// Value of the attribute can't be parsed
318    ParsingError,
319    /// Found attribute of type not mentioned in the specification
320    UnknownAttr,
321}
322
323#[derive(Clone, PartialEq, Eq)]
324pub struct ErrorContext {
325    pub attrs: &'static str,
326    pub attr: Option<&'static str>,
327    pub offset: usize,
328    pub reason: ErrorReason,
329}
330
331impl std::error::Error for ErrorContext {}
332
333impl From<ErrorContext> for std::io::Error {
334    fn from(value: ErrorContext) -> Self {
335        Self::other(value)
336    }
337}
338
339impl fmt::Debug for ErrorContext {
340    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
341        f.debug_struct("ErrorContext")
342            .field("message", &DisplayAsDebug(&self))
343            .field("reason", &self.reason)
344            .field("attrs", &self.attrs)
345            .field("attr", &self.attr)
346            .field("offset", &self.offset)
347            .finish()
348    }
349}
350
351impl fmt::Display for ErrorContext {
352    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
353        let attrs = self.attrs;
354        if matches!(self.reason, ErrorReason::AttrMissing) {
355            let attr = self.attr.unwrap();
356            write!(f, "Missing attribute {attr:?} in {attrs:?}")?;
357            return Ok(());
358        } else {
359            write!(f, "Error parsing ")?;
360            if let Some(attr) = self.attr {
361                write!(f, "attribute {attr:?} of {attrs:?}")?;
362            } else {
363                write!(f, "header of {attrs:?}")?;
364                if matches!(self.reason, ErrorReason::UnknownAttr) {
365                    write!(f, " (unknown attribute)")?;
366                }
367            }
368        }
369        write!(f, " at offset {}", self.offset)?;
370        Ok(())
371    }
372}
373
374impl ErrorContext {
375    #[cold]
376    pub(crate) fn new(
377        attrs: &'static str,
378        attr: Option<&'static str>,
379        orig_loc: usize,
380        loc: usize,
381    ) -> ErrorContext {
382        let ctx = ErrorContext {
383            attrs,
384            attr,
385            offset: Self::calc_offset(orig_loc, loc),
386            reason: if attr.is_some() {
387                ErrorReason::ParsingError
388            } else {
389                ErrorReason::UnknownAttr
390            },
391        };
392
393        if cfg!(test) {
394            panic!("{ctx}")
395        } else {
396            ctx
397        }
398    }
399
400    #[cold]
401    pub(crate) fn new_missing(
402        attrs: &'static str,
403        attr: &'static str,
404        orig_loc: usize,
405        loc: usize,
406    ) -> ErrorContext {
407        let ctx = ErrorContext {
408            attrs,
409            attr: Some(attr),
410            offset: Self::calc_offset(orig_loc, loc),
411            reason: ErrorReason::AttrMissing,
412        };
413
414        if cfg!(test) {
415            panic!("{ctx}")
416        } else {
417            ctx
418        }
419    }
420
421    pub(crate) fn calc_offset(orig_loc: usize, loc: usize) -> usize {
422        if orig_loc <= loc && loc - orig_loc <= u16::MAX as usize {
423            loc - orig_loc
424        } else {
425            0
426        }
427    }
428}
429
430#[derive(Clone, Copy)]
431pub struct MultiAttrIterable<I, T, V>
432where
433    I: Iterator<Item = Result<T, ErrorContext>>,
434{
435    pub(crate) inner: I,
436    pub(crate) f: fn(T) -> Option<V>,
437}
438
439impl<I, T, V> MultiAttrIterable<I, T, V>
440where
441    I: Iterator<Item = Result<T, ErrorContext>>,
442{
443    pub fn new(inner: I, f: fn(T) -> Option<V>) -> Self {
444        Self { inner, f }
445    }
446}
447
448impl<I, T, V> Iterator for MultiAttrIterable<I, T, V>
449where
450    I: Iterator<Item = Result<T, ErrorContext>>,
451{
452    type Item = V;
453    fn next(&mut self) -> Option<Self::Item> {
454        match self.inner.next() {
455            Some(Ok(val)) => (self.f)(val),
456            _ => None,
457        }
458    }
459}
460
461#[derive(Clone, Copy, Default)]
462pub struct ArrayIterable<I, T>
463where
464    I: Iterator<Item = Result<T, ErrorContext>>,
465{
466    pub(crate) inner: I,
467}
468
469impl<I, T> ArrayIterable<I, T>
470where
471    I: Iterator<Item = Result<T, ErrorContext>>,
472{
473    pub fn new(inner: I) -> Self {
474        Self { inner }
475    }
476}
477
478impl<I, T> Iterator for ArrayIterable<I, T>
479where
480    I: Iterator<Item = Result<T, ErrorContext>>,
481{
482    type Item = T;
483    fn next(&mut self) -> Option<Self::Item> {
484        match self.inner.next() {
485            Some(Ok(val)) => Some(val),
486            _ => None,
487        }
488    }
489}
490
491#[derive(Debug)]
492pub enum RequestBuf<'a> {
493    Ref(&'a mut Vec<u8>),
494    Own(Vec<u8>),
495}
496
497impl RequestBuf<'_> {
498    pub fn buf(&self) -> &Vec<u8> {
499        match self {
500            RequestBuf::Ref(buf) => buf,
501            RequestBuf::Own(buf) => buf,
502        }
503    }
504
505    pub fn buf_mut(&mut self) -> &mut Vec<u8> {
506        match self {
507            RequestBuf::Ref(buf) => buf,
508            RequestBuf::Own(buf) => buf,
509        }
510    }
511}
512
513impl Rec for RequestBuf<'_> {
514    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
515        self.buf_mut()
516    }
517    fn as_rec(&self) -> &Vec<u8> {
518        self.buf()
519    }
520}
521
522pub struct PushWriter<Prev: Rec> {
523    pub(crate) prev: Option<Prev>,
524    pub(crate) header_offset: Option<usize>,
525}
526
527impl<Prev: Rec> std::ops::Deref for PushWriter<Prev> {
528    type Target = Vec<u8>;
529    fn deref(&self) -> &Self::Target {
530        self.as_rec()
531    }
532}
533
534impl<Prev: Rec> std::ops::DerefMut for PushWriter<Prev> {
535    fn deref_mut(&mut self) -> &mut Self::Target {
536        self.as_rec_mut()
537    }
538}
539
540impl<Prev: Rec> std::io::Write for PushWriter<Prev> {
541    fn write(&mut self, buf: &[u8]) -> std::io::Result<usize> {
542        self.as_rec_mut().write(buf)
543    }
544    fn flush(&mut self) -> std::io::Result<()> {
545        self.as_rec_mut().flush()
546    }
547}
548
549impl<Prev: Rec> Rec for PushWriter<Prev> {
550    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
551        self.prev.as_mut().unwrap().as_rec_mut()
552    }
553    fn as_rec(&self) -> &Vec<u8> {
554        self.prev.as_ref().unwrap().as_rec()
555    }
556}
557
558impl<Prev: Rec> PushWriter<Prev> {
559    pub fn new(prev: Prev) -> Self {
560        Self {
561            prev: Some(prev),
562            header_offset: None,
563        }
564    }
565    pub fn end_nested(mut self) -> Prev {
566        let mut prev = self.prev.take().unwrap();
567        if let Some(header_offset) = &self.header_offset {
568            finalize_nested_header(prev.as_rec_mut(), *header_offset);
569        }
570        prev
571    }
572}
573
574impl<Prev: Rec> Drop for PushWriter<Prev> {
575    fn drop(&mut self) {
576        if let Some(prev) = &mut self.prev {
577            if let Some(header_offset) = &self.header_offset {
578                finalize_nested_header(prev.as_rec_mut(), *header_offset);
579            }
580        }
581    }
582}
583
584pub fn get_bits(buf: &[u8], byte_off: usize, bit_off: usize, bits: usize) -> u32 {
585    assert!(bit_off < 8);
586    assert!(bits <= 32);
587
588    let max_bytes = (bit_off + bits).div_ceil(8);
589    let first = byte_off;
590    let last = byte_off + max_bytes;
591
592    let mut reg_buf = [0u8; 8];
593    reg_buf[..max_bytes].copy_from_slice(&buf[first..last]);
594    let reg = u64::from_le_bytes(reg_buf) << (64 - bit_off - bits) >> (64 - bits);
595    reg.to_le() as u32
596}
597
598pub fn set_bits(buf: &mut [u8], byte_off: usize, bit_off: usize, bits: usize, val: u32) {
599    assert!(bit_off < 8);
600    assert!(bits <= 32);
601
602    let max_bytes = (bit_off + bits).div_ceil(8);
603    let first = byte_off;
604    let last = byte_off + max_bytes;
605
606    let mask = !(((1 << bits) - 1) << bit_off);
607    let reg = (val as u64) << bit_off;
608    let mut reg_buf = [0u8; 8];
609    reg_buf[..max_bytes].copy_from_slice(&buf[first..last]);
610    let reg = (u64::from_le_bytes(reg_buf) & mask) | reg;
611    let reg_buf = reg.to_le_bytes();
612    buf[first..last].copy_from_slice(&reg_buf[..max_bytes])
613}
614
615#[cfg(test)]
616mod tests {
617    use super::*;
618
619    #[test]
620    fn test_get_bits_basic() {
621        let buf = &[0b1010_0110u8, 0b0000_1100];
622
623        assert_eq!(get_bits(buf, 0, 0, 4), 0b0110);
624        assert_eq!(get_bits(buf, 0, 4, 4), 0b1010);
625        assert_eq!(get_bits(buf, 0, 4, 8), 0b1100_1010);
626    }
627
628    #[test]
629    fn test_set_bits_basic() {
630        let buf = &mut [0u8; 2];
631
632        set_bits(buf, 0, 0, 4, 0b0110);
633        assert_eq!(buf[0], 0b0110);
634
635        set_bits(buf, 0, 4, 4, 0b1111);
636        assert_eq!(buf[0], 0b1111_0110);
637
638        set_bits(buf, 0, 4, 8, 0b1100_1010);
639        assert_eq!(buf, &[0b1010_0110, 0b0000_1100]);
640    }
641}
netlink_bindings/utils.rs

netlink_bindings/
utils.rs
