WIP: Minimal kprobe loader with interface identification.

src/rust/lqos_sys/src/bpf/lqos_kern.c

@@ -1,8 +1,15 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 // Minimal XDP program that passes all packets.
 // Used to verify XDP functionality.
+#ifndef __TARGET_ARCH_x86
+#define __TARGET_ARCH_x86 1
+#endif
+
 #include <linux/bpf.h>
+#include <linux/ptrace.h>
 #include <bpf/bpf_helpers.h>
+#include <bpf/bpf_core_read.h>
+#include <bpf/bpf_tracing.h>
 #include <linux/in6.h>
 #include <linux/ip.h>
 #include <linux/ipv6.h>
@@ -51,6 +58,21 @@
 // If it stays at 255, we have a configuration error.
 int direction = 255;
 
+// Constant passed in during to load, containing
+// either -1 (none) or the ifindex of the interface
+// in each direction. Required because the kbprobe
+// is global.
+int to_internet_ifindex = -1;
+int to_isp_ifindex = -1;
+
+struct net_device {
+    int ifindex;
+} __attribute__((preserve_access_index));
+
+struct sk_buff {
+    struct net_device *dev;
+} __attribute__((preserve_access_index));
+
 // Also configured during loading. For "on a stick" support,
 // these are mapped to the respective VLAN facing directions.
 __be16 internet_vlan = 0; // Note: turn these into big-endian
@@ -511,4 +533,22 @@ int flow_reader(struct bpf_iter__bpf_map_elem *ctx)
     return 0;
 }
 
+/* Runs at packet transmit */
+SEC("kprobe/dev_hard_start_xmit")
+int kprobe_xmit(struct pt_regs *ctx) {
+    struct sk_buff *skb = (struct sk_buff *)PT_REGS_PARM1(ctx);
+    int ifindex;
+    int tracked_if = to_internet_ifindex;
+
+    if (!skb) return 0;
+    ifindex = BPF_CORE_READ(skb, dev, ifindex);
+
+    if (ifindex != tracked_if) {
+        tracked_if = to_isp_ifindex;
+        if (tracked_if < 0 || ifindex != tracked_if) return 0;
+    }
+
+    return 0;
+}
+
 char _license[] SEC("license") = "GPL";

src/rust/lqos_sys/src/bpf/wrapper.c

@@ -247,6 +247,32 @@ out:
 	return err;
 }
 
+struct bpf_link *attach_xmit_kprobe(struct lqos_kern *obj)
+{
+	struct bpf_link *link;
+	long err;
+
+	link = bpf_program__attach_kprobe(obj->progs.kprobe_xmit, false, "dev_hard_start_xmit");
+	err = libbpf_get_error(link);
+	if (err) {
+		const char *msg = "unknown";
+		if (err < 0) {
+			msg = strerror((int)-err);
+		}
+		fprintf(stderr, "bpf_program__attach_kprobe() fails (%ld: %s)\n", err, msg);
+		return NULL;
+	}
+
+	return link;
+}
+
+void destroy_bpf_link(struct bpf_link *link)
+{
+	if (link != NULL) {
+		bpf_link__destroy(link);
+	}
+}
+
 // Iterator code
 #include <stdio.h>
 #include <unistd.h>

src/rust/lqos_sys/src/bpf/wrapper.h

@@ -10,6 +10,8 @@ extern int tc_attach_egress(int ifindex, bool verbose, struct lqos_kern *obj);
 extern int tc_detach_egress(int ifindex, bool verbose, bool flush_hook, const char * ifname);
 extern int tc_attach_ingress(int ifindex, bool verbose, struct lqos_kern *obj);
 extern int tc_detach_ingress(int ifindex, bool verbose, bool flush_hook, const char * ifname);
+extern struct bpf_link * attach_xmit_kprobe(struct lqos_kern *obj);
+extern void destroy_bpf_link(struct bpf_link *link);
 extern __u64 max_tracker_ips();
 extern void do_not_print();
 int read_tp_buffer(struct bpf_program *prog, struct bpf_map *map);

src/rust/lqos_sys/src/kernel_wrapper.rs

@@ -1,5 +1,5 @@
 use crate::lqos_kernel::{
-    InterfaceDirection, attach_xdp_and_tc_to_interface,
+    AttachedPrograms, InterfaceDirection, attach_xdp_and_tc_to_interface,
     bpf::{self, ring_buffer_sample_fn},
     unload_xdp_from_interface,
 };
