ethercatsoe.c

Go to the documentation of this file.

/*
 * Simple Open EtherCAT Master Library 
 *
 * File    : ethercatsoe.c
 * Version : 1.2.5
 * Date    : 09-04-2011
 * Copyright (C) 2005-2011 Speciaal Machinefabriek Ketels v.o.f.
 * Copyright (C) 2005-2011 Arthur Ketels
 * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 
 * Thanks to Hidde Verhoef for testing and improving the SoE module
 *
 * SOEM is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License version 2 as published by the Free
 * Software Foundation.
 *
 * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 *
 * As a special exception, if other files instantiate templates or use macros
 * or inline functions from this file, or you compile this file and link it
 * with other works to produce a work based on this file, this file does not
 * by itself cause the resulting work to be covered by the GNU General Public
 * License. However the source code for this file must still be made available
 * in accordance with section (3) of the GNU General Public License.
 *
 * This exception does not invalidate any other reasons why a work based on
 * this file might be covered by the GNU General Public License.
 *
 * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual
 * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
 * the sole purpose of creating, using and/or selling or otherwise distributing
 * an EtherCAT network master provided that an EtherCAT Master License is obtained
 * from Beckhoff Automation GmbH.
 *
 * In case you did not receive a copy of the EtherCAT Master License along with
 * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany
 * (www.beckhoff.com).
 */

#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <unistd.h>
#include <youbot_driver/soem/ethercattype.h>
#include <youbot_driver/soem/nicdrv.h>
#include <youbot_driver/soem/ethercatbase.h>
#include <youbot_driver/soem/ethercatmain.h>
#include <youbot_driver/soem/ethercatsoe.h>

