new/kernel-rt/drivers/net/wireless/rockchip_wlan/uwe5621ds/unisocwifi/rtt.c

/*
 * Copyright (C) 2015 Spreadtrum Communications Inc.
 *
 * Abstract : This file is an implementation for cfg80211 subsystem
 *
 * Authors:
 * Chaojie Xu <chaojie.xu@spreadtrum.com>
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include <linux/etherdevice.h>
#include <net/cfg80211.h>
#include <net/netlink.h>
#include "sprdwl.h"
#include "rtt.h"

/* FTM session ID we use with FW */
#define FTM_ESSION_ID		1

/* fixed spare allocation we reserve in NL messages we allocate */
#define FTM_NL_EXTRA_ALLOC		32

/* approx maximum length for FTM_MEAS_RESULT NL80211 event */
#define FTM_MEAS_RESULT_MAX_LENGTH	2048

/* maximum number of allowed FTM measurements per burst */
#define FTM_MAX_MEAS_PER_BURST	31

/* initial token to use on non-secure FTM measurement */
#define FTM_DEFAULT_INITIAL_TOKEN	2

#define FTM_MAX_LCI_LENGTH		(240)
#define FTM_MAX_LCR_LENGTH		(240)

/* max rtt cmd response length */
#define RTT_RSP_LEN (128)

enum session_start_flags {
	FTM_SESSION_START_FLAG_SECURED	= 0x1,
	FTM_SESSION_START_FLAG_ASAP		= 0x2,
	FTM_SESSION_START_FLAG_LCI_REQ	= 0x4,
	FTM_SESSION_START_FLAG_LCR_REQ	= 0x8,
};

enum rtt_subcmd {
	RTT_ENABLE,
	RTT_DISABLE,
	RTT_GET_CAPABILITIES,
	RTT_RANGE_REQUEST,
	RTT_RANGE_CANCEL,
	RTT_SET_CLI,
	RTT_SET_CLR,
	RTT_GET_RESPONDER_INFO,
	RTT_ENABLE_RESPONDER,
	RTT_DISABLE_RESPONDER,
};

enum rtt_subevt {
	RTT_SESSION_END,
	RTT_PER_DEST_RES,
};

/* Responder FTM Results */
struct sprdwl_responder_ftm_res {
	u8 t1[6];
	u8 t2[6];
	u8 t3[6];
	u8 t4[6];
	__le16 tod_err;
	__le16 toa_err;
	__le16 tod_err_initiator;
	__le16 toa_err_initiator;
} __packed;

enum ftm_per_dest_res_status {
	FTM_PER_DEST_RES_NO_ERROR		= 0x00,
	FTM_PER_DEST_RES_TX_RX_FAIL		= 0x01,
	FTM_PER_DEST_RES_PARAM_DONT_MATCH	= 0x02,
};

enum ftm_per_dest_res_flags {
	FTM_PER_DEST_RES_REQ_START		= 0x01,
	FTM_PER_DEST_RES_BURST_REPORT_END	= 0x02,
	FTM_PER_DEST_RES_REQ_END		= 0x04,
	FTM_PER_DEST_RES_PARAM_UPDATE		= 0x08,
};

struct ftm_per_dest_res {
	/* FTM session ID */
	__le32 session_id;
	/* destination MAC address */
	u8 dst_mac[ETH_ALEN];
	/* wmi_tof_ftm_per_dest_res_flags_e */
	u8 flags;
	/* wmi_tof_ftm_per_dest_res_status_e */
	u8 status;
	/* responder ASAP */
	u8 responder_asap;
	/* responder number of FTM per burst */
	u8 responder_num_ftm_per_burst;
	/* responder number of FTM burst exponent */
	u8 responder_num_ftm_bursts_exp;
	/* responder burst duration ,wmi_tof_burst_duration_e */
	u8 responder_burst_duration;
	/* responder burst period, indicate interval between two consecutive
	 * burst instances, in units of 100 ms
	 */
	__le16 responder_burst_period;
	/* receive burst counter */
	__le16 bursts_cnt;
	/* tsf of responder start burst */
	__le32 tsf_sync;
	/* actual received ftm per burst */
	u8 actual_ftm_per_burst;
	u8 reserved0[7];
	struct sprdwl_responder_ftm_res responder_ftm_res[0];
} __packed;

struct ftm_dest_info {
	u8 channel;
	u8 flags;
	u8 initial_token;
	u8 num_of_ftm_per_burst;
	u8 num_of_bursts_exp;
	u8 burst_duration;
	/* Burst Period indicate interval between two consecutive burst
	 * instances, in units of 100 ms
	 */
	__le16 burst_period;
	u8 dst_mac[ETH_ALEN];
	__le16 reserved;
} __packed;

