microchip/wilc1000/mon.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (c) 2012 - 2018 Microchip Technology Inc., and its subsidiaries.
0004  * All rights reserved.
0005  */
0006
0007 #include "cfg80211.h"
0008
0009 struct wilc_wfi_radiotap_hdr {
0010     struct ieee80211_radiotap_header hdr;
0011     u8 rate;
0012 } __packed;
0013
0014 struct wilc_wfi_radiotap_cb_hdr {
0015     struct ieee80211_radiotap_header hdr;
0016     u8 rate;
0017     u8 dump;
0018     u16 tx_flags;
0019 } __packed;
0020
0021 #define TX_RADIOTAP_PRESENT ((1 << IEEE80211_RADIOTAP_RATE) |   \
0022                  (1 << IEEE80211_RADIOTAP_TX_FLAGS))
0023
0024 void wilc_wfi_monitor_rx(struct net_device *mon_dev, u8 *buff, u32 size)
0025 {
0026     u32 header, pkt_offset;
0027     struct sk_buff *skb = NULL;
0028     struct wilc_wfi_radiotap_hdr *hdr;
0029     struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
0030
0031     if (!mon_dev)
0032         return;
0033
0034     if (!netif_running(mon_dev))
0035         return;
0036
0037     /* Get WILC header */
0038     header = get_unaligned_le32(buff - HOST_HDR_OFFSET);
0039     /*
0040      * The packet offset field contain info about what type of management
0041      * the frame we are dealing with and ack status
0042      */
0043     pkt_offset = FIELD_GET(WILC_PKT_HDR_OFFSET_FIELD, header);
0044
0045     if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
0046         /* hostapd callback mgmt frame */
0047
0048         skb = dev_alloc_skb(size + sizeof(*cb_hdr));
0049         if (!skb)
0050             return;
0051
0052         skb_put_data(skb, buff, size);
0053
0054         cb_hdr = skb_push(skb, sizeof(*cb_hdr));
0055         memset(cb_hdr, 0, sizeof(*cb_hdr));
0056
0057         cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
0058
0059         cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
0060
0061         cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
0062
0063         cb_hdr->rate = 5;
0064
0065         if (pkt_offset & IS_MGMT_STATUS_SUCCES) {
0066             /* success */
0067             cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
0068         } else {
0069             cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
0070         }
0071
0072     } else {
0073         skb = dev_alloc_skb(size + sizeof(*hdr));
0074
0075         if (!skb)
0076             return;
0077
0078         skb_put_data(skb, buff, size);
0079         hdr = skb_push(skb, sizeof(*hdr));
0080         memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
0081         hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
0082         hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
0083         hdr->hdr.it_present = cpu_to_le32
0084                 (1 << IEEE80211_RADIOTAP_RATE);
0085         hdr->rate = 5;
0086     }
0087
0088     skb->dev = mon_dev;
0089     skb_reset_mac_header(skb);
0090     skb->ip_summed = CHECKSUM_UNNECESSARY;
0091     skb->pkt_type = PACKET_OTHERHOST;
0092     skb->protocol = htons(ETH_P_802_2);
0093     memset(skb->cb, 0, sizeof(skb->cb));
0094
0095     netif_rx(skb);
0096 }
0097
0098 struct tx_complete_mon_data {
0099     int size;
0100     void *buff;
0101 };
0102
0103 static void mgmt_tx_complete(void *priv, int status)
0104 {
0105     struct tx_complete_mon_data *pv_data = priv;
0106     /*
0107      * in case of fully hosting mode, the freeing will be done
0108      * in response to the cfg packet
0109      */
0110     kfree(pv_data->buff);
0111
0112     kfree(pv_data);
0113 }
0114
0115 static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
0116 {
0117     struct tx_complete_mon_data *mgmt_tx = NULL;
0118
0119     if (!dev)
0120         return -EFAULT;
0121
0122     netif_stop_queue(dev);
0123     mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC);
0124     if (!mgmt_tx)
0125         return -ENOMEM;
0126
0127     mgmt_tx->buff = kmemdup(buf, len, GFP_ATOMIC);
0128     if (!mgmt_tx->buff) {
0129         kfree(mgmt_tx);
0130         return -ENOMEM;
0131     }
0132
0133     mgmt_tx->size = len;
0134
0135     wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
0136                    mgmt_tx_complete);
0137
0138     netif_wake_queue(dev);
0139     return 0;
0140 }
0141
0142 static netdev_tx_t wilc_wfi_mon_xmit(struct sk_buff *skb,
0143                      struct net_device *dev)
0144 {
0145     u32 rtap_len, ret = 0;
0146     struct wilc_wfi_mon_priv  *mon_priv;
0147     struct sk_buff *skb2;
0148     struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
0149     u8 srcadd[ETH_ALEN];
0150     u8 bssid[ETH_ALEN];
0151
0152     mon_priv = netdev_priv(dev);
0153     if (!mon_priv)
0154         return -EFAULT;
0155
0156     rtap_len = ieee80211_get_radiotap_len(skb->data);
0157     if (skb->len < rtap_len)
0158         return -1;
0159
0160     skb_pull(skb, rtap_len);
0161
0162     if (skb->data[0] == 0xc0 && is_broadcast_ether_addr(&skb->data[4])) {
0163         skb2 = dev_alloc_skb(skb->len + sizeof(*cb_hdr));
0164         if (!skb2)
0165             return -ENOMEM;
0166
0167         skb_put_data(skb2, skb->data, skb->len);
0168
0169         cb_hdr = skb_push(skb2, sizeof(*cb_hdr));
0170         memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
0171
0172         cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
0173
0174         cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
0175
0176         cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
0177
0178         cb_hdr->rate = 5;
0179         cb_hdr->tx_flags = 0x0004;
0180
0181         skb2->dev = dev;
0182         skb_reset_mac_header(skb2);
0183         skb2->ip_summed = CHECKSUM_UNNECESSARY;
0184         skb2->pkt_type = PACKET_OTHERHOST;
0185         skb2->protocol = htons(ETH_P_802_2);
0186         memset(skb2->cb, 0, sizeof(skb2->cb));
0187
0188         netif_rx(skb2);
0189
0190         return 0;
0191     }
0192     skb->dev = mon_priv->real_ndev;
0193
0194     ether_addr_copy(srcadd, &skb->data[10]);
0195     ether_addr_copy(bssid, &skb->data[16]);
0196     /*
0197      * Identify if data or mgmt packet, if source address and bssid
0198      * fields are equal send it to mgmt frames handler
0199      */
0200     if (!(memcmp(srcadd, bssid, 6))) {
0201         ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
0202         if (ret)
0203             netdev_err(dev, "fail to mgmt tx\n");
0204         dev_kfree_skb(skb);
0205     } else {
0206         ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
0207     }
0208
0209     return ret;
0210 }
0211
0212 static const struct net_device_ops wilc_wfi_netdev_ops = {
0213     .ndo_start_xmit         = wilc_wfi_mon_xmit,
0214
0215 };
0216
0217 struct net_device *wilc_wfi_init_mon_interface(struct wilc *wl,
0218                            const char *name,
0219                            struct net_device *real_dev)
0220 {
0221     struct wilc_wfi_mon_priv *priv;
0222
0223     /* If monitor interface is already initialized, return it */
0224     if (wl->monitor_dev)
0225         return wl->monitor_dev;
0226
0227     wl->monitor_dev = alloc_etherdev(sizeof(struct wilc_wfi_mon_priv));
0228     if (!wl->monitor_dev)
0229         return NULL;
0230
0231     wl->monitor_dev->type = ARPHRD_IEEE80211_RADIOTAP;
0232     strlcpy(wl->monitor_dev->name, name, IFNAMSIZ);
0233     wl->monitor_dev->netdev_ops = &wilc_wfi_netdev_ops;
0234     wl->monitor_dev->needs_free_netdev = true;
0235
0236     if (register_netdevice(wl->monitor_dev)) {
0237         netdev_err(real_dev, "register_netdevice failed\n");
0238         free_netdev(wl->monitor_dev);
0239         return NULL;
0240     }
0241     priv = netdev_priv(wl->monitor_dev);
0242
0243     priv->real_ndev = real_dev;
0244
0245     return wl->monitor_dev;
0246 }
0247
0248 void wilc_wfi_deinit_mon_interface(struct wilc *wl, bool rtnl_locked)
0249 {
0250     if (!wl->monitor_dev)
0251         return;
0252
0253     if (rtnl_locked)
0254         unregister_netdevice(wl->monitor_dev);
0255     else
0256         unregister_netdev(wl->monitor_dev);
0257     wl->monitor_dev = NULL;
0258 }