#include "uds_fbl_mgr.h"
#include "uds_fbl_service10.h"
#include "uds_fbl_service11.h"
#include "uds_fbl_service22.h"
#include "BootDiagTp.h"

#define UDS_SESSION_DEFAULT_MASK        (0x01)
#define UDS_SESSION_PROGRAM_MASK        (0x02)
#define UDS_SESSION_EXTEND_MASK         (0x04)


#define UDS_LOCK_CHECK_DISABLE          (0x00)
#define UDS_LOCK_CHECK_ENABLE           (0x01)

typedef uint8_t (*UDS_SERVICE_PROCESS_FUNC)(UDSpacket_t *qUDSpacket);

typedef uint8_t (*UDS_SUB_SERVICE_PROCESS_FUNC)(UDSpacket_t *qUDSpacket);

typedef struct tagUDS_MAIN_SERVICE_INFO
{
    uint8_t                             sid;
    uint16_t                            minLength;
    bool                                SPRMB;
    UDS_SERVICE_PROCESS_FUNC            pFunc;
}UDS_MAIN_SERVICE_INFO;

typedef struct tagUDS_SUB_SERVICE_INFO
{
    uint32_t                            id;
    uint8_t                             sessionMask;
    uint8_t                             enbleLockCheck;
    uint16_t                            minLength;
    uint16_t                            maxLength;
    UDS_SUB_SERVICE_PROCESS_FUNC        pFunc;
}UDS_SUB_SERVICE_INFO;

static uint8_t processSubService_10(UDSpacket_t *qUDSpacket);
static uint8_t processSubService_11(UDSpacket_t *qUDSpacket);
static uint8_t processSubService_22(UDSpacket_t *qUDSpacket);
static uint8_t processSubService_28(UDSpacket_t *qUDSpacket);
static uint8_t processSubService_3E(UDSpacket_t *qUDSpacket);
static uint8_t processSubService_85(UDSpacket_t *qUDSpacket);
uint8_t SetDefaultDiagSession(UDSpacket_t *qUDSpacket);
uint8_t SetProgramDiagSession(UDSpacket_t *qUDSpacket);
uint8_t SetExtendDiagSession(UDSpacket_t *qUDSpacket);
uint8_t HardReset(UDSpacket_t *qUDSpacket);
uint8_t ReadServerSysSupplierId(UDSpacket_t *qUDSpacket);
uint8_t ReadECUHWVerNumberDataId(UDSpacket_t *qUDSpacket);
uint8_t ReadECUSWVerNumberDataId(UDSpacket_t *qUDSpacket);
uint8_t ReadHWPartNumber(UDSpacket_t *qUDSpacket);
uint8_t ReadSWPartNumber(UDSpacket_t *qUDSpacket);
uint8_t ReadFBLVersionNumber(UDSpacket_t *qUDSpacket);
uint8_t EnableRxAndTx(UDSpacket_t *qUDSpacket);
uint8_t EnableRxAndDisableTx(UDSpacket_t *qUDSpacket);
uint8_t FblDiagTesterPresent(UDSpacket_t *qUDSpacket);
uint8_t DiagTesterPresent(UDSpacket_t *qUDSpacket);
uint8_t Open_DTC(UDSpacket_t *qUDSpacket);
uint8_t Close_DTC(UDSpacket_t *qUDSpacket);

static UDS_MAIN_SERVICE_INFO g_uds_main_service_info_tbl[] = {
    {0x10,    2,    true ,    processSubService_10},
    {0x11,    2,    true ,    processSubService_11},
    {0x22,    3,    false,    processSubService_22},
    {0x28,    3,    true ,    processSubService_28},
    {0x3E,    2,    true ,    processSubService_3E},
    {0x85,    2,    true ,    processSubService_85}
};

static UDS_SUB_SERVICE_INFO g_uds_sub_service_info_10_tbl[] = {
    {
        0x01,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        SetDefaultDiagSession
    },
    {
        0x02,
        UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        SetProgramDiagSession
    },
    {
        0x03,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        SetExtendDiagSession
    }
};

