1 /*
2 * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
3 *
4 * Copyright (C) 1999-2013, 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
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 int dhd_add_monitor(char *name, struct net_device **new_ndev);
51 extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
52 int dhd_del_monitor(struct net_device *ndev);
53 int dhd_monitor_init(void *dhd_pub);
54 int 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
65 typedef 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
71 typedef 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
78 static dhd_linux_monitor_t g_monitor;
79
80 static struct net_device* lookup_real_netdev(char *name);
81 static monitor_interface* ndev_to_monif(struct net_device *ndev);
82 static int dhd_mon_if_open(struct net_device *ndev);
83 static int dhd_mon_if_stop(struct net_device *ndev);
84 static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
85 static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
86 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
87
88 static 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 */
lookup_real_netdev(char * name)107 static 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
ndev_to_monif(struct net_device * ndev)145 static 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
dhd_mon_if_open(struct net_device * ndev)157 static 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
dhd_mon_if_stop(struct net_device * ndev)165 static 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
dhd_mon_if_subif_start_xmit(struct sk_buff * skb,struct net_device * ndev)173 static 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 }
243 fail:
244 dev_kfree_skb(skb);
245 return 0;
246 }
247
dhd_mon_if_set_multicast_list(struct net_device * ndev)248 static 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
dhd_mon_if_change_mac(struct net_device * ndev,void * addr)261 static 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
dhd_add_monitor(char * name,struct net_device ** new_ndev)280 int 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
339 out:
340 if (ret && ndev)
341 free_netdev(ndev);
342
343 mutex_unlock(&g_monitor.lock);
344 return ret;
345
346 }
347
dhd_del_monitor(struct net_device * ndev)348 int 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
dhd_monitor_init(void * dhd_pub)384 int 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
dhd_monitor_uninit(void)394 int 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