ethercatmain.c

Go to the documentation of this file.

/*
 * Simple Open EtherCAT Master Library 
 *
 * File    : ethercatmain.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 
 *
 * 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 <arpa/inet.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>

#define EC_LOCALDELAY   200

typedef struct
{
  uint8 pushed;
  uint8 pulled;
  uint8 idx[EC_MAXBUF];
  void *data[EC_MAXBUF];
  uint16 length[EC_MAXBUF];
} ec_idxstackT;

typedef struct
  PACKED
  {
    uint16 comm;
    uint16 addr;
    uint16 d2;
  } ec_eepromt;

  typedef struct
  {
    int16 head;
    int16 tail;
    ec_errort Error[EC_MAXELIST + 1];
  } ec_eringt;

  typedef struct
  {
    ec_mbxheadert MbxHeader;
    uint16 CANOpen;
    uint16 ErrorCode;
    uint8 ErrorReg;
    uint8 bData;
    uint16 w1, w2;
  } ec_emcyt;

  ec_slavet ec_slave[EC_MAXSLAVE];
  int ec_slavecount;
  ec_groupt ec_group[EC_MAXGROUP];

  static uint8 esibuf[EC_MAXEEPBUF];
  static uint32 esimap[EC_MAXEEPBITMAP];
  static uint16 esislave = 0;
  static ec_eringt ec_elist;
  static ec_idxstackT ec_idxstack;

  boolean EcatError = FALSE;

  uint16 ec_DCtO;
  static uint16 ec_DCl;
  int64 ec_DCtime;

  void ec_pusherror(const ec_errort *Ec)
  {
    ec_elist.Error[ec_elist.head] = *Ec;
    ec_elist.Error[ec_elist.head].Signal = TRUE;
    ec_elist.head++;
    if (ec_elist.head > EC_MAXELIST)
      ec_elist.head = 0;
    if (ec_elist.head == ec_elist.tail)
      ec_elist.tail++;
    if (ec_elist.tail > EC_MAXELIST)
      ec_elist.tail = 0;
    EcatError = TRUE;
  }

  boolean ec_poperror(ec_errort *Ec)
  {
    boolean notEmpty = (ec_elist.head != ec_elist.tail);

    *Ec = ec_elist.Error[ec_elist.tail];
    ec_elist.Error[ec_elist.tail].Signal = FALSE;
    if (notEmpty)
    {
      ec_elist.tail++;
      if (ec_elist.tail > EC_MAXELIST)
        ec_elist.tail = 0;
    }
    else
      EcatError = FALSE;
    return notEmpty;
  }

  boolean ec_iserror(void)
  {
    return (ec_elist.head != ec_elist.tail);
  }

  void ec_packeterror(uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode)
  {
    ec_errort Ec;

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

  static void ec_mbxemergencyerror(uint16 Slave, uint16 ErrorCode, uint16 ErrorReg, uint8 b1, uint16 w1, uint16 w2)
  {
    ec_errort Ec;

    gettimeofday(&Ec.Time, 0);
    Ec.Slave = Slave;
    Ec.Index = 0;
    Ec.SubIdx = 0;
    Ec.Etype = EC_ERR_TYPE_EMERGENCY;
    Ec.ErrorCode = ErrorCode;
    Ec.ErrorReg = (uint8)ErrorReg;
    Ec.b1 = b1;
    Ec.w1 = w1;
    Ec.w2 = w2;
    ec_pusherror(&Ec);
  }

  int ec_init(const char * ifname)
  {
    return ec_setupnic(ifname, FALSE);
  }

  int ec_init_redundant(const char *ifname, const char *if2name)
  {
    int rval, zbuf;
    ec_etherheadert *ehp;

    ec_setupnic(ifname, FALSE);
    rval = ec_setupnic(if2name, TRUE);
    /* prepare "dummy" BRD tx frame for redundant operation */
    ehp = (ec_etherheadert *)&ec_txbuf2;
    ehp->sa1 = htons(secMAC[0]);
    zbuf = 0;
    ec_setupdatagram(&ec_txbuf2, EC_CMD_BRD, 0, 0x0000, 0x0000, 2, &zbuf);
    ec_txbuflength2 = ETH_HEADERSIZE+ EC_HEADERSIZE + EC_WKCSIZE + 2;

    return rval;
  }

  void ec_close(void)
  {
    ec_closenic();
  }
  ;

  uint8 ec_siigetbyte(uint16 slave, uint16 address)
  {
    uint16 configadr, eadr;
    uint64 edat;
    uint16 mapw, mapb;
    int lp, cnt;
    uint8 retval;

    retval = 0xff;
    if (slave != esislave) /* not the same slave? */
    {
      memset(esimap, 0x00, EC_MAXEEPBITMAP); /* clear esibuf cache map */
      esislave = slave;
    }
    if (address < EC_MAXEEPBUF)
    {
      mapw = address >> 5;
      mapb = address - (mapw << 5);
      if (esimap[mapw] & (uint32)(1 << mapb))
      {
        /* byte is already in buffer */
        retval = esibuf[address];
      }
      else
      {
        /* byte is not in buffer, put it there */
        configadr = ec_slave[slave].configadr;
        ec_eeprom2master(slave); /* set eeprom control to master */
        eadr = address >> 1;
        edat = ec_readeepromFP(configadr, eadr, EC_TIMEOUTEEP);
        /* 8 byte response */
        if (ec_slave[slave].eep_8byte)
        {
          put_unaligned64(edat, &esibuf[eadr << 1]);
          cnt = 8;
        }
        /* 4 byte response */
        else
        {
          put_unaligned32(edat, &esibuf[eadr << 1]);
          cnt = 4;
        }
        /* find bitmap location */
        mapw = eadr >> 4;
        mapb = (eadr << 1) - (mapw << 5);
        for (lp = 0; lp < cnt; lp++)
        {
          /* set bitmap for each byte that is read */
          esimap[mapw] |= (1 << mapb);
          mapb++;
          if (mapb > 31)
          {
            mapb = 0;
            mapw++;
          }
        }
        retval = esibuf[address];
      }
    }

    return retval;
  }

  int16 ec_siifind(uint16 slave, uint16 cat)
  {
    int16 a;
    uint16 p;
    uint8 eectl = ec_slave[slave].eep_pdi;

    a = ECT_SII_START << 1;
    /* read first SII section category */
    p = ec_siigetbyte(slave, a++);
    p += (ec_siigetbyte(slave, a++) << 8);
    /* traverse SII while category is not found and not EOF */
    while ((p != cat) && (p != 0xffff))
    {
      /* read section length */
      p = ec_siigetbyte(slave, a++);
      p += (ec_siigetbyte(slave, a++) << 8);
      /* locate next section category */
      a += p << 1;
      /* read section category */
      p = ec_siigetbyte(slave, a++);
      p += (ec_siigetbyte(slave, a++) << 8);
    }
    if (p != cat)
      a = 0;

    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */

    return a;
  }

  void ec_siistring(char *str, uint16 slave, uint16 Sn)
  {
    uint16 a, i, j, l, n, ba; //,p;
    char *ptr;
    uint8 eectl = ec_slave[slave].eep_pdi;

    ptr = str;
    //p = 0;
    a = ec_siifind(slave, ECT_SII_STRING); /* find string section */
    if (a > 0)
    {
      ba = a + 2; /* skip SII section header */
      n = ec_siigetbyte(slave, ba++); /* read number of strings in section */
      if (Sn <= n) /* is req string available? */
      {
        for (i = 1; i <= Sn; i++) /* walk through strings */
        {
          l = ec_siigetbyte(slave, ba++); /* length of this string */
          ptr = str;
          for (j = 1; j <= l; j++) /* copy one string */
          {
            *ptr = (char)ec_siigetbyte(slave, ba++);
            ptr++;
          }
        }
        *ptr = 0; /* add zero terminator */
      }
      else
      {
        ptr = str;
        *ptr = 0; /* empty string */
      }
    }
    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */
  }

  uint16 ec_siiFMMU(uint16 slave, ec_eepromFMMUt* FMMU)
  {
    uint16 a;
    uint8 eectl = ec_slave[slave].eep_pdi;

    FMMU->nFMMU = 0;
    FMMU->FMMU0 = 0;
    FMMU->FMMU1 = 0;
    FMMU->FMMU2 = 0;
    FMMU->FMMU3 = 0;
    FMMU->Startpos = ec_siifind(slave, ECT_SII_FMMU);

    if (FMMU->Startpos > 0)
    {
      a = FMMU->Startpos;
      FMMU->nFMMU = ec_siigetbyte(slave, a++);
      FMMU->nFMMU += (ec_siigetbyte(slave, a++) << 8);
      FMMU->nFMMU *= 2;
      FMMU->FMMU0 = ec_siigetbyte(slave, a++);
      FMMU->FMMU1 = ec_siigetbyte(slave, a++);
      if (FMMU->nFMMU > 2)
      {
        FMMU->FMMU2 = ec_siigetbyte(slave, a++);
        FMMU->FMMU3 = ec_siigetbyte(slave, a++);
      }
    }
    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */
    return FMMU->nFMMU;
  }

  uint16 ec_siiSM(uint16 slave, ec_eepromSMt* SM)
  {
    uint16 a, w; //,l;
    uint8 eectl = ec_slave[slave].eep_pdi;

    SM->nSM = 0;
    //l = 0;
    SM->Startpos = ec_siifind(slave, ECT_SII_SM);
    if (SM->Startpos > 0)
    {
      a = SM->Startpos;
      w = ec_siigetbyte(slave, a++);
      w += (ec_siigetbyte(slave, a++) << 8);
      SM->nSM = (w / 4);
      SM->PhStart = ec_siigetbyte(slave, a++);
      SM->PhStart += (ec_siigetbyte(slave, a++) << 8);
      SM->Plength = ec_siigetbyte(slave, a++);
      SM->Plength += (ec_siigetbyte(slave, a++) << 8);
      SM->Creg = ec_siigetbyte(slave, a++);
      SM->Sreg = ec_siigetbyte(slave, a++);
      SM->Activate = ec_siigetbyte(slave, a++);
      SM->PDIctrl = ec_siigetbyte(slave, a++);
    }
    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */
    return SM->nSM;
  }

  uint16 ec_siiSMnext(uint16 slave, ec_eepromSMt* SM, uint16 n)
  {
    uint16 a;
    uint16 retVal = 0;
    uint8 eectl = ec_slave[slave].eep_pdi;

    if (n < SM->nSM)
    {
      a = SM->Startpos + 2 + (n * 8);
      SM->PhStart = ec_siigetbyte(slave, a++);
      SM->PhStart += (ec_siigetbyte(slave, a++) << 8);
      SM->Plength = ec_siigetbyte(slave, a++);
      SM->Plength += (ec_siigetbyte(slave, a++) << 8);
      SM->Creg = ec_siigetbyte(slave, a++);
      SM->Sreg = ec_siigetbyte(slave, a++);
      SM->Activate = ec_siigetbyte(slave, a++);
      SM->PDIctrl = ec_siigetbyte(slave, a++);
      retVal = 1;
    }
    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */
    return retVal;
  }

  int ec_siiPDO(uint16 slave, ec_eepromPDOt* PDO, uint8 t)
  {
    uint16 a, w, c, e, er, Size;
    uint8 eectl = ec_slave[slave].eep_pdi;

    Size = 0;
    PDO->nPDO = 0;
    PDO->Length = 0;
    PDO->Index[1] = 0;
    for (c = 0; c < EC_MAXSM; c++)
      PDO->SMbitsize[c] = 0;
    if (t > 1)
      t = 1;
    PDO->Startpos = ec_siifind(slave, ECT_SII_PDO + t);
    if (PDO->Startpos > 0)
    {
      a = PDO->Startpos;
      w = ec_siigetbyte(slave, a++);
      w += (ec_siigetbyte(slave, a++) << 8);
      PDO->Length = w;
      c = 1;
      /* traverse through all PDOs */
      do
      {
        PDO->nPDO++;
        PDO->Index[PDO->nPDO] = ec_siigetbyte(slave, a++);
        PDO->Index[PDO->nPDO] += (ec_siigetbyte(slave, a++) << 8);
        PDO->BitSize[PDO->nPDO] = 0;
        c++;
        e = ec_siigetbyte(slave, a++);
        PDO->SyncM[PDO->nPDO] = ec_siigetbyte(slave, a++);
        a += 4;
        c += 2;
        if (PDO->SyncM[PDO->nPDO] < EC_MAXSM) /* active and in range SM? */
        {
          /* read all entries defined in PDO */
          for (er = 1; er <= e; er++)
          {
            c += 4;
            a += 5;
            PDO->BitSize[PDO->nPDO] += ec_siigetbyte(slave, a++);
            a += 2;
          }
          PDO->SMbitsize[PDO->SyncM[PDO->nPDO]] += PDO->BitSize[PDO->nPDO];
          Size += PDO->BitSize[PDO->nPDO];
          c++;
        }
        else /* PDO deactivated because SM is 0xff or > EC_MAXSM */
        {
          c += 4 * e;
          a += 8 * e;
          c++;
        }
        if (PDO->nPDO >= (EC_MAXEEPDO - 1))
          c = PDO->Length; /* limit number of PDO entries in buffer */
      } while (c < PDO->Length);
    }
    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */

    return (Size);
  }

  int ec_readstate(void)
  {
    uint16 slave, configadr, lowest, rval;
    ec_alstatust slstat;

    lowest = 0xff;
    ec_slave[0].ALstatuscode = 0;
    for (slave = 1; slave <= ec_slavecount; slave++)
    {
      configadr = ec_slave[slave].configadr;
      slstat.alstatus = 0;
      slstat.alstatuscode = 0;
      ec_FPRD(configadr, ECT_REG_ALSTAT, sizeof(slstat), &slstat, EC_TIMEOUTRET);
      rval = etohs(slstat.alstatus);
      ec_slave[slave].ALstatuscode = etohs(slstat.alstatuscode);
      if (rval < lowest)
        lowest = rval;
      ec_slave[slave].state = rval;
      ec_slave[0].ALstatuscode |= ec_slave[slave].ALstatuscode;
    }
    ec_slave[0].state = lowest;

    return lowest;
  }

  int ec_writestate(uint16 slave)
  {
    uint16 configadr, slstate;

    if (slave == 0)
    {
      slstate = htoes(ec_slave[slave].state);
      ec_BWR(0, ECT_REG_ALCTL, sizeof(slstate), &slstate, EC_TIMEOUTRET); /* write slave status */
    }
    else
    {
      configadr = ec_slave[slave].configadr;
      ec_FPWRw(configadr, ECT_REG_ALCTL, htoes(ec_slave[slave].state), EC_TIMEOUTRET); /* write slave status */
    }
    return 0;
  }

  uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout)
  {
    uint16 configadr, state, rval;
    struct timeval tv1, tv2, tve;
    ec_alstatust slstat;

    if (slave > ec_slavecount)
      return 0;
    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    configadr = ec_slave[slave].configadr;
    do
    {
      if (slave < 1)
      {
        rval = 0;
        ec_BRD(0, ECT_REG_ALSTAT, sizeof(rval), &rval, EC_TIMEOUTRET);
        rval = etohs(rval);
      }
      else
      {
        slstat.alstatus = 0;
        slstat.alstatuscode = 0;
        ec_FPRD(configadr, ECT_REG_ALSTAT, sizeof(slstat), &slstat, EC_TIMEOUTRET);
        rval = etohs(slstat.alstatus);
        ec_slave[slave].ALstatuscode = etohs(slstat.alstatuscode);
      }
      state = rval & 0x000f; /* read slave status */
      if (state != reqstate)
        usleep(1000);
      gettimeofday(&tv2, 0);
    } while ((state != reqstate) && timercmp(&tv2, &tve, <));
    ec_slave[slave].state = rval;

    return state;
  }

  uint8 ec_nextmbxcnt(uint8 cnt)
  {
    cnt++;
    if (cnt > 7)
      cnt = 1; /* wrap around to 1, not 0 */
    return cnt;
  }

  void ec_clearmbx(ec_mbxbuft *Mbx)
  {
    memset(*Mbx, 0x00, EC_MAXMBX);
  }

  int ec_mbxempty(uint16 slave, int timeout)
  {
    uint16 configadr;
    uint8 SMstat;
    int wkc;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    configadr = ec_slave[slave].configadr;
    do
    {
      wkc = ec_FPRD(configadr, ECT_REG_SM0STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
      SMstat = etohs(SMstat);
      if (((SMstat & 0x08) != 0) && (timeout > EC_LOCALDELAY))
        usleep(EC_LOCALDELAY);
      gettimeofday(&tv2, 0);
    } while (((wkc <= 0) || ((SMstat & 0x08) != 0)) && timercmp(&tv2, &tve, <));
    if ((wkc > 0) && ((SMstat & 0x08) == 0))
      return 1;
    return 0;
  }

  int ec_mbxsend(uint16 slave, ec_mbxbuft *mbx, int timeout)
  {
    uint16 mbxwo, mbxl, configadr;
    int wkc;

    wkc = 0;
    configadr = ec_slave[slave].configadr;
    mbxl = ec_slave[slave].mbx_l;
    if (mbxl > 0)
    {
      if (ec_mbxempty(slave, timeout))
      {
        mbxwo = ec_slave[slave].mbx_wo;
        /* write slave in mailbox */
        wkc = ec_FPWR(configadr, mbxwo, mbxl, mbx, EC_TIMEOUTRET);
      }
      else
        wkc = 0;
    }

    return wkc;
  }

  int ec_mbxreceive(uint16 slave, ec_mbxbuft *mbx, int timeout)
  {
    uint16 mbxro, mbxl, configadr;
    int wkc = 0;
    int wkc2;
    uint16 SMstat;
    uint8 SMcontr;
    ec_mbxheadert *mbxh;
    ec_emcyt *EMp;
    struct timeval mtv1, mtv2, mtve;

    configadr = ec_slave[slave].configadr;
    mbxl = ec_slave[slave].mbx_rl;
    if (mbxl > 0)
    {
      gettimeofday(&mtv1, 0);
      mtv2.tv_sec = 0;
      mtv2.tv_usec = timeout;
      timeradd(&mtv1, &mtv2, &mtve);
      wkc = 0;
      do /* wait for read mailbox available */
      {
        wkc = ec_FPRD(configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
        SMstat = etohs(SMstat);
        if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY))
          usleep(EC_LOCALDELAY);
        gettimeofday(&mtv2, 0);
      } while (((wkc <= 0) || ((SMstat & 0x08) == 0)) && timercmp(&mtv2, &mtve, <));

      if ((wkc > 0) && ((SMstat & 0x08) > 0)) /* read mailbox available ? */
      {
        mbxro = ec_slave[slave].mbx_ro;
        mbxh = (ec_mbxheadert *)mbx;
        do
        {
          wkc = ec_FPRD(configadr, mbxro, mbxl, mbx, EC_TIMEOUTRET); /* get mailbox */
          /* TODO : check for mailbox error response */
          if ((wkc > 0) && ((mbxh->mbxtype & 0x0f) == 0x03)) /* CoE response? */
          {
            EMp = (ec_emcyt *)mbx;
            if ((etohs(EMp->CANOpen) >> 12) == 0x01) /* Emergency request? */
            {
              ec_mbxemergencyerror(slave, etohs(EMp->ErrorCode), EMp->ErrorReg, EMp->bData, etohs(EMp->w1),
                                   etohs(EMp->w2));
              wkc = 0; /* prevent emergency to cascade up, it is already handled. */
            }
          }
          else
          {
            if (wkc <= 0) /* read mailbox lost */
            {
              SMstat ^= 0x0200; /* toggle repeat request */
              SMstat = htoes(SMstat);
              wkc2 = ec_FPWR(configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
              SMstat = etohs(SMstat);
              do /* wait for toggle ack */
              {
                wkc2 = ec_FPRD(configadr, ECT_REG_SM1CONTR, sizeof(SMcontr), &SMcontr, EC_TIMEOUTRET);
                gettimeofday(&mtv2, 0);
              } while (((wkc2 <= 0) || ((SMcontr & 0x02) != (HI_BYTE(SMstat) & 0x02))) && timercmp(&mtv2, &mtve, <));
              do /* wait for read mailbox available */
              {
                wkc2 = ec_FPRD(configadr, ECT_REG_SM1STAT, sizeof(SMstat), &SMstat, EC_TIMEOUTRET);
                SMstat = etohs(SMstat);
                if (((SMstat & 0x08) == 0) && (timeout > EC_LOCALDELAY))
                {
                  usleep(EC_LOCALDELAY);
                }
                gettimeofday(&mtv2, 0);
              } while (((wkc2 <= 0) || ((SMstat & 0x08) == 0)) && timercmp(&mtv2, &mtve, <));
            }
          }
        } while ((wkc <= 0) && timercmp(&mtv2, &mtve, <)); /* if WKC<=0 repeat */
      }
      else /* no read mailbox available */
      wkc = 0;
    }

    return wkc;
  }

  void ec_esidump(uint16 slave, uint8 *esibuf, uint8 test)
  {
    int address, incr;
    uint16 configadr;
    uint64 *p64;
    uint16 *p16;
    uint64 edat;
    uint8 eectl = ec_slave[slave].eep_pdi;

    ec_eeprom2master(slave); /* set eeprom control to master */
    configadr = ec_slave[slave].configadr;
    address = ECT_SII_START;
    p16 = (uint16*)esibuf;
    if (ec_slave[slave].eep_8byte)
      incr = 4;
    else
      incr = 2;
    do
    {
      edat = ec_readeepromFP(configadr, address, EC_TIMEOUTEEP);
      p64 = (uint64*)p16;
      *p64 = edat;
      p16 += incr;
      address += incr;
    } while ((address <= (EC_MAXEEPBUF >> 1)) && ((uint32)edat != 0xffffffff));

    if (eectl)
      ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */
  }

  uint32 ec_readeeprom(uint16 slave, uint16 eeproma, int timeout)
  {
    uint16 configadr;

    ec_eeprom2master(slave); /* set eeprom control to master */
    configadr = ec_slave[slave].configadr;
    return (ec_readeepromFP(configadr, eeproma, timeout));
  }

  int ec_writeeeprom(uint16 slave, uint16 eeproma, uint16 data, int timeout)
  {
    uint16 configadr;

    ec_eeprom2master(slave); /* set eeprom control to master */
    configadr = ec_slave[slave].configadr;
    return (ec_writeeepromFP(configadr, eeproma, data, timeout));
  }

  int ec_eeprom2master(uint16 slave)
  {
    int wkc = 1, cnt = 0;
    uint16 configadr;
    uint8 eepctl;

    if (ec_slave[slave].eep_pdi)
    {
      configadr = ec_slave[slave].configadr;
      eepctl = 2;
      do
      {
        wkc = ec_FPWR(configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl, EC_TIMEOUTRET); /* force Eeprom from PDI */
      } while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
      eepctl = 0;
      cnt = 0;
      do
      {
        wkc = ec_FPWR(configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl, EC_TIMEOUTRET); /* set Eeprom to master */
      } while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
      ec_slave[slave].eep_pdi = 0;
    }

    return wkc;
  }

  int ec_eeprom2pdi(uint16 slave)
  {
    int wkc = 1, cnt = 0;
    uint16 configadr;
    uint8 eepctl;

    if (!ec_slave[slave].eep_pdi)
    {
      configadr = ec_slave[slave].configadr;
      eepctl = 1;
      do
      {
        wkc = ec_FPWR(configadr, ECT_REG_EEPCFG, sizeof(eepctl), &eepctl, EC_TIMEOUTRET); /* set Eeprom to PDI */
      } while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
      ec_slave[slave].eep_pdi = 1;
    }

    return wkc;
  }

  uint16 ec_eeprom_waitnotbusyAP(uint16 aiadr, uint16 *estat, int timeout)
  {
    int wkc, cnt = 0, retval = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    do
    {
      if (cnt++)
        usleep(EC_LOCALDELAY);
      wkc = ec_APRD(aiadr, ECT_REG_EEPSTAT, sizeof(*estat), estat, EC_TIMEOUTRET);
      *estat = etohs(*estat);
      gettimeofday(&tv2, 0);
    } while (((wkc <= 0) || ((*estat & EC_ESTAT_BUSY) > 0)) && (timercmp(&tv2, &tve, <))); /* wait for eeprom ready */
    if ((*estat & EC_ESTAT_BUSY) == 0)
      retval = 1;
    return retval;
  }

  uint64 ec_readeepromAP(uint16 aiadr, uint16 eeproma, int timeout)
  {
    uint16 estat;
    uint32 edat32;
    uint64 edat64;
    ec_eepromt ed;
    int wkc, cnt, nackcnt = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    edat64 = 0;
    edat32 = 0;
    if (ec_eeprom_waitnotbusyAP(aiadr, &estat, timeout))
    {
      if (estat & EC_ESTAT_EMASK) /* error bits are set */
      {
        estat = htoes(EC_ECMD_NOP); /* clear error bits */
        wkc = ec_APWR(aiadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET);
      }

      do
      {
        ed.comm = htoes(EC_ECMD_READ);
        ed.addr = htoes(eeproma);
        ed.d2 = 0x0000;
        cnt = 0;
        do
          wkc = ec_APWR(aiadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET);
        while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
        if (wkc)
        {
          usleep(EC_LOCALDELAY);
          estat = 0x0000;
          if (ec_eeprom_waitnotbusyAP(aiadr, &estat, timeout))
          {
            if (estat & EC_ESTAT_NACK)
            {
              nackcnt++;
              usleep(EC_LOCALDELAY * 5);
            }
            else
            {
              nackcnt = 0;
              if (estat & EC_ESTAT_R64)
              {
                cnt = 0;
                do
                  wkc = ec_APRD(aiadr, ECT_REG_EEPDAT, sizeof(edat64), &edat64, EC_TIMEOUTRET);
                while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
              }
              else
              {
                cnt = 0;
                do
                  wkc = ec_APRD(aiadr, ECT_REG_EEPDAT, sizeof(edat32), &edat32, EC_TIMEOUTRET);
                while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
                edat64 = (uint64)edat32;
              }
            }
          }
        }
      } while ((nackcnt > 0) && (nackcnt < 3));
    }
    return edat64;
  }

  int ec_writeeepromAP(uint16 aiadr, uint16 eeproma, uint16 data, int timeout)
  {
    uint16 estat;
    ec_eepromt ed;
    int wkc, rval = 0, cnt = 0, nackcnt = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    if (ec_eeprom_waitnotbusyAP(aiadr, &estat, timeout))
    {
      if (estat & EC_ESTAT_EMASK) /* error bits are set */
      {
        estat = htoes(EC_ECMD_NOP); /* clear error bits */
        wkc = ec_APWR(aiadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET);
      }
      do
      {
        cnt = 0;
        do
          wkc = ec_APWR(aiadr, ECT_REG_EEPDAT, sizeof(data), &data, EC_TIMEOUTRET);
        while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));

        ed.comm = EC_ECMD_WRITE;
        ed.addr = eeproma;
        ed.d2 = 0x0000;
        cnt = 0;
        do
          wkc = ec_APWR(aiadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET);
        while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
        if (wkc)
        {
          usleep(EC_LOCALDELAY * 2);
          estat = 0x0000;
          if (ec_eeprom_waitnotbusyAP(aiadr, &estat, timeout))
          {
            if (estat & EC_ESTAT_NACK)
            {
              nackcnt++;
              usleep(EC_LOCALDELAY * 5);
            }
            else
            {
              nackcnt = 0;
              rval = 1;
            }
          }
        }

      } while ((nackcnt > 0) && (nackcnt < 3));
    }
    return rval;
  }

  uint16 ec_eeprom_waitnotbusyFP(uint16 configadr, uint16 *estat, int timeout)
  {
    int wkc, cnt = 0, retval = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    do
    {
      if (cnt++)
        usleep(EC_LOCALDELAY);
      wkc = ec_FPRD(configadr, ECT_REG_EEPSTAT, sizeof(*estat), estat, EC_TIMEOUTRET);
      *estat = etohs(*estat);
      gettimeofday(&tv2, 0);
    } while (((wkc <= 0) || ((*estat & EC_ESTAT_BUSY) > 0)) && (timercmp(&tv2, &tve, <))); /* wait for eeprom ready */
    if ((*estat & EC_ESTAT_BUSY) == 0)
      retval = 1;
    return retval;
  }

  uint64 ec_readeepromFP(uint16 configadr, uint16 eeproma, int timeout)
  {
    uint16 estat;
    uint32 edat32;
    uint64 edat64;
    ec_eepromt ed;
    int wkc, cnt, nackcnt = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    edat64 = 0;
    edat32 = 0;
    if (ec_eeprom_waitnotbusyFP(configadr, &estat, timeout))
    {
      if (estat & EC_ESTAT_EMASK) /* error bits are set */
      {
        estat = htoes(EC_ECMD_NOP); /* clear error bits */
        wkc = ec_FPWR(configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET);
      }

      do
      {
        ed.comm = htoes(EC_ECMD_READ);
        ed.addr = htoes(eeproma);
        ed.d2 = 0x0000;
        cnt = 0;
        do
          wkc = ec_FPWR(configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET);
        while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
        if (wkc)
        {
          usleep(EC_LOCALDELAY);
          estat = 0x0000;
          if (ec_eeprom_waitnotbusyFP(configadr, &estat, timeout))
          {
            if (estat & EC_ESTAT_NACK)
            {
              nackcnt++;
              usleep(EC_LOCALDELAY * 5);
            }
            else
            {
              nackcnt = 0;
              if (estat & EC_ESTAT_R64)
              {
                cnt = 0;
                do
                  wkc = ec_FPRD(configadr, ECT_REG_EEPDAT, sizeof(edat64), &edat64, EC_TIMEOUTRET);
                while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
              }
              else
              {
                cnt = 0;
                do
                  wkc = ec_FPRD(configadr, ECT_REG_EEPDAT, sizeof(edat32), &edat32, EC_TIMEOUTRET);
                while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
                edat64 = (uint64)edat32;
              }
            }
          }
        }
      } while ((nackcnt > 0) && (nackcnt < 3));
    }
    return edat64;
  }

  int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout)
  {
    uint16 estat;
    ec_eepromt ed;
    int wkc, rval = 0, cnt = 0, nackcnt = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    if (ec_eeprom_waitnotbusyFP(configadr, &estat, timeout))
    {
      if (estat & EC_ESTAT_EMASK) /* error bits are set */
      {
        estat = htoes(EC_ECMD_NOP); /* clear error bits */
        wkc = ec_FPWR(configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET);
      }
      do
      {
        cnt = 0;
        do
          wkc = ec_FPWR(configadr, ECT_REG_EEPDAT, sizeof(data), &data, EC_TIMEOUTRET);
        while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
        ed.comm = EC_ECMD_WRITE;
        ed.addr = eeproma;
        ed.d2 = 0x0000;
        cnt = 0;
        do
          wkc = ec_FPWR(configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET);
        while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
        if (wkc)
        {
          usleep(EC_LOCALDELAY * 2);
          estat = 0x0000;
          if (ec_eeprom_waitnotbusyFP(configadr, &estat, timeout))
          {
            if (estat & EC_ESTAT_NACK)
            {
              nackcnt++;
              usleep(EC_LOCALDELAY * 5);
            }
            else
            {
              nackcnt = 0;
              rval = 1;
            }
          }
        }
      } while ((nackcnt > 0) && (nackcnt < 3));
    }
    return rval;
  }

  void ec_readeeprom1(uint16 slave, uint16 eeproma)
  {
    uint16 configadr, estat;
    ec_eepromt ed;
    int wkc, cnt = 0;

    ec_eeprom2master(slave); /* set eeprom control to master */
    configadr = ec_slave[slave].configadr;
    if (ec_eeprom_waitnotbusyFP(configadr, &estat, EC_TIMEOUTEEP))
    {
      if (estat & EC_ESTAT_EMASK) /* error bits are set */
      {
        estat = htoes(EC_ECMD_NOP); /* clear error bits */
        wkc = ec_FPWR(configadr, ECT_REG_EEPCTL, sizeof(estat), &estat, EC_TIMEOUTRET);
      }
      ed.comm = htoes(EC_ECMD_READ);
      ed.addr = htoes(eeproma);
      ed.d2 = 0x0000;
      do
        wkc = ec_FPWR(configadr, ECT_REG_EEPCTL, sizeof(ed), &ed, EC_TIMEOUTRET);
      while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
    }
  }

  uint32 ec_readeeprom2(uint16 slave, int timeout)
  {
    uint16 estat, configadr;
    uint32 edat;
    int wkc, cnt = 0;
    struct timeval tv1, tv2, tve;

    gettimeofday(&tv1, 0);
    tv2.tv_sec = 0;
    tv2.tv_usec = timeout;
    timeradd(&tv1, &tv2, &tve);
    configadr = ec_slave[slave].configadr;
    edat = 0;
    estat = 0x0000;
    if (ec_eeprom_waitnotbusyFP(configadr, &estat, timeout))
    {
      do
        wkc = ec_FPRD(configadr, ECT_REG_EEPDAT, sizeof(edat), &edat, EC_TIMEOUTRET);
      while ((wkc <= 0) && (cnt++ < EC_DEFAULTRETRIES));
    }

    return edat;
  }

  static void ec_pushindex(uint8 idx, void *data, uint16 length)
  {
    if (ec_idxstack.pushed < EC_MAXBUF)
    {
      ec_idxstack.idx[ec_idxstack.pushed] = idx;
      ec_idxstack.data[ec_idxstack.pushed] = data;
      ec_idxstack.length[ec_idxstack.pushed] = length;
      ec_idxstack.pushed++;
    }
  }

  static int ec_pullindex(void)
  {
    int rval = -1;
    if (ec_idxstack.pulled < ec_idxstack.pushed)
    {
      rval = ec_idxstack.pulled;
      ec_idxstack.pulled++;
    }

    return rval;
  }

  int ec_send_processdata_group(uint8 group)
  {
    uint32 LogAdr;
    uint16 w1, w2;
    int length, sublength;
    uint8 idx;
    int wkc;
    void* data;
    boolean first = FALSE;
    uint16 currentsegment = 0;

    wkc = 0;
    if (ec_group[group].hasdc)
      first = TRUE;
    length = ec_group[group].Obytes + ec_group[group].Ibytes;
    LogAdr = ec_group[group].logstartaddr;
    if (length)
    {
      if (!group)
      {
        ec_idxstack.pushed = 0;
        ec_idxstack.pulled = 0;
      }
      wkc = 1;
      /* LRW blocked by one or more slaves ? */
      if (ec_group[group].blockLRW)
      {
        /* if inputs available generate LRD */
        if (ec_group[group].Ibytes)
        {
          currentsegment = ec_group[group].Isegment;
          data = ec_group[group].inputs;
          length = ec_group[group].Ibytes;
          LogAdr += ec_group[group].Obytes;
          /* segment transfer if needed */
          do
          {
            if (currentsegment == ec_group[group].Isegment)
              sublength = ec_group[group].IOsegment[currentsegment++] - ec_group[group].Ioffset;
            else
              sublength = ec_group[group].IOsegment[currentsegment++];
            /* get new index */
            idx = ec_getindex();
            w1 = LO_WORD(LogAdr);
            w2 = HI_WORD(LogAdr);
            ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LRD, idx, w1, w2, sublength, data);
            /* send frame */
            ec_outframe_red(idx);
            /* push index and data pointer on stack */
            ec_pushindex(idx, data, sublength);
            length -= sublength;
            LogAdr += sublength;
            data += sublength;
          } while (length && (currentsegment < ec_group[group].nsegments));
        }
        /* if outputs available generate LWR */
        if (ec_group[group].Obytes)
        {
          data = ec_group[group].outputs;
          length = ec_group[group].Obytes;
          LogAdr = ec_group[group].logstartaddr;
          currentsegment = 0;
          /* segment transfer if needed */
          do
          {
            sublength = ec_group[group].IOsegment[currentsegment++];
            if ((length - sublength) < 0)
              sublength = length;
            /* get new index */
            idx = ec_getindex();
            w1 = LO_WORD(LogAdr);
            w2 = HI_WORD(LogAdr);
            ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LWR, idx, w1, w2, sublength, data);
            /* send frame */
            ec_outframe_red(idx);
            /* push index and data pointer on stack */
            ec_pushindex(idx, data, sublength);
            length -= sublength;
            LogAdr += sublength;
            data += sublength;
          } while (length && (currentsegment < ec_group[group].nsegments));
        }
      }
      /* LRW can be used */
      else
      {
        if (ec_group[group].Obytes)
          data = ec_group[group].outputs;
        else
          data = ec_group[group].inputs;
        /* segment transfer if needed */
        do
        {
          sublength = ec_group[group].IOsegment[currentsegment++];
          /* get new index */
          idx = ec_getindex();
          w1 = LO_WORD(LogAdr);
          w2 = HI_WORD(LogAdr);
          ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LRW, idx, w1, w2, sublength, data);
          if (first)
          {
            ec_DCl = sublength;
            /* FPRMW in second datagram */
            ec_DCtO = ec_adddatagram(&ec_txbuf[idx], EC_CMD_FRMW, idx, FALSE,
                                     ec_slave[ec_group[group].DCnext].configadr, ECT_REG_DCSYSTIME, sizeof(ec_DCtime),
                                     &ec_DCtime);
            first = FALSE;
          }
          /* send frame */
          ec_outframe_red(idx);
          /* push index and data pointer on stack */
          ec_pushindex(idx, data, sublength);
          length -= sublength;
          LogAdr += sublength;
          data += sublength;
        } while (length && (currentsegment < ec_group[group].nsegments));
      }
    }

    return wkc;
  }

  int ec_receive_processdata_group(uint8 group, int timeout)
  {
    int pos, idx;
    int wkc = 0, wkc2;
    boolean first = FALSE;

    if (ec_group[group].hasdc)
      first = TRUE;
    /* get first index */
    pos = ec_pullindex();
    /* read the same number of frames as send */
    while (pos >= 0)
    {
      idx = ec_idxstack.idx[pos];
      wkc2 = ec_waitinframe(ec_idxstack.idx[pos], timeout);
      /* check if there is input data in frame */
      if ((wkc2 > EC_NOFRAME) && ((ec_rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD) || (ec_rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRW)))
            {
                        if(first)
                        {       
                        memcpy(ec_idxstack.data[pos], &ec_rxbuf[idx][EC_HEADERSIZE], ec_DCl);
                                memcpy(&wkc, &ec_rxbuf[idx][EC_HEADERSIZE + ec_DCl], EC_WKCSIZE);
                                wkc = etohs(wkc);
                                memcpy(&ec_DCtime, &ec_rxbuf[idx][ec_DCtO], sizeof(ec_DCtime));
                                ec_DCtime = etohll(ec_DCtime);
                                first = FALSE;
                        }
                        else
                        {       
                                /* copy input data back to process data buffer */
                            memcpy(ec_idxstack.data[pos], &ec_rxbuf[idx][EC_HEADERSIZE], ec_idxstack.length[pos]);
                                wkc += wkc2;
                        }       
            }
      /* release buffer */
      ec_setbufstat(idx, EC_BUF_EMPTY);
      /* get next index */
      pos = ec_pullindex();
    }

    return wkc;
  }

  int ec_send_processdata(void)
  {
    return ec_send_processdata_group(0);
  }

  int ec_receive_processdata(int timeout)
  {
    return ec_receive_processdata_group(0, timeout);
  }