bcmdhd.1.363.59.144.x.cn/wl_linux_mon.c

   1 /*
   2  * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
   3  *
   4  * Copyright (C) 1999-2016, Broadcom Corporation
   5  *
   6  *      Unless you and Broadcom execute a separate written software license
   7  * agreement governing use of this software, this software is licensed to you
   8  * under the terms of the GNU General Public License version 2 (the "GPL"),
   9  * available at http://www.broadcom.com/licenses/GPLv2.php, with the
  10  * following added to such license:
  11  *
  12  *      As a special exception, the copyright holders of this software give you
  13  * permission to link this software with independent modules, and to copy and
  14  * distribute the resulting executable under terms of your choice, provided that
  15  * you also meet, for each linked independent module, the terms and conditions of
  16  * the license of that module.  An independent module is a module which is not
  17  * derived from this software.  The special exception does not apply to any
  18  * modifications of the software.
  19  *
  20  *      Notwithstanding the above, under no circumstances may you combine this
  21  * software in any way with any other Broadcom software provided under a license
  22  * other than the GPL, without Broadcom's express prior written consent.
  23  *
  24  *
  25  * <<Broadcom-WL-IPTag/Open:>>
  26  *
  27  * $Id: wl_linux_mon.c 514727 2014-11-12 03:02:48Z $
  28  */
  29
  30 #include <osl.h>
  31 #include <linux/string.h>
  32 #include <linux/module.h>
  33 #include <linux/netdevice.h>
  34 #include <linux/etherdevice.h>
  35 #include <linux/if_arp.h>
  36 #include <linux/ieee80211.h>
  37 #include <linux/rtnetlink.h>
  38 #include <net/ieee80211_radiotap.h>
  39
  40 #include <wlioctl.h>
  41 #include <bcmutils.h>
  42 #include <dhd_dbg.h>
  43 #include <dngl_stats.h>
  44 #include <dhd.h>
  45
  46 typedef enum monitor_states
  47 {
  48         MONITOR_STATE_DEINIT = 0x0,
  49         MONITOR_STATE_INIT = 0x1,
  50         MONITOR_STATE_INTERFACE_ADDED = 0x2,
  51         MONITOR_STATE_INTERFACE_DELETED = 0x4
  52 } monitor_states_t;
  53 int dhd_add_monitor(char *name, struct net_device **new_ndev);
  54 extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
  55 int dhd_del_monitor(struct net_device *ndev);
  56 int dhd_monitor_init(void *dhd_pub);
  57 int dhd_monitor_uninit(void);
  58
  59 /**
  60  * Local declarations and defintions (not exposed)
  61  */
  62 #ifndef DHD_MAX_IFS
  63 #define DHD_MAX_IFS 16
  64 #endif
  65 #define MON_PRINT(format, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
  66 #define MON_TRACE MON_PRINT
  67
  68 typedef struct monitor_interface {
  69         int radiotap_enabled;
  70         struct net_device* real_ndev;   /* The real interface that the monitor is on */
  71         struct net_device* mon_ndev;
  72 } monitor_interface;
  73
  74 typedef struct dhd_linux_monitor {
  75         void *dhd_pub;
  76         monitor_states_t monitor_state;
  77         monitor_interface mon_if[DHD_MAX_IFS];
  78         struct mutex lock;              /* lock to protect mon_if */
  79 } dhd_linux_monitor_t;
  80
  81 static dhd_linux_monitor_t g_monitor;
  82
  83 static struct net_device* lookup_real_netdev(char *name);
  84 static monitor_interface* ndev_to_monif(struct net_device *ndev);
  85 static int dhd_mon_if_open(struct net_device *ndev);
  86 static int dhd_mon_if_stop(struct net_device *ndev);
  87 static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
  88 static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
  89 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
  90
  91 static const struct net_device_ops dhd_mon_if_ops = {
  92         .ndo_open               = dhd_mon_if_open,
  93         .ndo_stop               = dhd_mon_if_stop,
  94         .ndo_start_xmit         = dhd_mon_if_subif_start_xmit,
  95 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0))
  96         .ndo_set_rx_mode = dhd_mon_if_set_multicast_list,
  97 #else
  98         .ndo_set_multicast_list = dhd_mon_if_set_multicast_list,
  99 #endif
 100         .ndo_set_mac_address    = dhd_mon_if_change_mac,
 101 };
 102
 103 /**
 104  * Local static function defintions
 105  */
 106
 107 /* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
 108  * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
 109  */
 110 static struct net_device* lookup_real_netdev(char *name)
 111 {
 112         struct net_device *ndev_found = NULL;
 113
 114         int i;
 115         int len = 0;
 116         int last_name_len = 0;
 117         struct net_device *ndev;
 118
 119         /* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
 120          * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
 121          * iface would be mon-p2p0-0.
 122          */
 123         for (i = 0; i < DHD_MAX_IFS; i++) {
 124                 ndev = dhd_idx2net(g_monitor.dhd_pub, i);
 125
 126                 /* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
 127                  * it matches, then this netdev is the corresponding real_netdev.
 128                  */
 129                 if (ndev && strstr(ndev->name, "p2p-p2p0")) {
 130                         len = strlen("p2p");
 131                 } else {
 132                 /* if p2p- is not present, then the IFNAMSIZ have reached and name
 133                  * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
 134                  */
 135                         len = 0;
 136                 }
 137                 if (ndev && strstr(name, (ndev->name + len))) {
 138                         if (strlen(ndev->name) > last_name_len) {
 139                                 ndev_found = ndev;
 140                                 last_name_len = strlen(ndev->name);
 141                         }
 142                 }
 143         }
 144
 145         return ndev_found;
 146 }
 147
 148 static monitor_interface* ndev_to_monif(struct net_device *ndev)
 149 {
 150         int i;
 151
 152         for (i = 0; i < DHD_MAX_IFS; i++) {
 153                 if (g_monitor.mon_if[i].mon_ndev == ndev)
 154                         return &g_monitor.mon_if[i];
 155         }
 156
 157         return NULL;
 158 }
 159
 160 static int dhd_mon_if_open(struct net_device *ndev)
 161 {
 162         int ret = 0;
 163
 164         MON_PRINT("enter\n");
 165         return ret;
 166 }
 167
 168 static int dhd_mon_if_stop(struct net_device *ndev)
 169 {
 170         int ret = 0;
 171
 172         MON_PRINT("enter\n");
 173         return ret;
 174 }
 175
 176 static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 177 {
 178         int ret = 0;
 179         int rtap_len;
 180         int qos_len = 0;
 181         int dot11_hdr_len = 24;
 182         int snap_len = 6;
 183         unsigned char *pdata;
 184         unsigned short frame_ctl;
 185         unsigned char src_mac_addr[6];
 186         unsigned char dst_mac_addr[6];
 187         struct ieee80211_hdr *dot11_hdr;
 188         struct ieee80211_radiotap_header *rtap_hdr;
 189         monitor_interface* mon_if;
 190
 191         MON_PRINT("enter\n");
 192
 193         mon_if = ndev_to_monif(ndev);
 194         if (mon_if == NULL || mon_if->real_ndev == NULL) {
 195                 MON_PRINT(" cannot find matched net dev, skip the packet\n");
 196                 goto fail;
 197         }
 198
 199         if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
 200                 goto fail;
 201
 202         rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
 203         if (unlikely(rtap_hdr->it_version))
 204                 goto fail;
 205
 206         rtap_len = ieee80211_get_radiotap_len(skb->data);
 207         if (unlikely(skb->len < rtap_len))
 208                 goto fail;
 209
 210         MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
 211
 212         /* Skip the ratio tap header */
 213         skb_pull(skb, rtap_len);
 214
 215         dot11_hdr = (struct ieee80211_hdr *)skb->data;
 216         frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
 217         /* Check if the QoS bit is set */
 218         if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
 219                 /* Check if this ia a Wireless Distribution System (WDS) frame
 220                  * which has 4 MAC addresses
 221                  */
 222                 if (dot11_hdr->frame_control & 0x0080)
 223                         qos_len = 2;
 224                 if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
 225                         dot11_hdr_len += 6;
 226
 227                 memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
 228                 memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
 229
 230                 /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
 231                  * for two MAC addresses
 232                  */
 233                 skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
 234                 pdata = (unsigned char*)skb->data;
 235                 memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
 236                 memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
 237                 PKTSETPRIO(skb, 0);
 238
 239                 MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
 240
 241                 /* Use the real net device to transmit the packet */
 242                 ret = dhd_start_xmit(skb, mon_if->real_ndev);
 243
 244                 return ret;
 245         }
 246 fail:
 247         dev_kfree_skb(skb);
 248         return 0;
 249 }
 250
 251 static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
 252 {
 253         monitor_interface* mon_if;
 254
 255         mon_if = ndev_to_monif(ndev);
 256         if (mon_if == NULL || mon_if->real_ndev == NULL) {
 257                 MON_PRINT(" cannot find matched net dev, skip the packet\n");
 258         } else {
 259                 MON_PRINT("enter, if name: %s, matched if name %s\n",
 260                 ndev->name, mon_if->real_ndev->name);
 261         }
 262 }
 263
 264 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
 265 {
 266         int ret = 0;
 267         monitor_interface* mon_if;
 268
 269         mon_if = ndev_to_monif(ndev);
 270         if (mon_if == NULL || mon_if->real_ndev == NULL) {
 271                 MON_PRINT(" cannot find matched net dev, skip the packet\n");
 272         } else {
 273                 MON_PRINT("enter, if name: %s, matched if name %s\n",
 274                 ndev->name, mon_if->real_ndev->name);
 275         }
 276         return ret;
 277 }
 278
 279 /**
 280  * Global function definitions (declared in dhd_linux_mon.h)
 281  */
 282
 283 int dhd_add_monitor(char *name, struct net_device **new_ndev)
 284 {
 285         int i;
 286         int idx = -1;
 287         int ret = 0;
 288         struct net_device* ndev = NULL;
 289         dhd_linux_monitor_t **dhd_mon;
 290
 291         mutex_lock(&g_monitor.lock);
 292
 293         MON_TRACE("enter, if name: %s\n", name);
 294         if (!name || !new_ndev) {
 295                 MON_PRINT("invalid parameters\n");
 296                 ret = -EINVAL;
 297                 goto out;
 298         }
 299
 300         /*
 301          * Find a vacancy
 302          */
 303         for (i = 0; i < DHD_MAX_IFS; i++)
 304                 if (g_monitor.mon_if[i].mon_ndev == NULL) {
 305                         idx = i;
 306                         break;
 307                 }
 308         if (idx == -1) {
 309                 MON_PRINT("exceeds maximum interfaces\n");
 310                 ret = -EFAULT;
 311                 goto out;
 312         }
 313
 314         ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
 315         if (!ndev) {
 316                 MON_PRINT("failed to allocate memory\n");
 317                 ret = -ENOMEM;
 318                 goto out;
 319         }
 320
 321         ndev->type = ARPHRD_IEEE80211_RADIOTAP;
 322         strncpy(ndev->name, name, IFNAMSIZ);
 323         ndev->name[IFNAMSIZ - 1] = 0;
 324         ndev->netdev_ops = &dhd_mon_if_ops;
 325
 326         ret = register_netdevice(ndev);
 327         if (ret) {
 328                 MON_PRINT(" register_netdevice failed (%d)\n", ret);
 329                 goto out;
 330         }
 331
 332         *new_ndev = ndev;
 333         g_monitor.mon_if[idx].radiotap_enabled = TRUE;
 334         g_monitor.mon_if[idx].mon_ndev = ndev;
 335         g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
 336         dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
 337         *dhd_mon = &g_monitor;
 338         g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
 339         MON_PRINT("net device returned: 0x%p\n", ndev);
 340         MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
 341
 342 out:
 343         if (ret && ndev)
 344                 free_netdev(ndev);
 345
 346         mutex_unlock(&g_monitor.lock);
 347         return ret;
 348
 349 }
 350
 351 int dhd_del_monitor(struct net_device *ndev)
 352 {
 353         int i;
 354         if (!ndev)
 355                 return -EINVAL;
 356         mutex_lock(&g_monitor.lock);
 357         for (i = 0; i < DHD_MAX_IFS; i++) {
 358                 if (g_monitor.mon_if[i].mon_ndev == ndev ||
 359                         g_monitor.mon_if[i].real_ndev == ndev) {
 360
 361                         g_monitor.mon_if[i].real_ndev = NULL;
 362                         unregister_netdevice(g_monitor.mon_if[i].mon_ndev);
 363                         free_netdev(g_monitor.mon_if[i].mon_ndev);
 364                         g_monitor.mon_if[i].mon_ndev = NULL;
 365                         g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
 366                         break;
 367                 }
 368         }
 369
 370         if (g_monitor.monitor_state != MONITOR_STATE_INTERFACE_DELETED)
 371                 MON_PRINT("IF not found in monitor array, is this a monitor IF? 0x%p\n", ndev);
 372         mutex_unlock(&g_monitor.lock);
 373
 374         return 0;
 375 }
 376
 377 int dhd_monitor_init(void *dhd_pub)
 378 {
 379         if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
 380                 g_monitor.dhd_pub = dhd_pub;
 381                 mutex_init(&g_monitor.lock);
 382                 g_monitor.monitor_state = MONITOR_STATE_INIT;
 383         }
 384         return 0;
 385 }
 386
 387 int dhd_monitor_uninit(void)
 388 {
 389         int i;
 390         struct net_device *ndev;
 391         mutex_lock(&g_monitor.lock);
 392         if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
 393                 for (i = 0; i < DHD_MAX_IFS; i++) {
 394                         ndev = g_monitor.mon_if[i].mon_ndev;
 395                         if (ndev) {
 396                                 unregister_netdevice(ndev);
 397                                 free_netdev(ndev);
 398                                 g_monitor.mon_if[i].real_ndev = NULL;
 399                                 g_monitor.mon_if[i].mon_ndev = NULL;
 400                         }
 401                 }
 402                 g_monitor.monitor_state = MONITOR_STATE_DEINIT;
 403         }
 404         mutex_unlock(&g_monitor.lock);
 405         return 0;
 406 }