[linux-block.git] / net / mac80211 / s1g.c

// SPDX-License-Identifier: GPL-2.0
/*
 * S1G handling
 * Copyright(c) 2020 Adapt-IP
 */
#include <linux/ieee80211.h>
#include <net/mac80211.h>
#include "ieee80211_i.h"
#include "driver-ops.h"

void ieee80211_s1g_sta_rate_init(struct sta_info *sta)
{
	/* avoid indicating legacy bitrates for S1G STAs */
	sta->deflink.tx_stats.last_rate.flags |= IEEE80211_TX_RC_S1G_MCS;
	sta->deflink.rx_stats.last_rate =
			STA_STATS_FIELD(TYPE, STA_STATS_RATE_TYPE_S1G);
}

bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb)
{
	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;

	if (likely(!ieee80211_is_action(mgmt->frame_control)))
		return false;

	if (likely(mgmt->u.action.category != WLAN_CATEGORY_S1G))
		return false;

	return mgmt->u.action.u.s1g.action_code == WLAN_S1G_TWT_SETUP;
}

static void
ieee80211_s1g_send_twt_setup(struct ieee80211_sub_if_data *sdata, const u8 *da,
			     const u8 *bssid, struct ieee80211_twt_setup *twt)
{
	int len = IEEE80211_MIN_ACTION_SIZE + 4 + twt->length;
	struct ieee80211_local *local = sdata->local;
	struct ieee80211_mgmt *mgmt;
	struct sk_buff *skb;

	skb = dev_alloc_skb(local->hw.extra_tx_headroom + len);
	if (!skb)
		return;

	skb_reserve(skb, local->hw.extra_tx_headroom);
	mgmt = skb_put_zero(skb, len);
	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
					  IEEE80211_STYPE_ACTION);
	memcpy(mgmt->da, da, ETH_ALEN);
	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
	memcpy(mgmt->bssid, bssid, ETH_ALEN);

	mgmt->u.action.category = WLAN_CATEGORY_S1G;
	mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_SETUP;
	memcpy(mgmt->u.action.u.s1g.variable, twt, 3 + twt->length);

	IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
					IEEE80211_TX_INTFL_MLME_CONN_TX |
					IEEE80211_TX_CTL_REQ_TX_STATUS;
	ieee80211_tx_skb(sdata, skb);
}

static void
ieee80211_s1g_send_twt_teardown(struct ieee80211_sub_if_data *sdata,
				const u8 *da, const u8 *bssid, u8 flowid)
{
	struct ieee80211_local *local = sdata->local;
	struct ieee80211_mgmt *mgmt;
	struct sk_buff *skb;
	u8 *id;

	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
			    IEEE80211_MIN_ACTION_SIZE + 2);
	if (!skb)
		return;

	skb_reserve(skb, local->hw.extra_tx_headroom);
	mgmt = skb_put_zero(skb, IEEE80211_MIN_ACTION_SIZE + 2);
	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
					  IEEE80211_STYPE_ACTION);
	memcpy(mgmt->da, da, ETH_ALEN);
	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
	memcpy(mgmt->bssid, bssid, ETH_ALEN);

	mgmt->u.action.category = WLAN_CATEGORY_S1G;
	mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_TEARDOWN;
	id = (u8 *)mgmt->u.action.u.s1g.variable;
	*id = flowid;

	IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
					IEEE80211_TX_CTL_REQ_TX_STATUS;
	ieee80211_tx_skb(sdata, skb);
}

static void
ieee80211_s1g_rx_twt_setup(struct ieee80211_sub_if_data *sdata,
			   struct sta_info *sta, struct sk_buff *skb)
{
	struct ieee80211_mgmt *mgmt = (void *)skb->data;
	struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
	struct ieee80211_twt_params *twt_agrt = (void *)twt->params;

	twt_agrt->req_type &= cpu_to_le16(~IEEE80211_TWT_REQTYPE_REQUEST);

