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