use std::collections::BTreeMap;
use std::ffi::c_int;
pub(crate) const TC_H_ROOT: u32 = 0xFFFF_FFFF;
const TC_H_INGRESS: u32 = 0xFFFF_FFF1;
const TC_H_MAJ_MASK: u32 = 0xFFFF_0000;
const TC_H_MIN_MASK: u32 = 0x0000_FFFF;
const NLM_F_REPLACE: u16 = 0x100;
const NLM_F_EXCL: u16 = 0x200;
const NLM_F_CREATE: u16 = 0x400;
const TCA_KIND: u16 = 1;
const TCA_OPTIONS: u16 = 2;
const TCA_ETF_PARMS: u16 = 1;
const TC_ETF_OFFLOAD_ON: u32 = 1 << 1;
const ENOTSUPP: c_int = 524;
const KINDS: [&str; 15] = [
"pfifo_fast",
"pfifo",
"bfifo",
"noqueue",
"fq",
"fq_codel",
"codel",
"sfq",
"prio",
"tbf",
"htb",
"mq",
"mqprio",
"taprio",
"etf",
];
#[derive(Debug, Clone, PartialEq, Eq)]
pub(crate) struct Qdisc {
pub(crate) kind: String,
pub(crate) handle: u32,
pub(crate) etf: Option<(i32, i32, u32)>,
}
#[derive(Debug, Clone)]
pub(crate) struct NicQdiscs {
pub(crate) root: Qdisc,
pub(crate) children: BTreeMap<u32, Qdisc>,
}
pub(crate) struct Device<'a> {
pub(crate) num_tx_queues: usize,
pub(crate) real_tx_queues: usize,
pub(crate) no_queue: bool,
pub(crate) default_qdisc: &'a str,
pub(crate) etf_offload: Option<&'a [u16]>,
}
impl NicQdiscs {
pub(crate) fn initial(dev: &Device<'_>, etf: Option<(i32, i32, u32)>) -> Self {
let leaf = |kind: &str| Qdisc {
kind: kind.to_string(),
handle: 0,
etf: etf.filter(|_| kind == "etf"),
};
if dev.no_queue {
return NicQdiscs {
root: leaf("noqueue"),
children: BTreeMap::new(),
};
}
if dev.num_tx_queues > 1 && dev.default_qdisc != "etf" {
NicQdiscs {
root: leaf("mq"),
children: Self::defaults(dev),
}
} else {
NicQdiscs {
root: leaf(dev.default_qdisc),
children: BTreeMap::new(),
}
}
}
fn defaults(dev: &Device<'_>) -> BTreeMap<u32, Qdisc> {
(1..=dev.num_tx_queues as u32)
.map(|minor| (minor, Self::default_for(dev, minor)))
.collect()
}
fn default_for(dev: &Device<'_>, minor: u32) -> Qdisc {
let kind = if (minor as usize) <= dev.real_tx_queues {
dev.default_qdisc
} else {
"pfifo_fast"
};
Qdisc {
kind: kind.to_string(),
handle: 0,
etf: None,
}
}
fn classful(&self) -> bool {
matches!(self.root.kind.as_str(), "mq" | "mqprio" | "taprio")
}
fn create(
dev: &Device<'_>,
kind: &str,
handle: u32,
options: Option<&[u8]>,
queue: Option<u32>,
) -> Result<Qdisc, c_int> {
if !KINDS.contains(&kind) {
return Err(libc::ENOENT);
}
let mut etf = None;
if kind == "mq" && (queue.is_some() || dev.num_tx_queues < 2) {
return Err(libc::EOPNOTSUPP);
}
if kind == "etf" {
let parms = options
.and_then(|o| find_attr(o, TCA_ETF_PARMS))
.filter(|p| p.len() >= 12)
.ok_or(libc::EINVAL)?;
let word = |at: usize| i32::from_ne_bytes(parms[at..at + 4].try_into().unwrap());
let (delta, clockid, flags) = (word(0), word(4), word(8) as u32);
if clockid < 0 {
return Err(ENOTSUPP);
}
if clockid != libc::CLOCK_TAI || delta < 0 {
return Err(libc::EINVAL);
}
if flags & TC_ETF_OFFLOAD_ON != 0 {
let queues = dev.etf_offload.ok_or(libc::EOPNOTSUPP)?;
if !queues.contains(&(queue.unwrap_or(0) as u16)) {
return Err(libc::EINVAL);
}
}
etf = Some((delta, clockid, flags));
}
Ok(Qdisc {
kind: kind.to_string(),
handle,
etf,
})
}
pub(crate) fn modify(
&mut self,
dev: &Device<'_>,
auto: &mut u32,
flags: u16,
handle: u32,
parent: u32,
attrs: &[u8],
) -> Result<(), c_int> {
let kind = find_attr(attrs, TCA_KIND).map(|k| {
let end = k.iter().position(|&b| b == 0).unwrap_or(k.len());
String::from_utf8_lossy(&k[..end]).into_owned()
});
let options = find_attr(attrs, TCA_OPTIONS);
if parent == TC_H_INGRESS {
return Err(libc::EOPNOTSUPP);
}
if parent == 0 {
return Err(if handle == 0 {
libc::EINVAL
} else {
libc::ENOENT
});
}
let queue = if parent == TC_H_ROOT {
None
} else {
if self.root.handle == 0 || self.root.handle & TC_H_MAJ_MASK != parent & TC_H_MAJ_MASK {
return Err(libc::ENOENT);
}
if !self.classful() {
return Err(libc::EOPNOTSUPP);
}
let minor = parent & TC_H_MIN_MASK;
if minor == 0 || minor as usize > dev.num_tx_queues {
return Err(libc::ENOENT);
}
Some(minor - 1)
};
let existing = match queue {
None => &self.root,
Some(q) => &self.children[&(q + 1)],
};
let present = existing.handle != 0;
let differs = kind.as_deref().is_some_and(|k| k != existing.kind);
let both = |a: u16, b: u16| flags & (a | b) == a | b;
let create = if !present || handle == 0 || existing.handle != handle {
if handle != 0 {
if present && flags & NLM_F_REPLACE == 0 {
return Err(libc::EEXIST);
}
if handle & TC_H_MIN_MASK != 0 {
return Err(libc::EINVAL);
}
Some(true)
} else if !present
|| differs && (both(NLM_F_CREATE, NLM_F_REPLACE) || both(NLM_F_CREATE, NLM_F_EXCL))
{
Some(true)
} else if differs && flags & (NLM_F_CREATE | NLM_F_REPLACE | NLM_F_EXCL) == 0 {
Some(false)
} else {
None
}
} else {
None
};
match create {
None => {
if flags & NLM_F_EXCL != 0 {
return Err(libc::EEXIST);
}
if differs {
return Err(libc::EINVAL);
}
if options.is_some() && matches!(existing.kind.as_str(), "mq" | "etf") {
return Err(libc::EINVAL);
}
Ok(())
}
Some(needs_create_flag) => {
if needs_create_flag && flags & NLM_F_CREATE == 0 {
return Err(libc::ENOENT);
}
let kind = kind.ok_or(libc::EINVAL)?;
let next_auto = auto.wrapping_add(0x1_0000);
let new = Self::create(
dev,
&kind,
if handle != 0 { handle } else { next_auto },
options,
queue,
)?;
if handle == 0 {
*auto = next_auto;
}
match queue {
None => {
self.children = if new.kind == "mq" {
Self::defaults(dev)
} else {
BTreeMap::new()
};
self.root = new;
}
Some(q) => {
self.children.insert(q + 1, new);
}
}
Ok(())
}
}
}
pub(crate) fn delete(
&mut self,
dev: &Device<'_>,
handle: u32,
parent: u32,
) -> Result<(), c_int> {
if parent == 0 {
return Err(libc::EINVAL);
}
let queue = if parent == TC_H_ROOT {
None
} else {
if self.root.handle == 0
|| self.root.handle & TC_H_MAJ_MASK != parent & TC_H_MAJ_MASK
|| !self.classful()
{
return Err(libc::ENOENT);
}
let minor = parent & TC_H_MIN_MASK;
if minor == 0 || minor as usize > dev.num_tx_queues {
return Err(libc::ENOENT);
}
Some(minor)
};
let q = match queue {
None => &self.root,
Some(m) => &self.children[&m],
};
if handle != 0 && q.handle != handle {
return Err(libc::EINVAL);
}
if q.handle == 0 {
return Err(libc::ENOENT);
}
match queue {
None => *self = Self::initial(dev, None),
Some(m) => {
self.children.insert(m, Self::default_for(dev, m));
}
}
Ok(())
}
pub(crate) fn listing(&self, real_tx_queues: usize) -> Vec<(u32, u32, &Qdisc)> {
let mut out = vec![(self.root.handle, TC_H_ROOT, &self.root)];
let major = self.root.handle & TC_H_MAJ_MASK;
for (&minor, q) in &self.children {
if q.handle != 0 || minor as usize <= real_tx_queues {
out.push((q.handle, major | minor, q));
}
}
out
}
}
fn find_attr(attrs: &[u8], want: u16) -> Option<&[u8]> {
let mut pos = 0;
while pos + 4 <= attrs.len() {
let len = u16::from_ne_bytes([attrs[pos], attrs[pos + 1]]) as usize;
let ty = u16::from_ne_bytes([attrs[pos + 2], attrs[pos + 3]]) & 0x3FFF;
if len < 4 || pos + len > attrs.len() {
return None;
}
if ty == want {
return Some(&attrs[pos + 4..pos + len]);
}
pos += len.next_multiple_of(4);
}
None
}