1 // SPDX-License-Identifier: GPL-2.0
3 * Copyright (c) 2012 - 2018 Microchip Technology Inc., and its subsidiaries.
7 #include "wilc_wfi_cfgoperations.h"
9 struct wilc_wfi_radiotap_hdr
{
10 struct ieee80211_radiotap_header hdr
;
14 struct wilc_wfi_radiotap_cb_hdr
{
15 struct ieee80211_radiotap_header hdr
;
24 #define TX_RADIOTAP_PRESENT ((1 << IEEE80211_RADIOTAP_RATE) | \
25 (1 << IEEE80211_RADIOTAP_TX_FLAGS))
27 void wilc_wfi_monitor_rx(struct net_device
*mon_dev
, u8
*buff
, u32 size
)
29 u32 header
, pkt_offset
;
30 struct sk_buff
*skb
= NULL
;
31 struct wilc_wfi_radiotap_hdr
*hdr
;
32 struct wilc_wfi_radiotap_cb_hdr
*cb_hdr
;
37 if (!netif_running(mon_dev
))
41 memcpy(&header
, (buff
- HOST_HDR_OFFSET
), HOST_HDR_OFFSET
);
42 le32_to_cpus(&header
);
44 * The packet offset field contain info about what type of management
45 * the frame we are dealing with and ack status
47 pkt_offset
= GET_PKT_OFFSET(header
);
49 if (pkt_offset
& IS_MANAGMEMENT_CALLBACK
) {
50 /* hostapd callback mgmt frame */
52 skb
= dev_alloc_skb(size
+ sizeof(*cb_hdr
));
56 skb_put_data(skb
, buff
, size
);
58 cb_hdr
= skb_push(skb
, sizeof(*cb_hdr
));
59 memset(cb_hdr
, 0, sizeof(*cb_hdr
));
61 cb_hdr
->hdr
.it_version
= 0; /* PKTHDR_RADIOTAP_VERSION; */
63 cb_hdr
->hdr
.it_len
= cpu_to_le16(sizeof(*cb_hdr
));
65 cb_hdr
->hdr
.it_present
= cpu_to_le32(TX_RADIOTAP_PRESENT
);
69 if (pkt_offset
& IS_MGMT_STATUS_SUCCES
) {
71 cb_hdr
->tx_flags
= IEEE80211_RADIOTAP_F_TX_RTS
;
73 cb_hdr
->tx_flags
= IEEE80211_RADIOTAP_F_TX_FAIL
;
77 skb
= dev_alloc_skb(size
+ sizeof(*hdr
));
82 skb_put_data(skb
, buff
, size
);
83 hdr
= skb_push(skb
, sizeof(*hdr
));
84 memset(hdr
, 0, sizeof(struct wilc_wfi_radiotap_hdr
));
85 hdr
->hdr
.it_version
= 0; /* PKTHDR_RADIOTAP_VERSION; */
86 hdr
->hdr
.it_len
= cpu_to_le16(sizeof(*hdr
));
87 hdr
->hdr
.it_present
= cpu_to_le32
88 (1 << IEEE80211_RADIOTAP_RATE
);
93 skb_reset_mac_header(skb
);
94 skb
->ip_summed
= CHECKSUM_UNNECESSARY
;
95 skb
->pkt_type
= PACKET_OTHERHOST
;
96 skb
->protocol
= htons(ETH_P_802_2
);
97 memset(skb
->cb
, 0, sizeof(skb
->cb
));
102 struct tx_complete_mon_data
{
107 static void mgmt_tx_complete(void *priv
, int status
)
109 struct tx_complete_mon_data
*pv_data
= priv
;
111 * in case of fully hosting mode, the freeing will be done
112 * in response to the cfg packet
114 kfree(pv_data
->buff
);
119 static int mon_mgmt_tx(struct net_device
*dev
, const u8
*buf
, size_t len
)
121 struct tx_complete_mon_data
*mgmt_tx
= NULL
;
126 netif_stop_queue(dev
);
127 mgmt_tx
= kmalloc(sizeof(*mgmt_tx
), GFP_ATOMIC
);
131 mgmt_tx
->buff
= kmemdup(buf
, len
, GFP_ATOMIC
);
132 if (!mgmt_tx
->buff
) {
139 wilc_wlan_txq_add_mgmt_pkt(dev
, mgmt_tx
, mgmt_tx
->buff
, mgmt_tx
->size
,
142 netif_wake_queue(dev
);
146 static netdev_tx_t
wilc_wfi_mon_xmit(struct sk_buff
*skb
,
147 struct net_device
*dev
)
149 u32 rtap_len
, ret
= 0;
150 struct wilc_wfi_mon_priv
*mon_priv
;
151 struct sk_buff
*skb2
;
152 struct wilc_wfi_radiotap_cb_hdr
*cb_hdr
;
154 mon_priv
= netdev_priv(dev
);
158 rtap_len
= ieee80211_get_radiotap_len(skb
->data
);
159 if (skb
->len
< rtap_len
)
162 skb_pull(skb
, rtap_len
);
164 if (skb
->data
[0] == 0xc0 && is_broadcast_ether_addr(&skb
->data
[4])) {
165 skb2
= dev_alloc_skb(skb
->len
+ sizeof(*cb_hdr
));
169 skb_put_data(skb2
, skb
->data
, skb
->len
);
171 cb_hdr
= skb_push(skb2
, sizeof(*cb_hdr
));
172 memset(cb_hdr
, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr
));
174 cb_hdr
->hdr
.it_version
= 0; /* PKTHDR_RADIOTAP_VERSION; */
176 cb_hdr
->hdr
.it_len
= cpu_to_le16(sizeof(*cb_hdr
));
178 cb_hdr
->hdr
.it_present
= cpu_to_le32(TX_RADIOTAP_PRESENT
);
181 cb_hdr
->tx_flags
= 0x0004;
184 skb_reset_mac_header(skb2
);
185 skb2
->ip_summed
= CHECKSUM_UNNECESSARY
;
186 skb2
->pkt_type
= PACKET_OTHERHOST
;
187 skb2
->protocol
= htons(ETH_P_802_2
);
188 memset(skb2
->cb
, 0, sizeof(skb2
->cb
));
194 skb
->dev
= mon_priv
->real_ndev
;
196 memcpy(srcadd
, &skb
->data
[10], 6);
197 memcpy(bssid
, &skb
->data
[16], 6);
199 * Identify if data or mgmt packet, if source address and bssid
200 * fields are equal send it to mgmt frames handler
202 if (!(memcmp(srcadd
, bssid
, 6))) {
203 ret
= mon_mgmt_tx(mon_priv
->real_ndev
, skb
->data
, skb
->len
);
205 netdev_err(dev
, "fail to mgmt tx\n");
208 ret
= wilc_mac_xmit(skb
, mon_priv
->real_ndev
);
214 static const struct net_device_ops wilc_wfi_netdev_ops
= {
215 .ndo_start_xmit
= wilc_wfi_mon_xmit
,
219 struct net_device
*wilc_wfi_init_mon_interface(struct wilc
*wl
,
221 struct net_device
*real_dev
)
223 struct wilc_wfi_mon_priv
*priv
;
225 /*If monitor interface is already initialized, return it*/
227 return wl
->monitor_dev
;
229 wl
->monitor_dev
= alloc_etherdev(sizeof(struct wilc_wfi_mon_priv
));
230 if (!wl
->monitor_dev
)
233 wl
->monitor_dev
->type
= ARPHRD_IEEE80211_RADIOTAP
;
234 strncpy(wl
->monitor_dev
->name
, name
, IFNAMSIZ
);
235 wl
->monitor_dev
->name
[IFNAMSIZ
- 1] = 0;
236 wl
->monitor_dev
->netdev_ops
= &wilc_wfi_netdev_ops
;
238 if (register_netdevice(wl
->monitor_dev
)) {
239 netdev_err(real_dev
, "register_netdevice failed\n");
242 priv
= netdev_priv(wl
->monitor_dev
);
246 priv
->real_ndev
= real_dev
;
248 return wl
->monitor_dev
;
251 void wilc_wfi_deinit_mon_interface(struct wilc
*wl
)
253 if (!wl
->monitor_dev
)
256 unregister_netdev(wl
->monitor_dev
);
257 free_netdev(wl
->monitor_dev
);
258 wl
->monitor_dev
= NULL
;