Fixed bug when the actual remaining command data is less than the size

specified in the EtherCAT mailbox header.

Change-Id: I661c62af915b9455da1df49f5746953d41dc527a
Reviewed-on: https://code.wireshark.org/review/13595
Reviewed-by: Anders Broman <a.broman58@gmail.com>

2 files changed, 131 insertions, 115 deletions

diff --git a/plugins/ethercat/packet-ecatmb.c b/plugins/ethercat/packet-ecatmb.c
index 15e8c91716..5d63ba2f5b 100644
--- a/plugins/ethercat/packet-ecatmb.c
+++ b/plugins/ethercat/packet-ecatmb.c
@@ -29,6 +29,7 @@
 #include <string.h>
 
 #include <epan/packet.h>
+#include <epan/expert.h>
 
 #include "packet-ecatmb.h"
 
@@ -193,6 +194,13 @@ static int hf_ecat_mailbox_soe_data = -1;
 static int hf_ecat_mailbox_soe_frag = -1;
 static int hf_ecat_mailbox_soe_error = -1;
 
+static expert_field ei_ecat_mailbox_error       = EI_INIT;
+static expert_field ei_ecat_mailbox_coe_error   = EI_INIT;
+static expert_field ei_ecat_mailbox_eoe_error   = EI_INIT;
+static expert_field ei_ecat_mailbox_soe_error   = EI_INIT;
+static expert_field ei_ecat_mailbox_foe_error   = EI_INIT;
+
+
 static const value_string EcMBoxType[] =
 {
    {   0, "Invalid", },
@@ -295,14 +303,14 @@ static const true_false_string tfs_ecat_yes_no =
 
 void init_mbx_header(PETHERCAT_MBOX_HEADER pMbox, tvbuff_t *tvb, gint offset)
 {
-   pMbox->Length = tvb_get_letohs(tvb, offset); offset+=(int)sizeof(guint16);
-   pMbox->Address = tvb_get_letohs(tvb, offset); offset+=(int)sizeof(guint16);
+   pMbox->Length = tvb_get_letohs(tvb, offset); offset+=2;
+   pMbox->Address = tvb_get_letohs(tvb, offset); offset+=2;
    pMbox->aControlUnion.Control = tvb_get_letohs(tvb, offset);
 }
 
 static void init_eoe_header(PETHERCAT_EOE_HEADER pEoE, tvbuff_t *tvb, gint offset)
 {
-   pEoE->anEoeHeaderInfoUnion.Info = tvb_get_letohs(tvb, offset); offset+=(int)sizeof(guint16);
+   pEoE->anEoeHeaderInfoUnion.Info = tvb_get_letohs(tvb, offset); offset+=2;
    pEoE->anEoeHeaderDataUnion.Result = tvb_get_letohs(tvb, offset);
 }
 
@@ -328,7 +336,7 @@ static void init_coe_header(PETHERCAT_COE_HEADER pCoE, tvbuff_t *tvb, gint offse
 static void init_sdo_header(PETHERCAT_SDO_HEADER pSdo, tvbuff_t *tvb, gint offset)
 {
    pSdo->anSdoHeaderUnion.CS = tvb_get_guint8(tvb, offset++);
-   pSdo->Index = tvb_get_letohs(tvb, offset);offset+=(int)sizeof(guint16);
+   pSdo->Index = tvb_get_letohs(tvb, offset);offset+=2;
    pSdo->SubIndex = tvb_get_guint8(tvb, offset++);
    pSdo->Data = tvb_get_letohl(tvb, offset);
 }
@@ -337,7 +345,7 @@ static void init_sdo_info_header(PETHERCAT_SDO_INFO_HEADER pInfo, tvbuff_t *tvb,
 {
    pInfo->anSdoControlUnion.Control = tvb_get_guint8(tvb, offset++);
    pInfo->Reserved = tvb_get_guint8(tvb, offset);
-   pInfo->FragmentsLeft = (int)sizeof(guint16);
+   pInfo->FragmentsLeft = 2;
 }
 
 static void CANopenSdoReqFormatter(PETHERCAT_SDO_HEADER pSdo, char *szText, gint nMax)
@@ -523,6 +531,7 @@ static void dissect_ecat_coe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
             if( coe_length < ETHERCAT_COE_HEADER_LEN + ETHERCAT_SDO_HEADER_LEN )
             {
                col_append_str(pinfo->cinfo, COL_INFO, "Sdo Req - invalid length");
+               expert_add_info_format(pinfo, ecat_coe_tree, &ei_ecat_mailbox_coe_error, "Sdo Req - invalid length");
                break;
             }
 
@@ -612,6 +621,7 @@ static void dissect_ecat_coe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
             if( coe_length < ETHERCAT_COE_HEADER_LEN + ETHERCAT_SDO_HEADER_LEN )
             {
                col_append_str(pinfo->cinfo, COL_INFO, "Sdo Res - invalid length");
+               expert_add_info_format(pinfo, ecat_coe_tree, &ei_ecat_mailbox_coe_error, "Sdo Res - invalid length");
                break;
             }
 
@@ -687,6 +697,7 @@ static void dissect_ecat_coe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
             if( coe_length < ETHERCAT_COE_HEADER_LEN + ETHERCAT_SDO_INFO_LISTREQ_LEN )
             {
                col_append_str(pinfo->cinfo, COL_INFO, "Sdo Info - invalid length");
+               expert_add_info_format(pinfo, ecat_coe_tree, &ei_ecat_mailbox_coe_error, "Sdo Info - invalid length");
                break;
             }
 
@@ -805,6 +816,7 @@ static void dissect_ecat_coe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
    else
    {
       col_append_str(pinfo->cinfo, COL_INFO, "- invalid length");
+      expert_add_info(pinfo, tree, &ei_ecat_mailbox_coe_error);
    }
 }
 
@@ -894,6 +906,7 @@ static void dissect_ecat_soe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
    else
    {
       col_append_str(pinfo->cinfo, COL_INFO, "SoE - invalid length");
+      expert_add_info(pinfo, tree, &ei_ecat_mailbox_soe_error);
    }
 }
 
@@ -1014,7 +1027,10 @@ static void dissect_ecat_eoe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
                proto_tree_add_item(ecat_eoe_init_tree, hf_ecat_mailbox_eoe_init_dnsname, tvb, offset, 32, ENC_ASCII|ENC_NA);
             }
             else
+            {
                proto_item_append_text(anItem, " - Invalid length!");
+               expert_add_info(pinfo, anItem, &ei_ecat_mailbox_eoe_error);
+            }
             break;
 
          case EOE_TYPE_MACFILTER_REQ:
@@ -1046,13 +1062,16 @@ static void dissect_ecat_eoe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
                      proto_tree_add_item(ecat_eoe_macfilter_filter_tree, hf_ecat_mailbox_eoe_macfilter_filters[nCnt], tvb, offset+nCnt*ETHERNET_ADDRESS_LEN, ETHERNET_ADDRESS_LEN, ENC_NA);
                   offset+=16*ETHERNET_ADDRESS_LEN;
 
-                  anItem = proto_tree_add_item(ecat_eoe_macfilter_tree, hf_ecat_mailbox_eoe_macfilter_filtermask, tvb, offset, 4*(int)sizeof(guint32), ENC_NA);
+                  anItem = proto_tree_add_item(ecat_eoe_macfilter_tree, hf_ecat_mailbox_eoe_macfilter_filtermask, tvb, offset, 4*4, ENC_NA);
                   ecat_eoe_macfilter_filtermask_tree = proto_item_add_subtree(anItem, ett_ecat_mailbox_eoe_macfilter_filtermask);
                   for( nCnt=0; nCnt<options.v.MacFilterMaskCount; nCnt++)
-                     proto_tree_add_item(ecat_eoe_macfilter_filtermask_tree, hf_ecat_mailbox_eoe_macfilter_filtermasks[nCnt], tvb, offset+nCnt*(int)sizeof(guint32), (int)sizeof(guint32), ENC_NA);
+                     proto_tree_add_item(ecat_eoe_macfilter_filtermask_tree, hf_ecat_mailbox_eoe_macfilter_filtermasks[nCnt], tvb, offset+nCnt*4, 4, ENC_NA);
                }
                else
+               {
                   proto_item_append_text(anItem, " - Invalid length!");
+                  expert_add_info(pinfo, anItem, &ei_ecat_mailbox_eoe_error);
+               }
             }
             break;
 
@@ -1068,6 +1087,7 @@ static void dissect_ecat_eoe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
    }
    else
    {
+      expert_add_info(pinfo, tree, &ei_ecat_mailbox_eoe_error);
       col_append_str(pinfo->cinfo, COL_INFO, "EoE - invalid length!");
    }
 }
@@ -1165,6 +1185,7 @@ static void dissect_ecat_foe(tvbuff_t *tvb, gint offset, packet_info *pinfo, pro
    else
    {
       col_append_str(pinfo->cinfo, COL_INFO, "FoE - invalid length");
+      expert_add_info(pinfo, tree, &ei_ecat_mailbox_foe_error);
    }
 }
 
@@ -1184,41 +1205,36 @@ static int dissect_ecat_mailbox(tvbuff_t *tvb, packet_info *pinfo, proto_tree *t
 
       init_mbx_header(&hdr, tvb, offset);
 
-      if( mailbox_length >= ETHERCAT_MBOX_HEADER_LEN + hdr.Length )
-      {
-         col_append_str(pinfo->cinfo, COL_INFO, " Mbx(");
+      col_append_str(pinfo->cinfo, COL_INFO, " Mbx(");
 
-         if( tree )
-         {
-            /* Create the mailbox sub tree */
-            anItem = proto_tree_add_item(tree, proto_ecat_mailbox, tvb, 0, ETHERCAT_MBOX_HEADER_LEN+hdr.Length, ENC_NA);
-            ecat_mailbox_tree = proto_item_add_subtree(anItem, ett_ecat_mailbox);
+      /* Create the mailbox sub tree */
+      anItem = proto_tree_add_item(tree, proto_ecat_mailbox, tvb, 0, ETHERCAT_MBOX_HEADER_LEN+hdr.Length, ENC_NA);
+      ecat_mailbox_tree = proto_item_add_subtree(anItem, ett_ecat_mailbox);
 
-            /* Create a mailbox header subtree */
-            ecat_mailbox_header_tree = proto_tree_add_subtree(ecat_mailbox_tree, tvb, offset, ETHERCAT_MBOX_HEADER_LEN, ett_ecat_mailbox_header, NULL, "Header");
+      /* Create a mailbox header subtree */
+      ecat_mailbox_header_tree = proto_tree_add_subtree(ecat_mailbox_tree, tvb, offset, ETHERCAT_MBOX_HEADER_LEN, ett_ecat_mailbox_header, NULL, "Header");
 
-            /* Add length information to the mailbox header */
-            proto_tree_add_item(ecat_mailbox_header_tree, hf_ecat_mailboxlength, tvb, offset, sizeof(hdr.Length), ENC_LITTLE_ENDIAN);
-            offset+=(int)sizeof(hdr.Length);
+      /* Add length information to the mailbox header */
+      proto_tree_add_item(ecat_mailbox_header_tree, hf_ecat_mailboxlength, tvb, offset, 2, ENC_LITTLE_ENDIAN);
+      offset+=2;
 
-            /* Add address information to the mailbox header */
-            proto_tree_add_item(ecat_mailbox_header_tree, hf_ecat_mailboxaddress, tvb, offset, sizeof(hdr.Address), ENC_LITTLE_ENDIAN);
-            offset+=(int)sizeof(hdr.Address);
+      /* Add address information to the mailbox header */
+      proto_tree_add_item(ecat_mailbox_header_tree, hf_ecat_mailboxaddress, tvb, offset, 2, ENC_LITTLE_ENDIAN);
+      offset+=2;
 
-            /* Add priority information to the mailbox header */
-            proto_tree_add_item(ecat_mailbox_header_tree, hf_ecat_mailboxpriority, tvb, offset, 1, ENC_LITTLE_ENDIAN);
-            offset+=(int)sizeof(guint8);
+      /* Add priority information to the mailbox header */
+      proto_tree_add_item(ecat_mailbox_header_tree, hf_ecat_mailboxpriority, tvb, offset, 1, ENC_LITTLE_ENDIAN);
+      offset+=1;
 
-            /* Add type information to the mailbox header */
-            proto_tree_add_uint(ecat_mailbox_header_tree, hf_ecat_mailboxtype, tvb, offset, 1, hdr.aControlUnion.v.Type);
+      /* Add type information to the mailbox header */
+      proto_tree_add_uint(ecat_mailbox_header_tree, hf_ecat_mailboxtype, tvb, offset, 1, hdr.aControlUnion.v.Type);
 
-            /* Add counter information to the mailbox header */
-            proto_tree_add_uint(ecat_mailbox_header_tree, hf_ecat_mailboxcounter, tvb, offset, 1, hdr.aControlUnion.v.Counter);
-            offset++;
-         }
-         else
-            offset+=ETHERCAT_MBOX_HEADER_LEN;
+      /* Add counter information to the mailbox header */
+      proto_tree_add_uint(ecat_mailbox_header_tree, hf_ecat_mailboxcounter, tvb, offset, 1, hdr.aControlUnion.v.Counter);
+      offset++;
 
+      if( mailbox_length >= ETHERCAT_MBOX_HEADER_LEN + hdr.Length )
+      {
          next_tvb = tvb_new_subset_length (tvb, offset, hdr.Length);
          switch ( hdr.aControlUnion.v.Type )
          {
@@ -1245,11 +1261,15 @@ static int dissect_ecat_mailbox(tvbuff_t *tvb, packet_info *pinfo, proto_tree *t
          default:
             proto_tree_add_item(ecat_mailbox_tree, hf_ecat_mailboxdata, tvb, offset, hdr.Length, ENC_NA);
          }
-
-         col_append_str(pinfo->cinfo, COL_INFO, ")");
       }
+      else
+      {
+         anItem =proto_tree_add_item(ecat_mailbox_tree, hf_ecat_mailboxdata, tvb, offset, mailbox_length-ETHERCAT_MBOX_HEADER_LEN, ENC_NA);
+         expert_add_info_format(pinfo, anItem, &ei_ecat_mailbox_error,"Incorrect Mailbox data length(Expected:%d Actual:%d)", hdr.Length, mailbox_length-ETHERCAT_MBOX_HEADER_LEN);
+      }
+      col_append_str(pinfo->cinfo, COL_INFO, ")");
    }
-    return tvb_captured_length(tvb);
+   return tvb_captured_length(tvb);
 }
 
 void proto_register_ecat_mailbox(void)
@@ -1315,7 +1335,7 @@ void proto_register_ecat_mailbox(void)
       },
       { &hf_ecat_mailbox_eoe_last,
       { "Last Fragment", "ecat_mailbox.eoe.last",
-      FT_BOOLEAN, 32, TFS(&tfs_ecat_yes_no), 0x0, NULL, HFILL }
+      FT_UINT32, BASE_DEC, NULL, 0x0, NULL, HFILL }
       },
       { &hf_ecat_mailbox_eoe_timestampapp,
       { "Time Stamp Appended", "ecat_mailbox.eoe.timestampapp",
@@ -1937,8 +1957,23 @@ void proto_register_ecat_mailbox(void)
       &ett_ecat_mailbox_header
    };
 
+   static ei_register_info ei[] =
+   {
+      { &ei_ecat_mailbox_error, { "ecat_mailbox.invalid", PI_MALFORMED, PI_ERROR, "Malformed mailbox data", EXPFILL } },
+      { &ei_ecat_mailbox_coe_error, { "ecat_mailbox.coe.invalid", PI_MALFORMED, PI_ERROR, "Malformed CoE data", EXPFILL } },
+      { &ei_ecat_mailbox_foe_error, { "ecat_mailbox.foe.invalid", PI_MALFORMED, PI_ERROR, "Malformed FoE data", EXPFILL } },
+      { &ei_ecat_mailbox_soe_error, { "ecat_mailbox.soe.invalid", PI_MALFORMED, PI_ERROR, "Malformed SoE data", EXPFILL } },
+      { &ei_ecat_mailbox_eoe_error, { "ecat_mailbox.eoe.invalid", PI_MALFORMED, PI_ERROR, "Malformed EoE data", EXPFILL } },
+   };
+
+   expert_module_t *expert_module;
+
    proto_ecat_mailbox = proto_register_protocol("EtherCAT Mailbox Protocol",
       "ECAT_MAILBOX", "ecat_mailbox");
+
+   expert_module = expert_register_protocol(proto_ecat_mailbox);
+   expert_register_field_array(expert_module, ei, array_length(ei));
+
    proto_register_field_array(proto_ecat_mailbox, hf,array_length(hf));
    proto_register_subtree_array(ett, array_length(ett));
 
diff --git a/plugins/ethercat/packet-ethercat-datagram.c b/plugins/ethercat/packet-ethercat-datagram.c
index f9c9e1bb11..971cf30160 100644
--- a/plugins/ethercat/packet-ethercat-datagram.c
+++ b/plugins/ethercat/packet-ethercat-datagram.c
@@ -290,9 +290,9 @@ static void init_EcParserHDR(EcParserHDR* pHdr, tvbuff_t *tvb, gint offset)
 {
    pHdr->cmd = tvb_get_guint8(tvb, offset++);
    pHdr->idx = tvb_get_guint8(tvb, offset++);
-   pHdr->anAddrUnion.a.adp = tvb_get_letohs(tvb, offset); offset+=(int)sizeof(guint16);
-   pHdr->anAddrUnion.a.ado = tvb_get_letohs(tvb, offset); offset+=(int)sizeof(guint16);
-   pHdr->len = tvb_get_letohs(tvb, offset); offset+=(int)sizeof(guint16);
+   pHdr->anAddrUnion.a.adp = tvb_get_letohs(tvb, offset); offset+=2;
+   pHdr->anAddrUnion.a.ado = tvb_get_letohs(tvb, offset); offset+=2;
+   pHdr->len = tvb_get_letohs(tvb, offset); offset+=2;
    pHdr->intr = tvb_get_letohs(tvb, offset);
 }
 
@@ -302,7 +302,7 @@ static void init_dc_measure(guint32* pDC, tvbuff_t *tvb, gint offset)
    for ( i=0; i<4; i++ )
    {
       pDC[i] = tvb_get_letohl(tvb, offset);
-      offset+=(int)sizeof(guint32);
+      offset+=4;
    }
 }
 
@@ -313,7 +313,7 @@ static guint16 get_wc(EcParserHDR* pHdr, tvbuff_t *tvb, gint offset)
 
 static guint16 get_cmd_len(EcParserHDR* pHdr)
 {
-   return (EcParserHDR_Len+(pHdr->len&0x07ff)+sizeof(guint16)); /*Header + data + wc*/
+   return (EcParserHDR_Len+(pHdr->len&0x07ff)+2); /*Header + data + wc*/
 }
 
 
@@ -501,11 +501,10 @@ static int dissect_ecat_datagram(tvbuff_t *tvb, packet_info *pinfo, proto_tree *
                  *ecat_fmmu_active_tree = NULL, *ecat_fmmu_type_tree = NULL, *ecat_syncman_tree = NULL,
                  *ecat_syncflag_tree = NULL, *ecat_dc_tree = NULL;
       proto_item *hidden_item;
-
-      gint bMBox = FALSE;
       guint32 subsize;
       guint32 suboffset;
       guint32 len;
+      guint16 cnt;
       ETHERCAT_MBOX_HEADER mbox;
 
       suboffset = offset;
@@ -513,28 +512,7 @@ static int dissect_ecat_datagram(tvbuff_t *tvb, packet_info *pinfo, proto_tree *
 
       subsize = get_cmd_len(&ecHdr);
       len = ecHdr.len & 0x07ff;
-
-      if ( len >= ETHERCAT_MBOX_HEADER_LEN &&
-           (ecHdr.cmd==EC_CMD_TYPE_FPWR || ecHdr.cmd==EC_CMD_TYPE_FPRD || ecHdr.cmd==EC_CMD_TYPE_APWR || ecHdr.cmd==EC_CMD_TYPE_APRD) &&
-           ecHdr.anAddrUnion.a.ado>=0x1000
-         )
-      {
-         init_mbx_header(&mbox, tvb, suboffset+EcParserHDR_Len);
-
-         switch ( mbox.aControlUnion.v.Type )
-         {
-         case ETHERCAT_MBOX_TYPE_EOE:
-         case ETHERCAT_MBOX_TYPE_ADS:
-         case ETHERCAT_MBOX_TYPE_FOE:
-         case ETHERCAT_MBOX_TYPE_COE:
-         case ETHERCAT_MBOX_TYPE_SOE:
-            if ( /*pMBox->Length > 0 &&*/ mbox.Length <= 1500 /*&& (pMBox->Length+ETHERCAT_MBOX_HEADER_LEN) >= len*/ )
-            {
-               bMBox = TRUE;
-            }
-            break;
-         }
-      }
+      cnt = get_wc(&ecHdr, tvb, suboffset);
 
       if( tree )
       {
@@ -546,50 +524,49 @@ static int dissect_ecat_datagram(tvbuff_t *tvb, packet_info *pinfo, proto_tree *
          ecat_header_tree = proto_tree_add_subtree(ecat_datagram_tree, tvb, offset, EcParserHDR_Len, ett_ecat_header, NULL, "Header");
 
          EcCmdFormatter(ecHdr.cmd, szText, nMax);
-         aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_cmd, tvb, suboffset, sizeof(ecHdr.cmd), ENC_LITTLE_ENDIAN);
+         aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_cmd, tvb, suboffset, 1, ENC_LITTLE_ENDIAN);
          proto_item_set_text(aitem, "%s", szText);
          if( subCount < 10 ){
-            aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_cmd[subCount], tvb, suboffset, sizeof(ecHdr.cmd), ENC_LITTLE_ENDIAN);
+            aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_cmd[subCount], tvb, suboffset, 1, ENC_LITTLE_ENDIAN);
             PROTO_ITEM_SET_HIDDEN(aitem);
          }
