AMiRo-OS

/************************************************************************************//**

* \file         Source\xcp.c

* \brief        XCP 1.0 protocol core source file.

* \ingroup      Core

* \internal

*----------------------------------------------------------------------------------------

*                          C O P Y R I G H T

*----------------------------------------------------------------------------------------

*   Copyright (c) 2011  by Feaser    http://www.feaser.com    All rights reserved

*

*----------------------------------------------------------------------------------------

*                            L I C E N S E

*----------------------------------------------------------------------------------------

* This file is part of OpenBLT. OpenBLT is free software: you can redistribute it and/or

* modify it under the terms of the GNU General Public License as published by the Free

* Software Foundation, either version 3 of the License, or (at your option) any later

* version.

*

* OpenBLT is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;

* without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR

* PURPOSE. See the GNU General Public License for more details.

*

* You should have received a copy of the GNU General Public License along with OpenBLT.

* If not, see <http://www.gnu.org/licenses/>.

*

* A special exception to the GPL is included to allow you to distribute a combined work

* that includes OpenBLT without being obliged to provide the source code for any

* proprietary components. The exception text is included at the bottom of the license

* file <license.html>.

*

* \endinternal

****************************************************************************************/

/****************************************************************************************

* Include files

****************************************************************************************/

#include "boot.h"                                /* bootloader generic header          */

#include <helper.h>

/****************************************************************************************

* Defines

****************************************************************************************/

/** \brief XCP protocol layer version number (16-bit). */

#define XCP_VERSION_PROTOCOL_LAYER  (0x0100)

/** \brief XCP transport layer version number (16-bit). */

#define XCP_VERSION_TRANSPORT_LAYER (0x0100)

/* XCP packet identifiers */

/** \brief Command response packet identifier. */

#define XCP_PID_RES                 (0xff)

/** \brief Error packet identifier. */

#define XCP_PID_ERR                 (0xfe)

/* XCP error codes */

/** \brief Cmd processor synchronization error code. */

#define XCP_ERR_CMD_SYNCH           (0x00)

/** \brief Command was not executed error code. */

#define XCP_ERR_CMD_BUSY            (0x10)

/** \brief Unknown or unsupported command error code. */

#define XCP_ERR_CMD_UNKNOWN         (0x20)

/** \brief Parameter out of range error code. */

#define XCP_ERR_OUT_OF_RANGE        (0x22)

/** \brief Protected error code. Seed/key required. */

#define XCP_ERR_ACCESS_LOCKED       (0x25)

/** \brief Cal page not valid error code. */

#define XCP_ERR_PAGE_NOT_VALID      (0x26)

/** \brief Sequence error code. */

#define XCP_ERR_SEQUENCE            (0x29)

/** \brief Generic error code. */

#define XCP_ERR_GENERIC             (0x31)

/* XCP command codes */

/** \brief CONNECT command code. */

#define XCP_CMD_CONNECT             (0xff)

/** \brief DISCONNECT command code. */

#define XCP_CMD_DISCONNECT          (0xfe)

/** \brief GET_STATUS command code. */

#define XCP_CMD_GET_STATUS          (0xfd)

/** \brief SYNCH command code. */

#define XCP_CMD_SYNCH               (0xfc)

/** \brief GET_ID command code. */

#define XCP_CMD_GET_ID              (0xfa)

/** \brief GET_SEED command code. */

#define XCP_CMD_GET_SEED            (0xf8)

/** \brief UNLOCK command code. */

#define XCP_CMD_UNLOCK              (0xf7)

/** \brief SET_MTA command code. */

#define XCP_CMD_SET_MTA             (0xf6)

/** \brief UPLOAD command code. */

#define XCP_CMD_UPLOAD              (0xf5)

/** \brief SHORT_UPLOAD command code. */

#define XCP_CMD_SHORT_UPLOAD        (0xf4)

/** \brief BUILD_CHECKSUM command code. */

#define XCP_CMD_BUILD_CHECKSUM      (0xf3)

/** \brief DOWNLOAD command code. */

#define XCP_CMD_DOWNLOAD            (0xf0)

/** \brief DOWNLOAD_MAX command code. */

#define XCP_CMD_DOWLOAD_MAX         (0xee)

/** \brief SET_CALPAGE command code. */

#define XCP_CMD_SET_CAL_PAGE        (0xeb)

