hpm6750/controller_yy_app/middleware/CANopenNode/extra/CO_trace.c

/*
 * CANopen trace interface.
 *
 * @file        CO_trace.c
 * @author      Janez Paternoster
 * @copyright   2016 - 2020 Janez Paternoster
 *
 * This file is part of CANopenNode, an opensource CANopen Stack.
 * Project home page is <https://github.com/CANopenNode/CANopenNode>.
 * For more information on CANopen see <http://www.can-cia.org/>.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "extra/CO_trace.h"

#if (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE

#include <stdio.h>
#include <string.h>
#if !((CO_CONFIG_TRACE) & CO_CONFIG_TRACE_OWN_INTTYPES)
#include <inttypes.h> /* for PRIu32("u" or "lu") and PRId32("d" or "ld") */
#endif

/* Different functions for processing value for different data types. */
static int32_t getValueI8 (void *OD_variable) { return (int32_t) *((int8_t*)   OD_variable);}
static int32_t getValueI16(void *OD_variable) { return (int32_t) *((int16_t*)  OD_variable);}
static int32_t getValueI32(void *OD_variable) { return           *((int32_t*)  OD_variable);}
static int32_t getValueU8 (void *OD_variable) { return (int32_t) *((uint8_t*)  OD_variable);}
static int32_t getValueU16(void *OD_variable) { return (int32_t) *((uint16_t*) OD_variable);}
static int32_t getValueU32(void *OD_variable) { return           *((int32_t*)  OD_variable);}


/* Different functions for printing points for different data types. */
static uint32_t printPointCsv(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    return snprintf(s, size, "%" PRIu32 ";%" PRId32 "\n", timeStamp,              value);
}
static uint32_t printPointCsvUnsigned(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    return snprintf(s, size, "%" PRIu32 ";%" PRIu32 "\n", timeStamp, (uint32_t)   value);
}
static uint32_t printPointBinary(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    if (size < 8) return 0;
    uint32_t timeStampSw = CO_SWAP_32(timeStamp);
    int32_t valueSw = CO_SWAP_32(value);
    memcpy(s, &timeStampSw, sizeof(timeStampSw));
    memcpy(s+4, &valueSw, sizeof(valueSw));
    return 8;
}
static uint32_t printPointSvgStart(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    return snprintf(s, size, "M%" PRIu32 ",%" PRId32, timeStamp,             value);
}
static uint32_t printPointSvgStartUnsigned(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    return snprintf(s, size, "M%" PRIu32 ",%" PRIu32, timeStamp, (uint32_t)  value);
}
static uint32_t printPointSvg(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    return snprintf(s, size, "H%" PRIu32 "V%" PRId32, timeStamp,             value);
}
static uint32_t printPointSvgUnsigned(char *s, uint32_t size, uint32_t timeStamp, int32_t value) {
    return snprintf(s, size, "H%" PRIu32 "V%" PRIu32, timeStamp, (uint32_t)  value);
}


/* Collection of function pointers for fast processing based on specific data type. */
/* Rules for the array: There must be groups of six members (I8, I16, I32, U8, U16, U32)
 * in correct order and sequence, so findVariable() finds correct member. */
static const CO_trace_dataType_t dataTypes[] = {
    {getValueI8,  printPointCsv,              printPointCsv,         printPointCsv},
    {getValueI16, printPointCsv,              printPointCsv,         printPointCsv},
    {getValueI32, printPointCsv,              printPointCsv,         printPointCsv},
    {getValueU8,  printPointCsvUnsigned,      printPointCsvUnsigned, printPointCsvUnsigned},
    {getValueU16, printPointCsvUnsigned,      printPointCsvUnsigned, printPointCsvUnsigned},
    {getValueU32, printPointCsvUnsigned,      printPointCsvUnsigned, printPointCsvUnsigned},
    {getValueI8,  printPointBinary,           printPointBinary,      printPointBinary},
    {getValueI16, printPointBinary,           printPointBinary,      printPointBinary},
    {getValueI32, printPointBinary,           printPointBinary,      printPointBinary},
    {getValueU8,  printPointBinary,           printPointBinary,      printPointBinary},
    {getValueU16, printPointBinary,           printPointBinary,      printPointBinary},
    {getValueU32, printPointBinary,           printPointBinary,      printPointBinary},
    {getValueI8,  printPointSvgStart,         printPointSvg,         printPointSvg},
    {getValueI16, printPointSvgStart,         printPointSvg,         printPointSvg},
    {getValueI32, printPointSvgStart,         printPointSvg,         printPointSvg},
    {getValueU8,  printPointSvgStartUnsigned, printPointSvgUnsigned, printPointSvgUnsigned},
    {getValueU16, printPointSvgStartUnsigned, printPointSvgUnsigned, printPointSvgUnsigned},
    {getValueU32, printPointSvgStartUnsigned, printPointSvgUnsigned, printPointSvgUnsigned}
};


