qcacmn: Generate link specific probe response

Changes made to link generation api to handle probe response. Change-Id: I5b86599a9704e3c41bbb0d0127078d586e4d1d8c CRs-Fixed: 3158769
2022-03-24 09:50:28 -07:00
--- a/umac/mlo_mgr/inc/utils_mlo.h
+++ b/umac/mlo_mgr/inc/utils_mlo.h
@@ -95,7 +95,43 @@ util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
 			qdf_size_t *link_frame_len);

 /**
- * util_find_mlie() - Find the  Multi-Link element
+ * util_gen_link_probe_rsp() - Generate link specific probe response
+ * @frame: Pointer to original probe response. This should not contain the
+ * 802.11 header, and must start from the fixed fields in the probe
+ * response. This is required due to some caller semantics built into the end to
+ * end design.
+ * @frame_len: Length of original probe response
+ * @link_addr: Secondary link's MAC address
+ * @link_frame: Generated secondary link specific probe response. Note
+ * that this will start from the 802.11 header (unlike the original probe
+ * response). This should be ignored in the case of failure.
+ * @link_frame_maxsize: Maximum size of generated secondary link specific
+ * probe response
+ * @link_frame_len: Pointer to location where populated length of generated
+ * secondary link specific probe response should be written. This should
+ * be ignored in the case of failure.
+ *
+ * Generate a link specific logically equivalent probe response for the
+ * secondary link from the original probe response containing a Multi-Link
+ * element. This applies to both probe responses.
+ * Currently, only two link MLO is supported.
+ *
+ * Return: QDF_STATUS_SUCCESS in the case of success, QDF_STATUS value giving
+ * the reason for error in the case of failure.
+ */
+QDF_STATUS
+util_gen_link_probe_rsp(uint8_t *frame, qdf_size_t frame_len,
+			struct qdf_mac_addr link_addr,
+			uint8_t *link_frame,
+			qdf_size_t link_frame_maxsize,
+			qdf_size_t *link_frame_len);
+
+/**
+ * util_find_mlie - Find the first Multi-Link element or the start of the first
+ * Multi-Link element fragment sequence in a given buffer containing elements,
+ * if a Multi-Link element or element fragment sequence exists in the given
+ * buffer.
+ *
 * @buf: Buffer to be searched for the Multi-Link element or the start of the
 * Multi-Link element fragment sequence
 * @buflen: Length of the buffer
--- a/umac/mlo_mgr/src/utils_mlo.c
+++ b/umac/mlo_mgr/src/utils_mlo.c
@@ -243,6 +243,9 @@ static QDF_STATUS
 util_parse_bvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
 					qdf_size_t subelempayloadlen,
 					uint8_t *linkid,
+					uint16_t *beaconinterval,
+					bool *is_beaconinterval_valid,
+					bool *is_complete_profile,
 					bool *is_macaddr_valid,
 					struct qdf_mac_addr *macaddr,
 					bool is_staprof_reqd,
@@ -304,6 +307,9 @@ util_parse_bvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
 				       WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
 				       WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS);

+	if (completeprofile && is_complete_profile)
+		*is_complete_profile = true;
+
 	/* Check STA Info Length */
 	if (subelempayloadlen <
 		parsed_payload_len + WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE) {
@@ -361,6 +367,15 @@ util_parse_bvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
 			return QDF_STATUS_E_PROTO;
 		}

+		if (beaconinterval) {
+			qdf_mem_copy(beaconinterval,
+				     subelempayload + parsed_payload_len,
+				     WLAN_BEACONINTERVAL_LEN);
+			*beaconinterval = qdf_le16_to_cpu(*beaconinterval);
+
+			if (is_beaconinterval_valid)
+				*is_beaconinterval_valid = true;
+		}
 		parsed_payload_len += WLAN_BEACONINTERVAL_LEN;
 	}