/** \brief GET_CALPAGE command code. */

#define XCP_CMD_GET_CAL_PAGE        (0xea)

/** \brief PROGRAM_START command code. */

#define XCP_CMD_PROGRAM_START       (0xd2)

/** \brief PROGRAM_CLEAR command code. */

#define XCP_CMD_PROGRAM_CLEAR       (0xd1)

/** \brief PROGRAM command code. */

#define XCP_CMD_PROGRAM             (0xd0)

/** \brief PROGRAM_RESET command code. */

#define XCP_CMD_PROGRAM_RESET       (0xcf)

/** \brief PROGRAM_PREPARE command code. */

#define XCP_CMD_PROGRAM_PREPARE     (0xcc)

/** \brief PROGRAM_MAX command code. */

#define XCP_CMD_PROGRAM_MAX         (0xc9)

/****************************************************************************************

* Type definitions

****************************************************************************************/

/** \brief Struture type for grouping XCP internal module information. */

typedef struct

{

  blt_int8u  connected;                             /**< connection established                      */

#if (BOOT_GATE_ENABLE > 0)

  blt_int32u  other_connection;                      /**< connection to other device established      */

#endif

#if (BOOTLOADER_OF_MAIN_DEVICE > 0)

  blt_bool   wasMainConnection;                     /**< connection to serialboot (0x00) established */

#endif

  blt_int8u  oldData[BOOT_COM_RX_MAX_DATA];         /**< old packet data buffer                      */

  blt_int8u  protection;                            /**< protection state                            */

  blt_int8u  s_n_k_resource;                        /**< for seed/key sequence                       */

  blt_int8u  ctoData[BOOT_COM_RX_MAX_DATA];         /**< cto packet data buffer                      */

  blt_int8u  ctoPending;                            /**< cto transmission pending flag               */

  blt_int16s ctoLen;                                /**< cto current packet length                   */

  blt_int32u mta;                                   /**< memory transfer address                     */

} tXcpInfo;

/****************************************************************************************

* Function prototypes

****************************************************************************************/

/* transport layer specific functions */

#if (BOOT_GATE_ENABLE > 0)

static void      XcpTransmitPacket(blt_int8u *data, blt_int16s len, blt_bool fromGate);

#else

static void      XcpTransmitPacket(blt_int8u *data, blt_int16s len);

#endif /* BOOT_GATE_ENABLE > 0 */

/* application specific functions */

static blt_int8u XcpComputeChecksum(blt_int32u address, blt_int32u length,

                                    blt_int32u *checksum);

#if (XCP_SEED_KEY_PROTECTION_EN == 1)

static blt_int8u XcpGetSeed(blt_int8u resource, blt_int8u *seed);

static blt_int8u XcpVerifyKey(blt_int8u resource, blt_int8u *key, blt_int8u len);

#endif

/* general utility functions */

static void XcpProtectResources(void);

static void XcpSetCtoError(blt_int8u error);

/* XCP command processors */

static void XcpCmdConnect(blt_int8u *data, blt_int16s dataLength);

static void XcpCmdDisconnect(blt_int8u *data);

static void XcpCmdGetStatus(blt_int8u *data);

static void XcpCmdSynch(blt_int8u *data);

static void XcpCmdGetId(blt_int8u *data);

static void XcpCmdSetMta(blt_int8u *data);

static void XcpCmdUpload(blt_int8u *data);

static void XcpCmdShortUpload(blt_int8u *data);

static void XcpCmdBuildCheckSum(blt_int8u *data);

#if (XCP_SEED_KEY_PROTECTION_EN == 1)

static void XcpCmdGetSeed(blt_int8u *data);

static void XcpCmdUnlock(blt_int8u *data);

#endif

#if (XCP_RES_CALIBRATION_EN == 1)

static void XcpCmdDownload(blt_int8u *data);

static void XcpCmdDownloadMax(blt_int8u *data);

#endif

#if (XCP_RES_PAGING_EN == 1)

static void XcpCmdSetCalPage(blt_int8u *data);

static void XcpCmdGetCalPage(blt_int8u *data);

#endif

#if (XCP_RES_PROGRAMMING_EN == 1)

static void XcpCmdProgramMax(blt_int8u *data);

static void XcpCmdProgram(blt_int8u *data);

