@@ -4666,6 +4666,7 @@ static inline bool ieee80211_mle_size_ok(const u8 *data, size_t len)
enum ieee80211_mle_subelems {
IEEE80211_MLE_SUBELEM_PER_STA_PROFILE = 0,
+ IEEE80211_MLE_SUBELEM_FRAGMENT = 254,
};
#define IEEE80211_MLE_STA_CONTROL_LINK_ID 0x000f
@@ -1707,8 +1707,27 @@ struct ieee802_11_elems {
u8 tx_pwr_env_num;
u8 eht_cap_len;
+ /* mult-link element can be de-fragmented and thus u8 is not sufficient */
+ size_t multi_link_len;
+
+ /*
+ * store the per station profile pointer and length in case that the
+ * parsing also handled Multi-Link element parsing for a specific link
+ * ID.
+ */
+ struct ieee80211_mle_per_sta_profile *prof;
+ size_t sta_prof_len;
+
/* whether a parse error occurred while retrieving these elements */
bool parse_error;
+
+ /*
+ * scratch buffer that can be used for various element parsing related
+ * tasks, e.g., element de-fragmentation etc.
+ */
+ size_t scratch_len;
+ u8 *scratch_pos;
+ u8 scratch[];
};
static inline struct ieee80211_local *hw_to_local(
@@ -2197,9 +2216,13 @@ static inline void ieee80211_tx_skb(struct ieee80211_sub_if_data *sdata,
* represent a non-transmitting BSS in which case the data
* for that non-transmitting BSS is returned
* @link_id: the link ID to parse elements for, if a STA profile
- * is present in the multi-link element, or -1 to ignore
+ * is present in the multi-link element, or -1 to ignore;
+ * note that the code currently assumes parsing an association
+ * (or re-association) response frame if this is given
* @from_ap: frame is received from an AP (currently used only
* for EHT capabilities parsing)
+ * @scratch_len: if non zero, specifies the requested length of the scratch
+ * buffer; otherwise, 'len' is used.
*/
struct ieee80211_elems_parse_params {
const u8 *start;
@@ -2210,6 +2233,7 @@ struct ieee80211_elems_parse_params {
struct cfg80211_bss *bss;
int link_id;
bool from_ap;
+ size_t scratch_len;
};
struct ieee802_11_elems *
@@ -3923,11 +3923,12 @@ static bool ieee80211_assoc_config_link(struct ieee80211_link_data *link,
struct ieee80211_mgd_assoc_data *assoc_data = sdata->u.mgd.assoc_data;
struct ieee80211_bss_conf *bss_conf = link->conf;
struct ieee80211_local *local = sdata->local;
+ unsigned int link_id = link->link_id;
struct ieee80211_elems_parse_params parse_params = {
.start = elem_start,
.len = elem_len,
.bss = cbss,
- .link_id = link == &sdata->deflink ? -1 : link->link_id,
+ .link_id = link_id == assoc_data->assoc_link_id ? -1 : link_id,
.from_ap = true,
};
bool is_6ghz = cbss->channel->band == NL80211_BAND_6GHZ;
@@ -3942,8 +3943,18 @@ static bool ieee80211_assoc_config_link(struct ieee80211_link_data *link,
if (!elems)
return false;
- /* FIXME: use from STA profile element after parsing that */
- capab_info = le16_to_cpu(mgmt->u.assoc_resp.capab_info);
+ if (link_id == assoc_data->assoc_link_id) {
+ capab_info = le16_to_cpu(mgmt->u.assoc_resp.capab_info);
+ } else if (!elems->prof) {
+ ret = false;
+ goto out;
+ } else {
+ const u8 *ptr = elems->prof->variable +
+ elems->prof->sta_info_len - 1;
+
+ /* FIXME: need to also handle the status code */
+ capab_info = get_unaligned_le16(ptr);
+ }
if (!is_s1g && !elems->supp_rates) {
sdata_info(sdata, "no SuppRates element in AssocResp\n");
@@ -1026,8 +1026,10 @@ ieee80211_parse_extension_element(u32 *crc,
elems->eht_operation = data;
break;
case WLAN_EID_EXT_EHT_MULTI_LINK:
- if (ieee80211_mle_size_ok(data, len))
+ if (ieee80211_mle_size_ok(data, len)) {
elems->multi_link = (void *)data;
+ elems->multi_link_len = len;
+ }
break;
}
}
@@ -1497,6 +1499,145 @@ static size_t ieee802_11_find_bssid_profile(const u8 *start, size_t len,
return found ? profile_len : 0;
}
+static void ieee80211_defragment_element(struct ieee802_11_elems *elems,
+ void **elem_ptr, size_t *len,
+ size_t total_len, u8 frag_id)
+{
+ u8 *data = *elem_ptr, *pos, *start;
+ const struct element *elem;
+
+ /*
+ * Since 'data' points to the data of the element, not the element
+ * itself, allow 254 in case it was an extended element where the
+ * extended ID isn't part of the data we see here and thus not part of
+ * 'len' either.
+ */
+ if (!data || (*len != 254 && *len != 255))
+ return;
+
+ start = elems->scratch_pos;
+
+ if (WARN_ON(*len > (elems->scratch + elems->scratch_len -
+ elems->scratch_pos)))
+ return;
+
+ memcpy(elems->scratch_pos, data, *len);
+ elems->scratch_pos += *len;
+
+ pos = data + *len;
+ total_len -= *len;
+ for_each_element(elem, pos, total_len) {
+ if (elem->id != frag_id)
+ break;
+
+ if (WARN_ON(elem->datalen >
+ (elems->scratch + elems->scratch_len -
+ elems->scratch_pos)))
+ return;
+
+ memcpy(elems->scratch_pos, elem->data, elem->datalen);
+ elems->scratch_pos += elem->datalen;
+
+ *len += elem->datalen;
+ }
+
+ *elem_ptr = start;
+}
+
+static void ieee80211_mle_get_sta_prof(struct ieee802_11_elems *elems,
+ u8 link_id)
+{
+ const struct ieee80211_multi_link_elem *ml = elems->multi_link;
+ size_t ml_len = elems->multi_link_len;
+ const struct element *sub;
+
+ if (!ml || !ml_len)
+ return;
+
+ if (le16_get_bits(ml->control, IEEE80211_ML_CONTROL_TYPE) !=
+ IEEE80211_ML_CONTROL_TYPE_BASIC)
+ return;
+
+ for_each_mle_subelement(sub, (u8 *)ml, ml_len) {
+ struct ieee80211_mle_per_sta_profile *prof = (void *)sub->data;
+ u16 control;
+
+ if (sub->id != IEEE80211_MLE_SUBELEM_PER_STA_PROFILE)
+ continue;
+
+ if (!ieee80211_mle_sta_prof_size_ok(sub->data, sub->datalen))
+ return;
+
+ control = le16_to_cpu(prof->control);
+
+ if (link_id != u16_get_bits(control,
+ IEEE80211_MLE_STA_CONTROL_LINK_ID))
+ continue;
+
+ if (!(control & IEEE80211_MLE_STA_CONTROL_COMPLETE_PROFILE))
+ return;
+
+ elems->prof = prof;
+ elems->sta_prof_len = sub->datalen;
+
+ /* the sub element can be fragmented */
+ ieee80211_defragment_element(elems, (void **)&elems->prof,
+ &elems->sta_prof_len,
+ ml_len - (sub->data - (u8 *)ml),
+ IEEE80211_MLE_SUBELEM_FRAGMENT);
+ return;
+ }
+}
+
+static void ieee80211_mle_parse_link(struct ieee802_11_elems *elems,
+ struct ieee80211_elems_parse_params *params)
+{
+ struct ieee80211_mle_per_sta_profile *prof;
+ struct ieee80211_elems_parse_params sub = {
+ .action = params->action,
+ .from_ap = params->from_ap,
+ .link_id = -1,
+ };
+ const struct element *non_inherit = NULL;
+ const u8 *end;
+
+ if (params->link_id == -1)
+ return;
+
+ ieee80211_defragment_element(elems, (void **)&elems->multi_link,
+ &elems->multi_link_len,
+ elems->total_len - ((u8 *)elems->multi_link -
+ elems->ie_start),
+ WLAN_EID_FRAGMENT);
+
+ ieee80211_mle_get_sta_prof(elems, params->link_id);
+ prof = elems->prof;
+
+ if (!prof)
+ return;
+
+ /* check if we have the 4 bytes for the fixed part in assoc response */
+ if (elems->sta_prof_len < sizeof(*prof) + prof->sta_info_len - 1 + 4) {
+ elems->prof = NULL;
+ elems->sta_prof_len = 0;
+ return;
+ }
+
+ /*
+ * Skip the capability information and the status code that are expected
+ * as part of the station profile in association response frames. Note
+ * the -1 is because the 'sta_info_len' is accounted to as part of the
+ * per-STA profile, but not part of the 'u8 variable[]' portion.
+ */
+ sub.start = prof->variable + prof->sta_info_len - 1 + 4;
+ end = (const u8 *)prof + elems->sta_prof_len;
+ sub.len = end - sub.start;
+
+ non_inherit = cfg80211_find_ext_elem(WLAN_EID_EXT_NON_INHERITANCE,
+ sub.start, sub.len);
+ _ieee802_11_parse_elems_full(&sub, elems, non_inherit);
+}
+
struct ieee802_11_elems *
ieee802_11_parse_elems_full(struct ieee80211_elems_parse_params *params)
{
@@ -1504,12 +1645,15 @@ ieee802_11_parse_elems_full(struct ieee80211_elems_parse_params *params)
const struct element *non_inherit = NULL;
u8 *nontransmitted_profile;
int nontransmitted_profile_len = 0;
+ size_t scratch_len = params->scratch_len ?: 2 * params->len;
- elems = kzalloc(sizeof(*elems), GFP_ATOMIC);
+ elems = kzalloc(sizeof(*elems) + scratch_len, GFP_ATOMIC);
if (!elems)
return NULL;
elems->ie_start = params->start;
elems->total_len = params->len;
+ elems->scratch_len = scratch_len;
+ elems->scratch_pos = elems->scratch;
nontransmitted_profile = kmalloc(params->len, GFP_ATOMIC);
if (nontransmitted_profile) {
@@ -1537,6 +1681,8 @@ ieee802_11_parse_elems_full(struct ieee80211_elems_parse_params *params)
_ieee802_11_parse_elems_full(&sub, elems, NULL);
}
+ ieee80211_mle_parse_link(elems, params);
+
if (elems->tim && !elems->parse_error) {
const struct ieee80211_tim_ie *tim_ie = elems->tim;