[mirror_ubuntu-bionic-kernel.git] / net / mac80211 / tdls.c

/*
 * mac80211 TDLS handling code
 *
 * Copyright 2006-2010	Johannes Berg <johannes@sipsolutions.net>
 * Copyright 2014, Intel Corporation
 *
 * This file is GPLv2 as found in COPYING.
 */

#include <linux/ieee80211.h>
#include <net/cfg80211.h>
#include "ieee80211_i.h"
#include "driver-ops.h"

/* give usermode some time for retries in setting up the TDLS session */
#define TDLS_PEER_SETUP_TIMEOUT	(15 * HZ)

void ieee80211_tdls_peer_del_work(struct work_struct *wk)
{
	struct ieee80211_sub_if_data *sdata;
	struct ieee80211_local *local;

	sdata = container_of(wk, struct ieee80211_sub_if_data,
			     tdls_peer_del_work.work);
	local = sdata->local;

	mutex_lock(&local->mtx);
	if (!is_zero_ether_addr(sdata->tdls_peer)) {
		tdls_dbg(sdata, "TDLS del peer %pM\n", sdata->tdls_peer);
		sta_info_destroy_addr(sdata, sdata->tdls_peer);
		eth_zero_addr(sdata->tdls_peer);
	}
	mutex_unlock(&local->mtx);
}

static void ieee80211_tdls_add_ext_capab(struct sk_buff *skb)
{
	u8 *pos = (void *)skb_put(skb, 7);

	*pos++ = WLAN_EID_EXT_CAPABILITY;
	*pos++ = 5; /* len */
	*pos++ = 0x0;
	*pos++ = 0x0;
	*pos++ = 0x0;
	*pos++ = 0x0;
	*pos++ = WLAN_EXT_CAPA5_TDLS_ENABLED;
}

static u16 ieee80211_get_tdls_sta_capab(struct ieee80211_sub_if_data *sdata)
{
	struct ieee80211_local *local = sdata->local;
	u16 capab;

	capab = 0;
	if (ieee80211_get_sdata_band(sdata) != IEEE80211_BAND_2GHZ)
		return capab;

	if (!(local->hw.flags & IEEE80211_HW_2GHZ_SHORT_SLOT_INCAPABLE))
		capab |= WLAN_CAPABILITY_SHORT_SLOT_TIME;
	if (!(local->hw.flags & IEEE80211_HW_2GHZ_SHORT_PREAMBLE_INCAPABLE))
		capab |= WLAN_CAPABILITY_SHORT_PREAMBLE;

	return capab;
}

static void ieee80211_tdls_add_link_ie(struct sk_buff *skb, const u8 *src_addr,
				       const u8 *peer, const u8 *bssid)
{
	struct ieee80211_tdls_lnkie *lnkid;

	lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));

	lnkid->ie_type = WLAN_EID_LINK_ID;
	lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) - 2;

	memcpy(lnkid->bssid, bssid, ETH_ALEN);
	memcpy(lnkid->init_sta, src_addr, ETH_ALEN);
	memcpy(lnkid->resp_sta, peer, ETH_ALEN);
}

static int
ieee80211_prep_tdls_encap_data(struct wiphy *wiphy, struct net_device *dev,
			       const u8 *peer, u8 action_code, u8 dialog_token,
			       u16 status_code, struct sk_buff *skb)
{
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	enum ieee80211_band band = ieee80211_get_sdata_band(sdata);
	struct ieee80211_tdls_data *tf;

	tf = (void *)skb_put(skb, offsetof(struct ieee80211_tdls_data, u));

	memcpy(tf->da, peer, ETH_ALEN);
	memcpy(tf->sa, sdata->vif.addr, ETH_ALEN);
	tf->ether_type = cpu_to_be16(ETH_P_TDLS);
	tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;

	switch (action_code) {
	case WLAN_TDLS_SETUP_REQUEST:
		tf->category = WLAN_CATEGORY_TDLS;
		tf->action_code = WLAN_TDLS_SETUP_REQUEST;

		skb_put(skb, sizeof(tf->u.setup_req));
		tf->u.setup_req.dialog_token = dialog_token;
		tf->u.setup_req.capability =
			cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata));

		ieee80211_add_srates_ie(sdata, skb, false, band);
		ieee80211_add_ext_srates_ie(sdata, skb, false, band);
		ieee80211_tdls_add_ext_capab(skb);
		break;
	case WLAN_TDLS_SETUP_RESPONSE:
		tf->category = WLAN_CATEGORY_TDLS;
		tf->action_code = WLAN_TDLS_SETUP_RESPONSE;

		skb_put(skb, sizeof(tf->u.setup_resp));
		tf->u.setup_resp.status_code = cpu_to_le16(status_code);
		tf->u.setup_resp.dialog_token = dialog_token;
		tf->u.setup_resp.capability =
			cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata));

		ieee80211_add_srates_ie(sdata, skb, false, band);
		ieee80211_add_ext_srates_ie(sdata, skb, false, band);
		ieee80211_tdls_add_ext_capab(skb);
		break;
	case WLAN_TDLS_SETUP_CONFIRM:
		tf->category = WLAN_CATEGORY_TDLS;
		tf->action_code = WLAN_TDLS_SETUP_CONFIRM;

		skb_put(skb, sizeof(tf->u.setup_cfm));
		tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
		tf->u.setup_cfm.dialog_token = dialog_token;
		break;
	case WLAN_TDLS_TEARDOWN:
		tf->category = WLAN_CATEGORY_TDLS;
		tf->action_code = WLAN_TDLS_TEARDOWN;

		skb_put(skb, sizeof(tf->u.teardown));
		tf->u.teardown.reason_code = cpu_to_le16(status_code);
		break;
	case WLAN_TDLS_DISCOVERY_REQUEST:
		tf->category = WLAN_CATEGORY_TDLS;
		tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;

		skb_put(skb, sizeof(tf->u.discover_req));
		tf->u.discover_req.dialog_token = dialog_token;
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int
ieee80211_prep_tdls_direct(struct wiphy *wiphy, struct net_device *dev,
			   const u8 *peer, u8 action_code, u8 dialog_token,
			   u16 status_code, struct sk_buff *skb)
{
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	enum ieee80211_band band = ieee80211_get_sdata_band(sdata);
	struct ieee80211_mgmt *mgmt;

	mgmt = (void *)skb_put(skb, 24);
	memset(mgmt, 0, 24);
	memcpy(mgmt->da, peer, ETH_ALEN);
	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
	memcpy(mgmt->bssid, sdata->u.mgd.bssid, ETH_ALEN);

	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
					  IEEE80211_STYPE_ACTION);

	switch (action_code) {
	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
		skb_put(skb, 1 + sizeof(mgmt->u.action.u.tdls_discover_resp));
		mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
		mgmt->u.action.u.tdls_discover_resp.action_code =
			WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
		mgmt->u.action.u.tdls_discover_resp.dialog_token =
			dialog_token;
		mgmt->u.action.u.tdls_discover_resp.capability =
			cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata));

		ieee80211_add_srates_ie(sdata, skb, false, band);
		ieee80211_add_ext_srates_ie(sdata, skb, false, band);
		ieee80211_tdls_add_ext_capab(skb);
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int
ieee80211_tdls_prep_mgmt_packet(struct wiphy *wiphy, struct net_device *dev,
				const u8 *peer, u8 action_code,
				u8 dialog_token, u16 status_code,
				u32 peer_capability, bool initiator,
				const u8 *extra_ies, size_t extra_ies_len)
{
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	struct ieee80211_local *local = sdata->local;
	struct sk_buff *skb = NULL;
	bool send_direct;
	const u8 *init_addr, *rsp_addr;
	struct sta_info *sta;
	int ret;

	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
			    max(sizeof(struct ieee80211_mgmt),
				sizeof(struct ieee80211_tdls_data)) +
			    50 + /* supported rates */
			    7 + /* ext capab */
			    extra_ies_len +
			    sizeof(struct ieee80211_tdls_lnkie));
	if (!skb)
		return -ENOMEM;

	skb_reserve(skb, local->hw.extra_tx_headroom);

	switch (action_code) {
	case WLAN_TDLS_SETUP_REQUEST:
	case WLAN_TDLS_SETUP_RESPONSE:
	case WLAN_TDLS_SETUP_CONFIRM:
	case WLAN_TDLS_TEARDOWN:
	case WLAN_TDLS_DISCOVERY_REQUEST:
		ret = ieee80211_prep_tdls_encap_data(wiphy, dev, peer,
						     action_code, dialog_token,
						     status_code, skb);
		send_direct = false;
		break;
	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
		ret = ieee80211_prep_tdls_direct(wiphy, dev, peer, action_code,
						 dialog_token, status_code,
						 skb);
		send_direct = true;
		break;
	default:
		ret = -ENOTSUPP;
		break;
	}

	if (ret < 0)
		goto fail;

	if (extra_ies_len)
		memcpy(skb_put(skb, extra_ies_len), extra_ies, extra_ies_len);

	rcu_read_lock();
	sta = sta_info_get(sdata, peer);

	/* infer the initiator if we can, to support old userspace */
	switch (action_code) {
	case WLAN_TDLS_SETUP_REQUEST:
		if (sta)
			set_sta_flag(sta, WLAN_STA_TDLS_INITIATOR);
		/* fall-through */
	case WLAN_TDLS_SETUP_CONFIRM:
	case WLAN_TDLS_DISCOVERY_REQUEST:
		initiator = true;
		break;
	case WLAN_TDLS_SETUP_RESPONSE:
		/*
		 * In some testing scenarios, we send a request and response.
		 * Make the last packet sent take effect for the initiator
		 * value.
		 */
		if (sta)
			clear_sta_flag(sta, WLAN_STA_TDLS_INITIATOR);
		/* fall-through */
	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
		initiator = false;
		break;
	case WLAN_TDLS_TEARDOWN:
		/* any value is ok */
		break;
	default:
		ret = -ENOTSUPP;
		break;
	}

	if (initiator || (sta && test_sta_flag(sta, WLAN_STA_TDLS_INITIATOR))) {
		init_addr = sdata->vif.addr;
		rsp_addr = peer;
	} else {
		init_addr = peer;
		rsp_addr = sdata->vif.addr;
	}

	rcu_read_unlock();
	if (ret < 0)
		goto fail;

	ieee80211_tdls_add_link_ie(skb, init_addr, rsp_addr,
				   sdata->u.mgd.bssid);

	if (send_direct) {
		ieee80211_tx_skb(sdata, skb);
		return 0;
	}

	/*
	 * According to 802.11z: Setup req/resp are sent in AC_BK, otherwise
	 * we should default to AC_VI.
	 */
	switch (action_code) {
	case WLAN_TDLS_SETUP_REQUEST:
	case WLAN_TDLS_SETUP_RESPONSE:
		skb_set_queue_mapping(skb, IEEE80211_AC_BK);
		skb->priority = 2;
		break;
	default:
		skb_set_queue_mapping(skb, IEEE80211_AC_VI);
		skb->priority = 5;
		break;
	}

	/* disable bottom halves when entering the Tx path */
	local_bh_disable();
	ret = ieee80211_subif_start_xmit(skb, dev);
	local_bh_enable();

	return ret;