struct ftm_session_start {
	__le32 session_id;
	u8 num_of_aoa_measures;
	u8 aoa_type;
	__le16 num_of_dest;
	u8 reserved[4];
	struct ftm_dest_info dest_info[0];
} __packed;

struct sprdwl_cmd_rtt {
	u8 sub_cmd;
	__le16 len;
	u8 data[0];
} __packed;

static const struct
nla_policy sprdwl_nl80211_loc_policy[SPRDWL_VENDOR_ATTR_LOC_MAX + 1] = {
	[SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE] = { .type = NLA_U64 },
	[SPRDWL_VENDOR_ATTR_LOC_CAPA] = { .type = NLA_NESTED },
	[SPRDWL_VENDOR_ATTR_FTM_MEAS_PEERS] = { .type = NLA_NESTED },
	[SPRDWL_VENDOR_ATTR_FTM_MEAS_PEER_RESULTS] = { .type = NLA_NESTED },
	[SPRDWL_VENDOR_ATTR_FTM_RESPONDER_ENABLE] = { .type = NLA_FLAG },
	[SPRDWL_VENDOR_ATTR_LOC_SESSION_STATUS] = { .type = NLA_U32 },
	[SPRDWL_VENDOR_ATTR_FTM_INITIAL_TOKEN] = { .type = NLA_U8 },
	[SPRDWL_VENDOR_ATTR_AOA_TYPE] = { .type = NLA_U32 },
	[SPRDWL_VENDOR_ATTR_LOC_ANTENNA_ARRAY_MASK] = { .type = NLA_U32 },
	[SPRDWL_VENDOR_ATTR_FREQ] = { .type = NLA_U32 },
};

static const struct
nla_policy sprdwl_nl80211_ftm_peer_policy[
	SPRDWL_VENDOR_ATTR_FTM_PEER_MAX + 1] = {
	[SPRDWL_VENDOR_ATTR_FTM_PEER_MAC_ADDR] = { .len = ETH_ALEN },
	[SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAGS] = { .type = NLA_U32 },
	[SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_PARAMS] = { .type = NLA_NESTED },
	[SPRDWL_VENDOR_ATTR_FTM_PEER_SECURE_TOKEN_ID] = { .type = NLA_U8 },
	[SPRDWL_VENDOR_ATTR_FTM_PEER_FREQ] = { .type = NLA_U32 },
};

static const struct
nla_policy sprdwl_nl80211_ftm_meas_param_policy[
	SPRDWL_VENDOR_ATTR_FTM_PARAM_MAX + 1] = {
	[SPRDWL_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST] = { .type = NLA_U8 },
	[SPRDWL_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP] = { .type = NLA_U8 },
	[SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_DURATION] = { .type = NLA_U8 },
	[SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD] = { .type = NLA_U16 },
};

#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 0)
static u8 sprdwl_ftm_get_channel(struct wiphy *wiphy,
				 const u8 *mac_addr, u32 freq)
{
	struct cfg80211_bss *bss;
	struct ieee80211_channel *chan;
	u8 channel;

	if (freq) {
		chan = ieee80211_get_channel(wiphy, freq);
		if (!chan) {
			wl_err("invalid freq: %d\n", freq);
			return 0;
		}
		channel = chan->hw_value;
	} else {
		bss = cfg80211_get_bss(wiphy, NULL, mac_addr,
				       NULL, 0, WLAN_CAPABILITY_ESS,
				       WLAN_CAPABILITY_ESS);
		if (!bss) {
			wl_err("Unable to find BSS\n");
			return 0;
		}
		channel = bss->channel->hw_value;
		cfg80211_put_bss(wiphy, bss);
	}

	wl_info("target %pM at channel %d\n", mac_addr, channel);
	return channel;
}

static int sprdwl_ftm_parse_meas_params(struct sprdwl_vif *vif,
					struct nlattr *attr,
					struct sprdwl_ftm_meas_params *params)
{
	struct nlattr *tb[SPRDWL_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;
	}
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 14, 0)
	rc = nla_parse_nested(tb, SPRDWL_VENDOR_ATTR_FTM_PARAM_MAX,
			      attr, sprdwl_nl80211_ftm_meas_param_policy, NULL);
#else
	rc = nla_parse_nested(tb, SPRDWL_VENDOR_ATTR_FTM_PARAM_MAX,
				attr, sprdwl_nl80211_ftm_meas_param_policy);
#endif
	if (rc) {
		wl_ndev_log(L_ERR, vif->ndev,
			   "%s: invalid measurement params\n", __func__);
		return rc;
	}
	if (tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST])
		params->meas_per_burst = nla_get_u8(
			tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST]);
	if (tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP])
		params->num_of_bursts_exp = nla_get_u8(
			tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP]);
	if (tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_DURATION])
		params->burst_duration = nla_get_u8(
			tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_DURATION]);
	if (tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD])
		params->burst_period = nla_get_u16(
			tb[SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_PERIOD]);
	return 0;
}

