//
// Created by cfif on 16.03.2024.
//
#include <SystemDelayInterface.h>
#include "CanSerialPortFrameXCP.h"
#include "memory.h"
#include "AsciiStringAssmeblingUtils.h"
#include "SystemMutexCmsis.h"
#include "CanPorts.h"

#define LOG_SIGN "CAN_XCP"
#define LOGGER &env->logger

static unsigned long SeedToKeyHVAC_NAMI(unsigned char rnd, unsigned long dwSeedInfo) {
#define Mask 0x55F388A6UL

    unsigned char i;
    unsigned long key = dwSeedInfo;

    rnd += 35;
    if (rnd < 35) rnd = 255;

    for (i = 0; i < rnd; i++) {
        if (key & 0x80000000UL)
            key = (key << 1) ^ Mask;
        else
            key <<= 1;
    }

    return key;
}

void CanSerialPortFrameXcpInit(
        tCanSerialPortFrameXCP *env,
        tSerialPortFrameIO *ioCanFrame,
        tRtcIO *rtcIO,
        uint32_t startRamAdr,
        uint32_t endRamAdr,
        uint32_t startFlashAdr,
        uint32_t endFlashAdr,
        xcp_clear_flash_func *clear_flash_func,
        xcp_write_flash_func *write_flash_func,
        tLoggerInterface *logger
) {
    env->ioCanFrame = ioCanFrame;

    env->rtcIO = rtcIO;
    env->logger = logger;

    env->startRamAdr = startRamAdr;
    env->endRamAdr = endRamAdr;

    env->startFlashAdr = startFlashAdr;
    env->endFlashAdr = endFlashAdr;
    env->clear_flash_func = clear_flash_func;
    env->write_flash_func = write_flash_func;

    env->access = osMutexNew(NULL);

    InitThreadBlock(env->T_can_Listener_XCP, "CanListenerXCP", osPriorityNormal);
}

static uint8_t XCP_COMMAND_CONNECT_255(tCanSerialPortFrameXCP *env) {
    eXcpCommand_CONNECT *xcpCommand_CONNECT = (eXcpCommand_CONNECT *) env->response;

    xcpCommand_CONNECT->COM = XCP_COMMAND_CONNECT;

    xcpCommand_CONNECT->CAL_PAG = 0; // Калибровка и пагинация
    xcpCommand_CONNECT->reserve1 = 0;
    xcpCommand_CONNECT->DAQ = 1; // Поддержка DAQ
    xcpCommand_CONNECT->STIM = 1; // Стимуляция (формирование инициативных запросов)
    xcpCommand_CONNECT->PGM = 1; // Программирование FLASH
    xcpCommand_CONNECT->reserve2 = 0;
    xcpCommand_CONNECT->reserve3 = 0;
    xcpCommand_CONNECT->reserve4 = 0;

    xcpCommand_CONNECT->BYTE__ORDER = 0; // 0 - Intel 1 - Motorola
    xcpCommand_CONNECT->ADDRESS_GRANULARITY_0 = 0;
    xcpCommand_CONNECT->ADDRESS_GRANULARITY_1 = 0;
    xcpCommand_CONNECT->reserve5 = 0;
    xcpCommand_CONNECT->reserve6 = 0;
    xcpCommand_CONNECT->reserve7 = 0;
    xcpCommand_CONNECT->SLAVE_BLOCK_MODE = 0; // Блочный режим не поддерживается
    xcpCommand_CONNECT->OPTIONAL = 1; // Поддержка команды XCP_COMMAND_GET_STATUS

    xcpCommand_CONNECT->MAX_CTO_ = MAX_CTO;
    xcpCommand_CONNECT->MAX_DTO_ = MAX_DTO;

    xcpCommand_CONNECT->XCP_PROTOCOL_VER = 1;
    xcpCommand_CONNECT->XCP_TRANSPORT_VER = 1;

    return 8;
}

static uint8_t XCP_COMMAND_DISCONNECT_254(tCanSerialPortFrameXCP *env) {

    env->response[0] = XCP_COMMAND_CONNECT;

    return 1;
}

static uint8_t XCP_COMMAND_GET_STATUS_253(tCanSerialPortFrameXCP *env) {
    eXcpCommand_GET_STATUS *xcpCommand_GET_STATUS = (eXcpCommand_GET_STATUS *) env->response;

    xcpCommand_GET_STATUS->COM = XCP_COMMAND_CONNECT;

    xcpCommand_GET_STATUS->STORE_CAL_REQ = 0;
    xcpCommand_GET_STATUS->reserve1 = 0;
    xcpCommand_GET_STATUS->STORE_DAQ_REQ = 0;
    xcpCommand_GET_STATUS->CLEAR_DAQ_REQ = 0;
    xcpCommand_GET_STATUS->reserve2 = 0;
    xcpCommand_GET_STATUS->reserve3 = 0;
    xcpCommand_GET_STATUS->DAQ_RUNNING = 0;
    xcpCommand_GET_STATUS->RESUME = 0;

    xcpCommand_GET_STATUS->CAL_PAG = 0;
    xcpCommand_GET_STATUS->reserve5 = 0;
    xcpCommand_GET_STATUS->DAQ = 0;
    xcpCommand_GET_STATUS->STIM = 0;
    xcpCommand_GET_STATUS->PGM = 0;
    xcpCommand_GET_STATUS->reserve6 = 0;
    xcpCommand_GET_STATUS->reserve7 = 0;
    xcpCommand_GET_STATUS->reserve8 = 0;

    xcpCommand_GET_STATUS->STATE_NUMBER = 0;

    xcpCommand_GET_STATUS->SESSION_CONFIGURATION_ID = 0;

    return 8;
}

