1 /*
2 * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
3 *
4 * Copyright (C) 2020, Broadcom.
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 *
21 * <<Broadcom-WL-IPTag/Dual:>>
22 */
23
24 #include <osl.h>
25 #include <linux/string.h>
26 #include <linux/module.h>
27 #include <linux/netdevice.h>
28 #include <linux/etherdevice.h>
29 #include <linux/if_arp.h>
30 #include <linux/ieee80211.h>
31 #include <linux/rtnetlink.h>
32 #include <net/ieee80211_radiotap.h>
33
34 #if defined(BCMDONGLEHOST)
35 #include <wlioctl.h>
36 #include <bcmutils.h>
37 #include <dhd_dbg.h>
38 #include <dngl_stats.h>
39 #include <dhd.h>
40 #endif /* defined(BCMDONGLEHOST) */
41 #if defined(__linux__)
42 #include <bcmstdlib_s.h>
43 #endif /* defined(__linux__) */
44
45 typedef enum monitor_states
46 {
47 MONITOR_STATE_DEINIT = 0x0,
48 MONITOR_STATE_INIT = 0x1,
49 MONITOR_STATE_INTERFACE_ADDED = 0x2,
50 MONITOR_STATE_INTERFACE_DELETED = 0x4
51 } monitor_states_t;
52 /*
53 * Some external functions, TODO: move them to dhd_linux.h
54 */
55 int dhd_add_monitor(const char *name, 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
67 #define MON_PRINT(format, ...) printf("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
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 #if defined(BCMDONGLEHOST)
117 int i;
118 int len = 0;
119 int last_name_len = 0;
120 struct net_device *ndev;
121
122 /* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
123 * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
124 * iface would be mon-p2p0-0.
125 */
126 for (i = 0; i < DHD_MAX_IFS; i++) {
127 ndev = dhd_idx2net(g_monitor.dhd_pub, i);
128
129 /* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
130 * it matches, then this netdev is the corresponding real_netdev.
131 */
132 if (ndev && strstr(ndev->name, "p2p-p2p0")) {
133 len = strlen("p2p");
134 } else {
135 /* if p2p- is not present, then the IFNAMSIZ have reached and name
136 * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
137 */
138 len = 0;
139 }
140 if (ndev && strstr(name, (ndev->name + len))) {
141 if (strlen(ndev->name) > last_name_len) {
142 ndev_found = ndev;
143 last_name_len = strlen(ndev->name);
144 }
145 }
146 }
147 #endif /* defined(BCMDONGLEHOST) */
148
149 return ndev_found;
150 }
151
ndev_to_monif(struct net_device * ndev)152 static monitor_interface* ndev_to_monif(struct net_device *ndev)
153 {
154 int i;
155
156 for (i = 0; i < DHD_MAX_IFS; i++) {
157 if (g_monitor.mon_if[i].mon_ndev == ndev)
158 return &g_monitor.mon_if[i];
159 }
160
161 return NULL;
162 }
163
dhd_mon_if_open(struct net_device * ndev)164 static int dhd_mon_if_open(struct net_device *ndev)
165 {
166 int ret = 0;
167
168 MON_PRINT("enter\n");
169 return ret;
170 }
171
dhd_mon_if_stop(struct net_device * ndev)172 static int dhd_mon_if_stop(struct net_device *ndev)
173 {
174 int ret = 0;
175
176 MON_PRINT("enter\n");
177 return ret;
178 }
179
dhd_mon_if_subif_start_xmit(struct sk_buff * skb,struct net_device * ndev)180 static netdev_tx_t dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
181 {
182 int ret = 0;
183 int rtap_len;
184 int qos_len = 0;
185 int dot11_hdr_len = 24;
186 int snap_len = 6;
187 unsigned char *pdata;
188 unsigned short frame_ctl;
189 unsigned char src_mac_addr[6];
190 unsigned char dst_mac_addr[6];
191 struct ieee80211_hdr *dot11_hdr;
192 struct ieee80211_radiotap_header *rtap_hdr;
193 monitor_interface* mon_if;
194
195 MON_PRINT("enter\n");
196
197 mon_if = ndev_to_monif(ndev);
198 if (mon_if == NULL || mon_if->real_ndev == NULL) {
199 MON_PRINT(" cannot find matched net dev, skip the packet\n");
200 goto fail;
201 }
202
203 if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
204 goto fail;
205
206 rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
207 if (unlikely(rtap_hdr->it_version))
208 goto fail;
209
210 rtap_len = ieee80211_get_radiotap_len(skb->data);
211 if (unlikely(skb->len < rtap_len))
212 goto fail;
213
214 MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
215
216 /* Skip the ratio tap header */
217 skb_pull(skb, rtap_len);
218
219 dot11_hdr = (struct ieee80211_hdr *)skb->data;
220 frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
221 /* Check if the QoS bit is set */
222 if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
223 /* Check if this ia a Wireless Distribution System (WDS) frame
224 * which has 4 MAC addresses
225 */
226 if (dot11_hdr->frame_control & 0x0080)
227 qos_len = 2;
228 if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
229 dot11_hdr_len += 6;
230
231 eacopy(dot11_hdr->addr1, dst_mac_addr);
232 eacopy(dot11_hdr->addr2, src_mac_addr);
233
234 /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
235 * for two MAC addresses
236 */
237 skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
238 pdata = (unsigned char*)skb->data;
239 (void)memcpy_s(pdata, sizeof(dst_mac_addr), dst_mac_addr, sizeof(dst_mac_addr));
240 (void)memcpy_s(pdata + sizeof(dst_mac_addr), sizeof(src_mac_addr), src_mac_addr,
241 sizeof(src_mac_addr));
242 PKTSETPRIO(skb, 0);
243
244 MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
245
246 /* Use the real net device to transmit the packet */
247 #if defined(BCMDONGLEHOST)
248 ret = dhd_start_xmit(skb, mon_if->real_ndev);
249 #endif /* defined(BCMDONGLEHOST) */
250
251 return ret;
252 }
253 fail:
254 dev_kfree_skb(skb);
255 return 0;
256 }
257
dhd_mon_if_set_multicast_list(struct net_device * ndev)258 static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
259 {
260 monitor_interface* mon_if;
261
262 mon_if = ndev_to_monif(ndev);
263 if (mon_if == NULL || mon_if->real_ndev == NULL) {
264 MON_PRINT(" cannot find matched net dev, skip the packet\n");
265 } else {
266 MON_PRINT("enter, if name: %s, matched if name %s\n",
267 ndev->name, mon_if->real_ndev->name);
268 }
269 }
270
dhd_mon_if_change_mac(struct net_device * ndev,void * addr)271 static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
272 {
273 int ret = 0;
274 monitor_interface* mon_if;
275
276 mon_if = ndev_to_monif(ndev);
277 if (mon_if == NULL || mon_if->real_ndev == NULL) {
278 MON_PRINT(" cannot find matched net dev, skip the packet\n");
279 } else {
280 MON_PRINT("enter, if name: %s, matched if name %s\n",
281 ndev->name, mon_if->real_ndev->name);
282 }
283 return ret;
284 }
285
286 /**
287 * Global function definitions (declared in dhd_linux_mon.h)
288 */
289
dhd_add_monitor(const char * name,struct net_device ** new_ndev)290 int dhd_add_monitor(const char *name, struct net_device **new_ndev)
291 {
292 int i;
293 int idx = -1;
294 int ret = 0;
295 struct net_device* ndev = NULL;
296 dhd_linux_monitor_t **dhd_mon;
297
298 mutex_lock(&g_monitor.lock);
299
300 MON_TRACE("enter, if name: %s\n", name);
301 if (!name || !new_ndev) {
302 MON_PRINT("invalid parameters\n");
303 ret = -EINVAL;
304 goto out;
305 }
306
307 /*
308 * Find a vacancy
309 */
310 for (i = 0; i < DHD_MAX_IFS; i++)
311 if (g_monitor.mon_if[i].mon_ndev == NULL) {
312 idx = i;
313 break;
314 }
315 if (idx == -1) {
316 MON_PRINT("exceeds maximum interfaces\n");
317 ret = -EFAULT;
318 goto out;
319 }
320
321 ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
322 if (!ndev) {
323 MON_PRINT("failed to allocate memory\n");
324 ret = -ENOMEM;
325 goto out;
326 }
327
328 ndev->type = ARPHRD_IEEE80211_RADIOTAP;
329 strlcpy(ndev->name, name, sizeof(ndev->name));
330 ndev->netdev_ops = &dhd_mon_if_ops;
331
332 ret = register_netdevice(ndev);
333 if (ret) {
334 MON_PRINT(" register_netdevice failed (%d)\n", ret);
335 goto out;
336 }
337
338 *new_ndev = ndev;
339 g_monitor.mon_if[idx].radiotap_enabled = TRUE;
340 g_monitor.mon_if[idx].mon_ndev = ndev;
341 g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
342 dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
343 *dhd_mon = &g_monitor;
344 g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
345 MON_PRINT("net device returned: 0x%p\n", ndev);
346 MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
347
348 out:
349 if (ret && ndev)
350 free_netdev(ndev);
351
352 mutex_unlock(&g_monitor.lock);
353 return ret;
354
355 }
356
dhd_del_monitor(struct net_device * ndev)357 int dhd_del_monitor(struct net_device *ndev)
358 {
359 int i;
360 if (!ndev)
361 return -EINVAL;
362 mutex_lock(&g_monitor.lock);
363 for (i = 0; i < DHD_MAX_IFS; i++) {
364 if (g_monitor.mon_if[i].mon_ndev == ndev ||
365 g_monitor.mon_if[i].real_ndev == ndev) {
366
367 g_monitor.mon_if[i].real_ndev = NULL;
368 unregister_netdevice(g_monitor.mon_if[i].mon_ndev);
369 free_netdev(g_monitor.mon_if[i].mon_ndev);
370 g_monitor.mon_if[i].mon_ndev = NULL;
371 g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
372 break;
373 }
374 }
375
376 if (g_monitor.monitor_state != MONITOR_STATE_INTERFACE_DELETED)
377 MON_PRINT("IF not found in monitor array, is this a monitor IF? 0x%p\n", ndev);
378 mutex_unlock(&g_monitor.lock);
379
380 return 0;
381 }
382
dhd_monitor_init(void * dhd_pub)383 int dhd_monitor_init(void *dhd_pub)
384 {
385 if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
386 g_monitor.dhd_pub = dhd_pub;
387 mutex_init(&g_monitor.lock);
388 g_monitor.monitor_state = MONITOR_STATE_INIT;
389 }
390 return 0;
391 }
392
dhd_monitor_uninit(void)393 int dhd_monitor_uninit(void)
394 {
395 int i;
396 struct net_device *ndev;
397 if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
398 mutex_lock(&g_monitor.lock);
399 for (i = 0; i < DHD_MAX_IFS; i++) {
400 ndev = g_monitor.mon_if[i].mon_ndev;
401 if (ndev) {
402 unregister_netdevice(ndev);
403 free_netdev(ndev);
404 g_monitor.mon_if[i].real_ndev = NULL;
405 g_monitor.mon_if[i].mon_ndev = NULL;
406 }
407 }
408 g_monitor.monitor_state = MONITOR_STATE_DEINIT;
409 mutex_unlock(&g_monitor.lock);
410 }
411 return 0;
412 }
413