static void XcpCmdProgramStart(blt_int8u *data);

static void XcpCmdProgramClear(blt_int8u *data);

static void XcpCmdProgramReset(blt_int8u *data);

static void XcpCmdProgramPrepare(blt_int8u *data);

#endif

static void portedTransmission(blt_int8u *data);

/****************************************************************************************

* Hook functions

****************************************************************************************/

#if (XCP_RES_PAGING_EN == 1)

extern blt_int8u XcpCalSetPageHook(blt_int8u segment, blt_int8u page);

extern blt_int8u XcpCalGetPageHook(blt_int8u segment);

#endif

#if (XCP_CONNECT_MODE_HOOK_EN == 1)

extern blt_bool XcpConnectModeHook(blt_int8u mode);

#endif

#if (XCP_SEED_KEY_PROTECTION_EN == 1)

extern blt_int8u XcpGetSeedHook(blt_int8u resource, blt_int8u *seed);

extern blt_int8u XcpVerifyKeyHook(blt_int8u resource, blt_int8u *key, blt_int8u len);

#endif

/****************************************************************************************

* External functions

****************************************************************************************/

#if (BOOT_COM_ENABLE == 0)

/* in case no internally supported communication interface is used, a custom

 * communication module can be added. In order to use the XCP protocol in the custom

 * communication module, this hook function needs to be implemented. In the XCP protocol

 * is not needed, then simply remove the xcp.c source from the project.

 */

extern void XcpTransmitPacketHook(blt_int8u *data, blt_int16u len);

#endif

/****************************************************************************************

* Local constants

****************************************************************************************/

/** \brief String buffer with station id. */

static const blt_int8s xcpStationId[] = XCP_STATION_ID_STRING;

/****************************************************************************************

* Local data definitions

****************************************************************************************/

/** \brief Local variable for storing XCP internal module info. */

static tXcpInfo xcpInfo;

/************************************************************************************//**

** \brief     Initializes the XCP driver. Should be called once upon system startup.

** \return    none

**

****************************************************************************************/

void XcpInit(void)

{

  /* reset xcp module info */

  xcpInfo.connected = 0;

#if (BOOT_GATE_ENABLE > 0)

  xcpInfo.other_connection = 0;

#endif

#if (BOOTLOADER_OF_MAIN_DEVICE > 0)

  xcpInfo.wasMainConnection = BLT_FALSE;

#endif

  xcpInfo.mta = 0;

  xcpInfo.ctoPending = 0;

  xcpInfo.ctoLen = 0;

  xcpInfo.s_n_k_resource = 0;

  xcpInfo.protection = 0;

} /*** end of XcpInit ***/

/************************************************************************************//**

** \brief     Obtains information about the XCP connection state.

** \return    BLT_TRUE is an XCP connection is established, BLT_FALSE otherwise.

**

****************************************************************************************/

blt_bool XcpIsConnected(void)

{

#if (BOOT_GATE_ENABLE > 0)

  if (xcpInfo.connected == 0 && xcpInfo.other_connection == 0)

#else

  if (xcpInfo.connected == 0)

#endif

  {

    return BLT_FALSE;

  }

  return BLT_TRUE;

} /*** end of XcpIsConnected ***/

#if (BOOTLOADER_OF_MAIN_DEVICE > 0)

/************************************************************************************//**

** \brief     Obtains information about the XCP main connection state.

** \return    BLT_TRUE is an XCP connection to main was established, BLT_FALSE otherwise.

**

****************************************************************************************/

blt_bool XcpWasConnectedToMain(void)

{

  return xcpInfo.wasMainConnection;

} /*** end of XcpWasConnectedToMain ***/

#endif

/************************************************************************************//**

** \brief     Informs the core that a pending packet transmission was completed by

**            the transport layer.

** \return    none

**

****************************************************************************************/

void XcpPacketTransmitted(void)

{

  /* reset packet transmission pending flag */

  xcpInfo.ctoPending = 0;

} /*** end of XcpPacketTransmitted ***/

/************************************************************************************//**

** \brief     Informs the core that a new packet was received by the transport layer.

** \param     data       Pointer to byte buffer with packet data.

** \param     dataLength Number of data bytes that need to be transmitted.

** \return    none

**

****************************************************************************************/

#if (BOOT_GATE_ENABLE > 0)

