OpENer/source/src/cip/cipconnectionmanager.c

/*******************************************************************************
 * Copyright (c) 2009, Rockwell Automation, Inc.
 * All rights reserved.
 *
 ******************************************************************************/
#include <string.h>
#include <stdbool.h>

#include "cipconnectionmanager.h"

#include "opener_user_conf.h"
#include "cipcommon.h"
#include "cipmessagerouter.h"
#include "ciperror.h"
#include "endianconv.h"
#include "opener_api.h"
#include "encap.h"
#include "cipidentity.h"
#include "trace.h"
#include "cipclass3connection.h"
#include "cipioconnection.h"
#include "cipassembly.h"
#include "cpf.h"
#include "appcontype.h"
#include "encap.h"
#include "generic_networkhandler.h"
#include "cipepath.h"
#include "cipelectronickey.h"
#include "cipqos.h"

/* values needed from the CIP identity object */
extern EipUint16 vendor_id_;
extern EipUint16 device_type_;
extern EipUint16 product_code_;
extern CipRevision revision_;

#define CIP_CONN_TYPE_MASK 0x6000   /**< Bit mask filter on bit 13 & 14 */

const size_t g_kForwardOpenHeaderLength = 36; /**< the length in bytes of the forward open command specific data till the start of the connection path (including con path size)*/

/** @brief Compares the logical path on equality */
#define EQLOGICALPATH(x,y) ( ( (x) & 0xfc )==(y) )

static const int g_kNumberOfConnectableObjects = 2 +
                                                 OPENER_CIP_NUM_APPLICATION_SPECIFIC_CONNECTABLE_OBJECTS;

extern DoublyLinkedList connection_list;

typedef struct {
  EipUint32 class_id;
  OpenConnectionFunction open_connection_function;
} ConnectionManagementHandling;

/* global variables private */
/** List holding information on the object classes and open/close function
 * pointers to which connections may be established.
 */
ConnectionManagementHandling g_connection_management_list[2 +
                                                          OPENER_CIP_NUM_APPLICATION_SPECIFIC_CONNECTABLE_OBJECTS
];

/** buffer connection object needed for forward open */
CipConnectionObject g_dummy_connection_object;

/** @brief Holds the connection ID's "incarnation ID" in the upper 16 bits */
EipUint32 g_incarnation_id;

/* private functions */
EipStatus ForwardOpen(
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  struct sockaddr *originator_address);

EipStatus ForwardClose(
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  struct sockaddr *originator_address);

EipStatus GetConnectionOwner(
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  struct sockaddr *originator_address);

EipStatus AssembleForwardOpenResponse(
  CipConnectionObject *connection_object,
  CipMessageRouterResponse *message_router_response,
  EipUint8 general_status,
  EipUint16 extended_status);

EipStatus AssembleForwardCloseResponse(
  EipUint16 connection_serial_number,
  EipUint16 originatior_vendor_id,
  EipUint32 originator_serial_number,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  EipUint16 extended_error_code);

/** @brief check if the data given in the connection object match with an already established connection
 *
 * The comparison is done according to the definitions in the CIP specification Section 3-5.5.2:
 * The following elements have to be equal: Vendor ID, Connection Serial Number, Originator Serial Number
 * @param connection_object connection object containing the comparison elements from the forward open request
 * @return
 *    - NULL if no equal established connection exists
 *    - pointer to the equal connection object
 */
CipConnectionObject *CheckForExistingConnection(
  const CipConnectionObject *const connection_object);

/** @brief Compare the electronic key received with a forward open request with the device's data.
 *
 * @param key_format format identifier given in the forward open request
 * @param key_data pointer to the electronic key data received in the forward open request
 * @param extended_status the extended error code in case an error happened
 * @return general status on the establishment
 *    - EIP_OK ... on success
 *    - On an error the general status code to be put into the response
 */
EipStatus CheckElectronicKeyData(
  EipUint8 key_format,
  void *key_data,
  EipUint16 *extended_status);

/** @brief Parse the connection path of a forward open request
 *
 * This function will take the connection object and the received data stream and parse the connection path.
 * @param connection_object pointer to the connection object structure for which the connection should
 *                      be established
 * @param message_router_request pointer to the received request structure. The position of the data stream pointer has to be at the connection length entry
 * @param extended_status the extended error code in case an error happened
 * @return general status on the establishment
 *    - EIP_OK ... on success
 *    - On an error the general status code to be put into the response
 */
EipUint8 ParseConnectionPath(
  CipConnectionObject *connection_object,
  CipMessageRouterRequest *message_router_request,
  EipUint16 *extended_error);

ConnectionManagementHandling *GetConnectionManagementEntry(const EipUint32 class_id);

void InitializeConnectionManagerData(void);

void AddNullAddressItem(
  CipCommonPacketFormatData *common_data_packet_format_data);

/** @brief gets the padded logical path TODO: enhance documentation
 * @param logical_path_segment TheLogical Path Segment
 *
 * @return The padded logical path
 */
unsigned int GetPaddedLogicalPath(const EipUint8 **logical_path_segment) {
  unsigned int padded_logical_path = *(*logical_path_segment)++;

  if ( (padded_logical_path & 3) == 0 ) {
    padded_logical_path = *(*logical_path_segment)++;
  } else if ( (padded_logical_path & 3) == 1 ) {
    (*logical_path_segment)++; /* skip pad */
    padded_logical_path = *(*logical_path_segment)++;
    padded_logical_path |= *(*logical_path_segment)++ << 8;
  } else {
    OPENER_TRACE_ERR("illegal logical path segment\n");
  }
  return padded_logical_path;
}

/** @brief Generate a new connection Id utilizing the Incarnation Id as
 * described in the EIP specs.
 *
 * A unique connectionID is formed from the boot-time-specified "incarnation ID"
 * and the per-new-connection-incremented connection number/counter.
 * @return new connection id
 */
CipUint GetConnectionId(void) {
  static CipUint connection_id = 18;
  connection_id++;
  return ( g_incarnation_id | (connection_id & 0x0000FFFF) );
}

void InitializeConnectionManager(CipClass *class) {

  CipClass *meta_class = class->class_instance.cip_class;

  InsertAttribute( (CipInstance *) class, 1, kCipUint,
                   (void *) &class->revision,
                   kGetableSingleAndAll ); /* revision */
  InsertAttribute( (CipInstance *) class, 2, kCipUint,
                   (void *) &class->number_of_instances, kGetableSingleAndAll ); /*  largest instance number */
  InsertAttribute( (CipInstance *) class, 3, kCipUint,
                   (void *) &class->number_of_instances, kGetableSingle ); /* number of instances currently existing*/
  InsertAttribute( (CipInstance *) class, 4, kCipUint, (void *) &kCipUintZero,
                   kNotSetOrGetable ); /* optional attribute list - default = 0 */
  InsertAttribute( (CipInstance *) class, 5, kCipUint, (void *) &kCipUintZero,
                   kNotSetOrGetable ); /* optional service list - default = 0 */
  InsertAttribute( (CipInstance *) class, 6, kCipUint,
                   (void *) &meta_class->highest_attribute_number,
                   kGetableSingleAndAll ); /* max class attribute number*/
  InsertAttribute( (CipInstance *) class, 7, kCipUint,
                   (void *) &class->highest_attribute_number,
                   kGetableSingleAndAll ); /* max instance attribute number*/

}

