/**
 * Microcontroller specific code for CANopenNode nonvolatile variables.
 *
 * This file is a template for MM32MCU.
 *
 * @file        CO_flash.c
 * @ingroup     CO_flash
 *
 * 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 "CANopen.h"
#include "CO_flash.h"
#include "sfud.h"

//============================================================================
//                                LOCAL DATA
//============================================================================
extern struct sCO_OD_ROM CO_OD_ROM;

enum CO_OD_H1010_StoreParam_Sub
{
    OD_H1010_STORE_PARAM_COUNT,
    OD_H1010_STORE_PARAM_ALL,
    OD_H1010_STORE_PARAM_COMM,
    OD_H1010_STORE_PARAM_APP,
    OD_H1010_STORE_PARAM_MANUFACTURER,
    OD_H1010_STORE_PARAM_RESERVED       = 0x80
};

enum CO_OD_H1011_RestoreDefaultParam_Sub
{
    OD_H1011_RESTORE_PARAM_COUNT,
    OD_H1011_RESTORE_PARAM_ALL,
    OD_H1011_RESTORE_PARAM_COMM,
    OD_H1011_RESTORE_PARAM_APP,
    OD_H1011_RESTORE_PARAM_MANUFACTURER,
    OD_H1011_RESTORE_PARAM_RESERVED     = 0x80
};

enum CO_StorageFunctionality_Flags
{
    SAVES_PARAM_ON_COMMAND   = 0x01,
    SAVES_PARAM_AUTONOMOUSLY = 0x02
};

enum CO_RestoreFunctionality_Flags
{
    RESTORES_PARAMETERS   = 0x01
};

//============================================================================
/**
* Store parameters of object dictionary into flash memory.
* \param[in] FlashAddress Use CO_OD_Flash_Adress for the normal parameter
*                         block and CO_OD_Flash_Default_Param for the
*                         default parameters
*/
static CO_SDO_abortCode_t storeParameters(uint32_t FlashAddress, uint8_t ParametersSub)
{
    uint32_t addressPtr = 0x00;
    uint32_t addressFinal = 0x00;
    uint32_t* chunk;

    CO_UNUSED(ParametersSub);

    /* Check flash allocation */
    int32_t bytes_to_write = sizeof(CO_OD_ROM);
    if ((uint32_t)bytes_to_write > (FLASH_PAGE_SIZE*PAGES_PER_FLASH_AREA)) {
        return CO_SDO_AB_HW;
    }

    addressFinal = FlashAddress + bytes_to_write;

    sfud_erase(sfud_get_device(SFUD_QSPI_DEVICE_INDEX), FlashAddress, FLASH_PAGE_SIZE);

    /* Program data into flash word by word */
    chunk = (uint32_t*) &CO_OD_ROM;
    for (addressPtr = FlashAddress; addressPtr < addressFinal; addressPtr += 4) {

        if (sfud_write(sfud_get_device(SFUD_QSPI_DEVICE_INDEX), addressPtr, 4, (uint8_t *)chunk) != SFUD_SUCCESS){
            return CO_SDO_AB_HW;
        }

        chunk++;
    }

    return CO_SDO_AB_NONE;
}

static CO_SDO_abortCode_t restoreParameters(uint32_t FlashAddress, uint8_t ParametersSub)
{
    CO_UNUSED(ParametersSub);

    sfud_read(sfud_get_device(SFUD_QSPI_DEVICE_INDEX), FlashAddress, sizeof(CO_OD_ROM), (uint8_t *)&CO_OD_ROM);

    return CO_SDO_AB_NONE;
}

