| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 142 |
static netdev_tx_t wilc_wfi_mon_xmit(struct sk_buff *skb, |
| 143 |
struct net_device *dev) |
| 144 |
{ |
| 145 |
u32 rtap_len, ret = 0; |
| 146 |
struct wilc_wfi_mon_priv *mon_priv; |
| 147 |
struct sk_buff *skb2; |
| 148 |
struct wilc_wfi_radiotap_cb_hdr *cb_hdr; |
| 149 |
u8 srcadd[ETH_ALEN]; |
| 150 |
u8 bssid[ETH_ALEN]; |
| 151 |
|
| 152 |
mon_priv = netdev_priv(dev); |
| 153 |
if (!mon_priv) |
| 154 |
return -EFAULT; |
| 155 |
|
| 156 |
rtap_len = ieee80211_get_radiotap_len(skb->data); |
| 157 |
if (skb->len < rtap_len) |
| 158 |
return -1; |
| 159 |
|
| 160 |
skb_pull(skb, rtap_len); |
| 161 |
|
| 162 |
if (skb->data[0] == 0xc0 && is_broadcast_ether_addr(&skb->data[4])) { |
| 163 |
skb2 = dev_alloc_skb(skb->len + sizeof(*cb_hdr)); |
| 164 |
if (!skb2) |
| 165 |
return -ENOMEM; |
| 166 |
|
| 167 |
skb_put_data(skb2, skb->data, skb->len); |
| 168 |
|
| 169 |
cb_hdr = skb_push(skb2, sizeof(*cb_hdr)); |
| 170 |
memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr)); |
| 171 |
|
| 172 |
cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */ |
| 173 |
|
| 174 |
cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr)); |
| 175 |
|
| 176 |
cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT); |
| 177 |
|
| 178 |
cb_hdr->rate = 5; |
| 179 |
cb_hdr->tx_flags = 0x0004; |
| 180 |
|
| 181 |
skb2->dev = dev; |
| 182 |
skb_reset_mac_header(skb2); |
| 183 |
skb2->ip_summed = CHECKSUM_UNNECESSARY; |
| 184 |
skb2->pkt_type = PACKET_OTHERHOST; |
| 185 |
skb2->protocol = htons(ETH_P_802_2); |
| 186 |
memset(skb2->cb, 0, sizeof(skb2->cb)); |
| 187 |
|
| 188 |
netif_rx(skb2); |
| 189 |
|
| 190 |
return 0; |
| 191 |
} |
| 192 |
skb->dev = mon_priv->real_ndev; |
| 193 |
|
| 194 |
ether_addr_copy(srcadd, &skb->data[10]); |
| 195 |
ether_addr_copy(bssid, &skb->data[16]); |
| 196 |
/* |
| 197 |
* Identify if data or mgmt packet, if source address and bssid |
| 198 |
* fields are equal send it to mgmt frames handler |
| 199 |
*/ |
| 200 |
if (!(memcmp(srcadd, bssid, 6))) { |
| 201 |
ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len); |
| 202 |
if (ret) |
| 203 |
netdev_err(dev, "fail to mgmt tx\n"); |
| 204 |
dev_kfree_skb(skb); |
| 205 |
} else { |
| 206 |
ret = wilc_mac_xmit(skb, mon_priv->real_ndev); |
| 207 |
} |
| 208 |
|
| 209 |
return ret; |
| 210 |
} |
| 211 |
|
| 212 |
static const struct net_device_ops wilc_wfi_netdev_ops = { |
| 213 |
.ndo_start_xmit = wilc_wfi_mon_xmit, |
| 214 |
|
| 215 |
}; |
| 216 |
|
| 217 |
struct net_device *wilc_wfi_init_mon_interface(struct wilc *wl, |
| 218 |
const char *name, |
| 219 |
struct net_device *real_dev) |
| 220 |
{ |
| 221 |
struct wilc_wfi_mon_priv *priv; |
| 222 |
|
| 223 |
/* If monitor interface is already initialized, return it */ |
| 224 |
if (wl->monitor_dev) |
| 225 |
return wl->monitor_dev; |
| 226 |
|
| 227 |
wl->monitor_dev = alloc_etherdev(sizeof(struct wilc_wfi_mon_priv)); |
| 228 |
if (!wl->monitor_dev) |
| 229 |
return NULL; |
| 230 |
|
| 231 |
wl->monitor_dev->type = ARPHRD_IEEE80211_RADIOTAP; |
| 232 |
strscpy(wl->monitor_dev->name, name, IFNAMSIZ); |
| 233 |
wl->monitor_dev->netdev_ops = &wilc_wfi_netdev_ops; |
| 234 |
wl->monitor_dev->needs_free_netdev = true; |
| 235 |
|
| 236 |
if (register_netdevice(wl->monitor_dev)) { |
| 237 |
netdev_err(real_dev, "register_netdevice failed\n"); |
| 238 |
free_netdev(wl->monitor_dev); |
| 239 |
return NULL; |
| 240 |
} |
| 241 |
priv = netdev_priv(wl->monitor_dev); |
| 242 |
|
| 243 |
priv->real_ndev = real_dev; |
| 244 |
|
| 245 |
return wl->monitor_dev; |
| 246 |
} |
| 247 |
|
| 248 |
void wilc_wfi_deinit_mon_interface(struct wilc *wl, bool rtnl_locked) |
| 249 |
{ |
| 250 |
if (!wl->monitor_dev) |
| 251 |
return; |
| 252 |
|
| 253 |
if (rtnl_locked) |
| 254 |
unregister_netdevice(wl->monitor_dev); |
| 255 |
else |
| 256 |
unregister_netdev(wl->monitor_dev); |
| 257 |
wl->monitor_dev = NULL; |
| 258 |
} |