From 5c105d9f3fd086aff195d3849dcf847d6b0bd927 Mon Sep 17 00:00:00 2001 From: blogic Date: Fri, 5 Oct 2012 10:12:53 +0000 Subject: branch Attitude Adjustment git-svn-id: svn://svn.openwrt.org/openwrt/branches/attitude_adjustment@33625 3c298f89-4303-0410-b956-a3cf2f4a3e73 --- package/wprobe/Makefile | 143 ++++ package/wprobe/files/wprobe.config | 10 + package/wprobe/files/wprobe.init | 72 ++ package/wprobe/src/Makefile.inc | 12 + package/wprobe/src/exporter/Makefile | 5 + package/wprobe/src/exporter/wprobe-export.c | 304 +++++++ package/wprobe/src/exporter/wprobe-export.h | 34 + package/wprobe/src/filter/README.txt | 1 + package/wprobe/src/filter/gen_filter.pl | 63 ++ package/wprobe/src/filter/pfc.c | 58 ++ package/wprobe/src/kernel/Makefile | 5 + package/wprobe/src/kernel/linux/wprobe.h | 397 +++++++++ package/wprobe/src/kernel/wprobe-core.c | 1164 ++++++++++++++++++++++++++ package/wprobe/src/kernel/wprobe-dummy.c | 96 +++ package/wprobe/src/user/Makefile | 38 + package/wprobe/src/user/list.h | 601 +++++++++++++ package/wprobe/src/user/wprobe-lib.c | 1210 +++++++++++++++++++++++++++ package/wprobe/src/user/wprobe-util.c | 450 ++++++++++ package/wprobe/src/user/wprobe.h | 213 +++++ 19 files changed, 4876 insertions(+) create mode 100644 package/wprobe/Makefile create mode 100644 package/wprobe/files/wprobe.config create mode 100755 package/wprobe/files/wprobe.init create mode 100644 package/wprobe/src/Makefile.inc create mode 100644 package/wprobe/src/exporter/Makefile create mode 100644 package/wprobe/src/exporter/wprobe-export.c create mode 100644 package/wprobe/src/exporter/wprobe-export.h create mode 100644 package/wprobe/src/filter/README.txt create mode 100755 package/wprobe/src/filter/gen_filter.pl create mode 100644 package/wprobe/src/filter/pfc.c create mode 100644 package/wprobe/src/kernel/Makefile create mode 100644 package/wprobe/src/kernel/linux/wprobe.h create mode 100644 package/wprobe/src/kernel/wprobe-core.c create mode 100644 package/wprobe/src/kernel/wprobe-dummy.c create mode 100644 package/wprobe/src/user/Makefile create mode 100644 package/wprobe/src/user/list.h create mode 100644 package/wprobe/src/user/wprobe-lib.c create mode 100644 package/wprobe/src/user/wprobe-util.c create mode 100644 package/wprobe/src/user/wprobe.h (limited to 'package/wprobe') diff --git a/package/wprobe/Makefile b/package/wprobe/Makefile new file mode 100644 index 000000000..de1ba6d63 --- /dev/null +++ b/package/wprobe/Makefile @@ -0,0 +1,143 @@ +# +# Copyright (C) 2008-2010 OpenWrt.org +# +# This is free software, licensed under the GNU General Public License v2. +# See /LICENSE for more information. + +include $(TOPDIR)/rules.mk +include $(INCLUDE_DIR)/kernel.mk + +PKG_NAME:=wprobe +PKG_VERSION:=1 +PKG_RELEASE:=1 + +PKG_BUILD_DEPENDS:=PACKAGE_wprobe-export:libipfix + +PKG_CONFIG_DEPENDS = \ + CONFIG_PACKAGE_kmod-wprobe \ + CONFIG_PACKAGE_wprobe-export \ + +include $(INCLUDE_DIR)/package.mk + +# XXX: build failure on cris +# wprobe-lib.c:145: error: 'packed' attribute ignored for field of type 'struct ' +# wprobe-lib.c:149: error: 'packed' attribute ignored for field of type 'struct ' + +define Package/wprobe/Default + DEPENDS:=@(!(TARGET_ps3||TARGET_pxcab||cris)||BROKEN) +endef + +define KernelPackage/wprobe +$(call Package/wprobe/Default) + SUBMENU:=Network Support + TITLE:=Wireless driver probe infrastructure + MAINTAINER:=Felix Fietkau + FILES:= \ + $(PKG_BUILD_DIR)/kernel/wprobe.ko + AUTOLOAD:=$(call AutoLoad,01,wprobe) +endef + +define KernelPackage/wprobe/description + A module that exports measurement data from wireless driver to user space +endef + +define Package/wprobe-util +$(call Package/wprobe/Default) + SECTION:=net + CATEGORY:=Network + DEPENDS+=+kmod-wprobe +libnl-tiny + TITLE:=Wireless measurement utility +endef + +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 +$(call Package/wprobe/Default) + SECTION:=net + CATEGORY:=Network + DEPENDS+=+wprobe-util + TITLE:=Wireless measurement data exporter +endef + +define Package/wprobe-export/description + wprobe-export uses the wprobe kernel module to export + wireless driver measurement data via the IPFIX protocol +endef + +define Package/wprobe-export/conffiles +/etc/config/wprobe +endef + +define Build/Prepare + mkdir -p $(PKG_BUILD_DIR) + $(CP) src/* $(PKG_BUILD_DIR)/ +endef + +TARGET_CPPFLAGS := \ + -D_GNU_SOURCE \ + -I$(STAGING_DIR)/usr/include/libnl-tiny \ + $(TARGET_CPPFLAGS) + +ifdef CONFIG_PACKAGE_kmod-wprobe + define Build/Compile/kmod + $(MAKE) -C $(LINUX_DIR) \ + CROSS_COMPILE="$(KERNEL_CROSS)" \ + ARCH="$(LINUX_KARCH)" \ + SUBDIRS="$(PKG_BUILD_DIR)/kernel" \ + KERNELDIR=$(LINUX_DIR) \ + CC="$(TARGET_CC)" \ + EXTRA_CFLAGS="-I$(PKG_BUILD_DIR)/kernel" \ + modules + endef +endif + +define Build/Compile/lib + $(MAKE) -C $(PKG_BUILD_DIR)/user \ + $(TARGET_CONFIGURE_OPTS) \ + CFLAGS="$(TARGET_CFLAGS)" \ + CPPFLAGS="$(TARGET_CPPFLAGS) -I$(PKG_BUILD_DIR)/kernel" \ + LDFLAGS="$(TARGET_LDFLAGS)" \ + HOST_OS=Linux \ + LIBNL="-lnl-tiny" +endef + +ifdef CONFIG_PACKAGE_wprobe-export + define Build/Compile/exporter + $(MAKE) -C $(PKG_BUILD_DIR)/exporter \ + $(TARGET_CONFIGURE_OPTS) \ + CFLAGS="$(TARGET_CFLAGS)" \ + CPPFLAGS="$(TARGET_CPPFLAGS) -I$(PKG_BUILD_DIR)/kernel -I$(PKG_BUILD_DIR)/user" \ + LDFLAGS="$(TARGET_LDFLAGS)" \ + LIBS="$(PKG_BUILD_DIR)/user/libwprobe.a $(STAGING_DIR)/usr/lib/libipfix.a $(STAGING_DIR)/usr/lib/libipfixmisc.a -lnl-tiny -lm" + endef +endif + +define Build/Compile + $(Build/Compile/kmod) + $(Build/Compile/lib) + $(Build/Compile/exporter) +endef + +define Build/InstallDev + $(INSTALL_DIR) $(1)/usr/include/wprobe + $(CP) $(PKG_BUILD_DIR)/kernel/linux $(1)/usr/include/wprobe +endef + +define Package/wprobe-util/install + $(INSTALL_DIR) $(1)/sbin + $(INSTALL_BIN) $(PKG_BUILD_DIR)/user/wprobe-util $(1)/sbin/ +endef + +define Package/wprobe-export/install + $(INSTALL_DIR) $(1)/sbin $(1)/etc/init.d $(1)/etc/config + $(INSTALL_BIN) ./files/wprobe.init $(1)/etc/init.d/wprobe + $(INSTALL_BIN) ./files/wprobe.config $(1)/etc/config/wprobe + $(INSTALL_BIN) $(PKG_BUILD_DIR)/exporter/wprobe-export $(1)/sbin/ +endef + +$(eval $(call KernelPackage,wprobe)) +$(eval $(call BuildPackage,wprobe-util)) +$(eval $(call BuildPackage,wprobe-export)) diff --git a/package/wprobe/files/wprobe.config b/package/wprobe/files/wprobe.config new file mode 100644 index 000000000..63518ef6e --- /dev/null +++ b/package/wprobe/files/wprobe.config @@ -0,0 +1,10 @@ +config export + # uncomment this line to enable ipfix export: + # option type ipfix + option ifname ath0 + option host ipfix-col + option proto tcp + +# enable public wprobe protocol access +config export + option type wprobe diff --git a/package/wprobe/files/wprobe.init b/package/wprobe/files/wprobe.init new file mode 100755 index 000000000..b35246d42 --- /dev/null +++ b/package/wprobe/files/wprobe.init @@ -0,0 +1,72 @@ +#!/bin/sh /etc/rc.common +START=90 +EXPORTER=/sbin/wprobe-export +UTIL=/sbin/wprobe-util + +wprobe_ssd() { + local cmd="$1"; shift + local type="$1"; shift + local app="$1"; shift + start-stop-daemon "$cmd" -p "/var/run/wprobe-$type.pid" -b ${app:+-x "$app"} -m -- "$@" +} + +stop_wprobe() { + local type="$1" + [ -f "/var/run/wprobe-$type.pid" ] && wprobe_ssd -K "$type" + rm -f "/var/run/wprobe-$type.pid" +} + +config_wprobe() { + config_get ifname "$cfg" ifname + config_get interval "$cfg" interval + [ -n "$interval" ] || interval=100 + $UTIL "$ifname" -i "$interval" 2>/dev/null >/dev/null +} + +start_proxy() { + config_get port "$cfg" port + wprobe_ssd -S proxy "$UTIL" -P -p "${port:-17990}" +} + +start_ipfix() { + local cfg="$1" + config_get ifname "$cfg" ifname + config_get host "$cfg" host + config_get port "$cfg" port + config_get proto "$cfg" proto + case "$proto" in + sctp) proto="-s";; + tcp) proto="-t";; + udp) proto="-u";; + *) proto="-t";; + esac + [ -z "$ifname" -o -z "$host" ] && { + echo "wprobe-export: missing host or interface name in config $cfg" + return + } + config_wprobe "$cfg" + wprobe_ssd -S "export-$cfg" "$EXPORTER" "$proto" -i "$ifname" -c "$host" -p "${port:-4739}" +} + +start_export() { + local cfg="$1" + config_get export_type "$cfg" type + case "$export_type" in + ipfix) [ -x "$EXPORTER" ] && start_ipfix "$cfg";; + wprobe) start_proxy "$cfg";; + esac +} + +stop() { + for f in /var/run/wprobe-*.pid; do + CFG="${f%%.pid}" + CFG="${CFG##/var/run/wprobe-}" + stop_wprobe "$CFG" + done +} + +start() { + config_load wprobe + config_foreach config_wprobe interface + config_foreach start_export export +} diff --git a/package/wprobe/src/Makefile.inc b/package/wprobe/src/Makefile.inc new file mode 100644 index 000000000..5044c0a55 --- /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 -Wno-format +CFLAGS?=-O2 +CPPFLAGS= +LDFLAGS= +LIBS= + diff --git a/package/wprobe/src/exporter/Makefile b/package/wprobe/src/exporter/Makefile new file mode 100644 index 000000000..9f8150729 --- /dev/null +++ b/package/wprobe/src/exporter/Makefile @@ -0,0 +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 new file mode 100644 index 000000000..a0e52e2f4 --- /dev/null +++ b/package/wprobe/src/exporter/wprobe-export.c @@ -0,0 +1,304 @@ +/* +** exporter.c - example exporter +** +** Copyright Fraunhofer FOKUS +** +*/ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +static ipfix_datarecord_t g_data = { NULL, NULL, 0 }; +static int do_close = 0; + +struct wprobe_mapping { + int id; + bool counter; + const char *wprobe_id; + struct wprobe_value *val; +}; + +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(_array) (sizeof(_array) / sizeof((_array)[0])) +#endif + +#define WMAP(_id, _name, ...) \ + { \ + .counter = false, \ + .id = IPFIX_FT_WPROBE_##_id##_N, \ + .wprobe_id = _name \ + , ## __VA_ARGS__ \ + } + +#define WMAP_COUNTER(_id, _name, ...) \ + { \ + .counter = true, \ + .id = IPFIX_FT_WPROBE_##_id, \ + .wprobe_id = _name \ + , ## __VA_ARGS__ \ + } + + +#define WPROBE_OFFSET 2 + +static struct wprobe_mapping map_globals[] = { + WMAP(NOISE, "noise"), + WMAP(PHY_BUSY, "phy_busy"), + WMAP(PHY_RX, "phy_rx"), + WMAP(PHY_TX, "phy_tx"), + WMAP_COUNTER(FRAMES, "frames"), + WMAP_COUNTER(PROBEREQ, "probereq"), +}; + +static struct wprobe_mapping map_perlink[] = { + WMAP(IEEE_TX_RATE, "tx_rate"), + WMAP(IEEE_RX_RATE, "rx_rate"), + WMAP(RSSI, "rssi"), + WMAP(SIGNAL, "signal"), + WMAP(RETRANSMIT_200, "retransmit_200"), + WMAP(RETRANSMIT_400, "retransmit_400"), + WMAP(RETRANSMIT_800, "retransmit_800"), + WMAP(RETRANSMIT_1600, "retransmit_1600"), +}; + +static unsigned char link_local[6]; +static char link_default[6]; +static int nfields = 0; + +#define FOKUS_USERID 12325 + +static void +match_template(struct wprobe_mapping *map, int n, struct list_head *list) +{ + struct wprobe_attribute *attr; + int i, j, last = -1; + + list_for_each_entry(attr, list, list) { + for (i = 0; i < n; i++) { + j = (last + 1 + i) % n; + if (!strcmp(attr->name, map[j].wprobe_id)) + goto found; + } + continue; +found: + last = j; + map[j].val = &attr->val; + memset(&attr->val, 0, sizeof(attr->val)); + nfields++; + } +} + +/* name: export_ipfix_get_template() + */ +static ipfix_template_t * +prepare_template(ipfix_t *handle) +{ + ipfix_template_t *t = NULL; + int size = 3 * nfields + WPROBE_OFFSET; + int i; + + if (ipfix_new_data_template( handle, &t, size) < 0) { + mlogf( 0, "ipfix_new_template() failed: %s\n", strerror(errno) ); + exit(1); + } + + ipfix_add_field(handle, t, 0, IPFIX_FT_SOURCEMACADDRESS, 6); + ipfix_add_field(handle, t, 0, IPFIX_FT_DESTINATIONMACADDRESS, 6); + + g_data.lens = calloc(size, sizeof(g_data.lens[0])); + g_data.lens[0] = 6; + g_data.lens[1] = 6; + for (i = WPROBE_OFFSET; i < size; i++) + g_data.lens[i] = 4; + + g_data.addrs = calloc(size, sizeof(g_data.addrs[0])); + g_data.addrs[0] = link_local; + g_data.maxfields = WPROBE_OFFSET; + return t; +} + +static void +add_template_fields(ipfix_t *handle, ipfix_template_t *t, struct wprobe_mapping *map, int n) +{ + int f = g_data.maxfields; + int i; + + for (i = 0; i < n; i++) { + if (!map[i].val) + continue; + + if (map[i].counter) + g_data.addrs[f++] = &map[i].val->U32; + else + g_data.addrs[f++] = &map[i].val->n; + + if (ipfix_add_field( handle, t, FOKUS_USERID, map[i].id + 0, 4) < 0) + exit(1); + + if (map[i].counter) + continue; + + g_data.lens[f] = 8; + g_data.addrs[f++] = &map[i].val->s; + + g_data.lens[f] = 8; + g_data.addrs[f++] = &map[i].val->ss; + if (ipfix_add_field( handle, t, FOKUS_USERID, map[i].id + 1, 8) < 0) + exit(1); + if (ipfix_add_field( handle, t, FOKUS_USERID, map[i].id + 2, 8) < 0) + exit(1); + } + g_data.maxfields = f; +} + +static void +wprobe_dump_data(ipfix_t *ipfixh, ipfix_template_t *ipfixt, struct wprobe_iface *dev) +{ + struct wprobe_link *link; + + wprobe_update_links(dev); + wprobe_request_data(dev, NULL); + if (list_empty(&dev->links)) { + g_data.addrs[1] = link_default; + ipfix_export_array(ipfixh, ipfixt, g_data.maxfields, g_data.addrs, g_data.lens); + ipfix_export_flush(ipfixh); + } + list_for_each_entry(link, &dev->links, list) { + g_data.addrs[1] = link->addr; + wprobe_request_data(dev, link->addr); + ipfix_export_array(ipfixh, ipfixt, g_data.maxfields, g_data.addrs, g_data.lens); + ipfix_export_flush(ipfixh); + } +} + +int main ( int argc, char **argv ) +{ + struct wprobe_iface *dev = NULL; + ipfix_template_t *ipfixt = NULL; + ipfix_t *ipfixh = NULL; + int protocol = IPFIX_PROTO_TCP; + char *chost = NULL; + char *ifname = NULL; + int sourceid = 12345; + int port = IPFIX_PORTNO; + int verbose_level = 0; + int opt, i = 10; + char *err = NULL; + + while ((opt = getopt(argc, argv, "hi:c:p:vstu")) != EOF) { + switch (opt) { + case 'p': + if ((port=atoi(optarg)) <0) { + fprintf( stderr, "Invalid -p argument!\n" ); + exit(1); + } + break; + case 'i': + ifname = optarg; + break; + case 'c': + chost = optarg; + break; + + case 's': + protocol = IPFIX_PROTO_SCTP; + break; + + case 't': + protocol = IPFIX_PROTO_TCP; + break; + + case 'u': + protocol = IPFIX_PROTO_UDP; + break; + + case 'v': + verbose_level ++; + break; + + case 'h': + default: + fprintf(stderr, "usage: %s [-hstuv] -i -c [-p portno]\n" + " -h this help\n" + " -i wprobe interface\n" + " -c collector address\n" + " -p collector port number (default=%d)\n" + " -s send data via SCTP\n" + " -t send data via TCP (default)\n" + " -u send data via UDP\n" + " -v increase verbose level\n\n", + argv[0], IPFIX_PORTNO ); + exit(1); + } + } + + if (!ifname) { + fprintf(stderr, "No interface specified\n"); + return -1; + } + + if (!chost) { + fprintf(stderr, "No collector specified\n"); + return -1; + } + + dev = wprobe_get_auto(ifname, &err); + if (!dev || (list_empty(&dev->global_attr) && list_empty(&dev->link_attr))) { + fprintf(stderr, "Cannot connect to wprobe on interface '%s': %s\n", ifname, (err ? err : "Unknown error")); + return -1; + } + + match_template(map_globals, ARRAY_SIZE(map_globals), &dev->global_attr); + match_template(map_perlink, ARRAY_SIZE(map_perlink), &dev->link_attr); + if (nfields == 0) { + fprintf(stderr, "No usable attributes found\n"); + return -1; + } + + mlog_set_vlevel( verbose_level ); + if (ipfix_init() < 0) { + fprintf( stderr, "cannot init ipfix module: %s\n", strerror(errno) ); + exit(1); + } + + ipfix_add_vendor_information_elements(ipfix_ft_fokus); + if (ipfix_open(&ipfixh, sourceid, IPFIX_VERSION) < 0) { + fprintf( stderr, "ipfix_open() failed: %s\n", strerror(errno) ); + exit(1); + } + + if (ipfix_add_collector( ipfixh, chost, port, protocol ) < 0) { + fprintf( stderr, "ipfix_add_collector(%s,%d) failed: %s\n", + chost, port, strerror(errno)); + exit(1); + } + + fprintf(stderr, "Local link address: %02x:%02x:%02x:%02x:%02x:%02x\n", + link_local[0], link_local[1], link_local[2], + link_local[3], link_local[4], link_local[5]); + + ipfixt = prepare_template(ipfixh); + add_template_fields(ipfixh, ipfixt, map_globals, ARRAY_SIZE(map_globals)); + add_template_fields(ipfixh, ipfixt, map_perlink, ARRAY_SIZE(map_perlink)); + + while (!do_close) { + sleep(1); + wprobe_dump_data(ipfixh, ipfixt, dev); + } + + ipfix_delete_template( ipfixh, ipfixt ); + ipfix_close( ipfixh ); + ipfix_cleanup(); + exit(0); +} diff --git a/package/wprobe/src/exporter/wprobe-export.h b/package/wprobe/src/exporter/wprobe-export.h new file mode 100644 index 000000000..89da9e86e --- /dev/null +++ b/package/wprobe/src/exporter/wprobe-export.h @@ -0,0 +1,34 @@ +#include +#include +#include + +/** + * struct wprobe_value: data structure for attribute values + * see kernel api netlink attributes for more information + */ +struct wprobe_value { + /* attribute value */ + union data { + const char *STRING; + uint8_t U8; + uint16_t U16; + uint32_t U32; + uint64_t U64; + int8_t S8; + int16_t S16; + int32_t S32; + int64_t S64; + } data; + /* statistics */ + int64_t s, ss; + double avg, stdev; + unsigned int n; + void *usedata; /* Pointer to used data.something */ +}; + +struct exporter_data { + int id; /* ipfix id */ + int userid; /* focus or global */ + int size; /* size in byte*/ + struct wprobe_value val; +}; 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 () { + 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 +#include +#include +#include +#include +#include + +#include +#include + +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 \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/Makefile b/package/wprobe/src/kernel/Makefile new file mode 100644 index 000000000..2a9875311 --- /dev/null +++ b/package/wprobe/src/kernel/Makefile @@ -0,0 +1,5 @@ +EXTRA_CFLAGS += -I. + +obj-m := wprobe.o wprobe-dummy.o + +wprobe-objs := wprobe-core.o diff --git a/package/wprobe/src/kernel/linux/wprobe.h b/package/wprobe/src/kernel/linux/wprobe.h new file mode 100644 index 000000000..901daf3d1 --- /dev/null +++ b/package/wprobe/src/kernel/linux/wprobe.h @@ -0,0 +1,397 @@ +/* + * wprobe.h: API for the wireless probe interface + * Copyright (C) 2008-2009 Felix Fietkau + * + * 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. + */ + +#ifndef __WPROBE_H +#define __WPROBE_H + +#ifdef __KERNEL__ +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +/** + * enum wprobe_attr: netlink attribute list + * + * @WPROBE_ATTR_UNSPEC: unused + * + * @WPROBE_ATTR_INTERFACE: interface name to process query on (NLA_STRING) + * @WPROBE_ATTR_MAC: mac address (used for wireless links) (NLA_STRING) + * @WPROBE_ATTR_FLAGS: interface/link/attribute flags (see enum wprobe_flags) (NLA_U32)a + * @WPROBE_ATTR_DURATION: sampling duration (in milliseconds) (NLA_MSECS) + * + * @WPROBE_ATTR_ID: attribute id (NLA_U32) + * @WPROBE_ATTR_NAME: attribute name (NLA_STRING) + * @WPROBE_ATTR_TYPE: attribute type (NLA_U8) + * + * attribute values: + * + * @WPROBE_VAL_STRING: string value (NLA_STRING) + * @WPROBE_VAL_S8: signed 8-bit integer (NLA_U8) + * @WPROBE_VAL_S16: signed 16-bit integer (NLA_U16) + * @WPROBE_VAL_S32: signed 32-bit integer (NLA_U32) + * @WPROBE_VAL_S64: signed 64-bit integer (NLA_U64) + * @WPROBE_VAL_U8: unsigned 8-bit integer (NLA_U8) + * @WPROBE_VAL_U16: unsigned 16-bit integer (NLA_U16) + * @WPROBE_VAL_U32: unsigned 32-bit integer (NLA_U32) + * @WPROBE_VAL_U64: unsigned 64-bit integer (NLA_U64) + * + * statistics: + * @WPROBE_VAL_SUM: sum of all samples + * @WPROBE_VAL_SUM_SQ: sum of all samples^2 + * @WPROBE_VAL_SAMPLES: number of samples + * @WPROBE_VAL_SCALE_TIME: last time the samples were scaled down + * + * configuration: + * @WPROBE_ATTR_INTERVAL: (measurement interval in milliseconds) (NLA_MSECS) + * @WPROBE_ATTR_SAMPLES_MIN: minimum samples to keep during inactivity (NLA_U32) + * @WPROBE_ATTR_SAMPLES_MAX: maximum samples to keep before scaling down (NLA_U32) + * @WPROBE_ATTR_SAMPLES_SCALE_M: multiplier for scaling down samples (NLA_U32) + * @WPROBE_ATTR_SAMPLES_SCALE_D: divisor for scaling down samples (NLA_U32) + * + * @WPROBE_ATTR_LAST: unused + */ +enum wprobe_attr { + /* query attributes */ + WPROBE_ATTR_UNSPEC, + WPROBE_ATTR_INTERFACE, + WPROBE_ATTR_MAC, + WPROBE_ATTR_FLAGS, + + /* response data */ + WPROBE_ATTR_ID, + WPROBE_ATTR_NAME, + WPROBE_ATTR_TYPE, + WPROBE_ATTR_DURATION, + + /* value type attributes */ + WPROBE_VAL_STRING, + WPROBE_VAL_S8, + WPROBE_VAL_S16, + WPROBE_VAL_S32, + WPROBE_VAL_S64, + WPROBE_VAL_U8, + WPROBE_VAL_U16, + WPROBE_VAL_U32, + WPROBE_VAL_U64, + + /* aggregates for statistics */ + WPROBE_VAL_SUM, + WPROBE_VAL_SUM_SQ, + WPROBE_VAL_SAMPLES, + WPROBE_VAL_SCALE_TIME, + + /* config attributes */ + WPROBE_ATTR_INTERVAL, + WPROBE_ATTR_SAMPLES_MIN, + 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 +}; + + +/** + * enum wprobe_cmd: netlink commands for interacting with wprobe + * + * @WPROBE_CMD_UNSPEC: unused + * + * @WPROBE_CMD_GET_LIST: get global/link property list + * @WPROBE_CMD_GET_INFO: get global/link properties + * @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 + * + * options for GET_INFO and SET_FLAGS: + * - mac address set: per-link + * - mac address unset: globalsa + */ +enum wprobe_cmd { + WPROBE_CMD_UNSPEC, + WPROBE_CMD_GET_LIST, + WPROBE_CMD_GET_INFO, + WPROBE_CMD_SET_FLAGS, + 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 + * @WPROBE_F_NEWDATA: used to indicate that a value has been updated + */ +enum wprobe_flags { + WPROBE_F_KEEPSTAT = (1 << 0), + WPROBE_F_RESET = (1 << 1), + WPROBE_F_NEWDATA = (1 << 2), +}; + +#ifdef __KERNEL__ + +struct wprobe_link; +struct wprobe_item; +struct wprobe_source; +struct wprobe_value; + +/** + * struct wprobe_link - data structure describing a wireless link + * @iface: pointer to the wprobe_iface that this link belongs to + * @addr: BSSID of the remote link partner + * @flags: link flags (see wprobe_flags) + * @priv: user pointer + * + * @list: for internal use + * @val: for internal use + */ +struct wprobe_link { + struct list_head list; + struct wprobe_iface *iface; + char addr[ETH_ALEN]; + u32 flags; + void *priv; + struct wprobe_value *val; +}; + +/** + * struct wprobe_item - data structure describing the format of wprobe_link::data or wprobe_iface::data + * @name: name of the field + * @type: data type of this field + * @flags: measurement item flags (see wprobe_flags) + */ +struct wprobe_item { + const char *name; + enum wprobe_attr type; + u32 flags; +}; + +struct wprobe_value { + bool pending; + union { + /* + * the following are kept uppercase to allow + * for automated checking against WPROBE_VAL_* + * via BUG_ON() + */ + const char *STRING; + u8 U8; + u16 U16; + u32 U32; + u64 U64; + s8 S8; + s16 S16; + s32 S32; + s64 S64; + }; + s64 s, ss; + unsigned int n; + + /* timestamps */ + u64 first, last; + 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 + * + * @name: name of the interface + * @addr: local mac address of the interface + * @links: list of wireless links to poll + * @link_items: description of the per-link data structure + * @n_link_items: number of link description items + * @global_items: description of the per-interface data structure + * @n_global_items: number of per-interface description items + * @sync_data: callback allowing the driver to prepare data for the wprobe poll + * + * @list: head for the list of interfaces + * @priv: user pointer + * @lock: spinlock protecting value data access + * @val: internal use + * @query_val: internal use + * + * if sync_data is NULL, wprobe assumes that it can access the data structure + * at any time (in atomic context). if sync_data returns a negative error code, + * the poll request will not be handled for the given link + */ +struct wprobe_iface { + /* to be filled in by wprobe source drivers */ + const char *name; + const char *addr; + const struct wprobe_item *link_items; + int n_link_items; + const struct wprobe_item *global_items; + int n_global_items; + + int (*sync_data)(struct wprobe_iface *dev, struct wprobe_link *l, + struct wprobe_value *val, bool measure); + void *priv; + + /* handled by the wprobe core */ + struct list_head list; + struct list_head links; + spinlock_t lock; + struct wprobe_value *val; + struct wprobe_value *query_val; + struct wprobe_filter *active_filter; + + u32 measure_interval; + struct timer_list measure_timer; + + u32 scale_min; + u32 scale_max; + u32 scale_m; + u32 scale_d; +}; + + +#define WPROBE_FILL_BEGIN(_ptr, _list) do { \ + struct wprobe_value *__val = (_ptr); \ + const struct wprobe_item *__item = _list; \ + u64 __msecs = jiffies_to_msecs(jiffies) + +#define WPROBE_SET(_idx, _type, _value) \ + if (__item[_idx].type != WPROBE_VAL_##_type) { \ + printk("ERROR: invalid data type at %s:%d\n", __FILE__, __LINE__); \ + break; \ + } \ + __val[_idx].pending = true; \ + __val[_idx]._type = _value; \ + if (!__val[_idx].first) \ + __val[_idx].first = __msecs; \ + __val[_idx].first = __msecs + +#define WPROBE_FILL_END() \ +} while(0) + +/** + * wprobe_add_iface: register an interface with the wireless probe subsystem + * @dev: wprobe_iface structure describing the interface + */ +extern int __weak wprobe_add_iface(struct wprobe_iface *dev); + +/** + * wprobe_remove_iface: deregister an interface from the wireless probe subsystem + * @dev: wprobe_iface structure describing the interface + */ +extern void __weak wprobe_remove_iface(struct wprobe_iface *dev); + +/** + * wprobe_add_link: register a new wireless link + * @dev: wprobe_iface structure describing the interface + * @l: storage space for the wprobe_link structure + * @addr: mac address of the new link + * + * the entire wprobe_link structure is overwritten by this function call + */ +extern int __weak wprobe_add_link(struct wprobe_iface *dev, struct wprobe_link *l, const char *addr); + +/** + * wprobe_remove_link: deregister a previously registered wireless link + * @dev: wprobe_iface structure describing the interface + * @l: wprobe_link data structure + */ +extern void __weak wprobe_remove_link(struct wprobe_iface *dev, struct wprobe_link *l); + +/** + * wprobe_update_stats: update statistics after sampling values + * @dev: wprobe_iface structure describing the interface + * @l: wprobe_link data structure + * + * if l == NULL, then the stats for globals are updated + */ +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 new file mode 100644 index 000000000..6ec847a7a --- /dev/null +++ b/package/wprobe/src/kernel/wprobe-core.c @@ -0,0 +1,1164 @@ +/* + * wprobe-core.c: Wireless probe interface core + * Copyright (C) 2008-2009 Felix Fietkau + * + * 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 +#include +#include +#include +#include +#include +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26) +#include +#else +#include +#endif +#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0) +#include +#endif +#include +#include +#include + +#define static + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28) +#define list_for_each_rcu(pos, head) \ +for (pos = rcu_dereference((head)->next); \ +prefetch(pos->next), pos != (head); \ +pos = rcu_dereference(pos->next)) +#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; + +static struct genl_family wprobe_fam = { + .id = GENL_ID_GENERATE, + .name = "wprobe", + .hdrsize = 0, + .version = 1, + /* only the first set of attributes is used for queries */ + .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) +{ + unsigned long flags; + + INIT_LIST_HEAD(&l->list); + l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC); + if (!l->val) + return -ENOMEM; + + l->iface = s; + memcpy(&l->addr, addr, ETH_ALEN); + spin_lock_irqsave(&wprobe_lock, flags); + list_add_tail_rcu(&l->list, &s->links); + spin_unlock_irqrestore(&wprobe_lock, flags); + + return 0; +} +EXPORT_SYMBOL(wprobe_add_link); + +void +wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l) +{ + unsigned long flags; + + spin_lock_irqsave(&wprobe_lock, flags); + list_del_rcu(&l->list); + spin_unlock_irqrestore(&wprobe_lock, flags); + synchronize_rcu(); + kfree(l->val); +} +EXPORT_SYMBOL(wprobe_remove_link); + +static void +wprobe_measure_timer(unsigned long data) +{ + struct wprobe_iface *dev = (struct wprobe_iface *) data; + + /* set next measurement interval */ + mod_timer(&dev->measure_timer, jiffies + + msecs_to_jiffies(dev->measure_interval)); + + /* perform measurement */ + wprobe_sync_data(dev, NULL, false); +} + +int +wprobe_add_iface(struct wprobe_iface *s) +{ + unsigned long flags; + int vsize; + + /* reset only wprobe private area */ + memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list)); + + BUG_ON(!s->name); + INIT_LIST_HEAD(&s->list); + INIT_LIST_HEAD(&s->links); + setup_timer(&s->measure_timer, wprobe_measure_timer, (unsigned long) s); + + 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; + + /* initialize defaults to be able to handle overflow, + * user space will need to handle this if it keeps an + * internal histogram */ + s->scale_min = 20; + s->scale_max = (1 << 31); + + s->scale_m = 1; + s->scale_d = 10; + + spin_lock_irqsave(&wprobe_lock, flags); + list_add_rcu(&s->list, &wprobe_if); + spin_unlock_irqrestore(&wprobe_lock, flags); + + return 0; + +error: + if (s->val) + kfree(s->val); + return -ENOMEM; +} +EXPORT_SYMBOL(wprobe_add_iface); + +void +wprobe_remove_iface(struct wprobe_iface *s) +{ + unsigned long flags; + + BUG_ON(!list_empty(&s->links)); + + del_timer_sync(&s->measure_timer); + spin_lock_irqsave(&wprobe_lock, flags); + list_del_rcu(&s->list); + spin_unlock_irqrestore(&wprobe_lock, flags); + + /* wait for all queries to finish before freeing the + * temporary value storage buffer */ + synchronize_rcu(); + + kfree(s->val); + kfree(s->query_val); + if (s->active_filter) + wprobe_free_filter(s->active_filter); +} +EXPORT_SYMBOL(wprobe_remove_iface); + +static struct wprobe_iface * +wprobe_get_dev(struct nlattr *attr) +{ + struct wprobe_iface *dev = NULL; + struct wprobe_iface *p; + const char *name; + int i = 0; + + if (!attr) + return NULL; + + name = nla_data(attr); + list_for_each_entry_rcu(p, &wprobe_if, list) { + i++; + if (strcmp(name, p->name) != 0) + continue; + + dev = p; + break; + } + + return dev; +} + +int +wprobe_add_frame(struct wprobe_iface *dev, const struct wprobe_wlan_hdr *hdr, void *data, int len) +{ + struct wprobe_wlan_hdr *new_hdr; + 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; + + new_hdr = (struct wprobe_wlan_hdr *) skb_put(skb, f->hdrlen); + memcpy(new_hdr, hdr, sizeof(struct wprobe_wlan_hdr)); + new_hdr->len = cpu_to_be16(new_hdr->len); + + 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 LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,38) + if (sk_run_filter(skb, fi->filter) == 0) + continue; +#else + if (sk_run_filter(skb, fi->filter, fi->hdr.n_items) == 0) + continue; +#endif + + 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) +{ + struct wprobe_value *val; + unsigned long flags; + int n, err; + + if (l) { + n = dev->n_link_items; + val = l->val; + } else { + n = dev->n_global_items; + val = dev->val; + } + + spin_lock_irqsave(&dev->lock, flags); + err = dev->sync_data(dev, l, val, !query); + if (err) + goto done; + + if (query) + memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n); + + wprobe_update_stats(dev, l); +done: + spin_unlock_irqrestore(&dev->lock, flags); + return 0; +} +EXPORT_SYMBOL(wprobe_sync_data); + +static void +wprobe_scale_stats(struct wprobe_iface *dev, const struct wprobe_item *item, + struct wprobe_value *val, int n) +{ + u64 scale_ts = jiffies_64; + int i; + + for (i = 0; i < n; i++) { + if (!(item[i].flags & WPROBE_F_KEEPSTAT)) + continue; + + if (val[i].n <= dev->scale_min) + continue; + + /* FIXME: div_s64 seems to be very imprecise here, even when + * the values are scaled up */ + val[i].s *= dev->scale_m; + val[i].s = div_s64(val[i].s, dev->scale_d); + + val[i].ss *= dev->scale_m; + val[i].ss = div_s64(val[i].ss, dev->scale_d); + + val[i].n = (val[i].n * dev->scale_m) / dev->scale_d; + val[i].scale_timestamp = scale_ts; + } +} + + +void +wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l) +{ + const struct wprobe_item *item; + struct wprobe_value *val; + bool scale_stats = false; + int i, n; + + if (l) { + n = dev->n_link_items; + item = dev->link_items; + val = l->val; + } else { + n = dev->n_global_items; + item = dev->global_items; + val = dev->val; + } + + /* process statistics */ + for (i = 0; i < n; i++) { + s64 v; + + if (!val[i].pending) + continue; + + val[i].n++; + if ((item[i].flags & WPROBE_F_KEEPSTAT) && + (dev->scale_max > 0) && (val[i].n > dev->scale_max)) { + scale_stats = true; + } + + switch(item[i].type) { + case WPROBE_VAL_S8: + v = val[i].S8; + break; + case WPROBE_VAL_S16: + v = val[i].S16; + break; + case WPROBE_VAL_S32: + v = val[i].S32; + break; + case WPROBE_VAL_S64: + v = val[i].S64; + break; + case WPROBE_VAL_U8: + v = val[i].U8; + break; + case WPROBE_VAL_U16: + v = val[i].U16; + break; + case WPROBE_VAL_U32: + v = val[i].U32; + break; + case WPROBE_VAL_U64: + v = val[i].U64; + break; + default: + continue; + } + + val[i].s += v; + val[i].ss += v * v; + val[i].pending = false; + } + if (scale_stats) + wprobe_scale_stats(dev, item, val, n); +} +EXPORT_SYMBOL(wprobe_update_stats); + +static const struct nla_policy wprobe_policy[WPROBE_ATTR_LAST+1] = { + [WPROBE_ATTR_INTERFACE] = { .type = NLA_NUL_STRING }, + [WPROBE_ATTR_MAC] = { .type = NLA_STRING }, + [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 }, + + /* config */ + [WPROBE_ATTR_INTERVAL] = { .type = NLA_MSECS }, + [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] = { .type = NLA_BINARY, .len = 32768 }, +}; + +static bool +wprobe_check_ptr(struct list_head *list, struct list_head *ptr) +{ + struct list_head *p; + + list_for_each_rcu(p, list) { + if (ptr == p) + return true; + } + return false; +} + +static bool +wprobe_send_item_value(struct sk_buff *msg, struct netlink_callback *cb, + struct wprobe_iface *dev, struct wprobe_link *l, + const struct wprobe_item *item, + int i, u32 flags) +{ + struct genlmsghdr *hdr; + struct wprobe_value *val = dev->query_val; + u64 time = val[i].last - val[i].first; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO); + + NLA_PUT_U32(msg, WPROBE_ATTR_ID, i); + NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, flags); + NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type); + NLA_PUT_U64(msg, WPROBE_ATTR_DURATION, time); + + switch(item[i].type) { + case WPROBE_VAL_S8: + case WPROBE_VAL_U8: + NLA_PUT_U8(msg, item[i].type, val[i].U8); + break; + case WPROBE_VAL_S16: + case WPROBE_VAL_U16: + NLA_PUT_U16(msg, item[i].type, val[i].U16); + break; + case WPROBE_VAL_S32: + case WPROBE_VAL_U32: + NLA_PUT_U32(msg, item[i].type, val[i].U32); + break; + case WPROBE_VAL_S64: + case WPROBE_VAL_U64: + NLA_PUT_U64(msg, item[i].type, val[i].U64); + break; + case WPROBE_VAL_STRING: + if (val[i].STRING) + NLA_PUT_STRING(msg, item[i].type, val[i].STRING); + else + NLA_PUT_STRING(msg, item[i].type, ""); + /* bypass avg/stdev */ + goto done; + default: + /* skip unknown values */ + goto done; + } + if (item[i].flags & WPROBE_F_KEEPSTAT) { + NLA_PUT_U64(msg, WPROBE_VAL_SUM, val[i].s); + NLA_PUT_U64(msg, WPROBE_VAL_SUM_SQ, val[i].ss); + NLA_PUT_U32(msg, WPROBE_VAL_SAMPLES, (u32) val[i].n); + NLA_PUT_MSECS(msg, WPROBE_VAL_SCALE_TIME, val[i].scale_timestamp); + } +done: + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + +static bool +wprobe_send_item_info(struct sk_buff *msg, struct netlink_callback *cb, + struct wprobe_iface *dev, + const struct wprobe_item *item, int i) +{ + struct genlmsghdr *hdr; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST); + + if ((i == 0) && (dev->addr != NULL)) + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, dev->addr); + NLA_PUT_U32(msg, WPROBE_ATTR_ID, (u32) i); + NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, item[i].name); + NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type); + NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, item[i].flags); + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + + +static struct wprobe_link * +wprobe_find_link(struct wprobe_iface *dev, const char *mac) +{ + struct wprobe_link *l; + + list_for_each_entry_rcu(l, &dev->links, list) { + if (!memcmp(l->addr, mac, 6)) + return l; + } + return NULL; +} + +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; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS); + if (!hdr) + return false; + + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr); + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + +static int +wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb) +{ + struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0]; + struct wprobe_link *l; + 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; + } else { + if (!wprobe_check_ptr(&wprobe_if, &dev->list)) { + err = -ENODEV; + goto done; + } + } + + rcu_read_lock(); + list_for_each_entry_rcu(l, &dev->links, list) { + if (i < cb->args[1]) + continue; + + if (unlikely(!wprobe_dump_link(skb, l, cb))) + break; + + i++; + } + cb->args[1] = i; + rcu_read_unlock(); + err = skb->len; +done: + return err; +} + +#define WPROBE_F_LINK (1 << 31) /* for internal use */ +static int +wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb) +{ + struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0]; + struct wprobe_link *l = (struct wprobe_link *)cb->args[1]; + struct wprobe_value *val; + const struct wprobe_item *item; + struct genlmsghdr *hdr; + unsigned long flags; + int cmd, n, i = cb->args[3]; + u32 vflags = cb->args[2]; + int err = 0; + + hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh); + cmd = hdr->cmd; + + /* since the attribute value list might be too big for a single netlink + * message, the device, link and offset get stored in the netlink callback. + * if this is the first request, we need to do the full lookup for the device. + * + * access to the device and link structure is synchronized through rcu. + */ + rcu_read_lock(); + if (!dev) { + err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize, + wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy); + if (err) + goto done; + + err = -ENOENT; + dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]); + if (!dev) + goto done; + + if (cmd == WPROBE_CMD_GET_INFO) { + if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) { + l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC])); + if (!l) + goto done; + + vflags = l->flags; + } + + if (l) { + item = dev->link_items; + n = dev->n_link_items; + val = l->val; + } else { + item = dev->global_items; + n = dev->n_global_items; + val = dev->val; + } + + /* sync data and move to temp storage for the query */ + spin_lock_irqsave(&dev->lock, flags); + err = wprobe_sync_data(dev, l, true); + if (!err) + memcpy(dev->query_val, val, n * sizeof(struct wprobe_value)); + spin_unlock_irqrestore(&dev->lock, flags); + + if (err) + goto done; + } + + if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]) + vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]); + + if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) + vflags |= WPROBE_F_LINK; + + cb->args[0] = (long) dev; + cb->args[1] = (long) l; + cb->args[2] = vflags; + cb->args[3] = 0; + } else { + /* when pulling pointers from the callback, validate them + * against the list using rcu to make sure that we won't + * dereference pointers to free'd memory after the last + * grace period */ + err = -ENOENT; + if (!wprobe_check_ptr(&wprobe_if, &dev->list)) + goto done; + + if (l && !wprobe_check_ptr(&dev->links, &l->list)) + goto done; + } + + if (vflags & WPROBE_F_LINK) { + item = dev->link_items; + n = dev->n_link_items; + } else { + item = dev->global_items; + n = dev->n_global_items; + } + + err = 0; + switch(cmd) { + case WPROBE_CMD_GET_INFO: + while (i < n) { + if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags)) + break; + i++; + } + break; + case WPROBE_CMD_GET_LIST: + while (i < n) { + if (!wprobe_send_item_info(skb, cb, dev, item, i)) + break; + i++; + } + break; + default: + err = -EINVAL; + goto done; + } + cb->args[3] = i; + err = skb->len; + +done: + rcu_read_unlock(); + return err; +} +#undef WPROBE_F_LINK + +static int +wprobe_update_auto_measurement(struct wprobe_iface *dev, u32 interval) +{ + if (interval && (interval < WPROBE_MIN_INTERVAL)) + return -EINVAL; + + if (!interval && dev->measure_interval) + del_timer_sync(&dev->measure_timer); + + dev->measure_interval = interval; + if (!interval) + return 0; + + /* kick of a new measurement immediately */ + mod_timer(&dev->measure_timer, jiffies + 1); + + return 0; +} + +static int +wprobe_measure(struct sk_buff *skb, struct genl_info *info) +{ + struct wprobe_iface *dev; + struct wprobe_link *l = NULL; + int err = -ENOENT; + + rcu_read_lock(); + dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]); + if (!dev) + goto done; + + if (info->attrs[WPROBE_ATTR_MAC]) { + l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC])); + if (!l) + goto done; + } + + err = wprobe_sync_data(dev, l, false); + +done: + rcu_read_unlock(); + return err; +} + +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); + hdr->n_items = cur_is; + 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; + + hdr->name[31] = 0; + n_items = be32_to_cpu(hdr->n_items); + hdr->n_items = n_items; + + if (n_items > 1024) + goto overrun; + + 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) + 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; + unsigned long flags; + 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]); + if (!dev) + goto done_unlocked; + + err = -EINVAL; + spin_lock_irqsave(&dev->lock, flags); + if (info->attrs[WPROBE_ATTR_MAC]) { + /* not supported yet */ + 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 ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MIN])) + scale_min = nla_get_u32(attr); + else + scale_min = dev->scale_min; + + if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MAX])) + scale_max = nla_get_u32(attr); + else + scale_max = dev->scale_max; + + if ((!scale_min && !scale_max) || + (scale_min && scale_max && (scale_min < scale_max))) { + dev->scale_min = scale_min; + dev->scale_max = scale_max; + } else { + goto done; + } + } + + if (info->attrs[WPROBE_ATTR_SAMPLES_SCALE_M] && + info->attrs[WPROBE_ATTR_SAMPLES_SCALE_D]) { + + scale_m = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_SCALE_M]); + scale_d = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_SCALE_D]); + + if (!scale_d || (scale_m > scale_d)) + goto done; + + dev->scale_m = scale_m; + 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 */ + err = wprobe_update_auto_measurement(dev, + (u32) nla_get_u64(info->attrs[WPROBE_ATTR_INTERVAL])); + } + +done: + spin_unlock_irqrestore(&dev->lock, flags); +done_unlocked: + rcu_read_unlock(); + if (filter_free) { + synchronize_rcu(); + wprobe_free_filter(filter_free); + } + return err; +} + +static struct genl_ops wprobe_ops[] = { + { + .cmd = WPROBE_CMD_GET_INFO, + .dumpit = wprobe_dump_info, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_GET_LIST, + .dumpit = wprobe_dump_info, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_MEASURE, + .doit = wprobe_measure, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_GET_LINKS, + .dumpit = wprobe_dump_links, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_CONFIG, + .doit = wprobe_set_config, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_GET_FILTER, + .dumpit = wprobe_dump_filters, + .policy = wprobe_policy, + }, +}; + +static void __exit +wprobe_exit(void) +{ + BUG_ON(!list_empty(&wprobe_if)); + genl_unregister_family(&wprobe_fam); +} + + +static int __init +wprobe_init(void) +{ + int i, err; + + spin_lock_init(&wprobe_lock); + INIT_LIST_HEAD(&wprobe_if); + + err = genl_register_family(&wprobe_fam); + if (err) + return err; + + for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) { + err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]); + if (err) + goto error; + } + + return 0; + +error: + genl_unregister_family(&wprobe_fam); + return err; +} + +module_init(wprobe_init); +module_exit(wprobe_exit); +MODULE_LICENSE("GPL"); + diff --git a/package/wprobe/src/kernel/wprobe-dummy.c b/package/wprobe/src/kernel/wprobe-dummy.c new file mode 100644 index 000000000..4231223e0 --- /dev/null +++ b/package/wprobe/src/kernel/wprobe-dummy.c @@ -0,0 +1,96 @@ +/* + * wprobe-core.c: Wireless probe interface dummy driver + * Copyright (C) 2008-2009 Felix Fietkau + * + * 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 +#include +#include +#include +#include +#include +#include + +static const char local_addr[] = "\x00\x13\x37\xba\xbe\x00"; + +enum dummy_global_values { + DUMMY_GLOBAL_MEDIUM_BUSY +}; +enum dummy_link_values { + DUMMY_LINK_SNR +}; + +struct wprobe_item dummy_perlink[] = { + [DUMMY_LINK_SNR] = { + .name = "snr", + .type = WPROBE_VAL_U8, + .flags = WPROBE_F_KEEPSTAT, + }, +}; + +struct wprobe_item dummy_globals[] = { + [DUMMY_GLOBAL_MEDIUM_BUSY] = { + .name = "medium_busy", + .type = WPROBE_VAL_U8, + .flags = WPROBE_F_KEEPSTAT, + } +}; + +int dummy_sync(struct wprobe_iface *dev, struct wprobe_link *l, struct wprobe_value *val, bool measure) +{ + u8 intval = 0; + + get_random_bytes(&intval, 1); + if (l) { + WPROBE_FILL_BEGIN(val, dummy_perlink); + WPROBE_SET(DUMMY_LINK_SNR, U8, (intval % 40)); + WPROBE_FILL_END(); + } else { + WPROBE_FILL_BEGIN(val, dummy_globals); + WPROBE_SET(DUMMY_GLOBAL_MEDIUM_BUSY, U8, (intval % 100)); + WPROBE_FILL_END(); + } + return 0; +} + +static struct wprobe_iface dummy_dev = { + .name = "dummy", + .addr = local_addr, + .link_items = dummy_perlink, + .n_link_items = ARRAY_SIZE(dummy_perlink), + .global_items = dummy_globals, + .n_global_items = ARRAY_SIZE(dummy_globals), + .sync_data = dummy_sync, +}; + +static struct wprobe_link dummy_link; + +static int __init +wprobe_dummy_init(void) +{ + wprobe_add_iface(&dummy_dev); + wprobe_add_link(&dummy_dev, &dummy_link, "\x00\x13\x37\xda\xda\x00"); + return 0; +} + +static void __exit +wprobe_dummy_exit(void) +{ + wprobe_remove_link(&dummy_dev, &dummy_link); + wprobe_remove_iface(&dummy_dev); +} + +module_init(wprobe_dummy_init); +module_exit(wprobe_dummy_exit); + +MODULE_LICENSE("GPL"); diff --git a/package/wprobe/src/user/Makefile b/package/wprobe/src/user/Makefile new file mode 100644 index 000000000..01e83da3e --- /dev/null +++ b/package/wprobe/src/user/Makefile @@ -0,0 +1,38 @@ +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-util + +libwprobe.a: wprobe-lib.o + rm -f $@ + $(AR) rcu $@ $^ + $(RANLIB) $@ + +%.o: %.c + $(CC) $(WFLAGS) -c -o $@ $(CPPFLAGS) $(CFLAGS) $(EXTRA_CFLAGS) $< + +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/list.h b/package/wprobe/src/user/list.h new file mode 100644 index 000000000..2959a061d --- /dev/null +++ b/package/wprobe/src/user/list.h @@ -0,0 +1,601 @@ +#ifndef _LINUX_LIST_H +#define _LINUX_LIST_H + +#include +/** + * container_of - cast a member of a structure out to the containing structure + * @ptr: the pointer to the member. + * @type: the type of the container struct this is embedded in. + * @member: the name of the member within the struct. + * + */ +#ifndef container_of +#define container_of(ptr, type, member) ( \ + (type *)( (char *)ptr - offsetof(type,member) )) +#endif + + +/* + * Simple doubly linked list implementation. + * + * Some of the internal functions ("__xxx") are useful when + * manipulating whole lists rather than single entries, as + * sometimes we already know the next/prev entries and we can + * generate better code by using them directly rather than + * using the generic single-entry routines. + */ + +struct list_head { + struct list_head *next, *prev; +}; + +#define LIST_HEAD_INIT(name) { &(name), &(name) } + +#define LIST_HEAD(name) \ + struct list_head name = LIST_HEAD_INIT(name) + +static inline void INIT_LIST_HEAD(struct list_head *list) +{ + list->next = list; + list->prev = list; +} + +/* + * Insert a new entry between two known consecutive entries. + * + * This is only for internal list manipulation where we know + * the prev/next entries already! + */ +static inline void __list_add(struct list_head *new, + struct list_head *prev, + struct list_head *next) +{ + next->prev = new; + new->next = next; + new->prev = prev; + prev->next = new; +} + +/** + * list_add - add a new entry + * @new: new entry to be added + * @head: list head to add it after + * + * Insert a new entry after the specified head. + * This is good for implementing stacks. + */ +static inline void list_add(struct list_head *new, struct list_head *head) +{ + __list_add(new, head, head->next); +} + + +/** + * list_add_tail - add a new entry + * @new: new entry to be added + * @head: list head to add it before + * + * Insert a new entry before the specified head. + * This is useful for implementing queues. + */ +static inline void list_add_tail(struct list_head *new, struct list_head *head) +{ + __list_add(new, head->prev, head); +} + + +/* + * Delete a list entry by making the prev/next entries + * point to each other. + * + * This is only for internal list manipulation where we know + * the prev/next entries already! + */ +static inline void __list_del(struct list_head * prev, struct list_head * next) +{ + next->prev = prev; + prev->next = next; +} + +/** + * list_del - deletes entry from list. + * @entry: the element to delete from the list. + * Note: list_empty() on entry does not return true after this, the entry is + * in an undefined state. + */ +static inline void list_del(struct list_head *entry) +{ + __list_del(entry->prev, entry->next); + entry->next = NULL; + entry->prev = NULL; +} + +/** + * list_replace - replace old entry by new one + * @old : the element to be replaced + * @new : the new element to insert + * + * If @old was empty, it will be overwritten. + */ +static inline void list_replace(struct list_head *old, + struct list_head *new) +{ + new->next = old->next; + new->next->prev = new; + new->prev = old->prev; + new->prev->next = new; +} + +static inline void list_replace_init(struct list_head *old, + struct list_head *new) +{ + list_replace(old, new); + INIT_LIST_HEAD(old); +} + +/** + * list_del_init - deletes entry from list and reinitialize it. + * @entry: the element to delete from the list. + */ +static inline void list_del_init(struct list_head *entry) +{ + __list_del(entry->prev, entry->next); + INIT_LIST_HEAD(entry); +} + +/** + * list_move - delete from one list and add as another's head + * @list: the entry to move + * @head: the head that will precede our entry + */ +static inline void list_move(struct list_head *list, struct list_head *head) +{ + __list_del(list->prev, list->next); + list_add(list, head); +} + +/** + * list_move_tail - delete from one list and add as another's tail + * @list: the entry to move + * @head: the head that will follow our entry + */ +static inline void list_move_tail(struct list_head *list, + struct list_head *head) +{ + __list_del(list->prev, list->next); + list_add_tail(list, head); +} + +/** + * list_is_last - tests whether @list is the last entry in list @head + * @list: the entry to test + * @head: the head of the list + */ +static inline int list_is_last(const struct list_head *list, + const struct list_head *head) +{ + return list->next == head; +} + +/** + * list_empty - tests whether a list is empty + * @head: the list to test. + */ +static inline int list_empty(const struct list_head *head) +{ + return head->next == head; +} + +/** + * list_empty_careful - tests whether a list is empty and not being modified + * @head: the list to test + * + * Description: + * tests whether a list is empty _and_ checks that no other CPU might be + * in the process of modifying either member (next or prev) + * + * NOTE: using list_empty_careful() without synchronization + * can only be safe if the only activity that can happen + * to the list entry is list_del_init(). Eg. it cannot be used + * if another CPU could re-list_add() it. + */ +static inline int list_empty_careful(const struct list_head *head) +{ + struct list_head *next = head->next; + return (next == head) && (next == head->prev); +} + +static inline void __list_splice(struct list_head *list, + struct list_head *head) +{ + struct list_head *first = list->next; + struct list_head *last = list->prev; + struct list_head *at = head->next; + + first->prev = head; + head->next = first; + + last->next = at; + at->prev = last; +} + +/** + * list_splice - join two lists + * @list: the new list to add. + * @head: the place to add it in the first list. + */ +static inline void list_splice(struct list_head *list, struct list_head *head) +{ + if (!list_empty(list)) + __list_splice(list, head); +} + +/** + * list_splice_init - join two lists and reinitialise the emptied list. + * @list: the new list to add. + * @head: the place to add it in the first list. + * + * The list at @list is reinitialised + */ +static inline void list_splice_init(struct list_head *list, + struct list_head *head) +{ + if (!list_empty(list)) { + __list_splice(list, head); + INIT_LIST_HEAD(list); + } +} + +/** + * list_entry - get the struct for this entry + * @ptr: the &struct list_head pointer. + * @type: the type of the struct this is embedded in. + * @member: the name of the list_struct within the struct. + */ +#define list_entry(ptr, type, member) \ + container_of(ptr, type, member) + +/** + * list_first_entry - get the first element from a list + * @ptr: the list head to take the element from. + * @type: the type of the struct this is embedded in. + * @member: the name of the list_struct within the struct. + * + * Note, that list is expected to be not empty. + */ +#define list_first_entry(ptr, type, member) \ + list_entry((ptr)->next, type, member) + +/** + * list_for_each - iterate over a list + * @pos: the &struct list_head to use as a loop cursor. + * @head: the head for your list. + */ +#define list_for_each(pos, head) \ + for (pos = (head)->next; pos != (head); \ + pos = pos->next) + +/** + * __list_for_each - iterate over a list + * @pos: the &struct list_head to use as a loop cursor. + * @head: the head for your list. + * + * This variant differs from list_for_each() in that it's the + * simplest possible list iteration code, no prefetching is done. + * Use this for code that knows the list to be very short (empty + * or 1 entry) most of the time. + */ +#define __list_for_each(pos, head) \ + for (pos = (head)->next; pos != (head); pos = pos->next) + +/** + * list_for_each_prev - iterate over a list backwards + * @pos: the &struct list_head to use as a loop cursor. + * @head: the head for your list. + */ +#define list_for_each_prev(pos, head) \ + for (pos = (head)->prev; pos != (head); \ + pos = pos->prev) + +/** + * list_for_each_safe - iterate over a list safe against removal of list entry + * @pos: the &struct list_head to use as a loop cursor. + * @n: another &struct list_head to use as temporary storage + * @head: the head for your list. + */ +#define list_for_each_safe(pos, n, head) \ + for (pos = (head)->next, n = pos->next; pos != (head); \ + pos = n, n = pos->next) + +/** + * list_for_each_prev_safe - iterate over a list backwards safe against removal of list entry + * @pos: the &struct list_head to use as a loop cursor. + * @n: another &struct list_head to use as temporary storage + * @head: the head for your list. + */ +#define list_for_each_prev_safe(pos, n, head) \ + for (pos = (head)->prev, n = pos->prev; \ + pos != (head); \ + pos = n, n = pos->prev) + +/** + * list_for_each_entry - iterate over list of given type + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define list_for_each_entry(pos, head, member) \ + for (pos = list_entry((head)->next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_reverse - iterate backwards over list of given type. + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define list_for_each_entry_reverse(pos, head, member) \ + for (pos = list_entry((head)->prev, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.prev, typeof(*pos), member)) + +/** + * list_prepare_entry - prepare a pos entry for use in list_for_each_entry_continue() + * @pos: the type * to use as a start point + * @head: the head of the list + * @member: the name of the list_struct within the struct. + * + * Prepares a pos entry for use as a start point in list_for_each_entry_continue(). + */ +#define list_prepare_entry(pos, head, member) \ + ((pos) ? : list_entry(head, typeof(*pos), member)) + +/** + * list_for_each_entry_continue - continue iteration over list of given type + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Continue to iterate over list of given type, continuing after + * the current position. + */ +#define list_for_each_entry_continue(pos, head, member) \ + for (pos = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_continue_reverse - iterate backwards from the given point + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Start to iterate over list of given type backwards, continuing after + * the current position. + */ +#define list_for_each_entry_continue_reverse(pos, head, member) \ + for (pos = list_entry(pos->member.prev, typeof(*pos), member); \ + &pos->member != (head); \ + pos = list_entry(pos->member.prev, typeof(*pos), member)) + +/** + * list_for_each_entry_from - iterate over list of given type from the current point + * @pos: the type * to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate over list of given type, continuing from current position. + */ +#define list_for_each_entry_from(pos, head, member) \ + for (; &pos->member != (head); \ + pos = list_entry(pos->member.next, typeof(*pos), member)) + +/** + * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + */ +#define list_for_each_entry_safe(pos, n, head, member) \ + for (pos = list_entry((head)->next, typeof(*pos), member), \ + n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_for_each_entry_safe_continue + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate over list of given type, continuing after current point, + * safe against removal of list entry. + */ +#define list_for_each_entry_safe_continue(pos, n, head, member) \ + for (pos = list_entry(pos->member.next, typeof(*pos), member), \ + n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_for_each_entry_safe_from + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate over list of given type from current point, safe against + * removal of list entry. + */ +#define list_for_each_entry_safe_from(pos, n, head, member) \ + for (n = list_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.next, typeof(*n), member)) + +/** + * list_for_each_entry_safe_reverse + * @pos: the type * to use as a loop cursor. + * @n: another type * to use as temporary storage + * @head: the head for your list. + * @member: the name of the list_struct within the struct. + * + * Iterate backwards over list of given type, safe against removal + * of list entry. + */ +#define list_for_each_entry_safe_reverse(pos, n, head, member) \ + for (pos = list_entry((head)->prev, typeof(*pos), member), \ + n = list_entry(pos->member.prev, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = list_entry(n->member.prev, typeof(*n), member)) + +/* + * Double linked lists with a single pointer list head. + * Mostly useful for hash tables where the two pointer list head is + * too wasteful. + * You lose the ability to access the tail in O(1). + */ + +struct hlist_head { + struct hlist_node *first; +}; + +struct hlist_node { + struct hlist_node *next, **pprev; +}; + +#define HLIST_HEAD_INIT { .first = NULL } +#define HLIST_HEAD(name) struct hlist_head name = { .first = NULL } +#define INIT_HLIST_HEAD(ptr) ((ptr)->first = NULL) +static inline void INIT_HLIST_NODE(struct hlist_node *h) +{ + h->next = NULL; + h->pprev = NULL; +} + +static inline int hlist_unhashed(const struct hlist_node *h) +{ + return !h->pprev; +} + +static inline int hlist_empty(const struct hlist_head *h) +{ + return !h->first; +} + +static inline void __hlist_del(struct hlist_node *n) +{ + struct hlist_node *next = n->next; + struct hlist_node **pprev = n->pprev; + *pprev = next; + if (next) + next->pprev = pprev; +} + +static inline void hlist_del(struct hlist_node *n) +{ + __hlist_del(n); + n->next = NULL; + n->pprev = NULL; +} + +static inline void hlist_del_init(struct hlist_node *n) +{ + if (!hlist_unhashed(n)) { + __hlist_del(n); + INIT_HLIST_NODE(n); + } +} + + +static inline void hlist_add_head(struct hlist_node *n, struct hlist_head *h) +{ + struct hlist_node *first = h->first; + n->next = first; + if (first) + first->pprev = &n->next; + h->first = n; + n->pprev = &h->first; +} + + +/* next must be != NULL */ +static inline void hlist_add_before(struct hlist_node *n, + struct hlist_node *next) +{ + n->pprev = next->pprev; + n->next = next; + next->pprev = &n->next; + *(n->pprev) = n; +} + +static inline void hlist_add_after(struct hlist_node *n, + struct hlist_node *next) +{ + next->next = n->next; + n->next = next; + next->pprev = &n->next; + + if(next->next) + next->next->pprev = &next->next; +} + +#define hlist_entry(ptr, type, member) container_of(ptr,type,member) + +#define hlist_for_each(pos, head) \ + for (pos = (head)->first; pos; pos = pos->next) + +#define hlist_for_each_safe(pos, n, head) \ + for (pos = (head)->first; pos; pos = n) + +/** + * hlist_for_each_entry - iterate over list of given type + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @head: the head for your list. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry(tpos, pos, head, member) \ + for (pos = (head)->first; pos && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = pos->next) + +/** + * hlist_for_each_entry_continue - iterate over a hlist continuing after current point + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_continue(tpos, pos, member) \ + for (pos = (pos)->next; pos && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = pos->next) + +/** + * hlist_for_each_entry_from - iterate over a hlist continuing from current point + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_from(tpos, pos, member) \ + for (; pos && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = pos->next) + +/** + * hlist_for_each_entry_safe - iterate over list of given type safe against removal of list entry + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @n: another &struct hlist_node to use as temporary storage + * @head: the head for your list. + * @member: the name of the hlist_node within the struct. + */ +#define hlist_for_each_entry_safe(tpos, pos, n, head, member) \ + for (pos = (head)->first; \ + pos && ({ n = pos->next; 1; }) && \ + ({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \ + pos = n) + +#endif diff --git a/package/wprobe/src/user/wprobe-lib.c b/package/wprobe/src/user/wprobe-lib.c new file mode 100644 index 000000000..7a5460bb0 --- /dev/null +++ b/package/wprobe/src/user/wprobe-lib.c @@ -0,0 +1,1210 @@ +/* + * wprobe.c: Wireless probe user space library + * Copyright (C) 2008-2009 Felix Fietkau + * + * 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. + */ + +#define _ISOC99_SOURCE +#define _BSD_SOURCE +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef NO_LOCAL_ACCESS +#include +#include +#include +#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); + ret = write(socket, &mhdr, sizeof(mhdr)); + if (ret < 0) + goto out; + + 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); + +out: + 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 + if (err) + *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..654442f9c --- /dev/null +++ b/package/wprobe/src/user/wprobe-util.c @@ -0,0 +1,450 @@ +/* + * wprobe-test.c: Wireless probe user space test code + * Copyright (C) 2008-2009 Felix Fietkau + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#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, "", 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(stdout, "\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(stdout, (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(stdout, + (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(stdout, + (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 |:|-P [options]\n" +#else + "Usage: %s : [options]\n" +#endif + "\n" + "Options:\n" + " -a: Print attributes\n" + " -c: Only apply configuration\n" + " -d: Delay between measurement dumps (in milliseconds, default: 1000)\n" + " A value of 0 (zero) prints once and exits; useful for scripts\n" + " -f: Dump contents of layer 2 filter counters during measurement\n" + " -F : Apply layer 2 filters from \n" + " -h: This help text\n" + " -i : 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(stdout, "Global attribute: '%s' (%s)\n", + attr->name, attr_typestr[attr->type]); + } + list_for_each_entry(attr, &dev->link_attr, list) { + fprintf(stdout, "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(stdout, "Filter group: '%s' (tx/rx)\n", group); + for (i = 0; i < n_items; i++) { + fprintf(stdout, " - %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) +{ + do { + wprobe_update_links(dev); + wprobe_dump_data(dev); + if (print_filters) + wprobe_dump_filters(dev, simple_mode ? show_filter_simple : show_filter, NULL); + usleep(delay * 1000); + } + while (delay); +} + +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 = sizeof(struct sockaddr_in); + 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_attributes = false; + 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, "acd:fF:hi:msp:")) != -1) { + switch(ch) { + case 'a': + print_attributes = true; + break; + 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(stdout, "%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) { + if (print_attributes) + 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.h b/package/wprobe/src/user/wprobe.h new file mode 100644 index 000000000..706facc80 --- /dev/null +++ b/package/wprobe/src/user/wprobe.h @@ -0,0 +1,213 @@ +/* + * wprobe.h: Wireless probe user space library + * Copyright (C) 2008-2009 Felix Fietkau + * + * 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. + */ + +#ifndef __WPROBE_USER_H +#define __WPROBE_USER_H +#include +#include +#include +#include "list.h" + +/** + * struct wprobe_value: data structure for attribute values + * @STRING: string value (currently unsupported) + * @U8: unsigned 8-bit integer value + * @U16: unsigned 16-bit integer value + * @U32: unsigned 32-bit integer value + * @U64: unsigned 64-bit integer value + * @S8: signed 8-bit integer value + * @S16: signed 16-bit integer value + * @S32: signed 32-bit integer value + * @S64: signed 64-bit integer value + * + * @n: number of sample values + * @avg: average value + * @stdev: standard deviation + * @s: sum of all sample values (internal use) + * @ss: sum of all sample values squared (internal use) + */ +struct wprobe_value { + /* attribute value */ + union { + const char *STRING; + uint8_t U8; + uint16_t U16; + uint32_t U32; + uint64_t U64; + int8_t S8; + int16_t S16; + int32_t S32; + int64_t S64; + }; + /* statistics */ + int64_t s, ss; + float avg, stdev; + unsigned int n; +}; + +/** + * struct wprobe_attribute: data structures for attribute descriptions + * @list: linked list data structure for a list of attributes + * @id: attribute id + * @type: netlink type for the attribute (see kernel api documentation) + * @flags: attribute flags (see kernel api documentation) + * @val: cached version of the last netlink query, will be overwritten on each request + * @name: attribute name + */ +struct wprobe_attribute { + struct list_head list; + int id; + int type; + uint32_t flags; + struct wprobe_value val; + char name[]; +}; + +/** + * struct wprobe_link: data structure for the link description + * @list: linked list data structure for a list of links + * @flags: link flags (see kernel api documentation) + * @addr: mac address of the remote link partner + */ +struct wprobe_link { + struct list_head list; + uint32_t flags; + 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; + struct list_head link_attr; + struct list_head links; + + /* config */ + int interval; + int scale_min; + 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 + * @dev: wprobe device structure + * @list: linked list for storing link descriptions + * + * when wprobe_update_links is called multiple times, the linked list + * is updated with new link partners, old entries are automatically expired + */ +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 + * @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 + * you can use this function to do that. + */ +extern int wprobe_measure(struct wprobe_iface *dev); + +/** + * wprobe_get_dev: get a handle to a local wprobe device + * @ifname: name of the wprobe interface + * + * queries the wprobe interface for all attributes + * must be freed with wprobe_free_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 (local) or : (remote) + */ +extern struct wprobe_iface *wprobe_get_auto(const char *arg, char **err); + +/** + * wprobe_get_dev: free all device information + * @dev: wprobe device structure + */ +extern void wprobe_free_dev(struct wprobe_iface *dev); + +/** + * wprobe_apply_config: apply configuration data + * @dev: wprobe device structure + * + * uploads all configuration values from @dev that are not set to -1 + */ +extern int wprobe_apply_config(struct wprobe_iface *dev); + +/** + * wprobe_request_data: request new sampling values for the given list of attributes + * @dev: wprobe device structure + * @addr: (optional) mac address of the link partner + * + * if addr is unset, global values are stored in the global attributes list + * if addr is set, per-link values for the given address are stored in the link attributes list + */ +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 -- cgit v1.2.3