//============================================================================
/**
* Access to object dictionary OD_H1010_STORE_PARAM_FUNC
*/
static CO_SDO_abortCode_t CO_ODF_1010_StoreParam(CO_ODF_arg_t *ODF_arg)
{

    uint32_t* value = (uint32_t*)ODF_arg->data;

    if (ODF_arg->reading) {
        if(OD_H1010_STORE_PARAM_ALL == ODF_arg->subIndex) {
            *value = SAVES_PARAM_ON_COMMAND;
        }
        return CO_SDO_AB_NONE;
    }

    if(OD_H1010_STORE_PARAM_ALL != ODF_arg->subIndex) {
        return CO_SDO_AB_NONE;
    }

    if (*value != PARAM_STORE_PASSWORD) {
        return CO_SDO_AB_DATA_TRANSF;
    }

    return storeParameters(CO_OD_FLASH_PARAM_RUNTIME, ODF_arg->subIndex);
}

//============================================================================
/**
* Access to object dictionary OD_H1010_STORE_PARAM_FUNC
*/
static CO_SDO_abortCode_t CO_ODF_1011_RestoreParam(CO_ODF_arg_t *ODF_arg)
{
    uint32_t* value = (uint32_t*)ODF_arg->data;

    if (ODF_arg->reading) {
        if (OD_H1011_RESTORE_PARAM_ALL == ODF_arg->subIndex) {
            *value = RESTORES_PARAMETERS;
        }
        return CO_SDO_AB_NONE;
    }

    if (OD_H1011_RESTORE_PARAM_ALL != ODF_arg->subIndex) {
        return CO_SDO_AB_NONE;
    }

    if (*value != PARAM_RESTORE_PASSWORD) {
        return CO_SDO_AB_DATA_TRANSF;
    }

    CO_SDO_abortCode_t Result = restoreParameters(CO_OD_FLASH_PARAM_DEFAULT, ODF_arg->subIndex);

    if (Result != CO_SDO_AB_NONE) {
        return Result;
    }

    return storeParameters(CO_OD_FLASH_PARAM_RUNTIME, OD_H1011_RESTORE_PARAM_ALL);
}

//===========================================================================
/**
* Initialize flash library and data storage in flash
* We use two blocks in flash for data storage. One block is used for the
* default data that will be restored. The default parameters are stored
* at address CO_OD_Flash_Default_Param. The data that will be loaded at
* startup or saved if user modifies data is store at CO_OD_Flash_Adress.
*/
void CO_FlashInit(void)
{
    sfud_init(); /* init sfud. */
    sfud_qspi_fast_read_enable(sfud_get_device(SFUD_QSPI_DEVICE_INDEX), 4); /* enable qspi_read. */

    /* Before we can access the data, we need to make sure, that the flash
    block are properly initialized. We do this by reading the block into
    a local sCO_OD_ROM variable and verifying the FirstWord and LastWord
    members. */

    struct sCO_OD_ROM DefaultObjDicParam;
    sfud_read(sfud_get_device(SFUD_QSPI_DEVICE_INDEX), CO_OD_FLASH_PARAM_DEFAULT, sizeof(DefaultObjDicParam), (uint8_t *)&DefaultObjDicParam);

    /* If the default parameters are not present in flash, then we know that
    we need to create them for later restore. */

    if ((DefaultObjDicParam.FirstWord != CO_OD_FIRST_LAST_WORD) ||
    (DefaultObjDicParam.LastWord  != CO_OD_FIRST_LAST_WORD)) {
        storeParameters(CO_OD_FLASH_PARAM_RUNTIME, OD_H1010_STORE_PARAM_ALL);
        storeParameters(CO_OD_FLASH_PARAM_DEFAULT, OD_H1010_STORE_PARAM_ALL);
    }
    else {
        restoreParameters(CO_OD_FLASH_PARAM_RUNTIME, OD_H1010_STORE_PARAM_ALL);
    }
}

//===========================================================================
void CO_FlashRegisterODFunctions(CO_t* CO)
{
    CO_OD_configure(*(CO->SDO), OD_H1010_STORE_PARAM_FUNC,
    CO_ODF_1010_StoreParam, (void*)0, 0, 0);

    CO_OD_configure(*(CO->SDO), OD_H1011_REST_PARAM_FUNC,
    CO_ODF_1011_RestoreParam, (void*)0, 0, 0);
}