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