static int
sprdwl_ftm_validate_meas_params(struct sprdwl_vif *vif,
				struct sprdwl_ftm_meas_params *params)
{
	if (params->meas_per_burst > FTM_MAX_MEAS_PER_BURST ||
	    params->num_of_bursts_exp != 0) {
		wl_ndev_log(L_ERR, vif->ndev, "%s: invalid meas per burst\n", __func__);
		return -EINVAL;
	}

	return 0;
}

static int sprdwl_ftm_append_meas_params(struct sprdwl_priv *priv,
					 struct sk_buff *msg,
					 struct sprdwl_ftm_meas_params *params)
{
	struct nlattr *nl_p;

	nl_p = nla_nest_start(
		msg, SPRDWL_VENDOR_ATTR_FTM_PEER_RES_MEAS_PARAMS);
	if (!nl_p)
		goto out_put_failure;
	if (nla_put_u8(msg, SPRDWL_VENDOR_ATTR_FTM_PARAM_MEAS_PER_BURST,
		       params->meas_per_burst) ||
	    nla_put_u8(msg, SPRDWL_VENDOR_ATTR_FTM_PARAM_NUM_BURSTS_EXP,
		       params->num_of_bursts_exp) ||
	    nla_put_u8(msg, SPRDWL_VENDOR_ATTR_FTM_PARAM_BURST_DURATION,
		       params->burst_duration) ||
	    nla_put_u16(msg, SPRDWL_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 sprdwl_ftm_append_peer_meas_res(struct sprdwl_priv *priv,
					   struct sk_buff *msg,
					   struct sprdwl_ftm_peer_meas_res *res)
{
	struct nlattr *nl_mres, *nl_f;
	int i;

	if (nla_put(msg, SPRDWL_VENDOR_ATTR_FTM_PEER_RES_MAC_ADDR,
		    ETH_ALEN, res->mac_addr) ||
	    nla_put_u32(msg, SPRDWL_VENDOR_ATTR_FTM_PEER_RES_FLAGS,
			res->flags) ||
	    nla_put_u8(msg, SPRDWL_VENDOR_ATTR_FTM_PEER_RES_STATUS,
		       res->status))
		goto out_put_failure;
	if (res->status == SPRDWL_VENDOR_ATTR_FTM_PEER_RES_STATUS_FAILED &&
	    nla_put_u8(msg,
		       SPRDWL_VENDOR_ATTR_FTM_PEER_RES_VALUE_SECONDS,
		       res->value_seconds))
		goto out_put_failure;
	if (res->has_params &&
	    sprdwl_ftm_append_meas_params(priv, msg, &res->params))
		goto out_put_failure;
	nl_mres = nla_nest_start(msg, SPRDWL_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 LINUX_VERSION_CODE >= KERNEL_VERSION(4, 7, 0)
		if (nla_put_u64_64bit(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T1,
				res->meas[i].t1, 0) ||
		    nla_put_u64_64bit(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T2,
				res->meas[i].t2, 0) ||
		    nla_put_u64_64bit(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T3,
				res->meas[i].t3, 0) ||
		    nla_put_u64_64bit(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T4,
				res->meas[i].t4, 0))
			goto out_put_failure;
#else
		if (nla_put_u64(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T1,
				res->meas[i].t1) ||
		    nla_put_u64(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T2,
				res->meas[i].t2) ||
		    nla_put_u64(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T3,
				res->meas[i].t3) ||
		    nla_put_u64(msg, SPRDWL_VENDOR_ATTR_FTM_MEAS_T4,
				res->meas[i].t4))
			goto out_put_failure;
#endif
		nla_nest_end(msg, nl_f);
	}
	nla_nest_end(msg, nl_mres);
	return 0;
out_put_failure:
	wl_err("%s: fail to append peer result\n", __func__);
	return -ENOBUFS;
}

static void sprdwl_ftm_send_meas_result(struct sprdwl_priv *priv,
					struct sprdwl_ftm_peer_meas_res *res)
{
	struct sk_buff *skb = NULL;
	struct nlattr *nl_res;
	int rc = 0;

	wl_info("sending %d results for peer %pM\n",
		res->n_meas, res->mac_addr);

	skb = cfg80211_vendor_event_alloc(
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 83)
				priv->wiphy, NULL,
#else
				priv->wiphy,
#endif
				FTM_MEAS_RESULT_MAX_LENGTH,
				SPRD_VENDOR_EVENT_FTM_MEAS_RESULT_INDEX,
				GFP_KERNEL);
	if (!skb) {
		wl_err("fail to allocate measurement result\n");
		rc = -ENOMEM;
		goto out;
	}
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 7, 0)
	if (nla_put_u64_64bit(
		skb, SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE,
		priv->ftm.session_cookie, 0)) {
#else
	if (nla_put_u64(
		skb, SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE,
		priv->ftm.session_cookie)) {
#endif
		rc = -ENOBUFS;
		goto out;
	}

	nl_res = nla_nest_start(skb,
				SPRDWL_VENDOR_ATTR_FTM_MEAS_PEER_RESULTS);
	if (!nl_res) {
		rc = -ENOBUFS;
		goto out;
	}

	rc = sprdwl_ftm_append_peer_meas_res(priv, skb, res);
	if (rc)
		goto out;

	nla_nest_end(skb, nl_res);
	cfg80211_vendor_event(skb, GFP_KERNEL);
	skb = NULL;
out:
	if (skb)
		kfree_skb(skb);
	if (rc)
		wl_err("send peer result failed, err %d\n", rc);
}

static void sprdwl_ftm_send_peer_res(struct sprdwl_priv *priv)
{
	if (!priv->ftm.has_ftm_res || !priv->ftm.ftm_res)
		return;

	sprdwl_ftm_send_meas_result(priv, priv->ftm.ftm_res);
	priv->ftm.has_ftm_res = 0;
	priv->ftm.ftm_res->n_meas = 0;
}

static int
sprdwl_ftm_cfg80211_start_session(struct sprdwl_priv *priv,
				  struct sprdwl_vif *vif,
				  struct sprdwl_ftm_session_request *request)
{
	int ret = 0;
	bool has_lci = false, has_lcr = false;
	u8 max_meas = 0, channel, *ptr;
	u32 i, cmd_len;
	struct ftm_session_start *cmd;
	struct sprdwl_msg_buf *msg;
	struct sprdwl_cmd_rtt *rtt;

	mutex_lock(&priv->ftm.lock);
	if (priv->ftm.session_started) {
		wl_err("%s: FTM session already running\n", __func__);
		ret = -EALREADY;
		goto out;
	}

	for (i = 0; i < request->n_peers; i++) {
		if (request->peers[i].flags &
		    SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCI)
			has_lci = true;
		if (request->peers[i].flags &
		    SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCR)
			has_lcr = true;
		max_meas = max(max_meas,
			       request->peers[i].params.meas_per_burst);
	}

	priv->ftm.ftm_res = kzalloc(sizeof(*priv->ftm.ftm_res) +
		      max_meas * sizeof(struct sprdwl_ftm_peer_meas) +
		      (has_lci ? FTM_MAX_LCI_LENGTH : 0) +
		      (has_lcr ? FTM_MAX_LCR_LENGTH : 0), GFP_KERNEL);
	if (!priv->ftm.ftm_res) {
		ret = -ENOMEM;
		goto out;
	}
	ptr = (u8 *)priv->ftm.ftm_res;
	ptr += sizeof(struct sprdwl_ftm_peer_meas_res) +
	       max_meas * sizeof(struct sprdwl_ftm_peer_meas);
	if (has_lci) {
		priv->ftm.ftm_res->lci = ptr;
		ptr += FTM_MAX_LCI_LENGTH;
	}
	if (has_lcr)
		priv->ftm.ftm_res->lcr = ptr;
	priv->ftm.max_ftm_meas = max_meas;

	cmd_len = sizeof(struct ftm_session_start) +
		  request->n_peers * sizeof(struct ftm_dest_info);
	cmd = kzalloc(cmd_len, GFP_KERNEL);
	if (!cmd) {
		ret = -ENOMEM;
		goto out_ftm_res;
	}

	cmd->session_id = cpu_to_le32(FTM_ESSION_ID);
	cmd->num_of_dest = cpu_to_le16(request->n_peers);
	for (i = 0; i < request->n_peers; i++) {
		ether_addr_copy(cmd->dest_info[i].dst_mac,
				request->peers[i].mac_addr);
		channel = sprdwl_ftm_get_channel(priv->wiphy,
						 request->peers[i].mac_addr,
						 request->peers[i].freq);
		if (!channel) {
			wl_err("%s: can't find FTM target at index %d\n",
			       __func__, i);
			ret = -EINVAL;
			goto out_cmd;
		}
		cmd->dest_info[i].channel = channel - 1;
		if (request->peers[i].flags &
		    SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_SECURE) {
			cmd->dest_info[i].flags |=
				FTM_SESSION_START_FLAG_SECURED;
			cmd->dest_info[i].initial_token =
				request->peers[i].secure_token_id;
		} else {
			cmd->dest_info[i].initial_token =
				FTM_DEFAULT_INITIAL_TOKEN;
		}
		if (request->peers[i].flags &
		    SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_ASAP)
			cmd->dest_info[i].flags |=
				FTM_SESSION_START_FLAG_ASAP;
		if (request->peers[i].flags &
		    SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCI)
			cmd->dest_info[i].flags |=
				FTM_SESSION_START_FLAG_LCI_REQ;
		if (request->peers[i].flags &
		    SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAG_LCR)
			cmd->dest_info[i].flags |=
				FTM_SESSION_START_FLAG_LCR_REQ;
		cmd->dest_info[i].num_of_ftm_per_burst =
			request->peers[i].params.meas_per_burst;
		cmd->dest_info[i].num_of_bursts_exp =
			request->peers[i].params.num_of_bursts_exp;
		cmd->dest_info[i].burst_duration =
			request->peers[i].params.burst_duration;
		cmd->dest_info[i].burst_period =
			cpu_to_le16(request->peers[i].params.burst_period);
	}

	/* send range request data to the FW */
	msg = sprdwl_cmd_getbuf(priv, sizeof(struct sprdwl_cmd_rtt) + cmd_len,
				vif->ctx_id, SPRDWL_HEAD_RSP, WIFI_CMD_RTT);
	if (!msg) {
		ret = -ENOMEM;
		goto out_cmd;
	}
	rtt = (struct sprdwl_cmd_rtt *)msg->data;
	rtt->sub_cmd = RTT_GET_CAPABILITIES;
	rtt->len = cmd_len;
	memcpy(rtt->data, cmd, cmd_len);

	ret = sprdwl_cmd_send_recv(priv, msg,
				   CMD_WAIT_TIMEOUT, NULL, 0);
	if (ret) {
		wl_ndev_log(L_ERR, vif->ndev, "%s: ret=%d\n", __func__, ret);
	} else {
		priv->ftm.session_cookie = request->session_cookie;
		priv->ftm.session_started = 1;
	}
out_cmd:
	kfree(cmd);
out_ftm_res:
	if (ret) {
		kfree(priv->ftm.ftm_res);
		priv->ftm.ftm_res = NULL;
	}
out:
	mutex_unlock(&priv->ftm.lock);
	return ret;
}

static void
sprdwl_ftm_session_ended(struct sprdwl_priv *priv, u32 status)
{
	struct sk_buff *skb = NULL;

	mutex_lock(&priv->ftm.lock);

	if (!priv->ftm.session_started) {
		wl_err("%s: FTM session not started, ignoring\n", __func__);
		return;
	}

	wl_info("%s: finishing FTM session\n", __func__);

	/* send left-over results if any */
	sprdwl_ftm_send_peer_res(priv);

	priv->ftm.session_started = 0;
	kfree(priv->ftm.ftm_res);
	priv->ftm.ftm_res = NULL;

	skb = cfg80211_vendor_event_alloc(
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 83)
		priv->wiphy, NULL,
#else
		priv->wiphy,
#endif
		FTM_NL_EXTRA_ALLOC,
		SPRD_VENDOR_EVENT_FTM_SESSION_DONE_INDEX,
		GFP_KERNEL);
	if (!skb)
		goto out;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 7, 0)
	if (nla_put_u64_64bit(skb,
				SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE,
				priv->ftm.session_cookie, 0) ||
	    nla_put_u32(skb,
			SPRDWL_VENDOR_ATTR_LOC_SESSION_STATUS, status)) {
#else
	if (nla_put_u64(skb,
			SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE,
			priv->ftm.session_cookie) ||
	    nla_put_u32(skb,
			SPRDWL_VENDOR_ATTR_LOC_SESSION_STATUS, status)) {
#endif
		wl_err("%s: failed to fill session done event\n", __func__);
		goto out;
	}
	cfg80211_vendor_event(skb, GFP_KERNEL);
	skb = NULL;
out:
	kfree_skb(skb);
	mutex_unlock(&priv->ftm.lock);
}

void sprdwl_ftm_event_per_dest_res(struct sprdwl_priv *priv,
				   struct ftm_per_dest_res *res)
{
	u32 i, index;
	__le64 tmp = 0;
	u8 n_meas;