fail:
	dev_kfree_skb(skb);
	return ret;
}

static int
ieee80211_tdls_mgmt_setup(struct wiphy *wiphy, struct net_device *dev,
			  const u8 *peer, u8 action_code, u8 dialog_token,
			  u16 status_code, u32 peer_capability, bool initiator,
			  const u8 *extra_ies, size_t extra_ies_len)
{
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	struct ieee80211_local *local = sdata->local;
	int ret;

	mutex_lock(&local->mtx);

	/* we don't support concurrent TDLS peer setups */
	if (!is_zero_ether_addr(sdata->tdls_peer) &&
	    !ether_addr_equal(sdata->tdls_peer, peer)) {
		ret = -EBUSY;
		goto exit;
	}

	/*
	 * make sure we have a STA representing the peer so we drop or buffer
	 * non-TDLS-setup frames to the peer. We can't send other packets
	 * during setup through the AP path
	 */
	rcu_read_lock();
	if (!sta_info_get(sdata, peer)) {
		rcu_read_unlock();
		ret = -ENOLINK;
		goto exit;
	}
	rcu_read_unlock();

	ieee80211_flush_queues(local, sdata);

	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
					      dialog_token, status_code,
					      peer_capability, initiator,
					      extra_ies, extra_ies_len);
	if (ret < 0)
		goto exit;

	memcpy(sdata->tdls_peer, peer, ETH_ALEN);
	ieee80211_queue_delayed_work(&sdata->local->hw,
				     &sdata->tdls_peer_del_work,
				     TDLS_PEER_SETUP_TIMEOUT);

exit:
	mutex_unlock(&local->mtx);
	return ret;
}

static int
ieee80211_tdls_mgmt_teardown(struct wiphy *wiphy, struct net_device *dev,
			     const u8 *peer, u8 action_code, u8 dialog_token,
			     u16 status_code, u32 peer_capability,
			     bool initiator, const u8 *extra_ies,
			     size_t extra_ies_len)
{
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	struct ieee80211_local *local = sdata->local;
	struct sta_info *sta;
	int ret;

	/*
	 * No packets can be transmitted to the peer via the AP during setup -
	 * the STA is set as a TDLS peer, but is not authorized.
	 * During teardown, we prevent direct transmissions by stopping the
	 * queues and flushing all direct packets.
	 */
	ieee80211_stop_vif_queues(local, sdata,
				  IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
	ieee80211_flush_queues(local, sdata);

	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
					      dialog_token, status_code,
					      peer_capability, initiator,
					      extra_ies, extra_ies_len);
	if (ret < 0)
		sdata_err(sdata, "Failed sending TDLS teardown packet %d\n",
			  ret);

	/*
	 * Remove the STA AUTH flag to force further traffic through the AP. If
	 * the STA was unreachable, it was already removed.
	 */
	rcu_read_lock();
	sta = sta_info_get(sdata, peer);
	if (sta)
		clear_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
	rcu_read_unlock();

	ieee80211_wake_vif_queues(local, sdata,
				  IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);

	return 0;
}

int ieee80211_tdls_mgmt(struct wiphy *wiphy, struct net_device *dev,
			const u8 *peer, u8 action_code, u8 dialog_token,
			u16 status_code, u32 peer_capability,
			bool initiator, const u8 *extra_ies,
			size_t extra_ies_len)
{
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	int ret;

	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
		return -ENOTSUPP;

	/* make sure we are in managed mode, and associated */
	if (sdata->vif.type != NL80211_IFTYPE_STATION ||
	    !sdata->u.mgd.associated)
		return -EINVAL;

	switch (action_code) {
	case WLAN_TDLS_SETUP_REQUEST:
	case WLAN_TDLS_SETUP_RESPONSE:
		ret = ieee80211_tdls_mgmt_setup(wiphy, dev, peer, action_code,
						dialog_token, status_code,
						peer_capability, initiator,
						extra_ies, extra_ies_len);
		break;
	case WLAN_TDLS_TEARDOWN:
		ret = ieee80211_tdls_mgmt_teardown(wiphy, dev, peer,
						   action_code, dialog_token,
						   status_code,
						   peer_capability, initiator,
						   extra_ies, extra_ies_len);
		break;
	case WLAN_TDLS_DISCOVERY_REQUEST:
		/*
		 * Protect the discovery so we can hear the TDLS discovery
		 * response frame. It is transmitted directly and not buffered
		 * by the AP.
		 */
		drv_mgd_protect_tdls_discover(sdata->local, sdata);
		/* fall-through */
	case WLAN_TDLS_SETUP_CONFIRM:
	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
		/* no special handling */
		ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer,
						      action_code,
						      dialog_token,
						      status_code,
						      peer_capability,
						      initiator, extra_ies,
						      extra_ies_len);
		break;
	default:
		ret = -EOPNOTSUPP;
		break;
	}

	tdls_dbg(sdata, "TDLS mgmt action %d peer %pM status %d\n",
		 action_code, peer, ret);
	return ret;
}

