linux/drivers/staging/wilc1000/linux_mon.c
<<
>>
Prefs 
   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
  13struct wilc_wfi_radiotap_hdr {
  14        struct ieee80211_radiotap_header hdr;
  15        u8 rate;
  16} __packed;
  17
  18struct wilc_wfi_radiotap_cb_hdr {
  19        struct ieee80211_radiotap_header hdr;
  20        u8 rate;
  21        u8 dump;
  22        u16 tx_flags;
  23} __packed;
  24
  25static struct net_device *wilc_wfi_mon; /* global monitor netdev */
  26
  27static u8 srcadd[6];
  28static u8 bssid[6];
  29static 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
  47void 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                skb_put_data(skb, buff, size);
  76
  77                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                skb_put_data(skb, buff, size);
 104                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_reset_mac_header(skb);
 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
 123struct tx_complete_mon_data {
 124        int size;
 125        void *buff;
 126};
 127
 128static 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
 140static 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 */
 177static 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                skb_put_data(skb2, skb->data, skb->len);
 204
 205                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_reset_mac_header(skb2);
 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
 249static 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 */
 263struct 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 */
 304int 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}
 323
Hosted by Missing Link Electronics. The original LXR software by the LXR community, this experimental version by lxr@linux.no.