/* Find variable in Object Dictionary *****************************************/
static void findVariable(CO_trace_t *trace) {
    bool_t err = false;
    uint16_t index;
    uint8_t subIndex;
    uint8_t dataLen;
    void *OdDataPtr = NULL;
    unsigned dtIndex = 0;

    /* parse mapping */
    index = (uint16_t) ((*trace->map) >> 16);
    subIndex = (uint8_t) ((*trace->map) >> 8);
    dataLen = (uint8_t) (*trace->map);
    if ((dataLen & 0x07) != 0) { /* data length must be byte aligned */
        err = true;
    }
    dataLen >>= 3;   /* in bytes now */
    if (dataLen == 0) {
        dataLen = 4;
    }

    /* find mapped variable, if map available */
    if (!err && (index != 0 || subIndex != 0)) {
        uint16_t entryNo = CO_OD_find(trace->SDO, index);

        if (index >= 0x1000 && entryNo != 0xFFFF && subIndex <= trace->SDO->OD[entryNo].maxSubIndex) {
            OdDataPtr = CO_OD_getDataPointer(trace->SDO, entryNo, subIndex);
        }

        if (OdDataPtr != NULL) {
            uint16_t len = CO_OD_getLength(trace->SDO, entryNo, subIndex);

            if (len < dataLen) {
                dataLen = len;
            }
        }
        else {
            err = true;
        }
    }

    /* Get function pointers for correct data type */
    if (!err) {
        /* first sequence: data length */
        switch(dataLen) {
            case 1: dtIndex = 0; break;
            case 2: dtIndex = 1; break;
            case 4: dtIndex = 2; break;
            default: err = true; break;
        }
        /* second sequence: signed or unsigned */
        if (((*trace->format) & 1) == 1) {
            dtIndex += 3;
        }
        /* third sequence: Output type */
        dtIndex += ((*trace->format) >> 1) * 6;

        if (dtIndex > (sizeof(dataTypes) / sizeof(CO_trace_dataType_t))) {
            err = true;
        }
    }

    /* set output variables */
    if (!err) {
        if (OdDataPtr != NULL) {
            trace->OD_variable = OdDataPtr;
        }
        else {
            trace->OD_variable = trace->value;
        }
        trace->dt = &dataTypes[dtIndex];
    }
    else  {
        trace->OD_variable = NULL;
        trace->dt = NULL;
    }
}


/* OD function for accessing _OD_traceConfig_ (index 0x2300+) from SDO server.
 * For more information see file CO_SDOserver.h. */
static CO_SDO_abortCode_t CO_ODF_traceConfig(CO_ODF_arg_t *ODF_arg) {
    CO_trace_t *trace;
    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;

    trace = (CO_trace_t*) ODF_arg->object;

    switch(ODF_arg->subIndex) {
    case 1:     /* size */
        if (ODF_arg->reading) {
            CO_setUint32(ODF_arg->data, trace->bufferSize);
        }
        break;

    case 2:     /* axisNo (trace enabled if nonzero) */
        if (ODF_arg->reading) {
            uint8_t *value = (uint8_t*) ODF_arg->data;
            if (!trace->enabled) {
                *value = 0;
            }
        }
        else {
            uint8_t *value = (uint8_t*) ODF_arg->data;

            if (*value == 0) {
                trace->enabled = false;
            }
            else if (!trace->enabled) {
                if (trace->bufferSize == 0) {
                    ret = CO_SDO_AB_OUT_OF_MEM;
                }
                else {
                    /* set trace->OD_variable and trace->dt, based on 'map' and 'format' */
                    findVariable(trace);

                    if (trace->OD_variable != NULL) {
                        *trace->value = 0;
                        *trace->minValue = 0;
                        *trace->maxValue = 0;
                        *trace->triggerTime = 0;
                        trace->valuePrev = 0;
                        trace->readPtr = 0;
                        trace->writePtr = 0;
                        trace->enabled = true;
                    }
                    else {
                        ret = CO_SDO_AB_NO_MAP;
                    }
                }
            }
        }
        break;

    case 5:     /* map */
    case 6:     /* format */
        if (!ODF_arg->reading) {
            if (trace->enabled) {
                ret = CO_SDO_AB_INVALID_VALUE;
            }
        }
        break;
    }

    return ret;
}