+         suboffset+=1;
 
-         suboffset+= (int)sizeof(ecHdr.cmd);
-
-         proto_tree_add_item(ecat_header_tree, hf_ecat_idx, tvb, suboffset, sizeof(ecHdr.idx), ENC_LITTLE_ENDIAN);
+         proto_tree_add_item(ecat_header_tree, hf_ecat_idx, tvb, suboffset, 1, ENC_LITTLE_ENDIAN);
          if( subCount < 10 ){
-            aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_idx[subCount], tvb, suboffset, sizeof(ecHdr.idx), ENC_LITTLE_ENDIAN);
+            aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_idx[subCount], tvb, suboffset, 1, ENC_LITTLE_ENDIAN);
             PROTO_ITEM_SET_HIDDEN(aitem);
          }
-         suboffset+= (int)sizeof(ecHdr.idx);
+         suboffset+=1;
 
          switch ( ecHdr.cmd )
          {
          case 10:
          case 11:
          case 12:
-            proto_tree_add_item(ecat_header_tree, hf_ecat_lad, tvb, suboffset, sizeof(ecHdr.anAddrUnion.a.adp)+sizeof(ecHdr.anAddrUnion.a.ado), ENC_LITTLE_ENDIAN);
+            proto_tree_add_item(ecat_header_tree, hf_ecat_lad, tvb, suboffset, 4, ENC_LITTLE_ENDIAN);
             if( subCount < 10 ){
-               aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_lad[subCount], tvb, suboffset, sizeof(ecHdr.anAddrUnion.a.adp)+sizeof(ecHdr.anAddrUnion.a.ado), ENC_LITTLE_ENDIAN);
+               aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_lad[subCount], tvb, suboffset, 4, ENC_LITTLE_ENDIAN);
                PROTO_ITEM_SET_HIDDEN(aitem);
             }
 
