/*
 * Copyright 2024 Yuntu Microelectronics co.,ltd
 * All rights reserved.
 *
 * YUNTU Confidential. This software is owned or controlled by YUNTU and may
 * only be used strictly in accordance with the applicable license terms. By expressly
 * accepting such terms or by downloading, installing, activating and/or otherwise
 * using the software, you are agreeing that you have read, and that you agree to
 * comply with and are bound by, such license terms. If you do not agree to be
 * bound by the applicable license terms, then you may not retain, install,
 * activate or otherwise use the software. The production use license in
 * Section 2.3 is expressly granted for this software.
 */


#include <stdint.h>
#include <stddef.h>
#include "uds.h"
#include "device_registers.h"

/* UDS negative response*/
#define UDS_RESPONSE_NEGATIVE   (0x7Fu)
/* UDS positive response offset */
#define UDS_RESPONSE_OFFSET     (0x40)
/* UDS invalid/empty index */
#define UDS_INVALID_PENDING_INDEX (-1)


extern uds_u8_t * uds_receivedData[UDS_CHANNEL_NUM];
extern uds_u8_t * uds_transmitData[UDS_CHANNEL_NUM];

extern uds_bool_t resetCommand;
/**
 * @brief Uds state structure
 * 
 */
static uds_state_t uds_state[UDS_CHANNEL_NUM];


#ifdef UDS_ENABLE_CAN
/**
 * @brief The function changes the instance id of can_stack to the uds channel id.
 * 
 * @param CanTpId Channel id in Can_Stack
 * @return uds_channel_t Uds channel
 */
static uds_channel_t Uds_CanTpInstToUdsChannel(pdu_id_type CanTpId) {
    uds_channel_t channel = 0;
    for(; channel < UDS_CHANNEL_NUM; channel++) {
        if(uds_config[channel].channelType == UDS_CANTP)
        {
            if(uds_config[channel].channelId == CanTpId)
            {
                break;
            }
        }
    }
    return channel;
}
#endif


/**
 * @brief The function checks whether the received data is in the configuration.
 * 
 * @param channel Uds channel
 * @param data Data to match
 * @param dataLen Data length
 * @param matchId Match index in config
 * @return uds_bool_t Match result
 */
static uds_bool_t Uds_MatchData(uds_channel_t channel, uds_u8_t *data, uds_u16_t dataLen, uds_u32_t * const matchId)
{
    uds_bool_t matched = uds_false;

    for(uds_u32_t i = 0; i < uds_config[channel].ruleConfigNum; i++)
    {
        if(uds_config[channel].ruleConfig[i].rxMatchLength <= dataLen)
        {
            uds_u32_t j;
            for(j = 0; j < uds_config[channel].ruleConfig[i].rxMatchLength; j++)
            {
                if((data[j]&uds_config[channel].ruleConfig[i].rxMask[j]) != ((uds_config[channel].ruleConfig[i].rxData[j]) & (uds_config[channel].ruleConfig[i].rxMask[j])))
                {
                    break;
                }
            }
            if(j == uds_config[channel].ruleConfig[i].rxMatchLength)
            {
                matched = uds_true;
                *matchId = i;
                break;
            }
        }
       
    }
    return matched;
}

/**
 * @brief This function send the data by calling the sending function of Tp layer.
 * 
 * @param channel Uds channel
 * @param data Data to send
 * @param dataLen Data length
 * @return uds_bool_t 
 */
