fw/layer1: Add a priority field for sched_item

Each item has a priority associated to it. The standard is :
-4    -> Responses processing
-3    -> L1S parameters changes
-2    -> [Reserved for TPU window setup]
-1    -> (anything)
 0..7 -> Commands relative to time slot n
         (relative to current l1s main timeslot)
 8    -> (anything)
 9    -> [Reserved for TPU window cleanup]
10    -> (anthing)

Note that with this modification, an item scheduled for the
current frame from within a call back won't have its priority
respected !

Signed-off-by: Sylvain Munaut <tnt@246tNt.com>

9 files changed, 91 insertions, 50 deletions

diff --git a/src/target/firmware/include/layer1/tdma_sched.h b/src/target/firmware/include/layer1/tdma_sched.h
index 9486c4ab..2c5873c0 100644
--- a/src/target/firmware/include/layer1/tdma_sched.h
+++ b/src/target/firmware/include/layer1/tdma_sched.h
@@ -21,6 +21,7 @@ struct tdma_sched_item {
 	uint8_t p1;
 	uint8_t p2;
 	uint16_t p3;
+	int16_t prio;
 };
 
 /* A bucket inside the TDMA scheduler */
@@ -36,7 +37,8 @@ struct tdma_scheduler {
 };
 
 /* Schedule an item at 'frame_offset' TDMA frames in the future */
-int tdma_schedule(uint8_t frame_offset, tdma_sched_cb *cb, uint8_t p1, uint8_t p2, uint16_t p3);
+int tdma_schedule(uint8_t frame_offset, tdma_sched_cb *cb,
+                  uint8_t p1, uint8_t p2, uint16_t p3, int16_t prio);
 
 /* Schedule a set of items starting from 'frame_offset' TDMA frames in the future */
 int tdma_schedule_set(uint8_t frame_offset, const struct tdma_sched_item *item_set, uint16_t p3);
@@ -55,7 +57,7 @@ void tdma_sched_dump(void);
 
 
 extern int tdma_end_set(uint8_t p1, uint8_t p2, uint16_t p3);
-#define SCHED_ITEM(x, y, z)		{ .cb = x, .p1 = y, .p2 = z }
+#define SCHED_ITEM(x, p, y, z)		{ .cb = x, .p1 = y, .p2 = z, .prio = p }
 #define SCHED_END_FRAME()		{ .cb = NULL, .p1 = 0, .p2 = 0 }
 #define SCHED_END_SET()			{ .cb = &tdma_end_set, .p1 = 0, .p2 = 0 }
 
diff --git a/src/target/firmware/layer1/prim_fbsb.c b/src/target/firmware/layer1/prim_fbsb.c
index a399d903..3b9da228 100644
--- a/src/target/firmware/layer1/prim_fbsb.c
+++ b/src/target/firmware/layer1/prim_fbsb.c
@@ -288,11 +288,11 @@ static int l1s_sbdet_cmd(__unused uint8_t p1, __unused uint8_t p2,
 
 /* This is how it is done by the TSM30 */
 static const struct tdma_sched_item sb_sched_set[] = {
-	SCHED_ITEM(l1s_sbdet_cmd, 0, 1),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_sbdet_cmd, 0, 2),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_sbdet_cmd, 0, 0, 1),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_sbdet_cmd, 0, 0, 2),	SCHED_END_FRAME(),
 						SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_sbdet_resp, 0, 1),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_sbdet_resp, 0, 2),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_sbdet_resp, -4, 0, 1),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_sbdet_resp, -4, 0, 2),	SCHED_END_FRAME(),
 	SCHED_END_SET()
 };
 
@@ -505,20 +505,20 @@ static int l1s_fbdet_resp(__unused uint8_t p1, uint8_t attempt,
 
 /* FB detection */
 static const struct tdma_sched_item fb_sched_set[] = {
-	SCHED_ITEM(l1s_fbdet_cmd, 0, 0),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_cmd, 0, 0, 0),	SCHED_END_FRAME(),
 						SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 1),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 2),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 3),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 4),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 5),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 6),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 7),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 8),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 9),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 10),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 11),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_fbdet_resp, 0, 12),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 1),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 2),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 3),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 4),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 5),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 6),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 7),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 8),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 9),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 10),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 11),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_fbdet_resp, -4, 0, 12),	SCHED_END_FRAME(),
 	SCHED_END_SET()
 };
 
