/*
 * Copyright (c) 2013, Linz Center of Mechatronics GmbH (LCM), web: www.lcm.at
 * All rights reserved.
 */
/*
 * This file is licensed according to the BSD 3-clause license as follows:
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the "Linz Center of Mechatronics GmbH" and "LCM" nor
 *       the names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL "Linz Center of Mechatronics GmbH" BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * This file is part of X2C. web: x2c.lcm.at
 * $LastChangedRevision: 4016 $
 * $LastChangedDate:: 2026-01-21 18:34:50 +0100#$
 */
#include <stddef.h>
#include "TableStruct.h"
#include "Services.h"

/* Bootloader version */
#define DEVINFO_BOOTLOADER_VERSION (0x0006u)

#if defined(__GENERIC_TI_C28X__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8110)
#elif defined(__GENERIC_MICROCHIP_DSPIC__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8210)
#elif defined(__GENERIC_MICROCHIP_PIC32__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8220)
#elif defined(__GENERIC_MICROCHIP_PIC24__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8230)
#elif defined(X2C_GENERIC_MICROCHIP_DSPIC33A)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8240)
#elif defined(__GENERIC_ARM_ARMV7__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8310)
#elif defined(__GENERIC_ARM_ARMV6__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8320)
#elif defined(__GENERIC_X86__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8410)
#elif defined(__GENERIC_X64__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x8420)

/* TMS320F240x */
#elif defined(__TMS320F2401__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0111)
#elif defined(__TMS320F2402__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0112)
#elif defined(__TMS320F2403__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0113)
#elif defined(__TMS320F2406__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0114)
#elif defined(__TMS320F2407__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0115)

/* TMS320F280x */
#elif defined(__TMS320F2801__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0121)
#elif defined(__TMS320F2802__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0122)
#elif defined(__TMS320F2806__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0123)
#elif defined(__TMS320F2808__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0124)
#elif defined(__TMS320F2809__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0125)

/* TMS320F281x */
#elif defined(__TMS320F2810__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0131)
#elif defined(__TMS320F2811__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0132)
#elif defined(__TMS320F2812__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0133)

/* TMS320F2833x */
#elif defined(__TMS320F28332__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0141)
#elif defined(__TMS320F28334__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0142)
#elif defined(__TMS320F28335__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0143)

/* TMS320F2803x */
#elif defined(__TMS320F28035__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0151)
#elif defined(__TMS320F28034__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0152)

/* TMS320F2806x */
#elif defined(__TMS320F28069__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0161)

/* TM4C */
#elif defined(__TM4C123GH6__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0171)
#elif defined(__TM4C123BE6__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0172)
#elif defined(__TM4C1294NC__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0173)

/* TMS320F2802x */
#elif defined(__TMS320F28027__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0181)

/* TMS320F2837x */
#elif defined(X2C_TMS320F28379D)
/**
 * Keep for legacy purposes only - this device identifier uses a memory map without NVM reset management.
 * New implementations shall use the X2C_TMS320F28379D_NVM_RM device identifier.
 */
#define DEVINFO_PROCESSOR_ID ((uint16)0x0191)
#warning THIS DEVICE IDENTIFIER SHALL NOT BE USED FOR NEW IMPLEMENTATIONS - USE DEVICE IDENTIFIER 0x0192 INSTEAD
#elif defined(X2C_TMS320F28379D_NVM_RM)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0192)
#elif defined(X2C_TMS320F28377S)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0193)

/* TMS320F28004x */
#elif defined(X2C_TMS320F280049)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01A1)
#elif defined(X2C_TMS320F280041)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01A2)

/* TMS320F2838x */
#elif defined(X2C_TMS320F28388D)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01B1)
#elif defined(X2C_TMS320F28386D)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01B2)

/* TMS320F28003x */
#elif defined(X2C_TMS320F280039)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01C1)
#elif defined(X2C_TMS320F280037)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01C2)

/* TMS320F28001x */
#elif defined(X2C_TMS320F2800137)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01D1)

/* TMS320F28P55x */
#elif defined(X2C_TMS320F28P550SJ9)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01E1)
#elif defined(X2C_TMS320F28P559SJ9)
#define DEVINFO_PROCESSOR_ID ((uint16)0x01E2)