static uds_bool_t Uds_SendData(uds_channel_t channel, const uds_u8_t *data, uds_u16_t dataLen)
{
    uds_bool_t sent=uds_false;

    if(uds_config[channel].channelType == UDS_LINTP)
    {
#ifdef UDS_ENABLE_LIN
        l_ifc_handle inst = (l_ifc_handle)uds_config[channel].channelId;
        if(ld_tx_status(uds_config[channel].channelId) != LD_IN_PROGRESS) {
            ld_send_message(inst, (l_u16)dataLen, 0, (const l_u8*)data);
            uds_state[channel].channelState=UDS_SENDING;
            sent=uds_true;
        }
#endif
    } else if (uds_config[channel].channelType == UDS_CANTP)
    {
#ifdef UDS_ENABLE_CAN
        pdu_info_type_t sendPdu;
        sendPdu.sduLength = dataLen;
        sendPdu.metaDataPtr = NULL;
        sendPdu.sduDataPtr = (cantp_u8*)data;
        if(CanTp_Transmit(uds_config[channel].channelId, &sendPdu) == CANTP_E_OK) {
            uds_state[channel].channelState = UDS_SENDING;
            sent=uds_true;
        }
#endif
    }
    if(sent==uds_false)
    {
        uds_state[channel].channelState = UDS_IDLE;
        if(uds_config[channel].errorCallback != NULL) {
            uds_config[channel].errorCallback(channel, UDS_TX_ERROR);
        }

    }
    return sent;
}

/**
 * @brief This function checks whether the current session can be request.
 * 
 * @param channel Uds channel
 * @param supportedSessions sessions to check
 * @return uds_bool_t 
 */
static uds_bool_t Uds_IsSessionCanRequest(uds_channel_t channel, uds_u32_t supportedSessions)
{
    uds_bool_t status = uds_false;

    if ((supportedSessions & uds_state[channel].sessionMode) == uds_state[channel].sessionMode)
    {
        status = uds_true;
    }
    else
    {
        status = uds_false;
    }

    return status;
}

/**
 * @brief This function starts the s3 timer.
 * 
 * @param channel Uds channel
 */
static void Uds_StartS3Server(uds_channel_t channel)
{
    if(uds_state[channel].sessionMode != UDS_SESSION_DEFAULT)
    {
        uds_state[channel].serverTimerEnable = uds_true;
    }
}

/**
 * @brief This function stops the s3 timer.
 * 
 * @param channel Uds channel
 */
static void Uds_StopS3Server(uds_channel_t channel)
{
    uds_state[channel].serverTimerEnable = uds_false;
}

/**
 * @brief This function starts the s3 timer and refresh the value.
 * 
 * @param channel Uds channel
 */
static void Uds_RestartS3Server(uds_channel_t channel)
{
    if(uds_state[channel].sessionMode != UDS_SESSION_DEFAULT)
    {
        uds_state[channel].serverTimerEnable = uds_true;
        uds_state[channel].curServerTime = uds_config[channel].s3ServerTime;
    }
}

/**
 * @brief This function checks whether the current secure level can be request.
 * 
 * @param channel Uds channel
 * @param supportedSecureLevels Secure levels to check
 * @return uds_bool_t 
 */
static uds_bool_t Uds_IsSecureLevelCanRequest(uds_channel_t channel, const uds_u32_t supportedSecureLevels)
{
    uds_bool_t status = uds_false;

    if ((supportedSecureLevels & uds_state[channel].secureLevel) == uds_state[channel].secureLevel)
    {
        status = uds_true;
    }
    else
    {
        status = uds_false;
    }
    return status;
}

/**
 * @brief The function is called by Uds_MainFunction to check the received data.
 * 
 * @param channel Uds channel
 */
static void  Uds_MatchProcess(uds_channel_t channel) {
    uds_u32_t matchId = UDS_INVALID_PENDING_INDEX;
    uds_bool_t matched = uds_false;

    if(uds_state[channel].pendingDataIndex != UDS_INVALID_PENDING_INDEX)
    {
        matched = uds_true;
        matchId = uds_state[channel].pendingDataIndex;
    }
    else
    {
        /* first received*/
        if(uds_config[channel].rxCallback != NULL)
        {
            uds_config[channel].rxCallback(channel, uds_state[channel].receivedData, uds_state[channel].receivedDataLength);
        }
        matched = Uds_MatchData(channel, uds_state[channel].receivedData, uds_state[channel].receivedDataLength, &matchId);
    }
    /*First Data is SID */
    uds_sid_t sid = uds_state[channel].receivedData[0];
    if(matched == uds_false)
    {
        Uds_SendNegativeResponse(channel, sid, UDS_NRC_SNS,NULL,0);
    }
    else
    {
        uds_state[channel].pendingDataIndex = matchId;
        if((Uds_IsSessionCanRequest(channel, uds_config[channel].ruleConfig[matchId].sessionMask)) == uds_false)
        {
            /* The service can not be requested in active session, send negative response. */
            Uds_SendNegativeResponse(channel, sid, UDS_NRC_SNSIAS, NULL, 0);
            return;
        }
        else if((Uds_IsSecureLevelCanRequest(channel, uds_config[channel].ruleConfig[matchId].secureLevelMask)) == uds_false)
        {
            /* Security access denied, send negative response. */
            Uds_SendNegativeResponse(channel, sid, UDS_NRC_SAD, NULL, 0);
        } else {
            if(uds_config[channel].ruleConfig[matchId].callback != NULL)
            {
                uds_config[channel].ruleConfig[matchId].callback(channel, uds_state[channel].receivedData, uds_state[channel].receivedDataLength,uds_state[channel].pendingParam);
            } else {
                Uds_NoResponse(channel);
            }
        }
    }
}


/**
 * @brief This function initialize the uds state.
 * 
 */
void Uds_Init(void)
{
    Uds_ConfigInit();
    for(uds_channel_t channel = 0; channel < UDS_CHANNEL_NUM; channel++)
    {
        uds_state[channel].channel = channel;
        uds_state[channel].channelId = uds_config[channel].channelId;
        uds_state[channel].sessionMode = UDS_SESSION_DEFAULT;
        uds_state[channel].serverTimerEnable = uds_false;
        uds_state[channel].curServerTime = (uds_s32_t)uds_config[channel].s3ServerTime;
        uds_state[channel].secureLevel = UDS_SECURITY_LEVEL_NULL;
        uds_state[channel].channelState = UDS_IDLE;
        uds_state[channel].receivedDataLength = uds_config[channel].bufferSize;
        uds_state[channel].receivedData = uds_receivedData[channel];
        uds_state[channel].transmitData = uds_transmitData[channel];
        uds_state[channel].pendingDataIndex = UDS_INVALID_PENDING_INDEX;
        uds_state[channel].pendingParam = NULL;
        uds_state[channel].pendingNum = 0;
    }
}

/**
 * @brief This function send the positive response.
 * 
 * @param channel Uds channel
 * @param sid Service id
 * @param data Data to send
 * @param dataLen Data length
 * @return uds_bool_t 
 */
uds_bool_t Uds_SendPositiveResponse(uds_channel_t channel, const uds_sid_t sid, const uds_u8_t *data, const uds_u32_t dataLen)
{
    uds_bool_t ret = uds_false;
    uds_u8_t *sendData=uds_state[channel].transmitData;
    if(uds_state[channel].channelState == UDS_PROCESSING)
    {
        uds_state[channel].pendingDataIndex = UDS_INVALID_PENDING_INDEX;
        if(dataLen+1 > uds_config[channel].bufferSize)
        {
            if(uds_config[channel].errorCallback != NULL) {
                uds_config[channel].errorCallback(channel,UDS_PRE_TX_ERROR);
            }
            /*responseTooLong*/
            Uds_SendNegativeResponse(channel, sid, UDS_NRC_RTL, NULL, 0);
        } else {
            *sendData = sid + UDS_RESPONSE_OFFSET;
            for(uds_u32_t dataIndex = 0; dataIndex < dataLen; dataIndex++)
            {
                sendData[dataIndex+1] = data[dataIndex];
            }
            ret=Uds_SendData(channel,sendData,dataLen+1);
        }

    }
    return ret;
}

/**
 * @brief This function called when no response need to send.
 * 
 * @param channel Uds channel
 * @return uds_bool_t 
 */
uds_bool_t Uds_NoResponse(uds_channel_t channel)
{
    uds_bool_t ret = uds_false;

    if(uds_state[channel].channelState == UDS_PROCESSING)
    {
        uds_state[channel].channelState = UDS_IDLE;
        Uds_RestartS3Server(channel);
        ret = uds_true;
        if(resetCommand == uds_true)
        {
            NVIC_SystemReset();
        }
    }
    return ret;
}