void XcpPacketReceived(blt_int8u *data, blt_int16s dataLength, blt_bool fromGate)

#else

void XcpPacketReceived(blt_int8u *data, blt_int16s dataLength)

#endif /* BOOT_GATE_ENABLE > 0 */

{

  /* was this a connect command? */

  if (data[0] == XCP_CMD_CONNECT)

  {

    /* process the connect command */

    XcpCmdConnect(data, dataLength);

  }

  /* only continue if connected */

  else if (xcpInfo.connected == 1)

  {

    switch (data[0])

    {

      case XCP_CMD_UPLOAD:

        XcpCmdUpload(data);

        break;

      case XCP_CMD_SHORT_UPLOAD:

        XcpCmdShortUpload(data);

        break;

      case XCP_CMD_SET_MTA:

        XcpCmdSetMta(data);

        break;

      case XCP_CMD_BUILD_CHECKSUM:

        XcpCmdBuildCheckSum(data);

        break;

      case XCP_CMD_GET_ID:

        XcpCmdGetId(data);

        break;

      case XCP_CMD_SYNCH:

        XcpCmdSynch(data);

        break;

      case XCP_CMD_GET_STATUS:

        XcpCmdGetStatus(data);

        break;

      case XCP_CMD_DISCONNECT:

        XcpCmdDisconnect(data);

        break;

#if (XCP_RES_CALIBRATION_EN == 1)

      case XCP_CMD_DOWNLOAD:

        XcpCmdDownload(data);

        break;

      case XCP_CMD_DOWLOAD_MAX:

        XcpCmdDownloadMax(data);

        break;

#endif

#if (XCP_RES_PROGRAMMING_EN == 1)

      case XCP_CMD_PROGRAM_MAX:

        XcpCmdProgramMax(data);

        break;

      case XCP_CMD_PROGRAM:

        XcpCmdProgram(data);

        break;

      case XCP_CMD_PROGRAM_START:

        XcpCmdProgramStart(data);

        break;

      case XCP_CMD_PROGRAM_CLEAR:

        XcpCmdProgramClear(data);

        break;

      case XCP_CMD_PROGRAM_RESET:

        XcpCmdProgramReset(data);

        break;

      case XCP_CMD_PROGRAM_PREPARE:

        XcpCmdProgramPrepare(data);

        break;

#endif

#if (XCP_SEED_KEY_PROTECTION_EN == 1)

      case XCP_CMD_GET_SEED:

        XcpCmdGetSeed(data);

        break;

      case XCP_CMD_UNLOCK:

        XcpCmdUnlock(data);

        break;

#endif

#if (XCP_RES_PAGING_EN == 1)

      case XCP_CMD_SET_CAL_PAGE:

        XcpCmdSetCalPage(data);

        break;

      case XCP_CMD_GET_CAL_PAGE:

        XcpCmdGetCalPage(data);

        break;

#endif

      default:

        XcpSetCtoError(XCP_ERR_CMD_UNKNOWN);

        break;

    }

  }

#if (BOOT_GATE_ENABLE > 0)

  /* check if there is still an other connection */

  else if (xcpInfo.other_connection > 0) {

    /* Forwarding message */

    XcpPacketReceivedForwarding(data, dataLength);

    return;

  }

#endif /* (BOOT_GATE_ENABLE > 0) */

  else

  {

    /* return to make sure response packet is not send because we are not connected */

    return;

  }

  /* make sure the previous command was completed */

  if (xcpInfo.ctoPending == 1)

  {

    /* command overrun occurred */

    XcpSetCtoError(XCP_ERR_CMD_BUSY);

  }

  /* send the response if it contains something */

  if (xcpInfo.ctoLen > 0)

  {

    /* set cto packet transmission pending flag */

    xcpInfo.ctoPending = 1;

    /* transmit the cto response packet */

#if (BOOT_GATE_ENABLE > 0)

    XcpTransmitPacket(xcpInfo.ctoData, xcpInfo.ctoLen, fromGate);

#else

    XcpTransmitPacket(xcpInfo.ctoData, xcpInfo.ctoLen);

#endif /* BOOT_GATE_ENABLE > 0 */

  }

} /*** end of XcpPacketReceived ***/

#if (BOOT_GATE_ENABLE > 0)

