When the association is an UHR association, parse the UHR
element from the association response and update the
station's UHR capabilities accordingly.

Signed-off-by: Karthikeyan Kathirvel <karthikeyan.kathirvel@oss.qualcomm.com>
---
 include/net/mac80211.h     |  4 ++++
 net/mac80211/Makefile      |  3 ++-
 net/mac80211/cfg.c         |  6 +++++
 net/mac80211/ieee80211_i.h |  6 +++++
 net/mac80211/uhr.c         | 48 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 66 insertions(+), 1 deletion(-)
 create mode 100644 net/mac80211/uhr.c

diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 7f1d2953a074..73bd3331ad5d 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -760,6 +760,7 @@ struct ieee80211_parsed_tpe {
  *	(as opposed to hearing its value from another link's beacon).
  * @s1g_long_beacon_period: number of beacon intervals between each long
  *	beacon transmission.
+ * @uhr_support: does this BSS support UHR
  */
 struct ieee80211_bss_conf {
 	struct ieee80211_vif *vif;
@@ -861,6 +862,7 @@ struct ieee80211_bss_conf {
 	u8 bss_param_ch_cnt_link_id;
 
 	u8 s1g_long_beacon_period;
+	bool uhr_support;
 };
 
 /**
@@ -2442,6 +2444,7 @@ struct ieee80211_sta_aggregates {
  *	notifications and capabilities. The value is only valid after
  *	the station moves to associated state.
  * @txpwr: the station tx power configuration
+ * @uhr_cap: UHR capabilities of this STA
  *
  */
 struct ieee80211_link_sta {
@@ -2464,6 +2467,7 @@ struct ieee80211_link_sta {
 	u8 rx_nss;
 	enum ieee80211_sta_rx_bandwidth bandwidth;
 	struct ieee80211_sta_txpwr txpwr;
+	struct ieee80211_sta_uhr_cap uhr_cap;
 };
 
 /**
diff --git a/net/mac80211/Makefile b/net/mac80211/Makefile
index a33884967f21..33a290787241 100644
--- a/net/mac80211/Makefile
+++ b/net/mac80211/Makefile
@@ -36,7 +36,8 @@ mac80211-y := \
 	tdls.o \
 	ocb.o \
 	airtime.o \
-	eht.o
+	eht.o \
+	uhr.o
 
 mac80211-$(CONFIG_MAC80211_LEDS) += led.o
 mac80211-$(CONFIG_MAC80211_DEBUGFS) += \
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index b51c2c8584ae..94ded4a44c4d 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -2170,6 +2170,12 @@ static int sta_link_apply_parameters(struct ieee80211_local *local,
 		ieee80211_s1g_cap_to_sta_s1g_cap(sdata, params->s1g_capa,
 						 link_sta);
 
+	if (params->uhr_capa)
+		ieee80211_uhr_ie_to_sta_uhr(sdata, sband,
+					    params->uhr_capa,
+					    params->uhr_capa_len,
+					    link_sta);
+
 	ieee80211_sta_init_nss(link_sta);
 
 	if (params->opmode_notif_used) {
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 3a61710b1532..4ffdb8f75468 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -2846,6 +2846,12 @@ ieee80211_eht_cap_ie_to_sta_eht_cap(struct ieee80211_sub_if_data *sdata,
 				    const struct ieee80211_eht_cap_elem *eht_cap_ie_elem,
 				    u8 eht_cap_len,
 				    struct link_sta_info *link_sta);
+void
+ieee80211_uhr_ie_to_sta_uhr(struct ieee80211_sub_if_data *sdata,
+			    struct ieee80211_supported_band *sband,
+			    const struct ieee80211_uhr_cap_elem *uhr_cap_elem,
+			    u8 uhr_cap_len,
+			    struct link_sta_info *link_sta);
 void ieee80211_process_neg_ttlm_req(struct ieee80211_sub_if_data *sdata,
 				    struct ieee80211_mgmt *mgmt, size_t len);
 void ieee80211_process_neg_ttlm_res(struct ieee80211_sub_if_data *sdata,
diff --git a/net/mac80211/uhr.c b/net/mac80211/uhr.c
new file mode 100644
index 000000000000..da68088a4fe6
--- /dev/null
+++ b/net/mac80211/uhr.c
@@ -0,0 +1,48 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * UHR handling
+ *
+ * Copyright (c) Qualcomm Technologies, Inc. and/or its subsidiaries.
+ */
+
+#include "ieee80211_i.h"
+
+void
+ieee80211_uhr_ie_to_sta_uhr(struct ieee80211_sub_if_data *sdata,
+			    struct ieee80211_supported_band *sband,
+			    const struct ieee80211_uhr_cap_elem *uhr_cap_elem,
+			    u8 uhr_cap_len,
+			    struct link_sta_info *link_sta)
+{
+	struct ieee80211_sta_uhr_cap *uhr_cap = &link_sta->pub->uhr_cap;
+	u8 *pos = (u8 *)uhr_cap_elem;
+	u8 uhr_total_size = sizeof(uhr_cap->uhr_cap_elem) -
+		sizeof(struct ieee80211_dbe_cap);
+
+	memset(uhr_cap, 0, sizeof(*uhr_cap));
+
+	if (!uhr_cap_elem ||
+	    !ieee80211_get_uhr_iftype_cap_vif(sband, &sdata->vif))
+		return;
+
+	if (uhr_cap_elem->fixed.mac_cap_info[1] &
+	    IEEE80211_UHR_MAC_CAP1_DBE_SUPPORT) {
+		uhr_total_size += 1;
+
+		if (le64_to_cpu(uhr_cap_elem->fixed.dbe_cap.dbe_cap_param) &
+		    IEEE80211_UHR_DBE_CAP_EHT_MCS_BW_160MHZ_PRES)
+			uhr_total_size += 3;
+
+		if (le64_to_cpu(uhr_cap_elem->fixed.dbe_cap.dbe_cap_param) &
+		    IEEE80211_UHR_DBE_CAP_EHT_MCS_BW_320MHZ_PRES)
+			uhr_total_size += 3;
+	}
+
+	if (uhr_cap_len < uhr_total_size)
+		return;
+
+	/* Copy the static portion of the UHR capabilities */
+	memcpy(&uhr_cap->uhr_cap_elem, pos, sizeof(uhr_cap->uhr_cap_elem));
+
+	uhr_cap->has_uhr = true;
+}
-- 
2.34.1