EipStatus ConnectionManagerInit(EipUint16 unique_connection_id) {
  InitializeConnectionManagerData();

  CipClass *connection_manager = CreateCipClass(
    g_kCipConnectionManagerClassCode,   /* class ID */
    0,   /* # of class attributes */
    7,   /* # highest class attribute number*/
    2,   /* # of class services */
    0,   /* # of instance attributes */
    14,   /* # highest instance attribute number*/
    5,   /* # of instance services */
    1,   /* # of instances */
    "connection manager",   /* class name */
    1,   /* revision */
    &InitializeConnectionManager);   /* # function pointer for initialization*/
  if (connection_manager == NULL) {
    return kEipStatusError;
  }
  InsertService(connection_manager, kGetAttributeSingle, &GetAttributeSingle,
                "GetAttributeSingle");
  InsertService(connection_manager, kGetAttributeAll, &GetAttributeAll,
                "GetAttributeAll");
  InsertService(connection_manager, kForwardOpen, &ForwardOpen, "ForwardOpen");
  InsertService(connection_manager, kForwardClose, &ForwardClose,
                "ForwardClose");
  InsertService(connection_manager, kGetConnectionOwner, &GetConnectionOwner,
                "GetConnectionOwner");

  g_incarnation_id = ( (EipUint32) unique_connection_id ) << 16;

  AddConnectableObject(kCipMessageRouterClassCode, EstablishClass3Connection);
  AddConnectableObject(kCipAssemblyClassCode, EstablishIoConnection);

  return kEipStatusOk;
}

EipStatus HandleReceivedConnectedData(
  EipUint8 *data,
  int data_length,
  struct sockaddr_in *from_address
  ) {

  if ( ( CreateCommonPacketFormatStructure(data, data_length,
                                           &g_common_packet_format_data_item) )
       == kEipStatusError ) {
    return kEipStatusError;
  } else {
    /* check if connected address item or sequenced address item received, otherwise it is no connected message and should not be here */
    if ( (g_common_packet_format_data_item.address_item.type_id
          == kCipItemIdConnectionAddress)
         || (g_common_packet_format_data_item.address_item.type_id
             == kCipItemIdSequencedAddressItem) ) { /* found connected address item or found sequenced address item -> for now the sequence number will be ignored */
      if (g_common_packet_format_data_item.data_item.type_id
          == kCipItemIdConnectedDataItem) { /* connected data item received */

        CipConnectionObject *connection_object = GetConnectedObject(
          g_common_packet_format_data_item.address_item.data
          .connection_identifier);
        if (connection_object == NULL) {
          return kEipStatusError;
        }

        /* only handle the data if it is coming from the originator */
        if (connection_object->originator_address.sin_addr.s_addr
            == from_address->sin_addr.s_addr) {

          if ( SEQ_GT32(
                 g_common_packet_format_data_item.address_item.data.
                 sequence_number,
                 connection_object->eip_level_sequence_count_consuming) ) {
            /* reset the watchdog timer */
            ConnectionObjectResetInactivityWatchdogTimerValue(connection_object);

            /* only inform assembly object if the sequence counter is greater or equal */
            connection_object->eip_level_sequence_count_consuming =
              g_common_packet_format_data_item.address_item.data
              .sequence_number;

            if (NULL != connection_object->connection_receive_data_function) {
              return connection_object->connection_receive_data_function(
                       connection_object,
                       g_common_packet_format_data_item.data_item.data,
                       g_common_packet_format_data_item.data_item.length);
            }
          }
        } else {
          OPENER_TRACE_WARN(
            "Connected Message Data Received with wrong address information\n");
        }
      }
    }
  }
  return kEipStatusOk;
}

/** @brief Function prototype for all Forward Open handle functions
 *
 */
typedef EipStatus (*HandleForwardOpenRequestFunction)(
  CipConnectionObject *connection_object, CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response);

/** @brief Handles a Null Non Matching Forward Open Request
 *
 * Null, Non-Matching - Either ping device, or configure a device’s application,
 * or return  General Status kCipErrorConnectionFailure and
 * Extended Status kConnectionManagerExtendedStatusCodeNullForwardOpenNotSupported
 */
EipStatus HandleNullNonMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response);

EipStatus HandleNullNonMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response
  ) {
  OPENER_TRACE_INFO("Right now we cannot handle Null requests\n");
  return AssembleForwardOpenResponse(
           connection_object,
           message_router_response,
           kCipErrorConnectionFailure,
           kConnectionManagerExtendedStatusCodeNullForwardOpenNotSupported);
}

/** @brief Handles a Null Matching Forward Open request
 *
 * Either  reconfigure a target device’s application, or
 * return General Status kCipErrorConnectionFailure and
 * Extended Status kConnectionManagerExtendedStatusCodeNullForwardOpenNotSupported
 */
EipStatus HandleNullMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response);

EipStatus HandleNullMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response
  ) {
  OPENER_TRACE_INFO("Right now we cannot handle Null requests\n");
  return AssembleForwardOpenResponse(
           connection_object,
           message_router_response,
           kCipErrorConnectionFailure,
           kConnectionManagerExtendedStatusCodeNullForwardOpenNotSupported);
}

/** @brief Handles a Non Null Matching Forward Open Request
 *
 * Non-Null, Matching request - Return General Status = kCipErrorConnectionFailure,
 * Extended Status = kConnectionManagerExtendedStatusCodeErrorConnectionInUseOrDuplicateForwardOpen
 */
EipStatus HandleNonNullMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response);

EipStatus HandleNonNullMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response
  ) {
  OPENER_TRACE_INFO("Right now we cannot handle reconfiguration requests\n");
  return AssembleForwardOpenResponse(
           connection_object,
           message_router_response,
           kCipErrorConnectionFailure,
           kConnectionManagerExtendedStatusCodeErrorConnectionInUseOrDuplicateForwardOpen);
}

/** @brief Handles a Non Null Non Matching Forward Open Request
 *
 * Non-Null, Non-Matching request - Establish a new connection
 */
EipStatus HandleNonNullNonMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response);