/************************************************************************************//**

** \brief     Forwards the packet to other device.

** \param     data       Pointer to the byte buffer with packet data.

** \param     dataLength Number of data bytes that need to be transmitted.

** \return    none

**

****************************************************************************************/

void XcpPacketReceivedForwarding(blt_int8u *data, blt_int16s dataLength) {

  /* save id of connected device */

  blt_int32u connectionTo = xcpInfo.other_connection;

  /* proof commands */

  switch (data[0]) {

    /* if there is a disconnection command, everything else in xcp info

     * has to be set on disconnection

     */

    case XCP_CMD_DISCONNECT:

      xcpInfo.other_connection = 0;

      xcpInfo.connected = 0;

      setLed(0);

      break;

    /* if there is a reset command, it has to be handled like a disconnection

     * (above) because other devices shouldn't get the ability to reset

     * themselves alone without any general reset command to all devices

     */

    case XCP_CMD_PROGRAM_RESET:

      xcpInfo.other_connection = 0;

      xcpInfo.connected = 0;

      setLed(0);

      data[0] = XCP_CMD_DISCONNECT;

      break;

    default:

      break;

  }

  xcpInfo.ctoPending = 1;

#if (BOOT_DEBUGGING_UART2_ENABLE > 0)

  /* send debugging information */

  BuildData(&debugDataTCan, data, (blt_int8u)dataLength);

#endif

  /* send packet direct with CAN */

  GateTransmitPacketDirect(data, (blt_int8u)dataLength, connectionTo);

} /*** end of XcpPacketReceivedForwarding ***/

/************************************************************************************//**

** \brief     Transmits the packet from gateway to com.

** \param     data       Pointer to the byte buffer with packet data.

** \param     dataLength Number of data bytes that need to be transmitted.

** \return    none

**

****************************************************************************************/

void XcpGatewayPacketReceived(blt_int8u *data, blt_int16s dataLength) {

  xcpInfo.ctoPending = 1;

#if (BOOT_DEBUGGING_UART2_ENABLE > 0)

  /* send debugging information */

  BuildData(&debugDataRCan, data, (blt_int8u)dataLength);

#endif

  /* send packet direct with UART */

  ComTransmitPacketDirect(data, (blt_int8u)dataLength);

} /*** end of XcpGatewayPacketReceived ***/

#endif /* (BOOT_GATE_ENABLE > 0) */

/************************************************************************************//**

** \brief     Transmits the packet using the xcp transport layer.

** \param     data Pointer to the byte buffer with packet data.

** \param     len  Number of data bytes that need to be transmitted.

** \return    none

**

****************************************************************************************/

#if (BOOT_GATE_ENABLE > 0)

static void XcpTransmitPacket(blt_int8u *data, blt_int16s len, blt_bool fromGate)

#else

static void XcpTransmitPacket(blt_int8u *data, blt_int16s len)

#endif /* BOOT_GATE_ENABLE > 0 */

{

  /* submit packet to the communication interface for transmission */

#if (BOOT_GATE_ENABLE > 0)

  if (fromGate == BLT_TRUE) {

    GateTransmitPacket(data, len);

    return;

  }

#endif /* BOOT_GATE_ENABLE > 0 */

#if (BOOT_COM_ENABLE == 0)

  XcpTransmitPacketHook(data, len);

#else

  ComTransmitPacket(data, len);

#endif

} /*** end of XcpTransmitPacket ***/

/************************************************************************************//**

** \brief     Called by the BUILD_CHECKSUM command to perform a checksum calculation

**            over the specified memory region.

** \param     address   The start address of the memory region.

** \param     length    Length of the memory region in bytes.

** \param     checksum  Pointer to where the calculated checksum is to be stored.

** \return    Checksum type that was used during the checksum calculation.

**

****************************************************************************************/

static blt_int8u XcpComputeChecksum(blt_int32u address, blt_int32u length,

                                    blt_int32u *checksum)

{

  blt_int8u cs = 0;

  /* this example computes the checksum using the add byte to byte algorithm */

  while (length-- > 0)

  {

    cs += *((blt_int8u*)(blt_addr)address);

    address++;

  }

  *checksum = cs;

  return XCP_CS_ADD11;

} /*** end of XcpComputeChecksum ***/

#if (XCP_SEED_KEY_PROTECTION_EN == 1)