static uint8_t XCP_COMMAND_SYNCH_252(tCanSerialPortFrameXCP *env) {
    env->response[0] = XCP_COMMAND_DISCONNECT;
    env->response[1] = XCP_ERR_CMD_SYNCH;

    return 2;
}

static uint8_t XCP_GET_COMM_MODE_INFO_251(tCanSerialPortFrameXCP *env) {

    eXcpCommand_GET_COMM_MODE_INFO *xcpCommand_GET_COMM_MODE_INFO = (eXcpCommand_GET_COMM_MODE_INFO *) env->response;

    xcpCommand_GET_COMM_MODE_INFO->RESERVED1 = 0;

    xcpCommand_GET_COMM_MODE_INFO->MASTER_BLOCK_MODE = 1;
    xcpCommand_GET_COMM_MODE_INFO->INTERLEAVED_MODE = 0;
    xcpCommand_GET_COMM_MODE_INFO->RESERVED2 = 0;

    xcpCommand_GET_COMM_MODE_INFO->RESERVED3 = 0;
    xcpCommand_GET_COMM_MODE_INFO->MAX_BS = 255;
    xcpCommand_GET_COMM_MODE_INFO->MIN_ST = 0;
    xcpCommand_GET_COMM_MODE_INFO->QUEUE_SIZE = 0;
    xcpCommand_GET_COMM_MODE_INFO->VERSION = 1;

    return 8;
}

static uint8_t XCP_GET_SEED_248(tCanSerialPortFrameXCP *env) {
    eXcpCommand_REQ_GET_SEED *request = (eXcpCommand_REQ_GET_SEED *) env->canFrame.data;

    eXcpCommand_GET_SEED *xcpCommand_GET_SEED = (eXcpCommand_GET_SEED *) env->response;
    xcpCommand_GET_SEED->COM = XCP_COMMAND_CONNECT;
    xcpCommand_GET_SEED->LEN = 4;
    xcpCommand_GET_SEED->KEY = 0x11223344;

    env->RND = request->RESOURCE;

    return 6;
}

static uint8_t XCP_UNLOCK_247(tCanSerialPortFrameXCP *env) {
    eXcpCommand_REQ_UNLOCK *request = (eXcpCommand_REQ_UNLOCK *) env->canFrame.data;

    uint32_t securitySeedMy = SeedToKeyHVAC_NAMI(env->RND, 0x11223344);

    if (securitySeedMy != request->KEY) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_ACCESS_LOCKED;
    } else {
        env->response[0] = XCP_COMMAND_CONNECT;
        env->response[1] = 0;
    }

    return 2;
}

static uint8_t XCP_COMMAND_SET_MTA_246(tCanSerialPortFrameXCP *env) {
    eXcpCommand_SET_MTA *request = (eXcpCommand_SET_MTA *) env->canFrame.data;

    if (((request->ADR < env->startRamAdr) || (request->ADR > env->endRamAdr)) &&
        ((request->ADR < env->startFlashAdr) || (request->ADR > env->endFlashAdr))) {

        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_ACCESS_DENIED;

        return 2;
    }

    env->ADR_EXT = request->ADR_EX;
    env->ADR_MTA = request->ADR;

    env->response[0] = XCP_COMMAND_CONNECT;

    return 1;
}

static uint8_t XCP_COMMAND_UPLOAD_245(tCanSerialPortFrameXCP *env) {
    eXcpCommand_Upload *request = (eXcpCommand_Upload *) env->canFrame.data;

    env->response[0] = XCP_COMMAND_CONNECT;

    uint8_t fullBlock = request->COUNT / 7;
    uint8_t tailBlock = request->COUNT % 7;

    for (uint8_t i = 0; i < fullBlock; ++i) {
        memcpy(&env->response[1], (uint8_t *) (env->ADR_MTA), request->COUNT);
        env->ADR_MTA += 7;
        env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, 7, 1000);
    }

    if (tailBlock > 0) {
        memcpy(&env->response[1], (uint8_t *) (env->ADR_MTA), tailBlock);
        env->ADR_MTA += tailBlock;
        env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, tailBlock, 1000);
    }

    return 0;
}

static uint8_t XCP_COMMAND_SHORT_UPLOAD_244(tCanSerialPortFrameXCP *env) {
    eXcpCommand_SHORT_UPLOAD *request = (eXcpCommand_SHORT_UPLOAD *) env->canFrame.data;

    if (request->COUNT > (MAX_CTO - 1)) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    env->response[0] = XCP_COMMAND_CONNECT;

    memcpy(&env->response[1], (uint8_t *) request->ADR, request->COUNT);

    return request->COUNT + 1;
}

static uint8_t XCP_COMMAND_DOWNLOAD_240(tCanSerialPortFrameXCP *env) {

    eXcpCommand_Download *request = (eXcpCommand_Download *) env->canFrame.data;

    uint8_t COUNT = env->canFrame.dlc - 2;

    memcpy((uint8_t *) (env->ADR_MTA), &env->response[2], COUNT);
    env->ADR_MTA += COUNT;

    env->response[0] = XCP_COMMAND_CONNECT;
    env->downloadRemaining = request->COUNT - COUNT;

    return 1;
}

