Fixes #157 - Enables that OpENer sends UDP from correct source port

This fix implements that I/O producing connections now send via the
correct source port, utilizing raw sockets. This also changes, that
currently OpENer needs to be run as priviledged process or
CAP_NET_RAW to operate

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

source/src/ports/CMakeLists.txt

@@ -14,6 +14,6 @@ opener_common_includes()
 #######################################
 opener_platform_support("INCLUDES")
 
-set( PLATFORM_GENERIC_SRC generic_networkhandler.c socket_timer.c )
+set( PLATFORM_GENERIC_SRC generic_networkhandler.c socket_timer.c  udp_protocol.c)
 
 add_library( PLATFORM_GENERIC ${PLATFORM_GENERIC_SRC})

source/src/ports/generic_networkhandler.c

@@ -23,9 +23,12 @@
 #include "ciptcpipinterface.h"
 #include "opener_user_conf.h"
 #include "cipqos.h"
+#include "udp_protocol.h"
 
 #define MAX_NO_OF_TCP_SOCKETS 10
 
+extern CipTcpIpNetworkInterfaceConfiguration interface_configuration_;
+
 /** @brief handle any connection request coming in the TCP server socket.
  *
  */
@@ -302,7 +305,7 @@ void CheckAndHandleTcpListenerSocket(void) {
       return;
     }
 
-    if (SetQosOnSocket( new_socket, GetPriorityForSocket(0xFFF) ) <= 0) { /* got error */
+    if (SetQosOnSocket( new_socket, GetPriorityForSocket(0xFFF) ) != 0) { /* got error */
       int error_code = GetSocketErrorNumber();
       char *error_message = GetErrorMessage(error_code);
       OPENER_TRACE_ERR(
@@ -551,8 +554,23 @@ EipStatus SendUdpData(struct sockaddr_in *address,
                       EipUint8 *data,
                       EipUint16 data_length) {
 
+
+
   OPENER_TRACE_INFO("UDP port to be sent to: %x\n", ntohs(address->sin_port) );
-  int sent_length = sendto( socket, (char *) data, data_length, 0,
+  UDPHeader header = {
+    .source_port = 2222,
+    .destination_port = ntohs(address->sin_port),
+    .packet_length = kUpdHeaderLength + data_length,
+    .checksum = 0
+  };
+
+  char complete_message[data_length + kUpdHeaderLength];
+  memcpy(complete_message + kUpdHeaderLength, data, data_length);
+  UDPHeaderGenerate(&header, (char*)complete_message);
+  UDPHeaderSetChecksum(&header, htons(UDPHeaderCalculateChecksum(complete_message, 8+data_length, interface_configuration_.ip_address, address->sin_addr.s_addr)));
+  UDPHeaderGenerate(&header, (char*)complete_message);
+
+  int sent_length = sendto( socket, (char *) complete_message, data_length + kUpdHeaderLength, 0,
                             (struct sockaddr *) address, sizeof(*address) );
 
   if (sent_length < 0) {
@@ -566,7 +584,7 @@ EipStatus SendUdpData(struct sockaddr_in *address,
     return kEipStatusError;
   }
 
-  if (sent_length != data_length) {
+  if (sent_length != data_length + kUpdHeaderLength) {
     OPENER_TRACE_WARN(
       "data length sent_length mismatch; probably not all data was sent in SendUdpData, sent %d of %d\n",
       sent_length,
@@ -791,7 +809,7 @@ int CreateUdpSocket(UdpCommuncationDirection communication_direction,
   }
 
   if(kUdpCommuncationDirectionProducing == communication_direction) {
-    new_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+    new_socket = socket(AF_INET, SOCK_RAW, IPPROTO_UDP);
   }
 
   if (new_socket == kEipInvalidSocket) {
@@ -856,17 +874,6 @@ int CreateUdpSocket(UdpCommuncationDirection communication_direction,
                 (char *) &option_value,
                 sizeof(option_value) );
 
-    struct sockaddr_in source_addr = {
-      .sin_addr = INADDR_ANY,
-      .sin_family = AF_INET,
-      .sin_port = htons(0x08ae)
-    };
-
-    memset(source_addr.sin_zero, 0, 8 * sizeof(CipUsint) );
-
-    // The bind on UDP sockets is necessary as the ENIP spec wants the source port to be specified to 2222 = 0x08ae
-    bind(new_socket, (struct sockaddr *) &source_addr, sizeof(source_addr) );
-
     if (socket_data->sin_addr.s_addr
         == g_multicast_configuration.starting_multicast_address) {
       if (1 != g_time_to_live_value) { /* we need to set a TTL value for the socket */

source/src/ports/udp_protocol.c

@@ -0,0 +1,88 @@
+/*******************************************************************************
+ * Copyright (c) 2018, Rockwell Automation, Inc.
+ * All rights reserved.
+ *
+ ******************************************************************************/
+
+#include "udp_protocol.h"
+
+void UDPHeaderSetSourcePort(UDPHeader *const header, const uint16_t source_port) {
+  header->source_port = source_port;
+}
+
+uint16_t UDPHeaderGetSourcePort(const UDPHeader *const header) {
+  return header->source_port;
+}
+
+void UDPHeaderSetDestinationPort(UDPHeader *const header, const uint16_t destination_port) {
+  header->destination_port = destination_port;
+}
+
+uint16_t UDPHeaderGetDestinationPort(const UDPHeader *const header) {
+  return header->destination_port;
+}
+
+void UDPHeaderSetPacketLength(UDPHeader *const header, const uint16_t packet_length) {
+  header->packet_length = packet_length;
+}
+
+uint16_t UDPHeaderGetPacketLength(const UDPHeader *const header) {
+  return header->packet_length;
+}
+
+void UDPHeaderSetChecksum(UDPHeader *const header, const uint16_t checksum){
+  header->checksum = checksum;
+}
+
+uint16_t UDPHeaderGetChecksum(const UDPHeader *const header){
+  return header->checksum;
+}
+
+void UDPHeaderGenerate(const UDPHeader *header, char* message) {
+  *((uint16_t*)message) = htons(UDPHeaderGetSourcePort(header));
+  message += 2;
+  *((uint16_t*)message) = htons(UDPHeaderGetDestinationPort(header));
+  message += 2;
+  *((uint16_t*)message) = htons(UDPHeaderGetPacketLength(header));
+  message += 2;
+  *((uint16_t*)message) = htons(UDPHeaderGetChecksum(header));
+  message += 2;
+}
+
+uint16_t UDPHeaderCalculateChecksum(const void *udp_packet, const size_t udp_packet_length, const in_addr_t source_addr, const in_addr_t destination_addr) {
+  const uint16_t *udp_packet_words = udp_packet;
+  uint32_t checksum = 0;
+  size_t length = udp_packet_length;
+
+  // Process UDP packet
+  while(length > 1) {
+    checksum += *udp_packet_words++;
+    if(checksum & 0x8000000) {
+      checksum = (checksum & 0xFFFF) + (checksum >> 16);
+    }
+    length -= 2;
+  }
+
+  if(0 != length % 2) {
+    // Add padding if packet length is odd
+    checksum += *((uint8_t*)udp_packet_words);
+  }
+
+  //Process IP pseudo header
+  uint16_t *source_addr_as_words = (void*)&source_addr;
+  checksum += *source_addr_as_words + *(source_addr_as_words + 1);
+
+  uint16_t *destination_addr_as_words = (void*)&destination_addr;
+  checksum += *destination_addr_as_words + *(destination_addr_as_words + 1);
+
+  checksum += htons(IPPROTO_UDP);
+  checksum += htons(udp_packet_length);
+
+  //Add the carries
+  while(0 != checksum >> 16) {
+    checksum = (checksum & 0xFFFF) + (checksum >> 16);
+  }
+
+  // Return one's complement
+  return (uint16_t)(~checksum);
+}

source/src/ports/udp_protocol.h

@@ -0,0 +1,112 @@
+/*******************************************************************************
+ * Copyright (c) 2018, Rockwell Automation, Inc.
+ * All rights reserved.
+ *
+ ******************************************************************************/
+
+/** @file udp_protocol.h
+ *  @author Martin Melik Merkumians
+ *  @brief Includes a basic set of operations for UDP header creation and checksum calculation
+ *
+ *  In order to send UDP packets from a specified source port, the UDP header creation has to
+ *  be done by hand. This file specifies the interface for OpENer's UDP header management,
+ *  creation, and checksum calculation
+ */
+
+#ifndef SRC_PORTS_UDP_PROTOCOL_H_
+#define SRC_PORTS_UDP_PROTOCOL_H_
+
+#include <stdint.h>
+#include <stddef.h>
+#include <netinet/in.h>
+
+static size_t kUpdHeaderLength = 8U; /**< UDP header length in bytes */
+
+/** @brief Representing the needed information for the UDP header
+ *
+ * This struct represents the UDP header information
+ */
+typedef struct {
+    uint16_t source_port; /**< UDP source port */
+    uint16_t destination_port; /**< UDP destination port */
+    uint16_t packet_length; /**< UDP packet length (data + header) */
+    uint16_t checksum; /**< UDP checksum */
+} UDPHeader;
+
+/** @brief Sets source port field
+ *
+ * @param header The UDP header struct instance
+ * @param source_port Source port value to be set
+ */
+void UDPHeaderSetSourcePort(UDPHeader *const header, const uint16_t source_port);
+
+/** @brief Gets source port field
+ *
+ * @param header The header struct instance
+ * @return The source port
+ */
+uint16_t UDPHeaderGetSourcePort(const UDPHeader *const header);
+
+/** @brief Sets destination port field
+ *
+ * @param header The UDP header struct instance
+ * @param destination_port Destination port value to be set
+ */
+void UDPHeaderSetDestinationPort(UDPHeader *const header, const uint16_t destination_port);
+
+/** @brief Gets destination port field
+ *
+ * @param header The header struct instance
+ * @return The destination port
+ */
+uint16_t UDPHeaderGetDestinationPort(const UDPHeader *const header);
+
+/** @brief Sets packet length field
+ *
+ * @param header The UDP header struct instance
+ * @param packet_length Length value to be set
+ */
+void UDPHeaderSetPacketLength(UDPHeader *const header, const uint16_t packet_length);
+
+/** @brief Gets packet length field
+ *
+ * @param header The header struct instance
+ * @return The packet length
+ */
+uint16_t UDPHeaderGetPacketLength(const UDPHeader *const header);
+
+/** @brief Sets checksum field
+ *
+ * @param header The UDP header struct instance
+ * @param checksum Checksum value to be set
+ */
+void UDPHeaderSetChecksum(UDPHeader *const header, const uint16_t checksum);
+
+/** @brief Gets checksum field
+ *
+ * @param header The UDP header struct instance
+ * @return The packet length
+ */
+uint16_t UDPHeaderGetChecksum(const UDPHeader *const header);
+
+/** @brief Calculates the checksum based on the set UDP packet data and pseudo IP header
+ *
+ * @param udp_packet the UDP packet including the UDP header
+ * @param udp_packet_length UPD packet length
+ * @param source_addr The IP source address
+ * @param destination_addr The IP destination address
+ * @return The calculated checksum
+ */
+uint16_t UDPHeaderCalculateChecksum(const void *udp_packet, const size_t udp_packet_length, const in_addr_t source_addr, const in_addr_t destination_addr);
+
+/** @brief Generate the UDP header in the message according to the header
+ *
+ * The function generates the UDP header according to the header struct
+ * overwriting the first 8 bytes
+ *
+ * @param header The UDP header struct instance
+ * @param message The message buffer
+ */
+void UDPHeaderGenerate(const UDPHeader *header, char* message);
+
+#endif /* SRC_PORTS_UDP_PROTOCOL_H_ */