From aef4114abb524b9d2f09e007c86f8e08218ffa56 Mon Sep 17 00:00:00 2001
From: Pau Espin Pedrol <pespin@sysmocom.de>
Date: Mon, 27 Feb 2023 17:10:23 +0100
Subject: [PATCH] rlcmac: Fill radio_priority in Dl Ack/Nack Channel Request
 Description

Change-Id: I740fac72cb418d4b61499990b13208fcd4bb922f
---
 include/osmocom/gprs/rlcmac/llc_queue.h |  2 ++
 src/rlcmac/llc_queue.c                  | 36 +++++++++++++++++--------
 src/rlcmac/rlcmac_enc.c                 |  2 +-
 3 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/include/osmocom/gprs/rlcmac/llc_queue.h b/include/osmocom/gprs/rlcmac/llc_queue.h
index a0a91e4..80016e0 100644
--- a/include/osmocom/gprs/rlcmac/llc_queue.h
+++ b/include/osmocom/gprs/rlcmac/llc_queue.h
@@ -29,6 +29,7 @@ enum gprs_rlcmac_llc_queue_sapi_prio { /* lowest value has highest prio */
 };
 
 struct gprs_llc_prio_queue {
+	uint8_t radio_prio; /* Radio prio of this queue, range (1..4) */
 	struct gprs_codel codel_state;
 	struct llist_head queue; /* queued LLC DL data. See enum gprs_rlcmac_llc_queue_prio. */
 };
@@ -51,6 +52,7 @@ void gprs_rlcmac_llc_queue_set_codel_params(struct gprs_rlcmac_llc_queue *q, boo
 int gprs_rlcmac_llc_queue_enqueue(struct gprs_rlcmac_llc_queue *q, uint8_t *ll_pdu, unsigned int ll_pdu_len,
 				   enum osmo_gprs_rlcmac_llc_sapi sapi, uint8_t radio_prio);
 struct msgb *gprs_rlcmac_llc_queue_dequeue(struct gprs_rlcmac_llc_queue *q);
+uint8_t gprs_rlcmac_llc_queue_highest_radio_prio_pending(struct gprs_rlcmac_llc_queue *q);
 
 static inline size_t gprs_rlcmac_llc_queue_size(const struct gprs_rlcmac_llc_queue *q)
 {
diff --git a/src/rlcmac/llc_queue.c b/src/rlcmac/llc_queue.c
index 2c1aeea..47cb1e9 100644
--- a/src/rlcmac/llc_queue.c
+++ b/src/rlcmac/llc_queue.c
@@ -46,6 +46,7 @@ struct gprs_rlcmac_llc_queue *gprs_rlcmac_llc_queue_alloc(struct gprs_rlcmac_ent
 	q->avg_queue_delay = 0;
 	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
 		for (j = 0; j < ARRAY_SIZE(q->pq[i]); j++) {
+			q->pq[i][j].radio_prio = i + 1; /* range (1..4) */
 			INIT_LLIST_HEAD(&q->pq[i][j].queue);
 			gprs_codel_init(&q->pq[i][j].codel_state);
 		}
@@ -141,25 +142,31 @@ void gprs_rlcmac_llc_queue_clear(struct gprs_rlcmac_llc_queue *q)
 
 #define ALPHA 0.5f
 
+static struct gprs_llc_prio_queue *gprs_rlcmac_llc_queue_find_msg(struct gprs_rlcmac_llc_queue *q)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
+		for (j = 0; j < ARRAY_SIZE(q->pq[i]); j++) {
+			if (!llist_empty(&q->pq[i][j].queue))
+				return &q->pq[i][j];
+		}
+	}
+	return NULL;
+}
+
 static struct msgb *gprs_rlcmac_llc_queue_pick_msg(struct gprs_rlcmac_llc_queue *q, struct gprs_llc_prio_queue **prioq)
 {
 	struct msgb *msg;
 	struct timespec tv_now, tv_result;
 	uint32_t lifetime;
-	unsigned int i, j;
 	const struct llc_queue_entry_hdr *ehdr;
 
-	for (i = 0; i < ARRAY_SIZE(q->pq); i++) {
-		for (j = 0; j < ARRAY_SIZE(q->pq[i]); j++) {
-			if ((msg = msgb_dequeue(&q->pq[i][j].queue))) {
-				*prioq = &q->pq[i][j];
-				goto found;
-			}
-		}
-	}
-	return NULL;
+	*prioq = gprs_rlcmac_llc_queue_find_msg(q);
+	if (!(*prioq))
+		return NULL;
 
-found:
+	msg = msgb_dequeue(&(*prioq)->queue);
 	ehdr = msgb_l1(msg);
 
 	q->queue_size -= 1;
@@ -213,3 +220,10 @@ struct msgb *gprs_rlcmac_llc_queue_dequeue(struct gprs_rlcmac_llc_queue *q)
 	msgb_pull_to_l2(msg);
 	return msg;
 }
+
+uint8_t gprs_rlcmac_llc_queue_highest_radio_prio_pending(struct gprs_rlcmac_llc_queue *q)
+{
+	struct gprs_llc_prio_queue *prioq = gprs_rlcmac_llc_queue_find_msg(q);
+	OSMO_ASSERT(prioq);
+	return prioq->radio_prio;
+}
diff --git a/src/rlcmac/rlcmac_enc.c b/src/rlcmac/rlcmac_enc.c
index 4b8c469..6bceb8d 100644
--- a/src/rlcmac/rlcmac_enc.c
+++ b/src/rlcmac/rlcmac_enc.c
@@ -400,7 +400,7 @@ void gprs_rlcmac_enc_prepare_pkt_downlink_ack_nack(RlcMacUplink_t *block, const
 		Channel_Request_Description_t *chan_req = &ack->Channel_Request_Description;
 		ack->Exist_Channel_Request_Description = 1;
 		chan_req->PEAK_THROUGHPUT_CLASS = 0; /* TODO */
-		chan_req->RADIO_PRIORITY = 0; /* TODO */
+		chan_req->RADIO_PRIORITY = gprs_rlcmac_llc_queue_highest_radio_prio_pending(dl_tbf->tbf.gre->llc_queue);
 		chan_req->RLC_MODE = GPRS_RLCMAC_RLC_MODE_ACKNOWLEDGED;
 		chan_req->LLC_PDU_TYPE = GPRS_RLCMAC_LLC_PDU_TYPE_ACKNOWLEDGED;
 		chan_req->RLC_OCTET_COUNT = 0; /* TODO */