/* MSPM0G */
#elif defined(X2C_MSPM0G3507)
#define DEVINFO_PROCESSOR_ID (0x01F1u)
#elif defined(X2C_MSPM0G1107)
#define DEVINFO_PROCESSOR_ID (0x01F2u)

/* DSPIC33F */
#elif defined(__DSPIC33FJ256MC710__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0221)
#elif defined(__DSPIC33FJ128MC706__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0222)
#elif defined(__DSPIC33FJ128MC506__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0223)
#elif defined(__DSPIC33FJ64GS610__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0224)
#elif defined(__DSPIC33FJ64GS406__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0225)
#elif defined(__DSPIC33FJ12GP202__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0226)
#elif defined(__DSPIC33FJ128MC802__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0228)

/* dsPIC33E */
#elif defined(__DSPIC33EP256MC506__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0231)
#elif defined(__DSPIC33EP128GP502__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0232)
#elif defined(__DSPIC33EP32GP502__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0233)
#elif defined(__DSPIC33EP256GP502__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0234)
#elif defined(__DSPIC33EP256MC502__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0235)
#elif defined(__DSPIC33EP128MC202__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0236)
#elif defined(__DSPIC33EP128GM604__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0237)
#elif defined(X2C_DSPIC33EP512GM604)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0238)

/* PIC32MZ */
#elif defined(__PIC32MZ2048EC__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0241)

/* PIC32MX */
#elif defined(__PIC32MX170F256__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0251)

#elif defined(X2C_DSPIC33CK256MP505)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0261)

/* STM32F103xx */
#elif defined(__STM32F103VB__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0311)
#elif defined(__STM32F103T6__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0312)
#elif defined(__STM32F103V8__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0313)
#elif defined(__STM32F103T4__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0314)
#elif defined(__STM32F103ZC__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0315)
#elif defined(__STM32F103RB__) || defined(X2C_STM32F103RB)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0316)

/* STM32F101 */
#elif defined(__STM32F101C4__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0321)
#elif defined(__STM32F101C6__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0322)

/* STM32F100 */
#elif defined(__STM32F100C6__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0331)

/* STM32F4 */
#elif defined(__STM32F407ZG__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0341)
#elif defined(__STM32F407ZE__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0342)
#elif defined(__STM32F407VG__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0343)
#elif defined(X2C_STM32F446VE)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0344)
#elif defined(X2C_STM32F446RE)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0345)
#elif defined(X2C_STM32F407VE)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0346)

/* STM32F0 */
#elif defined(__STM32F051R8__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0351)
#elif defined(__STM32F051C8__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0352)
#elif defined(X2C_STM32F072RB)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0353)

/* STM32F3 */
#elif defined(__STM32F303RE__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0361)
#elif defined(__STM32F303RB__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0362)
#elif defined(__STM32F302RD__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0363)
#elif defined(X2C_STM32F302R8)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0364)

/* STM32G0 */
#elif defined(X2C_STM32G0B1RE)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0371)
#elif defined(X2C_STM32G0B1RB)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0372)

/* STM32G4 */
#elif defined(X2C_STM32G474RE)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0381)
#elif defined(X2C_STM32G474RB)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0382)
#elif defined(X2C_STM32G474ME)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0383)

/* STM32H5 */
#elif defined(X2C_STM32H562ZG)
#define DEVINFO_PROCESSOR_ID (0x0391u)
#elif defined(X2C_STM32H563ZI)
#define DEVINFO_PROCESSOR_ID (0x0392u)
#elif defined(X2C_STM32H562VG)
#define DEVINFO_PROCESSOR_ID (0x0393u)

/* MC56F8xxx */
#elif defined(__MC56F8XXX__)
/* mc56f8345 -> dsp id = 0x411 */
#define DEVINFO_PROCESSOR_ID ((uint16)0x0411)
#elif defined(__MPC5643L__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0421)

#elif defined(__RX62T__) || defined(__R5F562TA__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0511)
#elif defined(__RX62G__) || defined(__R5F562GA__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0512)

#elif defined(__KECONTROL__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0611)

#elif defined(__XMC4800F144K2048__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0711)
#elif defined(__XMC4800F100K2048__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0712)

#elif defined(__XMC4700F144K2048__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0721)
#elif defined(__XMC4700F100K2048__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0722)

