llc_queue: Refactor to handle codel_state per prio queue internally

A CoDel state per prio queue is needed, otherwise the sojourn time state
is not properly reset when a high prio packet is dequeued.

If we have a global codel state shared for all prio queues of an MS, then
basically high prio (GMM) packets will "never" be dropped because they are
handled/dequeued way quicker, so it's sojourn time will be below the
threshold most probably, stopping the "dropping" state for the rest of
lower prio packets.

The handling of different codel states is moved from MS object to the
llc_queue, also offloading already loaded dl_tbf.cpp in the process.
This will also allow in the future setting different CoDel parameters
for different priority queues if needed.

Tests need to be adapted since now the CoDel and PDU lifetime are
incorporated into the llc_queue_dequeue(). Also because dequeue now also
accesses related MS fields.

Related: OS#5508
Change-Id: I2bce2e82ab6389d8a70130a5c26a966a316b0fa4

6 files changed, 130 insertions, 116 deletions

diff --git a/src/gprs_ms.c b/src/gprs_ms.c
index 63f454f9..6dc11a6a 100644
--- a/src/gprs_ms.c
+++ b/src/gprs_ms.c
@@ -113,19 +113,15 @@ struct GprsMs *ms_alloc(struct gprs_rlcmac_bts *bts, uint32_t tlli)
 	ms->imsi[0] = '\0';
 	memset(&ms->timer, 0, sizeof(ms->timer));
 	ms->timer.cb = ms_release_timer_cb;
-	llc_queue_init(&ms->llc_queue);
+	llc_queue_init(&ms->llc_queue, ms);
 
 	ms_set_mode(ms, GPRS);
 
 	codel_interval = the_pcu->vty.llc_codel_interval_msec;
+	if (codel_interval == LLC_CODEL_USE_DEFAULT)
+		codel_interval = GPRS_CODEL_SLOW_INTERVAL_MS;
+	llc_queue_set_codel_interval(&ms->llc_queue, codel_interval);
 
-	if (codel_interval != LLC_CODEL_DISABLE) {
-		if (codel_interval == LLC_CODEL_USE_DEFAULT)
-			codel_interval = GPRS_CODEL_SLOW_INTERVAL_MS;
-		ms->codel_state = talloc(ms, struct gprs_codel);
-		gprs_codel_init(ms->codel_state);
-		gprs_codel_set_interval(ms->codel_state, codel_interval);
-	}
 	ms->last_cs_not_low = now_msec();
 	ms->app_info_pending = false;
 
diff --git a/src/gprs_ms.h b/src/gprs_ms.h
index 3ebf7a5b..c5ee01c9 100644
--- a/src/gprs_ms.h
+++ b/src/gprs_ms.h
@@ -92,8 +92,6 @@ struct GprsMs {
 	uint8_t reserved_dl_slots;
 	uint8_t reserved_ul_slots;
 	struct gprs_rlcmac_trx *current_trx;
-
-	struct gprs_codel *codel_state;
 	enum mcs_kind mode;
 
 	struct rate_ctr_group *ctrs;
@@ -218,11 +216,6 @@ static inline void ms_set_timeout(struct GprsMs *ms, unsigned secs)
 	ms->delay = secs;
 }
 
-static inline struct gprs_codel *ms_codel_state(const struct GprsMs *ms)
-{
-	return ms->codel_state;
-}
-
 static inline unsigned ms_nack_rate_dl(const struct GprsMs *ms)
 {
 	return ms->nack_rate_dl;
diff --git a/src/llc.c b/src/llc.c
index c8d74d89..ddc1e038 100644
--- a/src/llc.c
+++ b/src/llc.c
@@ -21,6 +21,7 @@
 #include <osmocom/core/msgb.h>
 
 #include "bts.h"
+#include "gprs_ms.h"
 #include "pcu_utils.h"
 #include "llc.h"
 
@@ -94,17 +95,32 @@ bool llc_is_user_data_frame(const uint8_t *data, size_t len)
 	return true;
 }
 
