net/mac80211/s1g.c

   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 }