Add Large Forward Open service

source/src/cip/cipclass3connection.c

@@ -21,7 +21,7 @@ void Class3ConnectionTimeoutHandler(CipConnectionObject *connection_object) {
                                                     CloseSessionBySessionHandle);
   CloseConnection(connection_object);
 }
-#define RESTRICT
+
 /**** Implementation ****/
 CipError EstablishClass3Connection(
   CipConnectionObject *RESTRICT const connection_object,

source/src/cip/cipconnectionmanager.c

@@ -30,6 +30,7 @@
 
 
 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)*/
+const size_t g_kLargeForwardOpenHeaderLength = 40; /**< the length in bytes of the large forward open command specific data till the start of the connection path (including con path size)*/
 
 static const int g_kNumberOfConnectableObjects = 2 +
                                                  OPENER_CIP_NUM_APPLICATION_SPECIFIC_CONNECTABLE_OBJECTS;
@@ -61,6 +62,13 @@ EipStatus ForwardOpen(
   const struct sockaddr *originator_address,
   const int encapsulation_session);
 
+EipStatus LargeForwardOpen(
+  CipInstance *instance,
+  CipMessageRouterRequest *message_router_request,
+  CipMessageRouterResponse *message_router_response,
+  const struct sockaddr *originator_address,
+  const int encapsulation_session);
+
 EipStatus ForwardClose(
   CipInstance *instance,
   CipMessageRouterRequest *message_router_request,
@@ -217,7 +225,7 @@ EipStatus ConnectionManagerInit(EipUint16 unique_connection_id) {
     2,   /* # of class services */
     0,   /* # of instance attributes */
     14,   /* # highest instance attribute number*/
-    5,   /* # of instance services */
+    6,   /* # of instance services */
     1,   /* # of instances */
     "connection manager",   /* class name */
     1,   /* revision */
@@ -230,6 +238,10 @@ EipStatus ConnectionManagerInit(EipUint16 unique_connection_id) {
   InsertService(connection_manager, kGetAttributeAll, &GetAttributeAll,
                 "GetAttributeAll");
   InsertService(connection_manager, kForwardOpen, &ForwardOpen, "ForwardOpen");
+  InsertService(connection_manager,
+                kLargeForwardOpen,
+                &LargeForwardOpen,
+                "LargeForwardOpen");
   InsertService(connection_manager, kForwardClose, &ForwardClose,
                 "ForwardClose");
   InsertService(connection_manager, kGetConnectionOwner, &GetConnectionOwner,
@@ -471,6 +483,33 @@ static const HandleForwardOpenRequestFunction
     HandleNullMatchingForwardOpenRequest
   } };
 
+EipStatus ForwardOpenRoutine(
+  CipInstance *instance,
+  CipMessageRouterRequest *message_router_request,
+  CipMessageRouterResponse *message_router_response,
+  const struct sockaddr *originator_address,
+  const int encapsulation_session
+  );
+
+/** @brief Check if resources for new connection available, generate ForwardOpen Reply message.
+ *
+ * Large Forward Open service calls Forward Open service
+ */
+EipStatus LargeForwardOpen(
+  CipInstance *instance,
+  CipMessageRouterRequest *message_router_request,
+  CipMessageRouterResponse *message_router_response,
+  const struct sockaddr *originator_address,
+  const int encapsulation_session
+  ) {
+  g_dummy_connection_object.is_large_forward_open = true;
+  return ForwardOpenRoutine(instance,
+                            message_router_request,
+                            message_router_response,
+                            originator_address,
+                            encapsulation_session);
+}
+
 /** @brief Check if resources for new connection available, generate ForwardOpen Reply message.
  *
  *  Forward Open four cases
@@ -493,6 +532,20 @@ static const HandleForwardOpenRequestFunction
  *              -1 .. error
  */
 EipStatus ForwardOpen(
+  CipInstance *instance,
+  CipMessageRouterRequest *message_router_request,
+  CipMessageRouterResponse *message_router_response,
+  const struct sockaddr *originator_address,
+  const int encapsulation_session
+  ) {
+  g_dummy_connection_object.is_large_forward_open = false;
+  return ForwardOpenRoutine(instance,
+                            message_router_request,
+                            message_router_response,
+                            originator_address,
+                            encapsulation_session);
+}
+EipStatus ForwardOpenRoutine(
   CipInstance *instance,
   CipMessageRouterRequest *message_router_request,
   CipMessageRouterResponse *message_router_response,
@@ -535,10 +588,11 @@ EipStatus ForwardOpen(
 
   if(kConnectionObjectConnectionTypeMulticast == t_to_o_connection_type) {
     /* for multicast, check if IP is within configured net because we send TTL 1 */
-    CipUdint originator_ip = ((struct sockaddr_in *)originator_address)->sin_addr.s_addr;
+    CipUdint originator_ip =
+      ( (struct sockaddr_in *)originator_address )->sin_addr.s_addr;
     CipUdint interface_ip = g_network_status.ip_address;
     CipUdint interface_mask = g_network_status.network_mask;
-    if((originator_ip & interface_mask)!=(interface_ip & interface_mask)) {
+    if( (originator_ip & interface_mask)!=(interface_ip & interface_mask) ) {
       return AssembleForwardOpenResponse(
         &g_dummy_connection_object, message_router_response,
         kCipErrorConnectionFailure,
@@ -768,7 +822,12 @@ EipStatus AssembleForwardOpenResponse(
 
   AddNullAddressItem(cip_common_packet_format_data);
 
-  message_router_response->reply_service = (0x80 | kForwardOpen);
+  CIPServiceCode service_code = kForwardOpen;
+  if (connection_object->is_large_forward_open) {
+    service_code = kLargeForwardOpen;
+  }
+
+  message_router_response->reply_service = (0x80 | service_code);
   message_router_response->general_status = general_status;
 
   if (kCipErrorSuccess == general_status) {
@@ -1081,14 +1140,19 @@ EipUint8 ParseConnectionPath(
   /* with 256 we mark that we haven't got a PIT segment */
   ConnectionObjectSetProductionInhibitTime(connection_object, 256);
 
-  if ( (g_kForwardOpenHeaderLength + remaining_path * 2)
+  size_t header_length = g_kForwardOpenHeaderLength;
+  if (connection_object->is_large_forward_open) {
+    header_length = g_kLargeForwardOpenHeaderLength;
+  }
+
+  if ( (header_length + 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)
+  if ( (header_length + remaining_path * 2)
        > message_router_request->request_path_size ) {
     /*there is not enough data in received packet */
     *extended_error = 0;

source/src/cip/cipconnectionobject.c

@@ -44,13 +44,13 @@
 #define CIP_CONNECTION_OBJECT_WATCHDOG_TIMEOUT_ACTION_DEFERRED_DELETE 3
 
 #define CIP_CONNECTION_OBJECT_CONNECTION_TYPE_NULL 0
-#define CIP_CONNECTION_OBJECT_CONNECTION_TYPE_MULTICAST (1 << 13)
-#define CIP_CONNECTION_OBJECT_CONNECTION_TYPE_POINT_TO_POINT (1 << 14)
+#define CIP_CONNECTION_OBJECT_CONNECTION_TYPE_MULTICAST 1
+#define CIP_CONNECTION_OBJECT_CONNECTION_TYPE_POINT_TO_POINT 2
 
 #define CIP_CONNECTION_OBJECT_PRIORITY_LOW 0
-#define CIP_CONNECTION_OBJECT_PRIORITY_HIGH (1 << 10)
-#define CIP_CONNECTION_OBJECT_PRIORITY_SCHEDULED (1 << 11)
-#define CIP_CONNECTION_OBJECT_PRIORITY_URGENT (3 << 10)
+#define CIP_CONNECTION_OBJECT_PRIORITY_HIGH 1
+#define CIP_CONNECTION_OBJECT_PRIORITY_SCHEDULED 2
+#define CIP_CONNECTION_OBJECT_PRIORITY_URGENT 3
 
 /** @brief Definition of the global connection list */
 DoublyLinkedList connection_list;
@@ -163,17 +163,30 @@ void ConnectionObjectInitializeFromMessage(
 
   ConnectionObjectSetInitialInactivityWatchdogTimerValue(connection_object);
 
-  //TODO: introduce setter function
-  connection_object->o_to_t_network_connection_parameters = GetIntFromMessage(
-    message);
+  if (connection_object->is_large_forward_open == true) {
+    ConnectionObjectSetOToTNetworkConnectionParameters(connection_object,
+                                                       GetDintFromMessage(
+                                                         message) );
+  } else {
+    ConnectionObjectSetOToTNetworkConnectionParameters(connection_object,
+                                                       GetIntFromMessage(
+                                                         message) );
+  }
 
   ConnectionObjectSetTToORequestedPacketInterval(connection_object,
                                                  GetDintFromMessage(message) );
 
   ConnectionObjectSetExpectedPacketRate(connection_object);
 
-  connection_object->t_to_o_network_connection_parameters = GetIntFromMessage(
-    message);
+  if (connection_object->is_large_forward_open == true) {
+    ConnectionObjectSetTToONetworkConnectionParameters(connection_object,
+                                                       GetDintFromMessage(
+                                                         message) );
+  } else {
+    ConnectionObjectSetTToONetworkConnectionParameters(connection_object,
+                                                       GetIntFromMessage(
+                                                         message) );
+  }
 
   connection_object->transport_class_trigger = GetSintFromMessage(message);
 }
@@ -677,22 +690,56 @@ void ConnectionObjectSetTToORequestedPacketInterval(
     requested_packet_interval;
 }
 
+void ConnectionObjectSetTToONetworkConnectionParameters(
+  CipConnectionObject *connection_object,
+  const CipDword connection_parameters) {
+  connection_object->t_to_o_network_connection_parameters =
+    connection_parameters;
+}
+
+void ConnectionObjectSetOToTNetworkConnectionParameters(
+  CipConnectionObject *connection_object,
+  const CipDword connection_parameters) {
+  connection_object->o_to_t_network_connection_parameters =
+    connection_parameters;
+}
+
+bool ConnectionObjectIsRedundantOwner(
+  const CipDword connection_parameters,
+  const CipBool is_lfo) {
+  if (is_lfo) {
+    return (connection_parameters & (1 << 31) );
+  } else {
+    return (connection_parameters & (1 << 15) );
+  }
+}
+
 bool ConnectionObjectIsOToTRedundantOwner(
   const CipConnectionObject *const connection_object) {
-  const CipWord kOwnerMask = 0x80;
-  return kOwnerMask & connection_object->o_to_t_network_connection_parameters;
+  return ConnectionObjectIsRedundantOwner(
+    connection_object->o_to_t_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 bool ConnectionObjectIsTToORedundantOwner(
   const CipConnectionObject *const connection_object) {
-  const CipWord kOwnerMask = 0x80;
-  return kOwnerMask & connection_object->t_to_o_network_connection_parameters;
+  return ConnectionObjectIsRedundantOwner(
+    connection_object->t_to_o_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 ConnectionObjectConnectionType ConnectionObjectGetConnectionType(
-  const CipWord connection_parameters) {
-  const CipWord kConnectionTypeMask = 3 << 13;
-  switch(connection_parameters & kConnectionTypeMask) {
+  const CipDword connection_parameters,
+  const CipBool is_lfo) {
+
+  CipUsint connection_type;
+  if (is_lfo) {
+    connection_type = (connection_parameters & (3 << 29) ) >> 29;
+  } else {
+    connection_type = (connection_parameters & (3 << 13) ) >> 13;
+  }
+
+  switch(connection_type) {
     case CIP_CONNECTION_OBJECT_CONNECTION_TYPE_NULL: return
         kConnectionObjectConnectionTypeNull;
     case CIP_CONNECTION_OBJECT_CONNECTION_TYPE_MULTICAST: return
@@ -706,20 +753,30 @@ ConnectionObjectConnectionType ConnectionObjectGetConnectionType(
 ConnectionObjectConnectionType ConnectionObjectGetOToTConnectionType(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetConnectionType(
-    connection_object->o_to_t_network_connection_parameters);
+    connection_object->o_to_t_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 ConnectionObjectConnectionType ConnectionObjectGetTToOConnectionType(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetConnectionType(
-    connection_object->t_to_o_network_connection_parameters);
+    connection_object->t_to_o_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 ConnectionObjectPriority ConnectionObjectGetPriority(
-  const CipWord connection_parameters) {
-  const CipWord kPriorityMask = 3 << 10;
+  const CipDword connection_parameters,
+  const CipBool is_lfo) {
+
+  CipUsint priority;
+  if (is_lfo) {
+    priority = (connection_parameters & (3 << 26) ) >> 26;
+  } else {
+    priority = (connection_parameters & (3 << 10) ) >> 10;
+  }
+
   ConnectionObjectPriority result;
-  switch(connection_parameters & kPriorityMask) {
+  switch(priority) {
     case CIP_CONNECTION_OBJECT_PRIORITY_LOW: result =
       kConnectionObjectPriorityLow; break;
     case CIP_CONNECTION_OBJECT_PRIORITY_HIGH: result =
@@ -738,20 +795,30 @@ ConnectionObjectPriority ConnectionObjectGetPriority(
 ConnectionObjectPriority ConnectionObjectGetOToTPriority(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetPriority(
-    connection_object->o_to_t_network_connection_parameters);
+    connection_object->o_to_t_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 ConnectionObjectPriority ConnectionObjectGetTToOPriority(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetPriority(
-    connection_object->t_to_o_network_connection_parameters);
+    connection_object->t_to_o_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 
 ConnectionObjectConnectionSizeType ConnectionObjectGetConnectionSizeType(
-  const CipWord connection_parameters) {
-  const CipWord kConnectionSizeTypeMask = 1 << 9;
-  if(connection_parameters & kConnectionSizeTypeMask) {
+  const CipDword connection_parameters,
+  const CipBool is_lfo) {
+
+  bool connection_size_type;
+  if (is_lfo) {
+    connection_size_type = (connection_parameters & (1 << 25) );
+  } else {
+    connection_size_type = (connection_parameters & (1 << 9) );
+  }
+
+  if (connection_size_type) {
     return kConnectionObjectConnectionSizeTypeVariable;
   } else {
     return kConnectionObjectConnectionSizeTypeFixed;
@@ -761,30 +828,43 @@ ConnectionObjectConnectionSizeType ConnectionObjectGetConnectionSizeType(
 ConnectionObjectConnectionSizeType ConnectionObjectGetOToTConnectionSizeType(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetConnectionSizeType(
-    connection_object->o_to_t_network_connection_parameters);
+    connection_object->o_to_t_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 ConnectionObjectConnectionSizeType ConnectionObjectGetTToOConnectionSizeType(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetConnectionSizeType(
-    connection_object->t_to_o_network_connection_parameters);
+    connection_object->t_to_o_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
-size_t ConnectionObjectGetConnectionSize(const CipWord connection_parameters) {
-  const CipWord kConnectionSizeMask = 0x01FF;
-  return connection_parameters & kConnectionSizeMask;
+size_t ConnectionObjectGetConnectionSize(
+  const CipDword connection_parameters,
+  const CipBool is_lfo) {
+  const CipDword kConnectionSizeMask = 0x000001FF;
+  const CipDword kConnectionSizeMaskLFO = 0x0000FFFF;
+
+  CipDword mask = kConnectionSizeMask;
+  if (is_lfo) {
+    mask = kConnectionSizeMaskLFO;
+  }
+
+  return connection_parameters & mask;
 }
 
 size_t ConnectionObjectGetOToTConnectionSize(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetConnectionSize(
-    connection_object->o_to_t_network_connection_parameters);
+    connection_object->o_to_t_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 size_t ConnectionObjectGetTToOConnectionSize(
   const CipConnectionObject *const connection_object) {
   return ConnectionObjectGetConnectionSize(
-    connection_object->t_to_o_network_connection_parameters);
+    connection_object->t_to_o_network_connection_parameters,
+    connection_object->is_large_forward_open);
 }
 
 void ConnectionObjectDeepCopy(