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 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 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 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