[mirror_ubuntu-jammy-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 "ieee80211_i.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;
	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);

	/* sanity check for initiator */
	switch (action_code) {
	case WLAN_TDLS_SETUP_REQUEST:
	case WLAN_TDLS_SETUP_CONFIRM:
	case WLAN_TDLS_DISCOVERY_REQUEST:
		if (!initiator) {
			ret = -EINVAL;
			goto fail;
		}
		break;
	case WLAN_TDLS_SETUP_RESPONSE:
	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
		if (initiator) {
			ret = -EINVAL;
			goto fail;
		}
		break;
	case WLAN_TDLS_TEARDOWN:
		/* any value is ok */
		break;
	default:
		ret = -ENOTSUPP;
		goto fail;
	}

	if (initiator) {
		init_addr = sdata->vif.addr;
		rsp_addr = peer;
	} else {
		init_addr = peer;
		rsp_addr = sdata->vif.addr;
	}

	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;
	}

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