static UDS_SUB_SERVICE_INFO g_uds_sub_service_info_11_tbl[] = {
    {
        0x01,
        UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        HardReset
    }
};

static UDS_SUB_SERVICE_INFO g_uds_sub_service_info_22_tbl[] = {
    {
        0xF18A,
        UDS_SESSION_PROGRAM_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        3,
        ReadServerSysSupplierId
    },
    {
        0xF089,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        3,
        ReadECUHWVerNumberDataId
    },
    {
        0xF189,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        3,
        ReadECUSWVerNumberDataId
    },
    {
        0xF187,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        3,
        ReadHWPartNumber
    },
    {
        0xF188,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        3,
        ReadSWPartNumber
    },
    {
        0xF170,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        3,
        ReadFBLVersionNumber
    }
};

static UDS_SUB_SERVICE_INFO g_uds_sub_service_info_28_tbl[] = {
    {
        0x00,
        UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        5,
        EnableRxAndTx
    },
    {
        0x03,
        UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        3,
        5,
        EnableRxAndDisableTx
    }
};

static UDS_SUB_SERVICE_INFO g_uds_sub_service_info_3E_tbl[] = {
    {
        0x00,
        UDS_SESSION_DEFAULT_MASK | UDS_SESSION_PROGRAM_MASK | UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        FblDiagTesterPresent
    }
};

static UDS_SUB_SERVICE_INFO g_uds_sub_service_info_85_tbl[] = {
    {
        0x01,
        UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        Open_DTC
    },
    {
        0x02,
        UDS_SESSION_EXTEND_MASK,
        UDS_LOCK_CHECK_DISABLE,
        2,
        2,
        Close_DTC
    }
};

/*===============================================================================================*/
/* uint8_t SetDefaultDiagSession(UDSpacket_t *qUDSpacket)                                        */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Set default session                                                                */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t SetDefaultDiagSession(UDSpacket_t *qUDSpacket)
{
    udsFbl_SetCurrentSessionMode(KWPDIAG_DEFAULT_SESS_ID);
    service11_set_rstflag((uint8_t)0x01,TRUE);
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->Data.Byte[2u] = P2serverValue_L;
    qUDSpacket->Data.Byte[3u] = P2serverValue_H;
    qUDSpacket->Data.Byte[4u] = P2_serverValue_L;
    qUDSpacket->Data.Byte[5u] = P2_serverValue_H;
    qUDSpacket->size = 6u;

    return UDSpositiveResp;
}

/*===============================================================================================*/
/* uint8_t SetProgramDiagSession(UDSpacket_t *qUDSpacket)                                        */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Set program session                                                                */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t SetProgramDiagSession(UDSpacket_t *qUDSpacket)
{
    udsFbl_SetCurrentSessionMode(KWPDIAG_PROG_SESS_ID);
    udsFbl_StartTimer(TIMER_UDS_SESSION);
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->Data.Byte[2u] = P2serverValue_L;
    qUDSpacket->Data.Byte[3u] = P2serverValue_H;
    qUDSpacket->Data.Byte[4u] = P2_serverValue_L;
    qUDSpacket->Data.Byte[5u] = P2_serverValue_H;
    qUDSpacket->size = 6u;

    return UDSpositiveResp;
}

/*===============================================================================================*/
/* uint8_t SetExtendDiagSession(UDSpacket_t *qUDSpacket)                                         */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Set extend session                                                                 */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t SetExtendDiagSession(UDSpacket_t *qUDSpacket)
{
    udsFbl_SetCurrentSessionMode(KWPDIAG_EXTEND_SESS_ID);
    udsFbl_StartTimer(TIMER_UDS_SESSION);
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->Data.Byte[2u] = P2serverValue_L;
    qUDSpacket->Data.Byte[3u] = P2serverValue_H;
    qUDSpacket->Data.Byte[4u] = P2_serverValue_L;
    qUDSpacket->Data.Byte[5u] = P2_serverValue_H;
    qUDSpacket->size = 6u;

    return UDSpositiveResp;
}

