From 30f857cb67e89ce0cab07ef85b4efd83b3b1ea4f Mon Sep 17 00:00:00 2001
From: Ankit Kumar <ankkuma@codeaurora.org>
Date: Fri, 4 Sep 2020 17:30:22 +0530
Subject: [PATCH] qcacmn: MCopy and lite monitor feature handling in case of
 frag

Add logic to handle mcopy and lite monitor feature
in case of frag.

Change-Id: I45fcbd22b1dcb1f68ee93a69f29fbac85cb0aafa
---
 dp/wifi3.0/dp_rx_mon.h      | 24 ++++++++++++++++++++++++
 dp/wifi3.0/dp_rx_mon_dest.c | 10 +++++++++-
 2 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/dp/wifi3.0/dp_rx_mon.h b/dp/wifi3.0/dp_rx_mon.h
index b7347a3b2966..85492d07d45e 100644
--- a/dp/wifi3.0/dp_rx_mon.h
+++ b/dp/wifi3.0/dp_rx_mon.h
@@ -632,6 +632,24 @@ uint8_t *dp_rx_mon_get_buffer_data(struct dp_rx_desc *rx_desc)
 {
 	return rx_desc->rx_buf_start;
 }
+
+/**
+ * dp_rx_mon_get_nbuf_80211_hdr() - Get 80211 hdr from nbuf
+ * @nbuf: qdf_nbuf_t
+ *
+ * This function must be called after moving radiotap header.
+ *
+ * Return: Ptr pointing to 80211 header or NULL.
+ */
+static inline
+qdf_frag_t dp_rx_mon_get_nbuf_80211_hdr(qdf_nbuf_t nbuf)
+{
+	/* Return NULL if nr_frag is Zero */
+	if (!qdf_nbuf_get_nr_frags(nbuf))
+		return NULL;
+
+	return qdf_nbuf_get_frag_addr(nbuf, 0);
+}
 #else
 
 #define DP_RX_MON_GET_NBUF_FROM_DESC(rx_desc) \
@@ -755,6 +773,12 @@ uint8_t *dp_rx_mon_get_buffer_data(struct dp_rx_desc *rx_desc)
 		data = qdf_nbuf_data(msdu);
 	return data;
 }
+
+static inline
+qdf_frag_t dp_rx_mon_get_nbuf_80211_hdr(qdf_nbuf_t nbuf)
+{
+	return qdf_nbuf_data(nbuf);
+}
 #endif
 
 /**
diff --git a/dp/wifi3.0/dp_rx_mon_dest.c b/dp/wifi3.0/dp_rx_mon_dest.c
index 568d9289b90a..e941de789a2d 100644
--- a/dp/wifi3.0/dp_rx_mon_dest.c
+++ b/dp/wifi3.0/dp_rx_mon_dest.c
@@ -1221,12 +1221,20 @@ static inline QDF_STATUS dp_send_mgmt_packet_to_stack(struct dp_soc *soc,
 {
 	uint32_t *nbuf_data;
 	struct ieee80211_frame *wh;
+	qdf_frag_t addr;
 
 	if (!nbuf)
 		return QDF_STATUS_E_INVAL;
 
+	/* Get addr pointing to80211 header */
+	addr = dp_rx_mon_get_nbuf_80211_hdr(nbuf);
+	if (qdf_unlikely(!addr)) {
+		qdf_nbuf_free(nbuf);
+		return QDF_STATUS_E_INVAL;
+	}
+
 	/*check if this is not a mgmt packet*/
-	wh = (struct ieee80211_frame *)qdf_nbuf_data(nbuf);
+	wh = (struct ieee80211_frame *)addr;
 	if (((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) !=
 	     IEEE80211_FC0_TYPE_MGT) &&
 	     ((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) !=