
/* ------------------------------ Includes --------------------------------- */
//#include "CommonMacros.h"
#include "UDS_CoreServices_CB.h"

#include "Std_Types.h"
#include "DiagnosticR/Comp_ISO_15765_3/Iso15765_3.h"
#include "DiagnosticR/Dem/Dem_Types.h"
#include "DiagnosticR/rte/Rte_Dem_Type.h"
#include "DiagnosticR/Dem/Dem.h"

//#include "Mcu.h"
//#include "Dio.h"
//#include "Nvm.h"
//#include "Dem.h"
//#include "revision.h"
#include "UDS_Services_Common.h"
//#include "WdgM_API.h"
//#include "BswM_API.h"
#include <stdlib.h>
#include "DiagnosticR/Comp_ISO_15765_3/Iso15765_3_Task.h"
//#include "Uja1169fdWdgUpdate_functions.h"
#include "DiagnosticR/FicOsek/FicOsekCom.h"
#include "OsekCom/OsekCom.h"
//#include "App_Boot.h"
//#include "ficoNVM.h"
//#include "AppMode_If.h"
//#include "QMGateway.h"
//#include "SecurityAlgorithm.h"
/* --------------------------- Type Definitions ---------------------------- */

/* ------------------------------- Defines --------------------------------- */
#define DTC_NUM_SUBFUNC             ((uint8)0x16)
#define VALUE_IS_NOT_USED           ((uint8)0x00)
#define BYTES_TO_DTC(hb, mb, lb)    (((uint32)(hb) << 16) | ((uint32)(mb) << 8) | (uint32)(lb))
#define DTC_HIGH_BYTE(dtc)          (((uint32)(dtc) >> 16) & 0xFFu)
#define DTC_MID_BYTE(dtc)           (((uint32)(dtc) >> 8) & 0xFFu)
#define DTC_LOW_BYTE(dtc)           ((uint32)(dtc) & 0xFFu)

#define DATA_SIZE_ZERO      ((UI_8)0)
#define DATA_SIZE_ONE       ((UI_8)1)
#define DATA_SIZE_TWO       ((UI_8)2)
#define DATA_SIZE_THREE     ((UI_8)3)
#define DATA_SIZE_FOUR      ((UI_8)4)
#define DATA_SIZE_PP        ((UI_8)100)
#define DATA_SIZE_ECU_SER_NO        ECU_SERIAL_NUMBER_SIZE

#define DATA_DEFAULT_SNAPSHOT_REC_NO      ((UI_8)0x01)
#define DATA_DEFAULT_SNAPSHOT_REC_NO2     ((UI_8)0x02)

/*---------------------------  Global variables ---------------------------- */

/* Current session of the diagnostics server layer */
static UI_8 current_session = UDS_DEFAULT_SESSION;

/* Global variable to request a reset to the whole DEFC system */
BOOL do_ecu_reset = FALSE;

/* Bootloader variable that tells if bootloader must respond or not to the session
change when coming from application (supress positive response)      */
UI_16 start_download_response;
static UI_16 *start_download = (UI_16 *)0x20000000;
volatile boolean suppress_positive_bit;
UI_8 manufactory_mode = 0xFF;
#if 0
#pragma ghs section bss=".fbl_shared"

volatile uint8 mcu_reset_type;

volatile uint8 suppress_positive_bit_valid_info;
uint8 security_faa_flag = 0;
static uint8 reset_flag = 0;

#pragma ghs section
#endif
BOOL clear_dtc_flag = FALSE;
/* ------------------------- Function --------------------------- */

/*****************************************************************************
 | Portability:  DEFC Project
 |----------------------------------------------------------------------------
 | Description:
 |  * Routine that commands the DEFCUdsTesterPresent diagnostic service and is
 |    responsible for giving the final answer to ISO_3 layer.
 |---------------------------------------------------------------------------
 | Arguments:
 |  - tp_type: tester present type requested
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
void UdsTesterPresent(UI_8 tp_type) {
	UI_8 response_mode;

	/* Only accepts if tester present type is 0x01 */
	if (tp_type == 0x01) {
		response_mode = ISO15765_3_POSITIVE_RESPONSE;
	} else {
		response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
	}
	/* We send the answer with the adequate measure */
	Iso15765_3SendResponse(response_mode);
}

