diff options
| -rw-r--r-- | package/madwifi/patches/416-wprobe.patch | 85 | ||||
| -rw-r--r-- | package/wprobe/Makefile | 15 | ||||
| -rw-r--r-- | package/wprobe/src/Makefile.inc | 12 | ||||
| -rw-r--r-- | package/wprobe/src/exporter/Makefile | 3 | ||||
| -rw-r--r-- | package/wprobe/src/exporter/wprobe-export.c | 2 | ||||
| -rw-r--r-- | package/wprobe/src/filter/README.txt | 1 | ||||
| -rwxr-xr-x | package/wprobe/src/filter/gen_filter.pl | 63 | ||||
| -rw-r--r-- | package/wprobe/src/filter/pfc.c | 58 | ||||
| -rw-r--r-- | package/wprobe/src/kernel/linux/wprobe.h | 81 | ||||
| -rw-r--r-- | package/wprobe/src/kernel/wprobe-core.c | 353 | ||||
| -rw-r--r-- | package/wprobe/src/user/Makefile | 31 | ||||
| -rw-r--r-- | package/wprobe/src/user/wprobe-info.c | 210 | ||||
| -rw-r--r-- | package/wprobe/src/user/wprobe-lib.c | 1203 | ||||
| -rw-r--r-- | package/wprobe/src/user/wprobe-util.c | 441 | ||||
| -rw-r--r-- | package/wprobe/src/user/wprobe.c | 571 | ||||
| -rw-r--r-- | package/wprobe/src/user/wprobe.h | 60 |
16 files changed, 2358 insertions, 831 deletions
diff --git a/package/madwifi/patches/416-wprobe.patch b/package/madwifi/patches/416-wprobe.patch index aa0c638c2..73cf85fae 100644 --- a/package/madwifi/patches/416-wprobe.patch +++ b/package/madwifi/patches/416-wprobe.patch @@ -1,6 +1,6 @@ --- /dev/null +++ b/ath/ath_wprobe.c -@@ -0,0 +1,392 @@ +@@ -0,0 +1,433 @@ +#include <net80211/ieee80211_node.h> +#include <linux/wprobe.h> + @@ -206,10 +206,38 @@ + if ((rate < 0) || (rate >= rt->rateCount)) + return -1; + -+ return rt->info[rate].rateKbps / 10; ++ return rt->info[rate].rateKbps; +} + +static void ++ath_wprobe_report_rx(struct ieee80211vap *vap, struct ath_rx_status *rs, struct sk_buff *skb) ++{ ++ const struct ieee80211_frame *wh = (struct ieee80211_frame *)skb->data; ++ struct wprobe_wlan_hdr hdr; ++ struct ath_vap *avp; ++ int hdrsize; ++ ++ if (wprobe_disabled()) ++ return; ++ ++ avp = ATH_VAP(vap); ++ avp->av_rxframes++; ++ if (wh->i_fc[0] == (IEEE80211_FC0_TYPE_MGT | IEEE80211_FC0_SUBTYPE_PROBE_REQ)) ++ avp->av_rxprobereq++; ++ ++ memset(&hdr, 0, sizeof(hdr)); ++ hdr.len = skb->len; ++ hdr.snr = rs->rs_rssi; ++ hdr.type = WPROBE_PKT_RX; ++ if ((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) == IEEE80211_FC0_TYPE_CTL) ++ hdrsize = sizeof(struct ieee80211_ctlframe_addr2); ++ else ++ hdrsize = ieee80211_hdrsize(skb->data); ++ wprobe_add_frame(&avp->av_wpif, &hdr, skb->data, hdrsize + 0x42); ++} ++ ++ ++static void +ath_node_sample_rx(struct ieee80211_node *ni, struct ath_rx_status *rs) +{ + struct ath_node *an = ATH_NODE(ni); @@ -237,7 +265,33 @@ +} + +static void -+ath_node_sample_tx(struct ieee80211_node *ni, struct ath_tx_status *ts, int len) ++ath_wprobe_report_tx(struct ieee80211vap *vap, struct ath_tx_status *ts, struct sk_buff *skb) ++{ ++ const struct ieee80211_frame *wh = (struct ieee80211_frame *)skb->data; ++ struct wprobe_wlan_hdr hdr; ++ struct ath_vap *avp; ++ int hdrsize; ++ ++ if (wprobe_disabled()) ++ return; ++ ++ avp = ATH_VAP(vap); ++ ++ memset(&hdr, 0, sizeof(hdr)); ++ hdr.len = skb->len; ++ hdr.snr = ts->ts_rssi; ++ hdr.type = WPROBE_PKT_TX; ++ if ((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) == IEEE80211_FC0_TYPE_CTL) ++ hdrsize = sizeof(struct ieee80211_ctlframe_addr2); ++ else ++ hdrsize = ieee80211_hdrsize(skb->data); ++ wprobe_add_frame(&avp->av_wpif, &hdr, skb->data, hdrsize + 0x42); ++} ++ ++ ++ ++static void ++ath_node_sample_tx(struct ieee80211_node *ni, struct ath_tx_status *ts, struct sk_buff *skb) +{ + struct ath_node *an = ATH_NODE(ni); + struct ieee80211vap *vap = ni->ni_vap; @@ -246,10 +300,12 @@ + struct wprobe_value *v = l->val; + unsigned long flags; + int rate, rexmit_counter; ++ int len = skb->len; + + if (wprobe_disabled() || !an->an_wplink_active || !l->val) + return; + ++ ath_wprobe_report_tx(vap, ts, skb); + rate = ath_lookup_rateval(ni, ts->ts_rate); + + spin_lock_irqsave(&l->iface->lock, flags); @@ -275,21 +331,6 @@ +} + +static void -+ath_wprobe_report_rx(struct ieee80211vap *vap, struct sk_buff *skb) -+{ -+ struct ieee80211_frame *wh = (struct ieee80211_frame *)skb->data; -+ struct ath_vap *avp; -+ -+ if (wprobe_disabled()) -+ return; -+ -+ avp = ATH_VAP(vap); -+ avp->av_rxframes++; -+ if (wh->i_fc[0] == (IEEE80211_FC0_TYPE_MGT | IEEE80211_FC0_SUBTYPE_PROBE_REQ)) -+ avp->av_rxprobereq++; -+} -+ -+static void +ath_wprobe_node_join(struct ieee80211vap *vap, struct ieee80211_node *ni) +{ + struct wprobe_iface *dev; @@ -432,7 +473,7 @@ } ATH_RSSI_LPF(ATH_NODE(ni)->an_avgrssi, rs->rs_rssi); + ath_node_sample_rx(ni, rs); -+ ath_wprobe_report_rx(ni->ni_vap, skb); ++ ath_wprobe_report_rx(ni->ni_vap, rs, skb); type = ieee80211_input(ni->ni_vap, ni, skb, rs->rs_rssi, bf->bf_tsf); ieee80211_unref_node(&ni); } else { @@ -442,12 +483,12 @@ vap = ieee80211_find_rxvap(ic, wh->i_addr1); - if (vap) + if (vap) { -+ ath_wprobe_report_rx(vap, skb); ++ ath_wprobe_report_rx(vap, rs, skb); ni = ieee80211_find_rxnode(ic, vap, wh); - else + } else { + TAILQ_FOREACH(vap, &ic->ic_vaps, iv_next) { -+ ath_wprobe_report_rx(vap, skb); ++ ath_wprobe_report_rx(vap, rs, skb); + } + vap = NULL; ni = NULL; @@ -465,7 +506,7 @@ sc->sc_stats.ast_tx_rssi = ts->ts_rssi; ATH_RSSI_LPF(an->an_halstats.ns_avgtxrssi, ts->ts_rssi); -+ ath_node_sample_tx(&an->an_node, ts, bf->bf_skb->len); ++ ath_node_sample_tx(&an->an_node, ts, bf->bf_skb); if (bf->bf_skb->priority == WME_AC_VO || bf->bf_skb->priority == WME_AC_VI) ni->ni_ic->ic_wme.wme_hipri_traffic++; diff --git a/package/wprobe/Makefile b/package/wprobe/Makefile index a28ad930d..1e32eee0c 100644 --- a/package/wprobe/Makefile +++ b/package/wprobe/Makefile @@ -30,22 +30,22 @@ define KernelPackage/wprobe/description A module that exports measurement data from wireless driver to user space endef -define Package/wprobe-info +define Package/wprobe-util SECTION:=net CATEGORY:=Network DEPENDS:=+kmod-wprobe +libnl-tiny TITLE:=Wireless measurement utility endef -define Package/wprobe-info/description - wprobe-info uses the wprobe kernel module to query +define Package/wprobe-util/description + wprobe-util uses the wprobe kernel module to query wireless driver measurement data from an interface endef define Package/wprobe-export SECTION:=net CATEGORY:=Network - DEPENDS:=+wprobe-info + DEPENDS:=+wprobe-util TITLE:=Wireless measurement data exporter endef @@ -82,6 +82,7 @@ define Build/Compile/lib CFLAGS="$(TARGET_CFLAGS)" \ CPPFLAGS="$(TARGET_CPPFLAGS) -I$(PKG_BUILD_DIR)/kernel" \ LDFLAGS="$(TARGET_LDFLAGS)" \ + HOST_OS=Linux \ LIBNL="-lnl-tiny" endef @@ -107,9 +108,9 @@ define Build/InstallDev $(CP) $(PKG_BUILD_DIR)/kernel/linux $(1)/usr/include/wprobe endef -define Package/wprobe-info/install +define Package/wprobe-util/install $(INSTALL_DIR) $(1)/sbin - $(INSTALL_BIN) $(PKG_BUILD_DIR)/user/wprobe-info $(1)/sbin/ + $(INSTALL_BIN) $(PKG_BUILD_DIR)/user/wprobe-util $(1)/sbin/ endef define Package/wprobe-export/install @@ -120,5 +121,5 @@ define Package/wprobe-export/install endef $(eval $(call KernelPackage,wprobe)) -$(eval $(call BuildPackage,wprobe-info)) +$(eval $(call BuildPackage,wprobe-util)) $(eval $(call BuildPackage,wprobe-export)) diff --git a/package/wprobe/src/Makefile.inc b/package/wprobe/src/Makefile.inc new file mode 100644 index 000000000..05a2dd979 --- /dev/null +++ b/package/wprobe/src/Makefile.inc @@ -0,0 +1,12 @@ +HOST_OS=$(shell uname) + +CC=gcc +AR=ar +RANLIB=ranlib + +WFLAGS = -Wall -Werror +CFLAGS?=-O2 +CPPFLAGS= +LDFLAGS= +LIBS= + diff --git a/package/wprobe/src/exporter/Makefile b/package/wprobe/src/exporter/Makefile index c8e489a6b..9f8150729 100644 --- a/package/wprobe/src/exporter/Makefile +++ b/package/wprobe/src/exporter/Makefile @@ -1,2 +1,5 @@ +include ../Makefile.inc +CPPFLAGS += -I../kernel -I../user + wprobe-export: wprobe-export.c $(CC) $(CPPFLAGS) $(CFLAGS) -o $@ $^ $(LDFLAGS) $(LIBS) diff --git a/package/wprobe/src/exporter/wprobe-export.c b/package/wprobe/src/exporter/wprobe-export.c index fa32e8b28..b09260692 100644 --- a/package/wprobe/src/exporter/wprobe-export.c +++ b/package/wprobe/src/exporter/wprobe-export.c @@ -252,7 +252,7 @@ int main ( int argc, char **argv ) return -1; } - dev = wprobe_get_dev(ifname); + dev = wprobe_get_auto(ifname); if (!dev || (list_empty(&dev->global_attr) && list_empty(&dev->link_attr))) { fprintf(stderr, "Cannot connect to wprobe on interface '%s'\n", ifname); return -1; diff --git a/package/wprobe/src/filter/README.txt b/package/wprobe/src/filter/README.txt new file mode 100644 index 000000000..6fa265e4f --- /dev/null +++ b/package/wprobe/src/filter/README.txt @@ -0,0 +1 @@ +To compile pfc you need at least libpcap version 1.0, as it requires proper radiotap header support diff --git a/package/wprobe/src/filter/gen_filter.pl b/package/wprobe/src/filter/gen_filter.pl new file mode 100755 index 000000000..f03f477a4 --- /dev/null +++ b/package/wprobe/src/filter/gen_filter.pl @@ -0,0 +1,63 @@ +#!/usr/bin/perl +use strict; + +# helpers for custom packet format +# bytes 0-7 are used by a dummy radiotap header +my $WLAN_LEN = "radio[8:2]"; +my $SNR = "radio[10:1]"; +my $DEFAULT = undef; + +my $MAGIC = "WPFF"; +my $VERSION = 1; # filter binary format version +my $HDRLEN = 3; # assumed storage space for custom fields + +my $output = "filter.bin"; +my $config = { + "packetsize" => [ + [ "small", "$WLAN_LEN < 250" ], + [ "medium", "$WLAN_LEN < 800" ], + [ "big", $DEFAULT ], + ], + "snr" => [ + [ "low", "$SNR < 10" ], + [ "medium", "$SNR < 20" ], + [ "high", $DEFAULT ], + ], + "type" => [ + [ "beacon", "type mgt subtype beacon" ], + [ "data", "type data subtype data" ], + [ "qosdata", "type data subtype qos-data" ], + [ "other", "$DEFAULT" ] + ] +}; + +sub escape_q($) { + my $str = shift; + $str =~ s/'/'\\''/g; + return $str; +} + +my $GROUPS = scalar(keys %$config); +open OUTPUT, ">$output" or die "Cannot open output file: $!\n"; +print OUTPUT pack("a4CCn", $MAGIC, $VERSION, $HDRLEN, $GROUPS); + +foreach my $groupname (keys %$config) { + my $default = 0; + my $group = $config->{$groupname}; + print OUTPUT pack("a32N", $groupname, scalar(@$group)); + foreach my $filter (@$group) { + if (!$filter->[1]) { + $default > 0 and print "Cannot add more than one default filter per group: $groupname -> ".$filter->[0]."\n"; + print OUTPUT pack("a32N", $filter->[0], 0); + $default++; + } else { + open FILTER, "./pfc '".escape_q($filter->[0])."' '".escape_q($filter->[1])."' |" + or die "Failed to run filter command for '".$filter->[0]."': $!\n"; + while (<FILTER>) { + print OUTPUT $_; + } + close FILTER; + $? and die "Filter '".$filter->[0]."' did not compile.\n"; + } + } +} diff --git a/package/wprobe/src/filter/pfc.c b/package/wprobe/src/filter/pfc.c new file mode 100644 index 000000000..76fb1412a --- /dev/null +++ b/package/wprobe/src/filter/pfc.c @@ -0,0 +1,58 @@ +#include <arpa/inet.h> +#include <sys/types.h> +#include <sys/time.h> +#include <string.h> +#include <stdint.h> +#include <stdlib.h> + +#include <pcap.h> +#include <pcap-bpf.h> + +struct wprobe_filter_hdr { + char name[32]; + uint32_t len; +} hdr; + +int main (int argc, char ** argv) +{ + struct bpf_program filter; + pcap_t *pc; + int i; + + if (argc != 3) + { + fprintf(stderr, "Usage: %s <name> <expression>\n", argv[0]); + return 1; + } + + pc = pcap_open_dead(DLT_IEEE802_11_RADIO, 256); + if (pcap_compile(pc, &filter, argv[2], 1, 0) != 0) + { + pcap_perror(pc, argv[0]); + exit(1); + } + + /* fix up for linux */ + for (i = 0; i < filter.bf_len; i++) { + struct bpf_insn *bi = &filter.bf_insns[i]; + switch(BPF_CLASS(bi->code)) { + case BPF_RET: + if (BPF_MODE(bi->code) == BPF_K) { + if (bi->k != 0) + bi->k = 65535; + } + break; + } + bi->code = ntohs(bi->code); + bi->k = ntohl(bi->k); + } + + memset(&hdr, 0, sizeof(hdr)); + strncpy(hdr.name, argv[1], sizeof(hdr.name)); + hdr.len = htonl(filter.bf_len); + fwrite(&hdr, sizeof(hdr), 1, stdout); + fwrite(filter.bf_insns, 8, filter.bf_len, stdout); + fflush(stdout); + + return 0; +} diff --git a/package/wprobe/src/kernel/linux/wprobe.h b/package/wprobe/src/kernel/linux/wprobe.h index 9536a929a..901daf3d1 100644 --- a/package/wprobe/src/kernel/linux/wprobe.h +++ b/package/wprobe/src/kernel/linux/wprobe.h @@ -23,6 +23,7 @@ #include <linux/module.h> #include <linux/list.h> #include <linux/timer.h> +#include <linux/filter.h> #include <net/genetlink.h> #endif @@ -103,6 +104,11 @@ enum wprobe_attr { WPROBE_ATTR_SAMPLES_MAX, WPROBE_ATTR_SAMPLES_SCALE_M, WPROBE_ATTR_SAMPLES_SCALE_D, + WPROBE_ATTR_FILTER, + + WPROBE_ATTR_FILTER_GROUP, + WPROBE_ATTR_RXCOUNT, + WPROBE_ATTR_TXCOUNT, WPROBE_ATTR_LAST }; @@ -118,6 +124,8 @@ enum wprobe_attr { * @WPROBE_CMD_SET_FLAGS: set global/link flags * @WPROBE_CMD_MEASURE: take a snapshot of the current data * @WPROBE_CMD_GET_LINKS: get a list of links + * @WPROBE_CMD_CONFIG: set config options + * @WPROBE_CMD_GET_FILTER: get counters for active filters * * @WPROBE_CMD_LAST: unused * @@ -133,13 +141,14 @@ enum wprobe_cmd { WPROBE_CMD_MEASURE, WPROBE_CMD_GET_LINKS, WPROBE_CMD_CONFIG, + WPROBE_CMD_GET_FILTER, WPROBE_CMD_LAST }; /** * enum wprobe_flags: flags for wprobe links and items * @WPROBE_F_KEEPSTAT: keep statistics for this link/device - * @WPROBE_F_RESET: reset statistics now (used only in WPROBE_CMD_SET_LINK) + * @WPROBE_F_RESET: reset statistics now * @WPROBE_F_NEWDATA: used to indicate that a value has been updated */ enum wprobe_flags { @@ -153,6 +162,7 @@ enum wprobe_flags { struct wprobe_link; struct wprobe_item; struct wprobe_source; +struct wprobe_value; /** * struct wprobe_link - data structure describing a wireless link @@ -170,7 +180,7 @@ struct wprobe_link { char addr[ETH_ALEN]; u32 flags; void *priv; - void *val; + struct wprobe_value *val; }; /** @@ -211,6 +221,58 @@ struct wprobe_value { u64 scale_timestamp; }; +struct wprobe_filter_item_hdr { + char name[32]; + __be32 n_items; +} __attribute__((packed)); + +struct wprobe_filter_item { + struct wprobe_filter_item_hdr hdr; + struct sock_filter filter[]; +} __attribute__((packed)); + +struct wprobe_filter_counter { + u64 tx; + u64 rx; +}; + +struct wprobe_filter_group { + const char *name; + int n_items; + struct wprobe_filter_item **items; + struct wprobe_filter_counter *counters; +}; + +struct wprobe_filter_hdr { + __u8 magic[4]; + __u8 version; + __u8 hdrlen; + __u16 n_groups; +} __attribute__((packed)); + +struct wprobe_filter { + spinlock_t lock; + struct sk_buff *skb; + void *data; + int n_groups; + int hdrlen; + struct wprobe_filter_item **items; + struct wprobe_filter_counter *counters; + struct wprobe_filter_group groups[]; +}; + +enum { + WPROBE_PKT_RX = 0x00, + WPROBE_PKT_TX = 0x10, +}; + +struct wprobe_wlan_hdr { + u16 len; + u8 snr; + u8 type; +} __attribute__((packed)); + + /** * struct wprobe_source - data structure describing a wireless interface * @@ -250,8 +312,9 @@ struct wprobe_iface { struct list_head list; struct list_head links; spinlock_t lock; - void *val; - void *query_val; + struct wprobe_value *val; + struct wprobe_value *query_val; + struct wprobe_filter *active_filter; u32 measure_interval; struct timer_list measure_timer; @@ -262,6 +325,7 @@ struct wprobe_iface { u32 scale_d; }; + #define WPROBE_FILL_BEGIN(_ptr, _list) do { \ struct wprobe_value *__val = (_ptr); \ const struct wprobe_item *__item = _list; \ @@ -319,6 +383,15 @@ extern void __weak wprobe_remove_link(struct wprobe_iface *dev, struct wprobe_li */ extern void __weak wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l); +/** + * wprobe_add_frame: add frame for layer 2 analysis + * @dev: wprobe_iface structure describing the interface + * @hdr: metadata for the frame + * @data: 802.11 header pointer + * @len: length of the 802.11 header + */ +extern int __weak wprobe_add_frame(struct wprobe_iface *dev, const struct wprobe_wlan_hdr *hdr, void *data, int len); + #endif /* __KERNEL__ */ #endif diff --git a/package/wprobe/src/kernel/wprobe-core.c b/package/wprobe/src/kernel/wprobe-core.c index 51ee7bc1d..d8f5a16f6 100644 --- a/package/wprobe/src/kernel/wprobe-core.c +++ b/package/wprobe/src/kernel/wprobe-core.c @@ -35,6 +35,8 @@ #endif #define WPROBE_MIN_INTERVAL 100 /* minimum measurement interval in msecs */ +#define WPROBE_MAX_FILTER_SIZE 1024 +#define WPROBE_MAX_FRAME_SIZE 1900 static struct list_head wprobe_if; static spinlock_t wprobe_lock; @@ -48,8 +50,17 @@ static struct genl_family wprobe_fam = { .maxattr = WPROBE_ATTR_LAST, }; +/* fake radiotap header */ +struct wprobe_rtap_hdr { + __u8 version; + __u8 padding; + __le16 len; + __le32 present; +}; + static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l); static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query); +static void wprobe_free_filter(struct wprobe_filter *f); int wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr) @@ -111,11 +122,11 @@ wprobe_add_iface(struct wprobe_iface *s) INIT_LIST_HEAD(&s->links); setup_timer(&s->measure_timer, wprobe_measure_timer, (unsigned long) s); - vsize = max(s->n_link_items, s->n_global_items); - s->val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC); + s->val = kzalloc(sizeof(struct wprobe_value) * s->n_global_items, GFP_ATOMIC); if (!s->val) goto error; + vsize = max(s->n_link_items, s->n_global_items); s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC); if (!s->query_val) goto error; @@ -160,6 +171,8 @@ wprobe_remove_iface(struct wprobe_iface *s) kfree(s->val); kfree(s->query_val); + if (s->active_filter) + wprobe_free_filter(s->active_filter); } EXPORT_SYMBOL(wprobe_remove_iface); @@ -187,6 +200,69 @@ wprobe_get_dev(struct nlattr *attr) return dev; } +int +wprobe_add_frame(struct wprobe_iface *dev, const struct wprobe_wlan_hdr *hdr, void *data, int len) +{ + struct wprobe_filter *f; + struct sk_buff *skb; + unsigned long flags; + int i, j; + + rcu_read_lock(); + f = rcu_dereference(dev->active_filter); + if (!f) + goto out; + + spin_lock_irqsave(&f->lock, flags); + + skb = f->skb; + skb->len = sizeof(struct wprobe_rtap_hdr); + skb->tail = skb->data + skb->len; + if (len + skb->len > WPROBE_MAX_FRAME_SIZE) + len = WPROBE_MAX_FRAME_SIZE - skb->len; + + memcpy(skb_put(skb, f->hdrlen), hdr, sizeof(struct wprobe_wlan_hdr)); + memcpy(skb_put(skb, len), data, len); + + for(i = 0; i < f->n_groups; i++) { + struct wprobe_filter_group *fg = &f->groups[i]; + bool found = false; + int def = -1; + + for (j = 0; j < fg->n_items; j++) { + struct wprobe_filter_item *fi = fg->items[j]; + + if (!fi->hdr.n_items) { + def = j; + continue; + } + if (sk_run_filter(skb, fi->filter, fi->hdr.n_items) == 0) + continue; + + found = true; + break; + } + if (!found && def >= 0) { + j = def; + found = true; + } + if (found) { + struct wprobe_filter_counter *c = &fg->counters[j]; + + if (hdr->type >= WPROBE_PKT_TX) + c->tx++; + else + c->rx++; + } + } + + spin_unlock_irqrestore(&f->lock, flags); +out: + rcu_read_unlock(); + return 0; +} +EXPORT_SYMBOL(wprobe_add_frame); + static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query) { @@ -325,6 +401,7 @@ static const struct nla_policy wprobe_policy[WPROBE_ATTR_LAST+1] = { [WPROBE_ATTR_SAMPLES_MAX] = { .type = NLA_U32 }, [WPROBE_ATTR_SAMPLES_SCALE_M] = { .type = NLA_U32 }, [WPROBE_ATTR_SAMPLES_SCALE_D] = { .type = NLA_U32 }, + [WPROBE_ATTR_FILTER] = { .type = NLA_BINARY, .len = 32768 }, }; static bool @@ -438,6 +515,86 @@ wprobe_find_link(struct wprobe_iface *dev, const char *mac) } static bool +wprobe_dump_filter_group(struct sk_buff *msg, struct wprobe_filter_group *fg, struct netlink_callback *cb) +{ + struct genlmsghdr *hdr; + struct nlattr *group, *item; + int i; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_FILTER); + if (!hdr) + return false; + + NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, fg->name); + group = nla_nest_start(msg, WPROBE_ATTR_FILTER_GROUP); + for (i = 0; i < fg->n_items; i++) { + struct wprobe_filter_item *fi = fg->items[i]; + struct wprobe_filter_counter *fc = &fg->counters[i]; + + item = nla_nest_start(msg, WPROBE_ATTR_FILTER_GROUP); + NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, fi->hdr.name); + NLA_PUT_U64(msg, WPROBE_ATTR_RXCOUNT, fc->rx); + NLA_PUT_U64(msg, WPROBE_ATTR_TXCOUNT, fc->tx); + nla_nest_end(msg, item); + } + + nla_nest_end(msg, group); + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + +static int +wprobe_dump_filters(struct sk_buff *skb, struct netlink_callback *cb) +{ + struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0]; + struct wprobe_filter *f; + int err = 0; + int i = 0; + + if (!dev) { + err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize, + wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy); + if (err) + goto done; + + dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]); + if (!dev) { + err = -ENODEV; + goto done; + } + + cb->args[0] = (long) dev; + cb->args[1] = 0; + } else { + if (!wprobe_check_ptr(&wprobe_if, &dev->list)) { + err = -ENODEV; + goto done; + } + } + + rcu_read_lock(); + f = rcu_dereference(dev->active_filter); + if (!f) + goto abort; + + for (i = cb->args[1]; i < f->n_groups; i++) { + if (unlikely(!wprobe_dump_filter_group(skb, &f->groups[i], cb))) + break; + } + cb->args[1] = i; +abort: + rcu_read_unlock(); + err = skb->len; +done: + return err; +} + +static bool wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb) { struct genlmsghdr *hdr; @@ -671,6 +828,158 @@ done: } static int +wprobe_check_filter(void *data, int datalen, int gs) +{ + struct wprobe_filter_item_hdr *hdr; + void *orig_data = data; + void *end = data + datalen; + int i, j, k, is, cur_is; + + for (i = j = is = 0; i < gs; i++) { + hdr = data; + data += sizeof(*hdr); + + if (data > end) + goto overrun; + + hdr->name[31] = 0; + cur_is = be32_to_cpu(hdr->n_items); + is += cur_is; + for (j = 0; j < cur_is; j++) { + struct sock_filter *sf; + int n_items; + + hdr = data; + data += sizeof(*hdr); + if (data > end) + goto overrun; + + if (hdr->n_items > 1024) + goto overrun; + + hdr->name[31] = 0; + hdr->n_items = n_items = be32_to_cpu(hdr->n_items); + sf = data; + if (n_items > 0) { + for (k = 0; k < n_items; k++) { + sf->code = be16_to_cpu(sf->code); + sf->k = be32_to_cpu(sf->k); + sf++; + } + if (sk_chk_filter(data, n_items) != 0) { + printk("%s: filter check failed at group %d, item %d\n", __func__, i, j); + return 0; + } + } + data += n_items * sizeof(struct sock_filter); + } + } + return is; + +overrun: + printk(KERN_ERR "%s: overrun during filter check at group %d, item %d, offset=%d, len=%d\n", __func__, i, j, (data - orig_data), datalen); + return 0; +} + +static void +wprobe_free_filter(struct wprobe_filter *f) +{ + if (f->skb) + kfree_skb(f->skb); + if (f->data) + kfree(f->data); + if (f->items) + kfree(f->items); + if (f->counters) + kfree(f->counters); + kfree(f); +} + + +static int +wprobe_set_filter(struct wprobe_iface *dev, void *data, int len) +{ + struct wprobe_filter_hdr *fhdr; + struct wprobe_rtap_hdr *rtap; + struct wprobe_filter *f; + int i, j, cur_is, is, gs; + + if (len < sizeof(*fhdr)) + return -EINVAL; + + fhdr = data; + data += sizeof(*fhdr); + len -= sizeof(*fhdr); + + if (memcmp(fhdr->magic, "WPFF", 4) != 0) { + printk(KERN_ERR "%s: filter rejected (invalid magic)\n", __func__); + return -EINVAL; + } + + gs = be16_to_cpu(fhdr->n_groups); + is = wprobe_check_filter(data, len, gs); + if (is == 0) + return -EINVAL; + + f = kzalloc(sizeof(struct wprobe_filter) + + gs * sizeof(struct wprobe_filter_group), GFP_ATOMIC); + if (!f) + return -ENOMEM; + + f->skb = alloc_skb(WPROBE_MAX_FRAME_SIZE, GFP_ATOMIC); + if (!f->skb) + goto error; + + f->data = kmalloc(len, GFP_ATOMIC); + if (!f->data) + goto error; + + f->items = kzalloc(sizeof(struct wprobe_filter_item *) * is, GFP_ATOMIC); + if (!f->items) + goto error; + + f->counters = kzalloc(sizeof(struct wprobe_filter_counter) * is, GFP_ATOMIC); + if (!f->counters) + goto error; + + spin_lock_init(&f->lock); + memcpy(f->data, data, len); + f->n_groups = gs; + + if (f->hdrlen < sizeof(struct wprobe_wlan_hdr)) + f->hdrlen = sizeof(struct wprobe_wlan_hdr); + + rtap = (struct wprobe_rtap_hdr *)skb_put(f->skb, sizeof(*rtap)); + memset(rtap, 0, sizeof(*rtap)); + rtap->len = cpu_to_le16(sizeof(struct wprobe_rtap_hdr) + f->hdrlen); + data = f->data; + + cur_is = 0; + for (i = 0; i < gs; i++) { + struct wprobe_filter_item_hdr *hdr = data; + struct wprobe_filter_group *g = &f->groups[i]; + + data += sizeof(*hdr); + g->name = hdr->name; + g->items = &f->items[cur_is]; + g->counters = &f->counters[cur_is]; + g->n_items = hdr->n_items; + + for (j = 0; j < g->n_items; j++) { + hdr = data; + f->items[cur_is++] = data; + data += sizeof(*hdr) + be32_to_cpu(hdr->n_items) * sizeof(struct sock_filter); + } + } + rcu_assign_pointer(dev->active_filter, f); + return 0; + +error: + wprobe_free_filter(f); + return -ENOMEM; +} + +static int wprobe_set_config(struct sk_buff *skb, struct genl_info *info) { struct wprobe_iface *dev; @@ -678,6 +987,8 @@ wprobe_set_config(struct sk_buff *skb, struct genl_info *info) int err = -ENOENT; u32 scale_min, scale_max; u32 scale_m, scale_d; + struct nlattr *attr; + struct wprobe_filter *filter_free = NULL; rcu_read_lock(); dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]); @@ -691,15 +1002,28 @@ wprobe_set_config(struct sk_buff *skb, struct genl_info *info) goto done; } + if (info->attrs[WPROBE_ATTR_FLAGS]) { + u32 flags = nla_get_u32(info->attrs[WPROBE_ATTR_FLAGS]); + + if (flags & BIT(WPROBE_F_RESET)) { + struct wprobe_link *l; + + memset(dev->val, 0, sizeof(struct wprobe_value) * dev->n_global_items); + list_for_each_entry_rcu(l, &dev->links, list) { + memset(l->val, 0, sizeof(struct wprobe_value) * dev->n_link_items); + } + } + } + if (info->attrs[WPROBE_ATTR_SAMPLES_MIN] || info->attrs[WPROBE_ATTR_SAMPLES_MAX]) { - if (info->attrs[WPROBE_ATTR_SAMPLES_MIN]) - scale_min = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_MIN]); + if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MIN])) + scale_min = nla_get_u32(attr); else scale_min = dev->scale_min; - if (info->attrs[WPROBE_ATTR_SAMPLES_MAX]) - scale_max = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_MAX]); + if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MAX])) + scale_max = nla_get_u32(attr); else scale_max = dev->scale_max; @@ -725,6 +1049,13 @@ wprobe_set_config(struct sk_buff *skb, struct genl_info *info) dev->scale_d = scale_d; } + if ((attr = info->attrs[WPROBE_ATTR_FILTER])) { + filter_free = rcu_dereference(dev->active_filter); + rcu_assign_pointer(dev->active_filter, NULL); + if (nla_len(attr) > 0) + wprobe_set_filter(dev, nla_data(attr), nla_len(attr)); + } + err = 0; if (info->attrs[WPROBE_ATTR_INTERVAL]) { /* change of measurement interval requested */ @@ -736,6 +1067,10 @@ done: spin_unlock_irqrestore(&dev->lock, flags); done_unlocked: rcu_read_unlock(); + if (filter_free) { + synchronize_rcu(); + wprobe_free_filter(filter_free); + } return err; } @@ -763,6 +1098,12 @@ static struct genl_ops wprobe_ops[] = { { .cmd = WPROBE_CMD_CONFIG, .doit = wprobe_set_config, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_GET_FILTER, + .dumpit = wprobe_dump_filters, + .policy = wprobe_policy, }, }; diff --git a/package/wprobe/src/user/Makefile b/package/wprobe/src/user/Makefile index e10dfc0d2..01e83da3e 100644 --- a/package/wprobe/src/user/Makefile +++ b/package/wprobe/src/user/Makefile @@ -1,21 +1,38 @@ -CFLAGS = -O2 -CPPFLAGS ?= -I../kernel -WFLAGS = -Wall -Werror +include ../Makefile.inc + +CPPFLAGS += -I../kernel LDFLAGS = +ifneq ($(HOST_OS),Linux) +USE_LIBNL_MICRO=1 +else +USE_LIBNL_MICRO= +endif + +ifeq ($(USE_LIBNL_MICRO),1) +LIBNL_PREFIX = /usr/local +LIBNL = $(LIBNL_PREFIX)/lib/libnl-micro.a +CPPFLAGS += -I$(LIBNL_PREFIX)/include/libnl-micro +EXTRA_CFLAGS += -DNO_LOCAL_ACCESS +else LIBNL = -lnl +endif + LIBM = -lm LIBS = $(LIBNL) $(LIBM) -all: libwprobe.a wprobe-info +all: libwprobe.a wprobe-util -libwprobe.a: wprobe.o +libwprobe.a: wprobe-lib.o rm -f $@ $(AR) rcu $@ $^ $(RANLIB) $@ %.o: %.c - $(CC) $(WFLAGS) -c -o $@ $(CPPFLAGS) $(CFLAGS) $< + $(CC) $(WFLAGS) -c -o $@ $(CPPFLAGS) $(CFLAGS) $(EXTRA_CFLAGS) $< -wprobe-info: wprobe-info.o wprobe.o +wprobe-util: wprobe-util.o wprobe-lib.o $(CC) -o $@ $^ $(LDFLAGS) $(LIBS) + +clean: + rm -f *.o *.a wprobe-util diff --git a/package/wprobe/src/user/wprobe-info.c b/package/wprobe/src/user/wprobe-info.c deleted file mode 100644 index 8361c0275..000000000 --- a/package/wprobe/src/user/wprobe-info.c +++ /dev/null @@ -1,210 +0,0 @@ -/* - * wprobe-test.c: Wireless probe user space test code - * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org> - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - */ - -#include <stdio.h> -#include <string.h> -#include <stdlib.h> -#include <inttypes.h> -#include <errno.h> -#include <stdint.h> -#include <getopt.h> -#include <stdbool.h> -#include <unistd.h> -#include <sys/types.h> -#include <sys/socket.h> -#include <linux/wprobe.h> -#include "wprobe.h" - -static const char * -wprobe_dump_value(struct wprobe_attribute *attr) -{ - static char buf[128]; - -#define HANDLE_TYPE(_type, _format) \ - case WPROBE_VAL_##_type: \ - snprintf(buf, sizeof(buf), _format, attr->val._type); \ - break - - switch(attr->type) { - HANDLE_TYPE(S8, "%d"); - HANDLE_TYPE(S16, "%d"); - HANDLE_TYPE(S32, "%d"); - HANDLE_TYPE(S64, "%lld"); - HANDLE_TYPE(U8, "%d"); - HANDLE_TYPE(U16, "%d"); - HANDLE_TYPE(U32, "%d"); - HANDLE_TYPE(U64, "%lld"); - case WPROBE_VAL_STRING: - /* FIXME: implement this */ - default: - strncpy(buf, "<unknown>", sizeof(buf)); - break; - } - if ((attr->flags & WPROBE_F_KEEPSTAT) && - (attr->val.n > 0)) { - int len = strlen(buf); - snprintf(buf + len, sizeof(buf) - len, " (avg: %.02f; stdev: %.02f, n=%d)", attr->val.avg, attr->val.stdev, attr->val.n); - } -#undef HANDLE_TYPE - - return buf; -} - - -static void -wprobe_dump_data(struct wprobe_iface *dev) -{ - struct wprobe_attribute *attr; - struct wprobe_link *link; - bool first = true; - - fprintf(stderr, "\n"); - wprobe_request_data(dev, NULL); - list_for_each_entry(attr, &dev->global_attr, list) { - fprintf(stderr, (first ? - "Global: %s=%s\n" : - " %s=%s\n"), - attr->name, - wprobe_dump_value(attr) - ); - first = false; - } - - list_for_each_entry(link, &dev->links, list) { - first = true; - wprobe_request_data(dev, link->addr); - list_for_each_entry(attr, &dev->link_attr, list) { - if (first) { - fprintf(stderr, - "%02x:%02x:%02x:%02x:%02x:%02x: %s=%s\n", - link->addr[0], link->addr[1], link->addr[2], - link->addr[3], link->addr[4], link->addr[5], - attr->name, - wprobe_dump_value(attr)); - first = false; - } else { - fprintf(stderr, - " %s=%s\n", - attr->name, - wprobe_dump_value(attr)); - } - } - } -} - -static const char *attr_typestr[] = { - [0] = "Unknown", - [WPROBE_VAL_STRING] = "String", - [WPROBE_VAL_U8] = "Unsigned 8 bit", - [WPROBE_VAL_U16] = "Unsigned 16 bit", - [WPROBE_VAL_U32] = "Unsigned 32 bit", - [WPROBE_VAL_U64] = "Unsigned 64 bit", - [WPROBE_VAL_S8] = "Signed 8 bit", - [WPROBE_VAL_S16] = "Signed 16 bit", - [WPROBE_VAL_S32] = "Signed 32 bit", - [WPROBE_VAL_S64] = "Signed 64 bit", -}; - -static int usage(const char *prog) -{ - fprintf(stderr, - "Usage: %s <interface> [options]\n" - "\n" - "Options:\n" - " -c: Only apply configuration\n" - " -h: This help text\n" - " -i <interval>: Set measurement interval\n" - " -m: Run measurement loop\n" - "\n" - , prog); - exit(1); -} - -static void show_attributes(struct wprobe_iface *dev) -{ - struct wprobe_attribute *attr; - list_for_each_entry(attr, &dev->global_attr, list) { - fprintf(stderr, "Global attribute: '%s' (%s)\n", - attr->name, attr_typestr[attr->type]); - } - list_for_each_entry(attr, &dev->link_attr, list) { - fprintf(stderr, "Link attribute: '%s' (%s)\n", - attr->name, attr_typestr[attr->type]); - } -} - -static void loop_measurement(struct wprobe_iface *dev) -{ - while (1) { - sleep(1); - wprobe_update_links(dev); - wprobe_dump_data(dev); - } -} - -int main(int argc, char **argv) -{ - struct wprobe_iface *dev; - const char *ifname; - const char *prog = argv[0]; - enum { - CMD_NONE, - CMD_CONFIG, - CMD_MEASURE, - } cmd = CMD_NONE; - int ch; - - if ((argc < 2) || (argv[1][0] == '-')) - return usage(prog); - - ifname = argv[1]; - dev = wprobe_get_dev(ifname); - argv++; - argc--; - - if (!dev || (list_empty(&dev->global_attr) && - list_empty(&dev->link_attr))) { - fprintf(stderr, "Interface '%s' not found\n", ifname); - return -1; - } - - while ((ch = getopt(argc, argv, "chi:m")) != -1) { - switch(ch) { - case 'c': - cmd = CMD_CONFIG; - break; - case 'm': - cmd = CMD_MEASURE; - break; - case 'i': - dev->interval = strtoul(optarg, NULL, 10); - break; - case 'h': - default: - usage(prog); - break; - } - } - - wprobe_apply_config(dev); - if (cmd != CMD_CONFIG) - show_attributes(dev); - if (cmd == CMD_MEASURE) - loop_measurement(dev); - - wprobe_free_dev(dev); - - return 0; -} diff --git a/package/wprobe/src/user/wprobe-lib.c b/package/wprobe/src/user/wprobe-lib.c new file mode 100644 index 000000000..a3b0fb5e5 --- /dev/null +++ b/package/wprobe/src/user/wprobe-lib.c @@ -0,0 +1,1203 @@ +/* + * wprobe.c: Wireless probe user space library + * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org> + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include <sys/types.h> +#include <sys/socket.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <getopt.h> +#include <unistd.h> +#include <stdbool.h> +#include <math.h> +#include <linux/wprobe.h> +#include <netlink/netlink.h> +#include <netlink/attr.h> +#include <netlink/genl/genl.h> +#ifndef NO_LOCAL_ACCESS +#include <netlink/genl/ctrl.h> +#include <netlink/genl/family.h> +#include <endian.h> +#endif +#include "wprobe.h" + +#define DEBUG 1 +#ifdef DEBUG +#define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__) +#else +#define DPRINTF(fmt, ...) do {} while (0) +#endif + +#if defined(BYTE_ORDER) && !defined(__BYTE_ORDER) +#define __LITTLE_ENDIAN LITTLE_ENDIAN +#define __BIG_ENDIAN BIG_ENDIAN +#define __BYTE_ORDER BYTE_ORDER +#endif + +#ifndef __BYTE_ORDER +#error Unknown endian type +#endif + +#define WPROBE_MAX_MSGLEN 65536 + +static inline __u16 __swab16(__u16 x) +{ + return x<<8 | x>>8; +} + +static inline __u32 __swab32(__u32 x) +{ + return x<<24 | x>>24 | + (x & (__u32)0x0000ff00UL)<<8 | + (x & (__u32)0x00ff0000UL)>>8; +} + +static inline __u64 __swab64(__u64 x) +{ + return x<<56 | x>>56 | + (x & (__u64)0x000000000000ff00ULL)<<40 | + (x & (__u64)0x0000000000ff0000ULL)<<24 | + (x & (__u64)0x00000000ff000000ULL)<< 8 | + (x & (__u64)0x000000ff00000000ULL)>> 8 | + (x & (__u64)0x0000ff0000000000ULL)>>24 | + (x & (__u64)0x00ff000000000000ULL)>>40; +} + + +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define SWAP16(var) var = __swab16(var) +#define SWAP32(var) var = __swab32(var) +#define SWAP64(var) var = __swab64(var) +#else +#define SWAP16(var) do {} while(0) +#define SWAP32(var) do {} while(0) +#define SWAP64(var) do {} while(0) +#endif + +int wprobe_port = 17990; +static struct nlattr *tb[WPROBE_ATTR_LAST+1]; +static struct nla_policy attribute_policy[WPROBE_ATTR_LAST+1] = { + [WPROBE_ATTR_ID] = { .type = NLA_U32 }, + [WPROBE_ATTR_MAC] = { .type = NLA_UNSPEC, .minlen = 6, .maxlen = 6 }, + [WPROBE_ATTR_NAME] = { .type = NLA_STRING }, + [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 }, + [WPROBE_ATTR_TYPE] = { .type = NLA_U8 }, + [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 }, + [WPROBE_VAL_S8] = { .type = NLA_U8 }, + [WPROBE_VAL_S16] = { .type = NLA_U16 }, + [WPROBE_VAL_S32] = { .type = NLA_U32 }, + [WPROBE_VAL_S64] = { .type = NLA_U64 }, + [WPROBE_VAL_U8] = { .type = NLA_U8 }, + [WPROBE_VAL_U16] = { .type = NLA_U16 }, + [WPROBE_VAL_U32] = { .type = NLA_U32 }, + [WPROBE_VAL_U64] = { .type = NLA_U64 }, + [WPROBE_VAL_SUM] = { .type = NLA_U64 }, + [WPROBE_VAL_SUM_SQ] = { .type = NLA_U64 }, + [WPROBE_VAL_SAMPLES] = { .type = NLA_U32 }, + [WPROBE_VAL_SCALE_TIME] = { .type = NLA_U64 }, + [WPROBE_ATTR_INTERVAL] = { .type = NLA_U64 }, + [WPROBE_ATTR_SAMPLES_MIN] = { .type = NLA_U32 }, + [WPROBE_ATTR_SAMPLES_MAX] = { .type = NLA_U32 }, + [WPROBE_ATTR_SAMPLES_SCALE_M] = { .type = NLA_U32 }, + [WPROBE_ATTR_SAMPLES_SCALE_D] = { .type = NLA_U32 }, + [WPROBE_ATTR_FILTER_GROUP] = { .type = NLA_NESTED }, + [WPROBE_ATTR_RXCOUNT] = { .type = NLA_U64 }, + [WPROBE_ATTR_TXCOUNT] = { .type = NLA_U64 }, +}; + +typedef int (*wprobe_cb_t)(struct nl_msg *, void *); + +struct wprobe_iface_ops { + int (*send_msg)(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t cb, void *arg); + void (*free)(struct wprobe_iface *dev); +}; + +struct wprobe_attr_cb { + struct list_head *list; + char *addr; +}; + +#define WPROBE_MAGIC_STR "WPROBE" +struct wprobe_init_hdr { + struct { + char magic[sizeof(WPROBE_MAGIC_STR)]; + + /* protocol version */ + uint8_t version; + + /* extra header length (unused for now) */ + uint16_t extra; + } pre __attribute__((packed)); + union { + struct { + uint16_t genl_family; + } v0 __attribute__((packed)); + }; +} __attribute__((packed)); + +struct wprobe_msg_hdr { + __u16 status; + __u16 error; + __u32 len; +}; + +enum wprobe_resp_status { + WPROBE_MSG_DONE = 0, + WPROBE_MSG_DATA = 1, +}; + +static inline void +wprobe_swap_msg_hdr(struct wprobe_msg_hdr *mhdr) +{ + SWAP16(mhdr->status); + SWAP16(mhdr->error); + SWAP32(mhdr->len); +} + +static int +save_attribute_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + const char *name = "N/A"; + struct wprobe_attribute *attr; + int type = 0; + struct wprobe_attr_cb *cb = arg; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (tb[WPROBE_ATTR_NAME]) + name = nla_data(tb[WPROBE_ATTR_NAME]); + + attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1); + if (!attr) + return -1; + + memset(attr, 0, sizeof(struct wprobe_attribute)); + + if (tb[WPROBE_ATTR_ID]) + attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]); + + if (tb[WPROBE_ATTR_MAC] && cb->addr) + memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6); + + if (tb[WPROBE_ATTR_FLAGS]) + attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]); + + if (tb[WPROBE_ATTR_TYPE]) + type = nla_get_u8(tb[WPROBE_ATTR_TYPE]); + + if ((type < WPROBE_VAL_STRING) || + (type > WPROBE_VAL_U64)) + type = 0; + + attr->type = type; + strcpy(attr->name, name); + INIT_LIST_HEAD(&attr->list); + list_add(&attr->list, cb->list); + return 0; +} + +static struct nl_msg * +wprobe_new_msg(struct wprobe_iface *dev, int cmd, bool dump) +{ + struct nl_msg *msg; + uint32_t flags = 0; + + msg = nlmsg_alloc_size(65536); + if (!msg) + return NULL; + + if (dump) + flags |= NLM_F_DUMP; + + genlmsg_put(msg, 0, 0, dev->genl_family, + 0, flags, cmd, 0); + + NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, dev->ifname); +nla_put_failure: + return msg; +} + + +static int +dump_attributes(struct wprobe_iface *dev, bool link, struct list_head *list, char *addr) +{ + struct nl_msg *msg; + struct wprobe_attr_cb cb; + + cb.list = list; + cb.addr = addr; + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_LIST, true); + if (!msg) + return -ENOMEM; + + if (link) + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, "\x00\x00\x00\x00\x00\x00"); + + return dev->ops->send_msg(dev, msg, save_attribute_handler, &cb); + +nla_put_failure: + nlmsg_free(msg); + return -EINVAL; +} + +static struct wprobe_iface * +wprobe_alloc_dev(void) +{ + struct wprobe_iface *dev; + + dev = malloc(sizeof(struct wprobe_iface)); + if (!dev) + return NULL; + + memset(dev, 0, sizeof(struct wprobe_iface)); + + dev->interval = -1; + dev->scale_min = -1; + dev->scale_max = -1; + dev->scale_m = -1; + dev->scale_d = -1; + dev->sockfd = -1; + + INIT_LIST_HEAD(&dev->global_attr); + INIT_LIST_HEAD(&dev->link_attr); + INIT_LIST_HEAD(&dev->links); + return dev; +} + +static int +wprobe_init_dev(struct wprobe_iface *dev) +{ + dump_attributes(dev, false, &dev->global_attr, NULL); + dump_attributes(dev, true, &dev->link_attr, NULL); + return 0; +} + +#ifndef NO_LOCAL_ACCESS +static int n_devs = 0; +static struct nl_sock *handle = NULL; +static struct nl_cache *cache = NULL; +static struct genl_family *family = NULL; + +static int +error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) +{ + int *ret = arg; + *ret = err->error; + return NL_STOP; +} + +static int +finish_handler(struct nl_msg *msg, void *arg) +{ + int *ret = arg; + *ret = 0; + return NL_SKIP; +} + +static int +ack_handler(struct nl_msg *msg, void *arg) +{ + int *ret = arg; + *ret = 0; + return NL_STOP; +} + +static void +wprobe_local_free(struct wprobe_iface *dev) +{ + /* should not happen */ + if (n_devs == 0) + return; + + if (--n_devs != 0) + return; + + if (cache) + nl_cache_free(cache); + if (handle) + nl_socket_free(handle); + handle = NULL; + cache = NULL; +} + +static int +wprobe_local_init(void) +{ + int ret; + + if (n_devs++ > 0) + return 0; + + handle = nl_socket_alloc(); + if (!handle) { + DPRINTF("Failed to create handle\n"); + goto err; + } + + if (genl_connect(handle)) { + DPRINTF("Failed to connect to generic netlink\n"); + goto err; + } + + ret = genl_ctrl_alloc_cache(handle, &cache); + if (ret < 0) { + DPRINTF("Failed to allocate netlink cache\n"); + goto err; + } + + family = genl_ctrl_search_by_name(cache, "wprobe"); + if (!family) { + DPRINTF("wprobe API not present\n"); + goto err; + } + return 0; + +err: + wprobe_local_free(NULL); + return -EINVAL; +} + + +static int +wprobe_local_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t callback, void *arg) +{ + struct nl_cb *cb; + int err = 0; + + cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) + goto out_no_cb; + + if (callback) + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg); + + err = nl_send_auto_complete(handle, msg); + if (err < 0) + goto out; + + err = 1; + + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); + nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + + while (err > 0) + nl_recvmsgs(handle, cb); + +out: + nl_cb_put(cb); +out_no_cb: + nlmsg_free(msg); + return err; +} + +static const struct wprobe_iface_ops wprobe_local_ops = { + .send_msg = wprobe_local_send_msg, + .free = wprobe_local_free, +}; + +struct wprobe_iface * +wprobe_get_dev(const char *ifname) +{ + struct wprobe_iface *dev; + + if (wprobe_local_init() != 0) + return NULL; + + dev = wprobe_alloc_dev(); + if (!dev) + goto error_alloc; + + dev->ifname = strdup(ifname); + dev->ops = &wprobe_local_ops; + dev->genl_family = genl_family_get_id(family); + + if (wprobe_init_dev(dev) < 0) + goto error; + + return dev; + +error: + free(dev); +error_alloc: + wprobe_local_free(NULL); + return NULL; +} + +#endif + +static void swap_nlmsghdr(struct nlmsghdr *nlh) +{ + SWAP32(nlh->nlmsg_len); + SWAP16(nlh->nlmsg_type); + SWAP16(nlh->nlmsg_flags); + SWAP32(nlh->nlmsg_seq); + SWAP32(nlh->nlmsg_pid); +} + +static void swap_genlmsghdr(struct genlmsghdr *gnlh) +{ +#if 0 /* probably unnecessary */ + SWAP16(gnlh->reserved); +#endif +} + +static void +wprobe_swap_nested(void *data, int len, bool outgoing) +{ + void *end = data + len; + + while (data < end) { + struct nlattr *nla = data; + unsigned int type, len; + + if (!outgoing) { + SWAP16(nla->nla_len); + SWAP16(nla->nla_type); + + /* required for further sanity checks */ + if (data + nla->nla_len > end) + nla->nla_len = end - data; + } + + len = NLA_ALIGN(nla->nla_len); + type = nla->nla_type & NLA_TYPE_MASK; + + if (type <= WPROBE_ATTR_LAST) { +#if __BYTE_ORDER == __LITTLE_ENDIAN + switch(attribute_policy[type].type) { + case NLA_U16: + SWAP16(*(__u16 *)nla_data(nla)); + break; + case NLA_U32: + SWAP32(*(__u32 *)nla_data(nla)); + break; + case NLA_U64: + SWAP64(*(__u64 *)nla_data(nla)); + break; + case NLA_NESTED: + wprobe_swap_nested(nla_data(nla), nla_len(nla), outgoing); + break; + } +#endif + } + data += len; + + if (outgoing) { + SWAP16(nla->nla_len); + SWAP16(nla->nla_type); + } + if (!nla->nla_len) + break; + } +} + +static struct nl_msg * +wprobe_msg_from_network(int socket, int len) +{ + struct genlmsghdr *gnlh; + struct nlmsghdr *nlh; + struct nl_msg *msg; + void *data; + + msg = nlmsg_alloc_size(len + 32); + if (!msg) + return NULL; + + nlh = nlmsg_hdr(msg); + if (read(socket, nlh, len) != len) + goto free; + + swap_nlmsghdr(nlh); + if (nlh->nlmsg_len > len) + goto free; + + gnlh = nlmsg_data(nlh); + swap_genlmsghdr(gnlh); + + data = genlmsg_data(gnlh); + wprobe_swap_nested(data, genlmsg_len(gnlh), false); + + return msg; +free: + nlmsg_free(msg); + return NULL; +} + +static int +wprobe_msg_to_network(int socket, struct nl_msg *msg) +{ + struct nlmsghdr *nlh = nlmsg_hdr(msg); + struct wprobe_msg_hdr mhdr; + struct genlmsghdr *gnlh; + void *buf, *data; + int buflen, datalen; + int ret; + + buflen = nlh->nlmsg_len; + buf = malloc(buflen); + if (!buf) + return -ENOMEM; + + memset(&mhdr, 0, sizeof(mhdr)); + mhdr.status = WPROBE_MSG_DATA; + mhdr.len = buflen; + wprobe_swap_msg_hdr(&mhdr); + write(socket, &mhdr, sizeof(mhdr)); + + memcpy(buf, nlh, buflen); + nlh = buf; + gnlh = nlmsg_data(nlh); + data = genlmsg_data(gnlh); + datalen = genlmsg_len(gnlh); + + wprobe_swap_nested(data, datalen, true); + swap_genlmsghdr(gnlh); + swap_nlmsghdr(nlh); + ret = write(socket, buf, buflen); + free(buf); + + return ret; +} + +static int +wprobe_remote_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t callback, void *arg) +{ + struct wprobe_msg_hdr mhdr; + int msgs = 0; + + wprobe_msg_to_network(dev->sockfd, msg); + nlmsg_free(msg); + do { + if (read(dev->sockfd, &mhdr, sizeof(mhdr)) != sizeof(mhdr)) { + DPRINTF("Failed to read response header\n"); + return -1; + } + wprobe_swap_msg_hdr(&mhdr); + + switch(mhdr.status) { + case WPROBE_MSG_DATA: + if (mhdr.len > WPROBE_MAX_MSGLEN) { + fprintf(stderr, "Invalid length in received response message.\n"); + exit(1); + } + + msg = wprobe_msg_from_network(dev->sockfd, mhdr.len); + if (!msg) + return -EINVAL; + + msgs++; + callback(msg, arg); + nlmsg_free(msg); + break; + } + } while (mhdr.status != WPROBE_MSG_DONE); + + if (mhdr.error) + return -mhdr.error; + else + return msgs; +} + + +static void +wprobe_socket_dev_free(struct wprobe_iface *dev) +{ + if (dev->sockfd >= 0) + close(dev->sockfd); +} + +static const struct wprobe_iface_ops wprobe_remote_ops = { + .send_msg = wprobe_remote_send_msg, + .free = wprobe_socket_dev_free, +}; + + +#ifndef NO_LOCAL_ACCESS +int +wprobe_server_init(int socket) +{ + struct wprobe_init_hdr hdr; + int ret; + + ret = wprobe_local_init(); + if (ret != 0) + return ret; + + memset(&hdr, 0, sizeof(hdr)); + memcpy(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(WPROBE_MAGIC_STR)); + hdr.pre.version = 0; + hdr.v0.genl_family = genl_family_get_id(family); + SWAP16(hdr.v0.genl_family); + write(socket, (unsigned char *)&hdr, sizeof(hdr)); + + return 0; +} + +static int +wprobe_server_cb(struct nl_msg *msg, void *arg) +{ + int *socket = arg; + int ret; + + ret = wprobe_msg_to_network(*socket, msg); + if (ret > 0) + ret = 0; + + return ret; +} + + +int +wprobe_server_handle(int socket) +{ + struct wprobe_msg_hdr mhdr; + struct nl_msg *msg; + int ret; + + ret = read(socket, &mhdr, sizeof(mhdr)); + if (ret != sizeof(mhdr)) { + if (ret <= 0) + return -1; + + DPRINTF("Failed to read request header\n"); + return -EINVAL; + } + wprobe_swap_msg_hdr(&mhdr); + + switch(mhdr.status) { + case WPROBE_MSG_DATA: + if (mhdr.len > WPROBE_MAX_MSGLEN) { + DPRINTF("Invalid length in received response message.\n"); + return -EINVAL; + } + msg = wprobe_msg_from_network(socket, mhdr.len); + break; + default: + DPRINTF("Invalid request header type\n"); + return -ENOENT; + } + + if (!msg) { + DPRINTF("Failed to get message\n"); + return -EINVAL; + } + + ret = wprobe_local_send_msg(NULL, msg, wprobe_server_cb, &socket); + + memset(&mhdr, 0, sizeof(mhdr)); + mhdr.status = WPROBE_MSG_DONE; + if (ret < 0) + mhdr.error = (uint16_t) -ret; + + ret = write(socket, (unsigned char *)&mhdr, sizeof(mhdr)); + if (ret > 0) + ret = 0; + + return ret; +} + +void +wprobe_server_done(void) +{ + wprobe_local_free(NULL); +} +#endif + +struct wprobe_iface * +wprobe_get_from_socket(int socket, const char *name) +{ + struct wprobe_iface *dev; + struct wprobe_init_hdr hdr; + + dev = wprobe_alloc_dev(); + if (!dev) + goto out; + + dev->ops = &wprobe_remote_ops; + dev->sockfd = socket; + dev->ifname = strdup(name); + + /* read version and header length */ + if (read(socket, &hdr.pre, sizeof(hdr.pre)) != sizeof(hdr.pre)) { + DPRINTF("Could not read header\n"); + goto error; + } + + /* magic not found */ + if (memcmp(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(hdr.pre.magic)) != 0) { + DPRINTF("Magic does not match\n"); + goto error; + } + + /* unsupported version */ + if (hdr.pre.version != 0) { + DPRINTF("Protocol version does not match\n"); + goto error; + } + + if (read(socket, &hdr.v0, sizeof(hdr.v0)) != sizeof(hdr.v0)) { + DPRINTF("Could not read header data\n"); + goto error; + } + + SWAP16(hdr.pre.extra); + SWAP16(hdr.v0.genl_family); + dev->genl_family = hdr.v0.genl_family; + + if (wprobe_init_dev(dev) < 0) { + DPRINTF("Could not initialize device\n"); + goto error; + } + +out: + return dev; + +error: + wprobe_free_dev(dev); + return NULL; +} + +struct wprobe_iface * +wprobe_get_auto(const char *arg, char **err) +{ + static struct sockaddr_in sa; + static char errbuf[512]; + + struct wprobe_iface *dev = NULL; + struct hostent *h; + char *devstr = strdup(arg); + char *sep = NULL; + int sock = -1; + int len; + + if (err) + *err = NULL; + + sep = strchr(devstr, ':'); + if (!sep) { +#ifndef NO_LOCAL_ACCESS + free(devstr); + return wprobe_get_dev(arg); +#else + *err = "Invalid argument"; + goto out; +#endif + } + + *sep = 0; + sep++; + + sock = socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) + goto syserr; + + h = gethostbyname(devstr); + if (!h) { + sprintf(errbuf, "Host not found"); + goto out_err; + } + + memcpy(&sa.sin_addr, h->h_addr, h->h_length); + sa.sin_family = AF_INET; + sa.sin_port = htons(wprobe_port); + if (connect(sock, (struct sockaddr *)&sa, sizeof(sa)) < 0) + goto syserr; + + dev = wprobe_get_from_socket(sock, sep); + if (!dev) { + sprintf(errbuf, "wprobe connection initialization failed"); + goto out_err; + } + goto out; + +syserr: + if (err) { + strcpy(errbuf, "Connection failed: "); + len = strlen(errbuf); + strerror_r(errno, errbuf + len, sizeof(errbuf) - len - 1); + } +out_err: + if (err) + *err = errbuf; + if (sock >= 0) + close(sock); +out: + if (devstr) + free(devstr); + return dev; +} + +static void +free_attr_list(struct list_head *list) +{ + struct wprobe_attribute *attr, *tmp; + + list_for_each_entry_safe(attr, tmp, list, list) { + list_del(&attr->list); + free(attr); + } +} + +void +wprobe_free_dev(struct wprobe_iface *dev) +{ + if (dev->ops->free) + dev->ops->free(dev); + free_attr_list(&dev->global_attr); + free_attr_list(&dev->link_attr); + free((void *)dev->ifname); + free(dev); +} + +static struct wprobe_link * +get_link(struct list_head *list, const char *addr) +{ + struct wprobe_link *l; + + list_for_each_entry(l, list, list) { + if (!memcmp(l->addr, addr, 6)) { + list_del_init(&l->list); + goto out; + } + } + + /* no previous link found, allocate a new one */ + l = malloc(sizeof(struct wprobe_link)); + if (!l) + goto out; + + memset(l, 0, sizeof(struct wprobe_link)); + memcpy(l->addr, addr, sizeof(l->addr)); + INIT_LIST_HEAD(&l->list); + +out: + return l; +} + +struct wprobe_save_cb { + struct list_head *list; + struct list_head old_list; +}; + +static int +save_link_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wprobe_link *link; + struct wprobe_save_cb *cb = arg; + const char *addr; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6)) + return -1; + + addr = nla_data(tb[WPROBE_ATTR_MAC]); + link = get_link(&cb->old_list, addr); + if (!link) + return -1; + + if (tb[WPROBE_ATTR_FLAGS]) + link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]); + + list_add_tail(&link->list, cb->list); + return 0; +} + + +int +wprobe_update_links(struct wprobe_iface *dev) +{ + struct wprobe_link *l, *tmp; + struct nl_msg *msg; + struct wprobe_save_cb cb; + int err; + + INIT_LIST_HEAD(&cb.old_list); + list_splice_init(&dev->links, &cb.old_list); + cb.list = &dev->links; + + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_LINKS, true); + if (!msg) + return -ENOMEM; + + err = dev->ops->send_msg(dev, msg, save_link_handler, &cb); + if (err < 0) + return err; + + list_for_each_entry_safe(l, tmp, &cb.old_list, list) { + list_del(&l->list); + free(l); + } + + return 0; +} + + +struct wprobe_filter_data +{ + wprobe_filter_cb cb; + void *arg; + struct wprobe_filter_item *buf; + int buflen; +}; + +static int +dump_filter_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wprobe_filter_data *data = arg; + struct nlattr *p; + const char *name; + int count = 0; + int len; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_FILTER_GROUP]) + return -1; + + name = nla_data(tb[WPROBE_ATTR_NAME]); + nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) { + count++; + } + + if (data->buflen < count) { + if (data->buf) + free(data->buf); + data->buflen = count; + data->buf = malloc(sizeof(struct wprobe_filter_item) * count); + memset(data->buf, 0, sizeof(struct wprobe_filter_item) * count); + } + + count = 0; + nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) { + struct wprobe_filter_item *fi; + + nla_parse(tb, WPROBE_ATTR_LAST, nla_data(p), + nla_len(p), attribute_policy); + + if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_RXCOUNT] + || !tb[WPROBE_ATTR_TXCOUNT]) + continue; + + fi = &data->buf[count++]; + strncpy(fi->name, nla_data(tb[WPROBE_ATTR_NAME]), sizeof(fi->name) - 1); + fi->name[sizeof(fi->name) - 1] = 0; + fi->rx = nla_get_u64(tb[WPROBE_ATTR_RXCOUNT]); + fi->tx = nla_get_u64(tb[WPROBE_ATTR_TXCOUNT]); + } + data->cb(data->arg, name, data->buf, count); + + return 0; +} + +int +wprobe_dump_filters(struct wprobe_iface *dev, wprobe_filter_cb cb, void *arg) +{ + struct wprobe_filter_data data; + struct nl_msg *msg; + int err; + + data.buf = 0; + data.buflen = 0; + data.cb = cb; + data.arg = arg; + + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_FILTER, true); + if (!msg) + return -ENOMEM; + + err = dev->ops->send_msg(dev, msg, dump_filter_handler, &data); + if (err < 0) + return err; + + return 0; +} + +int +wprobe_apply_config(struct wprobe_iface *dev) +{ + struct nl_msg *msg; + + msg = wprobe_new_msg(dev, WPROBE_CMD_CONFIG, false); + if (!msg) + return -ENOMEM; + + if (dev->interval >= 0) + NLA_PUT_MSECS(msg, WPROBE_ATTR_INTERVAL, dev->interval); + + if (dev->filter_len < 0) { + NLA_PUT(msg, WPROBE_ATTR_FILTER, 0, NULL); + dev->filter_len = 0; + } else if (dev->filter && dev->filter_len > 0) { + NLA_PUT(msg, WPROBE_ATTR_FILTER, dev->filter_len, dev->filter); + } + dev->filter = NULL; + + dev->ops->send_msg(dev, msg, NULL, NULL); + return 0; + +nla_put_failure: + nlmsg_free(msg); + return -ENOMEM; +} + +int +wprobe_measure(struct wprobe_iface *dev) +{ + struct nl_msg *msg; + + msg = wprobe_new_msg(dev, WPROBE_CMD_MEASURE, false); + if (!msg) + return -ENOMEM; + + dev->ops->send_msg(dev, msg, NULL, NULL); + return 0; +} + +struct wprobe_request_cb { + struct list_head *list; + struct list_head old_list; + char *addr; +}; + +static int +save_attrdata_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wprobe_request_cb *cb = arg; + struct wprobe_attribute *attr; + int type, id; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (!tb[WPROBE_ATTR_ID]) + return -1; + + if (!tb[WPROBE_ATTR_TYPE]) + return -1; + + id = nla_get_u32(tb[WPROBE_ATTR_ID]); + list_for_each_entry(attr, &cb->old_list, list) { + if (attr->id == id) + goto found; + } + /* not found */ + return -1; + +found: + list_del_init(&attr->list); + + type = nla_get_u8(tb[WPROBE_ATTR_TYPE]); + if (type != attr->type) { + DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n", + (cb->addr ? "link" : "global"), + attr->name, + type, attr->type); + goto out; + } + + if ((type < WPROBE_VAL_STRING) || + (type > WPROBE_VAL_U64)) + goto out; + + memset(&attr->val, 0, sizeof(attr->val)); + +#define HANDLE_INT_TYPE(_idx, _type) \ + case WPROBE_VAL_S##_type: \ + case WPROBE_VAL_U##_type: \ + attr->val.U##_type = nla_get_u##_type(tb[_idx]); \ + break + + switch(type) { + HANDLE_INT_TYPE(type, 8); + HANDLE_INT_TYPE(type, 16); + HANDLE_INT_TYPE(type, 32); + HANDLE_INT_TYPE(type, 64); + case WPROBE_VAL_STRING: + /* unimplemented */ + break; + } +#undef HANDLE_TYPE + + if (attr->flags & WPROBE_F_KEEPSTAT) { + if (tb[WPROBE_VAL_SUM]) + attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]); + + if (tb[WPROBE_VAL_SUM_SQ]) + attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]); + + if (tb[WPROBE_VAL_SAMPLES]) + attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]); + + if (attr->val.n > 0) { + float avg = ((float) attr->val.s) / attr->val.n; + float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg)); + if (isnan(stdev)) + stdev = 0.0f; + if (isnan(avg)) + avg = 0.0f; + attr->val.avg = avg; + attr->val.stdev = stdev; + } + } + +out: + list_add_tail(&attr->list, cb->list); + return 0; +} + + +int +wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr) +{ + struct wprobe_request_cb cb; + struct list_head *attrs; + struct nl_msg *msg; + int err; + + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_INFO, true); + if (!msg) + return -ENOMEM; + + if (addr) { + attrs = &dev->link_attr; + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr); + } else { + attrs = &dev->global_attr; + } + + INIT_LIST_HEAD(&cb.old_list); + list_splice_init(attrs, &cb.old_list); + cb.list = attrs; + + err = dev->ops->send_msg(dev, msg, save_attrdata_handler, &cb); + list_splice(&cb.old_list, attrs->prev); + return err; + +nla_put_failure: + nlmsg_free(msg); + return -ENOMEM; +} + + diff --git a/package/wprobe/src/user/wprobe-util.c b/package/wprobe/src/user/wprobe-util.c new file mode 100644 index 000000000..d5d440a30 --- /dev/null +++ b/package/wprobe/src/user/wprobe-util.c @@ -0,0 +1,441 @@ +/* + * wprobe-test.c: Wireless probe user space test code + * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org> + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include <sys/types.h> +#include <sys/socket.h> +#include <sys/wait.h> +#include <arpa/inet.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <inttypes.h> +#include <errno.h> +#include <stdint.h> +#include <getopt.h> +#include <stdbool.h> +#include <unistd.h> +#include <netdb.h> +#include <fcntl.h> +#include <signal.h> + +#include <linux/wprobe.h> +#include "wprobe.h" + +static bool simple_mode = false; + +static const char * +wprobe_dump_value(struct wprobe_attribute *attr) +{ + static char buf[128]; + +#define HANDLE_TYPE(_type, _format) \ + case WPROBE_VAL_##_type: \ + snprintf(buf, sizeof(buf), _format, attr->val._type); \ + break + + switch(attr->type) { + HANDLE_TYPE(S8, "%d"); + HANDLE_TYPE(S16, "%d"); + HANDLE_TYPE(S32, "%d"); + HANDLE_TYPE(S64, "%lld"); + HANDLE_TYPE(U8, "%d"); + HANDLE_TYPE(U16, "%d"); + HANDLE_TYPE(U32, "%d"); + HANDLE_TYPE(U64, "%lld"); + case WPROBE_VAL_STRING: + /* FIXME: implement this */ + default: + strncpy(buf, "<unknown>", sizeof(buf)); + break; + } + if ((attr->flags & WPROBE_F_KEEPSTAT) && + (attr->val.n > 0)) { + int len = strlen(buf); + if (simple_mode) + snprintf(buf + len, sizeof(buf) - len, ";%.02f;%.02f;%d;%lld;%lld", attr->val.avg, attr->val.stdev, attr->val.n, attr->val.s, attr->val.ss); + else + snprintf(buf + len, sizeof(buf) - len, " (avg: %.02f; stdev: %.02f, n=%d)", attr->val.avg, attr->val.stdev, attr->val.n); + } +#undef HANDLE_TYPE + + return buf; +} + + +static void +wprobe_dump_data(struct wprobe_iface *dev) +{ + struct wprobe_attribute *attr; + struct wprobe_link *link; + bool first = true; + + if (!simple_mode) + fprintf(stderr, "\n"); + wprobe_request_data(dev, NULL); + list_for_each_entry(attr, &dev->global_attr, list) { + if (simple_mode) { + if (first) + fprintf(stdout, "[global]\n"); + fprintf(stdout, "%s=%s\n", attr->name, wprobe_dump_value(attr)); + } else { + fprintf(stderr, (first ? + "Global: %s=%s\n" : + " %s=%s\n"), + attr->name, + wprobe_dump_value(attr) + ); + } + first = false; + } + + list_for_each_entry(link, &dev->links, list) { + first = true; + wprobe_request_data(dev, link->addr); + list_for_each_entry(attr, &dev->link_attr, list) { + if (first) { + fprintf((simple_mode ? stdout : stderr), + (simple_mode ? + "[%02x:%02x:%02x:%02x:%02x:%02x]\n%s=%s\n" : + "%02x:%02x:%02x:%02x:%02x:%02x: %s=%s\n"), + link->addr[0], link->addr[1], link->addr[2], + link->addr[3], link->addr[4], link->addr[5], + attr->name, + wprobe_dump_value(attr)); + first = false; + } else { + fprintf((simple_mode ? stdout : stderr), + (simple_mode ? "%s=%s\n" : + " %s=%s\n"), + attr->name, + wprobe_dump_value(attr)); + } + } + } + fflush(stdout); +} + +static const char *attr_typestr[] = { + [0] = "Unknown", + [WPROBE_VAL_STRING] = "String", + [WPROBE_VAL_U8] = "Unsigned 8 bit", + [WPROBE_VAL_U16] = "Unsigned 16 bit", + [WPROBE_VAL_U32] = "Unsigned 32 bit", + [WPROBE_VAL_U64] = "Unsigned 64 bit", + [WPROBE_VAL_S8] = "Signed 8 bit", + [WPROBE_VAL_S16] = "Signed 16 bit", + [WPROBE_VAL_S32] = "Signed 32 bit", + [WPROBE_VAL_S64] = "Signed 64 bit", +}; + +static int usage(const char *prog) +{ + fprintf(stderr, +#ifndef NO_LOCAL_ACCESS + "Usage: %s <interface>|<host>:<device>|-P [options]\n" +#else + "Usage: %s <host>:<device> [options]\n" +#endif + "\n" + "Options:\n" + " -c: Only apply configuration\n" + " -d: Delay between measurement dumps (in milliseconds, default: 1000)\n" + " -f: Dump contents of layer 2 filter counters during measurement\n" + " -F <file>: Apply layer 2 filters from <file>\n" + " -h: This help text\n" + " -i <interval>: Set measurement interval\n" + " -m: Run measurement loop\n" + " -p: Set the TCP port for server/client (default: 17990)\n" +#ifndef NO_LOCAL_ACCESS + " -P: Run in proxy mode (listen on network)\n" +#endif + "\n" + , prog); + exit(1); +} + +static void show_attributes(struct wprobe_iface *dev) +{ + struct wprobe_attribute *attr; + if (simple_mode) + return; + list_for_each_entry(attr, &dev->global_attr, list) { + fprintf(stderr, "Global attribute: '%s' (%s)\n", + attr->name, attr_typestr[attr->type]); + } + list_for_each_entry(attr, &dev->link_attr, list) { + fprintf(stderr, "Link attribute: '%s' (%s)\n", + attr->name, attr_typestr[attr->type]); + } +} + +static void show_filter_simple(void *arg, const char *group, struct wprobe_filter_item *items, int n_items) +{ + int i; + + fprintf(stdout, "[filter:%s]\n", group); + for (i = 0; i < n_items; i++) { + fprintf(stdout, "%s=%lld;%lld\n", + items[i].name, items[i].tx, items[i].rx); + } + fflush(stdout); +} + + +static void show_filter(void *arg, const char *group, struct wprobe_filter_item *items, int n_items) +{ + int i; + fprintf(stderr, "Filter group: '%s' (tx/rx)\n", group); + for (i = 0; i < n_items; i++) { + fprintf(stderr, " - %s (%lld/%lld)\n", + items[i].name, items[i].tx, items[i].rx); + } +} + +static void loop_measurement(struct wprobe_iface *dev, bool print_filters, unsigned long delay) +{ + while (1) { + usleep(delay * 1000); + wprobe_update_links(dev); + wprobe_dump_data(dev); + if (print_filters) + wprobe_dump_filters(dev, simple_mode ? show_filter_simple : show_filter, NULL); + } +} + +static void set_filter(struct wprobe_iface *dev, const char *filename) +{ + unsigned char *buf = NULL; + unsigned int buflen = 0; + unsigned int len = 0; + int fd; + + /* clear filter */ + if (filename[0] == 0) { + dev->filter_len = -1; + return; + } + + fd = open(filename, O_RDONLY); + if (fd < 0) { + perror("open filter"); + return; + } + + do { + int rlen; + + if (!buf) { + len = 0; + buflen = 1024; + buf = malloc(1024); + } else { + buflen *= 2; + buf = realloc(buf, buflen); + } + rlen = read(fd, buf + len, buflen - len); + if (rlen < 0) + break; + + len += rlen; + } while (len == buflen); + + dev->filter = buf; + dev->filter_len = len; + close(fd); +} + +#ifndef NO_LOCAL_ACCESS + +static void sigchld_handler(int s) +{ + while (waitpid(-1, NULL, WNOHANG) > 0); +} + +static int run_proxy(int port) +{ + struct sockaddr_in sa; + struct sigaction sig; + int v = 1; + int s; + + s = socket(AF_INET, SOCK_STREAM, 0); + if (s < 0) { + perror("socket"); + return 1; + } + + sig.sa_handler = sigchld_handler; // Signal Handler fuer Zombie Prozesse + sigemptyset(&sig.sa_mask); + sig.sa_flags = SA_RESTART; + sigaction(SIGCHLD, &sig, NULL); + + memset(&sa, 0, sizeof(sa)); + sa.sin_family = AF_INET; + sa.sin_addr.s_addr = htonl(INADDR_ANY); + sa.sin_port = htons(wprobe_port); + + setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &v, sizeof(v)); + if (bind(s, (struct sockaddr *) &sa, sizeof(sa)) < 0) { + perror("bind"); + return 1; + } + if (listen(s, 10)) { + perror("listen"); + return 1; + } + while(1) { + unsigned int addrlen; + int ret, c; + + c = accept(s, (struct sockaddr *)&sa, &addrlen); + if (c < 0) { + if (errno == EINTR) + continue; + + perror("accept"); + return 1; + } + if (fork() == 0) { + /* close server socket, stdin, stdout, stderr */ + close(s); + close(0); + close(1); + close(2); + + wprobe_server_init(c); + do { + ret = wprobe_server_handle(c); + } while (ret >= 0); + wprobe_server_done(); + close(c); + exit(0); + } + close(c); + } + + return 0; +} +#endif + +int main(int argc, char **argv) +{ + struct wprobe_iface *dev = NULL; + const char *ifname; + const char *prog = argv[0]; + char *err = NULL; + enum { + CMD_NONE, + CMD_CONFIG, + CMD_MEASURE, + CMD_PROXY, + } cmd = CMD_NONE; + const char *filter = NULL; + bool print_filters = false; + unsigned long delay = 1000; + int interval = -1; + int ch; + + if (argc < 2) + return usage(prog); + +#ifndef NO_LOCAL_ACCESS + if (!strcmp(argv[1], "-P")) { + while ((ch = getopt(argc - 1, argv + 1, "p:")) != -1) { + switch(ch) { + case 'p': + /* set port */ + wprobe_port = strtoul(optarg, NULL, 0); + break; + default: + return usage(prog); + } + } + return run_proxy(wprobe_port); + } +#endif + + if (argv[1][0] == '-') + return usage(prog); + + ifname = argv[1]; + argv++; + argc--; + + while ((ch = getopt(argc, argv, "cd:fF:hi:msp:")) != -1) { + switch(ch) { + case 'c': + cmd = CMD_CONFIG; + break; + case 'd': + delay = strtoul(optarg, NULL, 10); + break; + case 'm': + cmd = CMD_MEASURE; + break; + case 'i': + interval = strtoul(optarg, NULL, 10); + break; + case 'f': + print_filters = true; + break; + case 'F': + if (filter) { + fprintf(stderr, "Cannot set multiple filters\n"); + return usage(prog); + } + filter = optarg; + break; + case 's': + simple_mode = true; + break; + case 'p': + /* set port */ + wprobe_port = strtoul(optarg, NULL, 0); + break; + case 'h': + default: + usage(prog); + break; + } + } + + dev = wprobe_get_auto(ifname, &err); + if (!dev || (list_empty(&dev->global_attr) && + list_empty(&dev->link_attr))) { + if (err) + fprintf(stderr, "%s\n", err); + else + fprintf(stderr, "Interface '%s' not found\n", ifname); + return 1; + } + + if (filter || interval >= 0) { + if (filter) + set_filter(dev, filter); + if (interval >= 0) + dev->interval = interval; + + wprobe_apply_config(dev); + } + + if (cmd != CMD_CONFIG) + show_attributes(dev); + if (cmd == CMD_MEASURE) + loop_measurement(dev, print_filters, delay); + + wprobe_free_dev(dev); + + return 0; +} diff --git a/package/wprobe/src/user/wprobe.c b/package/wprobe/src/user/wprobe.c deleted file mode 100644 index e48f768e5..000000000 --- a/package/wprobe/src/user/wprobe.c +++ /dev/null @@ -1,571 +0,0 @@ -/* - * wprobe.c: Wireless probe user space library - * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org> - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - */ - -#include <stdio.h> -#include <string.h> -#include <stdlib.h> -#include <errno.h> -#include <getopt.h> -#include <sys/types.h> -#include <sys/socket.h> -#include <math.h> -#include <linux/wprobe.h> -#include <netlink/netlink.h> -#include <netlink/genl/genl.h> -#include <netlink/genl/ctrl.h> -#include <netlink/genl/family.h> -#include "wprobe.h" - -#define DEBUG 1 -#ifdef DEBUG -#define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__) -#else -#define DPRINTF(fmt, ...) do {} while (0) -#endif - -static int n_devs = 0; -static struct nl_sock *handle = NULL; -static struct nl_cache *cache = NULL; -static struct genl_family *family = NULL; -static struct nlattr *tb[WPROBE_ATTR_LAST+1]; -static struct nla_policy attribute_policy[WPROBE_ATTR_LAST+1] = { - [WPROBE_ATTR_ID] = { .type = NLA_U32 }, - [WPROBE_ATTR_MAC] = { .type = NLA_UNSPEC, .minlen = 6, .maxlen = 6 }, - [WPROBE_ATTR_NAME] = { .type = NLA_STRING }, - [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 }, - [WPROBE_ATTR_TYPE] = { .type = NLA_U8 }, - [WPROBE_VAL_S8] = { .type = NLA_U8 }, - [WPROBE_VAL_S16] = { .type = NLA_U16 }, - [WPROBE_VAL_S32] = { .type = NLA_U32 }, - [WPROBE_VAL_S64] = { .type = NLA_U64 }, - [WPROBE_VAL_U8] = { .type = NLA_U8 }, - [WPROBE_VAL_U16] = { .type = NLA_U16 }, - [WPROBE_VAL_U32] = { .type = NLA_U32 }, - [WPROBE_VAL_U64] = { .type = NLA_U64 }, - [WPROBE_VAL_SUM] = { .type = NLA_U64 }, - [WPROBE_VAL_SUM_SQ] = { .type = NLA_U64 }, - [WPROBE_VAL_SAMPLES] = { .type = NLA_U32 }, -}; - -static int -error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) -{ - int *ret = arg; - *ret = err->error; - return NL_STOP; -} - -static int -finish_handler(struct nl_msg *msg, void *arg) -{ - int *ret = arg; - *ret = 0; - return NL_SKIP; -} - -static int -ack_handler(struct nl_msg *msg, void *arg) -{ - int *ret = arg; - *ret = 0; - return NL_STOP; -} - - -static void -wprobe_free(void) -{ - /* should not happen */ - if (n_devs == 0) - return; - - if (--n_devs != 0) - return; - - if (cache) - nl_cache_free(cache); - if (handle) - nl_socket_free(handle); - handle = NULL; - cache = NULL; -} - -static int -wprobe_init(void) -{ - int ret; - - if (n_devs++ > 0) - return 0; - - handle = nl_socket_alloc(); - if (!handle) { - DPRINTF("Failed to create handle\n"); - goto err; - } - - if (genl_connect(handle)) { - DPRINTF("Failed to connect to generic netlink\n"); - goto err; - } - - ret = genl_ctrl_alloc_cache(handle, &cache); - if (ret < 0) { - DPRINTF("Failed to allocate netlink cache\n"); - goto err; - } - - family = genl_ctrl_search_by_name(cache, "wprobe"); - if (!family) { - DPRINTF("wprobe API not present\n"); - goto err; - } - return 0; - -err: - wprobe_free(); - return -EINVAL; -} - - -static struct nl_msg * -wprobe_new_msg(const char *ifname, int cmd, bool dump) -{ - struct nl_msg *msg; - uint32_t flags = 0; - - msg = nlmsg_alloc(); - if (!msg) - return NULL; - - if (dump) - flags |= NLM_F_DUMP; - - genlmsg_put(msg, 0, 0, genl_family_get_id(family), - 0, flags, cmd, 0); - - NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, ifname); -nla_put_failure: - return msg; -} - -static int -wprobe_send_msg(struct nl_msg *msg, void *callback, void *arg) -{ - struct nl_cb *cb; - int err = 0; - - cb = nl_cb_alloc(NL_CB_DEFAULT); - if (!cb) - goto out_no_cb; - - if (callback) - nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg); - - err = nl_send_auto_complete(handle, msg); - if (err < 0) - goto out; - - err = 1; - - nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); - nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); - nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); - - while (err > 0) - nl_recvmsgs(handle, cb); - -out: - nl_cb_put(cb); -out_no_cb: - nlmsg_free(msg); - return err; -} - -struct wprobe_attr_cb { - struct list_head *list; - char *addr; -}; - -static int -save_attribute_handler(struct nl_msg *msg, void *arg) -{ - struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); - const char *name = "N/A"; - struct wprobe_attribute *attr; - int type = 0; - struct wprobe_attr_cb *cb = arg; - - nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), - genlmsg_attrlen(gnlh, 0), attribute_policy); - - if (tb[WPROBE_ATTR_NAME]) - name = nla_data(tb[WPROBE_ATTR_NAME]); - - attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1); - if (!attr) - return -1; - - memset(attr, 0, sizeof(struct wprobe_attribute)); - - if (tb[WPROBE_ATTR_ID]) - attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]); - - if (tb[WPROBE_ATTR_MAC] && cb->addr) - memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6); - - if (tb[WPROBE_ATTR_FLAGS]) - attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]); - - if (tb[WPROBE_ATTR_TYPE]) - type = nla_get_u8(tb[WPROBE_ATTR_TYPE]); - - if ((type < WPROBE_VAL_STRING) || - (type > WPROBE_VAL_U64)) - type = 0; - - attr->type = type; - strcpy(attr->name, name); - INIT_LIST_HEAD(&attr->list); - list_add(&attr->list, cb->list); - return 0; -} - - -static int -dump_attributes(const char *ifname, bool link, struct list_head *list, char *addr) -{ - struct nl_msg *msg; - struct wprobe_attr_cb cb; - - cb.list = list; - cb.addr = addr; - msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LIST, true); - if (!msg) - return -ENOMEM; - - if (link) - NLA_PUT(msg, WPROBE_ATTR_MAC, 6, "\x00\x00\x00\x00\x00\x00"); - - return wprobe_send_msg(msg, save_attribute_handler, &cb); - -nla_put_failure: - nlmsg_free(msg); - return -EINVAL; -} - -struct wprobe_iface * -wprobe_get_dev(const char *ifname) -{ - struct wprobe_iface *dev; - - if (wprobe_init() != 0) - return NULL; - - dev = malloc(sizeof(struct wprobe_iface)); - if (!dev) - return NULL; - - memset(dev, 0, sizeof(struct wprobe_iface)); - dev->ifname = strdup(ifname); - if (!dev->ifname) - goto error; - - dev->interval = -1; - dev->scale_min = -1; - dev->scale_max = -1; - dev->scale_m = -1; - dev->scale_d = -1; - - INIT_LIST_HEAD(&dev->global_attr); - INIT_LIST_HEAD(&dev->link_attr); - INIT_LIST_HEAD(&dev->links); - - dump_attributes(ifname, false, &dev->global_attr, NULL); - dump_attributes(ifname, true, &dev->link_attr, NULL); - - return dev; - -error: - free(dev); - return NULL; -} - -static void -free_attr_list(struct list_head *list) -{ - struct wprobe_attribute *attr, *tmp; - - list_for_each_entry_safe(attr, tmp, list, list) { - list_del(&attr->list); - free(attr); - } -} - -void -wprobe_free_dev(struct wprobe_iface *dev) -{ - wprobe_free(); - free_attr_list(&dev->global_attr); - free_attr_list(&dev->link_attr); - free((void *)dev->ifname); - free(dev); -} - -static struct wprobe_link * -get_link(struct list_head *list, const char *addr) -{ - struct wprobe_link *l; - - list_for_each_entry(l, list, list) { - if (!memcmp(l->addr, addr, 6)) { - list_del_init(&l->list); - goto out; - } - } - - /* no previous link found, allocate a new one */ - l = malloc(sizeof(struct wprobe_link)); - if (!l) - goto out; - - memset(l, 0, sizeof(struct wprobe_link)); - memcpy(l->addr, addr, sizeof(l->addr)); - INIT_LIST_HEAD(&l->list); - -out: - return l; -} - -struct wprobe_save_cb { - struct list_head *list; - struct list_head old_list; -}; - -static int -save_link_handler(struct nl_msg *msg, void *arg) -{ - struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); - struct wprobe_link *link; - struct wprobe_save_cb *cb = arg; - const char *addr; - - nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), - genlmsg_attrlen(gnlh, 0), attribute_policy); - - if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6)) - return -1; - - addr = nla_data(tb[WPROBE_ATTR_MAC]); - link = get_link(&cb->old_list, addr); - if (!link) - return -1; - - if (tb[WPROBE_ATTR_FLAGS]) - link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]); - - list_add_tail(&link->list, cb->list); - return 0; -} - - -int -wprobe_update_links(struct wprobe_iface *dev) -{ - struct wprobe_link *l, *tmp; - struct nl_msg *msg; - struct wprobe_save_cb cb; - int err; - - INIT_LIST_HEAD(&cb.old_list); - list_splice_init(&dev->links, &cb.old_list); - cb.list = &dev->links; - - msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_GET_LINKS, true); - if (!msg) - return -ENOMEM; - - err = wprobe_send_msg(msg, save_link_handler, &cb); - if (err < 0) - return err; - - list_for_each_entry_safe(l, tmp, &cb.old_list, list) { - list_del(&l->list); - free(l); - } - - return 0; -} - -int -wprobe_apply_config(struct wprobe_iface *dev) -{ - struct nl_msg *msg; - - msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_CONFIG, false); - if (!msg) - return -ENOMEM; - - if (dev->interval >= 0) - NLA_PUT_MSECS(msg, WPROBE_ATTR_INTERVAL, dev->interval); - - wprobe_send_msg(msg, NULL, NULL); - return 0; - -nla_put_failure: - nlmsg_free(msg); - return -ENOMEM; -} - -int -wprobe_measure(struct wprobe_iface *dev) -{ - struct nl_msg *msg; - - msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_MEASURE, false); - if (!msg) - return -ENOMEM; - - wprobe_send_msg(msg, NULL, NULL); - return 0; -} - -struct wprobe_request_cb { - struct list_head *list; - struct list_head old_list; - char *addr; -}; - -static int -save_attrdata_handler(struct nl_msg *msg, void *arg) -{ - struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); - struct wprobe_request_cb *cb = arg; - struct wprobe_attribute *attr; - int type, id; - - nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), - genlmsg_attrlen(gnlh, 0), attribute_policy); - - if (!tb[WPROBE_ATTR_ID]) - return -1; - - if (!tb[WPROBE_ATTR_TYPE]) - return -1; - - id = nla_get_u32(tb[WPROBE_ATTR_ID]); - list_for_each_entry(attr, &cb->old_list, list) { - if (attr->id == id) - goto found; - } - /* not found */ - return -1; - -found: - list_del_init(&attr->list); - - type = nla_get_u8(tb[WPROBE_ATTR_TYPE]); - if (type != attr->type) { - DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n", - (cb->addr ? "link" : "global"), - attr->name, - type, attr->type); - goto out; - } - - if ((type < WPROBE_VAL_STRING) || - (type > WPROBE_VAL_U64)) - goto out; - - memset(&attr->val, 0, sizeof(attr->val)); - -#define HANDLE_INT_TYPE(_idx, _type) \ - case WPROBE_VAL_S##_type: \ - case WPROBE_VAL_U##_type: \ - attr->val.U##_type = nla_get_u##_type(tb[_idx]); \ - break - - switch(type) { - HANDLE_INT_TYPE(type, 8); - HANDLE_INT_TYPE(type, 16); - HANDLE_INT_TYPE(type, 32); - HANDLE_INT_TYPE(type, 64); - case WPROBE_VAL_STRING: - /* unimplemented */ - break; - } -#undef HANDLE_TYPE - - if (attr->flags & WPROBE_F_KEEPSTAT) { - if (tb[WPROBE_VAL_SUM]) - attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]); - - if (tb[WPROBE_VAL_SUM_SQ]) - attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]); - - if (tb[WPROBE_VAL_SAMPLES]) - attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]); - - if (attr->val.n > 0) { - float avg = ((float) attr->val.s) / attr->val.n; - float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg)); - if (isnan(stdev)) - stdev = 0.0f; - if (isnan(avg)) - avg = 0.0f; - attr->val.avg = avg; - attr->val.stdev = stdev; - } - } - -out: - list_add_tail(&attr->list, cb->list); - return 0; -} - - -int -wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr) -{ - struct wprobe_request_cb cb; - struct list_head *attrs; - struct nl_msg *msg; - int err; - - msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_GET_INFO, true); - if (!msg) - return -ENOMEM; - - if (addr) { - attrs = &dev->link_attr; - NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr); - } else { - attrs = &dev->global_attr; - } - - INIT_LIST_HEAD(&cb.old_list); - list_splice_init(attrs, &cb.old_list); - cb.list = attrs; - - err = wprobe_send_msg(msg, save_attrdata_handler, &cb); - list_splice(&cb.old_list, attrs->prev); - return err; - -nla_put_failure: - nlmsg_free(msg); - return -ENOMEM; -} - - diff --git a/package/wprobe/src/user/wprobe.h b/package/wprobe/src/user/wprobe.h index c0b4902f3..706facc80 100644 --- a/package/wprobe/src/user/wprobe.h +++ b/package/wprobe/src/user/wprobe.h @@ -87,8 +87,19 @@ struct wprobe_link { unsigned char addr[6]; }; +struct wprobe_filter_item { + char name[32]; + uint64_t rx; + uint64_t tx; +}; + +struct wprobe_iface_ops; struct wprobe_iface { + const struct wprobe_iface_ops *ops; + + int sockfd; const char *ifname; + unsigned int genl_family; char addr[6]; struct list_head global_attr; @@ -101,11 +112,23 @@ struct wprobe_iface { int scale_max; int scale_m; int scale_d; + + /* filter */ + void *filter; + + /* filter_len: + * set to -1 to drop the current filter + * automatically reset to 0 after config apply + */ + int filter_len; }; +typedef void (*wprobe_filter_cb)(void *arg, const char *group, struct wprobe_filter_item *items, int n_items); +extern int wprobe_port; + /** * wprobe_update_links: get a list of all link partners - * @ifname: name of the wprobe interface + * @dev: wprobe device structure * @list: linked list for storing link descriptions * * when wprobe_update_links is called multiple times, the linked list @@ -114,8 +137,16 @@ struct wprobe_iface { extern int wprobe_update_links(struct wprobe_iface *dev); /** + * wprobe_dump_filters: dump all layer 2 filter counters + * @dev: wprobe device structure + * @cb: callback (called once per filter group) + * @arg: user argument for the callback + */ +extern int wprobe_dump_filters(struct wprobe_iface *dev, wprobe_filter_cb cb, void *arg); + +/** * wprobe_measure: start a measurement request for all global attributes - * @ifname: name of the wprobe interface + * @dev: wprobe device structure * * not all attributes are automatically filled with data, since for some * it may be desirable to control the sampling interval from user space @@ -124,7 +155,7 @@ extern int wprobe_update_links(struct wprobe_iface *dev); extern int wprobe_measure(struct wprobe_iface *dev); /** - * wprobe_get_dev: get device information + * wprobe_get_dev: get a handle to a local wprobe device * @ifname: name of the wprobe interface * * queries the wprobe interface for all attributes @@ -133,6 +164,12 @@ extern int wprobe_measure(struct wprobe_iface *dev); extern struct wprobe_iface *wprobe_get_dev(const char *ifname); /** + * wprobe_get_auto: get a handle to a local or remote wprobe device + * @arg: pointer to the wprobe device, either <dev> (local) or <host>:<dev> (remote) + */ +extern struct wprobe_iface *wprobe_get_auto(const char *arg, char **err); + +/** * wprobe_get_dev: free all device information * @dev: wprobe device structure */ @@ -156,4 +193,21 @@ extern int wprobe_apply_config(struct wprobe_iface *dev); */ extern int wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr); +/** + * wprobe_server_init: send a wprobe server init message to a server's client socket + * @socket: socket of the connection to the client + */ +extern int wprobe_server_init(int socket); + +/** + * wprobe_server_handle: read a request from the client socket, process it, send the response + * @socket: socket of the connection to the client + */ +extern int wprobe_server_handle(int socket); + +/** + * wprobe_server_done: release memory allocated for the server connection + */ +extern void wprobe_server_done(void); + #endif |