EipStatus HandleNonNullNonMatchingForwardOpenRequest(
  CipConnectionObject *connection_object,
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response
  ) {

  EipUint16 connection_status = kConnectionManagerExtendedStatusCodeSuccess;

  /*check if the trigger type value is invalid or ok */
  if(kConnectionObjectTransportClassTriggerProductionTriggerInvalid == ConnectionObjectGetTransportClassTriggerProductionTrigger(&g_dummy_connection_object)) {
    return AssembleForwardOpenResponse(
             &g_dummy_connection_object,
             message_router_response,
             kCipErrorConnectionFailure,
             kConnectionManagerExtendedStatusCodeErrorTransportClassAndTriggerCombinationNotSupported);
  }

  EipUint32 temp = ParseConnectionPath(&g_dummy_connection_object,
                                       message_router_request,
                                       &connection_status);
  if (kEipStatusOk != temp) {
    return AssembleForwardOpenResponse(&g_dummy_connection_object,
                                       message_router_response, temp,
                                       connection_status);
  }

  /*parsing is now finished all data is available and check now establish the connection */
  ConnectionManagementHandling *connection_management_entry =
    GetConnectionManagementEntry( /* Gets correct open connection function for the targeted object */
      g_dummy_connection_object.configuration_path.class_id);
  if (NULL != connection_management_entry) {
    temp = connection_management_entry->open_connection_function(
      &g_dummy_connection_object, &connection_status);
  } else {
    temp = kEipStatusError;
    connection_status =
      kConnectionManagerExtendedStatusCodeInconsistentApplicationPathCombo;
  }

  if (kEipStatusOk != temp) {
    OPENER_TRACE_INFO("connection manager: connect failed\n");
    /* in case of error the dummy objects holds all necessary information */
    return AssembleForwardOpenResponse(&g_dummy_connection_object,
                                       message_router_response, temp,
                                       connection_status);
  } else {
    OPENER_TRACE_INFO("connection manager: connect succeeded\n");
    /* in case of success the new connection is added at the head of the connection list */
    return AssembleForwardOpenResponse(connection_list.first->data,
                                       message_router_response,
                                       kCipErrorSuccess, 0);
  }
}

/** @brief Array of Forward Open handle function pointers
 *
 * File scope variable
 * The first dimension handles if the request was a non-null request (0) or a null request (1),
 * the second dimension handles if the request was a non-matchin (0) or matching request (1)
 */
static const HandleForwardOpenRequestFunction
  handle_forward_open_request_functions[2][2] =
{ { HandleNonNullNonMatchingForwardOpenRequest,
    HandleNonNullMatchingForwardOpenRequest }, {
    HandleNullNonMatchingForwardOpenRequest,
    HandleNullMatchingForwardOpenRequest
  } };

/** @brief Check if resources for new connection available, generate ForwardOpen Reply message.
 *
 *  Forward Open four cases
 *  Non-Null/Not matching - open a connection
 *  Non-Null/Matching - error
 *  Null/Not matching - ping a device/configure
 *  Null/Matching - reconfigure
 *
 *  Null connection - both O->T and T->O connection parameter field are null
 *  Non-Null connection - one or both O->T and T->O connection parameter field are not null
 *  Matching - Connection Triad matches an existing connection
 *  (Connection Serial Number, Originator Vendor ID and Originator Serial Number)
 *
 *  @param instance	pointer to CIP object instance
 *  @param message_router_request		pointer to Message Router Request.
 *  @param message_router_response		pointer to Message Router Response.
 *      @return >0 .. success, 0 .. no reply to send back
 *              -1 .. error
 */
EipStatus ForwardOpen(
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  struct sockaddr *originator_address
  ) {
  (void) instance; /*suppress compiler warning */

  bool is_null_request = false; /* 1 = Null Request, 0 =  Non-Null Request  */
  bool is_matching_request = false; /* 1 = Matching Request, 0 = Non-Matching Request  */

  /*first check if we have already a connection with the given params */
  ConnectionObjectInitializeFromMessage(&(message_router_request->data),
                                     &g_dummy_connection_object);

  memcpy( &(g_dummy_connection_object.originator_address), originator_address,
          sizeof(g_dummy_connection_object.originator_address) );

  ConnectionObjectConnectionType o_to_t_connection_type = ConnectionObjectGetOToTConnectionType(&g_dummy_connection_object);
  ConnectionObjectConnectionType t_to_o_connection_type = ConnectionObjectGetTToOConnectionType(&g_dummy_connection_object);

  /* Check if both connection types are valid, otherwise send error response */
  if (kConnectionObjectConnectionTypeInvalid == o_to_t_connection_type) {
    return AssembleForwardOpenResponse(
             &g_dummy_connection_object, message_router_response,
             kCipErrorConnectionFailure,
             kConnectionManagerExtendedStatusCodeErrorInvalidOToTConnectionType);
  }

  if (kConnectionObjectConnectionTypeInvalid == t_to_o_connection_type) {
    return AssembleForwardOpenResponse(
             &g_dummy_connection_object, message_router_response,
             kCipErrorConnectionFailure,
             kConnectionManagerExtendedStatusCodeErrorInvalidTToOConnectionType);
  }

  /* Check if request is a Null request or a Non-Null request */
  if (kConnectionObjectConnectionTypeNull == o_to_t_connection_type
      && kConnectionObjectConnectionTypeNull == t_to_o_connection_type) {
    is_null_request = true;
    OPENER_TRACE_INFO("We have a Null request\n");
  } else {
    is_null_request = false;
    OPENER_TRACE_INFO("We have a Non-Null request\n");
  }

  /* Check if we have a matching or non matching request */
  if ( ( NULL != CheckForExistingConnection(&g_dummy_connection_object) ) ) {
    OPENER_TRACE_INFO("We have a Matching request\n");
    is_matching_request = true;

  } else {
    OPENER_TRACE_INFO("We have a Non-Matching request\n");
    is_matching_request = false;
  }

  HandleForwardOpenRequestFunction choosen_function =
    handle_forward_open_request_functions[is_null_request][is_matching_request];

  return choosen_function(&g_dummy_connection_object, instance,
                          message_router_request, message_router_response);
}