/*****************************************************************************
 | Portability:  DEFC Project
 |----------------------------------------------------------------------------
 | Description:
 |  * Routine that commands all the ReadDataByIdentifier diagnostics and is
 |    responsible for giving the final answer to ISO_3 layer.
 |---------------------------------------------------------------------------
 | Arguments:
 |  - id_h: higher byte of the identifier requested
 |  - id_h: lower byte of the identifier requested
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
UI_8 UdsReadDataByIdentifier(UI_8 id_h, UI_8 id_l)
{
	return (UI_8)UDS_ReadDiD_CB(id_h, id_l);
}


/*****************************************************************************
 | Portability:  DEFC Project
 |----------------------------------------------------------------------------
 | Description:
 |  * Routine that commands all the WriteDataByIdentifier diagnostics and is
 |    responsible for giving the final answer to ISO_3 layer.
 |---------------------------------------------------------------------------
 | Arguments:
 |  - id_h: higher byte of the identifier requested
 |  - id_h: lower byte of the identifier requested
 |  - data_buffer: data pointer of the received parameters for the write
 |  - size: length of the parameters received for the write
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
void UdsWriteDataByIdentifier(UI_8 id_h, UI_8 id_l, UI_8 *buf_data_rx, UI_16 size)
{
	(void)UDS_WriteDiD_CB(id_h, id_l, buf_data_rx, size);
}
/*****************************************************************************
 | Portability:  DEFC Project
 |----------------------------------------------------------------------------
 | Description:
 |  * Routine that commands the InputOutputControlByLocalId diagnostic
 |    service and is responsible for giving the final answer to ISO_3 layer
 |---------------------------------------------------------------------------
 | Arguments:
 |  - id: identifier of the requested input output control
 |  - size: length of the data received to control
 |  - data: data pointer to the received control data
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
#if 0
void UdsInputOutputControlByIdentifier(UI_8 id_h, UI_8 id_l, UI_8 io_ctrl_type, UI_8* buf_data_rx, UI_16 size) 
{
	(void)UDS_IOControl_CB(id_h, id_l, io_ctrl_type, buf_data_rx, size);
}
#endif 
/*****************************************************************************
 | Portability:  DEFC Project
 |----------------------------------------------------------------------------
 | Description:
 |  * Routine that commands all the WriteDataByIdentifier diagnostics and is
 |    responsible for giving the final answer to ISO_3 layer.
 |---------------------------------------------------------------------------
 | Arguments:
 |  - sub_function: sub function
 |  - data_buffer: data pointer of the received parameters for the write
 |  - size: length of the parameters received for the write
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
#if 0
void UdsControlDtcSetting(UI_8 sub_function,  UI_8 *buf_data_rx, UI_16 size)
{
	(void)UDS_DTCSetting_CB(sub_function,buf_data_rx, size);
}
#endif 
/*****************************************************************************
 | Portability:
 |----------------------------------------------------------------------------
 | Description:
 |  * Called when layer 3 changes session
 |---------------------------------------------------------------------------
 | Arguments:
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
//#include "WdgM_API.h"
//#include "WdgM_Types.h"
//extern uint8 application_active;
void UdsIniSession(void)
{
	/* Save the current session */
	current_session = Iso15765_3QueryActiveSession();

	switch(current_session){

    	case UDS_DEFAULT_SESSION:
            /* Nothing to be done for now in the GM project */
    	//	SET_FLAG_DTC_SETTING();

    	//	EnableTxFrm_shiftselectposition();
//    		EnableTxFrm_gsm_nm();

    		/* Enable Rx */
    	//	CAN_RX_CONFIG_ENABLE();

    		/* disable UDS IO control */
    		//uds_control_led = FALSE;
    		//uds_control_led_lmc = FALSE;
    		//uds_control_led_conbyte = 0;
            break;
        case UDS_EXT_DIAG_SESSION:
            /* Nothing to be done */
            break;
    	case UDS_PROGRAMMING_SESSION:
            /* Bootloader enters Programming session after reset */
            
            start_download[0] = START_DOWNLOAD_VALUE;

            DCACHE_CLEAN_BY_ADDR(start_download, 2);
            if(iso15765_3_supress_pos_resp == TRUE){
                start_download_response = START_DOWNLOAD_VALUE;
            }else{
                start_download_response = ((UI_8)0);
            }

            Iso15765_3SendResponse(UDS_ERR_RESPONSE_PENDING);
            suppress_positive_bit = 1;

            /* TODO: move reset after response in main loop */
           __NVIC_SystemReset();   
            break;
        case UDS_SYSTEM_SUPPLIER_SPECIFIC_SESSION_ID:
            /* Nothing to be done */
            break;

    	default:
    	    break;
	}

}

/*****************************************************************************
 | Portability:  DEFC Project
 |----------------------------------------------------------------------------
 | Description:
 |  * UdServices diagnostic initialitzation task. This routine should only
 |    be called one time at the microcontroller initialization
 |---------------------------------------------------------------------------
 | Arguments:
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
void UdsServerCallbacksInit(void) {

}

/*****************************************************************************
 | Portability:
 |----------------------------------------------------------------------------
 | Description:
 |  * UdServices diagnostic task executed in every main cycle
 |---------------------------------------------------------------------------
 | Arguments:
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
void UdsServerCallbacksTask(void) {

}

/*****************************************************************************
 | Portability:  Generic
 |----------------------------------------------------------------------------
 | Routine description:
 |  *
 |---------------------------------------------------------------------------
 | Parameters description:
 |  void : ...
 |  result none : ...
 /---------------------------------------------------------------------------*/
#if 1
void UdsExecuteResetNow(UI_8 reset_type)
{
	//reset_flag = reset_type;
	if (reset_type == UDS_HARD_RESET) {
		//RequestReleaseRun(SHUTDOWN_HARD_RESET);
		while(1)
		{
		}
		
	} else if (reset_type == UDS_KEY_ON_OFF_RESET) {
		Dem_SetOperationCycleState(DEM_IGNITION,DEM_CYCLE_STATE_END);
		//RequestReleaseRun(SHUTDOWN_HARD_RESET);

	}
	else if(reset_type == UDS_SOFT_RESET)
	{
		do_ecu_reset = TRUE;
		NVIC_SystemReset();
		//RequestReleaseRun(SHUTDOWN_SOFT_RESET);

	}
	else {
		/* Do nothing */
	}
}

boolean doReset(void)
{
	return do_ecu_reset;
}

BOOL CHECK_RESET_TYPE(UI_8 reset_type){
    BOOL result = FALSE;
    /* check supported reset sub function. Only hard reset is supported as per the requirements */
    if ((reset_type == UDS_HARD_RESET)||(reset_type == UDS_KEY_ON_OFF_RESET)) {
        result = TRUE;
    }else if(reset_type == UDS_SOFT_RESET)
    {
    	result = TRUE;
    }
    else {
        result = FALSE;
    }
    return result;
}


/*****************************************************************************
 | Portability:  Generic
 |----------------------------------------------------------------------------
 | Routine description:
 |  *
 |---------------------------------------------------------------------------
 | Parameters description:
 |  void : ...
 |  result none : ...
 /---------------------------------------------------------------------------*/