/*===============================================================================================*/
/* uint8_t HardReset(UDSpacket_t *qUDSpacket)                                                    */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Soft reset                                                                         */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t HardReset(UDSpacket_t *qUDSpacket)
{
    service11_set_rstflag((uint8_t)0x01,TRUE);

    qUDSpacket->size = 2;
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    return UDSpositiveResp;
}

/*===============================================================================================*/
/* uint8_t ReadServerSysSupplierId(UDSpacket_t *qUDSpacket)                      */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: functional address read program attempt counter                                    */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t ReadServerSysSupplierId(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    returnType = vd_s_ReadServerSysSupplierId(&qUDSpacket->Data.Byte[3]);
    if(UDSpositiveResp == returnType)
    {
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
        qUDSpacket->size = 11;
    }
    else
    {
        /* do nothing */
    }
    return returnType;
}

/*===============================================================================================*/
/* uint8_t ReadECUHWVerNumberDataId(UDSpacket_t *qUDSpacket)            */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: functional address read Vehicle Manufacturer Hardware PartNumber                   */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t ReadECUHWVerNumberDataId(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    returnType = vd_s_ReadECUHWVerNumberDataId(&qUDSpacket->Data.Byte[3]);
    if(UDSpositiveResp == returnType)
    {
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
        qUDSpacket->size = 14;
    }
    else
    {
        /* do nothing */
    }
    return returnType;
}

/*===============================================================================================*/
/* uint8_t ReadECUSWVerNumberDataId(UDSpacket_t *qUDSpacket)              */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: functional address read Vehicle Manufacturer Hardware Baseline                     */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t ReadECUSWVerNumberDataId(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    returnType = vd_s_ReadECUSWVerNumberDataId(&qUDSpacket->Data.Byte[3]);
    if(UDSpositiveResp == returnType)
    {
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
        qUDSpacket->size = 5;
    }
    else
    {
        /* do nothing */
    }

    return returnType;
}

/*===============================================================================================*/
/* uint8_t ReadHWPartNumber(UDSpacket_t *qUDSpacket)                                      */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: functional address read FingerPrint                                                */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t ReadHWPartNumber(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    returnType = vd_s_ReadHWPartNumber(&qUDSpacket->Data.Byte[3]);
    if(UDSpositiveResp == returnType)
    {
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
        qUDSpacket->size = 15;
    }
    else
    {
        /* do nothing */
    }

    return returnType;
}

/*===============================================================================================*/
/* uint8_t ReadSWPartNumber(UDSpacket_t *qUDSpacket)                          */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: functional address read Active Diagnostic Session                                  */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t ReadSWPartNumber(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    returnType = vd_s_ReadSWPartNumber(&qUDSpacket->Data.Byte[3]);
    if(UDSpositiveResp == returnType)
    {
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
        qUDSpacket->size = 4;
    }
    else
    {
        /* do nothing */
    }

    return returnType;
}

/*===============================================================================================*/
/* uint8_t ReadFBLVersionNumber(UDSpacket_t *qUDSpacket)                          */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: functional address read Bootloader Version Number                                  */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t ReadFBLVersionNumber(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    returnType = vd_s_ReadFBLVersionNumber(&qUDSpacket->Data.Byte[3]);
    if(UDSpositiveResp == returnType)
    {
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
        qUDSpacket->size = 6;
    }
    else
    {
        /* do nothing */
    }

    return returnType;
}