	mutex_lock(&priv->ftm.lock);

	if (!priv->ftm.session_started || !priv->ftm.ftm_res) {
		wl_err("%s: Session not running, ignoring res event\n",
		       __func__);
		goto out;
	}
	if (priv->ftm.has_ftm_res &&
	    !ether_addr_equal(res->dst_mac, priv->ftm.ftm_res->mac_addr)) {
		wl_err("%s: previous peer not properly terminated\n",
		       __func__);
		sprdwl_ftm_send_peer_res(priv);
	}

	if (!priv->ftm.has_ftm_res) {
		ether_addr_copy(priv->ftm.ftm_res->mac_addr, res->dst_mac);
		priv->ftm.has_ftm_res = 1;
	}

	n_meas = res->actual_ftm_per_burst;
	switch (res->status) {
	case FTM_PER_DEST_RES_NO_ERROR:
		priv->ftm.ftm_res->status =
			SPRDWL_VENDOR_ATTR_FTM_PEER_RES_STATUS_OK;
		break;
	case FTM_PER_DEST_RES_TX_RX_FAIL:
		/* FW reports corrupted results here, discard. */
		n_meas = 0;
		priv->ftm.ftm_res->status =
			SPRDWL_VENDOR_ATTR_FTM_PEER_RES_STATUS_OK;
		break;
	case FTM_PER_DEST_RES_PARAM_DONT_MATCH:
		priv->ftm.ftm_res->status =
			SPRDWL_VENDOR_ATTR_FTM_PEER_RES_STATUS_INVALID;
		break;
	default:
		wl_err("%s: unexpected status %d\n", __func__, res->status);
		priv->ftm.ftm_res->status =
			SPRDWL_VENDOR_ATTR_FTM_PEER_RES_STATUS_INVALID;
		break;
	}