BOOL UdsCheckResetIsPossible(UI_8 reset_type)
{
    BOOL result;
    /* Temporary all sessions allowed to reset */
    if (current_session) {
        result = TRUE;
    }
    else {
        result = FALSE;
    }
    return result;
}
#endif
/*****************************************************************************
 | Portability:
 |----------------------------------------------------------------------------
 | Description:
 |  * Callback for the routine control service callback.
 |---------------------------------------------------------------------------
 | Arguments:
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
#if 1
void UdsControlRoutine(UI_8 routine_ctrl_type, UI_8 id_high, UI_8 id_low, UI_8 *routine_entry_option, UI_16 size)
{
	(void)UDS_RoutineControl_CB(routine_ctrl_type, id_high, id_low, routine_entry_option, size);
}
#endif
/*****************************************************************************
 | Portability:
 |----------------------------------------------------------------------------
 | Description:
 |  * Callback for the read DTC callback.
 |---------------------------------------------------------------------------
 | Arguments:
 |---------------------------------------------------------------------------
 | Timing:
 | Tmax Int Dis:   ? cicles cpu | O(n): CTE
 | Tmax Int En :   ? cicles cpu | O(n): CTE
 | Tmax Total  :   ? cicles cpu | O(n): CTE
 /---------------------------------------------------------------------------*/