@@ -617,6 +632,9 @@ QDF_STATUS util_parse_partner_info_from_linkinfo(uint8_t *linkinfo,
 								      sizeof(struct subelem_header),
 								      subelemseqpayloadlen,
 								      &linkid,
+								      NULL,
+								      NULL,
+								      NULL,
 								      &is_macaddr_valid,
 								      &macaddr,
 								      false,
@@ -1059,14 +1077,16 @@ QDF_STATUS util_validate_sta_prof_ie(const uint8_t *sta_prof_ie,
 #define MLO_LINKSPECIFIC_ASSOC_REQ_FC1  0x00
 #define MLO_LINKSPECIFIC_ASSOC_RESP_FC0 0x10
 #define MLO_LINKSPECIFIC_ASSOC_RESP_FC1 0x00
+#define MLO_LINKSPECIFIC_PROBE_RESP_FC0 0x50
+#define MLO_LINKSPECIFIC_PROBE_RESP_FC1 0x00

 static
-QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
-					  uint8_t subtype,
-					  struct qdf_mac_addr link_addr,
-					  uint8_t *link_frame,
-					  qdf_size_t link_frame_maxsize,
-					  qdf_size_t *link_frame_len)
+QDF_STATUS util_gen_link_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
+				    uint8_t subtype,
+				    struct qdf_mac_addr link_addr,
+				    uint8_t *link_frame,
+				    qdf_size_t link_frame_maxsize,
+				    qdf_size_t *link_frame_len)
 {
 	/* Please see documentation for util_gen_link_assoc_req() and
 	 * util_gen_link_assoc_resp() for information on the inputs to and
@@ -1200,7 +1220,12 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 	 * subelement header and fragment headers if any.
 	 */
 	qdf_size_t subelemseqpayloadlen;
-
+	/* Pointer to Beacon interval in STA info field */
+	uint16_t beaconinterval;
+	/* Whether Beacon interval value valid */
+	bool is_beaconinterval_valid;
+	/* If Complete Profile or not*/
+	bool is_completeprofile;
 	qdf_size_t tmplen;
 	QDF_STATUS ret;

@@ -1217,7 +1242,8 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 	if ((subtype != WLAN_FC0_STYPE_ASSOC_REQ) &&
 	    (subtype != WLAN_FC0_STYPE_REASSOC_REQ) &&
 	    (subtype != WLAN_FC0_STYPE_ASSOC_RESP) &&
-	    (subtype != WLAN_FC0_STYPE_REASSOC_RESP)) {
+	    (subtype != WLAN_FC0_STYPE_REASSOC_RESP) &&
+	    (subtype != WLAN_FC0_STYPE_PROBE_RESP)) {
 		mlo_err("802.11 frame subtype %u is invalid", subtype);
 		return QDF_STATUS_E_INVAL;
 	}
@@ -1243,6 +1269,8 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 		frame_iesection_offset = WLAN_ASSOC_REQ_IES_OFFSET;
 	} else if (subtype == WLAN_FC0_STYPE_REASSOC_REQ) {
 		frame_iesection_offset = WLAN_REASSOC_REQ_IES_OFFSET;
+	} else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
+		frame_iesection_offset = WLAN_PROBE_RESP_IES_OFFSET;
 	} else {
 		/* This is a (re)association response */
 		frame_iesection_offset = WLAN_ASSOC_RSP_IES_OFFSET;
@@ -1473,12 +1501,17 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 	sta_prof_remlen = 0;
 	sta_prof_currpos = NULL;
 	is_reportedmacaddr_valid = false;
+	is_beaconinterval_valid = false;
+	is_completeprofile = false;

 	/* Parse per-STA profile */
 	ret = util_parse_bvmlie_perstaprofile_stactrl(persta_prof +
 						      sizeof(struct subelem_header),
 						      subelemseqpayloadlen,
 						      NULL,
+						      &beaconinterval,
+						      &is_beaconinterval_valid,
+						      &is_completeprofile,
 						      &is_reportedmacaddr_valid,
 						      &reportedmacaddr,
 						      true,
@@ -1489,6 +1522,11 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 		return ret;
 	}

+	if (subtype == WLAN_FC0_STYPE_PROBE_RESP && !is_completeprofile) {
+		mlo_err("Complete profile information is not present in per-STA profile of probe response frame");
+		return QDF_STATUS_E_NOSUPPORT;
+	}
+
 	/* We double check for a NULL STA Profile, though the helper function
 	 * above would have taken care of this. We need to get a non-NULL STA
 	 * profile, because we need to get at least the expected fixed fields,
@@ -1608,7 +1646,7 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 			mlo_debug("Reassoc req: Added Current AP address field (%u octets) to link specific frame",
 				  QDF_MAC_ADDR_SIZE);
 		}
-	} else {
+	} else if (subtype == WLAN_FC0_STYPE_ASSOC_RESP) {
 		/* This is a (re)association response */
 		mlo_debug("Populating fixed fields for (re)assoc resp in link specific frame");

@@ -1672,6 +1710,101 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 		link_frame_currlen += WLAN_AID_LEN;
 		mlo_debug("Added AID field (%u octets) to link specific frame",
 			  WLAN_AID_LEN);
+	} else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
+		/* This is a probe response */
+		mlo_debug("Populating fixed fields for probe response in link specific frame");
+
+		if (sta_prof_remlen < WLAN_TIMESTAMP_LEN) {
+			mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Timestamp Length %u",
+				   sta_prof_remlen,
+				   WLAN_TIMESTAMP_LEN);
+
+			qdf_mem_free(mlieseqpayload_copy);
+			return QDF_STATUS_E_PROTO;
+		}
+
+		/* Timestamp field information is specific to the link.
+		 * Copy this from the STA profile.
+		 */
+
+		if ((link_frame_maxsize - link_frame_currlen) <
+				WLAN_TIMESTAMP_LEN) {
+			mlo_err("Insufficent space in link specific frame for Timestamp Info field. Required: %u octets, available: %zu octets",
+				WLAN_TIMESTAMP_LEN,
+				(link_frame_maxsize - link_frame_currlen));
+
+			qdf_mem_free(mlieseqpayload_copy);
+			return QDF_STATUS_E_NOMEM;
+		}
+
+		qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
+			     WLAN_TIMESTAMP_LEN);
+		link_frame_currpos += WLAN_TIMESTAMP_LEN;
+		link_frame_currlen += WLAN_TIMESTAMP_LEN;
+		mlo_debug("Added Timestamp Info field (%u octets) to link specific frame",
+			  WLAN_TIMESTAMP_LEN);
+
+		sta_prof_currpos += WLAN_TIMESTAMP_LEN;
+		sta_prof_remlen -= WLAN_TIMESTAMP_LEN;
+
+		if (!is_beaconinterval_valid) {
+			mlo_err_rl("Beacon interval information not present in STA info field of per-STA profile");
+			qdf_mem_free(mlieseqpayload_copy);
+			return QDF_STATUS_E_PROTO;
+		}
+
+		/* Beacon Interval information copy this from
+		 * the STA info field.
+		 */
+		if ((link_frame_maxsize - link_frame_currlen) <
+				WLAN_BEACONINTERVAL_LEN) {
+			mlo_err("Insufficent space in link specific frame for Beacon Interval Info field. Required: %u octets, available: %zu octets",
+				WLAN_BEACONINTERVAL_LEN,
+				(link_frame_maxsize - link_frame_currlen));
+
+			qdf_mem_free(mlieseqpayload_copy);
+			return QDF_STATUS_E_NOMEM;
+		}
+
+		qdf_mem_copy(link_frame_currpos, &beaconinterval,
+			     WLAN_BEACONINTERVAL_LEN);
+		link_frame_currpos += WLAN_BEACONINTERVAL_LEN;
+		link_frame_currlen += WLAN_BEACONINTERVAL_LEN;
+		mlo_debug("Added Beacon Interval Info field (%u octets) to link specific frame",
+			  WLAN_BEACONINTERVAL_LEN);
+
+		if (sta_prof_remlen < WLAN_CAPABILITYINFO_LEN) {
+			mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info %u",
+				   sta_prof_remlen,
+				   WLAN_CAPABILITYINFO_LEN);
+
+			qdf_mem_free(mlieseqpayload_copy);
+			return QDF_STATUS_E_PROTO;
+		}
+
+		/* Capability information is specific to the link. Copy this
+		 * from the STA profile.
+		 */
+
+		if ((link_frame_maxsize - link_frame_currlen) <
+				WLAN_CAPABILITYINFO_LEN) {
+			mlo_err("Insufficent space in link specific frame for Capability Info field. Required: %u octets, available: %zu octets",
+				WLAN_CAPABILITYINFO_LEN,
+				(link_frame_maxsize - link_frame_currlen));
+
+			qdf_mem_free(mlieseqpayload_copy);
+			return QDF_STATUS_E_NOMEM;
+		}
+
+		qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
+			     WLAN_CAPABILITYINFO_LEN);
+		link_frame_currpos += WLAN_CAPABILITYINFO_LEN;
+		link_frame_currlen += WLAN_CAPABILITYINFO_LEN;
+		mlo_debug("Added Capablity Info field (%u octets) to link specific frame",
+			  WLAN_CAPABILITYINFO_LEN);
+
+		sta_prof_currpos += WLAN_CAPABILITYINFO_LEN;
+		sta_prof_remlen -= WLAN_CAPABILITYINFO_LEN;
 	}

 	sta_prof_iesection = sta_prof_currpos;