	for (i = 0; i < n_meas; i++) {
		index = priv->ftm.ftm_res->n_meas;
		if (index >= priv->ftm.max_ftm_meas) {
			wl_info("%s: Too many measurements\n", __func__);
			break;
		}
		memcpy(&tmp, res->responder_ftm_res[i].t1,
		       sizeof(res->responder_ftm_res[i].t1));
		priv->ftm.ftm_res->meas[index].t1 = le64_to_cpu(tmp);
		memcpy(&tmp, res->responder_ftm_res[i].t2,
		       sizeof(res->responder_ftm_res[i].t2));
		priv->ftm.ftm_res->meas[index].t2 = le64_to_cpu(tmp);
		memcpy(&tmp, res->responder_ftm_res[i].t3,
		       sizeof(res->responder_ftm_res[i].t3));
		priv->ftm.ftm_res->meas[index].t3 = le64_to_cpu(tmp);
		memcpy(&tmp, res->responder_ftm_res[i].t4,
		       sizeof(res->responder_ftm_res[i].t4));
		priv->ftm.ftm_res->meas[index].t4 = le64_to_cpu(tmp);
		priv->ftm.ftm_res->n_meas++;
	}

	if (res->flags & FTM_PER_DEST_RES_BURST_REPORT_END)
		sprdwl_ftm_send_peer_res(priv);
out:
	mutex_unlock(&priv->ftm.lock);
}

int sprdwl_event_ftm(struct sprdwl_vif *vif, u8 *data, u16 len)
{
	struct sprdwl_priv *priv = vif->priv;
	u8 sub_event;
	u32 status;
	struct ftm_per_dest_res *res;

	memcpy(&sub_event, data, sizeof(sub_event));
	data += sizeof(sub_event);
	len -= sizeof(sub_event);

	switch (sub_event) {
	case RTT_SESSION_END:
		memcpy(&status, data, sizeof(status));
		sprdwl_ftm_session_ended(priv, status);
		break;
	case RTT_PER_DEST_RES:
		res = (struct ftm_per_dest_res *)data;
		sprdwl_ftm_event_per_dest_res(priv, res);
		break;
	default:
		wl_ndev_log(L_ERR, vif->ndev, "%s: unknown FTM event\n", __func__);
		break;
	}
	return 0;
}

int sprdwl_ftm_get_capabilities(struct wiphy *wiphy,
				struct wireless_dev *wdev,
				const void *data, int len)
{
	struct sprdwl_msg_buf *msg;
	struct sprdwl_cmd_rtt *cmd;
	struct sprdwl_vif *vif = netdev_priv(wdev->netdev);
	u8 rsp[RTT_RSP_LEN] = {0x0};
	u16 rsp_len = RTT_RSP_LEN;
	int ret = 0;
	struct sk_buff *skb;
	struct nlattr *attr;

	/* get the capabilities from the FW */
	msg = sprdwl_cmd_getbuf(vif->priv, sizeof(struct sprdwl_cmd_rtt) + len,
				vif->ctx_id, SPRDWL_HEAD_RSP, WIFI_CMD_RTT);
	if (!msg)
		return -ENOMEM;
	cmd = (struct sprdwl_cmd_rtt *)msg->data;
	cmd->sub_cmd = RTT_GET_CAPABILITIES;
	cmd->len = len;
	memcpy(cmd->data, data, len);

	ret = sprdwl_cmd_send_recv(vif->priv, msg,
				   CMD_WAIT_TIMEOUT, rsp, &rsp_len);
	if (ret) {
		wl_ndev_log(L_ERR, vif->ndev,
			   "%s: ret=%d, rsp_len=%d\n", __func__, ret, rsp_len);
	}

	/* report capabilities */
	skb = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, RTT_RSP_LEN);
	if (!skb)
		return -ENOMEM;
	attr = nla_nest_start(skb, SPRDWL_VENDOR_ATTR_LOC_CAPA);
	if (!attr ||
	    nla_put_u32(skb, SPRDWL_VENDOR_ATTR_LOC_CAPA_FLAGS,
			SPRDWL_VENDOR_ATTR_LOC_CAPA_FLAG_FTM_RESPONDER |
			SPRDWL_VENDOR_ATTR_LOC_CAPA_FLAG_FTM_INITIATOR |
			SPRDWL_VENDOR_ATTR_LOC_CAPA_FLAG_ASAP |
			SPRDWL_VENDOR_ATTR_LOC_CAPA_FLAG_AOA) ||
	    nla_put_u16(skb, SPRDWL_VENDOR_ATTR_FTM_CAPA_MAX_NUM_SESSIONS,
			1) ||
	    nla_put_u16(skb, SPRDWL_VENDOR_ATTR_FTM_CAPA_MAX_NUM_PEERS, 1) ||
	    nla_put_u8(skb, SPRDWL_VENDOR_ATTR_FTM_CAPA_MAX_NUM_BURSTS_EXP,
		       0) ||
	    nla_put_u8(skb, SPRDWL_VENDOR_ATTR_FTM_CAPA_MAX_MEAS_PER_BURST,
		       4) ||
	    nla_put_u32(skb, SPRDWL_VENDOR_ATTR_AOA_CAPA_SUPPORTED_TYPES,
			BIT(SPRDWL_VENDOR_ATTR_AOA_TYPE_TOP_CIR_PHASE))) {
		wl_ndev_log(L_ERR, vif->ndev,
			   "%s: fail to fill capabilities\n", __func__);
		kfree_skb(skb);
		return -ENOMEM;
	}
	nla_nest_end(skb, attr);

