/*
 * CANopen Object Dictionary interface
 *
 * @file        CO_ODinterface.c
 * @author      Janez Paternoster
 * @copyright   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 <string.h>
#define OD_DEFINITION
#include "301/CO_ODinterface.h"


/* Read value from variable from Object Dictionary, see OD_subEntry_t *********/
static OD_size_t OD_readDirect(OD_stream_t *stream, uint8_t subIndex,
                               void *buf, OD_size_t count,
                               ODR_t *returnCode)
{
    (void) subIndex;

    if (stream == NULL || buf == NULL || returnCode == NULL) {
        if (returnCode != NULL) *returnCode = ODR_DEV_INCOMPAT;
        return 0;
    }

    OD_size_t n = stream->dataLength;
    const char *odData = (const char *)stream->dataObject;

    if (odData == NULL) {
        *returnCode = ODR_SUB_NOT_EXIST;
        return 0;
    }

    *returnCode = ODR_OK;

    if (stream->dataOffset > 0 || n > count) {
        /* data will be (are) read in several segments */
        n -= stream->dataOffset;
        odData += stream->dataOffset;

        if (n > count) {
            /* not enough space in destionation buffer */
            n = count;
            stream->dataOffset += n;
            *returnCode = ODR_PARTIAL;
        }
        else {
            stream->dataOffset = 0; /* copy finished, reset offset */
        }
    }

    memcpy(buf, odData, n);
    return n;
}

/* Write value to variable from Object Dictionary, see OD_subEntry_t **********/
static OD_size_t OD_writeDirect(OD_stream_t *stream, uint8_t subIndex,
                                const void *buf, OD_size_t count,
                                ODR_t *returnCode)
{
    (void) subIndex;

    if (stream == NULL || buf == NULL || returnCode == NULL) {
        if (returnCode != NULL) *returnCode = ODR_DEV_INCOMPAT;
        return 0;
    }

    OD_size_t n = stream->dataLength;
    char *odData = (char *)stream->dataObject;

    if (odData == NULL) {
        *returnCode = ODR_SUB_NOT_EXIST;
        return 0;
    }

    *returnCode = ODR_OK;

    if (stream->dataOffset > 0 || n > count) {
        /* data will be (are) written in several segments */
        n -= stream->dataOffset;
        odData += stream->dataOffset;

        if (n > count) {
            /* OD variable is larger than current ammount of data */
            n = count;
            stream->dataOffset += n;
            *returnCode = ODR_PARTIAL;
        }
        else {
            stream->dataOffset = 0; /* copy finished, reset offset */
        }
    }

    memcpy(odData, buf, n);
    return n;
}

/******************************************************************************/
const OD_entry_t *OD_find(const OD_t *od, uint16_t index) {
    unsigned int cur;
    unsigned int min = 0;
    unsigned int max = od->size - 1;

    if (od == NULL || od->size == 0) {
        return NULL;
    }

    /* Fast search in ordered Object Dictionary. If indexes are mixed,
     * this won't work. If Object Dictionary has up to 2^N entries, then N is
     * max number of loop passes. */
    while (min < max) {
        /* get entry between min and max */
        cur = (min + max) >> 1;
        const OD_entry_t* entry = &od->list[cur];

        if (index == entry->index) {
            return entry;
        }
        else if (index < entry->index) {
            max = cur;
            if(max > 0) max--;
        }
        else {
            min = cur + 1;
        }
    }

    if (min == max) {
        const OD_entry_t* entry = &od->list[min];
        if (index == entry->index) {
            return entry;
        }
    }

    return NULL;  /* entry does not exist in OD */
}

