/drivers/net/wireless/bcmdhd/src/wl/sys/wl_linux_mon.c
C | 425 lines | 308 code | 58 blank | 59 comment | 59 complexity | 59c3dc8445068234a2ec88e6cc0cf042 MD5 | raw file
- /*
- * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
- *
- * Copyright (C) 1999-2011, Broadcom Corporation
- *
- * Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2 (the "GPL"),
- * available at http://www.broadcom.com/licenses/GPLv2.php, with the
- * following added to such license:
- *
- * As a special exception, the copyright holders of this software give you
- * permission to link this software with independent modules, and to copy and
- * distribute the resulting executable under terms of your choice, provided that
- * you also meet, for each linked independent module, the terms and conditions of
- * the license of that module. An independent module is a module which is not
- * derived from this software. The special exception does not apply to any
- * modifications of the software.
- *
- * Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a license
- * other than the GPL, without Broadcom's express prior written consent.
- *
- * $Id: dhd_linux_mon.c 280623 2011-08-30 14:49:39Z $
- */
- #include <osl.h>
- #include <linux/string.h>
- #include <linux/module.h>
- #include <linux/netdevice.h>
- #include <linux/etherdevice.h>
- #include <linux/if_arp.h>
- #include <linux/ieee80211.h>
- #include <linux/rtnetlink.h>
- #include <net/ieee80211_radiotap.h>
- #include <wlioctl.h>
- #include <bcmutils.h>
- #include <dhd_dbg.h>
- #include <dngl_stats.h>
- #include <dhd.h>
- typedef enum monitor_states
- {
- MONITOR_STATE_DEINIT = 0x0,
- MONITOR_STATE_INIT = 0x1,
- MONITOR_STATE_INTERFACE_ADDED = 0x2,
- MONITOR_STATE_INTERFACE_DELETED = 0x4
- } monitor_states_t;
- int dhd_add_monitor(char *name, struct net_device **new_ndev);
- extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
- int dhd_del_monitor(struct net_device *ndev);
- int dhd_monitor_init(void *dhd_pub);
- int dhd_monitor_uninit(void);
- /**
- * Local declarations and defintions (not exposed)
- */
- #ifndef DHD_MAX_IFS
- #define DHD_MAX_IFS 16
- #endif
- #define MON_DEBUG 0
- #if MON_DEBUG
- #define MON_PRINT(format, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
- #define MON_TRACE MON_PRINT
- #else
- #define MON_PRINT(format, ...)
- #define MON_TRACE MON_PRINT
- #endif
- typedef struct monitor_interface {
- int radiotap_enabled;
- struct net_device* real_ndev; /* The real interface that the monitor is on */
- struct net_device* mon_ndev;
- } monitor_interface;
- typedef struct dhd_linux_monitor {
- void *dhd_pub;
- monitor_states_t monitor_state;
- monitor_interface mon_if[DHD_MAX_IFS];
- struct mutex lock; /* lock to protect mon_if */
- } dhd_linux_monitor_t;
- static dhd_linux_monitor_t g_monitor;
- static struct net_device* lookup_real_netdev(char *name);
- static monitor_interface* ndev_to_monif(struct net_device *ndev);
- static int dhd_mon_if_open(struct net_device *ndev);
- static int dhd_mon_if_stop(struct net_device *ndev);
- static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
- static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
- static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
- static const struct net_device_ops dhd_mon_if_ops = {
- .ndo_open = dhd_mon_if_open,
- .ndo_stop = dhd_mon_if_stop,
- .ndo_start_xmit = dhd_mon_if_subif_start_xmit,
- .ndo_set_rx_mode = dhd_mon_if_set_multicast_list,
- .ndo_set_mac_address = dhd_mon_if_change_mac,
- };
- /**
- * Local static function defintions
- */
- /* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
- * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
- */
- static struct net_device* lookup_real_netdev(char *name)
- {
- struct net_device *ndev_found = NULL;
- int i;
- int len = 0;
- int last_name_len = 0;
- struct net_device *ndev;
- /* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
- * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
- * iface would be mon-p2p0-0.
- */
- for (i = 0; i < DHD_MAX_IFS; i++) {
- ndev = dhd_idx2net(g_monitor.dhd_pub, i);
- /* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
- * it matches, then this netdev is the corresponding real_netdev.
- */
- if (ndev && strstr(ndev->name, "p2p-p2p0")) {
- len = strlen("p2p");
- } else {
- /* if p2p- is not present, then the IFNAMSIZ have reached and name
- * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
- */
- len = 0;
- }
- if (ndev && strstr(name, (ndev->name + len))) {
- if (strlen(ndev->name) > last_name_len) {
- ndev_found = ndev;
- last_name_len = strlen(ndev->name);
- }
- }
- }
- return ndev_found;
- }
- static monitor_interface* ndev_to_monif(struct net_device *ndev)
- {
- int i;
- for (i = 0; i < DHD_MAX_IFS; i++) {
- if (g_monitor.mon_if[i].mon_ndev == ndev)
- return &g_monitor.mon_if[i];
- }
- return NULL;
- }
- static int dhd_mon_if_open(struct net_device *ndev)
- {
- int ret = 0;
- MON_PRINT("enter\n");
- return ret;
- }
- static int dhd_mon_if_stop(struct net_device *ndev)
- {
- int ret = 0;
- MON_PRINT("enter\n");
- return ret;
- }
- static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
- {
- int ret = 0;
- int rtap_len;
- int qos_len = 0;
- int dot11_hdr_len = 24;
- int snap_len = 6;
- unsigned char *pdata;
- unsigned short frame_ctl;
- unsigned char src_mac_addr[6];
- unsigned char dst_mac_addr[6];
- struct ieee80211_hdr *dot11_hdr;
- struct ieee80211_radiotap_header *rtap_hdr;
- monitor_interface* mon_if;
- MON_PRINT("enter\n");
- mon_if = ndev_to_monif(ndev);
- if (mon_if == NULL || mon_if->real_ndev == NULL) {
- MON_PRINT(" cannot find matched net dev, skip the packet\n");
- goto fail;
- }
- if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
- goto fail;
- rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
- if (unlikely(rtap_hdr->it_version))
- goto fail;
- rtap_len = ieee80211_get_radiotap_len(skb->data);
- if (unlikely(skb->len < rtap_len))
- goto fail;
- MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
- /* Skip the ratio tap header */
- skb_pull(skb, rtap_len);
- dot11_hdr = (struct ieee80211_hdr *)skb->data;
- frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
- /* Check if the QoS bit is set */
- if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
- /* Check if this ia a Wireless Distribution System (WDS) frame
- * which has 4 MAC addresses
- */
- if (dot11_hdr->frame_control & 0x0080)
- qos_len = 2;
- if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
- dot11_hdr_len += 6;
- memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
- memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
- /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
- * for two MAC addresses
- */
- skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
- pdata = (unsigned char*)skb->data;
- memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
- memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
- PKTSETPRIO(skb, 0);
- MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
- /* Use the real net device to transmit the packet */
- ret = dhd_start_xmit(skb, mon_if->real_ndev);
- return ret;
- }
- fail:
- dev_kfree_skb(skb);
- return 0;
- }
- static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
- {
- monitor_interface* mon_if;
- mon_if = ndev_to_monif(ndev);
- if (mon_if == NULL || mon_if->real_ndev == NULL) {
- MON_PRINT(" cannot find matched net dev, skip the packet\n");
- } else {
- MON_PRINT("enter, if name: %s, matched if name %s\n",
- ndev->name, mon_if->real_ndev->name);
- }
- }
- static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
- {
- int ret = 0;
- monitor_interface* mon_if;
- mon_if = ndev_to_monif(ndev);
- if (mon_if == NULL || mon_if->real_ndev == NULL) {
- MON_PRINT(" cannot find matched net dev, skip the packet\n");
- } else {
- MON_PRINT("enter, if name: %s, matched if name %s\n",
- ndev->name, mon_if->real_ndev->name);
- }
- return ret;
- }
- /**
- * Global function definitions (declared in dhd_linux_mon.h)
- */
- int dhd_add_monitor(char *name, struct net_device **new_ndev)
- {
- int i;
- int idx = -1;
- int ret = 0;
- struct net_device* ndev = NULL;
- dhd_linux_monitor_t **dhd_mon;
- mutex_lock(&g_monitor.lock);
- MON_TRACE("enter, if name: %s\n", name);
- if (!name || !new_ndev) {
- MON_PRINT("invalid parameters\n");
- ret = -EINVAL;
- goto out;
- }
- /*
- * Find a vacancy
- */
- for (i = 0; i < DHD_MAX_IFS; i++)
- if (g_monitor.mon_if[i].mon_ndev == NULL) {
- idx = i;
- break;
- }
- if (idx == -1) {
- MON_PRINT("exceeds maximum interfaces\n");
- ret = -EFAULT;
- goto out;
- }
- ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
- if (!ndev) {
- MON_PRINT("failed to allocate memory\n");
- ret = -ENOMEM;
- goto out;
- }
- ndev->type = ARPHRD_IEEE80211_RADIOTAP;
- strncpy(ndev->name, name, IFNAMSIZ);
- ndev->name[IFNAMSIZ - 1] = 0;
- ndev->netdev_ops = &dhd_mon_if_ops;
- ret = register_netdevice(ndev);
- if (ret) {
- MON_PRINT(" register_netdevice failed (%d)\n", ret);
- goto out;
- }
- *new_ndev = ndev;
- g_monitor.mon_if[idx].radiotap_enabled = TRUE;
- g_monitor.mon_if[idx].mon_ndev = ndev;
- g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
- dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
- *dhd_mon = &g_monitor;
- g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
- MON_PRINT("net device returned: 0x%p\n", ndev);
- MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
- out:
- if (ret && ndev)
- free_netdev(ndev);
- mutex_unlock(&g_monitor.lock);
- return ret;
- }
- int dhd_del_monitor(struct net_device *ndev)
- {
- int i;
- bool rollback_lock = false;
- if (!ndev)
- return -EINVAL;
- mutex_lock(&g_monitor.lock);
- for (i = 0; i < DHD_MAX_IFS; i++) {
- if (g_monitor.mon_if[i].mon_ndev == ndev ||
- g_monitor.mon_if[i].real_ndev == ndev) {
- g_monitor.mon_if[i].real_ndev = NULL;
- if (rtnl_is_locked()) {
- rtnl_unlock();
- rollback_lock = true;
- }
- unregister_netdev(g_monitor.mon_if[i].mon_ndev);
- free_netdev(g_monitor.mon_if[i].mon_ndev);
- g_monitor.mon_if[i].mon_ndev = NULL;
- g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
- break;
- }
- }
- if (rollback_lock) {
- rtnl_lock();
- rollback_lock = false;
- }
- if (g_monitor.monitor_state !=
- MONITOR_STATE_INTERFACE_DELETED)
- MON_PRINT("interface not found in monitor IF array, is this a monitor IF? 0x%p\n",
- ndev);
- mutex_unlock(&g_monitor.lock);
- return 0;
- }
- int dhd_monitor_init(void *dhd_pub)
- {
- if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
- g_monitor.dhd_pub = dhd_pub;
- mutex_init(&g_monitor.lock);
- g_monitor.monitor_state = MONITOR_STATE_INIT;
- }
- return 0;
- }
- int dhd_monitor_uninit(void)
- {
- int i;
- struct net_device *ndev;
- bool rollback_lock = false;
- mutex_lock(&g_monitor.lock);
- if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
- for (i = 0; i < DHD_MAX_IFS; i++) {
- ndev = g_monitor.mon_if[i].mon_ndev;
- if (ndev) {
- if (rtnl_is_locked()) {
- rtnl_unlock();
- rollback_lock = true;
- }
- unregister_netdev(ndev);
- free_netdev(ndev);
- g_monitor.mon_if[i].real_ndev = NULL;
- g_monitor.mon_if[i].mon_ndev = NULL;
- if (rollback_lock) {
- rtnl_lock();
- rollback_lock = false;
- }
- }
- }
- g_monitor.monitor_state = MONITOR_STATE_DEINIT;
- }
- mutex_unlock(&g_monitor.lock);
- return 0;
- }