static uint8_t XCP_COMMAND_DOWNLOAD_NEXT_239(tCanSerialPortFrameXCP *env) {

    eXcpCommand_Download *request = (eXcpCommand_Download *) env->canFrame.data;

    uint8_t COUNT = env->canFrame.dlc - 2;

    memcpy((uint8_t *) (env->ADR_MTA), &env->response[2], COUNT);
    env->ADR_MTA += COUNT;

    if (request->COUNT != env->downloadRemaining) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_SEQUENCE;
        env->response[2] = env->downloadRemaining;

        return 3;
    }

    env->downloadRemaining -= COUNT;

    if (env->downloadRemaining == 0) {
        env->response[0] = XCP_COMMAND_CONNECT;
        return 1;
    }

    return 0;
}

static uint8_t XCP_COMMAND_DOWNLOAD_MAX_238(tCanSerialPortFrameXCP *env) {

    uint8_t COUNT = env->canFrame.dlc - 1;

    memcpy((uint8_t *) (env->ADR_MTA), &env->response[1], COUNT);
    env->ADR_MTA += COUNT;

    env->response[0] = XCP_COMMAND_CONNECT;

    return 1;
}


static uint8_t XCP_CLEAR_DAQ_LIST_227(tCanSerialPortFrameXCP *env) {

    env->response[0] = XCP_COMMAND_CONNECT;

    return 1;
}


static uint8_t XCP_GET_DAQ_LIST_INFO_216(tCanSerialPortFrameXCP *env) {
    eXcpCommand_Get_DAQ_List_Info_Request *request = (eXcpCommand_Get_DAQ_List_Info_Request *) env->canFrame.data;

    if (request->DAQ_LIST_NUMBER > MAX_DAO) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    eXcpCommand_Get_DAQ_List_Info *xcpCommand_Get_DAQ_List_Info = (eXcpCommand_Get_DAQ_List_Info *) env->response;

    xcpCommand_Get_DAQ_List_Info->COM = XCP_COMMAND_CONNECT;

    xcpCommand_Get_DAQ_List_Info->PREDEFINED = 0;
    xcpCommand_Get_DAQ_List_Info->EVENT_FIXED = 0;
    xcpCommand_Get_DAQ_List_Info->DAQ = 1;
    xcpCommand_Get_DAQ_List_Info->STIM = 1;
    xcpCommand_Get_DAQ_List_Info->RESERVER = 0;

    xcpCommand_Get_DAQ_List_Info->MAX_ODT_ = MAX_ODT;
    xcpCommand_Get_DAQ_List_Info->MAX_ODT_ENTRIES_ = MAX_ODT_ENTRIES_SIZE;
    xcpCommand_Get_DAQ_List_Info->FIXED_EVENT = 0;

    return 6;
}

