From Roland Knall:

openSAFETY - Names for certain SOD objects, automatic SCM UDID detecion.

https://bugs.wireshark.org/bugzilla/show_bug.cgi?id=7335

svn path=/trunk/; revision=43109

1 files changed, 108 insertions, 18 deletions

diff --git a/epan/dissectors/packet-opensafety.c b/epan/dissectors/packet-opensafety.c
index 903f5d24b6..0e47a99948 100644
--- a/epan/dissectors/packet-opensafety.c
+++ b/epan/dissectors/packet-opensafety.c
@@ -254,6 +254,24 @@ static const true_false_string opensafety_sacmd_blk  = { "Block Transfer", "Norm
 
 #define OPENSAFETY_SPDO_CONNECTION_VALID  0x04
 
+static const value_string sod_idx_names[] = {
+        /* SSDO dictionary names, only names that are in common use are presented */
+
+        { 0x10180000,  "Device Vendor Information" },
+        { 0x10180001,  "VendorID" },
+        { 0x10180002,  "ProductCode" },
+        { 0x10180003,  "RevisionNumber" },
+        { 0x10180004,  "SerialNumber" },
+        { 0x10180005,  "FirmWareChecksum" },
+        { 0x10180006,  "Parameter Checksum" },
+        { 0x10180007,  "Parameter Timestamp" },
+
+        { 0x10190000,  "Unique Device ID" },
+        { 0x101A0000,  "Parameter Download" },
+        { 0x101B0000,  "SCM Parameters" },
+
+        { 0, NULL }
+};
 
 static const value_string abort_codes[] = {
 
@@ -378,6 +396,7 @@ static int hf_oss_ssdo_sacmd_end_segment     = -1;
 static int hf_oss_ssdo_sacmd_block_transfer  = -1;
 
 static int hf_oss_scm_udid           = -1;
+static int hf_oss_scm_udid_auto      = -1;
 static int hf_oss_scm_udid_valid     = -1;
 
 static int hf_oss_spdo_connection_valid   = -1;
@@ -390,12 +409,16 @@ static int hf_oss_spdo_time_request_to    = -1;
 static int hf_oss_spdo_time_request_from  = -1;
 
 static const char *global_scm_udid = "00:00:00:00:00:00";
+static gboolean global_scm_udid_autoset = TRUE;
 static gboolean global_udp_frame2_first = FALSE;
 static gboolean global_mbtcp_big_endian = FALSE;
 static guint global_network_udp_port = UDP_PORT_OPENSAFETY;
 static guint global_network_udp_port_sercosiii = UDP_PORT_SIII;
 
 static gboolean bDissector_Called_Once_Before = FALSE;
+/* Using local_scm_udid as read variable for global_scm_udid, to
+ * enable automatic detection of scm udid */
+static char *local_scm_udid = NULL;
 
 /* Resets the dissector in case the dissection is malformed and the dissector crashes */
 static void
@@ -404,6 +427,13 @@ reset_dissector(void)
     bDissector_Called_Once_Before = FALSE;
 }
 
+static void
+setup_dissector(void)
+{
+    if ( local_scm_udid != NULL )
+        local_scm_udid = NULL;
+}
+
 void proto_register_opensafety(void);
 void proto_reg_handoff_opensafety(void);
 
@@ -737,8 +767,8 @@ dissect_opensafety_ssdo_message(tvbuff_t *message_tvb , packet_info *pinfo, prot
 {
     proto_item *item;
     proto_tree *ssdo_tree, *ssdo_sacmd_tree;
-    guint16     taddr = 0    ;
-    guint32     abortcode;
+    guint16     taddr = 0, sdn = 0;
+    guint32     abortcode, ssdoIndex, ssdoSubIndex;
     guint8      db0Offset, db0, sacmd, payloadOffset, payloadSize, n;
     guint       dataLength;
     gint        calcDataLength;
@@ -761,10 +791,10 @@ dissect_opensafety_ssdo_message(tvbuff_t *message_tvb , packet_info *pinfo, prot
     {
         /* taddr is the 4th octet in the second frame */
         taddr = OSS_FRAME_ADDR(bytes, frameStart2 + 3);
+        sdn =  ( OSS_FRAME_ADDR(bytes, frameStart1) ^ OSS_FRAME_ADDR(bytes, frameStart2) );
 
-        PACKET_SENDER_RECEIVER( pinfo, OSS_FRAME_ADDR(bytes, frameStart1), frameStart1, taddr, frameStart2 + 3,
-                                frameStart2,
-                                ( ( OSS_FRAME_ADDR(bytes, frameStart1) ) ^ ( OSS_FRAME_ADDR(bytes, frameStart2) ) ));
+        PACKET_SENDER_RECEIVER ( pinfo, OSS_FRAME_ADDR(bytes, frameStart1), frameStart1, taddr,
+        		frameStart2 + 3, frameStart2, sdn );
     }
     else if ( ! isResponse )
     {
@@ -847,9 +877,24 @@ dissect_opensafety_ssdo_message(tvbuff_t *message_tvb , packet_info *pinfo, prot
             ( sacmd == OPENSAFETY_MSG_SSDO_ABORT )
     )
     {
-        proto_tree_add_uint(ssdo_tree, hf_oss_ssdo_sod_index, message_tvb, db0Offset + 1, 2,
-                ((guint16)(bytes[db0Offset + 2] << 8) + bytes[db0Offset + 1]));
-        proto_tree_add_uint(ssdo_tree, hf_oss_ssdo_sod_subindex, message_tvb, db0Offset + 3, 1, bytes[db0Offset + 3]);
+        ssdoIndex = ((guint16)(bytes[db0Offset + 2] << 8) + bytes[db0Offset + 1]);
+        ssdoSubIndex = bytes[db0Offset + 3];
+
+        proto_tree_add_uint_format_value(ssdo_tree, hf_oss_ssdo_sod_index, message_tvb, db0Offset + 1, 2,
+                ssdoIndex, "0x%04X (%s)", ssdoIndex,
+                val_to_str_const(((guint32) (ssdoIndex << 16)), sod_idx_names, "Unknown") );
+        col_append_fstr(pinfo->cinfo, COL_INFO, " [%s", val_to_str_const(((guint32) (ssdoIndex << 16)), sod_idx_names, "0x%04X"));
+
+        /* Some SOD downloads (0x101A for instance) don't have sub-indeces */
+        if ( ssdoSubIndex != 0x0 )
+        {
+            proto_tree_add_uint_format_value(ssdo_tree, hf_oss_ssdo_sod_subindex, message_tvb, db0Offset + 3, 1,
+                ssdoSubIndex, "0x%02X (%s)", ssdoSubIndex,
+                val_to_str_const(((guint32) (ssdoIndex << 16) + ssdoSubIndex), sod_idx_names, "Unknown") );
+            col_append_fstr(pinfo->cinfo, COL_INFO, " - %s",
+                    val_to_str_const(((guint32) (ssdoIndex << 16) + ssdoSubIndex), sod_idx_names, "0x%02X"));
+        }
+        col_append_fstr(pinfo->cinfo, COL_INFO, "%s", "]" );
         payloadOffset += 3;
     }
 
@@ -936,9 +981,10 @@ dissect_opensafety_snmt_message(tvbuff_t *message_tvb, packet_info *pinfo , prot
 {
     proto_item *item;
     proto_tree *snmt_tree;
-    guint16     addr, taddr;
+    guint16     addr, taddr, sdn;
     guint8      db0, byte;
     guint       dataLength;
+    char       *tempString;
 
     dataLength = OSS_FRAME_LENGTH(bytes, frameStart1);
 
@@ -946,6 +992,8 @@ dissect_opensafety_snmt_message(tvbuff_t *message_tvb, packet_info *pinfo , prot
     addr = OSS_FRAME_ADDR(bytes, frameStart1);
     /* taddr is the 4th octet in the second frame */
     taddr = OSS_FRAME_ADDR(bytes, frameStart2 + 3);
+    /* domain is xor'ed on the first field in the second frame. As this is also addr, it is easy to obtain */
+    sdn = OSS_FRAME_ADDR(bytes, frameStart2) ^ addr;
 
     db0 = -1;
     if (dataLength > 0)
@@ -954,13 +1002,12 @@ dissect_opensafety_snmt_message(tvbuff_t *message_tvb, packet_info *pinfo , prot
     if ( ( (OSS_FRAME_ID(bytes, frameStart1) ^ OPENSAFETY_MSG_SNMT_SERVICE_RESPONSE) == 0 ) &&
          ( (db0 ^ OPENSAFETY_MSG_SNMT_EXT_SCM_SET_TO_STOP) == 0 || (db0 ^ OPENSAFETY_MSG_SNMT_EXT_SCM_SET_TO_OP) == 0 ) )
     {
-        PACKET_RECEIVER( pinfo, addr, OSS_FRAME_POS_ADDR + frameStart1, frameStart2,
-                         ( OSS_FRAME_ADDR(bytes, frameStart1) ^ OSS_FRAME_ADDR(bytes, frameStart2) ) );
+        PACKET_RECEIVER( pinfo, addr, OSS_FRAME_POS_ADDR + frameStart1, frameStart2, sdn );
     }
     else
     {
-        PACKET_SENDER_RECEIVER ( pinfo, taddr, frameStart2 + 3, addr, OSS_FRAME_POS_ADDR + frameStart1, frameStart2,
-                                 ( OSS_FRAME_ADDR(bytes, frameStart1) ^ OSS_FRAME_ADDR(bytes, frameStart2) ) );
+    	PACKET_SENDER_RECEIVER ( pinfo, taddr, OSS_FRAME_POS_ADDR + frameStart1, frameStart2 + 3, addr,
+    			frameStart2, sdn );
     }
 
     item = proto_tree_add_uint_format_value(opensafety_tree, hf_oss_msg_category, message_tvb, OSS_FRAME_POS_ID + frameStart1, 1,
@@ -1007,8 +1054,21 @@ dissect_opensafety_snmt_message(tvbuff_t *message_tvb, packet_info *pinfo , prot
         }
         else if ( (db0 ^ OPENSAFETY_MSG_SNMT_EXT_SN_ASSIGNED_UDID_SCM) == 0 )
         {
-            proto_tree_add_ether(snmt_tree, hf_oss_snmt_udid, message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1,
+            item = proto_tree_add_ether(snmt_tree, hf_oss_snmt_udid, message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1,
                     6, tvb_get_ptr(message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1, 6));
+
+            if ( global_scm_udid_autoset == TRUE )
+            {
+                tempString = ep_alloc0(128 * sizeof(char));
+                g_snprintf ( tempString, 18, "%s", tvb_bytes_to_str_punct(message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1, 6, ':' ) );
+                if ( memcmp ( global_scm_udid, tempString, 17 ) != 0 )
+                {
+					local_scm_udid = se_alloc0(18 * sizeof(char));
+					g_snprintf(local_scm_udid, 18, "%s", tempString );
+					expert_add_info_format(pinfo, item, PI_PROTOCOL, PI_NOTE, "Auto detected payload as SCM UDID [%s].", tempString);
+                }
+            }
+
         }
 
     }
@@ -1026,8 +1086,21 @@ dissect_opensafety_snmt_message(tvbuff_t *message_tvb, packet_info *pinfo , prot
         {
             proto_tree_add_uint(snmt_tree, hf_oss_snmt_master, message_tvb, OSS_FRAME_POS_ADDR + frameStart1, 2, addr);
             proto_tree_add_uint(snmt_tree, hf_oss_snmt_slave, message_tvb, frameStart2 + 3, 2, taddr);
-            proto_tree_add_ether(snmt_tree, hf_oss_snmt_udid, message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1,
+            item = proto_tree_add_ether(snmt_tree, hf_oss_snmt_udid, message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1,
                     6, tvb_get_ptr(message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1, 6));
+
+            if ( global_scm_udid_autoset == TRUE )
+            {
+                tempString = ep_alloc0(18 * sizeof(char));
+                g_snprintf ( tempString, 18, "%s", tvb_bytes_to_str_punct(message_tvb, OSS_FRAME_POS_DATA + frameStart1 + 1, 6, ':' ) );
+                if ( memcmp ( global_scm_udid, tempString, 17 ) != 0 )
+                {
+                    local_scm_udid = se_alloc0(18 * sizeof(char));
+                    g_snprintf(local_scm_udid, 18, "%s", tempString );
+                    expert_add_info_format(pinfo, item, PI_PROTOCOL, PI_NOTE, "Auto detected payload as SCM UDID [%s].", tempString);
+                }
+            }
+
         }
         else
         {
@@ -1149,7 +1222,8 @@ dissect_opensafety_message(guint16 frameStart1, guint16 frameStart2, guint8 type
         {
             validSCMUDID = FALSE;
             scmUDID = g_byte_array_new();
-            if ( hex_str_to_bytes(global_scm_udid, scmUDID, TRUE) && scmUDID->len == 6 )
+
+            if ( hex_str_to_bytes((local_scm_udid != NULL ? local_scm_udid : global_scm_udid), scmUDID, TRUE) && scmUDID->len == 6 )
             {
                 validSCMUDID = TRUE;
                 bytes = unxorFrame(length, bytes, frameStart1, frameStart2, scmUDID->data);
@@ -1164,9 +1238,12 @@ dissect_opensafety_message(guint16 frameStart1, guint16 frameStart2, guint8 type
                 }
             }
 
-            if ( strlen ( global_scm_udid ) > 0  && scmUDID->len == 6 )
+            if ( strlen ( (local_scm_udid != NULL ? local_scm_udid : global_scm_udid) ) > 0  && scmUDID->len == 6 )
             {
-            	item = proto_tree_add_string(opensafety_tree, hf_oss_scm_udid, message_tvb, 0, 0, global_scm_udid);
+            	if ( local_scm_udid != NULL )
+            		item = proto_tree_add_string(opensafety_tree, hf_oss_scm_udid_auto, message_tvb, 0, 0, local_scm_udid);
+            	else
+            		item = proto_tree_add_string(opensafety_tree, hf_oss_scm_udid, message_tvb, 0, 0, global_scm_udid);
             	PROTO_ITEM_SET_GENERATED(item);
             }
 
@@ -1612,6 +1689,9 @@ proto_register_opensafety(void)
         { &hf_oss_scm_udid,
           { "SCM UDID Configured",    "opensafety.scm_udid",
             FT_STRING,   BASE_NONE, NULL,   0x0, NULL, HFILL } },
+        { &hf_oss_scm_udid_auto,
+          { "SCM UDID Auto Detect",    "opensafety.scm_udid_auto",
+            FT_STRING,   BASE_NONE, NULL,   0x0, NULL, HFILL } },
         { &hf_oss_scm_udid_valid,
           { "SCM UDID Valid",    "opensafety.scm_udid_valid",
             FT_BOOLEAN,   BASE_NONE, NULL,   0x0, NULL, HFILL } },
@@ -1798,6 +1878,10 @@ proto_register_opensafety(void)
                  "SCM UDID (xx:xx:xx:xx:xx:xx)",
                  "To be able to fully dissect SSDO and SPDO packages, a valid UDID for the SCM has to be provided",
                  &global_scm_udid);
+    prefs_register_bool_preference(opensafety_module, "scm_udid_autoset",
+                 "Set SCM UDID if detected in stream",
+                 "Automatically assign a detected SCM UDID (by reading SNMT->SNTM_assign_UDID_SCM) and set it for the file",
+                 &global_scm_udid_autoset);
     prefs_register_uint_preference(opensafety_module, "network_udp_port",
                 "Port used for Generic UDP",
                 "Port used by any UDP demo implementation to transport data", 10,
@@ -1857,6 +1941,12 @@ proto_reg_handoff_opensafety(void)
              */
             dissector_add_uint("ethertype", ETHERTYPE_PROFINET, find_dissector("opensafety_pnio"));
         }
+
+        register_init_routine ( setup_dissector );
+
+        /* registering frame end routine, to prevent a malformed dissection preventing
+         * further dissector calls (see bug #6950) */
+        register_frame_end_routine(reset_dissector);
     }
 
 }