#elif defined(__XMC4400F100K512__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0731)

#elif defined(__XMC4500F100K1024__)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0741)

#elif defined(X2C_TC387QP)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0751)
#elif defined(X2C_TC375TP)
#define DEVINFO_PROCESSOR_ID ((uint16)0x0752)

#else
#error PROCESSOR TYPE NOT DEFINED
#endif

/* Service table definition */
static tSERVICE_Table serviceTable;

/* Private functions */
static void getDeviceInfo(tProtocol* protocol);
static void getTargetState(tProtocol* protocol);
static void setTargetState(tProtocol* protocol);
static void loadParameter(tProtocol* protocol);
static void saveParameter(tProtocol* protocol);
static void loadMaskParameter(tProtocol* protocol);
static void saveMaskParameter(tProtocol* protocol);


/* Send Service Not Available error */
void sendSvNotAvailable(tProtocol* protocol)
{
    protocol->ucFRAMESize = 2u;
    protocol->ucFRAMEData[1] = ERRORServiceNotAvail;
    protocol->pSnd_Enable(protocol);
}

/* Send error */
void sendError(tProtocol* protocol, uint8 errorId)
{
    protocol->ucFRAMESize = 2u;
    protocol->ucFRAMEData[1] = errorId;
    protocol->pSnd_Enable(protocol);
}

/* Returns target device information */
static void getDeviceInfo(tProtocol* protocol)
{
    uint8 i;
    uint32 tableStructAddr;

    /* Check frame buffer length */
    if (protocol->ucMaxCommSize < 47u)
    {
        sendError(protocol, ERRORSizeTooLarge);
        return;
    }

    protocol->ucFRAMESize = 47u;
    protocol->ucFRAMEData[1] = ERRORSuccess;

    /* Bootloader version */
    protocol->ucFRAMEData[2] = (uint8)(DEVINFO_BOOTLOADER_VERSION & 0xFFu);
    protocol->ucFRAMEData[3] = (uint8)((uint16)DEVINFO_BOOTLOADER_VERSION >> 8u);

    /* Frame program version */
    protocol->ucFRAMEData[4] = (uint8)(TableStruct->framePrgVersion & 0xFFu);
    protocol->ucFRAMEData[5] = (uint8)(TableStruct->framePrgVersion >> 8u);

    /* Max communication size */
    protocol->ucFRAMEData[6] = (uint8)(protocol->ucMaxCommSize & 0xFFu);
    protocol->ucFRAMEData[7] = (uint8)(protocol->ucMaxCommSize >> 8u);

    protocol->ucFRAMEData[8] = (uint8)(DEVINFO_PROCESSOR_ID & 0xFFu);
    protocol->ucFRAMEData[9] = (uint8)((uint16)DEVINFO_PROCESSOR_ID >> 8u);

    /**
     * Bootloader compilation date as ASCII string:
     * 9 ... 11 -> first 3 letters of month (e.g. Oct, Dec)
     * 12 ... 13 -> day as DD
     * 14 ... 17 -> year as YYYY
     */
    protocol->ucFRAMEData[10] = (uint8)__DATE__[0];
    protocol->ucFRAMEData[11] = (uint8)__DATE__[1];
    protocol->ucFRAMEData[12] = (uint8)__DATE__[2];
    protocol->ucFRAMEData[13] = (uint8)__DATE__[4];
    protocol->ucFRAMEData[14] = (uint8)__DATE__[5];
    protocol->ucFRAMEData[15] = (uint8)__DATE__[7];
    protocol->ucFRAMEData[16] = (uint8)__DATE__[8];
    protocol->ucFRAMEData[17] = (uint8)__DATE__[9];
    protocol->ucFRAMEData[18] = (uint8)__DATE__[10];

    /**
     * Bootloader compilation time as ASCII string:
     * 18 ... 19 -> hour as HH
     * 20 ... 21 -> minute as MM
     */
    protocol->ucFRAMEData[19] = (uint8)__TIME__[0];
    protocol->ucFRAMEData[20] = (uint8)__TIME__[1];
    protocol->ucFRAMEData[21] = (uint8)__TIME__[3];
    protocol->ucFRAMEData[22] = (uint8)__TIME__[4];

    if (TableStruct->framePrgCompDateTime == NULL)
    {
        i = 12u;
        do
        {
            protocol->ucFRAMEData[23u + i] = (uint8)'-';
        } while (i-- > 0u);
    }
    else
    {
        /**
         * Frame program compilation date as ASCII string:
         * 22 ... 24 -> first 3 letters of month (e.g. Oct, Dec)
         * 25 ... 26 -> day as DD
         * 27 ... 30 -> year as YYYY
         * Frame program compilation time as ASCII string:
         * 31 ... 32 -> hour as HH
         * 23 ... 24 -> minute as MM
         */
        protocol->ucFRAMEData[23] = TableStruct->framePrgCompDateTime[0];
        protocol->ucFRAMEData[24] = TableStruct->framePrgCompDateTime[1];
        protocol->ucFRAMEData[25] = TableStruct->framePrgCompDateTime[2];
        protocol->ucFRAMEData[26] = TableStruct->framePrgCompDateTime[4];
        protocol->ucFRAMEData[27] = TableStruct->framePrgCompDateTime[5];
        protocol->ucFRAMEData[28] = TableStruct->framePrgCompDateTime[7];
        protocol->ucFRAMEData[29] = TableStruct->framePrgCompDateTime[8];
        protocol->ucFRAMEData[30] = TableStruct->framePrgCompDateTime[9];
        protocol->ucFRAMEData[31] = TableStruct->framePrgCompDateTime[10];
        protocol->ucFRAMEData[32] = TableStruct->framePrgCompDateTime[11];
        protocol->ucFRAMEData[33] = TableStruct->framePrgCompDateTime[12];
        protocol->ucFRAMEData[34] = TableStruct->framePrgCompDateTime[14];
        protocol->ucFRAMEData[35] = TableStruct->framePrgCompDateTime[15];
    }

    protocol->ucFRAMEData[36] = (uint8)TableStruct->DSPState & 0xFFu;
    protocol->ucFRAMEData[37] = (uint8)(TableStruct->eventType & 0xFFu);
    protocol->ucFRAMEData[38] = (uint8)(TableStruct->eventType >> 8u);
    protocol->ucFRAMEData[39] = (uint8)(TableStruct->eventId & 0xFFu);
    protocol->ucFRAMEData[40] = (uint8)(TableStruct->eventId >> 8u);
    protocol->ucFRAMEData[41] = (uint8)(TableStruct->eventId >> 16u);
    protocol->ucFRAMEData[42] = (uint8)(TableStruct->eventId >> 24u);

    tableStructAddr = (uint32)TableStruct;
    protocol->ucFRAMEData[43] = (uint8)(tableStructAddr & 0xFFu);
    protocol->ucFRAMEData[44] = (uint8)((tableStructAddr >> 8u) & 0xFFu);
    protocol->ucFRAMEData[45] = (uint8)((tableStructAddr >> 16u) & 0xFFu);
    protocol->ucFRAMEData[46] = (uint8)((tableStructAddr >> 24u) & 0xFFu);

    protocol->pSnd_Enable(protocol);
}

static void loadMaskParameter(tProtocol* protocol)
{
    tMaskParameterEntry mpEntry;
    uint16 paramId, tblIndex;
    uint8 error;

    if (protocol->ucFRAMESize != 3u)
    {
        sendError(protocol, ERRORFormat);
        return;
    }

    paramId = ((uint16)protocol->ucFRAMEData[2] << 8u) | protocol->ucFRAMEData[1];
    error = getMaskParamIndex(TableStruct->maskParameterTable, paramId, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorMaskParamTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, SvErrorInvalidParamId);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }

    mpEntry = TableStruct->maskParameterTable[tblIndex];
    if (mpEntry.loadMaskParameter(mpEntry.maskParameter, &protocol->ucFRAMEData[2], &protocol->ucFRAMESize, protocol->ucMaxCommSize - 2u) != 0u)
    {
        sendError(protocol, SvErrorLoadMaskParam);
    }
    else
    {
        protocol->ucFRAMESize += 2u; /* add overhead size */
        protocol->ucFRAMEData[1] = ERRORSuccess;
        protocol->pSnd_Enable(protocol);
    }
}

