6 files changed, 96 insertions, 11 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 56319b51d170..576b86f81d1b 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -36,6 +36,24 @@ static bool nl80211_type_check(enum nl80211_iftype type)
         }
 }
 
+static bool nl80211_params_check(enum nl80211_iftype type,
+                                 struct vif_params *params)
+{
+        if (!nl80211_type_check(type))
+                return false;
+
+        if (params->use_4addr > 0) {
+                switch(type) {
+                case NL80211_IFTYPE_AP_VLAN:
+                case NL80211_IFTYPE_STATION:
+                        break;
+                default:
+                        return false;
+                }
+        }
+        return true;
+}
+
 static int ieee80211_add_iface(struct wiphy *wiphy, char *name,
                                enum nl80211_iftype type, u32 *flags,
                                struct vif_params *params)
@@ -45,7 +63,7 @@ static int ieee80211_add_iface(struct wiphy *wiphy, char *name,
         struct ieee80211_sub_if_data *sdata;
         int err;
 
-        if (!nl80211_type_check(type))
+        if (!nl80211_params_check(type, params))
                 return -EINVAL;
 
         err = ieee80211_if_add(local, name, &dev, type, params);
@@ -75,7 +93,7 @@ static int ieee80211_change_iface(struct wiphy *wiphy,
         if (netif_running(dev))
                 return -EBUSY;
 
-        if (!nl80211_type_check(type))
+        if (!nl80211_params_check(type, params))
                 return -EINVAL;
 
         sdata = IEEE80211_DEV_TO_SUB_IF(dev);
@@ -89,6 +107,9 @@ static int ieee80211_change_iface(struct wiphy *wiphy,
                                             params->mesh_id_len,
                                             params->mesh_id);
 
+        if (params->use_4addr >= 0)
+                sdata->use_4addr = !!params->use_4addr;
+
         if (sdata->vif.type != NL80211_IFTYPE_MONITOR || !flags)
                 return 0;
 
@@ -806,6 +827,13 @@ static int ieee80211_change_station(struct wiphy *wiphy,
                         return -EINVAL;
                 }
 
+                if (vlansdata->use_4addr) {
+                        if (vlansdata->u.vlan.sta)
+                                return -EBUSY;
+
+                        rcu_assign_pointer(vlansdata->u.vlan.sta, sta);
+                }
+
                 sta->sdata = vlansdata;
                 ieee80211_send_layer2_update(sta);
         }
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 1f4f88a8f80c..19b0c128d940 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -208,6 +208,9 @@ struct ieee80211_if_wds {
 
 struct ieee80211_if_vlan {
         struct list_head list;
+
+        /* used for all tx if the VLAN is configured to 4-addr mode */
+        struct sta_info *sta;
 };
 
 struct mesh_stats {
@@ -457,6 +460,8 @@ struct ieee80211_sub_if_data {
         int force_unicast_rateidx; /* forced TX rateidx for unicast frames */
         int max_ratectrl_rateidx; /* max TX rateidx for rate control */
 
+        bool use_4addr; /* use 4-address frames */
+
         union {
                 struct ieee80211_if_ap ap;
                 struct ieee80211_if_wds wds;
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 8495161b99b8..1f02b0610e82 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -752,6 +752,7 @@ int ieee80211_if_change_type(struct ieee80211_sub_if_data *sdata,
                 ieee80211_mandatory_rates(sdata->local,
                         sdata->local->hw.conf.channel->band);
         sdata->drop_unencrypted = 0;
+        sdata->use_4addr = 0;
 
         return 0;
 }
@@ -819,6 +820,9 @@ int ieee80211_if_add(struct ieee80211_local *local, const char *name,
                                             params->mesh_id_len,
                                             params->mesh_id);
 
+        if (params && params->use_4addr >= 0)
+                sdata->use_4addr = !!params->use_4addr;
+
         mutex_lock(&local->iflist_mtx);
         list_add_tail_rcu(&sdata->list, &local->interfaces);
         mutex_unlock(&local->iflist_mtx);
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 4066570dd414..76478362a538 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1181,6 +1181,13 @@ __ieee80211_data_to_8023(struct ieee80211_rx_data *rx)
 {
         struct net_device *dev = rx->dev;
         struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
+
+        if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN && !sdata->use_4addr &&
+            ieee80211_has_a4(hdr->frame_control))
+                return -1;
+        if (sdata->use_4addr && is_multicast_ether_addr(hdr->addr1))
+                return -1;
 
         return ieee80211_data_to_8023(rx->skb, dev->dev_addr, sdata->vif.type);
 }
@@ -1534,6 +1541,7 @@ ieee80211_rx_h_data(struct ieee80211_rx_data *rx)
 {
         struct net_device *dev = rx->dev;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
         __le16 fc = hdr->frame_control;
         int err;
 
@@ -1543,6 +1551,14 @@ ieee80211_rx_h_data(struct ieee80211_rx_data *rx)
         if (unlikely(!ieee80211_is_data_present(hdr->frame_control)))
                 return RX_DROP_MONITOR;
 
+        /*
+         * Allow the cooked monitor interface of an AP to see 4-addr frames so
+         * that a 4-addr station can be detected and moved into a separate VLAN
+         */
+        if (ieee80211_has_a4(hdr->frame_control) &&
+            sdata->vif.type == NL80211_IFTYPE_AP)
+                return RX_DROP_MONITOR;
+
         err = __ieee80211_data_to_8023(rx);
         if (unlikely(err))
                 return RX_DROP_UNUSABLE;
@@ -1983,7 +1999,7 @@ static int prepare_for_handlers(struct ieee80211_sub_if_data *sdata,
 
         switch (sdata->vif.type) {
         case NL80211_IFTYPE_STATION:
-                if (!bssid)
+                if (!bssid && !sdata->use_4addr)
                         return 0;
                 if (!multicast &&
                     compare_ether_addr(sdata->dev->dev_addr, hdr->addr1) != 0) {
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index be59456e8a42..396a94806de9 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -509,6 +509,9 @@ static void __sta_info_unlink(struct sta_info **sta)
         local->num_sta--;
         local->sta_generation++;
 
+        if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
+                rcu_assign_pointer(sdata->u.vlan.sta, NULL);
+
         if (local->ops->sta_notify) {
                 if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
                         sdata = container_of(sdata->bss,
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index bfaa43e096d2..2f3345c5f7cf 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -1051,7 +1051,10 @@ ieee80211_tx_prepare(struct ieee80211_sub_if_data *sdata,
 
         hdr = (struct ieee80211_hdr *) skb->data;
 
-        tx->sta = sta_info_get(local, hdr->addr1);
+        if ((sdata->vif.type == NL80211_IFTYPE_AP_VLAN) && sdata->use_4addr)
+                tx->sta = rcu_dereference(sdata->u.vlan.sta);
+        if (!tx->sta)
+                tx->sta = sta_info_get(local, hdr->addr1);
 
         if (tx->sta && ieee80211_is_data_qos(hdr->frame_control) &&
             (local->hw.flags & IEEE80211_HW_AMPDU_AGGREGATION)) {
@@ -1613,7 +1616,7 @@ netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb,
         const u8 *encaps_data;
         int encaps_len, skip_header_bytes;
         int nh_pos, h_pos;
-        struct sta_info *sta;
+        struct sta_info *sta = NULL;
         u32 sta_flags = 0;
 
         if (unlikely(skb->len < ETH_HLEN)) {
@@ -1630,8 +1633,25 @@ netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb,
         fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
 
         switch (sdata->vif.type) {
-        case NL80211_IFTYPE_AP:
         case NL80211_IFTYPE_AP_VLAN:
+                rcu_read_lock();
+                if (sdata->use_4addr)
+                        sta = rcu_dereference(sdata->u.vlan.sta);
+                if (sta) {
+                        fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
+                        /* RA TA DA SA */
+                        memcpy(hdr.addr1, sta->sta.addr, ETH_ALEN);
+                        memcpy(hdr.addr2, dev->dev_addr, ETH_ALEN);
+                        memcpy(hdr.addr3, skb->data, ETH_ALEN);
+                        memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
+                        hdrlen = 30;
+                        sta_flags = get_sta_flags(sta);
+                }
+                rcu_read_unlock();
+                if (sta)
+                        break;
+                /* fall through */
+        case NL80211_IFTYPE_AP:
                 fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
                 /* DA BSSID SA */
                 memcpy(hdr.addr1, skb->data, ETH_ALEN);
@@ -1705,12 +1725,21 @@ netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb,
                 break;
 #endif
         case NL80211_IFTYPE_STATION:
-                fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
-                /* BSSID SA DA */
                 memcpy(hdr.addr1, sdata->u.mgd.bssid, ETH_ALEN);
-                memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
-                memcpy(hdr.addr3, skb->data, ETH_ALEN);
-                hdrlen = 24;
+                if (sdata->use_4addr && ethertype != ETH_P_PAE) {
+                        fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
+                        /* RA TA DA SA */
+                        memcpy(hdr.addr2, dev->dev_addr, ETH_ALEN);
+                        memcpy(hdr.addr3, skb->data, ETH_ALEN);
+                        memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
+                        hdrlen = 30;
+                } else {
+                        fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
+                        /* BSSID SA DA */
+                        memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
+                        memcpy(hdr.addr3, skb->data, ETH_ALEN);
+                        hdrlen = 24;
+                }
                 break;
         case NL80211_IFTYPE_ADHOC:
                 /* DA SA BSSID */