CANopenNode/storage/CO_storage.c

/*
 * CANopen data storage base object
 *
 * @file        CO_storage.c
 * @author      Janez Paternoster
 * @copyright   2021 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 "storage/CO_storage.h"

#if ((CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE) != 0

/*
 * Custom function for writing OD object "Store parameters"
 *
 * For more information see file CO_ODinterface.h, OD_IO_t.
 */
static ODR_t OD_write_1010(OD_stream_t *stream, const void *buf,
                           OD_size_t count, OD_size_t *countWritten)
{
    /* verify arguments */
    if ((stream == NULL) || (stream->subIndex == 0U) || (buf == NULL) || (count != 4U)
        || (countWritten == NULL)
    ) {
        return ODR_DEV_INCOMPAT;
    }

    CO_storage_t *storage = stream->object;

    if ((stream->subIndex == 0U) || (storage->store == NULL) || !storage->enabled) {
        return ODR_READONLY;
    }

    uint32_t val = CO_getUint32(buf);
    if (val != 0x65766173U) {
        return ODR_DATA_TRANSF;
    }

    /* loop through entries and store relevant */
    uint8_t found = 0;
    ODR_t returnCode = ODR_OK;

    for (uint8_t i = 0; i < storage->entriesCount; i++) {
        CO_storage_entry_t *entry = &storage->entries[i];

        if ((stream->subIndex == 1U) || (entry->subIndexOD == stream->subIndex)) {
            if (found == 0U) {
                found = 1;
            }
            if ((entry->attr & (uint8_t)CO_storage_cmd) != 0U) {
                ODR_t code = storage->store(entry, storage->CANmodule);
                if (code != ODR_OK) {
                    returnCode = code;
                }
                found = 2;
            }
        }
    }

    if (found != 2U) {
        returnCode = (found == 0U) ? ODR_SUB_NOT_EXIST : ODR_READONLY;
    }

    if (returnCode == ODR_OK) {
        *countWritten = sizeof(uint32_t);
    }
    return returnCode;
}


/*
 * Custom function for writing OD object "Restore default parameters"
 *
 * For more information see file CO_ODinterface.h, OD_IO_t.
 */
static ODR_t OD_write_1011(OD_stream_t *stream, const void *buf,
                           OD_size_t count, OD_size_t *countWritten)
{
    /* verify arguments */
    if ((stream == NULL) || (stream->subIndex == 0U) || (buf == NULL) || (count != 4U)
        || (countWritten == NULL)
    ) {
        return ODR_DEV_INCOMPAT;
    }

    CO_storage_t *storage = stream->object;

    if ((stream->subIndex == 0U) || (storage->restore == NULL) || !storage->enabled){
        return ODR_READONLY;
    }

    uint32_t val = CO_getUint32(buf);
    if (val != 0x64616F6CU) {
        return ODR_DATA_TRANSF;
    }

    /* loop through entries and store relevant */
    uint8_t found = 0;
    ODR_t returnCode = ODR_OK;

    for (uint8_t i = 0; i < storage->entriesCount; i++) {
        CO_storage_entry_t *entry = &storage->entries[i];

        if ((stream->subIndex == 1U) || (entry->subIndexOD == stream->subIndex)) {
            if (found == 0U) {
                found = 1;
            }
            if ((entry->attr & (uint8_t)CO_storage_restore) != 0U) {
                ODR_t code = storage->restore(entry, storage->CANmodule);
                if (code != ODR_OK) {
                    returnCode = code;
                }
                found = 2;
            }
        }
    }

    if (found != 2U) {
        returnCode = (found == 0U) ? ODR_SUB_NOT_EXIST : ODR_READONLY;
    }

    if (returnCode == ODR_OK) {
        *countWritten = sizeof(uint32_t);
    }
    return returnCode;
}


CO_ReturnError_t CO_storage_init(CO_storage_t *storage,
                                 CO_CANmodule_t *CANmodule,
                                 OD_entry_t *OD_1010_StoreParameters,
                                 OD_entry_t *OD_1011_RestoreDefaultParameters,
                                 ODR_t (*store)(CO_storage_entry_t *entry,
                                                CO_CANmodule_t *CANmodule),
                                 ODR_t (*restore)(CO_storage_entry_t *entry,
                                                  CO_CANmodule_t *CANmodule),
                                 CO_storage_entry_t *entries,
                                 uint8_t entriesCount)
{
    /* verify arguments */
    if ((storage == NULL) || (CANmodule == NULL) || (OD_1010_StoreParameters == NULL) || (OD_1011_RestoreDefaultParameters == NULL)
        || (store == NULL) || (restore == NULL) || (entries == NULL)
    ) {
        return CO_ERROR_ILLEGAL_ARGUMENT;
    }

    /* Configure object variables */
    storage->CANmodule = CANmodule;
    storage->store = store;
    storage->restore = restore;
    storage->entries = entries;
    storage->entriesCount = entriesCount;

    /* configure extensions */
    storage->OD_1010_extension.object = storage;
    storage->OD_1010_extension.read = OD_readOriginal;
    storage->OD_1010_extension.write = OD_write_1010;
    (void)OD_extension_init(OD_1010_StoreParameters, &storage->OD_1010_extension);

    storage->OD_1011_extension.object = storage;
    storage->OD_1011_extension.read = OD_readOriginal;
    storage->OD_1011_extension.write = OD_write_1011;
    (void)OD_extension_init(OD_1011_RestoreDefaultParameters,
                      &storage->OD_1011_extension);

    return CO_ERROR_NO;
}

#endif /* (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE */