NASA cFS POSIX message queue latency benchmarking

mq_bench_app.c

#include <time.h>
#include <signal.h>
#include "sample1_app.h"
#include "sample1_app_events.h"
#include "sample1_app_msgids.h"
#include "sample1_app_perfids.h"
#include "sample1_app_version.h"
#include "sample1_data.h"

#include "app_lib.h"
#include "cfe.h"

#define APP_ERR 0xffffffff
#define NR_REC 17000000

typedef struct __attribute__((aligned(32)))
{
    uint8    TlmHeader[CFE_SB_TLM_HDR_SIZE];
    /*
    ** Task command interface counters...
    */
    uint8    CmdCounter;
    uint8    ErrCounter;

    struct timespec ts; /* timestamp */
} MQ_BenchMsg_t;

MQ_BenchMsg_t msg;


/*
** static global data
*/
static App_Handle_t handle;
static CFE_SB_PipeId_t SB_pipe;

static uint32 ChildTaskId;

void RecvTask(void)
{
    uint32 Status;
    uint32 idx = 0;

    Status = CFE_ES_RegisterChildTask();
    if (Status != CFE_SUCCESS) {
        OS_printf("%s: Error resigtering child task.\n", __func__);
        return;
    }

    /* XXX Arguments in the following 2 API calls affect the queue
     * performance a lot.
     */

    /* The 2nd param refers to POSIX pipe depth, which is 10 by default.
     * See mq_overview(7).
     */
    Status = CFE_SB_CreatePipe(&SB_pipe,
                               10,
                               "MQ_BENCH_PIPE");
    if (Status != CFE_SUCCESS) {
        OS_printf("%s: Error creating SB pipe.\n", __func__);
        return;
    }

    /* the -ex version of this API provides QoS param, it is however not
     * impl-ed yet per the doc version 5.4 page 55.
     *
     * The 4th param limits the amount of messages w/ this msg ID to stay
     * this pipe.
     */
    //Status = CFE_SB_Subscribe(0x1234, /* message ID */
    //                          SB_pipe);
    Status = CFE_SB_SubscribeEx(0x1234, /* message ID */
                                SB_pipe,
                                CFE_SB_Default_Qos,
                                4);

    if (Status != CFE_SUCCESS) {
        OS_printf("%s: Error subscribing SB message.\n", __func__);
        return;
    }

    OS_printf("MQ_BENCH: child started.\n");

    for (;;) {
        MQ_BenchMsg_t *msg;
        struct timespec ts;

        CFE_SB_RcvMsg((CFE_SB_MsgPtr_t*) &msg, SB_pipe, CFE_SB_PEND_FOREVER);
        clock_gettime(CLOCK_MONOTONIC, &ts);

        if (idx++ != NR_REC) {
            /* delta[idx++] = ((ts.tv_sec * 1000000000) + ts.tv_nsec) -
                              ((msg->ts.tv_sec * 1000000000) + msg->ts.tv_nsec));
            */
            fprintf(stderr,"%ld\n", ((ts.tv_sec * 1000000000) + ts.tv_nsec) -
                           ((msg->ts.tv_sec * 1000000000) + msg->ts.tv_nsec));
        } else
            break;
    }
    exit(0);
}

void MQ_BENCH_AppMain(void)
{
    uint32 RunStatus;

    CFE_ES_PerfLogEntry(SAMPLE1_APP_PERF_ID);

    if ((handle = SAMPLE1_AppInit()) == APP_HANDLE_INVALID) {
        CFE_ES_PerfLogExit(SAMPLE1_APP_PERF_ID);
        OS_printf("%s: Init Fail.\n", __func__);
        goto err_exit;
    }

    RunStatus = CFE_ES_CreateChildTask(
        &ChildTaskId, "MQ_BENCH_RECV_TASK", RecvTask, NULL, 4096,
        99 /* XXX should be the equal or less than that of the parent */,
        0  /* flag, which currently has no impl*/);
    if (RunStatus != CFE_SUCCESS) {
        OS_printf("%s: Error creating child task.\n", __func__);
        goto err_exit;
    }

    CFE_SB_InitMsg(&msg,
                   0x1234, /* msg ID */
                   sizeof(MQ_BenchMsg_t),
                   TRUE);

    OS_printf("MQ_BENCH: started. sz: %u, payload: %u\n", CFE_SB_TLM_HDR_SIZE,
              sizeof(MQ_BenchMsg_t));

    /* walk the buffer beforehand */
    /* for (int i = 0; i < NR_REC; i++)
        delta[i] = 0; */

    //RunStatus = App_LoopRun(handle, SAMPLE1_APP_PERF_ID);
    for(;;) {
        /* using raw call to reduce possible overhead of timekeeping API of cFS */
        clock_gettime(CLOCK_MONOTONIC, &msg.ts);

        /* per the doc, this is a non-blocking call except that the receiver
           has a higher prio. than that of the sender */
        CFE_SB_SendMsg((CFE_SB_Msg_t *) &msg);
    }

    CFE_ES_ExitApp(RunStatus);
    return;

err_exit:
    RunStatus = CFE_ES_APP_ERROR;
    CFE_ES_ExitApp(RunStatus);
    return;
}

App_Handle_t SAMPLE1_AppInit(void)
{
    /*
    ** Register the app with Executive services
    */
    CFE_ES_RegisterApp();
    return 0;
}