diff --git a/src/target/firmware/layer1/prim_freq.c b/src/target/firmware/layer1/prim_freq.c
index c6e614fc..7878be68 100644
--- a/src/target/firmware/layer1/prim_freq.c
+++ b/src/target/firmware/layer1/prim_freq.c
@@ -74,7 +74,7 @@ static int l1s_freq_cmd(__unused uint8_t p1, __unused uint8_t p2, __unused uint1
 
 /* sched set for frequency change */
 const struct tdma_sched_item freq_sched_set[] = {
-	SCHED_ITEM(l1s_freq_cmd, 1, 0),
+	SCHED_ITEM(l1s_freq_cmd, -3, 1, 0),
 	SCHED_END_SET()
 };
 
diff --git a/src/target/firmware/layer1/prim_pm.c b/src/target/firmware/layer1/prim_pm.c
index 9bf39212..f36beed2 100644
--- a/src/target/firmware/layer1/prim_pm.c
+++ b/src/target/firmware/layer1/prim_pm.c
@@ -135,9 +135,9 @@ static int l1s_pm_resp(uint8_t num_meas, __unused uint8_t p2,
 }
 
 static const struct tdma_sched_item pm_sched_set[] = {
-	SCHED_ITEM(l1s_pm_cmd, 1, 0),		SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_pm_cmd, 0, 1, 0),	SCHED_END_FRAME(),
 						SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_pm_resp, 1, 0),		SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_pm_resp, -4, 1, 0),	SCHED_END_FRAME(),
 	SCHED_END_SET()
 };
 
diff --git a/src/target/firmware/layer1/prim_rach.c b/src/target/firmware/layer1/prim_rach.c
index 02671af5..11c9a7ef 100644
--- a/src/target/firmware/layer1/prim_rach.c
+++ b/src/target/firmware/layer1/prim_rach.c
@@ -99,9 +99,9 @@ static int l1s_tx_rach_resp(__unused uint8_t p1, __unused uint8_t burst_id,
 
 /* sched sets for uplink */
 const struct tdma_sched_item rach_sched_set_ul[] = {
-	SCHED_ITEM(l1s_tx_rach_cmd, 1, 0),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_tx_rach_cmd, 3, 1, 0),	SCHED_END_FRAME(),
 						SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_tx_rach_resp, 1, 0),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_tx_rach_resp, -4, 1, 0),	SCHED_END_FRAME(),
 	SCHED_END_SET()
 };
 
diff --git a/src/target/firmware/layer1/prim_rx_nb.c b/src/target/firmware/layer1/prim_rx_nb.c
index 74f78296..f05459cd 100644
--- a/src/target/firmware/layer1/prim_rx_nb.c
+++ b/src/target/firmware/layer1/prim_rx_nb.c
@@ -195,11 +195,11 @@ static int l1s_nb_cmd(__unused uint8_t p1, uint8_t burst_id,
 }
 
 const struct tdma_sched_item nb_sched_set[] = {
-	SCHED_ITEM(l1s_nb_cmd, 0, 0),					SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_nb_cmd, 0, 1),					SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_nb_resp, 0, 0), SCHED_ITEM(l1s_nb_cmd, 0, 2),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_nb_resp, 0, 1), SCHED_ITEM(l1s_nb_cmd, 0, 3),	SCHED_END_FRAME(),
-				       SCHED_ITEM(l1s_nb_resp, 0, 2),	SCHED_END_FRAME(),
-				       SCHED_ITEM(l1s_nb_resp, 0, 3),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_nb_cmd, 0, 0, 0),						SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_nb_cmd, 0, 0, 1),						SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_nb_resp, -4, 0, 0),	SCHED_ITEM(l1s_nb_cmd, 0, 0, 2),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_nb_resp, -4, 0, 1),	SCHED_ITEM(l1s_nb_cmd, 0, 0, 3),	SCHED_END_FRAME(),
+						SCHED_ITEM(l1s_nb_resp, -4, 0, 2),	SCHED_END_FRAME(),
+						SCHED_ITEM(l1s_nb_resp, -4, 0, 3),	SCHED_END_FRAME(),
 	SCHED_END_SET()
 };
diff --git a/src/target/firmware/layer1/prim_tx_nb.c b/src/target/firmware/layer1/prim_tx_nb.c
index f1398e0d..8da0d404 100644
--- a/src/target/firmware/layer1/prim_tx_nb.c
+++ b/src/target/firmware/layer1/prim_tx_nb.c
@@ -178,28 +178,28 @@ void l1s_tx_test(uint8_t base_fn, uint8_t type)
 	printf("Starting TX %d\n", type);
 
 	if (type == 0) {// one normal burst
-		tdma_schedule(base_fn, &l1s_tx_cmd, 0, 0, 0);
-		tdma_schedule(base_fn + 2, &l1s_tx_resp, 0, 0, 0);
+		tdma_schedule(base_fn, &l1s_tx_cmd, 0, 0, 0, 3);
+		tdma_schedule(base_fn + 2, &l1s_tx_resp, 0, 0, 0, 3);
 	} else if (type == 2) { // four normal burst
-		tdma_schedule(base_fn, &l1s_tx_cmd, 2, 0, 0);
-		tdma_schedule(base_fn + 1, &l1s_tx_cmd, 2, 1, 0);
-		tdma_schedule(base_fn + 2, &l1s_tx_resp, 2, 0, 0);
-		tdma_schedule(base_fn + 2, &l1s_tx_cmd, 2, 2, 0);
-		tdma_schedule(base_fn + 3, &l1s_tx_resp, 2, 1, 0);
-		tdma_schedule(base_fn + 3, &l1s_tx_cmd, 2, 3, 0);
-		tdma_schedule(base_fn + 4, &l1s_tx_resp, 2, 2, 0);
-		tdma_schedule(base_fn + 5, &l1s_tx_resp, 2, 3, 0);
+		tdma_schedule(base_fn, &l1s_tx_cmd, 2, 0, 0, 3);
+		tdma_schedule(base_fn + 1, &l1s_tx_cmd, 2, 1, 0, 3);
+		tdma_schedule(base_fn + 2, &l1s_tx_resp, 2, 0, 0, 3);
+		tdma_schedule(base_fn + 2, &l1s_tx_cmd, 2, 2, 0, 3);
+		tdma_schedule(base_fn + 3, &l1s_tx_resp, 2, 1, 0, 3);
+		tdma_schedule(base_fn + 3, &l1s_tx_cmd, 2, 3, 0, 3);
+		tdma_schedule(base_fn + 4, &l1s_tx_resp, 2, 2, 0, 3);
+		tdma_schedule(base_fn + 5, &l1s_tx_resp, 2, 3, 0, 3);
 	}
 }
 
 /* sched sets for uplink */
 const struct tdma_sched_item nb_sched_set_ul[] = {
-	SCHED_ITEM(l1s_tx_cmd, 2, 0),					SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_tx_cmd, 2, 1),					SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_tx_resp, 2, 0), SCHED_ITEM(l1s_tx_cmd, 2, 2),	SCHED_END_FRAME(),
-	SCHED_ITEM(l1s_tx_resp, 2, 1), SCHED_ITEM(l1s_tx_cmd, 2, 3),	SCHED_END_FRAME(),
-				       SCHED_ITEM(l1s_tx_resp, 2, 2),	SCHED_END_FRAME(),
-				       SCHED_ITEM(l1s_tx_resp, 2, 3),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_tx_cmd, 3, 2, 0),						SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_tx_cmd, 3, 2, 1),						SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_tx_resp, -4, 2, 0),	SCHED_ITEM(l1s_tx_cmd, 3, 2, 2),	SCHED_END_FRAME(),
+	SCHED_ITEM(l1s_tx_resp, -4, 2, 1),	SCHED_ITEM(l1s_tx_cmd, 3, 2, 3),	SCHED_END_FRAME(),
+						SCHED_ITEM(l1s_tx_resp, -4, 2, 2),	SCHED_END_FRAME(),
+						SCHED_ITEM(l1s_tx_resp, -4, 2, 3),	SCHED_END_FRAME(),
 	SCHED_END_SET()
 };
 