EipStatus ForwardClose(
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  struct sockaddr *originator_address
  ) {
  /*Suppress compiler warning*/
  (void) instance;

  /* check connection_serial_number && originator_vendor_id && originator_serial_number if connection is established */
  ConnectionManagerExtendedStatusCode connection_status =
    kConnectionManagerExtendedStatusCodeErrorConnectionTargetConnectionNotFound;

  /* set AddressInfo Items to invalid TypeID to prevent assembleLinearMsg to read them */
  g_common_packet_format_data_item.address_info_item[0].type_id = 0;
  g_common_packet_format_data_item.address_info_item[1].type_id = 0;

  message_router_request->data += 2; /* ignore Priority/Time_tick and Time-out_ticks */

  EipUint16 connection_serial_number = GetIntFromMessage(
    &message_router_request->data);
  EipUint16 originator_vendor_id = GetIntFromMessage(
    &message_router_request->data);
  EipUint32 originator_serial_number = GetDintFromMessage(
    &message_router_request->data);

  OPENER_TRACE_INFO("ForwardClose: ConnSerNo %d\n", connection_serial_number);

  DoublyLinkedListNode *node = connection_list.first;

  while (NULL != node) {
    /* this check should not be necessary as only established connections should be in the active connection list */
  CipConnectionObject *connection_object = node->data;
    if ( (kConnectionObjectStateEstablished == ConnectionObjectGetState(connection_object))
         || (kConnectionObjectStateTimedOut == ConnectionObjectGetState(connection_object)) ) {
      if ( (connection_object->connection_serial_number
            == connection_serial_number)
           && (connection_object->originator_vendor_id == originator_vendor_id)
           && (connection_object->originator_serial_number
               == originator_serial_number) ) {
        /* found the corresponding connection object -> close it */
        OPENER_ASSERT(NULL != connection_object->connection_close_function);
        if ( ( (struct sockaddr_in *) originator_address )->sin_addr.s_addr
             == connection_object->originator_address.sin_addr.s_addr ) {
          connection_object->connection_close_function(connection_object);
          connection_status = kConnectionManagerExtendedStatusCodeSuccess;
        } else {
          connection_status = kConnectionManagerExtendedStatusWrongCloser;
        }
        break;
      }
    }
    node = node->next;
  }
  if(
    kConnectionManagerExtendedStatusCodeErrorConnectionTargetConnectionNotFound
    ==
    connection_status) {
    OPENER_TRACE_INFO(
      "Connection not found! Requested connection triad: %u, %u, %u\n",
      connection_serial_number,
      originator_vendor_id,
      originator_serial_number);
  }

  return AssembleForwardCloseResponse(connection_serial_number,
                                      originator_vendor_id,
                                      originator_serial_number,
                                      message_router_request,
                                      message_router_response,
                                      connection_status);
}

/* TODO: Not implemented */
EipStatus GetConnectionOwner(
  CipInstance *instance,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  struct sockaddr *originator_address
  ) {
  /* suppress compiler warnings */
  (void) instance;
  (void) message_router_request;
  (void) message_router_response;

  return kEipStatusOk;
}

EipStatus ManageConnections(MilliSeconds elapsed_time) {
  //OPENER_TRACE_INFO("Entering ManageConnections\n");
  /*Inform application that it can execute */
  HandleApplication();
  ManageEncapsulationMessages(elapsed_time);

  DoublyLinkedListNode *node = connection_list.first;

  while (NULL != node) {
    //OPENER_TRACE_INFO("Entering Connection Object loop\n");
    CipConnectionObject *connection_object = node->data;
    if (kConnectionObjectStateEstablished == ConnectionObjectGetState(connection_object)) {
      if ( (NULL != connection_object->consuming_instance) || /* we have a consuming connection check inactivity watchdog timer */
           (kConnectionObjectTransportClassTriggerDirectionServer == ConnectionObjectGetTransportClassTriggerDirection(connection_object)) ) /* all server connections have to maintain an inactivity watchdog timer */
      {
        if (elapsed_time >= connection_object->inactivity_watchdog_timer) {
          /* we have a timed out connection perform watchdog time out action*/
          OPENER_TRACE_INFO(">>>>>>>>>>Connection ConnNr: %u timed out\n",
                            connection_object->connection_serial_number);
          OPENER_ASSERT(NULL != connection_object->connection_timeout_function);
          connection_object->connection_timeout_function(connection_object);
        } else {
          connection_object->inactivity_watchdog_timer -= elapsed_time;
        }
      }
      /* only if the connection has not timed out check if data is to be send */
      if (kConnectionObjectStateEstablished == ConnectionObjectGetState(connection_object)) {
        /* client connection */
        if ( (0 != connection_object->expected_packet_rate)
             && (kEipInvalidSocket
                 != connection_object->socket[
                   kUdpCommuncationDirectionProducing
                 ]) )                                                              /* only produce for the master connection */
        {
          if ( kConnectionObjectTransportClassTriggerProductionTriggerCyclic
               != ConnectionObjectGetTransportClassTriggerProductionTrigger(connection_object) ) {
            /* non cyclic connections have to decrement production inhibit timer */
            if (0 <= connection_object->production_inhibit_timer) {
              connection_object->production_inhibit_timer -= elapsed_time;
            }
          }
          connection_object->transmission_trigger_timer -= elapsed_time;
          if (connection_object->transmission_trigger_timer <= 0) { /* need to send package */
            OPENER_ASSERT(
              NULL != connection_object->connection_send_data_function);
            EipStatus eip_status = connection_object
                                   ->connection_send_data_function(
              connection_object);
            if (eip_status == kEipStatusError) {
              OPENER_TRACE_ERR(
                "sending of UDP data in manage Connection failed\n");
            }
            /* reload the timer value */
            connection_object->transmission_trigger_timer = connection_object
                                                            ->
                                                            expected_packet_rate;
            if ( kConnectionObjectTransportClassTriggerProductionTriggerCyclic
                 != ConnectionObjectGetTransportClassTriggerProductionTrigger(connection_object) ) {
              /* non cyclic connections have to reload the production inhibit timer */
              connection_object->production_inhibit_timer = connection_object
                                                            ->
                                                            production_inhibit_time;
            }
          }
        }
      }
    }
    node = node->next;
  }
  return kEipStatusOk;
}

/** @brief Assembles the Forward Open Response
 *
 * @param connection_object pointer to connection Object
 * @param message_router_response pointer to message router response
 * @param general_status the general status of the response
 * @param extended_status extended status in the case of an error otherwise 0
 * @return status
 *   kEipStatusOk .. no reply need to be sent back
 *   kEipStatusOkSend .. need to send reply
 *   kEipStatusError .. error
 */
