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