/************************************************************************************//**

** \brief     Provides a seed to the XCP master that will be used for the key

**            generation when the master attempts to unlock the specified resource.

**            Called by the GET_SEED command.

** \param     resource  Resource that the seed if requested for (XCP_RES_XXX).

** \param     seed      Pointer to byte buffer wher the seed will be stored.

** \return    Length of the seed in bytes.

**

****************************************************************************************/

static blt_int8u XcpGetSeed(blt_int8u resource, blt_int8u *seed)

{

  /* pass request on to the application through a hook function */

  return XcpGetSeedHook(resource, seed);

} /*** end of XcpGetSeed ***/

/************************************************************************************//**

** \brief     Called by the UNLOCK command and checks if the key to unlock the

**            specified resource was correct. If so, then the resource protection

**            will be removed.

** \param     resource  resource to unlock (XCP_RES_XXX).

** \param     key       pointer to the byte buffer holding the key.

** \param     len       length of the key in bytes.

** \return    1 if the key was correct, 0 otherwise.

**

****************************************************************************************/

static blt_int8u XcpVerifyKey(blt_int8u resource, blt_int8u *key, blt_int8u len)

{

  /* pass request on to the application through a hook function */

  return XcpVerifyKeyHook(resource, key, len);

} /*** end of XcpVerifyKey ***/

#endif /* XCP_SEED_KEY_PROTECTION_EN == 1 */

/************************************************************************************//**

** \brief     Utility function to protects all the available resources.

** \return    none

**

****************************************************************************************/

static void XcpProtectResources(void)

{

  xcpInfo.protection = 0;

#if (XCP_SEED_KEY_PROTECTION_EN == 1)

  #if (XCP_RES_CALIBRATION_EN == 1)

  xcpInfo.protection |= XCP_RES_CALPAG;

  #endif

  #if (XCP_RES_PAGING_EN == 1)

  xcpInfo.protection |= XCP_RES_CALPAG;

  #endif

  #if (XCP_RES_PROGRAMMING_EN == 1)

  xcpInfo.protection |= XCP_RES_PGM;

  #endif

  #if (XCP_RES_DATA_ACQUISITION_EN == 1)

  xcpInfo.protection |= XCP_RES_DAQ;

  #endif

  #if (XCP_RES_DATA_STIMULATION_EN == 1)

  xcpInfo.protection |= XCP_RES_STIM;

  #endif

#endif /* XCP_SEED_KEY_PROTECTION_EN == 1 */

} /*** end of XcpProtectResources ***/

/************************************************************************************//**

** \brief     Prepares the cto packet data for the specified error.

** \param     error XCP error code (XCP_ERR_XXX).

** \return    none

**

****************************************************************************************/

static void XcpSetCtoError(blt_int8u error)

{

  /* prepare the error packet */

  xcpInfo.ctoData[0] = XCP_PID_ERR;

  xcpInfo.ctoData[1] = error;

  xcpInfo.ctoLen = 2;

} /*** end of XcpSetCtoError ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the CONNECT command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdConnect(blt_int8u *data, blt_int16s dataLength)

{

  /* suppress compiler warning for unused parameter */

  data = data;

  #if (BOOT_FILE_SYS_ENABLE > 0)

  /* reject the connection if the file module is not idle. this means that a firmware

   * update from the locally attached storage is in progress

   */

  if (FileIsIdle() == BLT_FALSE)

  {

    /* command not processed because we are busy */

    XcpSetCtoError(XCP_ERR_CMD_BUSY);

    return;

  }

  #endif

//  #if (XCP_CONNECT_MODE_HOOK_EN == 1)

//  /* pass on the mode to a application specific hook function. This function can determine

//   * is the mode is supported or not. A return value of BLT_FALSE causes the CONNECT command

//   * to be ignored. Note that this mode could potentially be used to specify a node ID in a

//   * multi XCP slave system.

//   */

//  if (XcpConnectModeHook(data[1]) == BLT_FALSE)

//  {

//    /* set the response length to 0 to suppress it */

//    xcpInfo.ctoLen = 0;

//    return;

//  }