/*===============================================================================================*/
/* uint8_t EnableRxAndTx(UDSpacket_t *qUDSpacket)                                                */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: enable can rx and tx message                                                       */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t EnableRxAndTx(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    uint8_t comType;                                 /* communication type */

    comType = qUDSpacket->Data.Byte[2];
    if((0x03 != comType) && (0x02 != comType) && (0x01 != comType))
    {
        returnType = UDSrequestOutOfRange;
    }
    else
    {
        qUDSpacket->size = 2;
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    }

    return returnType;
}

/*===============================================================================================*/
/* uint8_t EnableRxAndDisableTx(UDSpacket_t *qUDSpacket)                                               */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: disable can rx and tx message                                                      */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t EnableRxAndDisableTx(UDSpacket_t *qUDSpacket)
{
    uint8_t returnType = UDSpositiveResp;
    uint8_t comType;                                 /* communication type */

    comType = qUDSpacket->Data.Byte[2];
    if((0x03 != comType) && (0x02 != comType) && (0x01 != comType))
    {
        returnType = UDSrequestOutOfRange;
    }
    else
    {
        qUDSpacket->size = 2;
        qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    }

    return returnType;
}

/*===============================================================================================*/
/* uint8_t FblDiagTesterPresent(UDSpacket_t *qUDSpacket)                                         */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Handle $3E $00 service(session keep)                                               */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t FblDiagTesterPresent(UDSpacket_t *qUDSpacket)
{
    udsFbl_StartTimer(TIMER_UDS_SESSION);
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->size = 2;
    return UDSpositiveResp;
}

/*===============================================================================================*/
/* uint8_t DiagTesterPresent(UDSpacket_t *qUDSpacket)                                            */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Handle $3E $80 service(session keep)                                               */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t DiagTesterPresent(UDSpacket_t *qUDSpacket)
{
    udsFbl_StartTimer(TIMER_UDS_SESSION);
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->size = 2;
    return UDSnoReply;
}

/*===============================================================================================*/
/* uint8_t Open_DTC(UDSpacket_t *qUDSpacket)                                                     */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Handle $85 service(open dtc setting)                                               */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t Open_DTC(UDSpacket_t *qUDSpacket)
{
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->size = 2;

    return (UDSpositiveResp);
}

/*===============================================================================================*/
/* uint8_t Close_DTC(UDSpacket_t *qUDSpacket)                                                    */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Handle $85 service(close dtc setting)                                              */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t Close_DTC(UDSpacket_t *qUDSpacket)
{
    qUDSpacket->Data.Byte[0] = UDS_GET_POSITIVE_RSP(qUDSpacket->Data.Byte[0]);
    qUDSpacket->size = 2;

    return (UDSpositiveResp);
}

/*===============================================================================================*/
/* uint8_t getUdsMainServiceIndex(uint8_t sid)                                                   */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Get service index from diag service table                                          */
/*  Arguments: sid : service identifer                                                           */
/*  Return: index                                                                                */
/*===============================================================================================*/
uint8_t getUdsMainServiceIndex(uint8_t sid)
{
    uint8_t sidIndex = 0;
    for(sidIndex = 0; sidIndex < sizeof(g_uds_main_service_info_tbl)/sizeof(g_uds_main_service_info_tbl[0]); sidIndex ++)
    {
        if(sid == g_uds_main_service_info_tbl[sidIndex].sid)
        {
            break;
        }
    }
    if(sidIndex >= sizeof(g_uds_main_service_info_tbl)/sizeof(g_uds_main_service_info_tbl[0]))
    {
        sidIndex = 0xFF;
    }
    return sidIndex;
}

/*********************************************************************************************
 * Prototype
 * uint8_t processUdsMainServiceReq(uint8_t sid, UDSpacket_t *qUDSpacket)

 * Parameter
 * pdiagdata [in] :
 * diagrequestlength [in] :
 *
 * Functional Description: handle main diag serivce
 *
 * Particularities and Limitations
 *
 * Expected Caller Context
 *
 *********************************************************************************************/
