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->tx_stats.last_rate.flags |= IEEE80211_TX_RC_S1G_MCS; 15 sta->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 drv_add_twt_setup(sdata->local, sdata, &sta->sta, twt); 116 out: 117 ieee80211_s1g_send_twt_setup(sdata, mgmt->sa, sdata->vif.addr, twt); 118 } 119 120 static void 121 ieee80211_s1g_rx_twt_teardown(struct ieee80211_sub_if_data *sdata, 122 struct sta_info *sta, struct sk_buff *skb) 123 { 124 struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data; 125 126 drv_twt_teardown_request(sdata->local, sdata, &sta->sta, 127 mgmt->u.action.u.s1g.variable[0]); 128 } 129 130 static void 131 ieee80211_s1g_tx_twt_setup_fail(struct ieee80211_sub_if_data *sdata, 132 struct sta_info *sta, struct sk_buff *skb) 133 { 134 struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data; 135 struct ieee80211_twt_setup *twt = (void *)mgmt->u.action.u.s1g.variable; 136 struct ieee80211_twt_params *twt_agrt = (void *)twt->params; 137 u8 flowid = le16_get_bits(twt_agrt->req_type, 138 IEEE80211_TWT_REQTYPE_FLOWID); 139 140 drv_twt_teardown_request(sdata->local, sdata, &sta->sta, flowid); 141 142 ieee80211_s1g_send_twt_teardown(sdata, mgmt->sa, sdata->vif.addr, 143 flowid); 144 } 145 146 void ieee80211_s1g_rx_twt_action(struct ieee80211_sub_if_data *sdata, 147 struct sk_buff *skb) 148 { 149 struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data; 150 struct ieee80211_local *local = sdata->local; 151 struct sta_info *sta; 152 153 mutex_lock(&local->sta_mtx); 154 155 sta = sta_info_get_bss(sdata, mgmt->sa); 156 if (!sta) 157 goto out; 158 159 switch (mgmt->u.action.u.s1g.action_code) { 160 case WLAN_S1G_TWT_SETUP: 161 ieee80211_s1g_rx_twt_setup(sdata, sta, skb); 162 break; 163 case WLAN_S1G_TWT_TEARDOWN: 164 ieee80211_s1g_rx_twt_teardown(sdata, sta, skb); 165 break; 166 default: 167 break; 168 } 169 170 out: 171 mutex_unlock(&local->sta_mtx); 172 } 173 174 void ieee80211_s1g_status_twt_action(struct ieee80211_sub_if_data *sdata, 175 struct sk_buff *skb) 176 { 177 struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data; 178 struct ieee80211_local *local = sdata->local; 179 struct sta_info *sta; 180 181 mutex_lock(&local->sta_mtx); 182 183 sta = sta_info_get_bss(sdata, mgmt->da); 184 if (!sta) 185 goto out; 186 187 switch (mgmt->u.action.u.s1g.action_code) { 188 case WLAN_S1G_TWT_SETUP: 189 /* process failed twt setup frames */ 190 ieee80211_s1g_tx_twt_setup_fail(sdata, sta, skb); 191 break; 192 default: 193 break; 194 } 195 196 out: 197 mutex_unlock(&local->sta_mtx); 198 } 199