@@ -1705,14 +1838,15 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
 					frame_iesection_len);

 	if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
-	    (subtype == WLAN_FC0_STYPE_REASSOC_REQ)) {
+	    (subtype == WLAN_FC0_STYPE_REASSOC_REQ) ||
+	    (subtype == WLAN_FC0_STYPE_PROBE_RESP)) {
 		/* Sanity check that the SSID element is present for the
 		 * reporting STA. There is no stipulation in the standard for
 		 * the STA profile in this regard, so we do not check the STA
 		 * profile for the SSID element.
 		 */
 		if (!reportingsta_ie) {
-			mlo_err_rl("SSID element not found for reporting STA for (re)association request.");
+			mlo_err_rl("SSID element not found in reporting STA of the frame.");
 			qdf_mem_free(mlieseqpayload_copy);
 			return QDF_STATUS_E_PROTO;
 		}
@@ -2065,6 +2199,16 @@ QDF_STATUS util_gen_link_assoc_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,

 		link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_ASSOC_REQ_FC0;
 		link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_ASSOC_REQ_FC1;
+	} else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
+		qdf_mem_copy(link_frame_hdr->i_addr3, &link_addr,
+			     QDF_MAC_ADDR_SIZE);
+		qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
+			     QDF_MAC_ADDR_SIZE);
+		qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
+			     QDF_MAC_ADDR_SIZE);
+
+		link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_PROBE_RESP_FC0;
+		link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_PROBE_RESP_FC1;
 	} else {
 		/* This is a (re)association response */

@@ -2095,7 +2239,7 @@ util_gen_link_assoc_req(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
 			qdf_size_t link_frame_maxsize,
 			qdf_size_t *link_frame_len)
 {
-	return util_gen_link_assoc_reqrsp_cmn(frame, frame_len,
+	return util_gen_link_reqrsp_cmn(frame, frame_len,
 			(isreassoc ? WLAN_FC0_STYPE_REASSOC_REQ :
 				WLAN_FC0_STYPE_ASSOC_REQ),
 			link_addr, link_frame, link_frame_maxsize,
@@ -2109,13 +2253,26 @@ util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
 			qdf_size_t link_frame_maxsize,
 			qdf_size_t *link_frame_len)
 {
-	return util_gen_link_assoc_reqrsp_cmn(frame, frame_len,
+	return util_gen_link_reqrsp_cmn(frame, frame_len,
 			(isreassoc ?  WLAN_FC0_STYPE_REASSOC_RESP :
 				WLAN_FC0_STYPE_ASSOC_RESP),
 			link_addr, link_frame, link_frame_maxsize,
 			link_frame_len);
 }

+QDF_STATUS
+util_gen_link_probe_rsp(uint8_t *frame, qdf_size_t frame_len,
+			struct qdf_mac_addr link_addr,
+			uint8_t *link_frame,
+			qdf_size_t link_frame_maxsize,
+			qdf_size_t *link_frame_len)
+{
+	return util_gen_link_reqrsp_cmn(frame, frame_len,
+			 WLAN_FC0_STYPE_PROBE_RESP,
+			link_addr, link_frame, link_frame_maxsize,
+			link_frame_len);
+}
+
 QDF_STATUS
 util_find_mlie(uint8_t *buf, qdf_size_t buflen, uint8_t **mlieseq,
 	       qdf_size_t *mlieseqlen)
@@ -2788,4 +2945,5 @@ util_get_bvmlie_persta_partner_info(uint8_t *mlieseq,

 	return QDF_STATUS_SUCCESS;
 }
+
 #endif