uint8_t processUdsMainServiceReq(uint8_t sid, UDSpacket_t *qUDSpacket)
{
    uint8_t mainServiceIndex = 0xFF;
    uint8_t responseByte     = 0u;
    uint8_t ssp;

    if (NULL != qUDSpacket->Data.Byte)
    {
        /* Get current SID index */
        mainServiceIndex = getUdsMainServiceIndex(sid);
        ssp = UDS_GET_SUB_FUNCTION_SUPPRESS_POSRSP(qUDSpacket->Data.Byte[1]);
        if (0xFF != mainServiceIndex)
        {
            if((ssp == TRUE) && (g_uds_main_service_info_tbl[mainServiceIndex].SPRMB == true))
            {
                udsFbl_Set_Func_SPRMB(true);
            }
            else
            {
                udsFbl_Set_Func_SPRMB(false);
            }

            if (g_uds_main_service_info_tbl[mainServiceIndex].minLength > qUDSpacket->size)
            {
                responseByte = UDSincorrectMessageLengthOrInvalidFormat;
            }
            else
            {
                /* Process Main Service */
                if (NULL != g_uds_main_service_info_tbl[mainServiceIndex].pFunc)
                {
                    responseByte = g_uds_main_service_info_tbl[mainServiceIndex].pFunc(qUDSpacket);
                }
            }
        }
        else
        {
            responseByte = UDSserviceNotSupported;
        }
    }
    return responseByte;
}

/*===============================================================================================*/
/* uint8_t processUdsFunctionAddrReq(UDSpacket_t *qUDSpacket)                                    */
/* ----------------------------------------------------------------------------------------------*/
/*  Function: Handle functional addressing diag request                                          */
/*  Arguments: qUDSpacket : CAN message structure pointer                                        */
/*  Return: response type                                                                        */
/*===============================================================================================*/
uint8_t processUdsFunctionAddrReq(UDSpacket_t *qUDSpacket)
{
    uint8_t sid;
    uint8_t responseByte = 0u;

    if ( (NULL != qUDSpacket->Data.Byte) && (qUDSpacket->size >= 2))
    {
        sid = qUDSpacket->Data.Byte[0];
        /* Process Main Service by SID */
        responseByte = processUdsMainServiceReq(sid, qUDSpacket);
    }
    else
    {
        /* Do Nothing */
    }
    return responseByte;
}

/*================================================================================================*/
/* uint8_t getSubServiceIndex(uint32_t id, const UDS_SUB_SERVICE_INFO *pSubTbl, uint16_t tblSize) */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Get sub-function index from sub-service table                                       */
/*  Arguments: id : sub-funtion                                                                   */
/*  pSubTbl : sub service info table                                                              */
/*  tblSize : sub service info table size                                                         */
/*  Return: index                                                                                 */
/*================================================================================================*/
uint8_t getSubServiceIndex(uint32_t id, const UDS_SUB_SERVICE_INFO *pSubTbl, uint16_t tblSize)
{
    uint8_t subIdIndex = 0x0;
    for(subIdIndex = 0; subIdIndex < tblSize; subIdIndex ++)
    {
        if(id == pSubTbl[subIdIndex].id)
        {
            break;
        }
    }
    if(subIdIndex >= tblSize)
    {
        subIdIndex = 0xFF;
    }
    return subIdIndex;
}