	return cfg80211_vendor_cmd_reply(skb);
}

int sprdwl_ftm_start_session(struct wiphy *wiphy,
			     struct wireless_dev *wdev,
			     const void *data, int data_len)
{
	struct sprdwl_priv *priv = wiphy_priv(wiphy);
	struct sprdwl_vif *vif = netdev_priv(wdev->netdev);
	struct sprdwl_ftm_session_request *request;
	struct nlattr *tb[SPRDWL_VENDOR_ATTR_LOC_MAX + 1];
	struct nlattr *tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MAX + 1];
	struct nlattr *peer;
	int rc, n_peers = 0, index = 0, rem;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 14, 0)
	rc = nla_parse(tb, SPRDWL_VENDOR_ATTR_LOC_MAX, data, data_len,
		       sprdwl_nl80211_loc_policy, NULL);
#else
	rc = nla_parse(tb, SPRDWL_VENDOR_ATTR_LOC_MAX, data, data_len,
			sprdwl_nl80211_loc_policy);
#endif
	if (rc) {
		wl_ndev_log(L_ERR, vif->ndev, "%s: invalid FTM attribute\n", __func__);
		return rc;
	}

	if (!tb[SPRDWL_VENDOR_ATTR_FTM_MEAS_PEERS]) {
		wl_ndev_log(L_ERR, vif->ndev, "%s: no peers specified\n", __func__);
		return -EINVAL;
	}

	if (!tb[SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE]) {
		wl_ndev_log(L_ERR, vif->ndev,
			   "%s: session cookie not specified\n", __func__);
		return -EINVAL;
	}

	nla_for_each_nested(peer, tb[SPRDWL_VENDOR_ATTR_FTM_MEAS_PEERS],
			    rem)
		n_peers++;

	if (!n_peers) {
		wl_ndev_log(L_ERR, vif->ndev, "%s: empty peer list\n", __func__);
		return -EINVAL;
	}

	/* for now only allow measurement for a single peer */
	if (n_peers != 1) {
		wl_ndev_log(L_ERR, vif->ndev,
			   "%s: only single peer allowed\n", __func__);
		return -EINVAL;
	}

	request = kzalloc(sizeof(*request) +
			  n_peers * sizeof(struct sprdwl_ftm_meas_peer_info),
			  GFP_KERNEL);
	if (!request)
		return -ENOMEM;

	request->session_cookie =
		nla_get_u64(tb[SPRDWL_VENDOR_ATTR_FTM_SESSION_COOKIE]);
	request->n_peers = n_peers;
	nla_for_each_nested(peer, tb[SPRDWL_VENDOR_ATTR_FTM_MEAS_PEERS],
			    rem) {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 14, 0)
		rc = nla_parse_nested(tb2, SPRDWL_VENDOR_ATTR_FTM_PEER_MAX,
				      peer, sprdwl_nl80211_ftm_peer_policy, NULL);