	/* broadcast TWT not supported yet */
	if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST) {
		twt_agrt->req_type &=
			~cpu_to_le16(IEEE80211_TWT_REQTYPE_SETUP_CMD);
		twt_agrt->req_type |=
			le16_encode_bits(TWT_SETUP_CMD_REJECT,
					 IEEE80211_TWT_REQTYPE_SETUP_CMD);
		goto out;
	}

	/* TWT Information not supported yet */
	twt->control |= IEEE80211_TWT_CONTROL_RX_DISABLED;

	drv_add_twt_setup(sdata->local, sdata, &sta->sta, twt);
out:
	ieee80211_s1g_send_twt_setup(sdata, mgmt->sa, sdata->vif.addr, twt);
}

static void
ieee80211_s1g_rx_twt_teardown(struct ieee80211_sub_if_data *sdata,
			      struct sta_info *sta, struct sk_buff *skb)
{
	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;

	drv_twt_teardown_request(sdata->local, sdata, &sta->sta,
				 mgmt->u.action.u.s1g.variable[0]);
}

static void
ieee80211_s1g_tx_twt_setup_fail(struct ieee80211_sub_if_data *sdata,
				struct sta_info *sta, struct sk_buff *skb)
{
	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
	struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
	struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
	u8 flowid = le16_get_bits(twt_agrt->req_type,
				  IEEE80211_TWT_REQTYPE_FLOWID);

	drv_twt_teardown_request(sdata->local, sdata, &sta->sta, flowid);

	ieee80211_s1g_send_twt_teardown(sdata, mgmt->sa, sdata->vif.addr,
					flowid);
}

void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata,
				 struct sk_buff *skb)
{
	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
	struct ieee80211_local *local = sdata->local;
	struct sta_info *sta;

	mutex_lock(&local->sta_mtx);

	sta = sta_info_get_bss(sdata, mgmt->sa);
	if (!sta)
		goto out;

	switch (mgmt->u.action.u.s1g.action_code) {
	case WLAN_S1G_TWT_SETUP:
		ieee80211_s1g_rx_twt_setup(sdata, sta, skb);
		break;
	case WLAN_S1G_TWT_TEARDOWN:
		ieee80211_s1g_rx_twt_teardown(sdata, sta, skb);
		break;
	default:
		break;
	}

out:
	mutex_unlock(&local->sta_mtx);
}

void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
				     struct sk_buff *skb)
{
	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
	struct ieee80211_local *local = sdata->local;
	struct sta_info *sta;

	mutex_lock(&local->sta_mtx);

	sta = sta_info_get_bss(sdata, mgmt->da);
	if (!sta)
		goto out;

	switch (mgmt->u.action.u.s1g.action_code) {
	case WLAN_S1G_TWT_SETUP:
		/* process failed twt setup frames */
		ieee80211_s1g_tx_twt_setup_fail(sdata, sta, skb);
		break;
	default:
		break;
	}

out:
	mutex_unlock(&local->sta_mtx);
}
Commit	Line	Data
12bf8fad TP	1	// SPDX-License-Identifier: GPL-2.0
	2	/*
	3	* S1G handling
	4	* Copyright(c) 2020 Adapt-IP
	5	*/
	6	#include <linux/ieee80211.h>
	7	#include <net/mac80211.h>
	8	#include "ieee80211_i.h"