-void llc_queue_init(struct gprs_llc_queue *q)
+void llc_queue_init(struct gprs_llc_queue *q, struct GprsMs *ms)
 {
 	unsigned int i;
 
+	q->ms = ms;
 	q->queue_size = 0;
 	q->queue_octets = 0;
 	q->avg_queue_delay = 0;
-	for (i = 0; i < ARRAY_SIZE(q->queue); i++)
-		INIT_LLIST_HEAD(&q->queue[i]);
+	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
+		INIT_LLIST_HEAD(&q->pq[i].queue);
+		gprs_codel_init(&q->pq[i].codel_state);
+	}
 }
 
+/* interval=0 -> don't use codel in the LLC queue */
+void llc_queue_set_codel_interval(struct gprs_llc_queue *q, unsigned int interval)
+{
+	unsigned int i;
+	if (interval == LLC_CODEL_DISABLE) {
+		q->use_codel = false;
+		return;
+	}
+	q->use_codel = true;
+	for (i = 0; i < ARRAY_SIZE(q->pq); i++)
+		gprs_codel_set_interval(&q->pq[i].codel_state, interval);
+}
 
 static enum gprs_llc_queue_prio llc_sapi2prio(uint8_t sapi)
 {
@@ -137,7 +153,7 @@ void llc_queue_enqueue(struct gprs_llc_queue *q, struct msgb *llc_msg, const str
 	osmo_clock_gettime(CLOCK_MONOTONIC, &meta_storage->recv_time);
 	meta_storage->expire_time = *expire_time;
 
-	msgb_enqueue(&q->queue[prio], llc_msg);
+	msgb_enqueue(&q->pq[prio].queue, llc_msg);
 }
 
 void llc_queue_clear(struct gprs_llc_queue *q, struct gprs_rlcmac_bts *bts)
@@ -145,8 +161,8 @@ void llc_queue_clear(struct gprs_llc_queue *q, struct gprs_rlcmac_bts *bts)
 	struct msgb *msg;
 	unsigned int i;
 
-	for (i = 0; i < ARRAY_SIZE(q->queue); i++) {
-		while ((msg = msgb_dequeue(&q->queue[i]))) {
+	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
+		while ((msg = msgb_dequeue(&q->pq[i].queue))) {
 			if (bts)
 				bts_do_rate_ctr_inc(bts, CTR_LLC_FRAME_DROPPED);
 			msgb_free(msg);
@@ -166,13 +182,13 @@ void llc_queue_move_and_merge(struct gprs_llc_queue *q, struct gprs_llc_queue *o
 	size_t queue_octets = 0;
 	INIT_LLIST_HEAD(&new_queue);
 
-	for (i = 0; i < ARRAY_SIZE(q->queue); i++) {
+	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
 		while (1) {
 			if (msg1 == NULL)
-				msg1 = msgb_dequeue(&q->queue[i]);
+				msg1 = msgb_dequeue(&q->pq[i].queue);
 
 			if (msg2 == NULL)
-				msg2 = msgb_dequeue(&o->queue[i]);
+				msg2 = msgb_dequeue(&o->pq[i].queue);
 
 			if (msg1 == NULL && msg2 == NULL)
 				break;
@@ -201,9 +217,9 @@ void llc_queue_move_and_merge(struct gprs_llc_queue *q, struct gprs_llc_queue *o
 			queue_octets += msgb_length(msg);
 		}
 
-		OSMO_ASSERT(llist_empty(&q->queue[i]));
-		OSMO_ASSERT(llist_empty(&o->queue[i]));
-		llist_splice_init(&new_queue, &q->queue[i]);
+		OSMO_ASSERT(llist_empty(&q->pq[i].queue));
+		OSMO_ASSERT(llist_empty(&o->pq[i].queue));
+		llist_splice_init(&new_queue, &q->pq[i].queue);
 	}
 
 	o->queue_size = 0;
@@ -214,7 +230,7 @@ void llc_queue_move_and_merge(struct gprs_llc_queue *q, struct gprs_llc_queue *o
 
 #define ALPHA 0.5f
 
-struct msgb *llc_queue_dequeue(struct gprs_llc_queue *q, const struct MetaInfo **info)
+static struct msgb *llc_queue_pick_msg(struct gprs_llc_queue *q, enum gprs_llc_queue_prio *prio)
 {
 	struct msgb *msg;
 	struct timespec *tv, tv_now, tv_result;
@@ -222,18 +238,17 @@ struct msgb *llc_queue_dequeue(struct gprs_llc_queue *q, const struct MetaInfo *
 	unsigned int i;
 	const struct MetaInfo *meta_storage;
 
-	for (i = 0; i < ARRAY_SIZE(q->queue); i++) {
-		if ((msg = msgb_dequeue(&q->queue[i])))
+	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
+		if ((msg = msgb_dequeue(&q->pq[i].queue))) {
+			*prio = (enum gprs_llc_queue_prio)i;
 			break;
+		}
 	}
 	if (!msg)
 		return NULL;
 
 	meta_storage = (struct MetaInfo *)&msg->cb[0];
 
-	if (info)
-		*info = meta_storage;
-
 	q->queue_size -= 1;
 	q->queue_octets -= msgb_length(msg);
 
@@ -248,6 +263,85 @@ struct msgb *llc_queue_dequeue(struct gprs_llc_queue *q, const struct MetaInfo *
 	return msg;
 }
 
+struct msgb *llc_queue_dequeue(struct gprs_llc_queue *q)
+{
+	struct msgb *msg;
+	struct timespec tv_now, tv_now2;
+	uint32_t octets = 0, frames = 0;
+	struct gprs_rlcmac_bts *bts = q->ms->bts;
+	struct gprs_pcu *pcu = bts->pcu;
+	struct timespec hyst_delta = {0, 0};
+	const unsigned keep_small_thresh = 60;
+	enum gprs_llc_queue_prio prio;
+
+	if (pcu->vty.llc_discard_csec)
+		csecs_to_timespec(pcu->vty.llc_discard_csec, &hyst_delta);
+
+	osmo_clock_gettime(CLOCK_MONOTONIC, &tv_now);
+	timespecadd(&tv_now, &hyst_delta, &tv_now2);
+
+	while ((msg = llc_queue_pick_msg(q, &prio))) {
+		const struct MetaInfo *info = (const struct MetaInfo *)&msg->cb[0];
+		const struct timespec *tv_disc = &info->expire_time;
+		const struct timespec *tv_recv = &info->recv_time;
+
+		gprs_bssgp_update_queue_delay(tv_recv, &tv_now);
+
+		if (q->use_codel) {
+			int bytes = llc_queue_octets(q);
+			if (gprs_codel_control(&q->pq[prio].codel_state, tv_recv, &tv_now, bytes))
+				goto drop_frame;
+		}
+
+		/* Is the age below the low water mark? */
+		if (!llc_queue_is_frame_expired(&tv_now2, tv_disc))
+			break;
+
+		/* Is the age below the high water mark */
+		if (!llc_queue_is_frame_expired(&tv_now, tv_disc)) {
+			/* Has the previous message not been dropped? */
+			if (frames == 0)
+				break;
+
+			/* Hysteresis mode, try to discard LLC messages until
+			 * the low water mark has been reached */
+
+			/* Check whether to abort the hysteresis mode */
+
+			/* Is the frame small, perhaps only a TCP ACK? */
+			if (msg->len <= keep_small_thresh)
+				break;
+
+			/* Is it a GMM message? */
+			if (!llc_is_user_data_frame(msg->data, msg->len))
+				break;
+		}
+
+		bts_do_rate_ctr_inc(bts, CTR_LLC_FRAME_TIMEDOUT);
+drop_frame:
+		frames++;
+		octets += msg->len;
+		msgb_free(msg);
+		bts_do_rate_ctr_inc(bts, CTR_LLC_FRAME_DROPPED);
+		continue;
+	}
+
+	if (frames) {
+		LOGPMS(q->ms, DTBFDL, LOGL_NOTICE, "Discarding LLC PDU "
+			"because lifetime limit reached, "
+			"count=%u new_queue_size=%zu\n",
+			  frames, llc_queue_size(q));
+		if (frames > 0xff)
+			frames = 0xff;
+		if (octets > 0xffffff)
+			octets = 0xffffff;
+		if (pcu->bssgp.bctx)
+			bssgp_tx_llc_discarded(pcu->bssgp.bctx, ms_tlli(q->ms), frames, octets);
+	}
+
+	return msg;
+}
+
 void llc_queue_calc_pdu_lifetime(struct gprs_rlcmac_bts *bts, const uint16_t pdu_delay_csec, struct timespec *tv)
 {
 	uint16_t delay_csec;
diff --git a/src/llc.h b/src/llc.h
index adfab657..fe14c6aa 100644
--- a/src/llc.h
+++ b/src/llc.h
@@ -28,9 +28,12 @@ extern "C" {
 #include <osmocom/core/msgb.h>
 #include <osmocom/core/endian.h>
 
+#include "gprs_codel.h"
+
 #define LLC_MAX_LEN 1543
 
 struct gprs_rlcmac_bts;
+struct GprsMs;
 
 struct gprs_llc_hdr {
 #if OSMO_IS_LITTLE_ENDIAN
@@ -110,22 +113,29 @@ enum gprs_llc_queue_prio { /* lowest value has highest prio */
 	LLC_QUEUE_PRIO_OTHER, /* Other SAPIs */
 	_LLC_QUEUE_PRIO_SIZE /* used to calculate size of enum */
 };
+struct gprs_llc_prio_queue {
+	struct gprs_codel codel_state;
+	struct llist_head queue; /* queued LLC DL data. See enum gprs_llc_queue_prio. */
+};
 struct gprs_llc_queue {
+	struct GprsMs *ms; /* backpointer */
 	uint32_t avg_queue_delay; /* Average delay of data going through the queue */
 	size_t queue_size;
 	size_t queue_octets;
-	struct llist_head queue[_LLC_QUEUE_PRIO_SIZE]; /* queued LLC DL data. See enum gprs_llc_queue_prio. */
+	bool use_codel;
+	struct gprs_llc_prio_queue pq[_LLC_QUEUE_PRIO_SIZE]; /* queued LLC DL data. See enum gprs_llc_queue_prio. */
 };
 
 void llc_queue_calc_pdu_lifetime(struct gprs_rlcmac_bts *bts, const uint16_t pdu_delay_csec,
 		struct timespec *tv);
 bool llc_queue_is_frame_expired(const struct timespec *tv_now, const struct timespec *tv);
 
-void llc_queue_init(struct gprs_llc_queue *q);
+void llc_queue_init(struct gprs_llc_queue *q, struct GprsMs *ms);
 void llc_queue_clear(struct gprs_llc_queue *q, struct gprs_rlcmac_bts *bts);
+void llc_queue_set_codel_interval(struct gprs_llc_queue *q, unsigned int interval);
 void llc_queue_move_and_merge(struct gprs_llc_queue *q, struct gprs_llc_queue *o);
 void llc_queue_enqueue(struct gprs_llc_queue *q, struct msgb *llc_msg, const struct timespec *expire_time);
-struct msgb *llc_queue_dequeue(struct gprs_llc_queue *q, const struct MetaInfo **info);
+struct msgb *llc_queue_dequeue(struct gprs_llc_queue *q);
 
 static inline size_t llc_queue_size(const struct gprs_llc_queue *q)
 {
diff --git a/src/tbf_dl.cpp b/src/tbf_dl.cpp
index 01b8f684..9c99cf6d 100644
--- a/src/tbf_dl.cpp
+++ b/src/tbf_dl.cpp
@@ -343,83 +343,6 @@ int dl_tbf_handle(struct gprs_rlcmac_bts *bts,
 	return rc;
 }
 
-struct msgb *gprs_rlcmac_dl_tbf::llc_dequeue(bssgp_bvc_ctx *bctx)
-{
-	struct msgb *msg;
-	struct timespec tv_now, tv_now2;
-	uint32_t octets = 0, frames = 0;
-	struct timespec hyst_delta = {0, 0};
-	const unsigned keep_small_thresh = 60;
-	const MetaInfo *info;
-
-	if (the_pcu->vty.llc_discard_csec)
-		csecs_to_timespec(the_pcu->vty.llc_discard_csec, &hyst_delta);
-
-	osmo_clock_gettime(CLOCK_MONOTONIC, &tv_now);
-	timespecadd(&tv_now, &hyst_delta, &tv_now2);
-
-	while ((msg = llc_queue_dequeue(llc_queue(), &info))) {
-		const struct timespec *tv_disc = &info->expire_time;
-		const struct timespec *tv_recv = &info->recv_time;
-
-		gprs_bssgp_update_queue_delay(tv_recv, &tv_now);
-
-		if (ms() && ms_codel_state(ms())) {
-			int bytes = llc_queue_octets(llc_queue());
-			if (gprs_codel_control(ms_codel_state(ms()),
-					tv_recv, &tv_now, bytes))
-				goto drop_frame;
-		}
-
-		/* Is the age below the low water mark? */
-		if (!llc_queue_is_frame_expired(&tv_now2, tv_disc))
-			break;
-
-		/* Is the age below the high water mark */
-		if (!llc_queue_is_frame_expired(&tv_now, tv_disc)) {
-			/* Has the previous message not been dropped? */
-			if (frames == 0)
-				break;
-
-			/* Hysteresis mode, try to discard LLC messages until
-			 * the low water mark has been reached */
-
-			/* Check whether to abort the hysteresis mode */
-
-			/* Is the frame small, perhaps only a TCP ACK? */
-			if (msg->len <= keep_small_thresh)
-				break;
-
-			/* Is it a GMM message? */
-			if (!llc_is_user_data_frame(msg->data, msg->len))
-				break;
-		}
-
-		bts_do_rate_ctr_inc(bts, CTR_LLC_FRAME_TIMEDOUT);
-drop_frame:
-		frames++;
-		octets += msg->len;
-		msgb_free(msg);
-		bts_do_rate_ctr_inc(bts, CTR_LLC_FRAME_DROPPED);
-		continue;
-	}
-
-	if (frames) {
-		LOGPTBFDL(this, LOGL_NOTICE, "Discarding LLC PDU "
-			"because lifetime limit reached, "
-			"count=%u new_queue_size=%zu\n",
-			  frames, llc_queue_size(llc_queue()));
-		if (frames > 0xff)
-			frames = 0xff;
-		if (octets > 0xffffff)
-			octets = 0xffffff;
-		if (bctx)
-			bssgp_tx_llc_discarded(bctx, tlli(), frames, octets);
-	}
-
-	return msg;
-}
-
 bool gprs_rlcmac_dl_tbf::restart_bsn_cycle()
 {
 	/* If V(S) == V(A) and finished state, we would have received
@@ -628,7 +551,7 @@ void gprs_rlcmac_dl_tbf::schedule_next_frame()
 		return;
 
 	/* dequeue next LLC frame, if any */
-	msg = llc_dequeue(bts->pcu->bssgp.bctx);
+	msg = llc_queue_dequeue(llc_queue());
 	if (!msg)
 		return;
 
diff --git a/src/tbf_dl.h b/src/tbf_dl.h
index c0c0284f..0f531eca 100644
--- a/src/tbf_dl.h
+++ b/src/tbf_dl.h
@@ -57,8 +57,6 @@ struct gprs_rlcmac_dl_tbf : public gprs_rlcmac_tbf {
 	void set_window_size();
 	void update_coding_scheme_counter_dl(enum CodingScheme cs);
 
-	struct msgb *llc_dequeue(bssgp_bvc_ctx *bctx);
-
 	/* Please note that all variables here will be reset when changing
 	 * from WAIT RELEASE back to FLOW state (re-use of TBF).
 	 * All states that need reset must be in this struct, so this is why