/**
 * @brief This function send the negative response.
 * 
 * @param channel Uds channel
 * @param sid Service id
 * @param nrc Negative response code
 * @param data Data to send
 * @param dataLen Data length
 * @return uds_bool_t 
 */
uds_bool_t Uds_SendNegativeResponse(uds_channel_t channel, const uds_sid_t sid, uds_nrc_t nrc, const uds_u8_t *data, const uds_u32_t dataLen)
{
    uds_bool_t ret = uds_false;
    uds_u8_t *sendData=uds_state[channel].transmitData;
    if(uds_state[channel].channelState == UDS_PROCESSING)
    {
        if(dataLen+2 > uds_config[channel].bufferSize)
        {
            if(uds_config[channel].errorCallback != NULL) {
                uds_config[channel].errorCallback(channel,UDS_PRE_TX_ERROR);
            }
            /*responseTooLong*/
            Uds_SendNegativeResponse(channel, sid, UDS_NRC_RTL, NULL, 0);
        } else {
            if(nrc != UDS_NRC_RCRRP)
            {
                uds_state[channel].pendingDataIndex = UDS_INVALID_PENDING_INDEX;
            } else {
                uds_state[channel].pendingNum++;
            }

            *sendData = UDS_RESPONSE_NEGATIVE;
            sendData[1] = sid;
            sendData[2] = nrc;
            for(uds_u32_t dataIndex = 0; dataIndex < dataLen; dataIndex++)
            {
                sendData[dataIndex+3] = data[dataIndex];
            }
            ret=Uds_SendData(channel,sendData,dataLen+3);
        }
    }
    return ret;
}

/**
 * @brief This function is used to set the active session of the channel.
 * 
 * @param channel Uds channel
 * @param sessionMode Session mode
 */
void Uds_SetSession(uds_channel_t channel, const uds_u8_t sessionMode)
{
    uds_u32_t newSessionMode;
    /* support session number from 1-32*/
    if(sessionMode>0&&sessionMode<=32) {
        if(sessionMode == UDS_SESSION_DEFAULT)
        {
            Uds_StopS3Server(channel);
        }
        newSessionMode=1<<(sessionMode-1);
        if(newSessionMode != uds_state[channel].sessionMode)
        {
            uds_state[channel].sessionMode = newSessionMode;
            uds_state[channel].secureLevel = UDS_SECURITY_LEVEL_NULL;
        }
      
    }
}

/**
 * @brief This function is used to get the active session of the channel.
 * 
 * @param channel Uds channel
 * @return uds_u8_t 
 */
uds_u8_t Uds_GetSession(uds_channel_t channel)
{
    uds_u32_t session = UDS_SESSION_DEFAULT;
    uds_u8_t cnt = 0;
    if(uds_state[channel].sessionMode > UDS_SESSION_DEFAULT) {
        uds_u32_t tmp = uds_state[channel].sessionMode;
        while (tmp > 1)
        {
            tmp >>= 1;
            cnt++;
        }
        session = cnt+1;
    }
    return session;
}

/**
 * @brief This function is used to service s3 timer.
 * 
 * @param expireMillisecond Called period
 */
void Uds_TimeService(uds_u32_t expireMillisecond)
{
    for(uds_channel_t channel = 0; channel < UDS_CHANNEL_NUM; channel++)
    {
        if (uds_state[channel].serverTimerEnable == uds_true)
        {
            if(uds_state[channel].curServerTime > 0)
            {
                uds_state[channel].curServerTime -= expireMillisecond;
            }
            else
            {
                uds_state[channel].serverTimerEnable = uds_false;
                uds_state[channel].curServerTime = 0;
                uds_state[channel].sessionMode = UDS_SESSION_DEFAULT;
                uds_state[channel].secureLevel = UDS_SECURITY_LEVEL_NULL;
            }
        }
    }
}