int ieee80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
			const u8 *peer, enum nl80211_tdls_operation oper)
{
	struct sta_info *sta;
	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	struct ieee80211_local *local = sdata->local;
	int ret;

	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
		return -ENOTSUPP;

	if (sdata->vif.type != NL80211_IFTYPE_STATION)
		return -EINVAL;

	switch (oper) {
	case NL80211_TDLS_ENABLE_LINK:
	case NL80211_TDLS_DISABLE_LINK:
		break;
	case NL80211_TDLS_TEARDOWN:
	case NL80211_TDLS_SETUP:
	case NL80211_TDLS_DISCOVERY_REQ:
		/* We don't support in-driver setup/teardown/discovery */
		return -ENOTSUPP;
	}

	mutex_lock(&local->mtx);
	tdls_dbg(sdata, "TDLS oper %d peer %pM\n", oper, peer);

	switch (oper) {
	case NL80211_TDLS_ENABLE_LINK:
		rcu_read_lock();
		sta = sta_info_get(sdata, peer);
		if (!sta) {
			rcu_read_unlock();
			ret = -ENOLINK;
			break;
		}

		set_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
		rcu_read_unlock();

		WARN_ON_ONCE(is_zero_ether_addr(sdata->tdls_peer) ||
			     !ether_addr_equal(sdata->tdls_peer, peer));
		ret = 0;
		break;
	case NL80211_TDLS_DISABLE_LINK:
		/* flush a potentially queued teardown packet */
		ieee80211_flush_queues(local, sdata);

		ret = sta_info_destroy_addr(sdata, peer);
		break;
	default:
		ret = -ENOTSUPP;
		break;
	}

	if (ret == 0 && ether_addr_equal(sdata->tdls_peer, peer)) {
		cancel_delayed_work(&sdata->tdls_peer_del_work);
		eth_zero_addr(sdata->tdls_peer);
	}

	mutex_unlock(&local->mtx);
	return ret;
}

void ieee80211_tdls_oper_request(struct ieee80211_vif *vif, const u8 *peer,
				 enum nl80211_tdls_operation oper,
				 u16 reason_code, gfp_t gfp)
{
	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);

	if (vif->type != NL80211_IFTYPE_STATION || !vif->bss_conf.assoc) {
		sdata_err(sdata, "Discarding TDLS oper %d - not STA or disconnected\n",
			  oper);
		return;
	}

	cfg80211_tdls_oper_request(sdata->dev, peer, oper, reason_code, gfp);
}
EXPORT_SYMBOL(ieee80211_tdls_oper_request);
Commit	Line	Data
95224fe8 AN	1	/*
	2	* mac80211 TDLS handling code
	3	*
	4	* Copyright 2006-2010 Johannes Berg <johannes@sipsolutions.net>
	5	* Copyright 2014, Intel Corporation
	6	*
	7	* This file is GPLv2 as found in COPYING.
	8	*/
	9
	10	#include <linux/ieee80211.h>
c887f0d3	11	#include <net/cfg80211.h>
95224fe8	12	#include "ieee80211_i.h"
ee10f2c7	13	#include "driver-ops.h"
95224fe8	14
17e6a59a AN	15	/* give usermode some time for retries in setting up the TDLS session */
	16	#define TDLS_PEER_SETUP_TIMEOUT (15 * HZ)
	17
	18	void ieee80211_tdls_peer_del_work(struct work_struct *wk)
	19	{
	20	struct ieee80211_sub_if_data *sdata;
	21	struct ieee80211_local *local;
	22
	23	sdata = container_of(wk, struct ieee80211_sub_if_data,
	24	tdls_peer_del_work.work);
	25	local = sdata->local;
	26
	27	mutex_lock(&local->mtx);
	28	if (!is_zero_ether_addr(sdata->tdls_peer)) {
	29	tdls_dbg(sdata, "TDLS del peer %pM\n", sdata->tdls_peer);
	30	sta_info_destroy_addr(sdata, sdata->tdls_peer);
	31	eth_zero_addr(sdata->tdls_peer);
	32	}
	33	mutex_unlock(&local->mtx);
	34	}
	35
95224fe8 AN	36	static void ieee80211_tdls_add_ext_capab(struct sk_buff *skb)
	37	{
	38	u8 pos = (void )skb_put(skb, 7);
	39
	40	*pos++ = WLAN_EID_EXT_CAPABILITY;
	41	pos++ = 5; / len */
	42	*pos++ = 0x0;
	43	*pos++ = 0x0;
	44	*pos++ = 0x0;
	45	*pos++ = 0x0;
	46	*pos++ = WLAN_EXT_CAPA5_TDLS_ENABLED;
	47	}
	48
	49	static u16 ieee80211_get_tdls_sta_capab(struct ieee80211_sub_if_data *sdata)
	50	{
	51	struct ieee80211_local *local = sdata->local;
	52	u16 capab;
	53
	54	capab = 0;
	55	if (ieee80211_get_sdata_band(sdata) != IEEE80211_BAND_2GHZ)
	56	return capab;
	57
	58	if (!(local->hw.flags & IEEE80211_HW_2GHZ_SHORT_SLOT_INCAPABLE))
	59	capab \|= WLAN_CAPABILITY_SHORT_SLOT_TIME;
	60	if (!(local->hw.flags & IEEE80211_HW_2GHZ_SHORT_PREAMBLE_INCAPABLE))
	61	capab \|= WLAN_CAPABILITY_SHORT_PREAMBLE;
	62
	63	return capab;
	64	}
	65