#if 1
void UdsReadDtc(UI_8 id, UI_8 *data, UI_16 size) {
    tp_uds_read_dtc_information_resp resp;
    Dem_ReturnSetFilterType setDtcFilterResult = DEM_FILTER_ACCEPTED;
    Dem_ReturnGetStatusOfDTCType GetStatusOfDtc = DEM_STATUS_OK;
    Dem_EventStatusExtendedType dtcStatus = DEM_EVENT_STATUS_PASSED;
    Dem_ReturnGetNextFilteredDTCType getNextFilteredDtcResult = DEM_FILTERED_OK;
    Dem_ReturnGetNumberOfFilteredDTCType getNumerResult;
    uint16 numberOfFilteredDtc = 0;
    Std_ReturnType result = E_OK;
    uint32 DtcNumber = 0;
    uint32 dtc = 0x000000;
    uint16 nrOfDtcs = 0;
    uint16 requested_status_mask = 0x00;
    uint8 dtcStatusMask = 0xFF;     /* polyspace MISRA-C:10.1 [Justified:Low] "0xFF perfectly fits an object 8 bits wide." */
    uint8 RecordNumber = 0;
    uint8 response_mode = ISO15765_3_POSITIVE_RESPONSE;

    /* Snapshot data */
    uint8 record = 1;
    uint16 nofBytesCopied = 0;
    uint16 bufSizeLeft = 0;
    uint8 snapshot_data[128] = {0};
    uint8 snapshot_data_tores[2][20] = {0};
    uint8 index = 0;
    /* Extended date */
    uint8 startRecNum;
   	uint8 endRecNum;
//   	uint8 extended_data[10] = {0};

    /* prepares response estructure */
    resp = ISO15765_3_GET_RESP_DATA(tp_uds_read_dtc_information_resp);
    switch (id) {
        /* 0x01 Subservice from Read DTC */
        case REPORT_NUMBER_OF_DTC_BY_STATUS_MASK:
        {
            if(DATA_SIZE_ONE==size){
                /* filter the DTC for the corresponding status mask requested */
                requested_status_mask = *data;
                setDtcFilterResult = Dem_SetDTCFilter(requested_status_mask, DEM_DTC_KIND_ALL_DTCS, DEM_DTC_FORMAT_UDS, DEM_DTC_ORIGIN_PRIMARY_MEMORY, DEM_FILTER_WITH_SEVERITY_NO, VALUE_IS_NOT_USED, DEM_FILTER_FOR_FDC_NO);      /* polyspace MISRA-C:10.1 [Justified:Low] "I can't see the object of type UI_16." */
                if (setDtcFilterResult == DEM_FILTER_ACCEPTED) {

                    /* Fill the anwer with all the DTC founded */
                    getNumerResult = Dem_GetNumberOfFilteredDtc(&numberOfFilteredDtc);
                    if (getNumerResult == DEM_NUMBER_OK) {
                        /* Create positive response (ISO 14229-1 table 251) */
                        resp->subservice = 0x01;
                        resp->buffer_dades[0] = requested_status_mask;/*data;*/              /* DTCStatusAvailabilityMask */    /* polyspace MISRA-C:10.1 [Justified:Low] "To be analyzed by the software component owner." */
                        resp->buffer_dades[1] = 0x01;                                       /* DTC format ISO 14229-1 */
                        resp->buffer_dades[2] = (numberOfFilteredDtc >> 8);                 /* DTCCountHighByte */      /* polyspace MISRA-C:10.1 [Justified:Low] "No problem here because we are discarding half the bits, so only 8 are left." */
                        resp->buffer_dades[3] = (numberOfFilteredDtc & 0xFFu);              /* DTCCountLowByte */   /* polyspace MISRA-C:10.1 [Justified:Low] "0xFF perfectly fits an object 8 bits wide." */
                        Iso15765_3IncrementResponseSize(5);
                    }
                    else {
                        /* NOTE: What to do? */
                        response_mode = UDS_ERR_GENERAL_REJECT;
                    }
                }
                else {
                    response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                }
            }else{
                response_mode = UDS_ERR_INVALID_FORMAT;
            }
        }
        break;
        /* 0x02 Subservice from Read DTC */
        case REPORT_DTC_BY_STATUS_MASK:
        {
            if(DATA_SIZE_ONE==size){
                /* filter the DTC for the corresponding status mask requested */
                requested_status_mask = *data;
                setDtcFilterResult = Dem_SetDTCFilter(requested_status_mask, DEM_DTC_KIND_ALL_DTCS, DEM_DTC_FORMAT_UDS, DEM_DTC_ORIGIN_PRIMARY_MEMORY, DEM_FILTER_WITH_SEVERITY_NO, VALUE_IS_NOT_USED, DEM_FILTER_FOR_FDC_NO);      /* polyspace MISRA-C:10.1 [Justified:Low] "I can't see the object of type UI_16." */
                if (setDtcFilterResult == DEM_FILTER_ACCEPTED) {
                    result = Dem_GetDTCStatusAvailabilityMask(&dtcStatusMask);
                    if (result != E_OK) {
                        dtcStatusMask = 0;
                    }

                    /* Get the number of DTCs to set a limit in the while */
                    getNumerResult = Dem_GetNumberOfFilteredDtc(&numberOfFilteredDtc);

                    if (dtcStatusMask != 0x00) {    /** @req DCM008 */
                        getNextFilteredDtcResult = Dem_GetNextFilteredDTC(&dtc, &dtcStatus);
                        /* Fill the answer with all the DTC founded */
                        while ((getNextFilteredDtcResult == DEM_FILTERED_OK) && ((numberOfFilteredDtc) >= (nrOfDtcs-1))) {
							if (nrOfDtcs < 1024)	/* see AXOECUGBA-1104 */
							{
								resp->buffer_dades[(nrOfDtcs*4)+1] = DTC_HIGH_BYTE(dtc);        /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
								resp->buffer_dades[(nrOfDtcs*4)+2] = DTC_MID_BYTE(dtc);         /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
								resp->buffer_dades[(nrOfDtcs*4)+3] = DTC_LOW_BYTE(dtc);         /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
								resp->buffer_dades[(nrOfDtcs*4)+4] = dtcStatus;
							}	/* else: we would be accessing outside resp->buffer_dades */
                            nrOfDtcs++;

                            getNextFilteredDtcResult = Dem_GetNextFilteredDTC(&dtc, &dtcStatus);
                        }

                        if (getNextFilteredDtcResult != DEM_FILTERED_NO_MATCHING_DTC) {
                            response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                        }
                    }
                    resp->subservice = 0x02;
                    resp->buffer_dades[0] = 0xFF;/*(uint8) nrOfDtcs;  */     /* polyspace MISRA-C:10.1 [Justified:Low] "It is expected that nrOfDtcs will have a small enough value." */
                    Iso15765_3IncrementResponseSize((nrOfDtcs*4)+2);
                }
                else {
                    response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                }
            }else{
                response_mode = UDS_ERR_INVALID_FORMAT;
            }
        }
        break;
        /* 0x04 Subservice from Read DTC */
        case REPORT_DTC_SNAPSHOT_RECORD_BY_DTC_NUMBER:
        {
            /* Now let's assume DTC has 3 bytes. */
            DtcNumber = (((uint32)data[0])<<16) + (((uint32)data[1])<<8) + ((uint32)data[2]);       /* polyspace MISRA-C:17.4 [Justified:Low] "The only potential problem here would be accessing outside the memory buffer pointed by the pointer; we trust Polyspace to check whether this happens." */
            GetStatusOfDtc = Dem_GetStatusOfDTC(DtcNumber, DEM_DTC_ORIGIN_PRIMARY_MEMORY, &dtcStatus); /** @req DCM383 */

            if(GetStatusOfDtc != DEM_STATUS_OK){
                response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
            }else if(DATA_SIZE_FOUR != size){
            	response_mode = UDS_ERR_INVALID_FORMAT;
            }else{

                resp->buffer_dades[0] = DTC_HIGH_BYTE(DtcNumber);   /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
                resp->buffer_dades[1] = DTC_MID_BYTE(DtcNumber);    /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
                resp->buffer_dades[2] = DTC_LOW_BYTE(DtcNumber);    /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
                resp->buffer_dades[3] = dtcStatus;
                resp->subservice = 0x04;
                Iso15765_3IncrementResponseSize(5);

                /*** start DTC snapshot read ***/
                RecordNumber = data[3];     /* polyspace MISRA-C:17.4 [Justified:Low] "The only potential problem here would be accessing outside the memory buffer pointed by the pointer; we trust Polyspace to check whether this happens." */
                /* !req DCM372 *//* !req DCM702 */
                if(( (0xFF == RecordNumber) || (RecordNumber == 0x01) || (RecordNumber == 0x02)))       /* polyspace MISRA-C:10.1 [Justified:Low] "0xFF is an 8-bit integer value." */
                {
                    /* Request for all freeze frames */
                    Dem_ReturnGetFreezeFrameDataByDTCType ret = DEM_GET_FFDATABYDTC_OK;
                    /* Check the maxim 10 records availables */
                    for (record = 0; record <= 2; record++)
                    {
                        bufSizeLeft = 512; /* PATCH RSBWGB-1386: make sure condition bufSize >= FFDataSize */
                        ret = Dem_GetFreezeFrameDataByDTC(DtcNumber, DEM_DTC_ORIGIN_PRIMARY_MEMORY,
                        /*         record, (uint8*)(resp->buffer_dades[4]), &bufSizeLeft);     */
                                record, snapshot_data, &bufSizeLeft);

                        if( ret == DEM_GET_FFDATABYDTC_OK)
                        {
                            nofBytesCopied += bufSizeLeft;/* !req DCM441 */
                            if(record == 1)
                            {	for(index = 0; index <= 19; index++)
                            	snapshot_data_tores[0][index] = snapshot_data[index];
                            }
                            else if(record == 2)
                            {
                            	for(index = 0; index <= 19; index++)
                                snapshot_data_tores[1][index] = snapshot_data[index];
                            }
                        }
                        else
                        {
                        }
                        bufSizeLeft = 0;

                    }

                    //while( (bufSizeLeft <  nofBytesCopied) && (bufSizeLeft < 128))	/* see AXOECUGBA-1104 -- 128 is the size (in cells) of snapshot_data */
                    //{
                       // resp->buffer_dades[bufSizeLeft+4] = snapshot_data[bufSizeLeft];
                       // bufSizeLeft++;
                   // }
                    //<<
                    if(RecordNumber == 0x02)
                    {
                    	resp->buffer_dades[4] = 0x02;
                    	for(index = 0; index <= 18; index++ )
                    	{
                    		resp->buffer_dades[index + 5] = snapshot_data_tores[1][index + 1];
                    	}
                    	Iso15765_3IncrementResponseSize(20);
                    }

                    else if(RecordNumber == 0xFF)
                    {
                    	resp->buffer_dades[4] = DATA_DEFAULT_SNAPSHOT_REC_NO;
                    	for(index = 0; index <= 18; index++)
						{
							resp->buffer_dades[index + 5] = snapshot_data_tores[0][index + 1];
						}

                    	for(index = 0; index <= 19; index++)
						{
							resp->buffer_dades[index + 24] = snapshot_data_tores[1][index];
						}

                    	resp->buffer_dades[24] = 0x02;
                    	resp->buffer_dades[25] = 0x05;
                    	resp->buffer_dades[26] = 0xB0;
                    	resp->buffer_dades[27] = 0x01;
                    	resp->buffer_dades[30] = 0xB0;
						resp->buffer_dades[31] = 0x03;
						resp->buffer_dades[33] = 0xB0;
						resp->buffer_dades[34] = 0x04;
						resp->buffer_dades[36] = 0xB0;
						resp->buffer_dades[37] = 0x05;
						resp->buffer_dades[40] = 0xB0;
						resp->buffer_dades[41] = 0x06;
                    	Iso15765_3IncrementResponseSize(40);
                    }
                    else
                    {
                    	resp->buffer_dades[4] = DATA_DEFAULT_SNAPSHOT_REC_NO;
                    	for(index = 0; index <= 18; index++)
						{
							resp->buffer_dades[index + 5] = snapshot_data_tores[0][index + 1];
						}
                    	Iso15765_3IncrementResponseSize(20);
                    }
                    resp->buffer_dades[5] = 0x05;
                	resp->buffer_dades[6] = 0xB0;
                	resp->buffer_dades[7] = 0x01;
                	resp->buffer_dades[10] = 0xB0;
                	resp->buffer_dades[11] = 0x03;
					resp->buffer_dades[13] = 0xB0;
				    resp->buffer_dades[14] = 0x04;
					resp->buffer_dades[16] = 0xB0;
					resp->buffer_dades[17] = 0x05;
					resp->buffer_dades[20] = 0xB0;
					resp->buffer_dades[21] = 0x06;
                    //>>
                    //Iso15765_3IncrementResponseSize(nofBytesCopied);
                    if(nofBytesCopied == 0)
                    {
                    	//Iso15765_3IncrementResponseSize(20);
                    }
                }/*** finish DTC snapshot read ***/
                else
                {
                	response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                }

            }
        }
        break;

        /* 0x06 Subservice from Read DTC */
        case REPORT_DTC_EXTENDED_DATA_RECORD_BY_DTC_NUMBER:
        {
        	if (DATA_SIZE_FOUR == size){
				uint8 recNum;
				/* Now let's assume DTC has 3 bytes. */
				DtcNumber = (((uint32)data[0])<<16) + (((uint32)data[1])<<8) + ((uint32)data[2]);       /* polyspace MISRA-C:17.4 [Justified:Low] "The only potential problem here would be accessing outside the memory buffer pointed by the pointer; we trust Polyspace to check whether this happens." */
				GetStatusOfDtc = Dem_GetStatusOfDTC(DtcNumber, DEM_DTC_ORIGIN_PRIMARY_MEMORY, &dtcStatus); /** @req DCM383 */

				if(GetStatusOfDtc != DEM_STATUS_OK){
					response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
				}else{
					resp->buffer_dades[0] = DTC_HIGH_BYTE(DtcNumber);   /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
					resp->buffer_dades[1] = DTC_MID_BYTE(DtcNumber);    /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
					resp->buffer_dades[2] = DTC_LOW_BYTE(DtcNumber);    /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
					resp->buffer_dades[3] = dtcStatus;
					resp->subservice = 0x06;
	//                Iso15765_3IncrementResponseSize(5);


					 /*** start extended data read ***/
					RecordNumber = data[3];//Consider more about the 0xFF;

					if(RecordNumber == 0xFF || RecordNumber == 0x01 || RecordNumber == 0x02 || RecordNumber == 0x03 ||
							RecordNumber == 0x05 || RecordNumber == 0x04)
					{

						startRecNum = 1;
						endRecNum = 5;
	#if 0
						/* because of the definition of DPT in GAC, and Autosar can not fullfill the requirement */

					switch (RecordNumber){
						case 0xFF:	// Report all Extended Data Records for a particular DTC
						startRecNum = 0x00;
						endRecNum = 0xEF;
						break;

						case 0xFE:	// Report all OBD Extended Data Records for a particular DTC
						startRecNum = 0x90;
						endRecNum = 0xEF;
						break;

						default:	// Report one specific Extended Data Records for a particular DTC
						startRecNum = data[3];
						endRecNum = startRecNum;
						break;
					}
	#endif


						uint8 txIndex = 4;
						uint16 recLength = 0;
						Dem_ReturnGetExtendedDataRecordByDTCType getExtendedDataRecordByDtcResult;
						//boolean foundValidRecordNumber = FALSE;
						UI_8 extended_data[20] = {0,0,0,0,1,0,2,0,3,0,4,0,5,0};

						for (recNum = startRecNum; recNum <= endRecNum; recNum++) {
							recLength = 512 - (txIndex + 1);	// Calculate what's left in buffer
							getExtendedDataRecordByDtcResult = Dem_GetExtendedDataRecordByDTC(DtcNumber, DEM_DTC_ORIGIN_PRIMARY_MEMORY, recNum, &extended_data[txIndex+1], &recLength);
							if (getExtendedDataRecordByDtcResult == DEM_RECORD_OK) {
								//foundValidRecordNumber = TRUE;
								if (recLength > 0) {
										extended_data[txIndex++] = recNum;
										 /* Instead of calling Dem_GetSizeOfExtendedDataRecordByDTC() the result from Dem_GetExtendedDataRecordByDTC() is used */
										 /* !req DCM478 */
										 txIndex += recLength;
								}
							}
		//		            if (!foundValidRecordNumber) {
		//
		//                        response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
		//                    }
						}

						switch (RecordNumber)
						{
							case 0xFF:
								resp->buffer_dades[4] = extended_data[4];
								resp->buffer_dades[5] = extended_data[5];
								resp->buffer_dades[6] = extended_data[6];
								resp->buffer_dades[7] = extended_data[7];
								resp->buffer_dades[8] = extended_data[8];
								resp->buffer_dades[9] = 0;//extended_data[9];
								resp->buffer_dades[10] = extended_data[10];
								resp->buffer_dades[11] = extended_data[11];
								resp->buffer_dades[12] = extended_data[12];
								resp->buffer_dades[13] = extended_data[13];
								Iso15765_3IncrementResponseSize(15); //14,11
								break;
							case 0x01:
								resp->buffer_dades[4] = extended_data[4];
								resp->buffer_dades[5] = extended_data[5];
								Iso15765_3IncrementResponseSize(7);
								break;
							case 0x02:
								resp->buffer_dades[4] = extended_data[6];
								resp->buffer_dades[5] = extended_data[7];
								Iso15765_3IncrementResponseSize(7);
								break;
							case 0x03:
								resp->buffer_dades[4] = extended_data[8];
								resp->buffer_dades[5] = 0;//extended_data[9];
								Iso15765_3IncrementResponseSize(7);
								break;
							case 0x04:
								resp->buffer_dades[4] = extended_data[10];
								resp->buffer_dades[5] = extended_data[11];
								Iso15765_3IncrementResponseSize(7);
								break;
							case 0x05:
								resp->buffer_dades[4] = extended_data[12];
								resp->buffer_dades[5] = extended_data[13];
								Iso15765_3IncrementResponseSize(7);
								break;
							default:
								resp->buffer_dades[4] = extended_data[4];
								resp->buffer_dades[5] = extended_data[5];
								Iso15765_3IncrementResponseSize(7);
								break;
						}

					}
					else{
						response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
					}
				}
			}
			else{
				response_mode = UDS_ERR_INVALID_FORMAT;
			}
		}
        break;


        /* 0x0A Subservice from Read DTC */
        case REPORT_SUPPORTED_DTC:
        {
            if(DATA_SIZE_ZERO==size){
                /* filter the DTC for the DEM_DTC_STATUS_MASK_ALL status mask */
                setDtcFilterResult = Dem_SetDTCFilter(DEM_DTC_STATUS_MASK_ALL, DEM_DTC_KIND_ALL_DTCS, DEM_DTC_FORMAT_UDS, DEM_DTC_ORIGIN_PRIMARY_MEMORY, DEM_FILTER_WITH_SEVERITY_NO, VALUE_IS_NOT_USED, DEM_FILTER_FOR_FDC_NO);
                if (setDtcFilterResult == DEM_FILTER_ACCEPTED) {
                    result = Dem_GetDTCStatusAvailabilityMask(&dtcStatusMask);
                    if (result != E_OK) {
                        dtcStatusMask = 0;
                    }

                    /* Get the number of DTCs to set a limit in the while */
                    getNumerResult = Dem_GetNumberOfFilteredDtc(&numberOfFilteredDtc);
                    if (dtcStatusMask != 0x00) {    /** @req DCM008 */
                        getNextFilteredDtcResult = Dem_GetNextFilteredDTC(&dtc, &dtcStatus);
                        /* Fill the answer with all the DTC founded */
                        while ((getNextFilteredDtcResult == DEM_FILTERED_OK) && ((numberOfFilteredDtc) >= (nrOfDtcs-1))) {
							if (nrOfDtcs < 1024)	/* see AXOECUGBA-1104 */
							{
								resp->buffer_dades[(nrOfDtcs*4)+1] = DTC_HIGH_BYTE(dtc);        /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
								resp->buffer_dades[(nrOfDtcs*4)+2] = DTC_MID_BYTE(dtc);         /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
								resp->buffer_dades[(nrOfDtcs*4)+3] = DTC_LOW_BYTE(dtc);         /* polyspace MISRA-C:10.1 [Justified:Low] "This macro only yields 8 bits." */
								resp->buffer_dades[(nrOfDtcs*4)+4] = dtcStatus;
							}	/* else: we would be accessing outside resp->buffer_dades */
                            nrOfDtcs++;

                            getNextFilteredDtcResult = Dem_GetNextFilteredDTC(&dtc, &dtcStatus);
                        }

                        if (getNextFilteredDtcResult != DEM_FILTERED_NO_MATCHING_DTC) {
                            response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                        }
                    }
                    resp->subservice = 0x0A;
                    resp->buffer_dades[0] = 0xFF/*nrOfDtcs*/;       /* polyspace MISRA-C:10.1 [Justified:Low] "It is expected that nrOfDtcs will have a small enough value." */
                    Iso15765_3IncrementResponseSize((nrOfDtcs*4)+2);
                }
                else {
                    response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                }
            }else{
                response_mode = UDS_ERR_INVALID_FORMAT;
            }
            break;
        }
        /* Not supported subfunction */
        default:
        {
            response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
            break;
        }

    } /* UDS service switch case end */


    /* Initates response sending with approppriate size */
    Iso15765_3SendResponse(response_mode);
}
#endif
/* for hardware reset  to store DTC data */
/*
void write_nvm_all_callback(uint8 ServiceId, NvM_RequestResultType JobResult)
{
	if ((reset_flag == UDS_HARD_RESET)||(reset_flag == UDS_KEY_ON_OFF_RESET)){
		DisableWdgTrigger();
		reset_flag = 0;
	}
	if (clear_dtc_flag == TRUE){
		clear_dtc_flag = FALSE;
		Iso15765_3SendResponse(ISO15765_3_POSITIVE_RESPONSE);
	}
}
*/
void UdsClearDtc(UI_8 groupOfDTCHighByte, UI_8 groupOfDTCMiddleByte, UI_8 groupOfDTCLowByte){
    Dem_ReturnClearDTCType result;
    //tp_uds_control_routine resp;
    UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;

   // memset(&Gateway_DEM_Data,0,sizeof(Gateway_DEM_Type));

   // resp = ISO15765_3_GET_RESP_DATA(tp_uds_control_routine);
    result = Dem_ClearDTC(BYTES_TO_DTC(groupOfDTCHighByte, groupOfDTCMiddleByte, groupOfDTCLowByte),
    		DEM_DTC_FORMAT_UDS, DEM_DTC_ORIGIN_PRIMARY_MEMORY); /** @req DCM005 */

    switch (result) {
    case DEM_CLEAR_OK:
        /* Create positive response *//* @req DCM705 */
    	//NvM_WriteAll();
    	clear_dtc_flag = TRUE;
    	response_mode = UDS_ERR_RESPONSE_PENDING;
        break;
    case DEM_CLEAR_FAILED:
        response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT; /* @req DCM707 */
        break;
    case DEM_CLEAR_WRONG_DTC:
    default:
        response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE; /* @req DCM708 *//* !req DCM706 */
        break;
    }
    Iso15765_3SendResponse(response_mode);
}

