[PATCH v6 19/21] pdcp: add support for status report

[Anoob Joseph <anoobj@marvell.com>]

From: Volodymyr Fialko <vfialko@marvell.com>

Implement status report generation for PDCP entity.

Signed-off-by: Anoob Joseph <anoobj@marvell.com>
Signed-off-by: Volodymyr Fialko <vfialko@marvell.com>
---
 lib/pdcp/pdcp_cnt.c      | 158 ++++++++++++++++++++++++++++++++++++---
 lib/pdcp/pdcp_cnt.h      |  11 ++-
 lib/pdcp/pdcp_ctrl_pdu.c |  34 ++++++++-
 lib/pdcp/pdcp_ctrl_pdu.h |   3 +-
 lib/pdcp/pdcp_entity.h   |   2 +
 lib/pdcp/pdcp_process.c  |   9 ++-
 lib/pdcp/pdcp_process.h  |  13 ++++
 lib/pdcp/rte_pdcp.c      |  34 ++++++---
 8 files changed, 236 insertions(+), 28 deletions(-)

diff --git a/lib/pdcp/pdcp_cnt.c b/lib/pdcp/pdcp_cnt.c
index c9b952184b..af027b00d3 100644
--- a/lib/pdcp/pdcp_cnt.c
+++ b/lib/pdcp/pdcp_cnt.c
@@ -2,28 +2,164 @@
  * Copyright(C) 2023 Marvell.
  */
 
+#include <rte_bitmap.h>
 #include <rte_pdcp.h>
 
 #include "pdcp_cnt.h"
+#include "pdcp_ctrl_pdu.h"
 #include "pdcp_entity.h"
 
+#define SLAB_BYTE_SIZE (RTE_BITMAP_SLAB_BIT_SIZE / 8)
+
+uint32_t
+pdcp_cnt_bitmap_get_memory_footprint(const struct rte_pdcp_entity_conf *conf)
+{
+	uint32_t n_bits = pdcp_window_size_get(conf->pdcp_xfrm.sn_size);
+
+	return rte_bitmap_get_memory_footprint(n_bits);
+}
+
 int
-pdcp_cnt_ring_create(struct rte_pdcp_entity *en, const struct rte_pdcp_entity_conf *conf)
+pdcp_cnt_bitmap_create(struct entity_priv_dl_part *dl, void *bitmap_mem, uint32_t window_size)
 {
-	struct entity_priv_dl_part *en_priv_dl;
-	uint32_t window_sz;
+	uint32_t mem_size = rte_bitmap_get_memory_footprint(window_size);
 
-	if (en == NULL || conf == NULL)
+	dl->bitmap.bmp = rte_bitmap_init(window_size, bitmap_mem, mem_size);
+	if (dl->bitmap.bmp == NULL)
 		return -EINVAL;
 
-	if (conf->pdcp_xfrm.pkt_dir == RTE_SECURITY_PDCP_UPLINK)
-		return 0;
+	dl->bitmap.size = window_size;
 
-	en_priv_dl = entity_dl_part_get(en);
-	window_sz = pdcp_window_size_get(conf->pdcp_xfrm.sn_size);
+	return 0;
+}
 
-	RTE_SET_USED(window_sz);
-	RTE_SET_USED(en_priv_dl);
+void
+pdcp_cnt_bitmap_set(struct pdcp_cnt_bitmap bitmap, uint32_t count)
+{
+	rte_bitmap_set(bitmap.bmp, count % bitmap.size);
+}
 