//  #endif

  /* enable resource protection */

  XcpProtectResources();

  /* extract the device ID.

   * NOTE: For legacy support the following code depends on the data length.

   *       If two bytes were received the system switches to legacy mode.

   *       In legacy mode, the device ID was only one byte.

   */

  blt_int32u deviceID;

  if (dataLength == 2) {

    deviceID = ((blt_int32u)0 << 24) | \

               ((blt_int32u)0 << 16) | \

               ((blt_int32u)0 <<  8) | \

               ((blt_int32u)data[1]);

  } else if (dataLength == 5) {

    deviceID = ((blt_int32u)data[4] << 24) | \

               ((blt_int32u)data[3] << 16) | \

               ((blt_int32u)data[2] <<  8) | \

               ((blt_int32u)data[1]);

  } else {

    blinkSOSinf();

  }

  /* indicate that the connection is established */

#if (BOOT_GATE_ENABLE > 0)

  if (deviceID == 0 ||

      deviceID == (blt_int32u)BOOT_COM_DEVICE_ID ||

      deviceID == (blt_int32u)BOOT_COM_DEVICE_LEGACY_ID) {

#endif

    xcpInfo.connected = 1;

    setLed(1);

#if (BOOT_GATE_ENABLE > 0)

    xcpInfo.other_connection = 0;

#endif

#if (BOOTLOADER_OF_MAIN_DEVICE > 0)

    if (deviceID == 0x00) {

      xcpInfo.wasMainConnection = BLT_TRUE;

    }

#endif

    /* set packet id to command response packet */

    xcpInfo.ctoData[0] = XCP_PID_RES;

    /* report available resources */

    xcpInfo.ctoData[1] = 0;

#if (XCP_RES_CALIBRATION_EN == 1)

    xcpInfo.ctoData[1] |= XCP_RES_CALPAG;

#endif

#if (XCP_RES_PAGING_EN == 1)

    xcpInfo.ctoData[1] |= XCP_RES_CALPAG;

#endif

#if (XCP_RES_PROGRAMMING_EN == 1)

    xcpInfo.ctoData[1] |= XCP_RES_PGM;

#endif

#if (XCP_RES_DATA_ACQUISITION_EN == 1)

    xcpInfo.ctoData[1] |= XCP_RES_DAQ;

#endif

#if (XCP_RES_DATA_STIMULATION_EN == 1)

    xcpInfo.ctoData[1] |= XCP_RES_STIM;

#endif

    /* report communication mode info. only byte granularity is supported */

    xcpInfo.ctoData[2] = 0;

    /* configure for motorola or intel byte ordering */

    xcpInfo.ctoData[2] |= XCP_MOTOROLA_FORMAT;

    /* report max cto data length */

    xcpInfo.ctoData[3] = (blt_int8u)XCP_CTO_PACKET_LEN;

    /* report max dto data length */

#if (XCP_MOTOROLA_FORMAT == 0)

    xcpInfo.ctoData[4] = (blt_int8u)XCP_DTO_PACKET_LEN;

    xcpInfo.ctoData[5] = (blt_int8u)(XCP_DTO_PACKET_LEN >> 8);

#else

    xcpInfo.ctoData[4] = (blt_int8u)(XCP_DTO_PACKET_LEN >> 8);

    xcpInfo.ctoData[5] = (blt_int8u)XCP_DTO_PACKET_LEN;

#endif

    /* report msb of protocol layer version number */

    xcpInfo.ctoData[6] = XCP_VERSION_PROTOCOL_LAYER >> 8;

    /* report msb of transport layer version number */

    xcpInfo.ctoData[7] = XCP_VERSION_TRANSPORT_LAYER >> 8;

    /* set packet length */

    xcpInfo.ctoLen = 8;

#if (BOOT_GATE_ENABLE > 0)

  } else {

    xcpInfo.other_connection = deviceID;

    setLed(1);

    xcpInfo.connected = 0;

    XcpPacketReceivedForwarding(data, dataLength);

    xcpInfo.ctoLen = 0;

    // TODO better cancel definition!

//    if (xcpInfo.ctoLen == 0) {

//      xcpInfo.other_connection = 0;

//    }

  }

#endif

} /*** end of XcpCmdConnect ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the DISCONNECT command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdDisconnect(blt_int8u *data)

{

  /* suppress compiler warning for unused parameter */

  data = data;

  /* indicate that the xcp connection is disconnected */

  xcpInfo.connected = 0;

#if (BOOT_GATE_ENABLE > 0)

  xcpInfo.other_connection = 0;