/******************************************************************************/
ODR_t OD_getSub(const OD_entry_t *entry, uint8_t subIndex,
                OD_subEntry_t *subEntry, OD_stream_t *stream)
{
    if (entry == NULL) return ODR_IDX_NOT_EXIST;
    else if (subIndex > entry->maxSubIndex) return ODR_SUB_NOT_EXIST;
    else if (subEntry == NULL || stream == NULL) return ODR_DEV_INCOMPAT;

    const void *odObject = entry->odObject;
    bool_t io_configured = false;
    uint8_t odBasicType = entry->odObjectType & ODT_TYPE_MASK;

    if (odObject == NULL) return ODR_DEV_INCOMPAT;

    /* common properties */
    subEntry->index = entry->index;
    subEntry->subIndex = subIndex;
    subEntry->maxSubIndex = entry->maxSubIndex;
    subEntry->storageGroup = entry->storageGroup;
    stream->dataOffset = 0;

    /* Object type is extended */
    if ((entry->odObjectType & ODT_EXTENSION_MASK) != 0) {
        const OD_obj_extended_t *odoe = (const OD_obj_extended_t *)odObject;

        if (odoe->extIO != NULL && odoe->extIO->object != NULL) {
            subEntry->read = odoe->extIO->read;
            subEntry->write = odoe->extIO->write;
            stream->dataObject = odoe->extIO->object;
            io_configured = true;
        }
        subEntry->flagsPDO = odoe->flagsPDO;
        odObject = odoe->odObjectOriginal;
        if (odObject == NULL) return ODR_DEV_INCOMPAT;
    }
    else {
        subEntry->flagsPDO = NULL;
    }

    if (!io_configured) {
        subEntry->read = OD_readDirect;
        subEntry->write = OD_writeDirect;
    }

    if (odBasicType == ODT_VAR) {
        const OD_obj_var_t *odo = (const OD_obj_var_t *)odObject;

        subEntry->lowLimit = 0;
        subEntry->highLimit = -1;
        subEntry->attribute = odo->attribute;
        if (!io_configured) stream->dataObject = odo->data;
        stream->dataLength = odo->dataLength;
    }
    else if (odBasicType == ODT_VARL) {
        const OD_obj_varLimits_t *odo = (const OD_obj_varLimits_t *)odObject;

        subEntry->lowLimit = odo->limit.low;
        subEntry->highLimit = odo->limit.high;
        subEntry->attribute = odo->attribute;
        if (!io_configured) stream->dataObject = odo->data;
        stream->dataLength = odo->dataLength;
    }
    else if (odBasicType == ODT_ARR) {
        const OD_obj_array_t *odo = (const OD_obj_array_t *)odObject;

        subEntry->lowLimit = 0;
        subEntry->highLimit = -1;
        if (subIndex == 0) {
            subEntry->attribute = odo->attribute0;
            if (!io_configured) stream->dataObject = odo->data0;
            stream->dataLength = 1;
        }
        else {
            char *data = (char *)odo->data;

            subEntry->attribute = odo->attribute;
            if (!io_configured) {
                int i = subIndex - 1;
                if (data == NULL) return ODR_DEV_INCOMPAT;
                stream->dataObject = data + odo->dataElementSizeof * i;
            }
            stream->dataLength = odo->dataElementLength;
        }
    }
    else if (odBasicType == ODT_ARRL) {
        const OD_obj_arrayLimAttr_t *odo =
            (const OD_obj_arrayLimAttr_t *)odObject;

        if (subIndex == 0) {
            subEntry->lowLimit = 0;
            subEntry->highLimit = -1;
            subEntry->attribute = odo->attribute0;
            if (!io_configured) stream->dataObject = odo->data0;
            stream->dataLength = 1;
        }
        else {
            char *data = (char *)odo->data;
            int i = subIndex - 1;

            if (odo->limits == NULL || odo->attributes == NULL)
                return ODR_DEV_INCOMPAT;
            subEntry->lowLimit = odo->limits[i].low;
            subEntry->highLimit = odo->limits[i].high;
            subEntry->attribute = odo->attributes[i];
            if (!io_configured) {
                if (data == NULL) return ODR_DEV_INCOMPAT;
                stream->dataObject = data + odo->dataElementSizeof * i;
            }
            stream->dataLength = odo->dataElementLength;
        }
    }
    else if (odBasicType == ODT_REC) {
        const OD_obj_var_t *odo_rec = (const OD_obj_var_t *)odObject;
        const OD_obj_var_t *odo = &odo_rec[subIndex];

        subEntry->lowLimit = 0;
        subEntry->highLimit = -1;
        subEntry->attribute = odo->attribute;
        if (!io_configured) stream->dataObject = odo->data;
        stream->dataLength = odo->dataLength;
    }
    else if (odBasicType == ODT_RECL) {
        const OD_obj_varLimits_t *odo_rec = (const OD_obj_varLimits_t*)odObject;
        const OD_obj_varLimits_t *odo = &odo_rec[subIndex];

        subEntry->lowLimit = odo->limit.low;
        subEntry->highLimit = odo->limit.high;
        subEntry->attribute = odo->attribute;
        if (!io_configured) stream->dataObject = odo->data;
        stream->dataLength = odo->dataLength;
    }
    else {
        return ODR_DEV_INCOMPAT;
    }

    return ODR_OK;
}

