ethercateoe.c

Go to the documentation of this file.

/*
 * Licensed under the GNU General Public License version 2 with exceptions. See
 * LICENSE file in the project root for full license information
 */

#include <stdio.h>
#include <string.h>
#include "osal.h"
#include "oshw.h"
#include "ethercat.h"

static void EOE_ip_uint32_to_byte(eoe_ip4_addr_t * ip, uint8_t * byte_ip)
{
   byte_ip[3] = eoe_ip4_addr1(ip); /* 1st octet */
   byte_ip[2] = eoe_ip4_addr2(ip); /* 2nd octet */
   byte_ip[1] = eoe_ip4_addr3(ip); /* 3ed octet */
   byte_ip[0] = eoe_ip4_addr4(ip); /* 4th octet */
}

static void EOE_ip_byte_to_uint32(uint8_t * byte_ip, eoe_ip4_addr_t * ip)
{
   EOE_IP4_ADDR_TO_U32(ip,
      byte_ip[3],  /* 1st octet */
      byte_ip[2],  /* 2nd octet */
      byte_ip[1],  /* 3ed octet */
      byte_ip[0]); /* 4th octet */
}

int ecx_EOEdefinehook(ecx_contextt *context, void *hook)
{
   context->EOEhook = hook;
   return 1;
}

int ecx_EOEsetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout)
{
   ec_EOEt *EOEp, *aEOEp;  
   ec_mbxbuft MbxIn, MbxOut;  
   uint16 frameinfo1, result;
   uint8 cnt, data_offset;
   uint8 flags = 0;
   int wkc;

   ec_clearmbx(&MbxIn);
   /* Empty slave out mailbox if something is in. Timout set to 0 */
   wkc = ecx_mbxreceive(context,  slave, (ec_mbxbuft *)&MbxIn, 0);
   ec_clearmbx(&MbxOut);
   aEOEp = (ec_EOEt *)&MbxIn;
   EOEp = (ec_EOEt *)&MbxOut;  
   EOEp->mbxheader.address = htoes(0x0000);
   EOEp->mbxheader.priority = 0x00;
   data_offset = EOE_PARAM_OFFSET;
   
   /* get new mailbox count value, used as session handle */
   cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
   context->slavelist[slave].mbx_cnt = cnt;

   EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */

   EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_INIT_REQ) | 
      EOE_HDR_FRAME_PORT_SET(port) |
      EOE_HDR_LAST_FRAGMENT);
   EOEp->frameinfo2 = 0;
  
   /* The EoE frame will include "empty" IP/DNS entries, makes wireshark happy.
    * Specification say they are optional, TwinCAT include empty entries.
    */
   if (ipparam->mac_set)
   {
      flags |= EOE_PARAM_MAC_INCLUDE;
      memcpy(&EOEp->data[data_offset], ipparam->mac.addr, EOE_ETHADDR_LENGTH);
   }
   data_offset += EOE_ETHADDR_LENGTH;
   if (ipparam->ip_set)
   {
      flags |= EOE_PARAM_IP_INCLUDE;
      EOE_ip_uint32_to_byte(&ipparam->ip, &EOEp->data[data_offset]);
   }
   data_offset += 4;
   if (ipparam->subnet_set)
   {
      flags |= EOE_PARAM_SUBNET_IP_INCLUDE;
      EOE_ip_uint32_to_byte(&ipparam->subnet, &EOEp->data[data_offset]);
   }
   data_offset += 4;
   if (ipparam->default_gateway_set)
   {
      flags |= EOE_PARAM_DEFAULT_GATEWAY_INCLUDE;
      EOE_ip_uint32_to_byte(&ipparam->default_gateway, &EOEp->data[data_offset]);
   }
   data_offset += 4;
   if (ipparam->dns_ip_set)
   {
      flags |= EOE_PARAM_DNS_IP_INCLUDE;
      EOE_ip_uint32_to_byte(&ipparam->dns_ip, &EOEp->data[data_offset]);
   }
   data_offset += 4;
   if (ipparam->dns_name_set)
   {
      flags |= EOE_PARAM_DNS_NAME_INCLUDE;
      memcpy(&EOEp->data[data_offset], (void *)ipparam->dns_name, EOE_DNS_NAME_LENGTH);
   }
   data_offset += EOE_DNS_NAME_LENGTH;

   EOEp->mbxheader.length = htoes(EOE_PARAM_OFFSET + data_offset);
   EOEp->data[0] = flags;

   /* send EoE request to slave */
   wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);

   if (wkc > 0) /* succeeded to place mailbox in slave ? */
   {
      /* clean mailboxbuffer */
      ec_clearmbx(&MbxIn);
      /* read slave response */
      wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
      if (wkc > 0) /* succeeded to read slave response ? */
      {
         /* slave response should be FoE */
         if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
         {
            frameinfo1 = etohs(aEOEp->frameinfo1);
            result = etohs(aEOEp->result);
            if ((EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_INIT_RESP) ||
                (result != EOE_RESULT_SUCCESS))
            {
               wkc = -result;
            }
         }
         else
         {
            /* unexpected mailbox received */
            wkc = -EC_ERR_TYPE_PACKET_ERROR;
         }
      }
   }
   return wkc;
}