3b3a0162 JB	66	static void ieee80211_tdls_add_link_ie(struct sk_buff skb, const u8 src_addr,
3b3a0162 JB	67	const u8 peer, const u8 bssid)
95224fe8 AN	68	{
	69	struct ieee80211_tdls_lnkie *lnkid;
	70
	71	lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
	72
	73	lnkid->ie_type = WLAN_EID_LINK_ID;
	74	lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) - 2;
	75
	76	memcpy(lnkid->bssid, bssid, ETH_ALEN);
	77	memcpy(lnkid->init_sta, src_addr, ETH_ALEN);
	78	memcpy(lnkid->resp_sta, peer, ETH_ALEN);
	79	}
	80
	81	static int
	82	ieee80211_prep_tdls_encap_data(struct wiphy wiphy, struct net_device dev,
3b3a0162	83	const u8 *peer, u8 action_code, u8 dialog_token,
95224fe8 AN	84	u16 status_code, struct sk_buff *skb)
	85	{
	86	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	87	enum ieee80211_band band = ieee80211_get_sdata_band(sdata);
	88	struct ieee80211_tdls_data *tf;
	89
	90	tf = (void *)skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
	91
	92	memcpy(tf->da, peer, ETH_ALEN);
	93	memcpy(tf->sa, sdata->vif.addr, ETH_ALEN);
	94	tf->ether_type = cpu_to_be16(ETH_P_TDLS);
	95	tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
	96
	97	switch (action_code) {
	98	case WLAN_TDLS_SETUP_REQUEST:
	99	tf->category = WLAN_CATEGORY_TDLS;
	100	tf->action_code = WLAN_TDLS_SETUP_REQUEST;
	101
	102	skb_put(skb, sizeof(tf->u.setup_req));
	103	tf->u.setup_req.dialog_token = dialog_token;
	104	tf->u.setup_req.capability =
	105	cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata));
	106
	107	ieee80211_add_srates_ie(sdata, skb, false, band);
	108	ieee80211_add_ext_srates_ie(sdata, skb, false, band);
	109	ieee80211_tdls_add_ext_capab(skb);
	110	break;
	111	case WLAN_TDLS_SETUP_RESPONSE:
	112	tf->category = WLAN_CATEGORY_TDLS;
	113	tf->action_code = WLAN_TDLS_SETUP_RESPONSE;
	114
	115	skb_put(skb, sizeof(tf->u.setup_resp));
	116	tf->u.setup_resp.status_code = cpu_to_le16(status_code);
	117	tf->u.setup_resp.dialog_token = dialog_token;
	118	tf->u.setup_resp.capability =
	119	cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata));
	120
	121	ieee80211_add_srates_ie(sdata, skb, false, band);
	122	ieee80211_add_ext_srates_ie(sdata, skb, false, band);
	123	ieee80211_tdls_add_ext_capab(skb);
	124	break;
	125	case WLAN_TDLS_SETUP_CONFIRM:
	126	tf->category = WLAN_CATEGORY_TDLS;
	127	tf->action_code = WLAN_TDLS_SETUP_CONFIRM;
	128
	129	skb_put(skb, sizeof(tf->u.setup_cfm));
	130	tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
	131	tf->u.setup_cfm.dialog_token = dialog_token;
	132	break;
	133	case WLAN_TDLS_TEARDOWN:
	134	tf->category = WLAN_CATEGORY_TDLS;
	135	tf->action_code = WLAN_TDLS_TEARDOWN;
	136
	137	skb_put(skb, sizeof(tf->u.teardown));
	138	tf->u.teardown.reason_code = cpu_to_le16(status_code);
	139	break;
	140	case WLAN_TDLS_DISCOVERY_REQUEST:
	141	tf->category = WLAN_CATEGORY_TDLS;
	142	tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;
	143
	144	skb_put(skb, sizeof(tf->u.discover_req));
	145	tf->u.discover_req.dialog_token = dialog_token;
	146	break;
	147	default:
148	return -EINVAL;
149	}
150
151	return 0;
152	}
153
154	static int
155	ieee80211_prep_tdls_direct(struct wiphy wiphy, struct net_device dev,
3b3a0162	156	const u8 *peer, u8 action_code, u8 dialog_token,
95224fe8 AN	157	u16 status_code, struct sk_buff *skb)
	158	{
	159	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	160	enum ieee80211_band band = ieee80211_get_sdata_band(sdata);
	161	struct ieee80211_mgmt *mgmt;
	162
	163	mgmt = (void *)skb_put(skb, 24);
	164	memset(mgmt, 0, 24);
	165	memcpy(mgmt->da, peer, ETH_ALEN);
	166	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
	167	memcpy(mgmt->bssid, sdata->u.mgd.bssid, ETH_ALEN);
	168
	169	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT \|
	170	IEEE80211_STYPE_ACTION);
	171
	172	switch (action_code) {
	173	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
	174	skb_put(skb, 1 + sizeof(mgmt->u.action.u.tdls_discover_resp));
	175	mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
	176	mgmt->u.action.u.tdls_discover_resp.action_code =
	177	WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
	178	mgmt->u.action.u.tdls_discover_resp.dialog_token =
	179	dialog_token;
	180	mgmt->u.action.u.tdls_discover_resp.capability =
	181	cpu_to_le16(ieee80211_get_tdls_sta_capab(sdata));
	182
	183	ieee80211_add_srates_ie(sdata, skb, false, band);
	184	ieee80211_add_ext_srates_ie(sdata, skb, false, band);
	185	ieee80211_tdls_add_ext_capab(skb);
	186	break;
	187	default:
	188	return -EINVAL;
	189	}
	190
	191	return 0;
	192	}
	193