EipStatus AssembleForwardOpenResponse(
  CipConnectionObject *connection_object,
  CipMessageRouterResponse *message_router_response,
  EipUint8 general_status,
  EipUint16 extended_status
  ) {
  /* write reply information in CPF struct dependent of pa_status */
  CipCommonPacketFormatData *cip_common_packet_format_data =
    &g_common_packet_format_data_item;
  EipByte *message = message_router_response->data;
  cip_common_packet_format_data->item_count = 2;
  cip_common_packet_format_data->data_item.type_id =
    kCipItemIdUnconnectedDataItem;

  AddNullAddressItem(cip_common_packet_format_data);

  message_router_response->reply_service = (0x80 | kForwardOpen);
  message_router_response->general_status = general_status;

  if (kCipErrorSuccess == general_status) {
    OPENER_TRACE_INFO("assembleFWDOpenResponse: sending success response\n");
    message_router_response->data_length = 26; /* if there is no application specific data */
    message_router_response->size_of_additional_status = 0;

    if (cip_common_packet_format_data->address_info_item[0].type_id != 0) {
      cip_common_packet_format_data->item_count = 3;
      if (cip_common_packet_format_data->address_info_item[1].type_id != 0) {
        cip_common_packet_format_data->item_count = 4; /* there are two sockaddrinfo items to add */
      }
    }

    AddDintToMessage(connection_object->cip_consumed_connection_id, &message);
    AddDintToMessage(connection_object->cip_produced_connection_id, &message);
  } else {
    /* we have an connection creation error */
    OPENER_TRACE_INFO("AssembleForwardOpenResponse: sending error response\n");
    ConnectionObjectSetState(connection_object, kConnectionObjectStateNonExistent);
    message_router_response->data_length = 10;

    switch (general_status) {
      case kCipErrorNotEnoughData:
      case kCipErrorTooMuchData: {
        message_router_response->size_of_additional_status = 0;
        break;
      }

      default: {
        switch (extended_status) {
          case
            kConnectionManagerExtendedStatusCodeErrorInvalidOToTConnectionSize:
          {
            message_router_response->size_of_additional_status = 2;
            message_router_response->additional_status[0] = extended_status;
            message_router_response->additional_status[1] = connection_object
                                                            ->
                                                            correct_originator_to_target_size;
            break;
          }

          case
            kConnectionManagerExtendedStatusCodeErrorInvalidTToOConnectionSize:
          {
            message_router_response->size_of_additional_status = 2;
            message_router_response->additional_status[0] = extended_status;
            message_router_response->additional_status[1] = connection_object
                                                            ->
                                                            correct_target_to_originator_size;
            break;
          }

          default: {
            message_router_response->size_of_additional_status = 1;
            message_router_response->additional_status[0] = extended_status;
            break;
          }
        }
        break;
      }
    }
  }

  AddIntToMessage(connection_object->connection_serial_number, &message);
  AddIntToMessage(connection_object->originator_vendor_id, &message);
  AddDintToMessage(connection_object->originator_serial_number, &message);

  if (kCipErrorSuccess == general_status) {
    /* set the actual packet rate to requested packet rate */
    AddDintToMessage(connection_object->o_to_t_requested_packet_interval,
                     &message);
    AddDintToMessage(connection_object->t_to_o_requested_packet_interval,
                     &message);
  }

  *message = 0; /* remaining path size - for routing devices relevant */
  message++;
  *message = 0; /* reserved */
  message++;

  return kEipStatusOkSend; /* send reply */
}

/**
 * @brief Adds a Null Address Item to the common data packet format data
 * @param common_data_packet_format_data The CPF data packet where the Null Address Item shall be added
 */
void AddNullAddressItem(
  CipCommonPacketFormatData *common_data_packet_format_data) {
  /* Precondition: Null Address Item only valid in unconnected messages */
  assert(
    common_data_packet_format_data->data_item.type_id
    == kCipItemIdUnconnectedDataItem);

  common_data_packet_format_data->address_item.type_id = kCipItemIdNullAddress;
  common_data_packet_format_data->address_item.length = 0;
}

/*   INT8 assembleFWDCloseResponse(UINT16 pa_ConnectionSerialNr, UINT16 pa_OriginatorVendorID, UINT32 pa_OriginatorSerialNr, S_CIP_MR_Request *pa_MRRequest, S_CIP_MR_Response *pa_MRResponse, S_CIP_CPF_Data *pa_CPF_data, INT8 pa_status, INT8 *pa_msg)
 *   create FWDClose response dependent on status.
 *      pa_ConnectionSerialNr	requested ConnectionSerialNr
 *      pa_OriginatorVendorID	requested OriginatorVendorID
 *      pa_OriginatorSerialNr	requested OriginalSerialNr
 *      pa_MRRequest		pointer to message router request
 *      pa_MRResponse		pointer to message router response
 *      pa_CPF_data		pointer to CPF Data Item
 *      pa_status		status of FWDClose
 *      pa_msg			pointer to memory where reply has to be stored
 *  return status
 *                      0 .. no reply need to ne sent back
 *                      1 .. need to send reply
 *                     -1 .. error
 */
EipStatus AssembleForwardCloseResponse(
  EipUint16 connection_serial_number,
  EipUint16 originatior_vendor_id,
  EipUint32 originator_serial_number,
  CipMessageRouterRequest *message_router_request,
  CipMessageRouterResponse *message_router_response,
  EipUint16 extended_error_code
  ) {
  /* write reply information in CPF struct dependent of pa_status */
  CipCommonPacketFormatData *common_data_packet_format_data =
    &g_common_packet_format_data_item;
  EipByte *message = message_router_response->data;
  common_data_packet_format_data->item_count = 2;
  common_data_packet_format_data->data_item.type_id =
    kCipItemIdUnconnectedDataItem;

  AddNullAddressItem(common_data_packet_format_data);

  AddIntToMessage(connection_serial_number, &message);
  AddIntToMessage(originatior_vendor_id, &message);
  AddDintToMessage(originator_serial_number, &message);

  message_router_response->reply_service = (0x80
                                            | message_router_request->service);
  message_router_response->data_length = 10; /* if there is no application specific data */

  if (kConnectionManagerExtendedStatusCodeSuccess == extended_error_code) {
    *message = 0; /* no application data */
    message_router_response->general_status = kCipErrorSuccess;
    message_router_response->size_of_additional_status = 0;
  } else {
    *message = *message_router_request->data; /* remaining path size */
    if (kConnectionManagerExtendedStatusWrongCloser == extended_error_code) {
      message_router_response->general_status = kCipErrorPrivilegeViolation;
    } else {
      message_router_response->general_status = kCipErrorConnectionFailure;
      message_router_response->additional_status[0] = extended_error_code;
      message_router_response->size_of_additional_status = 1;
    }
  }

  message++;
  *message = 0; /* reserved */
  message++;

  return kEipStatusOkSend;
}

CipConnectionObject *GetConnectedObject(const EipUint32 connection_id) {
  DoublyLinkedListNode *iterator = connection_list.first;

  while(NULL != iterator->next) {
    if(kConnectionObjectStateEstablished == ConnectionObjectGetState(iterator->data) &&
        connection_id == ConnectionObjectGetCipConsumedConnectionID(iterator->data)) {
      return iterator->data;
    }
    iterator = iterator->next;
  }
  return NULL;
}

CipConnectionObject *GetConnectedOutputAssembly(const EipUint32 output_assembly_id) {
  DoublyLinkedListNode *iterator = connection_list.first;

  while(NULL != iterator->next) {
      if(kConnectionObjectStateEstablished == ConnectionObjectGetState(iterator->data) &&
          output_assembly_id == ((CipConnectionObject*)iterator->data)->produced_path.instance_id ) {
        return iterator->data;
      }
      iterator = iterator->next;
    }

  return NULL;
}

bool EqualConnectionTriad(const CipConnectionObject const* object1, const CipConnectionObject const* object2) {
  if ( (object1->connection_serial_number
              == object2->connection_serial_number)
             && (object1->originator_vendor_id
                 == object2->originator_vendor_id)
             && (object1->originator_serial_number
                 == object2->originator_serial_number) ) {
    return true;
  }
  return false;
}

CipConnectionObject *CheckForExistingConnection(
  const CipConnectionObject *const connection_object) {

  DoublyLinkedListNode *iterator = connection_list.first;

    while(NULL != iterator) {
        if(kConnectionObjectStateEstablished == ConnectionObjectGetState(iterator->data)) {
          if(EqualConnectionTriad(connection_object, iterator->data)){
            return iterator->data;
          }
        }
        iterator = iterator->next;
      }

    return NULL;
}

