qcacmn: Add logic to stitch MPDU for LPC

Add logic to stitch MPDU from MSDU and
hold MPDU till PPDU_END tlv to update radiotap
header fields before submitting to stack for
local packet capture mode.

CRs-Fixed: 3821723
Change-Id: I7ac8127c1c0abfc747f37139c741dc69fb79a2a4

dp/wifi3.0/monitor/1.0/dp_rx_mon_dest_1.0.c

@@ -1456,6 +1456,30 @@ void dp_rx_pdev_mon_desc_pool_free(struct dp_pdev *pdev)
 		dp_rx_pdev_mon_cmn_desc_pool_free(pdev, mac_id);
 }
 
+#ifdef WLAN_FEATURE_LOCAL_PKT_CAPTURE
+static inline void
+dp_rx_lpc_lock_create(struct dp_mon_pdev *mon_pdev)
+{
+	qdf_spinlock_create(&mon_pdev->lpc_lock);
+}
+
+static inline void
+dp_rx_lpc_lock_destroy(struct dp_mon_pdev *mon_pdev)
+{
+	qdf_spinlock_destroy(&mon_pdev->lpc_lock);
+}
+#else
+static inline void
+dp_rx_lpc_lock_create(struct dp_mon_pdev *mon_pdev)
+{
+}
+
+static inline void
+dp_rx_lpc_lock_destroy(struct dp_mon_pdev *mon_pdev)
+{
+}
+#endif
+
 static void
 dp_rx_pdev_mon_cmn_desc_pool_deinit(struct dp_pdev *pdev, int mac_id)
 {
@@ -1476,6 +1500,7 @@ dp_rx_pdev_mon_desc_pool_deinit(struct dp_pdev *pdev)
 	for (mac_id = 0; mac_id < NUM_RXDMA_STATUS_RINGS_PER_PDEV; mac_id++)
 		dp_rx_pdev_mon_cmn_desc_pool_deinit(pdev, mac_id);
 	qdf_spinlock_destroy(&pdev->monitor_pdev->mon_lock);
+	dp_rx_lpc_lock_destroy(pdev->monitor_pdev);
 }
 
 static void
@@ -1499,6 +1524,7 @@ dp_rx_pdev_mon_desc_pool_init(struct dp_pdev *pdev)
 	for (mac_id = 0; mac_id < NUM_RXDMA_STATUS_RINGS_PER_PDEV; mac_id++)
 		dp_rx_pdev_mon_cmn_desc_pool_init(pdev, mac_id);
 	qdf_spinlock_create(&pdev->monitor_pdev->mon_lock);
+	dp_rx_lpc_lock_create(pdev->monitor_pdev);
 }
 
 void

dp/wifi3.0/monitor/1.0/dp_rx_mon_status_1.0.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2017-2021, The Linux Foundation. All rights reserved.
- * Copyright (c) 2021-2023 Qualcomm Innovation Center, Inc. All rights reserved.
+ * Copyright (c) 2021-2024 Qualcomm Innovation Center, Inc. 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 above
@@ -474,6 +474,9 @@ dp_rx_mon_status_process_tlv(struct dp_soc *soc, struct dp_intr *int_ctx,
 						ppdu_info, pdev->soc->hal_soc,
 						status_nbuf);
 
+				if (qdf_unlikely(IS_LOCAL_PKT_CAPTURE_RUNNING(mon_pdev, is_local_pkt_capture_running)))
+					dp_rx_handle_local_pkt_capture(pdev, ppdu_info, status_nbuf, tlv_status);
+
 				dp_rx_mon_update_dbg_ppdu_stats(ppdu_info,
 								rx_mon_stats);
 
