
/*----------------------------- includes ---------------------------------*/
//#include "Global.h"
#include "Std_Types.h"
#include "DiagnosticR/Comp_HAL_Autosar_Wrappers/Timer.h"

//#include "TP.h"
#include "DiagnosticR/Comp_ISO_15765_2/TP.h"
//#include "Iso15765_3_CFG.h"
#include "DiagnosticR/UDS/Iso15765_layer3/Iso15765_3_CFG.h"
#include "Iso15765_3_Task.h"
#include "Iso15765_3.h"
#include "DiagnosticR/ProjectCfg.h"

/*-------------------------- Secondary State Machines ---------------------*/


/*----------------------------- macros -----------------------------------*/

/* Decode if asnwer is wanted to UDS request */
#define	SET_POS_RESP_SUPRESS_FLAG(flag) \
	flag=((buf_data_rx[(UI_8)1]&UDS_SUPRESS_POS_RESP_IND)==UDS_SUPRESS_POS_RESP_IND)

/*----------------------------- defines ----------------------------------*/

/* UdsControRoutine: Supported Compression and Encryption Methode */
#define COMPRESSION_METHOD_NONE ((UI_8)0)
#define ENCRYPTING_METHOD_NONE	((UI_8)0)

/* Download Process Maximum Size of each Data Block */
#define MAX_BLOCK_LENGTH ((UI_16)(MAX(TP_DIAG_RX_BUFFER,TP_DIAG_TX_BUFFER)-2))

/* Size of a diagnostic negative response */
#define TP_FRM_NEG_BUF_SIZE ((UI_8)3) 

/* Bit de subfuncio indicatiu de supresio de resposta positiva */
#define UDS_SUPRESS_POS_RESP_IND	((UI_8)0x80)

/* P2 is the maximum time between request-response */
#define TIMER_P2_RESET (TimerDeltaCicleConsulta())

#define REQUEST_SEED_MASK ((UI_8)0x01)

/*-------------------------- Data Types ----------------------------------*/

/*--------------------------- variables globals --------------------------*/

/* Force a Response without having a request: Usefull on start-up answers */
/* Called from Iso15765_3_RequestRespondCtrl.c */
UI_8 force_response = (UI_8) 0x00;

/* Download Control */
t_download_control download_control;


/* Global variables of Security Task*/
#if(UDS_SERVICE_SECURITY_ACCESS_STATUS==SERVICE_ENABLED)
t_security_control security_control;
#endif

/* Response Control */
BOOL iso15765_3_supress_pos_resp;
UI_8 iso15765_3_current_service = (UI_8) 0;
BOOL pending_response = FALSE;
UI_8 pending_response_mode;
t_clock timer_p2;
t_clock timer_s3;
t_iso15765_3_req_mode iso15765_3_request_mode;

//DMP: It cannot be integrated with the current ISO 2
//#if (FUN_DIAG_STATUS == FUN_DIAG_ENABLED)
/* Buffer used to receive TP diagnostic requests FUN */
UI_8 diag_fun_rx_buf[TP_DIAG_FUN_RX_LEN];
//#endif

#ifdef LIN_ISO3
/* Buffer used to receive TP diagnostic requests */
UI_8 diag_rx_buf[TP_DIAG_RX_TX_LEN];
/* Buffer used to send TP positive responses frames */
UI_8 diag_tx_buf[TP_DIAG_RX_TX_LEN];
#else
/* Buffer used to receive TP diagnostic requests PHY */
UI_8 diag_phy_rx_buf[TP_DIAG_RX_LEN];
/* Buffer used to send TP positive responses frames */
UI_8 diag_tx_buf[TP_DIAG_TX_LEN];
#endif
/* Buffer used to send TP negative responses frames */
UI_8 diag_tx_neg[TP_FRM_NEG_BUF_SIZE];
/* Size of the last diagnostic received */
UI_16 iso15765_3_last_frame_size = (UI_16) 0;

/* Size of the constructed transmission diagnostic frame that will be sent */
static UI_16 diag_tx_size_pos;

/*------------------------- Prototypes Local Functions  ---------------------*/
#ifdef UDS_SERVICE_REQUEST_DOWNLOAD
static void UdsRequestDownload(UI_8 data_format, UI_8 addr_len_format,
        UI_8 *mem_addr, UI_8 *mem_size);
#endif
#ifdef UDS_SERVICE_TRANSFER_DATA
static void UdsTransferData(UI_8 block_seq, UI_16 size, UI_8 *data_buf);
#endif
#ifdef UDS_SERVICE_REQUEST_TRANSFER_EXIT
static void UdsTransferExit(UI_16 size, UI_8 *data_buf);
#endif

#ifndef UDS_SERVICE_SECURITY_ACCESS

UI_8 UdsSecurityTaskQueryAccessStatus(void) {
    return UDS_ECU_UNLOCKED_LEVEL1;
}
#endif

/*-------------------------------- Functions ------------------------------*/
#ifdef UDS_INI_DEFAULT_SESSION_FROM_BOOTLOADER
extern void UDS_INI_DEFAULT_SESSION_FROM_BOOTLOADER(void);
#endif