17e6a59a AN	194	static int
	195	ieee80211_tdls_prep_mgmt_packet(struct wiphy wiphy, struct net_device dev,
	196	const u8 *peer, u8 action_code,
	197	u8 dialog_token, u16 status_code,
2fb6b9b8 AN	198	u32 peer_capability, bool initiator,
2fb6b9b8 AN	199	const u8 *extra_ies, size_t extra_ies_len)
95224fe8 AN	200	{
	201	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	202	struct ieee80211_local *local = sdata->local;
	203	struct sk_buff *skb = NULL;
	204	bool send_direct;
2fb6b9b8	205	const u8 init_addr, rsp_addr;
626911cc	206	struct sta_info *sta;
95224fe8 AN	207	int ret;
95224fe8 AN	208
95224fe8 AN	209	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
	210	max(sizeof(struct ieee80211_mgmt),
	211	sizeof(struct ieee80211_tdls_data)) +
	212	50 + /* supported rates */
	213	7 + /* ext capab */
	214	extra_ies_len +
	215	sizeof(struct ieee80211_tdls_lnkie));
	216	if (!skb)
	217	return -ENOMEM;
	218
	219	skb_reserve(skb, local->hw.extra_tx_headroom);
	220
	221	switch (action_code) {
	222	case WLAN_TDLS_SETUP_REQUEST:
	223	case WLAN_TDLS_SETUP_RESPONSE:
	224	case WLAN_TDLS_SETUP_CONFIRM:
	225	case WLAN_TDLS_TEARDOWN:
	226	case WLAN_TDLS_DISCOVERY_REQUEST:
	227	ret = ieee80211_prep_tdls_encap_data(wiphy, dev, peer,
	228	action_code, dialog_token,
	229	status_code, skb);
	230	send_direct = false;
	231	break;
	232	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
	233	ret = ieee80211_prep_tdls_direct(wiphy, dev, peer, action_code,
	234	dialog_token, status_code,
	235	skb);
	236	send_direct = true;
	237	break;
	238	default:
	239	ret = -ENOTSUPP;
	240	break;
	241	}
	242
	243	if (ret < 0)
	244	goto fail;
	245
	246	if (extra_ies_len)
	247	memcpy(skb_put(skb, extra_ies_len), extra_ies, extra_ies_len);
	248
626911cc AN	249	rcu_read_lock();
	250	sta = sta_info_get(sdata, peer);
	251
	252	/* infer the initiator if we can, to support old userspace */
95224fe8 AN	253	switch (action_code) {
95224fe8 AN	254	case WLAN_TDLS_SETUP_REQUEST:
626911cc AN	255	if (sta)
	256	set_sta_flag(sta, WLAN_STA_TDLS_INITIATOR);
	257	/* fall-through */
95224fe8	258	case WLAN_TDLS_SETUP_CONFIRM:
95224fe8	259	case WLAN_TDLS_DISCOVERY_REQUEST:
626911cc	260	initiator = true;
95224fe8 AN	261	break;
95224fe8 AN	262	case WLAN_TDLS_SETUP_RESPONSE:
626911cc AN	263	/*
	264	* In some testing scenarios, we send a request and response.
	265	* Make the last packet sent take effect for the initiator
	266	* value.
	267	*/
	268	if (sta)
	269	clear_sta_flag(sta, WLAN_STA_TDLS_INITIATOR);
	270	/* fall-through */
95224fe8	271	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
626911cc	272	initiator = false;
2fb6b9b8 AN	273	break;
	274	case WLAN_TDLS_TEARDOWN:
	275	/* any value is ok */
95224fe8 AN	276	break;
	277	default:
	278	ret = -ENOTSUPP;
626911cc	279	break;
95224fe8 AN	280	}
95224fe8 AN	281
626911cc	282	if (initiator \|\| (sta && test_sta_flag(sta, WLAN_STA_TDLS_INITIATOR))) {
2fb6b9b8 AN	283	init_addr = sdata->vif.addr;
	284	rsp_addr = peer;
	285	} else {
	286	init_addr = peer;
	287	rsp_addr = sdata->vif.addr;
	288	}
	289
626911cc AN	290	rcu_read_unlock();
	291	if (ret < 0)
	292	goto fail;
	293
2fb6b9b8 AN	294	ieee80211_tdls_add_link_ie(skb, init_addr, rsp_addr,
	295	sdata->u.mgd.bssid);
	296