@@ -529,12 +532,7 @@ dp_rx_mon_status_process_tlv(struct dp_soc *soc, struct dp_intr *int_ctx,
 				qdf_nbuf_free(status_nbuf);
 		} else if (qdf_unlikely(IS_LOCAL_PKT_CAPTURE_RUNNING(mon_pdev,
 				is_local_pkt_capture_running))) {
-			int ret;
-
-			ret = dp_rx_handle_local_pkt_capture(pdev, ppdu_info,
-							     status_nbuf);
-			if (ret)
-				qdf_nbuf_free(status_nbuf);
+			qdf_nbuf_free(status_nbuf);
 		} else if (qdf_unlikely(mon_pdev->mcopy_mode)) {
 			dp_rx_process_mcopy_mode(soc, pdev,
 						 ppdu_info, tlv_status,

dp/wifi3.0/monitor/dp_mon.h

@@ -1253,6 +1253,14 @@ struct  dp_mon_pdev {
 	uint8_t phy_ppdu_id_size;
 #ifdef WLAN_FEATURE_LOCAL_PKT_CAPTURE
 	bool is_local_pkt_capture_running;
+	/* Maintain MSDU list on PPDU */
+	qdf_nbuf_queue_t msdu_queue;
+	/* Maintain MPDU list of PPDU */
+	qdf_nbuf_queue_t mpdu_queue;
+	/* To  check if 1st MPDU of PPDU */
+	bool first_mpdu;
+	/* LPC lock */
+	qdf_spinlock_t lpc_lock;
 #endif
 };
 

dp/wifi3.0/monitor/dp_mon_filter.c

@@ -1021,6 +1021,25 @@ static void dp_mon_reset_local_pkt_capture_rx_filter(struct dp_pdev *pdev)
 	dp_mon_pdev_filter_init(mon_pdev);
 }
 
+static inline void
+dp_mon_init_local_pkt_capture_queue(struct dp_mon_pdev *mon_pdev)
+{
+	qdf_spin_lock_bh(&mon_pdev->lpc_lock);
+	qdf_nbuf_queue_init(&mon_pdev->msdu_queue);
+	qdf_nbuf_queue_init(&mon_pdev->mpdu_queue);
+	mon_pdev->first_mpdu = true;
+	qdf_spin_unlock_bh(&mon_pdev->lpc_lock);
+}
+
+static inline void
+dp_mon_free_local_pkt_capture_queue(struct dp_mon_pdev *mon_pdev)
+{
+	qdf_spin_lock_bh(&mon_pdev->lpc_lock);
+	qdf_nbuf_queue_free(&mon_pdev->msdu_queue);
+	qdf_nbuf_queue_free(&mon_pdev->mpdu_queue);
+	qdf_spin_unlock_bh(&mon_pdev->lpc_lock);
+}
+
 QDF_STATUS dp_mon_start_local_pkt_capture(struct cdp_soc_t *cdp_soc,
 					  uint8_t pdev_id,
 					  struct cdp_monitor_filter *filter)
@@ -1070,6 +1089,7 @@ QDF_STATUS dp_mon_start_local_pkt_capture(struct cdp_soc_t *cdp_soc,
 
 	dp_mon_filter_debug("local pkt capture tx filter set");
 
+	dp_mon_init_local_pkt_capture_queue(mon_pdev);
 	dp_mon_set_local_pkt_capture_running(mon_pdev, true);
 	return status;
 }
@@ -1096,6 +1116,7 @@ QDF_STATUS dp_mon_stop_local_pkt_capture(struct cdp_soc_t *cdp_soc,
 		return QDF_STATUS_SUCCESS;
 	}
 
+	dp_mon_set_local_pkt_capture_running(mon_pdev, false);
 	qdf_spin_lock_bh(&mon_pdev->mon_lock);
 	dp_mon_reset_local_pkt_capture_rx_filter(pdev);
 	status = dp_mon_filter_update(pdev);
@@ -1112,7 +1133,8 @@ QDF_STATUS dp_mon_stop_local_pkt_capture(struct cdp_soc_t *cdp_soc,
 	qdf_spin_unlock_bh(&mon_pdev->mon_lock);
 	dp_mon_filter_debug("local pkt capture stopped");
 
-	dp_mon_set_local_pkt_capture_running(mon_pdev, false);
+	dp_mon_free_local_pkt_capture_queue(mon_pdev);
+
 	return QDF_STATUS_SUCCESS;
 }
 

dp/wifi3.0/monitor/dp_rx_mon.c

@@ -1667,48 +1667,187 @@ dp_rx_handle_smart_mesh_mode(struct dp_soc *soc, struct dp_pdev *pdev,
 }
 
 #ifdef WLAN_FEATURE_LOCAL_PKT_CAPTURE
-int dp_rx_handle_local_pkt_capture(struct dp_pdev *pdev,
-				   struct hal_rx_ppdu_info *ppdu_info,
-				   qdf_nbuf_t nbuf)
+/**
+ * dp_rx_mon_stitch_mpdu() - Stich MPDU from MSDU
+ * @mon_pdev: mon_pdev handle
+ * @tail: 1st MSDU of next MPDU
+ *
+ * Return: mpdu buf
+ */
+static qdf_nbuf_t
+dp_rx_mon_stitch_mpdu(struct dp_mon_pdev *mon_pdev, qdf_nbuf_t tail)
+{
+	qdf_nbuf_t head, nbuf, next;
+	qdf_nbuf_t mpdu_buf = NULL, head_frag_list = NULL;
+	uint32_t is_first_frag, frag_list_sum_len = 0;
+
+	if (!(qdf_nbuf_is_queue_empty(&mon_pdev->msdu_queue))) {
+		head = qdf_nbuf_queue_remove(&mon_pdev->msdu_queue);
+		nbuf = head;
+		mpdu_buf = qdf_nbuf_copy(head);
+		if (qdf_unlikely(!mpdu_buf))
+			goto fail;
+
+		is_first_frag = 1;
+
+		while (nbuf) {
+			/* Find the 1st msdu to append in mpdu_buf->frag_list */
+			if (nbuf != head && is_first_frag) {
+				is_first_frag = 0;
+				head_frag_list  = nbuf;
+			}
+
+			/* calculate frag_list length */
+			if (!is_first_frag)
+				frag_list_sum_len += qdf_nbuf_len(nbuf);
+
+			if (qdf_nbuf_queue_first(&mon_pdev->msdu_queue) == tail)
+				break;
+
+			next = qdf_nbuf_queue_remove(&mon_pdev->msdu_queue);
+			qdf_nbuf_set_next(nbuf, next);
+			nbuf = next;
+		}
+
+		qdf_nbuf_append_ext_list(mpdu_buf, head_frag_list,
+					 frag_list_sum_len);
+		qdf_nbuf_free(head);
+	}
+
+	return mpdu_buf;
+
+fail:
+	dp_err_rl("nbuf copy failed len: %d Q1: %d Q2: %d", qdf_nbuf_len(nbuf),
+		  qdf_nbuf_queue_len(&mon_pdev->msdu_queue),
+		  qdf_nbuf_queue_len(&mon_pdev->mpdu_queue));
+
+	/* Drop all MSDU of MPDU */
+	while (nbuf) {
+		qdf_nbuf_free(nbuf);
+		if (qdf_nbuf_queue_first(&mon_pdev->msdu_queue) == tail)
+			break;
+		nbuf = qdf_nbuf_queue_remove(&mon_pdev->msdu_queue);
+	}
+
+	return NULL;
+}
+
+/**
+ * dp_rx_mon_send_mpdu() - Send MPDU to stack
+ * @pdev: DP pdev handle
+ * @mon_pdev: mon_pdev handle
+ * @mpdu_buf: buffer to submit
+ *
+ * Return: None
+ */
+static inline void
+dp_rx_mon_send_mpdu(struct dp_pdev *pdev, struct dp_mon_pdev *mon_pdev,
+		    qdf_nbuf_t mpdu_buf)
 {
-	uint16_t size;
 	struct dp_mon_vdev *mon_vdev;
-	struct dp_mon_pdev *mon_pdev = pdev->monitor_pdev;
 
-	if (!mon_pdev->mvdev) {
+	if (qdf_unlikely(!mon_pdev->mvdev)) {
 		dp_info_rl("Monitor vdev is NULL !!");
-		return 1;
+		qdf_nbuf_free(mpdu_buf);
+		return;
+	}
+
+	mon_pdev->ppdu_info.rx_status.ppdu_id =
+			mon_pdev->ppdu_info.com_info.ppdu_id;
+	mon_pdev->ppdu_info.rx_status.device_id = pdev->soc->device_id;
+	mon_pdev->ppdu_info.rx_status.chan_noise_floor =
+			pdev->chan_noise_floor;
+
+	if (!qdf_nbuf_update_radiotap(&mon_pdev->ppdu_info.rx_status, mpdu_buf,
+				      qdf_nbuf_headroom(mpdu_buf))) {
+		DP_STATS_INC(pdev, dropped.mon_radiotap_update_err, 1);
+		qdf_nbuf_free(mpdu_buf);
+		dp_err("radiotap_update_err");
+		return;
 	}
 
 	mon_vdev = mon_pdev->mvdev->monitor_vdev;
+	if (qdf_likely(mon_vdev && mon_vdev->osif_rx_mon))
+		mon_vdev->osif_rx_mon(mon_pdev->mvdev->osif_vdev,
+				      mpdu_buf, NULL);
+	else
+		qdf_nbuf_free(mpdu_buf);
+}
 
-	if (!ppdu_info->msdu_info.first_msdu_payload) {
-		dp_info_rl("First msdu payload not present");
-		return 1;
+int dp_rx_handle_local_pkt_capture(struct dp_pdev *pdev,
+				   struct hal_rx_ppdu_info *ppdu_info,
+				   qdf_nbuf_t nbuf, uint32_t tlv_status)
+{
+	struct dp_mon_pdev *mon_pdev = pdev->monitor_pdev;
+	qdf_nbuf_t buf, last;
+	uint16_t size;
+
+	qdf_spin_lock_bh(&mon_pdev->lpc_lock);
+	switch (tlv_status) {
+	case HAL_TLV_STATUS_MPDU_START:
+	{
+		/* Only Add MPDU to queue if multiple MPDUs present in PPDU */
+		if (qdf_unlikely(mon_pdev->first_mpdu)) {
+			mon_pdev->first_mpdu = false;
+			break;
+		}
+
+		/* last nbuf of queue points to 1st MSDU of next MPDU */
+		last = qdf_nbuf_queue_last(&mon_pdev->msdu_queue);
+		buf = dp_rx_mon_stitch_mpdu(mon_pdev, last);
+		/* Add MPDU to queue */
+		if (qdf_likely(buf))
+			qdf_nbuf_queue_add(&mon_pdev->mpdu_queue, buf);
+		break;
 	}
 
-	/* Adding 8 bytes to get to start of 802.11 frame after phy_ppdu_id */
-	size = (ppdu_info->msdu_info.first_msdu_payload -
-		qdf_nbuf_data(nbuf)) + mon_pdev->phy_ppdu_id_size;
-	ppdu_info->msdu_info.first_msdu_payload = NULL;
+	case HAL_TLV_STATUS_HEADER:
+	{
+		buf = qdf_nbuf_clone(nbuf);
+		if (qdf_unlikely(!buf))
+			break;
 
-	if (!qdf_nbuf_pull_head(nbuf, size)) {
-		dp_info_rl("No header present");
-		return 1;
+		/* Adding 8 bytes to get to start of 802.11 frame
+		 * after phy_ppdu_id
+		 */
+		size = (ppdu_info->msdu_info.first_msdu_payload -
+			qdf_nbuf_data(buf)) + mon_pdev->phy_ppdu_id_size;
+
+		if (qdf_unlikely(!qdf_nbuf_pull_head(buf, size))) {
+			qdf_nbuf_free(buf);
+			dp_info("No header present");
+			break;
+		}
+
+		/* Only retain RX MSDU payload in the skb */
+		qdf_nbuf_trim_tail(buf, qdf_nbuf_len(buf) -
+				ppdu_info->msdu_info.payload_len +
+				mon_pdev->phy_ppdu_id_size);
+
+		/* Add MSDU to Queue */
+		qdf_nbuf_queue_add(&mon_pdev->msdu_queue, buf);
+		break;
 	}
 
-	/* Only retain RX MSDU payload in the skb */
-	qdf_nbuf_trim_tail(nbuf, qdf_nbuf_len(nbuf) -
-			   ppdu_info->msdu_info.payload_len +
-			   mon_pdev->phy_ppdu_id_size);
-	if (!qdf_nbuf_update_radiotap(&mon_pdev->ppdu_info.rx_status, nbuf,
-				      qdf_nbuf_headroom(nbuf))) {
-		DP_STATS_INC(pdev, dropped.mon_radiotap_update_err, 1);
-		return 1;
+	case HAL_TLV_STATUS_PPDU_DONE:
+	{
+		while ((buf = qdf_nbuf_queue_remove(&mon_pdev->mpdu_queue)))
+			dp_rx_mon_send_mpdu(pdev, mon_pdev, buf);
+
+		/* Stich and send Last MPDU of PPDU */
+		buf = dp_rx_mon_stitch_mpdu(mon_pdev, NULL);
+		if (buf)
+			dp_rx_mon_send_mpdu(pdev, mon_pdev, buf);
+
+		mon_pdev->first_mpdu = true;
+		break;
+	}
+
+	default:
+		break;
 	}
 
-	if (mon_vdev && mon_vdev->osif_rx_mon)
-		mon_vdev->osif_rx_mon(mon_pdev->mvdev->osif_vdev, nbuf, NULL);
+	qdf_spin_unlock_bh(&mon_pdev->lpc_lock);
 
 	return 0;
 }

dp/wifi3.0/monitor/dp_rx_mon.h

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2016-2021 The Linux Foundation. All rights reserved.
- * Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved.
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. 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
@@ -824,18 +824,19 @@ dp_mon_rx_stats_update_rssi_dbm_params(struct dp_mon_pdev *mon_pdev,
  * @pdev: Datapath PDEV handle
  * @ppdu_info: Structure for rx ppdu info
  * @nbuf: Qdf nbuf abstraction for linux skb
+ * @tlv_status: TLV status
  *
  * Return: 0 on success, 1 on failure
  */
 int
 dp_rx_handle_local_pkt_capture(struct dp_pdev *pdev,
 			      struct hal_rx_ppdu_info *ppdu_info,
-			      qdf_nbuf_t nbuf);
+			      qdf_nbuf_t nbuf, uint32_t tlv_status);
 #else
 static inline int
 dp_rx_handle_local_pkt_capture(struct dp_pdev *pdev,
 			      struct hal_rx_ppdu_info *ppdu_info,
-			      qdf_nbuf_t nbuf)
+			      qdf_nbuf_t nbuf, uint32_t tlv_status)
 {
 	return 0;
 }

hal/wifi3.0/be/hal_be_api_mon.h

@@ -3635,8 +3635,7 @@ hal_rx_status_get_tlv_info_generic_be(void *rx_tlv_hdr, void *ppduinfo,
 		ppdu_info->ppdu_msdu_info[ppdu_info->fcs_ok_cnt].first_msdu_payload =
 			rx_tlv;
 		ppdu_info->ppdu_msdu_info[ppdu_info->fcs_ok_cnt].payload_len = tlv_len;
-		if (!ppdu_info->msdu_info.first_msdu_payload)
-			ppdu_info->msdu_info.first_msdu_payload = rx_tlv;
+		ppdu_info->msdu_info.first_msdu_payload = rx_tlv;
 		ppdu_info->msdu_info.payload_len = tlv_len;
 		ppdu_info->user_id = user_id;
 		ppdu_info->hdr_len = tlv_len;