/* OD function for accessing _OD_trace_ (index 0x2400+) from SDO server.
 * For more information see file CO_SDOserver.h. */
static CO_SDO_abortCode_t CO_ODF_trace(CO_ODF_arg_t *ODF_arg) {
    CO_trace_t *trace;
    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;

    trace = (CO_trace_t*) ODF_arg->object;

    switch(ODF_arg->subIndex) {
    case 1:     /* size */
        if (ODF_arg->reading) {
            uint32_t size = trace->bufferSize;
            uint32_t wp = trace->writePtr;
            uint32_t rp = trace->readPtr;

            if (wp >= rp) {
                CO_setUint32(ODF_arg->data,  wp - rp);
            }
            else {
                CO_setUint32(ODF_arg->data, size - rp + wp);
            }
        }
        else {
            if (CO_getUint32(ODF_arg->data) == 0) {
                /* clear buffer, handle race conditions */
                while(trace->readPtr != 0 || trace->writePtr != 0) {
                    trace->readPtr = 0;
                    trace->writePtr = 0;
                    *trace->triggerTime = 0;
                }
            }
            else {
                ret = CO_SDO_AB_INVALID_VALUE;
            }
        }
        break;

    case 5:     /* plot */
        if (ODF_arg->reading) {
            /* This plot will be transmitted as domain data type. String data
             * will be printed directly to SDO buffer. If there is more data
             * to print, than is the size of SDO buffer, then this function
             * will be called multiple times until internal trace buffer is
             * empty. Internal trace buffer is circular buffer. It is accessed
             * by this function and by higher priority thread. If this buffer
             * is full, there is a danger for race condition. First records
             * from trace buffer may be overwritten somewhere between. If this
             * is detected, then do{}while() loop tries printing again. */
            if (trace->bufferSize == 0 || ODF_arg->dataLength < 100) {
                ret = CO_SDO_AB_OUT_OF_MEM;
            }
            else if (trace->readPtr == trace->writePtr) {
                ret = CO_SDO_AB_NO_DATA;
            }
            else {
                uint32_t rp, t, v, len, freeLen;
                char *s;
                bool_t readPtrOverflowed; /* for handling race conditions */

                /* repeat everything, if trace->readPtr was overflowed in CO_trace_process */
                do {
                    readPtrOverflowed = false;
                    s = (char*) ODF_arg->data;
                    freeLen = ODF_arg->dataLength;

                    rp = trace->readPtr;

                    /* start plot, increment variables, verify overflow */
                    if (ODF_arg->firstSegment) {
                        t = trace->timeBuffer[rp];
                        v = trace->valueBuffer[rp];
                        rp ++;
                        if (++trace->readPtr == trace->bufferSize) {
                            trace->readPtr = 0;
                            if (rp != trace->bufferSize) {
                                readPtrOverflowed = true;
                                continue;
                            }
                            rp = 0;
                        }
                        if (rp != trace->readPtr) {
                            readPtrOverflowed = true;
                            continue;
                        }
                        len = trace->dt->printPointStart(s, freeLen, t, v);
                        s += len;
                        freeLen -= len;
                    }

                    /* print other points */
                    if (rp != trace->writePtr) {
                    for(;;) {
                        t = trace->timeBuffer[rp];
                        v = trace->valueBuffer[rp];
                        rp ++;
                        if (++trace->readPtr == trace->bufferSize) {
                            trace->readPtr = 0;
                            if (rp != trace->bufferSize && ODF_arg->firstSegment) {
                                readPtrOverflowed = true;
                                break;
                            }
                            rp = 0;
                        }
                        if (rp != trace->readPtr && ODF_arg->firstSegment) {
                            readPtrOverflowed = true;
                            break;
                        }

                        /* If internal buffer is empty, end transfer */
                        if (rp == trace->writePtr) {
                            /* If there is last time stamp, point will be printed at the end */
                            if (t != trace->lastTimeStamp) {
                                len = trace->dt->printPoint(s, freeLen, t, v);
                                s += len;
                                freeLen -= len;
                            }
                            ODF_arg->lastSegment = true;
                            break;
                        }
                        len = trace->dt->printPoint(s, freeLen, t, v);
                        s += len;
                        freeLen -= len;

                        /* if output buffer is full, next data will be sent later */
                        if (freeLen < 50) {
                            ODF_arg->lastSegment = false;
                            break;
                        }
                    }
                    }

                    /* print last point */
                    if (!readPtrOverflowed && ODF_arg->lastSegment) {
                        v = trace->valuePrev;
                        t = trace->lastTimeStamp;
                        len = trace->dt->printPointEnd(s, freeLen, t, v);
                        s += len;
                        freeLen -= len;
                    }
                } while(readPtrOverflowed);

                ODF_arg->dataLength -= freeLen;
            }
        }
        break;
    }

    return ret;
}