95224fe8 AN	297	if (send_direct) {
	298	ieee80211_tx_skb(sdata, skb);
	299	return 0;
	300	}
	301
	302	/*
	303	* According to 802.11z: Setup req/resp are sent in AC_BK, otherwise
	304	* we should default to AC_VI.
	305	*/
	306	switch (action_code) {
	307	case WLAN_TDLS_SETUP_REQUEST:
	308	case WLAN_TDLS_SETUP_RESPONSE:
	309	skb_set_queue_mapping(skb, IEEE80211_AC_BK);
	310	skb->priority = 2;
	311	break;
	312	default:
	313	skb_set_queue_mapping(skb, IEEE80211_AC_VI);
	314	skb->priority = 5;
	315	break;
	316	}
	317
	318	/* disable bottom halves when entering the Tx path */
	319	local_bh_disable();
	320	ret = ieee80211_subif_start_xmit(skb, dev);
	321	local_bh_enable();
	322
	323	return ret;
	324
	325	fail:
	326	dev_kfree_skb(skb);
	327	return ret;
	328	}
	329
191dd469 AN	330	static int
	331	ieee80211_tdls_mgmt_setup(struct wiphy wiphy, struct net_device dev,
	332	const u8 *peer, u8 action_code, u8 dialog_token,
	333	u16 status_code, u32 peer_capability, bool initiator,
	334	const u8 *extra_ies, size_t extra_ies_len)
17e6a59a AN	335	{
	336	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	337	struct ieee80211_local *local = sdata->local;
	338	int ret;
	339
17e6a59a AN	340	mutex_lock(&local->mtx);
	341
	342	/* we don't support concurrent TDLS peer setups */
	343	if (!is_zero_ether_addr(sdata->tdls_peer) &&
191dd469	344	!ether_addr_equal(sdata->tdls_peer, peer)) {
17e6a59a AN	345	ret = -EBUSY;
	346	goto exit;
	347	}
	348
7adc3e46 AN	349	/*
	350	* make sure we have a STA representing the peer so we drop or buffer
	351	* non-TDLS-setup frames to the peer. We can't send other packets
	352	* during setup through the AP path
	353	*/
	354	rcu_read_lock();
	355	if (!sta_info_get(sdata, peer)) {
	356	rcu_read_unlock();
	357	ret = -ENOLINK;
	358	goto exit;
	359	}
	360	rcu_read_unlock();
	361
db67d661 AN	362	ieee80211_flush_queues(local, sdata);
db67d661 AN	363
17e6a59a AN	364	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
17e6a59a AN	365	dialog_token, status_code,
2fb6b9b8 AN	366	peer_capability, initiator,
2fb6b9b8 AN	367	extra_ies, extra_ies_len);
17e6a59a AN	368	if (ret < 0)
	369	goto exit;
	370
191dd469 AN	371	memcpy(sdata->tdls_peer, peer, ETH_ALEN);
	372	ieee80211_queue_delayed_work(&sdata->local->hw,
	373	&sdata->tdls_peer_del_work,
	374	TDLS_PEER_SETUP_TIMEOUT);
17e6a59a AN	375
	376	exit:
	377	mutex_unlock(&local->mtx);
191dd469 AN	378	return ret;
	379	}
	380
db67d661 AN	381	static int
	382	ieee80211_tdls_mgmt_teardown(struct wiphy wiphy, struct net_device dev,
	383	const u8 *peer, u8 action_code, u8 dialog_token,
	384	u16 status_code, u32 peer_capability,
	385	bool initiator, const u8 *extra_ies,
	386	size_t extra_ies_len)
	387	{
	388	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	389	struct ieee80211_local *local = sdata->local;
	390	struct sta_info *sta;
	391	int ret;
	392
	393	/*
	394	* No packets can be transmitted to the peer via the AP during setup -
	395	* the STA is set as a TDLS peer, but is not authorized.
	396	* During teardown, we prevent direct transmissions by stopping the
	397	* queues and flushing all direct packets.
	398	*/
	399	ieee80211_stop_vif_queues(local, sdata,
	400	IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
	401	ieee80211_flush_queues(local, sdata);
	402
	403	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
	404	dialog_token, status_code,
	405	peer_capability, initiator,
	406	extra_ies, extra_ies_len);
	407	if (ret < 0)
	408	sdata_err(sdata, "Failed sending TDLS teardown packet %d\n",
	409	ret);
	410
	411	/*
	412	* Remove the STA AUTH flag to force further traffic through the AP. If
	413	* the STA was unreachable, it was already removed.
	414	*/
	415	rcu_read_lock();
	416	sta = sta_info_get(sdata, peer);
	417	if (sta)
	418	clear_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
	419	rcu_read_unlock();
	420
	421	ieee80211_wake_vif_queues(local, sdata,
	422	IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
	423
	424	return 0;
	425	}
	426
191dd469 AN	427	int ieee80211_tdls_mgmt(struct wiphy wiphy, struct net_device dev,
	428	const u8 *peer, u8 action_code, u8 dialog_token,
	429	u16 status_code, u32 peer_capability,
	430	bool initiator, const u8 *extra_ies,
	431	size_t extra_ies_len)
	432	{
	433	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	434	int ret;
	435
	436	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
	437	return -ENOTSUPP;
	438
	439	/* make sure we are in managed mode, and associated */
	440	if (sdata->vif.type != NL80211_IFTYPE_STATION \|\|
	441	!sdata->u.mgd.associated)
	442	return -EINVAL;
	443
	444	switch (action_code) {
	445	case WLAN_TDLS_SETUP_REQUEST:
	446	case WLAN_TDLS_SETUP_RESPONSE:
	447	ret = ieee80211_tdls_mgmt_setup(wiphy, dev, peer, action_code,
	448	dialog_token, status_code,
	449	peer_capability, initiator,
	450	extra_ies, extra_ies_len);
	451	break;
	452	case WLAN_TDLS_TEARDOWN:
db67d661 AN	453	ret = ieee80211_tdls_mgmt_teardown(wiphy, dev, peer,
	454	action_code, dialog_token,
	455	status_code,
	456	peer_capability, initiator,
	457	extra_ies, extra_ies_len);
	458	break;
191dd469	459	case WLAN_TDLS_DISCOVERY_REQUEST:
ee10f2c7 AN	460	/*
	461	* Protect the discovery so we can hear the TDLS discovery
	462	* response frame. It is transmitted directly and not buffered
	463	* by the AP.
	464	*/
	465	drv_mgd_protect_tdls_discover(sdata->local, sdata);
	466	/* fall-through */
	467	case WLAN_TDLS_SETUP_CONFIRM:
191dd469 AN	468	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
	469	/* no special handling */
	470	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer,
	471	action_code,
	472	dialog_token,
	473	status_code,
	474	peer_capability,
	475	initiator, extra_ies,
	476	extra_ies_len);
	477	break;
	478	default:
	479	ret = -EOPNOTSUPP;
	480	break;
	481	}
17e6a59a AN	482
	483	tdls_dbg(sdata, "TDLS mgmt action %d peer %pM status %d\n",
	484	action_code, peer, ret);
	485	return ret;
	486	}
	487