int ecx_EOEgetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout)
{
   ec_EOEt *EOEp, *aEOEp;
   ec_mbxbuft MbxIn, MbxOut;
   uint16 frameinfo1, eoedatasize;
   uint8 cnt, data_offset;
   uint8 flags = 0;
   int wkc;

   /* Empty slave out mailbox if something is in. Timout set to 0 */
   wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
   ec_clearmbx(&MbxOut);
   aEOEp = (ec_EOEt *)&MbxIn;
   EOEp = (ec_EOEt *)&MbxOut;
   EOEp->mbxheader.address = htoes(0x0000);
   EOEp->mbxheader.priority = 0x00;
   data_offset = EOE_PARAM_OFFSET;

   /* get new mailbox count value, used as session handle */
   cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
   context->slavelist[slave].mbx_cnt = cnt;

   EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */

   EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_GET_IP_PARAM_REQ) | 
      EOE_HDR_FRAME_PORT_SET(port) |
      EOE_HDR_LAST_FRAGMENT);
   EOEp->frameinfo2 = 0;

   EOEp->mbxheader.length = htoes(0x0004); 
   EOEp->data[0] = flags;

   /* send EoE request to slave */
   wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
   if (wkc > 0) /* succeeded to place mailbox in slave ? */
   {
      /* clean mailboxbuffer */
      ec_clearmbx(&MbxIn);
      /* read slave response */
      wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
      if (wkc > 0) /* succeeded to read slave response ? */
      {
         /* slave response should be FoE */
         if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
         {
            frameinfo1 = etohs(aEOEp->frameinfo1);
            eoedatasize = etohs(aEOEp->mbxheader.length) - 0x0004;
            if (EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_GET_IP_PARAM_RESP)
            {
               wkc = -EOE_RESULT_UNSUPPORTED_FRAME_TYPE;
            }
            else
            {
               /* The EoE frame will include "empty" IP/DNS entries,  makes 
                * wireshark happy. Specification say they are optional, TwinCAT 
                * include empty entries.
                */
               flags = aEOEp->data[0];
               if (flags & EOE_PARAM_MAC_INCLUDE)
               {
                  memcpy(ipparam->mac.addr, 
                     &aEOEp->data[data_offset], 
                     EOE_ETHADDR_LENGTH);
                  ipparam->mac_set = 1;
               }
               data_offset += EOE_ETHADDR_LENGTH;
               if (flags & EOE_PARAM_IP_INCLUDE)
               {
                  EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
                     &ipparam->ip);
                  ipparam->ip_set = 1;
               }
               data_offset += 4;
               if (flags & EOE_PARAM_SUBNET_IP_INCLUDE)
               {
                  EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
                     &ipparam->subnet);
                  ipparam->subnet_set = 1;
               }
               data_offset += 4;
               if (flags & EOE_PARAM_DEFAULT_GATEWAY_INCLUDE)
               {
                  EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
                     &ipparam->default_gateway);
                  ipparam->default_gateway_set = 1;
               }
               data_offset += 4;
               if (flags & EOE_PARAM_DNS_IP_INCLUDE)
               {
                  EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
                     &ipparam->dns_ip);
                  ipparam->dns_ip_set = 1;
               }
               data_offset += 4;
               if (flags & EOE_PARAM_DNS_NAME_INCLUDE)
               {
                  uint16_t dns_len;
                  if ((eoedatasize - data_offset) < EOE_DNS_NAME_LENGTH)
                  {
                     dns_len = (eoedatasize - data_offset);
                  }
                  else
                  {
                     dns_len = EOE_DNS_NAME_LENGTH;
                  }     
                  /* Assume ZERO terminated string */
                  memcpy(ipparam->dns_name, &aEOEp->data[data_offset], dns_len);
                  ipparam->dns_name_set = 1;
               }
               data_offset += EOE_DNS_NAME_LENGTH;
               /* Something os not correct, flag the error */
               if(data_offset > eoedatasize)
               {
                  wkc = -EC_ERR_TYPE_MBX_ERROR;
               }
            }
         }
         else
         {
            /* unexpected mailbox received */
            wkc = -EC_ERR_TYPE_PACKET_ERROR;
         }
      }
   }
   return wkc;
}

