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 */
9 #include "wilc_wfi_cfgoperations.h"
10 #include "wilc_wlan_if.h"
11 #include "wilc_wlan.h"
12
13 struct wilc_wfi_radiotap_hdr {
14 struct ieee80211_radiotap_header hdr;
15 u8 rate;
16 } __packed;
17
18 struct wilc_wfi_radiotap_cb_hdr {
19 struct ieee80211_radiotap_header hdr;
20 u8 rate;
21 u8 dump;
22 u16 tx_flags;
23 } __packed;
24
25 static struct net_device *wilc_wfi_mon; /* global monitor netdev */
26
27 static u8 srcadd[6];
28 static u8 bssid[6];
29 static u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
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
WILC_WFI_monitor_rx(u8 * buff,u32 size)47 void WILC_WFI_monitor_rx(u8 *buff, u32 size)
48 {
49 u32 header, pkt_offset;
50 struct sk_buff *skb = NULL;
51 struct wilc_wfi_radiotap_hdr *hdr;
52 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
53
54 if (!wilc_wfi_mon)
55 return;
56
57 if (!netif_running(wilc_wfi_mon))
58 return;
59
60 /* Get WILC header */
61 memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
62 /*
63 * The packet offset field contain info about what type of management
64 * the frame we are dealing with and ack status
65 */
66 pkt_offset = GET_PKT_OFFSET(header);
67
68 if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
69 /* hostapd callback mgmt frame */
70
71 skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr));
72 if (!skb)
73 return;
74
75 memcpy(skb_put(skb, size), buff, size);
76
77 cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *)skb_push(skb, sizeof(*cb_hdr));
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 {
98 skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr));
99
100 if (!skb)
101 return;
102
103 memcpy(skb_put(skb, size), buff, size);
104 hdr = (struct wilc_wfi_radiotap_hdr *)skb_push(skb, sizeof(*hdr));
105 memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
106 hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
107 hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr));
108 hdr->hdr.it_present = cpu_to_le32
109 (1 << IEEE80211_RADIOTAP_RATE); /* | */
110 hdr->rate = 5; /* txrate->bitrate / 5; */
111 }
112
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);
121 }
122
123 struct tx_complete_mon_data {
124 int size;
125 void *buff;
126 };
127
mgmt_tx_complete(void * priv,int status)128 static void mgmt_tx_complete(void *priv, int status)
129 {
130 struct tx_complete_mon_data *pv_data = priv;
131 /*
132 * in case of fully hosting mode, the freeing will be done
133 * in response to the cfg packet
134 */
135 kfree(pv_data->buff);
136
137 kfree(pv_data);
138 }
139
mon_mgmt_tx(struct net_device * dev,const u8 * buf,size_t len)140 static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
141 {
142 struct tx_complete_mon_data *mgmt_tx = NULL;
143
144 if (!dev)
145 return -EFAULT;
146
147 netif_stop_queue(dev);
148 mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC);
149 if (!mgmt_tx)
150 return -ENOMEM;
151
152 mgmt_tx->buff = kmalloc(len, GFP_ATOMIC);
153 if (!mgmt_tx->buff) {
154 kfree(mgmt_tx);
155 return -ENOMEM;
156 }
157
158 mgmt_tx->size = len;
159
160 memcpy(mgmt_tx->buff, buf, len);
161 wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
162 mgmt_tx_complete);
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 */
WILC_WFI_mon_xmit(struct sk_buff * skb,struct net_device * dev)177 static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
178 struct net_device *dev)
179 {
180 u32 rtap_len, ret = 0;
181 struct WILC_WFI_mon_priv *mon_priv;
182
183 struct sk_buff *skb2;
184 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
185
186 if (!wilc_wfi_mon)
187 return -EFAULT;
188
189 mon_priv = netdev_priv(wilc_wfi_mon);
190 if (!mon_priv)
191 return -EFAULT;
192 rtap_len = ieee80211_get_radiotap_len(skb->data);
193 if (skb->len < rtap_len)
194 return -1;
195
196 skb_pull(skb, rtap_len);
197
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 if (!skb2)
201 return -ENOMEM;
202
203 memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
204
205 cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *)skb_push(skb2, sizeof(*cb_hdr));
206 memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
207
208 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
209
210 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
211
212 cb_hdr->hdr.it_present = cpu_to_le32(
213 (1 << IEEE80211_RADIOTAP_RATE) |
214 (1 << IEEE80211_RADIOTAP_TX_FLAGS));
215
216 cb_hdr->rate = 5; /* txrate->bitrate / 5; */
217 cb_hdr->tx_flags = 0x0004;
218
219 skb2->dev = wilc_wfi_mon;
220 skb_set_mac_header(skb2, 0);
221 skb2->ip_summed = CHECKSUM_UNNECESSARY;
222 skb2->pkt_type = PACKET_OTHERHOST;
223 skb2->protocol = htons(ETH_P_802_2);
224 memset(skb2->cb, 0, sizeof(skb2->cb));
225
226 netif_rx(skb2);
227
228 return 0;
229 }
230 skb->dev = mon_priv->real_ndev;
231
232 /* Identify if Ethernet or MAC header (data or mgmt) */
233 memcpy(srcadd, &skb->data[10], 6);
234 memcpy(bssid, &skb->data[16], 6);
235 /* if source address and bssid fields are equal>>Mac header */
236 /*send it to mgmt frames handler */
237 if (!(memcmp(srcadd, bssid, 6))) {
238 ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
239 if (ret)
240 netdev_err(dev, "fail to mgmt tx\n");
241 dev_kfree_skb(skb);
242 } else {
243 ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
244 }
245
246 return ret;
247 }
248
249 static const struct net_device_ops wilc_wfi_netdev_ops = {
250 .ndo_start_xmit = WILC_WFI_mon_xmit,
251
252 };
253
254 /**
255 * @brief WILC_WFI_init_mon_interface
256 * @details
257 * @param[in]
258 * @return int : Return 0 on Success
259 * @author mdaftedar
260 * @date 12 JUL 2012
261 * @version 1.0
262 */
WILC_WFI_init_mon_interface(const char * name,struct net_device * real_dev)263 struct net_device *WILC_WFI_init_mon_interface(const char *name,
264 struct net_device *real_dev)
265 {
266 u32 ret = 0;
267 struct WILC_WFI_mon_priv *priv;
268
269 /*If monitor interface is already initialized, return it*/
270 if (wilc_wfi_mon)
271 return wilc_wfi_mon;
272
273 wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv));
274 if (!wilc_wfi_mon)
275 return NULL;
276 wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP;
277 strncpy(wilc_wfi_mon->name, name, IFNAMSIZ);
278 wilc_wfi_mon->name[IFNAMSIZ - 1] = 0;
279 wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops;
280
281 ret = register_netdevice(wilc_wfi_mon);
282 if (ret) {
283 netdev_err(real_dev, "register_netdevice failed\n");
284 return NULL;
285 }
286 priv = netdev_priv(wilc_wfi_mon);
287 if (!priv)
288 return NULL;
289
290 priv->real_ndev = real_dev;
291
292 return wilc_wfi_mon;
293 }
294
295 /**
296 * @brief WILC_WFI_deinit_mon_interface
297 * @details
298 * @param[in]
299 * @return int : Return 0 on Success
300 * @author mdaftedar
301 * @date 12 JUL 2012
302 * @version 1.0
303 */
WILC_WFI_deinit_mon_interface(void)304 int WILC_WFI_deinit_mon_interface(void)
305 {
306 bool rollback_lock = false;
307
308 if (wilc_wfi_mon) {
309 if (rtnl_is_locked()) {
310 rtnl_unlock();
311 rollback_lock = true;
312 }
313 unregister_netdev(wilc_wfi_mon);
314
315 if (rollback_lock) {
316 rtnl_lock();
317 rollback_lock = false;
318 }
319 wilc_wfi_mon = NULL;
320 }
321 return 0;
322 }
323