Add support for early versions of CIP specification

http://read.pudn.com/downloads589/doc/2410232/CIP%20Vol1_3.3.pdf Page 95 Section 3-5.5.1.1 connection parameters are only 16 bits Also adds an option to not fully close the conncetion. This allows reconnection for slightly offspec devices.
ros-drivers · Sep 22, 2020 · e31f62d · e31f62d
1 parent a775b74
commit e31f62d
Show file tree

Hide file tree

Showing 5 changed files with 137 additions and 27 deletions.
diff --git a/include/odva_ethernetip/connection.h b/include/odva_ethernetip/connection.h
@@ -92,7 +92,17 @@ class Connection
   /**
    * Create the forward open request from the data in this connection object
    */
-  shared_ptr<ForwardOpenRequest> createForwardOpenRequest();
+  shared_ptr<ForwardOpenRequest> createForwardOpenRequest()
+  {
+    // Maintains backwards compatability
+    return createForwardOpenRequest(false);
+  }
+
+  /**
+   * Create the forward open request from the data in this connection object
+   * @param use_legacy_forward_open_request use 16 bit connection parameters instead of news 32 bit parameters
+   */
+  shared_ptr<ForwardOpenRequest> createForwardOpenRequest(bool use_legacy_forward_open_request);
 
   /**
    * Create a forward close request from the data in this connection object

diff --git a/include/odva_ethernetip/forward_open_request.h b/include/odva_ethernetip/forward_open_request.h
@@ -77,12 +77,26 @@ class ForwardOpenRequest : public Serializable
   EIP_USINT timeout_multiplyer;
   EIP_UDINT o_to_t_rpi;
   EIP_DWORD o_to_t_conn_params;
+  EIP_WORD o_to_t_conn_params_legacy;
   EIP_UDINT t_to_o_rpi;
   EIP_DWORD t_to_o_conn_params;
+  EIP_WORD t_to_o_conn_params_legacy;
   EIP_BYTE conn_type;
 
+  bool use_legacy_forward_open_request;
+
+  ForwardOpenRequest()
+    : use_legacy_forward_open_request(false)
+  {
+  }
+
+  ForwardOpenRequest(bool use_legacy_forward_open_request)
+    : use_legacy_forward_open_request(use_legacy_forward_open_request)
+  {
+  }
+
   /**
-   * Helper to calculate connection parameters
+   * Helper to calculate connection parameters for current 32 bit connection parameters
    * @param size Maximum size of the messages in the connection in byte
    * @param variable if set to true, variable message sizes
    * @param priority Priority value for the connection
@@ -96,13 +110,35 @@ class ForwardOpenRequest : public Serializable
       | (type & 0x03) << 29 | (shared ? 0x80000000 : 0);
   }
 
+  /**
+   * Helper to calculate connection parameters for legacy 16 bit connection parameters
+   * @param size Maximum size of the messages in the connection in byte
+   * @param variable if set to true, variable message sizes
+   * @param priority Priority value for the connection
+   * @param type Connection type / class info
+   * @param shared If set to true, then a shared connection
+   */
+  static EIP_WORD calcConnectionParamsLegacy(EIP_UINT size, bool variable, EIP_BYTE priority,
+    EIP_BYTE type, bool shared)
+  {
+    return (size & 0x1FF) | (variable ? 0x200 : 0) | (priority & 0x03) << 10
+      | (type & 0x03) << 13 | (shared ? 0x8000 : 0);
+  }
+
   /**
    * Shortcut to set the origin to target parameters.
    */
   EIP_DWORD setOriginToTargetParams(EIP_UINT size, bool variable, EIP_BYTE priority,
     EIP_BYTE type, bool shared)
   {
-    o_to_t_conn_params = calcConnectionParams(size, variable, priority, type, shared);
+    if (use_legacy_forward_open_request)
+    {
+      o_to_t_conn_params_legacy = calcConnectionParamsLegacy(size, variable, priority, type, shared);
+    }
+    else
+    {
+      o_to_t_conn_params = calcConnectionParams(size, variable, priority, type, shared);
+    }
     return 0;
   }
 
@@ -112,7 +148,14 @@ class ForwardOpenRequest : public Serializable
   EIP_DWORD setTargetToOriginParams(EIP_UINT size, bool variable, EIP_BYTE priority,
     EIP_BYTE type, bool shared)
   {
-    t_to_o_conn_params = calcConnectionParams(size, variable, priority, type, shared);
+    if (use_legacy_forward_open_request)
+    {
+      t_to_o_conn_params_legacy = calcConnectionParamsLegacy(size, variable, priority, type, shared);
+    }
+    else
+    {
+      t_to_o_conn_params = calcConnectionParams(size, variable, priority, type, shared);
+    }
     return 0;
   }
 