/******************************************************************************/
uint32_t OD_getSDOabortCode(ODR_t returnCode) {
    static const uint32_t abortCodes[ODR_COUNT] = {
        0x00000000UL, /* No abort */
        0x05040005UL, /* Out of memory */
        0x06010000UL, /* Unsupported access to an object */
        0x06010001UL, /* Attempt to read a write only object */
        0x06010002UL, /* Attempt to write a read only object */
        0x06020000UL, /* Object does not exist in the object dictionary */
        0x06040041UL, /* Object cannot be mapped to the PDO */
        0x06040042UL, /* Num and len of object to be mapped exceeds PDO len */
        0x06040043UL, /* General parameter incompatibility reasons */
        0x06040047UL, /* General internal incompatibility in device */
        0x06060000UL, /* Access failed due to hardware error */
        0x06070010UL, /* Data type does not match, length does not match */
        0x06070012UL, /* Data type does not match, length too high */
        0x06070013UL, /* Data type does not match, length too short */
        0x06090011UL, /* Sub index does not exist */
        0x06090030UL, /* Invalid value for parameter (download only). */
        0x06090031UL, /* Value range of parameter written too high */
        0x06090032UL, /* Value range of parameter written too low */
        0x06090036UL, /* Maximum value is less than minimum value. */
        0x060A0023UL, /* Resource not available: SDO connection */
        0x08000000UL, /* General error */
        0x08000020UL, /* Data cannot be transferred or stored to application */
        0x08000021UL, /* Data cannot be transf. because of local control */
        0x08000022UL, /* Data cannot be tran. because of present device state */
        0x08000023UL, /* Object dict. not present or dynamic generation fails */
        0x08000024UL  /* No data available */
    };

    return (returnCode < 0 || returnCode >= ODR_COUNT) ?
        abortCodes[ODR_DEV_INCOMPAT] : abortCodes[returnCode];
}

/******************************************************************************/
bool_t OD_extensionIO_init(const OD_entry_t *entry,
                           void *object,
                           OD_size_t (*read)(OD_stream_t *stream,
                                             uint8_t subIndex,
                                             void *buf,
                                             OD_size_t count,
                                             ODR_t *returnCode),
                           OD_size_t (*write)(OD_stream_t *stream,
                                              uint8_t subIndex,
                                              const void *buf,
                                              OD_size_t count,
                                              ODR_t *returnCode))
{
    if (entry == NULL || (entry->odObjectType & ODT_EXTENSION_MASK) == 0) {
        return false;
    }

    const OD_obj_extended_t *odo = (const OD_obj_extended_t *) entry->odObject;

    odo->extIO->object = object;
    odo->extIO->read = read;
    odo->extIO->write = write;

    return true;
}

/******************************************************************************/
void OD_updateStorageGroup(OD_t *od, uint8_t storageGroup) {
    if (od == NULL) return;

    int i;

    for (i = 0; i < od->size; i++) {
        const OD_entry_t *entry = &od->list[i];
        const OD_obj_extended_t *odoe;
        uint8_t subIndex;
        uint8_t odBasicType;
        const void *orig;

        /* skip non-matched storage group and non-extended OD objects */
        if (entry->storageGroup != storageGroup
            || (entry->odObjectType & ODT_EXTENSION_MASK) != 0)
            continue;

        /* skip if extIO read or original object is not specified */
        odoe = (const OD_obj_extended_t *)entry->odObject;
        orig = odoe->odObjectOriginal;
        if (odoe->extIO == NULL || orig == NULL
            || odoe->extIO->object == NULL || odoe->extIO->read == NULL)
            continue;

        /* update each sub-index separately */
        odBasicType = entry->odObjectType & ODT_TYPE_MASK;
        for (subIndex = 0; subIndex < entry->maxSubIndex; subIndex++) {
            OD_attr_t attr;
            void *dataObject = NULL;
            OD_size_t dataLength, dataReadTotal;

            if (odBasicType == ODT_VAR) {
                const OD_obj_var_t *odo = (const OD_obj_var_t *)orig;

                attr = odo->attribute;
                dataObject = odo->data;
                dataLength = odo->dataLength;
            }
            else if (odBasicType == ODT_VARL) {
                const OD_obj_varLimits_t *odo = (const OD_obj_varLimits_t*)orig;

                attr = odo->attribute;
                dataObject = odo->data;
                dataLength = odo->dataLength;
            }
            else if (odBasicType == ODT_ARR) {
                const OD_obj_array_t *odo = (const OD_obj_array_t *)orig;

                if (subIndex == 0) {
                    attr = odo->attribute0;
                    dataObject = odo->data0;
                    dataLength = 1;
                }
                else {
                    char *data = (char *)odo->data;
                    if (data != NULL) {
                        int i = subIndex - 1;
                        attr = odo->attribute;
                        dataObject = data + odo->dataElementSizeof * i;
                        dataLength = odo->dataElementLength;
                    }
                }
            }
            else if (odBasicType == ODT_ARRL) {
                const OD_obj_arrayLimAttr_t *odo =
                    (const OD_obj_arrayLimAttr_t *)orig;

                if (subIndex == 0) {
                    attr = odo->attribute0;
                    dataObject = odo->data0;
                    dataLength = 1;
                }
                else {
                    char *data = (char *)odo->data;
                    if (data != NULL && odo->attributes != NULL) {
                        int i = subIndex - 1;
                        attr = odo->attributes[i];
                        dataObject = data + odo->dataElementSizeof * i;
                        dataLength = odo->dataElementLength;
                    }
                }
            }
            else if (odBasicType == ODT_REC) {
                const OD_obj_var_t *odo_rec = (const OD_obj_var_t *)orig;
                const OD_obj_var_t *odo = &odo_rec[subIndex];

                attr = odo->attribute;
                dataObject = odo->data;
                dataLength = odo->dataLength;
            }
            else if (odBasicType == ODT_RECL) {
                const OD_obj_varLimits_t *odo_rec
                    = (const OD_obj_varLimits_t*)orig;
                const OD_obj_varLimits_t *odo = &odo_rec[subIndex];

                attr = odo->attribute;
                dataObject = odo->data;
                dataLength = odo->dataLength;
            }

            if (dataObject == NULL || (attr&ODA_NOINIT) != 0 || dataLength == 0)
                continue;

            /* copy data to group structure, several times if data is large */
            dataReadTotal = 0;
            do {
                char buf[32];
                ODR_t returnCode;
                char *dest = (char *)dataObject + dataReadTotal;
                OD_size_t dataRead = odoe->extIO->read(odoe->extIO->object,
                                                       subIndex,
                                                       buf, sizeof(buf),
                                                       &returnCode);
                dataReadTotal += dataRead;
                if (dataRead == 0 || dataReadTotal > dataLength) break;
                memcpy(dest, buf, dataRead);
            } while (dataReadTotal == dataLength);
        } /* for (subIndex) */
    } /* for (entry) */
}


