1 // SPDX-License-Identifier: GPL-2.0
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 */
10 #include "wilc_wfi_cfgoperations.h"
11 #include "wilc_wlan_if.h"
12 #include "wilc_wlan.h"
13
14 struct wilc_wfi_radiotap_hdr {
15 struct ieee80211_radiotap_header hdr;
16 u8 rate;
17 } __packed;
18
19 struct wilc_wfi_radiotap_cb_hdr {
20 struct ieee80211_radiotap_header hdr;
21 u8 rate;
22 u8 dump;
23 u16 tx_flags;
24 } __packed;
25
26 static struct net_device *wilc_wfi_mon; /* global monitor netdev */
27
28 static u8 srcadd[6];
29 static u8 bssid[6];
30 static u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
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
WILC_WFI_monitor_rx(u8 * buff,u32 size)48 void WILC_WFI_monitor_rx(u8 *buff, u32 size)
49 {
50 u32 header, pkt_offset;
51 struct sk_buff *skb = NULL;
52 struct wilc_wfi_radiotap_hdr *hdr;
53 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
54
55 if (!wilc_wfi_mon)
56 return;
57
58 if (!netif_running(wilc_wfi_mon))
59 return;
60
61 /* Get WILC header */
62 memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
63 /*
64 * The packet offset field contain info about what type of management
65 * the frame we are dealing with and ack status
66 */
67 pkt_offset = GET_PKT_OFFSET(header);
68
69 if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
70 /* hostapd callback mgmt frame */
71
72 skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr));
73 if (!skb)
74 return;
75
76 skb_put_data(skb, buff, size);
77
78 cb_hdr = skb_push(skb, sizeof(*cb_hdr));
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 {
99 skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr));
100
101 if (!skb)
102 return;
103
104 skb_put_data(skb, buff, size);
105 hdr = skb_push(skb, sizeof(*hdr));
106 memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
107 hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
108 hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr));
109 hdr->hdr.it_present = cpu_to_le32
110 (1 << IEEE80211_RADIOTAP_RATE); /* | */
111 hdr->rate = 5; /* txrate->bitrate / 5; */
112 }
113
114 skb->dev = wilc_wfi_mon;
115 skb_reset_mac_header(skb);
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);
122 }
123
124 struct tx_complete_mon_data {
125 int size;
126 void *buff;
127 };
128
mgmt_tx_complete(void * priv,int status)129 static void mgmt_tx_complete(void *priv, int status)
130 {
131 struct tx_complete_mon_data *pv_data = priv;
132 /*
133 * in case of fully hosting mode, the freeing will be done
134 * in response to the cfg packet
135 */
136 kfree(pv_data->buff);
137
138 kfree(pv_data);
139 }
140
mon_mgmt_tx(struct net_device * dev,const u8 * buf,size_t len)141 static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
142 {
143 struct tx_complete_mon_data *mgmt_tx = NULL;
144
145 if (!dev)
146 return -EFAULT;
147
148 netif_stop_queue(dev);
149 mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC);
150 if (!mgmt_tx)
151 return -ENOMEM;
152
153 mgmt_tx->buff = kmalloc(len, GFP_ATOMIC);
154 if (!mgmt_tx->buff) {
155 kfree(mgmt_tx);
156 return -ENOMEM;
157 }
158
159 mgmt_tx->size = len;
160
161 memcpy(mgmt_tx->buff, buf, len);
162 wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
163 mgmt_tx_complete);
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 */
WILC_WFI_mon_xmit(struct sk_buff * skb,struct net_device * dev)178 static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
179 struct net_device *dev)
180 {
181 u32 rtap_len, ret = 0;
182 struct WILC_WFI_mon_priv *mon_priv;
183
184 struct sk_buff *skb2;
185 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
186
187 if (!wilc_wfi_mon)
188 return -EFAULT;
189
190 mon_priv = netdev_priv(wilc_wfi_mon);
191 if (!mon_priv)
192 return -EFAULT;
193 rtap_len = ieee80211_get_radiotap_len(skb->data);
194 if (skb->len < rtap_len)
195 return -1;
196
197 skb_pull(skb, rtap_len);
198
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));
201 if (!skb2)
202 return -ENOMEM;
203
204 skb_put_data(skb2, skb->data, skb->len);
205
206 cb_hdr = skb_push(skb2, sizeof(*cb_hdr));
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;
221 skb_reset_mac_header(skb2);
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;
232
233 /* Identify if Ethernet or MAC header (data or mgmt) */
234 memcpy(srcadd, &skb->data[10], 6);
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 */
238 if (!(memcmp(srcadd, bssid, 6))) {
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");
242 dev_kfree_skb(skb);
243 } else {
244 ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
245 }
246
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
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 */
WILC_WFI_init_mon_interface(const char * name,struct net_device * real_dev)264 struct net_device *WILC_WFI_init_mon_interface(const char *name,
265 struct net_device *real_dev)
266 {
267 u32 ret = 0;
268 struct WILC_WFI_mon_priv *priv;
269
270 /*If monitor interface is already initialized, return it*/
271 if (wilc_wfi_mon)
272 return wilc_wfi_mon;
273
274 wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv));
275 if (!wilc_wfi_mon)
276 return NULL;
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) {
284 netdev_err(real_dev, "register_netdevice failed\n");
285 return NULL;
286 }
287 priv = netdev_priv(wilc_wfi_mon);
288 if (!priv)
289 return NULL;
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 */
WILC_WFI_deinit_mon_interface(void)305 int WILC_WFI_deinit_mon_interface(void)
306 {
307 bool rollback_lock = false;
308
309 if (wilc_wfi_mon) {
310 if (rtnl_is_locked()) {
311 rtnl_unlock();
312 rollback_lock = true;
313 }
314 unregister_netdev(wilc_wfi_mon);
315
316 if (rollback_lock) {
317 rtnl_lock();
318 rollback_lock = false;
319 }
320 wilc_wfi_mon = NULL;
321 }
322 return 0;
323 }
324