int ecx_EOEsend(ecx_contextt *context, uint16 slave, uint8 port, int psize, void *p, int timeout)
{
   ec_EOEt *EOEp;
   ec_mbxbuft MbxOut;
   uint16 frameinfo1, frameinfo2;
   uint16 txframesize, txframeoffset;
   uint8 cnt, txfragmentno;  
   boolean  NotLast;
   int wkc, maxdata;
   const uint8 * buf = p;
   static uint8_t txframeno = 0;

   ec_clearmbx(&MbxOut);
   EOEp = (ec_EOEt *)&MbxOut;
   EOEp->mbxheader.address = htoes(0x0000);
   EOEp->mbxheader.priority = 0x00;
   /* data section=mailbox size - 6 mbx - 4 EoEh */
   maxdata = context->slavelist[slave].mbx_l - 0x0A; 
   txframesize = psize;
   txfragmentno = 0;
   txframeoffset = 0;
   NotLast = TRUE;

   do
   {
      txframesize = psize - txframeoffset;
      if (txframesize > maxdata)
      {
         /* Adjust to even 32-octect blocks */
         txframesize = ((maxdata >> 5) << 5);
      }

      if (txframesize == (psize - txframeoffset))
      {
         frameinfo1 = (EOE_HDR_LAST_FRAGMENT_SET(1) | EOE_HDR_FRAME_PORT_SET(port));
         NotLast = FALSE;
      }
      else
      {
         frameinfo1 = EOE_HDR_FRAME_PORT_SET(port);
      }

      frameinfo2 = EOE_HDR_FRAG_NO_SET(txfragmentno);
      if (txfragmentno > 0)
      {
         frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET((txframeoffset >> 5)));
      }
      else
      {
         frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET(((psize + 31) >> 5)));
         txframeno++;
      }
      frameinfo2 = frameinfo2 | EOE_HDR_FRAME_NO_SET(txframeno);

      /* get new mailbox count value, used as session handle */
      cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
      context->slavelist[slave].mbx_cnt = cnt;

      EOEp->mbxheader.length = htoes(4 + txframesize); /* no timestamp */
      EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */

      EOEp->frameinfo1 = htoes(frameinfo1);
      EOEp->frameinfo2 = htoes(frameinfo2);

      memcpy(EOEp->data, &buf[txframeoffset], txframesize);

      /* send EoE request to slave */
      wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, timeout);
      if ((NotLast == TRUE)  && (wkc > 0))
      {
         txframeoffset += txframesize;
         txfragmentno++;
      }
   } while ((NotLast == TRUE) && (wkc > 0));
   
   return wkc;
}


