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