]>
Commit | Line | Data |
---|---|---|
b2441318 | 1 | // SPDX-License-Identifier: GPL-2.0 |
c5c77ba1 JK |
2 | /*! |
3 | * @file linux_mon.c | |
4 | * @brief File Operations OS wrapper functionality | |
5 | * @author mdaftedar | |
6 | * @sa wilc_wfi_netdevice.h | |
7 | * @date 01 MAR 2012 | |
8 | * @version 1.0 | |
9 | */ | |
c5c77ba1 | 10 | #include "wilc_wfi_cfgoperations.h" |
c5c77ba1 JK |
11 | #include "wilc_wlan_if.h" |
12 | #include "wilc_wlan.h" | |
9690df3f | 13 | |
c5c77ba1 JK |
14 | struct wilc_wfi_radiotap_hdr { |
15 | struct ieee80211_radiotap_header hdr; | |
16 | u8 rate; | |
d8c7d2b3 | 17 | } __packed; |
c5c77ba1 JK |
18 | |
19 | struct wilc_wfi_radiotap_cb_hdr { | |
20 | struct ieee80211_radiotap_header hdr; | |
21 | u8 rate; | |
22 | u8 dump; | |
23 | u16 tx_flags; | |
d8c7d2b3 | 24 | } __packed; |
c5c77ba1 | 25 | |
c5c77ba1 JK |
26 | static struct net_device *wilc_wfi_mon; /* global monitor netdev */ |
27 | ||
add0f9fe | 28 | static u8 srcadd[6]; |
1608c403 AB |
29 | static u8 bssid[6]; |
30 | static u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; | |
c5c77ba1 JK |
31 | /** |
32 | * @brief WILC_WFI_monitor_rx | |
33 | * @details | |
34 | * @param[in] | |
35 | * @return int : Return 0 on Success | |
36 | * @author mdaftedar | |
37 | * @date 12 JUL 2012 | |
38 | * @version 1.0 | |
39 | */ | |
40 | ||
41 | #define IEEE80211_RADIOTAP_F_TX_RTS 0x0004 /* used rts/cts handshake */ | |
42 | #define IEEE80211_RADIOTAP_F_TX_FAIL 0x0001 /* failed due to excessive*/ | |
43 | #define IS_MANAGMEMENT 0x100 | |
44 | #define IS_MANAGMEMENT_CALLBACK 0x080 | |
45 | #define IS_MGMT_STATUS_SUCCES 0x040 | |
46 | #define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff) | |
47 | ||
fbc2fe16 | 48 | void WILC_WFI_monitor_rx(u8 *buff, u32 size) |
c5c77ba1 | 49 | { |
fbc2fe16 | 50 | u32 header, pkt_offset; |
c5c77ba1 JK |
51 | struct sk_buff *skb = NULL; |
52 | struct wilc_wfi_radiotap_hdr *hdr; | |
53 | struct wilc_wfi_radiotap_cb_hdr *cb_hdr; | |
54 | ||
a1436579 | 55 | if (!wilc_wfi_mon) |
c5c77ba1 JK |
56 | return; |
57 | ||
ccff7f81 | 58 | if (!netif_running(wilc_wfi_mon)) |
c5c77ba1 | 59 | return; |
c5c77ba1 JK |
60 | |
61 | /* Get WILC header */ | |
62 | memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET); | |
1b7c69e8 AJ |
63 | /* |
64 | * The packet offset field contain info about what type of management | |
65 | * the frame we are dealing with and ack status | |
66 | */ | |
c5c77ba1 JK |
67 | pkt_offset = GET_PKT_OFFSET(header); |
68 | ||
69 | if (pkt_offset & IS_MANAGMEMENT_CALLBACK) { | |
c5c77ba1 JK |
70 | /* hostapd callback mgmt frame */ |
71 | ||
72 | skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
ccff7f81 | 73 | if (!skb) |
c5c77ba1 | 74 | return; |
c5c77ba1 | 75 | |
59ae1d12 | 76 | skb_put_data(skb, buff, size); |
c5c77ba1 | 77 | |
d58ff351 | 78 | cb_hdr = skb_push(skb, sizeof(*cb_hdr)); |
c5c77ba1 JK |
79 | memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr)); |
80 | ||
81 | cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ | |
82 | ||
83 | cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
84 | ||
85 | cb_hdr->hdr.it_present = cpu_to_le32( | |
86 | (1 << IEEE80211_RADIOTAP_RATE) | | |
87 | (1 << IEEE80211_RADIOTAP_TX_FLAGS)); | |
88 | ||
89 | cb_hdr->rate = 5; /* txrate->bitrate / 5; */ | |
90 | ||
91 | if (pkt_offset & IS_MGMT_STATUS_SUCCES) { | |
92 | /* success */ | |
93 | cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS; | |
94 | } else { | |
95 | cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL; | |
96 | } | |
97 | ||
98 | } else { | |
c5c77ba1 JK |
99 | skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr)); |
100 | ||
ccff7f81 | 101 | if (!skb) |
c5c77ba1 | 102 | return; |
c5c77ba1 | 103 | |
59ae1d12 | 104 | skb_put_data(skb, buff, size); |
d58ff351 | 105 | hdr = skb_push(skb, sizeof(*hdr)); |
c5c77ba1 JK |
106 | memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr)); |
107 | hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ | |
c5c77ba1 | 108 | hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr)); |
c5c77ba1 | 109 | hdr->hdr.it_present = cpu_to_le32 |
1b7c69e8 | 110 | (1 << IEEE80211_RADIOTAP_RATE); /* | */ |
c5c77ba1 | 111 | hdr->rate = 5; /* txrate->bitrate / 5; */ |
c5c77ba1 JK |
112 | } |
113 | ||
c5c77ba1 | 114 | skb->dev = wilc_wfi_mon; |
fe211cd8 | 115 | skb_reset_mac_header(skb); |
c5c77ba1 JK |
116 | skb->ip_summed = CHECKSUM_UNNECESSARY; |
117 | skb->pkt_type = PACKET_OTHERHOST; | |
118 | skb->protocol = htons(ETH_P_802_2); | |
119 | memset(skb->cb, 0, sizeof(skb->cb)); | |
120 | ||
121 | netif_rx(skb); | |
c5c77ba1 JK |
122 | } |
123 | ||
124 | struct tx_complete_mon_data { | |
125 | int size; | |
126 | void *buff; | |
127 | }; | |
128 | ||
129 | static void mgmt_tx_complete(void *priv, int status) | |
130 | { | |
6bcc1e1e | 131 | struct tx_complete_mon_data *pv_data = priv; |
1b7c69e8 AJ |
132 | /* |
133 | * in case of fully hosting mode, the freeing will be done | |
134 | * in response to the cfg packet | |
135 | */ | |
c5c77ba1 JK |
136 | kfree(pv_data->buff); |
137 | ||
138 | kfree(pv_data); | |
c5c77ba1 | 139 | } |
964d8936 | 140 | |
c5c77ba1 JK |
141 | static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len) |
142 | { | |
c5c77ba1 JK |
143 | struct tx_complete_mon_data *mgmt_tx = NULL; |
144 | ||
60959e53 | 145 | if (!dev) |
e6e12661 | 146 | return -EFAULT; |
c5c77ba1 JK |
147 | |
148 | netif_stop_queue(dev); | |
fa611271 | 149 | mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC); |
e5349952 | 150 | if (!mgmt_tx) |
b026f6e8 | 151 | return -ENOMEM; |
c5c77ba1 | 152 | |
30ef5c8b | 153 | mgmt_tx->buff = kmalloc(len, GFP_ATOMIC); |
a1436579 | 154 | if (!mgmt_tx->buff) { |
f638dd39 | 155 | kfree(mgmt_tx); |
b026f6e8 | 156 | return -ENOMEM; |
c5c77ba1 JK |
157 | } |
158 | ||
159 | mgmt_tx->size = len; | |
160 | ||
c5c77ba1 | 161 | memcpy(mgmt_tx->buff, buf, len); |
829c477f | 162 | wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size, |
c9d4834d | 163 | mgmt_tx_complete); |
c5c77ba1 JK |
164 | |
165 | netif_wake_queue(dev); | |
166 | return 0; | |
167 | } | |
168 | ||
169 | /** | |
170 | * @brief WILC_WFI_mon_xmit | |
171 | * @details | |
172 | * @param[in] | |
173 | * @return int : Return 0 on Success | |
174 | * @author mdaftedar | |
175 | * @date 12 JUL 2012 | |
176 | * @version 1.0 | |
177 | */ | |
178 | static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb, | |
179 | struct net_device *dev) | |
180 | { | |
ccff7f81 | 181 | u32 rtap_len, ret = 0; |
c5c77ba1 JK |
182 | struct WILC_WFI_mon_priv *mon_priv; |
183 | ||
184 | struct sk_buff *skb2; | |
185 | struct wilc_wfi_radiotap_cb_hdr *cb_hdr; | |
186 | ||
a1436579 | 187 | if (!wilc_wfi_mon) |
e6e12661 | 188 | return -EFAULT; |
c5c77ba1 | 189 | |
c5c77ba1 | 190 | mon_priv = netdev_priv(wilc_wfi_mon); |
60959e53 | 191 | if (!mon_priv) |
e6e12661 | 192 | return -EFAULT; |
c5c77ba1 | 193 | rtap_len = ieee80211_get_radiotap_len(skb->data); |
60959e53 | 194 | if (skb->len < rtap_len) |
c5c77ba1 | 195 | return -1; |
60959e53 | 196 | |
c5c77ba1 JK |
197 | skb_pull(skb, rtap_len); |
198 | ||
c5c77ba1 JK |
199 | if (skb->data[0] == 0xc0 && (!(memcmp(broadcast, &skb->data[4], 6)))) { |
200 | skb2 = dev_alloc_skb(skb->len + sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
9e966527 PB |
201 | if (!skb2) |
202 | return -ENOMEM; | |
c5c77ba1 | 203 | |
59ae1d12 | 204 | skb_put_data(skb2, skb->data, skb->len); |
c5c77ba1 | 205 | |
d58ff351 | 206 | cb_hdr = skb_push(skb2, sizeof(*cb_hdr)); |
c5c77ba1 JK |
207 | memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr)); |
208 | ||
209 | cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ | |
210 | ||
211 | cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
212 | ||
213 | cb_hdr->hdr.it_present = cpu_to_le32( | |
214 | (1 << IEEE80211_RADIOTAP_RATE) | | |
215 | (1 << IEEE80211_RADIOTAP_TX_FLAGS)); | |
216 | ||
217 | cb_hdr->rate = 5; /* txrate->bitrate / 5; */ | |
218 | cb_hdr->tx_flags = 0x0004; | |
219 | ||
220 | skb2->dev = wilc_wfi_mon; | |
fe211cd8 | 221 | skb_reset_mac_header(skb2); |
c5c77ba1 JK |
222 | skb2->ip_summed = CHECKSUM_UNNECESSARY; |
223 | skb2->pkt_type = PACKET_OTHERHOST; | |
224 | skb2->protocol = htons(ETH_P_802_2); | |
225 | memset(skb2->cb, 0, sizeof(skb2->cb)); | |
226 | ||
227 | netif_rx(skb2); | |
228 | ||
229 | return 0; | |
230 | } | |
231 | skb->dev = mon_priv->real_ndev; | |
c5c77ba1 | 232 | |
c5c77ba1 | 233 | /* Identify if Ethernet or MAC header (data or mgmt) */ |
add0f9fe | 234 | memcpy(srcadd, &skb->data[10], 6); |
c5c77ba1 JK |
235 | memcpy(bssid, &skb->data[16], 6); |
236 | /* if source address and bssid fields are equal>>Mac header */ | |
237 | /*send it to mgmt frames handler */ | |
add0f9fe | 238 | if (!(memcmp(srcadd, bssid, 6))) { |
2eaf35c1 LK |
239 | ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len); |
240 | if (ret) | |
241 | netdev_err(dev, "fail to mgmt tx\n"); | |
c5c77ba1 | 242 | dev_kfree_skb(skb); |
b4a53a62 | 243 | } else { |
0e1af73d | 244 | ret = wilc_mac_xmit(skb, mon_priv->real_ndev); |
b4a53a62 | 245 | } |
c5c77ba1 | 246 | |
c5c77ba1 JK |
247 | return ret; |
248 | } | |
249 | ||
250 | static const struct net_device_ops wilc_wfi_netdev_ops = { | |
251 | .ndo_start_xmit = WILC_WFI_mon_xmit, | |
252 | ||
253 | }; | |
254 | ||
c5c77ba1 JK |
255 | /** |
256 | * @brief WILC_WFI_init_mon_interface | |
257 | * @details | |
258 | * @param[in] | |
259 | * @return int : Return 0 on Success | |
260 | * @author mdaftedar | |
261 | * @date 12 JUL 2012 | |
262 | * @version 1.0 | |
263 | */ | |
1b7c69e8 AJ |
264 | struct net_device *WILC_WFI_init_mon_interface(const char *name, |
265 | struct net_device *real_dev) | |
c5c77ba1 | 266 | { |
e6e12661 | 267 | u32 ret = 0; |
c5c77ba1 JK |
268 | struct WILC_WFI_mon_priv *priv; |
269 | ||
270 | /*If monitor interface is already initialized, return it*/ | |
c22177db | 271 | if (wilc_wfi_mon) |
c5c77ba1 | 272 | return wilc_wfi_mon; |
c5c77ba1 JK |
273 | |
274 | wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv)); | |
60959e53 | 275 | if (!wilc_wfi_mon) |
c5c77ba1 | 276 | return NULL; |
c5c77ba1 JK |
277 | wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP; |
278 | strncpy(wilc_wfi_mon->name, name, IFNAMSIZ); | |
279 | wilc_wfi_mon->name[IFNAMSIZ - 1] = 0; | |
280 | wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops; | |
281 | ||
282 | ret = register_netdevice(wilc_wfi_mon); | |
283 | if (ret) { | |
d892c97c | 284 | netdev_err(real_dev, "register_netdevice failed\n"); |
c5c77ba1 JK |
285 | return NULL; |
286 | } | |
287 | priv = netdev_priv(wilc_wfi_mon); | |
60959e53 | 288 | if (!priv) |
c5c77ba1 | 289 | return NULL; |
c5c77ba1 JK |
290 | |
291 | priv->real_ndev = real_dev; | |
292 | ||
293 | return wilc_wfi_mon; | |
294 | } | |
295 | ||
296 | /** | |
297 | * @brief WILC_WFI_deinit_mon_interface | |
298 | * @details | |
299 | * @param[in] | |
300 | * @return int : Return 0 on Success | |
301 | * @author mdaftedar | |
302 | * @date 12 JUL 2012 | |
303 | * @version 1.0 | |
304 | */ | |
a96c47e1 | 305 | int WILC_WFI_deinit_mon_interface(void) |
c5c77ba1 JK |
306 | { |
307 | bool rollback_lock = false; | |
308 | ||
a1436579 | 309 | if (wilc_wfi_mon) { |
c5c77ba1 JK |
310 | if (rtnl_is_locked()) { |
311 | rtnl_unlock(); | |
312 | rollback_lock = true; | |
313 | } | |
c5c77ba1 | 314 | unregister_netdev(wilc_wfi_mon); |
c5c77ba1 JK |
315 | |
316 | if (rollback_lock) { | |
317 | rtnl_lock(); | |
318 | rollback_lock = false; | |
319 | } | |
320 | wilc_wfi_mon = NULL; | |
321 | } | |
e6e12661 | 322 | return 0; |
c5c77ba1 | 323 | } |