f5a4c24e	9	#include "driver-ops.h"
12bf8fad TP	10
	11	void ieee80211_s1g_sta_rate_init(struct sta_info *sta)
	12	{
	13	/* avoid indicating legacy bitrates for S1G STAs */
046d2e7c S	14	sta->deflink.tx_stats.last_rate.flags \|= IEEE80211_TX_RC_S1G_MCS;
046d2e7c S	15	sta->deflink.rx_stats.last_rate =
12bf8fad TP	16	STA_STATS_FIELD(TYPE, STA_STATS_RATE_TYPE_S1G);
12bf8fad TP	17	}
f5a4c24e LB	18
	19	bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb)
	20	{
	21	struct ieee80211_mgmt mgmt = (struct ieee80211_mgmt )skb->data;
	22
	23	if (likely(!ieee80211_is_action(mgmt->frame_control)))
	24	return false;
	25
	26	if (likely(mgmt->u.action.category != WLAN_CATEGORY_S1G))
	27	return false;
	28
	29	return mgmt->u.action.u.s1g.action_code == WLAN_S1G_TWT_SETUP;
	30	}
	31
	32	static void
	33	ieee80211_s1g_send_twt_setup(struct ieee80211_sub_if_data sdata, const u8 da,
	34	const u8 bssid, struct ieee80211_twt_setup twt)
	35	{
	36	int len = IEEE80211_MIN_ACTION_SIZE + 4 + twt->length;
	37	struct ieee80211_local *local = sdata->local;
	38	struct ieee80211_mgmt *mgmt;
	39	struct sk_buff *skb;
	40
	41	skb = dev_alloc_skb(local->hw.extra_tx_headroom + len);
	42	if (!skb)
	43	return;
	44
	45	skb_reserve(skb, local->hw.extra_tx_headroom);
	46	mgmt = skb_put_zero(skb, len);
	47	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT \|
	48	IEEE80211_STYPE_ACTION);
	49	memcpy(mgmt->da, da, ETH_ALEN);
	50	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
	51	memcpy(mgmt->bssid, bssid, ETH_ALEN);
	52
	53	mgmt->u.action.category = WLAN_CATEGORY_S1G;
	54	mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_SETUP;
	55	memcpy(mgmt->u.action.u.s1g.variable, twt, 3 + twt->length);
	56
	57	IEEE80211_SKB_CB(skb)->flags \|= IEEE80211_TX_INTFL_DONT_ENCRYPT \|
	58	IEEE80211_TX_INTFL_MLME_CONN_TX \|
	59	IEEE80211_TX_CTL_REQ_TX_STATUS;
	60	ieee80211_tx_skb(sdata, skb);
	61	}
	62
	63	static void
	64	ieee80211_s1g_send_twt_teardown(struct ieee80211_sub_if_data *sdata,
	65	const u8 da, const u8 bssid, u8 flowid)
	66	{
	67	struct ieee80211_local *local = sdata->local;
	68	struct ieee80211_mgmt *mgmt;
	69	struct sk_buff *skb;
	70	u8 *id;
	71
	72	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
	73	IEEE80211_MIN_ACTION_SIZE + 2);
	74	if (!skb)
	75	return;
	76
	77	skb_reserve(skb, local->hw.extra_tx_headroom);
	78	mgmt = skb_put_zero(skb, IEEE80211_MIN_ACTION_SIZE + 2);
	79	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT \|
	80	IEEE80211_STYPE_ACTION);
	81	memcpy(mgmt->da, da, ETH_ALEN);
82	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
83	memcpy(mgmt->bssid, bssid, ETH_ALEN);
84
85	mgmt->u.action.category = WLAN_CATEGORY_S1G;
86	mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_TEARDOWN;
87	id = (u8 *)mgmt->u.action.u.s1g.variable;
88	*id = flowid;
89
90	IEEE80211_SKB_CB(skb)->flags \|= IEEE80211_TX_INTFL_DONT_ENCRYPT \|
91	IEEE80211_TX_CTL_REQ_TX_STATUS;
92	ieee80211_tx_skb(sdata, skb);
93	}
94
95	static void
96	ieee80211_s1g_rx_twt_setup(struct ieee80211_sub_if_data *sdata,
97	struct sta_info sta, struct sk_buff skb)
98	{
99	struct ieee80211_mgmt mgmt = (void )skb->data;
100	struct ieee80211_twt_setup twt = (void )mgmt->u.action.u.s1g.variable;
101	struct ieee80211_twt_params twt_agrt = (void )twt->params;
102
103	twt_agrt->req_type &= cpu_to_le16(~IEEE80211_TWT_REQTYPE_REQUEST);
104
105	/* broadcast TWT not supported yet */
106	if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST) {
7ff379ba JB	107	twt_agrt->req_type &=
	108	~cpu_to_le16(IEEE80211_TWT_REQTYPE_SETUP_CMD);
	109	twt_agrt->req_type \|=
	110	le16_encode_bits(TWT_SETUP_CMD_REJECT,
	111	IEEE80211_TWT_REQTYPE_SETUP_CMD);
f5a4c24e LB	112	goto out;
	113	}
	114