@@ -11,6 +11,7 @@ use parking_lot::Mutex;
 /// world insists on it.
 pub(crate) struct LqosKernBpfWrapper {
     ptr: *mut bpf::lqos_kern,
+    kprobe_link: *mut bpf::bpf_link,
 }
 
 impl LqosKernBpfWrapper {
@@ -22,6 +23,14 @@ impl LqosKernBpfWrapper {
 unsafe impl Sync for LqosKernBpfWrapper {}
 unsafe impl Send for LqosKernBpfWrapper {}
 
+impl Drop for LqosKernBpfWrapper {
+    fn drop(&mut self) {
+        unsafe {
+            bpf::destroy_bpf_link(self.kprobe_link);
+        }
+    }
+}
+
 pub(crate) static BPF_SKELETON: Lazy<Mutex<Option<LqosKernBpfWrapper>>> =
     Lazy::new(|| Mutex::new(None));
 
@@ -58,21 +67,34 @@ impl LibreQoSKernels {
             to_isp: to_isp.to_string(),
             on_a_stick: false,
         };
-        let skeleton = attach_xdp_and_tc_to_interface(
+        let to_internet_ifindex =
+            crate::lqos_kernel::interface_name_to_index(&kernel.to_internet)? as i32;
+        let to_isp_ifindex = crate::lqos_kernel::interface_name_to_index(&kernel.to_isp)? as i32;
+        let AttachedPrograms {
+            skeleton,
+            kprobe_link,
+        } = attach_xdp_and_tc_to_interface(
             &kernel.to_internet,
+            to_internet_ifindex,
+            to_isp_ifindex,
+            true,
             InterfaceDirection::Internet,
             heimdall_event_handler,
             flowbee_event_handler,
         )?;
         attach_xdp_and_tc_to_interface(
             &kernel.to_isp,
+            to_internet_ifindex,
+            to_isp_ifindex,
+            false,
             InterfaceDirection::IspNetwork,
             heimdall_event_handler,
             flowbee_event_handler,
         )?;
-        BPF_SKELETON
-            .lock()
-            .replace(LqosKernBpfWrapper { ptr: skeleton });
+        BPF_SKELETON.lock().replace(LqosKernBpfWrapper {
+            ptr: skeleton,
+            kprobe_link,
+        });
         Ok(kernel)
     }
 
@@ -100,21 +122,32 @@ impl LibreQoSKernels {
             to_isp: String::new(),
             on_a_stick: true,
         };
-        let skeleton = attach_xdp_and_tc_to_interface(
+        let stick_ifindex =
+            crate::lqos_kernel::interface_name_to_index(&kernel.to_internet)? as i32;
+        let AttachedPrograms {
+            skeleton,
+            kprobe_link,
+        } = attach_xdp_and_tc_to_interface(
             &kernel.to_internet,
+            stick_ifindex,
+            -1,
+            true,
             InterfaceDirection::OnAStick(internet_vlan, isp_vlan, stick_offset),
             heimdall_event_handler,
             flowbee_event_handler,
         )?;
-        BPF_SKELETON
-            .lock()
-            .replace(LqosKernBpfWrapper { ptr: skeleton });
+        BPF_SKELETON.lock().replace(LqosKernBpfWrapper {
+            ptr: skeleton,
+            kprobe_link,
+        });
         Ok(kernel)
     }
 }
 
 impl Drop for LibreQoSKernels {
     fn drop(&mut self) {
+        let skeleton = BPF_SKELETON.lock().take();
+        drop(skeleton);
         if !self.on_a_stick {
             let _ = unload_xdp_from_interface(&self.to_internet);
             let _ = unload_xdp_from_interface(&self.to_isp);

src/rust/lqos_sys/src/lqos_kernel.rs

@@ -282,12 +282,20 @@ pub enum InterfaceDirection {
     OnAStick(u16, u16, u32),
 }
 
+pub(crate) struct AttachedPrograms {
+    pub(crate) skeleton: *mut lqos_kern,
+    pub(crate) kprobe_link: *mut bpf::bpf_link,
+}
+
 pub fn attach_xdp_and_tc_to_interface(
     interface_name: &str,
+    to_internet_ifindex: i32,
+    to_isp_ifindex: i32,
+    attach_xmit_kprobe: bool,
     direction: InterfaceDirection,
     heimdall_event_handler: bpf::ring_buffer_sample_fn,
     flowbee_event_handler: bpf::ring_buffer_sample_fn,
-) -> Result<*mut lqos_kern> {
+) -> Result<AttachedPrograms> {
     check_root()?;
     // If ABI changes were made to pinned maps, ensure we do not silently reuse
     // incompatible versions that truncate struct values.
@@ -303,6 +311,8 @@ pub fn attach_xdp_and_tc_to_interface(
             InterfaceDirection::IspNetwork => 2,
             InterfaceDirection::OnAStick(..) => 3,
         };
+        (*(*skeleton).data).to_internet_ifindex = to_internet_ifindex;
+        (*(*skeleton).data).to_isp_ifindex = to_isp_ifindex;
         if let InterfaceDirection::OnAStick(internet, isp, stick_offset) = direction {
             (*(*skeleton).bss).internet_vlan = internet.to_be();
             (*(*skeleton).bss).isp_vlan = isp.to_be();
@@ -315,6 +325,15 @@ pub fn attach_xdp_and_tc_to_interface(
         attach_xdp_best_available(interface_index, prog_fd, interface_name)?;
         skeleton
     };
+    let kprobe_link = if attach_xmit_kprobe {
+        let link = unsafe { bpf::attach_xmit_kprobe(skeleton) };
+        if link.is_null() {
+            return Err(Error::msg("Unable to attach kprobe to dev_hard_start_xmit"));
+        }
+        link
+    } else {
+        std::ptr::null_mut()
+    };
 
     // Configure CPU Maps
     let shaping_physical_cpus: Vec<u32> = match lqos_config::load_config() {
@@ -477,7 +496,10 @@ pub fn attach_xdp_and_tc_to_interface(
         }
     }
 
-    Ok(skeleton)
+    Ok(AttachedPrograms {
+        skeleton,
+        kprobe_link,
+    })
 }
 
 /// Safety: Direct calls to C functions