diff --git a/src/target/firmware/layer1/sync.c b/src/target/firmware/layer1/sync.c
index 7d7d29f6..7062fe63 100644
--- a/src/target/firmware/layer1/sync.c
+++ b/src/target/firmware/layer1/sync.c
@@ -307,7 +307,7 @@ static int l1s_abort_cmd(__unused uint8_t p1, __unused uint8_t p2,
 void l1s_dsp_abort(void)
 {
 	/* abort right now */
-	tdma_schedule(0, &l1s_abort_cmd, 0, 0, 0);
+	tdma_schedule(0, &l1s_abort_cmd, 0, 0, 0, 10);
 }
 
 void l1s_tx_apc_helper(void)
diff --git a/src/target/firmware/layer1/tdma_sched.c b/src/target/firmware/layer1/tdma_sched.c
index 82892103..889d9e44 100644
--- a/src/target/firmware/layer1/tdma_sched.c
+++ b/src/target/firmware/layer1/tdma_sched.c
@@ -51,7 +51,8 @@ static uint8_t wrap_bucket(uint8_t offset)
 }
 
 /* Schedule an item at 'frame_offset' TDMA frames in the future */
-int tdma_schedule(uint8_t frame_offset, tdma_sched_cb *cb, uint8_t p1, uint8_t p2, uint16_t p3)
+int tdma_schedule(uint8_t frame_offset, tdma_sched_cb *cb,
+                  uint8_t p1, uint8_t p2, uint16_t p3, int16_t prio)
 {
 	struct tdma_scheduler *sched = &l1s.tdma_sched;
 	uint8_t bucket_nr = wrap_bucket(frame_offset);
@@ -69,6 +70,7 @@ int tdma_schedule(uint8_t frame_offset, tdma_sched_cb *cb, uint8_t p1, uint8_t p
 	sched_item->p1 = p1;
 	sched_item->p2 = p2;
 	sched_item->p3 = p3;
+	sched_item->prio = prio;
 
 	return 0;
 }
@@ -120,19 +122,55 @@ void tdma_sched_advance(void)
 	sched->cur_bucket = next_bucket;
 }
 
+/* Sort a bucket entries by priority */
+static void _tdma_sched_bucket_sort(struct tdma_sched_bucket *bucket, int *seq)
+{
+	int i, j, k;
+	struct tdma_sched_item *item_i, *item_j;
+
+	/* initial sequence */
+		/* we need all the items because some call back may schedule
+		 * new call backs 'on the fly' */
+	for (i=0; i<TDMASCHED_NUM_CB; i++)
+		seq[i] = i;
+
+	/* iterate over items in this bucket and sort them */
+	for (i=0; i<bucket->num_items; i++)
+	{
+		item_i = &bucket->item[seq[i]];
+
+		for (j=i+1; j<bucket->num_items; j++)
+		{
+			item_j = &bucket->item[seq[j]];
+
+			if (item_i->prio > item_j->prio)
+			{
+				item_i = item_j;
+				k      = seq[i];
+				seq[i] = seq[j];
+				seq[j] = k;
+			}
+		}
+	}
+}
+
 /* Execute pre-scheduled events for current frame */
 int tdma_sched_execute(void)
 {
 	struct tdma_scheduler *sched = &l1s.tdma_sched;
 	struct tdma_sched_bucket *bucket;
 	int i, num_events = 0;
+	int seq[TDMASCHED_NUM_CB];
 
 	/* determine current bucket */
 	bucket = &sched->bucket[sched->cur_bucket];
 
+	/* get sequence in priority order */
+	_tdma_sched_bucket_sort(bucket, seq);
+
 	/* iterate over items in this bucket and call callback function */
 	for (i = 0; i < bucket->num_items; i++) {
-		struct tdma_sched_item *item = &bucket->item[i];
+		struct tdma_sched_item *item = &bucket->item[seq[i]];
 		int rc;
 
 		num_events++;
@@ -145,7 +183,8 @@ int tdma_sched_execute(void)
 		}
 		/* if the cb() we just called has scheduled more items for the
 		 * current TDMA, bucket->num_items will have increased and we
-		 * will simply continue to execute them as intended */
+		 * will simply continue to execute them as intended. Priorities
+		 * won't work though ! */
 	}
 
 	/* clear/reset the bucket */