-	return 0;
+bool
+pdcp_cnt_bitmap_is_set(struct pdcp_cnt_bitmap bitmap, uint32_t count)
+{
+	return rte_bitmap_get(bitmap.bmp, count % bitmap.size);
+}
+
+void
+pdcp_cnt_bitmap_range_clear(struct pdcp_cnt_bitmap bitmap, uint32_t start, uint32_t stop)
+{
+	uint32_t i;
+
+	for (i = start; i < stop; i++)
+		rte_bitmap_clear(bitmap.bmp, i % bitmap.size);
+}
+
+uint16_t
+pdcp_cnt_get_bitmap_size(uint32_t pending_bytes)
+{
+	/*
+	 * Round up bitmap size to slab size to operate only on slabs sizes, instead of individual
+	 * bytes
+	 */
+	return RTE_ALIGN_MUL_CEIL(pending_bytes, SLAB_BYTE_SIZE);
+}
+
+static __rte_always_inline uint64_t
+leftover_get(uint64_t slab, uint32_t shift, uint64_t mask)
+{
+	return (slab & mask) << shift;
+}
+
+void
+pdcp_cnt_report_fill(struct pdcp_cnt_bitmap bitmap, struct entity_state state,
+		     uint8_t *data, uint16_t data_len)
+{
+	uint64_t slab = 0, next_slab = 0, leftover;
+	uint32_t zeros, report_len, diff;
+	uint32_t slab_id, next_slab_id;
+	uint32_t pos = 0, next_pos = 0;
+
+	const uint32_t start_count = state.rx_deliv + 1;
+	const uint32_t nb_slabs = bitmap.size / RTE_BITMAP_SLAB_BIT_SIZE;
+	const uint32_t nb_data_slabs = data_len / SLAB_BYTE_SIZE;
+	const uint32_t start_slab_id = start_count / RTE_BITMAP_SLAB_BIT_SIZE;
+	const uint32_t stop_slab_id = (start_slab_id + nb_data_slabs) % nb_slabs;
+	const uint32_t shift = start_count % RTE_BITMAP_SLAB_BIT_SIZE;
+	const uint32_t leftover_shift = shift ? RTE_BITMAP_SLAB_BIT_SIZE - shift : 0;
+	const uint8_t *data_end = RTE_PTR_ADD(data, data_len + SLAB_BYTE_SIZE);
+
+	/* NOTE: Mask required to workaround case - when shift is not needed */
+	const uint64_t leftover_mask = shift ? ~0 : 0;
+
+	/* NOTE: implement scan init at to set custom position */
+	__rte_bitmap_scan_init(bitmap.bmp);
+	while (true) {
+		assert(rte_bitmap_scan(bitmap.bmp, &pos, &slab) == 1);
+		slab_id = pos / RTE_BITMAP_SLAB_BIT_SIZE;
+		if (slab_id >= start_slab_id)
+			break;
+	}
+
+	report_len = nb_data_slabs;
+
+	if (slab_id > start_slab_id) {
+		/* Zero slabs at beginning */
+		zeros = (slab_id - start_slab_id - 1) * SLAB_BYTE_SIZE;
+		memset(data, 0, zeros);
+		data = RTE_PTR_ADD(data, zeros);
+		leftover = leftover_get(slab, leftover_shift, leftover_mask);
+		memcpy(data, &leftover, SLAB_BYTE_SIZE);
+		data = RTE_PTR_ADD(data, SLAB_BYTE_SIZE);
+		report_len -= (slab_id - start_slab_id);
+	}
+
+	while (report_len) {
+		rte_bitmap_scan(bitmap.bmp, &next_pos, &next_slab);
+		next_slab_id = next_pos / RTE_BITMAP_SLAB_BIT_SIZE;
+		diff = (next_slab_id + nb_slabs - slab_id) % nb_slabs;
+
+		/* If next_slab_id == slab_id - overlap */
+		diff += !(next_slab_id ^ slab_id) * nb_slabs;
+
+		/* Size check - next slab is outsize of size range */
+		if (diff > report_len) {
+			next_slab = 0;
+			next_slab_id = stop_slab_id;
+			diff = report_len;
+		}
+
+		report_len -= diff;
+
+		/* Calculate gap between slabs, taking wrap around into account */
+		zeros = (next_slab_id + nb_slabs - slab_id - 1) % nb_slabs;
+		if (zeros) {
+			/* Non continues slabs, align them individually */
+			slab >>= shift;
+			memcpy(data, &slab, SLAB_BYTE_SIZE);
+			data = RTE_PTR_ADD(data, SLAB_BYTE_SIZE);
+
+			/* Fill zeros between slabs */
+			zeros = (zeros - 1) * SLAB_BYTE_SIZE;
+			memset(data, 0, zeros);
+			data = RTE_PTR_ADD(data, zeros);
+
+			/* Align beginning of next slab */
+			leftover = leftover_get(next_slab, leftover_shift, leftover_mask);
+			memcpy(data, &leftover, SLAB_BYTE_SIZE);
+			data = RTE_PTR_ADD(data, SLAB_BYTE_SIZE);
+		} else {
+			/* Continues slabs, combine them */
+			uint64_t new_slab = (slab >> shift) |
+					leftover_get(next_slab, leftover_shift, leftover_mask);
+			memcpy(data, &new_slab, SLAB_BYTE_SIZE);
+			data = RTE_PTR_ADD(data, SLAB_BYTE_SIZE);
+		}
+
+		slab = next_slab;
+		pos = next_pos;
+		slab_id = next_slab_id;
+
+	};
+
+	assert(data < data_end);
 }