EipStatus CheckElectronicKeyData(
  EipUint8 key_format,
  void *key_data,
  EipUint16 *extended_status
  ) {
  bool compatiblity_mode = ElectronicKeyFormat4GetMajorRevisionCompatibility(key_data);

  /* Default return value */
  *extended_status = kConnectionManagerExtendedStatusCodeSuccess;

  /* Check key format */
  if (4 != key_format) {
    *extended_status =
      kConnectionManagerExtendedStatusCodeErrorInvalidSegmentTypeInPath;
    return kEipStatusError;
  }

  /* Check VendorID and ProductCode, must match, or 0 */
  if ( ( (ElectronicKeyFormat4GetVendorId(key_data) != vendor_id_) && (ElectronicKeyFormat4GetVendorId(key_data) != 0) )
       || ( (ElectronicKeyFormat4GetProductCode(key_data) != product_code_)
            && (ElectronicKeyFormat4GetProductCode(key_data) != 0) ) ) {
    *extended_status =
      kConnectionManagerExtendedStatusCodeErrorVendorIdOrProductcodeError;
    return kEipStatusError;
  } else {
    /* VendorID and ProductCode are correct */

    /* Check DeviceType, must match or 0 */
    if ( (ElectronicKeyFormat4GetDeviceType(key_data) != device_type_)
         && (ElectronicKeyFormat4GetDeviceType(key_data) != 0) ) {
      *extended_status =
        kConnectionManagerExtendedStatusCodeErrorDeviceTypeError;
      return kEipStatusError;
    } else {
      /* VendorID, ProductCode and DeviceType are correct */

      if (false == compatiblity_mode) {
        /* Major = 0 is valid */
        if (0 == ElectronicKeyFormat4GetDeviceType(key_data)) {
          return kEipStatusOk;
        }

        /* Check Major / Minor Revision, Major must match, Minor match or 0 */
        if ( (ElectronicKeyFormat4GetMajorRevision(key_data) != revision_.major_revision)
             || ( (ElectronicKeyFormat4GetMinorRevision(key_data) != revision_.minor_revision)
                  && (ElectronicKeyFormat4GetMinorRevision(key_data) != 0) ) ) {
          *extended_status =
            kConnectionManagerExtendedStatusCodeErrorRevisionMismatch;
          return kEipStatusError;
        }
      } else {
        /* Compatibility mode is set */

        /* Major must match, Minor != 0 and <= MinorRevision */
        if ( (ElectronicKeyFormat4GetMajorRevision(key_data) == revision_.major_revision)
             && (ElectronicKeyFormat4GetMinorRevision(key_data) > 0)
             && (ElectronicKeyFormat4GetMinorRevision(key_data) <= revision_.minor_revision) ) {
          return kEipStatusOk;
        } else {
          *extended_status =
            kConnectionManagerExtendedStatusCodeErrorRevisionMismatch;
          return kEipStatusError;
        }
      } /* end if CompatiblityMode handling */
    }
  }

  return
    (*extended_status == kConnectionManagerExtendedStatusCodeSuccess) ?
    kEipStatusOk : kEipStatusError;
}

