/drivers/net/wireless/bcmdhd/wl_linux_mon.c

  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}