net/mac80211/s1g.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * S1G handling
0004  * Copyright(c) 2020 Adapt-IP
0005  */
0006 #include <linux/ieee80211.h>
0007 #include <net/mac80211.h>
0008 #include "ieee80211_i.h"
0009 #include "driver-ops.h"
0010
0011 void ieee80211_s1g_sta_rate_init(struct sta_info *sta)
0012 {
0013     /* avoid indicating legacy bitrates for S1G STAs */
0014     sta->deflink.tx_stats.last_rate.flags |= IEEE80211_TX_RC_S1G_MCS;
0015     sta->deflink.rx_stats.last_rate =
0016             STA_STATS_FIELD(TYPE, STA_STATS_RATE_TYPE_S1G);
0017 }
0018
0019 bool ieee80211_s1g_is_twt_setup(struct sk_buff *skb)
0020 {
0021     struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
0022
0023     if (likely(!ieee80211_is_action(mgmt->frame_control)))
0024         return false;
0025
0026     if (likely(mgmt->u.action.category != WLAN_CATEGORY_S1G))
0027         return false;
0028
0029     return mgmt->u.action.u.s1g.action_code == WLAN_S1G_TWT_SETUP;
0030 }
0031
0032 static void
0033 ieee80211_s1g_send_twt_setup(struct ieee80211_sub_if_data *sdata, const u8 *da,
0034                  const u8 *bssid, struct ieee80211_twt_setup *twt)
0035 {
0036     int len = IEEE80211_MIN_ACTION_SIZE + 4 + twt->length;
0037     struct ieee80211_local *local = sdata->local;
0038     struct ieee80211_mgmt *mgmt;
0039     struct sk_buff *skb;
0040
0041     skb = dev_alloc_skb(local->hw.extra_tx_headroom + len);
0042     if (!skb)
0043         return;
0044
0045     skb_reserve(skb, local->hw.extra_tx_headroom);
0046     mgmt = skb_put_zero(skb, len);
0047     mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
0048                       IEEE80211_STYPE_ACTION);
0049     memcpy(mgmt->da, da, ETH_ALEN);
0050     memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
0051     memcpy(mgmt->bssid, bssid, ETH_ALEN);
0052
0053     mgmt->u.action.category = WLAN_CATEGORY_S1G;
0054     mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_SETUP;
0055     memcpy(mgmt->u.action.u.s1g.variable, twt, 3 + twt->length);
0056
0057     IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
0058                     IEEE80211_TX_INTFL_MLME_CONN_TX |
0059                     IEEE80211_TX_CTL_REQ_TX_STATUS;
0060     ieee80211_tx_skb(sdata, skb);
0061 }
0062
0063 static void
0064 ieee80211_s1g_send_twt_teardown(struct ieee80211_sub_if_data *sdata,
0065                 const u8 *da, const u8 *bssid, u8 flowid)
0066 {
0067     struct ieee80211_local *local = sdata->local;
0068     struct ieee80211_mgmt *mgmt;
0069     struct sk_buff *skb;
0070     u8 *id;
0071
0072     skb = dev_alloc_skb(local->hw.extra_tx_headroom +
0073                 IEEE80211_MIN_ACTION_SIZE + 2);
0074     if (!skb)
0075         return;
0076
0077     skb_reserve(skb, local->hw.extra_tx_headroom);
0078     mgmt = skb_put_zero(skb, IEEE80211_MIN_ACTION_SIZE + 2);
0079     mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
0080                       IEEE80211_STYPE_ACTION);
0081     memcpy(mgmt->da, da, ETH_ALEN);
0082     memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
0083     memcpy(mgmt->bssid, bssid, ETH_ALEN);
0084
0085     mgmt->u.action.category = WLAN_CATEGORY_S1G;
0086     mgmt->u.action.u.s1g.action_code = WLAN_S1G_TWT_TEARDOWN;
0087     id = (u8 *)mgmt->u.action.u.s1g.variable;
0088     *id = flowid;
0089
0090     IEEE80211_SKB_CB(skb)->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
0091                     IEEE80211_TX_CTL_REQ_TX_STATUS;
0092     ieee80211_tx_skb(sdata, skb);
0093 }
0094
0095 static void
0096 ieee80211_s1g_rx_twt_setup(struct ieee80211_sub_if_data *sdata,
0097                struct sta_info *sta, struct sk_buff *skb)
0098 {
0099     struct ieee80211_mgmt *mgmt = (void *)skb->data;
0100     struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
0101     struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
0102
0103     twt_agrt->req_type &= cpu_to_le16(~IEEE80211_TWT_REQTYPE_REQUEST);
0104
0105     /* broadcast TWT not supported yet */
0106     if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST) {
0107         twt_agrt->req_type &=
0108             ~cpu_to_le16(IEEE80211_TWT_REQTYPE_SETUP_CMD);
0109         twt_agrt->req_type |=
0110             le16_encode_bits(TWT_SETUP_CMD_REJECT,
0111                      IEEE80211_TWT_REQTYPE_SETUP_CMD);
0112         goto out;
0113     }
0114
0115     drv_add_twt_setup(sdata->local, sdata, &sta->sta, twt);
0116 out:
0117     ieee80211_s1g_send_twt_setup(sdata, mgmt->sa, sdata->vif.addr, twt);
0118 }
0119
0120 static void
0121 ieee80211_s1g_rx_twt_teardown(struct ieee80211_sub_if_data *sdata,
0122                   struct sta_info *sta, struct sk_buff *skb)
0123 {
0124     struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
0125
0126     drv_twt_teardown_request(sdata->local, sdata, &sta->sta,
0127                  mgmt->u.action.u.s1g.variable[0]);
0128 }
0129
0130 static void
0131 ieee80211_s1g_tx_twt_setup_fail(struct ieee80211_sub_if_data *sdata,
0132                 struct sta_info *sta, struct sk_buff *skb)
0133 {
0134     struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
0135     struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable;
0136     struct ieee80211_twt_params *twt_agrt = (void *)twt->params;
0137     u8 flowid = le16_get_bits(twt_agrt->req_type,
0138                   IEEE80211_TWT_REQTYPE_FLOWID);
0139
0140     drv_twt_teardown_request(sdata->local, sdata, &sta->sta, flowid);
0141
0142     ieee80211_s1g_send_twt_teardown(sdata, mgmt->sa, sdata->vif.addr,
0143                     flowid);
0144 }
0145
0146 void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata,
0147                  struct sk_buff *skb)
0148 {
0149     struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
0150     struct ieee80211_local *local = sdata->local;
0151     struct sta_info *sta;
0152
0153     mutex_lock(&local->sta_mtx);
0154
0155     sta = sta_info_get_bss(sdata, mgmt->sa);
0156     if (!sta)
0157         goto out;
0158
0159     switch (mgmt->u.action.u.s1g.action_code) {
0160     case WLAN_S1G_TWT_SETUP:
0161         ieee80211_s1g_rx_twt_setup(sdata, sta, skb);
0162         break;
0163     case WLAN_S1G_TWT_TEARDOWN:
0164         ieee80211_s1g_rx_twt_teardown(sdata, sta, skb);
0165         break;
0166     default:
0167         break;
0168     }
0169
0170 out:
0171     mutex_unlock(&local->sta_mtx);
0172 }
0173
0174 void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata,
0175                      struct sk_buff *skb)
0176 {
0177     struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
0178     struct ieee80211_local *local = sdata->local;
0179     struct sta_info *sta;
0180
0181     mutex_lock(&local->sta_mtx);
0182
0183     sta = sta_info_get_bss(sdata, mgmt->da);
0184     if (!sta)
0185         goto out;
0186
0187     switch (mgmt->u.action.u.s1g.action_code) {
0188     case WLAN_S1G_TWT_SETUP:
0189         /* process failed twt setup frames */
0190         ieee80211_s1g_tx_twt_setup_fail(sdata, sta, skb);
0191         break;
0192     default:
0193         break;
0194     }
0195
0196 out:
0197     mutex_unlock(&local->sta_mtx);
0198 }