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