qcacmn: Add ml ie to link specific prb rsp generation

Currently link specific probe response ie does not include
the basic variant ml ie. This results in multi link connection failure.

To solve this add basic variant mlo ie in the link probe response.

Change-Id: I7d6e9bcc461bf4e855c5613ad28a08749303a81d
CRs-Fixed: 3301288

umac/mlo_mgr/src/utils_mlo.c

@@ -1474,6 +1474,123 @@ QDF_STATUS util_validate_sta_prof_ie(const uint8_t *sta_prof_ie,
 	return QDF_STATUS_SUCCESS;
 }
 
+#ifdef CONN_MGR_ADV_FEATURE
+/**
+ * util_add_mlie_for_prb_rsp_gen - Add the basic variant Multi-Link element
+ * when generating link specific probe response.
+ * @reportingsta_ie: Pointer to the reportingsta ie
+ * @reportingsta_ie_len: Length for reporting sta ie
+ * @plink_frame_currpos: Pointer to Link frame current pos
+ * @plink_frame_currlen: Current length of link frame.
+ * @linkid: Link Id value
+ *
+ * Add the basic variant Multi-Link element when
+ * generating link specific probe response.
+ *
+ * Return: QDF_STATUS_SUCCESS in the case of success, QDF_STATUS value giving
+ * the reason for error in the case of failure
+ */
+static QDF_STATUS
+util_add_mlie_for_prb_rsp_gen(const uint8_t *reportingsta_ie,
+			      qdf_size_t reportingsta_ie_len,
+			      uint8_t **plink_frame_currpos,
+			      qdf_size_t *plink_frame_currlen,
+			      uint8_t linkid)
+{
+	uint8_t mlie_len = 0;
+	uint8_t common_info_len = 0;
+	struct wlan_ie_multilink ml_ie_ff;
+	uint16_t mlcontrol;
+	uint16_t presencebm;
+	uint8_t *mlie_frame = NULL;
+	uint8_t link_id_offset = sizeof(struct wlan_ie_multilink) +
+				QDF_MAC_ADDR_SIZE +
+				WLAN_ML_BV_CINFO_LENGTH_SIZE;
+	uint8_t *link_frame_currpos = *plink_frame_currpos;
+	qdf_size_t link_frame_currlen = *plink_frame_currlen;
+	QDF_STATUS status = QDF_STATUS_SUCCESS;
+
+	status = util_get_mlie_common_info_len((uint8_t *)reportingsta_ie,
+					       reportingsta_ie_len,
+					       &common_info_len);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		mlo_err("Failed while parsing the common info length");
+		return status;
+	}
+
+	/* common info len + bvmlie fixed fields */
+	mlie_len = common_info_len + sizeof(struct wlan_ie_multilink);
+
+	mlo_debug_rl("mlie_len %d, common_info_len %d, link_id_offset %d",
+		     mlie_len,
+		     common_info_len,
+		     link_id_offset);
+
+	mlie_frame = qdf_mem_malloc(mlie_len);
+	if (!mlie_frame)
+		return QDF_STATUS_E_NOMEM;
+
+	/* Copy ml ie fixed fields */
+	qdf_mem_copy(&ml_ie_ff,
+		     reportingsta_ie,
+		     sizeof(struct wlan_ie_multilink));
+
+	ml_ie_ff.elem_len = mlie_len - sizeof(struct ie_header);
+
+	mlcontrol = qdf_le16_to_cpu(ml_ie_ff.mlcontrol);
+	presencebm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
+				  WLAN_ML_CTRL_PBM_BITS);
+	qdf_set_bit(WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P,
+		    (unsigned long *)&presencebm);
+
+	QDF_SET_BITS(ml_ie_ff.mlcontrol,
+		     WLAN_ML_CTRL_PBM_IDX,
+		     WLAN_ML_CTRL_PBM_BITS,
+		     presencebm);
+
+	qdf_mem_copy(mlie_frame,
+		     &ml_ie_ff,
+		     sizeof(struct wlan_ie_multilink));
+
+	qdf_mem_copy(mlie_frame + sizeof(struct wlan_ie_multilink),
+		     reportingsta_ie + sizeof(struct wlan_ie_multilink),
+		     mlie_len - sizeof(struct wlan_ie_multilink));
+
+	if (linkid == 0xFF) {
+		qdf_mem_free(mlie_frame);
+		mlo_err("Link id is invalid");
+		return QDF_STATUS_E_INVAL;
+	}
+	mlie_frame[link_id_offset] = (mlie_frame[link_id_offset] & ~0x0f) |
+				   (linkid & 0x0f);
+	qdf_mem_copy(link_frame_currpos,
+		     mlie_frame,
+		     mlie_len);
+
+	mlo_debug("Add mlie for link id %d", linkid);
+	QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_PE, QDF_TRACE_LEVEL_DEBUG,
+			   mlie_frame, mlie_len);
+
+	link_frame_currpos += mlie_len;
+	link_frame_currlen += mlie_len;
+	*plink_frame_currpos = link_frame_currpos;
+	*plink_frame_currlen = link_frame_currlen;
+	qdf_mem_free(mlie_frame);
+
+	return QDF_STATUS_SUCCESS;
+}
+#else
+static QDF_STATUS
+util_add_mlie_for_prb_rsp_gen(const uint8_t *reportingsta_ie,
+			      qdf_size_t reportingsta_ie_len,
+			      uint8_t **plink_frame_currpos,
+			      qdf_size_t *plink_frame_currlen,
+			      uint8_t linkid)
+{
+	return QDF_STATUS_SUCCESS;
+}
+#endif
+
 #define MLO_LINKSPECIFIC_ASSOC_REQ_FC0  0x00
 #define MLO_LINKSPECIFIC_ASSOC_REQ_FC1  0x00
 #define MLO_LINKSPECIFIC_ASSOC_RESP_FC0 0x10
@@ -1635,6 +1752,7 @@ QDF_STATUS util_gen_link_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 	bool is_completeprofile;
 	qdf_size_t tmplen;
 	QDF_STATUS ret;
+	uint8_t linkid = 0xFF;
 
 	if (!frame) {
 		mlo_err("Pointer to original frame is NULL");
@@ -1918,7 +2036,7 @@ QDF_STATUS util_gen_link_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 	ret = util_parse_bvmlie_perstaprofile_stactrl(persta_prof +
 						      sizeof(struct subelem_header),
 						      subelemseqpayloadlen,
-						      NULL,
+						      &linkid,
 						      &beaconinterval,
 						      &is_beaconinterval_valid,
 						      &tsfoffset,
@@ -2299,6 +2417,18 @@ QDF_STATUS util_gen_link_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 					frame_iesection) == frame_iesection_len)
 				break;
 
+			/* Add BV ML IE for link specific probe response */
+			if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
+				ret = util_add_mlie_for_prb_rsp_gen(reportingsta_ie,
+								    reportingsta_ie[TAG_LEN_POS],
+								    &link_frame_currpos,
+								    &link_frame_currlen,
+								    linkid);
+				if (QDF_IS_STATUS_ERROR(ret)) {
+					qdf_mem_free(mlieseqpayload_copy);
+					return ret;
+				}
+			}
 			reportingsta_ie += reportingsta_ie_size;
 
 			ret = util_validate_reportingsta_ie(reportingsta_ie,