/*=========================================================================================================================================*/
/* static uint8_t processSubService_common(uint32_t subId, UDSpacket_t *qUDSpacket, const UDS_SUB_SERVICE_INFO *pSubTbl, uint16_t tblSize) */
/* ----------------------------------------------------------------------------------------------------------------------------------------*/
/*  Function: Common service handle framework                                                                                              */
/*  Arguments: subId : sub function                                                                                                        */
/*             qUDSpacket : CAN message structure pointer                                                                                  */
/*  pSubTbl : sub service info table                                                                                                       */
/*  tblSize : sub service info table size                                                                                                  */
/*  Return: response type                                                                                                                  */
/*=========================================================================================================================================*/
static uint8_t processSubService_common(uint32_t subId, UDSpacket_t *qUDSpacket, const UDS_SUB_SERVICE_INFO *pSubTbl, uint16_t tblSize)
{
    uint8_t  subServiceIndex     = 0xFF;
    uint8_t  currentSession      = 0;
    uint8_t  currentLockState    = 0;
    uint8_t  responseByte        = 0u;
    uint16_t len                 = qUDSpacket->size;

    subServiceIndex  = getSubServiceIndex(subId, pSubTbl, tblSize);
    currentSession   = udsFbl_GetCurrentSessionMode();
    currentLockState = udsFbl_Get_Security_Access_Sts();
    if (0xFF == subServiceIndex)
    {
        if(pSubTbl == g_uds_sub_service_info_22_tbl)
        {
            responseByte = UDSrequestOutOfRange;
        }
        else
        {
            responseByte = UDSsubfunctionNotSupported;
        }
    }
    else if ((pSubTbl[subServiceIndex].minLength > len ) || (pSubTbl[subServiceIndex].maxLength < len ))
    {
        responseByte = UDSincorrectMessageLengthOrInvalidFormat;
    }
    else if ( 0 == (currentSession & pSubTbl[subServiceIndex].sessionMask))
    {
        if(pSubTbl == g_uds_sub_service_info_10_tbl)
        {
            responseByte = UDSsubFuncNotSupportedInActSes;
        }
        else if((pSubTbl == g_uds_sub_service_info_28_tbl) || (pSubTbl == g_uds_sub_service_info_85_tbl))
        {
            responseByte = UDSconditionNotCorrect;
        }
    }
    else if ((UDS_LOCK_CHECK_ENABLE == pSubTbl[subServiceIndex].enbleLockCheck) && (currentLockState != true))
    {
        responseByte = UDSsecurityAccessDenied;
    }
    else
    {
        /* Process SubService */
        if (NULL != pSubTbl[subServiceIndex].pFunc)
        {
            responseByte = pSubTbl[subServiceIndex].pFunc(qUDSpacket);
        }
    }
    return responseByte;
}

/*================================================================================================*/
/* static uint8_t processSubService_10(UDSpacket_t *qUDSpacket)                                   */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Handle $10 service(functional addressing)                                           */
/*  Arguments: qUDSpacket : CAN message structure pointer                                         */
/*  Return: response type                                                                         */
/*================================================================================================*/
static uint8_t processSubService_10(UDSpacket_t *qUDSpacket)
{
    uint32_t subId        = 0;
    uint8_t  responseByte = 0x0u;

    if (NULL != qUDSpacket->Data.Byte)
    {
        subId = UDS_GET_SUB_FUNCTION(qUDSpacket->Data.Byte[1]);
        responseByte = processSubService_common(subId, qUDSpacket, g_uds_sub_service_info_10_tbl, sizeof(g_uds_sub_service_info_10_tbl)/sizeof(g_uds_sub_service_info_10_tbl[0]));
    }
    return responseByte;
}

/*================================================================================================*/
/* static uint8_t processSubService_11(UDSpacket_t *qUDSpacket)                                   */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Handle $11 service(functional addressing)                                           */
/*  Arguments: qUDSpacket : CAN message structure pointer                                         */
/*  Return: response type                                                                         */
/*================================================================================================*/
static uint8_t processSubService_11(UDSpacket_t* qUDSpacket)
{
    uint32_t subId        = 0;
    uint8_t  responseByte = 0x0u;

    if (NULL != qUDSpacket->Data.Byte)
    {
        subId = UDS_GET_SUB_FUNCTION(qUDSpacket->Data.Byte[1]);
        responseByte = processSubService_common(subId, qUDSpacket, g_uds_sub_service_info_11_tbl, sizeof(g_uds_sub_service_info_11_tbl)/sizeof(g_uds_sub_service_info_11_tbl[0]));
    }
    return responseByte;
}