#endif

  setLed(0);

  /* enable resource protection */

  XcpProtectResources();

  /* set packet id to command response packet */

  xcpInfo.ctoData[0] = XCP_PID_RES;

  /* set packet length */

  xcpInfo.ctoLen = 1;

} /*** end of XcpCmdDisconnect ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the GET_STATUS command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdGetStatus(blt_int8u *data)

{

  /* suppress compiler warning for unused parameter */

  data = data;

  /* set packet id to command response packet */

  xcpInfo.ctoData[0] = XCP_PID_RES;

  /* report session status */

  xcpInfo.ctoData[1] = 0;

  /* report current resource protection status */

  xcpInfo.ctoData[2] = xcpInfo.protection;

  /* reset reserved and session configuration id values */

  xcpInfo.ctoData[3] = 0;

  xcpInfo.ctoData[4] = 0;

  xcpInfo.ctoData[5] = 0;

  /* set packet length */

  xcpInfo.ctoLen = 6;

} /*** end of XcpCmdGetStatus ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the SYNCH command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdSynch(blt_int8u *data)

{

  /* suppress compiler warning for unused parameter */

  data = data;

  /* synch requires a negative response */

  XcpSetCtoError(XCP_ERR_CMD_SYNCH);

} /*** end of XcpCmdSynch ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the GET_ID command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdGetId(blt_int8u *data)

{

  /* suppress compiler warning for unused parameter */

  data = data;

  /* set packet id to command response packet */

  xcpInfo.ctoData[0] = XCP_PID_RES;

  /* point mta to start of station id string */

  xcpInfo.mta = (blt_int32u)&xcpStationId[0];

  /* set station id mode to 0 */

  xcpInfo.ctoData[1] = 0;

  /* reset reserved values */

  xcpInfo.ctoData[2] = 0;

  xcpInfo.ctoData[3] = 0;

  /* store station id length (excl. null termination) for response packet */

  *(blt_int32u*)&xcpInfo.ctoData[4] = (sizeof(xcpStationId)/sizeof(xcpStationId[0])) - 1;

  /* set packet length */

  xcpInfo.ctoLen = 8;

} /*** end of XcpCmdGetId ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the SET_MTA command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdSetMta(blt_int8u *data)

{

  /* set packet id to command response packet */

  xcpInfo.ctoData[0] = XCP_PID_RES;

  /* update mta. current implementation ignores address extension */

  xcpInfo.mta = *(blt_int32u*)&data[4];

  /* set packet length */

  xcpInfo.ctoLen = 1;

} /*** end of XcpCmdSetMta ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the UPLOAD command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdUpload(blt_int8u *data)

{

  /* validate length of upload request */

  if (data[1] > (XCP_CTO_PACKET_LEN-1))

  {

    /* requested data length is too long */

    XcpSetCtoError(XCP_ERR_OUT_OF_RANGE);

    return;

  }

  /* copy the data from memory to the data packet */

  CpuMemCopy(((blt_addr)(blt_int32u)&xcpInfo.ctoData[1]),(blt_addr)xcpInfo.mta, data[1]);

  /* set packet id to command response packet */

  xcpInfo.ctoData[0] = XCP_PID_RES;

  /* post increment the mta */

  xcpInfo.mta += data[1];

  /* set packet length */

  xcpInfo.ctoLen = data[1]+1;

} /*** end of XcpCmdUpload ***/

/************************************************************************************//**

** \brief     XCP command processor function which handles the SHORT_UPLOAD command as

**            defined by the protocol.

** \param     data Pointer to a byte buffer with the packet data.

** \return    none

**

****************************************************************************************/

static void XcpCmdShortUpload(blt_int8u *data)

{

  /* validate length of upload request */

  if (data[1] > (XCP_CTO_PACKET_LEN-1))

  {

    /* requested data length is too long */

    XcpSetCtoError(XCP_ERR_OUT_OF_RANGE);

    return;

  }

  /* update mta. current implementation ignores address extension */

  xcpInfo.mta = *(blt_int32u*)&data[4];

  /* copy the data from memory to the data packet */

  CpuMemCopy((blt_addr)((blt_int32u)&xcpInfo.ctoData[1]),(blt_addr)xcpInfo.mta, data[1]);

  /* set packet id to command response packet */

  xcpInfo.ctoData[0] = XCP_PID_RES;

  /* post increment the mta */