diff options
Diffstat (limited to 'drivers/net/wireless/ath/wil6210/ftm.c')
| -rw-r--r-- | drivers/net/wireless/ath/wil6210/ftm.c | 903 |
1 files changed, 903 insertions, 0 deletions
diff --git a/drivers/net/wireless/ath/wil6210/ftm.c b/drivers/net/wireless/ath/wil6210/ftm.c new file mode 100644 index 000000000000..5cf07343a33c --- /dev/null +++ b/drivers/net/wireless/ath/wil6210/ftm.c @@ -0,0 +1,903 @@ +/* + * Copyright (c) 2016, The Linux Foundation. All rights reserved. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#include <linux/etherdevice.h> +#include <net/netlink.h> +#include "wil6210.h" +#include "ftm.h" +#include "wmi.h" + +/* FTM session ID we use with FW */ +#define WIL_FTM_FW_SESSION_ID 1 + +/* fixed spare allocation we reserve in NL messages we allocate */ +#define WIL_FTM_NL_EXTRA_ALLOC 32 + +/* approx maximum length for FTM_MEAS_RESULT NL80211 event */ +#define WIL_FTM_MEAS_RESULT_MAX_LENGTH 2048 + +/* timeout for waiting for standalone AOA measurement, milliseconds */ +#define WIL_AOA_MEASUREMENT_TIMEOUT 1000 + +/* maximum number of allowed FTM measurements per burst */ +#define WIL_FTM_MAX_MEAS_PER_BURST 31 + +/* initial token to use on non-secure FTM measurement */ +#define WIL_TOF_FTM_DEFAULT_INITIAL_TOKEN 2 + +#define WIL_TOF_FTM_MAX_LCI_LENGTH (240) +#define WIL_TOF_FTM_MAX_LCR_LENGTH (240) + +static const struct +nla_policy wil_nl80211_loc_policy[QCA_WLAN_VENDOR_ATTR_LOC_MAX + 1] = { + [QCA_WLAN_VENDOR_ATTR_FTM_SESSION_COOKIE] = { .type = NLA_U64 }, + [QCA_WLAN_VENDOR_ATTR_LOC_CAPA] = { .type = NLA_NESTED }, + [QCA_WLAN_VENDOR_ATTR_FTM_MEAS_PEERS] = { .type = NLA_NESTED }, + [QCA_WLAN_VENDOR_ATTR_FTM_MEAS_PEER_RESULTS] = { .type = NLA_NESTED }, + [QCA_WLAN_VENDOR_ATTR_FTM_RESPONDER_ENABLE] = { .type = NLA_FLAG }, + [QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS] = { .type = NLA_U32 }, + [QCA_WLAN_VENDOR_ATTR_FTM_INITIAL_TOKEN] = { .type = NLA_U8 }, + [QCA_WLAN_VENDOR_ATTR_AOA_TYPE] = { .type = NLA_U32 }, + [QCA_WLAN_VENDOR_ATTR_LOC_ANTENNA_ARRAY_MASK] = { .type = NLA_U32 }, +}; + +static const struct +nla_policy wil_nl80211_ftm_peer_policy[ + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAX + 1] = { + [QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAC_ADDR] = { .len = ETH_ALEN }, + [QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAGS] = { .type = NLA_U32 }, + [QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_PARAMS] = { .type = NLA_NESTED }, + [QCA_WLAN_VENDOR_ATTR_FTM_PEER_SECURE_TOKEN_ID] = { .type = NLA_U8 }, +}; + +static const struct +nla_policy wil_nl80211_ftm_meas_param_policy[ + QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MAX + 1] = { + [QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST] = { .type = NLA_U8 }, + [QCA_WLAN_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP] = { .type = NLA_U8 }, + [QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_DURATION] = { .type = NLA_U8 }, + [QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD] = { .type = NLA_U16 }, +}; + +static int wil_ftm_parse_meas_params(struct wil6210_priv *wil, + struct nlattr *attr, + struct wil_ftm_meas_params *params) +{ + struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MAX + 1]; + int rc; + + if (!attr) { + /* temporary defaults for one-shot measurement */ + params->meas_per_burst = 1; + params->burst_period = 5; /* 500 milliseconds */ + return 0; + } + rc = nla_parse_nested(tb, QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MAX, + attr, wil_nl80211_ftm_meas_param_policy); + if (rc) { + wil_err(wil, "invalid measurement params\n"); + return rc; + } + if (tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST]) + params->meas_per_burst = nla_get_u8( + tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST]); + if (tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP]) + params->num_of_bursts_exp = nla_get_u8( + tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP]); + if (tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_DURATION]) + params->burst_duration = nla_get_u8( + tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_DURATION]); + if (tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD]) + params->burst_period = nla_get_u16( + tb[QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD]); + return 0; +} + +static int wil_ftm_validate_meas_params(struct wil6210_priv *wil, + struct wil_ftm_meas_params *params) +{ + /* temporary allow only single-burst */ + if (params->meas_per_burst > WIL_FTM_MAX_MEAS_PER_BURST || + params->num_of_bursts_exp != 0) { + wil_err(wil, "invalid measurement params\n"); + return -EINVAL; + } + + return 0; +} + +static int wil_ftm_append_meas_params(struct wil6210_priv *wil, + struct sk_buff *msg, + struct wil_ftm_meas_params *params) +{ + struct nlattr *nl_p; + + nl_p = nla_nest_start( + msg, QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_MEAS_PARAMS); + if (!nl_p) + goto out_put_failure; + if (nla_put_u8(msg, QCA_WLAN_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST, + params->meas_per_burst) || + nla_put_u8(msg, QCA_WLAN_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP, + params->num_of_bursts_exp) || + nla_put_u8(msg, QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_DURATION, + params->burst_duration) || + nla_put_u16(msg, QCA_WLAN_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD, + params->burst_period)) + goto out_put_failure; + nla_nest_end(msg, nl_p); + return 0; +out_put_failure: + return -ENOBUFS; +} + +static int wil_ftm_append_peer_meas_res(struct wil6210_priv *wil, + struct sk_buff *msg, + struct wil_ftm_peer_meas_res *res) +{ + struct nlattr *nl_mres, *nl_f; + int i; + + if (nla_put(msg, QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_MAC_ADDR, + ETH_ALEN, res->mac_addr) || + nla_put_u32(msg, QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_FLAGS, + res->flags) || + nla_put_u8(msg, QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_STATUS, + res->status)) + goto out_put_failure; + if (res->status == QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_STATUS_FAILED && + nla_put_u8(msg, + QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_VALUE_SECONDS, + res->value_seconds)) + goto out_put_failure; + if (res->has_params && + wil_ftm_append_meas_params(wil, msg, &res->params)) + goto out_put_failure; + nl_mres = nla_nest_start(msg, QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_MEAS); + if (!nl_mres) + goto out_put_failure; + for (i = 0; i < res->n_meas; i++) { + nl_f = nla_nest_start(msg, i); + if (!nl_f) + goto out_put_failure; + if (nla_put_u64(msg, QCA_WLAN_VENDOR_ATTR_FTM_MEAS_T1, + res->meas[i].t1) || + nla_put_u64(msg, QCA_WLAN_VENDOR_ATTR_FTM_MEAS_T2, + res->meas[i].t2) || + nla_put_u64(msg, QCA_WLAN_VENDOR_ATTR_FTM_MEAS_T3, + res->meas[i].t3) || + nla_put_u64(msg, QCA_WLAN_VENDOR_ATTR_FTM_MEAS_T4, + res->meas[i].t4)) + goto out_put_failure; + nla_nest_end(msg, nl_f); + } + nla_nest_end(msg, nl_mres); + return 0; +out_put_failure: + wil_err(wil, "fail to append peer result\n"); + return -ENOBUFS; +} + +static void wil_ftm_send_meas_result(struct wil6210_priv *wil, + struct wil_ftm_peer_meas_res *res) +{ + struct sk_buff *vendor_event = NULL; + struct nlattr *nl_res; + int rc = 0; + + wil_dbg_misc(wil, "sending %d results for peer %pM\n", + res->n_meas, res->mac_addr); + + vendor_event = cfg80211_vendor_event_alloc( + wil_to_wiphy(wil), + wil->wdev, + WIL_FTM_MEAS_RESULT_MAX_LENGTH, + QCA_NL80211_VENDOR_EVENT_FTM_MEAS_RESULT_INDEX, + GFP_KERNEL); + if (!vendor_event) { + wil_err(wil, "fail to allocate measurement result\n"); + rc = -ENOMEM; + goto out; + } + + if (nla_put_u64( + vendor_event, QCA_WLAN_VENDOR_ATTR_FTM_SESSION_COOKIE, + wil->ftm.session_cookie)) { + rc = -ENOBUFS; + goto out; + } + + nl_res = nla_nest_start(vendor_event, + QCA_WLAN_VENDOR_ATTR_FTM_MEAS_PEER_RESULTS); + if (!nl_res) { + rc = -ENOBUFS; + goto out; + } + + rc = wil_ftm_append_peer_meas_res(wil, vendor_event, res); + if (rc) + goto out; + + nla_nest_end(vendor_event, nl_res); + cfg80211_vendor_event(vendor_event, GFP_KERNEL); + vendor_event = NULL; +out: + if (vendor_event) + kfree_skb(vendor_event); + if (rc) + wil_err(wil, "send peer result failed, err %d\n", rc); +} + +static void wil_ftm_send_peer_res(struct wil6210_priv *wil) +{ + if (!wil->ftm.has_ftm_res || !wil->ftm.ftm_res) + return; + + wil_ftm_send_meas_result(wil, wil->ftm.ftm_res); + wil->ftm.has_ftm_res = 0; + wil->ftm.ftm_res->n_meas = 0; +} + +static void wil_aoa_measurement_timeout(struct work_struct *work) +{ + struct wil_ftm_priv *ftm = container_of(work, struct wil_ftm_priv, + aoa_timeout_work); + struct wil6210_priv *wil = container_of(ftm, struct wil6210_priv, ftm); + struct wil_aoa_meas_result res; + + wil_dbg_misc(wil, "AOA measurement timeout\n"); + + memset(&res, 0, sizeof(res)); + ether_addr_copy(res.mac_addr, wil->ftm.aoa_peer_mac_addr); + res.type = wil->ftm.aoa_type; + res.status = QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_ABORTED; + wil_aoa_cfg80211_meas_result(wil, &res); +} + +static int +wil_ftm_cfg80211_start_session(struct wil6210_priv *wil, + struct wil_ftm_session_request *request) +{ + int rc = 0; + bool has_lci = false, has_lcr = false; + u8 max_meas = 0, *ptr; + u32 i, cmd_len; + struct wmi_tof_session_start_cmd *cmd; + + mutex_lock(&wil->ftm.lock); + if (wil->ftm.session_started) { + wil_err(wil, "FTM session already running\n"); + rc = -EAGAIN; + goto out; + } + /* for now allow measurement to associated AP only */ + if (!test_bit(wil_status_fwconnected, wil->status)) { + wil_err(wil, "must be associated\n"); + rc = -ENOTSUPP; + goto out; + } + + for (i = 0; i < request->n_peers; i++) { + if (request->peers[i].flags & + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCI) + has_lci = true; + if (request->peers[i].flags & + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCR) + has_lcr = true; + max_meas = max(max_meas, + request->peers[i].params.meas_per_burst); + } + + wil->ftm.ftm_res = kzalloc(sizeof(*wil->ftm.ftm_res) + + max_meas * sizeof(struct wil_ftm_peer_meas) + + (has_lci ? WIL_TOF_FTM_MAX_LCI_LENGTH : 0) + + (has_lcr ? WIL_TOF_FTM_MAX_LCR_LENGTH : 0), GFP_KERNEL); + if (!wil->ftm.ftm_res) { + rc = -ENOMEM; + goto out; + } + ptr = (u8 *)wil->ftm.ftm_res; + ptr += sizeof(struct wil_ftm_peer_meas_res) + + max_meas * sizeof(struct wil_ftm_peer_meas); + if (has_lci) { + wil->ftm.ftm_res->lci = ptr; + ptr += WIL_TOF_FTM_MAX_LCI_LENGTH; + } + if (has_lcr) + wil->ftm.ftm_res->lcr = ptr; + wil->ftm.max_ftm_meas = max_meas; + + cmd_len = sizeof(struct wmi_tof_session_start_cmd) + + request->n_peers * sizeof(struct wmi_ftm_dest_info); + cmd = kzalloc(cmd_len, GFP_KERNEL); + if (!cmd) { + rc = -ENOMEM; + goto out_ftm_res; + } + + cmd->session_id = cpu_to_le32(WIL_FTM_FW_SESSION_ID); + cmd->num_of_dest = cpu_to_le16(request->n_peers); + for (i = 0; i < request->n_peers; i++) { + ether_addr_copy(cmd->ftm_dest_info[i].dst_mac, + request->peers[i].mac_addr); + cmd->ftm_dest_info[i].channel = request->peers[i].channel; + if (request->peers[i].flags & + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_SECURE) { + cmd->ftm_dest_info[i].flags |= + WMI_TOF_SESSION_START_FLAG_SECURED; + cmd->ftm_dest_info[i].initial_token = + request->peers[i].secure_token_id; + } else { + cmd->ftm_dest_info[i].initial_token = + WIL_TOF_FTM_DEFAULT_INITIAL_TOKEN; + } + if (request->peers[i].flags & + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_ASAP) + cmd->ftm_dest_info[i].flags |= + WMI_TOF_SESSION_START_FLAG_ASAP; + if (request->peers[i].flags & + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCI) + cmd->ftm_dest_info[i].flags |= + WMI_TOF_SESSION_START_FLAG_LCI_REQ; + if (request->peers[i].flags & + QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCR) + cmd->ftm_dest_info[i].flags |= + WMI_TOF_SESSION_START_FLAG_LCR_REQ; + cmd->ftm_dest_info[i].num_of_ftm_per_burst = + request->peers[i].params.meas_per_burst; + cmd->ftm_dest_info[i].num_of_bursts_exp = + request->peers[i].params.num_of_bursts_exp; + cmd->ftm_dest_info[i].burst_duration = + request->peers[i].params.burst_duration; + cmd->ftm_dest_info[i].burst_period = + cpu_to_le16(request->peers[i].params.burst_period); + } + + rc = wmi_send(wil, WMI_TOF_SESSION_START_CMDID, cmd, cmd_len); + kfree(cmd); + + if (rc) + goto out_ftm_res; + + wil->ftm.session_cookie = request->session_cookie; + wil->ftm.session_started = 1; + +out_ftm_res: + if (rc) { + kfree(wil->ftm.ftm_res); + wil->ftm.ftm_res = NULL; + } +out: + mutex_unlock(&wil->ftm.lock); + return rc; +} + +static void +wil_ftm_cfg80211_session_ended(struct wil6210_priv *wil, u32 status) +{ + struct sk_buff *vendor_event = NULL; + + mutex_lock(&wil->ftm.lock); + + if (!wil->ftm.session_started) { + wil_dbg_misc(wil, "FTM session not started, ignoring event\n"); + goto out; + } + + /* finish the session */ + wil_dbg_misc(wil, "finishing FTM session\n"); + + /* send left-over results if any */ + wil_ftm_send_peer_res(wil); + + wil->ftm.session_started = 0; + kfree(wil->ftm.ftm_res); + wil->ftm.ftm_res = NULL; + + vendor_event = cfg80211_vendor_event_alloc( + wil_to_wiphy(wil), + wil->wdev, + WIL_FTM_NL_EXTRA_ALLOC, + QCA_NL80211_VENDOR_EVENT_FTM_SESSION_DONE_INDEX, + GFP_KERNEL); + if (!vendor_event) + goto out; + + if (nla_put_u64(vendor_event, + QCA_WLAN_VENDOR_ATTR_FTM_SESSION_COOKIE, + wil->ftm.session_cookie) || + nla_put_u32(vendor_event, + QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS, status)) { + wil_err(wil, "failed to fill session done event\n"); + goto out; + } + cfg80211_vendor_event(vendor_event, GFP_KERNEL); + vendor_event = NULL; +out: + kfree_skb(vendor_event); + mutex_unlock(&wil->ftm.lock); +} + +static void wil_aoa_timer_fn(ulong x) +{ + struct wil6210_priv *wil = (void *)x; + + wil_dbg_misc(wil, "AOA timer\n"); + schedule_work(&wil->ftm.aoa_timeout_work); +} + +static int +wil_aoa_cfg80211_start_measurement(struct wil6210_priv *wil, + struct wil_aoa_meas_request *request) +{ + int rc = 0; + struct cfg80211_bss *bss; + struct wmi_aoa_meas_cmd cmd; + + mutex_lock(&wil->ftm.lock); + + if (wil->ftm.aoa_started) { + wil_err(wil, "AOA measurement already running\n"); + rc = -EAGAIN; + goto out; + } + if (request->type >= QCA_WLAN_VENDOR_ATTR_AOA_TYPE_MAX) { + wil_err(wil, "invalid AOA type: %d\n", request->type); + rc = -EINVAL; + goto out; + } + + bss = cfg80211_get_bss(wil_to_wiphy(wil), NULL, request->mac_addr, + NULL, 0, + IEEE80211_BSS_TYPE_ANY, IEEE80211_PRIVACY_ANY); + if (!bss) { + wil_err(wil, "Unable to find BSS\n"); + rc = -ENOENT; + goto out; + } + + memset(&cmd, 0, sizeof(cmd)); + ether_addr_copy(cmd.mac_addr, request->mac_addr); + cmd.channel = bss->channel->hw_value - 1; + cmd.aoa_meas_type = request->type; + + rc = wmi_send(wil, WMI_AOA_MEAS_CMDID, &cmd, sizeof(cmd)); + if (rc) + goto out_bss; + + ether_addr_copy(wil->ftm.aoa_peer_mac_addr, request->mac_addr); + mod_timer(&wil->ftm.aoa_timer, + jiffies + msecs_to_jiffies(WIL_AOA_MEASUREMENT_TIMEOUT)); + wil->ftm.aoa_started = 1; +out_bss: + cfg80211_put_bss(wil_to_wiphy(wil), bss); +out: + mutex_unlock(&wil->ftm.lock); + return rc; +} + +void wil_aoa_cfg80211_meas_result(struct wil6210_priv *wil, + struct wil_aoa_meas_result *result) +{ + struct sk_buff *vendor_event = NULL; + + mutex_lock(&wil->ftm.lock); + + if (!wil->ftm.aoa_started) { + wil_info(wil, "AOA not started, not sending result\n"); + goto out; + } + + wil_dbg_misc(wil, "sending AOA measurement result\n"); + + vendor_event = cfg80211_vendor_event_alloc( + wil_to_wiphy(wil), + wil->wdev, + result->length + WIL_FTM_NL_EXTRA_ALLOC, + QCA_NL80211_VENDOR_EVENT_AOA_MEAS_RESULT_INDEX, + GFP_KERNEL); + if (!vendor_event) { + wil_err(wil, "fail to allocate measurement result\n"); + goto out; + } + + if (nla_put(vendor_event, QCA_WLAN_VENDOR_ATTR_MAC_ADDR, + ETH_ALEN, result->mac_addr) || + nla_put_u32(vendor_event, QCA_WLAN_VENDOR_ATTR_AOA_TYPE, + result->type) || + nla_put_u32(vendor_event, QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS, + result->status) || + nla_put_u32(vendor_event, + QCA_WLAN_VENDOR_ATTR_LOC_ANTENNA_ARRAY_MASK, + result->antenna_array_mask)) { + wil_err(wil, "failed to fill vendor event\n"); + goto out; + } + + if (result->length > 0 && + nla_put(vendor_event, QCA_WLAN_VENDOR_ATTR_AOA_MEAS_RESULT, + result->length, result->data)) { + wil_err(wil, "failed to fill vendor event with AOA data\n"); + goto out; + } + + cfg80211_vendor_event(vendor_event, GFP_KERNEL); + + del_timer_sync(&wil->ftm.aoa_timer); + wil->ftm.aoa_started = 0; +out: + mutex_unlock(&wil->ftm.lock); +} + +void wil_ftm_evt_session_ended(struct wil6210_priv *wil, + struct wmi_tof_session_end_event *evt) +{ + u32 status; + + switch (evt->status) { + case WMI_TOF_SESSION_END_NO_ERROR: + status = QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_OK; + break; + case WMI_TOF_SESSION_END_PARAMS_ERROR: + status = QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_INVALID; + break; + case WMI_TOF_SESSION_END_FAIL: + status = QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_FAILED; + break; + case WMI_TOF_SESSION_END_ABORTED: + status = QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_ABORTED; + break; + default: + status = QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_FAILED; + break; + } + + wil_ftm_cfg80211_session_ended(wil, status); +} + +void wil_ftm_evt_per_dest_res(struct wil6210_priv *wil, + struct wmi_tof_ftm_per_dest_res_event *evt) +{ + u32 i, index; + __le64 tmp = 0; + u8 n_meas; + + mutex_lock(&wil->ftm.lock); + + if (!wil->ftm.session_started || !wil->ftm.ftm_res) { + wil_dbg_misc(wil, "Session not running, ignoring res event\n"); + goto out; + } + if (wil->ftm.has_ftm_res && + !ether_addr_equal(evt->dst_mac, wil->ftm.ftm_res->mac_addr)) { + wil_dbg_misc(wil, + "Results for previous peer not properly terminated\n"); + wil_ftm_send_peer_res(wil); + } + + if (!wil->ftm.has_ftm_res) { + ether_addr_copy(wil->ftm.ftm_res->mac_addr, evt->dst_mac); + wil->ftm.has_ftm_res = 1; + } + + n_meas = evt->actual_ftm_per_burst; + switch (evt->status) { + case WMI_PER_DEST_RES_NO_ERROR: + wil->ftm.ftm_res->status = + QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_STATUS_OK; + break; + case WMI_PER_DEST_RES_TX_RX_FAIL: + /* FW reports corrupted results here, discard. */ + n_meas = 0; + wil->ftm.ftm_res->status = + QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_STATUS_OK; + break; + case WMI_PER_DEST_RES_PARAM_DONT_MATCH: + wil->ftm.ftm_res->status = + QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_STATUS_INVALID; + break; + default: + wil_err(wil, "unexpected status %d\n", evt->status); + wil->ftm.ftm_res->status = + QCA_WLAN_VENDOR_ATTR_FTM_PEER_RES_STATUS_INVALID; + break; + } + + for (i = 0; i < n_meas; i++) { + index = wil->ftm.ftm_res->n_meas; + if (index >= wil->ftm.max_ftm_meas) { + wil_dbg_misc(wil, "Too many measurements, some lost\n"); + break; + } + memcpy(&tmp, evt->responder_ftm_res[i].t1, + sizeof(evt->responder_ftm_res[i].t1)); + wil->ftm.ftm_res->meas[index].t1 = le64_to_cpu(tmp); + memcpy(&tmp, evt->responder_ftm_res[i].t2, + sizeof(evt->responder_ftm_res[i].t2)); + wil->ftm.ftm_res->meas[index].t2 = le64_to_cpu(tmp); + memcpy(&tmp, evt->responder_ftm_res[i].t3, + sizeof(evt->responder_ftm_res[i].t3)); + wil->ftm.ftm_res->meas[index].t3 = le64_to_cpu(tmp); + memcpy(&tmp, evt->responder_ftm_res[i].t4, + sizeof(evt->responder_ftm_res[i].t4)); + wil->ftm.ftm_res->meas[index].t4 = le64_to_cpu(tmp); + wil->ftm.ftm_res->n_meas++; + } + + if (evt->flags & WMI_PER_DEST_RES_BURST_REPORT_END) + wil_ftm_send_peer_res(wil); +out: + mutex_unlock(&wil->ftm.lock); +} + +void wil_aoa_evt_meas(struct wil6210_priv *wil, + struct wmi_aoa_meas_event *evt, + int len) +{ + int data_len = len - offsetof(struct wmi_aoa_meas_event, meas_data); + struct wil_aoa_meas_result *res; + + data_len = min_t(int, le16_to_cpu(evt->length), data_len); + + res = kmalloc(sizeof(*res) + data_len, GFP_KERNEL); + if (!res) + return; + + ether_addr_copy(res->mac_addr, evt->mac_addr); + res->type = evt->aoa_meas_type; + res->antenna_array_mask = le32_to_cpu(evt->meas_rf_mask); + res->status = evt->meas_status; + res->length = data_len; + memcpy(res->data, evt->meas_data, data_len); + + wil_dbg_misc(wil, "AOA result status %d type %d mask %d length %d\n", + res->status, res->type, + res->antenna_array_mask, res->length); + + wil_aoa_cfg80211_meas_result(wil, res); + kfree(res); +} + +int wil_ftm_get_capabilities(struct wiphy *wiphy, struct wireless_dev *wdev, + const void *data, int data_len) +{ + struct wil6210_priv *wil = wiphy_to_wil(wiphy); + struct sk_buff *skb; + struct nlattr *attr; + + if (!test_bit(WMI_FW_CAPABILITY_FTM, wil->fw_capabilities)) + return -ENOTSUPP; + + /* we should get the capabilities from the FW. for now, + * report dummy capabilities for one shot measurement + */ + skb = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, 128); + if (!skb) + return -ENOMEM; + attr = nla_nest_start(skb, QCA_WLAN_VENDOR_ATTR_LOC_CAPA); + if (!attr || + nla_put_u32(skb, QCA_WLAN_VENDOR_ATTR_LOC_CAPA_FLAGS, + QCA_WLAN_VENDOR_ATTR_LOC_CAPA_FLAG_FTM_RESPONDER | + QCA_WLAN_VENDOR_ATTR_LOC_CAPA_FLAG_FTM_INITIATOR | + QCA_WLAN_VENDOR_ATTR_LOC_CAPA_FLAG_ASAP | + QCA_WLAN_VENDOR_ATTR_LOC_CAPA_FLAG_AOA) || + nla_put_u16(skb, QCA_WLAN_VENDOR_ATTR_FTM_CAPA_MAX_NUM_SESSIONS, + 1) || + nla_put_u16(skb, QCA_WLAN_VENDOR_ATTR_FTM_CAPA_MAX_NUM_PEERS, 1) || + nla_put_u8(skb, QCA_WLAN_VENDOR_ATTR_FTM_CAPA_MAX_NUM_BURSTS_EXP, + 0) || + nla_put_u8(skb, QCA_WLAN_VENDOR_ATTR_FTM_CAPA_MAX_MEAS_PER_BURST, + 4) || + nla_put_u32(skb, QCA_WLAN_VENDOR_ATTR_AOA_CAPA_SUPPORTED_TYPES, + BIT(QCA_WLAN_VENDOR_ATTR_AOA_TYPE_TOP_CIR_PHASE))) { + wil_err(wil, "fail to fill get_capabilities reply\n"); + kfree_skb(skb); + return -ENOMEM; + } + nla_nest_end(skb, attr); + + return cfg80211_vendor_cmd_reply(skb); +} + +int wil_ftm_start_session(struct wiphy *wiphy, struct wireless_dev *wdev, + const void *data, int data_len) +{ + struct wil6210_priv *wil = wiphy_to_wil(wiphy); + struct wil_ftm_session_request *request; + struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_LOC_MAX + 1]; + struct nlattr *tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAX + 1]; + struct nlattr *peer; + int rc, n_peers = 0, index = 0, tmp; + struct cfg80211_bss *bss; + + if (!test_bit(WMI_FW_CAPABILITY_FTM, wil->fw_capabilities)) + return -ENOTSUPP; + + rc = nla_parse(tb, QCA_WLAN_VENDOR_ATTR_LOC_MAX, data, data_len, + wil_nl80211_loc_policy); + if (rc) { + wil_err(wil, "Invalid ATTR\n"); + return rc; + } + + if (!tb[QCA_WLAN_VENDOR_ATTR_FTM_MEAS_PEERS]) { + wil_err(wil, "no peers specified\n"); + return -EINVAL; + } + + if (!tb[QCA_WLAN_VENDOR_ATTR_FTM_SESSION_COOKIE]) { + wil_err(wil, "session cookie not specified\n"); + return -EINVAL; + } + + nla_for_each_nested(peer, tb[QCA_WLAN_VENDOR_ATTR_FTM_MEAS_PEERS], + tmp) + n_peers++; + + if (!n_peers) { + wil_err(wil, "empty peer list\n"); + return -EINVAL; + } + + /* for now only allow measurement for a single peer */ + if (n_peers != 1) { + wil_err(wil, "only single peer allowed\n"); + return -EINVAL; + } + + request = kzalloc(sizeof(*request) + + n_peers * sizeof(struct wil_ftm_meas_peer_info), + GFP_KERNEL); + if (!request) + return -ENOMEM; + + request->session_cookie = + nla_get_u64(tb[QCA_WLAN_VENDOR_ATTR_FTM_SESSION_COOKIE]); + request->n_peers = n_peers; + nla_for_each_nested(peer, tb[QCA_WLAN_VENDOR_ATTR_FTM_MEAS_PEERS], + tmp) { + rc = nla_parse_nested(tb2, QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAX, + peer, wil_nl80211_ftm_peer_policy); + if (rc) { + wil_err(wil, "Invalid peer ATTR\n"); + goto out; + } + if (!tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAC_ADDR] || + nla_len(tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAC_ADDR]) + != ETH_ALEN) { + wil_err(wil, "Peer MAC address missing or invalid\n"); + rc = -EINVAL; + goto out; + } + memcpy(request->peers[index].mac_addr, + nla_data(tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MAC_ADDR]), + ETH_ALEN); + bss = cfg80211_get_bss(wiphy, NULL, + request->peers[index].mac_addr, NULL, 0, + IEEE80211_BSS_TYPE_ANY, + IEEE80211_PRIVACY_ANY); + if (!bss) { + wil_err(wil, "invalid bss at index %d\n", index); + rc = -ENOENT; + goto out; + } + request->peers[index].channel = bss->channel->hw_value - 1; + cfg80211_put_bss(wiphy, bss); + if (tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAGS]) + request->peers[index].flags = nla_get_u32( + tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_FLAGS]); + if (tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_SECURE_TOKEN_ID]) + request->peers[index].secure_token_id = nla_get_u8( + tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_SECURE_TOKEN_ID]); + rc = wil_ftm_parse_meas_params( + wil, + tb2[QCA_WLAN_VENDOR_ATTR_FTM_PEER_MEAS_PARAMS], + &request->peers[index].params); + if (!rc) + rc = wil_ftm_validate_meas_params( + wil, &request->peers[index].params); + if (rc) + goto out; + index++; + } + + rc = wil_ftm_cfg80211_start_session(wil, request); +out: + kfree(request); + return rc; +} + +int wil_ftm_abort_session(struct wiphy *wiphy, struct wireless_dev *wdev, + const void *data, int data_len) +{ + struct wil6210_priv *wil = wiphy_to_wil(wiphy); + + wil_dbg_misc(wil, "stub\n"); + return -ENOTSUPP; +} + +int wil_ftm_configure_responder(struct wiphy *wiphy, struct wireless_dev *wdev, + const void *data, int data_len) +{ + struct wil6210_priv *wil = wiphy_to_wil(wiphy); + + wil_dbg_misc(wil, "stub\n"); + return -ENOTSUPP; +} + +int wil_aoa_start_measurement(struct wiphy *wiphy, struct wireless_dev *wdev, + const void *data, int data_len) +{ + struct wil6210_priv *wil = wiphy_to_wil(wiphy); + struct wil_aoa_meas_request request; + struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_LOC_MAX + 1]; + int rc; + + if (!test_bit(WMI_FW_CAPABILITY_FTM, wil->fw_capabilities)) + return -ENOTSUPP; + + wil_dbg_misc(wil, "AOA start measurement\n"); + + rc = nla_parse(tb, QCA_WLAN_VENDOR_ATTR_LOC_MAX, data, data_len, + wil_nl80211_loc_policy); + if (rc) { + wil_err(wil, "Invalid ATTR\n"); + return rc; + } + + if (!tb[QCA_WLAN_VENDOR_ATTR_MAC_ADDR] || + !tb[QCA_WLAN_VENDOR_ATTR_AOA_TYPE]) { + wil_err(wil, "Must specify MAC address and type\n"); + return -EINVAL; + } + + memset(&request, 0, sizeof(request)); + ether_addr_copy(request.mac_addr, + nla_data(tb[QCA_WLAN_VENDOR_ATTR_MAC_ADDR])); + request.type = nla_get_u32(tb[QCA_WLAN_VENDOR_ATTR_AOA_TYPE]); + + rc = wil_aoa_cfg80211_start_measurement(wil, &request); + return rc; +} + +int wil_aoa_abort_measurement(struct wiphy *wiphy, struct wireless_dev *wdev, + const void *data, int data_len) +{ + struct wil6210_priv *wil = wiphy_to_wil(wiphy); + + wil_dbg_misc(wil, "stub\n"); + return -ENOTSUPP; +} + +void wil_ftm_init(struct wil6210_priv *wil) +{ + mutex_init(&wil->ftm.lock); + setup_timer(&wil->ftm.aoa_timer, wil_aoa_timer_fn, (ulong)wil); + INIT_WORK(&wil->ftm.aoa_timeout_work, wil_aoa_measurement_timeout); +} + +void wil_ftm_deinit(struct wil6210_priv *wil) +{ + del_timer_sync(&wil->ftm.aoa_timer); + cancel_work_sync(&wil->ftm.aoa_timeout_work); + kfree(wil->ftm.ftm_res); +} + +void wil_ftm_stop_operations(struct wil6210_priv *wil) +{ + wil_ftm_cfg80211_session_ended( + wil, QCA_WLAN_VENDOR_ATTR_LOC_SESSION_STATUS_ABORTED); +} |