/*================================================================================================*/
/* static uint8_t processSubService_22(UDSpacket_t *qUDSpacket)                                   */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Handle $22 service(functional addressing)                                           */
/*  Arguments: qUDSpacket : CAN message structure pointer                                         */
/*  Return: response type                                                                         */
/*================================================================================================*/
static uint8_t processSubService_22(UDSpacket_t *qUDSpacket)
{
    uint32_t subId        = 0;
    uint8_t  responseByte = 0x0u;

    if (NULL != qUDSpacket->Data.Byte)
    {
        subId = ((uint32_t)qUDSpacket->Data.Byte[1]) << 8;
        subId |= (uint32_t)qUDSpacket->Data.Byte[2];
        /*Do not support Functional address for 22 service*/
        //responseByte = processSubService_common(subId, qUDSpacket, g_uds_sub_service_info_22_tbl, sizeof(g_uds_sub_service_info_22_tbl)/sizeof(g_uds_sub_service_info_22_tbl[0]));
    }
    /*Do not support Functional address for 22 service*/
    responseByte = UDSrequestOutOfRange;
    return responseByte;
}

/*================================================================================================*/
/* static uint8_t processSubService_28(UDSpacket_t *qUDSpacket)                                   */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Handle $28 service(functional addressing)                                           */
/*  Arguments: qUDSpacket : CAN message structure pointer                                         */
/*  Return: response type                                                                         */
/*================================================================================================*/
static uint8_t processSubService_28(UDSpacket_t* qUDSpacket)
{
    uint32_t subId        = 0;
    uint8_t  responseByte = 0x0u;

    if (NULL != qUDSpacket->Data.Byte)
    {
        subId = UDS_GET_SUB_FUNCTION(qUDSpacket->Data.Byte[1]);
        responseByte = processSubService_common(subId, qUDSpacket, g_uds_sub_service_info_28_tbl, sizeof(g_uds_sub_service_info_28_tbl)/sizeof(g_uds_sub_service_info_28_tbl[0]));
    }
    return responseByte;
}

/*================================================================================================*/
/* static uint8_t processSubService_3E(UDSpacket_t *qUDSpacket)                                   */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Handle $3E service(functional addressing)                                           */
/*  Arguments: qUDSpacket : CAN message structure pointer                                         */
/*  Return: response type                                                                         */
/*================================================================================================*/
static uint8_t processSubService_3E(UDSpacket_t* qUDSpacket)
{
    uint32_t subId        = 0;
    uint8_t  responseByte = 0x0u;

    if (NULL != qUDSpacket->Data.Byte)
    {
        subId = UDS_GET_SUB_FUNCTION(qUDSpacket->Data.Byte[1]);
        responseByte = processSubService_common(subId, qUDSpacket, g_uds_sub_service_info_3E_tbl, sizeof(g_uds_sub_service_info_3E_tbl)/sizeof(g_uds_sub_service_info_3E_tbl[0]));
    }
    return responseByte;
}

/*================================================================================================*/
/* static uint8_t processSubService_85(UDSpacket_t *qUDSpacket)                                   */
/* -----------------------------------------------------------------------------------------------*/
/*  Function: Handle $85 service(functional addressing)                                           */
/*  Arguments: qUDSpacket : CAN message structure pointer                                         */
/*  Return: response type                                                                         */
/*================================================================================================*/
static uint8_t processSubService_85(UDSpacket_t* qUDSpacket)
{
    uint32_t subId        = 0;
    uint8_t  responseByte = 0x0u;

    if (NULL != qUDSpacket->Data.Byte)
    {
        subId = UDS_GET_SUB_FUNCTION(qUDSpacket->Data.Byte[1]);
        responseByte = processSubService_common(subId, qUDSpacket, g_uds_sub_service_info_85_tbl, sizeof(g_uds_sub_service_info_85_tbl)/sizeof(g_uds_sub_service_info_85_tbl[0]));
    }
    return responseByte;
}