Add Large Forward Open service

Signed-off-by: Vincent Prince <vincent.prince.fr@gmail.com>

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,7 @@ 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 +480,21 @@ static const HandleForwardOpenRequestFunction
     HandleNullMatchingForwardOpenRequest
   } };
 
+/** @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 ForwardOpen(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
@@ -768,7 +792,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 +1110,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,26 @@ 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 +686,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 +749,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 +791,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 +824,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(

source/src/cip/cipconnectionobject.h

@@ -145,10 +145,10 @@ struct cip_connection_object {
   CipUdint originator_serial_number;
 
   CipUdint o_to_t_requested_packet_interval;
-  CipWord o_to_t_network_connection_parameters;
+  CipDword o_to_t_network_connection_parameters;
 
   CipUdint t_to_o_requested_packet_interval;
-  CipWord t_to_o_network_connection_parameters;
+  CipDword t_to_o_network_connection_parameters;
 
   CipUint sequence_count_producing; /**< sequence Count for Class 1 Producing
                                          Connections */
@@ -189,6 +189,7 @@ struct cip_connection_object {
   ConnectionReceiveDataFunction connection_receive_data_function;
 
   ENIPMessage last_reply_sent;
+  CipBool is_large_forward_open;
 };
 
 /** @brief Extern declaration of the global connection list */
@@ -388,6 +389,14 @@ void ConnectionObjectSetTToORequestedPacketInterval(
   CipConnectionObject *connection_object,
   const CipUdint requested_packet_interval);
 
+void ConnectionObjectSetTToONetworkConnectionParameters(
+  CipConnectionObject *connection_object,
+  const CipDword connection_parameters);
+
+void ConnectionObjectSetOToTNetworkConnectionParameters(
+  CipConnectionObject *connection_object,
+  const CipDword connection_parameters);
+
 bool ConnectionObjectIsTToORedundantOwner(
   const CipConnectionObject *const connection_object);
 

source/src/cip/ciptypes.h

@@ -96,6 +96,7 @@ typedef enum {
   /* Start CIP object-specific services */
   kEthLinkGetAndClear = 0x4C, /**< Ethernet Link object's Get_And_Clear service */
   kForwardOpen = 0x54,
+  kLargeForwardOpen = 0x5B,
   kForwardClose = 0x4E,
   kUnconnectedSend = 0x52,
   kGetConnectionOwner = 0x5A

source/src/ports/POSIX/sample_application/opener_user_conf.h

@@ -220,6 +220,6 @@ static const MilliSeconds kOpenerTimerTickInMilliSeconds = 10;
  *  This buffer size will be used for any received message.
  *  The same buffer is used for the replied explicit message.
  */
-#define PC_OPENER_ETHERNET_BUFFER_SIZE 512
+#define PC_OPENER_ETHERNET_BUFFER_SIZE 8192
 
 #endif /*OPENER_USER_CONF_H_*/