Fixes most encap errors

Signed-off-by: Martin Melik Merkumians <melik-merkumians@acin.tuwien.ac.at>

source/src/cip/cipmessagerouter.c

@@ -172,7 +172,7 @@ EipStatus RegisterCipClass(CipClass *cip_class) {
 
 EipStatus NotifyMessageRouter(EipUint8 *data,
                               int data_length,
-                              struct sockaddr *originator_address,
+                              const struct sockaddr *const originator_address,
                               const int encapsulation_session) {
   EipStatus eip_status = kEipStatusOkSend;
   EipByte status = kCipErrorSuccess;

source/src/cip/cipmessagerouter.h

@@ -40,7 +40,7 @@ void DeleteAllClasses(void);
  */
 EipStatus NotifyMessageRouter(EipUint8 *data,
                               int data_length,
-                              struct sockaddr *originator_address,
+                              const struct sockaddr *const originator_address,
                               const int encapsulation_session);
 
 /*! Register a class at the message router.

source/src/enet_encap/cpf.c

@@ -66,7 +66,7 @@ int NotifyCommonPacketFormat(EncapsulationData *const receive_data,
 
 int NotifyConnectedCommonPacketFormat(
   const EncapsulationData *const received_data,
-  struct sockaddr *originator_address,
+  const struct sockaddr *const originator_address,
   ENIPMessage *const outgoing_message) {
 
   int return_value = CreateCommonPacketFormatStructure(
@@ -87,7 +87,7 @@ int NotifyConnectedCommonPacketFormat(
         /* reset the watchdog timer */
         ConnectionObjectResetInactivityWatchdogTimerValue(connection_object);
 
-        /*TODO check connection id  and sequence count    */
+        /*TODO check connection id  and sequence count */
         if (g_common_packet_format_data_item.data_item.type_id
             == kCipItemIdConnectedDataItem) { /* connected data item received*/
           EipUint8 *buffer = g_common_packet_format_data_item.data_item.data;

source/src/enet_encap/cpf.h

@@ -94,7 +94,7 @@ int NotifyCommonPacketFormat(EncapsulationData *const received_data,
  */
 int NotifyConnectedCommonPacketFormat(
   const EncapsulationData *const received_data,
-  struct sockaddr *originator_address,
+  const struct sockaddr *const originator_address,
   ENIPMessage *const outgoing_message);
 
 /** @ingroup ENCAP

source/src/enet_encap/encap.c

@@ -89,14 +89,6 @@ DelayedEncapsulationMessage g_delayed_encapsulation_messages[
   ENCAP_NUMBER_OF_SUPPORTED_DELAYED_ENCAP_MESSAGES];
 
 /*** private functions ***/
-void HandleReceivedListServicesCommand(
-  const EncapsulationData *const receive_data,
-  ENIPMessage *const outgoing_message);
-
-void HandleReceivedListInterfacesCommand(
-  const EncapsulationData *const receive_data,
-  ENIPMessage *const outgoing_message);
-
 void HandleReceivedListIdentityCommandTcp(
   const EncapsulationData *const receive_data,
   ENIPMessage *const outgoing_message);
@@ -106,10 +98,6 @@ void HandleReceivedListIdentityCommandUdp(const int socket,
                                           const EncapsulationData *const receive_data,
                                           ENIPMessage *const outgoing_message);
 
-void HandleReceivedRegisterSessionCommand(int socket,
-                                          const EncapsulationData *const receive_data,
-                                          ENIPMessage *const outgoing_message);
-
 EipStatus HandleReceivedUnregisterSessionCommand(
   const EncapsulationData *const receive_data,
   ENIPMessage *const outgoing_message);
@@ -133,8 +121,6 @@ int GetFreeSessionIndex(void);
 SessionStatus CheckRegisteredSessions(
   const EncapsulationData *const receive_data);
 
-int EncapsulateData(const EncapsulationData *const send_data);
-
 void DetermineDelayTime(const EipByte *const buffer_start,
                         DelayedEncapsulationMessage *const delayed_message_buffer);
 
@@ -256,7 +242,7 @@ int HandleReceivedExplictTcpData(int socket,
     }
   }
 
-  return return_value;
+  return outgoing_message->used_message_length;
 }
 
 int HandleReceivedExplictUdpData(const int socket,
@@ -318,6 +304,7 @@ int HandleReceivedExplictUdpData(const int socket,
         case (kEncapsulationCommandSendUnitData):
         default:
           OPENER_TRACE_INFO("No command\n");
+          //TODO: Check this
           encapsulation_data.status =
             kEncapsulationProtocolInvalidCommand;
           encapsulation_data.data_length = 0;
@@ -333,19 +320,6 @@ int HandleReceivedExplictUdpData(const int socket,
   return outgoing_message->used_message_length;
 }
 
-/* Probably not needed with new approach */
-int EncapsulateData(const EncapsulationData *const send_data) {
-  CipOctet *communcation_buffer = send_data->communication_buffer_start + 2;
-  AddIntToMessage(send_data->data_length, &communcation_buffer);
-  /*the CommBuf should already contain the correct session handle*/
-  MoveMessageNOctets(4, (const CipOctet **) &communcation_buffer);
-  AddDintToMessage(send_data->status, &communcation_buffer);
-  /*the CommBuf should already contain the correct sender context*/
-  /*the CommBuf should already contain the correct  options value*/
-
-  return ENCAPSULATION_HEADER_LENGTH + send_data->data_length;
-}
-
 void GenerateEncapsulationHeader(const EncapsulationData *const receive_data,
                                  const size_t command_specific_data_length,
                                  const size_t session_handle,
@@ -402,9 +376,11 @@ void HandleReceivedListServicesCommand(
   outgoing_message->used_message_length += AddIntToMessage(
     g_interface_information.capability_flags,
     &outgoing_message->current_message_position);
-  memcpy(&outgoing_message->current_message_position,
+  memcpy(outgoing_message->current_message_position,
          g_interface_information.name_of_service,
          sizeof(g_interface_information.name_of_service) );
+  outgoing_message->used_message_length +=
+    sizeof(g_interface_information.name_of_service);
 }
 
 void HandleReceivedListInterfacesCommand(
@@ -494,15 +470,6 @@ void EncapsulateListIdentityResponseMessage(
     kItemIDCipIdentity,
     &outgoing_message->current_message_position);
 
-
-  EipByte *id_length_buffer = outgoing_message->current_message_position;
-//  outgoing_message->used_message_length += MoveMessageNOctets(2,
-//                                                              (const CipOctet **) (
-//                                                                &
-//                                                                outgoing_message
-//                                                                ->
-//                                                                current_message_position) ); /*at this place the real length will be inserted below*/
-
   outgoing_message->used_message_length += AddIntToMessage(
     kEncapsulationCommandListIdentityCommandSpecificLength,
     &outgoing_message->current_message_position);
@@ -546,9 +513,6 @@ void EncapsulateListIdentityResponseMessage(
   *outgoing_message->current_message_position++ = 0xFF;
   outgoing_message->used_message_length++;
 
-//  outgoing_message->used_message_length += AddIntToMessage(
-//    outgoing_message->current_message_position - id_length_buffer - 2,
-//    &id_length_buffer);                     /* the -2 is for not counting the length field*/
 }
 
 void DetermineDelayTime(const EipByte *const buffer_start,
@@ -580,7 +544,7 @@ void EncapsulateRegisterSessionCommandResponseMessage(
   GenerateEncapsulationHeader(receive_data,
                               kListInterfacesCommandSpecificDataLength,
                               session_handle,
-                              kEncapsulationProtocolSuccess,
+                              encapsulation_protocol_status,
                               outgoing_message);
 
   outgoing_message->used_message_length += AddIntToMessage(1,
@@ -602,7 +566,6 @@ void HandleReceivedRegisterSessionCommand(int socket,
   EncapsulationProtocolErrorCode encapsulation_protocol_status =
     kEncapsulationProtocolSuccess;
 
-  const EipUint8 *receive_data_buffer = NULL;
   EipUint16 protocol_version =
     GetIntFromMessage(
       (const EipUint8 **const ) &receive_data->current_communication_buffer_position);
@@ -610,7 +573,6 @@ void HandleReceivedRegisterSessionCommand(int socket,
     GetIntFromMessage(
       (const EipUint8 **const ) &receive_data->current_communication_buffer_position);
 
-
   /* check if requested protocol version is supported and the register session option flag is zero*/
   if ( (0 < protocol_version)
        && (protocol_version <= kSupportedProtocolVersion)
@@ -664,9 +626,7 @@ void HandleReceivedRegisterSessionCommand(int socket,
 EipStatus HandleReceivedUnregisterSessionCommand(
   const EncapsulationData *const receive_data,
   ENIPMessage *const outgoing_message) {
-
   OPENER_TRACE_INFO("encap.c: Unregister Session Command\n");
-
   if ( (0 < receive_data->session_handle) && (receive_data->session_handle <=
                                               OPENER_NUMBER_OF_SUPPORTED_SESSIONS) )
   {

source/src/enet_encap/encap.h

@@ -95,6 +95,7 @@ void CloseClass3ConnectionBasedOnSession(size_t encapsulation_session_handle);
 
 /* No reason to use this functions outside the encapsulation layer, they are here for testing */
 typedef struct enip_message ENIPMessage;
+
 void EncapsulateListIdentityResponseMessage(
   const EncapsulationData *const receive_data,
   ENIPMessage *const outgoing_message);
@@ -103,4 +104,16 @@ EipInt16 CreateEncapsulationStructure(const EipUint8 *receive_buffer,
                                       int receive_buffer_length,
                                       EncapsulationData *const encapsulation_data);
 
+void HandleReceivedListServicesCommand(
+  const EncapsulationData *const receive_data,
+  ENIPMessage *const outgoing_message);
+
+void HandleReceivedListInterfacesCommand(
+  const EncapsulationData *const receive_data,
+  ENIPMessage *const outgoing_message);
+
+void HandleReceivedRegisterSessionCommand(int socket,
+                                          const EncapsulationData *const receive_data,
+                                          ENIPMessage *const outgoing_message);
+
 #endif /* OPENER_ENCAP_H_ */

source/tests/enet_encap/encaptest.cpp

@@ -35,10 +35,10 @@ TEST(EncapsulationProtocol, AnswerListIdentityRequest) {
     "\x01\x00\x0c\x00\xe9\xfd\x02\x01\x00\x00\x15\xcd\x5b\x07\x09\x4f" \
     "\x70\x45\x4e\x65\x72\x20\x50\x43\xff";
 
-  EncapsulationData receive_data;
   ENIPMessage outgoing_message;
   InitializeENIPMessage(&outgoing_message);
 
+  EncapsulationData receive_data;
   CreateEncapsulationStructure(incoming_message,
                                sizeof(incoming_message),
                                &receive_data);
@@ -46,3 +46,60 @@ TEST(EncapsulationProtocol, AnswerListIdentityRequest) {
   EncapsulateListIdentityResponseMessage(&receive_data, &outgoing_message);
 
 }
+
+TEST(EncapsulationProtocol, AnswerListServicesRequest) {
+  CipOctet incoming_message[] =
+    "\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xe0\xdd\x00\x00" \
+    "\x00\x00\x00\x00\x00\x00\x00\x00";
+
+  CipOctet expected_outgoing_message[] =
+    "\x04\x00\x1a\x00\x00\x00\x00\x00\x00\x00\x00\x00\xe0\xdd\x00\x00" \
+    "\x00\x00\x00\x00\x00\x00\x00\x00";
+
+  ENIPMessage outgoing_message;
+  InitializeENIPMessage(&outgoing_message);
+
+  EncapsulationData recieved_data = {0};
+  CreateEncapsulationStructure(incoming_message,
+                               sizeof(incoming_message),
+                               &recieved_data);
+
+  HandleReceivedListServicesCommand(&recieved_data, &outgoing_message);
+
+}
+
+TEST(EncapsulationProtocol, AnswerListInterfacesRequest) {
+  CipOctet incoming_message[] = "";
+
+  CipOctet expected_outgoing_message[] = "";
+
+  ENIPMessage outgoing_message;
+  InitializeENIPMessage(&outgoing_message);
+
+  EncapsulationData received_data = {0};
+  CreateEncapsulationStructure(incoming_message,
+                               sizeof(incoming_message),
+                               &received_data);
+
+  HandleReceivedListInterfacesCommand(&received_data, &outgoing_message);
+}
+
+TEST(EncapsulationProtocol, AnswerRegisterSessionRequestWrongProtocolVersion) {
+
+  CipOctet incoming_message[] =
+    "\x65\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x67\x88\x00\x00" \
+    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
+
+  CipOctet expected_outgoing_message[] = "";
+
+  ENIPMessage outgoing_message;
+  InitializeENIPMessage(&outgoing_message);
+
+  EncapsulationData received_data = {0};
+  CreateEncapsulationStructure(incoming_message,
+                               sizeof(incoming_message),
+                               &received_data);
+
+  HandleReceivedRegisterSessionCommand(0, &received_data, &outgoing_message);
+
+}