#else
		rc = nla_parse_nested(tb2, SPRDWL_VENDOR_ATTR_FTM_PEER_MAX,
				peer, sprdwl_nl80211_ftm_peer_policy);
#endif
		if (rc) {
			wl_ndev_log(L_ERR, vif->ndev,
				   "%s: invalid peer attribute\n", __func__);
			goto out;
		}
		if (!tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MAC_ADDR] ||
		    nla_len(tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MAC_ADDR])
			    != ETH_ALEN) {
			wl_ndev_log(L_ERR, vif->ndev,
				   "%s: peer MAC address missing or invalid\n",
				   __func__);
			rc = -EINVAL;
			goto out;
		}
		memcpy(request->peers[index].mac_addr,
		       nla_data(tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MAC_ADDR]),
		       ETH_ALEN);
		if (tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_FREQ])
			request->peers[index].freq = nla_get_u32(
				tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_FREQ]);
		if (tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAGS])
			request->peers[index].flags = nla_get_u32(
				tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_FLAGS]);
		if (tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_SECURE_TOKEN_ID])
			request->peers[index].secure_token_id = nla_get_u8(
			   tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_SECURE_TOKEN_ID]);
		rc = sprdwl_ftm_parse_meas_params(
			vif,
			tb2[SPRDWL_VENDOR_ATTR_FTM_PEER_MEAS_PARAMS],
			&request->peers[index].params);
		if (!rc)
			rc = sprdwl_ftm_validate_meas_params(
				vif, &request->peers[index].params);
		if (rc)
			goto out;
		index++;
	}

	rc = sprdwl_ftm_cfg80211_start_session(priv, vif, request);