EipUint8 ParseConnectionPath(
  CipConnectionObject *connection_object,
  CipMessageRouterRequest *message_router_request,
  EipUint16 *extended_error
  ) {
  const EipUint8 *message = message_router_request->data;
  const size_t connection_path_size = GetSintFromMessage(&message); /* length in words */
  size_t remaining_path = connection_path_size;
  CipClass *class = NULL;

  CipDword class_id = 0x0;
  CipDword instance_id = 0x0;

  /* with 256 we mark that we haven't got a PIT segment */
  ConnectionObjectSetProductionInhibitTime(connection_object, 256);

  if ( (g_kForwardOpenHeaderLength + remaining_path * 2)
       < message_router_request->request_path_size ) {
    /* the received packet is larger than the data in the path */
    *extended_error = 0;
    return kCipErrorTooMuchData;
  }

  if ( (g_kForwardOpenHeaderLength + remaining_path * 2)
       > message_router_request->request_path_size ) {
    /*there is not enough data in received packet */
    *extended_error = 0;
    OPENER_TRACE_INFO("Message not long enough for path\n");
    return kCipErrorNotEnoughData;
  }

  if (remaining_path > 0) {
    /* first look if there is an electronic key */
    if ( kSegmentTypeLogicalSegment == GetPathSegmentType(message) ) {
      if ( kLogicalSegmentLogicalTypeSpecial
           == GetPathLogicalSegmentLogicalType(message) ) {
        if ( kLogicalSegmentSpecialTypeLogicalFormatElectronicKey
             == GetPathLogicalSegmentSpecialTypeLogicalType(message) ) {
          if ( kElectronicKeySegmentFormatKeyFormat4
               == GetPathLogicalSegmentElectronicKeyFormat(message) ) {
            /* Check if there is enough data for holding the electronic key segment */
            if (remaining_path < 5) {
              *extended_error = 0;
              OPENER_TRACE_INFO("Message not long enough for electronic key\n");
              return kCipErrorNotEnoughData;
            }
            /* Electronic key format 4 found */
            ElectronicKeyFormat4 *electronic_key = ElectronicKeyFormat4New();
            GetElectronicKeyFormat4FromMessage(&message, electronic_key);
            /* logical electronic key found */
            connection_object->electronic_key.key_data = electronic_key;

            ElectronicKeyFormat4Delete(&electronic_key);

            remaining_path -= 5; /*length of the electronic key*/
            OPENER_TRACE_INFO(
              "key: ven ID %d, dev type %d, prod code %d, major %d, minor %d\n",
              ElectronicKeyFormat4GetVendorId(connection_object->electronic_key.key_data),
              ElectronicKeyFormat4GetDeviceType(connection_object->electronic_key.key_data),
              ElectronicKeyFormat4GetProductCode(connection_object->electronic_key.key_data),
              ElectronicKeyFormat4GetMajorRevision(connection_object->electronic_key.key_data),
              ElectronicKeyFormat4GetMinorRevision(connection_object->electronic_key.key_data));
            if ( kEipStatusOk
                 != CheckElectronicKeyData(
                   connection_object->electronic_key.key_format,
                   &(connection_object->electronic_key.key_data),
                   extended_error) ) {
              return kCipErrorConnectionFailure;
            }
          }

        } else {
          OPENER_TRACE_INFO("no key\n");
        }
      }
    }

    //TODO: Refactor this afterwards
    if ( kConnectionObjectTransportClassTriggerProductionTriggerCyclic
         != ConnectionObjectGetTransportClassTriggerProductionTrigger(connection_object) ) {
      /*non cyclic connections may have a production inhibit */
      if ( kSegmentTypeNetworkSegment == GetPathSegmentType(message) ) {
        NetworkSegmentSubtype network_segment_subtype =
          GetPathNetworkSegmentSubtype(message);
        if (kNetworkSegmentSubtypeProductionInhibitTimeInMilliseconds
            == network_segment_subtype) {
          OPENER_TRACE_INFO("PIT segment available - value: %u\n",message[1]);
          connection_object->production_inhibit_time = message[1];
          message += 2;
          remaining_path -= 1;
        }
      }
    }

    if (kSegmentTypeLogicalSegment == GetPathSegmentType(message) && kLogicalSegmentLogicalTypeClassId == GetPathLogicalSegmentLogicalType(message) ) {

      class_id = CipEpathGetLogicalValue(&message);
      class = GetCipClass(class_id);
      if (NULL == class) {
        OPENER_TRACE_ERR("classid %" PRIx32 " not found\n",
                         class_id);

        if (class_id >= 0xC8) { /*reserved range of class ids */
          *extended_error =
            kConnectionManagerExtendedStatusCodeErrorInvalidSegmentTypeInPath;
        } else {
          *extended_error =
            kConnectionManagerExtendedStatusCodeInconsistentApplicationPathCombo;
        }
        return kCipErrorConnectionFailure;
      }

      OPENER_TRACE_INFO("classid %" PRIx32 " (%s)\n",
                        class_id,
                        class->class_name);
    } else {
      *extended_error =
        kConnectionManagerExtendedStatusCodeErrorInvalidSegmentTypeInPath;
      return kCipErrorConnectionFailure;
    }
    remaining_path -= 1; /* 1 16Bit word for the class part of the path */

    /* Get instance ID */
    if ( kSegmentTypeLogicalSegment == GetPathSegmentType(message) && kLogicalSegmentLogicalTypeInstanceId == GetPathLogicalSegmentLogicalType(message)  ) { /* store the configuration ID for later checking in the application connection types */
      instance_id = CipEpathGetLogicalValue(&message);

      OPENER_TRACE_INFO("Configuration instance id %" PRId32 "\n",
          instance_id);
      if ( NULL == GetCipInstance(class, instance_id) ) {
        /*according to the test tool we should respond with this extended error code */
        *extended_error =
          kConnectionManagerExtendedStatusCodeErrorInvalidSegmentTypeInPath;
        return kCipErrorConnectionFailure;
      }
      /* 1 or 2 16Bit words for the configuration instance part of the path  */
      remaining_path -= (instance_id > 0xFF) ? 2 : 1; //TODO: 32 bit case missing
    } else {
      OPENER_TRACE_INFO("no config data\n");
    }

    if ( kConnectionObjectTransportClassTriggerTransportClass3 == ConnectionObjectGetTransportClassTriggerTransportClass(connection_object) ) {
    /*we have Class 3 connection*/
      if (remaining_path > 0) {
        OPENER_TRACE_WARN(
          "Too much data in connection path for class 3 connection\n");
        *extended_error =
          kConnectionManagerExtendedStatusCodeErrorInvalidSegmentTypeInPath;
        return kCipErrorConnectionFailure;
      }

      /* connection end point has to be the message router instance 1 */
      if ( (class_id != kCipMessageRouterClassCode)
           || (1 != instance_id) ) {
        *extended_error =
          kConnectionManagerExtendedStatusCodeInconsistentApplicationPathCombo;
        return kCipErrorConnectionFailure;
      }
      /* Configuration connection point is producing connection point */
      CipConnectionPathEpath connection_epath = {
          .class_id = class_id,
          .instance_id = instance_id,
          .attribute_id_or_connection_point = 0
      };

      memcpy(&(connection_object->configuration_path), &connection_epath, sizeof(connection_object->configuration_path));
      memcpy(&(connection_object->produced_path), &connection_epath, sizeof(connection_object->produced_path));

    /* End class 3 connection handling */
    } else { /* we have an IO connection */
        ConnectionObjectConnectionType originator_to_target_connection_type = ConnectionObjectGetOToTConnectionType(
        connection_object);
        ConnectionObjectConnectionType target_to_originator_connection_type = ConnectionObjectGetTToOConnectionType(
        connection_object);

        connection_object->consumed_connection_path_length = 0;
        connection_object->consumed_connection_path = NULL;
      //connection_object->connection_path.connection_point[1] = 0; /* set not available path to Invalid */

      size_t number_of_encoded_paths = 0;
      if (kConnectionObjectConnectionTypeNull == originator_to_target_connection_type) {
        if (kConnectionObjectConnectionTypeNull == target_to_originator_connection_type) { /* configuration only connection */
          number_of_encoded_paths = 0;
          OPENER_TRACE_WARN("assembly: type invalid\n");
        } else { /* 1 path -> path is for production */
          OPENER_TRACE_INFO("assembly: type produce\n");
          number_of_encoded_paths = 1;
        }
      } else {
        if (kConnectionObjectConnectionTypeNull == target_to_originator_connection_type) { /* 1 path -> path is for consumption */
          OPENER_TRACE_INFO("assembly: type consume\n");
          number_of_encoded_paths = 1;
        } else { /* 2 paths -> 1st for production 2nd for consumption */
          OPENER_TRACE_INFO("assembly: type bidirectional\n");
          number_of_encoded_paths = 2;
        }
      }

      for (size_t i = 0; i < number_of_encoded_paths; i++) /* process up to 2 encoded paths */
      {
        if ( kLogicalSegmentLogicalTypeInstanceId == GetPathLogicalSegmentLogicalType(message) || kLogicalSegmentLogicalTypeConnectionPoint == GetPathLogicalSegmentLogicalType(message) ) /* Connection Point interpreted as InstanceNr -> only in Assembly Objects */
        { /* Attribute Id or Connection Point */
          CipDword attribute_id = CipEpathGetLogicalValue(&message);
          CipConnectionPathEpath connection_epath = {
                    .class_id = class_id,
                    .instance_id = instance_id,
                    .attribute_id_or_connection_point = attribute_id
                };
          /* TODO: Remainder of old implementation, look for better way of doing this */
          if(0 == i){
            memcpy(&(connection_object->produced_path), &connection_epath, sizeof(connection_object->produced_path));
          }
          if(1 == i){
            memcpy(&(connection_object->consumed_path), &connection_epath, sizeof(connection_object->consumed_path));
          }
          OPENER_TRACE_INFO(
            "connection point %" PRIu32 "\n",
            attribute_id);
          if ( NULL
               == GetCipInstance(
                 class,
                 attribute_id) ) { /* Old code - Probably here the attribute ID marks the instance for the assembly object  */
            *extended_error =
              kConnectionManagerExtendedStatusCodeInconsistentApplicationPathCombo;
            return kCipErrorConnectionFailure;
          }
          /* 1 or 2 16Bit word for the connection point part of the path */
          remaining_path -= (attribute_id > 0xFF) ? 2 : 1;
        } else {
          *extended_error =
            kConnectionManagerExtendedStatusCodeErrorInvalidSegmentTypeInPath;
          return kCipErrorConnectionFailure;
        }
      }

      g_config_data_length = 0;
      g_config_data_buffer = NULL;

      while (remaining_path > 0) { /* remaining_path_size something left in the path should be configuration data */

        SegmentType segment_type = GetPathSegmentType(message);
        switch (segment_type) {
          case kSegmentTypeDataSegment: {
            DataSegmentSubtype data_segment_type = GetPathDataSegmentSubtype(
              message);
            switch (data_segment_type) {
              case kDataSegmentSubtypeSimpleData:
                g_config_data_length = message[1] * 2; /*data segments store length 16-bit word wise */
                g_config_data_buffer = (EipUint8 *) message + 2;
                remaining_path -= (g_config_data_length + 2) / 2;
                message += (g_config_data_length + 2);
                break;
              default:
                OPENER_TRACE_ERR("Not allowed in connection manager");
                break;
            }
          }
          break;
          case kSegmentTypeNetworkSegment: {
            NetworkSegmentSubtype subtype = GetPathNetworkSegmentSubtype(
              message);
            switch (subtype) {
              case kNetworkSegmentSubtypeProductionInhibitTimeInMilliseconds:
                if ( kConnectionObjectTransportClassTriggerProductionTriggerCyclic
                     != ConnectionObjectGetTransportClassTriggerProductionTrigger(connection_object) ) {
                  /* only non cyclic connections may have a production inhibit */
                  connection_object->production_inhibit_time = message[1];
                  message += 2;
                  remaining_path -= 2;
                } else {
                  *extended_error = connection_path_size - remaining_path; /*offset in 16Bit words where within the connection path the error happend*/
                  return kCipErrorPathSegmentError; /*status code for invalid segment type*/
                }
              default:
                OPENER_TRACE_ERR("Not allowed in connection manager");
                break;
            }
          }
          break;

          default:
            OPENER_TRACE_WARN(
              "No data segment identifier found for the configuration data\n");
            *extended_error = connection_path_size - remaining_path; /*offset in 16Bit words where within the connection path the error happend*/
            return kConnectionManagerGeneralStatusPathSegmentErrorInUnconnectedSend;
            break;
        }
      }
    }
  }

  OPENER_TRACE_INFO("Resulting PIT value: %u\n",
                    connection_object->production_inhibit_time);
  /*save back the current position in the stream allowing followers to parse anything thats still there*/
  message_router_request->data = message;
  return kEipStatusOk;
}

