1 /* SPDX-License-Identifier: GPL-2.0 */
2 /*
3 * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
4 *
5 * Copyright (C) 1999-2019, Broadcom.
6 *
7 * Unless you and Broadcom execute a separate written software license
8 * agreement governing use of this software, this software is licensed to you
9 * under the terms of the GNU General Public License version 2 (the "GPL"),
10 * available at http://www.broadcom.com/licenses/GPLv2.php, with the
11 * following added to such license:
12 *
13 * As a special exception, the copyright holders of this software give you
14 * permission to link this software with independent modules, and to copy and
15 * distribute the resulting executable under terms of your choice, provided that
16 * you also meet, for each linked independent module, the terms and conditions of
17 * the license of that module. An independent module is a module which is not
18 * derived from this software. The special exception does not apply to any
19 * modifications of the software.
20 *
21 * Notwithstanding the above, under no circumstances may you combine this
22 * software in any way with any other Broadcom software provided under a license
23 * other than the GPL, without Broadcom's express prior written consent.
24 *
25 *
26 * <<Broadcom-WL-IPTag/Open:>>
27 *
28 * $Id: wl_linux_mon.c 576195 2015-08-01 18:21:54Z $
29 */
30
31 #include <osl.h>
32 #include <linux/string.h>
33 #include <linux/module.h>
34 #include <linux/netdevice.h>
35 #include <linux/etherdevice.h>
36 #include <linux/if_arp.h>
37 #include <linux/ieee80211.h>
38 #include <linux/rtnetlink.h>
39 #include <net/ieee80211_radiotap.h>
40
41 #include <wlioctl.h>
42 #include <bcmutils.h>
43 #include <dhd_dbg.h>
44 #include <dngl_stats.h>
45 #include <dhd.h>
46
47 typedef enum monitor_states
48 {
49 MONITOR_STATE_DEINIT = 0x0,
50 MONITOR_STATE_INIT = 0x1,
51 MONITOR_STATE_INTERFACE_ADDED = 0x2,
52 MONITOR_STATE_INTERFACE_DELETED = 0x4
53 } monitor_states_t;
54 int dhd_add_monitor(const char *name, struct net_device **new_ndev);
55 int magiclink_add_p2p(const char *name, struct ether_addr *p2p_dev_addr, struct net_device **new_ndev);
56 extern netdev_tx_t dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
57 int dhd_del_monitor(struct net_device *ndev);
58 int dhd_monitor_init(void *dhd_pub);
59 int dhd_monitor_uninit(void);
60
61 /**
62 * Local declarations and defintions (not exposed)
63 */
64 #ifndef DHD_MAX_IFS
65 #define DHD_MAX_IFS 16
66 #endif // endif
67 #define MON_PRINT(format, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
68 #define MON_TRACE MON_PRINT
69
70 typedef struct monitor_interface {
71 int radiotap_enabled;
72 struct net_device* real_ndev; /* The real interface that the monitor is on */
73 struct net_device* mon_ndev;
74 } monitor_interface;
75
76 typedef struct dhd_linux_monitor {
77 void *dhd_pub;
78 monitor_states_t monitor_state;
79 monitor_interface mon_if[DHD_MAX_IFS];
80 struct mutex lock; /* lock to protect mon_if */
81 } dhd_linux_monitor_t;
82
83 static dhd_linux_monitor_t g_monitor;
84
85 static struct net_device* lookup_real_netdev(const char *name);
86 static monitor_interface* ndev_to_monif(struct net_device *ndev);
87 static int dhd_mon_if_open(struct net_device *ndev);
88 static int dhd_mon_if_stop(struct net_device *ndev);
89 static netdev_tx_t dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
90 static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
91 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
92
93 static const struct net_device_ops dhd_mon_if_ops = {
94 .ndo_open = dhd_mon_if_open,
95 .ndo_stop = dhd_mon_if_stop,
96 .ndo_start_xmit = dhd_mon_if_subif_start_xmit,
97 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0))
98 .ndo_set_rx_mode = dhd_mon_if_set_multicast_list,
99 #else
100 .ndo_set_multicast_list = dhd_mon_if_set_multicast_list,
101 #endif // endif
102 .ndo_set_mac_address = dhd_mon_if_change_mac,
103 };
104
105 /**
106 * Local static function defintions
107 */
108
109 /* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
110 * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
111 */
lookup_real_netdev(const char * name)112 static struct net_device* lookup_real_netdev(const char *name)
113 {
114 struct net_device *ndev_found = NULL;
115
116 int i;
117 int len = 0;
118 int last_name_len = 0;
119 struct net_device *ndev;
120
121 /* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
122 * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
123 * iface would be mon-p2p0-0.
124 */
125 for (i = 0; i < DHD_MAX_IFS; i++) {
126 ndev = dhd_idx2net(g_monitor.dhd_pub, i);
127
128 /* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
129 * it matches, then this netdev is the corresponding real_netdev.
130 */
131 if (ndev && strstr(ndev->name, "p2p-p2p0")) {
132 len = strlen("p2p");
133 } else {
134 /* if p2p- is not present, then the IFNAMSIZ have reached and name
135 * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
136 */
137 len = 0;
138 }
139 if (ndev && strstr(name, (ndev->name + len))) {
140 if (strlen(ndev->name) > last_name_len) {
141 ndev_found = ndev;
142 last_name_len = strlen(ndev->name);
143 }
144 }
145 }
146
147 return ndev_found;
148 }
149
ndev_to_monif(struct net_device * ndev)150 static monitor_interface* ndev_to_monif(struct net_device *ndev)
151 {
152 int i;
153
154 for (i = 0; i < DHD_MAX_IFS; i++) {
155 if (g_monitor.mon_if[i].mon_ndev == ndev)
156 return &g_monitor.mon_if[i];
157 }
158
159 return NULL;
160 }
161
dhd_mon_if_open(struct net_device * ndev)162 static int dhd_mon_if_open(struct net_device *ndev)
163 {
164 int ret = 0;
165
166 MON_PRINT("enter\n");
167 return ret;
168 }
169
dhd_mon_if_stop(struct net_device * ndev)170 static int dhd_mon_if_stop(struct net_device *ndev)
171 {
172 int ret = 0;
173
174 MON_PRINT("enter\n");
175 return ret;
176 }
177
dhd_mon_if_subif_start_xmit(struct sk_buff * skb,struct net_device * ndev)178 static netdev_tx_t dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
179 {
180 int ret = 0;
181 int rtap_len;
182 int qos_len = 0;
183 int dot11_hdr_len = 24;
184 int snap_len = 6;
185 unsigned char *pdata;
186 unsigned short frame_ctl;
187 unsigned char src_mac_addr[6];
188 unsigned char dst_mac_addr[6];
189 struct ieee80211_hdr *dot11_hdr;
190 struct ieee80211_radiotap_header *rtap_hdr;
191 monitor_interface* mon_if;
192
193 MON_PRINT("enter\n");
194
195 mon_if = ndev_to_monif(ndev);
196 if (mon_if == NULL || mon_if->real_ndev == NULL) {
197 MON_PRINT(" cannot find matched net dev, skip the packet\n");
198 goto fail;
199 }
200
201 if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
202 goto fail;
203
204 rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
205 if (unlikely(rtap_hdr->it_version))
206 goto fail;
207
208 rtap_len = ieee80211_get_radiotap_len(skb->data);
209 if (unlikely(skb->len < rtap_len))
210 goto fail;
211
212 MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
213
214 /* Skip the ratio tap header */
215 skb_pull(skb, rtap_len);
216
217 dot11_hdr = (struct ieee80211_hdr *)skb->data;
218 frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
219 /* Check if the QoS bit is set */
220 if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
221 /* Check if this ia a Wireless Distribution System (WDS) frame
222 * which has 4 MAC addresses
223 */
224 if (dot11_hdr->frame_control & 0x0080)
225 qos_len = 2;
226 if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
227 dot11_hdr_len += 6;
228
229 memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
230 memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
231
232 /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
233 * for two MAC addresses
234 */
235 skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
236 pdata = (unsigned char*)skb->data;
237 memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
238 memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
239 PKTSETPRIO(skb, 0);
240
241 MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
242
243 /* Use the real net device to transmit the packet */
244 ret = dhd_start_xmit(skb, mon_if->real_ndev);
245
246 return ret;
247 }
248 fail:
249 dev_kfree_skb(skb);
250 return 0;
251 }
252
dhd_mon_if_set_multicast_list(struct net_device * ndev)253 static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
254 {
255 monitor_interface* mon_if;
256
257 mon_if = ndev_to_monif(ndev);
258 if (mon_if == NULL || mon_if->real_ndev == NULL) {
259 MON_PRINT(" cannot find matched net dev, skip the packet\n");
260 } else {
261 MON_PRINT("enter, if name: %s, matched if name %s\n",
262 ndev->name, mon_if->real_ndev->name);
263 }
264 }
265
dhd_mon_if_change_mac(struct net_device * ndev,void * addr)266 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
267 {
268 int ret = 0;
269 monitor_interface* mon_if;
270
271 mon_if = ndev_to_monif(ndev);
272 if (mon_if == NULL || mon_if->real_ndev == NULL) {
273 MON_PRINT(" cannot find matched net dev, skip the packet\n");
274 } else {
275 MON_PRINT("enter, if name: %s, matched if name %s\n",
276 ndev->name, mon_if->real_ndev->name);
277 }
278 return ret;
279 }
280
281 /**
282 * Global function definitions (declared in dhd_linux_mon.h)
283 */
284
dhd_add_monitor(const char * name,struct net_device ** new_ndev)285 int dhd_add_monitor(const char *name, struct net_device **new_ndev)
286 {
287 int i;
288 int idx = -1;
289 int ret = 0;
290 struct net_device* ndev = NULL;
291 dhd_linux_monitor_t **dhd_mon;
292
293 mutex_lock(&g_monitor.lock);
294
295 MON_TRACE("enter, if name: %s\n", name);
296 if (!name || !new_ndev) {
297 MON_PRINT("invalid parameters\n");
298 ret = -EINVAL;
299 goto out;
300 }
301
302 /*
303 * Find a vacancy
304 */
305 for (i = 0; i < DHD_MAX_IFS; i++)
306 if (g_monitor.mon_if[i].mon_ndev == NULL) {
307 idx = i;
308 break;
309 }
310 if (idx == -1) {
311 MON_PRINT("exceeds maximum interfaces\n");
312 ret = -EFAULT;
313 goto out;
314 }
315
316 ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
317 if (!ndev) {
318 MON_PRINT("failed to allocate memory\n");
319 ret = -ENOMEM;
320 goto out;
321 }
322
323 ndev->type = ARPHRD_IEEE80211_RADIOTAP;
324 strncpy(ndev->name, name, IFNAMSIZ);
325 ndev->name[IFNAMSIZ - 1] = 0;
326 ndev->netdev_ops = &dhd_mon_if_ops;
327
328 ret = register_netdevice(ndev);
329 if (ret) {
330 MON_PRINT(" register_netdevice failed (%d)\n", ret);
331 goto out;
332 }
333
334 *new_ndev = ndev;
335 g_monitor.mon_if[idx].radiotap_enabled = TRUE;
336 g_monitor.mon_if[idx].mon_ndev = ndev;
337 g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
338 dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
339 *dhd_mon = &g_monitor;
340 g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
341 MON_PRINT("net device returned: 0x%p\n", ndev);
342 MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
343
344 out:
345 if (ret && ndev)
346 free_netdev(ndev);
347
348 mutex_unlock(&g_monitor.lock);
349 return ret;
350
351 }
352
magiclink_add_p2p(const char * name,struct ether_addr * p2p_dev_addr,struct net_device ** new_ndev)353 int magiclink_add_p2p(const char *name, struct ether_addr *p2p_dev_addr, struct net_device **new_ndev)
354 {
355 int i;
356 int idx = -1;
357 int ret = 0;
358 struct net_device* ndev = NULL;
359 dhd_linux_monitor_t **dhd_mon;
360 mutex_lock(&g_monitor.lock);
361
362 MON_TRACE("enter, if name: %s\n", name);
363 if (!name || !new_ndev) {
364 MON_PRINT("invalid parameters\n");
365 ret = -EINVAL;
366 goto out;
367 }
368
369 /*
370 * Find a vacancy
371 */
372 for (i = 0; i < DHD_MAX_IFS; i++)
373 if (g_monitor.mon_if[i].mon_ndev == NULL) {
374 idx = i;
375 break;
376 }
377 if (idx == -1) {
378 MON_PRINT("exceeds maximum interfaces\n");
379 ret = -EFAULT;
380 goto out;
381 }
382
383 ndev = alloc_etherdev(sizeof(struct net_device));
384 if (!ndev) {
385 MON_PRINT("failed to allocate memory\n");
386 ret = -ENOMEM;
387 goto out;
388 }
389
390 strncpy(ndev->name, name, IFNAMSIZ);
391 ndev->name[IFNAMSIZ - 1] = 0;
392 ndev->netdev_ops = &dhd_mon_if_ops;
393
394 memcpy(ndev->dev_addr, p2p_dev_addr, sizeof(struct ether_addr));
395 ret = register_netdevice(ndev);
396 if (ret) {
397 MON_PRINT(" register_netdevice failed (%d)\n", ret);
398 goto out;
399 }
400
401 *new_ndev = ndev;
402 g_monitor.mon_if[idx].radiotap_enabled = TRUE;
403 g_monitor.mon_if[idx].mon_ndev = ndev;
404 g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
405 dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
406 *dhd_mon = &g_monitor;
407 g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
408 MON_PRINT("net device returned: 0x%p\n", ndev);
409 MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
410 out:
411 if (ret && ndev)
412 free_netdev(ndev);
413
414 mutex_unlock(&g_monitor.lock);
415 return ret;
416
417 }
418
dhd_del_monitor(struct net_device * ndev)419 int dhd_del_monitor(struct net_device *ndev)
420 {
421 int i;
422 if (!ndev)
423 return -EINVAL;
424 mutex_lock(&g_monitor.lock);
425 for (i = 0; i < DHD_MAX_IFS; i++) {
426 if (g_monitor.mon_if[i].mon_ndev == ndev ||
427 g_monitor.mon_if[i].real_ndev == ndev) {
428
429 g_monitor.mon_if[i].real_ndev = NULL;
430 unregister_netdevice(g_monitor.mon_if[i].mon_ndev);
431 free_netdev(g_monitor.mon_if[i].mon_ndev);
432 g_monitor.mon_if[i].mon_ndev = NULL;
433 g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
434 break;
435 }
436 }
437
438 if (g_monitor.monitor_state != MONITOR_STATE_INTERFACE_DELETED)
439 MON_PRINT("IF not found in monitor array, is this a monitor IF? 0x%p\n", ndev);
440 mutex_unlock(&g_monitor.lock);
441
442 return 0;
443 }
444
dhd_monitor_init(void * dhd_pub)445 int dhd_monitor_init(void *dhd_pub)
446 {
447 if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
448 g_monitor.dhd_pub = dhd_pub;
449 mutex_init(&g_monitor.lock);
450 g_monitor.monitor_state = MONITOR_STATE_INIT;
451 }
452 return 0;
453 }
454
dhd_monitor_uninit(void)455 int dhd_monitor_uninit(void)
456 {
457 int i;
458 struct net_device *ndev;
459 if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
460 mutex_lock(&g_monitor.lock);
461 for (i = 0; i < DHD_MAX_IFS; i++) {
462 ndev = g_monitor.mon_if[i].mon_ndev;
463 if (ndev) {
464 unregister_netdevice(ndev);
465 free_netdev(ndev);
466 g_monitor.mon_if[i].real_ndev = NULL;
467 g_monitor.mon_if[i].mon_ndev = NULL;
468 }
469 }
470 g_monitor.monitor_state = MONITOR_STATE_DEINIT;
471 mutex_unlock(&g_monitor.lock);
472 }
473 return 0;
474 }
475