qcacld-3.0: Fill csa offset in mlo IE when sending beacon template

Fill csa/ecsa offset in mlo IE when sending beacon template wmi cmd
to FW

Change-Id: If097bdfbf36a8d1d6ff41daf571ac8e3567ab45d
CRs-Fixed: 3023049

core/mac/src/pe/include/sch_global.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, 2017-2019 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2013-2014, 2017-2019, 2021 The Linux Foundation. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for
  * any purpose with or without fee is hereby granted, provided that the
@@ -43,6 +43,43 @@
 #define tSchBeaconStruct tSirProbeRespBeacon
 #define tpSchBeaconStruct struct sSirProbeRespBeacon *
 
+#ifdef WLAN_FEATURE_11BE_MLO
+/**
+ * struct ml_sch_partner_info - Partner link information
+ * @vdev_id: Vdev id
+ * @beacon_interval: Beacon interval
+ * @bcn_csa_cnt_ofst: CSA swith count offset in beacon frame
+ * @bcn_ext_csa_cnt_ofst: ECSA switch count offset in beacon frame
+ * @link_info_sta_prof_ofst: offset sta profile in link info.
+ *                           If per sta profile exists, this value is non zero
+ * @prb_csa_cnt_ofst: CSA swith count offset in probe frame
+ * @prb_ext_csa_cnt_ofst: ECSA switch count offset in probe frame
+ * @csa_ext_csa_exist: csa or ext csa exists
+ */
+struct ml_sch_partner_info {
+	uint32_t vdev_id;
+	uint32_t beacon_interval;
+	uint32_t bcn_csa_cnt_ofst;
+	uint32_t bcn_ext_csa_cnt_ofst;
+	uint16_t link_info_sta_prof_ofst;
+	uint32_t prb_csa_cnt_ofst;
+	uint32_t prb_ext_csa_cnt_ofst;
+	bool csa_ext_csa_exist;
+};
+
+/**
+ * struct mlo_sch_partner_links - ML partner links
+ * @num_links: Number of links
+ * @mlo_ie_link_info_ofst: offset of link info in mlo IE
+ * @partner_info: Partner link info
+ */
+struct mlo_sch_partner_links {
+	uint8_t num_links;
+	uint16_t mlo_ie_link_info_ofst;
+	struct ml_sch_partner_info partner_info[WLAN_UMAC_MLO_MAX_VDEVS];
+};
+#endif
+
 /**
  * struct sch_context - SCH global context
  * @beacon_interval: global beacon interval
@@ -57,6 +94,9 @@ struct sch_context {
 	uint16_t p2p_ie_offset;
 	uint32_t csa_count_offset;
 	uint32_t ecsa_count_offset;
+#ifdef WLAN_FEATURE_11BE_MLO
+	struct mlo_sch_partner_links sch_mlo_partner;
+#endif
 };
 
 #endif

core/mac/src/pe/sch/sch_api.c

@@ -81,10 +81,44 @@ static void lim_notify_link_info(struct pe_session *pe_session)
 		lim_mlo_release_vdev_ref(wlan_vdev_list[link]);
 	}
 }
+
+/**
+ * lim_update_sch_mlo_partner() - update partner information needed in mlo IE
+ * @mac: pointer to mac
+ * @pe_session: pointer to pe session
+ * @bcn_param: pointer to tpSendbeaconParams
+ *
+ * Return: void
+ */
+static void lim_update_sch_mlo_partner(struct mac_context *mac,
+				       struct pe_session *pe_session,
+				       tpSendbeaconParams bcn_param)
+{
+	int link;
+	struct ml_sch_partner_info *sch_info;
+	struct ml_bcn_partner_info *bcn_info;
+
+	bcn_param->mlo_partner.num_links = mac->sch.sch_mlo_partner.num_links;
+	for (link = 0; link < mac->sch.sch_mlo_partner.num_links; link++) {
+		sch_info = &mac->sch.sch_mlo_partner.partner_info[link];
+		bcn_info = &bcn_param->mlo_partner.partner_info[link];
+		bcn_info->vdev_id = sch_info->vdev_id;
+		bcn_info->beacon_interval = sch_info->beacon_interval;
+		bcn_info->csa_switch_count_offset = sch_info->bcn_csa_cnt_ofst;
+		bcn_info->ext_csa_switch_count_offset =
+					sch_info->bcn_ext_csa_cnt_ofst;
+	}
+}
 #else
 static void lim_notify_link_info(struct pe_session *pe_session)
 {
 }
