iwlwifi: mvm: add multicast filtering support

[deliverable/linux.git] / drivers / net / wireless / iwlwifi / mvm / mac80211.c

diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 9a27f57c5e98466054206305f2751a8728b2f1bb..01b58fcd89134b5145144fe031ff0b10f7c8bbcc 100644 (file)
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -755,26 +755,109 @@ static int iwl_mvm_mac_config(struct ieee80211_hw *hw, u32 changed)
        return 0;
 }
 
+struct iwl_mvm_mc_iter_data {
+       struct iwl_mvm *mvm;
+       int port_id;
+};
+
+static void iwl_mvm_mc_iface_iterator(void *_data, u8 *mac,
+                                     struct ieee80211_vif *vif)
+{
+       struct iwl_mvm_mc_iter_data *data = _data;
+       struct iwl_mvm *mvm = data->mvm;
+       struct iwl_mcast_filter_cmd *cmd = mvm->mcast_filter_cmd;
+       int ret, len;
+
+       /* if we don't have free ports, mcast frames will be dropped */
+       if (WARN_ON_ONCE(data->port_id >= MAX_PORT_ID_NUM))
+               return;
+
+       if (vif->type != NL80211_IFTYPE_STATION ||
+           !vif->bss_conf.assoc)
+               return;
+
+       cmd->port_id = data->port_id++;
+       memcpy(cmd->bssid, vif->bss_conf.bssid, ETH_ALEN);
+       len = roundup(sizeof(*cmd) + cmd->count * ETH_ALEN, 4);
+
+       ret = iwl_mvm_send_cmd_pdu(mvm, MCAST_FILTER_CMD, CMD_SYNC, len, cmd);
+       if (ret)
+               IWL_ERR(mvm, "mcast filter cmd error. ret=%d\n", ret);
+}
+
+static void iwl_mvm_recalc_multicast(struct iwl_mvm *mvm)
+{
+       struct iwl_mvm_mc_iter_data iter_data = {
+               .mvm = mvm,
+       };
+
+       lockdep_assert_held(&mvm->mutex);
+
+       if (WARN_ON_ONCE(!mvm->mcast_filter_cmd))
+               return;
+
+       ieee80211_iterate_active_interfaces(
+               mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+               iwl_mvm_mc_iface_iterator, &iter_data);
+}
+
+static u64 iwl_mvm_prepare_multicast(struct ieee80211_hw *hw,
+                                    struct netdev_hw_addr_list *mc_list)
+{
+       struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+       struct iwl_mcast_filter_cmd *cmd;
+       struct netdev_hw_addr *addr;
+       int addr_count = netdev_hw_addr_list_count(mc_list);
+       bool pass_all = false;
+       int len;
+
+       if (addr_count > MAX_MCAST_FILTERING_ADDRESSES) {
+               pass_all = true;
+               addr_count = 0;
+       }
+
+       len = roundup(sizeof(*cmd) + addr_count * ETH_ALEN, 4);
+       cmd = kzalloc(len, GFP_ATOMIC);
+       if (!cmd)
+               return 0;
+
+       if (pass_all) {
+               cmd->pass_all = 1;
+               return (u64)(unsigned long)cmd;
+       }
+
+       netdev_hw_addr_list_for_each(addr, mc_list) {
+               IWL_DEBUG_MAC80211(mvm, "mcast addr (%d): %pM\n",
+                                  cmd->count, addr->addr);
+               memcpy(&cmd->addr_list[cmd->count * ETH_ALEN],
+                      addr->addr, ETH_ALEN);
+               cmd->count++;
+       }
+
+       return (u64)(unsigned long)cmd;
+}
+
 static void iwl_mvm_configure_filter(struct ieee80211_hw *hw,
                                     unsigned int changed_flags,
                                     unsigned int *total_flags,
                                     u64 multicast)
 {
-       *total_flags = 0;
-}
+       struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+       struct iwl_mcast_filter_cmd *cmd = (void *)(unsigned long)multicast;
 
-static int iwl_mvm_configure_mcast_filter(struct iwl_mvm *mvm,
-                                         struct ieee80211_vif *vif)
-{
-       struct iwl_mcast_filter_cmd mcast_filter_cmd = {
-               .pass_all = 1,
-       };
+       mutex_lock(&mvm->mutex);
 
-       memcpy(mcast_filter_cmd.bssid, vif->bss_conf.bssid, ETH_ALEN);
+       /* replace previous configuration */
+       kfree(mvm->mcast_filter_cmd);
+       mvm->mcast_filter_cmd = cmd;
 
-       return iwl_mvm_send_cmd_pdu(mvm, MCAST_FILTER_CMD, CMD_SYNC,
-                                   sizeof(mcast_filter_cmd),
-                                   &mcast_filter_cmd);
+       if (!cmd)
+               goto out;
+
+       iwl_mvm_recalc_multicast(mvm);
+out:
+       mutex_unlock(&mvm->mutex);
+       *total_flags = 0;
 }
 
 static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
@@ -797,7 +880,6 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
                                IWL_ERR(mvm, "failed to update quotas\n");
                                return;
                        }
-                       iwl_mvm_configure_mcast_filter(mvm, vif);
 
                        if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART,
                                     &mvm->status)) {
@@ -841,6 +923,8 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
                                IWL_ERR(mvm, "failed to update quotas\n");
                }
 
+               iwl_mvm_recalc_multicast(mvm);
+
                /* reset rssi values */
                mvmvif->bf_data.ave_beacon_signal = 0;
 
@@ -1764,6 +1848,7 @@ struct ieee80211_ops iwl_mvm_hw_ops = {
        .add_interface = iwl_mvm_mac_add_interface,
        .remove_interface = iwl_mvm_mac_remove_interface,
        .config = iwl_mvm_mac_config,
+       .prepare_multicast = iwl_mvm_prepare_multicast,
        .configure_filter = iwl_mvm_configure_filter,
        .bss_info_changed = iwl_mvm_bss_info_changed,
        .hw_scan = iwl_mvm_mac_hw_scan,