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>

1 files changed, 42 insertions, 3 deletions

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 */