drivers/staging/wilc1000/linux_mon.c

   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 "wilc_wfi_cfgoperations.h"
   8
   9 struct wilc_wfi_radiotap_hdr {
  10         struct ieee80211_radiotap_header hdr;
  11         u8 rate;
  12 } __packed;
  13
  14 struct wilc_wfi_radiotap_cb_hdr {
  15         struct ieee80211_radiotap_header hdr;
  16         u8 rate;
  17         u8 dump;
  18         u16 tx_flags;
  19 } __packed;
  20
  21 static struct net_device *wilc_wfi_mon; /* global monitor netdev */
  22
  23 static u8 srcadd[6];
  24 static u8 bssid[6];
  25
  26 #define IEEE80211_RADIOTAP_F_TX_RTS     0x0004  /* used rts/cts handshake */
  27 #define IEEE80211_RADIOTAP_F_TX_FAIL    0x0001  /* failed due to excessive*/
  28
  29 #define TX_RADIOTAP_PRESENT ((1 << IEEE80211_RADIOTAP_RATE) |   \
  30                              (1 << IEEE80211_RADIOTAP_TX_FLAGS))
  31
  32 void wilc_wfi_monitor_rx(u8 *buff, u32 size)
  33 {
  34         u32 header, pkt_offset;
  35         struct sk_buff *skb = NULL;
  36         struct wilc_wfi_radiotap_hdr *hdr;
  37         struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
  38
  39         if (!wilc_wfi_mon)
  40                 return;
  41
  42         if (!netif_running(wilc_wfi_mon))
  43                 return;
  44
  45         /* Get WILC header */
  46         memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
  47         le32_to_cpus(&header);
  48         /*
  49          * The packet offset field contain info about what type of management
  50          * the frame we are dealing with and ack status
  51          */
  52         pkt_offset = GET_PKT_OFFSET(header);
  53
  54         if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
  55                 /* hostapd callback mgmt frame */
  56
  57                 skb = dev_alloc_skb(size + sizeof(*cb_hdr));
  58                 if (!skb)
  59                         return;
  60
  61                 skb_put_data(skb, buff, size);
  62
  63                 cb_hdr = skb_push(skb, sizeof(*cb_hdr));
  64                 memset(cb_hdr, 0, sizeof(*cb_hdr));
  65
  66                 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
  67
  68                 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
  69
  70                 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
  71
  72                 cb_hdr->rate = 5;
  73
  74                 if (pkt_offset & IS_MGMT_STATUS_SUCCES) {
  75                         /* success */
  76                         cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
  77                 } else {
  78                         cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
  79                 }
  80
  81         } else {
  82                 skb = dev_alloc_skb(size + sizeof(*hdr));
  83
  84                 if (!skb)
  85                         return;
  86
  87                 skb_put_data(skb, buff, size);
  88                 hdr = skb_push(skb, sizeof(*hdr));
  89                 memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
  90                 hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
  91                 hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
  92                 hdr->hdr.it_present = cpu_to_le32
  93                                 (1 << IEEE80211_RADIOTAP_RATE);
  94                 hdr->rate = 5;
  95         }
  96
  97         skb->dev = wilc_wfi_mon;
  98         skb_reset_mac_header(skb);
  99         skb->ip_summed = CHECKSUM_UNNECESSARY;
 100         skb->pkt_type = PACKET_OTHERHOST;
 101         skb->protocol = htons(ETH_P_802_2);
 102         memset(skb->cb, 0, sizeof(skb->cb));
 103
 104         netif_rx(skb);
 105 }
 106
 107 struct tx_complete_mon_data {
 108         int size;
 109         void *buff;
 110 };
 111
 112 static void mgmt_tx_complete(void *priv, int status)
 113 {
 114         struct tx_complete_mon_data *pv_data = priv;
 115         /*
 116          * in case of fully hosting mode, the freeing will be done
 117          * in response to the cfg packet
 118          */
 119         kfree(pv_data->buff);
 120
 121         kfree(pv_data);
 122 }
 123
 124 static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
 125 {
 126         struct tx_complete_mon_data *mgmt_tx = NULL;
 127
 128         if (!dev)
 129                 return -EFAULT;
 130
 131         netif_stop_queue(dev);
 132         mgmt_tx = kmalloc(sizeof(*mgmt_tx), GFP_ATOMIC);
 133         if (!mgmt_tx)
 134                 return -ENOMEM;
 135
 136         mgmt_tx->buff = kmemdup(buf, len, GFP_ATOMIC);
 137         if (!mgmt_tx->buff) {
 138                 kfree(mgmt_tx);
 139                 return -ENOMEM;
 140         }
 141
 142         mgmt_tx->size = len;
 143
 144         wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
 145                                    mgmt_tx_complete);
 146
 147         netif_wake_queue(dev);
 148         return 0;
 149 }
 150
 151 static netdev_tx_t wilc_wfi_mon_xmit(struct sk_buff *skb,
 152                                      struct net_device *dev)
 153 {
 154         u32 rtap_len, ret = 0;
 155         struct wilc_wfi_mon_priv  *mon_priv;
 156         struct sk_buff *skb2;
 157         struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
 158
 159         if (!wilc_wfi_mon)
 160                 return -EFAULT;
 161
 162         mon_priv = netdev_priv(wilc_wfi_mon);
 163         if (!mon_priv)
 164                 return -EFAULT;
 165         rtap_len = ieee80211_get_radiotap_len(skb->data);
 166         if (skb->len < rtap_len)
 167                 return -1;
 168
 169         skb_pull(skb, rtap_len);
 170
 171         if (skb->data[0] == 0xc0 && is_broadcast_ether_addr(&skb->data[4])) {
 172                 skb2 = dev_alloc_skb(skb->len + sizeof(*cb_hdr));
 173                 if (!skb2)
 174                         return -ENOMEM;
 175
 176                 skb_put_data(skb2, skb->data, skb->len);
 177
 178                 cb_hdr = skb_push(skb2, sizeof(*cb_hdr));
 179                 memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
 180
 181                 cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
 182
 183                 cb_hdr->hdr.it_len = cpu_to_le16(sizeof(*cb_hdr));
 184
 185                 cb_hdr->hdr.it_present = cpu_to_le32(TX_RADIOTAP_PRESENT);
 186
 187                 cb_hdr->rate = 5;
 188                 cb_hdr->tx_flags = 0x0004;
 189
 190                 skb2->dev = wilc_wfi_mon;
 191                 skb_reset_mac_header(skb2);
 192                 skb2->ip_summed = CHECKSUM_UNNECESSARY;
 193                 skb2->pkt_type = PACKET_OTHERHOST;
 194                 skb2->protocol = htons(ETH_P_802_2);
 195                 memset(skb2->cb, 0, sizeof(skb2->cb));
 196
 197                 netif_rx(skb2);
 198
 199                 return 0;
 200         }
 201         skb->dev = mon_priv->real_ndev;
 202
 203         memcpy(srcadd, &skb->data[10], 6);
 204         memcpy(bssid, &skb->data[16], 6);
 205         /*
 206          * Identify if data or mgmt packet, if source address and bssid
 207          * fields are equal send it to mgmt frames handler
 208          */
 209         if (!(memcmp(srcadd, bssid, 6))) {
 210                 ret = mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
 211                 if (ret)
 212                         netdev_err(dev, "fail to mgmt tx\n");
 213                 dev_kfree_skb(skb);
 214         } else {
 215                 ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
 216         }
 217
 218         return ret;
 219 }
 220
 221 static const struct net_device_ops wilc_wfi_netdev_ops = {
 222         .ndo_start_xmit         = wilc_wfi_mon_xmit,
 223
 224 };
 225
 226 struct net_device *wilc_wfi_init_mon_interface(const char *name,
 227                                                struct net_device *real_dev)
 228 {
 229         struct wilc_wfi_mon_priv *priv;
 230
 231         /*If monitor interface is already initialized, return it*/
 232         if (wilc_wfi_mon)
 233                 return wilc_wfi_mon;
 234
 235         wilc_wfi_mon = alloc_etherdev(sizeof(struct wilc_wfi_mon_priv));
 236         if (!wilc_wfi_mon)
 237                 return NULL;
 238         wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP;
 239         strncpy(wilc_wfi_mon->name, name, IFNAMSIZ);
 240         wilc_wfi_mon->name[IFNAMSIZ - 1] = 0;
 241         wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops;
 242
 243         if (register_netdevice(wilc_wfi_mon)) {
 244                 netdev_err(real_dev, "register_netdevice failed\n");
 245                 return NULL;
 246         }
 247         priv = netdev_priv(wilc_wfi_mon);
 248         if (!priv)
 249                 return NULL;
 250
 251         priv->real_ndev = real_dev;
 252
 253         return wilc_wfi_mon;
 254 }
 255
 256 int wilc_wfi_deinit_mon_interface(void)
 257 {
 258         bool rollback_lock = false;
 259
 260         if (wilc_wfi_mon) {
 261                 if (rtnl_is_locked()) {
 262                         rtnl_unlock();
 263                         rollback_lock = true;
 264                 }
 265                 unregister_netdev(wilc_wfi_mon);
 266
 267                 if (rollback_lock) {
 268                         rtnl_lock();
 269                         rollback_lock = false;
 270                 }
 271                 wilc_wfi_mon = NULL;
 272         }
 273         return 0;
 274 }