int ecx_EOErecv(ecx_contextt *context, uint16 slave, uint8 port, int * psize, void *p, int timeout)
{
   ec_EOEt *aEOEp;
   ec_mbxbuft MbxIn;
   uint16 frameinfo1, frameinfo2, rxframesize, rxframeoffset, eoedatasize;
   uint8 rxfragmentno, rxframeno;
   boolean NotLast;
   int wkc, buffersize;
   uint8 * buf = p;
   
   ec_clearmbx(&MbxIn);
   aEOEp = (ec_EOEt *)&MbxIn;
   NotLast = TRUE;
   buffersize = *psize;
   rxfragmentno = 0;
   
   /* Hang for a while if nothing is in */
   wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);

   while ((wkc > 0) && (NotLast == TRUE))
   {
      /* slave response should be FoE */
      if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
      {
        
         eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004;
         frameinfo1 = etohs(aEOEp->frameinfo1);
         frameinfo2 = etohs(aEOEp->frameinfo2);

         if (rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2))
         {
            if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0)
            {
               wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
               /* Exit here*/
               break;
            }
         }

         if (rxfragmentno == 0)
         {
            rxframeoffset = 0;
            rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2);
            rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5);
            if (rxframesize > buffersize)
            {
               wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
               /* Exit here*/
               break;
            }
            if (port != EOE_HDR_FRAME_PORT_GET(frameinfo1))
            {
               wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
               /* Exit here*/
               break;
            }
         }
         else
         {
            if (rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2))
            {
               wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
               /* Exit here*/
               break;
            }
            else if (rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5))
            {
               wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
               /* Exit here*/
               break;
            }
         }

         if ((rxframeoffset + eoedatasize) <= buffersize)
         {
            memcpy(&buf[rxframeoffset], aEOEp->data, eoedatasize);
            rxframeoffset += eoedatasize;
            rxfragmentno++;
         }

         if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1))
         {
            /* Remove timestamp */
            if (EOE_HDR_TIME_APPEND_GET(frameinfo1))
            {
               rxframeoffset -= 4;
            }
            NotLast = FALSE;
            *psize = rxframeoffset;
         }
         else
         {
            /* Hang for a while if nothing is in */
            wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
         }
      }
      else
      {
         /* unexpected mailbox received */
         wkc = -EC_ERR_TYPE_PACKET_ERROR;
      }
   }
   return wkc;
}

int ecx_EOEreadfragment(
   ec_mbxbuft * MbxIn,
   uint8 * rxfragmentno,
   uint16 * rxframesize, 
   uint16 * rxframeoffset, 
   uint16 * rxframeno,
   int * psize, 
   void *p)
{  
   uint16 frameinfo1, frameinfo2,  eoedatasize;
   int wkc;
   ec_EOEt * aEOEp;
   uint8 * buf;
   
   aEOEp = (ec_EOEt *)MbxIn;
   buf = p;
   wkc = 0;

   /* slave response should be EoE */
   if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
   {
      eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004;
      frameinfo1 = etohs(aEOEp->frameinfo1);
      frameinfo2 = etohs(aEOEp->frameinfo2);

      /* Retrive fragment number, is it what we expect? */
      if (*rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2))
      {
         /* If expected fragment number is not 0, reset working variables */
         if (*rxfragmentno != 0)
         {
            *rxfragmentno = 0;
            *rxframesize = 0;
            *rxframeoffset = 0;
            *rxframeno = 0;
         }

         /* If incoming fragment number is not 0 we can't recover, exit */
         if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0)
         {
            wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
            return wkc;
         }
      }

      /* Is it a new frame?*/
      if (*rxfragmentno == 0)
      {
         *rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5);
         *rxframeoffset = 0;
         *rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2);
      }
      else
      {
         /* If we're inside a frame, make sure it is the same */
         if (*rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2))
         {
            *rxfragmentno = 0;
            *rxframesize = 0;
            *rxframeoffset = 0;
            *rxframeno = 0;
            wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
            return wkc;
         }
         else if (*rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5))
         {
            *rxfragmentno = 0;
            *rxframesize = 0;
            *rxframeoffset = 0;
            *rxframeno = 0;
            wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
            return wkc;
         }
      }

      /* Make sure we're inside expected frame size */
      if (((*rxframeoffset + eoedatasize) <= *rxframesize) && 
         ((*rxframeoffset + eoedatasize) <= *psize))
      {
         memcpy(&buf[*rxframeoffset], aEOEp->data, eoedatasize);
         *rxframeoffset += eoedatasize;
         *rxfragmentno += 1;
      }

      /* Is it the last fragment */
      if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1))
      {
         /* Remove timestamp */
         if (EOE_HDR_TIME_APPEND_GET(frameinfo1))
         {
            *rxframeoffset -= 4;
         }
         *psize = *rxframeoffset;
         *rxfragmentno = 0;
         *rxframesize = 0;
         *rxframeoffset = 0;
         *rxframeno = 0;
         wkc = 1;
      }
   }
   else
   {
      /* unexpected mailbox received */
      wkc = -EC_ERR_TYPE_PACKET_ERROR;
   }
   return wkc;
}