out:
	kfree(request);
	return rc;
}

int sprdwl_ftm_abort_session(struct wiphy *wiphy,
			     struct wireless_dev *wdev,
			     const void *data, int len)
{
	struct sprdwl_msg_buf *msg;
	struct sprdwl_cmd_rtt *cmd;
	struct sprdwl_priv *priv = wiphy_priv(wiphy);
	struct sprdwl_vif *vif = netdev_priv(wdev->netdev);
	int ret;

	mutex_lock(&priv->ftm.lock);
	if (!priv->ftm.session_started) {
		wl_ndev_log(L_ERR, vif->ndev,
			   "%s: FTM session not started\n", __func__);
		return -EAGAIN;
	}
	/* send cancel range request */
	msg = sprdwl_cmd_getbuf(priv, sizeof(struct sprdwl_cmd_rtt) + len,
				vif->ctx_id, 0, WIFI_CMD_RTT);
	if (!msg)
		return -ENOMEM;
	cmd = (struct sprdwl_cmd_rtt *)msg->data;
	cmd->sub_cmd = RTT_RANGE_CANCEL;
	cmd->len = len;
	memcpy(cmd->data, data, len);

	ret = sprdwl_cmd_send_recv(priv, msg,
				   CMD_WAIT_TIMEOUT, NULL, 0);
	if (ret)
		wl_ndev_log(L_ERR, vif->ndev, "%s: ret=%d\n", __func__, ret);

	mutex_unlock(&priv->ftm.lock);

	return ret;
}

int sprdwl_ftm_get_responder_info(struct wiphy *wiphy,
				  struct wireless_dev *wdev,
				  const void *data, int len)
{
	struct sprdwl_vif *vif = netdev_priv(wdev->netdev);

	/* get responder info */
	wl_ndev_log(L_INFO, vif->ndev, "%s: not implemented yet\n", __func__);
	return -ENOTSUPP;
}

int sprdwl_ftm_configure_responder(struct wiphy *wiphy,
				   struct wireless_dev *wdev,
				   const void *data, int data_len)
{
	struct sprdwl_vif *vif = netdev_priv(wdev->netdev);

	/* enable or disable responder */
	wl_ndev_log(L_INFO, vif->ndev, "%s: not implemented yet\n", __func__);
	return -ENOTSUPP;
}

void sprdwl_ftm_init(struct sprdwl_priv *priv)
{
	mutex_init(&priv->ftm.lock);
}

void sprdwl_ftm_deinit(struct sprdwl_priv *priv)
{
	kfree(priv->ftm.ftm_res);
}

void sprdwl_ftm_stop_operations(struct sprdwl_priv *priv)
{
	sprdwl_ftm_session_ended(
		priv, SPRDWL_VENDOR_ATTR_LOC_SESSION_STATUS_ABORTED);
}
#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 0) */