-            suboffset += (int)((sizeof(ecHdr.anAddrUnion.a.adp)+sizeof(ecHdr.anAddrUnion.a.ado)));
+            suboffset+=4;
             break;
          default:
-            proto_tree_add_item(ecat_header_tree, hf_ecat_adp, tvb, suboffset, sizeof(ecHdr.anAddrUnion.a.adp), ENC_LITTLE_ENDIAN);
+            proto_tree_add_item(ecat_header_tree, hf_ecat_adp, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
             if( subCount < 10 ){
-               aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_adp[subCount], tvb, suboffset, sizeof(ecHdr.anAddrUnion.a.adp), ENC_LITTLE_ENDIAN);
+               aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_adp[subCount], tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
                PROTO_ITEM_SET_HIDDEN(aitem);
             }
 
-            suboffset+= (int)sizeof(ecHdr.anAddrUnion.a.adp);
-            proto_tree_add_item(ecat_header_tree, hf_ecat_ado, tvb, suboffset, sizeof(ecHdr.anAddrUnion.a.ado), ENC_LITTLE_ENDIAN);
+            suboffset+=2;
+            proto_tree_add_item(ecat_header_tree, hf_ecat_ado, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
             if( subCount < 10 ){
-               aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_ado[subCount], tvb, suboffset, sizeof(ecHdr.anAddrUnion.a.ado), ENC_LITTLE_ENDIAN);
+               aitem = proto_tree_add_item(ecat_header_tree, hf_ecat_sub_ado[subCount], tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
                PROTO_ITEM_SET_HIDDEN(aitem);
             }
 
-            suboffset+= (int)sizeof(ecHdr.anAddrUnion.a.ado);
+            suboffset+=2;
          }
 
          {
@@ -597,20 +574,20 @@ static int dissect_ecat_datagram(tvbuff_t *tvb, packet_info *pinfo, proto_tree *
 
             /* Add information about the length field (11 bit length, 3 bits
                reserved, 1 bit circulating frame and 1 bit more in a sub tree */
-            length_sub_tree = proto_tree_add_subtree_format(ecat_header_tree, tvb, suboffset, sizeof(ecHdr.len),
+            length_sub_tree = proto_tree_add_subtree_format(ecat_header_tree, tvb, suboffset, 2,
                                         ett_ecat_length, NULL, "Length     : %d (0x%x) - %s - %s",
                                         len, len, ecHdr.len & 0x4000 ? "Roundtrip" : "No Roundtrip", ecHdr.len & 0x8000 ? "More Follows..." : "Last Sub Command");
 
-            proto_tree_add_item(length_sub_tree, hf_ecat_length_len, tvb, suboffset, sizeof(ecHdr.len), ENC_LITTLE_ENDIAN);
-            proto_tree_add_item(length_sub_tree, hf_ecat_length_r, tvb, suboffset, sizeof(ecHdr.len), ENC_LITTLE_ENDIAN);
-            proto_tree_add_item(length_sub_tree, hf_ecat_length_c, tvb, suboffset, sizeof(ecHdr.len), ENC_LITTLE_ENDIAN);
-            proto_tree_add_item(length_sub_tree, hf_ecat_length_m, tvb, suboffset, sizeof(ecHdr.len), ENC_LITTLE_ENDIAN);
+            proto_tree_add_item(length_sub_tree, hf_ecat_length_len, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
+            proto_tree_add_item(length_sub_tree, hf_ecat_length_r, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
+            proto_tree_add_item(length_sub_tree, hf_ecat_length_c, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
+            proto_tree_add_item(length_sub_tree, hf_ecat_length_m, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
 
-            suboffset+= (int)sizeof(ecHdr.len);
+            suboffset+=2;
          }
 
-         proto_tree_add_item(ecat_header_tree, hf_ecat_int, tvb, suboffset, sizeof(ecHdr.intr), ENC_LITTLE_ENDIAN);
-         suboffset+= (int)sizeof(ecHdr.intr);
+         proto_tree_add_item(ecat_header_tree, hf_ecat_int, tvb, suboffset, 2, ENC_LITTLE_ENDIAN);
+         suboffset+=2;
       }
       else
       {
@@ -720,7 +697,7 @@ static int dissect_ecat_datagram(tvbuff_t *tvb, packet_info *pinfo, proto_tree *
             }
          }
       }
-      else if ( (ecHdr.cmd == 1 || ecHdr.cmd == 4) && ecHdr.anAddrUnion.a.ado == 0x900 && ecHdr.len >= 16 )
+      else if ( (ecHdr.cmd == 1 || ecHdr.cmd == 4) && ecHdr.anAddrUnion.a.ado == 0x900 && ecHdr.len >= 16 && cnt > 0 )
       {
          if (tree)
          {
@@ -797,46 +774,50 @@ static int dissect_ecat_datagram(tvbuff_t *tvb, packet_info *pinfo, proto_tree *
             }
          }
       }
-      else if ( bMBox )
+      else
       {
-         const guint MBoxLength = mbox.Length + 6 /* MBOX header length */;
-
-         next_tvb = tvb_new_subset_length(tvb, suboffset, MBoxLength);
-         call_dissector(ecat_mailbox_handle, next_tvb, pinfo, ecat_datagram_tree);
+         guint startOfData = offset + EcParserHDR_Len;
+         guint dataLength = len;
 
-         if( tree )
+         if ( len >= ETHERCAT_MBOX_HEADER_LEN &&
+           ((ecHdr.cmd==EC_CMD_TYPE_FPWR || ecHdr.cmd == EC_CMD_TYPE_APWR) || ((ecHdr.cmd==EC_CMD_TYPE_FPRD  || ecHdr.cmd==EC_CMD_TYPE_APRD) && cnt==1) ) &&
+           ecHdr.anAddrUnion.a.ado>=0x1000
+         )
          {
-            const guint startOfData = offset + EcParserHDR_Len + MBoxLength;
-            const guint dataLength = (ecHdr.len & 0x7ff) - MBoxLength;
-            if ( dataLength > 0 )
+            init_mbx_header(&mbox, tvb, startOfData);
+            switch ( mbox.aControlUnion.v.Type )
             {
-               /* Allow sub dissectors to have a chance with this data */
-               if(!dissector_try_heuristic(heur_subdissector_list, tvb, pinfo, ecat_datagram_tree, &hdtbl_entry, NULL))
+               case ETHERCAT_MBOX_TYPE_EOE:
+               case ETHERCAT_MBOX_TYPE_ADS:
+               case ETHERCAT_MBOX_TYPE_FOE:
+               case ETHERCAT_MBOX_TYPE_COE:
+               case ETHERCAT_MBOX_TYPE_SOE:
+               if ( mbox.Length <= 1500 )
                {
-                  /* No sub dissector did recognize this data, dissect it as data only */
-                  proto_tree_add_item(ecat_datagram_tree, hf_ecat_data, tvb, startOfData, dataLength, ENC_NA);
-               }
+                  guint MBoxLength = mbox.Length + ETHERCAT_MBOX_HEADER_LEN;
+                  if ( MBoxLength > len )
+                     MBoxLength = len;
 
-               if( subCount < 10 ){
-                  aitem = proto_tree_add_item(ecat_datagram_tree, hf_ecat_sub_data[subCount], tvb, startOfData, dataLength, ENC_NA);
-                  PROTO_ITEM_SET_HIDDEN(aitem);
+                  next_tvb = tvb_new_subset_length(tvb, startOfData, MBoxLength);
+                  call_dissector_only(ecat_mailbox_handle, next_tvb, pinfo, ecat_datagram_tree, NULL);
+
+                  startOfData += MBoxLength;
+                  dataLength -= MBoxLength;
                }
+               break;
             }
          }
-      }
-      else
-      {
-         if( tree )
+         if( dataLength > 0 )
          {
             /* Allow sub dissectors to have a chance with this data */
             if(!dissector_try_heuristic(heur_subdissector_list, tvb, pinfo, ecat_datagram_tree, &hdtbl_entry, NULL))
             {
                /* No sub dissector did recognize this data, dissect it as data only */
-               proto_tree_add_item(ecat_datagram_tree, hf_ecat_data, tvb, suboffset, ecHdr.len & 0x07ff, ENC_NA);
+               proto_tree_add_item(ecat_datagram_tree, hf_ecat_data, tvb, startOfData, dataLength, ENC_NA);
             }
 
             if( subCount < 10 ){
-               aitem = proto_tree_add_item(ecat_datagram_tree, hf_ecat_sub_data[subCount], tvb, offset + EcParserHDR_Len, ecHdr.len & 0x07ff, ENC_NA);
+               aitem = proto_tree_add_item(ecat_datagram_tree, hf_ecat_sub_data[subCount], tvb, startOfData, dataLength, ENC_NA);
                PROTO_ITEM_SET_HIDDEN(aitem);
             }
          }