RK0: The Real-Time Kernel '0'

RK_SEMA work1Sema;
RK_SEMA work2Sema;

VOID kApplicationInit(VOID)
{
/* semaphores init at 0 */
	kSemaInit(&work1Sema, RK_SEMA_BIN, 0);
	kSemaInit(&work2Sema, RK_SEMA_BIN, 0);

}

VOID Task1(VOID* args)
{
    RK_UNUSEARGS
	while (1)
	{
	    doWork1();
		kSemaPost(&work1Sema);
		kSemaPend(&work2Sema, RK_WAIT_FOREVER);
		 /* T1 finished. Waiting for T2. */

	}
}

VOID Task2(VOID* args)
{
    RK_UNUSEARGS
	while (1)
	{
		 kSemaPend(&work1Sema, RK_WAIT_FOREVER);
		 doWork2();
		 kSemaPost(&work2Sema);
	}
}

RK_SEMA  item;
RK_SEMA  space;
RK_MUTEX lock;
#define N (K)
typedef struct mesg
{
 UINT field1;
 UINT field2;
 UINT field3;
 UINT field4;
} Mesg_t; /* a 16-byte message */

/* a ring buffer of messages */
Mesg_t mailbox[N]={0};

kApplicationInit(VOID)
{
    kSemaInit(&item,  RK_SEMA_COUNT, 0); /* no items */
    kSemaInit(&space, RK_SEMA_COUNT, N); /* N buffers available */
    kMutexInit(&lock);
}
/* circular buffer handling omitted */

/* wait for space, lock, write, unlock, signal there is item */
VOID PostMail(Mesg_t* sendPtr)
{
    kSemaWait(&space, RK_WAIT_FOREVER);
    kMutexLock(&lock,  RK_WAIT_FOREVER);
    memcpy(&mailbox[tail], sendPtr, sizeof(Mesg_t));
    kMutexUnlock(&lock);
    kSemaSignal(&item);
}

/* wait for item, lock, read, unlock, signal there is space */
VOID PendMail(Mesg_t* recvPtr)
{
    kSemaWait(&item, RK_WAIT_FOREVER);
    kMutexLock(&lock,  RK_WAIT_FOREVER);
    memcpy(recvPtr, &mailbox[head], sizeof(Mesg_t));
    kMutexUnlock(&lock);
    kSemaSignal(&space);
}

/* Task1 has the Highest nominal priority */
/* Task2 has the Medium nominal priority */
/* Task3 has Lowest nominal priority */

/* Note Task3 starts as 1 and 2 are delayed */

RK_DECLARE_TASK(task1Handle, Task1, stack1, STACKSIZE)
RK_DECLARE_TASK(task2Handle, Task2, stack2, STACKSIZE)
RK_DECLARE_TASK(task3Handle, Task3, stack3, STACKSIZE)


RK_MUTEX mutexA;
RK_MUTEX mutexB;

VOID kApplicationInit(VOID)
{
	kassert(!kCreateTask(&task1Handle, Task1, RK_NO_ARGS, "Task1", stack1, \
		STACKSIZE, 1, RK_PREEMPT));
	kassert(!kCreateTask(&task2Handle, Task2, RK_NO_ARGS, "Task2", stack2, \
		STACKSIZE, 2, RK_PREEMPT));
	kassert(!kCreateTask(&task3Handle, Task3, RK_NO_ARGS, "Task3", stack3, \
		STACKSIZE, 3, RK_PREEMPT));

/* mutexes initialised with priority inheritance enabled */
	kMutexInit(&mutexA, RK_INHERIT);
	kMutexInit(&mutexB, RK_INHERIT);
}



