1
2
3
4
5
6
7#include "cfg80211.h"
8
9struct wilc_wfi_radiotap_hdr {
10 struct ieee80211_radiotap_header hdr;
11 u8 rate;
12} __packed;
13
14struct wilc_wfi_radiotap_cb_hdr {
15 struct ieee80211_radiotap_header hdr;
16 u8 rate;
17 u8 dump;
18 u16 tx_flags;
19} __packed;
20
21#define TX_RADIOTAP_PRESENT ((1 << IEEE80211_RADIOTAP_RATE) | \
22 (1 << IEEE80211_RADIOTAP_TX_FLAGS))
23
24void wilc_wfi_monitor_rx(struct net_device *mon_dev, u8 *buff, u32 size)
25{
26 u32 header, pkt_offset;
27 struct sk_buff *skb = NULL;
28 struct wilc_wfi_radiotap_hdr *hdr;
29 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
30
31 if (!mon_dev)
32 return;
33
34 if (!netif_running(mon_dev))
35 return;
36
37
38 header = get_unaligned_le32(buff - HOST_HDR_OFFSET);
39
40
41
42
43 pkt_offset = FIELD_GET(WILC_PKT_HDR_OFFSET_FIELD, header);
44
45 if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
46
47
48 skb = dev_alloc_skb(size + sizeof(*cb_hdr));
49 if (!skb)
50 return;
51
52 skb_put_data(skb, buff, size);
53
54 cb_hdr = skb_push(skb, sizeof(*cb_hdr));
55 memset(cb_hdr, 0, sizeof(*cb_hdr));
56
57 cb_hdr->hdr.it_version = 0;
58
59 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
60
61 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
62
63 cb_hdr->rate = 5;
64
65 if (pkt_offset & IS_MGMT_STATUS_SUCCES) {
66
67 cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
68 } else {
69 cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
70 }
71
72 } else {
73 skb = dev_alloc_skb(size + sizeof(*hdr));
74
75 if (!skb)
76 return;
77
78 skb_put_data(skb, buff, size);
79 hdr = skb_push(skb, sizeof(*hdr));
80 memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
81 hdr->hdr.it_version = 0;
82 hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
83 hdr->hdr.it_present = cpu_to_le32
84 (1 << IEEE80211_RADIOTAP_RATE);
85 hdr->rate = 5;
86 }
87
88 skb->dev = mon_dev;
89 skb_reset_mac_header(skb);
90 skb->ip_summed = CHECKSUM_UNNECESSARY;
91 skb->pkt_type = PACKET_OTHERHOST;
92 skb->protocol = htons(ETH_P_802_2);
93 memset(skb->cb, 0, sizeof(skb->cb));
94
95 netif_rx(skb);
96}
97
98struct tx_complete_mon_data {
99 int size;
100 void *buff;
101};
102
103static void mgmt_tx_complete(void *priv, int status)
104{
105 struct tx_complete_mon_data *pv_data = priv;
106
107
108
109
110 kfree(pv_data->buff);
111
112 kfree(pv_data);
113}
114
115static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
116{
117 struct tx_complete_mon_data *mgmt_tx = NULL;
118
119 if (!dev)
120 return -EFAULT;
121
122 netif_stop_queue(dev);
123 mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC);
124 if (!mgmt_tx)
125 return -ENOMEM;
126
127 mgmt_tx->buff = kmemdup(buf, len, GFP_ATOMIC);
128 if (!mgmt_tx->buff) {
129 kfree(mgmt_tx);
130 return -ENOMEM;
131 }
132
133 mgmt_tx->size = len;
134
135 wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
136 mgmt_tx_complete);
137
138 netif_wake_queue(dev);
139 return 0;
140}
141
142static netdev_tx_t wilc_wfi_mon_xmit(struct sk_buff *skb,
143 struct net_device *dev)
144{
145 u32 rtap_len, ret = 0;
146 struct wilc_wfi_mon_priv *mon_priv;
147 struct sk_buff *skb2;
148 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
149 u8 srcadd[ETH_ALEN];
150 u8 bssid[ETH_ALEN];
151
152 mon_priv = netdev_priv(dev);
153 if (!mon_priv)
154 return -EFAULT;
155
156 rtap_len = ieee80211_get_radiotap_len(skb->data);
157 if (skb->len < rtap_len)
158 return -1;
159
160 skb_pull(skb, rtap_len);
161
162 if (skb->data[0] == 0xc0 && is_broadcast_ether_addr(&skb->data[4])) {
163 skb2 = dev_alloc_skb(skb->len + sizeof(*cb_hdr));
164 if (!skb2)
165 return -ENOMEM;
166
167 skb_put_data(skb2, skb->data, skb->len);
168
169 cb_hdr = skb_push(skb2, sizeof(*cb_hdr));
170 memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
171
172 cb_hdr->hdr.it_version = 0;
173
174 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
175
176 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
177
178 cb_hdr->rate = 5;
179 cb_hdr->tx_flags = 0x0004;
180
181 skb2->dev = dev;
182 skb_reset_mac_header(skb2);
183 skb2->ip_summed = CHECKSUM_UNNECESSARY;
184 skb2->pkt_type = PACKET_OTHERHOST;
185 skb2->protocol = htons(ETH_P_802_2);
186 memset(skb2->cb, 0, sizeof(skb2->cb));
187
188 netif_rx(skb2);
189
190 return 0;
191 }
192 skb->dev = mon_priv->real_ndev;
193
194 ether_addr_copy(srcadd, &skb->data[10]);
195 ether_addr_copy(bssid, &skb->data[16]);
196
197
198
199
200 if (!(memcmp(srcadd, bssid, 6))) {
201 ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
202 if (ret)
203 netdev_err(dev, "fail to mgmt tx\n");
204 dev_kfree_skb(skb);
205 } else {
206 ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
207 }
208
209 return ret;
210}
211
212static const struct net_device_ops wilc_wfi_netdev_ops = {
213 .ndo_start_xmit = wilc_wfi_mon_xmit,
214
215};
216
217struct net_device *wilc_wfi_init_mon_interface(struct wilc *wl,
218 const char *name,
219 struct net_device *real_dev)
220{
221 struct wilc_wfi_mon_priv *priv;
222
223
224 if (wl->monitor_dev)
225 return wl->monitor_dev;
226
227 wl->monitor_dev = alloc_etherdev(sizeof(struct wilc_wfi_mon_priv));
228 if (!wl->monitor_dev)
229 return NULL;
230
231 wl->monitor_dev->type = ARPHRD_IEEE80211_RADIOTAP;
232 strlcpy(wl->monitor_dev->name, name, IFNAMSIZ);
233 wl->monitor_dev->netdev_ops = &wilc_wfi_netdev_ops;
234 wl->monitor_dev->needs_free_netdev = true;
235
236 if (cfg80211_register_netdevice(wl->monitor_dev)) {
237 netdev_err(real_dev, "register_netdevice failed\n");
238 free_netdev(wl->monitor_dev);
239 return NULL;
240 }
241 priv = netdev_priv(wl->monitor_dev);
242
243 priv->real_ndev = real_dev;
244
245 return wl->monitor_dev;
246}
247
248void wilc_wfi_deinit_mon_interface(struct wilc *wl, bool rtnl_locked)
249{
250 if (!wl->monitor_dev)
251 return;
252
253 if (rtnl_locked)
254 cfg80211_unregister_netdevice(wl->monitor_dev);
255 else
256 unregister_netdev(wl->monitor_dev);
257 wl->monitor_dev = NULL;
258}
259