typedef struct
  PACKED
  {
    ec_mbxheadert MbxHeader;
    uint8 opCode :3;
    uint8 incomplete :1;
    uint8 error :1;
    uint8 driveNo :3;
    uint8 elementflags;
    union
    {
      uint16 idn;
      uint16 fragmentsleft;
    };
  } ec_SoEt;

  static ec_SoEmappingt SoEmapping;
  static ec_SoEattributet SoEattribute;

  void ec_SoEerror(uint16 Slave, uint16 idn, uint16 Error)
  {
    ec_errort Ec;

    gettimeofday(&Ec.Time, 0);
    Ec.Slave = Slave;
    Ec.Index = idn;
    Ec.SubIdx = 0;
    EcatError = TRUE;
    Ec.Etype = EC_ERR_TYPE_SOE_ERROR;
    Ec.ErrorCode = Error;
    ec_pusherror(&Ec);
  }

  int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
  {
    ec_SoEt *SoEp, *aSoEp;
    uint16 totalsize, framedatasize;
    int wkc;
    uint8 *bp;
    uint8 *mp;
    uint16 *errorcode;
    ec_mbxbuft MbxIn, MbxOut;
    uint8 cnt;
    boolean NotLast;

    ec_clearmbx(&MbxIn);
    /* Empty slave out mailbox if something is in. Timeout set to 0 */
    wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
    ec_clearmbx(&MbxOut);
    aSoEp = (ec_SoEt *)&MbxIn;
    SoEp = (ec_SoEt *)&MbxOut;
    SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
    SoEp->MbxHeader.address = htoes(0x0000);
    SoEp->MbxHeader.priority = 0x00;
    /* get new mailbox count value, used as session handle */
    cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
    ec_slave[slave].mbx_cnt = cnt;
    SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
    SoEp->opCode = ECT_SOE_READREQ;
    SoEp->incomplete = 0;
    SoEp->error = 0;
    SoEp->driveNo = driveNo;
    SoEp->elementflags = elementflags;
    SoEp->idn = htoes(idn);
    totalsize = 0;
    bp = p;
    mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
    NotLast = TRUE;
    /* send SoE request to slave */
    wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
    if (wkc > 0) /* succeeded to place mailbox in slave ? */
    {
      while (NotLast)
      {
        /* clean mailboxbuffer */
        ec_clearmbx(&MbxIn);
        /* read slave response */
        wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, timeout);
        if (wkc > 0) /* succeeded to read slave response ? */
        {
          /* slave response should be SoE, ReadRes */
          if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && (aSoEp->opCode == ECT_SOE_READRES)
              && (aSoEp->error == 0) && (aSoEp->driveNo == driveNo) && (aSoEp->elementflags == elementflags))
          {
            framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert);
            totalsize += framedatasize;
            /* Does parameter fit in parameter buffer ? */
            if (totalsize <= *psize)
            {
              /* copy parameter data in parameter buffer */
              memcpy(bp, mp, framedatasize);
              /* increment buffer pointer */
              bp += framedatasize;
            }
            else
            {
              framedatasize -= totalsize - *psize;
              totalsize = *psize;
              /* copy parameter data in parameter buffer */
              if (framedatasize > 0)
                memcpy(bp, mp, framedatasize);
            }

            if (!aSoEp->incomplete)
            {
              NotLast = FALSE;
              *psize = totalsize;
            }
          }
          /* other slave response */
          else
          {
            NotLast = FALSE;
            if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && (aSoEp->opCode == ECT_SOE_READRES)
                && (aSoEp->error == 1))
            {
              mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
              errorcode = (uint16 *)mp;
              ec_SoEerror(slave, idn, *errorcode);
            }
            else
            {
              ec_packeterror(slave, idn, 0, 1); /* Unexpected frame returned */
            }
            wkc = 0;
          }
        }
        else
        {
          NotLast = FALSE;
          ec_packeterror(slave, idn, 0, 4); /* no response */
        }
      }
    }
    return wkc;
  }

  int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
  {
    ec_SoEt *SoEp, *aSoEp;
    uint16 framedatasize, maxdata;
    int wkc;
    uint8 *mp;
    uint8 *hp;
    uint16 *errorcode;
    ec_mbxbuft MbxIn, MbxOut;
    uint8 cnt;
    boolean NotLast;

    ec_clearmbx(&MbxIn);
    /* Empty slave out mailbox if something is in. Timeout set to 0 */
    wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
    ec_clearmbx(&MbxOut);
    aSoEp = (ec_SoEt *)&MbxIn;
    SoEp = (ec_SoEt *)&MbxOut;
    SoEp->MbxHeader.address = htoes(0x0000);
    SoEp->MbxHeader.priority = 0x00;
    SoEp->opCode = ECT_SOE_WRITEREQ;
    SoEp->error = 0;
    SoEp->driveNo = driveNo;
    SoEp->elementflags = elementflags;
    hp = p;
    mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
    maxdata = ec_slave[slave].mbx_l - sizeof(ec_SoEt);
    NotLast = TRUE;
    while (NotLast)
    {
      framedatasize = psize;
      NotLast = FALSE;
      SoEp->idn = htoes(idn);
      SoEp->incomplete = 0;
      if (framedatasize > maxdata)
      {
        framedatasize = maxdata; /*  segmented transfer needed  */
        NotLast = TRUE;
        SoEp->incomplete = 1;
        SoEp->fragmentsleft = psize / maxdata;
      }
      SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize);
      /* get new mailbox counter, used for session handle */
      cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
      ec_slave[slave].mbx_cnt = cnt;
      SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
      /* copy parameter data to mailbox */
      memcpy(mp, hp, framedatasize);
      hp += framedatasize;
      psize -= framedatasize;
      /* send SoE request to slave */
      wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
      if (wkc > 0) /* succeeded to place mailbox in slave ? */
      {
        if (!NotLast || !ec_mbxempty(slave, timeout))
        {
          /* clean mailboxbuffer */
          ec_clearmbx(&MbxIn);
          /* read slave response */
          wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, timeout);
          if (wkc > 0) /* succeeded to read slave response ? */
          {
            NotLast = FALSE;
            /* slave response should be SoE, WriteRes */
            if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && (aSoEp->opCode == ECT_SOE_WRITERES)
                && (aSoEp->error == 0) && (aSoEp->driveNo == driveNo) && (aSoEp->elementflags == elementflags))
            {
              /* SoE write succeeded */
            }
            /* other slave response */
            else
            {
              if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && (aSoEp->opCode == ECT_SOE_READRES)
                  && (aSoEp->error == 1))
              {
                mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
                errorcode = (uint16 *)mp;
                ec_SoEerror(slave, idn, *errorcode);
              }
              else
              {
                ec_packeterror(slave, idn, 0, 1); /* Unexpected frame returned */
              }
              wkc = 0;
            }
          }
          else
          {
            ec_packeterror(slave, idn, 0, 4); /* no response */
          }
        }
      }
    }
    return wkc;
  }

  int ec_readIDNmap(uint16 slave, int *Osize, int *Isize)
  {
    int retVal = 0;
    int wkc;
    int psize;
    uint16 entries, itemcount;

    *Isize = 0;
    *Osize = 0;
    psize = sizeof(SoEmapping);
    /* read output mapping via SoE */
    wkc = ec_SoEread(slave, 0, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
    if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0)
        && (entries <= EC_SOE_MAXMAPPING))
    {
      /* command word (uint16) is always mapped but not in list */
      *Osize = 16;
      for (itemcount = 0; itemcount < entries; itemcount++)
      {
        psize = sizeof(SoEattribute);
        /* read attribute of each IDN in mapping list */
        wkc = ec_SoEread(slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
        if ((wkc > 0) && (!SoEattribute.list))
        {
          /* length : 0 = 8bit, 1 = 16bit .... */
          *Osize += (int)8 << SoEattribute.length;
        }
      }
    }
    psize = sizeof(SoEmapping);
    /* read input mapping via SoE */
    wkc = ec_SoEread(slave, 0, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
    if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0)
        && (entries <= EC_SOE_MAXMAPPING))
    {
      /* status word (uint16) is always mapped but not in list */
      *Isize = 16;
      for (itemcount = 0; itemcount < entries; itemcount++)
      {
        psize = sizeof(SoEattribute);
        /* read attribute of each IDN in mapping list */
        wkc = ec_SoEread(slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
        if ((wkc > 0) && (!SoEattribute.list))
        {
          /* length : 0 = 8bit, 1 = 16bit .... */
          *Isize += (int)8 << SoEattribute.length;
        }
      }
    }

    /* found some I/O bits ? */
    if ((*Isize > 0) || (*Osize > 0))
    {
      retVal = 1;
    }
    return retVal;
  }