Commit | Line | Data |
---|---|---|
c5c77ba1 JK |
1 | /*! |
2 | * @file linux_mon.c | |
3 | * @brief File Operations OS wrapper functionality | |
4 | * @author mdaftedar | |
5 | * @sa wilc_wfi_netdevice.h | |
6 | * @date 01 MAR 2012 | |
7 | * @version 1.0 | |
8 | */ | |
c5c77ba1 JK |
9 | #include "wilc_wfi_cfgoperations.h" |
10 | #include "linux_wlan_common.h" | |
11 | #include "wilc_wlan_if.h" | |
12 | #include "wilc_wlan.h" | |
9690df3f | 13 | |
c5c77ba1 JK |
14 | |
15 | struct wilc_wfi_radiotap_hdr { | |
16 | struct ieee80211_radiotap_header hdr; | |
17 | u8 rate; | |
c5c77ba1 JK |
18 | } __attribute__((packed)); |
19 | ||
20 | struct wilc_wfi_radiotap_cb_hdr { | |
21 | struct ieee80211_radiotap_header hdr; | |
22 | u8 rate; | |
23 | u8 dump; | |
24 | u16 tx_flags; | |
c5c77ba1 JK |
25 | } __attribute__((packed)); |
26 | ||
27 | extern linux_wlan_t *g_linux_wlan; | |
28 | ||
29 | static struct net_device *wilc_wfi_mon; /* global monitor netdev */ | |
30 | ||
c5c77ba1 | 31 | extern int mac_xmit(struct sk_buff *skb, struct net_device *dev); |
c5c77ba1 JK |
32 | |
33 | ||
63d03e47 GKH |
34 | u8 srcAdd[6]; |
35 | u8 bssid[6]; | |
36 | u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; | |
c5c77ba1 JK |
37 | /** |
38 | * @brief WILC_WFI_monitor_rx | |
39 | * @details | |
40 | * @param[in] | |
41 | * @return int : Return 0 on Success | |
42 | * @author mdaftedar | |
43 | * @date 12 JUL 2012 | |
44 | * @version 1.0 | |
45 | */ | |
46 | ||
47 | #define IEEE80211_RADIOTAP_F_TX_RTS 0x0004 /* used rts/cts handshake */ | |
48 | #define IEEE80211_RADIOTAP_F_TX_FAIL 0x0001 /* failed due to excessive*/ | |
49 | #define IS_MANAGMEMENT 0x100 | |
50 | #define IS_MANAGMEMENT_CALLBACK 0x080 | |
51 | #define IS_MGMT_STATUS_SUCCES 0x040 | |
52 | #define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff) | |
53 | ||
fbc2fe16 | 54 | void WILC_WFI_monitor_rx(u8 *buff, u32 size) |
c5c77ba1 | 55 | { |
fbc2fe16 | 56 | u32 header, pkt_offset; |
c5c77ba1 JK |
57 | struct sk_buff *skb = NULL; |
58 | struct wilc_wfi_radiotap_hdr *hdr; | |
59 | struct wilc_wfi_radiotap_cb_hdr *cb_hdr; | |
60 | ||
61 | PRINT_INFO(HOSTAPD_DBG, "In monitor interface receive function\n"); | |
62 | ||
c5c77ba1 JK |
63 | /* Bug 4601 */ |
64 | if (wilc_wfi_mon == NULL) | |
65 | return; | |
66 | ||
67 | if (!netif_running(wilc_wfi_mon)) { | |
68 | PRINT_INFO(HOSTAPD_DBG, "Monitor interface already RUNNING\n"); | |
69 | return; | |
70 | } | |
71 | ||
72 | /* Get WILC header */ | |
73 | memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET); | |
74 | ||
75 | /* The packet offset field conain info about what type of managment frame */ | |
76 | /* we are dealing with and ack status */ | |
77 | pkt_offset = GET_PKT_OFFSET(header); | |
78 | ||
79 | if (pkt_offset & IS_MANAGMEMENT_CALLBACK) { | |
80 | ||
81 | /* hostapd callback mgmt frame */ | |
82 | ||
83 | skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
84 | if (skb == NULL) { | |
85 | PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb"); | |
86 | return; | |
87 | } | |
88 | ||
89 | memcpy(skb_put(skb, size), buff, size); | |
90 | ||
91 | cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb, sizeof(*cb_hdr)); | |
92 | memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
93 | ||
94 | cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ | |
95 | ||
96 | cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
97 | ||
98 | cb_hdr->hdr.it_present = cpu_to_le32( | |
99 | (1 << IEEE80211_RADIOTAP_RATE) | | |
100 | (1 << IEEE80211_RADIOTAP_TX_FLAGS)); | |
101 | ||
102 | cb_hdr->rate = 5; /* txrate->bitrate / 5; */ | |
103 | ||
104 | if (pkt_offset & IS_MGMT_STATUS_SUCCES) { | |
105 | /* success */ | |
106 | cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS; | |
107 | } else { | |
108 | cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL; | |
109 | } | |
110 | ||
111 | } else { | |
112 | ||
113 | skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr)); | |
114 | ||
115 | if (skb == NULL) { | |
116 | PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb"); | |
117 | return; | |
118 | } | |
119 | ||
c5c77ba1 JK |
120 | memcpy(skb_put(skb, size), buff, size); |
121 | hdr = (struct wilc_wfi_radiotap_hdr *) skb_push(skb, sizeof(*hdr)); | |
122 | memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr)); | |
123 | hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ | |
c5c77ba1 JK |
124 | hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr)); |
125 | PRINT_INFO(HOSTAPD_DBG, "Radiotap len %d\n", hdr->hdr.it_len); | |
126 | hdr->hdr.it_present = cpu_to_le32 | |
127 | (1 << IEEE80211_RADIOTAP_RATE); /* | */ | |
c5c77ba1 JK |
128 | PRINT_INFO(HOSTAPD_DBG, "Presentflags %d\n", hdr->hdr.it_present); |
129 | hdr->rate = 5; /* txrate->bitrate / 5; */ | |
130 | ||
131 | } | |
132 | ||
c5c77ba1 JK |
133 | |
134 | ||
135 | skb->dev = wilc_wfi_mon; | |
136 | skb_set_mac_header(skb, 0); | |
137 | skb->ip_summed = CHECKSUM_UNNECESSARY; | |
138 | skb->pkt_type = PACKET_OTHERHOST; | |
139 | skb->protocol = htons(ETH_P_802_2); | |
140 | memset(skb->cb, 0, sizeof(skb->cb)); | |
141 | ||
142 | netif_rx(skb); | |
143 | ||
144 | ||
145 | } | |
146 | ||
147 | struct tx_complete_mon_data { | |
148 | int size; | |
149 | void *buff; | |
150 | }; | |
151 | ||
152 | static void mgmt_tx_complete(void *priv, int status) | |
153 | { | |
154 | ||
c5c77ba1 | 155 | struct tx_complete_mon_data *pv_data = (struct tx_complete_mon_data *)priv; |
63d03e47 | 156 | u8 *buf = pv_data->buff; |
c5c77ba1 JK |
157 | |
158 | ||
159 | ||
160 | if (status == 1) { | |
161 | if (INFO || buf[0] == 0x10 || buf[0] == 0xb0) | |
162 | PRINT_INFO(HOSTAPD_DBG, "Packet sent successfully - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff); | |
163 | } else { | |
164 | PRINT_INFO(HOSTAPD_DBG, "Couldn't send packet - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff); | |
165 | } | |
166 | ||
167 | ||
c5c77ba1 JK |
168 | |
169 | /* incase of fully hosting mode, the freeing will be done in response to the cfg packet */ | |
c5c77ba1 JK |
170 | kfree(pv_data->buff); |
171 | ||
172 | kfree(pv_data); | |
c5c77ba1 JK |
173 | } |
174 | static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len) | |
175 | { | |
c5c77ba1 JK |
176 | struct tx_complete_mon_data *mgmt_tx = NULL; |
177 | ||
178 | if (dev == NULL) { | |
179 | PRINT_D(HOSTAPD_DBG, "ERROR: dev == NULL\n"); | |
e6e12661 | 180 | return -EFAULT; |
c5c77ba1 | 181 | } |
c5c77ba1 JK |
182 | |
183 | netif_stop_queue(dev); | |
25fe2274 | 184 | mgmt_tx = kmalloc(sizeof(struct tx_complete_mon_data), GFP_ATOMIC); |
c5c77ba1 JK |
185 | if (mgmt_tx == NULL) { |
186 | PRINT_ER("Failed to allocate memory for mgmt_tx structure\n"); | |
e6e12661 | 187 | return -EFAULT; |
c5c77ba1 JK |
188 | } |
189 | ||
30ef5c8b | 190 | mgmt_tx->buff = kmalloc(len, GFP_ATOMIC); |
c5c77ba1 JK |
191 | if (mgmt_tx->buff == NULL) { |
192 | PRINT_ER("Failed to allocate memory for mgmt_tx buff\n"); | |
f638dd39 | 193 | kfree(mgmt_tx); |
e6e12661 | 194 | return -EFAULT; |
c5c77ba1 JK |
195 | |
196 | } | |
197 | ||
198 | mgmt_tx->size = len; | |
199 | ||
c5c77ba1 | 200 | memcpy(mgmt_tx->buff, buf, len); |
c5c77ba1 JK |
201 | g_linux_wlan->oup.wlan_add_mgmt_to_tx_que(mgmt_tx, mgmt_tx->buff, mgmt_tx->size, mgmt_tx_complete); |
202 | ||
203 | netif_wake_queue(dev); | |
204 | return 0; | |
205 | } | |
206 | ||
207 | /** | |
208 | * @brief WILC_WFI_mon_xmit | |
209 | * @details | |
210 | * @param[in] | |
211 | * @return int : Return 0 on Success | |
212 | * @author mdaftedar | |
213 | * @date 12 JUL 2012 | |
214 | * @version 1.0 | |
215 | */ | |
216 | static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb, | |
217 | struct net_device *dev) | |
218 | { | |
4e4467fd | 219 | u32 rtap_len, i, ret = 0; |
c5c77ba1 JK |
220 | struct WILC_WFI_mon_priv *mon_priv; |
221 | ||
222 | struct sk_buff *skb2; | |
223 | struct wilc_wfi_radiotap_cb_hdr *cb_hdr; | |
224 | ||
225 | /* Bug 4601 */ | |
226 | if (wilc_wfi_mon == NULL) | |
e6e12661 | 227 | return -EFAULT; |
c5c77ba1 | 228 | |
c5c77ba1 JK |
229 | mon_priv = netdev_priv(wilc_wfi_mon); |
230 | ||
231 | if (mon_priv == NULL) { | |
232 | PRINT_ER("Monitor interface private structure is NULL\n"); | |
e6e12661 | 233 | return -EFAULT; |
c5c77ba1 JK |
234 | } |
235 | ||
c5c77ba1 JK |
236 | |
237 | rtap_len = ieee80211_get_radiotap_len(skb->data); | |
238 | if (skb->len < rtap_len) { | |
239 | PRINT_ER("Error in radiotap header\n"); | |
240 | return -1; | |
241 | } | |
242 | /* skip the radiotap header */ | |
243 | PRINT_INFO(HOSTAPD_DBG, "Radiotap len: %d\n", rtap_len); | |
244 | ||
245 | if (INFO) { | |
246 | for (i = 0; i < rtap_len; i++) | |
247 | PRINT_INFO(HOSTAPD_DBG, "Radiotap_hdr[%d] %02x\n", i, skb->data[i]); | |
248 | } | |
249 | /* Skip the ratio tap header */ | |
250 | skb_pull(skb, rtap_len); | |
251 | ||
252 | if (skb->data[0] == 0xc0) | |
253 | PRINT_INFO(HOSTAPD_DBG, "%x:%x:%x:%x:%x%x\n", skb->data[4], skb->data[5], skb->data[6], skb->data[7], skb->data[8], skb->data[9]); | |
254 | ||
255 | if (skb->data[0] == 0xc0 && (!(memcmp(broadcast, &skb->data[4], 6)))) { | |
256 | skb2 = dev_alloc_skb(skb->len + sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
257 | ||
258 | memcpy(skb_put(skb2, skb->len), skb->data, skb->len); | |
259 | ||
260 | cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb2, sizeof(*cb_hdr)); | |
261 | memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
262 | ||
263 | cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ | |
264 | ||
265 | cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr)); | |
266 | ||
267 | cb_hdr->hdr.it_present = cpu_to_le32( | |
268 | (1 << IEEE80211_RADIOTAP_RATE) | | |
269 | (1 << IEEE80211_RADIOTAP_TX_FLAGS)); | |
270 | ||
271 | cb_hdr->rate = 5; /* txrate->bitrate / 5; */ | |
272 | cb_hdr->tx_flags = 0x0004; | |
273 | ||
274 | skb2->dev = wilc_wfi_mon; | |
275 | skb_set_mac_header(skb2, 0); | |
276 | skb2->ip_summed = CHECKSUM_UNNECESSARY; | |
277 | skb2->pkt_type = PACKET_OTHERHOST; | |
278 | skb2->protocol = htons(ETH_P_802_2); | |
279 | memset(skb2->cb, 0, sizeof(skb2->cb)); | |
280 | ||
281 | netif_rx(skb2); | |
282 | ||
283 | return 0; | |
284 | } | |
285 | skb->dev = mon_priv->real_ndev; | |
286 | ||
287 | PRINT_INFO(HOSTAPD_DBG, "Skipping the radiotap header\n"); | |
288 | ||
289 | ||
290 | ||
291 | /* actual deliver of data is device-specific, and not shown here */ | |
292 | PRINT_INFO(HOSTAPD_DBG, "SKB netdevice name = %s\n", skb->dev->name); | |
293 | PRINT_INFO(HOSTAPD_DBG, "MONITOR real dev name = %s\n", mon_priv->real_ndev->name); | |
294 | ||
c5c77ba1 JK |
295 | /* Identify if Ethernet or MAC header (data or mgmt) */ |
296 | memcpy(srcAdd, &skb->data[10], 6); | |
297 | memcpy(bssid, &skb->data[16], 6); | |
298 | /* if source address and bssid fields are equal>>Mac header */ | |
299 | /*send it to mgmt frames handler */ | |
300 | if (!(memcmp(srcAdd, bssid, 6))) { | |
301 | mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len); | |
302 | dev_kfree_skb(skb); | |
303 | } else | |
304 | ret = mac_xmit(skb, mon_priv->real_ndev); | |
c5c77ba1 | 305 | |
c5c77ba1 JK |
306 | return ret; |
307 | } | |
308 | ||
309 | static const struct net_device_ops wilc_wfi_netdev_ops = { | |
310 | .ndo_start_xmit = WILC_WFI_mon_xmit, | |
311 | ||
312 | }; | |
313 | ||
c5c77ba1 JK |
314 | /** |
315 | * @brief WILC_WFI_mon_setup | |
316 | * @details | |
317 | * @param[in] | |
318 | * @return int : Return 0 on Success | |
319 | * @author mdaftedar | |
320 | * @date 12 JUL 2012 | |
321 | * @version 1.0 | |
322 | */ | |
323 | static void WILC_WFI_mon_setup(struct net_device *dev) | |
324 | { | |
325 | ||
326 | dev->netdev_ops = &wilc_wfi_netdev_ops; | |
c5c77ba1 JK |
327 | PRINT_INFO(CORECONFIG_DBG, "In Ethernet setup function\n"); |
328 | ether_setup(dev); | |
7d9e437d | 329 | dev->priv_flags |= IFF_NO_QUEUE; |
c5c77ba1 | 330 | dev->type = ARPHRD_IEEE80211_RADIOTAP; |
8ca1b55a | 331 | eth_zero_addr(dev->dev_addr); |
c5c77ba1 | 332 | |
c5c77ba1 | 333 | { |
c5c77ba1 | 334 | unsigned char mac_add[] = {0x00, 0x50, 0xc2, 0x5e, 0x10, 0x8f}; |
c5c77ba1 JK |
335 | memcpy(dev->dev_addr, mac_add, ETH_ALEN); |
336 | } | |
c5c77ba1 JK |
337 | |
338 | } | |
339 | ||
340 | /** | |
341 | * @brief WILC_WFI_init_mon_interface | |
342 | * @details | |
343 | * @param[in] | |
344 | * @return int : Return 0 on Success | |
345 | * @author mdaftedar | |
346 | * @date 12 JUL 2012 | |
347 | * @version 1.0 | |
348 | */ | |
057d1e97 | 349 | struct net_device *WILC_WFI_init_mon_interface(const char *name, struct net_device *real_dev) |
c5c77ba1 JK |
350 | { |
351 | ||
352 | ||
e6e12661 | 353 | u32 ret = 0; |
c5c77ba1 JK |
354 | struct WILC_WFI_mon_priv *priv; |
355 | ||
356 | /*If monitor interface is already initialized, return it*/ | |
357 | if (wilc_wfi_mon) { | |
358 | return wilc_wfi_mon; | |
359 | } | |
c5c77ba1 JK |
360 | |
361 | wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv)); | |
362 | if (!wilc_wfi_mon) { | |
363 | PRINT_ER("failed to allocate memory\n"); | |
364 | return NULL; | |
365 | ||
366 | } | |
367 | ||
368 | wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP; | |
369 | strncpy(wilc_wfi_mon->name, name, IFNAMSIZ); | |
370 | wilc_wfi_mon->name[IFNAMSIZ - 1] = 0; | |
371 | wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops; | |
372 | ||
373 | ret = register_netdevice(wilc_wfi_mon); | |
374 | if (ret) { | |
375 | PRINT_ER(" register_netdevice failed (%d)\n", ret); | |
376 | return NULL; | |
377 | } | |
378 | priv = netdev_priv(wilc_wfi_mon); | |
379 | if (priv == NULL) { | |
380 | PRINT_ER("private structure is NULL\n"); | |
381 | return NULL; | |
382 | } | |
383 | ||
384 | priv->real_ndev = real_dev; | |
385 | ||
386 | return wilc_wfi_mon; | |
387 | } | |
388 | ||
389 | /** | |
390 | * @brief WILC_WFI_deinit_mon_interface | |
391 | * @details | |
392 | * @param[in] | |
393 | * @return int : Return 0 on Success | |
394 | * @author mdaftedar | |
395 | * @date 12 JUL 2012 | |
396 | * @version 1.0 | |
397 | */ | |
a96c47e1 | 398 | int WILC_WFI_deinit_mon_interface(void) |
c5c77ba1 JK |
399 | { |
400 | bool rollback_lock = false; | |
401 | ||
402 | if (wilc_wfi_mon != NULL) { | |
403 | PRINT_D(HOSTAPD_DBG, "In Deinit monitor interface\n"); | |
404 | PRINT_D(HOSTAPD_DBG, "RTNL is being locked\n"); | |
405 | if (rtnl_is_locked()) { | |
406 | rtnl_unlock(); | |
407 | rollback_lock = true; | |
408 | } | |
409 | PRINT_D(HOSTAPD_DBG, "Unregister netdev\n"); | |
410 | unregister_netdev(wilc_wfi_mon); | |
c5c77ba1 JK |
411 | |
412 | if (rollback_lock) { | |
413 | rtnl_lock(); | |
414 | rollback_lock = false; | |
415 | } | |
416 | wilc_wfi_mon = NULL; | |
417 | } | |
e6e12661 | 418 | return 0; |
c5c77ba1 JK |
419 | |
420 | } |