1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2012 - 2018 Microchip Technology Inc., and its subsidiaries.
4 * All rights reserved.
5 */
6
7 #include "cfg80211.h"
8
9 struct wilc_wfi_radiotap_hdr {
10 struct ieee80211_radiotap_header_fixed hdr;
11 u8 rate;
12 } __packed;
13
14 struct wilc_wfi_radiotap_cb_hdr {
15 struct ieee80211_radiotap_header_fixed hdr;
16 u8 rate;
17 u8 dump;
18 u16 tx_flags;
19 } __packed;
20
21 #define TX_RADIOTAP_PRESENT ((1 << IEEE80211_RADIOTAP_RATE) | \
22 (1 << IEEE80211_RADIOTAP_TX_FLAGS))
23
wilc_wfi_monitor_rx(struct net_device * mon_dev,u8 * buff,u32 size)24 void wilc_wfi_monitor_rx(struct net_device *mon_dev, u8 *buff, u32 size)
25 {
26 u32 header, pkt_offset;
27 struct sk_buff *skb = NULL;
28 struct wilc_wfi_radiotap_hdr *hdr;
29 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
30
31 if (!mon_dev)
32 return;
33
34 if (!netif_running(mon_dev))
35 return;
36
37 /* Get WILC header */
38 header = get_unaligned_le32(buff - HOST_HDR_OFFSET);
39 /*
40 * The packet offset field contain info about what type of management
41 * the frame we are dealing with and ack status
42 */
43 pkt_offset = FIELD_GET(WILC_PKT_HDR_OFFSET_FIELD, header);
44
45 if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
46 /* hostapd callback mgmt frame */
47
48 skb = dev_alloc_skb(size + sizeof(*cb_hdr));
49 if (!skb)
50 return;
51
52 skb_put_data(skb, buff, size);
53
54 cb_hdr = skb_push(skb, sizeof(*cb_hdr));
55 memset(cb_hdr, 0, sizeof(*cb_hdr));
56
57 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
58
59 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
60
61 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
62
63 cb_hdr->rate = 5;
64
65 if (pkt_offset & IS_MGMT_STATUS_SUCCES) {
66 /* success */
67 cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
68 } else {
69 cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
70 }
71
72 } else {
73 skb = dev_alloc_skb(size + sizeof(*hdr));
74
75 if (!skb)
76 return;
77
78 skb_put_data(skb, buff, size);
79 hdr = skb_push(skb, sizeof(*hdr));
80 memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
81 hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
82 hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
83 hdr->hdr.it_present = cpu_to_le32
84 (1 << IEEE80211_RADIOTAP_RATE);
85 hdr->rate = 5;
86 }
87
88 skb->dev = mon_dev;
89 skb_reset_mac_header(skb);
90 skb->ip_summed = CHECKSUM_UNNECESSARY;
91 skb->pkt_type = PACKET_OTHERHOST;
92 skb->protocol = htons(ETH_P_802_2);
93 memset(skb->cb, 0, sizeof(skb->cb));
94
95 netif_rx(skb);
96 }
97
98 struct tx_complete_mon_data {
99 int size;
100 void *buff;
101 };
102
mgmt_tx_complete(void * priv,int status)103 static void mgmt_tx_complete(void *priv, int status)
104 {
105 struct tx_complete_mon_data *pv_data = priv;
106 /*
107 * in case of fully hosting mode, the freeing will be done
108 * in response to the cfg packet
109 */
110 kfree(pv_data->buff);
111
112 kfree(pv_data);
113 }
114
mon_mgmt_tx(struct net_device * dev,const u8 * buf,size_t len)115 static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
116 {
117 struct tx_complete_mon_data *mgmt_tx = NULL;
118
119 if (!dev)
120 return -EFAULT;
121
122 netif_stop_queue(dev);
123 mgmt_tx = kmalloc_obj(*mgmt_tx, GFP_ATOMIC);
124 if (!mgmt_tx)
125 return -ENOMEM;
126
127 mgmt_tx->buff = kmemdup(buf, len, GFP_ATOMIC);
128 if (!mgmt_tx->buff) {
129 kfree(mgmt_tx);
130 return -ENOMEM;
131 }
132
133 mgmt_tx->size = len;
134
135 wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
136 mgmt_tx_complete);
137
138 netif_wake_queue(dev);
139 return 0;
140 }
141
wilc_wfi_mon_xmit(struct sk_buff * skb,struct net_device * dev)142 static netdev_tx_t wilc_wfi_mon_xmit(struct sk_buff *skb,
143 struct net_device *dev)
144 {
145 struct ieee80211_radiotap_header_fixed *rtap_hdr;
146 struct ieee80211_hdr_3addr *hdr;
147 unsigned int hdr_len;
148 u32 rtap_len, ret = 0;
149 struct wilc_wfi_mon_priv *mon_priv;
150 struct sk_buff *skb2;
151 struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
152 u8 srcadd[ETH_ALEN];
153 u8 bssid[ETH_ALEN];
154
155 mon_priv = netdev_priv(dev);
156 if (!mon_priv)
157 return -EFAULT;
158
159 if (skb->len < sizeof(*rtap_hdr))
160 goto drop;
161
162 rtap_hdr = (void *)skb->data;
163 if (rtap_hdr->it_version)
164 goto drop;
165
166 rtap_len = ieee80211_get_radiotap_len(skb->data);
167 if (rtap_len < sizeof(*rtap_hdr) || skb->len < rtap_len)
168 goto drop;
169
170 skb_pull(skb, rtap_len);
171 hdr_len = ieee80211_get_hdrlen_from_skb(skb);
172 if (hdr_len < sizeof(*hdr))
173 goto drop;
174
175 hdr = (void *)skb->data;
176
177 if (ieee80211_is_deauth(hdr->frame_control) &&
178 is_broadcast_ether_addr(hdr->addr1)) {
179 skb2 = dev_alloc_skb(skb->len + sizeof(*cb_hdr));
180 if (!skb2)
181 return -ENOMEM;
182
183 skb_put_data(skb2, skb->data, skb->len);
184
185 cb_hdr = skb_push(skb2, sizeof(*cb_hdr));
186 memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
187
188 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
189
190 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
191
192 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
193
194 cb_hdr->rate = 5;
195 cb_hdr->tx_flags = 0x0004;
196
197 skb2->dev = dev;
198 skb_reset_mac_header(skb2);
199 skb2->ip_summed = CHECKSUM_UNNECESSARY;
200 skb2->pkt_type = PACKET_OTHERHOST;
201 skb2->protocol = htons(ETH_P_802_2);
202 memset(skb2->cb, 0, sizeof(skb2->cb));
203
204 netif_rx(skb2);
205
206 return 0;
207 }
208 skb->dev = mon_priv->real_ndev;
209
210 ether_addr_copy(srcadd, hdr->addr2);
211 ether_addr_copy(bssid, hdr->addr3);
212 /*
213 * Identify if data or mgmt packet, if source address and bssid
214 * fields are equal send it to mgmt frames handler
215 */
216 if (!(memcmp(srcadd, bssid, 6))) {
217 ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
218 if (ret)
219 netdev_err(dev, "fail to mgmt tx\n");
220 dev_kfree_skb(skb);
221 } else {
222 ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
223 }
224
225 return ret;
226
227 drop:
228 dev_kfree_skb(skb);
229 return NETDEV_TX_OK;
230 }
231
232 static const struct net_device_ops wilc_wfi_netdev_ops = {
233 .ndo_start_xmit = wilc_wfi_mon_xmit,
234
235 };
236
wilc_wfi_init_mon_interface(struct wilc * wl,const char * name,struct net_device * real_dev)237 struct net_device *wilc_wfi_init_mon_interface(struct wilc *wl,
238 const char *name,
239 struct net_device *real_dev)
240 {
241 struct wilc_wfi_mon_priv *priv;
242
243 /* If monitor interface is already initialized, return it */
244 if (wl->monitor_dev)
245 return wl->monitor_dev;
246
247 wl->monitor_dev = alloc_etherdev(sizeof(struct wilc_wfi_mon_priv));
248 if (!wl->monitor_dev)
249 return NULL;
250
251 wl->monitor_dev->type = ARPHRD_IEEE80211_RADIOTAP;
252 strscpy(wl->monitor_dev->name, name, IFNAMSIZ);
253 wl->monitor_dev->netdev_ops = &wilc_wfi_netdev_ops;
254 wl->monitor_dev->needs_free_netdev = true;
255
256 if (register_netdevice(wl->monitor_dev)) {
257 netdev_err(real_dev, "register_netdevice failed\n");
258 free_netdev(wl->monitor_dev);
259 return NULL;
260 }
261 priv = netdev_priv(wl->monitor_dev);
262
263 priv->real_ndev = real_dev;
264
265 return wl->monitor_dev;
266 }
267
wilc_wfi_deinit_mon_interface(struct wilc * wl,bool rtnl_locked)268 void wilc_wfi_deinit_mon_interface(struct wilc *wl, bool rtnl_locked)
269 {
270 if (!wl->monitor_dev)
271 return;
272
273 if (rtnl_locked)
274 unregister_netdevice(wl->monitor_dev);
275 else
276 unregister_netdev(wl->monitor_dev);
277 wl->monitor_dev = NULL;
278 }
279