/******************************************************************************/
ODR_t OD_get_i8(const OD_entry_t *entry, uint16_t subIndex, int8_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_i16(const OD_entry_t *entry, uint16_t subIndex, int16_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_i32(const OD_entry_t *entry, uint16_t subIndex, int32_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_i64(const OD_entry_t *entry, uint16_t subIndex, int64_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_u8(const OD_entry_t *entry, uint16_t subIndex, uint8_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_u16(const OD_entry_t *entry, uint16_t subIndex, uint16_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_u32(const OD_entry_t *entry, uint16_t subIndex, uint32_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_u64(const OD_entry_t *entry, uint16_t subIndex, uint64_t *val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_r32(const OD_entry_t *entry, uint16_t subIndex, float32_t *val){
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

ODR_t OD_get_r64(const OD_entry_t *entry, uint16_t subIndex, float64_t *val){
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(*val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.read(&st, subIndex, val, sizeof(*val), &ret);
    return ret;
}

/******************************************************************************/
ODR_t OD_set_i8(const OD_entry_t *entry, uint16_t subIndex, int8_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) ret = OD_checkLimits(&subEntry, val);
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_i16(const OD_entry_t *entry, uint16_t subIndex, int16_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) ret = OD_checkLimits(&subEntry, val);
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_i32(const OD_entry_t *entry, uint16_t subIndex, int32_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) ret = OD_checkLimits(&subEntry, val);
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_i64(const OD_entry_t *entry, uint16_t subIndex, int64_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_u8(const OD_entry_t *entry, uint16_t subIndex, uint8_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) ret = OD_checkLimits(&subEntry, val);
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_u16(const OD_entry_t *entry, uint16_t subIndex, uint16_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) ret = OD_checkLimits(&subEntry, val);
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_u32(const OD_entry_t *entry, uint16_t subIndex, uint32_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);
    int32_t lowLimit = subEntry.lowLimit;
    int32_t highLimit = subEntry.highLimit;

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK && lowLimit <= highLimit) {
        if (val > 0x7FFF || val > highLimit) ret = ODR_VALUE_HIGH;
        else if (val < lowLimit) ret = ODR_VALUE_LOW;
    }
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_u64(const OD_entry_t *entry, uint16_t subIndex, uint64_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_r32(const OD_entry_t *entry, uint16_t subIndex, float32_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}

ODR_t OD_set_r64(const OD_entry_t *entry, uint16_t subIndex, float64_t val) {
    OD_subEntry_t subEntry; OD_stream_t st;
    ODR_t ret = OD_getSub(entry, subIndex, &subEntry, &st);

    if (ret == ODR_OK && st.dataLength != sizeof(val)) ret = ODR_TYPE_MISMATCH;
    if (ret == ODR_OK) subEntry.write(&st, subIndex, &val, sizeof(val), &ret);
    return ret;
}