#ifdef UDS_READ_APP_VALID
extern BOOL UDS_READ_APP_VALID(void);
#endif

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Rutina d'inicialitzacio del Uds.
|---------------------------------------------------------------------------
| Arguments:
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
void InicialitzaIso15765_3Task(void) {
    /* Global Initializations */
    iso15765_3_current_service = (UI_8) 0;
    download_control.download_status = DOWNLOAD_STATUS_IDLE;

    /* Compulsary State Machines Inits */
    Iso15765_3_RequestRespondCtrlInicialitza();

    /* Optional State Machines Initialization */
#ifdef UDS_SERVICE_SESSION_CONTROL
    Iso15765_3_SessionCtrlTaskInicialitza();
#endif	
#ifdef UDS_SERVICE_ECU_RESET
    Iso15765_3_EcuResetTaskInicialitza();
#endif	
#ifdef UDS_SERVICE_SECURITY_ACCESS
   // Iso15765_3_SecurityTaskInicialitza();
#endif

}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Tasca principal del Uds que ha de ser cridada de forma periodica en
|    el bucle principal del programa. 
|---------------------------------------------------------------------------
| Arguments:
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
void Iso15765_3Task(void) {
    /* Compulsary State Machines */
    Iso15765_3_RequestRespondCtrl();

    /* Optional State Machines */
#ifdef UDS_SERVICE_SESSION_CONTROL
    Iso15765_3_SessionCtrlTask();
#endif
#ifdef UDS_SERVICE_ECU_RESET
    Iso15765_3_EcuResetTask();
#endif
#ifdef UDS_SERVICE_SECURITY_ACCESS
   // Iso15765_3_SecurityTask();
#endif

}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|   Used by the callbacks to increment the size of the positive response
|---------------------------------------------------------------------------
| Arguments:
|   resp_frm_size: NUmber of bytes added to the response buffer 
|                  by the callback
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
void Iso15765_3IncrementResponseSize(UI_16 resp_frm_size) {
    diag_tx_size_pos += resp_frm_size;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|   Used by the callbacks to increment the size of the positive response
|---------------------------------------------------------------------------
| Arguments:
|   resp_frm_size: NUmber of bytes added to the response buffer 
|                  by the callback
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
UI_16 Iso15765_3GetResponseSize(void) {
    return diag_tx_size_pos;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Rutina que activa el mecanisme de resposta a una peticio de servei de
|    forma asincrona.
|  * Tot servei ha d'acabar amb la crida a aquesta rutina.
|  * Quan s'envia com a resposta ISO15765_3_ERR_RESPONSE_PENDING s'activa
|    l'enviament periodic de respostes negatives amb peticio d'extensio
|    de temps fins que s'envia amb aquesta rutina una altre resposta.
|---------------------------------------------------------------------------
| Arguments:
|   resp_mode: ISO15765_3_POSITIVE_RESPONSE positive response
|              ISO15765_3_ERR_RESPONSE_PENDING pending response
|              Any other value is considered an error code (neg response)
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
void Iso15765_3SendResponse(UI_8 resp_mode)//DMP: Revisar bug TCU on respostes sincrones xocaven amb request asincrones
{
    /* JCL: ISO14299 -> If the response is a NRC = PENDING the positive
         answer must be sent. */
    if (resp_mode == UDS_ERR_RESPONSE_PENDING) {
        iso15765_3_supress_pos_resp = FALSE;
    }
    /* Save the asynchronous response pending type */
    pending_response_mode = resp_mode;
    /* Set the flag to indicate the response pending to the synchronous mechanism */
    pending_response = TRUE;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Rutina que s'ha de cridar de forma sincrona que desancadena l'enviament
|    de resposta a la capa inferior
|  * Aquesta rutina nomes pot ser cridada de forma sincrona dins la maquina
|    d'estats de capa 3
|---------------------------------------------------------------------------
| Arguments:
|   resp_mode: ISO15765_3_POSITIVE_RESPONSE positive response
|              ISO15765_3_ERR_RESPONSE_PENDING pending response
|              Any other value is considered an error code (neg response)
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
void Iso15765_3SendResponseSync(UI_8 resp_mode)//DMP: Revisar bug TCU on respoestes sincrones xocaven amb request asincrones
{ /* ISO3_SEND */
    /* Reset the unblock_after_tx flag, will be set only in specific cases */
    tp_frm[DIAG_ISO15765_2_HDL].unblock_after_tx = FALSE;

    /* Positive Response */
    if (resp_mode == ISO15765_3_POSITIVE_RESPONSE) {
        /* Positive response disabled by the service for this request */
        if (iso15765_3_supress_pos_resp == TRUE) {
            if (iso15765_3_request_mode == ISO15765_3_FUN_REQUEST) {
                tp_fun_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
                /* Unblock ISO15765_2 to accept the entry of new services */
                tp_frm[DIAG_ISO15765_2_HDL].iso15765_2_block_rx = FALSE;
            } else {
                tp_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
                /* Unblock ISO15765_2 to accept the entry of new services */
                tp_frm[DIAG_ISO15765_2_HDL].iso15765_2_block_rx = FALSE;
            }
        }/* Positive response requested */
        else {
            /* Make sure no funcional answer is triggered */
            if (iso15765_3_request_mode == ISO15765_3_FUN_REQUEST) {
                tp_fun_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
                /* Unblock ISO15765_2 to accept the entry of new services */
                tp_frm[DIAG_ISO15765_2_HDL].iso15765_2_block_rx = FALSE;
            }
            /* Book the positive sending */
            tp_frm[DIAG_ISO15765_2_HDL].size_tx = diag_tx_size_pos;
            tp_frm[DIAG_ISO15765_2_HDL].data_tx = diag_tx_buf;
            tp_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
            tp_frm[DIAG_ISO15765_2_HDL].sts_tx = TP_FRM_TX_REQ;
            tp_frm[DIAG_ISO15765_2_HDL].unblock_after_tx = TRUE;
        }
    }/* Response Pending */
    else if (resp_mode == UDS_ERR_RESPONSE_PENDING) {
        diag_tx_neg[0] = UDS_NEG_RESP_CODE;
        /* Get the id serviced directly from the RX buffer because this */
        /* service has not been processed */
        diag_tx_neg[1] = tp_frm[DIAG_ISO15765_2_HDL].data_rx[0];
        diag_tx_neg[2] = resp_mode;

        tp_frm[DIAG_ISO15765_2_HDL].data_tx = diag_tx_neg;
        tp_frm[DIAG_ISO15765_2_HDL].size_tx = UDS_NEG_RESP_SIZE;
        tp_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
        tp_frm[DIAG_ISO15765_2_HDL].sts_tx = TP_FRM_TX_REQ;
        tp_frm[DIAG_ISO15765_2_HDL].unblock_after_tx = TRUE;

        timer_p2 = TIMER_P2_RESET;
    }/* Negative Response */
    else {
        /* Response not needed */
        /* ISO makes us to remove the response in the following cases */
        if ((iso15765_3_request_mode == ISO15765_3_FUN_REQUEST) &&
                ((resp_mode == UDS_ERR_SERVICE_NOT_SUPPORTED) ||
                (resp_mode == UDS_ERR_SUBFUNCTION_NOT_SUPPORTED) ||
                (resp_mode == UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION) ||
                /* DMN: No response should be sent in functional: page 23-24 of ISO 14229-1: "Road vehicles - Unified diagnostic services (UDS)" */
                (resp_mode == UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION) ||
                (resp_mode == UDS_ERR_REQUEST_OUT_OF_RANGE))) {
            //tp_fun_frm.sts=TP_FRM_IDLE;
            /* Unblock ISO15765_2 to accept the entry of new services */
            //UnblockIso15765_2();
            tp_fun_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
            /* Unblock ISO15765_2 to accept the entry of new services */
            tp_frm[DIAG_ISO15765_2_HDL].iso15765_2_block_rx = FALSE;
        }/* Needed Response */
        else {
            /* Make sure no funcional answer is triggered */
            if (iso15765_3_request_mode == ISO15765_3_FUN_REQUEST) {
                tp_fun_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
                /* Unblock ISO15765_2 to accept the entry of new services */
                tp_frm[DIAG_ISO15765_2_HDL].iso15765_2_block_rx = FALSE;
            }
            diag_tx_neg[0] = UDS_NEG_RESP_CODE;
            diag_tx_neg[1] = iso15765_3_current_service;
            diag_tx_neg[2] = resp_mode;
            tp_frm[DIAG_ISO15765_2_HDL].data_tx = diag_tx_neg;
            tp_frm[DIAG_ISO15765_2_HDL].size_tx = UDS_NEG_RESP_SIZE;
            tp_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
            tp_frm[DIAG_ISO15765_2_HDL].sts_tx = TP_FRM_TX_REQ;
            tp_frm[DIAG_ISO15765_2_HDL].unblock_after_tx = TRUE;
        }
    }

    /* Clear the asyncrhonous response pending flag */
    pending_response = FALSE;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Rutina que recupera la ISO15765_3 en cas que la capa d'aplicacio no 
|    hagi donat resposta durant un timeout de error maxim. Aquesta rutina
|    posara la capa 2 i la capa 3 en estat idle per tornar a acceptar peticions
|    de diagnostic.
|---------------------------------------------------------------------------
| Arguments:
|   resp_mode: ISO15765_3_POSITIVE_RESPONSE positive response
|              ISO15765_3_ERR_RESPONSE_PENDING pending response
|              Any other value is considered an error code (neg response)
|   unblock_iso2: flag that says to ISO15765_2 if has to unblock himself
|                 at the end of this next transmission
|---------------------------------------------------------------------------
| 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 Iso15765_3AbortResponse(void) {

    /* Check the last request mode */
    if (iso15765_3_request_mode == ISO15765_3_FUN_REQUEST) {
        /* Set the diagnostic status as idle to be prepared for new requests */
        tp_fun_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
    } else {
        /* Set the diagnostic status as idle to be prepared for new requests */
        tp_frm[DIAG_ISO15765_2_HDL].sts_rx = TP_FRM_RX_IDLE;
        tp_frm[DIAG_ISO15765_2_HDL].sts_tx = TP_FRM_TX_IDLE;
    }
    /* Unblock ISO2 to allow new requests be processed */
    tp_frm[DIAG_ISO15765_2_HDL].iso15765_2_block_rx = FALSE;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Funcio de consulta del mode d'adreçament de la peticio de servei actual
|---------------------------------------------------------------------------
| Arguments:
|  resultat: KWP_PHY_REQUEST si s'ha fet una peticio fisica (punt a punt)
|            KWP_FUN_REQUEST si s'ha fet una peticio funcional (broadcast)
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
t_iso15765_3_req_mode Iso15765_3QueryRequestMode(void) {
    return iso15765_3_request_mode;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|   Used by any function to know the size of the digan frame
|---------------------------------------------------------------------------
| 
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE
| Tmax Int En :   ? CPU cycles | O(n): CTE
| Tmax Total  :   ? CPU cycles | O(n): CTE
/---------------------------------------------------------------------------*/
UI_16 Iso15765_3QueryRequestSize(void) {
    return iso15765_3_last_frame_size;
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|  * Rutina descodificadora del servei requerit en la peticio de servei actual
|---------------------------------------------------------------------------
| Arguments:
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
void Iso15765_3Servicios(void) { /* ISO3_ENTRY */

    BOOL request_size_ok = TRUE;
    BOOL error_detected = FALSE;
    UI_8 *buf_data_rx = NULL;
    SI_8 service_subfunction = 0;
    UI_16 size = 0;
    UI_16 aux_index = 0;
    UI_8 current_session_mask = 0;
    UI_8 response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
    UI_8 response_mode_aux = UDS_ERR_REQUEST_OUT_OF_RANGE;

    /* Get the addressing mode */
    /* Physical requests get priority */
    if (tp_frm[DIAG_ISO15765_2_HDL].sts_rx == TP_FRM_RX_NOTIF) {
        iso15765_3_request_mode = ISO15765_3_PHY_REQUEST;
        buf_data_rx = &tp_frm[DIAG_ISO15765_2_HDL].data_rx[0];
        size = tp_frm[DIAG_ISO15765_2_HDL].size_rx;
    } else if (tp_fun_frm[DIAG_ISO15765_2_HDL].sts_rx == TP_FRM_RX_NOTIF) {
        iso15765_3_request_mode = ISO15765_3_FUN_REQUEST;
        buf_data_rx = &tp_fun_frm[DIAG_ISO15765_2_HDL].data_rx[0];
        size = tp_fun_frm[DIAG_ISO15765_2_HDL].size;
    } else {
        buf_data_rx = NULL;
        error_detected = TRUE;
    }

    /* By default, we set the suppression of pos response to FALSE */
    /* Each service will modify this according to the incoming frame */
    /* if needed */
    iso15765_3_supress_pos_resp = FALSE;

    /* Save the iso15765_3 frame size to be able to get it from other functions */
    iso15765_3_last_frame_size = size;

    /* Get the requested service */
    if ((size >= (UI_8) 1) && (error_detected == FALSE) && (buf_data_rx != NULL)) {
        iso15765_3_current_service = buf_data_rx[(UI_8) 0];
        service_subfunction = (buf_data_rx[(UI_8) 1] & UDS_SUBFUNCTION_MASK);
        /* Set the byte 0 of the positive response frame */
        /* Reset the counter of TX Frame Length */
        diag_tx_buf[0] = iso15765_3_current_service + UDS_POS_RESP_CODE;
        diag_tx_size_pos = 1;
    } else {
        iso15765_3_current_service = (UI_8) 0;
    }

    /* Integrated Session Control Checking */
    current_session_mask = Iso15765_3QueryActiveSessionMask();

    /* Switch to the requested service routine */
    switch (iso15765_3_current_service) {
#ifdef LIN_SERVICE_ASSIGN_FRAME_IDENTIFIER
        case LIN_ASSIGN_FRAME_IDENTIFIER:
            if ((current_session_mask & LIN_SERVICE_ASSIGN_FRAME_IDENTIFIER) != 0x00) {
                /* Leave positive-response-not-requested flag to FALSE */
                /* Call service routine */
                if (size >= (UI_8) 5) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    LIN_ASSIGN_FRAME_IDENTIFIER_CALLBACK(buf_data_rx[(UI_8) 1], buf_data_rx[(UI_8) 2], buf_data_rx[(UI_8) 3], buf_data_rx[(UI_8) 4], buf_data_rx[(UI_8) 5]);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif

#ifdef LIN_SERVICE_ASSIGN_FRAME_IDENTIFIER_RANGE
        case LIN_ASSIGN_FRAME_IDENTIFIER_RANGE:
            if ((current_session_mask & LIN_SERVICE_ASSIGN_FRAME_IDENTIFIER_RANGE) != 0x00) {
                /* Leave positive-response-not-requested flag to FALSE */
                /* Call service routine */
                if (size >= (UI_8) 5) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    LIN_ASSIGN_FRAME_IDENTIFIER_RANGE_CALLBACK(buf_data_rx[(UI_8) 1], &buf_data_rx[(UI_8) 2]);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif

#ifdef LIN_SERVICE_READ_BY_IDENTIFIER
        case LIN_READ_BY_IDENTIFIER:
            if ((current_session_mask & LIN_SERVICE_READ_BY_IDENTIFIER) != 0x00) {
                /* Leave positive-response-not-requested flag to FALSE */
                /* Call service routine */
                if (size >= (UI_8) 1) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    LIN_READ_BY_IDENTIFIER_CALLBACK(buf_data_rx[(UI_8) 1]);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif

#ifdef LIN_SERVICE_ASSIGN_NAD
        case LIN_ASSIGN_NAD:
            if ((current_session_mask & LIN_SERVICE_ASSIGN_NAD) != 0x00) {
                /* Leave positive-response-not-requested flag to FALSE */
                /* Call service routine */
                if (size >= (UI_8) 5) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    LIN_ASSIGN_NAD_CALLBACK(*(UI_16*) & buf_data_rx[(UI_8) 1], *(UI_16*) & buf_data_rx[(UI_8) 3], buf_data_rx[(UI_8) 5]);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif


#ifdef LIN_SERVICE_CONDITIONAL_CHANGE_NAD
        case LIN_CONDITIONAL_CHANGE_NAD:
            if ((current_session_mask & LIN_SERVICE_CONDITIONAL_CHANGE_NAD) != 0x00) {
                /* Leave positive-response-not-requested flag to FALSE */
                /* Call service routine */
                if (size >= (UI_8) 5) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    LIN_CONDITIONAL_CHANGE_NAD_CALLBACK(buf_data_rx[(UI_8) 1], buf_data_rx[(UI_8) 2], buf_data_rx[(UI_8) 3], buf_data_rx[(UI_8) 4], buf_data_rx[(UI_8) 5]);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif


#ifdef UDS_SERVICE_SESSION_CONTROL		
        case UDS_DIAGNOSTIC_SESSION_CONTROL:
            if ((current_session_mask & UDS_SERVICE_SESSION_CONTROL) != 0x00) {
#ifndef ISO15765_3_GEELY
                /* In GEELY UDS the first thing to check is the adressing.  Should not respond with 
                 bad format in functional mode*/
                if (size >= (UI_8) 2) {
#endif
                    /* Check positive-response-not-requested flag */
                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
#if (ISO_3_MODE == ISO15765_3_BL)
#ifndef ISO15765_3_GEELY                    
                    /* For this reason the changes are under compiler defines */
                    if ((current_session_mask == UDS_PROGRAMMING_SESSION_MASK) && (service_subfunction == UDS_EXT_DIAG_SESSION)) {
#else
                    /* In bootloader, we support an extended session, however, we do not support to transition from programming to extended */
                    /* In bootloader, we support an extended session, however, we do not support to transition from programming to extended */
                    /* In Geely specification, we can only reach programming session from extended session */
                    if (
                            ((current_session_mask == UDS_PROGRAMMING_SESSION_MASK) && (service_subfunction == UDS_EXT_DIAG_SESSION)) ||
                            (((current_session_mask != UDS_EXT_DIAG_SESSION_MASK) && (current_session_mask != UDS_PROGRAMMING_SESSION_MASK)) && (service_subfunction == UDS_PROGRAMMING_SESSION))
                            ) {
#endif
                        /* Negative response: We do not support this in active programming session */
                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                    }/* Call service routine only in Phy request and no prog session*/
                    else if ((iso15765_3_request_mode != ISO15765_3_FUN_REQUEST) || ((UI_8) service_subfunction != UDS_PROGRAMMING_SESSION)) {
#ifdef ISO15765_3_GEELY
                        /* In GEELY UDS the first thing to check is the adressing.  Should not respond with 
                         bad format in functional mode*/
                        if (size >= (UI_8) 2) {
#endif                    
                            request_size_ok = TRUE;
                            UDServiceSessionCtrl(service_subfunction, size);
#ifdef ISO15765_3_GEELY
                            /* In GEELY UDS the first thing to check is the adressing.  Should not respond with 
                             bad format in functional mode*/
                        } else {
                            request_size_ok = FALSE;
                        }
#endif                    
                    } else if (size > (UI_8) 2) {
                        /* Programming session in functional addressing with wrong size -> Invalid format*/
                        request_size_ok = FALSE;
                    } else {
                        /* Programming session in functional addressing with correct size -> Subfunction not supported (not sent in functional addressing)*/
                        request_size_ok = TRUE;
                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                    }
#else
                    /* Call service routine only in Phy request and no prog session*/
                    if ((iso15765_3_request_mode != ISO15765_3_FUN_REQUEST) || ((UI_8) service_subfunction != UDS_PROGRAMMING_SESSION)) {
#ifndef ISO15765_3_GEELY
                        if (size >= (UI_8) 2) {
                            request_size_ok = TRUE;
                            UDServiceSessionCtrl(service_subfunction, size);
#else
                        /* In Geely specification, we can only reach programming session from extended session */
                        /* For this reason the changes are under compiler defines */
                        if ((current_session_mask != UDS_EXT_DIAG_SESSION_MASK) && (service_subfunction == UDS_PROGRAMMING_SESSION)) {
                            Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                        } else {
                            if (size >= (UI_8) 2) {
                                request_size_ok = TRUE;
                                UDServiceSessionCtrl(service_subfunction, size);
                            } else {
                                request_size_ok = FALSE;
                            }
                        }
#endif
#ifndef ISO15765_3_GEELY
                        } else {
                            request_size_ok = FALSE;
                        }
#endif
                    } else if (size > (UI_8) 2) {
                        /* Programming session in functional addressing with wrong size -> Invalid format*/
                        request_size_ok = FALSE;
                    } else {
                        /* Programming session in functional addressing with correct size -> Subfunction not supported (not sent in functional addressing)*/
                        request_size_ok = TRUE;
                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                    }
#endif
#ifndef ISO15765_3_GEELY
                    /* In GEELY UDS the first thing to check is the adressing.  Should not respond with 
                     bad format in functional mode*/
                } else {
                    request_size_ok = FALSE;
                }
#endif
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif		
#ifdef UDS_SERVICE_ECU_RESET
        case UDS_ECU_RESET:
            if ((current_session_mask & UDS_SERVICE_ECU_RESET) != 0x00) {
                /* ISO14229-1 - Only 2 bytes in this service */
                if (size == (UI_8) 2) {
                    /* Check positive-response-not-requested flag */
                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                    /* Call service routine */
                    UDSResetRequest(service_subfunction, (size - 1));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_SECURITY_ACCESS
        case UDS_SECURITY_ACCESS:
            if ((current_session_mask & UDS_SERVICE_SECURITY_ACCESS) != 0x00) {
                if ((size >= (UI_8) 2) && (Iso15765_3QueryRequestMode() == ISO15765_3_PHY_REQUEST)) {
                    /* Check positive-response-not-requested flag,
                     * Suppress bit not supported in Geely */
                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                        if ((service_subfunction & 0x03) != 0) {
#if(ISO15765_3_VARIANT  == ISO15765_3_FIAT)
                            /* CHECK VALID SEED REQUESTS FOR FIAT */
                            if (((current_session_mask == UDS_PROGRAMMING_SESSION_MASK) && (service_subfunction == 0x01)) ||
                                    ((current_session_mask == UDS_EXT_DIAG_SESSION_MASK) && (service_subfunction == 0x05))) {
                                /* ECU is requesting a seed */
                                if (size == 2) {
                                    UdsSecurityTaskRequestSeed(service_subfunction);
                                } else {
                                    request_size_ok = FALSE;
                                }
                            } else if ((service_subfunction == 0x01) || (service_subfunction == 0x05)) {
                                Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                            } else {
                                Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                            }

#else

                            if(service_subfunction == 0x03)
                            {
								if(size == 2)
								{
									/* GO DIRECTLY TO THE REQUEST SEED CALLBACK */
									/* ECU is requesting a seed */
									//UdsSecurityTaskRequestSeed(service_subfunction);
								}else{
									 request_size_ok = FALSE;
								}
                            }else{
                            	 Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                            }
#endif
                        } else {
#if(ISO15765_3_VARIANT  == ISO15765_3_FIAT)
                            /* FIRST CHECK THE VALID SUBFUNCTIONS FOR FIAT */
                            if (((current_session_mask == UDS_PROGRAMMING_SESSION_MASK) && (service_subfunction == 0x02)) ||
                                    ((current_session_mask == UDS_EXT_DIAG_SESSION_MASK) && (service_subfunction == 0x06))) {
                                if (size == ((UI_8) 2 + SECURITY_KEY_SIZE)) {
                                    /* ECU is sending the security key */
                                    UdsSecurityTaskCheckKey(service_subfunction, &buf_data_rx[2]);
                                } else {
                                    request_size_ok = FALSE;
                                }
                            } else if ((service_subfunction == 0x02) || (service_subfunction == 0x06)) {
                                Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                            } else {
                                Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                            }
#else
#ifndef ISO15765_3_GEELY
                            if(service_subfunction == 0x04)
                            {
								/* We don't know the valid subfunctions, go directly to the FSM. */
								if (size >= ((UI_8) 2 + SECURITY_KEY_SIZE)) {
									/* ECU is sending the security key */
									//UdsSecurityTaskCheckKey(service_subfunction, &buf_data_rx[2]);
								} else {
									request_size_ok = FALSE;
								}
                            }else{
                            	Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                            }
#else
                            /* In Geely the sessions are checked before checking the length */
                            switch (service_subfunction) {
                                case 0x12:
                                    if (current_session_mask == UDS_PROGRAMMING_SESSION_MASK) {
                                        if (size == ((UI_8) 2 + SECURITY_KEY_SIZE)) {
                                            /* ECU is sending the security key */
                                            UdsSecurityTaskCheckKey(service_subfunction, &buf_data_rx[2]);
                                        } else {
                                            request_size_ok = FALSE;
                                        }
                                    } else {
                                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                                    }
                                    break;
                                case 0x02:
                                    if (current_session_mask == UDS_EXT_DIAG_SESSION_MASK) {
                                        if (size == ((UI_8) 2 + SECURITY_KEY_SIZE)) {
                                            /* ECU is sending the security key */
                                            UdsSecurityTaskCheckKey(service_subfunction, &buf_data_rx[2]);
                                        } else {
                                            request_size_ok = FALSE;
                                        }
                                    } else {
                                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                                    }
                                    break;
                                case 0x62:
                                    if (current_session_mask == UDS_SYSTEM_SUPPLIER_SPECIFIC_SESSION_MASK) {
                                        if (size == ((UI_8) 2 + SECURITY_KEY_SIZE)) {
                                            /* ECU is sending the security key */
                                            UdsSecurityTaskCheckKey(service_subfunction, &buf_data_rx[2]);
                                        } else {
                                            request_size_ok = FALSE;
                                        }
                                    } else {
                                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPORTED_IN_ACTIVE_SESSION);
                                    }
                                    break;
                                default:
                                    Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                                    break;
                            }

#endif
#endif
                        }
#ifndef ISO15765_3_GEELY                
                } else if ((size >= (UI_8) 2) && (Iso15765_3QueryRequestMode() != ISO15765_3_PHY_REQUEST)) {
                    request_size_ok = TRUE;
                    /* Service not supported in functional addressing */
                    Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
                } else {
                    /* Wrong size */
                    request_size_ok = FALSE;
                }
#else
                    /* In Geely we must answer service not supported in functional, even before
                     checking the number of paramters */
                } else if (Iso15765_3QueryRequestMode() != ISO15765_3_PHY_REQUEST) {
                    /* Service not supported in functional addressing */
                    Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
                } else {
                    if (size >= (UI_8) 2) {
                        request_size_ok = TRUE;
                    } else {
                        /* Wrong size */
                        request_size_ok = FALSE;
                    }
                }
#endif
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_TESTER_PRESENT
        case UDS_TESTER_PRESENT:
            if ((current_session_mask & UDS_SERVICE_TESTER_PRESENT) != 0x00) {
                if ((size == (UI_8) 2) && (service_subfunction == (UI_8) 0x00)) {
                    tp_uds_tester_present_resp resp;
                    /* Check positive-response-not-requested flag */
                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                    /* Send Tester Present Echo */
                    resp = ISO15765_3_GET_RESP_DATA(tp_uds_tester_present_resp);
                    resp->zero = (UI_8) 0;
                    Iso15765_3IncrementResponseSize(1);
                    Iso15765_3SendResponse(ISO15765_3_POSITIVE_RESPONSE);
                    request_size_ok = TRUE;
                } else if ((size >= (UI_8) 2) && (service_subfunction != (UI_8) 0x00)) {
                    /* According to ISO 14229, other subfunctions */
                    /* different to 0 are reserved */
                    Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_CONTROL_DTC_SETTING
        case UDS_CONTROL_DTC_SETTING:
            if ((current_session_mask & UDS_SERVICE_CONTROL_DTC_SETTING) != 0x00) {
                if (size >= (UI_8) 2) {
                    /* Check positive-response-not-requested flag */
                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                    /* Call service routine */
                    UDS_SERVICE_CONTROL_DTC_SETTING_CALLBACK(
                            service_subfunction,
                            (UI_8*) (&buf_data_rx[(UI_8) 2]),
                            (size - 2));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_READ_DATA_BY_IDENTIFIER
        case UDS_READ_DATA_BY_IDENTIFIER:
            if ((current_session_mask & UDS_SERVICE_READ_DATA_BY_IDENTIFIER) != 0x00) {
                if (size == (UI_8) 3) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    response_mode = UDS_SERVICE_READ_DATA_BY_IDENTIFIER_CALLBACK(
                            (UI_8) buf_data_rx[1],
                            (UI_8) buf_data_rx[2]);
                    Iso15765_3SendResponse(response_mode);
                    request_size_ok = TRUE;
                }/* multi-parameters */
                else if ((size >= (UI_16) 5) && ((size % (UI_16) 2) == (UI_16) 1)) {
                    aux_index = 1;
                    while ((aux_index < size)&&(response_mode != UDS_ERR_BUSY_REPEAT_REQUEST)) {
                        response_mode_aux = UDS_SERVICE_READ_DATA_BY_IDENTIFIER_CALLBACK(
                                (UI_8) buf_data_rx[aux_index],
                                (UI_8) buf_data_rx[aux_index + (UI_16) 1]);
                        aux_index = aux_index + (UI_16) 2;

                        /* If any of the RDIs is POSITIVE -> the answer will be positive*/
                        if (response_mode_aux == ISO15765_3_POSITIVE_RESPONSE) {
                            response_mode = ISO15765_3_POSITIVE_RESPONSE;
                        }/* If any of the RDIs response is BUSY -> cancel the operation and send the BUSY response */
                        else if (response_mode_aux == UDS_ERR_BUSY_REPEAT_REQUEST) {
                            response_mode = UDS_ERR_BUSY_REPEAT_REQUEST;
                        }
                    }
                    Iso15765_3SendResponse(response_mode);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_READ_MEMORY_BY_ADDRESS   //NOT IMPLEMENTED YET
        case UDS_READ_MEMORY_BY_ADDRESS:
            if ((current_session_mask & UDS_SERVICE_READ_MEMORY_BY_ADDRESS) != 0x00) {
                if (size >= (UI_8) 2) {
                    /* Get Parameter: Size in bytes of mem_addr */
                    UI_8 mem_addr_len = buf_data_rx[1] & (UI_8) 0x0F;
                    /* Get Parameter: Size in bytes of mem_size */
                    UI_8 mem_size_len = buf_data_rx[1] >> (UI_8) 0x04;

                    if (size >= (mem_addr_len + mem_size_len + (UI_8) 2)) {
                        /* Leave positive-response-not-requested flag to FALSE*/
                        /* Call service routine */
                        UDS_SERVICE_READ_MEMORY_BY_ADDRESS_CALLBACK(mem_size_len,
                                mem_addr_len,
                                &buf_data_rx[(UI_8) 2],
                                &buf_data_rx[(UI_8) 2 + mem_addr_len]);
                        request_size_ok = TRUE;
                    } else {
                        request_size_ok = FALSE;
                    }
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_WRITE_DATA_BY_IDENTIFIER
        case UDS_WRITE_DATA_BY_IDENTIFIER:
            if ((current_session_mask & UDS_SERVICE_WRITE_DATA_BY_IDENTIFIER) != 0x00) {

#ifndef ISO15765_3_GEELY
                /* In Geely we must discard first by functional and then by format */
                if (size < (UI_8) 2) {
                    request_size_ok = FALSE;
                } else
#endif                
                    if (Iso15765_3QueryRequestMode() == ISO15765_3_PHY_REQUEST) {
#ifndef ISO15765_3_GEELY
                    if (size >= (UI_8) 4) {
#else
                    /* For GEELY there is no need of data, as the security must be 
                     checked before checking the format */
                    if (size >= (UI_8) 3) {
#endif
                        /* Leave positive-response-not-requested flag to FALSE*/
                        /* Call service routine */
                        UDS_SERVICE_WRITE_DATA_BY_IDENTIFIER_CALLBACK(
                                (UI_8) buf_data_rx[1],
                                (UI_8) buf_data_rx[2],
                                (UI_8*)&(buf_data_rx[3]),
                                (UI_16) (size - 3));
                        request_size_ok = TRUE;
                    } else {
                        request_size_ok = FALSE;
                    }
                } else {
#ifndef ISO15765_3_GEELY                    
                    /* This is a service without subfunction, according to that
                     * we should send a CONDITIONS NOT CORRECT if we are in
                     * functional addressing */
                    Iso15765_3SendResponse(UDS_ERR_CONDITIONS_NOT_CORRECT);
#else
                    /* If we are in functional, we should not answer */
                    request_size_ok = TRUE;
                    Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
#endif
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_WRITE_MEMORY_BY_ADDRESS
        case UDS_WRITE_MEMORY_BY_ADDRESS: //NOT IMPLEMENTED YET
            if ((current_session_mask & UDS_SERVICE_WRITE_MEMORY_BY_ADDRESS) != 0x00) {
                if (size >= (UI_8) 6) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    UDS_SERVICE_WRITE_MEMORY_BY_ADDRESS_CALLBACK(buf_data_rx[(UI_8) 1],
                            &buf_data_rx[(UI_8) 2],
                            (size - 2));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_CLEAR_DIAGNOSTIC_INFORMATION
        case UDS_CLEAR_DIAGNOSTIC_INFORMATION:
            if ((current_session_mask & UDS_SERVICE_CLEAR_DIAGNOSTIC_INFORMATION) != 0x00) {
                /* Leave positive-response-not-requested flag to FALSE */
                /* Call service routine */
                if (size == (UI_8) 4) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    UDS_SERVICE_CLEAR_DIAGNOSTIC_INFORMATION_CALLBACK(buf_data_rx[(UI_8) 1], buf_data_rx[(UI_8) 2], buf_data_rx[(UI_8) 3]);
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_READ_DTC_INFORMATION
        case UDS_READ_DTC_INFORMATION:
            if ((current_session_mask & UDS_SERVICE_READ_DTC_INFORMATION) != 0x00) {
                if (size >= (UI_8) 2) {
                    /* Check positive-response-not-requested flag */
                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                    /* Call service routine */
                   UDS_SERVICE_READ_DTC_INFORMATION_CALLBACK(service_subfunction,
                            &buf_data_rx[(UI_8) 2],
                            (size - (UI_8) 2));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif 
#ifdef UDS_SERVICE_IO_CTRL_BY_ID 
        case UDS_INPUT_OUTPUT_CONTROL_BY_ID:
            if ((current_session_mask & UDS_SERVICE_IO_CTRL_BY_ID) != 0x00) {

                if (size >= (UI_8) 3) {
                    /* Leave positive-response-not-requested flag to FALSE */
                    /* Call service routine */
                   // UDS_SERVICE_IO_CTRL_BY_ID_CALLBACK(buf_data_rx[(UI_8) 1],
                        //    buf_data_rx[(UI_8) 2],
                           // buf_data_rx[(UI_8) 3],
                           // (UI_8*) (&(buf_data_rx[(UI_8) 4])),
                           // (size - 3));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_ROUTINE_CONTROL
        case UDS_CONTROL_ROUTINE:
            if ((current_session_mask & UDS_SERVICE_ROUTINE_CONTROL) != 0x00) {
#ifndef ISO15765_3_GEELY
                if (size > (UI_8) 1) {
#endif
                    if (Iso15765_3QueryRequestMode() == ISO15765_3_PHY_REQUEST) {
#ifdef ISO15765_3_GEELY
                        /* In Geely we must first discard functional addressing requests and 
                         then discard by format */
                        if (size > (UI_8) 1) {
#endif                        
                            if ((service_subfunction == UDS_CTRL_ROUTINE_START) ||
                                    (service_subfunction == UDS_CTRL_ROUTINE_STOP) ||
                                    (service_subfunction == UDS_CTRL_ROUTINE_RESULTS)) {
                                if (size >= (UI_8) 4) {
                                    /* Check positive-response-not-requested flag */
                                    SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                                    /* Call service routine */
                                    UDS_SERVICE_ROUTINE_CONTROL_CALLBACK(
                                            service_subfunction,
                                            (UI_8) buf_data_rx[2],
                                            (UI_8) buf_data_rx[3],
                                            &(buf_data_rx[(UI_8) 4]),
                                           (UI_16) (size - (UI_8) 4));
                                    request_size_ok = TRUE;
                                } else {
                                    request_size_ok = FALSE;
                                }
                            } else {
                                Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                            }
#ifdef ISO15765_3_GEELY                        
                        } else {
                            request_size_ok = FALSE;
                        }
#endif
                    } else {
                        /* Not supported in functional addressing */
                        request_size_ok = TRUE;
                        Iso15765_3SendResponse(UDS_ERR_SUBFUNCTION_NOT_SUPPORTED);
                    }
#ifndef ISO15765_3_GEELY                
                } else {
                    request_size_ok = FALSE;
                }
#endif
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_COMMUNICATION_CONTROL
        case UDS_COMMUNICATION_CONTROL:
            if ((current_session_mask & UDS_SERVICE_COMMUNICATION_CONTROL) != 0x00) {
                if (size > (UI_8) 1) { /* We can check the subfunction */
#ifndef ISO15765_3_GEELY                
                    if (size == (UI_8) 3) {
#endif
                        /* Check positive-response-not-requested flag */
                        SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
                        /* Call service routine */
#ifndef ISO15765_3_GEELY                        
                      // UDS_COMMUNICATION_CONTROL_CALLBACK(service_subfunction, (UI_8) buf_data_rx[2]);
#else
                        UDS_COMMUNICATION_CONTROL_CALLBACK(service_subfunction, (UI_8) buf_data_rx[2], size);
#endif
                        request_size_ok = TRUE;
#ifndef ISO15765_3_GEELY                
                    } else {
                        request_size_ok = FALSE;
                    }
#endif
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
            break;
#endif
#ifdef UDS_SERVICE_REQUEST_DOWNLOAD
        case UDS_REQUEST_DOWNLOAD:
#ifdef ISO15765_3_GEELY
            /* In Geely, this service is not allowed in functiona mode */
            if (iso15765_3_request_mode != ISO15765_3_FUN_REQUEST){
#endif            
            if ((current_session_mask & UDS_SERVICE_REQUEST_DOWNLOAD) != 0x00) {
                if (size >= (UI_8) 2) {
                    UI_8 mem_addr_len = buf_data_rx[2] & (UI_8) 0x0F;
                    UI_8 mem_size_len = buf_data_rx[2] >> (UI_8) 0x04;
                    if (size == (mem_addr_len + mem_size_len + (UI_8) 3)) {
                        /* Leave positive-response-not-requested flag to FALSE*/
                        /* Call service routine */
                        UdsRequestDownload(buf_data_rx[1],
                                buf_data_rx[2],
                                &(buf_data_rx[3]),
                                &(buf_data_rx[3 + mem_addr_len]));
                        request_size_ok = TRUE;
                    } else {
                        request_size_ok = FALSE;
                    }
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
#ifdef ISO15765_3_GEELY
            } else {
                /* In functional mode, the service is not supported */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
            }
#endif            
            break;
#endif
#ifdef UDS_SERVICE_TRANSFER_DATA
        case UDS_TRANSFER_DATA:
#ifdef ISO15765_3_GEELY
            /* In Geely, this service is not allowed in functiona mode */
            if (iso15765_3_request_mode != ISO15765_3_FUN_REQUEST){
#endif                       
            if ((current_session_mask & UDS_SERVICE_TRANSFER_DATA) != 0x00) {
                if (size >= (UI_8) 3) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    UdsTransferData(buf_data_rx[(UI_8) 1],
                            (size - (UI_8) 2),
                            &(buf_data_rx[(UI_8) 2]));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
#ifdef ISO15765_3_GEELY
            } else {
                /* In functional mode, the service is not supported */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
            }
#endif            
            
            break;
#endif
#ifdef UDS_SERVICE_REQUEST_TRANSFER_EXIT
        case UDS_REQUEST_TRANSFER_EXIT:
#ifdef ISO15765_3_GEELY
            /* In Geely, this service is not allowed in functiona mode */
            if (iso15765_3_request_mode != ISO15765_3_FUN_REQUEST){
#endif                         
            if ((current_session_mask & UDS_SERVICE_REQUEST_TRANSFER_EXIT) != 0x00) {
                if (size >= (UI_8) 1) {
                    /* Leave positive-response-not-requested flag to FALSE*/
                    /* Call service routine */
                    UdsTransferExit((size - (UI_16) 1),
                            &(buf_data_rx[(UI_8) 1]));
                    request_size_ok = TRUE;
                } else {
                    request_size_ok = FALSE;
                }
            } else {
                /* Service not supported in current session */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION);
            }
#ifdef ISO15765_3_GEELY
            } else {
                /* In functional mode, the service is not supported */
                Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
            }
#endif                    
            break;
#endif
        default:
#ifndef UDS_EXTRA_SERVICES
            /* Unknown service: Send negative response */
            /* Enviament de la resposta negativa */
            Iso15765_3SendResponse(UDS_ERR_SERVICE_NOT_SUPPORTED);
#else
            /* Check positive-response-not-requested flag */
            SET_POS_RESP_SUPRESS_FLAG(iso15765_3_supress_pos_resp);
            UDS_EXTRA_SERVICES_CALLBACK(iso15765_3_current_service, service_subfunction, buf_data_rx, size);
#endif
            break;

    }

    /* Wrong format detected in request frame */
    if (request_size_ok == FALSE) {
        /* Send Negative Response */
        Iso15765_3SendResponse(UDS_ERR_INVALID_FORMAT);
    } else {
        /* N/A */
    }
}

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Functionalities Contract Description:
|  * Rutina d'atencio al servei Diagnostic Session Control
|  * Aquest servei s'utilitza per gestionar la sessio de diagnostic activa
|  * La seva implementacio es obligatoria
|  * S'ha d'enviar la resposta positiva abans que la nova sessio s'activi
|  * Les diferents rutines i funcions del protocol ISO15765_· que es poden
|    fer servir per implementar aquest servei son:
|    Iso15765_3ChangeActiveSession(...) per canviar la sessio activa 
|    Iso15765_3QueryActiveSession() per consultar la sessio activa actual.
|    Iso_15765_3QuerySecurityAccessStatus() per consultar el nivell de
|                                           seguretat actual.
|---------------------------------------------------------------------------
| Parameters Explanation:
|   session: Nova sessio que es vol que sigui la sessio activa
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE 
| Tmax Int En :   ? CPU cycles | O(n): CTE 
| Tmax Total  :   ? CPU cycles | O(n): CTE 
/---------------------------------------------------------------------------*/
#ifdef UDS_SERVICE_SESSION_CONTROL		

void UDServiceSessionCtrl(UI_8 session, UI_8 size) {
    UI_8 response_mode;
    tp_uds_diagnostic_session_control_resp resp;

    /* Obtenim la variable que ens dona acces */
    /* als camps de dades de la resposta      */
    resp = ISO15765_3_GET_RESP_DATA(tp_uds_diagnostic_session_control_resp);

#ifdef UDS_USER_DEFINED_SESSION
    if ((session == UDS_DEFAULT_SESSION) ||
            (session == UDS_EXT_DIAG_SESSION) ||
            (session == UDS_PROGRAMMING_SESSION) ||
            (session == UDS_USER_DEFINED_SESSION)) {
#else
    if ((session == UDS_DEFAULT_SESSION) ||
            (session == UDS_EXT_DIAG_SESSION) ||
            (session == UDS_PROGRAMMING_SESSION)) {
#endif
        if (size == 2) {
            /* Prepare Response */
            resp->diagnostic_session = session;
            resp->p2_high = (UI_8) (P2_MAX >> ((UI_8) 8));
            resp->p2_low = (UI_8) (P2_MAX);
            resp->p2_ext_high = (UI_8) (P2_EXT_MAX >> ((UI_8) 8));
            resp->p2_ext_low = (UI_8) (P2_EXT_MAX);
            Iso15765_3IncrementResponseSize(5);
#if (ISO_3_MODE == ISO15765_3_BL)
#ifdef UDS_READ_APP_VALID
            if ((session == UDS_DEFAULT_SESSION) && (UDS_READ_APP_VALID() == TRUE)) {
                /* IRIS_031_330AI_S3_005432: Do not respond positive response */
                /* until we are sure that the application is running */
                response_mode = UDS_ERR_RESPONSE_PENDING;
#ifdef UDS_INI_DEFAULT_SESSION_FROM_BOOTLOADER
                /* Indicate to the application that it must respond the ACK */
                UDS_INI_DEFAULT_SESSION_FROM_BOOTLOADER();
#endif
#else
            if (FALSE) {
                /* Always positive response if not defined UDS_READ_APP_VALID */
#endif
            } else {
                response_mode = ISO15765_3_POSITIVE_RESPONSE;
            }
#else
            response_mode = ISO15765_3_POSITIVE_RESPONSE;
#endif

            /* Inform the state machine about the request */
            Iso15765_3ChangeActiveSession(session);
        } else {
            response_mode = UDS_ERR_INVALID_FORMAT;
        }
    } else {
        response_mode = UDS_ERR_SUBFUNCTION_NOT_SUPPORTED;
    }

#if (ISO_3_MODE == ISO15765_3_BL)
    /* Send the answer to the request */
    Iso15765_3SendResponse(response_mode);
#else
    if ((response_mode == ISO15765_3_POSITIVE_RESPONSE) && (session == UDS_PROGRAMMING_SESSION)) {
        Iso15765_3SendResponse(UDS_ERR_RESPONSE_PENDING);
    } else {
        /* Send the answer to the request */
        Iso15765_3SendResponse(response_mode);
    }
#endif
} /* UDServiceSessionCtrl */

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Functionalities Contract Description:
|  * Routine to force the system to enter in PROGRAMMING SESSION.
|  * This is used when changing from default to programming session (in most
|  *  of the cases this implies a reset).
|  * This allows to send the response from the correct session.
|---------------------------------------------------------------------------
| Parameters Explanation:
|   session: Nova sessio que es vol que sigui la sessio activa
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? CPU cycles | O(n): CTE
| Tmax Int En :   ? CPU cycles | O(n): CTE
| Tmax Total  :   ? CPU cycles | O(n): CTE
/---------------------------------------------------------------------------*/
void Iso15765_3_ForceSessionChange(UI_8 session, BOOL supress_positive_response) {
    /* Call directly the diagnostic UDS service with the requested programming session */

    /* Set the type of the request */
    iso15765_3_request_mode = ISO15765_3_PHY_REQUEST;
    /* Set the current service ID */
    iso15765_3_current_service = UDS_DIAGNOSTIC_SESSION_CONTROL;
    /* Positive response supression flag */
    iso15765_3_supress_pos_resp = supress_positive_response;
    /* Force the response */
    force_response = 0x10;

    UDServiceSessionCtrl(session, 2);
#ifdef UDS_INI_SESSION_CALLBACK
    UDS_INI_SESSION_CALLBACK();
#endif


}

#endif

/*****************************************************************************
| Portability:  Generica
|----------------------------------------------------------------------------
| Description:
|   * Routine that check if a response has been sent. This routine must be
|     called after sending a response to check when this response has been
|     sent.
|---------------------------------------------------------------------------
| Arguments:
|   * return TRUE if the response has been sent or if a TX problem has been
|     detected. Return FALSE if the response is still pending to be sent.
|---------------------------------------------------------------------------
| Timing:
| Tmax Int Dis:   ? cicles cpu | O(n): CTE 
| Tmax Int En :   ? cicles cpu | O(n): CTE 
| Tmax Total  :   ? cicles cpu | O(n): CTE 
/---------------------------------------------------------------------------*/
BOOL Iso15765_3GetResponseSent(void) {
    BOOL response_sent;
    //t_iso15765_3_req_mode result;

   // result = Iso15765_3QueryRequestMode();
    /* Check if the response has been sent */
    if ((pending_response == FALSE) &&
            ((tp_frm[DIAG_ISO15765_2_HDL].sts_tx == TP_FRM_TX_IDLE) || (tp_frm[DIAG_ISO15765_2_HDL].sts_tx == TP_FRM_TX_ERR_NOTIF))) {
        response_sent = TRUE;
    } else {
        response_sent = FALSE;
    }

    return response_sent;
}


/*****************************************************************************
| Portability:  DZ60
|----------------------------------------------------------------------------
| Functionalities Contract Description:
|  * Rutina d'atencio al servei Request Download
|  * Aquest servei s'utilitza per indicar i configurar l'inici d'una
|    transferencia de dades de l'eina de verificacio cap a l'ECU
|  * Les diferents rutines i funcions del protocol Iso15765_3 que es poden
|    fer servir per implementar aquest servei son:
|    Iso15765_3QueryActiveSession() per consultar la sessio activa actual.
|    Iso15765_3QuerySecurityAccessStatus() per consultar el nivell de seguretat
|                                          actual.
|---------------------------------------------------------------------------
| Parameters Explanation:
|   data_format: bit [7..4]: Compression method.
|              bit [3..0]: Encrypting method.
|              The value 0x00 specifies no compression nor encrypting is used.
|   addr_len_format: bit [7..4]: Length in bytes of the mem_size parameter
|                       bit [3..0]: Length in bytes of the mem_addr parameter
|   mem_addr: adreça de memoria on comença el buffer que s'ha d'omplir
|   mem_size: numero de bytes que s'han de transferir.
/---------------------------------------------------------------------------*/
#ifdef UDS_SERVICE_REQUEST_DOWNLOAD

static void UdsRequestDownload(UI_8 data_format, UI_8 addr_len_format,
        UI_8 *mem_addr, UI_8 *mem_size) {
    UI_8 response_mode;
    tp_uds_request_download resp;
    /* Temporal Working Register */
    UI_32 work_reg;
    BOOL dev_ready = FALSE;

    asm("debug_request_download:");

    resp = ISO15765_3_GET_RESP_DATA(tp_uds_request_download);
    /* By default, set negative response */
    response_mode = ISO15765_3_POSITIVE_RESPONSE;
    /* Programming session? */
    if (Iso15765_3QueryActiveSession() != UDS_DEFAULT_SESSION) {
        /* security level ok */
        if (UdsSecurityTaskQueryAccessStatus() > UDS_ECU_LOCKED) {
            /* Comprovem que no estiguem ja en proces de download */
            if (download_control.download_status != DOWNLOAD_STATUS_ONGOING) {
                /* download process initialization */
                download_control.download_compression = (UI_8) (data_format >> (UI_8) 4);
                download_control.download_encrypting = (UI_8) (data_format & (UI_8) 0x0F);

                /* Comprovacio dels metodes de compressio i encriptacio */
                if ((download_control.download_compression == COMPRESSION_METHOD_NONE) &&
                        (download_control.download_encrypting == ENCRYPTING_METHOD_NONE)) {

                    /* Parse Address and Size parameters Length */
                    UI_8 mem_addr_len = addr_len_format & (UI_8) 0x0F;
                    UI_8 mem_size_len = addr_len_format >> (UI_8) 0x04;
                    UI_8 i;

                    /* Cas en que la mida dels parametres mem_addr i */
                    /* mem_size es correcte                          */
                    /* multidevice extended address (5 bytes) */
                    if ((mem_addr_len <= (UI_8) 5) && (mem_addr_len > (UI_8) 0) &&
                            (mem_size_len <= (UI_8) 4) && (mem_size_len > (UI_8) 0)) {
                        if (mem_addr_len >= ISO15765_3_MIN_ADDRESS_LENGTH_MULTIDEVICE) {
                            /* mem_adr[0] is the device selection byte   */
                            download_control.device_addressed = mem_addr[0];
                        } else {
                            /* If the memory address is 2 bytes or less we
                             consider that we can only have one device. */
                            download_control.device_addressed = DEVICE_0;
                        }
#ifdef UDS_REQUEST_DOWNLOAD_DEVICE_READY
                        dev_ready = UDS_REQUEST_DOWNLOAD_DEVICE_READY(download_control.device_addressed);
#else
                        dev_ready = TRUE;
#endif
                        if (dev_ready == TRUE) {
                            /* Parse base address (starts at byte 1) */
                            if (mem_addr_len >= ISO15765_3_MIN_ADDRESS_LENGTH_MULTIDEVICE) {
                                work_reg = mem_addr[1];
                                for (i = 2; i < mem_addr_len; i++) {
                                    work_reg <<= (UI_8) 8;
                                    work_reg |= mem_addr[i];
                                }

                            }/* For addresses with less than ISO15765_3_MIN_ADDRESS_LENGTH_MULTIDEVICE
                             * bytes we must count the
                             * addres from position 0 */
                            else {
                                work_reg = mem_addr[0];
                                for (i = 1; i < mem_addr_len; i++) {
                                    work_reg <<= (UI_8) 8;
                                    work_reg |= mem_addr[i];
                                }
                            }

                            download_control.base_addr = (t_iso3_addr) work_reg;


                            /* Parse Total Download Size */
                            work_reg = mem_size[0];
                            for (i = 1; i < mem_size_len; i++) {
                                work_reg <<= (UI_8) 8;
                                work_reg |= mem_size[i];
                            }
                            download_control.last_addr = download_control.base_addr +
                                    (t_iso3_addr) work_reg - (t_iso3_addr) 1;
                            /* Prepare Response Buffer in case the response is ok */

#if (ISO15765_3_VARIANT == ISO15765_3_FIAT)
                            /* IT SEEMS THAT DIANALYZER HAS A BUG!!! */
                            resp->len_format_id = 0x20;
#else
                            resp->len_format_id = 0;
#endif

                            /* Set Maximum Size for a Tranfer Request */
                            resp->max_num_of_block_len[0] = (UI_8) (MAX_BLOCK_LENGTH >> (UI_8) 8);
                            resp->max_num_of_block_len[1] = (UI_8) MAX_BLOCK_LENGTH;
                            Iso15765_3IncrementResponseSize(3);

                            /* Download Status Variables Init */
                            download_control.dest_addr = download_control.base_addr;
                            download_control.block_seq = 1;

                            /* Callback to the correct device */
                            switch (download_control.device_addressed) {
#ifdef DEVICE_0_REQUEST_DOWNLOAD_CALLBACK
                                case DEVICE_0:
                                    response_mode = DEVICE_0_REQUEST_DOWNLOAD_CALLBACK(
                                            download_control.base_addr, download_control.last_addr);
                                    break;
#endif
#ifdef DEVICE_1_REQUEST_DOWNLOAD_CALLBACK
                                case DEVICE_1:
                                    response_mode = DEVICE_1_REQUEST_DOWNLOAD_CALLBACK(
                                            download_control.base_addr, download_control.last_addr);
                                    break;
#endif
#ifdef DEVICE_2_REQUEST_DOWNLOAD_CALLBACK
                                case DEVICE_2:
                                    response_mode = DEVICE_2_REQUEST_DOWNLOAD_CALLBACK(
                                            download_control.base_addr, download_control.last_addr);
                                    break;
#endif
#ifdef DEVICE_3_REQUEST_DOWNLOAD_CALLBACK
                                case DEVICE_3:
                                    response_mode = DEVICE_3_REQUEST_DOWNLOAD_CALLBACK(
                                            download_control.base_addr, download_control.last_addr);
                                    break;
#endif
#ifdef DEVICE_4_REQUEST_DOWNLOAD_CALLBACK
                                case DEVICE_4:
                                    response_mode = DEVICE_4_REQUEST_DOWNLOAD_CALLBACK(
                                            download_control.base_addr, download_control.last_addr);
                                    break;
#endif
#ifdef DEVICE_5_REQUEST_DOWNLOAD_CALLBACK
                                case DEVICE_5:
                                    response_mode = DEVICE_5_REQUEST_DOWNLOAD_CALLBACK(
                                            download_control.base_addr, download_control.last_addr);
                                    break;
#endif
                                default:
#ifdef UDS_REQUEST_DOWNLOAD_ERROR_CALLBACK
                                    response_mode = UDS_REQUEST_DOWNLOAD_ERROR_CALLBACK(UDS_REQUEST_DOWNLOAD_ERROR_INVALID_DEVICE);
                                    if (response_mode == 0xFF) {
                                        response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT;
                                    }
#else
                                    response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT;
#endif
                                    break;
                            }
                            if ((response_mode == ISO15765_3_POSITIVE_RESPONSE) || (response_mode == UDS_ERR_RESPONSE_PENDING)) {
                                download_control.download_status = DOWNLOAD_STATUS_ONGOING;
                            }
                        } else {
                            response_mode = UDS_ERR_BUSY_REPEAT_REQUEST;
                        }
                    } else {
                        response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                    } /* end if/else check max length addr and size */
                } else {
#ifdef UDS_REQUEST_DOWNLOAD_ERROR_CALLBACK
                    response_mode = UDS_REQUEST_DOWNLOAD_ERROR_CALLBACK(UDS_REQUEST_DOWNLOAD_ERROR_INCOMPATIBLE_TYPE);
                    if (response_mode == 0xFF) {
                        response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                    }
#else
                    response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
#endif
                } /* end if/else download methodes checking */
            } else {
                /* Download already on going. This request is a sequence error */
                response_mode = UDS_ERR_REQUEST_SEQUENCE_ERROR;
            } /* end if/else state process */
        } else {
#ifdef UDS_REQUEST_DOWNLOAD_ERROR_CALLBACK
            response_mode = UDS_REQUEST_DOWNLOAD_ERROR_CALLBACK(UDS_REQUEST_DOWNLOAD_ERROR_ECU_LOCKED);
            if (response_mode == 0xFF) {
                response_mode = UDS_ERR_SECURITY_ACCESS_DENIED;
            }
#else
            response_mode = UDS_ERR_SECURITY_ACCESS_DENIED;
#endif
        }
    } else {
        response_mode = UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION;
    } /* end if/else comprovacio sessio */

    Iso15765_3SendResponse(response_mode);
} /* UdsRequestDownload */
#endif

/*****************************************************************************
| Portability:  DZ60
|----------------------------------------------------------------------------
| Functionalities Contract Description:
|  * Rutina d'atencio al servei Transfer Data
|  * Aquest servei s'utilitza per transferencia de dades.
|  * Es pot enviar resposta negativa UDS_ERR_RESPONSE_PENDING per mantenir
|    el contingut del buffer de dades si s'ha configurat el parametre
|    NP_TX_RX_OFFSET amb valor major o igual a 3
|  * Les diferents rutines i funcions del protocolIso15765_3 que es poden
|    fer servir per implementar aquest servei son:
|    Iso15765_3QueryActiveSession() per consultar la sessio activa actual.
|    Iso15765_3QuerySecurityAccessStatus() per consultar el nivell de seguretat
|                                          actual.
|---------------------------------------------------------------------------
| Parameters Explanation:
|   block_seq: Numero de sequencia de TranferData desde el Up/DownLoad Request
|              começa per 0x01 i al arribar a 0xFF continua per 0x00
|   size: Mida del buffer de dades
|   data: Buffer de dades
/---------------------------------------------------------------------------*/
#ifdef UDS_SERVICE_TRANSFER_DATA

static void UdsTransferData(UI_8 block_seq, UI_16 size, UI_8 *data_buf) {
    UI_8 response_mode;
    tp_uds_transfer_data resp;
    t_iso3_addr transfer_end_addr;

    resp = ISO15765_3_GET_RESP_DATA(tp_uds_transfer_data);
    response_mode = ISO15765_3_POSITIVE_RESPONSE;

    /* Programming session */
    if (Iso15765_3QueryActiveSession() != UDS_DEFAULT_SESSION) {

        /* Comprovem que  estiguem en proces de download */
        if (download_control.download_status == DOWNLOAD_STATUS_ONGOING) {

            /* We assume here. We are always doing downloads: PC -> ECU */
            /* Get the Response frame ready */
            resp->block_seq = block_seq;
            Iso15765_3IncrementResponseSize(1);

            /* Comprovacio de la mida del bloc */
            transfer_end_addr = download_control.dest_addr +
                    (t_iso3_addr) size - (t_iso3_addr) 1;
            if ((size <= MAX_BLOCK_LENGTH) &&
                    (transfer_end_addr <= download_control.last_addr)) {

                /* Comprovacio de block sequence */
                if (download_control.block_seq == block_seq) {
                    /* Keep the size of the data buffer being processed */
                    download_control.block_size = size;
                    download_control.block_ptr = (t_iso3_addr) (* data_buf);

                    /* Callback to the correct device */
                    switch (download_control.device_addressed) {
#ifdef DEVICE_0_TRANSFER_DATA_CALLBACK
                        case DEVICE_0:
                            response_mode = DEVICE_0_TRANSFER_DATA_CALLBACK(data_buf, size);
                            break;
#endif
#ifdef DEVICE_1_TRANSFER_DATA_CALLBACK
                        case DEVICE_1:
                            response_mode = DEVICE_1_TRANSFER_DATA_CALLBACK(data_buf, size);
                            break;
#endif
#ifdef DEVICE_2_TRANSFER_DATA_CALLBACK
                        case DEVICE_2:
                            response_mode = DEVICE_2_TRANSFER_DATA_CALLBACK(data_buf, size);
                            break;
#endif
#ifdef DEVICE_3_TRANSFER_DATA_CALLBACK
                        case DEVICE_3:
                            response_mode = DEVICE_3_TRANSFER_DATA_CALLBACK(data_buf, size);
                            break;
#endif
#ifdef DEVICE_4_TRANSFER_DATA_CALLBACK
                        case DEVICE_4:
                            response_mode = DEVICE_4_TRANSFER_DATA_CALLBACK(data_buf, size);
                            break;
#endif
#ifdef DEVICE_5_TRANSFER_DATA_CALLBACK
                        case DEVICE_5:
                            response_mode = DEVICE_5_TRANSFER_DATA_CALLBACK(data_buf, size);
                            break;
#endif
                        default:
                            response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT;
                            break;
                    }
                    /* Check if the transfer data wants to abort download */
                    if (response_mode == UDS_ERR_TRANSFER_SUSPENDED) {
                        download_control.download_status = DOWNLOAD_STATUS_FINISHED_NOK;
                    } else {
                        /* N/A */
                    }
                } else {
#ifdef UDS_TRANSFER_DATA_ERROR_CALLBACK
                    response_mode = UDS_TRANSFER_DATA_ERROR_CALLBACK(UDS_TRANSFER_DATA_ERROR_INVALID_BLOCK);
                    if (response_mode == 0xFF) {
                        response_mode = UDS_ERR_WRONG_BLOCK_SEQUENCE;
                    }
#else
                    response_mode = UDS_ERR_WRONG_BLOCK_SEQUENCE;
#endif

                } /* end if/else block_seq  */
            } else {
#ifdef UDS_TRANSFER_DATA_ERROR_CALLBACK
                response_mode = UDS_TRANSFER_DATA_ERROR_CALLBACK(UDS_TRANSFER_DATA_ERROR_OUT_OF_RANGE);
                if (response_mode == 0xFF) {
                    response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
                }
#else
                response_mode = UDS_ERR_REQUEST_OUT_OF_RANGE;
#endif
            } /* end if/else size check */
        } else {
            /* Download sequence error */
            response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT;
        } /* end if/else status check */
    } else {
        response_mode = UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION;
    } /* end if/else session check */

    Iso15765_3SendResponse(response_mode);

} /* UdsTransferData */
#endif


/*****************************************************************************
| Portability:  DZ60
|----------------------------------------------------------------------------
| Functionalities Contract Description:
|  * Rutina d'atencio al servei Request Transfer Exit
|  * Aquest servei s'utilitza per indicar el final d'una transferencia de
|    dades.
|  * Es pot enviar resposta negativa UDS_ERR_RESPONSE_PENDING per mantenir
|    el contingut del buffer de dades si s'ha configurat el parametre
|    NO_TX_RX_OFFSET amb valor major o igual a 2
|  * Les diferents rutines i funcions del modul protocolIso15765_3 que es
|    poden fer servir per implementar aquest servei son:
|    Iso15765_3QueryActiveSession() per consultar la sessio activa actual.
|    Iso15765_3QuerySecurityAccessStatus() per consultar el nivell de
|    seguretat actual.
|---------------------------------------------------------------------------
| Parameters Explanation:
|   size: Mida del buffer de dades
|   data_buf: Buffer de dades
/---------------------------------------------------------------------------*/
#ifdef UDS_SERVICE_REQUEST_TRANSFER_EXIT

static void UdsTransferExit(UI_16 size, UI_8 *data_buf) {
    UI_8 response_mode;

    /* By default set negative response */
    response_mode = ISO15765_3_POSITIVE_RESPONSE;

    /* Comprovem si estem en sessio de programacio */
    if (Iso15765_3QueryActiveSession() != UDS_DEFAULT_SESSION) {
        if (Iso15765_3QueryRequestMode() == ISO15765_3_PHY_REQUEST) {
            /* Comprovem que  estiguem en proces de download */
            if (download_control.download_status == DOWNLOAD_STATUS_ONGOING) {

                switch (download_control.device_addressed) {
#ifdef DEVICE_0_REQUEST_TRANSFER_EXIT_CALLBACK
                    case DEVICE_0:
                        response_mode = DEVICE_0_REQUEST_TRANSFER_EXIT_CALLBACK(
                                data_buf, size);
                        break;
#endif
#ifdef DEVICE_1_REQUEST_TRANSFER_EXIT_CALLBACK
                    case DEVICE_1:
                        response_mode = DEVICE_1_REQUEST_TRANSFER_EXIT_CALLBACK(
                                data_buf, size);
                        break;
#endif
#ifdef DEVICE_2_REQUEST_TRANSFER_EXIT_CALLBACK
                    case DEVICE_2:
                        response_mode = DEVICE_2_REQUEST_TRANSFER_EXIT_CALLBACK(
                                data_buf, size);
                        break;
#endif
#ifdef DEVICE_3_REQUEST_TRANSFER_EXIT_CALLBACK
                    case DEVICE_3:
                        response_mode = DEVICE_3_REQUEST_TRANSFER_EXIT_CALLBACK(
                                data_buf, size);
                        break;
#endif
#ifdef DEVICE_4_REQUEST_TRANSFER_EXIT_CALLBACK
                    case DEVICE_4:
                        response_mode = DEVICE_4_REQUEST_TRANSFER_EXIT_CALLBACK(
                                data_buf, size);
                        break;
#endif
#ifdef DEVICE_5_REQUEST_TRANSFER_EXIT_CALLBACK
                    case DEVICE_5:
                        response_mode = DEVICE_5_REQUEST_TRANSFER_EXIT_CALLBACK(
                                data_buf, size);
                        break;
#endif
                    default:
                        response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT;
                        break;
                }
                /* callback function examines whether the download process
                   has finished or not by looking at if
                   download_offset >= download_size                       */
                if (response_mode == ISO15765_3_POSITIVE_RESPONSE) {
                    download_control.download_status = DOWNLOAD_STATUS_FINISHED_OK;
                } else {
                    download_control.download_status = DOWNLOAD_STATUS_FINISHED_NOK;
                }
            } else {
                /* Download Sequence Error */
                response_mode = UDS_ERR_REQUEST_SEQUENCE_ERROR;
            } /* end if/else process status */
        } else {
            response_mode = UDS_ERR_CONDITIONS_NOT_CORRECT;
        }
    } else {
        response_mode = UDS_ERR_SERVICE_NOT_SUPORTED_IN_ACTIVE_SESSION;
    } /* end if/else sessio status */

    /* Respones Sending */
    Iso15765_3SendResponse(response_mode);
} /* UdsRequestTransferExit */
#endif


/************************************************
 Security failed attempts counter section 
 * This section implements the security failed attempts counter 
 * structured data type, including its actions and accessors.
 */

UI_8 security_failed_attempts = 0; /*!< The internal counter */

/**
 * Initialize the security attempts counter.
 * - When no NVM support, equals to zero.
 * - When NVM support, call the callback function to load the data in NVM.
 */
void Iso15765_3SecurityAttemptsInitialize(void) {
#ifdef UDS_SECURITY_COUNTER_NVM_LOAD_CALLBACK            
    security_failed_attempts = UDS_SECURITY_COUNTER_NVM_LOAD_CALLBACK();
    if (security_failed_attempts >= MAX_SECURITY_ATTEMPTS) {
        security_failed_attempts = MAX_SECURITY_ATTEMPTS;
    }
#else
    security_failed_attempts = (UI_8) 0;
#endif
}

/**
 * Clears the counter.
 * If NVM support, execute the callback to store the new value in NVM.
 */
void Iso15765_3SecurityAttemptsClear(void) {
    if (security_failed_attempts != (UI_8) 0) {
        security_failed_attempts = 0;
#ifdef UDS_SECURITY_COUNTER_NVM_CALLBACK            
       // UDS_SECURITY_COUNTER_NVM_CALLBACK(&security_failed_attempts);
#endif
    }
}

/**
 * Increments the counter.
 * If NVM support, execute the callback to store the new value in NVM.
 */
void Iso15765_3SecurityAttemptsIncrement(void) {
    if (security_failed_attempts < MAX_SECURITY_ATTEMPTS) {
        if ((security_failed_attempts + (UI_8) 1) >= MAX_SECURITY_ATTEMPTS) {
            security_failed_attempts = MAX_SECURITY_ATTEMPTS;
        } else {
            security_failed_attempts += (UI_8) 1;
        }
#ifdef UDS_SECURITY_COUNTER_NVM_CALLBACK            
       // UDS_SECURITY_COUNTER_NVM_CALLBACK(&security_failed_attempts);
#endif
    }
}

/**
 * Decrements the counter.
 * If NVM support, execute the callback to store the new value in NVM.
 */
void Iso15765_3SecurityAttemptsDecrement(void) {
    if (security_failed_attempts > (UI_8) 0) {
        if (security_failed_attempts <= (UI_8) 1) {
            security_failed_attempts = (UI_8) 0;
        } else {
            security_failed_attempts -= (UI_8) 1;
        }
#ifdef UDS_SECURITY_COUNTER_NVM_CALLBACK            
      //  UDS_SECURITY_COUNTER_NVM_CALLBACK(&security_failed_attempts);
#endif        
    }
}

/**
 * Checks if the maximum number of attempts has been reached.
 */
BOOL Iso15765_3SecurityAttemptsExceeded(void) {
    BOOL ret = FALSE;
    if (security_failed_attempts >= MAX_SECURITY_ATTEMPTS) {
        ret = TRUE;
    }
    return ret;
}

/**
 * Function to determine if the allowed attempts have been reached (about to be exceeded on next try)
 */
/*
BOOL Iso15765_3SecurityAttemptsReached(void) {
    BOOL ret = FALSE;
    if (security_failed_attempts >= (MAX_SECURITY_ATTEMPTS-(UI_8)1)) {
        ret = TRUE;
    }
    return ret;
}
*/
/**
 * Accessor for the counter value.
 * @return the current value of the counter.
 */
UI_8 Iso15765_3SecurityAttemptsGet(void) {
    UI_8 ret = 0xFF;
    ret = security_failed_attempts;
    return ret;
}
/**
 * Determines if the consecutive seed request must be considered as failed attempts, 
 * depending on the UDS variant.
 * @return TRUE if a consecutive seed request must increment the failed attempts counter.
 */
BOOL Iso15765_3VariantConsecutiveSeedIncrAttempt(void){
#ifdef ISO15765_3_GAC
    return TRUE;
#else
#ifdef ISO15765_3_GEELY
    return TRUE;
#else 
    return FALSE;
#endif
#endif    
}    