VOID Task3(VOID *args)
{
	RK_UNUSEARGS
	while (1)
	{
		printf("@ %lums: [TL] Attempting to LOCK 'A' | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kMutexLock(&mutexA, RK_WAIT_FOREVER);

		printf("@ %lums: [TL] LOCKED 'A' (in CS) | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kBusyWait(60); /* <-- important */

		printf("@%lums: [TL] About to UNLOCK 'A' | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kMutexUnlock(&mutexA);

		printf("--->");
		printf("@%lums: [TL] Exit CS | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kSleep(4);
	}
}

VOID Task2(VOID *args)
{
	RK_UNUSEARGS
	while (1)
	{
		kSleep(5);

		printf("@%lums: [TM] Attempting to LOCK 'B' | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);
		kMutexLock(&mutexB, RK_WAIT_FOREVER);

		printf("@%lums: [TM] LOCKED 'B', now trying to LOCK 'A' | Eff: %d | Nom: %d\r\n",
			   kTickGet(), runPtr->priority, runPtr->prioReal);
		kMutexLock(&mutexA, RK_WAIT_FOREVER);

		printf("@%lums: [TM] LOCKED 'A' (in CS) | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);
		kMutexUnlock(&mutexA);

		printf("@%lums: [TM] UNLOCKING 'B' | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kMutexUnlock(&mutexB);

		printf("--->");

		printf("@%lums: [TM] Exit CS | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);
	}
}

VOID Task1(VOID *args)
{
	RK_UNUSEARGS
	while (1)
	{
		kSleep(2);

		printf("@%lums: [TH] Attempting to LOCK 'B'| Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kMutexLock(&mutexB, RK_WAIT_FOREVER);

		printf("@%lums: [TH] LOCKED 'B' (in CS)  | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);

		kMutexUnlock(&mutexB);

		printf("--->");

		printf("@%lums: [TH] Exit CS | Eff: %d | Nom: %d\r\n", kTickGet(),
			   runPtr->priority, runPtr->prioReal);
	}
}

>>>> TL locks 'A'. Higher priority tasks are sleeping. <<<<

@ 14720ms: [TL] Attempting to LOCK 'A' | Eff: 3 | Nom: 3
@ 14720ms: [TL] LOCKED 'A' (in CS) | Eff: 3 | Nom: 3

@14721ms: [TM] Attempting to LOCK 'B' | Eff: 2 | Nom: 2

>>>> TM acquires 'B' and is blocked by TL on 'A'. TL inherits TM's  priority. <<<<

@14721ms: [TM] LOCKED 'B', now trying to LOCK 'A' | Eff: 2 | Nom: 2

>>>> TH will blocked by TM on 'B': <<<<

@14722ms: [TH] Attempting to LOCK 'B'| Eff: 1 | Nom: 1

>>>> TM inherits TH's priority. TL inherits TH's priority via TM. <<<<

@14780ms: [TL] About to UNLOCK 'A' | Eff: 1 | Nom: 3

>>>> Upon unlocking 'A', TL is preempted by TM. It means TL's priority has been restored, as it is no longer blocking a higher priority task. <<<<


>>>> Now TM acquires 'A' <<<<

@14780ms: [TM] LOCKED 'A' (in CS) | Eff: 1 | Nom: 2

>>>> After releasing 'A', but before releasing 'B', TM's priority is still '1', as it is blocking TH while holding 'B'. <<<<

@14780ms: [TM] UNLOCKING 'B' | Eff: 1 | Nom: 2

>>>> Upon unlocking 'B' TM is preempted by TH. (TM's priority has been restored.) <<<<

@14780ms: [TH] LOCKED 'B' (in CS)  | Eff: 1 | Nom: 1

>>> RESULT: even though priority inversion was enforced, tasks leave the nested lock ordered by their nominal priority. <<<

--->@14780ms: [TH] Exit CS | Eff: 1 | Nom: 1
--->@14780ms: [TM] Exit CS | Eff: 2 | Nom: 2
--->@14780ms: [TL] Exit CS | Eff: 3 | Nom: 3

/* PSEUDO CODE */

/* MONITOR ADT: */

/** DATA **/
UINT currentItemNum : number of items in the buffer
UINT const maxItemNum     : buffer capacity (slots)
RK_MUTEX mutex      : monitor lock
RK_SLEEP_QUEUE waitingItem : tasks waiting for an item
RK_SLEEP_QUEUE waitingSlot : tasks waiting for a slot

/** PROCEDURES **/
VOID INSERT(ITEM_t) : insert an item
VOID EXTRACT(ITEM_t*) : extract an item

*/


/**********************************************
 Producer-Consumer Solution with a Mesa Monitor
***********************************************/


/* MONITOR PROCEDURES */

/* INSERT AN ITEM */

MONITOR.INSERT(ITEM_t item)
{

    LOCK(&mutex); /* lock monitor */

    /* condition for producers is having
    free slots */
    while (!(currentItemNum < maxItemNum))
    {
        /* is full */
        DISABLE_PREEMPTION(); /* disable scheduler */

        UNLOCK(&mutex);

        WAIT(&waitingSlot);

        ENABLE_PREEMPTION();

    /***********************************************
    !IMPORTANT!: if preemption was allowed and  unlock(&mutex) switched a higher priority task to READY, the active task would be preempted,
    compromising the entire synch logic.
    *************************************************/

        /* when waking acquire mutex again */
        LOCK(&mutex);
    }

    /* buffer has slots, mutex is held */
    depositItem(item);
    /* increase number of items */
    currentItemNum++;

    /* signal any tasks waiting for an item */
    SIGNAL(&waitingItem);

    /* release monitor and leave */
    UNLOCK(&mutex);
    return;
}

/* PROCEDURE: EXTRACT AN ITEM */

MONITOR.EXTRACT(ITEM_t* item):
{
    LOCK(&mutex); /* lock monitor */
    while (!(currentItemNum > 0))
    {
        /* is empty */
        DISABLE_PREEMPTION();

        UNLOCK(&mutex);

        WAIT(&waitingItem);

        ENABLE_PREEMPTION();

        /* when waking acquire mutex again */
        LOCK(&mutex);
    }
    *item = extractItem();
    /* decrease number of items */
    currentItemNum--;
    /* signal any tasks waiting for a free slot  */
    SIGNAL(&waitingSlot);
    /* release monitor and leave */
    UNLOCK(&mutex);
   return;

}

/* Synchronisation Barrier */

typedef struct
{
    RK_MUTEX lock;
    RK_SLEEP_QUEUE allSynch;
    UINT count;        /* number of tasks in the barrier */
    UINT round;        /* increased every time all tasks synch     */
    UINT nRequired; /* number of tasks required */



} Barrier_t;

VOID BarrierInit(Barrier_t *const barPtr, UINT nRequired)
{
    kMutexInit(&barPtr->lock, RK_INHERIT);
    kEventInit(&barPtr->allSynch);
    barPtr->count = 0;
    barPtr->round = 0;
    barPtr->Required = nRequired;

}

VOID BarrierWait(Barrier_t *const barPtr)
{
    UINT myRound = 0;
    kMutexLock(&barPtr->lock, RK_WAIT_FOREVER);

    /* save round number */
    myRound = barPtr->round;
    /* increase count on this round */
    barPtr->count++;

    if (barPtr->count == barPtr->nRequired)
    {
        /* reset counter, inc round, broadcast to sleeping tasks */
        barPtr->round++;
        barPtr->count = 0;
        kCondVarBroadcast(&barPtr->allSynch);
    }
    else
    {
        /* a proper wake signal might happen after inc round */
        while ((UINT)(barPtr->round - myRound) == 0U)
        {
            RK_ERR err = kCondVarWait(&barPtr->allSynch, &barPtr->lock, RK_WAIT_FOREVER);
            kassert(err==RK_SUCCESS);
        }
    }

    kMutexUnlock(&barPtr->lock);

}


#define N_REQUIRED 3

Barrier_t syncBarrier;

VOID kApplicationInit(VOID)
{

    kassert(!kCreateTask(&task1Handle, Task1, RK_NO_ARGS, "Task1", stack1, STACKSIZE, 2, RK_PREEMPT));
    kassert(!kCreateTask(&task2Handle, Task2, RK_NO_ARGS, "Task2", stack2, STACKSIZE, 3, RK_PREEMPT));
    kassert(!kCreateTask(&task3Handle, Task3, RK_NO_ARGS, "Task3", stack3, STACKSIZE, 1, RK_PREEMPT));
	BarrierInit(&syncBarrier, N_REQUIRED);
}
VOID Task1(VOID* args)
{
    RK_UNUSEARGS
    while (1)
    {
        kPuts("Task 1 is waiting at the barrier...\r\n");
        BarrierWait(&syncBarrier);
        kPuts("Task 1 passed the barrier!\r\n");
		kSleep(8);

    }
}

VOID Task2(VOID* args)
{
    RK_UNUSEARGS
    while (1)
    {
        kPuts("Task 2 is waiting at the barrier...\r\n");
        BarrierWait(&syncBarrier);
        kPuts("Task 2 passed the barrier!\r\n");
		kSleep(5);
	}
}

VOID Task3(VOID* args)
{
    RK_UNUSEARGS
    while (1)
    {
        kPuts("Task 3 is waiting at the barrier...\r\n");
        BarrierWait(&syncBarrier);
        kPuts("Task 3 passed the barrier!\r\n");
        kSleep(3);
	}
}

/* RW-Lock */

/* a single writer is allowed if there are no readers */
/* several readers are allowed if there is no writer*/
typedef struct
{
	RK_MUTEX	 lock;
	RK_SLEEP_QUEUE	 writersGo;
	RK_SLEEP_QUEUE	 readersGo;
	INT			 rwCount; /* number of active readers if > 0 */
						  /* active writer if -1             */

}RwLock_t;

VOID RwLockInit(RwLock_t *const rwLockPtr)
{

	kMutexInit(&rwLockPtr->lock, RK_INHERIT);
	kEventInit(&rwLockPtr->writersGo);
	kEventInit(&rwLockPtr->readersGo);
	rwLockPtr->rwCount = 0;
}

/* A writer can acquire if  rwCount = 0 */
/* An active writer is indicated by rwCount = -1; */
VOID RwLockAcquireWrite(RwLock_t *const rwLockPtr)
{
	kMutexLock(&rwLockPtr->lock, RK_WAIT_FOREVER);
	/* if different than 0, there are either writers or readers */
	/* sleep to be signalled */
	while (rwLockPtr->rwCount != 0)
	{
	    kCondVarWait(&rwLockPtr->writersGo, &rwLockPtr->lock, RK_WAIT_FOREVER);
	    /* mutex is locked when waking up*/
	}
	/* woke here, set an active writer */
	rwLockPtr->rwCount = -1;
	kMutexUnlock(&rwLockPtr->lock);
}

/* a writer releases, waking up all waiting readers, if any */
/* if there are no readers, a writer can get in */
VOID RwLockReleaseWrite(RwLock_t *const rwLockPtr)
{
	kMutexLock(&rwLockPtr->lock, RK_WAIT_FOREVER);

	rwLockPtr->rwCount = 0; /* indicate no writers*/

	/* if there are waiting readers, flush */
	ULONG nWaitingReaders=0;
	kEventQuery(&rwLockPtr->readersGo, &nWaitingReaders);
	if (nWaitingReaders > 0)
	{
	    /* condVarBroadcast is just an alias for an event flush */
		kEventFlush(&rwLockPtr->readersGo);
	}
	else
	{
		/* wake up a single writer if any */
		kEventSignal(&rwLockPtr->writersGo);
	}
	kMutexUnlock(&rwLockPtr->lock);
}

/* a reader can acquire if there are no writers */
VOID RwLockAcquireRead(RwLock_t *const rwLockPtr)
{
	kMutexLock(&rwLockPtr->lock, RK_WAIT_FOREVER);
	/* if there is an active writer, sleep */
	while (rwLockPtr->rwCount < 0)
	{
	    kCondVarWait(&rwLockPtr->readersGo, &rwLockPtr->lock, RK_WAIT_FOREVER);
	    /* mutex is locked when waking up*/
	}
	/* increase rwCount, so its > 0, indicating readers */
	rwLockPtr->rwCount ++;
	kMutexUnlock(&rwLockPtr->lock);
}

/* a reader releases and wakes a single writer */
/* if it is the last reader */
VOID RwLockReleaseRead(RwLock_t *const rwLockPtr)
{
	kMutexLock(&rwLockPtr->lock, RK_WAIT_FOREVER);
	rwLockPtr->rwCount --;
	if (rwLockPtr->rwCount == 0)
	{
		kEventSignal(&rwLockPtr->writersGo);
	}
	kMutexUnlock(&rwLockPtr->lock);
}

/* Optimised deep copy; guaranteed mesgSize>0 */
/* destPtr and srcPtr are pointers to a word */
#define RK_CPY(destPtr, srcPtr, mesgSize) \
do {                                   \
      while (--mesgSize)               \
      {                                \
     /* if mesgSize is 1, this is NOT executed */
        *(destPtr++) = *(srcPtr++)     \
      };                               \
     /* the last or the only copy is executed now */
     *(destPtr++) = *(srcPtr++)       \
   } while(0U)

#define kPend(timeout)                                \
    do                                                \
    {                                                 \
        kSignalGet(0x1, RK_FLAGS_ANY, NULL, timeout); \
    } while (0)

#define kSignal(taskhandle)           \
    do                                \
    {                                 \
        kSignalSet(taskhandle, 0x01); \
    } while (0)



typedef enum
{
	TEMPERATURE=1, HUMIDITY, CO2, FLOW
}SensorType_t;



/* sensor types */
struct sensorMsg
{
    SensorType_t sensorType;
    ULONG sensorValue;

};

typedef struct sensorMsg Mesg_t;

#define N_MESSAGE 8
#define MESSAGE_SIZE (sizeof(Mesg_t) + sizeof(ULONG) - 1)/(sizeof(ULONG) /* WORDS! */
#define N_SENSOR    4
#define AVG_WINDOW_SIZE   10 /* 10 samples */

RK_STREAM sensorStream;/* the stream kobject */
Mesg_t mesgBuf[N_MESSAGE] = {0};/* queue buffer */
RK_TIMER timerT1;
RK_MBOX sensorBox;
static Mesg_t sample = {0};
static UINT sampleErr;

VOID callBackISR(VOID* ARGS);

VOID kApplicationInit( VOID)
{
    RK_ERR err = kStreamInit(&sensorStream, (VOID*) mesgBuf, MESSAGE_SIZE,
    N_MESSAGE);

    kassert(err==RK_SUCCESS);

    /* timer @ every 10 ms */
    err = kTimerInit(&timerT1, 0, 10, callBackISR, NULL, RK_TIMER_RELOAD);
    kassert( err==RK_SUCCESS);

    err = kMboxInit(&sensorBox, NULL);
    kassert( err==RK_SUCCESS);
}

VOID callBackISR(VOID *args)
{
    RK_UNUSEARGS
    sample.sensorType = (rand() % 4) + 1;
    switch (sample.sensorType)
    {
        case TEMPERATURE:
            sample.sensorValue = ( ULONG) rand() % 50;
            break;
        case HUMIDITY:
            sample.sensorValue = ( ULONG) rand() % 100;
            break;
        case CO2:
            sample.sensorValue = ( ULONG) rand() % 1000;
            break;
        case FLOW:
            sample.sensorValue = ( ULONG) rand() % 10;
            break;
        default:
            break;
    }
    RK_ERR err = kMboxPost( &sensorBox, &sample, RK_NO_WAIT);
    if (err != RK_SUCCESS)
        sampleErr ++;

}

/* Producer - higher priority, blocks on mailbox */
VOID Task1(VOID *args)
{
    RK_UNUSEARGS
    Mesg_t *recvSample = NULL;
    while (1)
    {
        RK_ERR errmbox = kMboxPend( &sensorBox, ( VOID**) &recvSample,
                RK_WAIT_FOREVER);
        kassert( errmbox==RK_SUCCESS);
        RK_ERR err = kStreamSend( &sensorStream, &sample, RK_NO_WAIT);
        kassert(err >= 0); /* either succesful or unsuccesful */
        if (err == RK_SUCCESS)
        {
            CHAR const *sensorTypeStr = NULL;
            if (recvSample->sensorType == 1)
                sensorTypeStr = "TEMP";
            if (recvSample->sensorType == 2)
                sensorTypeStr = "HUM";
            if (recvSample->sensorType == 3)
                sensorTypeStr = "CO2";
            if (recvSample->sensorType == 4)
                    sensorTypeStr = "FLOW";
            printf( "ENQ: [@%lums, %s, %lu] \r\n", kTickGet(), sensorTypeStr,
                        recvSample->sensorValue);
        }
        else if (err == RK_ERR_STREAM_FULL)
        {
            kSignal(task2Handle);
        }
    }
}

/* for each sensor:
     . a ring buffer of AVG_WINDOW_SIZE values
 . sum of values
 . an index table (=enum - 1 eg., HUMIDITY IDX=2-1=1)
 */
static ULONG ringBuf[N_SENSOR][AVG_WINDOW_SIZE];
static ULONG ringSum[N_SENSOR] = {0};
static UINT ringIndex[N_SENSOR] = {0};

void Task2( void *args)
{

    RK_UNUSEARGS
    Mesg_t readSample;
    while (1)
    {
        RK_ERR err = kStreamRecv(&sensorStream, (VOID*)&readSample,
        RK_NO_WAIT);
        if (err == RK_SUCCESS)
        {
            UINT sensorIdx = readSample.sensorType - 1;

/* remove oldest sample */
            ULONG oldest = ringBuf[sensorIdx][ringIndex[sensorIdx]];
            ringSum[sensorIdx] -= oldest;

/* push new sample */
            ringBuf[sensorIdx][ringIndex[sensorIdx]] = readSample.sensorValue;
            ringSum[sensorIdx] += readSample.sensorValue;

/* index incr-wrap */
            ringIndex[sensorIdx] ++;
            ringIndex[sensorIdx] %= AVG_WINDOW_SIZE;

/* simple average */
            ULONG avg = ringSum[sensorIdx] / AVG_WINDOW_SIZE;


            CHAR const *sensorTypeStr = NULL;
            if (readSample.sensorType == 1)
                sensorTypeStr = "TEMP";
            if (readSample.sensorType == 2)
                sensorTypeStr = "HUM";
            if (readSample.sensorType == 3)
                sensorTypeStr = "CO2";
            if (readSample.sensorType == 4)
                sensorTypeStr = "FLOW";

            printf( "DEQ: [@%lums, %s, %lu] | AVG: %lu \r\n", kTickGet(),
                    sensorTypeStr, readSample.sensorValue, avg);

        }
        else
        {
            kPend(RK_WAIT_FOREVER);
        }

    }
}

OUTPUT:

ENQ: [@550ms, CO2, 571]
ENQ: [@560ms, FLOW, 4]
ENQ: [@570ms, FLOW, 4]
ENQ: [@580ms, HUM, 25]
ENQ: [@590ms, CO2, 931]
ENQ: [@600ms, CO2, 487]
ENQ: [@610ms, FLOW, 7]
ENQ: [@620ms, HUM, 79]

>>> Queue is full. Now offload and process. Note the order remains <<<

DEQ: [@630ms, CO2, 571] | AVG: 460
DEQ: [@631ms, FLOW, 4] | AVG: 5
DEQ: [@632ms, FLOW, 4] | AVG: 5
DEQ: [@633ms, HUM, 25] | AVG: 52
DEQ: [@634ms, CO2, 931] | AVG: 553
DEQ: [@635ms, CO2, 487] | AVG: 549
DEQ: [@636ms, FLOW, 7] | AVG: 5
DEQ: [@637ms, HUM, 79] | AVG: 55

>>> Consumer is preempted <<<
ENQ: [@640ms, CO2, 913]
ENQ: [@650ms, CO2, 134]
ENQ: [@660ms, HUM, 47]
ENQ: [@670ms, HUM, 30]
ENQ: [@680ms, TEMP, 7]
ENQ: [@690ms, CO2, 726]
ENQ: [@700ms, FLOW, 7]
ENQ: [@710ms, TEMP, 43]

DEQ: [@720ms, CO2, 913] | AVG: 578
DEQ: [@721ms, CO2, 134] | AVG: 543
DEQ: [@722ms, HUM, 47] | AVG: 51
DEQ: [@723ms, HUM, 30] | AVG: 44
DEQ: [@724ms, TEMP, 7] | AVG: 20
DEQ: [@725ms, CO2, 726] | AVG: 592
DEQ: [@726ms, FLOW, 7] | AVG: 5
DEQ: [@727ms, TEMP, 43] | AVG: 23

/* Notify Callback on Queues */

/* each queue has registered this send callback */
VOID sendNotify(RK_QUEUE *qPtr)
{
    UINT i = 0;
    for (i = 0; i < 3; ++i)
    {
        if (queues[i] == qPtr)
        {
            ULONG qFlag = 1UL << i;
            kSignalSet(superHandle, qFlag);
            break;
        }
    }
}

/* tasks sending follow this pattern */

VOID Task2(VOID *args)
{
    RK_UNUSEARGS

    UINT num = 0x20;

    while (1)
    {
        MESG_t *ptr;
        ptr = (MESG_t *)kMemAlloc(&queueMem);
        if (ptr)
        {
            ptr->num = num++;
            ptr->senderID = RK_RUNNING_PID;
            RK_ERR err = kQueuePost(&queue2, (VOID *)ptr, RK_NO_WAIT);
            if (err != RK_SUCCESS)
            {
                kMemFree(&qMem, (VOID *)ptr);
                kprintf("Q2 FULL\n\r");
            }
         }
        kSleepPeriodic(50);
    }
}

/* supervisor listening on 3 queues */

/*
although it has a higher period, it has also the highest priority, what is acceptable for supervisors
you see the tasks posting to queues are keeping its two succesfull posts the supervisor drains
every 100ms
*/

VOID SupervisorTask(VOID *args)
{    RK_UNUSEARGS

    static ULONG gotFlags = 0UL;

    while (1)
    {
        gotFlags = 0UL;
        kSignalGet(0x7, RK_FLAGS_ANY, &gotFlags, RK_NO_WAIT);

        UINT k = 0;

        /* we drain each queue that is set */
        for (k = 0; k < 3; ++k)
        {
            if (gotFlags & (1UL << k))
            {
                VOID *recvPtr = NULL;

                while (
                    kQueuePend(queues[k], &recvPtr,
                               RK_NO_WAIT) == RK_SUCCESS)
                {
                    MESG_t *m = (MESG_t*)recvPtr;
                    /*1-copy message passing */
                    UINT id = m->senderID;
                    UINT num = m->num;

                    kMemFree(&queueMem, recvPtr);

                    UINT sel = k + 1; /* bit position */
                    logPost(" sel: %d, senderID: %d, payload: 0x%02X", sel, id, num);

                }
            }
        }
        kSleepPeriodic(100);
    }
}

OUTPUT:
36500 ms ::  sel: 1, senderID: 5, payload: 0x2E8
36500 ms ::  sel: 1, senderID: 5, payload: 0x2E9
36500 ms ::  sel: 2, senderID: 3, payload: 0x2F8
36500 ms ::  sel: 2, senderID: 3, payload: 0x2F9
36500 ms ::  sel: 3, senderID: 4, payload: 0x308
36500 ms ::  sel: 3, senderID: 4, payload: 0x309
36600 ms ::  sel: 1, senderID: 5, payload: 0x2EA
36600 ms ::  sel: 1, senderID: 5, payload: 0x2EB
36600 ms ::  sel: 2, senderID: 3, payload: 0x2FA
36600 ms ::  sel: 2, senderID: 3, payload: 0x2FB
36600 ms ::  sel: 3, senderID: 4, payload: 0x30A
36600 ms ::  sel: 3, senderID: 4, payload: 0x30B
36700 ms ::  sel: 1, senderID: 5, payload: 0x2EC
36700 ms ::  sel: 1, senderID: 5, payload: 0x2ED
36700 ms ::  sel: 2, senderID: 3, payload: 0x2FC
36700 ms ::  sel: 2, senderID: 3, payload: 0x2FD
36700 ms ::  sel: 3, senderID: 4, payload: 0x30C
36700 ms ::  sel: 3, senderID: 4, payload: 0x30D
36800 ms ::  sel: 1, senderID: 5, payload: 0x2EE
36800 ms ::  sel: 1, senderID: 5, payload: 0x2EF
36800 ms ::  sel: 2, senderID: 3, payload: 0x2FE
36800 ms ::  sel: 2, senderID: 3, payload: 0x2FF
36800 ms ::  sel: 3, senderID: 4, payload: 0x30E
36800 ms ::  sel: 3, senderID: 4, payload: 0x30F
36900 ms ::  sel: 1, senderID: 5, payload: 0x2F0
36900 ms ::  sel: 1, senderID: 5, payload: 0x2F1
36900 ms ::  sel: 2, senderID: 3, payload: 0x300