static void saveMaskParameter(tProtocol* protocol)
{
    tMaskParameterEntry mpEntry;
    uint16 paramId, tblIndex;
    uint8 error;

    if (protocol->ucFRAMESize < 3u)
    {
        sendError(protocol, ERRORFormat);
        return;
    }

    paramId = ((uint16)protocol->ucFRAMEData[2] << 8u) | protocol->ucFRAMEData[1];
    error = getMaskParamIndex(TableStruct->maskParameterTable, paramId, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorMaskParamTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, SvErrorInvalidParamId);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }

    mpEntry = TableStruct->maskParameterTable[tblIndex];
    if (mpEntry.saveMaskParameter(mpEntry.block, mpEntry.maskParameter, &protocol->ucFRAMEData[3], protocol->ucFRAMESize - 3u) != 0u)
    {
        sendError(protocol, SvErrorSaveMaskParam);
    }
    else
    {
        protocol->ucFRAMESize = 2u;
        protocol->ucFRAMEData[1] = ERRORSuccess;
        protocol->pSnd_Enable(protocol);
    }
}

/* Saves block data by parameter identifier */
static void saveParameter(tProtocol* protocol)
{
    uint16 blockId, paramId, tblIndex;
    uint8 error;
    void* blockAddr;

    if (protocol->ucFRAMESize < 3u)
    {
        sendError(protocol, ERRORFormat);
        return;
    }

    /* Get Parameter ID from frame */
    paramId = ((uint16)protocol->ucFRAMEData[2] << 8u) | protocol->ucFRAMEData[1];

    error = getBlockParamIndex(TableStruct->TParamTable, paramId, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorParamTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, SvErrorInvalidParamId);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }
    blockAddr = TableStruct->TParamTable[tblIndex].pAdr;

    /* Try to execute limit-save function if limit-save function table is initialized */
    if (TableStruct->TLimitSaveFncTable != NULL)
    {
        uint16 i = 0u;
        /* Check if Parameter ID has Limit function */
        while ((paramId != TableStruct->TLimitSaveFncTable[i].uiParID) && (TableStruct->TLimitSaveFncTable[i].uiParID != 0u))
        {
            i++;
        }

        if (TableStruct->TLimitSaveFncTable[i].uiParID != 0u)
        {
            /* If Limit-Save function fails -> send error */
            if (TableStruct->TLimitSaveFncTable[i].pFLimitSave(blockAddr, &protocol->ucFRAMEData[3], protocol->ucFRAMESize - 3u) != 0u)
            {
                sendError(protocol, ERRORParLimit);
            }
            return;
        }
    }

    /* Get block functions */
    blockId = *(uint16*)blockAddr;
    error = getBlockFunctionIndex(TableStruct->TFncTable, blockId, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorFncTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, ERRORBlkID);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }

    /* If default Save function fails -> send error */
    if (TableStruct->TFncTable[tblIndex].pFSave(blockAddr, &protocol->ucFRAMEData[3], protocol->ucFRAMESize - 3u) != 0u)
    {
        sendError(protocol, ERRORFormat);
    }
    else
    {
        protocol->ucFRAMESize = 2u;
        protocol->ucFRAMEData[1] = ERRORSuccess;
        protocol->pSnd_Enable(protocol);
    }
}


/* Loads block data by parameter identifier */
static void loadParameter(tProtocol* protocol)
{
    void* blockAddr;
    uint16 blockId, paramId, tblIndex;
    uint8 error;

    if (protocol->ucFRAMESize != 3u)
    {
        sendError(protocol, ERRORFormat);
        return;
    }

    /* Get Parameter ID from frame */
    paramId = ((uint16)protocol->ucFRAMEData[2] << 8u) | protocol->ucFRAMEData[1];

    /* Look for Parameter ID in parameter table */
    error = getBlockParamIndex(TableStruct->TParamTable, paramId, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorParamTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, SvErrorInvalidParamId);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }
    blockAddr = TableStruct->TParamTable[tblIndex].pAdr;

    /* Get block functions */
    blockId = *(uint16*)blockAddr;
    error = getBlockFunctionIndex(TableStruct->TFncTable, blockId, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorFncTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, ERRORBlkID);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }

    /* Check Load function return value */
    if (TableStruct->TFncTable[tblIndex].pFLoad(blockAddr, &protocol->ucFRAMEData[2], &protocol->ucFRAMESize, protocol->ucMaxCommSize - 2u) != 0u)
    {
        sendError(protocol, ERRORFormat);
    }
    else
    {
        protocol->ucFRAMESize += 2u; /* add overhead size */
        protocol->ucFRAMEData[1] = ERRORSuccess;
        protocol->pSnd_Enable(protocol);
    }
}