diff --git a/lib/pdcp/pdcp_cnt.h b/lib/pdcp/pdcp_cnt.h
index bbda478b55..5941b7a406 100644
--- a/lib/pdcp/pdcp_cnt.h
+++ b/lib/pdcp/pdcp_cnt.h
@@ -9,6 +9,15 @@
 
 #include "pdcp_entity.h"
 
-int pdcp_cnt_ring_create(struct rte_pdcp_entity *en, const struct rte_pdcp_entity_conf *conf);
+uint32_t pdcp_cnt_bitmap_get_memory_footprint(const struct rte_pdcp_entity_conf *conf);
+int pdcp_cnt_bitmap_create(struct entity_priv_dl_part *dl, void *bitmap_mem, uint32_t window_size);
+
+void pdcp_cnt_bitmap_set(struct pdcp_cnt_bitmap bitmap, uint32_t count);
+bool pdcp_cnt_bitmap_is_set(struct pdcp_cnt_bitmap bitmap, uint32_t count);
+void pdcp_cnt_bitmap_range_clear(struct pdcp_cnt_bitmap bitmap, uint32_t start, uint32_t stop);
+
+uint16_t pdcp_cnt_get_bitmap_size(uint32_t pending_bytes);
+void pdcp_cnt_report_fill(struct pdcp_cnt_bitmap bitmap, struct entity_state state,
+			  uint8_t *data, uint16_t data_len);
 
 #endif /* PDCP_CNT_H */
diff --git a/lib/pdcp/pdcp_ctrl_pdu.c b/lib/pdcp/pdcp_ctrl_pdu.c
index feb05fd863..e0ac2d3720 100644
--- a/lib/pdcp/pdcp_ctrl_pdu.c
+++ b/lib/pdcp/pdcp_ctrl_pdu.c
@@ -8,6 +8,14 @@
 
 #include "pdcp_ctrl_pdu.h"
 #include "pdcp_entity.h"
+#include "pdcp_cnt.h"
+
+static inline uint16_t
+round_up_bits(uint32_t bits)
+{
+	/* round up to the next multiple of 8 */
+	return RTE_ALIGN_MUL_CEIL(bits, 8) / 8;
+}
 
 static __rte_always_inline void
 pdcp_hdr_fill(struct rte_pdcp_up_ctrl_pdu_hdr *pdu_hdr, uint32_t rx_deliv)
@@ -19,11 +27,13 @@ pdcp_hdr_fill(struct rte_pdcp_up_ctrl_pdu_hdr *pdu_hdr, uint32_t rx_deliv)
 }
 
 int
-pdcp_ctrl_pdu_status_gen(struct entity_priv *en_priv, struct rte_mbuf *m)
+pdcp_ctrl_pdu_status_gen(struct entity_priv *en_priv, struct entity_priv_dl_part *dl,
+			 struct rte_mbuf *m)
 {
 	struct rte_pdcp_up_ctrl_pdu_hdr *pdu_hdr;
-	uint32_t rx_deliv;
-	int pdu_sz;
+	uint32_t rx_deliv, actual_sz;
+	uint16_t pdu_sz, bitmap_sz;
+	uint8_t *data;
 
 	if (!en_priv->flags.is_status_report_required)
 		return -EINVAL;
@@ -42,5 +52,21 @@ pdcp_ctrl_pdu_status_gen(struct entity_priv *en_priv, struct rte_mbuf *m)
 		return 0;
 	}
 
-	return -ENOTSUP;
+	actual_sz = RTE_MIN(round_up_bits(en_priv->state.rx_next - rx_deliv - 1),
+			RTE_PDCP_CTRL_PDU_SIZE_MAX - pdu_sz);
+	bitmap_sz = pdcp_cnt_get_bitmap_size(actual_sz);
+
+	data = (uint8_t *)rte_pktmbuf_append(m, pdu_sz + bitmap_sz);
+	if (data == NULL)
+		return -ENOMEM;
+
+	m->pkt_len = pdu_sz + actual_sz;
+	m->data_len = pdu_sz + actual_sz;
+
+	pdcp_hdr_fill((struct rte_pdcp_up_ctrl_pdu_hdr *)data, rx_deliv);
+
+	data = RTE_PTR_ADD(data, pdu_sz);
+	pdcp_cnt_report_fill(dl->bitmap, en_priv->state, data, bitmap_sz);
+
+	return 0;
 }
diff --git a/lib/pdcp/pdcp_ctrl_pdu.h b/lib/pdcp/pdcp_ctrl_pdu.h
index a2424fbd10..2a87928b88 100644
--- a/lib/pdcp/pdcp_ctrl_pdu.h
+++ b/lib/pdcp/pdcp_ctrl_pdu.h
@@ -10,6 +10,7 @@
 #include "pdcp_entity.h"
 
 int
