Trace function

Example implementation of a trace function.

The function traces PxTrace() call arguments alongside the timestamp when the trace entry is added to the buffer. The atomic reading of the STM is used to get a 64-bit value timestamp. The trace function first checks if the current task should be traced or not. If not, the function still traces the Enter scheduling service as a special case to trace task scheduling. If the task should be traced, the entry is added in the following two cases:

the call is done by the user
the PXROS-HR service should be traced

The user argument of the trace entry is used to mark that the PxTrace() call of the PXROS-HR services was done in a handler.

#include "bsp.h"
#include "pxdef.h"
#include "pxtrace_internal.h"
#include "pxservices.h"

#define PXSV_USER   300

typedef struct
{
    PxUShort_t Type; /* Service type */
    PxUShort_t From; /* Task ID that called PxTrace */
    PxUShort_t Error; /* Error passed to PxTrace */
    PxUShort_t UserArg; /* User-defined argument */
    PxULong_t Data1; /* Argument 1 to PxTrace */
    PxULong_t Data2; /* Argument 2 to PxTrace */
} TraceUserEntry_t;

typedef struct
{
    unsigned long long timestamp;
    TraceUserEntry_t traceUserEntry;
} TraceEntry_t;

/**************************************************************************************
 * _IPxvGetCurrentTime
 *
 * Description
 *    Atomic time reading function from the STM. The time stamp is 64bit value
 *************************************************************************************/
__attribute__ ((always_inline)) static inline long long _IPxvGetCurrentTime()
{
    union
    {
        struct
        {
            PxUInt_t lo;
            PxUInt_t up;
        } u;
        PxUInt64_t ll;
    } timestamp;

    timestamp.u.lo = STM0_TIM0.U;
    timestamp.u.up = STM0_CAP.U;

    return timestamp.ll;
}

/**************************************************************************************
 * traceAddEntry
 *
 * Description
 *    Adds a new entry log into the trace buffer, Time stamp and user entry data
 *************************************************************************************/
__attribute__ ((always_inline)) static inline void traceAddEntry(
    TraceUserEntry_t userentry)
{
    /* Get the next trace entry */
    TraceEntry_t *PxvEntry = (TraceEntry_t*) IPxTraceGetNextEntry();

    if (PxvEntry != 0)
    {
        /* Adds the time stamp to buffer entry */
        PxvEntry->timestamp = _IPxvGetCurrentTime();
        /* Adds the user data to buffer entry */
        PxvEntry->traceUserEntry = userentry;
    }
}

/**************************************************************************************
 * Trace function
 *
 * Description
 *    The function adds the log/report into the trace buffer.
 *************************************************************************************/
void traceFunc(PxInt_t svType, PxArg_t Arg1, PxArg_t Arg2, PxError_t err)
{
    PxBool_t addEntry = 0;
    TraceUserEntry_t userentry;
    PxTask_t runtask = IPxTraceGetRunTaskID();
    PxUInt_t Task = runtask.id;
    PxUShort_t userArg = 0;
    PxBool_t traceRuntask = IPxTraceCheckTask(runtask);
    PxULong_t GroupMask = IPxTraceGetGroupMask();

    /* Should the PxTrace call from the task be traced?
     * The case for Scheduling events will be treated in the else case */
    if (traceRuntask)
    {
        /* Check if the call is done by a user */
        if (svType >= PXSV_USER)
        {
            /* Check if user PxTrace call should be traced */
            if (GroupMask & PXVT_USER)
            {
                addEntry = true;
            }
        }
        else
        {
            /* Check if the service should be traced */
            if (IPxTraceCheckService(svType))
            {
                /* Is the call coming from a handler? */
                if (PxTaskIdIsValid(IPxTraceGetHandlerTaskID()))
                {
                    userArg = 1; // mark that the call is done by a handler
                }

                addEntry = true;
            }
        }
    }
    else
    {
        /* Special case */
        if (svType == PXSvTraceEnterScheduling)
        {
            addEntry = true;
        }
    }

    if (addEntry)
    {
        userentry.Type = (PxUShort_t) svType;
        userentry.From = (PxUShort_t) Task;
        userentry.Error = (PxUShort_t) err;
        userentry.UserArg = (PxUShort_t) userArg;
        userentry.Data1 = (PxULong_t) Arg1;
        userentry.Data2 = (PxULong_t) Arg2;

        traceAddEntry(userentry);
    }
}