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

netlink_bindings/
utils.rs
