drivers/staging/wilc1000/linux_mon.c

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