RK0: The Real-Time Kernel '0'

VOID Task1(VOID* args)
{
    RK_UNUSEARGS
    UINT count = 0;
    while (1)
    {

       logPost("Task1 released.");

        count += 1U;
        if (count >= 5)
        {
            kDelay(25); /* spin */
            count=0;
        }

        kSleepRelease(300); /*P=300 ticks; tick=1ms*/


    }
}

/* Every 3rd call both tasks will add up a delay longer than the task's period */

VOID HTask(VOID* args) /* higher priority: Period is 300 ticks */
{
    RK_UNUSEARGS
    UINT count = 0;
    while (1)
    {

        logPost("Higher: begin\r\n");
        /* wake here */
        count += 1U;
        kDelay(5);
        if (count >= 3)
        {
            kSleep(400); /* suspend */
            count=0;
        }
        logPost("Higher: end\r\n");
        kSleepRelease(300);

    }
}


VOID LTask(VOID *args) /* lower priority: Period is 400 ticks */
{
    RK_UNUSEARGS
    RK_TICK anchor = kTickGet();
    UINT count=0;
    while (1)
    {

        logPost("Lower: begin\r\n");
        /* wake here */
        count += 1U;
        kDelay(5);
        if (count >= 3)
        {
            kSleep(500);
            count=0;
        }
        logPost("Lower: end\r\n");
        kSleepUntil(&anchor, 400);
    }
}

       0 ms :: Higher: begin
       5 ms :: Higher: end
       5 ms :: Lower: begin
      10 ms :: Lower: end
     300 ms :: Higher: begin
     305 ms :: Higher: end
     405 ms :: Lower: begin
     410 ms :: Lower: end

    /* H 3rd run, expected next at 900ms */
     600 ms :: Higher: begin


   /* L 3rd run, expected next at 1205 ms */
     805 ms :: Lower: begin

    /* H Drift: 1005ms - 600ms = 405 ms > 300ms */
    1005 ms :: Higher end

    /* H released again @ next multiple of 300. */
    1200 ms :: Higher: begin
    1205 ms :: Higher: end

   /* L Drift: 1310ms - 805ms = 505 ms > 400ms */
    1310 ms :: Lower: end
    /* it runs again  immediately */
    1310 ms :: Lower: begin

/* a ring buffer of items */
#define BUFSIZ (K)
static ITEM_t buf[BUFSIZ]={0};
static UINT getIdx = 0U;
static UINT putIdx = 0U;
/* getIdx==putIdx==0 could either mean FULL or EMPTY for a regular
circular buffer with wrap-around. When using semaphores they define the state.
*/

RK_SEMAPHORE  itemSema; /* counting semaphore for number of items in the buffer */
RK_SEMAPHORE  slotSema; /* counting semaphore for number of free slots in the buffer */
RK_SEMAPHORE  mutexSema; /* binary semaphore for mutual exclusion (it is not a LOCK, there is no ownership notion) */


VOID kApplicationInit(VOID)
{

  /*buffer is initialised empty */
    kSemaphoreInit
    (   &itemSema,
        0,   /* no item  */
        K,   /* max items */
        RK_WAIT_PRIORITY
    );

    kSemaphoreInit
    (   &slotSema,
        K, /* K free slots */
        K, /* max slots */
        RK_WAIT_PRIORITY
    );

    /* and free */
    kSemaphoreInit
    (   &mutexSema,
        1, /* free to access */
        1, /* 1 max task allowed */
        RK_WAIT_PRIORITY
    );


}

VOID PutItem(ITEM_t const * const insertItemPtr)
{
    RK_ERR err = -1;

    /* wait for room */
    err = kSemaphorePend(&slotSema, RK_WAIT_FOREVER);
    K_ASSERT(err == RK_ERR_SUCCESS);

    /* wait for availability */
    err = kSemaphorePend(&mutexSema,  RK_WAIT_FOREVER);
    K_ASSERT(err == RK_ERR_SUCCESS);
    buf[putIdx] = *insertItemPtr;
    putIdx += 1U;
    putIdx %= BUFSIZ;

    /* signal availability */
    err = kSemaphorePost(&mutexSema);
    K_ASSERT(err == RK_ERR_SUCCESS);

    /* signal item */
    err = kSemaphorePost(&itemSema);
    K_ASSERT(err == RK_ERR_SUCCESS);
}


 VOID GetItem(ITEM_t * const extractItemPtr)
{
    RK_ERR err = -1;
    /* wait for an item */
    err = kSemaphorePend(&itemSema, RK_WAIT_FOREVER);
    K_ASSERT(err == RK_ERR_SUCCESS);

    /* wait for availability */
    err = kSemaphorePend(&mutexSema,  RK_WAIT_FOREVER);
    K_ASSERT(err == RK_ERR_SUCCESS);

    *extractItemPtr = buf[getIdx];
    getIdx += 1U;
    getIdx %= BUFSIZ;

    /* signal availability */
    err = kSemaphorePost(&mutexSema);
    K_ASSERT(err == RK_ERR_SUCCESS);

    /* signal room */
    err = kSemaphorePost(&slotSema);
    K_ASSERT(err == RK_ERR_SUCCESS);
}