/**
 * @brief This function is used to set the param of service callback function.
 * 
 * @param channel Uds channel
 * @param param Param
 * @return uds_bool_t 
 */
uds_bool_t Uds_SetupTmpParam(uds_channel_t channel, void *param)
{
    uds_bool_t ret = uds_false;
    if(uds_state[channel].channelState == UDS_PROCESSING)
    {
        uds_state[channel].pendingParam = param;
        ret = uds_true;
    }
    return ret;
}

/**
 * @brief This function is used to set the secure level of the channel.
 * 
 * @param channel Uds channel
 * @param secureLevel Secure level
 */
void Uds_SetSecureLevel(uds_channel_t channel, const uds_u32_t secureLevel)
{
    /* support secureLevel number from 1-32*/
    if(secureLevel>0&&secureLevel<=32) {
        uds_state[channel].secureLevel = 1<<secureLevel;
    }
}

/**
 * @brief This function is used to get the secure level of the channel.
 * 
 * @param channel Uds channel
 * @return uds_u32_t 
 */
uds_u32_t Uds_GetSecureLevel(uds_channel_t channel)
{
    uds_u32_t secureLevel = UDS_SECURITY_LEVEL_NULL;
    uds_u8_t cnt = 0;
    if(uds_state[channel].secureLevel > UDS_SECURITY_LEVEL_NULL) {
        uds_u32_t tmp = uds_state[channel].secureLevel;
        while (tmp > 1)
        {
            tmp >>= 1;
            cnt++;
        }
        secureLevel = cnt;
    }
    return secureLevel;
}


#ifdef UDS_ENABLE_CAN
/**
 * @brief This function is the transmit callback of can_stack.
 * 
 * @param txSduId The can_stack channel id.
 * @param res Result of transmit.
 */
void Uds_CanTpTxConfirm(pdu_id_type txSduId, cantp_core_res_type res)
{
    uds_channel_t channel = Uds_CanTpInstToUdsChannel(txSduId);

    if(UDS_SENDING == uds_state[channel].channelState)
    {
        if(uds_state[channel].pendingDataIndex == UDS_INVALID_PENDING_INDEX) {
            uds_state[channel].channelState = UDS_IDLE;
        } else {
            /* 0x78, processing again */
            uds_state[channel].channelState = UDS_PROCESSING;
        }
        if(res != CANTP_CORE_N_OK)
        {
            Uds_StartS3Server(channel);
            uds_state[channel].channelState = UDS_IDLE;
            if(uds_config[channel].errorCallback!=NULL) {
                uds_config[channel].errorCallback(channel, UDS_TX_ERROR);
            }

        } else {
            Uds_RestartS3Server(channel);
        }
        if(resetCommand == uds_true)
        {
            NVIC_SystemReset();
        }
    } else {
        /* never be here */
    }
}
/**
 * @brief This function is the receive callback of can_stack.
 * 
 * @param txSduId The can_stack channel id.
 * @param res Result of receive.
 */
void Uds_CanTpRxIndicate(pdu_id_type txSduId, cantp_core_res_type res) {
    uds_channel_t channel = Uds_CanTpInstToUdsChannel(txSduId);
    if(uds_state[channel].channelState == UDS_IDLE) {
        if(res==CANTP_CORE_N_OK) {
            uds_state[channel].channelState = UDS_RECEIVING;
            Uds_StopS3Server(channel);
        } else {
            if(uds_config[channel].errorCallback!=NULL) {
                uds_config[channel].errorCallback(channel,UDS_RX_ERROR);
            }
        }

    }

}
#endif


/**
 * @brief Main function of uds.
 * 
 */
