1 From: Lorenzo Bianconi <lorenzo@kernel.org>
2 Date: Mon, 23 Aug 2021 20:02:39 +0200
3 Subject: [PATCH] mac80211: introduce individual TWT support in AP mode
5 Introduce TWT action frames parsing support to mac80211.
6 Currently just individual TWT agreement are support in AP mode.
7 Whenever the AP receives a TWT action frame from an associated client,
8 after performing sanity checks, it will notify the underlay driver with
9 requested parameters in order to check if they are supported and if there
10 is enough room for a new agreement. The driver is expected to set the
11 agreement result and report it to mac80211.
13 Drivers supporting this have two new callbacks:
14 - add_twt_setup (mandatory)
15 - twt_teardown_request (optional)
17 mac80211 will send an action frame reply according to the result
18 reported by the driver.
20 Tested-by: Peter Chiu <chui-hao.chiu@mediatek.com>
21 Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
22 Link: https://lore.kernel.org/r/257512f2e22ba42b9f2624942a128dd8f141de4b.1629741512.git.lorenzo@kernel.org
23 [use le16p_replace_bits(), minor cleanups, use (void *) casts,
24 fix to use ieee80211_get_he_iftype_cap() correctly]
25 Signed-off-by: Johannes Berg <johannes.berg@intel.com>
28 --- a/include/net/mac80211.h
29 +++ b/include/net/mac80211.h
30 @@ -4219,6 +4219,11 @@ struct ieee80211_ops {
31 void (*sta_set_decap_offload)(struct ieee80211_hw *hw,
32 struct ieee80211_vif *vif,
33 struct ieee80211_sta *sta, bool enabled);
34 + void (*add_twt_setup)(struct ieee80211_hw *hw,
35 + struct ieee80211_sta *sta,
36 + struct ieee80211_twt_setup *twt);
37 + void (*twt_teardown_request)(struct ieee80211_hw *hw,
38 + struct ieee80211_sta *sta, u8 flowid);
42 --- a/net/mac80211/driver-ops.h
43 +++ b/net/mac80211/driver-ops.h
44 @@ -1429,4 +1429,40 @@ static inline void drv_sta_set_decap_off
45 trace_drv_return_void(local);
48 +static inline void drv_add_twt_setup(struct ieee80211_local *local,
49 + struct ieee80211_sub_if_data *sdata,
50 + struct ieee80211_sta *sta,
51 + struct ieee80211_twt_setup *twt)
53 + struct ieee80211_twt_params *twt_agrt;
57 + if (!check_sdata_in_driver(sdata))
60 + twt_agrt = (void *)twt->params;
62 + trace_drv_add_twt_setup(local, sta, twt, twt_agrt);
63 + local->ops->add_twt_setup(&local->hw, sta, twt);
64 + trace_drv_return_void(local);
67 +static inline void drv_twt_teardown_request(struct ieee80211_local *local,
68 + struct ieee80211_sub_if_data *sdata,
69 + struct ieee80211_sta *sta,
73 + if (!check_sdata_in_driver(sdata))
76 + if (!local->ops->twt_teardown_request)
79 + trace_drv_twt_teardown_request(local, sta, flowid);
80 + local->ops->twt_teardown_request(&local->hw, sta, flowid);
81 + trace_drv_return_void(local);
84 #endif /* __MAC80211_DRIVER_OPS */
85 --- a/net/mac80211/ieee80211_i.h
86 +++ b/net/mac80211/ieee80211_i.h
87 @@ -954,6 +954,7 @@ struct ieee80211_sub_if_data {
89 struct work_struct work;
90 struct sk_buff_head skb_queue;
91 + struct sk_buff_head status_queue;
94 enum ieee80211_smps_mode smps_mode;
95 @@ -2093,6 +2094,11 @@ ieee80211_he_op_ie_to_bss_conf(struct ie
98 void ieee80211_s1g_sta_rate_init(struct sta_info *sta);
99 +bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb);
100 +void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata,
101 + struct sk_buff *skb);
102 +void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
103 + struct sk_buff *skb);
105 /* Spectrum management */
106 void ieee80211_process_measurement_req(struct ieee80211_sub_if_data *sdata,
107 --- a/net/mac80211/iface.c
108 +++ b/net/mac80211/iface.c
109 @@ -563,6 +563,7 @@ static void ieee80211_do_stop(struct iee
111 ieee80211_free_keys(sdata, true);
112 skb_queue_purge(&sdata->skb_queue);
113 + skb_queue_purge(&sdata->status_queue);
116 spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
117 @@ -1070,6 +1071,7 @@ int ieee80211_add_virtual_monitor(struct
120 skb_queue_head_init(&sdata->skb_queue);
121 + skb_queue_head_init(&sdata->status_queue);
122 INIT_WORK(&sdata->work, ieee80211_iface_work);
125 @@ -1442,6 +1444,24 @@ static void ieee80211_if_setup_no_queue(
129 +static void ieee80211_iface_process_status(struct ieee80211_sub_if_data *sdata,
130 + struct sk_buff *skb)
132 + struct ieee80211_mgmt *mgmt = (void *)skb->data;
134 + if (ieee80211_is_action(mgmt->frame_control) &&
135 + mgmt->u.action.category == WLAN_CATEGORY_S1G) {
136 + switch (mgmt->u.action.u.s1g.action_code) {
137 + case WLAN_S1G_TWT_TEARDOWN:
138 + case WLAN_S1G_TWT_SETUP:
139 + ieee80211_s1g_status_twt_action(sdata, skb);
147 static void ieee80211_iface_work(struct work_struct *work)
149 struct ieee80211_sub_if_data *sdata =
150 @@ -1519,6 +1539,16 @@ static void ieee80211_iface_work(struct
154 + } else if (ieee80211_is_action(mgmt->frame_control) &&
155 + mgmt->u.action.category == WLAN_CATEGORY_S1G) {
156 + switch (mgmt->u.action.u.s1g.action_code) {
157 + case WLAN_S1G_TWT_TEARDOWN:
158 + case WLAN_S1G_TWT_SETUP:
159 + ieee80211_s1g_rx_twt_action(sdata, skb);
164 } else if (ieee80211_is_ext(mgmt->frame_control)) {
165 if (sdata->vif.type == NL80211_IFTYPE_STATION)
166 ieee80211_sta_rx_queued_ext(sdata, skb);
167 @@ -1574,6 +1604,12 @@ static void ieee80211_iface_work(struct
171 + /* process status queue */
172 + while ((skb = skb_dequeue(&sdata->status_queue))) {
173 + ieee80211_iface_process_status(sdata, skb);
177 /* then other type-dependent work */
178 switch (sdata->vif.type) {
179 case NL80211_IFTYPE_STATION:
180 @@ -1637,6 +1673,7 @@ static void ieee80211_setup_sdata(struct
183 skb_queue_head_init(&sdata->skb_queue);
184 + skb_queue_head_init(&sdata->status_queue);
185 INIT_WORK(&sdata->work, ieee80211_iface_work);
186 INIT_WORK(&sdata->recalc_smps, ieee80211_recalc_smps_work);
187 INIT_WORK(&sdata->csa_finalize_work, ieee80211_csa_finalize_work);
188 --- a/net/mac80211/rx.c
189 +++ b/net/mac80211/rx.c
190 @@ -3208,6 +3208,68 @@ ieee80211_rx_h_mgmt_check(struct ieee802
195 +ieee80211_process_rx_twt_action(struct ieee80211_rx_data *rx)
197 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)rx->skb->data;
198 + struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb);
199 + struct ieee80211_sub_if_data *sdata = rx->sdata;
200 + const struct ieee80211_sta_he_cap *hecap;
201 + struct ieee80211_supported_band *sband;
203 + /* TWT actions are only supported in AP for the moment */
204 + if (sdata->vif.type != NL80211_IFTYPE_AP)
207 + if (!rx->local->ops->add_twt_setup)
210 + sband = rx->local->hw.wiphy->bands[status->band];
211 + hecap = ieee80211_get_he_iftype_cap(sband,
212 + ieee80211_vif_type_p2p(&sdata->vif));
216 + if (!(hecap->he_cap_elem.mac_cap_info[0] &
217 + IEEE80211_HE_MAC_CAP0_TWT_RES))
223 + switch (mgmt->u.action.u.s1g.action_code) {
224 + case WLAN_S1G_TWT_SETUP: {
225 + struct ieee80211_twt_setup *twt;
227 + if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE +
228 + 1 + /* action code */
229 + sizeof(struct ieee80211_twt_setup) +
230 + 2 /* TWT req_type agrt */)
233 + twt = (void *)mgmt->u.action.u.s1g.variable;
234 + if (twt->element_id != WLAN_EID_S1G_TWT)
237 + if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE +
238 + 4 + /* action code + token + tlv */
242 + return true; /* queue the frame */
244 + case WLAN_S1G_TWT_TEARDOWN:
245 + if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE + 2)
248 + return true; /* queue the frame */
256 static ieee80211_rx_result debug_noinline
257 ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
259 @@ -3487,6 +3549,17 @@ ieee80211_rx_h_action(struct ieee80211_r
260 !mesh_path_sel_is_hwmp(sdata))
263 + case WLAN_CATEGORY_S1G:
264 + switch (mgmt->u.action.u.s1g.action_code) {
265 + case WLAN_S1G_TWT_SETUP:
266 + case WLAN_S1G_TWT_TEARDOWN:
267 + if (ieee80211_process_rx_twt_action(rx))
277 --- a/net/mac80211/s1g.c
278 +++ b/net/mac80211/s1g.c
280 #include <linux/ieee80211.h>
281 #include <net/mac80211.h>
282 #include "ieee80211_i.h"
283 +#include "driver-ops.h"
285 void ieee80211_s1g_sta_rate_init(struct sta_info *sta)
287 @@ -14,3 +15,182 @@ void ieee80211_s1g_sta_rate_init(struct
288 sta->rx_stats.last_rate =
289 STA_STATS_FIELD(TYPE, STA_STATS_RATE_TYPE_S1G);
292 +bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb)
294 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
296 + if (likely(!ieee80211_is_action(mgmt->frame_control)))
299 + if (likely(mgmt->u.action.category != WLAN_CATEGORY_S1G))
302 + return mgmt->u.action.u.s1g.action_code == WLAN_S1G_TWT_SETUP;
306 +ieee80211_s1g_send_twt_setup(struct ieee80211_sub_if_data *sdata, const u8 *da,
307 + const u8 *bssid, struct ieee80211_twt_setup *twt)
309 + int len = IEEE80211_MIN_ACTION_SIZE + 4 + twt->length;
310 + struct ieee80211_local *local = sdata->local;
311 + struct ieee80211_mgmt *mgmt;
312 + struct sk_buff *skb;
314 + skb = dev_alloc_skb(local->hw.extra_tx_headroom + len);
318 + skb_reserve(skb, local->hw.extra_tx_headroom);
319 + mgmt = skb_put_zero(skb, len);
320 + mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
321 + IEEE80211_STYPE_ACTION);
322 + memcpy(mgmt->da, da, ETH_ALEN);
323 + memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
324 + memcpy(mgmt->bssid, bssid, ETH_ALEN);
326 + mgmt->u.action.category = WLAN_CATEGORY_S1G;
327 + mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_SETUP;
328 + memcpy(mgmt->u.action.u.s1g.variable, twt, 3 + twt->length);
330 + IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
331 + IEEE80211_TX_INTFL_MLME_CONN_TX |
332 + IEEE80211_TX_CTL_REQ_TX_STATUS;
333 + ieee80211_tx_skb(sdata, skb);
337 +ieee80211_s1g_send_twt_teardown(struct ieee80211_sub_if_data *sdata,
338 + const u8 *da, const u8 *bssid, u8 flowid)
340 + struct ieee80211_local *local = sdata->local;
341 + struct ieee80211_mgmt *mgmt;
342 + struct sk_buff *skb;
345 + skb = dev_alloc_skb(local->hw.extra_tx_headroom +
346 + IEEE80211_MIN_ACTION_SIZE + 2);
350 + skb_reserve(skb, local->hw.extra_tx_headroom);
351 + mgmt = skb_put_zero(skb, IEEE80211_MIN_ACTION_SIZE + 2);
352 + mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
353 + IEEE80211_STYPE_ACTION);
354 + memcpy(mgmt->da, da, ETH_ALEN);
355 + memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
356 + memcpy(mgmt->bssid, bssid, ETH_ALEN);
358 + mgmt->u.action.category = WLAN_CATEGORY_S1G;
359 + mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_TEARDOWN;
360 + id = (u8 *)mgmt->u.action.u.s1g.variable;
363 + IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
364 + IEEE80211_TX_CTL_REQ_TX_STATUS;
365 + ieee80211_tx_skb(sdata, skb);
369 +ieee80211_s1g_rx_twt_setup(struct ieee80211_sub_if_data *sdata,
370 + struct sta_info *sta, struct sk_buff *skb)
372 + struct ieee80211_mgmt *mgmt = (void *)skb->data;
373 + struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
374 + struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
376 + twt_agrt->req_type &= cpu_to_le16(~IEEE80211_TWT_REQTYPE_REQUEST);
378 + /* broadcast TWT not supported yet */
379 + if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST) {
380 + le16p_replace_bits(&twt_agrt->req_type,
381 + TWT_SETUP_CMD_REJECT,
382 + IEEE80211_TWT_REQTYPE_SETUP_CMD);
386 + drv_add_twt_setup(sdata->local, sdata, &sta->sta, twt);
388 + ieee80211_s1g_send_twt_setup(sdata, mgmt->sa, sdata->vif.addr, twt);
392 +ieee80211_s1g_rx_twt_teardown(struct ieee80211_sub_if_data *sdata,
393 + struct sta_info *sta, struct sk_buff *skb)
395 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
397 + drv_twt_teardown_request(sdata->local, sdata, &sta->sta,
398 + mgmt->u.action.u.s1g.variable[0]);
402 +ieee80211_s1g_tx_twt_setup_fail(struct ieee80211_sub_if_data *sdata,
403 + struct sta_info *sta, struct sk_buff *skb)
405 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
406 + struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
407 + struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
408 + u8 flowid = le16_get_bits(twt_agrt->req_type,
409 + IEEE80211_TWT_REQTYPE_FLOWID);
411 + drv_twt_teardown_request(sdata->local, sdata, &sta->sta, flowid);
413 + ieee80211_s1g_send_twt_teardown(sdata, mgmt->sa, sdata->vif.addr,
417 +void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata,
418 + struct sk_buff *skb)
420 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
421 + struct ieee80211_local *local = sdata->local;
422 + struct sta_info *sta;
424 + mutex_lock(&local->sta_mtx);
426 + sta = sta_info_get_bss(sdata, mgmt->sa);
430 + switch (mgmt->u.action.u.s1g.action_code) {
431 + case WLAN_S1G_TWT_SETUP:
432 + ieee80211_s1g_rx_twt_setup(sdata, sta, skb);
434 + case WLAN_S1G_TWT_TEARDOWN:
435 + ieee80211_s1g_rx_twt_teardown(sdata, sta, skb);
442 + mutex_unlock(&local->sta_mtx);
445 +void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
446 + struct sk_buff *skb)
448 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
449 + struct ieee80211_local *local = sdata->local;
450 + struct sta_info *sta;
452 + mutex_lock(&local->sta_mtx);
454 + sta = sta_info_get_bss(sdata, mgmt->da);
458 + switch (mgmt->u.action.u.s1g.action_code) {
459 + case WLAN_S1G_TWT_SETUP:
460 + /* process failed twt setup frames */
461 + ieee80211_s1g_tx_twt_setup_fail(sdata, sta, skb);
468 + mutex_unlock(&local->sta_mtx);
470 --- a/net/mac80211/status.c
471 +++ b/net/mac80211/status.c
472 @@ -705,13 +705,26 @@ static void ieee80211_report_used_skb(st
473 /* Check to see if packet is a TDLS teardown packet */
474 if (ieee80211_is_data(hdr->frame_control) &&
475 (ieee80211_get_tdls_action(skb, hdr_size) ==
476 - WLAN_TDLS_TEARDOWN))
477 + WLAN_TDLS_TEARDOWN)) {
478 ieee80211_tdls_td_tx_handle(local, sdata, skb,
481 + } else if (ieee80211_s1g_is_twt_setup(skb)) {
483 + struct sk_buff *qskb;
485 + qskb = skb_clone(skb, GFP_ATOMIC);
487 + skb_queue_tail(&sdata->status_queue,
489 + ieee80211_queue_work(&local->hw,
494 ieee80211_mgd_conn_tx_status(sdata,
501 --- a/net/mac80211/trace.h
502 +++ b/net/mac80211/trace.h
503 @@ -2804,6 +2804,73 @@ DEFINE_EVENT(sta_flag_evt, drv_sta_set_d
504 TP_ARGS(local, sdata, sta, enabled)
507 +TRACE_EVENT(drv_add_twt_setup,
508 + TP_PROTO(struct ieee80211_local *local,
509 + struct ieee80211_sta *sta,
510 + struct ieee80211_twt_setup *twt,
511 + struct ieee80211_twt_params *twt_agrt),
513 + TP_ARGS(local, sta, twt, twt_agrt),
518 + __field(u8, dialog_token)
519 + __field(u8, control)
520 + __field(__le16, req_type)
521 + __field(__le64, twt)
522 + __field(u8, duration)
523 + __field(__le16, mantissa)
524 + __field(u8, channel)
530 + __entry->dialog_token = twt->dialog_token;
531 + __entry->control = twt->control;
532 + __entry->req_type = twt_agrt->req_type;
533 + __entry->twt = twt_agrt->twt;
534 + __entry->duration = twt_agrt->min_twt_dur;
535 + __entry->mantissa = twt_agrt->mantissa;
536 + __entry->channel = twt_agrt->channel;
540 + LOCAL_PR_FMT STA_PR_FMT
541 + " token:%d control:0x%02x req_type:0x%04x"
542 + " twt:%llu duration:%d mantissa:%d channel:%d",
543 + LOCAL_PR_ARG, STA_PR_ARG, __entry->dialog_token,
544 + __entry->control, le16_to_cpu(__entry->req_type),
545 + le64_to_cpu(__entry->twt), __entry->duration,
546 + le16_to_cpu(__entry->mantissa), __entry->channel
550 +TRACE_EVENT(drv_twt_teardown_request,
551 + TP_PROTO(struct ieee80211_local *local,
552 + struct ieee80211_sta *sta, u8 flowid),
554 + TP_ARGS(local, sta, flowid),
559 + __field(u8, flowid)
565 + __entry->flowid = flowid;
569 + LOCAL_PR_FMT STA_PR_FMT " flowid:%d",
570 + LOCAL_PR_ARG, STA_PR_ARG, __entry->flowid
574 #endif /* !__MAC80211_DRIVER_TRACE || TRACE_HEADER_MULTI_READ */
576 #undef TRACE_INCLUDE_PATH