@@ -131,21 +174,36 @@ class ForwardOpenRequest : public Serializable
    */
   virtual size_t getLength() const
   {
-    return sizeof(timeout_tick_size)
-          + sizeof(timeout_ticks)
-          + sizeof(o_to_t_connection_id)
-          + sizeof(t_to_o_connection_id)
-          + sizeof(connection_sn)
-          + sizeof(originator_vendor_id)
-          + sizeof(originator_sn)
-          + sizeof(timeout_multiplyer)
-          + sizeof(o_to_t_rpi)
-          + sizeof(o_to_t_conn_params)
-          + sizeof(t_to_o_rpi)
-          + sizeof(t_to_o_conn_params)
-          + sizeof(conn_type)
-          + 3 // reserved bytes
-          + path_.getLength();
+    size_t ret = sizeof(timeout_tick_size);
+    ret += sizeof(timeout_ticks);
+    ret += sizeof(o_to_t_connection_id);
+    ret += sizeof(t_to_o_connection_id);
+    ret += sizeof(connection_sn);
+    ret += sizeof(originator_vendor_id);
+    ret += sizeof(originator_sn);
+    ret += sizeof(timeout_multiplyer);
+    ret += sizeof(o_to_t_rpi);
+    if (use_legacy_forward_open_request)
+    {
+      ret += sizeof(o_to_t_conn_params_legacy);
+    }
+    else
+    {
+      ret += sizeof(o_to_t_conn_params);
+    }
+    ret += sizeof(t_to_o_rpi);
+    if (use_legacy_forward_open_request)
+    {
+      ret += sizeof(t_to_o_conn_params_legacy);
+    }
+    else
+    {
+      ret += sizeof(t_to_o_conn_params);
+    }
+    ret += sizeof(conn_type);
+    ret += 3; // reserved bytes
+    ret += path_.getLength();
+    return ret;
   }
 
   /**
@@ -169,9 +227,23 @@ class ForwardOpenRequest : public Serializable
     writer.write(reserved);
     writer.write(reserved);
     writer.write(o_to_t_rpi);
-    writer.write(o_to_t_conn_params);
+    if (use_legacy_forward_open_request)
+    {
+      writer.write(o_to_t_conn_params_legacy);
+    }
+    else
+    {
+      writer.write(o_to_t_conn_params);
+    }
     writer.write(t_to_o_rpi);
-    writer.write(t_to_o_conn_params);
+    if (use_legacy_forward_open_request)
+    {
+      writer.write(t_to_o_conn_params_legacy);
+    }
+    else
+    {
+      writer.write(t_to_o_conn_params);
+    }
     writer.write(conn_type);
     path_.serialize(writer);
     return writer;

diff --git a/include/odva_ethernetip/session.h b/include/odva_ethernetip/session.h
@@ -87,6 +87,11 @@ class Session
    */
   void close();
 
+  /**
+   * Close the session without unregistering the session and just closing the port
+   */
+  void closeWithoutUnregister();
+
   /**
    * Get the ID number assigned to this session by the target
    * @return session ID number
@@ -153,7 +158,23 @@ class Session
    * @param t_to_o Target to origin connection info
    */
   int createConnection(const EIP_CONNECTION_INFO_T& o_to_t,
-    const EIP_CONNECTION_INFO_T& t_to_o);
+    const EIP_CONNECTION_INFO_T& t_to_o)
+  {
+    // Maintains backwards compatability
+    return createConnection(o_to_t, t_to_o, 0x5B, false);
+  }
+
+  /**
+   * Create an Ethernet/IP Connection for sending implicit messages
+   * @param o_to_t Origin to target connection info
+   * @param t_to_o Target to origin connection info
+   * @param service Service code to send
+   * @param use_legacy_forward_open_request use 16 bit connection parameters instead of news 32 bit parameters
+   */
+  int createConnection(const EIP_CONNECTION_INFO_T& o_to_t,
+    const EIP_CONNECTION_INFO_T& t_to_o,
+    EIP_USINT service,
+    bool use_legacy_forward_open_request);
 
   /**
    * Close the given connection number

diff --git a/src/connection.cpp b/src/connection.cpp
@@ -56,9 +56,9 @@ void Connection::setConnectionPoints(EIP_USINT origin, EIP_USINT target)
   path_.addLogicalConnectionPoint(target);
 }
 
-shared_ptr<ForwardOpenRequest> Connection::createForwardOpenRequest()
+shared_ptr<ForwardOpenRequest> Connection::createForwardOpenRequest(bool use_legacy_forward_open_request)
 {
-  shared_ptr<ForwardOpenRequest> req = make_shared<ForwardOpenRequest> ();
+  shared_ptr<ForwardOpenRequest> req = make_shared<ForwardOpenRequest> (use_legacy_forward_open_request);
 
   req->originator_vendor_id = originator_vendor_id;
   req->originator_sn = originator_sn;

diff --git a/src/session.cpp b/src/session.cpp
@@ -158,6 +158,11 @@ void Session::close()
 
   CONSOLE_BRIDGE_logInform("Session closed");
 
+  closeWithoutUnregister();
+}
+
+void Session::closeWithoutUnregister()
+{
   socket_->close();
   io_socket_->close();
   session_id_ = 0;
@@ -294,7 +299,9 @@ RRDataResponse Session::sendRRDataCommand(EIP_USINT service, const Path& path,
 }
 
 int Session::createConnection(const EIP_CONNECTION_INFO_T& o_to_t,
-  const EIP_CONNECTION_INFO_T& t_to_o)
+  const EIP_CONNECTION_INFO_T& t_to_o,
+  EIP_USINT service,
+  bool use_legacy_forward_open_request)
 {
   Connection conn(o_to_t, t_to_o);
   conn.originator_vendor_id = my_vendor_id_;
@@ -303,8 +310,8 @@ int Session::createConnection(const EIP_CONNECTION_INFO_T& o_to_t,
   conn.o_to_t_connection_id = next_connection_id_++;
   conn.t_to_o_connection_id = next_connection_id_++;
 
-  shared_ptr<ForwardOpenRequest> req = conn.createForwardOpenRequest();
-  RRDataResponse resp_data = sendRRDataCommand(0x5B, Path(0x06, 1), req);
+  shared_ptr<ForwardOpenRequest> req = conn.createForwardOpenRequest(use_legacy_forward_open_request);
+  RRDataResponse resp_data = sendRRDataCommand(service, Path(0x06, 1), req);
   ForwardOpenSuccess result;
   resp_data.getResponseDataAs(result);
   if (!conn.verifyForwardOpenResult(result))