6c1ff4d8ad419d3b78b3694071f2d657bb3a2b8e
[linux-2.6.git] / drivers / net / wireless / bcmdhd / dhd_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: dhd_linux_mon.c,v 1.131.2.55 2011-02-09 05:31:56 Exp $
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
43 typedef 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;
50 extern 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, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
56 #define MON_TRACE MON_PRINT
57
58 typedef 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
64 typedef 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
71 static dhd_linux_monitor_t g_monitor;
72
73 static struct net_device* lookup_real_netdev(char *name);
74 static monitor_interface* ndev_to_monif(struct net_device *ndev);
75 static int dhd_mon_if_open(struct net_device *ndev);
76 static int dhd_mon_if_stop(struct net_device *ndev);
77 static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
78 static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
79 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
80
81 static 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  */
96 static struct net_device* lookup_real_netdev(char *name)
97 {
98         int i;
99         int last_name_len = 0;
100         struct net_device *ndev;
101         struct net_device *ndev_found = NULL;
102
103         /* We want to find interface "p2p-eth0-0" for monitor interface "mon.p2p-eth0-0", so
104          * we skip "eth0" even if "mon.p2p-eth0-0" contains "eth0"
105          */
106         for (i = 0; i < DHD_MAX_IFS; i++) {
107                 ndev = dhd_idx2net(g_monitor.dhd_pub, i);
108                 if (ndev && strstr(name, ndev->name)) {
109                         if (strlen(ndev->name) > last_name_len)
110                                 ndev_found = ndev;
111                 }
112         }
113
114         return ndev_found;
115 }
116
117 static monitor_interface* ndev_to_monif(struct net_device *ndev)
118 {
119         int i;
120
121         for (i = 0; i < DHD_MAX_IFS; i++) {
122                 if (g_monitor.mon_if[i].mon_ndev == ndev)
123                         return &g_monitor.mon_if[i];
124         }
125
126         return NULL;
127 }
128
129 static int dhd_mon_if_open(struct net_device *ndev)
130 {
131         int ret = 0;
132
133         MON_PRINT("enter\n");
134         return ret;
135 }
136
137 static int dhd_mon_if_stop(struct net_device *ndev)
138 {
139         int ret = 0;
140
141         MON_PRINT("enter\n");
142         return ret;
143 }
144
145 static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
146 {
147         int ret = 0;
148         int rtap_len;
149         int qos_len = 0;
150         int dot11_hdr_len = 24;
151         int snap_len = 6;
152         unsigned char *pdata;
153         unsigned short frame_ctl;
154         unsigned char src_mac_addr[6];
155         unsigned char dst_mac_addr[6];
156         struct ieee80211_hdr *dot11_hdr;
157         struct ieee80211_radiotap_header *rtap_hdr;
158         monitor_interface* mon_if;
159
160         MON_PRINT("enter\n");
161
162         mon_if = ndev_to_monif(ndev);
163         if (mon_if == NULL || mon_if->real_ndev == NULL) {
164                 MON_PRINT(" cannot find matched net dev, skip the packet\n");
165                 goto fail;
166         }
167
168         if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
169                 goto fail;
170
171         rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
172         if (unlikely(rtap_hdr->it_version))
173                 goto fail;
174
175         rtap_len = ieee80211_get_radiotap_len(skb->data);
176         if (unlikely(skb->len < rtap_len))
177                 goto fail;
178
179         MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
180
181         /* Skip the ratio tap header */
182         skb_pull(skb, rtap_len);
183
184         dot11_hdr = (struct ieee80211_hdr *)skb->data;
185         frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
186         /* Check if the QoS bit is set */
187         if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
188                 /* Check if this ia a Wireless Distribution System (WDS) frame
189                  * which has 4 MAC addresses
190                  */
191                 if (dot11_hdr->frame_control & 0x0080)
192                         qos_len = 2;
193                 if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
194                         dot11_hdr_len += 6;
195
196                 memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
197                 memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
198
199                 /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
200                  * for two MAC addresses
201                  */
202                 skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
203                 pdata = (unsigned char*)skb->data;
204                 memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
205                 memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
206
207                 MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
208
209                 /* Use the real net device to transmit the packet */
210                 ret = dhd_start_xmit(skb, mon_if->real_ndev);
211
212                 return ret;
213         }
214 fail:
215         dev_kfree_skb(skb);
216         return 0;
217 }
218
219 static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
220 {
221         monitor_interface* mon_if;
222
223         mon_if = ndev_to_monif(ndev);
224         if (mon_if == NULL || mon_if->real_ndev == NULL) {
225                 MON_PRINT(" cannot find matched net dev, skip the packet\n");
226         }
227
228         MON_PRINT("enter, if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
229 }
230
231 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
232 {
233         int ret = 0;
234         monitor_interface* mon_if;
235
236         mon_if = ndev_to_monif(ndev);
237         if (mon_if == NULL || mon_if->real_ndev == NULL) {
238                 MON_PRINT(" cannot find matched net dev, skip the packet\n");
239         }
240
241         MON_PRINT("enter, if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
242         return ret;
243 }
244
245 /**
246  * Global function definitions (declared in dhd_linux_mon.h)
247  */
248
249 int dhd_add_monitor(char *name, struct net_device **new_ndev)
250 {
251         int i;
252         int idx = -1;
253         int ret = 0;
254         struct net_device* ndev = NULL;
255         dhd_linux_monitor_t **dhd_mon;
256
257         mutex_lock(&g_monitor.lock);
258
259         MON_TRACE("enter, if name: %s\n", name);
260         if (!name || !new_ndev) {
261                 MON_PRINT("invalid parameters\n");
262                 ret = -EINVAL;
263                 goto out;
264         }
265
266         /*
267          * Find a vacancy
268          */
269         for (i = 0; i < DHD_MAX_IFS; i++)
270                 if (g_monitor.mon_if[i].mon_ndev == NULL) {
271                         idx = i;
272                         break;
273                 }
274         if (idx == -1) {
275                 MON_PRINT("exceeds maximum interfaces\n");
276                 ret = -EFAULT;
277                 goto out;
278         }
279
280         ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
281         if (!ndev) {
282                 MON_PRINT("failed to allocate memory\n");
283                 ret = -ENOMEM;
284                 goto out;
285         }
286
287         ndev->type = ARPHRD_IEEE80211_RADIOTAP;
288         strncpy(ndev->name, name, IFNAMSIZ);
289         ndev->name[IFNAMSIZ - 1] = 0;
290         ndev->netdev_ops = &dhd_mon_if_ops;
291
292         ret = register_netdevice(ndev);
293         if (ret) {
294                 MON_PRINT(" register_netdevice failed (%d)\n", ret);
295                 goto out;
296         }
297
298         *new_ndev = ndev;
299         g_monitor.mon_if[idx].radiotap_enabled = TRUE;
300         g_monitor.mon_if[idx].mon_ndev = ndev;
301         g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
302         dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
303         *dhd_mon = &g_monitor;
304         g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
305         MON_PRINT("net device returned: 0x%p\n", ndev);
306         MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
307
308 out:
309         if (ret && ndev)
310                 free_netdev(ndev);
311
312         mutex_unlock(&g_monitor.lock);
313         return ret;
314
315 }
316
317 int dhd_del_monitor(struct net_device *ndev)
318 {
319         int i;
320         bool rollback_lock = false;
321         if (!ndev)
322                 return -EINVAL;
323         mutex_lock(&g_monitor.lock);
324         for (i = 0; i < DHD_MAX_IFS; i++) {
325                 if (g_monitor.mon_if[i].mon_ndev == ndev ||
326                         g_monitor.mon_if[i].real_ndev == ndev) {
327                         g_monitor.mon_if[i].real_ndev = NULL;
328                         if (rtnl_is_locked()) {
329                                 rtnl_unlock();
330                                 rollback_lock = true;
331                         }
332                         unregister_netdev(g_monitor.mon_if[i].mon_ndev);
333                         free_netdev(g_monitor.mon_if[i].mon_ndev);
334                         g_monitor.mon_if[i].mon_ndev = NULL;
335                         g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
336                         break;
337                 }
338         }
339         if (rollback_lock) {
340                 rtnl_lock();
341                 rollback_lock = false;
342         }
343
344         if (g_monitor.monitor_state !=
345         MONITOR_STATE_INTERFACE_DELETED)
346                 MON_PRINT("interface not found in monitor IF array, is this a monitor IF? 0x%p\n",
347                         ndev);
348         mutex_unlock(&g_monitor.lock);
349
350         return 0;
351 }
352
353 int dhd_monitor_init(void *dhd_pub)
354 {
355         if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
356                 g_monitor.dhd_pub = dhd_pub;
357                 mutex_init(&g_monitor.lock);
358                 g_monitor.monitor_state = MONITOR_STATE_INIT;
359         }
360         return 0;
361 }
362
363 int dhd_monitor_uninit(void)
364 {
365         int i;
366         struct net_device *ndev;
367         bool rollback_lock = false;
368         mutex_lock(&g_monitor.lock);
369         if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
370                 for (i = 0; i < DHD_MAX_IFS; i++) {
371                         ndev = g_monitor.mon_if[i].mon_ndev;
372                         if (ndev) {
373                                 if (rtnl_is_locked()) {
374                                         rtnl_unlock();
375                                         rollback_lock = true;
376                                 }
377                                 unregister_netdev(ndev);
378                                 free_netdev(ndev);
379                                 g_monitor.mon_if[i].real_ndev = NULL;
380                                 g_monitor.mon_if[i].mon_ndev = NULL;
381                                 if (rollback_lock) {
382                                         rtnl_lock();
383                                         rollback_lock = false;
384                                 }
385                         }
386                 }
387                 g_monitor.monitor_state = MONITOR_STATE_DEINIT;
388         }
389         mutex_unlock(&g_monitor.lock);
390         return 0;
391 }