-pdcp_ctrl_pdu_status_gen(struct entity_priv *en_priv, struct rte_mbuf *m);
+pdcp_ctrl_pdu_status_gen(struct entity_priv *en_priv, struct entity_priv_dl_part *dl,
+			 struct rte_mbuf *m);
 
 #endif /* PDCP_CTRL_PDU_H */
diff --git a/lib/pdcp/pdcp_entity.h b/lib/pdcp/pdcp_entity.h
index efc74ba9b9..a9b1428c7a 100644
--- a/lib/pdcp/pdcp_entity.h
+++ b/lib/pdcp/pdcp_entity.h
@@ -182,6 +182,8 @@ struct entity_priv_dl_part {
 	struct pdcp_t_reordering t_reorder;
 	/** Reorder packet buffer */
 	struct pdcp_reorder reorder;
+	/** Bitmap memory region */
+	uint8_t bitmap_mem[0];
 };
 
 struct entity_priv_ul_part {
diff --git a/lib/pdcp/pdcp_process.c b/lib/pdcp/pdcp_process.c
index daf2c27363..774f5646d8 100644
--- a/lib/pdcp/pdcp_process.c
+++ b/lib/pdcp/pdcp_process.c
@@ -10,6 +10,7 @@
 #include <rte_pdcp.h>
 #include <rte_pdcp_hdr.h>
 
+#include "pdcp_cnt.h"
 #include "pdcp_crypto.h"
 #include "pdcp_entity.h"
 #include "pdcp_process.h"
@@ -857,11 +858,15 @@ pdcp_post_process_update_entity_state(const struct rte_pdcp_entity *entity,
 	if (count >= en_priv->state.rx_next)
 		en_priv->state.rx_next = count + 1;
 
+	if (unlikely(pdcp_cnt_bitmap_is_set(dl->bitmap, count)))
+		return -EEXIST;
+
+	pdcp_cnt_bitmap_set(dl->bitmap, count);
 	pdcp_packet_strip(mb, hdr_trim_sz, trim_mac);
 
 	if (en_priv->flags.is_out_of_order_delivery) {
 		out_mb[0] = mb;
-		en_priv->state.rx_deliv = count + 1;
+		pdcp_rx_deliv_set(entity, count + 1);
 
 		return 1;
 	}
@@ -888,7 +893,7 @@ pdcp_post_process_update_entity_state(const struct rte_pdcp_entity *entity,
 		}
 
 		/* Processed should never exceed the window size */
-		en_priv->state.rx_deliv = count + processed;
+		pdcp_rx_deliv_set(entity, count + processed);
 
 	} else {
 		if (!reorder->is_active)
diff --git a/lib/pdcp/pdcp_process.h b/lib/pdcp/pdcp_process.h
index a52f769b82..fa3d93b405 100644
--- a/lib/pdcp/pdcp_process.h
+++ b/lib/pdcp/pdcp_process.h
@@ -8,6 +8,9 @@
 #include <rte_mbuf_dyn.h>
 #include <rte_pdcp.h>
 
+#include <pdcp_entity.h>
+#include <pdcp_cnt.h>
+
 typedef uint32_t rte_pdcp_dynfield_t;
 
 extern int rte_pdcp_dynfield_offset;
@@ -21,4 +24,14 @@ pdcp_dynfield(struct rte_mbuf *mbuf)
 int
 pdcp_process_func_set(struct rte_pdcp_entity *entity, const struct rte_pdcp_entity_conf *conf);
 
+static inline void
+pdcp_rx_deliv_set(const struct rte_pdcp_entity *entity, uint32_t rx_deliv)
+{
+	struct entity_priv_dl_part *dl = entity_dl_part_get(entity);
+	struct entity_priv *en_priv = entity_priv_get(entity);
+
+	pdcp_cnt_bitmap_range_clear(dl->bitmap, en_priv->state.rx_deliv, rx_deliv);
+	en_priv->state.rx_deliv = rx_deliv;
+}
+
 #endif /* PDCP_PROCESS_H */
diff --git a/lib/pdcp/rte_pdcp.c b/lib/pdcp/rte_pdcp.c
index 819c66bd08..9865c620b7 100644
--- a/lib/pdcp/rte_pdcp.c
+++ b/lib/pdcp/rte_pdcp.c
@@ -14,6 +14,8 @@
 
 #define RTE_PDCP_DYNFIELD_NAME "rte_pdcp_dynfield"
 
+static int bitmap_mem_offset;
+
 int rte_pdcp_dynfield_offset = -1;
 
 static int
@@ -39,9 +41,12 @@ pdcp_entity_size_get(const struct rte_pdcp_entity_conf *conf)
 
 	size = sizeof(struct rte_pdcp_entity) + sizeof(struct entity_priv);
 
-	if (conf->pdcp_xfrm.pkt_dir == RTE_SECURITY_PDCP_DOWNLINK)
+	if (conf->pdcp_xfrm.pkt_dir == RTE_SECURITY_PDCP_DOWNLINK) {
 		size += sizeof(struct entity_priv_dl_part);
-	else if (conf->pdcp_xfrm.pkt_dir == RTE_SECURITY_PDCP_UPLINK)
+		size = RTE_CACHE_LINE_ROUNDUP(size);
+		bitmap_mem_offset = size;
+		size += pdcp_cnt_bitmap_get_memory_footprint(conf);
+	} else if (conf->pdcp_xfrm.pkt_dir == RTE_SECURITY_PDCP_UPLINK)
 		size += sizeof(struct entity_priv_ul_part);
 	else
 		return -EINVAL;
@@ -54,11 +59,24 @@ pdcp_dl_establish(struct rte_pdcp_entity *entity, const struct rte_pdcp_entity_c
 {
 	const uint32_t window_size = pdcp_window_size_get(conf->pdcp_xfrm.sn_size);
 	struct entity_priv_dl_part *dl = entity_dl_part_get(entity);
+	void *bitmap_mem;
+	int ret;
 
 	entity->max_pkt_cache = RTE_MAX(entity->max_pkt_cache, window_size);
 	dl->t_reorder.handle = conf->t_reordering;
 
-	return pdcp_reorder_create(&dl->reorder, window_size);
+	ret = pdcp_reorder_create(&dl->reorder, window_size);
+	if (ret)
+		return ret;
+
+	bitmap_mem = RTE_PTR_ADD(entity, bitmap_mem_offset);
+	ret = pdcp_cnt_bitmap_create(dl, bitmap_mem, window_size);
+	if (ret) {
+		pdcp_reorder_destroy(&dl->reorder);
+		return ret;
+	}
+
+	return 0;
 }
 
 struct rte_pdcp_entity *
@@ -136,10 +154,6 @@ rte_pdcp_entity_establish(const struct rte_pdcp_entity_conf *conf)
 			goto crypto_sess_destroy;
 	}
 
-	ret = pdcp_cnt_ring_create(entity, conf);
-	if (ret)
-		goto crypto_sess_destroy;
-
 	return entity;
 
 crypto_sess_destroy:
@@ -218,6 +232,7 @@ struct rte_mbuf *
 rte_pdcp_control_pdu_create(struct rte_pdcp_entity *pdcp_entity,
 			    enum rte_pdcp_ctrl_pdu_type type)
 {
+	struct entity_priv_dl_part *dl;
 	struct entity_priv *en_priv;
 	struct rte_mbuf *m;
 	int ret;
@@ -228,6 +243,7 @@ rte_pdcp_control_pdu_create(struct rte_pdcp_entity *pdcp_entity,
 	}
 
 	en_priv = entity_priv_get(pdcp_entity);
+	dl = entity_dl_part_get(pdcp_entity);
 
 	m = rte_pktmbuf_alloc(en_priv->ctrl_pdu_pool);
 	if (m == NULL) {
@@ -237,7 +253,7 @@ rte_pdcp_control_pdu_create(struct rte_pdcp_entity *pdcp_entity,
 
 	switch (type) {
 	case RTE_PDCP_CTRL_PDU_TYPE_STATUS_REPORT:
-		ret = pdcp_ctrl_pdu_status_gen(en_priv, m);
+		ret = pdcp_ctrl_pdu_status_gen(en_priv, dl, m);
 		break;
 	default:
 		ret = -ENOTSUP;
@@ -283,7 +299,7 @@ rte_pdcp_t_reordering_expiry_handle(const struct rte_pdcp_entity *entity, struct
 	 * - update RX_DELIV to the COUNT value of the first PDCP SDU which has not been delivered
 	 *   to upper layers, with COUNT value >= RX_REORD;
 	 */
-	en_priv->state.rx_deliv = en_priv->state.rx_reord + nb_seq;
+	pdcp_rx_deliv_set(entity, en_priv->state.rx_reord + nb_seq);
 
 	/*
 	 * - if RX_DELIV < RX_NEXT:
-- 
2.25.1