static uint8_t XCP_FREE_DAQ_214(tCanSerialPortFrameXCP *env) {
    env->DAQ_COUNT = 0;
    memset(env->DAQ, 0, sizeof(env->DAQ));

//    for (uint16_t i = 0; i < MAX_DAO; ++i) {

//        env->DAQ[i].SELECTED = 0;
//        env->DAQ[i].RUNNING = 0;
//        env->DAQ[i].ODT_COUNT = 0;

//        for (uint8_t j = 0; j < MAX_ODT; ++j) {
//            env->DAQ[i].ODT_NUMBER[j].ODT_ENTRIES_COUNT = 0;
//        }
//    }

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_ALLOC_DAQ_213(tCanSerialPortFrameXCP *env) {
    eXcpCommand_ALLOC_DAQ *request = (eXcpCommand_ALLOC_DAQ *) env->canFrame.data;

    env->DAQ_COUNT = request->DAQ_COUNT;

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_ALLOC_ODT_212(tCanSerialPortFrameXCP *env) {
    eXcpCommand_ALLOC_ODT *request = (eXcpCommand_ALLOC_ODT *) env->canFrame.data;

    env->DAQ[request->DAQ_LIST_NUMBER].ODT_COUNT = request->ODT_COUNT;

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}


static uint8_t XCP_ALLOC_ODT_ENTRY_211(tCanSerialPortFrameXCP *env) {
    eXcpCommand_ALLOC_ODT_ENTRY *request = (eXcpCommand_ALLOC_ODT_ENTRY *) env->canFrame.data;

    env->DAQ[request->DAQ_LIST_NUMBER].ODT_NUMBER[request->ODT_NUMBER].ODT_ENTRIES_COUNT = request->ODT_ENTRIES_COUNT;

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}


static uint8_t XCP_SET_DAQ_PTR_226(tCanSerialPortFrameXCP *env) {
    eXcpCommand_SET_DAQ_PTR *request = (eXcpCommand_SET_DAQ_PTR *) env->canFrame.data;

    if ((request->DAQ_LIST_NUMBER > MAX_DAO) ||
        (request->ODT_NUMBER > MAX_ODT) ||
        (request->ODT_ENTRY_NUMBER > MAX_ODT_ENTRIES_SIZE)) {

        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    env->DAQ_LIST_NUMBER = request->DAQ_LIST_NUMBER;
    env->ODT_NUMBER = request->ODT_NUMBER;
    env->ODT_ENTRY_NUMBER = request->ODT_ENTRY_NUMBER;


    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_WRITE_DAQ_225(tCanSerialPortFrameXCP *env) {
    eXcpCommand_WRITE_DAQ *request = (eXcpCommand_WRITE_DAQ *) env->canFrame.data;

    if (env->ODT_ENTRY_NUMBER > MAX_ODT_ENTRIES_SIZE) {

        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].BIT_OFFSET[env->ODT_ENTRY_NUMBER] = request->BIT_OFFSET;
    env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].ADR_EXT[env->ODT_ENTRY_NUMBER] = request->ADR_EXT;
    env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].ODT_ENTRIES_ADR[env->ODT_ENTRY_NUMBER] = request->ADR;
    env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].ODT_ENTRIES_SIZE[env->ODT_ENTRY_NUMBER] = request->ODT_ENTRY_SIZE;

    ++env->ODT_ENTRY_NUMBER;

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_SET_DAQ_LIST_MODE_224(tCanSerialPortFrameXCP *env) {
    eXcpCommand_SET_DAQ_LIST_MODE *request = (eXcpCommand_SET_DAQ_LIST_MODE *) env->canFrame.data;

    if ((request->DAQ_LIST_NUMBER > MAX_DAO) || (request->EVENT_CHANNEL_NUMBER > MAX_EVENT_CHANNEL)) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    env->DAQ[request->DAQ_LIST_NUMBER].EVENT_CHANNEL = request->EVENT_CHANNEL_NUMBER;

    env->DAQ[request->DAQ_LIST_NUMBER].ALTERNATING = request->ALTERNATING;
    env->DAQ[request->DAQ_LIST_NUMBER].DIRECTION = request->DIRECTION;
    env->DAQ[request->DAQ_LIST_NUMBER].DTO_CTR = request->DTO_CTR;
    env->DAQ[request->DAQ_LIST_NUMBER].TIMESTAMP = request->TIMESTAMP;
    env->DAQ[request->DAQ_LIST_NUMBER].PID_OFF = request->PID_OFF;

    env->DAQ[request->DAQ_LIST_NUMBER].TRANSMISSION_RATE_PRESCALER = request->TRANSMISSION_RATE_PRESCALER;
    env->DAQ[request->DAQ_LIST_NUMBER].DAQ_LIST_PRIORITY = request->DAQ_LIST_PRIORITY;

    env->DAQ[request->DAQ_LIST_NUMBER].PRESCALER_COUNTER = request->TRANSMISSION_RATE_PRESCALER;

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_START_STOP_DAQ_LIST_222(tCanSerialPortFrameXCP *env) {
    eXcpCommand_START_STOP_DAQ_LIST *request = (eXcpCommand_START_STOP_DAQ_LIST *) env->canFrame.data;

    if (request->DAQ_LIST_NUMBER > MAX_DAO) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    if (request->MODE == XCP_STOP) {
        env->DAQ[request->DAQ_LIST_NUMBER].RUNNING = 0;
    }

    if (request->MODE == XCP_START) {
        env->DAQ[request->DAQ_LIST_NUMBER].RUNNING = 1;
    }

    if (request->MODE == XCP_SELECT) {
        env->DAQ[request->DAQ_LIST_NUMBER].SELECTED = 1;
    }

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_START_STOP_SYNCH_221(tCanSerialPortFrameXCP *env) {
    eXcpCommand_START_STOP_SYNCH *request = (eXcpCommand_START_STOP_SYNCH *) env->canFrame.data;

    if (request->MODE == XCP_SYNC_START_SEL) {
        for (uint16 i = 0; i < MAX_DAO; ++i) {
            if (env->DAQ[i].SELECTED) {
                env->DAQ[i].RUNNING = 1;
            }
        }
    }

    if (request->MODE == XCP_SYNC_STOP_ALL) {
        for (uint16 i = 0; i < MAX_DAO; ++i) {
            env->DAQ[i].RUNNING = 0;
        }
    }

    if (request->MODE == XCP_SYNC_STOP_SEL) {
        for (uint16 i = 0; i < MAX_DAO; ++i) {
            if (env->DAQ[i].SELECTED) {
                env->DAQ[i].RUNNING = 1;
            }
        }
    }

    env->response[0] = XCP_COMMAND_CONNECT;
    return 2;
}

static uint8_t XCP_READ_DAQ_219(tCanSerialPortFrameXCP *env) {
    eXcpCommand_WRITE_DAQ *response = (eXcpCommand_WRITE_DAQ *) env->response;

    response->COM = XCP_COMMAND_CONNECT;
    response->BIT_OFFSET = env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].BIT_OFFSET[env->ODT_ENTRY_NUMBER];
    response->ADR_EXT = env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].ADR_EXT[env->ODT_ENTRY_NUMBER];
    response->ADR = env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].ODT_ENTRIES_ADR[env->ODT_ENTRY_NUMBER];
    response->ODT_ENTRY_SIZE = env->DAQ[env->DAQ_LIST_NUMBER].ODT_NUMBER[env->ODT_NUMBER].ODT_ENTRIES_SIZE[env->ODT_ENTRY_NUMBER];

    ++env->ODT_ENTRY_NUMBER;

    return 8;
}

static uint8_t XCP_GET_DAQ_PROCESSOR_INFO_218(tCanSerialPortFrameXCP *env) {

    eXcpCommand_GET_DAQ_PROCESSOR_INFO *response = (eXcpCommand_GET_DAQ_PROCESSOR_INFO *) env->response;

    response->COM = XCP_COMMAND_CONNECT;

    response->DAQ_CONFIG_TYPE = 1; // dynamic DAQ list configuration
    response->PRESCALER_SUPPORTED = 1; // prescaler supported
    response->RESUME_SUPPORTED = 0; // DAQ lists can not be set to RESUME mode
    response->BIT_STIM_SUPPORTED = 0; // bitwise data stimulation not supported
    response->TIMESTAMP_SUPPORTED = 0; // time-stamped mode not supported
    response->PID_OFF_SUPPORTED = 0; // Identification field can not be switched off
    response->OVERLOAD_MSB = 0; // No overload indication
    response->OVERLOAD_EVENT = 0; // No overload indication

    response->MAX_DAQ = MAX_DAO;
    response->MAX_EVENT_CHANNEL_ = MAX_EVENT_CHANNEL;
    response->MIN_DAQ = 0;

    response->Optimisation_Type_0 = 0;
    response->Optimisation_Type_1 = 1;
    response->Optimisation_Type_2 = 0;
    response->Optimisation_Type_3 = 1;
    response->Address_Extension_ODT = 0;
    response->Address_Extension_DAQ = 0;
    response->Identification_Field_Type_0 = 1; // Relative ODT number, absolute DAQ list number (BYTE)
    response->Identification_Field_Type_1 = 0; // Relative ODT number, absolute DAQ list number (BYTE)


    return 8;
}

static uint8_t XCP_GET_DAQ_RESOLUTION_INFO_217(tCanSerialPortFrameXCP *env) {
    eXcpCommand_GET_DAQ_RESOLUTION_INFO *response = (eXcpCommand_GET_DAQ_RESOLUTION_INFO *) env->response;

    response->COM = XCP_COMMAND_CONNECT;

    response->GRANULARITY_ODT_ENTRY_SIZE_DAQ = 1;
    response->MAX_ODT_ENTRY_SIZE_DAQ = MAX_ODT_ENTRIES_SIZE;
    response->GRANULARITY_ODT_ENTRY_SIZE_STIM = 1;
    response->MAX_ODT_ENTRY_SIZE_STIM = MAX_ODT_ENTRIES_SIZE;
    response->TIMESTAMP_MODE = 0;
    response->TIMESTAMP_TICKS = 0;

    return 8;
}


static uint8_t XCP_GET_DAQ_LIST_MODE_223(tCanSerialPortFrameXCP *env) {

    eXcpCommand_GET_DAQ_LIST_MODE_GET *request = (eXcpCommand_GET_DAQ_LIST_MODE_GET *) env->canFrame.data;

    if (request->DAQ_LIST_NUMBER > MAX_DAO) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    eXcpCommand_GET_DAQ_LIST_MODE *response = (eXcpCommand_GET_DAQ_LIST_MODE *) env->response;

    response->COM = XCP_COMMAND_CONNECT;

    response->SELECTED = env->DAQ[request->DAQ_LIST_NUMBER].SELECTED;
    response->DIRECTION = env->DAQ[request->DAQ_LIST_NUMBER].DIRECTION;
    response->RESERVED1 = 0;
    response->DTO_CTR = env->DAQ[request->DAQ_LIST_NUMBER].DTO_CTR;
    response->TIMESTAMP = env->DAQ[request->DAQ_LIST_NUMBER].TIMESTAMP;
    response->PID_OFF = env->DAQ[request->DAQ_LIST_NUMBER].PID_OFF;
    response->RUNNING = env->DAQ[request->DAQ_LIST_NUMBER].RUNNING;
    response->RESUME = 0;

    response->RESERVED2 = 0;
    response->CURRENT_EVENT_CHANNEL_NUMBER = env->DAQ[request->DAQ_LIST_NUMBER].EVENT_CHANNEL;
    response->CURRENT_PRESCALER = env->DAQ[request->DAQ_LIST_NUMBER].TRANSMISSION_RATE_PRESCALER;
    response->CURRENT_DAQ_LIST_PRIORITY = env->DAQ[request->DAQ_LIST_NUMBER].DAQ_LIST_PRIORITY;

    return 8;
}


static uint8_t XCP_GET_DAQ_EVENT_INFO_215(tCanSerialPortFrameXCP *env) {

    eXcpCommand_GET_DAQ_EVENT_INFO_GET *request = (eXcpCommand_GET_DAQ_EVENT_INFO_GET *) env->canFrame.data;

    if (request->EVENT_CHANNEL_NUMBER > MAX_EVENT_CHANNEL) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    eXcpCommand_GET_DAQ_EVENT_INFO *response = (eXcpCommand_GET_DAQ_EVENT_INFO *) env->response;

    response->COM = XCP_COMMAND_CONNECT;

    response->RESERVED1 = 0;
    response->RESERVED2 = 0;

    if (request->EVENT_CHANNEL_NUMBER == 0) { // DTO-STIM
        response->DAQ = 1;
        response->STM = 0;
    } else { // DTO-DAQ
        response->DAQ = 0;
        response->STM = 1;
    }

    response->RESERVED3 = 0;
    response->RESERVED4 = 0;
    response->CONSISTENCY_DAQ = 0;
    response->CONSISTENCY_EVENT = 0;

    response->MAX_DAQ_LIST = 255;
    response->EVENT_CHANNEL_NAME_LENGTH = 0;
    response->EVENT_CHANNEL_TIME_CYCLE = 1;
    response->EVENT_CHANNEL_TIME_UNIT = 7; // EVENT_CHANNEL _TIME_UNIT_10MS
    response->EVENT_CHANNEL_PRIORITY = 255;

    return 7;
}

static uint8_t XCP_GET_DAQ_STM(tCanSerialPortFrameXCP *env) {
    eXcpCommand_GET_DAQ_STM *response = (eXcpCommand_GET_DAQ_STM *) env->canFrame.data;

    uint8_t *COUNTER = &env->DAQ[response->DAQ].ODT_NUMBER[response->ODT].DATA_STM_COUNTER;

    memcpy(env->DAQ[response->DAQ].ODT_NUMBER[response->ODT].DATA_STM[*COUNTER].DATA, &env->canFrame.data[3],
           env->canFrame.dlc - 2);

    ++env->DAQ[response->DAQ].ODT_NUMBER[response->ODT].DATA_STM_COUNTER;

    if (*COUNTER == env->DAQ[response->DAQ].ODT_COUNT) {
        *COUNTER = 0;
        env->DAQ[response->DAQ].IS_STIM_RECEIVED = 1;
    }

    return 0;
}

static uint8_t XCP_PROGRAM_START_210(tCanSerialPortFrameXCP *env) {
    eXcpCommand_PROGRAM_START *response = (eXcpCommand_PROGRAM_START *) env->response;

    response->COM = XCP_COMMAND_CONNECT;
    response->RESERVED1 = 0;

    response->MASTER_BLOCK_MODE = PROGRAM_MASTER_BLOCK_MODE;
    response->INTERLEAVED_MODE = 0;
    response->RESERVED2 = 0;
    response->RESERVED3 = 0;
    response->RESERVED4 = 0;
    response->RESERVED5 = 0;
    response->SLAVE_BLOCK_MODE = 1;
    response->RESERVED6 = 0;

    response->MAX_CTO_PGM = 8;
    response->MAX_BS_PGM = 255; // Максимальный размер блока в режиме программирования. Определяет, сколько пакетов можно отправить подряд в блочном режиме
    response->MIN_ST_PGM = 1; // Минимальное время шага (Minimum Separation Time). Определяет минимальную паузу (в миллисекундах)
    response->QUEUE_SIZE_PGM = 10; // Размер очереди. Указывает, сколько пакетов типа CTO мастер может отправить "подряд"

    return 8;
}

static uint8_t XCP_PROGRAM_CLEAR_209(tCanSerialPortFrameXCP *env) {
    eXcpCommand_XCP_PROGRAM_CLEAR_209 *request = (eXcpCommand_XCP_PROGRAM_CLEAR_209 *) env->canFrame.data;

    if (request->MODE == 0) {

        if ((env->ADR_MTA < env->startFlashAdr) || ((env->ADR_MTA + request->CLEAR_RANGE) > env->endFlashAdr)) {

            env->response[0] = XCP_COMMAND_DISCONNECT;
            env->response[1] = XCP_ERR_ACCESS_DENIED;

            return 2;
        }

        env->clear_flash_func(env->ADR_MTA, request->CLEAR_RANGE);
    } else {

        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_PROGRAM_208(tCanSerialPortFrameXCP *env) {
    eXcpCommand_Program_ProgramNext_208_202 *request = (eXcpCommand_Program_ProgramNext_208_202 *) env->canFrame.data;

    uint8_t COUNT = env->canFrame.dlc - 2;

    env->write_flash_func(env->ADR_MTA, &env->canFrame.data[2], request->COUNT);

    env->ADR_MTA += request->COUNT;

#ifndef PROGRAM_MASTER_BLOCK_MODE
    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
#else
    if (request->COUNT == 0) {
        env->response[0] = XCP_COMMAND_CONNECT;
        return 1;
    }

    return 0;
#endif
}

static uint8_t XCP_PROGRAM_NEXT_202(tCanSerialPortFrameXCP *env) {
    eXcpCommand_Program_ProgramNext_208_202 *request = (eXcpCommand_Program_ProgramNext_208_202 *) env->canFrame.data;

    uint8_t COUNT = env->canFrame.dlc - 2;

    env->write_flash_func(env->ADR_MTA, &env->canFrame.data[2], request->COUNT);

    env->ADR_MTA += request->COUNT;

    return 0;
}

static uint8_t XCP_PROGRAM_RESET_207(tCanSerialPortFrameXCP *env) {
    env->response[0] = XCP_COMMAND_CONNECT;
    return 1;
}

static uint8_t XCP_GET_PGM_PROCESSOR_INFO_206(tCanSerialPortFrameXCP *env) {
    eXcpCommand_PGM_PROCESSOR_INFO_206 *response = (eXcpCommand_PGM_PROCESSOR_INFO_206 *) env->response;

    response->COM = XCP_COMMAND_CONNECT;

    response->ABSOLUTE_MODE = 1;
    response->FUNCTIONAL_MODE = 0;
    response->COMPRESSION_SUPPORTED = 0;
    response->COMPRESSION_REQUIRED = 0;
    response->ENCRYPTION_SUPPORTED = 0;
    response->ENCRYPTION_REQUIRED = 0;
    response->NON_SEQ_PGM_SUPPORTED = 0;
    response->NON_SEQ_PGM_REQUIRED = 0;

    response->MAX_SECTOR = PROGRAM_MAX_SECTOR;

    return 3;
}

static uint8_t XCP_GET_SECTOR_INFO_205(tCanSerialPortFrameXCP *env) {
    eXcpCommand_REQ_GET_SECTOR_INFO_205 *request = (eXcpCommand_REQ_GET_SECTOR_INFO_205 *) env->canFrame.data;

    if (request->SECTOR_NUMBER > PROGRAM_MAX_SECTOR) {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_OUT_OF_RANGE;

        return 2;
    }

    eXcpCommand_RESP_GET_SECTOR_INFO_205 *response = (eXcpCommand_RESP_GET_SECTOR_INFO_205 *) env->response;\
    response->COM = XCP_COMMAND_CONNECT;
    response->CLEAR_SEQUENCE_NUMBER = request->SECTOR_NUMBER;
    response->PROGRAM_SEQUENCE_NUMBER = request->SECTOR_NUMBER;

    response->ABSOLUTE_MODE = 1;
    response->FUNCTIONAL_MODE = 0;
    response->PAGED_MODE = 0;
    response->SEQUENTIAL_ONLY = 1;
    response->ERASE_BEFORE_WRITE = 1;
    response->VERIFY_AFTER_WRITE = 0;
    response->RESERVED1 = 0;
    response->RESERVED2 = 0;

    if (request->MODE == 0) {
        response->SECTOR_INFO = request->SECTOR_NUMBER * FLASH_PAGE_SIZE + env->startFlashAdr;
    } else if (request->MODE == 1) {
        response->SECTOR_INFO = FLASH_PAGE_SIZE;
    } else if (request->MODE == 2) {
        env->response[1] = 0;
        return 2;
    } else {
        env->response[0] = XCP_COMMAND_DISCONNECT;
        env->response[1] = XCP_ERR_CMD_SYNTAX;
        return 2;
    }

    return 8;
}

const xcp_func_ptr fXcp_func_ptr[256] = {
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        XCP_PROGRAM_NEXT_202,
        NULL,
        NULL,
        XCP_GET_SECTOR_INFO_205,
        XCP_GET_PGM_PROCESSOR_INFO_206,
        XCP_PROGRAM_RESET_207,
        XCP_PROGRAM_208,
        XCP_PROGRAM_CLEAR_209,
        XCP_PROGRAM_START_210,
        XCP_ALLOC_ODT_ENTRY_211,
        XCP_ALLOC_ODT_212,
        XCP_ALLOC_DAQ_213,
        XCP_FREE_DAQ_214,
        XCP_GET_DAQ_EVENT_INFO_215,
        XCP_GET_DAQ_LIST_INFO_216,
        XCP_GET_DAQ_RESOLUTION_INFO_217,
        XCP_GET_DAQ_PROCESSOR_INFO_218,
        XCP_READ_DAQ_219,
        NULL,
        XCP_START_STOP_SYNCH_221,
        XCP_START_STOP_DAQ_LIST_222,
        XCP_GET_DAQ_LIST_MODE_223,
        XCP_SET_DAQ_LIST_MODE_224,
        XCP_WRITE_DAQ_225,
        XCP_SET_DAQ_PTR_226,
        XCP_CLEAR_DAQ_LIST_227,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        XCP_COMMAND_DOWNLOAD_MAX_238,
        XCP_COMMAND_DOWNLOAD_NEXT_239,
        XCP_COMMAND_DOWNLOAD_240,
        NULL,
        NULL,
        NULL,
        XCP_COMMAND_SHORT_UPLOAD_244,
        XCP_COMMAND_UPLOAD_245,
        XCP_COMMAND_SET_MTA_246,
        XCP_UNLOCK_247,
        XCP_GET_SEED_248,
        NULL,
        NULL,
        XCP_GET_COMM_MODE_INFO_251,
        XCP_COMMAND_SYNCH_252,
        XCP_COMMAND_GET_STATUS_253,
        XCP_COMMAND_DISCONNECT_254,
        XCP_COMMAND_CONNECT_255
};


uint8_t vCanSerialPortFrameXcpReceive(tCanSerialPortFrameXCP *env, uint32 timeout) {
    uint8_t response_size = 0;

    uint16_t recv = env->ioCanFrame->receive(env->ioCanFrame->env, PROTOCOL_XCP, (uint8_t *) &env->canFrame, 1,
                                             timeout);

    if (recv == 0)
        return 0;

    if (osMutexAcquire(env->access, 100) == osOK) {

        eXcpTypeCommand com = env->canFrame.data[0];

        if (com > 0xBF) { // CMD

            if (fXcp_func_ptr[com] != NULL) {

                response_size = fXcp_func_ptr[com](env);

                if (response_size) {
                    env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, response_size, timeout);
                }

            } else {

            }

        } else { // ODT
            XCP_GET_DAQ_STM(env);
        }

        osMutexRelease(env->access);
    }

    return response_size;
}

_Noreturn void CanXcpProcessing_Service_Dto_Stim_Task(tCanSerialPortFrameXCP *env) {
    uint8_t size;

    while (1) {

        if (osMutexAcquire(env->access, 100) == osOK) {

            for (uint16 i = 0; i < env->DAQ_COUNT; ++i) {
                if (env->DAQ[i].RUNNING) {


                    if ((env->DAQ[i].EVENT_CHANNEL == 0) && (env->DAQ[i].PRESCALER_COUNTER == 0)) {

                        env->DAQ[i].PRESCALER_COUNTER = env->DAQ[i].TRANSMISSION_RATE_PRESCALER;

                        if (env->DAQ[i].DIRECTION == 1) { // STIM - DTO

                            if (env->DAQ[i].IS_STIM_RECEIVED) {

                                for (uint8_t j = 0; j < env->DAQ[i].ODT_COUNT; ++j) {

                                    size = 0;

                                    for (uint8_t k = 0; k < MAX_ODT_ENTRIES_SIZE; ++k) {

                                        uint8_t ODT_ENTRIES_SIZE = env->DAQ[i].ODT_NUMBER[j].ODT_ENTRIES_SIZE[k];

                                        if (ODT_ENTRIES_SIZE) {

                                            memcpy(&env->DATA[size],
                                                   env->DAQ[i].ODT_NUMBER[j].DATA_STM[k].DATA,
                                                   ODT_ENTRIES_SIZE);

                                            size += ODT_ENTRIES_SIZE;

                                        }

                                    }

                                    uint8_t size_stim = 0;

                                    for (uint8_t k = 0; k < MAX_ODT_ENTRIES_SIZE; ++k) {


                                        uint8_t ODT_ENTRIES_SIZE = env->DAQ[i].ODT_NUMBER[j].ODT_ENTRIES_SIZE[k];

                                        if (ODT_ENTRIES_SIZE) {

                                            if ((size_stim + ODT_ENTRIES_SIZE) <= size) {
                                                memcpy((uint8_t *) env->DAQ[i].ODT_NUMBER[j].ODT_ENTRIES_ADR[k],
                                                       &env->DAQ[i].ODT_NUMBER[j].DATA_STM[k].DATA[size_stim],
                                                       ODT_ENTRIES_SIZE);

                                                size_stim += ODT_ENTRIES_SIZE;
                                            } else {
                                                asm("nop");
                                            }

                                        }


                                    }


                                }

                                env->DAQ[i].IS_STIM_RECEIVED = 0;

                            } else {
                                asm("nop");
                            }
                        } else {
                            asm("nop");
                        }

                    } else {
                        --env->DAQ[i].PRESCALER_COUNTER;
                    }


                }


            }

            osMutexRelease(env->access);
        }

        SystemDelayMs(10);
    }


}


_Noreturn void CanXcpProcessing_Service_Dto_Daq_Task(tCanSerialPortFrameXCP *env) {
    uint8_t size;

    while (1) {

        if (osMutexAcquire(env->access, 100) == osOK) {

            for (uint16 i = 0; i < env->DAQ_COUNT; ++i) {
                if (env->DAQ[i].RUNNING) {


                    if ((env->DAQ[i].EVENT_CHANNEL == 1) && (env->DAQ[i].PRESCALER_COUNTER == 0)) {

                        env->DAQ[i].PRESCALER_COUNTER = env->DAQ[i].TRANSMISSION_RATE_PRESCALER;

                        if (env->DAQ[i].DIRECTION == 0) { // DAQ - DTO
                            for (uint8_t j = 0; j < env->DAQ[i].ODT_COUNT; ++j) {

                                env->response[0] = j; // ODT
                                env->response[1] = i; // DAQ
                                size = 0;

                                for (uint8_t k = 0; k < MAX_ODT_ENTRIES_SIZE; ++k) {

                                    uint8_t ODT_ENTRIES_SIZE = env->DAQ[i].ODT_NUMBER[j].ODT_ENTRIES_SIZE[k];

                                    if (ODT_ENTRIES_SIZE) {

                                        memcpy(&env->DATA[size],
                                               (uint8_t *) env->DAQ[i].ODT_NUMBER[j].ODT_ENTRIES_ADR[k],
                                               ODT_ENTRIES_SIZE);

                                        size += ODT_ENTRIES_SIZE;

                                    }

                                }

                                uint8_t count_size = size / BODY_MAX_LEN;
                                uint8_t count_size_tail = size % BODY_MAX_LEN;

                                size = 0;

                                for (uint8_t k = 0; k < count_size; ++k) {
                                    memcpy(&env->response[2], &env->DATA[size], BODY_MAX_LEN);
                                    env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, MAX_DTO, 1000);
                                    size += BODY_MAX_LEN;
                                }

                                if (count_size_tail) {
                                    memcpy(&env->response[2], &env->DATA[size], count_size_tail);
                                    env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, count_size_tail,
                                                              1000);
                                }


                            }
                        } else {
                            asm("nop");
                        }

                    } else {
                        --env->DAQ[i].PRESCALER_COUNTER;
                    }


                }


            }

            osMutexRelease(env->access);
        }

        SystemDelayMs(10);
    }


}

void CanXcpProcessing_Service_Start(tCanSerialPortFrameXCP *env) {
    ThreadBlock_Start(env->T_can_Listener_Dto_Stim_XCP, env, CanXcpProcessing_Service_Dto_Stim_Task);
    ThreadBlock_Start(env->T_can_Listener_Dto_Daq_XCP, env, CanXcpProcessing_Service_Dto_Daq_Task);
}

_Noreturn void CanXcpProcessing_ListenerTask(tCanSerialPortFrameXCP *env) {
    while (1) {
        vCanSerialPortFrameXcpReceive(env, 1000);
    }
}

void CanXcpProcessing_Listener_Start(tCanSerialPortFrameXCP *env) {
    ThreadBlock_Start(env->T_can_Listener_XCP, env, CanXcpProcessing_ListenerTask);
    CanXcpProcessing_Service_Start(env);
}