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#[derive(Debug, Clone, Copy)]
282pub struct Header {
283    pub r#type: u16,
284    pub is_nested: bool,
285}
286
287pub fn chop_header<'a>(buf: &'a [u8], pos: &mut usize) -> Option<(Header, &'a [u8])> {
288    let buf = &buf[*pos..];
289
290    if buf.len() < 4 {
291        return None;
292    }
293
294    let len = parse_u16(&buf[0..2]).unwrap();
295    let r#type = parse_u16(&buf[2..4]).unwrap();
296
297    let next_len = nla_align_up(len as usize);
298
299    if len < 4 || buf.len() < len as usize {
300        return None;
301    }
302
303    let next = &buf[4..len as usize];
304    *pos += next_len.min(buf.len());
305
306    Some((
307        Header {
308            r#type: nla_type(r#type),
309            is_nested: r#type & NLA_F_NESTED != 0,
310        },
311        next,
312    ))
313}
314
315pub trait Rec {
316    fn as_rec_mut(&mut self) -> &mut Vec<u8>;
317    fn as_rec(&self) -> &Vec<u8>;
318}
319
320impl Rec for Vec<u8> {
321    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
322        self
323    }
324    fn as_rec(&self) -> &Vec<u8> {
325        self
326    }
327}
328
329impl Rec for &mut Vec<u8> {
330    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
331        self
332    }
333    fn as_rec(&self) -> &Vec<u8> {
334        self
335    }
336}
337
338#[derive(Debug, Clone, PartialEq, Eq)]
339pub enum ErrorReason {
340    /// Only used in `.get_<attr>()` methods
341    AttrMissing,
342    /// Value of the attribute can't be parsed
343    ParsingError,
344    /// Found attribute of type not mentioned in the specification
345    UnknownAttr,
346}
347
348#[derive(Clone, PartialEq, Eq)]
349pub struct ErrorContext {
350    pub attrs: &'static str,
351    pub attr: Option<&'static str>,
352    pub offset: usize,
353    pub reason: ErrorReason,
354}
355
356impl std::error::Error for ErrorContext {}
357
358impl From<ErrorContext> for std::io::Error {
359    fn from(value: ErrorContext) -> Self {
360        Self::other(value)
361    }
362}
363
364impl fmt::Debug for ErrorContext {
365    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
366        f.debug_struct("ErrorContext")
367            .field("message", &DisplayAsDebug(&self))
368            .field("reason", &self.reason)
369            .field("attrs", &self.attrs)
370            .field("attr", &self.attr)
371            .field("offset", &self.offset)
372            .finish()
373    }
374}
375
376impl fmt::Display for ErrorContext {
377    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
378        let attrs = self.attrs;
379        if matches!(self.reason, ErrorReason::AttrMissing) {
380            let attr = self.attr.unwrap();
381            write!(f, "Missing attribute {attr:?} in {attrs:?}")?;
382            return Ok(());
383        } else {
384            write!(f, "Error parsing ")?;
385            if let Some(attr) = self.attr {
386                write!(f, "attribute {attr:?} of {attrs:?}")?;
387            } else {
388                write!(f, "header of {attrs:?}")?;
389                if matches!(self.reason, ErrorReason::UnknownAttr) {
390                    write!(f, " (unknown attribute)")?;
391                }
392            }
393        }
394        write!(f, " at offset {} (0x{:02x})", self.offset, self.offset)?;
395        Ok(())
396    }
397}
398
399impl ErrorContext {
400    #[cold]
401    pub(crate) fn new(
402        attrs: &'static str,
403        attr: Option<&'static str>,
404        orig_loc: usize,
405        loc: usize,
406    ) -> ErrorContext {
407        let ctx = ErrorContext {
408            attrs,
409            attr,
410            offset: Self::calc_offset(orig_loc, loc),
411            reason: if attr.is_some() {
412                ErrorReason::ParsingError
413            } else {
414                ErrorReason::UnknownAttr
415            },
416        };
417
418        if cfg!(test) {
419            panic!("{ctx}")
420        } else {
421            ctx
422        }
423    }
424
425    #[cold]
426    pub(crate) fn new_missing(
427        attrs: &'static str,
428        attr: &'static str,
429        orig_loc: usize,
430        loc: usize,
431    ) -> ErrorContext {
432        let ctx = ErrorContext {
433            attrs,
434            attr: Some(attr),
435            offset: Self::calc_offset(orig_loc, loc),
436            reason: ErrorReason::AttrMissing,
437        };
438
439        if cfg!(test) {
440            panic!("{ctx}")
441        } else {
442            ctx
443        }
444    }
445
446    pub(crate) fn calc_offset(orig_loc: usize, loc: usize) -> usize {
447        if orig_loc <= loc && loc - orig_loc <= u16::MAX as usize {
448            loc - orig_loc
449        } else {
450            0
451        }
452    }
453}
454
455#[derive(Clone, Copy)]
456pub struct MultiAttrIterable<I, T, V>
457where
458    I: Iterator<Item = Result<T, ErrorContext>>,
459{
460    pub(crate) inner: I,
461    pub(crate) f: fn(T) -> Option<V>,
462}
463
464impl<I, T, V> MultiAttrIterable<I, T, V>
465where
466    I: Iterator<Item = Result<T, ErrorContext>>,
467{
468    pub fn new(inner: I, f: fn(T) -> Option<V>) -> Self {
469        Self { inner, f }
470    }
471}
472
473impl<I, T, V> Iterator for MultiAttrIterable<I, T, V>
474where
475    I: Iterator<Item = Result<T, ErrorContext>>,
476{
477    type Item = V;
478    fn next(&mut self) -> Option<Self::Item> {
479        match self.inner.next() {
480            Some(Ok(val)) => (self.f)(val),
481            _ => None,
482        }
483    }
484}
485
486#[derive(Clone, Copy, Default)]
487pub struct ArrayIterable<I, T>
488where
489    I: Iterator<Item = Result<T, ErrorContext>>,
490{
491    pub(crate) inner: I,
492}
493
494impl<I, T> ArrayIterable<I, T>
495where
496    I: Iterator<Item = Result<T, ErrorContext>>,
497{
498    pub fn new(inner: I) -> Self {
499        Self { inner }
500    }
501}
502
503impl<I, T> Iterator for ArrayIterable<I, T>
504where
505    I: Iterator<Item = Result<T, ErrorContext>>,
506{
507    type Item = T;
508    fn next(&mut self) -> Option<Self::Item> {
509        match self.inner.next() {
510            Some(Ok(val)) => Some(val),
511            _ => None,
512        }
513    }
514}
515
516#[derive(Debug)]
517pub enum RequestBuf<'a> {
518    Ref(&'a mut Vec<u8>),
519    Own(Vec<u8>),
520}
521
522impl RequestBuf<'_> {
523    pub fn buf(&self) -> &Vec<u8> {
524        match self {
525            RequestBuf::Ref(buf) => buf,
526            RequestBuf::Own(buf) => buf,
527        }
528    }
529
530    pub fn buf_mut(&mut self) -> &mut Vec<u8> {
531        match self {
532            RequestBuf::Ref(buf) => buf,
533            RequestBuf::Own(buf) => buf,
534        }
535    }
536}
537
538impl Rec for RequestBuf<'_> {
539    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
540        self.buf_mut()
541    }
542    fn as_rec(&self) -> &Vec<u8> {
543        self.buf()
544    }
545}
546
547pub struct PushWriter<Prev: Rec> {
548    pub(crate) prev: Option<Prev>,
549    pub(crate) header_offset: Option<usize>,
550}
551
552impl<Prev: Rec> std::ops::Deref for PushWriter<Prev> {
553    type Target = Vec<u8>;
554    fn deref(&self) -> &Self::Target {
555        self.as_rec()
556    }
557}
558
559impl<Prev: Rec> std::ops::DerefMut for PushWriter<Prev> {
560    fn deref_mut(&mut self) -> &mut Self::Target {
561        self.as_rec_mut()
562    }
563}
564
565impl<Prev: Rec> std::io::Write for PushWriter<Prev> {
566    fn write(&mut self, buf: &[u8]) -> std::io::Result<usize> {
567        self.as_rec_mut().write(buf)
568    }
569    fn flush(&mut self) -> std::io::Result<()> {
570        self.as_rec_mut().flush()
571    }
572}
573
574impl<Prev: Rec> Rec for PushWriter<Prev> {
575    fn as_rec_mut(&mut self) -> &mut Vec<u8> {
576        self.prev.as_mut().unwrap().as_rec_mut()
577    }
578    fn as_rec(&self) -> &Vec<u8> {
579        self.prev.as_ref().unwrap().as_rec()
580    }
581}
582
583impl<Prev: Rec> PushWriter<Prev> {
584    pub fn new(prev: Prev) -> Self {
585        Self {
586            prev: Some(prev),
587            header_offset: None,
588        }
589    }
590    pub fn end_nested(mut self) -> Prev {
591        let mut prev = self.prev.take().unwrap();
592        if let Some(header_offset) = &self.header_offset {
593            finalize_nested_header(prev.as_rec_mut(), *header_offset);
594        }
595        prev
596    }
597}
598
599impl<Prev: Rec> Drop for PushWriter<Prev> {
600    fn drop(&mut self) {
601        if let Some(prev) = &mut self.prev {
602            if let Some(header_offset) = &self.header_offset {
603                finalize_nested_header(prev.as_rec_mut(), *header_offset);
604            }
605        }
606    }
607}
608
609pub fn get_bits(buf: &[u8], byte_off: usize, bit_off: usize, bits: usize) -> u32 {
610    assert!(bit_off < 8);
611    assert!(bits <= 32);
612
613    let max_bytes = (bit_off + bits).div_ceil(8);
614    let first = byte_off;
615    let last = byte_off + max_bytes;
616
617    let mut reg_buf = [0u8; 8];
618    reg_buf[..max_bytes].copy_from_slice(&buf[first..last]);
619    let reg = u64::from_le_bytes(reg_buf) << (64 - bit_off - bits) >> (64 - bits);
620    reg.to_le() as u32
621}
622
623pub fn set_bits(buf: &mut [u8], byte_off: usize, bit_off: usize, bits: usize, val: u32) {
624    assert!(bit_off < 8);
625    assert!(bits <= 32);
626
627    let max_bytes = (bit_off + bits).div_ceil(8);
628    let first = byte_off;
629    let last = byte_off + max_bytes;
630
631    let mask = !(((1 << bits) - 1) << bit_off);
632    let reg = (val as u64) << bit_off;
633    let mut reg_buf = [0u8; 8];
634    reg_buf[..max_bytes].copy_from_slice(&buf[first..last]);
635    let reg = (u64::from_le_bytes(reg_buf) & mask) | reg;
636    let reg_buf = reg.to_le_bytes();
637    buf[first..last].copy_from_slice(&reg_buf[..max_bytes])
638}
639
640#[cfg(test)]
641mod tests {
642    use super::*;
643
644    #[test]
645    fn test_get_bits_basic() {
646        let buf = &[0b1010_0110u8, 0b0000_1100];
647
648        assert_eq!(get_bits(buf, 0, 0, 4), 0b0110);
649        assert_eq!(get_bits(buf, 0, 4, 4), 0b1010);
650        assert_eq!(get_bits(buf, 0, 4, 8), 0b1100_1010);
651    }
652
653    #[test]
654    fn test_set_bits_basic() {
655        let buf = &mut [0u8; 2];
656
657        set_bits(buf, 0, 0, 4, 0b0110);
658        assert_eq!(buf[0], 0b0110);
659
660        set_bits(buf, 0, 4, 4, 0b1111);
661        assert_eq!(buf[0], 0b1111_0110);
662
663        set_bits(buf, 0, 4, 8, 0b1100_1010);
664        assert_eq!(buf, &[0b1010_0110, 0b0000_1100]);
665    }
666}
netlink_bindings/utils.rs

netlink_bindings/
utils.rs