/*PSEUDOCODE*/

/* Binary Semaphores*/
SEMA work1Sema;
SEMA work2Sema;

/* Both binary semaphores are initialised to 0. */
AppInit():
BinarySemaInit(work1Sema, 0U);
BinarySemaInit(work2Sema, 0U);

/* Tasks synchronising so both proceed after work1 AND work2 completes */
Task1():
        while(TRUE)
        {

           work1();
           /* a V/post/signal on semaphore does not block */
           while(V(work1Sema) == RK_ERR_SEMA_FULL)
           {
                /* task2 hasnt consumed the token yet */
                kSleep(1);
           }  /* signal/post on work1sema */
           P(work2Sema);  /* wait/pend on work2sema */


Task2():
        while(TRUE)
        {
           work2();
           while(V(work2Sema) == RK_ERR_SEMA_FULL)
           {
                kSleep(1);
           }  /* signal/post on work1sema */
           P(work2Sema);

        }

/* 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_PRIO_INHERITANCE, RK_NO_CEILING);
    kSleepQueueInit(&barPtr->allSynch, RK_WAIT_FIFO);
    barPtr->count = 0;
    barPtr->round = 0;
    barPtr->nRequired = 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
    {
        /* sequence: 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);
            K_ASSERT(err==RK_ERR_SUCCESS);
        }
    }
    kMutexUnlock(&barPtr->lock);
}


#define N_REQUIRED 3

Barrier_t syncBarrier;

VOID kApplicationInit(VOID)
{

    K_ASSERT(!kTaskInit(&task1Handle, Task1, RK_NO_ARGS, "Task1", stack1, STACKSIZE, 2, RK_PREEMPT));
    K_ASSERT(!kTaskInit(&task2Handle, Task2, RK_NO_ARGS, "Task2", stack2, STACKSIZE, 3, RK_PREEMPT));
    K_ASSERT(!kTaskInit(&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_PRIO_INHERITANCE, RK_NO_CEILING);
    kSleepQueueInit(&rwLockPtr->writersGo, RK_WAIT_PRIORITY);
    kSleepQueueInit(&rwLockPtr->readersGo, RK_WAIT_PRIORITY);
    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;
    kSleepQueueQuery(&rwLockPtr->readersGo, &nWaitingReaders);
    if (nWaitingReaders > 0)
    {
        /* condVarBroadcast is just an alias for an event flush */
         kCondVarBroadcast(&rwLockPtr->readersGo);
    }
    else
    {
        /* wake up a single writer if any */
         kCondVarSignal(&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)
    {
         kCondVarSignal(&rwLockPtr->writersGo);
    }
    kMutexUnlock(&rwLockPtr->lock);
}

#define STATE_RECEIVERS 3U
#define STATE_BCAST_CEILING_PRIO 2U

/* Declare RK_MBOX stateMbox, backed by a single StateFrame slot. */
RK_DECLARE_MBOX(stateMbox, stateMboxBuf, StateFrame)

VOID kApplicationInit(VOID)
{
    RK_ERR err = kMboxInitCeiling(&stateMbox, stateMboxBuf,
                                  RK_MBOX_MESG_SIZE(StateFrame),
                                  STATE_BCAST_CEILING_PRIO);
    K_ASSERT(err == RK_ERR_SUCCESS);

    /* create the broadcaster and STATE_RECEIVERS receiver tasks */
}

VOID BroadcasterTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        UINT nReceivers = 0U;
        StateFrame frame = BuildStateFrame();

        RK_ERR err = kMboxBroadcastWaitN(&stateMbox, &frame,
                                         STATE_RECEIVERS,
                                         RK_WAIT_FOREVER,
                                         &nReceivers);
        K_ASSERT(err == RK_ERR_SUCCESS);
        logPost("broadcast to %u receivers", nReceivers);

        kSleepRelease(RK_MS_TO_TICKS(200));
    }
}

VOID ReceiverTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        StateFrame frame;
        RK_ERR err = kMboxBroadcastRecv(&stateMbox, &frame,
                                        RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);
        ProcessFrame(&frame);
    }
}

/* Application Logger pattern */