#if 0
static const UI_8 seed_gen_tbl[16] = {
      0xA9, 0x85, 0xD6, 0xD3, 0x54, 0x1D, 0xAC, 0x25,
      0x5D, 0x43, 0x18, 0x1E, 0x51, 0xFC, 0xCA, 0x63
};

UI_8 UdsSecurityGetSeed(UI_8 seed_type, UI_8 * seed)
{
	UI_8 i = 0;
	UI_8 seed_rand = 0;
	static UI_8 dummy[SECURITY_SEED_SIZE] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	UI_8 response = ISO15765_3_POSITIVE_RESPONSE;

	if (0x03 == seed_type) {
		tp_uds_security_access_seed_resp resp;
		if(UdsSecurityTaskQueryIfConsecutiveRequestSeed() == FALSE)
		{
			seed_rand = UdsSecurityTaskSeedGeneric();
			for(i = 0;i < sizeof(dummy);i++)
			{
				dummy[i] = seed_gen_tbl[i] ^ seed_rand;
			}
		}
		memcpy(seed, dummy, sizeof(dummy));
		resp = ISO15765_3_GET_RESP_DATA(tp_uds_security_access_seed_resp);
		resp->access_mode = 0x03;
		Iso15765_3IncrementResponseSize(1);
		for(i = 0;i < sizeof(dummy);i++)
		{
			resp->seed[i] = seed[i];
			Iso15765_3IncrementResponseSize(1);
		}

	} else {
		response = UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION;
	}

	return (response);
}

void UdsSecurityNvmCallback(UI_8 *counter)
{
    if(*counter < MAX_SECURITY_ATTEMPTS)
    {
    	security_faa_flag = 0;
    }else{
    	security_faa_flag = 1;
    }
}

static void manufactoryModeInit(void)
{
//   manufactory_mode = GetNvmManufactoryMode();
//
//   if(manufactory_mode >= 0x10)
//   {
//      UdsSecurityTaskForceLockState(UDS_ECU_UNLOCKED_LEVEL1);
//   }else{
//       UdsSecurityTaskForceLockState(UDS_ECU_LOCKED);
//   }
}

UI_8 UdsSecurityLoadNvmCallback(void)
{
	UI_8 counter = 0;

	manufactoryModeInit();

	security_faa_flag = GetNvmFaa();

	if(security_faa_flag == 1)
	{
		counter = MAX_SECURITY_ATTEMPTS;
	}

	return counter;
}

BOOL UdsSecurityCheckKey(UI_8 key_type, UI_8 * seed, UI_8 * key, UI_8 * response_mode)
{
	BOOL ret = FALSE;
	UI_8 cal_key[SECURITY_KEY_SIZE] = {0};
	UI_8 i;

	/* calculate the key */
	AES_cbc_encrypt_process(seed, cal_key, SECURITY_KEY_SIZE, TRUE);
	/* Compare received key */
	for (i = 0; i < SECURITY_KEY_SIZE; i++)
	{
		if (key[i] != cal_key[i]){
			break;
		}
	}

	if (key_type== 0x04){
		if (i == SECURITY_KEY_SIZE){
			tp_uds_security_access_key_resp resp;
			resp = ISO15765_3_GET_RESP_DATA(tp_uds_security_access_key_resp);
			resp->access_mode = 0x04;
			Iso15765_3IncrementResponseSize(1);
			*response_mode = ISO15765_3_POSITIVE_RESPONSE;
			ret = TRUE;
		}else{
			*response_mode = UDS_ERR_INVALID_KEY;
		}

	}else{
		*response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
	}

	return ret;
}

void UdsReadMemoryByAddress(UI_8 mem_size_len, UI_8 mem_addr_len, UI_8 *mem_addr, UI_8 *mem_size)
{
	UI_32 memoryAddress = 0;
	UI_32 length = 0;
	UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;

	if ((mem_size_len == (UI_8)2) && (mem_addr_len == (UI_8)4)) {
		/* Parse address */
		for (UI_8 i = 0; i < mem_addr_len; i++){
			memoryAddress <<= 8;
			memoryAddress += (UI_32)mem_addr[i];
		}
		/* Parse size */
		for (UI_8 i = 0; i < mem_size_len; i++){
			length <<= 8;
			length += (UI_32)mem_size[i];
		}
		/* Need to implement the process
		 * Check address and data length
		 * Read memory data            */
		response_mode = UDS_ERR_WRONG_BLOCK_SEQUENCE;
	} else {
		response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
	}
	Iso15765_3SendResponse(response_mode);
}

void UdsWriteMemoryByAddr(UI_8 addr_data_size, UI_8 *data_buffer, UI_16 size)
{
	UI_32 memoryAddress = 0;
	UI_32 length = 0;
	UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;
    UI_8 mem_addr_len = addr_data_size & (UI_8) 0x0F;
    UI_8 mem_size_len = addr_data_size >> (UI_8) 0x04;
    UI_8 * data;
    UI_16 data_size = 0;

	if ((mem_size_len == (UI_8)2) && (mem_addr_len == (UI_8)4)) {
		/* Parse address */
		for (UI_8 i = 0; i < mem_addr_len; i++){
			memoryAddress <<= 8;
			memoryAddress += (UI_32)data_buffer[i];
		}
		/* Parse size */
		for (UI_8 i = mem_addr_len; i < (mem_size_len + mem_addr_len); i++){
			length <<= 8;
			length += (UI_32)data_buffer[i];
		}
		data = &data_buffer[mem_size_len + mem_addr_len];
		data_size = size - (mem_size_len + mem_addr_len);
		/* Need to implement the process
		 * Check address and data length
		 * Write memory data            */
		response_mode = UDS_ERR_WRONG_BLOCK_SEQUENCE;
	} else {
		response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
	}
	Iso15765_3SendResponse(response_mode);

}
static UI_8 EnableCommunicationControl(UI_8 comtype)
{
	UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;

	switch(comtype)
	{
		case 1:/* enable the normal communication TX*/
			EnableTxFrm_shiftselectposition(); // EnableTxFrm_gsm_1();

			/* Enable Rx */
			CAN_RX_CONFIG_ENABLE();
			break;
		case 2:/* enable the network communication TX */
//			EnableTxFrm_gsm_nm();

			/* Enable Rx */
			CAN_RX_CONFIG_ENABLE();
			break;
		case 3:/* enable all communication TX */
			EnableTxFrm_shiftselectposition();//EnableTxFrm_gsm_1();
//			EnableTxFrm_gsm_nm();

			/* Enable Rx */
			CAN_RX_CONFIG_ENABLE();
			break;
		default:
			response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
			break;
	}

	return response_mode;
}

static UI_8 DisableCommunicationControl(UI_8 comtype)
{
	UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;

	switch(comtype)
	{
		case 1:/* disable normal communication TX*/
			DisableTxFrm_shiftselectposition();//DisableTxFrm_gsm_1();

			/* Disable RX */
			CAN_RX_CONFIG_DISABLE();
			break;
		case 2:/* disable network communication TX*/
//			DisableTxFrm_gsm_nm();

			/* Disable RX */
			CAN_RX_CONFIG_DISABLE();
			break;
		case 3:/* disable normal and network communication TX */
			DisableTxFrm_shiftselectposition(); // DisableTxFrm_gsm_1();
//			DisableTxFrm_gsm_nm();

			/* Disable RX */
			CAN_RX_CONFIG_DISABLE();
			break;
		default:
			response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
			break;
	}

	return response_mode;
}

//<<
static UI_8 EnaRxDisTxCommunicationControl(UI_8 comtype)
{
	UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;

	switch(comtype)
	{
		case 1:/* disable normal communication TX*/
			DisableTxFrm_shiftselectposition();//DisableTxFrm_gsm_1();

			/* Enable RX */
			CAN_RX_CONFIG_ENABLE();
			break;
		case 2:/* disable network communication TX*/
//			DisableTxFrm_gsm_nm();

			/* Enable RX */
			CAN_RX_CONFIG_ENABLE();
			break;
		case 3:/* disable normal and network communication TX */
			DisableTxFrm_shiftselectposition(); // DisableTxFrm_gsm_1();
//			DisableTxFrm_gsm_nm();

			/* Enable RX */
			CAN_RX_CONFIG_ENABLE();
			break;
		default:
			response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
			break;
	}

	return response_mode;
}
//>>

void UdsCommunicationControl(UI_8 subfunction, UI_8 comtype)
{
	UI_8 response_mode = ISO15765_3_POSITIVE_RESPONSE;
	tp_uds_communication_control_resp resp;

	resp = ISO15765_3_GET_RESP_DATA(tp_uds_communication_control_resp);
	resp->ctrl_type = subfunction;
	Iso15765_3IncrementResponseSize(1);

	switch(subfunction)
	{
		case 0:
			response_mode = EnableCommunicationControl(comtype);
			break;
		case 1:
			response_mode = EnaRxDisTxCommunicationControl(comtype);
			break;
		case 3:
			response_mode = DisableCommunicationControl(comtype);
			break;
		default:
			response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
			break;
	}
	Iso15765_3SendResponse(response_mode);
}
#endif

/* Cyclic task. It forwards the signals from Safety partition (SafetyGateway) to QM partition */
void QMGatewayDEMTask(void){
	static uint8 testing = 0;
	//Dem_ReportErrorStatus(DemConf_DemEventParameter_F00617_HIGH_VOLTAGE,Gateway_DEM_Data.event_status_DEM_BAT_HIGH);
	//Dem_ReportErrorStatus(DemConf_DemEventParameter_F00616_LOW_VOLTAGE,Gateway_DEM_Data.event_status_DEM_BAT_LOW);

	//Dem_ReportErrorStatus(DemConf_DemEventParameter_C07300_BUS_OFF,Gateway_DEM_Data.event_status_DEM_BUS_OFF);

	//Dem_ReportErrorStatus(DemConf_DemEventParameter_A06600_TouErr,Gateway_DEM_Data.event_status_DEM_Melexis_DIE1_Fault);
	//Dem_ReportErrorStatus(DemConf_DemEventParameter_A06700_PressErr,Gateway_DEM_Data.event_status_DEM_Melexis_DIE2_Fault);
	//Dem_ReportErrorStatus(DemConf_DemEventParameter_A06800_MorErr,Gateway_DEM_Data.event_status_DEM_Melexis_Heavy_Fault);
	//Dem_ReportErrorStatus(DemConf_DemEventParameter_F00617_HIGH_VOLTAGE,FALSE);
	if(testing == 0)
	{
		Dem_ReportErrorStatus(DemConf_DemEventParameter_F00616_LOW_VOLTAGE,TRUE);
		Dem_ReportErrorStatus(DemConf_DemEventParameter_F00617_HIGH_VOLTAGE,TRUE);
		testing = 1;
	}
}