void Uds_MainFunction(void)
{
    for(uds_channel_t channel = 0; channel < UDS_CHANNEL_NUM; channel++)
    {
        if(uds_config[channel].channelType == UDS_LINTP)
        {
#ifdef UDS_ENABLE_LIN
            l_u8 linStatus;
            l_ifc_handle inst = (l_ifc_handle)uds_config[channel].channelId;

            switch(uds_state[channel].channelState) {
            case UDS_IDLE:
                linStatus=ld_rx_status(uds_config[channel].channelId);
                if(linStatus != LD_IN_PROGRESS)
                {
                    /* start recv, UDS server work in lin slave node, nad is unused */
                    uds_state[channel].channelState = UDS_RECEIVING;
                    uds_state[channel].receivedDataLength = uds_config[channel].bufferSize;
                    ld_receive_message(inst, &uds_state[channel].receivedDataLength, NULL, uds_state[channel].receivedData);
                }
                break;
            case UDS_RECEIVING:
                linStatus=ld_rx_status(uds_config[channel].channelId);
                if(linStatus == LD_COMPLETED)
                {
                    uds_state[channel].pendingDataIndex = UDS_INVALID_PENDING_INDEX;
                    uds_state[channel].channelState = UDS_PROCESSING;
                    uds_state[channel].pendingNum = 0;
                    Uds_StopS3Server(channel);

                } else if(linStatus == LD_IN_PROGRESS) {
                    /* nothing to do */
                } else {
                    /* failed*/
                    uds_state[channel].channelState = UDS_IDLE;
                    if(uds_config[channel].errorCallback!=NULL) {
                        uds_config[channel].errorCallback(channel,UDS_RX_ERROR);
                    }
                }
                break;
            case UDS_PROCESSING:
                Uds_MatchProcess(channel);
                break;
            case UDS_SENDING:
                linStatus=ld_tx_status(uds_config[channel].channelId);
                if(linStatus != LD_IN_PROGRESS)
                {
                    if(uds_state[channel].pendingDataIndex==UDS_INVALID_PENDING_INDEX) {
                        uds_state[channel].channelState = UDS_IDLE;
                    } else {
                        /* 0x78, processing again */
                        uds_state[channel].channelState = UDS_PROCESSING;
                    }
                    if(linStatus==LD_COMPLETED) {
                        Uds_RestartS3Server(channel);
                        if(resetCommand == uds_true) {
                            NVIC_SystemReset();
                        }
                    } else {
                        Uds_StartS3Server(channel);
                        uds_state[channel].channelState = UDS_IDLE;
                        if(uds_config[channel].errorCallback!=NULL) {
                            uds_config[channel].errorCallback(channel,UDS_TX_ERROR);
                        }
                    }
                }
                break;
            default:
                break;
            }
#endif
        } else if(uds_config[channel].channelType == UDS_CANTP)
        {
#ifdef UDS_ENABLE_CAN
            sdu_id_type CanTpId = uds_config[channel].channelId;
            pdu_info_type_t pduInfo;
            cantp_copy_res_type res = CANTP_COPY_EMPTY;
            switch(uds_state[channel].channelState) {
            case UDS_IDLE:
                /* Do nothing */
                break;
            case UDS_RECEIVING:
                pduInfo.sduLength = (pdu_length_type)uds_config[channel].bufferSize;
                pduInfo.sduDataPtr = (cantp_u8*)uds_state[channel].receivedData;
                pduInfo.metaDataPtr = NULL;
                res = CanTp_CopyRxSdu(CanTpId, &pduInfo);
                if(res!=CANTP_COPY_EMPTY)
                {
                    uds_state[channel].pendingDataIndex = UDS_INVALID_PENDING_INDEX;
                    uds_state[channel].pendingNum = 0;
                    if(res==CANTP_COPY_OK)
                    {
                        uds_state[channel].channelState = UDS_PROCESSING;
                        uds_state[channel].receivedDataLength = (uds_u16_t)pduInfo.sduLength;
                    } else {
                        uds_state[channel].channelState = UDS_IDLE;
                        Uds_StartS3Server(channel);
                        if(uds_config[channel].errorCallback!=NULL) {
                            uds_config[channel].errorCallback(channel,UDS_RX_ERROR);
                        }
                    }
                }
                break;
            case UDS_PROCESSING:
                Uds_MatchProcess(channel);
                break;
            case UDS_SENDING:
            /* noting do in txConformation*/
            default:
                break;
            }

#endif
        }
    }
}