/* standard log structure */
struct log
{
    RK_TICK t; /* timestamp */
    CHAR s[LOGLEN]; /*formatted string */
    UINT    level; /* level 0=message, 1=fault */
} K_ALIGN(4);

typedef struct log Log_t;

/* logger mem allocator + mem pool */
static RK_MEM_PARTITION qMem;
static Log_t logBufPool[LOGPOOLSIZ] K_ALIGN(4);

/* backing buffer for the logger queue */
/* (messages are 1-word-size, number equals the pool) */
RK_DECLARE_MESG_QUEUE_BUF(logQBuf, VOID *, LOGPOOLSIZ)

/* logger mail queue */
static RK_MESG_QUEUE logQ;

/* a sender will allocate a buffer write the log
message and enqueue on the mail queue, not-blocking
if the queue is full it returns the buffer immediately
if the memory pool is empty it drops the operation */

```c
/* excerpt of logPost(...) */
VOID logPost(/*formatted string */)
    ---snip---
    Log_t *logPtr = (Log_t*)kMemPartitionAlloc(&qMem);
    RK_BARRIER
    if (logPtr) /* available buffer */
    {

       < fill the buffer >

        if (kMesgQueueSend(&logQ, &logPtr, RK_NO_WAIT) != RK_ERR_SUCCESS)
        {
            /* queue is full, deallocate buf */
            RK_ERR err = kMemPartitionFree(&qMem, logPtr);
            K_ASSERT(err==RK_ERR_SUCCESS);
        }

    }
    ---snip---

/* excerpt of the logger task */

static VOID LoggerTask(VOID *args)
{
    RK_UNUSEARGS
    while (1)
    {

        VOID *recvPtr = NULL;

        /* drain the queue: keep receiving while successful */
        while (kMesgQueueRecv(&logQ, &recvPtr, RK_WAIT_FOREVER) ==
               RK_ERR_SUCCESS)
        {
            < print buffer contents >

            /* deallocate */
            RK_ERR err = kMemPartitionFree(&qMem, recvPtr);
            K_ASSERT(err == RK_ERR_SUCCESS);

        }
    }
}

/* Many-to-1 queue channels */

/* Consumer Select queue based on its event
flags, that a succesfull send triggers  */

#define LOG_PRIORITY 4 /* keep logger as lowest-priority user task */
#define STACKSIZE 256

#define NQUEUES 3 /* number of queues */
#define QSIZ 8 /* depth of each queue */

#define Q0_FLAG   RK_EVENT_1 // (1<<0)
#define Q1_FLAG   RK_EVENT_2 // (1<<1)
#define Q2_FLAG   RK_EVENT_3 // (1<<2)
#define QFLAGS   (ULONG)(Q0_FLAG | Q1_FLAG | Q2_FLAG)

typedef struct
{
    RK_TASK_HANDLE producer;
    UINT payload;
} MESG_t;

/* Succesful Send callbacks */
/* each callback follows this pattern */
static inline
VOID sendNotify0(RK_MESG_QUEUE *qPtr)
{
    (VOID)qPtr;
    kTaskEventSet(consumerHandle, Q0_FLAG);
    /* Q1 flag for queue1 and so forth */
}

/* each callback in installed using kMesgQueueInstallSendCbk
on kApplicationInit() */

/* helper to send */
static inline
VOID enqueueSample(RK_MESG_QUEUE *qPtr UINT payload)
{
    MESG_t mesg = {
        .payload = payload,
        .producer = RK_RUNNING_HANDLE,
    };
    RK_ERR err = kMesgQueueSend(qPtr, &mesg, RK_WAIT_FOREVER);
    K_ASSERT(err == RK_ERR_SUCCESS);
}


VOID Prod0Task(VOID *args)
{
    RK_UNUSEARGS
    UINT seq = 0U;

    while (1)
    {
        enqueueSample(&queues[0], seq++);
        kSleepRelease(25); /* fast producer */
    }
}

VOID Prod1Task(VOID *args)
{
    UINT seq = 0U;
    RK_UNUSEARGS

    while (1)
    {
        enqueueSample(&queues[1], seq++);

        /* every fourth sample, also tickle the third queue */
        if ((seq & 0x3U) == 0U)
        {
            enqueueSample(&queues[2], seq);
        }

        kSleepRelease(60);
    }
}

/* Consumer listens on all queues, selecting those on its signal flags. */
VOID ConsumerTask(VOID *args)
{
    RK_UNUSEARGS

    MESG_t recv = {0};
    ULONG flags = 0UL;

    while (1)
    {
        flags = 0UL;
        kTaskEventGet(QFLAGS, RK_EVENT_FLAGS_ANY, &flags,
                           RK_WAIT_FOREVER);

        for (UINT i = 0; i < NQUEUES; ++i)
        {
            if (flags & (1UL << i))
            {
                while (kMesgQueueRecv(&queues[i], (VOID*)&recv, RK_NO_WAIT) ==
                       RK_ERR_SUCCESS)
                {
                    logPost("Q%u <- sender=%s payload=%u", i,
                            RK_TASK_NAME(recv.producer), recv.payload);
                }
            }
        }
    }
}

       0 ms :: Q1 <- sender=Prod1 payload=0
     250 ms :: Q0 <- sender=Prod0 payload=1
     500 ms :: Q0 <- sender=Prod0 payload=2
     600 ms :: Q1 <- sender=Prod1 payload=1
     750 ms :: Q0 <- sender=Prod0 payload=3
    1000 ms :: Q0 <- sender=Prod0 payload=4
    1200 ms :: Q1 <- sender=Prod1 payload=2
    1250 ms :: Q0 <- sender=Prod0 payload=5
    1500 ms :: Q0 <- sender=Prod0 payload=6
    1750 ms :: Q0 <- sender=Prod0 payload=7
    1800 ms :: Q1 <- sender=Prod1 payload=3
    1800 ms :: Q2 <- sender=Prod1 payload=4
    2000 ms :: Q0 <- sender=Prod0 payload=8
    2250 ms :: Q0 <- sender=Prod0 payload=9
    2400 ms :: Q1 <- sender=Prod1 payload=4
    2500 ms :: Q0 <- sender=Prod0 payload=10
    2750 ms :: Q0 <- sender=Prod0 payload=11
    3000 ms :: Q0 <- sender=Prod0 payload=12
    3000 ms :: Q1 <- sender=Prod1 payload=5
    3250 ms :: Q0 <- sender=Prod0 payload=13
    3500 ms :: Q0 <- sender=Prod0 payload=14
    3600 ms :: Q1 <- sender=Prod1 payload=6
    3750 ms :: Q0 <- sender=Prod0 payload=15
    4000 ms :: Q0 <- sender=Prod0 payload=16
    4200 ms :: Q1 <- sender=Prod1 payload=7
    4200 ms :: Q2 <- sender=Prod1 payload=8
    4250 ms :: Q0 <- sender=Prod0 payload=17
    4500 ms :: Q0 <- sender=Prod0 payload=18
    4750 ms :: Q0 <- sender=Prod0 payload=19
    4800 ms :: Q1 <- sender=Prod1 payload=8
    5000 ms :: Q0 <- sender=Prod0 payload=20
    5250 ms :: Q0 <- sender=Prod0 payload=21

#define STACKSIZE 192U

typedef struct
{
    UINT seq;
    ULONG value;
} Sample_t;

RK_DECLARE_TASK(producerHandle, ProducerTask, producerStack, STACKSIZE)
RK_DECLARE_TASK(controllerHandle, ControllerTask, controllerStack, STACKSIZE)

static Sample_t sample;
static RK_CHANNEL sampleChannel = {0};

extern VOID applyControl_(ULONG value);

VOID kApplicationInit(VOID)
{
    RK_ERR err = kTaskInit(&controllerHandle, ControllerTask, RK_NO_ARGS,
                             "Ctrl", controllerStack, STACKSIZE, 2,
                             RK_PREEMPT);
    K_ASSERT(err == RK_ERR_SUCCESS);

    err = kTaskInit(&producerHandle, ProducerTask, RK_NO_ARGS,
                      "Prod", producerStack, STACKSIZE, 2, RK_PREEMPT);
    K_ASSERT(err == RK_ERR_SUCCESS);

    err = kChannelInit(&sampleChannel, &controllerHandle, SYNCH_SEND);
    K_ASSERT(err == RK_ERR_SUCCESS);
}

VOID ProducerTask(VOID *args)
{
    RK_UNUSEARGS

    sample.seq = 0U;
    while (1)
    {
        sample.seq++;
        sample.value = 1000UL + sample.seq;

        /*
         * Send means send-and-wait-until-copied. The source storage remains
         * valid until kChannelSend() returns.
         */
        RK_ERR err = kChannelSend(&sampleChannel, &sample,
                                     sizeof(sample), RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);

        kSleep(RK_MS_TO_TICKS(100));
    }
}

VOID ControllerTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        Sample_t received = {0U, 0UL};
        ULONG mesgBytes = 0UL;
        RK_ERR err = kChannelRecv(&sampleChannel, &received, sizeof(received),
                                  &mesgBytes, RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);
        K_ASSERT(mesgBytes == sizeof(received));

        applyControl_(received.value);
    }
}

#define STACKSIZE 192U

typedef struct
{
    UINT opcode;
    ULONG value;
} Request_t;

typedef struct
{
    RK_ERR status;
    ULONG result;
} Reply_t;

RK_DECLARE_TASK(clientHandle, ClientTask, clientStack, STACKSIZE)
RK_DECLARE_TASK(serverHandle, ServerTask, serverStack, STACKSIZE)

static RK_CHANNEL serviceChannel = {0};

VOID kApplicationInit(VOID)
{
    RK_ERR err = kTaskInit(&serverHandle, ServerTask, RK_NO_ARGS,
                           "Srv", serverStack, STACKSIZE, 2, RK_PREEMPT);
    K_ASSERT(err == RK_ERR_SUCCESS);

    err = kTaskInit(&clientHandle, ClientTask, RK_NO_ARGS,
                    "Cli", clientStack, STACKSIZE, 3, RK_PREEMPT);
    K_ASSERT(err == RK_ERR_SUCCESS);

    err = kChannelInit(&serviceChannel, &serverHandle, SYNCH_INVOCATION);
    K_ASSERT(err == RK_ERR_SUCCESS);
}

VOID ClientTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        Request_t req = {1U, 42UL};
        Reply_t reply = {RK_ERR_SUCCESS, 0UL};
        ULONG replyBytes = 0UL;
        RK_CHANNEL_ATTR attr = {&req, sizeof(req), &reply,
                              sizeof(reply), &replyBytes};

        RK_ERR err = kChannelCall(&serviceChannel, &attr, RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);
        K_ASSERT(replyBytes == sizeof(reply));

        kSleep(RK_MS_TO_TICKS(100));
    }
}

VOID ServerTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        RK_CHANNEL_CALL_DATA call = {0};
        Request_t req = {0U, 0UL};
        ULONG reqBytes = 0UL;

        RK_ERR err = kChannelAccept(&serviceChannel, &call, &req, sizeof(req),
                                      &reqBytes,
                                      RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);
        K_ASSERT(reqBytes == sizeof(req));

        Reply_t reply = {RK_ERR_SUCCESS, req.value + 1UL};
        err = kChannelReply(&call, &reply, sizeof(reply));
        K_ASSERT(err == RK_ERR_SUCCESS);
    }
}

#define STACKSIZE 256U
#define ASYNC_POOL_DEPTH 4U

typedef struct
{
    UINT src;
    UINT seq;
    ULONG value;
} AsyncPayload_t;

RK_DECLARE_TASK(rxHandle, RxTask, rxStack, STACKSIZE)
RK_DECLARE_TASK(txHandle, TxTask, txStack, STACKSIZE)
RK_DECLARE_MESG_POOL(asyncPool, asyncPoolBuf,
                     AsyncPayload_t, ASYNC_POOL_DEPTH)

static RK_MESG_ENDPOINT rxEndpoint = {0};
static RK_MESG_ENDPOINT txEndpoint = {0};

extern VOID consume_(ULONG value);

VOID kApplicationInit(VOID)
{
    RK_ERR err = kTaskInit(&rxHandle, RxTask, RK_NO_ARGS,
                           "Rx", rxStack, STACKSIZE, 3, RK_PREEMPT);
    K_ASSERT(err == RK_ERR_SUCCESS);

    err = kTaskInit(&txHandle, TxTask, RK_NO_ARGS,
                    "Tx", txStack, STACKSIZE, 4, RK_PREEMPT);
    K_ASSERT(err == RK_ERR_SUCCESS);

    /*
     * Pool setup: ASYNC_POOL_DEPTH fixed-size RK_MESG blocks, each carrying one
     * AsyncPayload_t payload. The pool ceiling is 3, so admitted owners and
     * allocation waiters run at least at priority 3 until they leave the pool
     * dependency.
     */
    err = kMesgPoolInit(&asyncPool, asyncPoolBuf,
                        sizeof(AsyncPayload_t), ASYNC_POOL_DEPTH, 3);
    K_ASSERT(err == RK_ERR_SUCCESS);

    err = kMesgEndpointInit(&rxEndpoint, rxHandle, RK_MESG_SEND_RECV);
    K_ASSERT(err == RK_ERR_SUCCESS);
    err = kMesgEndpointInit(&txEndpoint, txHandle, RK_MESG_SEND_ONLY);
    K_ASSERT(err == RK_ERR_SUCCESS);
}

VOID TxTask(VOID *args)
{
    RK_UNUSEARGS

    UINT seq = 0U;
    while (1)
    {
        /*
         * Allocation: Tx waits here for pool availability. Send remains
         * nonblocking with respect to endpoint queue space.
         */
        RK_MESG *mesgPtr = NULL;
        RK_ERR err = kMesgAlloc(&asyncPool, &mesgPtr, RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);
        K_ASSERT(mesgPtr != NULL);

        /* Fill the typed payload behind the RK_MESG header. */
        AsyncPayload_t *payloadPtr =
            RK_MESG_PAYLOAD(mesgPtr, AsyncPayload_t);
        payloadPtr->src = 1U;
        payloadPtr->seq = ++seq;
        payloadPtr->value = 1000UL + seq;

        err = kMesgSend(rxHandle, mesgPtr);
        if (err != RK_ERR_SUCCESS)
        {
            /*
             * Send failure did not transfer ownership, so Tx must not leak the
             * block. Do not touch mesgPtr after a successful send.
             */
            RK_ERR freeErr = kMesgFree(mesgPtr);
            K_ASSERT(freeErr == RK_ERR_SUCCESS);
        }
        K_ASSERT(err == RK_ERR_SUCCESS);

        kSleep(RK_MS_TO_TICKS(100));
    }
}

VOID RxTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        RK_MESG *mesgPtr = NULL;

        /*
         * Receive: ANY accepts the first queued or future message from any sender.
         * On success, Rx owns mesgPtr and must eventually free it.
         */
        RK_ERR err = kMesgWait(RK_ANY_TASK, &mesgPtr, RK_WAIT_FOREVER);
        K_ASSERT(err == RK_ERR_SUCCESS);

        K_ASSERT(kMesgGetSenderHandle(mesgPtr) == txHandle);
        RK_PID senderID = 0U;
        err = kMesgGetSenderID(mesgPtr, &senderID);
        K_ASSERT(err == RK_ERR_SUCCESS);
        K_ASSERT(senderID == kTaskGetID(txHandle));

        AsyncPayload_t *payloadPtr =
            RK_MESG_PAYLOAD(mesgPtr, AsyncPayload_t);
        consume_(payloadPtr->value);

        /* Return the received message to its originating pool. */
        err = kMesgFree(mesgPtr);
        K_ASSERT(err == RK_ERR_SUCCESS);
    }
}

/* --- EXCERPT --- */

typedef struct
{
    LONG normalForceMN; /* normal contact force [mN] */
    LONG torqueMNm;     /* wrist torque around x [mNm] */
    UINT contact;       /* binary contact flag */
    RK_TICK tsMs;       /* sample timestamp [ms] */
} FORCE_MRM_MESG_t;

typedef struct
{
    LONG txMilli;   /* tangential x component in milli-units (1000 -> 1.0) */
    LONG tyMilli;   /* tangential y component in milli-units (1000 -> 1.0) */
    UINT strategy;  /* selected exploration strategy/phase */
    RK_TICK tsMs;   /* sample timestamp [ms] */
} PATH_MRM_MESG_t;

typedef struct
{
    LONG vxMmps; /* commanded tangential velocity x [mm/s] */
    LONG vyMmps; /* commanded tangential velocity y [mm/s] */
    LONG vzMmps; /* commanded normal velocity from force loop [mm/s] */
    LONG xMm;    /* integrated x position [mm] */
    LONG yMm;    /* integrated y position [mm] */
    LONG zMm;    /* integrated z position [mm] */
} ROBOT_CMD_MESG_t;

/* Two shared MRM channels: latest force and latest path guidance. */
static RK_MRM mrmForce;
static RK_MRM mrmPath;


/* priority assigned higher for the shortest period */
#define PERIOD_FORCE_MS 20U /* highest critical */

#define PERIOD_CONTROL_MS 28U /* middle critical */

#define PERIOD_VISION_MS 80U /* lowest critical */

#define PERIOD_DISPLAY_MS 100U /* soft real-time task */

VOID ForceTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        /*  acquire simulated force sample and publish latest  */
        RK_TICK nowMs = kTickGetMs();
        FORCE_MRM_MESG_t mesg = simulateForceSensor_(nowMs);
        RK_ERR err = publishLatest_(&mrmForce, &mesg);
        K_ASSERT(err == RK_ERR_SUCCESS);

        logPost("[FORCE ] Fn=%ldmN T=%ldmNm contact=%u", mesg.normalForceMN,
                mesg.torqueMNm, mesg.contact);

        sleepHardTask_(periodForceTicks, "FORCE");
    }
}

VOID VisionTask(VOID *args)
{
    RK_UNUSEARGS

    while (1)
    {
        /*  compute next tangential direction and publish latest one */
        RK_TICK nowMs = kTickGetMs();
        PATH_MRM_MESG_t mesg = simulatePathFromVision_(nowMs);
        RK_ERR err = publishLatest_(&mrmPath, &mesg);
        K_ASSERT(err == RK_ERR_SUCCESS);

        logPost("[VISION] path=(%ld,%ld) strategy=%u", mesg.txMilli,
                mesg.tyMilli, mesg.strategy);

        sleepHardTask_(periodVisionTicks, "VISION");
    }
}

VOID ControlTask(VOID *args)
{
    RK_UNUSEARGS
    ROBOT_CMD_MESG_t cmd = {0};

    while (1)
    {
        FORCE_MRM_MESG_t forceMsg;
        PATH_MRM_MESG_t pathMsg;

        /*  read latest force/path snapshots  */
        RK_MRM_BUF *forceBufPtr = kMRMGet(&mrmForce, &forceMsg);
        RK_MRM_BUF *pathBufPtr = kMRMGet(&mrmPath, &pathMsg);
        while (forceBufPtr == NULL || pathBufPtr == NULL)
        {
            /*   wait until initial samples are available. */
            forceBufPtr = kMRMGet(&mrmForce, &forceMsg);
            pathBufPtr = kMRMGet(&mrmPath, &pathMsg);

            kSleep(1);
            continue;
        }
        kMRMUnget(&mrmForce, forceBufPtr);
        kMRMUnget(&mrmPath, pathBufPtr);

        /* normal-axis loop: force error -> bounded z velocity command. */
        LONG forceErr = FORCE_DES_MN - forceMsg.normalForceMN;
        LONG vzMmps = clampLong_(forceErr / 20L, -60L, 60L);

        /* tangential motion from vision direction vector in milli-units. */
        LONG vxCmd = (pathMsg.txMilli * TANGENTIAL_VEL_MMPS) / 1000L;
        LONG vyCmd = (pathMsg.tyMilli * TANGENTIAL_VEL_MMPS) / 1000L;

        if (forceMsg.contact == 0U)
        {
            /* No contact: stop tangential exploration and approach surface. */
            vxCmd = 0L;
            vyCmd = 0L;
            vzMmps = 30L;
        }

        cmd.vxMmps = vxCmd;
        cmd.vyMmps = vyCmd;
        cmd.vzMmps = vzMmps;

        /*  commanded velocities to a simple discrete position estimate. */
        cmd.xMm += (vxCmd * (LONG)periodControlEffMs) / 1000L;
        cmd.yMm += (vyCmd * (LONG)periodControlEffMs) / 1000L;
        cmd.zMm += (vzMmps * (LONG)periodControlEffMs) / 1000L;

        /* sim sensor closed-loop : higher positive vzMmps raises normal force. */
        gSimNormalForceMN = clampLong_(
            gSimNormalForceMN + (vzMmps * (LONG)periodControlEffMs) / 2L,
            0L,
            12000L);

        logPost("[CONTROL] v=(%ld,%ld) vz=%ld Fn=%ld err=%ld pos=(%ld,%ld,%ld)",
                cmd.vxMmps, cmd.vyMmps, cmd.vzMmps, forceMsg.normalForceMN,
                forceErr, cmd.xMm, cmd.yMm, cmd.zMm);

        sleepHardTask_(periodControlTicks, "CONTROL");
    }
}

VOID DisplayTask(VOID *args)
{
    RK_UNUSEARGS

    RK_TICK anchor = kTickGet();
    while (1)
    {
        FORCE_MRM_MESG_t forceMsg;
        PATH_MRM_MESG_t pathMsg;

        /*  less critical task: consume latest state for telemetry/visualization only. */
        RK_MRM_BUF *forceBufPtr = kMRMGet(&mrmForce, &forceMsg);
        RK_MRM_BUF *pathBufPtr = kMRMGet(&mrmPath, &pathMsg);


        kMRMUnget(&mrmForce, forceBufPtr);
        kMRMUnget(&mrmPath, pathBufPtr);

        logPost("[DISPLAY] Fn=%ld path=(%ld,%ld) ts=(%lu,%lu)",
                forceMsg.normalForceMN, pathMsg.txMilli, pathMsg.tyMilli,
                (ULONG)forceMsg.tsMs, (ULONG)pathMsg.tsMs);

        kSleepUntil(&anchor, periodDisplayTicks);
    }
}

      0 ms :: [FORCE ] Fn=4300mN T=-350mNm contact=1
       0 ms :: [VISION] path=(1000,0) strategy=0
       0 ms :: [DISPLAY] Fn=4300 path=(1000,0) ts=(0,0)
       2 ms :: [CONTROL] v=(80,0) vz=35 Fn=4300 err=700 pos=(2,0,0)
      20 ms :: [FORCE ] Fn=4790mN T=-350mNm contact=1
      28 ms :: [CONTROL] v=(80,0) vz=10 Fn=4790 err=210 pos=(4,0,0)
      40 ms :: [FORCE ] Fn=5000mN T=-315mNm contact=1
      56 ms :: [CONTROL] v=(80,0) vz=0 Fn=5000 err=0 pos=(6,0,0)
      60 ms :: [FORCE ] Fn=5000mN T=-315mNm contact=1
      80 ms :: [FORCE ] Fn=5070mN T=-280mNm contact=1
      80 ms :: [VISION] path=(1000,0) strategy=0
      84 ms :: [CONTROL] v=(80,0) vz=-3 Fn=5070 err=-70 pos=(8,0,0)
     100 ms :: [FORCE ] Fn=5028mN T=-280mNm contact=1
     100 ms :: [DISPLAY] Fn=5028 path=(1000,0) ts=(100,80)
     112 ms :: [CONTROL] v=(80,0) vz=-1 Fn=5028 err=-28 pos=(10,0,0)
     120 ms :: [FORCE ] Fn=5084mN T=-245mNm contact=1
     140 ms :: [FORCE ] Fn=5084mN T=-245mNm contact=1
     140 ms :: [CONTROL] v=(80,0) vz=-4 Fn=5084 err=-84 pos=(12,0,0)
     160 ms :: [FORCE ] Fn=5098mN T=-210mNm contact=1
     160 ms :: [VISION] path=(1000,0) strategy=0
     168 ms :: [CONTROL] v=(80,0) vz=-4 Fn=5098 err=-98 pos=(14,0,0)
     180 ms :: [FORCE ] Fn=5042mN T=-210mNm contact=1
     196 ms :: [CONTROL] v=(80,0) vz=-2 Fn=5042 err=-42 pos=(16,0,0)
     200 ms :: [FORCE ] Fn=5084mN T=-175mNm contact=1
     200 ms :: [DISPLAY] Fn=5084 path=(1000,0) ts=(200,160)
     220 ms :: [FORCE ] Fn=5084mN T=-175mNm contact=1
     224 ms :: [CONTROL] v=(80,0) vz=-4 Fn=5084 err=-84 pos=(18,0,0)
     240 ms :: [FORCE ] Fn=5098mN T=-140mNm contact=1
     240 ms :: [VISION] path=(1000,0) strategy=0
     252 ms :: [CONTROL] v=(80,0) vz=-4 Fn=5098 err=-98 pos=(20,0,0)
     260 ms :: [FORCE ] Fn=5042mN T=-140mNm contact=1
     280 ms :: [FORCE ] Fn=5112mN T=-105mNm contact=1
     280 ms :: [CONTROL] v=(80,0) vz=-5 Fn=5112 err=-112 pos=(22,0,0)
     300 ms :: [FORCE ] Fn=5042mN T=-105mNm contact=1
     300 ms :: [DISPLAY] Fn=5042 path=(1000,0) ts=(300,240)
     308 ms :: [CONTROL] v=(80,0) vz=-2 Fn=5042 err=-42 pos=(24,0,0)

    .
    .
    .

   30800 ms :: [VISION] path=(0,1000) strategy=1
   30800 ms :: [DISPLAY] Fn=5112 path=(0,1000) ts=(30800,30800)
   30820 ms :: [FORCE ] Fn=5042mN T=140mNm contact=1
   30828 ms :: [CONTROL] v=(0,80) vz=-2 Fn=5042 err=-42 pos=(1116,1088,36)
   30840 ms :: [FORCE ] Fn=5084mN T=175mNm contact=1
   30856 ms :: [CONTROL] v=(0,80) vz=-4 Fn=5084 err=-84 pos=(1116,1090,36)
   30860 ms :: [FORCE ] Fn=5028mN T=175mNm contact=1
   30880 ms :: [FORCE ] Fn=5098mN T=210mNm contact=1
   30880 ms :: [VISION] path=(0,1000) strategy=1
   30884 ms :: [CONTROL] v=(0,80) vz=-4 Fn=5098 err=-98 pos=(1116,1092,36)
   30900 ms :: [FORCE ] Fn=5042mN T=210mNm contact=1
   30900 ms :: [DISPLAY] Fn=5042 path=(0,1000) ts=(30900,30880)
   30912 ms :: [CONTROL] v=(0,80) vz=-2 Fn=5042 err=-42 pos=(1116,1094,36)
   30920 ms :: [FORCE ] Fn=5084mN T=245mNm contact=1
   30940 ms :: [FORCE ] Fn=5084mN T=245mNm contact=1
   30940 ms :: [CONTROL] v=(0,80) vz=-4 Fn=5084 err=-84 pos=(1116,1096,36)
   30960 ms :: [FORCE ] Fn=5098mN T=280mNm contact=1