1 files changed, 85 insertions, 0 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 914aef7e7afd..51622333d460 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -790,6 +790,48 @@ static int ieee80211_set_probe_resp(struct ieee80211_sub_if_data *sdata,
         return 0;
 }
 
+static int ieee80211_set_ftm_responder_params(
+                                struct ieee80211_sub_if_data *sdata,
+                                const u8 *lci, size_t lci_len,
+                                const u8 *civicloc, size_t civicloc_len)
+{
+        struct ieee80211_ftm_responder_params *new, *old;
+        struct ieee80211_bss_conf *bss_conf;
+        u8 *pos;
+        int len;
+
+        if ((!lci || !lci_len) && (!civicloc || !civicloc_len))
+                return 1;
+
+        bss_conf = &sdata->vif.bss_conf;
+        old = bss_conf->ftmr_params;
+        len = lci_len + civicloc_len;
+
+        new = kzalloc(sizeof(*new) + len, GFP_KERNEL);
+        if (!new)
+                return -ENOMEM;
+
+        pos = (u8 *)(new + 1);
+        if (lci_len) {
+                new->lci_len = lci_len;
+                new->lci = pos;
+                memcpy(pos, lci, lci_len);
+                pos += lci_len;
+        }
+
+        if (civicloc_len) {
+                new->civicloc_len = civicloc_len;
+                new->civicloc = pos;
+                memcpy(pos, civicloc, civicloc_len);
+                pos += civicloc_len;
+        }
+
+        bss_conf->ftmr_params = new;
+        kfree(old);
+
+        return 0;
+}
+
 static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
                                    struct cfg80211_beacon_data *params,
                                    const struct ieee80211_csa_settings *csa)
@@ -863,6 +905,20 @@ static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
         if (err == 0)
                 changed |= BSS_CHANGED_AP_PROBE_RESP;
 
+        if (params->ftm_responder != -1) {
+                sdata->vif.bss_conf.ftm_responder = params->ftm_responder;
+                err = ieee80211_set_ftm_responder_params(sdata,
+                                                         params->lci,
+                                                         params->lci_len,
+                                                         params->civicloc,
+                                                         params->civicloc_len);
+
+                if (err < 0)
+                        return err;
+
+                changed |= BSS_CHANGED_FTM_RESPONDER;
+        }
+
         rcu_assign_pointer(sdata->u.ap.beacon, new);
 
         if (old)
@@ -1063,6 +1119,9 @@ static int ieee80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
                 kfree_rcu(old_probe_resp, rcu_head);
         sdata->u.ap.driver_smps_mode = IEEE80211_SMPS_OFF;
 
+        kfree(sdata->vif.bss_conf.ftmr_params);
+        sdata->vif.bss_conf.ftmr_params = NULL;
+
         __sta_info_flush(sdata, true);
         ieee80211_free_keys(sdata, true);
 
@@ -2875,6 +2934,20 @@ cfg80211_beacon_dup(struct cfg80211_beacon_data *beacon)
                 memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
                 pos += beacon->probe_resp_len;
         }
+        if (beacon->ftm_responder)
+                new_beacon->ftm_responder = beacon->ftm_responder;
+        if (beacon->lci) {
+                new_beacon->lci_len = beacon->lci_len;
+                new_beacon->lci = pos;
+                memcpy(pos, beacon->lci, beacon->lci_len);
+                pos += beacon->lci_len;
+        }
+        if (beacon->civicloc) {
+                new_beacon->civicloc_len = beacon->civicloc_len;
+                new_beacon->civicloc = pos;
+                memcpy(pos, beacon->civicloc, beacon->civicloc_len);
+                pos += beacon->civicloc_len;
+        }
 
         return new_beacon;
 }
@@ -3765,6 +3838,17 @@ out:
         return ret;
 }
 
+static int
+ieee80211_get_ftm_responder_stats(struct wiphy *wiphy,
+                                  struct net_device *dev,
+                                  struct cfg80211_ftm_responder_stats *ftm_stats)
+{
+        struct ieee80211_local *local = wiphy_priv(wiphy);
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+
+        return drv_get_ftm_responder_stats(local, sdata, ftm_stats);
+}
+
 const struct cfg80211_ops mac80211_config_ops = {
         .add_virtual_intf = ieee80211_add_iface,
         .del_virtual_intf = ieee80211_del_iface,
@@ -3859,4 +3943,5 @@ const struct cfg80211_ops mac80211_config_ops = {
         .set_multicast_to_unicast = ieee80211_set_multicast_to_unicast,
         .tx_control_port = ieee80211_tx_control_port,
         .get_txq_stats = ieee80211_get_txq_stats,
+        .get_ftm_responder_stats = ieee80211_get_ftm_responder_stats,
 };