Implement filter setting

1 files changed, 36 insertions, 3 deletions

diff --git a/bpf-interface.scm b/bpf-interface.scm
index 2022b52..5a90e22 100644
--- a/bpf-interface.scm
+++ b/bpf-interface.scm
@@ -4,17 +4,16 @@
 ;;; Copyright (c) 2013 by Peter Bex, see file COPYING.BSD
 ;;;
 ;;; TODO: Think about FreeBSD/OpenBSD extensions like LOCK, DIRFILT.
-;;;       also NetBSD's "number of accepted packets" in GSTATS.
 ;;;       OS X seems to support only the smallest common denominator.
 (module bpf-interface
   (bpf-open bpf-close bpf? bpf-buffer-length bpf-flush!
             bpf-interface bpf-interface-set! bpf-stats
             bpf-datalink-type bpf-datalink-type-set! bpf-list-datalink-types
-            bpf-read-timeout bpf-read-timeout-set!)
+            bpf-read-timeout bpf-read-timeout-set! bpf-filter-set!)
 
 (import chicken scheme foreign)
 
-(use posix lolevel srfi-1)
+(use posix lolevel srfi-1 srfi-4 bitstring)
 
 (foreign-declare "#include <errno.h>")
 (foreign-declare "#include <string.h>")
@@ -210,4 +209,38 @@
         (cond-expand (netbsd (add-stat! captured "bs_capt")))
         result))))
 
+;; Maybe success should be cached inside the bpf object?
+(define (check-version! bpf)
+  (let ((expected-major (foreign-value "BPF_MAJOR_VERSION" int))
+        (expected-minor (foreign-value "BPF_MINOR_VERSION" int))
+        (v (make-blob (foreign-value "sizeof(struct bpf_version)" int))))
+    (ioctl (bpf-fd bpf) (foreign-value "BIOCVERSION" int) (location v))
+    (let-location ((major int) (minor int))
+      ((foreign-lambda* void (((c-pointer "struct bpf_version") v)
+                              ((c-pointer int) major) ((c-pointer int) minor))
+         "*major = v->bv_major;"
+         "*minor = v->bv_minor;")
+       (location v) (location major) (location minor))
+      ;; "Version numbers are compatible if the major numbers match and
+      ;; the application minor is less than or equal to the kernel minor."
+      (unless (and (= major expected-major)
+                   (<= expected-minor minor))
+        (error "BPF filter language version mismatch!"
+               (cons major minor)
+               (cons expected-major expected-minor))))))
+
+(define (bpf-filter-set! bpf filter)
+  (check-version! bpf)
+  (let ((prog (make-blob (foreign-value "sizeof(struct bpf_program)" int)))
+        (insns (cond ((bitstring? filter) (bitstring->blob filter))
+                     ((or (blob? filter) (string? filter)) filter)
+                     ((u8vector? filter) (u8vector->blob/shared filter))
+                     (else (error "Unsupported filter data type" filter)))))
+    ((foreign-lambda* void (((c-pointer "struct bpf_program") p)
+                            (scheme-pointer i) (unsigned-int len))
+       "p->bf_len = len;"
+       "p->bf_insns = i;") (location prog) insns (fx/ (##sys#size insns) 8))
+    (ioctl (bpf-fd bpf) (foreign-value "BIOCSETF" int) (location prog))
+    (void)))
+
 )