void CloseConnection(CipConnectionObject *RESTRICT connection_object) {
  ConnectionObjectSetState(connection_object, kConnectionObjectStateNonExistent);
  if ( kConnectionObjectTransportClassTriggerTransportClass3 != ConnectionObjectGetTransportClassTriggerTransportClass(connection_object) ) {
    /* only close the UDP connection for not class 3 connections */
    CloseUdpSocket(
      connection_object->socket[kUdpCommuncationDirectionConsuming]);
    connection_object->socket[kUdpCommuncationDirectionConsuming] =
      kEipInvalidSocket;
    CloseUdpSocket(
      connection_object->socket[kUdpCommuncationDirectionProducing]);
    connection_object->socket[kUdpCommuncationDirectionProducing] =
      kEipInvalidSocket;
  }
  RemoveFromActiveConnections(connection_object);

}


void AddNewActiveConnection(const CipConnectionObject *const connection_object) {
  DoublyLinkedListInsertAtTail(&connection_list, connection_object);
}

void RemoveFromActiveConnections(CipConnectionObject *const connection_object) {

  for(DoublyLinkedListNode *iterator = connection_list.first; iterator != connection_list.last; iterator = iterator->next) {
    if(iterator->data == connection_object) {
      ConnectionObjectInitializeEmpty(connection_object);
      DoublyLinkedListRemoveNode(&connection_list, &iterator);
      break;
    }
  }

//  if (NULL != connection_object->first_connection_object) {
//    connection_object->first_connection_object->next_connection_object =
//      connection_object->next_connection_object;
//  } else {
//    g_active_connection_list = connection_object->next_connection_object;
//  }
//  if (NULL != connection_object->next_connection_object) {
//    connection_object->next_connection_object->first_connection_object =
//      connection_object->first_connection_object;
//  }
//
//  connection_object->first_connection_object = NULL;
//  connection_object->next_connection_object = NULL;
//  connection_object->state = kConnectionStateNonExistent;
}

EipBool8 IsConnectedOutputAssembly(const EipUint32 instance_number) {
  EipBool8 is_connected = false;

  DoublyLinkedListNode *node = connection_list.first;

  while (NULL != node) {
    CipDword produced_connection_point = ((CipConnectionObject*)node->data)->produced_path.attribute_id_or_connection_point;
    if (instance_number == produced_connection_point) {
      is_connected = true;
      break;
    }
    node = node->next;
  }
  return is_connected;
}

EipStatus AddConnectableObject(
  const EipUint32 class_id,
  OpenConnectionFunction open_connection_function
  ) {
  EipStatus status = kEipStatusError;

  /*parsing is now finished all data is available and check now establish the connection */
  for (size_t i = 0; i < g_kNumberOfConnectableObjects; ++i) {
    if ( (0 == g_connection_management_list[i].class_id)
         || (class_id == g_connection_management_list[i].class_id) ) {
      g_connection_management_list[i].class_id = class_id;
      g_connection_management_list[i].open_connection_function =
        open_connection_function;
      status = kEipStatusOk;
      break;
    }
  }

  return status;
}

ConnectionManagementHandling *
GetConnectionManagementEntry(const EipUint32 class_id) {

  ConnectionManagementHandling *connection_management_entry = NULL;

  for (int i = 0; i < g_kNumberOfConnectableObjects; ++i) {
    if (class_id == g_connection_management_list[i].class_id) {
      connection_management_entry = &(g_connection_management_list[i]);
      break;
    }
  }
  return connection_management_entry;
}

EipStatus TriggerConnections(
  unsigned int output_assembly,
  unsigned int input_assembly
  ) {
  EipStatus status = kEipStatusError;

  DoublyLinkedListNode *node = connection_list.first;
  while (NULL != node) {
  CipConnectionObject *connection_object = node->data;
    if ( (output_assembly == connection_object->produced_path.attribute_id_or_connection_point)
         && (input_assembly ==
             connection_object->consumed_path.attribute_id_or_connection_point) ) {
      if ( kConnectionObjectTransportClassTriggerProductionTriggerApplicationObject
           == ConnectionObjectGetTransportClassTriggerProductionTrigger(connection_object) ) {
        /* produce at the next allowed occurrence */
        connection_object->transmission_trigger_timer = connection_object
                                                ->production_inhibit_time;
        status = kEipStatusOk;
      }
      break;
    }
  }
  return status;
}

void InitializeConnectionManagerData() {
  memset( g_connection_management_list, 0,
          g_kNumberOfConnectableObjects *
          sizeof(ConnectionManagementHandling) );
  InitializeClass3ConnectionData();
  InitializeIoConnectionData();
}