/******************************************************************************/
void CO_trace_init(
        CO_trace_t             *trace,
        CO_SDO_t               *SDO,
        uint8_t                 enabled,
        uint32_t               *timeBuffer,
        int32_t                *valueBuffer,
        uint32_t                bufferSize,
        uint32_t               *map,
        uint8_t                *format,
        uint8_t                *trigger,
        int32_t                *threshold,
        int32_t                *value,
        int32_t                *minValue,
        int32_t                *maxValue,
        uint32_t               *triggerTime,
        uint16_t                idx_OD_traceConfig,
        uint16_t                idx_OD_trace)
{
    trace->SDO = SDO;
    trace->enabled = (enabled != 0) ? true : false;
    trace->timeBuffer = timeBuffer;
    trace->valueBuffer = valueBuffer;
    trace->bufferSize = bufferSize;
    trace->writePtr = 0;
    trace->readPtr = 0;
    trace->lastTimeStamp = 0;
    trace->map = map;
    trace->format = format;
    trace->trigger = trigger;
    trace->threshold = threshold;
    trace->value = value;
    trace->minValue = minValue;
    trace->maxValue = maxValue;
    trace->triggerTime = triggerTime;
    *trace->value = 0;
    *trace->minValue = 0;
    *trace->maxValue = 0;
    *trace->triggerTime = 0;
    trace->valuePrev = 0;

    /* set trace->OD_variable and trace->dt, based on 'map' and 'format' */
    findVariable(trace);

    if (timeBuffer == NULL || valueBuffer == NULL) {
        trace->bufferSize = 0;
    }

    if ( trace->bufferSize == 0 || trace->OD_variable == NULL) {
        trace->enabled = false;
    }

    CO_OD_configure(SDO, idx_OD_traceConfig, CO_ODF_traceConfig, (void*)trace, 0, 0);
    CO_OD_configure(SDO, idx_OD_trace, CO_ODF_trace, (void*)trace, 0, 0);
}


/******************************************************************************/
void CO_trace_process(CO_trace_t *trace, uint32_t timestamp) {
    if (trace->enabled) {

        int32_t val = trace->dt->pGetValue(trace->OD_variable);

        if (val != trace->valuePrev) {
            /* Verify, if value passed threshold */
            if ((*trace->trigger & 1) != 0 && trace->valuePrev < *trace->threshold && val >= *trace->threshold) {
                *trace->triggerTime = timestamp;
            }
            if ((*trace->trigger & 2) != 0 && trace->valuePrev < *trace->threshold && val >= *trace->threshold) {
                *trace->triggerTime = timestamp;
            }

            /* Write value and verify min/max */
            if (trace->value != trace->OD_variable) {
                *trace->value = val;
            }
            trace->valuePrev = val;
            if (*trace->minValue > val) {
                *trace->minValue = val;
            }
            if (*trace->maxValue < val) {
                *trace->maxValue = val;
            }

            /* write buffers and update pointers */
            trace->timeBuffer[trace->writePtr] = timestamp;
            trace->valueBuffer[trace->writePtr] = val;
            if (++trace->writePtr == trace->bufferSize) {
                trace->writePtr = 0;
            }
            if (trace->writePtr == trace->readPtr) {
                if (++trace->readPtr == trace->bufferSize) {
                    trace->readPtr = 0;
                }
            }
        }
        else {
            /* if buffer is empty, make first record */
            if (trace->writePtr == trace->readPtr) {
                /* write buffers and update pointers */
                trace->timeBuffer[trace->writePtr] = timestamp;
                trace->valueBuffer[trace->writePtr] = val;
                if (++trace->writePtr == trace->bufferSize) {
                    trace->writePtr = 0;
                }
            }
        }
        trace->lastTimeStamp = timestamp;
    }
}

#endif /* (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE */