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