1 From f5a4c24e689f54e66201f04d343bdd2e8a1d7923 Mon Sep 17 00:00:00 2001
2 From: Lorenzo Bianconi <lorenzo@kernel.org>
3 Date: Mon, 23 Aug 2021 20:02:39 +0200
4 Subject: [PATCH] mac80211: introduce individual TWT support in AP mode
6 Introduce TWT action frames parsing support to mac80211.
7 Currently just individual TWT agreement are support in AP mode.
8 Whenever the AP receives a TWT action frame from an associated client,
9 after performing sanity checks, it will notify the underlay driver with
10 requested parameters in order to check if they are supported and if there
11 is enough room for a new agreement. The driver is expected to set the
12 agreement result and report it to mac80211.
14 Drivers supporting this have two new callbacks:
15 - add_twt_setup (mandatory)
16 - twt_teardown_request (optional)
18 mac80211 will send an action frame reply according to the result
19 reported by the driver.
21 Tested-by: Peter Chiu <chui-hao.chiu@mediatek.com>
22 Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
23 Link: https://lore.kernel.org/r/257512f2e22ba42b9f2624942a128dd8f141de4b.1629741512.git.lorenzo@kernel.org
24 [use le16p_replace_bits(), minor cleanups, use (void *) casts,
25 fix to use ieee80211_get_he_iftype_cap() correctly]
26 Signed-off-by: Johannes Berg <johannes.berg@intel.com>
28 include/net/mac80211.h | 12 +++
29 net/mac80211/driver-ops.h | 36 ++++++++
30 net/mac80211/ieee80211_i.h | 6 ++
31 net/mac80211/iface.c | 41 +++++++++
32 net/mac80211/rx.c | 73 +++++++++++++++
33 net/mac80211/s1g.c | 180 +++++++++++++++++++++++++++++++++++++
34 net/mac80211/status.c | 17 +++-
35 net/mac80211/trace.h | 67 ++++++++++++++
36 8 files changed, 430 insertions(+), 2 deletions(-)
38 --- a/include/net/mac80211.h
39 +++ b/include/net/mac80211.h
40 @@ -3930,6 +3930,13 @@ struct ieee80211_prep_tx_info {
41 * @set_sar_specs: Update the SAR (TX power) settings.
42 * @sta_set_decap_offload: Called to notify the driver when a station is allowed
43 * to use rx decapsulation offload
44 + * @add_twt_setup: Update hw with TWT agreement parameters received from the peer.
45 + * This callback allows the hw to check if requested parameters
46 + * are supported and if there is enough room for a new agreement.
47 + * The hw is expected to set agreement result in the req_type field of
49 + * @twt_teardown_request: Update the hw with TWT teardown request received
52 struct ieee80211_ops {
53 void (*tx)(struct ieee80211_hw *hw,
54 @@ -4253,6 +4260,11 @@ struct ieee80211_ops {
55 void (*sta_set_decap_offload)(struct ieee80211_hw *hw,
56 struct ieee80211_vif *vif,
57 struct ieee80211_sta *sta, bool enabled);
58 + void (*add_twt_setup)(struct ieee80211_hw *hw,
59 + struct ieee80211_sta *sta,
60 + struct ieee80211_twt_setup *twt);
61 + void (*twt_teardown_request)(struct ieee80211_hw *hw,
62 + struct ieee80211_sta *sta, u8 flowid);
66 --- a/net/mac80211/driver-ops.h
67 +++ b/net/mac80211/driver-ops.h
68 @@ -1447,4 +1447,40 @@ static inline void drv_sta_set_decap_off
69 trace_drv_return_void(local);
72 +static inline void drv_add_twt_setup(struct ieee80211_local *local,
73 + struct ieee80211_sub_if_data *sdata,
74 + struct ieee80211_sta *sta,
75 + struct ieee80211_twt_setup *twt)
77 + struct ieee80211_twt_params *twt_agrt;
81 + if (!check_sdata_in_driver(sdata))
84 + twt_agrt = (void *)twt->params;
86 + trace_drv_add_twt_setup(local, sta, twt, twt_agrt);
87 + local->ops->add_twt_setup(&local->hw, sta, twt);
88 + trace_drv_return_void(local);
91 +static inline void drv_twt_teardown_request(struct ieee80211_local *local,
92 + struct ieee80211_sub_if_data *sdata,
93 + struct ieee80211_sta *sta,
97 + if (!check_sdata_in_driver(sdata))
100 + if (!local->ops->twt_teardown_request)
103 + trace_drv_twt_teardown_request(local, sta, flowid);
104 + local->ops->twt_teardown_request(&local->hw, sta, flowid);
105 + trace_drv_return_void(local);
108 #endif /* __MAC80211_DRIVER_OPS */
109 --- a/net/mac80211/ieee80211_i.h
110 +++ b/net/mac80211/ieee80211_i.h
111 @@ -949,6 +949,7 @@ struct ieee80211_sub_if_data {
113 struct work_struct work;
114 struct sk_buff_head skb_queue;
115 + struct sk_buff_head status_queue;
118 enum ieee80211_smps_mode smps_mode;
119 @@ -2083,6 +2084,11 @@ ieee80211_he_op_ie_to_bss_conf(struct ie
122 void ieee80211_s1g_sta_rate_init(struct sta_info *sta);
123 +bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb);
124 +void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata,
125 + struct sk_buff *skb);
126 +void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
127 + struct sk_buff *skb);
129 /* Spectrum management */
130 void ieee80211_process_measurement_req(struct ieee80211_sub_if_data *sdata,
131 --- a/net/mac80211/iface.c
132 +++ b/net/mac80211/iface.c
133 @@ -552,6 +552,7 @@ static void ieee80211_do_stop(struct iee
135 ieee80211_free_keys(sdata, true);
136 skb_queue_purge(&sdata->skb_queue);
137 + skb_queue_purge(&sdata->status_queue);
140 spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
141 @@ -1055,6 +1056,7 @@ int ieee80211_add_virtual_monitor(struct
144 skb_queue_head_init(&sdata->skb_queue);
145 + skb_queue_head_init(&sdata->status_queue);
146 INIT_WORK(&sdata->work, ieee80211_iface_work);
149 @@ -1459,6 +1461,16 @@ static void ieee80211_iface_process_skb(
153 + } else if (ieee80211_is_action(mgmt->frame_control) &&
154 + mgmt->u.action.category == WLAN_CATEGORY_S1G) {
155 + switch (mgmt->u.action.u.s1g.action_code) {
156 + case WLAN_S1G_TWT_TEARDOWN:
157 + case WLAN_S1G_TWT_SETUP:
158 + ieee80211_s1g_rx_twt_action(sdata, skb);
163 } else if (ieee80211_is_ext(mgmt->frame_control)) {
164 if (sdata->vif.type == NL80211_IFTYPE_STATION)
165 ieee80211_sta_rx_queued_ext(sdata, skb);
166 @@ -1514,6 +1526,24 @@ static void ieee80211_iface_process_skb(
170 +static void ieee80211_iface_process_status(struct ieee80211_sub_if_data *sdata,
171 + struct sk_buff *skb)
173 + struct ieee80211_mgmt *mgmt = (void *)skb->data;
175 + if (ieee80211_is_action(mgmt->frame_control) &&
176 + mgmt->u.action.category == WLAN_CATEGORY_S1G) {
177 + switch (mgmt->u.action.u.s1g.action_code) {
178 + case WLAN_S1G_TWT_TEARDOWN:
179 + case WLAN_S1G_TWT_SETUP:
180 + ieee80211_s1g_status_twt_action(sdata, skb);
188 static void ieee80211_iface_work(struct work_struct *work)
190 struct ieee80211_sub_if_data *sdata =
191 @@ -1543,6 +1573,16 @@ static void ieee80211_iface_work(struct
195 + /* process status queue */
196 + while ((skb = skb_dequeue(&sdata->status_queue))) {
197 + kcov_remote_start_common(skb_get_kcov_handle(skb));
199 + ieee80211_iface_process_status(sdata, skb);
202 + kcov_remote_stop();
205 /* then other type-dependent work */
206 switch (sdata->vif.type) {
207 case NL80211_IFTYPE_STATION:
208 @@ -1606,6 +1646,7 @@ static void ieee80211_setup_sdata(struct
211 skb_queue_head_init(&sdata->skb_queue);
212 + skb_queue_head_init(&sdata->status_queue);
213 INIT_WORK(&sdata->work, ieee80211_iface_work);
214 INIT_WORK(&sdata->recalc_smps, ieee80211_recalc_smps_work);
215 INIT_WORK(&sdata->csa_finalize_work, ieee80211_csa_finalize_work);
216 --- a/net/mac80211/rx.c
217 +++ b/net/mac80211/rx.c
218 @@ -3211,6 +3211,68 @@ ieee80211_rx_h_mgmt_check(struct ieee802
223 +ieee80211_process_rx_twt_action(struct ieee80211_rx_data *rx)
225 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)rx->skb->data;
226 + struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb);
227 + struct ieee80211_sub_if_data *sdata = rx->sdata;
228 + const struct ieee80211_sta_he_cap *hecap;
229 + struct ieee80211_supported_band *sband;
231 + /* TWT actions are only supported in AP for the moment */
232 + if (sdata->vif.type != NL80211_IFTYPE_AP)
235 + if (!rx->local->ops->add_twt_setup)
238 + sband = rx->local->hw.wiphy->bands[status->band];
239 + hecap = ieee80211_get_he_iftype_cap(sband,
240 + ieee80211_vif_type_p2p(&sdata->vif));
244 + if (!(hecap->he_cap_elem.mac_cap_info[0] &
245 + IEEE80211_HE_MAC_CAP0_TWT_RES))
251 + switch (mgmt->u.action.u.s1g.action_code) {
252 + case WLAN_S1G_TWT_SETUP: {
253 + struct ieee80211_twt_setup *twt;
255 + if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE +
256 + 1 + /* action code */
257 + sizeof(struct ieee80211_twt_setup) +
258 + 2 /* TWT req_type agrt */)
261 + twt = (void *)mgmt->u.action.u.s1g.variable;
262 + if (twt->element_id != WLAN_EID_S1G_TWT)
265 + if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE +
266 + 4 + /* action code + token + tlv */
270 + return true; /* queue the frame */
272 + case WLAN_S1G_TWT_TEARDOWN:
273 + if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE + 2)
276 + return true; /* queue the frame */
284 static ieee80211_rx_result debug_noinline
285 ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
287 @@ -3490,6 +3552,17 @@ ieee80211_rx_h_action(struct ieee80211_r
288 !mesh_path_sel_is_hwmp(sdata))
291 + case WLAN_CATEGORY_S1G:
292 + switch (mgmt->u.action.u.s1g.action_code) {
293 + case WLAN_S1G_TWT_SETUP:
294 + case WLAN_S1G_TWT_TEARDOWN:
295 + if (ieee80211_process_rx_twt_action(rx))
305 --- a/net/mac80211/s1g.c
306 +++ b/net/mac80211/s1g.c
308 #include <linux/ieee80211.h>
309 #include <net/mac80211.h>
310 #include "ieee80211_i.h"
311 +#include "driver-ops.h"
313 void ieee80211_s1g_sta_rate_init(struct sta_info *sta)
315 @@ -14,3 +15,182 @@ void ieee80211_s1g_sta_rate_init(struct
316 sta->rx_stats.last_rate =
317 STA_STATS_FIELD(TYPE, STA_STATS_RATE_TYPE_S1G);
320 +bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb)
322 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
324 + if (likely(!ieee80211_is_action(mgmt->frame_control)))
327 + if (likely(mgmt->u.action.category != WLAN_CATEGORY_S1G))
330 + return mgmt->u.action.u.s1g.action_code == WLAN_S1G_TWT_SETUP;
334 +ieee80211_s1g_send_twt_setup(struct ieee80211_sub_if_data *sdata, const u8 *da,
335 + const u8 *bssid, struct ieee80211_twt_setup *twt)
337 + int len = IEEE80211_MIN_ACTION_SIZE + 4 + twt->length;
338 + struct ieee80211_local *local = sdata->local;
339 + struct ieee80211_mgmt *mgmt;
340 + struct sk_buff *skb;
342 + skb = dev_alloc_skb(local->hw.extra_tx_headroom + len);
346 + skb_reserve(skb, local->hw.extra_tx_headroom);
347 + mgmt = skb_put_zero(skb, len);
348 + mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
349 + IEEE80211_STYPE_ACTION);
350 + memcpy(mgmt->da, da, ETH_ALEN);
351 + memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
352 + memcpy(mgmt->bssid, bssid, ETH_ALEN);
354 + mgmt->u.action.category = WLAN_CATEGORY_S1G;
355 + mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_SETUP;
356 + memcpy(mgmt->u.action.u.s1g.variable, twt, 3 + twt->length);
358 + IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
359 + IEEE80211_TX_INTFL_MLME_CONN_TX |
360 + IEEE80211_TX_CTL_REQ_TX_STATUS;
361 + ieee80211_tx_skb(sdata, skb);
365 +ieee80211_s1g_send_twt_teardown(struct ieee80211_sub_if_data *sdata,
366 + const u8 *da, const u8 *bssid, u8 flowid)
368 + struct ieee80211_local *local = sdata->local;
369 + struct ieee80211_mgmt *mgmt;
370 + struct sk_buff *skb;
373 + skb = dev_alloc_skb(local->hw.extra_tx_headroom +
374 + IEEE80211_MIN_ACTION_SIZE + 2);
378 + skb_reserve(skb, local->hw.extra_tx_headroom);
379 + mgmt = skb_put_zero(skb, IEEE80211_MIN_ACTION_SIZE + 2);
380 + mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
381 + IEEE80211_STYPE_ACTION);
382 + memcpy(mgmt->da, da, ETH_ALEN);
383 + memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
384 + memcpy(mgmt->bssid, bssid, ETH_ALEN);
386 + mgmt->u.action.category = WLAN_CATEGORY_S1G;
387 + mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_TEARDOWN;
388 + id = (u8 *)mgmt->u.action.u.s1g.variable;
391 + IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
392 + IEEE80211_TX_CTL_REQ_TX_STATUS;
393 + ieee80211_tx_skb(sdata, skb);
397 +ieee80211_s1g_rx_twt_setup(struct ieee80211_sub_if_data *sdata,
398 + struct sta_info *sta, struct sk_buff *skb)
400 + struct ieee80211_mgmt *mgmt = (void *)skb->data;
401 + struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
402 + struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
404 + twt_agrt->req_type &= cpu_to_le16(~IEEE80211_TWT_REQTYPE_REQUEST);
406 + /* broadcast TWT not supported yet */
407 + if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST) {
408 + le16p_replace_bits(&twt_agrt->req_type,
409 + TWT_SETUP_CMD_REJECT,
410 + IEEE80211_TWT_REQTYPE_SETUP_CMD);
414 + drv_add_twt_setup(sdata->local, sdata, &sta->sta, twt);
416 + ieee80211_s1g_send_twt_setup(sdata, mgmt->sa, sdata->vif.addr, twt);
420 +ieee80211_s1g_rx_twt_teardown(struct ieee80211_sub_if_data *sdata,
421 + struct sta_info *sta, struct sk_buff *skb)
423 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
425 + drv_twt_teardown_request(sdata->local, sdata, &sta->sta,
426 + mgmt->u.action.u.s1g.variable[0]);
430 +ieee80211_s1g_tx_twt_setup_fail(struct ieee80211_sub_if_data *sdata,
431 + struct sta_info *sta, struct sk_buff *skb)
433 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
434 + struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
435 + struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
436 + u8 flowid = le16_get_bits(twt_agrt->req_type,
437 + IEEE80211_TWT_REQTYPE_FLOWID);
439 + drv_twt_teardown_request(sdata->local, sdata, &sta->sta, flowid);
441 + ieee80211_s1g_send_twt_teardown(sdata, mgmt->sa, sdata->vif.addr,
445 +void ieee80211_s1g_rx_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->sa);
458 + switch (mgmt->u.action.u.s1g.action_code) {
459 + case WLAN_S1G_TWT_SETUP:
460 + ieee80211_s1g_rx_twt_setup(sdata, sta, skb);
462 + case WLAN_S1G_TWT_TEARDOWN:
463 + ieee80211_s1g_rx_twt_teardown(sdata, sta, skb);
470 + mutex_unlock(&local->sta_mtx);
473 +void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
474 + struct sk_buff *skb)
476 + struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
477 + struct ieee80211_local *local = sdata->local;
478 + struct sta_info *sta;
480 + mutex_lock(&local->sta_mtx);
482 + sta = sta_info_get_bss(sdata, mgmt->da);
486 + switch (mgmt->u.action.u.s1g.action_code) {
487 + case WLAN_S1G_TWT_SETUP:
488 + /* process failed twt setup frames */
489 + ieee80211_s1g_tx_twt_setup_fail(sdata, sta, skb);
496 + mutex_unlock(&local->sta_mtx);
498 --- a/net/mac80211/status.c
499 +++ b/net/mac80211/status.c
500 @@ -705,13 +705,26 @@ static void ieee80211_report_used_skb(st
501 /* Check to see if packet is a TDLS teardown packet */
502 if (ieee80211_is_data(hdr->frame_control) &&
503 (ieee80211_get_tdls_action(skb, hdr_size) ==
504 - WLAN_TDLS_TEARDOWN))
505 + WLAN_TDLS_TEARDOWN)) {
506 ieee80211_tdls_td_tx_handle(local, sdata, skb,
509 + } else if (ieee80211_s1g_is_twt_setup(skb)) {
511 + struct sk_buff *qskb;
513 + qskb = skb_clone(skb, GFP_ATOMIC);
515 + skb_queue_tail(&sdata->status_queue,
517 + ieee80211_queue_work(&local->hw,
522 ieee80211_mgd_conn_tx_status(sdata,
529 --- a/net/mac80211/trace.h
530 +++ b/net/mac80211/trace.h
531 @@ -2825,6 +2825,73 @@ DEFINE_EVENT(sta_flag_evt, drv_sta_set_d
532 TP_ARGS(local, sdata, sta, enabled)
535 +TRACE_EVENT(drv_add_twt_setup,
536 + TP_PROTO(struct ieee80211_local *local,
537 + struct ieee80211_sta *sta,
538 + struct ieee80211_twt_setup *twt,
539 + struct ieee80211_twt_params *twt_agrt),
541 + TP_ARGS(local, sta, twt, twt_agrt),
546 + __field(u8, dialog_token)
547 + __field(u8, control)
548 + __field(__le16, req_type)
549 + __field(__le64, twt)
550 + __field(u8, duration)
551 + __field(__le16, mantissa)
552 + __field(u8, channel)
558 + __entry->dialog_token = twt->dialog_token;
559 + __entry->control = twt->control;
560 + __entry->req_type = twt_agrt->req_type;
561 + __entry->twt = twt_agrt->twt;
562 + __entry->duration = twt_agrt->min_twt_dur;
563 + __entry->mantissa = twt_agrt->mantissa;
564 + __entry->channel = twt_agrt->channel;
568 + LOCAL_PR_FMT STA_PR_FMT
569 + " token:%d control:0x%02x req_type:0x%04x"
570 + " twt:%llu duration:%d mantissa:%d channel:%d",
571 + LOCAL_PR_ARG, STA_PR_ARG, __entry->dialog_token,
572 + __entry->control, le16_to_cpu(__entry->req_type),
573 + le64_to_cpu(__entry->twt), __entry->duration,
574 + le16_to_cpu(__entry->mantissa), __entry->channel
578 +TRACE_EVENT(drv_twt_teardown_request,
579 + TP_PROTO(struct ieee80211_local *local,
580 + struct ieee80211_sta *sta, u8 flowid),
582 + TP_ARGS(local, sta, flowid),
587 + __field(u8, flowid)
593 + __entry->flowid = flowid;
597 + LOCAL_PR_FMT STA_PR_FMT " flowid:%d",
598 + LOCAL_PR_ARG, STA_PR_ARG, __entry->flowid
602 #endif /* !__MAC80211_DRIVER_TRACE || TRACE_HEADER_MULTI_READ */
604 #undef TRACE_INCLUDE_PATH