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" 9 #include "driver-ops.h" 10 11 void ieee80211_s1g_sta_rate_init(struct sta_info *sta) 12 { 13 /* avoid indicating legacy bitrates for S1G STAs */ 14 sta->deflink.tx_stats.last_rate.flags |= IEEE80211_TX_RC_S1G_MCS; 15 sta->deflink.rx_stats.last_rate = 16 STA_STATS_FIELD(TYPE, STA_STATS_RATE_TYPE_S1G); 17 } 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) { 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); 112 goto out; 113 } 114 115 /* TWT Information not supported yet */ 116 twt->control |= IEEE80211_TWT_CONTROL_RX_DISABLED; 117 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 } 202