drivers/staging/wilc1000/linux_mon.c

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