+
+static void lim_update_sch_mlo_partner(struct mac_context *mac,
+				       struct pe_session *pe_session,
+				       tpSendbeaconParams bcn_param)
+{
+}
 #endif
 
 QDF_STATUS sch_send_beacon_req(struct mac_context *mac, uint8_t *beaconPayload,
@@ -127,7 +161,7 @@ QDF_STATUS sch_send_beacon_req(struct mac_context *mac, uint8_t *beaconPayload,
 		beaconParams->csa_count_offset = mac->sch.csa_count_offset;
 		beaconParams->ecsa_count_offset = mac->sch.ecsa_count_offset;
 	}
-
+	lim_update_sch_mlo_partner(mac, pe_session, beaconParams);
 	beaconParams->vdev_id = pe_session->smeSessionId;
 	beaconParams->reason = reason;
 

core/mac/src/pe/sch/sch_beacon_gen.c

@@ -48,6 +48,53 @@
 
 const uint8_t p2p_oui[] = { 0x50, 0x6F, 0x9A, 0x9 };
 
+/**
+ * sch_get_csa_ecsa_count_offset() - get the offset of Switch count field
+ * @ie: pointer to the beggining of IEs in the beacon frame buffer
+ * @ie_len: length of the IEs in the buffer
+ * @csa_count_offset: pointer to the csa_count_offset variable in the caller
+ * @ecsa_count_offset: pointer to the ecsa_count_offset variable in the caller
+ *
+ * Gets the offset of the switch count field in the CSA/ECSA IEs from the start
+ * of the IEs buffer.
+ *
+ * Return: None
+ */
+static void sch_get_csa_ecsa_count_offset(const uint8_t *ie, uint32_t ie_len,
+					  uint32_t *csa_count_offset,
+					  uint32_t *ecsa_count_offset)
+{
+	const uint8_t *ptr = ie;
+	uint8_t elem_id;
+	uint16_t elem_len;
+	uint32_t offset = 0;
+
+	/* IE is not present */
+	if (!ie_len)
+		return;
+
+	while (ie_len >= 2) {
+		elem_id = ptr[0];
+		elem_len = ptr[1];
+		ie_len -= 2;
+		offset += 2;
+
+		if (elem_id == DOT11F_EID_CHANSWITCHANN &&
+		    elem_len == 3)
+			*csa_count_offset = offset +
+					SCH_CSA_SWITCH_COUNT_OFFSET;
+
+		if (elem_id == DOT11F_EID_EXT_CHAN_SWITCH_ANN &&
+		    elem_len == 4)
+			*ecsa_count_offset = offset +
+					SCH_ECSA_SWITCH_COUNT_OFFSET;
+
+		ie_len -= elem_len;
+		offset += elem_len;
+		ptr += (elem_len + 2);
+	}
+}
+
 #ifdef WLAN_FEATURE_11BE_MLO
 /**
  * lim_update_link_info() - update mlo_link_info
@@ -196,6 +243,63 @@ static void lim_update_link_info(struct mac_context *mac_ctx,
 			     sizeof(bcn_2->eht_cap));
 	}
 }
+
+static void lim_upt_mlo_partner_info(struct mac_context *mac,
+				     struct pe_session *session,
+				     uint8_t *ie, uint32_t ie_len,
+				     uint16_t ie_offset)
+{
+	const uint8_t *mlo_ie;
+	uint16_t subie_len;
+	const uint8_t *subie_sta_prof;
+	uint16_t subie_sta_prof_len;
+	int link;
+	struct ml_sch_partner_info *sch_info;
+	uint16_t per_sta_ofst = mac->sch.sch_mlo_partner.mlo_ie_link_info_ofst;
+
+	mlo_ie = wlan_get_ext_ie_ptr_from_ext_id(MLO_IE_OUI_TYPE,
+						 MLO_IE_OUI_SIZE,
+						 ie, ie_len);
+	/* IE is not present */
+	if (!mlo_ie) {
+		pe_err("no mlo ie in mlo ap vdev id %d", session->vdev_id);
+		return;
+	}
+	for (link = 0; link < mac->sch.sch_mlo_partner.num_links; link++) {
+		sch_info = &mac->sch.sch_mlo_partner.partner_info[link];
+		if (!sch_info->link_info_sta_prof_ofst)
+			continue;
+		if (!sch_info->csa_ext_csa_exist) {
+			per_sta_ofst += 1; /* subelement ID */
+			subie_len = mlo_ie[per_sta_ofst];
+			per_sta_ofst += 1; /* length */
+			per_sta_ofst += subie_len; /* payload of per sta info */
+			continue;
+		}
+		subie_sta_prof = mlo_ie + per_sta_ofst +
+					sch_info->link_info_sta_prof_ofst;
+		per_sta_ofst += 1; /* subelement ID */
+		subie_len = mlo_ie[per_sta_ofst];
+		per_sta_ofst += 1; /* length */
+		per_sta_ofst += subie_len; /* payload of per sta info */
+		subie_sta_prof_len = subie_len + 2 -
+					sch_info->link_info_sta_prof_ofst;
+		sch_get_csa_ecsa_count_offset(subie_sta_prof,
+					      subie_sta_prof_len,
+					      &sch_info->bcn_csa_cnt_ofst,
+					      &sch_info->bcn_ext_csa_cnt_ofst);
+		/* plus offset from IE of sta prof to ie */
+		if (sch_info->bcn_csa_cnt_ofst)
+			sch_info->bcn_csa_cnt_ofst += ie_offset +
+						subie_sta_prof - ie;
+		if (sch_info->bcn_ext_csa_cnt_ofst)
+			sch_info->bcn_ext_csa_cnt_ofst += ie_offset +
+						subie_sta_prof - ie;
+		pe_debug("vdev %d mlo csa_count_offset %d ecsa_count_offset %d",
+			 sch_info->vdev_id, sch_info->bcn_csa_cnt_ofst,
+			 sch_info->bcn_ext_csa_cnt_ofst);
+	}
+}
 #else
 static void lim_update_link_info(struct mac_context *mac_ctx,
 				 struct pe_session *session,
@@ -203,6 +307,13 @@ static void lim_update_link_info(struct mac_context *mac_ctx,
 				 tDot11fBeacon2 *bcn_2)
 {
 }
+
+static void lim_upt_mlo_partner_info(struct mac_context *mac,
+				     struct pe_session *session,
+				     uint8_t *ie, uint32_t ie_len,
+				     uint16_t ie_offset)
+{
+}
 #endif
 
 static QDF_STATUS sch_get_p2p_ie_offset(uint8_t *pextra_ie,
@@ -322,53 +433,6 @@ sch_append_addn_ie(struct mac_context *mac_ctx, struct pe_session *session,
 	return status;
 }
 
-/**
- * sch_get_csa_ecsa_count_offset() - get the offset of Switch count field
- * @ie: pointer to the beggining of IEs in the beacon frame buffer
- * @ie_len: length of the IEs in the buffer
- * @csa_count_offset: pointer to the csa_count_offset variable in the caller
- * @ecsa_count_offset: pointer to the ecsa_count_offset variable in the caller
- *
- * Gets the offset of the switch count field in the CSA/ECSA IEs from the start
- * of the IEs buffer.
- *
- * Return: None
- */
-static void sch_get_csa_ecsa_count_offset(uint8_t *ie, uint32_t ie_len,
-					  uint32_t *csa_count_offset,
-					  uint32_t *ecsa_count_offset)
-{
-	uint8_t *ptr = ie;
-	uint8_t elem_id;
-	uint16_t elem_len;
-	uint32_t offset = 0;
-
-	/* IE is not present */
-	if (!ie_len)
-		return;
-
-	while (ie_len >= 2) {
-		elem_id = ptr[0];
-		elem_len = ptr[1];
-		ie_len -= 2;
-		offset += 2;
-
-		if (elem_id == DOT11F_EID_CHANSWITCHANN &&
-		    elem_len == 3)
-			*csa_count_offset = offset +
-					SCH_CSA_SWITCH_COUNT_OFFSET;
-
-		if (elem_id == DOT11F_EID_EXT_CHAN_SWITCH_ANN &&
-		    elem_len == 4)
-			*ecsa_count_offset = offset +
-					SCH_ECSA_SWITCH_COUNT_OFFSET;
-
-		ie_len -= elem_len;
-		offset += elem_len;
-		ptr += (elem_len + 2);
-	}
-}
-
 /**
  * sch_set_fixed_beacon_fields() - sets the fixed params in beacon frame
  * @mac_ctx:       mac global context
@@ -823,10 +887,17 @@ sch_set_fixed_beacon_fields(struct mac_context *mac_ctx, struct pe_session *sess
 				session->schBeaconOffsetBegin + TIM_IE_SIZE +
 				ecsa_count_offset;
 
-	pe_debug("csa_count_offset %d ecsa_count_offset %d",
+	pe_debug("vdev id %d csa_count_offset %d ecsa_count_offset %d",
+		 session->vdev_id,
 		 mac_ctx->sch.csa_count_offset,
 		 mac_ctx->sch.ecsa_count_offset);
 
+	if (wlan_vdev_mlme_is_mlo_ap(session->vdev))
+		lim_upt_mlo_partner_info(mac_ctx, session,
+					 session->pSchBeaconFrameEnd, n_bytes,
+					 session->schBeaconOffsetBegin +
+					 TIM_IE_SIZE);
+
 	extra_ie = session->pSchBeaconFrameEnd + n_bytes;
 	extra_ie_offset = n_bytes;
 

core/mac/src/sys/legacy/src/utils/src/parser_api.c

@@ -7533,21 +7533,40 @@ QDF_STATUS populate_dot11f_bcn_mlo_ie(struct mac_context *mac_ctx,
 	uint16_t vdev_count;
 	struct wlan_objmgr_vdev *wlan_vdev_list[WLAN_UMAC_MLO_MAX_VDEVS];
 	struct pe_session *link_session;
+	uint16_t tmp_offset = 0;
+	struct ml_sch_partner_info *tmp_info;
+	struct mlo_sch_partner_links *sch_info;
 
+	qdf_mem_zero(&mac_ctx->sch.sch_mlo_partner,
+		     sizeof(mac_ctx->sch.sch_mlo_partner));
+	sch_info = &mac_ctx->sch.sch_mlo_partner;
 	mlo_ie->present = 1;
 	mlo_ie->mld_mac_addr_present = 1;
 	mlo_ie->type = 0;
+	tmp_offset += 1; /* Element ID */
+	tmp_offset += 1; /* length */
+	tmp_offset += 1; /* Element ID extension */
+	tmp_offset += 2; /* Multi-link control */
 	qdf_mem_copy(mlo_ie->mld_mac_addr.info.mld_mac_addr,
 		     session->vdev->mlo_dev_ctx->mld_addr.bytes,
 		     sizeof(mlo_ie->mld_mac_addr.info.mld_mac_addr));
+	tmp_offset += 6; /* mld mac addr */
 	mlo_ie->link_id_info_present = 1;
 	mlo_ie->link_id_info.info.link_id = wlan_vdev_get_link_id(
 						session->vdev);
+	tmp_offset += 1; /* link id */
 	mlo_ie->bss_param_change_cnt_present = 1;
 	mlo_ie->bss_param_change_cnt.info.bss_param_change_count =
 		session->mlo_link_info.link_ie.bss_param_change_cnt;
+	tmp_offset += 1; /* bss parameters change count */
+	mlo_ie->mld_capab_present = 1;
+	tmp_offset += 2; /* mld capabilities */
+	sch_info->num_links = 0;
 
 	lim_get_mlo_vdev_list(session, &vdev_count, wlan_vdev_list);
+	mlo_ie->mld_capabilities.info.max_simultaneous_link_num =
+							vdev_count - 1;
+	sch_info->mlo_ie_link_info_ofst = tmp_offset;
 	for (link = 0; link < vdev_count; link++) {
 		if (!wlan_vdev_list[link])
 			continue;
@@ -7561,20 +7580,32 @@ QDF_STATUS populate_dot11f_bcn_mlo_ie(struct mac_context *mac_ctx,
 		}
 		link_session = pe_find_session_by_vdev_id(
 			mac_ctx, wlan_vdev_list[link]->vdev_objmgr.vdev_id);
+		if (!link_session) {
+			lim_mlo_release_vdev_ref(wlan_vdev_list[link]);
+			continue;
+		}
 		sta_pro = &mlo_ie->sta_profile[num_sta_pro];
 		link_ie = &link_session->mlo_link_info.link_ie;
+		tmp_info = &sch_info->partner_info[sch_info->num_links++];
+		tmp_info->vdev_id = wlan_vdev_get_id(wlan_vdev_list[link]);
+		tmp_info->beacon_interval =
+			link_session->beaconParams.beaconInterval;
 		sta_pro->present = 0;
+		tmp_offset = 1; /* sub element id */
+		tmp_offset += 1; /* length */
 		if (link_ie->link_csa.present) {
 			qdf_mem_copy(&sta_pro->ChanSwitchAnn,
 				     &link_ie->link_csa,
 				     sizeof(sta_pro->ChanSwitchAnn));
 			sta_pro->present = 1;
+			tmp_info->csa_ext_csa_exist = true;
 		}
 		if (link_ie->link_ecsa.present) {
 			qdf_mem_copy(&sta_pro->ext_chan_switch_ann,
 				     &link_ie->link_ecsa,
 				     sizeof(sta_pro->ext_chan_switch_ann));
 			sta_pro->present = 1;
+			tmp_info->csa_ext_csa_exist = true;
 		}
 		if (link_ie->link_quiet.present) {
 			qdf_mem_copy(&sta_pro->Quiet,
@@ -7592,11 +7623,9 @@ QDF_STATUS populate_dot11f_bcn_mlo_ie(struct mac_context *mac_ctx,
 			sta_pro->link_id = wlan_vdev_get_link_id(
 							link_session->vdev);
 			sta_pro->complete_profile = 0;
-			sta_pro->sta_mac_addr_present = 1;
-			qdf_mem_copy(sta_pro->sta_mac_addr.info.sta_mac_addr,
-				     link_session->self_mac_addr,
-				     sizeof(tSirMacAddr));
 			num_sta_pro++;
+			tmp_offset += 2; /* sta control */
+			tmp_info->link_info_sta_prof_ofst = tmp_offset;
 		}
 		lim_mlo_release_vdev_ref(wlan_vdev_list[link]);
 	}

core/wma/inc/wma_if.h

@@ -484,6 +484,9 @@ typedef struct {
 	uint32_t ecsa_count_offset;
 	enum sir_bcn_update_reason reason;
 	QDF_STATUS status;
+#ifdef WLAN_FEATURE_11BE_MLO
+	struct mlo_bcn_templ_partner_links mlo_partner;
+#endif
 } tSendbeaconParams, *tpSendbeaconParams;
 
 /**

core/wma/src/wma_mgmt.c

@@ -2075,6 +2075,49 @@ static int wmi_unified_probe_rsp_tmpl_send(tp_wma_handle wma,
 						   &params);
 }
 
+#ifdef WLAN_FEATURE_11BE_MLO
+/**
+ * wma_upt_mlo_partner_info() - update mlo info in beacon template
+ * @params: beacon template params
+ * @bcn_param: beacon params
+ * @bytes_to_strip: bytes to strip
+ *
+ * Return: Void
+ */
+static void wma_upt_mlo_partner_info(struct beacon_tmpl_params *params,
+				     const tpSendbeaconParams bcn_param,
+				     uint8_t bytes_to_strip)
+{
+	struct ml_bcn_partner_info *bcn_info;
+	struct ml_bcn_partner_info *info;
+	int link;
+
+	params->mlo_partner.num_links = bcn_param->mlo_partner.num_links;
+	for (link = 0; link < params->mlo_partner.num_links; link++) {
+		bcn_info = &bcn_param->mlo_partner.partner_info[link];
+		info = &params->mlo_partner.partner_info[link];
+		info->vdev_id = bcn_info->vdev_id;
+		info->beacon_interval = bcn_info->beacon_interval;
+		if (bcn_info->csa_switch_count_offset &&
+		    bcn_info->csa_switch_count_offset > bytes_to_strip)
+			info->csa_switch_count_offset =
+				bcn_info->csa_switch_count_offset -
+					bytes_to_strip;
+		if (bcn_info->ext_csa_switch_count_offset &&
+		    bcn_info->ext_csa_switch_count_offset > bytes_to_strip)
+			info->ext_csa_switch_count_offset =
+				bcn_info->ext_csa_switch_count_offset -
+					bytes_to_strip;
+	}
+}
+#else
+static void wma_upt_mlo_partner_info(struct beacon_tmpl_params *params,
+				     const tpSendbeaconParams bcn_param,
+				     uint8_t bytes_to_strip)
+{
+}
+#endif
+
 /**
  * wma_unified_bcn_tmpl_send() - send beacon template to fw
  * @wma:wma handle
@@ -2166,6 +2209,8 @@ static QDF_STATUS wma_unified_bcn_tmpl_send(tp_wma_handle wma,
 		params.ext_csa_switch_count_offset =
 			bcn_info->ecsa_count_offset - bytes_to_strip;
 
+	wma_upt_mlo_partner_info(&params, bcn_info, bytes_to_strip);
+
 	ret = wmi_unified_beacon_tmpl_send_cmd(wma->wmi_handle,
 				 &params);
 	if (QDF_IS_STATUS_ERROR(ret))