/**
 * @brief Loads In- or Outport block data.
 *
 * @param[in,out] protocol Protocol
 */
static void loadIoBlockData(tProtocol* protocol)
{
    uint16 id, tblIndex;
    uint8 error, size, isOutport;
    const tIoParamIdEntry* paramTbl;
    void* addr;

    if (protocol->ucFRAMESize != 4u)
    {
        sendError(protocol, ERRORFormat);
        return;
    }

    id = ((uint16)protocol->ucFRAMEData[2] << 8u) | protocol->ucFRAMEData[1];
    isOutport = protocol->ucFRAMEData[3];

    if (isOutport)
    {
        paramTbl = TableStruct->outportParamTable;
        if (paramTbl == NULL)
        {
            sendError(protocol, SvErrorOutportParamTableNotInit);
            return;
        }
    }
    else
    {
        paramTbl = TableStruct->inportParamTable;
        if (paramTbl == NULL)
        {
            sendError(protocol, SvErrorInportParamTableNotInit);
            return;
        }
    }

    error = getIoParamIndex(paramTbl, id, &tblIndex);
    if (error != ERROR_SUCCESS)
    {
        switch (error)
        {
            case ERROR_TABLE_NOT_INIT:
                sendError(protocol, SvErrorParamTableNotInit);
                break;
            case ERROR_INVALID_ID:
                sendError(protocol, SvErrorInvalidParamId);
                break;
            default:
                sendError(protocol, SvErrorUnknown);
                break;
        }
        return;
    }

    /* If Outport type -> Pointer does point to a pointer instead of a value */
    if (isOutport)
    {
        addr = *(void**)paramTbl[tblIndex].data;
    }
    else
    {
        addr = paramTbl[tblIndex].data;
    }

    /* Data type byte size */
    size = paramTbl[tblIndex].size;
    switch (size)
    {
        case 1:
        {
            uint8 data = *(uint8*)addr;
            protocol->ucFRAMEData[2] = data;
            break;
        }
        case 2:
        {
            uint16 data = *(uint16*)addr;
            protocol->ucFRAMEData[2] = (uint8)(data & 0xFFu);
            protocol->ucFRAMEData[3] = (uint8)((data >> 8u) & 0xFFu);
            break;
        }
        case 4:
        {
            uint32 data = *(uint32*)addr;
            protocol->ucFRAMEData[2] = (uint8)(data & 0xFFu);
            protocol->ucFRAMEData[3] = (uint8)((data >> 8u) & 0xFFu);
            protocol->ucFRAMEData[4] = (uint8)((data >> 16u) & 0xFFu);
            protocol->ucFRAMEData[5] = (uint8)((data >> 24u) & 0xFFu);
            break;
        }
        case 8:
        {
            uint64 data = *(uint64*)addr;
            protocol->ucFRAMEData[2] = (uint8)(data & 0xFFu);
            protocol->ucFRAMEData[3] = (uint8)((data >> 8u) & 0xFFu);
            protocol->ucFRAMEData[4] = (uint8)((data >> 16u) & 0xFFu);
            protocol->ucFRAMEData[5] = (uint8)((data >> 24u) & 0xFFu);
            protocol->ucFRAMEData[6] = (uint8)((data >> 32u) & 0xFFu);
            protocol->ucFRAMEData[7] = (uint8)((data >> 40u) & 0xFFu);
            protocol->ucFRAMEData[8] = (uint8)((data >> 48u) & 0xFFu);
            protocol->ucFRAMEData[9] = (uint8)((data >> 56u) & 0xFFu);
            break;
        }
        default:
            sendError(protocol, SvErrorInvalidDataType);
            return;
    }

    protocol->ucFRAMEData[1] = ERRORSuccess;
    protocol->ucFRAMESize = (uint16)size + 2u;
    protocol->pSnd_Enable(protocol);
}