30ac96f7 HH	115	/* TWT Information not supported yet */
	116	twt->control \|= IEEE80211_TWT_CONTROL_RX_DISABLED;
	117
f5a4c24e LB	118	drv_add_twt_setup(sdata->local, sdata, &sta->sta, twt);
	119	out:
	120	ieee80211_s1g_send_twt_setup(sdata, mgmt->sa, sdata->vif.addr, twt);
	121	}
	122
	123	static void
	124	ieee80211_s1g_rx_twt_teardown(struct ieee80211_sub_if_data *sdata,
	125	struct sta_info sta, struct sk_buff skb)
	126	{
	127	struct ieee80211_mgmt mgmt = (struct ieee80211_mgmt )skb->data;
	128
	129	drv_twt_teardown_request(sdata->local, sdata, &sta->sta,
	130	mgmt->u.action.u.s1g.variable[0]);
	131	}
	132
	133	static void
	134	ieee80211_s1g_tx_twt_setup_fail(struct ieee80211_sub_if_data *sdata,
	135	struct sta_info sta, struct sk_buff skb)
	136	{
	137	struct ieee80211_mgmt mgmt = (struct ieee80211_mgmt )skb->data;
	138	struct ieee80211_twt_setup twt = (void )mgmt->u.action.u.s1g.variable;
	139	struct ieee80211_twt_params twt_agrt = (void )twt->params;
	140	u8 flowid = le16_get_bits(twt_agrt->req_type,
	141	IEEE80211_TWT_REQTYPE_FLOWID);
	142
	143	drv_twt_teardown_request(sdata->local, sdata, &sta->sta, flowid);
	144
	145	ieee80211_s1g_send_twt_teardown(sdata, mgmt->sa, sdata->vif.addr,
	146	flowid);
	147	}
	148
	149	void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata,
	150	struct sk_buff *skb)
	151	{
	152	struct ieee80211_mgmt mgmt = (struct ieee80211_mgmt )skb->data;
	153	struct ieee80211_local *local = sdata->local;
	154	struct sta_info *sta;
	155
	156	mutex_lock(&local->sta_mtx);
	157
	158	sta = sta_info_get_bss(sdata, mgmt->sa);
	159	if (!sta)
	160	goto out;
	161
	162	switch (mgmt->u.action.u.s1g.action_code) {
	163	case WLAN_S1G_TWT_SETUP:
	164	ieee80211_s1g_rx_twt_setup(sdata, sta, skb);
	165	break;
	166	case WLAN_S1G_TWT_TEARDOWN:
	167	ieee80211_s1g_rx_twt_teardown(sdata, sta, skb);
	168	break;
	169	default:
	170	break;
	171	}
	172
	173	out:
	174	mutex_unlock(&local->sta_mtx);
	175	}
	176
	177	void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
	178	struct sk_buff *skb)
	179	{
	180	struct ieee80211_mgmt mgmt = (struct ieee80211_mgmt )skb->data;
	181	struct ieee80211_local *local = sdata->local;
182	struct sta_info *sta;
183
184	mutex_lock(&local->sta_mtx);
185
186	sta = sta_info_get_bss(sdata, mgmt->da);
187	if (!sta)
188	goto out;
189
190	switch (mgmt->u.action.u.s1g.action_code) {
191	case WLAN_S1G_TWT_SETUP:
192	/* process failed twt setup frames */
193	ieee80211_s1g_tx_twt_setup_fail(sdata, sta, skb);
194	break;
195	default:
196	break;
197	}
198
199	out:
200	mutex_unlock(&local->sta_mtx);
201	}