259 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (c) 2012 - 2018 Microchip Technology Inc., and its subsidiaries.4 * All rights reserved.5 */6 7#include "cfg80211.h"8 9struct wilc_wfi_radiotap_hdr {10 struct ieee80211_radiotap_header_fixed hdr;11 u8 rate;12} __packed;13 14struct 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 24void 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 management41 * the frame we are dealing with and ack status42 */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_le3284 (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 98struct tx_complete_mon_data {99 int size;100 void *buff;101};102 103static 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 done108 * in response to the cfg packet109 */110 kfree(pv_data->buff);111 112 kfree(pv_data);113}114 115static 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(sizeof(*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 142static 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 bssid198 * fields are equal send it to mgmt frames handler199 */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 212static const struct net_device_ops wilc_wfi_netdev_ops = {213 .ndo_start_xmit = wilc_wfi_mon_xmit,214 215};216 217struct 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 248void 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 else256 unregister_netdev(wl->monitor_dev);257 wl->monitor_dev = NULL;258}259