/* Sets target state */
static void setTargetState(tProtocol* protocol)
{
    uint8 error = ERRORSuccess;

    switch (protocol->ucFRAMEData[1])
    {
    case 0:
        TableStruct->DSPState = BOOTLOADER_STATE;
        break;
    case 1:
        TableStruct->DSPState = PRG_LOADED_STATE;
        break;
    case 2:
        TableStruct->DSPState = IDLE_STATE;
        break;
    case 3:
        TableStruct->DSPState = INIT_STATE;
        break;
    case 4:
        TableStruct->DSPState = RUN_STATE_POWER_OFF;
        break;
    case 5:
        TableStruct->DSPState = RUN_STATE_POWER_ON;
        break;
    default:
        error = SvErrorInvalidDspState;
        break;
    }

    protocol->ucFRAMEData[1] = error;
    protocol->ucFRAMESize = 2u;
    protocol->pSnd_Enable(protocol);
}

/* Returns target state */
static void getTargetState(tProtocol* protocol)
{
    protocol->ucFRAMEData[1] = ERRORSuccess;
    protocol->ucFRAMEData[2] = (uint8)TableStruct->DSPState & 0xFFu;
    protocol->ucFRAMESize = 3u;
    protocol->pSnd_Enable(protocol);
}

/**
 * This function initializes the protocol service table.
 * @param protocol Protocol
 */
void initServiceTable(tProtocol* protocol)
{
    uint16 i;

    for (i = 0; i <= MAX_SERVICE_ID; i++)
    {
        serviceTable[i] = &sendSvNotAvailable;
    }

    /* Add service table pointer to protocol structure */
    protocol->pServiceTable = (void*)serviceTable;
}

/*
 * This function adds all core services.
 * @param protocol Protocol
 */
void addCoreServices(tProtocol* protocol)
{
    tSERVICEFunction* svTable = (tSERVICEFunction*)protocol->pServiceTable;
    svTable[SV_ID_SVDEVICEINFO] = &getDeviceInfo;
    svTable[SV_ID_GETTARGETSTATE] = &getTargetState;
    svTable[SV_ID_SETTARGETSTATE] = &setTargetState;
    svTable[SV_ID_SVSAVEPARAM] = &saveParameter;
    svTable[SV_ID_SVLOADPARAM] = &loadParameter;
    svTable[SV_ID_LOAD_IO_PARAM] = &loadIoBlockData;
    svTable[SV_ID_SAVEMASKPARAM] = &saveMaskParameter;
    svTable[SV_ID_LOADMASKPARAM] = &loadMaskParameter;
}

/**
 * This functions adds core services required for X2C Bootloader operation.
 * The following services are added:
 * - #getDeviceInfo Get target device information
 * - #getTargetState Get target state
 * - #setTargetState Set target state (may be moved to the Application core services)
 * @param protocol Protocol
 */
void addCoreBootloaderServices(tProtocol* protocol)
{
    tSERVICEFunction* svTable = (tSERVICEFunction*)protocol->pServiceTable;
    svTable[SV_ID_SVDEVICEINFO] = &getDeviceInfo;
    svTable[SV_ID_GETTARGETSTATE] = &getTargetState;
    svTable[SV_ID_SETTARGETSTATE] = &setTargetState;
}

/**
 * This functions adds core services required for X2C Application operation.
 * The following services are added:
 * - #saveParameter Block parameter download (by Parameter ID)
 * - #loadParameter Block parameter upload (by Parameter ID)
 * - #loadIoBlockData Upload IO block data (by Parameter ID)
 * - #saveMaskParameter Download Mask parameter data for CoT Blocks
 * - #loadMaskParameter Upload Mask parameter data for CoT Blocks
 * @param protocol Protocol
 */
void addCoreApplicationServices(tProtocol* protocol)
{
    tSERVICEFunction* svTable = (tSERVICEFunction*)protocol->pServiceTable;
    svTable[SV_ID_SVSAVEPARAM] = &saveParameter;
    svTable[SV_ID_SVLOADPARAM] = &loadParameter;
    svTable[SV_ID_LOAD_IO_PARAM] = &loadIoBlockData;
    svTable[SV_ID_SAVEMASKPARAM] = &saveMaskParameter;
    svTable[SV_ID_LOADMASKPARAM] = &loadMaskParameter;
}