95224fe8	488	int ieee80211_tdls_oper(struct wiphy wiphy, struct net_device dev,
3b3a0162	489	const u8 *peer, enum nl80211_tdls_operation oper)
95224fe8 AN	490	{
	491	struct sta_info *sta;
	492	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
17e6a59a AN	493	struct ieee80211_local *local = sdata->local;
17e6a59a AN	494	int ret;
95224fe8 AN	495
	496	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
	497	return -ENOTSUPP;
	498
	499	if (sdata->vif.type != NL80211_IFTYPE_STATION)
	500	return -EINVAL;
	501
17e6a59a AN	502	switch (oper) {
	503	case NL80211_TDLS_ENABLE_LINK:
	504	case NL80211_TDLS_DISABLE_LINK:
	505	break;
	506	case NL80211_TDLS_TEARDOWN:
	507	case NL80211_TDLS_SETUP:
	508	case NL80211_TDLS_DISCOVERY_REQ:
	509	/* We don't support in-driver setup/teardown/discovery */
	510	return -ENOTSUPP;
	511	}
	512
	513	mutex_lock(&local->mtx);
95224fe8 AN	514	tdls_dbg(sdata, "TDLS oper %d peer %pM\n", oper, peer);
	515
	516	switch (oper) {
	517	case NL80211_TDLS_ENABLE_LINK:
	518	rcu_read_lock();
	519	sta = sta_info_get(sdata, peer);
	520	if (!sta) {
	521	rcu_read_unlock();
17e6a59a AN	522	ret = -ENOLINK;
17e6a59a AN	523	break;
95224fe8 AN	524	}
	525
	526	set_sta_flag(sta, WLAN_STA_TDLS_PEER_AUTH);
	527	rcu_read_unlock();
17e6a59a AN	528
	529	WARN_ON_ONCE(is_zero_ether_addr(sdata->tdls_peer) \|\|
	530	!ether_addr_equal(sdata->tdls_peer, peer));
	531	ret = 0;
95224fe8 AN	532	break;
95224fe8 AN	533	case NL80211_TDLS_DISABLE_LINK:
db67d661 AN	534	/* flush a potentially queued teardown packet */
	535	ieee80211_flush_queues(local, sdata);
	536
17e6a59a AN	537	ret = sta_info_destroy_addr(sdata, peer);
17e6a59a AN	538	break;
95224fe8	539	default:
17e6a59a AN	540	ret = -ENOTSUPP;
17e6a59a AN	541	break;
95224fe8 AN	542	}
95224fe8 AN	543
17e6a59a AN	544	if (ret == 0 && ether_addr_equal(sdata->tdls_peer, peer)) {
	545	cancel_delayed_work(&sdata->tdls_peer_del_work);
	546	eth_zero_addr(sdata->tdls_peer);
	547	}
	548
	549	mutex_unlock(&local->mtx);
	550	return ret;
95224fe8	551	}
c887f0d3 AN	552
	553	void ieee80211_tdls_oper_request(struct ieee80211_vif vif, const u8 peer,
	554	enum nl80211_tdls_operation oper,
	555	u16 reason_code, gfp_t gfp)
	556	{
	557	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
	558
	559	if (vif->type != NL80211_IFTYPE_STATION \|\| !vif->bss_conf.assoc) {
	560	sdata_err(sdata, "Discarding TDLS oper %d - not STA or disconnected\n",
	561	oper);
	562	return;
	563	}
	564
	565	cfg80211_tdls_oper_request(sdata->dev, peer, oper, reason_code, gfp);
	566	}
	567	EXPORT_SYMBOL(ieee80211_tdls_oper_request);